From 00707ddb89392211a9128ca483c70c985eedccca Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Wed, 11 Mar 2026 13:15:08 +0000 Subject: [PATCH 001/251] mujoco warp documentation: per-world meshes --- doc/mjwarp/index.rst | 284 ++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 281 insertions(+), 3 deletions(-) diff --git a/doc/mjwarp/index.rst b/doc/mjwarp/index.rst index 85ab28ce..53386a11 100644 --- a/doc/mjwarp/index.rst +++ b/doc/mjwarp/index.rst @@ -460,10 +460,288 @@ Certain fields are safe to modify directly without compilation, enabling on-devi `GitHub issue 893 `__ tracks adding on-device updates for a subset of fields. -.. admonition:: Heterogeneous worlds - :class: note +Per-world meshes +---------------- - Heterogeneous worlds, for example: per-world meshes or number of degrees of freedom, are not currently available. +Per-world meshes enable heterogeneous worlds where different worlds simulate different meshes. The workflow +is: + +1. Create an :ref:`mjSpec` with **all** mesh assets and the **maximum** number of geom slots needed across variants. +2. Compile each variant by mutating the spec and calling ``spec.compile()``. +3. Compile a **base** model and create :class:`mjw.Model ` from it. +4. Override the relevant :class:`mjw.Model ` fields with per-world arrays built from the compiled + variants. + +**Example 1 — Geom-level** randomization (1 body, 1 geom, 2 mesh assets): + +The base scene includes all mesh assets. The geom references one mesh (``mesh_a``); a second mesh +(``mesh_b``) is available for per-world substitution. + +.. code-block:: xml + + + + + + + + + + + + + + +.. code-block:: python + + nworld = 4 + + # base spec: 1 body with 1 mesh geom, all mesh assets + spec = mujoco.MjSpec() + mesh_a = spec.add_mesh() + mesh_a.name = "mesh_a" + mesh_a.uservert = [0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1] + + mesh_b = spec.add_mesh() + mesh_b.name = "mesh_b" + mesh_b.uservert = [0, 0, 0, 2, 0, 0, 0, 2, 0, 0, 0, 2] + + body = spec.worldbody.add_body() + body.pos = [0, 0, 1] + body.add_freejoint() + geom = body.add_geom() + geom.name = "obj" + geom.type = mujoco.mjtGeom.mjGEOM_MESH + geom.meshname = "mesh_a" + + # compile each variant + geom.meshname = "mesh_a" + mjm_a = spec.compile() + geom.meshname = "mesh_b" + mjm_b = spec.compile() + + # restore and compile base + geom.meshname = "mesh_a" + mjm = spec.compile() + + m = mjw.put_model(mjm) + d = mjw.make_data(mjm, nworld=nworld) + + # build per-world arrays: worlds 0-1 use mesh_a, worlds 2-3 use mesh_b + geom_id = mujoco.mj_name2id(mjm, mujoco.mjtObj.mjOBJ_GEOM, "obj") + variants = [mjm_a, mjm_b] + assignment = [0, 0, 1, 1] # variant index per world + + # build per-world arrays + dataid = np.tile(mjm.geom_dataid, (nworld, 1)) + geom_size = np.zeros((nworld, mjm.ngeom, 3)) + geom_aabb = np.zeros((nworld, mjm.ngeom, 2, 3)) + geom_rbound = np.zeros((nworld, mjm.ngeom)) + geom_pos = np.zeros((nworld, mjm.ngeom, 3)) + body_mass = np.zeros((nworld, mjm.nbody)) + body_subtreemass = np.zeros((nworld, mjm.nbody)) + body_inertia = np.zeros((nworld, mjm.nbody, 3)) + body_invweight0 = np.zeros((nworld, mjm.nbody, 2)) + body_ipos = np.zeros((nworld, mjm.nbody, 3)) + body_iquat = np.zeros((nworld, mjm.nbody, 4)) + + for w in range(nworld): + ref = variants[assignment[w]] + dataid[w, geom_id] = ref.geom_dataid[geom_id] + geom_size[w] = ref.geom_size + geom_aabb[w] = ref.geom_aabb.reshape(mjm.ngeom, 2, 3) + geom_rbound[w] = ref.geom_rbound + geom_pos[w] = ref.geom_pos + body_mass[w] = ref.body_mass + body_subtreemass[w] = ref.body_subtreemass + body_inertia[w] = ref.body_inertia + body_invweight0[w] = ref.body_invweight0 + body_ipos[w] = ref.body_ipos + body_iquat[w] = ref.body_iquat + + m.geom_dataid = wp.array(dataid, dtype=int) + m.geom_size = wp.array(geom_size, dtype=wp.vec3) + m.geom_aabb = wp.array(geom_aabb, dtype=wp.vec3) + m.geom_rbound = wp.array(geom_rbound, dtype=float) + m.geom_pos = wp.array(geom_pos, dtype=wp.vec3) + m.body_mass = wp.array(body_mass, dtype=float) + m.body_subtreemass = wp.array(body_subtreemass, dtype=float) + m.body_inertia = wp.array(body_inertia, dtype=wp.vec3) + m.body_invweight0 = wp.array(body_invweight0, dtype=wp.vec2) + m.body_ipos = wp.array(body_ipos, dtype=wp.vec3) + m.body_iquat = wp.array(body_iquat, dtype=wp.quat) + +**Example 2 — Body-level** randomization (1 body, 1 or 2 geoms, 3 mesh assets): + +.. admonition:: Maximum geom count + :class: important + + For body-level randomization, the base ``mjModel`` provided to ``mjw.put_model`` should specify the **maximum number + of geoms** required across all variants. Geom slots that are unused in a particular variant can be disabled + (e.g., ``contype=0``, ``conaffinity=0``, ``dataid=-1``), but they should still be present as part of the body in the + base model. + +.. code-block:: xml + + + + + + + + + + + + + + + + +.. code-block:: python + + nworld = 6 + + # base spec: body with 2 geom slots (max across variants), all mesh assets + spec = mujoco.MjSpec() + for name, scale in [("mA", 1), ("mB", 2), ("mC", 3)]: + mesh = spec.add_mesh() + mesh.name = name + mesh.uservert = [0, 0, 0, scale, 0, 0, 0, scale, 0, 0, 0, scale] + + body = spec.worldbody.add_body() + body.name = "obj" + body.pos = [0, 0, 1] + body.add_freejoint() + + g0 = body.add_geom() + g0.name = "obj_0" + g0.type = mujoco.mjtGeom.mjGEOM_MESH + g0.meshname = "mA" + + # null geom slot: disabled collision, no mesh + g1 = body.add_geom() + g1.name = "obj_1" + g1.size = [0.001, 0, 0] + g1.contype = 0 + g1.conaffinity = 0 + g1.mass = 0 + + # variant A: 1 geom (mesh mA), g1 stays null + mjm_a = spec.compile() + + # variant B: 2 geoms (mesh mB + mC) + g0.meshname = "mB" + g1.type = mujoco.mjtGeom.mjGEOM_MESH + g1.meshname = "mC" + g1.contype = 1 + g1.conaffinity = 1 + mjm_b = spec.compile() + + # restore base and compile + g0.meshname = "mA" + g1.type = mujoco.mjtGeom.mjGEOM_SPHERE + g1.contype = 0 + g1.conaffinity = 0 + mjm = spec.compile() + + m = mjw.put_model(mjm) + d = mjw.make_data(mjm, nworld=nworld) + + # worlds 0-2: variant A (1 active geom), worlds 3-5: variant B (2 active geoms) + variants = [mjm_a, mjm_b] + assignment = [0, 0, 0, 1, 1, 1] + + geom0_id = mujoco.mj_name2id(mjm, mujoco.mjtObj.mjOBJ_GEOM, "obj_0") + geom1_id = mujoco.mj_name2id(mjm, mujoco.mjtObj.mjOBJ_GEOM, "obj_1") + body_id = mjm.geom_bodyid[geom0_id] + + # build per-world arrays + dataid = np.tile(mjm.geom_dataid, (nworld, 1)) + geom_size = np.zeros((nworld, mjm.ngeom, 3)) + geom_rbound = np.zeros((nworld, mjm.ngeom)) + geom_aabb = np.zeros((nworld, mjm.ngeom, 2, 3)) + geom_pos = np.zeros((nworld, mjm.ngeom, 3)) + body_mass = np.zeros((nworld, mjm.nbody)) + body_subtreemass = np.zeros((nworld, mjm.nbody)) + body_inertia = np.zeros((nworld, mjm.nbody, 3)) + body_invweight0 = np.zeros((nworld, mjm.nbody, 2)) + body_ipos = np.zeros((nworld, mjm.nbody, 3)) + body_iquat = np.zeros((nworld, mjm.nbody, 4)) + + for w in range(nworld): + ref = variants[assignment[w]] + dataid[w] = ref.geom_dataid + # disable unused geom slot for variant A + if assignment[w] == 0: + dataid[w, geom1_id] = -1 + geom_size[w] = ref.geom_size + geom_rbound[w] = ref.geom_rbound + geom_aabb[w] = ref.geom_aabb.reshape(mjm.ngeom, 2, 3) + geom_pos[w] = ref.geom_pos + body_mass[w] = ref.body_mass + body_subtreemass[w] = ref.body_subtreemass + body_inertia[w] = ref.body_inertia + body_invweight0[w] = ref.body_invweight0 + body_ipos[w] = ref.body_ipos + body_iquat[w] = ref.body_iquat + + m.geom_dataid = wp.array(dataid, dtype=int) + m.geom_size = wp.array(geom_size, dtype=wp.vec3) + m.geom_rbound = wp.array(geom_rbound, dtype=float) + m.geom_aabb = wp.array(geom_aabb, dtype=wp.vec3) + m.geom_pos = wp.array(geom_pos, dtype=wp.vec3) + m.body_mass = wp.array(body_mass, dtype=float) + m.body_subtreemass = wp.array(body_subtreemass, dtype=float) + m.body_inertia = wp.array(body_inertia, dtype=wp.vec3) + m.body_invweight0 = wp.array(body_invweight0, dtype=wp.vec2) + m.body_ipos = wp.array(body_ipos, dtype=wp.vec3) + m.body_iquat = wp.array(body_iquat, dtype=wp.quat) + +**Batched fields** — fields that must be overridden for per-world meshes: + +.. list-table:: + :width: 90% + :align: left + :widths: 3 2 3 + :header-rows: 1 + + * - Field + - dtype + - Shape + * - ``geom_dataid`` + - ``int`` + - ``(nworld, ngeom)`` + * - ``geom_size`` + - ``wp.vec3`` + - ``(nworld, ngeom)`` + * - ``geom_aabb`` + - ``wp.vec3`` + - ``(nworld, ngeom, 2)`` + * - ``geom_rbound`` + - ``float`` + - ``(nworld, ngeom)`` + * - ``geom_pos`` + - ``wp.vec3`` + - ``(nworld, ngeom)`` + * - ``body_mass`` + - ``float`` + - ``(nworld, nbody)`` + * - ``body_subtreemass`` + - ``float`` + - ``(nworld, nbody)`` + * - ``body_inertia`` + - ``wp.vec3`` + - ``(nworld, nbody)`` + * - ``body_invweight0`` + - ``wp.vec2`` + - ``(nworld, nbody)`` + * - ``body_ipos`` + - ``wp.vec3`` + - ``(nworld, nbody)`` + * - ``body_iquat`` + - ``wp.quat`` + - ``(nworld, nbody)`` Batch Rendering =============== From 0b54638df486242b54953274b105282b0b157f8a Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Wed, 1 Apr 2026 01:53:33 -0700 Subject: [PATCH 002/251] Import NVIDIA/warp from GitHub. PiperOrigin-RevId: 892794388 Change-Id: Ic681071dc68bfaecaaab4e131c1c464dca99f9b5 --- mjx/cuda_requirements.txt | 10 +- .../warp/_src/jax_experimental/custom_call.py | 6 +- .../warp/_src/jax_experimental/ffi.py | 178 ++++++++++++++---- mjx/pyproject.toml | 2 +- 4 files changed, 152 insertions(+), 44 deletions(-) diff --git a/mjx/cuda_requirements.txt b/mjx/cuda_requirements.txt index 0f9ae0d5..2ced5d12 100644 --- a/mjx/cuda_requirements.txt +++ b/mjx/cuda_requirements.txt @@ -16,8 +16,8 @@ jax-cuda12-pjrt==0.5.3; python_version >= '3.10' \ jax-cuda12-pjrt==0.4.30; python_version == '3.9' \ --hash=sha256:895d0198ad99638fcaf976c47592e2a543eef79ea15fabd24a402d055390c328 \ --hash=sha256:c36fb1e0c236563bf3a87e70f4d1ab28a31d7cf5d722c9ede30c4172116e8bcb -warp-lang==1.11.1 \ - --hash=sha256:1ad11f1fa775269e991a3d55039152c8a504baf86701c849b485cb8e66c49d15 \ - --hash=sha256:8b098f41e71d421d80ee7562e38aa8380ff6b0d3b4c6ee866cfbdef733ac5bdc \ - --hash=sha256:5d0904b0eefcc81f39ba65375427a3de99006088aa43e24a9011263f07d0cd07 \ - --hash=sha256:15dc10aa51fb0fdbe1ca16d52e5fadca35a47ffd9d0c636826506f96bb2e7c41 +warp-lang==1.12.0 \ + --hash=sha256:c78c3701d5cad86c30ef5017410d294ec46a396bb0d502ee1c98743494f3a62f \ + --hash=sha256:a1436f60a1881cd94f787e751a83fc0987626be2d3e2b4e74c64a6947c6d1266 \ + --hash=sha256:a2d6decba693aba5b828573c4414fd6a3f4c4a934db9c322736ef2b3fa99fe76 \ + --hash=sha256:697248edd2f1e2952f50e3db33b214af76173641a8894aacc467bed6dc247f8a diff --git a/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/custom_call.py b/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/custom_call.py index b46a1071..0adf6435 100644 --- a/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/custom_call.py +++ b/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/custom_call.py @@ -19,7 +19,7 @@ from functools import reduce import warp as wp from warp._src.context import type_str from warp._src.jax import get_jax_device -from warp._src.types import array_t, launch_bounds_t, strides_from_shape +from warp._src.types import array_t, launch_bounds_t, matches_array_class, strides_from_shape from warp._src.utils import warn _wp_module_name_ = "warp.jax_experimental.custom_call" @@ -340,7 +340,7 @@ def _create_jax_warp_primitive(): wtype = warg.type rtt = ir.RankedTensorType(actual.type) - if not isinstance(wtype, wp.array): + if not matches_array_class(wtype, wp.array): raise Exception("Only contiguous arrays are supported for Jax kernel arguments") if not base_type_is_compatible(wtype.dtype, rtt.element_type): @@ -364,7 +364,7 @@ def _create_jax_warp_primitive(): for warg in wp_kernel.adj.args[len(args) :]: wtype = warg.type - if not isinstance(wtype, wp.array): + if not matches_array_class(wtype, wp.array): raise Exception("Only contiguous arrays are supported for Jax kernel arguments") # Infer dimensions from the first input. diff --git a/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/ffi.py b/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/ffi.py index f5c925dd..e9fe408f 100644 --- a/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/ffi.py +++ b/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/ffi.py @@ -29,19 +29,23 @@ import warp as wp from warp._src.codegen import get_full_arg_spec, make_full_qualified_name from warp._src.context import CudaMemcpyKind from warp._src.jax import get_jax_device -from warp._src.types import array_t, launch_bounds_t, strides_from_shape, type_size_in_bytes, type_to_warp +from warp._src.types import ( + array_t, + launch_bounds_t, + matches_array_class, + strides_from_shape, + type_size_in_bytes, + type_to_warp, +) from .xla_ffi import * _wp_module_name_ = "warp.jax_experimental.ffi" -# Type alias for differentiable kernel cache key -DiffKernelCacheKey = tuple[Callable, tuple, int, str, tuple[str, ...]] - # Holders for the custom callbacks to keep them alive. -_FFI_KERNEL_REGISTRY: dict[str, FfiKernel] = {} -_FFI_DIFF_KERNEL_REGISTRY: dict[DiffKernelCacheKey, Callable] = {} -_FFI_CALLABLE_REGISTRY: dict[str, FfiCallable] = {} +_FFI_KERNEL_REGISTRY: dict[tuple, FfiKernel] = {} +_FFI_DIFF_KERNEL_REGISTRY: dict[tuple, Callable] = {} +_FFI_CALLABLE_REGISTRY: dict[tuple, FfiCallable] = {} _FFI_CALLBACK_REGISTRY: dict[str, ctypes.CFUNCTYPE] = {} _FFI_REGISTRY_LOCK = threading.Lock() @@ -61,6 +65,21 @@ def check_jax_version(): raise RuntimeError(msg) +def collapse_batch_dims(shape, desired_ndim): + # roll leading batch dims into one + while len(shape) > desired_ndim: + shape = (shape[0] * shape[1], *shape[2:]) + return shape + + +def compute_batch_size(shape, batch_ndim): + # compute product of batch dims at front + batch_size = 1 + for i in range(batch_ndim): + batch_size *= shape[i] + return batch_size + + class GraphMode(IntEnum): """CUDA graph capture modes for :func:`warp.jax_experimental.jax_callable`. @@ -91,7 +110,7 @@ class FfiArg: self.name = name self.type = type self.in_out = in_out - self.is_array = isinstance(type, wp.array) + self.is_array = matches_array_class(type, wp.array) if self.is_array: if hasattr(type.dtype, "_wp_scalar_type_"): @@ -125,7 +144,15 @@ class FfiLaunchDesc: class FfiKernel: def __init__( - self, kernel, num_outputs, vmap_method, launch_dims, output_dims, in_out_argnames, module_preload_mode + self, + kernel, + num_outputs, + vmap_method, + launch_dims, + output_dims, + in_out_argnames, + module_preload_mode, + has_side_effect=False, ): self.kernel = kernel self.name = generate_unique_name(kernel.func) @@ -134,6 +161,7 @@ class FfiKernel: self.launch_dims = launch_dims self.output_dims = output_dims self.module_preload_mode = module_preload_mode + self.has_side_effect = has_side_effect self.first_array_arg = None self.launch_id = 0 self.launch_descriptors = {} @@ -250,7 +278,8 @@ class FfiKernel: out_types.append(get_jax_output_type(input_arg, input_value.shape)) # launch dimensions - if launch_dims is None: + infer_launch_dims = launch_dims is None + if infer_launch_dims: # use the shape of the first input array if self.first_array_arg is not None: launch_dims = get_warp_shape(self.input_args[self.first_array_arg], args[self.first_array_arg].shape) @@ -284,6 +313,7 @@ class FfiKernel: out_types, vmap_method=vmap_method, input_output_aliases=self.input_output_aliases, + has_side_effect=self.has_side_effect, ) # preload on the specified devices @@ -303,7 +333,9 @@ class FfiKernel: # save launch data to be retrieved by callback launch_id = self.launch_id - self.launch_descriptors[launch_id] = FfiLaunchDesc(static_inputs, launch_dims) + self.launch_descriptors[launch_id] = FfiLaunchDesc( + static_inputs, launch_dims if not infer_launch_dims else None + ) self.launch_id += 1 return call(*args, launch_id=launch_id) @@ -343,19 +375,23 @@ class FfiKernel: assert num_inputs == self.num_inputs assert num_outputs == self.num_outputs - launch_bounds = launch_bounds_t(launch_desc.launch_dims) - # first kernel param is the launch bounds kernel_params = (ctypes.c_void_p * (1 + self.num_kernel_args))() - kernel_params[0] = ctypes.addressof(launch_bounds) - arg_refs = [] + batch_size = None # input and in-out args for i, input_arg in enumerate(self.input_args): if input_arg.is_array: buffer = inputs[i].contents - shape = buffer.dims[: input_arg.type.ndim] + shape = buffer.dims[: buffer.rank - input_arg.dtype_ndim] + if buffer.rank > input_arg.jax_ndim: + # handle batching + shape = collapse_batch_dims(shape, input_arg.type.ndim) + if batch_size is None: + batch_size = compute_batch_size( + buffer.dims[: buffer.rank], buffer.rank - input_arg.jax_ndim + ) strides = strides_from_shape(shape, input_arg.type.dtype) arg = array_t(buffer.data, 0, input_arg.type.ndim, shape, strides) kernel_params[i + 1] = ctypes.addressof(arg) @@ -370,12 +406,34 @@ class FfiKernel: # pure output args (skip in-out FFI buffers) for i, output_arg in enumerate(self.output_args): buffer = outputs[i + self.num_in_out].contents - shape = buffer.dims[: output_arg.type.ndim] + shape = buffer.dims[: buffer.rank - output_arg.dtype_ndim] + if buffer.rank > output_arg.jax_ndim: + # handle batching + shape = collapse_batch_dims(shape, output_arg.type.ndim) + if batch_size is None: + batch_size = compute_batch_size( + buffer.dims[: buffer.rank], buffer.rank - output_arg.jax_ndim + ) strides = strides_from_shape(shape, output_arg.type.dtype) arg = array_t(buffer.data, 0, output_arg.type.ndim, shape, strides) kernel_params[num_inputs + i + 1] = ctypes.addressof(arg) arg_refs.append(arg) # keep a reference + # determine launch bounds + if launch_desc.launch_dims is None: + # infer launch dims from argument shape, works with vmap + arr = arg_refs[self.first_array_arg] + launch_dims = arr.shape[: arr.ndim] + else: + # use specified launch dims + launch_dims = launch_desc.launch_dims + if batch_size is not None: + # roll batch size into the first launch dimension + launch_dims = (batch_size * launch_dims[0], *launch_dims[1:]) + + launch_bounds = launch_bounds_t(launch_dims) + kernel_params[0] = ctypes.addressof(launch_bounds) + # get device and stream device = wp.get_cuda_device(get_device_ordinal_from_callframe(call_frame.contents)) stream = get_stream_from_callframe(call_frame.contents) @@ -808,7 +866,7 @@ class FfiCallable: for i, arg in enumerate(self.input_args): if arg.is_array: buffer = inputs[i].contents - shape = buffer.dims[: buffer.rank - arg.dtype_ndim] + shape = collapse_batch_dims(buffer.dims[: buffer.rank - arg.dtype_ndim], arg.type.ndim) arr = wp.array(ptr=buffer.data, dtype=arg.type.dtype, shape=shape, device=device) arg_list.append(arr) else: @@ -819,7 +877,7 @@ class FfiCallable: # pure output args (skip in-out FFI buffers) for i, arg in enumerate(self.output_args): buffer = outputs[i + self.num_in_out].contents - shape = buffer.dims[: buffer.rank - arg.dtype_ndim] + shape = collapse_batch_dims(buffer.dims[: buffer.rank - arg.dtype_ndim], arg.type.ndim) arr = wp.array(ptr=buffer.data, dtype=arg.type.dtype, shape=shape, device=device) arg_list.append(arr) @@ -1095,6 +1153,7 @@ def jax_kernel( in_out_argnames=None, module_preload_mode=ModulePreloadMode.CURRENT_DEVICE, enable_backward: bool = False, + has_side_effect: bool = False, ): """Create a JAX callback from a Warp kernel. @@ -1103,21 +1162,23 @@ def jax_kernel( Args: kernel: The Warp kernel to launch. num_outputs: Specify the number of output arguments if greater than 1. - This must include the number of ``in_out_arguments``. + This must include the number of ``in_out_arguments``. vmap_method: String specifying how the callback transforms under ``vmap()``. - This argument can also be specified for individual calls. + This argument can also be specified for individual calls. launch_dims: Specify the default kernel launch dimensions. If None, launch - dimensions are inferred from the shape of the first array argument. - This argument can also be specified for individual calls. + dimensions are inferred from the shape of the first array argument. + This argument can also be specified for individual calls. output_dims: Specify the default dimensions of output arrays. If None, output - dimensions are inferred from the launch dimensions. - This argument can also be specified for individual calls. + dimensions are inferred from the launch dimensions. + This argument can also be specified for individual calls. in_out_argnames: Names of arguments that are both inputs and outputs (aliased buffers). These must be array arguments that appear before any pure output arguments in the kernel signature. The number of in-out arguments is included in ``num_outputs``. Not supported when ``enable_backward=True``. module_preload_mode: Specify the devices where the module should be preloaded. enable_backward: Enable automatic differentiation for this kernel. + has_side_effect: Whether the custom call has side effects. When True, + the FFI call will be executed even when the outputs are not used. Limitations: - All kernel arguments must be contiguous arrays or scalars. @@ -1129,21 +1190,41 @@ def jax_kernel( check_jax_version() + if isinstance(output_dims, dict): + hashable_output_dims = tuple(sorted(output_dims.items())) + elif hasattr(output_dims, "__len__"): + hashable_output_dims = tuple(output_dims) + else: + hashable_output_dims = output_dims + + if hasattr(launch_dims, "__len__"): + hashable_launch_dims = tuple(launch_dims) + else: + hashable_launch_dims = launch_dims + if not enable_backward: key = ( kernel.func, kernel.sig, num_outputs, vmap_method, - tuple(launch_dims) if launch_dims else launch_dims, - tuple(sorted(output_dims.items())) if output_dims else output_dims, + hashable_launch_dims, + hashable_output_dims, module_preload_mode, + has_side_effect, ) with _FFI_REGISTRY_LOCK: if key not in _FFI_KERNEL_REGISTRY: new_kernel = FfiKernel( - kernel, num_outputs, vmap_method, launch_dims, output_dims, in_out_argnames, module_preload_mode + kernel, + num_outputs, + vmap_method, + launch_dims, + output_dims, + in_out_argnames, + module_preload_mode, + has_side_effect=has_side_effect, ) _FFI_KERNEL_REGISTRY[key] = new_kernel @@ -1173,7 +1254,7 @@ def jax_kernel( static_args = [] for i, p in enumerate(parameters[:num_inputs]): param_type = p.annotation - if not isinstance(param_type, wp.array): + if not matches_array_class(param_type, wp.array): if param_type in wp._src.types.value_types: static_args.append(i) else: @@ -1183,7 +1264,7 @@ def jax_kernel( # determine launch dimensions from the shape of the first input array for i, p in enumerate(parameters[:num_inputs]): param_type = p.annotation - if isinstance(param_type, wp.array): + if matches_array_class(param_type, wp.array): arg = call_args[i] arg_shape = tuple(arg.shape) if hasattr(param_type.dtype, "_wp_scalar_type_"): @@ -1203,7 +1284,13 @@ def jax_kernel( fwd_kernel_wrapper.__annotations__ = {p.name: p.annotation for p in parameters} fwd_kernel_wrapper.__annotations__["return"] = None - jax_fwd_kernel = jax_callable(fwd_kernel_wrapper, num_outputs=num_outputs, vmap_method=vmap_method) + jax_fwd_kernel = jax_callable( + fwd_kernel_wrapper, + num_outputs=num_outputs, + vmap_method=vmap_method, + module_preload_mode=module_preload_mode, + has_side_effect=has_side_effect, + ) # backward arguments only include static args once bwd_arg_count = 2 * parameter_count - len(static_args) @@ -1285,6 +1372,8 @@ def jax_kernel( bwd_kernel_wrapper, num_outputs=len(bwd_input_params) - len(static_args), vmap_method=vmap_method, + module_preload_mode=module_preload_mode, + has_side_effect=has_side_effect, ) differentiable_input_indices = [i for i in range(num_inputs) if i not in static_args] @@ -1331,7 +1420,7 @@ def jax_kernel( if ann is None: continue # Check if annotation is a warp array type (annotation is an instance of wp.array) - is_array_ann = isinstance(ann, wp.array) + is_array_ann = matches_array_class(ann, wp.array) if not is_array_ann: continue dtype_ndim = 0 @@ -1355,6 +1444,15 @@ def jax_kernel( jax_func = jax.custom_vjp(jax_fwd_kernel, nondiff_argnums=tuple(static_args)) jax_func.defvjp(fwd_function, bwd_function) + key = ( + kernel.func, + kernel.sig, + num_outputs, + vmap_method, + module_preload_mode, + has_side_effect, + ) + if static_args: static_names = [parameters[i].name for i in static_args] @@ -1364,7 +1462,7 @@ def jax_kernel( _user_callable.__signature__ = signature # Cache differentiable wrapper - key = (kernel.func, kernel.sig, num_outputs, vmap_method, tuple(sorted(static_names))) + key = (*key, tuple(sorted(static_names))) with _FFI_REGISTRY_LOCK: cached = _FFI_DIFF_KERNEL_REGISTRY.get(key) if cached is None: @@ -1373,7 +1471,7 @@ def jax_kernel( return _FFI_DIFF_KERNEL_REGISTRY[key] # Cache differentiable wrapper (no static args) - key = (kernel.func, kernel.sig, num_outputs, vmap_method, ()) + key = (*key, ()) with _FFI_REGISTRY_LOCK: cached = _FFI_DIFF_KERNEL_REGISTRY.get(key) if cached is None: @@ -1426,6 +1524,8 @@ def jax_callable( graph_cache_max: Maximum number of cached graphs captured using ``GraphMode.WARP``. If ``None``, use ``warp.jax_experimental.get_jax_callable_default_graph_cache_max()``. module_preload_mode: Specify the devices where the module should be preloaded. + has_side_effect: Whether the custom call has side effects. When True, + the FFI call will be executed even when the outputs are not used. Limitations: - All kernel arguments must be contiguous arrays or scalars. @@ -1440,14 +1540,22 @@ def jax_callable( if graph_cache_max is None: graph_cache_max = FfiCallable.default_graph_cache_max + if isinstance(output_dims, dict): + hashable_output_dims = tuple(sorted(output_dims.items())) + elif hasattr(output_dims, "__len__"): + hashable_output_dims = tuple(output_dims) + else: + hashable_output_dims = output_dims + # Note: we don't include graph_cache_max in the key, it is applied below. key = ( func, num_outputs, graph_mode, vmap_method, - tuple(sorted(output_dims.items())) if output_dims else output_dims, + hashable_output_dims, module_preload_mode, + has_side_effect, ) with _FFI_REGISTRY_LOCK: diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index ff7133cd..60063b70 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -36,7 +36,7 @@ dependencies = [ [project.optional-dependencies] warp = [ - "warp-lang==1.11.1", + "warp-lang==1.12.0", ] [project.scripts] From 6d320384172a3dfcfc26bdd1d2bb659c7dd749a5 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 1 Apr 2026 03:47:08 -0700 Subject: [PATCH 003/251] Move all texture creation functions into texture_util. PiperOrigin-RevId: 892838123 Change-Id: I84a536b111c3d6b5391513d6623c2e9a4d02f3df --- .../filament/filament/filament_context.cc | 25 +++++---- .../filament/filament/model_util.cc | 19 ------- .../filament/filament/model_util.h | 10 ---- .../filament/filament/render_target_util.cc | 34 +----------- .../filament/filament/render_target_util.h | 15 ++---- .../filament/filament/scene_view.cc | 4 +- .../filament/filament/texture_util.cc | 54 ++++++++++++++++++- .../filament/filament/texture_util.h | 25 +++++++++ 8 files changed, 99 insertions(+), 87 deletions(-) diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index d783cefa..973425d1 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -45,6 +45,7 @@ #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/render_target_util.h" #include "experimental/filament/filament/scene_view.h" +#include "experimental/filament/filament/texture_util.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -62,16 +63,18 @@ FilamentContext::FilamentContext(const mjrFilamentConfig* config) engine_ = engine_builder.build(); renderer_ = engine_->createRenderer(); - #ifdef __EMSCRIPTEN__ - window_swap_chain_ = engine_->createSwapChain(nullptr); - #else +#ifdef __EMSCRIPTEN__ + window_swap_chain_ = engine_->createSwapChain(nullptr); +#else if (config_.native_window) { window_swap_chain_ = engine_->createSwapChain(config_.native_window); } else { - window_swap_chain_ = engine_->createSwapChain(config_.width, config_.height); + window_swap_chain_ = + engine_->createSwapChain(config_.width, config_.height); } - #endif - offscreen_swap_chain_ = engine_->createSwapChain(config_.width, config_.height); +#endif + offscreen_swap_chain_ = + engine_->createSwapChain(config_.width, config_.height); object_manager_ = std::make_unique(engine_); } @@ -183,11 +186,13 @@ void FilamentContext::SetFrameBuffer(int framebuffer) { void FilamentContext::PrepareRenderTargets(int width, int height) { color_target_ = std::make_unique( - engine_, kRenderTargetColor, kRenderTargetDepth); + engine_, RenderTargetTextureType::kColor, + RenderTargetTextureType::kDepth); color_target_->Prepare(width, height); depth_target_ = std::make_unique( - engine_, kRenderTargetDepthColor, kRenderTargetDepth); + engine_, RenderTargetTextureType::kDepthColor, + RenderTargetTextureType::kDepth); depth_target_->Prepare(width, height); } @@ -310,8 +315,6 @@ double FilamentContext::GetFrameRate() const { return 1.0e9 / static_cast(ns); } -void FilamentContext::UpdateGui() { - DrawGui(scene_view_.get()); -} +void FilamentContext::UpdateGui() { DrawGui(scene_view_.get()); } } // namespace mujoco diff --git a/src/experimental/filament/filament/model_util.cc b/src/experimental/filament/filament/model_util.cc index 78fca85c..62e72d73 100644 --- a/src/experimental/filament/filament/model_util.cc +++ b/src/experimental/filament/filament/model_util.cc @@ -486,23 +486,4 @@ filament::IndexBuffer* CreateIndexBuffer(filament::Engine* engine, FillSequence); } } - -filament::Texture* CreateTexture(filament::Engine* engine, const mjModel* model, - int id, TextureType texture_type) { - if (id < 0 || id >= model->ntex) { - mju_error("Invalid texture index %d", id); - } - - const int width = model->tex_width[id]; - const int height = model->tex_height[id]; - const bool is_srgb = model->tex_colorspace[id] == mjCOLORSPACE_SRGB; - const int num_channels = model->tex_nchannel[id]; - const mjtByte* data = model->tex_data + model->tex_adr[id]; - filament::Texture* texture = - texture_type == TextureType::kNormal2d - ? Create2dTexture(engine, width, height, num_channels, data, is_srgb) - : CreateCubeTexture(engine, width, height, num_channels, data, - is_srgb); - return texture; -} } // namespace mujoco diff --git a/src/experimental/filament/filament/model_util.h b/src/experimental/filament/filament/model_util.h index 57171674..edf14fed 100644 --- a/src/experimental/filament/filament/model_util.h +++ b/src/experimental/filament/filament/model_util.h @@ -37,12 +37,6 @@ enum class MeshType { kHeightField, }; -// The types of textures stored in the mjModel. -enum class TextureType { - kNormal2d, - kCube, -}; - // Generates a filament VertexBuffer for a given mesh in the mjModel. filament::VertexBuffer* CreateVertexBuffer(filament::Engine* engine, const mjModel* model, int id, @@ -54,10 +48,6 @@ filament::IndexBuffer* CreateIndexBuffer(filament::Engine* engine, const mjModel* model, int id, MeshType mesh_type); -// Generates a filament Texture for a given 2D texture in the mjModel. -filament::Texture* CreateTexture(filament::Engine* engine, const mjModel* model, - int id, TextureType texture_type); - // Reads a value with the given name from the mjModel's data sections. The // default_value is returned if the named element is not found. template diff --git a/src/experimental/filament/filament/render_target_util.cc b/src/experimental/filament/filament/render_target_util.cc index 52cbc25c..306bb258 100644 --- a/src/experimental/filament/filament/render_target_util.cc +++ b/src/experimental/filament/filament/render_target_util.cc @@ -21,39 +21,7 @@ namespace mujoco { -static filament::Texture* CreateRenderTargetTexture( - filament::Engine* engine, int width, int height, - RenderTargetTextureType type) { - filament::Texture::Builder builder; - builder.width(width); - builder.height(height); - switch (type) { - case kRenderTargetColor: - builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT | - filament::Texture::Usage::BLIT_SRC); - builder.format(filament::Texture::InternalFormat::RGB8); - break; - case kRenderTargetDepth: - builder.usage(filament::Texture::Usage::DEPTH_ATTACHMENT | - filament::Texture::Usage::SAMPLEABLE); - builder.format(filament::Texture::InternalFormat::DEPTH32F); - break; - case kRenderTargetDepthColor: - builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT | - filament::Texture::Usage::BLIT_SRC); - builder.format(filament::Texture::InternalFormat::R32F); - break; - case kRenderTargetReflectionColor: - builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT | - filament::Texture::Usage::BLIT_SRC | - filament::Texture::Usage::SAMPLEABLE); - builder.format(filament::Texture::InternalFormat::RGBA8); - break; - default: - mju_error("Unknown type: %d", static_cast(type)); - } - return builder.build(*engine); -} + RenderTargetAndTextures::RenderTargetAndTextures(filament::Engine* engine, RenderTargetTextureType color, diff --git a/src/experimental/filament/filament/render_target_util.h b/src/experimental/filament/filament/render_target_util.h index e8dfb939..c7651741 100644 --- a/src/experimental/filament/filament/render_target_util.h +++ b/src/experimental/filament/filament/render_target_util.h @@ -17,19 +17,10 @@ #include #include +#include "experimental/filament/filament/texture_util.h" namespace mujoco { -// The different types of textures we can create for a render target. -enum RenderTargetTextureType { - kRenderTargetNone, - kRenderTargetColor, - kRenderTargetDepth, - kRenderTargetDepthColor, - kRenderTargetReflectionColor, - kNumRenderTargetTextureTypes, -}; - // Manages a filament RenderTarget and the textures which are bound to it. class RenderTargetAndTextures { public: @@ -63,8 +54,8 @@ class RenderTargetAndTextures { filament::Texture* color_texture_ = nullptr; filament::Texture* depth_texture_ = nullptr; filament::RenderTarget* render_target_ = nullptr; - RenderTargetTextureType color_type_ = kRenderTargetNone; - RenderTargetTextureType depth_type_ = kRenderTargetNone; + RenderTargetTextureType color_type_; + RenderTargetTextureType depth_type_; int width_ = 0; int height_ = 0; }; diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index aba4e0b8..9b64243e 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -50,6 +50,7 @@ #include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/render_target_util.h" +#include "experimental/filament/filament/texture_util.h" namespace mujoco { @@ -516,7 +517,8 @@ void SceneView::AddReflectiveDrawable(Drawable* drawable) { filament::Engine* engine = object_mgr_->GetEngine(); while (reflect_targets_.size() < reflectives_.size()) { reflect_targets_.push_back(std::make_unique( - engine, kRenderTargetReflectionColor, kRenderTargetDepth)); + engine, RenderTargetTextureType::kReflectionColor, + RenderTargetTextureType::kDepth)); } // Prepare a render target for the reflective drawable. diff --git a/src/experimental/filament/filament/texture_util.cc b/src/experimental/filament/filament/texture_util.cc index 1cf5567c..c003f403 100644 --- a/src/experimental/filament/filament/texture_util.cc +++ b/src/experimental/filament/filament/texture_util.cc @@ -26,7 +26,6 @@ #include #include - namespace mujoco { static filament::Texture::Format GetTextureFormat(int num_channels) { @@ -168,6 +167,40 @@ filament::Texture* CreateCubeTexture(filament::Engine* engine, int width, return texture; } +filament::Texture* CreateRenderTargetTexture( + filament::Engine* engine, int width, int height, + RenderTargetTextureType type) { + filament::Texture::Builder builder; + builder.width(width); + builder.height(height); + switch (type) { + case RenderTargetTextureType::kColor: + builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT | + filament::Texture::Usage::BLIT_SRC); + builder.format(filament::Texture::InternalFormat::RGB8); + break; + case RenderTargetTextureType::kDepth: + builder.usage(filament::Texture::Usage::DEPTH_ATTACHMENT | + filament::Texture::Usage::SAMPLEABLE); + builder.format(filament::Texture::InternalFormat::DEPTH32F); + break; + case RenderTargetTextureType::kDepthColor: + builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT | + filament::Texture::Usage::BLIT_SRC); + builder.format(filament::Texture::InternalFormat::R32F); + break; + case RenderTargetTextureType::kReflectionColor: + builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT | + filament::Texture::Usage::BLIT_SRC | + filament::Texture::Usage::SAMPLEABLE); + builder.format(filament::Texture::InternalFormat::RGBA8); + break; + default: + mju_error("Unknown type: %d", static_cast(type)); + } + return builder.build(*engine); +} + filament::Texture* CreateKtxTexture( filament::Engine* engine, const uint8_t* data, int size, filament::math::float3* spherical_harmonics_out) { @@ -178,4 +211,23 @@ filament::Texture* CreateKtxTexture( const bool is_srgb = false; return ktxreader::Ktx1Reader::createTexture(engine, bundle, is_srgb); } + +filament::Texture* CreateTexture(filament::Engine* engine, const mjModel* model, + int id, TextureType texture_type) { + if (id < 0 || id >= model->ntex) { + mju_error("Invalid texture index %d", id); + } + + const int width = model->tex_width[id]; + const int height = model->tex_height[id]; + const bool is_srgb = model->tex_colorspace[id] == mjCOLORSPACE_SRGB; + const int num_channels = model->tex_nchannel[id]; + const mjtByte* data = model->tex_data + model->tex_adr[id]; + filament::Texture* texture = + texture_type == TextureType::kNormal2d + ? Create2dTexture(engine, width, height, num_channels, data, is_srgb) + : CreateCubeTexture(engine, width, height, num_channels, data, + is_srgb); + return texture; +} } // namespace mujoco diff --git a/src/experimental/filament/filament/texture_util.h b/src/experimental/filament/filament/texture_util.h index 3136b81c..dd267af8 100644 --- a/src/experimental/filament/filament/texture_util.h +++ b/src/experimental/filament/filament/texture_util.h @@ -20,10 +20,26 @@ #include #include #include +#include // Functions for creating filament textures. namespace mujoco { +// The types of textures we can create. +enum class TextureType { + kNormal2d, + kCube, + kKtx, +}; + +// The different types of textures we can create for a render target. +enum class RenderTargetTextureType { + kColor, + kDepth, + kDepthColor, + kReflectionColor, +}; + // Creates a filament Texture for the given 2D texture. filament::Texture* Create2dTexture(filament::Engine* engine, int width, int height, int num_channels, @@ -39,6 +55,15 @@ filament::Texture* CreateKtxTexture( filament::Engine* engine, const uint8_t* data, int size, filament::math::float3* spherical_harmonics_out); +// Creates a filament Texture for the given texture in the mjModel. +filament::Texture* CreateTexture(filament::Engine* engine, const mjModel* model, + int id, TextureType texture_type); + +// Creates a filament Texture for the given render target. +filament::Texture* CreateRenderTargetTexture(filament::Engine* engine, + int width, int height, + RenderTargetTextureType type); + } // namespace mujoco #endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_TEXTURE_UTIL_H_ From 95666da0397c9f9459ed2c4d392e91326205df71 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 1 Apr 2026 03:51:23 -0700 Subject: [PATCH 004/251] Introduce a Texture class that wraps filament::Texture. PiperOrigin-RevId: 892839636 Change-Id: I3c08fba0a514cfb758ec2842d8510c6d88300874 --- .../filament/filament/drawable.cc | 8 +- src/experimental/filament/filament/drawable.h | 3 +- .../filament/filament/material.cc | 85 ++----- src/experimental/filament/filament/material.h | 20 +- .../filament/filament/model_objects.cc | 64 ++--- .../filament/filament/model_objects.h | 11 +- .../filament/filament/object_manager.cc | 62 ++--- .../filament/filament/object_manager.h | 16 +- .../filament/filament/render_target_util.cc | 36 +-- .../filament/filament/render_target_util.h | 12 +- .../filament/filament/texture_util.cc | 239 +++++++++--------- .../filament/filament/texture_util.h | 51 ++-- 12 files changed, 292 insertions(+), 315 deletions(-) diff --git a/src/experimental/filament/filament/drawable.cc b/src/experimental/filament/filament/drawable.cc index 30f1b9f0..55250600 100644 --- a/src/experimental/filament/filament/drawable.cc +++ b/src/experimental/filament/filament/drawable.cc @@ -37,6 +37,7 @@ #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/model_objects.h" #include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/filament/texture_util.h" namespace mujoco { @@ -226,7 +227,7 @@ void Drawable::SetDrawMode(Material::DrawMode mode) { renderables_.SetMaterialInstance(material_.GetMaterialInstance(mode)); } -void Drawable::UpdateReflectionTexture(const filament::Texture* tex) { +void Drawable::UpdateReflectionTexture(const Texture* tex) { material_.UpdateReflectionTexture(tex); } @@ -422,7 +423,7 @@ void Drawable::UpdateMaterial(const mjvGeom& geom, bool use_segid_color, } else { material_.SetNormalMaterialType(ObjectManager::kPhongColor); } - } else if (textures.color->getTarget() == + } else if (textures.color->GetFilamentTexture()->getTarget() == filament::Texture::Sampler::SAMPLER_CUBEMAP) { if (color.a < 1.0f) { material_.SetNormalMaterialType(ObjectManager::kPhongCubeFade); @@ -490,7 +491,8 @@ void Drawable::UpdateMaterial(const mjvGeom& geom, bool use_segid_color, // the programmatic UVs. if (textures.color) { - if (textures.color->getTarget() == filament::Texture::Sampler::SAMPLER_2D) { + if (textures.color->GetFilamentTexture()->getTarget() == + filament::Texture::Sampler::SAMPLER_2D) { // For 2D textures, `tex_repeat` specifies how many times the texture // image is repeated. The `tex_uniform` flag determines if the repetition // is applied at in object space (false) or in world space (true). diff --git a/src/experimental/filament/filament/drawable.h b/src/experimental/filament/filament/drawable.h index 19002de6..06796aed 100644 --- a/src/experimental/filament/filament/drawable.h +++ b/src/experimental/filament/filament/drawable.h @@ -26,6 +26,7 @@ #include "experimental/filament/filament/model_objects.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/renderables.h" +#include "experimental/filament/filament/texture_util.h" namespace mujoco { @@ -68,7 +69,7 @@ class Drawable { // Sets the reflection texture for the drawable. We have a separate setter // because we need to render the reflection texture before it can be applied // to the material. - void UpdateReflectionTexture(const filament::Texture* tex); + void UpdateReflectionTexture(const Texture* tex); private: void AddMesh(int data_id); diff --git a/src/experimental/filament/filament/material.cc b/src/experimental/filament/filament/material.cc index 08c167a2..e04e3f81 100644 --- a/src/experimental/filament/filament/material.cc +++ b/src/experimental/filament/filament/material.cc @@ -21,6 +21,7 @@ #include #include #include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/filament/texture_util.h" namespace mujoco { @@ -71,7 +72,7 @@ void Material::UpdateTextures(const Textures& textures) { UpdateMaterialInstances(); } -void Material::UpdateReflectionTexture(const filament::Texture* tex) { +void Material::UpdateReflectionTexture(const Texture* tex) { textures_.reflection = tex; UpdateMaterialInstances(); } @@ -128,70 +129,26 @@ void Material::UpdateMaterialInstances() { sampler.setMinFilter( filament::TextureSampler::MinFilter::LINEAR_MIPMAP_LINEAR); - if (material->hasParameter("BaseColor")) { - if (textures_.color) { - instance->setParameter("BaseColor", textures_.color, sampler); - } else { - auto* fallback = object_mgr_->GetFallbackTexture(mjTEXROLE_RGB); - instance->setParameter("BaseColor", fallback, sampler); + auto TrySetTexture = [&](const char* name, const Texture* texture, + mjtTextureRole role) { + if (material->hasParameter(name)) { + if (texture) { + instance->setParameter(name, texture->GetFilamentTexture(), sampler); + } else { + auto* fallback = object_mgr_->GetFallbackTexture(role); + instance->setParameter(name, fallback->GetFilamentTexture(), sampler); + } } - } - if (material->hasParameter("Normal")) { - if (textures_.normal) { - instance->setParameter("Normal", textures_.normal, sampler); - } else { - auto* fallback = object_mgr_->GetFallbackTexture(mjTEXROLE_NORMAL); - instance->setParameter("Normal", fallback, sampler); - } - } - if (material->hasParameter("Metallic")) { - if (textures_.metallic) { - instance->setParameter("Metallic", textures_.metallic, sampler); - } else { - auto* fallback = object_mgr_->GetFallbackTexture(mjTEXROLE_METALLIC); - instance->setParameter("Metallic", fallback, sampler); - } - } - if (material->hasParameter("Roughness")) { - if (textures_.roughness) { - instance->setParameter("Roughness", textures_.roughness, sampler); - } else { - auto* fallback = object_mgr_->GetFallbackTexture(mjTEXROLE_ROUGHNESS); - instance->setParameter("Roughness", fallback, sampler); - } - } - if (material->hasParameter("Occlusion")) { - if (textures_.occlusion) { - instance->setParameter("Occlusion", textures_.occlusion, sampler); - } else { - auto* fallback = object_mgr_->GetFallbackTexture(mjTEXROLE_OCCLUSION); - instance->setParameter("Occlusion", fallback, sampler); - } - } - if (material->hasParameter("ORM")) { - if (textures_.orm) { - instance->setParameter("ORM", textures_.orm, sampler); - } else { - auto* fallback = object_mgr_->GetFallbackTexture(mjTEXROLE_ORM); - instance->setParameter("ORM", fallback, sampler); - } - } - if (material->hasParameter("Emissive")) { - if (textures_.emissive) { - instance->setParameter("Emissive", textures_.emissive, sampler); - } else { - auto* fallback = object_mgr_->GetFallbackTexture(mjTEXROLE_EMISSIVE); - instance->setParameter("Emissive", fallback, sampler); - } - } - if (material->hasParameter("Reflection")) { - if (textures_.reflection) { - instance->setParameter("Reflection", textures_.reflection, sampler); - } else { - auto* fallback = object_mgr_->GetFallbackTexture(mjTEXROLE_USER); - instance->setParameter("Reflection", fallback, sampler); - } - } + }; + + TrySetTexture("BaseColor", textures_.color, mjTEXROLE_RGB); + TrySetTexture("Normal", textures_.normal, mjTEXROLE_NORMAL); + TrySetTexture("Metallic", textures_.metallic, mjTEXROLE_METALLIC); + TrySetTexture("Roughness", textures_.roughness, mjTEXROLE_ROUGHNESS); + TrySetTexture("Occlusion", textures_.occlusion, mjTEXROLE_OCCLUSION); + TrySetTexture("ORM", textures_.orm, mjTEXROLE_ORM); + TrySetTexture("Emissive", textures_.emissive, mjTEXROLE_EMISSIVE); + TrySetTexture("Reflection", textures_.reflection, mjTEXROLE_USER); } } // namespace mujoco diff --git a/src/experimental/filament/filament/material.h b/src/experimental/filament/filament/material.h index 51ccac71..849cb89e 100644 --- a/src/experimental/filament/filament/material.h +++ b/src/experimental/filament/filament/material.h @@ -17,11 +17,11 @@ #include #include -#include #include #include #include #include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/filament/texture_util.h" namespace mujoco { @@ -39,14 +39,14 @@ class Material { // The textures that can be assigned to the drawable's material. struct Textures { - const filament::Texture* color = nullptr; - const filament::Texture* normal = nullptr; - const filament::Texture* metallic = nullptr; - const filament::Texture* roughness = nullptr; - const filament::Texture* occlusion = nullptr; - const filament::Texture* orm = nullptr; - const filament::Texture* emissive = nullptr; - const filament::Texture* reflection = nullptr; + const Texture* color = nullptr; + const Texture* normal = nullptr; + const Texture* metallic = nullptr; + const Texture* roughness = nullptr; + const Texture* occlusion = nullptr; + const Texture* orm = nullptr; + const Texture* emissive = nullptr; + const Texture* reflection = nullptr; }; // The parameters that can be applied to the drawable's material. @@ -82,7 +82,7 @@ class Material { // Update the reflection texture. We do this separately since the reflection // texture needs to be rendered before it can be applied to the material. - void UpdateReflectionTexture(const filament::Texture* tex); + void UpdateReflectionTexture(const Texture* tex); // Returns the material instance assigned to the draw mode. filament::MaterialInstance* GetMaterialInstance(DrawMode mode) { diff --git a/src/experimental/filament/filament/model_objects.cc b/src/experimental/filament/filament/model_objects.cc index 217648ae..0529d90e 100644 --- a/src/experimental/filament/filament/model_objects.cc +++ b/src/experimental/filament/filament/model_objects.cc @@ -15,6 +15,7 @@ #include "experimental/filament/filament/model_objects.h" #include +#include #include #include @@ -80,9 +81,7 @@ ModelObjects::~ModelObjects() { engine_->destroy(iter.vertex_buffer); engine_->destroy(iter.index_buffer); } - for (auto& iter : textures_) { - engine_->destroy(iter.second); - } + textures_.clear(); } void ModelObjects::UploadMesh(const mjModel* model, int id) { @@ -126,25 +125,30 @@ void ModelObjects::UploadTexture(const mjModel* model, int id) { mju_error("Invalid texture index: %d", id); } - if (auto iter = textures_.find(id); iter != textures_.end()) { - engine_->destroy(iter->second); - } + const int width = model->tex_width[id]; + const int height = model->tex_height[id]; + const int num_channels = model->tex_nchannel[id]; + const int tex_type = model->tex_type[id]; + const mjtByte* data = model->tex_data + model->tex_adr[id]; + const mjtColorSpace color_space = (mjtColorSpace)model->tex_colorspace[id]; - const int texture_type = model->tex_type[id]; - if (model->tex_height[id] == 1) { - const mjtByte* bytes = model->tex_data + model->tex_adr[id]; - const int num_bytes = model->tex_width[id]; - textures_[id] = - CreateKtxTexture(engine_, bytes, num_bytes, spherical_harmonics_[id]); - } else if (texture_type == mjTEXTURE_2D) { - textures_[id] = CreateTexture(engine_, model, id, TextureType::kNormal2d); - } else if (texture_type == mjTEXTURE_CUBE) { - textures_[id] = CreateTexture(engine_, model, id, TextureType::kCube); - } else if (texture_type == mjTEXTURE_SKYBOX) { - textures_[id] = CreateTexture(engine_, model, id, TextureType::kCube); - } else { - mju_error("Unsupported: Texture type: %d", texture_type); - } + const TextureType type = [&] { + if (height == 1) { + return TextureType::kKtx; + } else if (tex_type == mjTEXTURE_2D) { + return TextureType::kNormal2d; + } else if (tex_type == mjTEXTURE_CUBE) { + return TextureType::kCube; + } else if (tex_type == mjTEXTURE_SKYBOX) { + return TextureType::kCube; + } else { + mju_error("Unsupported texture type: %d", tex_type); + return TextureType::kNormal2d; + } + }(); + + textures_[id] = std::make_unique(engine_, type, color_space, width, + height, num_channels, data); } void ModelObjects::UploadHeightField(const mjModel* model, int id) { @@ -194,12 +198,12 @@ const FilamentBuffers* ModelObjects::GetShapeBuffer(ShapeType shape) const { return &shapes_[shape]; } -const filament::Texture* ModelObjects::GetTexture(int tex_id) const { +const Texture* ModelObjects::GetTexture(int tex_id) const { auto it = textures_.find(tex_id); - return it != textures_.end() ? it->second : nullptr; + return it != textures_.end() ? it->second.get() : nullptr; } -const filament::Texture* ModelObjects::GetTexture(int mat_id, int role) const { +const Texture* ModelObjects::GetTexture(int mat_id, int role) const { if (mat_id < 0 || mat_id >= model_->nmat || role < 0 || role >= mjNTEXROLE) { return nullptr; } @@ -210,15 +214,11 @@ const filament::Texture* ModelObjects::GetTexture(int mat_id, int role) const { filament::IndirectLight* ModelObjects::CreateIndirectLight(int tex_id, float intensity) { filament::Texture* texture = nullptr; + const Texture::SphericalHarmonics* spherical_harmonics = nullptr; auto texture_iter = textures_.find(tex_id); if (texture_iter != textures_.end()) { - texture = texture_iter->second; - } - - SphericalHarmonics* spherical_harmonics = nullptr; - auto sh_iter = spherical_harmonics_.find(tex_id); - if (sh_iter != spherical_harmonics_.end()) { - spherical_harmonics = &sh_iter->second; + texture = texture_iter->second->GetFilamentTexture(); + spherical_harmonics = texture_iter->second->GetSphericalHarmonics(); } filament::IndirectLight::Builder builder; @@ -240,7 +240,7 @@ filament::Skybox* ModelObjects::CreateSkybox() { for (auto& iter : textures_) { const int texture_type = model_->tex_type[iter.first]; if (texture_type == mjTEXTURE_SKYBOX) { - skybox_texture = iter.second; + skybox_texture = iter.second->GetFilamentTexture(); break; } } diff --git a/src/experimental/filament/filament/model_objects.h b/src/experimental/filament/filament/model_objects.h index 5de28f8a..94fc2987 100644 --- a/src/experimental/filament/filament/model_objects.h +++ b/src/experimental/filament/filament/model_objects.h @@ -16,6 +16,7 @@ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_OBJECTS_H_ #include +#include #include #include @@ -25,6 +26,7 @@ #include #include #include "experimental/filament/filament/buffer_util.h" +#include "experimental/filament/filament/texture_util.h" namespace mujoco { @@ -62,8 +64,8 @@ class ModelObjects { const FilamentBuffers* GetShapeBuffer(ShapeType shape) const; const FilamentBuffers* GetMeshBuffer(int data_id) const; const FilamentBuffers* GetHeightFieldBuffer(int hfield_id) const; - const filament::Texture* GetTexture(int tex_id) const; - const filament::Texture* GetTexture(int mat_id, int role) const; + const Texture* GetTexture(int tex_id) const; + const Texture* GetTexture(int mat_id, int role) const; filament::Skybox* CreateSkybox(); filament::IndirectLight* CreateIndirectLight(int tex_id, float intensity); @@ -78,8 +80,6 @@ class ModelObjects { ModelObjects& operator=(const ModelObjects&) = delete; private: - using SphericalHarmonics = filament::math::float3[9]; - const mjModel* model_ = nullptr; filament::Engine* engine_ = nullptr; std::vector skyboxes_; @@ -88,8 +88,7 @@ class ModelObjects { std::unordered_map meshes_; std::unordered_map convex_hulls_; std::unordered_map height_fields_; - std::unordered_map textures_; - std::unordered_map spherical_harmonics_; + std::unordered_map> textures_; float specular_multiplier_ = 0.2f; float shininess_multiplier_ = 0.1f; float emissive_multiplier_ = 0.3f; diff --git a/src/experimental/filament/filament/object_manager.cc b/src/experimental/filament/filament/object_manager.cc index 4bf5a0cd..7699e655 100644 --- a/src/experimental/filament/filament/object_manager.cc +++ b/src/experimental/filament/filament/object_manager.cc @@ -15,6 +15,7 @@ #include "experimental/filament/filament/object_manager.h" #include +#include #include #include @@ -86,22 +87,27 @@ ObjectManager::ObjectManager(filament::Engine* engine) materials_[kUnlitUi] = LoadMaterial("unlit_ui.filamat"); static uint8_t black_rgb[3] = {0, 0, 0}; - fallback_black_ = Create2dTexture(engine_, 1, 1, 3, black_rgb, false); + fallback_black_ = std::make_unique( + engine_, TextureType::kNormal2d, mjCOLORSPACE_LINEAR, 1, 1, 3, black_rgb); static uint8_t white_rgb[3] = {255, 255, 255}; - fallback_white_ = Create2dTexture(engine_, 1, 1, 3, white_rgb, false); + fallback_white_ = std::make_unique( + engine_, TextureType::kNormal2d, mjCOLORSPACE_LINEAR, 1, 1, 3, white_rgb); static uint8_t normal_data[3] = {128, 128, 255}; - fallback_normal_ = Create2dTexture(engine_, 1, 1, 3, normal_data, false); + fallback_normal_ = + std::make_unique(engine_, TextureType::kNormal2d, + mjCOLORSPACE_LINEAR, 1, 1, 3, normal_data); static uint8_t orm_data[3] = {0, 255, 0}; - fallback_orm_ = Create2dTexture(engine_, 1, 1, 3, orm_data, false); + fallback_orm_ = std::make_unique( + engine_, TextureType::kNormal2d, mjCOLORSPACE_LINEAR, 1, 1, 3, orm_data); - fallback_textures_[mjTEXROLE_USER] = fallback_black_; - fallback_textures_[mjTEXROLE_RGB] = fallback_white_; - fallback_textures_[mjTEXROLE_OCCLUSION] = fallback_white_; - fallback_textures_[mjTEXROLE_ROUGHNESS] = fallback_white_; - fallback_textures_[mjTEXROLE_METALLIC] = fallback_black_; - fallback_textures_[mjTEXROLE_NORMAL] = fallback_normal_; - fallback_textures_[mjTEXROLE_EMISSIVE] = fallback_black_; - fallback_textures_[mjTEXROLE_ORM] = fallback_orm_; + fallback_textures_[mjTEXROLE_USER] = fallback_black_.get(); + fallback_textures_[mjTEXROLE_RGB] = fallback_white_.get(); + fallback_textures_[mjTEXROLE_OCCLUSION] = fallback_white_.get(); + fallback_textures_[mjTEXROLE_ROUGHNESS] = fallback_white_.get(); + fallback_textures_[mjTEXROLE_METALLIC] = fallback_black_.get(); + fallback_textures_[mjTEXROLE_NORMAL] = fallback_normal_.get(); + fallback_textures_[mjTEXROLE_EMISSIVE] = fallback_black_.get(); + fallback_textures_[mjTEXROLE_ORM] = fallback_orm_.get(); LoadFallbackIndirectLight("ibl.ktx", 1.0f); } @@ -110,17 +116,10 @@ ObjectManager::~ObjectManager() { if (fallback_indirect_light_) { engine_->destroy(fallback_indirect_light_); } - if (fallback_indirect_light_texture_) { - engine_->destroy(fallback_indirect_light_texture_); - } + fallback_indirect_light_texture_.reset(); for (auto& iter : materials_) { engine_->destroy(iter); } - // fallback_textures_ maps to these textures. - engine_->destroy(fallback_white_); - engine_->destroy(fallback_black_); - engine_->destroy(fallback_normal_); - engine_->destroy(fallback_orm_); } filament::Material* ObjectManager::GetMaterial(MaterialType type) const { @@ -130,7 +129,7 @@ filament::Material* ObjectManager::GetMaterial(MaterialType type) const { return materials_[type]; } -const filament::Texture* ObjectManager::GetFallbackTexture( +const Texture* ObjectManager::GetFallbackTexture( mjtTextureRole role) const { if (role < 0 || role >= mjNTEXROLE) { mju_error("Invalid texture role: %d", role); @@ -144,10 +143,7 @@ filament::IndirectLight* ObjectManager::GetFallbackIndirectLight() { void ObjectManager::LoadFallbackIndirectLight( std::string_view filename, float intensity) { - if (fallback_indirect_light_texture_ != nullptr) { - engine_->destroy(fallback_indirect_light_texture_); - fallback_indirect_light_texture_ = nullptr; - } + fallback_indirect_light_texture_.reset(); if (fallback_indirect_light_ != nullptr) { engine_->destroy(fallback_indirect_light_); fallback_indirect_light_ = nullptr; @@ -158,18 +154,22 @@ void ObjectManager::LoadFallbackIndirectLight( return; } - filament::math::float3 spherical_harmonics[9]; - fallback_indirect_light_texture_ = - CreateKtxTexture(engine_, reinterpret_cast(asset.payload), - asset.size, spherical_harmonics); + fallback_indirect_light_texture_ = std::make_unique( + engine_, TextureType::kKtx, mjCOLORSPACE_AUTO, asset.size, 1, 1, + reinterpret_cast(asset.payload)); if (fallback_indirect_light_texture_ == nullptr) { return; } + const Texture::SphericalHarmonics* spherical_harmonics = + fallback_indirect_light_texture_->GetSphericalHarmonics(); + // Build the indirect light. filament::IndirectLight::Builder builder; - builder.reflections(fallback_indirect_light_texture_); - builder.irradiance(3, spherical_harmonics); + builder.reflections(fallback_indirect_light_texture_->GetFilamentTexture()); + if (spherical_harmonics) { + builder.irradiance(3, *spherical_harmonics); + } builder.intensity(intensity); // Rotate the light to match mujoco's Z-up convention. builder.rotation(filament::math::mat3f::rotation( diff --git a/src/experimental/filament/filament/object_manager.h b/src/experimental/filament/filament/object_manager.h index 37459b4d..64d9eadf 100644 --- a/src/experimental/filament/filament/object_manager.h +++ b/src/experimental/filament/filament/object_manager.h @@ -16,12 +16,14 @@ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_OBJECT_MANAGER_H_ #include +#include #include #include #include #include #include +#include "experimental/filament/filament/texture_util.h" namespace mujoco { @@ -60,7 +62,7 @@ class ObjectManager { filament::Material* GetMaterial(MaterialType type) const; // Returns the fallback Texture with the given role. - const filament::Texture* GetFallbackTexture(mjtTextureRole role) const; + const Texture* GetFallbackTexture(mjtTextureRole role) const; // Returns the fallback IndirectLight. filament::IndirectLight* GetFallbackIndirectLight(); @@ -74,12 +76,12 @@ class ObjectManager { private: filament::Engine* engine_ = nullptr; std::array materials_; - std::array fallback_textures_; - filament::Texture* fallback_white_ = nullptr; - filament::Texture* fallback_black_ = nullptr; - filament::Texture* fallback_normal_ = nullptr; - filament::Texture* fallback_orm_ = nullptr; - filament::Texture* fallback_indirect_light_texture_ = nullptr; + std::array fallback_textures_; + std::unique_ptr fallback_white_ = nullptr; + std::unique_ptr fallback_black_ = nullptr; + std::unique_ptr fallback_normal_ = nullptr; + std::unique_ptr fallback_orm_ = nullptr; + std::unique_ptr fallback_indirect_light_texture_ = nullptr; filament::IndirectLight* fallback_indirect_light_ = nullptr; }; diff --git a/src/experimental/filament/filament/render_target_util.cc b/src/experimental/filament/filament/render_target_util.cc index 306bb258..ef221e4e 100644 --- a/src/experimental/filament/filament/render_target_util.cc +++ b/src/experimental/filament/filament/render_target_util.cc @@ -14,15 +14,15 @@ #include "experimental/filament/filament/render_target_util.h" +#include + #include #include #include -#include +#include "experimental/filament/filament/texture_util.h" namespace mujoco { - - RenderTargetAndTextures::RenderTargetAndTextures(filament::Engine* engine, RenderTargetTextureType color, RenderTargetTextureType depth) @@ -41,15 +41,15 @@ void RenderTargetAndTextures::Prepare(int width, int height) { height_ = height; color_texture_ = - CreateRenderTargetTexture(engine_, width, height, color_type_); + std::make_unique(engine_, color_type_, width, height); depth_texture_ = - CreateRenderTargetTexture(engine_, width, height, depth_type_); + std::make_unique(engine_, depth_type_, width, height); filament::RenderTarget::Builder builder; builder.texture(filament::RenderTarget::AttachmentPoint::COLOR, - color_texture_); + color_texture_->GetFilamentTexture()); builder.texture(filament::RenderTarget::AttachmentPoint::DEPTH, - depth_texture_); + depth_texture_->GetFilamentTexture()); render_target_ = builder.build(*engine_); } @@ -58,14 +58,20 @@ void RenderTargetAndTextures::Destroy() { engine_->destroy(render_target_); render_target_ = nullptr; } - if (color_texture_) { - engine_->destroy(color_texture_); - color_texture_ = nullptr; - } - if (depth_texture_) { - engine_->destroy(depth_texture_); - depth_texture_ = nullptr; - } + color_texture_.reset(); + depth_texture_.reset(); +} + +Texture* RenderTargetAndTextures::GetColorTexture() const { + return color_texture_.get(); +} + +Texture* RenderTargetAndTextures::GetDepthTexture() const { + return depth_texture_.get(); +} + +filament::RenderTarget* RenderTargetAndTextures::GetRenderTarget() const { + return render_target_; } } // namespace mujoco diff --git a/src/experimental/filament/filament/render_target_util.h b/src/experimental/filament/filament/render_target_util.h index c7651741..d2d581a9 100644 --- a/src/experimental/filament/filament/render_target_util.h +++ b/src/experimental/filament/filament/render_target_util.h @@ -15,6 +15,8 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDER_TARGET_UTIL_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDER_TARGET_UTIL_H_ +#include + #include #include #include "experimental/filament/filament/texture_util.h" @@ -39,21 +41,21 @@ class RenderTargetAndTextures { void Prepare(int width, int height); // Returns the color texture. - filament::Texture* GetColorTexture() const { return color_texture_; } + Texture* GetColorTexture() const; // Returns the depth texture. - filament::Texture* GetDepthTexture() const { return depth_texture_; } + Texture* GetDepthTexture() const; // Returns the render target. - filament::RenderTarget* GetRenderTarget() const { return render_target_; } + filament::RenderTarget* GetRenderTarget() const; private: void Destroy(); filament::Engine* engine_ = nullptr; - filament::Texture* color_texture_ = nullptr; - filament::Texture* depth_texture_ = nullptr; filament::RenderTarget* render_target_ = nullptr; + std::unique_ptr color_texture_ = nullptr; + std::unique_ptr depth_texture_ = nullptr; RenderTargetTextureType color_type_; RenderTargetTextureType depth_type_; int width_ = 0; diff --git a/src/experimental/filament/filament/texture_util.cc b/src/experimental/filament/filament/texture_util.cc index c003f403..20b63393 100644 --- a/src/experimental/filament/filament/texture_util.cc +++ b/src/experimental/filament/filament/texture_util.cc @@ -23,7 +23,6 @@ #include #include #include -#include #include namespace mujoco { @@ -69,107 +68,24 @@ static filament::Texture::InternalFormat GetTextureInternalFormat( } } -filament::Texture* Create2dTexture(filament::Engine* engine, int width, - int height, int num_channels, - const uint8_t* data, bool is_srgb) { - if (num_channels != 1 && num_channels != 3 && num_channels != 4) { - mju_error("Unsupported number of channels: %d", num_channels); - return nullptr; - } - - filament::Texture::Builder builder; - builder.width(width); - builder.height(height); - builder.format(GetTextureInternalFormat(num_channels, is_srgb)); - builder.sampler(filament::Texture::Sampler::SAMPLER_2D); - if (!is_srgb) { - builder.usage(filament::Texture::Usage::GEN_MIPMAPPABLE | - filament::Texture::Usage::SAMPLEABLE | - filament::Texture::Usage::UPLOADABLE); - } - filament::Texture* texture = builder.build(*engine); - - if (data) { - const size_t num_bytes = width * height * sizeof(uint8_t) * num_channels; - const filament::Texture::Format format = GetTextureFormat(num_channels); - texture->setImage( - *engine, 0, - filament::Texture::PixelBufferDescriptor( - data, num_bytes, format, filament::Texture::Type::UBYTE)); - if (!is_srgb) { - texture->generateMipmaps(*engine); - } - } - return texture; -} - -filament::Texture* CreateCubeTexture(filament::Engine* engine, int width, - int height, int num_channels, - const uint8_t* data, bool is_srgb) { - if (num_channels != 3) { - mju_error("Only support RGB cubemaps."); - return nullptr; - } - - const int kNumFacesPerCube = 6; - - int face_height = height; - if (width != height) { - if (width * kNumFacesPerCube != height) { - mju_error("Cube maps must contain 6 square images."); - } - face_height = height / kNumFacesPerCube; - } - if (width != face_height) { - mju_error("Cube map faces must be square."); - } - - filament::Texture::Builder builder; - builder.width(width); - builder.height(face_height); - builder.format(GetTextureInternalFormat(num_channels, is_srgb)); - builder.sampler(filament::Texture::Sampler::SAMPLER_CUBEMAP); - if (!is_srgb) { - builder.usage(filament::Texture::Usage::GEN_MIPMAPPABLE | - filament::Texture::Usage::SAMPLEABLE | - filament::Texture::Usage::UPLOADABLE); - } - filament::Texture* texture = builder.build(*engine); - - const int face_size = width * face_height * num_channels; - const int num_bytes = face_size * kNumFacesPerCube; - - uint8_t* buffer = new uint8_t[num_bytes]; - auto callback = +[](void* buffer, size_t size, void* user) { - delete [] reinterpret_cast(buffer); - }; - - filament::Texture::FaceOffsets offsets(face_size); - if (width == height) { - // Copy the image to all the faces. - for (int i = 0; i < kNumFacesPerCube; ++i) { - std::memcpy(buffer + (i * face_size), data, face_size); - } +Texture::Texture(filament::Engine* engine, TextureType texture_type, + mjtColorSpace color_space, int width, int height, + int num_channels, const uint8_t* data) + : engine_(engine) { + const bool is_srgb = color_space == mjCOLORSPACE_SRGB; + if (texture_type == TextureType::kCube) { + CreateCubeTexture(width, height, num_channels, data, is_srgb); + } else if (texture_type == TextureType::kNormal2d) { + Create2dTexture(width, height, num_channels, data, is_srgb); + } else if (texture_type == TextureType::kKtx) { + CreateKtxTexture(data, width * height * num_channels); } else { - // Use the cubemap as is. - std::memcpy(buffer, data, num_bytes); + mju_error("Unsupported texture type: %d", static_cast(texture_type)); } - - if (data) { - filament::Texture::PixelBufferDescriptor desc( - buffer, num_bytes, filament::Texture::Format::RGB, - filament::Texture::Type::UBYTE, callback); - texture->setImage(*engine, 0, std::move(desc), offsets); - if (!is_srgb) { - texture->generateMipmaps(*engine); - } - } - return texture; } -filament::Texture* CreateRenderTargetTexture( - filament::Engine* engine, int width, int height, - RenderTargetTextureType type) { +Texture::Texture(filament::Engine* engine, RenderTargetTextureType type, + int width, int height) : engine_(engine) { filament::Texture::Builder builder; builder.width(width); builder.height(height); @@ -198,36 +114,115 @@ filament::Texture* CreateRenderTargetTexture( default: mju_error("Unknown type: %d", static_cast(type)); } - return builder.build(*engine); + texture_ = builder.build(*engine); } -filament::Texture* CreateKtxTexture( - filament::Engine* engine, const uint8_t* data, int size, - filament::math::float3* spherical_harmonics_out) { +void Texture::Create2dTexture(int width, int height, int num_channels, + const uint8_t* data, bool is_srgb) { + if (num_channels != 1 && num_channels != 3 && num_channels != 4) { + mju_error("Unsupported number of channels: %d", num_channels); + return; + } + + filament::Texture::Builder builder; + builder.width(width); + builder.height(height); + builder.format(GetTextureInternalFormat(num_channels, is_srgb)); + builder.sampler(filament::Texture::Sampler::SAMPLER_2D); + if (!is_srgb) { + builder.usage(filament::Texture::Usage::GEN_MIPMAPPABLE | + filament::Texture::Usage::SAMPLEABLE | + filament::Texture::Usage::UPLOADABLE); + } + texture_ = builder.build(*engine_); + + if (data) { + const size_t num_bytes = width * height * sizeof(uint8_t) * num_channels; + const filament::Texture::Format format = GetTextureFormat(num_channels); + texture_->setImage( + *engine_, 0, + filament::Texture::PixelBufferDescriptor( + data, num_bytes, format, filament::Texture::Type::UBYTE)); + if (!is_srgb) { + texture_->generateMipmaps(*engine_); + } + } +} + +void Texture::CreateCubeTexture(int width, int height, int num_channels, + const uint8_t* data, bool is_srgb) { + if (num_channels != 3) { + mju_error("Only support RGB cubemaps."); + return; + } + + const int kNumFacesPerCube = 6; + + int face_height = height; + if (width != height) { + if (width * kNumFacesPerCube != height) { + mju_error("Cube maps must contain 6 square images."); + } + face_height = height / kNumFacesPerCube; + } + if (width != face_height) { + mju_error("Cube map faces must be square."); + } + + filament::Texture::Builder builder; + builder.width(width); + builder.height(face_height); + builder.format(GetTextureInternalFormat(num_channels, is_srgb)); + builder.sampler(filament::Texture::Sampler::SAMPLER_CUBEMAP); + if (!is_srgb) { + builder.usage(filament::Texture::Usage::GEN_MIPMAPPABLE | + filament::Texture::Usage::SAMPLEABLE | + filament::Texture::Usage::UPLOADABLE); + } + texture_ = builder.build(*engine_); + + const int face_size = width * face_height * num_channels; + const int num_bytes = face_size * kNumFacesPerCube; + + uint8_t* buffer = new uint8_t[num_bytes]; + auto callback = +[](void* buffer, size_t size, void* user) { + delete [] reinterpret_cast(buffer); + }; + + filament::Texture::FaceOffsets offsets(face_size); + if (width == height) { + // Copy the image to all the faces. + for (int i = 0; i < kNumFacesPerCube; ++i) { + std::memcpy(buffer + (i * face_size), data, face_size); + } + } else { + // Use the cubemap as is. + std::memcpy(buffer, data, num_bytes); + } + + if (data) { + filament::Texture::PixelBufferDescriptor desc( + buffer, num_bytes, filament::Texture::Format::RGB, + filament::Texture::Type::UBYTE, callback); + texture_->setImage(*engine_, 0, std::move(desc), offsets); + if (!is_srgb) { + texture_->generateMipmaps(*engine_); + } + } +} + +void Texture::CreateKtxTexture(const uint8_t* data, int size) { image::Ktx1Bundle* bundle = new image::Ktx1Bundle(data, size); - if (spherical_harmonics_out) { - bundle->getSphericalHarmonics(spherical_harmonics_out); - } + has_spherical_harmonics_ = true; + bundle->getSphericalHarmonics(spherical_harmonics_); const bool is_srgb = false; - return ktxreader::Ktx1Reader::createTexture(engine, bundle, is_srgb); + texture_ = ktxreader::Ktx1Reader::createTexture(engine_, bundle, is_srgb); } -filament::Texture* CreateTexture(filament::Engine* engine, const mjModel* model, - int id, TextureType texture_type) { - if (id < 0 || id >= model->ntex) { - mju_error("Invalid texture index %d", id); +Texture::~Texture() { + if (texture_) { + engine_->destroy(texture_); } - - const int width = model->tex_width[id]; - const int height = model->tex_height[id]; - const bool is_srgb = model->tex_colorspace[id] == mjCOLORSPACE_SRGB; - const int num_channels = model->tex_nchannel[id]; - const mjtByte* data = model->tex_data + model->tex_adr[id]; - filament::Texture* texture = - texture_type == TextureType::kNormal2d - ? Create2dTexture(engine, width, height, num_channels, data, is_srgb) - : CreateCubeTexture(engine, width, height, num_channels, data, - is_srgb); - return texture; } + } // namespace mujoco diff --git a/src/experimental/filament/filament/texture_util.h b/src/experimental/filament/filament/texture_util.h index dd267af8..42925325 100644 --- a/src/experimental/filament/filament/texture_util.h +++ b/src/experimental/filament/filament/texture_util.h @@ -40,30 +40,43 @@ enum class RenderTargetTextureType { kReflectionColor, }; -// Creates a filament Texture for the given 2D texture. -filament::Texture* Create2dTexture(filament::Engine* engine, int width, - int height, int num_channels, - const uint8_t* data, bool is_srgb); +class Texture { + public: + // Creates a texture with the given data. + Texture(filament::Engine* engine, TextureType texture_type, + mjtColorSpace color_space, int width, int height, int num_channels, + const uint8_t* data); -// Creates a filament Texture for the given cube texture. -filament::Texture* CreateCubeTexture(filament::Engine* engine, int width, - int height, int num_channels, - const uint8_t* data, bool is_srgb); + // Creates a texture for use with a render target. + Texture(filament::Engine* engine, RenderTargetTextureType type, int width, + int height); -// Creates a filament Texture for the given KTX payload. -filament::Texture* CreateKtxTexture( - filament::Engine* engine, const uint8_t* data, int size, - filament::math::float3* spherical_harmonics_out); + ~Texture(); -// Creates a filament Texture for the given texture in the mjModel. -filament::Texture* CreateTexture(filament::Engine* engine, const mjModel* model, - int id, TextureType texture_type); + filament::Texture* GetFilamentTexture() const { return texture_; } -// Creates a filament Texture for the given render target. -filament::Texture* CreateRenderTargetTexture(filament::Engine* engine, - int width, int height, - RenderTargetTextureType type); + using SphericalHarmonics = filament::math::float3[9]; + const SphericalHarmonics* GetSphericalHarmonics() const { + return has_spherical_harmonics_ ? &spherical_harmonics_ : nullptr; + } + + Texture(const Texture&) = delete; + Texture& operator=(const Texture&) = delete; + + private: + + void Create2dTexture(int width, int height, int num_channels, + const uint8_t* data, bool is_srgb); + void CreateCubeTexture(int width, int height, int num_channels, + const uint8_t* data, bool is_srgb); + void CreateKtxTexture(const uint8_t* data, int size); + + filament::Engine* engine_ = nullptr; + filament::Texture* texture_ = nullptr; + SphericalHarmonics spherical_harmonics_; + bool has_spherical_harmonics_ = false; +}; } // namespace mujoco #endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_TEXTURE_UTIL_H_ From 9fa9193a627baab4395d4fa949aa57bccdb958bb Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 1 Apr 2026 04:31:54 -0700 Subject: [PATCH 005/251] Split Texture creation into two structs. TextureConfig describes the properties of the texture (e.g. width, height, pixel format, etc.). TextureData contains the binary payload of the texture. PiperOrigin-RevId: 892854578 Change-Id: I604b926ac38ff1050cac8fdf9ec767b789c0798e --- .../filament/filament/model_objects.cc | 57 ++-- .../filament/filament/object_manager.cc | 65 +++- .../filament/filament/texture_util.cc | 306 ++++++++++-------- .../filament/filament/texture_util.h | 93 +++++- 4 files changed, 338 insertions(+), 183 deletions(-) diff --git a/src/experimental/filament/filament/model_objects.cc b/src/experimental/filament/filament/model_objects.cc index 0529d90e..b65bb1be 100644 --- a/src/experimental/filament/filament/model_objects.cc +++ b/src/experimental/filament/filament/model_objects.cc @@ -125,30 +125,43 @@ void ModelObjects::UploadTexture(const mjModel* model, int id) { mju_error("Invalid texture index: %d", id); } - const int width = model->tex_width[id]; - const int height = model->tex_height[id]; - const int num_channels = model->tex_nchannel[id]; - const int tex_type = model->tex_type[id]; - const mjtByte* data = model->tex_data + model->tex_adr[id]; - const mjtColorSpace color_space = (mjtColorSpace)model->tex_colorspace[id]; + TextureConfig config; + DefaultTextureConfig(&config); + config.width = model->tex_width[id]; + config.height = model->tex_height[id]; + config.target = (mjtTexture)model->tex_type[id]; + config.color_space = (mjtColorSpace)model->tex_colorspace[id]; + switch (model->tex_nchannel[id]) { + case 1: + config.format = mjPIXEL_FORMAT_R8; + break; + case 3: + config.format = mjPIXEL_FORMAT_RGB8; + break; + case 4: + config.format = mjPIXEL_FORMAT_RGBA8; + break; + default: + mju_error("Unsupported texture format: %d", model->tex_nchannel[id]); + break; + } + if (config.height == 1 && model->tex_nchannel[id] == 1) { + config.format = mjPIXEL_FORMAT_KTX; + } - const TextureType type = [&] { - if (height == 1) { - return TextureType::kKtx; - } else if (tex_type == mjTEXTURE_2D) { - return TextureType::kNormal2d; - } else if (tex_type == mjTEXTURE_CUBE) { - return TextureType::kCube; - } else if (tex_type == mjTEXTURE_SKYBOX) { - return TextureType::kCube; - } else { - mju_error("Unsupported texture type: %d", tex_type); - return TextureType::kNormal2d; - } - }(); - textures_[id] = std::make_unique(engine_, type, color_space, width, - height, num_channels, data); + TextureData payload; + DefaultTextureData(&payload); + payload.bytes = model->tex_data + model->tex_adr[id]; + payload.nbytes = + model->tex_width[id] * model->tex_height[id] * model->tex_nchannel[id]; + // We assume that the model has the same lifetime as the engine. + payload.user_data = nullptr; + payload.release_callback = nullptr; + + auto texture = std::make_unique(engine_, config); + texture->Upload(payload); + textures_[id] = std::move(texture); } void ModelObjects::UploadHeightField(const mjModel* model, int id) { diff --git a/src/experimental/filament/filament/object_manager.cc b/src/experimental/filament/filament/object_manager.cc index 7699e655..01895161 100644 --- a/src/experimental/filament/filament/object_manager.cc +++ b/src/experimental/filament/filament/object_manager.cc @@ -14,6 +14,7 @@ #include "experimental/filament/filament/object_manager.h" +#include #include #include #include @@ -87,18 +88,35 @@ ObjectManager::ObjectManager(filament::Engine* engine) materials_[kUnlitUi] = LoadMaterial("unlit_ui.filamat"); static uint8_t black_rgb[3] = {0, 0, 0}; - fallback_black_ = std::make_unique( - engine_, TextureType::kNormal2d, mjCOLORSPACE_LINEAR, 1, 1, 3, black_rgb); static uint8_t white_rgb[3] = {255, 255, 255}; - fallback_white_ = std::make_unique( - engine_, TextureType::kNormal2d, mjCOLORSPACE_LINEAR, 1, 1, 3, white_rgb); static uint8_t normal_data[3] = {128, 128, 255}; - fallback_normal_ = - std::make_unique(engine_, TextureType::kNormal2d, - mjCOLORSPACE_LINEAR, 1, 1, 3, normal_data); static uint8_t orm_data[3] = {0, 255, 0}; - fallback_orm_ = std::make_unique( - engine_, TextureType::kNormal2d, mjCOLORSPACE_LINEAR, 1, 1, 3, orm_data); + + TextureConfig config; + DefaultTextureConfig(&config); + config.width = 1; + config.height = 1; + config.target = mjTEXTURE_2D; + config.format = mjPIXEL_FORMAT_RGB8; + config.color_space = mjCOLORSPACE_LINEAR; + + auto CreateFallbackTexture = [this, &config](uint8_t color[3]) { + auto texture = std::make_unique(engine_, config); + + TextureData payload; + DefaultTextureData(&payload); + payload.bytes = color; + payload.nbytes = 3; + payload.release_callback = nullptr; + payload.user_data = nullptr; + texture->Upload(payload); + return texture; + }; + + fallback_black_ = CreateFallbackTexture(black_rgb); + fallback_white_ = CreateFallbackTexture(white_rgb); + fallback_normal_ = CreateFallbackTexture(normal_data); + fallback_orm_ = CreateFallbackTexture(orm_data); fallback_textures_[mjTEXROLE_USER] = fallback_black_.get(); fallback_textures_[mjTEXROLE_RGB] = fallback_white_.get(); @@ -149,14 +167,33 @@ void ObjectManager::LoadFallbackIndirectLight( fallback_indirect_light_ = nullptr; } - Asset asset(filename); - if (asset.size == 0) { + Asset* asset = new Asset(filename); + auto release_asset = +[](void* user_data) { + delete static_cast(user_data); + }; + if (asset->size == 0) { + release_asset(asset); return; } - fallback_indirect_light_texture_ = std::make_unique( - engine_, TextureType::kKtx, mjCOLORSPACE_AUTO, asset.size, 1, 1, - reinterpret_cast(asset.payload)); + TextureConfig config; + DefaultTextureConfig(&config); + config.width = 1; + config.height = 1; + config.target = mjTEXTURE_CUBE; + config.format = mjPIXEL_FORMAT_KTX; + config.color_space = mjCOLORSPACE_AUTO; + + fallback_indirect_light_texture_ = std::make_unique(engine_, config); + + TextureData payload; + DefaultTextureData(&payload); + payload.bytes = asset->payload; + payload.nbytes = static_cast(asset->size); + payload.release_callback = release_asset; + payload.user_data = asset; + + fallback_indirect_light_texture_->Upload(payload); if (fallback_indirect_light_texture_ == nullptr) { return; } diff --git a/src/experimental/filament/filament/texture_util.cc b/src/experimental/filament/filament/texture_util.cc index 20b63393..b711d248 100644 --- a/src/experimental/filament/filament/texture_util.cc +++ b/src/experimental/filament/filament/texture_util.cc @@ -27,61 +27,124 @@ namespace mujoco { -static filament::Texture::Format GetTextureFormat(int num_channels) { - switch (num_channels) { - case 1: +static constexpr int kNumFacesPerCube = 6; + +static bool IsCompressed(const TextureConfig& config) { + return config.format == mjPIXEL_FORMAT_KTX; +} + +static bool IsCubeMap(const TextureConfig& config) { + return config.target == mjTEXTURE_CUBE || config.target == mjTEXTURE_SKYBOX; +} + +static int GetFaceHeight(const TextureConfig& config) { + int face_height = config.height; + if (config.width != config.height) { + if (config.width * kNumFacesPerCube != config.height) { + mju_error("Cube maps must contain 6 square images."); + } + face_height = config.height / kNumFacesPerCube; + } + if (config.width != face_height) { + mju_error("Cube map faces must be square."); + } + return face_height; +} + +static int GetNumChannels(const TextureConfig& config) { + switch (config.format) { + case mjPIXEL_FORMAT_R8: + return 1; + case mjPIXEL_FORMAT_RGB8: + return 3; + case mjPIXEL_FORMAT_RGBA8: + return 4; + default: + mju_error("Unsupported format: %d", (int)config.format); + return 0; + } +} + +static filament::Texture::Format GetTextureFormat(const TextureConfig& config) { + switch (config.format) { + case mjPIXEL_FORMAT_R8: return filament::Texture::Format::R; - case 3: + case mjPIXEL_FORMAT_RGB8: return filament::Texture::Format::RGB; - case 4: + case mjPIXEL_FORMAT_RGBA8: return filament::Texture::Format::RGBA; default: - mju_error("Unsupported number of channels: %d", num_channels); + mju_error("Unsupported format: %d", (int)config.format); return filament::Texture::Format::UNUSED; } } static filament::Texture::InternalFormat GetTextureInternalFormat( - int num_channels, bool is_srgb) { - if (is_srgb) { - switch (num_channels) { - case 3: + const TextureConfig& config) { + if (config.color_space == mjCOLORSPACE_SRGB) { + switch (config.format) { + case mjPIXEL_FORMAT_RGB8: return filament::Texture::InternalFormat::SRGB8; - case 4: + case mjPIXEL_FORMAT_RGBA8: return filament::Texture::InternalFormat::SRGB8_A8; default: - mju_error("Unsupported number of channels: %d", num_channels); + mju_error("Unsupported format: %d", (int)config.format); return filament::Texture::InternalFormat::UNUSED; } } else { - switch (num_channels) { - case 1: + switch (config.format) { + case mjPIXEL_FORMAT_R8: return filament::Texture::InternalFormat::R8; - case 3: + case mjPIXEL_FORMAT_RGB8: return filament::Texture::InternalFormat::RGB8; - case 4: + case mjPIXEL_FORMAT_RGBA8: return filament::Texture::InternalFormat::RGBA8; default: - mju_error("Unsupported number of channels: %d", num_channels); + mju_error("Unsupported format: %d", (int)config.format); return filament::Texture::InternalFormat::UNUSED; } } } -Texture::Texture(filament::Engine* engine, TextureType texture_type, - mjtColorSpace color_space, int width, int height, - int num_channels, const uint8_t* data) - : engine_(engine) { - const bool is_srgb = color_space == mjCOLORSPACE_SRGB; - if (texture_type == TextureType::kCube) { - CreateCubeTexture(width, height, num_channels, data, is_srgb); - } else if (texture_type == TextureType::kNormal2d) { - Create2dTexture(width, height, num_channels, data, is_srgb); - } else if (texture_type == TextureType::kKtx) { - CreateKtxTexture(data, width * height * num_channels); - } else { - mju_error("Unsupported texture type: %d", static_cast(texture_type)); +void DefaultTextureData(TextureData* data) { + std::memset(data, 0, sizeof(TextureData)); +} + +void DefaultTextureConfig(TextureConfig* config) { + std::memset(config, 0, sizeof(TextureConfig)); +} + +Texture::Texture(filament::Engine* engine, const TextureConfig& config) + : engine_(engine), config_(config) { + if (IsCompressed(config_)) { + // We defer creation of compressed textures until Upload() is called. In + // the meantime, we don't really know anything about the texture (e.g. + // width, height, etc.). + return; } + + filament::Texture::Builder builder; + builder.width(config_.width); + builder.height(config_.height); + builder.format(GetTextureInternalFormat(config_)); + + if (IsCubeMap(config_)) { + if (config_.format != mjPIXEL_FORMAT_RGB8) { + mju_error("Only support RGB cubemaps."); + return; + } + builder.height(GetFaceHeight(config_)); + builder.sampler(filament::Texture::Sampler::SAMPLER_CUBEMAP); + } else { + builder.sampler(filament::Texture::Sampler::SAMPLER_2D); + } + + if (config_.color_space != mjCOLORSPACE_SRGB) { + builder.usage(filament::Texture::Usage::GEN_MIPMAPPABLE | + filament::Texture::Usage::SAMPLEABLE | + filament::Texture::Usage::UPLOADABLE); + } + texture_ = builder.build(*engine_); } Texture::Texture(filament::Engine* engine, RenderTargetTextureType type, @@ -117,112 +180,91 @@ Texture::Texture(filament::Engine* engine, RenderTargetTextureType type, texture_ = builder.build(*engine); } -void Texture::Create2dTexture(int width, int height, int num_channels, - const uint8_t* data, bool is_srgb) { - if (num_channels != 1 && num_channels != 3 && num_channels != 4) { - mju_error("Unsupported number of channels: %d", num_channels); - return; - } - - filament::Texture::Builder builder; - builder.width(width); - builder.height(height); - builder.format(GetTextureInternalFormat(num_channels, is_srgb)); - builder.sampler(filament::Texture::Sampler::SAMPLER_2D); - if (!is_srgb) { - builder.usage(filament::Texture::Usage::GEN_MIPMAPPABLE | - filament::Texture::Usage::SAMPLEABLE | - filament::Texture::Usage::UPLOADABLE); - } - texture_ = builder.build(*engine_); - - if (data) { - const size_t num_bytes = width * height * sizeof(uint8_t) * num_channels; - const filament::Texture::Format format = GetTextureFormat(num_channels); - texture_->setImage( - *engine_, 0, - filament::Texture::PixelBufferDescriptor( - data, num_bytes, format, filament::Texture::Type::UBYTE)); - if (!is_srgb) { - texture_->generateMipmaps(*engine_); - } - } -} - -void Texture::CreateCubeTexture(int width, int height, int num_channels, - const uint8_t* data, bool is_srgb) { - if (num_channels != 3) { - mju_error("Only support RGB cubemaps."); - return; - } - - const int kNumFacesPerCube = 6; - - int face_height = height; - if (width != height) { - if (width * kNumFacesPerCube != height) { - mju_error("Cube maps must contain 6 square images."); - } - face_height = height / kNumFacesPerCube; - } - if (width != face_height) { - mju_error("Cube map faces must be square."); - } - - filament::Texture::Builder builder; - builder.width(width); - builder.height(face_height); - builder.format(GetTextureInternalFormat(num_channels, is_srgb)); - builder.sampler(filament::Texture::Sampler::SAMPLER_CUBEMAP); - if (!is_srgb) { - builder.usage(filament::Texture::Usage::GEN_MIPMAPPABLE | - filament::Texture::Usage::SAMPLEABLE | - filament::Texture::Usage::UPLOADABLE); - } - texture_ = builder.build(*engine_); - - const int face_size = width * face_height * num_channels; - const int num_bytes = face_size * kNumFacesPerCube; - - uint8_t* buffer = new uint8_t[num_bytes]; - auto callback = +[](void* buffer, size_t size, void* user) { - delete [] reinterpret_cast(buffer); - }; - - filament::Texture::FaceOffsets offsets(face_size); - if (width == height) { - // Copy the image to all the faces. - for (int i = 0; i < kNumFacesPerCube; ++i) { - std::memcpy(buffer + (i * face_size), data, face_size); - } - } else { - // Use the cubemap as is. - std::memcpy(buffer, data, num_bytes); - } - - if (data) { - filament::Texture::PixelBufferDescriptor desc( - buffer, num_bytes, filament::Texture::Format::RGB, - filament::Texture::Type::UBYTE, callback); - texture_->setImage(*engine_, 0, std::move(desc), offsets); - if (!is_srgb) { - texture_->generateMipmaps(*engine_); - } - } -} - -void Texture::CreateKtxTexture(const uint8_t* data, int size) { - image::Ktx1Bundle* bundle = new image::Ktx1Bundle(data, size); - has_spherical_harmonics_ = true; - bundle->getSphericalHarmonics(spherical_harmonics_); - const bool is_srgb = false; - texture_ = ktxreader::Ktx1Reader::createTexture(engine_, bundle, is_srgb); -} - Texture::~Texture() { + ReleaseData(); if (texture_) { engine_->destroy(texture_); } } +void Texture::Upload(const TextureData& data) { + user_data_ = data.user_data; + release_callback_ = data.release_callback; + + if (data.bytes == nullptr || data.nbytes == 0) { + ReleaseData(); + return; + } + + if (config_.format == mjPIXEL_FORMAT_KTX) { + image::Ktx1Bundle* bundle = new image::Ktx1Bundle( + reinterpret_cast(data.bytes), data.nbytes); + has_spherical_harmonics_ = true; + bundle->getSphericalHarmonics(spherical_harmonics_); + const bool is_srgb = false; + texture_ = ktxreader::Ktx1Reader::createTexture(engine_, bundle, is_srgb); + config_.width = texture_->getWidth(); + config_.height = texture_->getHeight(); + ReleaseData(); + return; + } + + const int num_channels = GetNumChannels(config_); + const filament::Texture::Type type = filament::Texture::Type::UBYTE; + const filament::Texture::Format format = GetTextureFormat(config_); + + if (!IsCubeMap(config_)) { + if (config_.width * config_.height * num_channels != data.nbytes) { + mju_error("Texture size does not match data size."); + } + + auto callback = +[](void* buffer, size_t size, void* user) { + reinterpret_cast(user)->ReleaseData(); + }; + filament::Texture::PixelBufferDescriptor desc(data.bytes, data.nbytes, + format, type, callback, this); + texture_->setImage(*engine_, 0, std::move(desc)); + } else { + const int face_size = config_.width * GetFaceHeight(config_) * num_channels; + const int num_bytes = face_size * kNumFacesPerCube; + filament::Texture::FaceOffsets offsets(face_size); + + if (config_.width == config_.height) { + uint8_t* copy = new uint8_t[num_bytes]; + auto release_callback = +[](void* buffer, size_t size, void* user) { + delete [] reinterpret_cast(buffer); + }; + for (int i = 0; i < kNumFacesPerCube; ++i) { + std::memcpy(copy + (i * face_size), data.bytes, face_size); + } + filament::Texture::PixelBufferDescriptor desc(copy, num_bytes, format, + type, release_callback); + texture_->setImage(*engine_, 0, std::move(desc), offsets); + ReleaseData(); + } else { + if (num_bytes != data.nbytes) { + mju_error("Texture size does not match data size."); + } + auto callback = +[](void* buffer, size_t size, void* user) { + reinterpret_cast(user)->ReleaseData(); + }; + filament::Texture::PixelBufferDescriptor desc( + data.bytes, data.nbytes, format, type, callback, this); + texture_->setImage(*engine_, 0, std::move(desc), offsets); + } + } + + if (config_.color_space != mjCOLORSPACE_SRGB) { + texture_->generateMipmaps(*engine_); + } +} + +void Texture::ReleaseData() { + if (release_callback_) { + release_callback_(user_data_); + release_callback_ = nullptr; + user_data_ = nullptr; + } +} + } // namespace mujoco diff --git a/src/experimental/filament/filament/texture_util.h b/src/experimental/filament/filament/texture_util.h index 42925325..bba7b16e 100644 --- a/src/experimental/filament/filament/texture_util.h +++ b/src/experimental/filament/filament/texture_util.h @@ -15,7 +15,7 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_TEXTURE_UTIL_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_TEXTURE_UTIL_H_ -#include +#include #include #include @@ -25,14 +25,16 @@ // Functions for creating filament textures. namespace mujoco { -// The types of textures we can create. -enum class TextureType { +// The types of textures we can create. For internal use only. +enum class TextureTarget { + // A standard 2D image with a width and a height. kNormal2d, + // A 2D texture split up into the 6 faces of a cube. kCube, - kKtx, }; // The different types of textures we can create for a render target. +// For internal use only. enum class RenderTargetTextureType { kColor, kDepth, @@ -40,23 +42,85 @@ enum class RenderTargetTextureType { kReflectionColor, }; +// Pixel formats for textures. +typedef enum mjtPixelFormat_ { + mjPIXEL_FORMAT_UNKNOWN = 0, + mjPIXEL_FORMAT_R8, + mjPIXEL_FORMAT_RGB8, + mjPIXEL_FORMAT_RGBA8, + mjPIXEL_FORMAT_DEPTH32F, + mjPIXEL_FORMAT_KTX, +} mjtPixelFormat; + +// The binary contents of a texture. +struct TextureData { + // Pointer to the image data. If null, an empty texture will be created. + void* bytes; + + // The number of bytes in the image data. + size_t nbytes; + + // Because rendering may be multithreaded, we cannot make assumptions about + // when the image data will finish uploading to the GPU. As such, we will use + // this callback to notify callers when it is safe to free the image data. + void (*release_callback)(void* user_data); + + // User data to pass to the release callback. + void* user_data; +}; + +// Initializes the TextureData to default values. +void DefaultTextureData(TextureData* data); + +// Defines the basic properties of a texture. +struct TextureConfig { + // The width of the texture. For compressed textures (e.g. KTX), this is the + // number of bytes in the compressed data. + int width; + + // The height of the texture. For compressed textures (e.g. KTX), this should + // be 0. + int height; + + // The target of the texture (e.g. 2D, cube, etc.) + mjtTexture target; + + // The format of the pixels in the texture (e.g. RGB8, RGBA8, KTX, etc.) + mjtPixelFormat format; + + // The color space of the texture (e.g. LINEAR, sRGB, etc.) + mjtColorSpace color_space; +}; + +// Initializes the TextureConfig to default values. +void DefaultTextureConfig(TextureConfig* config); + +// Wrapper around a filament::Texture. class Texture { public: // Creates a texture with the given data. - Texture(filament::Engine* engine, TextureType texture_type, - mjtColorSpace color_space, int width, int height, int num_channels, - const uint8_t* data); + Texture(filament::Engine* engine, const TextureConfig& config); - // Creates a texture for use with a render target. + // Creates a texture for use with a render target, for internal use. Texture(filament::Engine* engine, RenderTargetTextureType type, int width, int height); ~Texture(); + // Uploads the given data to the texture. + void Upload(const TextureData& data); + + // Returns the width of the texture. + int GetWidth() const { return config_.width; } + + // Returns the height of the texture. + int GetHeight() const { return config_.height; } + + // Returns the underlying filament texture. filament::Texture* GetFilamentTexture() const { return texture_; } + // Returns any spherical harmonics data associated with the texture. using SphericalHarmonics = filament::math::float3[9]; - const SphericalHarmonics* GetSphericalHarmonics() const { return has_spherical_harmonics_ ? &spherical_harmonics_ : nullptr; } @@ -65,17 +129,16 @@ class Texture { Texture& operator=(const Texture&) = delete; private: - - void Create2dTexture(int width, int height, int num_channels, - const uint8_t* data, bool is_srgb); - void CreateCubeTexture(int width, int height, int num_channels, - const uint8_t* data, bool is_srgb); - void CreateKtxTexture(const uint8_t* data, int size); + void ReleaseData(); filament::Engine* engine_ = nullptr; filament::Texture* texture_ = nullptr; + TextureConfig config_; SphericalHarmonics spherical_harmonics_; bool has_spherical_harmonics_ = false; + + void* user_data_ = nullptr; + void (*release_callback_)(void* user_data) = nullptr; }; } // namespace mujoco From 565e46da424d42dd4d7b2be4daf7a453cd9fc8fa Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 1 Apr 2026 05:05:34 -0700 Subject: [PATCH 006/251] Update GuiView to use Texture class. PiperOrigin-RevId: 892866156 Change-Id: I2a2e9ef51df51472b675d69edcf2d5ba291721ea --- .../filament/filament/gui_view.cc | 126 ++++++++---------- src/experimental/filament/filament/gui_view.h | 4 +- 2 files changed, 61 insertions(+), 69 deletions(-) diff --git a/src/experimental/filament/filament/gui_view.cc b/src/experimental/filament/filament/gui_view.cc index 8fa83cae..70b21cf1 100644 --- a/src/experimental/filament/filament/gui_view.cc +++ b/src/experimental/filament/filament/gui_view.cc @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -31,6 +32,7 @@ #include #include #include "experimental/filament/filament/buffer_util.h" +#include "experimental/filament/filament/texture_util.h" #include "experimental/filament/filament/vertex_util.h" namespace mujoco { @@ -67,9 +69,7 @@ GuiView::~GuiView() { for (auto& instance : instances_) { engine_->destroy(instance); } - for (auto& texture : textures_) { - engine_->destroy(texture.second); - } + textures_.clear(); engine_->destroyCameraComponent(camera_->getEntity()); engine_->destroy(view_); engine_->destroy(scene_); @@ -98,62 +98,52 @@ uintptr_t GuiView::UploadImage(uintptr_t tex_id, const uint8_t* pixels, mju_error("Unsupported image bpp. Got %d, wanted 3 or 4", bpp); } - const auto internal_format = - bpp == 4 ? filament::Texture::InternalFormat::RGBA8 - : filament::Texture::InternalFormat::RGB8; - const auto texture_format = bpp == 4 ? filament::Texture::Format::RGBA - : filament::Texture::Format::RGB; - - filament::Texture* texture = nullptr; - if (tex_id == 0) { - texture = filament::Texture::Builder() - .width(width) - .height(height) - .levels(1) - .format(internal_format) - .sampler(filament::Texture::Sampler::SAMPLER_2D) - .build(*engine_); - tex_id = textures_.size() + 1; - textures_[tex_id] = texture; - } else { - auto iter = textures_.find(tex_id); - if (iter == textures_.end()) { - mju_error("Texture not found: %lu", tex_id); - } - texture = iter->second; - - if (pixels == nullptr) { - // A nullptr implies that the user wants to destroy the texture. - engine_->destroy(texture); + if (pixels == nullptr) { + // If the pixels are nullptr, we destroy the texture. + if (tex_id != 0) { textures_.erase(tex_id); - return 0; - } else if (texture->getWidth() != width || texture->getHeight() != height) { - // Recreate the texture if the dimensions have changed. - engine_->destroy(texture); - texture = filament::Texture::Builder() - .width(width) - .height(height) - .levels(1) - .format(internal_format) - .sampler(filament::Texture::Sampler::SAMPLER_2D) - .build(*engine_); - textures_[tex_id] = texture; } + return 0; + } + + // Assign a new texture ID. + if (tex_id == 0) { + tex_id = textures_.size() + 1; + } + + std::unique_ptr& texture = textures_[tex_id]; + + // If the texture does not exist or the dimensions have changed, we create a + // new texture. + if (texture == nullptr || texture->GetWidth() != width || + texture->GetHeight() != height) { + TextureConfig config; + DefaultTextureConfig(&config); + config.width = width; + config.height = height; + config.target = mjTEXTURE_2D; + config.format = bpp == 4 ? mjPIXEL_FORMAT_RGBA8 : mjPIXEL_FORMAT_RGB8; + config.color_space = mjCOLORSPACE_LINEAR; + texture = std::make_unique(engine_, config); } // Create a copy of the image to pass it to filament as we don't know the // lifetime of the data. - const int num_bytes = width * height * bpp; + const size_t num_bytes = width * height * bpp; std::byte* bytes = new std::byte[num_bytes]; - std::memcpy(bytes, pixels, num_bytes); - const auto callback = [](void* buffer, size_t size, void* user) { - auto* ptr = reinterpret_cast(user); - delete[] ptr; + const auto callback = +[](void* user) { + delete[] reinterpret_cast(user); }; - filament::Texture::PixelBufferDescriptor pb(bytes, num_bytes, texture_format, - filament::Texture::Type::UBYTE, - callback); - texture->setImage(*engine_, 0, std::move(pb)); + + TextureData texture_data; + DefaultTextureData(&texture_data); + texture_data.bytes = bytes; + texture_data.nbytes = num_bytes; + texture_data.user_data = bytes; + texture_data.release_callback = callback; + + std::memcpy(bytes, pixels, num_bytes); + texture->Upload(texture_data); return tex_id; } @@ -162,40 +152,39 @@ void GuiView::CreateTexture(ImTextureData* data) { mju_error("Unsupported texture format."); } - filament::Texture* texture = - filament::Texture::Builder() - .width(data->Width) - .height(data->Height) - .levels(1) - .format(filament::Texture::InternalFormat::RGBA8) - .sampler(filament::Texture::Sampler::SAMPLER_2D) - .build(*engine_); + TextureConfig config; + DefaultTextureConfig(&config); + config.width = data->Width; + config.height = data->Height; + config.target = mjTEXTURE_2D; + config.format = mjPIXEL_FORMAT_RGBA8; + config.color_space = mjCOLORSPACE_LINEAR; const uintptr_t tex_id = textures_.size() + 1; - textures_[tex_id] = texture; + textures_[tex_id] = std::make_unique(engine_, config); data->SetTexID((ImTextureID)tex_id); UpdateTexture(data); } void GuiView::UpdateTexture(ImTextureData* data) { - const int size = data->Width * data->Height * 4; - filament::Texture::PixelBufferDescriptor pb(data->GetPixels(), size, - filament::Texture::Format::RGBA, - filament::Texture::Type::UBYTE); auto iter = textures_.find(data->TexID); if (iter == textures_.end()) { mju_error("Texture not found: %llu", data->TexID); } - filament::Texture* texture = iter->second; - texture->setImage(*engine_, 0, std::move(pb)); + TextureData texture_data; + DefaultTextureData(&texture_data); + texture_data.bytes = data->GetPixels(); + texture_data.nbytes = data->Width * data->Height * 4; + texture_data.user_data = nullptr; + texture_data.release_callback = nullptr; + iter->second->Upload(texture_data); data->SetStatus(ImTextureStatus_OK); } void GuiView::DestroyTexture(ImTextureData* data) { auto iter = textures_.find(data->TexID); if (iter != textures_.end()) { - engine_->destroy(iter->second); textures_.erase(data->TexID); data->SetTexID(ImTextureID_Invalid); data->SetStatus(ImTextureStatus_Destroyed); @@ -361,7 +350,8 @@ filament::MaterialInstance* GuiView::GetMaterialInstance(int index, } filament::MaterialInstance* instance = instances_[index]; - instance->setParameter("glyph", iter->second, filament::TextureSampler()); + instance->setParameter("glyph", iter->second->GetFilamentTexture(), + filament::TextureSampler()); instance->setScissor(rect.left, rect.bottom, rect.width, rect.height); return instance; } diff --git a/src/experimental/filament/filament/gui_view.h b/src/experimental/filament/filament/gui_view.h index 2adb6d07..b18ccb79 100644 --- a/src/experimental/filament/filament/gui_view.h +++ b/src/experimental/filament/filament/gui_view.h @@ -16,6 +16,7 @@ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_GUI_VIEW_H_ #include +#include #include #include @@ -29,6 +30,7 @@ #include #include #include "experimental/filament/filament/buffer_util.h" +#include "experimental/filament/filament/texture_util.h" namespace mujoco { @@ -71,7 +73,7 @@ class GuiView { utils::Entity renderable_; std::vector buffers_; std::vector instances_; - std::unordered_map textures_; + std::unordered_map> textures_; int num_elements_ = 0; }; From 6da210c794d49c102a159c034456301cb27b5e55 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 1 Apr 2026 05:13:19 -0700 Subject: [PATCH 007/251] Rename texture_util to texture. PiperOrigin-RevId: 892868713 Change-Id: I113132dc9e4535e94d48c4e71520acb159e2e216 --- src/experimental/filament/CMakeLists.txt | 4 ++-- src/experimental/filament/filament/drawable.cc | 2 +- src/experimental/filament/filament/drawable.h | 2 +- src/experimental/filament/filament/filament_context.cc | 2 +- src/experimental/filament/filament/gui_view.cc | 2 +- src/experimental/filament/filament/gui_view.h | 2 +- src/experimental/filament/filament/material.cc | 2 +- src/experimental/filament/filament/material.h | 2 +- src/experimental/filament/filament/model_objects.cc | 2 +- src/experimental/filament/filament/model_objects.h | 2 +- src/experimental/filament/filament/model_util.cc | 1 - src/experimental/filament/filament/object_manager.cc | 2 +- src/experimental/filament/filament/object_manager.h | 2 +- src/experimental/filament/filament/render_target_util.cc | 2 +- src/experimental/filament/filament/render_target_util.h | 2 +- src/experimental/filament/filament/scene_view.cc | 2 +- .../filament/filament/{texture_util.cc => texture.cc} | 2 +- .../filament/filament/{texture_util.h => texture.h} | 6 +++--- 18 files changed, 20 insertions(+), 21 deletions(-) rename src/experimental/filament/filament/{texture_util.cc => texture.cc} (99%) rename src/experimental/filament/filament/{texture_util.h => texture.h} (95%) diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index 2a0c5c8f..457a5db9 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -59,8 +59,8 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/renderables.h filament/scene_view.cc filament/scene_view.h - filament/texture_util.cc - filament/texture_util.h + filament/texture.cc + filament/texture.h filament/vertex_util.cc filament/vertex_util.h ) diff --git a/src/experimental/filament/filament/drawable.cc b/src/experimental/filament/filament/drawable.cc index 55250600..f32afe1a 100644 --- a/src/experimental/filament/filament/drawable.cc +++ b/src/experimental/filament/filament/drawable.cc @@ -37,7 +37,7 @@ #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/model_objects.h" #include "experimental/filament/filament/object_manager.h" -#include "experimental/filament/filament/texture_util.h" +#include "experimental/filament/filament/texture.h" namespace mujoco { diff --git a/src/experimental/filament/filament/drawable.h b/src/experimental/filament/filament/drawable.h index 06796aed..2f2dc81e 100644 --- a/src/experimental/filament/filament/drawable.h +++ b/src/experimental/filament/filament/drawable.h @@ -26,7 +26,7 @@ #include "experimental/filament/filament/model_objects.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/renderables.h" -#include "experimental/filament/filament/texture_util.h" +#include "experimental/filament/filament/texture.h" namespace mujoco { diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index 973425d1..cbd06da2 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -45,7 +45,7 @@ #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/render_target_util.h" #include "experimental/filament/filament/scene_view.h" -#include "experimental/filament/filament/texture_util.h" +#include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { diff --git a/src/experimental/filament/filament/gui_view.cc b/src/experimental/filament/filament/gui_view.cc index 70b21cf1..1876936e 100644 --- a/src/experimental/filament/filament/gui_view.cc +++ b/src/experimental/filament/filament/gui_view.cc @@ -32,7 +32,7 @@ #include #include #include "experimental/filament/filament/buffer_util.h" -#include "experimental/filament/filament/texture_util.h" +#include "experimental/filament/filament/texture.h" #include "experimental/filament/filament/vertex_util.h" namespace mujoco { diff --git a/src/experimental/filament/filament/gui_view.h b/src/experimental/filament/filament/gui_view.h index b18ccb79..2b189324 100644 --- a/src/experimental/filament/filament/gui_view.h +++ b/src/experimental/filament/filament/gui_view.h @@ -30,7 +30,7 @@ #include #include #include "experimental/filament/filament/buffer_util.h" -#include "experimental/filament/filament/texture_util.h" +#include "experimental/filament/filament/texture.h" namespace mujoco { diff --git a/src/experimental/filament/filament/material.cc b/src/experimental/filament/filament/material.cc index e04e3f81..aaf9c6ba 100644 --- a/src/experimental/filament/filament/material.cc +++ b/src/experimental/filament/filament/material.cc @@ -21,7 +21,7 @@ #include #include #include "experimental/filament/filament/object_manager.h" -#include "experimental/filament/filament/texture_util.h" +#include "experimental/filament/filament/texture.h" namespace mujoco { diff --git a/src/experimental/filament/filament/material.h b/src/experimental/filament/filament/material.h index 849cb89e..b717b9c4 100644 --- a/src/experimental/filament/filament/material.h +++ b/src/experimental/filament/filament/material.h @@ -21,7 +21,7 @@ #include #include #include "experimental/filament/filament/object_manager.h" -#include "experimental/filament/filament/texture_util.h" +#include "experimental/filament/filament/texture.h" namespace mujoco { diff --git a/src/experimental/filament/filament/model_objects.cc b/src/experimental/filament/filament/model_objects.cc index b65bb1be..3df4f2e2 100644 --- a/src/experimental/filament/filament/model_objects.cc +++ b/src/experimental/filament/filament/model_objects.cc @@ -27,7 +27,7 @@ #include "experimental/filament/filament/buffer_util.h" #include "experimental/filament/filament/builtins.h" #include "experimental/filament/filament/model_util.h" -#include "experimental/filament/filament/texture_util.h" +#include "experimental/filament/filament/texture.h" namespace mujoco { diff --git a/src/experimental/filament/filament/model_objects.h b/src/experimental/filament/filament/model_objects.h index 94fc2987..7007e81a 100644 --- a/src/experimental/filament/filament/model_objects.h +++ b/src/experimental/filament/filament/model_objects.h @@ -26,7 +26,7 @@ #include #include #include "experimental/filament/filament/buffer_util.h" -#include "experimental/filament/filament/texture_util.h" +#include "experimental/filament/filament/texture.h" namespace mujoco { diff --git a/src/experimental/filament/filament/model_util.cc b/src/experimental/filament/filament/model_util.cc index 62e72d73..2bd581dd 100644 --- a/src/experimental/filament/filament/model_util.cc +++ b/src/experimental/filament/filament/model_util.cc @@ -32,7 +32,6 @@ #include #include "experimental/filament/filament/buffer_util.h" #include "experimental/filament/filament/math_util.h" -#include "experimental/filament/filament/texture_util.h" #include "experimental/filament/filament/vertex_util.h" namespace mujoco { diff --git a/src/experimental/filament/filament/object_manager.cc b/src/experimental/filament/filament/object_manager.cc index 01895161..80679284 100644 --- a/src/experimental/filament/filament/object_manager.cc +++ b/src/experimental/filament/filament/object_manager.cc @@ -28,7 +28,7 @@ #include #include #include -#include "experimental/filament/filament/texture_util.h" +#include "experimental/filament/filament/texture.h" #include "user/user_resource.h" namespace mujoco { diff --git a/src/experimental/filament/filament/object_manager.h b/src/experimental/filament/filament/object_manager.h index 64d9eadf..52320347 100644 --- a/src/experimental/filament/filament/object_manager.h +++ b/src/experimental/filament/filament/object_manager.h @@ -23,7 +23,7 @@ #include #include #include -#include "experimental/filament/filament/texture_util.h" +#include "experimental/filament/filament/texture.h" namespace mujoco { diff --git a/src/experimental/filament/filament/render_target_util.cc b/src/experimental/filament/filament/render_target_util.cc index ef221e4e..2e4ce391 100644 --- a/src/experimental/filament/filament/render_target_util.cc +++ b/src/experimental/filament/filament/render_target_util.cc @@ -19,7 +19,7 @@ #include #include #include -#include "experimental/filament/filament/texture_util.h" +#include "experimental/filament/filament/texture.h" namespace mujoco { diff --git a/src/experimental/filament/filament/render_target_util.h b/src/experimental/filament/filament/render_target_util.h index d2d581a9..ce6b96dc 100644 --- a/src/experimental/filament/filament/render_target_util.h +++ b/src/experimental/filament/filament/render_target_util.h @@ -19,7 +19,7 @@ #include #include -#include "experimental/filament/filament/texture_util.h" +#include "experimental/filament/filament/texture.h" namespace mujoco { diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 9b64243e..7009dd17 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -50,7 +50,7 @@ #include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/render_target_util.h" -#include "experimental/filament/filament/texture_util.h" +#include "experimental/filament/filament/texture.h" namespace mujoco { diff --git a/src/experimental/filament/filament/texture_util.cc b/src/experimental/filament/filament/texture.cc similarity index 99% rename from src/experimental/filament/filament/texture_util.cc rename to src/experimental/filament/filament/texture.cc index b711d248..1ce62774 100644 --- a/src/experimental/filament/filament/texture_util.cc +++ b/src/experimental/filament/filament/texture.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/filament/filament/texture_util.h" +#include "experimental/filament/filament/texture.h" #include #include diff --git a/src/experimental/filament/filament/texture_util.h b/src/experimental/filament/filament/texture.h similarity index 95% rename from src/experimental/filament/filament/texture_util.h rename to src/experimental/filament/filament/texture.h index bba7b16e..44b0ea31 100644 --- a/src/experimental/filament/filament/texture_util.h +++ b/src/experimental/filament/filament/texture.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_TEXTURE_UTIL_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_TEXTURE_UTIL_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_TEXTURE_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_TEXTURE_H_ #include @@ -142,4 +142,4 @@ class Texture { }; } // namespace mujoco -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_TEXTURE_UTIL_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_TEXTURE_H_ From 70a7647ad9bb92c8d87a05be02ca2c0500f0e1f4 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 1 Apr 2026 07:49:53 -0700 Subject: [PATCH 008/251] Add `` actuator and related docs and tests. PiperOrigin-RevId: 892927987 Change-Id: I38ed6412801341ba03ddf5fe7b93a6081df24d37 --- doc/APIreference/functions.rst | 9 + doc/XMLreference.rst | 219 +++ doc/XMLschema.rst | 168 +++ doc/_static/dcmotor.pdf | Bin 0 -> 599056 bytes doc/changelog.rst | 3 + doc/dcmotor/buildpdf.sh | 21 + doc/dcmotor/dcmotor.tex | 1416 +++++++++++++++++++ doc/dcmotor/refs.bib | 82 ++ doc/includes/references.h | 9 +- include/mujoco/mjmodel.h | 5 +- include/mujoco/mujoco.h | 6 + python/mujoco/introspect/enums.py | 9 +- python/mujoco/introspect/functions.py | 80 ++ python/mujoco/specs.cc | 25 + python/mujoco/specs_test.py | 7 + src/engine/engine_derivative.c | 33 + src/engine/engine_forward.c | 269 +++- src/engine/engine_support.c | 59 +- src/engine/engine_util_misc.c | 20 + src/engine/engine_util_misc.h | 17 + src/user/user_api.cc | 161 +++ src/user/user_objects.cc | 10 +- src/xml/xml_native_reader.cc | 79 +- src/xml/xml_native_reader.h | 2 +- src/xml/xml_native_writer.cc | 2 +- test/engine/engine_derivative_test.cc | 16 +- test/engine/engine_forward_test.cc | 943 ++++++++++++ test/engine/testdata/derivative/dcmotor.xml | 35 + test/xml/xml_native_reader_test.cc | 319 +++++ unity/Runtime/Bindings/MjBindings.cs | 9 +- wasm/codegen/generated/bindings.cc | 16 + 31 files changed, 3994 insertions(+), 55 deletions(-) create mode 100644 doc/_static/dcmotor.pdf create mode 100755 doc/dcmotor/buildpdf.sh create mode 100644 doc/dcmotor/dcmotor.tex create mode 100644 doc/dcmotor/refs.bib create mode 100644 test/engine/testdata/derivative/dcmotor.xml diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index df6381f5..fa61555b 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -4658,6 +4658,15 @@ Set actuator to muscle; return error if any.a Set actuator to active adhesion; return error if any. +.. _mjs_setToDCMotor: + +`mjs_setToDCMotor <#mjs_setToDCMotor>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mjs_setToDCMotor + +Set actuator to DC motor; return error if any. + .. _AddAssets: Assets diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 7e90fa14..2552f932 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -6323,6 +6323,174 @@ This element has a subset of the common attributes and two custom attributes. to the target body. +.. _actuator-dcmotor: + +:el-prefix:`actuator/` |-| **dcmotor** |*| +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +This element creates a DC motor actuator. Note that :el:`dcmotor` is quite different from the :ref:`general actuation +model`. Unlike the general model where the components of force generation are independent affine functions +mapping from control to force, :el:`dcmotor` relies on highly coupled physical dynamics. See the `DC motor technical +note <_static/dcmotor.pdf>`__ for complete mathematical formulations and parameter semantics, but we include a few +important notes here: + +- Note that while :ref:`resistance`, :ref:`motorconst` and + :ref:`nominal` are each optional, some combination of them is required. + See Section 2.1 of the `technical note <_static/dcmotor.pdf>`__. +- The control :ref:`input` semantic is either the voltage applied to the motor terminals, or a + position or velocity target for a PID :ref:`controller`. +- Optional features include electrical dynamics (:ref:`inductance`), + :ref:`cogging torque`, :ref:`thermal resistance variation`, and + :ref:`LuGre` friction. + +The underlying :el:`general` attributes are set to the :el:`dcmotor` type, and their associated parameter arrays are +computed internally: + +========= ======= ========= ======== +Attribute Setting Attribute Setting +========= ======= ========= ======== +dyntype dcmotor dynprm computed +gaintype dcmotor gainprm computed +biastype dcmotor biasprm computed +========= ======= ========= ======== + +This element has the following custom attributes in addition to the common attributes: + +.. _actuator-dcmotor-name: + +.. _actuator-dcmotor-class: + +.. _actuator-dcmotor-group: + +.. _actuator-dcmotor-delay: + +.. _actuator-dcmotor-nsample: + +.. _actuator-dcmotor-interp: + +.. _actuator-dcmotor-ctrllimited: + +.. _actuator-dcmotor-ctrlrange: + +.. _actuator-dcmotor-lengthrange: + +.. _actuator-dcmotor-gear: + +.. _actuator-dcmotor-damping: + +.. _actuator-dcmotor-armature: + +.. _actuator-dcmotor-cranklength: + +.. _actuator-dcmotor-joint: + +.. _actuator-dcmotor-jointinparent: + +.. _actuator-dcmotor-tendon: + +.. _actuator-dcmotor-cranksite: + +.. _actuator-dcmotor-slidersite: + +.. _actuator-dcmotor-site: + +.. _actuator-dcmotor-refsite: + +.. _actuator-dcmotor-user: + +.. |actuator/dcmotor attrib list| replace:: + :at:`name`, :at:`class`, :at:`group`, :at:`nsample`, :at:`interp`, :at:`delay`, :at:`ctrllimited`, :at:`ctrlrange`, + :at:`lengthrange`, :at:`gear`, :at:`damping`, :at:`armature`, :at:`cranklength`, :at:`joint`, :at:`jointinparent`, + :at:`tendon`, :at:`cranksite`, :at:`slidersite`, :at:`site`, :at:`refsite`, :at:`user` + +|actuator/dcmotor attrib list| + Same as in actuator/ :ref:`general `. + +.. _actuator-dcmotor-resistance: + +:at:`resistance`: :at-val:`real, optional` + Terminal resistance :math:`R` in Ohm. (see `tech note <_static/dcmotor.pdf>`__ for details) + +.. _actuator-dcmotor-motorconst: + +:at:`motorconst`: :at-val:`real(2), optional` + Motor constants, defined as :at:`motorconst` = ":at-val:`Kt` :at-val:`Ke`" (N·m/A, equivalently V·s/rad). + :at-val:`Kt` is the torque constant and :at-val:`Ke` the back-EMF constant; they can differ when magnetic saturation + is present. If both are positive, the effective constant is :math:`K = \sqrt{K_t K_e}` (geometric mean). If only one + is positive, :math:`K` equals that value; a single value is interpreted as :math:`K_t = K_e`. If your datasheet gives + the speed constant :math:`K_v` in rad/(V·s), use :math:`K_e = 1/K_v`. (see `tech note <_static/dcmotor.pdf>`__ for + details) + +.. _actuator-dcmotor-nominal: + +:at:`nominal`: :at-val:`real(3), optional` + Nominal operating point, defined as :at:`nominal` = ":at-val:`voltage` :at-val:`stall_torque` + :at-val:`no_load_speed`". The compiler derives :math:`K =` :at-val:`voltage` / :at-val:`no_load_speed` and :math:`R = + K` · :at-val:`voltage` / :at-val:`stall_torque`. (see `tech note <_static/dcmotor.pdf>`__ for details) + +.. _actuator-dcmotor-inductance: + +:at:`inductance`: :at-val:`real(2), "0 0"` + Electrical dynamics, defined as :at:`inductance` = ":at-val:`L` :at-val:`timeconst`" (Henry, seconds). These are + alternative specifications: :at-val:`L` is the winding inductance and :at-val:`timeconst` :math:`= L/R` is the + electrical time constant. Specify one; if both are given, :at-val:`L` takes precedence. If both are 0 (the default), + no electrical dynamics are modeled and the current is computed algebraically. Adds one activation variable for + armature current. (see `tech note <_static/dcmotor.pdf>`__ for details) + +.. _actuator-dcmotor-thermal: + +:at:`thermal`: :at-val:`real(6), "0 0 0 0 0 0"` + Thermal model, defined as :at:`thermal` = ":at-val:`resistance` :at-val:`capacitance` :at-val:`timeconst` + :at-val:`tempcoef` :at-val:`reftemp` :at-val:`ambient`" (K/W, J/K, s, 1/K, °C, °C). The first three sub-values + specify the thermal time constant: :at-val:`timeconst` = :at-val:`resistance` :math:`\times` :at-val:`capacitance`. + Specify either :at-val:`timeconst` directly, or :at-val:`resistance` and :at-val:`capacitance`; if all three are + given, :at-val:`timeconst` takes precedence. If all are 0 (the default), thermal modeling is disabled. Adds one + activation variable for winding temperature. (see `tech note <_static/dcmotor.pdf>`__ for details) + +.. _actuator-dcmotor-saturation: + +:at:`saturation`: :at-val:`real(4), "0 0 0 0"` + Limits on the actuator, defined as :at:`saturation` = ":at-val:`torque` :at-val:`current` :at-val:`voltage` + :at-val:`current_rate`". :at-val:`torque` and :at-val:`current` are alternative specifications of the maximum + continuous torque: if :at-val:`current` is given, :at-val:`torque` :math:`= K \cdot` :at-val:`current`; if both are + given, :at-val:`torque` takes precedence. Sets :at:`forcerange` to [:math:`-\tau_{\max},\, \tau_{\max}`]. + :at-val:`voltage` sets the maximum voltage :math:`V_{\max}`. :at-val:`current_rate` sets the maximum rate of change + of current :math:`(di/dt)_{\max}` (requires :ref:`inductance`). A value of 0 (the + default) for any sub-value disables the respective limit. (see `tech note <_static/dcmotor.pdf>`__ for details) + +.. _actuator-dcmotor-cogging: + +:at:`cogging`: :at-val:`real(3), "0 0 0"` + Cogging torque, defined as :at:`cogging` = ":at-val:`amplitude` :at-val:`poles` :at-val:`phase`" (N·m, integer, rad). + Adds a position-dependent torque :math:`= \textsf{amplitude} \cdot \sin(\textsf{poles} \cdot \theta + + \textsf{phase})`. Disabled when :at-val:`amplitude` = 0 (the default). + (see `tech note <_static/dcmotor.pdf>`__ for details) + +.. _actuator-dcmotor-lugre: + +:at:`lugre`: :at-val:`real(6), "0 0 0 0 0 0"` + LuGre friction, defined as :at:`lugre` = ":at-val:`stiffness` :at-val:`damping` :at-val:`viscous` :at-val:`coulomb` + :at-val:`static` :at-val:`stribeck`" (N·m/rad, N·m·s/rad, N·m·s/rad, N·m, N·m, rad/s). Disabled when + :at-val:`stiffness` = 0 (the default). Adds one activation variable for bristle deflection. Note that the + :at-val:`viscous` coefficient is mapped directly to the actuator :ref:`damping` array + (specifically the linear term, :at-val:`damping[0]`). If both are specified, their values are summed. + (see `tech note <_static/dcmotor.pdf>`__ for details) + +.. _actuator-dcmotor-input: + +:at:`input`: :at-val:`[voltage, position, velocity], "voltage"` + Specifies the input signal semantics. In "voltage" mode, the control directly sets applied motor voltage. In + "position" or "velocity" modes, the PID :ref:`controller` uses the control as a + reference setpoint relative to the joint trajectory. (see `tech note <_static/dcmotor.pdf>`__ for details) + +.. _actuator-dcmotor-controller: + +:at:`controller`: :at-val:`real(5), "0 0 0 0 0"` + PID controller parameters, defined as :at:`controller` = ":at-val:`kp` :at-val:`ki` :at-val:`kd` + :at-val:`slewmax` :at-val:`Imax`". Depending on the :at:`input` mode, the controller stabilizes either position or + velocity. If the :at:`input` mode is voltage, the controller is ignored. A value of 0 (the default) disables the + respective feature: :at-val:`slewmax` = 0 means no slew-rate limiting, :at-val:`Imax` = 0 means no anti-windup + clamping. (see `tech note <_static/dcmotor.pdf>`__ for details) + .. _actuator-plugin: :el-prefix:`actuator/` |-| **plugin** |?| @@ -9887,6 +10055,57 @@ refsite, tendon, slidersite, cranksite. All :ref:`adhesion ` attributes are available here except: name, class, body. +.. _default-dcmotor: + +.. _default-dcmotor-ctrllimited: + +.. _default-dcmotor-ctrlrange: + +.. _default-dcmotor-gear: + +.. _default-dcmotor-damping: + +.. _default-dcmotor-armature: + +.. _default-dcmotor-cranklength: + +.. _default-dcmotor-user: + +.. _default-dcmotor-group: + +.. _default-dcmotor-delay: + +.. _default-dcmotor-nsample: + +.. _default-dcmotor-interp: + +.. _default-dcmotor-motorconst: + +.. _default-dcmotor-resistance: + +.. _default-dcmotor-nominal: + +.. _default-dcmotor-saturation: + +.. _default-dcmotor-inductance: + +.. _default-dcmotor-cogging: + +.. _default-dcmotor-controller: + +.. _default-dcmotor-input: + +.. _default-dcmotor-thermal: + +.. _default-dcmotor-lugre: + +:el-prefix:`default/` |-| **dcmotor** |?| +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +All :ref:`dcmotor ` attributes are available here except: name, class, joint, jointinparent, site, +refsite, tendon, slidersite, cranksite. + + .. _custom: **custom** |*| diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 74f72561..4dbc6c29 100755 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -2984,6 +2984,105 @@ :ref:`gain` + .. dropdown:: :ref:`dcmotor` |*| + + .. grid:: 2 3 4 4 + :gutter: 0 + + .. grid-item:: + :ref:`name` + + .. grid-item:: + :ref:`class` + + .. grid-item:: + :ref:`group` + + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`ctrllimited` + + .. grid-item:: + :ref:`ctrlrange` + + .. grid-item:: + :ref:`lengthrange` + + .. grid-item:: + :ref:`gear` + + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + + .. grid-item:: + :ref:`cranklength` + + .. grid-item:: + :ref:`user` + + .. grid-item:: + :ref:`joint` + + .. grid-item:: + :ref:`jointinparent` + + .. grid-item:: + :ref:`tendon` + + .. grid-item:: + :ref:`slidersite` + + .. grid-item:: + :ref:`cranksite` + + .. grid-item:: + :ref:`site` + + .. grid-item:: + :ref:`refsite` + + .. grid-item:: + :ref:`motorconst` + + .. grid-item:: + :ref:`resistance` + + .. grid-item:: + :ref:`nominal` + + .. grid-item:: + :ref:`saturation` + + .. grid-item:: + :ref:`inductance` + + .. grid-item:: + :ref:`cogging` + + .. grid-item:: + :ref:`controller` + + .. grid-item:: + :ref:`thermal` + + .. grid-item:: + :ref:`lugre` + + .. grid-item:: + :ref:`input` + + .. dropdown:: :ref:`plugin` |*| .. grid:: 2 3 4 4 @@ -6146,6 +6245,75 @@ :ref:`delay` + .. dropdown:: :ref:`dcmotor` :octicon:`dot` + + .. grid:: 2 3 4 4 + :gutter: 0 + + .. grid-item:: + :ref:`ctrllimited` + + .. grid-item:: + :ref:`ctrlrange` + + .. grid-item:: + :ref:`gear` + + .. grid-item:: + :ref:`damping` + + .. grid-item:: + :ref:`armature` + + .. grid-item:: + :ref:`cranklength` + + .. grid-item:: + :ref:`user` + + .. grid-item:: + :ref:`group` + + .. grid-item:: + :ref:`nsample` + + .. grid-item:: + :ref:`interp` + + .. grid-item:: + :ref:`delay` + + .. grid-item:: + :ref:`motorconst` + + .. grid-item:: + :ref:`resistance` + + .. grid-item:: + :ref:`nominal` + + .. grid-item:: + :ref:`saturation` + + .. grid-item:: + :ref:`inductance` + + .. grid-item:: + :ref:`cogging` + + .. grid-item:: + :ref:`controller` + + .. grid-item:: + :ref:`input` + + .. grid-item:: + :ref:`thermal` + + .. grid-item:: + :ref:`lugre` + + .. dropdown:: :ref:`custom` |*| diff --git a/doc/_static/dcmotor.pdf b/doc/_static/dcmotor.pdf new file mode 100644 index 0000000000000000000000000000000000000000..caf8a36c1ce25a04fb7a8c06d2b45dc5f7cbc829 GIT binary patch literal 599056 zcma%>Q;aVR?54-IZTmN#Ib++lZF|nxI%C_mZQHhO@Au#A-DZ>RRg?Cn>D#ne|-iRgbD45OH(jf<%h5u=!m zp^K@Asj@bo?C-Wo{lN+_WQ`|sk8f2ZDZdbwLU@#FCQ&nBG^F?j+68G0EPW!L7w>jM& z)QX?CR+Bsq307 zm2(3tYJiX^m8A}wy5uX;IR*w9n#~@ zP7F`U$rn;M_f6Tj*1uB>(TtnAub1dX4NvlR4w!f2&o^_s!}9`Abo_K!8V^3W`E4;Mfnhp`XzqbcUv#>0s;Hw@p{4HH9Rf@^nUP4i~;?TY^KuZzIRPYNuH^*RsTG} zfd5NT@pV2yg$Io)gE=5eM#QjEJAb^oM|7u;?^YNnNn!Es4$cb-@A7rWhx1)=_mzJuI z0Ks}gM>+Ed7({`F_*cT}KwW||6-&Sk?YFOjYlf%thZy0@nx6HUEOIn{jW1jTQyK}$ z07{PLPH4BwFS&3z0&b0+Wus+O=~8wa)?Py{5?h@}c#201c7}!l@<(%~)K57BWni?Y z(hH6f!_TDK^#y&KaqIHIcTQcz)*aX3YrgZour4}fRzB$P%I0N&+=DOdcis4$v}kA7 zr7+!+{rY9@)QFf6?M>h>Hq`Wt_QzK7vX>gs*&0hARL|xg3mb-#BTei4pB42pV^EM z+XU|;qFc_PpK^72ViUXTU4V?MouAzHBop+Gb?rUPxd_4N$;+eLipk13BCDrq#nrBw zu`AY*3)vO)Ce{K>M`?w5lxc%Ag-|}Yon^ic;;vXN z+|!2HJfxUX)3D)*NjIp8{OO7X^-k+wa1MiXT*%${qBH)vVE=M628uk4$AIwHz+-(MJts8eI{>DlMC`e05Bws?< zk}k-l%S9848_u2OQ(9)Zao)ex31+6x1~u{60C{8IIZr61#5+<^sfi5ub7FIKjGVV-0Ps0>4rAT-V1w5;RX8K_VQgSo` z!~!goeQ}KhXi8YN|CVQN>p9Kg)1PxLRNcL?7NTT$0)`f|aMM#+q9rC0;t3q*IM&KH z7XFTNBw7f`1c^YD&-nC^TANR*pDiP1nl>T`hE*Q)r_plG6Jc$B>!#Fd)J$9;Xh2kG z%+OJCo@xEH&jMltG|Of~omo_e%Lavqt=FYU%sVvG^JppjTSZY=j=Fuom zm|YrmC9X2nxS}D`l{w`aksI1Y-~L$R^4|X<$Z>}zwh}Ax3bJFSc2!1A%^c*$FixBj zW37Ea<-s;vslOj@t(rL3lZB|{f>LNiro=zwCi|x}o6{-&AvwBj%HY-{FWo{vE?`$_ zU*I5THS;;Eb|A&9Q9-n3r5#1W!jYHQD-kC=s_Bv=vFNmPZ_OS2G`QH|b9 zkq!py`C&0H=&M(fBkZHCekRv5B+}D1QqsI?KnY%OWcC|xz^x+lPD^aSrgSAia_syg z@jyDaUqSKj*v&r_K{aA_%Bheift^fjk4pc*9E`nnRc9r74D8r-1$P|nnA_V%LqX7* z!Zsf(qlIXd-{$o(%WXYC`Pz5qT8=NHn;+8QpB9rQDD_SBz?{O#545g5)mf1shkLTC zhRT6pB`2mUEzEjs2KskNFoH9*E-os}h%xq=1l&&wo7tk2OM@s(7ao%nO6@5H5Z8js zzgVAM-l`3_ZKP+YN1_h4O8COlvsh=@^B>pRDSQo`qj`w(ZH{MqJC@(m9iP+1BAhq! zs_A5LEERA%HPcxwZOVPdtQ6b3R2kHX+?-ll48BLk^DP z&xl;rqPI#Y&+5w1b z_cZWpjwS?kVHG8SCZ9_e?pc25sMGAma<*1X7JOwETSjy4Tl`qc3FI?thGKS%Ea!}M zux*nwyCJ#|tsokKipT-$Yad9p+(%?|Fj7x7Bt2;<#s;APcr|6b+8{Slg!+@K3urlZ zhow?whx$i)VUP1lLMTELhUkC6&6L* z83ro`f&#MS$N(#i?%rGWOytSVzeYtVm69t#r#TBpFMvAflzf;aJDSy#1;g>NNaD6L z-lbq*H@B|nmW0NK&X_QfVCGP?#3J3e3>q$xN8Su;9*Fi!yLxxw220*1L{f$`$T!tZ zy6@!6t%R;DRs)F}3VU_wU$xz<#xR^PT?urrJsPT>KrDDW}9UO9i}4b;$_P`GMg zuQptRl!B_jM0Akj40qPJ6rbED_n;6j0Yo4M{&_<>*HGe{+auz|hEv}kI}4{=QRlc& z^$^RUbgnF9HL@N{AHuZzIPCUc$B-k>=gNw1OIjS0*~9>B6zqMp12hqoH%)0; zpuuIPek&i172Vrp?5`<;6a;WFgf(Jk{T)!1DR|~i%10j)8;TD_{ zHJq!a+c5Z9sAIM6sCfx84G8%#ANTaNmDoaG(hyC*0%_VbIBZXPg(Q(x-nne3(-KMk z{JFJdju1T(`>1<<*SG)sVe9>YBdSlS?A|y z=Fs?c+%C6f(#;->gTQkD7<3zSjfqNbM9p;R3TG-o34#6KQe>X4f}a&sC>Bx?A$zk6 z1c?lu{XtWQTA&j@{i#8rr9z7IB7rA_kENBpSfZO%N4bz7jr1K*63Yuxxlk15w7wCz zICY{1D-i7@dN^cjP!f5mHgaUpo#9ImKe8eD0L&Jc1|)<}dr~O(I;#cSLn?E$183e( z*<^f>)FeL{)?)~@n>-v|i)P9C1mvM=G5o(SJ*6mDoG3US!<`|lnYBg>s*J&aT1So>2>KD zPRu@|2*K116=^blvG?;kHir=XaFk!{euwVqjSy71k2 zvGNnudA@(m?nlh>1an%kPnA-`viR2{Bh1}wM*g1FJ8M8 zZq*)6Me`|y$TJZlVcoK#c!mW4ceTF0BdZ`!KhtD)pt2$42IC}TtZh@{x+nxT^}BU5 z9qH&@p)HVe1)%Dks-Xny5n75E{G%~xax8zCUNZ$tB%P^e9r|k!!+JwCi*5lu@-%ZN z$F7gU*Aj719t;04I%uQ z5K%~SqaRx;K!DEPt5>lUTe`Rs1Jj%56^7p2*%yNwy=PpGf!Gw1g+8h_J_ zJ2HSpU^)~>dJ|?wzN_03o9pNW>a=@RW3gVPb|ZbX{DN`gHvJK`=n9zQhvFq#63F<` zpD-eFU=VAKE01VTCG+Dq`>FefRf|%%Re++0R72=@75y?|mK#W^%77)j zKdftL1>b-bRB80L?pnu)G!SZq7ZF!Z0nxl&O|ooxjzZpe)fktU6dNm=vW7FWWCC&Z zMMZ}PB9$a)JU6;@ge~;hpJSBqxOSSusL##J-?t0$F4M@(p1ap5v$j*VMUO7I`#3=b z@XB^Ax+9~JJ#(PJxiu`Zwyiq;ajQjLZyn%Id6eu!OUC5^Y$?c}4u@aM6VO>$wAb8}bjjyGT)_Xc z0_^ypu;8u#mNV?Dy@{t$?AC;>@c@V#p-+>W1vJ_IrLg3}tw90sGBQh3V7W91g0LHg zmDB36CR~-iUIl>7ZWKzgxW_RRG=&)JOF)5dX}(aviDNt~hJseza-ULnhQDu1&gJQnk1h*4nQPW+~cqeUXAjPj_Zumqg5d;arTdfXMg?$itD3tfHAc*`9J9RKkt9AGk}Bb{|3+i z7A7v%|4#sY?W>(gJKTKor&q|26L7&(x{`+R@}pWc-OBBca^r4y>BVkbkJoK#ud7!|7@ka; z@3Y&*54mT-|nv7 z-yU|&Tg1!h>qRer_l1*ATGZ1`p*FvbYc*UYM0Nk~3lgZziuYLY;A2hz0Yx8{yJ z8wQYQnYW(l8-T{Ai?D*;YsyOumly zXD4Ng&(iOLR<*EW9C|totL}EqZ&A%wO+vK+m^|NBzAf8agyYEz!qv)Uc+SEWOpJjr z$=M0j%*^vap=?;aOHS;{F!3kaY4y(IealvmS~7`MX~NpB^x83|$zJ!=jZ)4T z$*#J-o3_63ECvS&VHLNAzO%gLN7YwLF+{%hN7Iw-RP}|;&S!eHCc3s6D<*2WQ(ynq zZF~&KDsIBViHzRa+4FRn1M^=@(=Ihvg>x5$J$uuSh9KAw>bHF_?GsaaI?@MgAZ?&2x_d^0=<+_Wuba^OcpaB0Wcx?JUV0co8EgQFaZilH<$Y){D? zsvkkNURmZcGAvzul8V+Y>*u^QLs%TlAYtjfF6EE_mXlH4w39CVO+VvYZrKN|PFH(e%# z`(WS{|Ce&|P~RxpvbAsbFeCT(eGv4|bGl4W|G%uJ51-DrK{rq3lZQ$1eWtT1KT0m+ z(f7ZBoP7Kx#N?zG5^RMNpM1H<@Gs+6gKKxgqI2BsH{r5)N>>Q$myO_crKBT3Ps40w zjlZAlpKb~a^5(z2H5hz_;tO>?Tr=Y)E%Tmq6JPF~$I}_ye2X`pm4jcCpSIZ-a&hyC zM?A?U?qVnY9a>EcIV`3_TUviugcnMlR$T1rEv_*(;5!hkw=<2ZsI|(8PaRH#Wgx7U zkbD7_LZ*iY0-O05+z^EM(5{~#Y1Fh-iY-N>82eKS9KE|+FucxW(DYXRN zc6;~D4%T7izOW0vhr^3Z%KDwp@^-#o*z03Co;cBX0$~z>iLMu1GT>}nRs=V|m4ADS zN4GF@wK5hJ)E!J2@wap9cyapqTiI1m`I4D$&at`j5@o6F{l)fOTHM8^%WihRsk-Ba zIP*LIR%#zJl=#=VdEJ*0Saej=OM{-{Dxmps(5{&$NqlofK3*SIV9jj%7Rau#ibFqz zUqd_M*2ek3?UH@~47b zf9D~ecnq)1n)K(q3EXw(V^I>!9j#xh-DQRzRbsk3pFI&8m9pS}E|DGce;NZI9^s;G2J=U5V;|1)0|N?b_R`cd_Hz zJur615kDVZ9mSnTEquh+i%KZp>*!bUH)48^n>RoIoA2ermE%wvqKbDjS6IE|m8A*i z2rhxz*?!)=yV$sfyNWle77t}VzOCfikm-9v-6w>>m}4_l;ZO7YVG|$rR0OUmYiwYu zPFq+42?pM--3j5^RdT5gp|$Wc0EDVJS}OhuVx^Ph;@{MG7)3Pf1FzBX;far=p| z=N`s<8(n`#N@Nq(p!s~T_~(;K1oL9Sb)G+#OQ~$qnxA*_x<68Q_Dps^=bMDf!nb*# zMy+Az#RLShckLD75q8Np&jrtWI#H$p#wz=0KI`g7dp%Xo$QY1^5wNA4;43cv8oWv+ zSQL=1tQx3t1<66{TD!HoS9yf5#n1t8z}$D24e>7uv(p|AGrWBTV^GiUy30*W;h)ce z74E*e&G)2WA&6m>%I(P}2#p(mDh9W0m!td(j%j0$BB-l~&~xkTw0^{@Zlz>ur$}^V z-i#_TmiX2Fzz1}$tG;hv!y10R)NQC3Rw+!n=@&a|{%p~^s{*4D*6Xyk_r-aPd<#NynxvwpNbJuhh8ORA>J7K$i7kHkC&Enqf~l+U7cbkK@TQW^!md#M03OXMBVK zMOnB(K{w?!MOoopqgf$5=&hWlCHO@p({;xiqd`s?WGYYL-^Btm%?MOjl%>;z1Q zU-Ub7-F}*L51g!s`?OcsP>DfTzP&Y}G9}~^s!cB;uUGW$RR-j|9r&d4JuDwV~n&A;Gc^rwSL!W&l#G&P@tY&?_GDn{y|UP z`tZ{<0+kyyEEvvnYwvNNY(3lF58_jz0X#y<$@D3i?$68iE4@@7#P))a z8#<&Ns0ftNay!97-+Pjfj;8e<4(j^lEGrlN`gPtOQ{+V^yQZ3=>^MAz1P!7br$=}} z#f!BK>~=x&$Mdi76E%-SZFo5@biECg7V>tQ;@8GbBlmut8CShupi*ANVv*qT`yPyL zmU<7x8GLdO>WuZi$&fvth3p%4I5P1K9`Ue$l11k5$NA}p4VQb{yQxq+i0t01tlCIi zbGxMC+z%8N$2x>JwAU>u^MxuW>6;q|bC>Qcls}4+W0%?);HfNJIRAb2Q?P_k9JMp8~xW(3=$M^pzU+nSBGVfIm84Os!k4ZG&)A0(=!etrrH5+2K^={w}i8I&C) z-GMsGh#{7fr}J@@A3NgMEn2BChB&mT$Or7QZ-N(~rYIx1vJ`WM?y6@Fg5Od+MH!P& zAWa;(WT=|+*f|)2&>3C{s$zxla?|eY<)Q1ye#&G^aiItbrk?I@S0AC$uX#06qf@-S zxV6#*P;M}N5zB_s4Z5DQ*-rjfyg?H-(!Zbh`H{u`g(-pJpAQocvBQ>sb^O3gFJT8N z?KUudzkG>VB{NDb@YLnWn5)I6kEQE-(+@f0NxttP>ECu0MmSN*mx`&?FWnAI=>gI3 zmnC(0PC+mM?${tBR5JL$%Lyr*Sh)T+Hk_^%xuc=0y7jE{Iecg2^iEdyC9sIeer87A zm$7*Rqm5KcwEi;SSX)%NvM1``exdZ$3J0y1nX1J$9F-4-biN!}nV@)vYl$lG!Ky1h;-9|3L5fJ?Jkoi_f4?Rd#C+GNgP-A}GpR+_%ziDPw_IRVJbWzD+)0OKXDmAVR zKpVZc)gtb=1$Kledc}|JnPlik-HW~5{EK^;58<)1`|-T*MJ3zS11fNbvVdbr9goJ{o;{mN|c+F&72z^-pm|+Pj64TAAypEH`A* zv>anKcBmDz{>kmXn=#Ar`uOns*6?F3&1SBG+>1$)u9h!3RVeAl6&ZD`qhVutq^pl6 za=trLT#}(1O+}gXoSW~C*JvRSgQ|B|fqqm?c4Q>Z3d_qXO1@SscV|0q{f-XIQ((+X zSd4y<-oIRb^jr4T6SP-7(|ZU!kM#=QcwIyu;g=ToC@-<5wfdtQP!peZXmYKe7RCbl zMQVN@Xyk0hNxSp&+P9O3^=N=(-cRm8Z$c#?LN)ND@ujI%IXdc1^V6)6A1~Fx-$W)U zu?(mxPmYh}Gn803c7E+pZ_;n9l*8p*u@=045Q$mGle5A1uL|Jt*SNU=pY4+V(ooUn zunfqrCLfa53pG_@J0|P9;pfAjIE6qX#ISV=Md?)WsA@v#E1u*Z*FPe3%2C&?I^?-* z>Ph`usk-%t@ML(d6(j^l^9#m(Dc|r&>?A=w=5aMy=U0f3F>rD&+z}^H=itr-C+X|! ztRdRmztvAWi)h5fjEPGoC`Sun@1*njTG!bgd15Q!Fjf6ub5VoM zdN1!rI^m$;JRH|4;ohk+j=bv8G^-_$d}s1;Q`ccJO{;#Ff-gl+aK{sp#Ctqd$ zNWOR+h~Rq`j-*S+k_vr6r}tTEDa2@HhYM}Eu1*HYPdUx*N?*`w7BiopU-zaVM4q)Z zd;Ee=wP4T-*I>-*5lfKQzp4vjc6?A9bUp1x81gDu8WAhp<@ANT8p7tj{0dPYiS;{N zLXtlXDSt{Fr>|_pN;PLyz?AuS(y3w**;#^?O)8D`rwuAd=(s>P)ZeM#!c`_+jEz!z zcQpa=I?U_(eqjI-<|;<>+wSFgEW_&s`{y^a%wra47LG>fW{8#Z5Tr6p=4C@AXW>l< zinQ2Dq~>^6^xL${YMuf2wyUhQC&!lyCm{;v*d&L!%!ng@wLwM+p95($_BnlYA&sMIw-%W94j=0iO)+Rm|2*U@mHu?)tYr2 zGFJ>a@eTy1SHn007ewSYJ%bDV<1s7?+86)Q2=5BBF~Xd?#}$P-+w9~ZZ>~I-l~zyF z15X40*03?w*$HQ;iM@CnVC%zOWSQ9r+#Jgg%X=LYHqT5@l^puu#P67FxEU0ffk@Mj zrH`d(y^JGUcQ`JU&BlYPaeaSKG8J|3he3(F^|*Ro zwHTN{8j7DaAl|n^M^(`Vw`oW0#ISBWl_|FvMjst1E;8~=7+L;-#KJuWp(B4 zxLtJJ+hT7X^-4DAh&UYw33 zd?)PTij#;tpcN*gWj=MD%q7;Ih!`Uf@RFR;)F603stYQW2#NV7A;OgojW9KE)Xeh+CA=jq|wHe{S^ z-ut*Hb)`Rbzs^vk(6MHL3ChCA;sgqVT11|TG2uieTat4O%OEL~X04$i#XfrDZ%T$e zLGq8tNMx4NbSn-m0A5No?q_bQ62(6d%i=?G?67HyGe}OvFMu^sm?;^m#i5aEO-vI< zTpkVp6d}0E$L%E}W$%#I@=myeL>@^pr>cVbqsIaW2o5oFbYn0jgJaGEdiFA*vAgQv{vEf{z!n7*=i7O;6ak}?mrz)!W~#JWI2$_V=nm+3{ntxU|f#~ zBtr#aoCC!_MxRCbxOADo<82=%LqD-^==k>N$#j9ERe6ZWbT|!VQ?EWnH2rKuUYD0F zB&bOWz(iVWmk;qGow-L&=#8|-G;%5@l4M?A;58o>(B~WGfq9{$zf#h zK@7s+(`O{XMBs$P-g(rzrmz5_qu~CUK(9G=b)!%M7BZuH9Z=o8hJ1bj7ctBhM7qsq zwHA0L110SUERYidk+Ytq?q#uBB2Z$ z$*>%BIh@8?ZrKLzRQIXDatKJ9CZ9+}FG+C#OhcM?FdljVl@{4BiDygI7gQLv>tSIA zTN{mh3hiV=I*blpD&O?N>0jQs2$ca`P{1~sV;9CEN@8*-4#l8~QOk}5rYeGHWxtW| zIF{gL(v0YU8PAdp z!$6x|q7jTdgq4&~soJ=ub>FN20yihFS;{O3x2zJ}3u**sRwQcSyx@>oz)#Eu#+HXA z7;=kZt$f3<-_R%E06*C$Ic(gIy7fY-F1H%#Dqh}auJPG$;sdKDSZ%G8tw_yIBf{+T z2R#ign>P7RhlvbMa?|Vthm~BG^TlJCb1{nN-b46Fm@Spc8thbQdcVUQaYGEuZN5T& zGA&(A!`U4+T%3H#Olpa~j3`uNazPX++aQdn(moM}bUB-@@z~Q8Ds-IayOr$iT3Vo_ z@r+Ml$G&WQ+?Q~48K?AQ#lI;>2^9aLAw!JV+GJzA73Z0yy ztpR??Ql9D_K%-E|VhK!lwwpa~z_p^gI5k?4o4fx=f;ci`g!?ayJZ*?rh||zEUS*p~ zne92%;&YLG@H5rm@eD(#(H_-6{u@z|*i=(aKeX|jD(CE+=13wwCovPL=>7|z{FJyJ># zz;#}8lOl&zS64rbcC(78MnGV8OO>WlI2g7jn^Lv^yGrF_8V#yor=z+k{}3+J_+Vrp zw^BFn7H%f9zx1^qty_l12#*d-2qfGlTjz!Zd>P+7MF#z%Li=Nu?#noDU)ZriG9*7R z`FOTtIuJc;)RDi!C_GpK6Nk2cItS}#U2M87{?8zu_ClB(1t1|1+G@K17v9Z8(IE7& z7?(|wFMJXDQlZ?Xh$RJBxwf&|$NXFplH!HVCEKzon3w|i)OIOZN8X^B$FS!InqHx#Z$AG7^Je>`Vpq;ez?L9k$Qh|_C14MDAf zLUr}8&{5mWMtFa1Yq7Dc;a`G4$A)I(y*{9rKS47v4HC&XY?r@naq7x{V`rL!%c}3r7PL>u_EMd6GmvtwlnR>91rE3gW@yA&rmcQ)Tu$@c~lu9Y!N_g9=P8sUPUsC z=gI)mUCjdm)PSP=`H^gB#$AlDiT3im#V?5h(*m?Nf&yER<)ttivo#T`CDy#qylE9` z!2&cJxr_@&{NcB-@|wfX60u8`ZmNG9UD=9ZR)Psr={?|V-0JinWtsVCXn~=e%ZE*_ z6P@J~kbsP=(v^ND3qb7qg77$FfksSva%VcovYb%_#9jKg$40D zyCw{`Y&G?+QA|CvI7?UP)Me(p6?=xe)ACc?$(Dpx`$o$AQ;?NvjANV~NPn+I+|q#( z7Oh=)t_L9&5i%!>tBM6=VEH5`H(Q?2rYbmqTOg27%=v_FzT@n|i4dQ5rt5Xt7%!Wt zAZESglifY$7Q(R4?_$GI)VGR`yCcA-6{S8ey(2ml*V@bVL@y(dUyfH1kcGa%#a+F} zN=0v6icoWxTY-q1ZP=r6L#yZDl4V(vuk^e$eu6+*gAt2|??h5WamsSKjil<_OkwWw zBIoPj#f~ntuO}yRZumfb_syMMG^*QxJzssf+AZv_=nS{V6Z6_n~$Klu~jgF zfgTV(&KZ^47GRp31(+)^t3yAr1IBiNda}8v)Y^j3g4G$hi_J-_&Nqz$6%_8$JnX{& z1fD4)EY2;!(CkqFs4T`cjw$9Vj!%-%RR|8XEci{G6+?`{Y%JBHp%XRIy%tn~JfkZF zn_A{Z?zAIawBhcYy0$2E{4N4ko3#E!(m7%Qtwf5%^Y)&V@!%)+jCpfdcN6MNCWrUL-wB&LzWy zqHBXGiPeNcGG2L3q1P8|0@0JoF=5BDpsKzigCxO{(7a5c*$0=!8OefiOvd|{Yob2~ zO%ZZUCVx2ISs!gKbLPRUm1E||N!>vc1`t0QzWcOfkEks^)>%}hzddS)R-}WdJt+;> z4?uk&iwQxefGDMeW~>WatzU})uwj%PKC_06RJ5D`noKh5D(Uwp(vm@qDOD5C>3u2J zOq-jGb83OAz_Bgg1^VM@_gE+pptCy*+D73>t-*^h_F2&-`^DebWwjs>Hf0+#6IP~{ zf0|g!jyxAw^>L;p85+pdG$YU!(o*TEH7NQsa@i=XN!2%35t^$Q9^E^{T%cMUT%sW_>eNHK;y`) z&$Rb~qPTIF6ur=bw2`XL!;+_YnZ-vuQ%9;;PoEn&9l#IZFqbSU4o624A+uy~y0#~nTa=Q6M^fhTl0 zC}XBw5Nq%jRl-KN_BS$2t$-hO%;{ntUG@j)?$sdz-1^5DY}Ua!I~6PtS*`;sdC^*6 zSKR`(8Njv%BfZm{FnwKglu*0`9Kcc27Yfh32+3=$+A5;p==+C za!k)lhd(h>Zo^4)pfAd9xGetJ7E$0ZU7DnSc2dYzQDrP#mi2W_G}(69cx=+<;9Z?Q zbVc>sUNYc_Sd-;&Lru&EGh>`xmkoDQF5M=x=D^>S-Fi{#>eg}Kx7?a!a9^C;4tC{e z-j)q~Z8+X;^5OvAm0fmE{Mdf-ua(PFSD2&cJ;S7SnZ)i0LEJ`(umgzJUKD8O%+nv8 zsrtA`3wyDVZ5t)pX-d`l6|0{uRED3Y&bUO`bR2VT4`ka#$k$?&X|pcTGhQqcyG|r~ zsmg9YlSz> zbJXuexD%Q-=JOD|0Y?n zYq#!YZqXOrtZlo-IDNVF=p^s`zS#34cWgat)6UtdHMvpeb*C-*TwcPYsr^*l*bcn$ zyLNAt>C#u+spY#>uXwH8@P5AC7JB!S_3i!X(XQOBy}a!One)}m`_&Wt#Si}ZgxOZd z^|}>s(N8(+QJeP#F8r(zdo7NgnIz117x7|Dlu3v<@4@2t6X&{s@JN7LuI7xTJBaW0 zMj?G(GZ3|?nyP0hgf~?b@7Dt;O7c_-(&f}B#K-6KXr|lPagZpobz5jV?TTcgx#LGP zh{xBkGX3#8RRdIrg(LCJvV~A)h&yDay9S_QfvhI6KL?X|49)GTazuO0NfrFlK{5)` z>5B|T)u>chWLEKy&>+S)tAs7Y4iyGNxLXbVz+1mU^kWUbBUKSrfBWNZN0@f~h^`la z3&Kw36t5yVJd#`N0$dLW^@`&q z5rm`)FShiF;G1$NKe3L-g%L8xaY{PN#@sPIiG8QYA*|gO1N)>=T?dO{% zmnTDUM;M6;l#I)oV34esv=>ymaMSU%NU;&P2D6ey5lvXl^s7?o$Hu7AA&$ zN!gm$rp1cAxPTNS9eE(SmF+r>Mq)`-B;s{ZU`1Qg1BVQ8{k2egEavXiwXQR+1Ke00 zdA`8Pc-j<2sSBv~cPq&v9C7q1`b4dWd9X7=dBF-z)e3J$pEx!^%Ue<%Mg~H|T^)@M zQZY@ekNjyP4b8xm8enuhBULN7FM~$j_4eGcQmadlPWT3DGjVehe(GRiWM(f9=E@`@ zSu(1fnFiP?H<5LyC_U7quZS|AT0Pz_rP;E9Pf=7ik)SHFG1^NY*gp}6C%+w)J|cX5 zf_*k3I;<^Pq{?Bu0HtZ}-|Cc1sVZy#qk~r+|Gv^2TMY=B!!0cGONko`QU|pFrH}n0 z4Ama}zi`gEl`MHF86xNzXx(xgs|3>Z+;A#1b73kb*bu1MZZ)Vnln8P*5m(`A->D~p zMihbN5STb95y7RPjcLf`9V{3rbP}CD2r&&*7$Rx9|5N{Z8OKRYA(NFBlZSt`p9xp zk{bcwda%SsTxonPhQ=}lyyFZ$6_=K_!u-Pk>hf&?DeXMIY{Cp(^d&(f`%$l?j{it< zA9R|2$%1SUoRplF()m`oT`*=J#ujiQ<=pWZPM5(Moy5lWYJnUH^aPU!q-wJYc=;YO zU9xC&ip z%gR`%G>MIE^r<6~3qc?in_!i9-VLKGqZKXqmyo(s*i)YtTG2po*P;2Xc!i=Dco;S_ z+7!w|H$+OCdW+ulHrc2M)rdxj)GGYAg}CluZ!$BBu5N51Wl{W?;tRA*(XZtBN%KoK zN(C86l2Bq=F72K2W7;ro5n35i7AY+;V>qz}<%?c`S{vxn>G_Bo#PHxX(7&1Q! z`p$cfV91*bM72EaUtnXVgozR7ei)DmDJbX;2FrbEgvFWZrcX5r2b+J9^r7C}s<3o1 zpaZK#tBQ2&<)WX^y?zFnX>Bh>I{rdTyv?;C2`rcRMzXBI5Sb|@>o5@+usn$_1_rem zvptLs1(&%GPB-}ar^v57^N<`~8p9dCgz0 zhhkbKBP!dU67O+doTBcV%{>z zvE@+prE;No857SeM)PGf7{)F(WGA&3TLXvLe+pkmN#)i%5mnwqkdhUo&FfdpJ4BHU zHgH#O7<4tj<&@S8cj#&b**B(scu{79@X#Oe5R}>HUDc}D?lNddW0sx#>|wESMrZ%X zEeH5mnRv^YftTQtybtG|kuJ~oAc{$F$fUcMG1qDrr+btI+%;c83xX!r&4lKnBOr1AYVTHlF?@D*S#i-*H+M&t%>T>p>n*J7loA<9+?K)`v`rM2(So^V= zuGc&y*lg2wQnsH`aKG5QE|;Dz!3@grvt0)M++6Kx9*Xv{80+$Q6zp_sGx>=9!R(L= z#OvvDdz=8~+2s;$uB@K`a=z|Ppg(R1+Z6kHJ2oF}otiA%ESl|JJ*H)vrg`#h+dhs* zX4(XI?5_;A901Kv-7UULJWGzwIqR-~*KM9YNwt>1UpT0g0zPT*2T=#-ro}#8%8j`d z=;g5O`P)HxvRSzt=TF0jEJW&jpg{PlP4FR(=3KrVT~Gfje!;xC;T`>)4%wM<8{UAv znges4w1z%H?>C=O5kyAjbfrsNbPb}alYog^=QP+TNx`pkx}#14?~YxqPW6es?M2hp z&cRiOdO7=(yS<^_9H2&=jTt?WPNexkW7vBufzJ36;3d>Kk9W@8#B*MO;)L{)>!{*( z6PVOq0UMtjHC6rrOhf&$H|R|ZfxIoeBw~h56SOl`_8)Q4-)|iGwD(%N>eT_Jv4=1C z_XvW1|Kyia9D8YsG3|MMoIH8?JL6XxPd$E4B8i;bRuwzd?w6apj6A0fmF?Hd-x7U2 zNhv(5o0fc{c-?h6J#!|adF|NkFfT-6Rsj=C35_S=g-UqgqYmWKefbYFvETZ|*2(1(&aP&G%QYFF4s! zTDwJKFK&jZ^|ESw>mTo`3Unh5^bM7I!jV&dvG*sebD7`buswB2_G@i<7S)zR@?Q#b z{U+yph=RP@eSJojwuPP4Ao)(fIX-KO!*N?ulX>D`17*^EA_Uko`1on>hdzbl)*sf9 zjb;vc0JBG$w#QrXu>#jO$n0#dKiZ5$1{K$iFQTbMXMrz;2?5Xn(3a>6E@6#Lrsi{V zG^Kxspp1c*1kY#g%P2I!Wp?Mzzh4E;2bDTgZy#+Oy6Nm}9?l)0_v15fwbPt0#~vz^ zi;>3oVibL+X?ZzICggQdiIdB3NbQ^!q?g$aVN$~6*J$(((Q;w1N zQ1<6bV}=Bg)bxQG5wVAv|0?Pf8GA>=qoM#dcuJ)$>Obo~vsXsAclh}P&xN>fx{7*x z_J@&{I%pratnXSHS8)SziM)kbCexg)!=>f5)B1O%9GbD2DuT-Tg2 zYw>I5pXWb(cQ=2NDPon#+lhTGrh7dXQt&GfCb)cm@^wBr>JH@3Z#2s&5ZTRmvnR1u z%Asp#zwxrxu|+Tf*1l0oOgs_+@u401X@!b)>Dta(VugYvsQO|6N_#l8C9k0$6N~l? z7(1y`e@NfR2M#&2KSth19D&TmcLEKD*;$Fw_8Kk~w#KJ@JXKwdyy)J`BPMsJ-CfSK7k7MoMg^6YHUO;)1A{FoWF!4yh;3a}xmWjMp!UkIb-rN^ zt0ne5GHnPW%M z_iXkuBAq_Zcoe%y*K!4w#JX0gdVskC%5Un6!KAHf**}>|NcDV za^3aIQ>Yg^erqOHNc&ia2<81`$>%LvZuRQcxvB&!;A!Vjds`0LfFN_^Kzp|P6H&)N z^L4e_p8bRJwF_64RPf5M*u&;lt|A_l|HHM9G0VY79{J}t$dRH-G|twF$auDl5>N=u z1@9CENZl>WJwl{{OY5^|>%Pmq@1*Cl+MoWsR{qUxxqWK03cXXz80_~H>F1y&_SUeV zO&v~_rMK~?U|v3#!uW=`p7C6GK-Qhf-Y0Wy34_>wh~4n~2ksLWD<|83%Y6dsY9$|V zCi!0*6#A{fxvdpN;orw_t&FXqUlGo^D$dM?fMDB(s}|$pnrI$wub-ym63A#Py0&cQ zhIlt9*)pLpqQvy28ghU$lj|OHlM~m>bC~{9nOV>}G)Ly7;e+bdZCe6ww}W(oD*}d>h<`$zMdU{ z)#dkxJLu0Vt~u<1)H{bPcP~vrg4H+RU6_i%Y@Z*|?Q?%ex%`K(QBH0Em7w5d{LEdx zLvO%ic{b8sAw!jAdFUpcfI>$s?x3EUDh}(nPZcYM2ZTS?$ASM|8d(sU&CtBH-6TxV zHpB3G-v`##!7eU7teUSig^RoRYtpB|&P=3c;}Jn_Ua=6K!kDfi?mcaIUA&QL&9b-)&}sQ{^Y!~x#xsP@5eIq-GOKQ6oAO4wQaWxI68-MgGfpmVx=sdGZaMH7_3FN6}Atn z*5sz4-}MN@GoE3-Uh)g!FROFkVcM1M0DuQ3t99?U1}$R6YAQiK;=z6}c?!6=fwj-e zYv)B1gzBojL4d$8bF+*{P^5bgplZy29QPCqbS81gZ)!H1fo^BmG>ZXVeH)oX&e~b# zTW`q~AL&DY*uM(52WbuGo%?L! zAL3dj8Yz`Bh_mGVF7t?=AhtMV2RmWuHgt<;%ouC$4kCv1ab&5gtPt6h3!lO#jw%rl zA0~$huP?~aiXzZAYrdpGoX&mQ6AoksRA-qDAjkCQr*nYCu0Ubsy=F&nnjj^1j|gmK z57588f>?;aX6ga!APIH`5FxtMcKK)N1N51o$9d0iLT>f%AmesQ8g#?9TOc2879+d} zNYWLJ6Rs^5K8VAS_jthYFB5`t?4}mrawhMkf+gDzGY8S4-PP6nn=iFpc8WS!I@Xr$ z={s)^zz^d9%^wk4B|jqUWPcaou=>3se%M&R$&YUer&~h4?ojqTs3DUE1DYx~-<;TS zLh!m_+_cloEUsKb#ryL;hFR^9U66|Y;eMKzt=8e*e{4i#&wxb#$U~DRQG?pH6DqWE zl7Ys%;Tn239uZ0?G0O@w8uC5fde@fF{SQ7qI|O)M{m$apI=7Rjw@K#ym%WH>o^6=#bmU(5$EzvuY zOCADf$8Ll6zjTb0ueA$zM$!&OgU#fEgkA%@42gpIy*mNOb$Ic;il}}Ef67(|cWBoD@p3Apgf325yLJ?smw={1#5O=Km0m>TgP*+<6StDJt zB({&E@NA&McbZ_4r`23)RwqS{BSAVo2>S zfKAf#L%dT!J3mc_?PRSV2{A*Tu98dY)^KBg=lj)b57(W)=6^8E72>a&VrklEDZ=BN zbd%xaisdos-!c4HuUp!%e9i{$h?MM>s1=!Yiflo<6k* zMKKEEv3Rpr^S0k({0VxIE8p=JvqlqG%QyX}s5t`)P7 z+0T9~tq*sZEm=@9JW_s>m~~;mG(C=S;kO;X)#@CqASjl+29Ra3oRZId6Tdo*q#k={ zlitGh^eRb&w2}wY(4y8P4_7bNdWK(2%8vy5c4y79yv3ZNz2L^6;D=pEg^&{BrlFXd zlN|$Tj`+Al{M6fkAPX{wtkIZZj2Vtx#B`&0h!!diUFrJC0e^I1zEr0O`Zn@FmHkSp z47;b}m-`2NC0)mO z+e{4^`bR?}3uU&V1RgYH$xCtGSMFG!d2F1D_=NYvCdE3Wcls5oqLOXwV_KPNBHEHl^TBJUvo<~^a4+6G8C`di%W;%>xVCN=`Ep@qm8 z)MJ|KT?5`9n6nyfvV7k@7EUp@#63kBXV&~T$AM`K&Kddk7nH~s^He2=o(!w)*D8>v z9XkRR<$|qi1Wmo{Q}Os+^zv_=HH-%m3u2K~OMSnFy3_{5R<=>7@>PIs=OHSow|kg9 zqTan@>!N<6O=-VtPe!xDKEq%B-5@ijSL<4~#C1$K_`uAFCW2>>x{)Ujs>C!DTq^tmHT+$w)|C z!WMtI#;N<6s#i>{A+bIMIPrlm4}ha+3+mu+y&q)@>P0QmP5YnvY5m@jr`~ogW*%xK z$Um*!diI*K+h(aZYPO7W6oCXOFP_Z1nNXY^Q>YFy9<`x@Sn>CzyI@Q}#T7KgE7p7m zLYjn&i`x?)HeFuKt4aR-;=jA2c-05>E~?!&>xj?qIB!&W=l2ZiZ`_2KD=P!$L&>3@ zTM%Dq=`4z36T6`)K-6}F%N_YA`iji~C5O(hf@@@d&x5D4Fju~)TUyes@Bk9OpLPpG z`E-5k%6b!V?N>x;2gnJ+bc*A?r}aIt;4=-Xst@JGYos6?&J6*Ht(ngyaf4x#H#2w~@tlGwN11UKu*map3`pU07wP(DW+EXwY@O{H+WXsm(=`!Ro=;dO| zopM@^KmJ6a=HLS{1qWOMP-m^Q=J9x(rMwAWO{u?GL_1nzc@u^FDicNGS_%3Ae z2~%qJ6q%HL1)oxQLFqXHy@h|(G*#B=Zsb|swbr*HeqlgV!hZ&Pm*6WN!)%>xfii5{?q0|-PM*= zudScTx((x#jL+H2U(?)<`qzHmREgVE=^ORN;1B+i>O&8tXd?bG&8TF zP@s)Zyk*ZyG8L`(DG2J?De#_d(~kfXKK5;nj@c2Y`myr3j7+Z?KFefSi}l^?sX-lc z1>tg#nVrO+%jD9+7LULdGU)dDXt$njUg9;tvuDd)ikW#Ew3&i#Gku^qRq-=Y0bs4_@tu7oEP+)@14CA0sy@MW^g=qG88%g-wwNW>~0+j9F3d zOx&lz%_0W=z297E)(ytr4M_86vp=l0VX9Qz|)XUR-&`(NvyRZp*k?)q&b|kEP^D`sbqh_YK%o%EGOua zng=z6VNB0jL~+4gTZS$&oH(&cBR)BFSwNYt(P*^V^8Zqjdqiwx??!O4Bmda4`TauS zMJ6y{61Aqzgmzwgok2)mM;QZgc>hfQqU;FuK|!Ht^OThlXt*$<8e~OrP|-LfO9$U` zOu~8@qx2|^cUjBngjB&pgVAwJr!}XWOZh^Y7b(i-XvgUSQ4TD~(BG4D*s0_yV}2IA zSfXbHmGZtONuxTl$v|tBez$3f6lM`u1s{}|eo~E46w=@peWSy&1rLChx>Qb32P5p5 zWQmNTwt`{s)}lsNCS;_7P$_4~G9qyp6>$Y?Dg8bXc}Oogl954}crLX$%*n>0u5d>} zo)WEsrY;MwDnp0X5bPc`4-Y1S2!HlHU%rw9{PuT?)d?SmB{W7+{LGti8E_FQ7SXK0t}5khRaBUDvVsTwXtp3t$W()fHfMNiLt<6aKY?F0 zLXFmHu1K{kRNzId2P6$jnWd|A2pl4Cn8xc7E^e&_*k1- zDcAb^(~WPvOS%z73#DX>78aG{7JIVJVT?_6?MNaZN_X+K;|e?(^x2QtLyG@Vf{(F2@suRTA;Q#YTa9!LUhAM?Q#Dc{!fw$>xDnK@;cyDZOuIEF zN|(}piKGk zHcTy7XRQz4CxaN|C{s5YAowQ)(RRsUK+M48Uf`U@&=Aycp9(21>Lv=%i4X2FjbO;) z2t9X7rxk3H?BxZiQ^P+{V6myo*^7j_>_e$)v348?SzYSB!_5^2M`nILS-3N*FA3EV zjF>DUBW;nRuS>f98OxdIJG+h`-FM(9Frs*E@rSvp3bFWRLBSyGX9^c~Bwy44m*K=l z)&t=EyYNjtg2P!HSuOX*3Hjefb@silP((v<`N1HV`3=6(d^nsk++bL03k+}pXjy$T(cpG^TMYD@x*7cTY8$trGr~>e@cs!VPklDu75qV{&^k=<8oeG_W zL|MwAaYAx8Lk}Be81eVlAlL$BGG5A_PJwlQrZHIiif3nSGH9eZ9lT(ljfpt6z~m-j zp~W*?Ivnq*6Hbidk*k~g23ovNn_k-miS_sVKVp8gk|a&BzaDfx-&Kx?D*LCudo7-E z5DKcE^VasibL5WmxwqD-;uy?a116)+cn)2aHO)|&CKhU%rsKz-y&iR)@-I5Q(`+`7 z^Jm%5=?=PIp6E|qb%A!!=?%^VpBqW zWx~1}>(Ok~qt%i*dEKj3QD`M$Yt-%@g6972{&ttLM2)ZOjC6n&NiQEFwL3U!+bwD1 zUwh$qIBS)(|C(RcYwKPO9@E=&Wef}EOjQu-kHS?3gB#A`w72B8+X#c_{62diBIA(t z9+Yr&+;Uz;>hD_uW!z=-aF@CPONXvQqbJvX`IT>b)$rQaJM~PaVkf@PiJQIw&{7&; zeEFw)*?KPRR%sa~yoBmj7^_dg`DNA<%RQkRb=U5ED(fOZGe3dDurtM<>#kc$b+dH6 zgxEoL=RmhNQ=6+Yf;<}d(4->yJ5poNYJZ9( zu@?Xd2|0abeo4=Adp@VMXci``I`*Y|qwFOrCu`uw$_U30zJWUBSthv^=$iyBQD&*N z${w$W$lu2kOu0E=Z3+ds+1fG>5-SIAKdcg?)9KPZ>o8P;*!KPTnpdZWW1QEcSn?hP zoe$%pS7_+DTZ#T0D{Y+RX$TC>l{=n>-X)*sr%XAg^!68}`e7uJmmMSCTOA{~XGn-w z_sqwb32B(`swh&(j9KSlSIjj7TFk7fE|Hd)?3%81VKxv|3hue>PSpGw4VmLbi5@mb zOsXBMio<-HKmB<6jHmXZ=7Z;!q+bXGFZ@TYN5l>P|=G=Sr0ah#>lO zD&DV@Zckii`xjXkYT`|XQj4rZ{}A*H#+Vx*ePBJg?s0pp^kLf0WAi;mu^2-=Xia_S z8Sv!(YOO4K(`>G#`(|;VsTd=8_TtM)#VSEo{Yh}2hx`=hVKZ+bm> z9;FP>W?=aubHE$?DrLB65+fqeB78)>#7s41(I}_Xc<*v*5EFGQw?kj=*;=l7v6f1hxm-$?*^S>RABzZ zIj&szZ@NKuk|xjPh|(4p&_hDmzQ{klNqRHqkxjK0I$7$-zViNug#j`(f+Qlq-{WX) ztzLV5mHa_}=WHR7k%_GkqpMey%l*B8x2)yqotQA@X%)6Q>;=m+8-&3sZyBf97&+_G zt~cnKJK?O~bk?8GJjG}}r?oNdRN}dZ_@Qd;vrITi_tOBUR3h~7{S;cQRt%e)Qc+E= z&)Cq;Z}SkEWP@8ECP!}cyY#qQIRtaPO$>o^WSsP7)DN#=4pSd?)tLK|1zft5%#DcZ zqiIkp!fE_p@TQ*m@(E`Q!>OVs;xw?E^_WW_aeldq1{z;L@6@qL5fO3AiWd5vT2gL# zbOsilk7Nqsc&b zf+%Nhl=98b8^`BIGj#Vw!E>m{rEONGzszL-_)*Wz&i|DX|2Mx+)yvV0gjvzpTE*24 zfmwlsm4)U1SI9WKxRP+Q{rI1P88%k#|5|A5(Xw;cl)(6`GidDhlNNC8yk>?i4*a%i z6$pjA-_ijXrZJ6bTpllqFJijt>$9M7vf4Huj>s#A<7|QX{E@~ zz4K80P#I-A%RsQ?TY}LWa7~btHt!y}o|a)%PFqJ49?cQ?mbn^dHPS6vtX1GZh=>(y zp={~PFz?-xKqYE8DpZ0%o>ZP{OjQ8g6D5b|*cfOUG;G}>%!25NqA5g1DE9N_4(+m^ z*>i^AS-emZ<=vQN8k!{ineax6tA0Khr$k0;ibSN6CNP5*`53K`Lr%B~uNxN8ER}Eo zZ8B;cOp7Owyq5nyZuf+gBG`Z>Pn=C#2+^MvA|?Jk> zF^X33Lrlt31YeqPrX>uwbXa3$-zBPkd>TW2VL%1;n2gX!X>EBuak$@7cpI~!VN~&u zt0`X<4@LD)BMWEdH|-2KKZ-3A=yi-(a*jh2?|AIRVRWHl(UhCCu=V+ST;qTUvyom%+^);jT+=Ckhl83k#U}C}P-@p)02FpoOgo1Y=M45N> zlnHo$UPR$=#MOkx6Nrl`or;~aoW$Zy1V-L?(qAYk{b~G{yJciz) zKqt^AIi~D}=cK@3)yqaL-0MjyPX3mr*KnzJTWGkB>(e$(y%MSd~mu>4I=#f zpe7wg^o}`WEgZx4)?F$tkra;q&X0AUArLU>?Uvf{bo&)WM(jnkkuLDe~VjOjPZS_w_t*FoY8?d?jUN+UbY?m{APpC}= z_DUOwDs7ur*`HBQ6PzlNM8!kpQOClg*X&voBulfH~FO>L50oMcGa&1JWrOW^QZx?-sGcBwhD zYnv_IxU26P@M~zu^Lo@-qs zWR_1Jqlv=3G`zF#cxC!7J+xE3kHGLAkSztuE6$*fr|gp$h}W&a`kgTD0?}ML(%ufk z!7=&aZ}kv5o2jW7yBb%HtGu{4CJVjbC;War)d31-dw*<#$kqJe#Mq7N6urZu{&VU4@Q-)BbxS#58kWRn|P+{wMu#F3U+>O?W@{%iJYs!Jr?UP zHC+@))YQe7L-`&&Q`@oicfTH+G*w@Uw2hlDqnuNE=F&&1at^0nhfDk)IP+&mnSc8{ z{@M+T&Cp(}gdz8eOec4w8-RGzYNis@V?c0x*StQJ=CwOrt=(xcS{ik$9xhAGR(IHV zcuFru`J44c1JKk{dw8C)y){eep_#POUx6Djdvtl^^@#$<(N9uHMi^~KTsqdh=qD%K z) ztv53^MLq~$ft+J|8#-Rf8RD7Nym@5moBkz`yM?9pizrxXB_wnlo&+3%v;*1Iwa^nT z&MJzdSf07$y|4PV3x|LhHPYEpz6CxKAvpVW+=iG$LlO#NAXp(9iR%Hr>_o)6W6}v^ z%p5n@e4&=<;#nbK5(hg9Hx%+T%{_HiF)KA)8@clHD(6~!NhOL17BQG}zFR#QP?5o! zhFFe1jib}EDSGb7U9-ry7{p}Gnfo0doNm6O|AR)~vJrH>3L9 z%aJfzj;|yAg z0yMSITzqh=q+vH!DNiF^HfFh zcFiFZ&Wy>=1WBc6b3jTzkz@-O05XuDT)GzK<#R<~*ETpIYs5CVAbA^5vxCN7OYwOk zX{=1iBNS%vc^=GU$zhX|_&SM%!a3@2(Ly9p^sF)@P;!qpL$m5^@$7@_A@BVcv(*b?=RT7vPK zUs`g)?WK{Aasfl6)fK@i;Y0AFH#Ybc%l=WzApLIy+pH1Ob(a0Ml|^q0z__V@E3GE zTRYL-jOCgZqwpI<;iacB9PsOJtm4!70#SzV2Ocr7uWr-4dmU2JW!h9O=-80F9NEJa zzm~jeS?X!7BK57=@JDY}yY#$+eDG#do^2_ zqJlO2x>b+OI8&3sPG8xV1(%vz`@2ONuKg*66|6vO46x@z|{)@q${H$4@f%9pBJCKE^N$K;*?d`&Y0dAaL#z# z9kNUob1uDX)$R1nuON-%%bkGd&uRz%ggIg2KD;&AkOaB zp{KrVd##>r&5Qll0HQjgYpy;owV~Hr#|PJrKA+ZG+P9wQjf9(`%G=RIWD-e2t}7_lPmV`YU1-4T{5u?6bxNVE;QzH)f#zPDe!0`vi6^j$Z{{1O5VUY+L3Pv!Eih&L(x`S^O4 z6dj%bl0ARVoIE;yPWryTj;Hsm7M_LyG~9t#{+?S7aa<7>L%_`iobB5>)Uoo9gHKOx zXO}^5504pC&zmLTyjN<0J>h_Eo-Lxh`;%%^4X(F?$7QOrj?YiE+l#kHQG*};Uf$Hv zPFwN%Uy|z$9{@swyCHCw(VV2_4DdWUIr60q!M^|lgMf}rp!40!ffe9EGS0w$gzSAX=lS!;*LJmyG6Z|5?=^XRX4z|fD`Zo(b^!;q_o-~*r8 zb9BTHbB|K?Kg1{=ujBNWOgjL8xANf9M~mb?R5RY+VTG&Lf}F0O0;drCM0qH0#TWs; zW8xEk34D9D`#J-2|T$YG#KCiN5$Kn+Mfc7^4DY*b)sE?@j&enB@ZD;7i z({)E4g$T(r=@U@--0Ii9@t9|`wCznT%8o(~dmG|k_OE5gQ^S)_!0xIsEPmAo zDBrmo@b%hv7I85a;yeJn$F!fZgicMofwLW05;myE{O~QVc7t9&oAc!4*Z+8M^cJY4 zHD|345M|A-R7Py`erGSc>FrSnTXPlz1?PU=9e)6q!+`0MUXj$EM&)f`ZOE6ZovGPZd)l*Br-KRv~OzQW(K6^+OdVu#K&GX)uBh7^;XWIku zeH!&zM0v4)9=-s567|=)V7%w8V>PN0tbFlq@;S9{Se!34gmV*Q3DD!H88V(0exro- zsTE_JZ?J8W8Xer9+c;Oi=zR6>!txh;?P3St1g8C^CDtsptuqz|VYq#YVf@FzFW1<%O5PYBeCzC1zRT*b z#}AEw&&jsfHpe_iuNXJ{7(G3p|HCV)xm)Z`gSn2u)VXBmUw5;a6{?7MuIlUFS9QO} zkp^2P9oPT{iRK?X@+OFOwnm(74G(JwxH{Rx%ZA?NuR=+3^DeZAJBG>N+dVhq+kYJ_N7yCpbvF02ipdCcg+?6HzW5-LddK*hfg!nBdIw z-{#YY_NOkHVdl0p588QFORw**ML%V>nV&B_31Xn*Sq z?AqKuiw-sc3mem`UekwHITJqjH!@b7%M8RuJ%Izv*}-xvATecVe55!72T?1^y;wMD7U)oMp<||b-wdANn`|y zlwR@o5`%iwu?YN7kM{f2zge@`6Z_|mV(jG#=9v?7N7Eq zD-NiFEN>)B`~rOYcgM#!)DIt7-A8r7d+P8GxBQ?lnhzEx9yJc7Nn^ zmd|0Jh9xXzBqI9RgUxPHX&S~KtpA`w+sc0#A0eG4IMrb&xRzAx6$I@S`g*ne0783Ot68GP0VOV$HSY%ZZ9#scNf!M z$ZnAVU!oOmTT*Kv^}9N3E2Q9#lc^aI?~^f)E{F0eE${5tZuTu}8FR#<<{= zv=@bYS2{H3wnT?)Grdrajbqgu{sXOyN5B`HVVdxK86&w%bV@Z`q(QdW0$nJxJ0JZJ z3zrl{??s-b5ea7{!bFc$8)}_`Wdt`-l*dC^AB~RDK}l{5gTCjIR(rHn1!-Wc)%_z^ zU4GJ2eoNvMeBGWwF5Gk9UV*KRp1vate}J$tYU|_s%7!EpcH1oewz8E(kGLPAr8H&V#z&tA?y-6z4t=#orl7BdMTE#aT4N=D-Ay zI@I>LT}bJne_DlllS$`vVUwM%BQ){;Q&N(09;pQgiD*%oG^y({6-Rv)*M4<43*_Xfp?*nhq2P&NKbNZsI~_I zN6N{dx@g-964_26xPplf>@gw7a%c!3fjLP5Dns;%bx6yZ?*e&?kiz%$B7d{_aucE> zDa74i3ExY1A|niARtPTc&O+}*8uCGfp?qsr$ECuTP}o9zHW{r%4pjJ^HwiB`eAzXO z8;_cT09y)9yG(C&)PbEqPON9ul0}O-ZATRsBX(o*uq%ni>hv9~qjMr~h!J{(&gN)X zJa9Z1XJF^`3MsD89s(VZeLoqT8YGuw=@@S_oTRKkAB|c^o2)C|m=OLXZWI?hj}%u% zjgDuq+NAkgBNfo-Lnkm2WJ2=209QqoPAG)MP>+#+*1x@cniT~)LWFh-V3g7jyM{0| zqe8@h{w{()xio=LOFt}Tb9H1Now_0^a&zlCr%5nrK&}5gtBesf0&TVz+&S>dgTWd= zzNtcDm}m@cd@le`tZFN3j)5SCN{fZZPK$dDHA7i#3ugQy1p#f~T)!UbVjz41IBIHE zdw%PAPK~HP`by6lXw(rza;$20oce-w1?4VqEK#V2NHHQbK!Gk%pGDUmF62axUM=&B zWML?JG2QTqVEJ1li*VP^R4gQ&xl4seeIhGl9MqsP>SaS#+j6WmgKpaSq^f64NW%QW zc*a__=FSyvn5HenJ+oBevX=f8aiWAq2M{=#H*9CawCP9`jIbyLlyOXJEZw36OZ0ck ze0+33YLh_nXgzXjrG6ITRM-qiS$i;MBJlLLofsSxlrGV^Eo-j|I)Pc?Mn;NVO<2?D zPt%HBU!WIaAKu2;1fT-!9v+J`;o|yCAjO^XUL{KIN6@)-x!QvLvr>O+mw%H`BE|_% z!lTqBZ*9AoIZOL8xd{xfc!_s0-mo&0VgFHOWY`9b_J+b0e&xXMBh%V|a#c6|;7?oC zQ*cp(-+v-w(djsp1JTplM@C<0;B)G1YbTv2L2yDsO6%7?4tf6$TeJ zr<9TlURBlp8o~C9&5l#>ZACpoIdZt>eyh4|`;{qFjsW@nqRt+#GBwF|^E z!_;-s)>ji4Z=7ah=Ts%T#}E`>z!LV`Y`)S9=37xW-v^4D1j8jD(JSc9<&uJi5a>wb z@DJb21j&cvm3l@7yF)f?c$hhw{RzfG8Piq6g;`~_Dne(p%nDhtf-xn=nA#Tl2U7-L zt*Bx^AQA7+LhDUBSy@KdsYo*Ov(62FBT>oV%s+`vBgQSNM8sY1>JMF8?jKcr>i*U# z$_Ypr*Vw*OC_LWolGn9oL;>RlIsJqUB!*-%rCf=fQunz*{7}zvpsC7c6!eN-WoI*b zj+iHwdLDf1m4xb!`r|!Rq;^!5p(67^qRL&cd2ui>kaJ=RUWrQC)tyI+g*UY5b)+kB z&gL|RkFl`WipvhZ{0JtS*My^m+YWh+Lo~nhh%=Z}?}mrbBL`gCivA~SL^(c<(Ium&ID{R94i^3@P3oo#ia6S!!T}d zkO6{a!K+OS-vk@vci$$&MpS~nW?5)-%e%}l9hz@qaE+QiWu9ncP#O76s>(qp%A66U zBBCi^rI`oHFG1RN(NGG%*_^u?YgbBC$a&~msO4U{l*jJnGhE$D^eh!trR&*)1DUha zIl)+=G6wV4qoovwv;i!PaAdyLRo+d)BrBF(J!g*yh0vjsVWnoGjz-E{cGeKW!CIV7 zRt)Hlj(S^*4Z#TCOt%){sPme_Icp{isb3d589}C-xOpjMpVZ7?d+?^MtiVIoK!kuf zK&eY2O~=SdDKP|H*>Xpn4?~m;Pwr2y7{>ytrA2gz?~@ExP2%;uo%n(s0w%DPyxXY5s^8Nmo}4 ztni@nzbXOoIFiv_j`9^zxq0OpBiv}V=NZPj%p!F(**TGi8%`Cl=Bhwil-aGl&v7Ew zgOCx>)Eb_cMhasY2?0k2NE>^PH}YBjVIOn#Zhc6%)xR5$lCIQA?W2x69EXS$W@`j2%+%R0Zai%yKYn#|?io@iz&5OA9?~*kfOBut& z$bb*B2{IfShQCgk5t|Ajy|gZp#)tmXF1{4| z{7#8TP#%NCwo!d~oD}HCqf8hv6f0QSk8%`Vo2|YJDNPit$7%SW33&|Jg_ywDFF zrm90)BN;LrZU*IQ3iDQe6%jHV|C*(2yIrQdLNMeqE5@aqe(1vFjTLlc|GJ}?sO52#9ZSv7H*u5S zNrkppU{?LmZCAkx;KL+FTJ((F3#UtRv1#*~1JQHb=;2A0RwJzStY|olSIAmGs+~%R zez)Xz=N4o>l~nj&JU>u&p~*m<{5ZKTKRhZ;%}G!T5|(}Wrv5(;8hB-NF~ z6vq+Q9jXRSS)h33u#wV3u5T_`)p(Zdl9^K?VV>^l?a}l9cF!r185u6Hi$d9tCWI zUbqCy@Cfb^lY#}{g)=fM&qFr8Y#ah(_=KKtNRtxO)&r%jJ4Jg>UrxR=++3*mIRwT$ z)_8fc35$#)m)V1)v$rQ#-$fpQH2i{n1Vo>S)8P@;fA?OGVJ+ti)Gv3PKE5mh0^)>u z9SI626BTbqsJ~8bgN@0Cx^9j|B1XCZrg~p24XIdKn(-CPBk4FLxrxpX6HO|XT59aI zf>;=}adi5liwvWpna*-^-2m4573}oqSlYnx6_KN`3+EjiW6KrRa0);EG&v>ef=ZP~xH$)kGa%`p7?Y0#yY6ZOmCs*tfUrl38F3eqb^LB5JhHx0 zx3-|lRW=(yE~kFp0J!M8JIAuX4#4WaS$L&_o^_`X+o4(4GU{_#*(}P*Z<`D}C8)Qj z_BoTbRo@BTuu*?Lp3*!zfKX@QJZ=P>A8;`oJnOTxe~2XmYI81aCWh5vd@h=L)iw&s zJ=@cV+_#Vp%zsk@8-(shw@Y6XhHm#Zprz5Y?$pJy@%dsgZ>=Jh*3bfkBZG*lJY;v`}Qx&m|X%rP-8+_oIieP@}ippj8?6UOQXQ zo1=<*eZscwz=-#%&@rDKwdr2K)U(m^)bGBv@i^DNOTTX6?P4>sGxOny?mv%=9g+?=sdqdzUG z(Cx__zZUHK3QBzUmdLFg$az_Df8=;j<=}tMHECG3Rh)7$Z#Pz-fW=4fR^o3G(Lzj? zJA0;0bSXsXp7XMSNt?HY50DY`n|NgB z#^IJwa2sh=r2ZNUrOil^E~Da>ka@(|63CYMDKB*nq%SZ4JQ{I?f3m-KARs`uJ*eRK z6OSK`!R{PI@{Nfz6!Ej-bpV|xL17HBh4rg(-(*1W%~b@_aOorN=Su`MK)_-DcMMh& z+*1{s?JK4KcY5jP=A+W&jh&f#7XUGTcl+=0`bJ^cwn#VtJ`sShuV$2~Eo0-DQd ztr4`*Sv0X1fK&^o0V@PV*xh4)hxA5=6fYVWbh&dK0yB=MTUZoGgI<03!UaMc@*M|N z4Mljm7(fK0)fai#Y$nE< z&U&1I1o&tnVRf}z+3OE{eC!Ca50-q$4GUvAAb+X{A(Y(J_Q1?$gd2<}v$t`{P1t8WzMc*@ zUy;b}TlIqhnYAcUuoJn5Uj@`(5>~-b;u&*=@dXp15a3_OhrZe~{u$%kCca=W_R<%V zI!+UeYRwI?|A5Xt@#N@_mq-Cmpg>IsAMoJIHbD3@7-}eIcOe)z88h5h8a++!No_uB zj5j2c*HmYcHgsM*;Io3o;E&;$oz^`&b{Q`k`)8aQmaaSlFm|VBbO~O6aa?te)J3<) zKHd)sdg?ZDn{AN{Wt;^{FxqsP@LG;3mc_i5_gUjy@9u?Q=;YzRO&a>3)Efj07q)p|$^-V#2h{uhOwxy`>H_gL z5v$~!;&H!h?6i2uJBac@K&X5v&=(n2%CuLyv)1mk_>&+($Ga{j@3Fo!@bK^?|Aa0@ za&a4cI2`Lh_XhDBDVw0}9aKu}nr!2M^@HZ``4@MAAD&a(3t1fUnm)|6xfUv)kxt~9 z4opp@LHO4PE}IG*D?ME(TtTjNR7Hx$dp;bs9y zmaX^v`G3W@=(hqeavTFM4!rFNX*Fq)mMV=}R$Q5P18Kuys>vG0)~9o|y7N35)ZtQn zxx^o}V{WT$-ttIMU=^c_v}B&;dd<1EUA8t2oeUhF#2<^%;xHp)Q^AHEajP9PH_dX? ze~696w0jgUR#meVso`q-V0g7>MW=}BFp8x_l9?-)oxJPD>%#32)TCJtY%6vHb86OD z=C`96Xms7((Ir>xtifZ`>cog;f|?(tViIXE7c7GuTkC%6n+pTo#^@Z)vouK6?~0c- zbWmH%o0>f&Z0bX(dG4orwH=eST>L#8PlObXH)%lt#r=^|(Mz5j`o0094K`<0a+6&=nCKecvCY@f@ z{|x0YA{WH3!7G(ZlUuETu9%;lM48Hvhla~L|IlU8K!|CkEKdXaqH2X3l+D(3Mp`;H z^AzQpRgk;ma+{AL)?xgF$LTmCOZD`Sfm?>bBOa>%vpEtwX_Oi%itDg>& z2b0;Uu(+SZirIJ-DoJ9(2IYR*Ve?eb}CjbPgTKoEQf zN?ap2@aJ+geHY=)2p%F8V<@HS1}4QORUjFK&z(sy+{`?5w)Qv6)`h>O$T8<~kf|O& zb>zxNaQq=^%+k+!f|?~Nu^4|;mtc3RsAO5Xd%7}Y8uKQW%oA~ZX}K&o6=xXOekN7; z6{r)*56Eprub=sL73)(+9>f3wSm%9H$|7skzP6@qQFoSkKoZ7L@=?TW{UvZZ<~6yk z2VwG|N-!d+n@NA@qOx33{Oozco7EzN7?PKGNle*xq0yB$tI)oRlOfBpd6_AK4O>cN z;Y>?{ld6?V*sZE5oB!D=*WHQ+K84)mm($%;@%DC}KA2he-q5L&rW?b)7`mXD!|Hd$lyRwPKOn&p+gP(HIoO8qUFu2s} zn9wGrZ3PiNM(OalJi9JH*}MExQ0jf6jc67_1mT4>N8@Lo>FwP zD8wTMLFcHX%djt&GuI=8R#Lsah$?IK$pL1pZZ-Qal`H0DnN{_*p@wNV-X^fe{pMLh zfep^qwpB_#5=A|>K0r1ITK+OL?Yr*Qsq#?ycJI+h&-gV1pD{Ac}fMOmaaUFOZj|F8MGpG;H9n zmGTNwazSt{EX~4l#PxEcvZZ)P>m5)1j41hF|So>Y~`Urh`>H1qJcVq^AGk8yeSn z<~fo#Kj0)&{n=vcU-Eg>9W~B>z|oEL&sD?QyzAWRd>E7-HUy_=j<0fha@X_4M^0vEl?p0p0-^u`wcgt-;B~T zns@)Z7%wXeIajzxTOq55V$Kx-3O9l^dggz@M|lSAI|*uD-p062u)?=gZl*tD%-U6O zvD`QJVf0YR;am%=QB6DZdzCojI+9B5pr#)L>spY<@e}~S2!GhjEJ}qWu@B_hF*_1V zg`O_8BG%lfs2RVCdtT5xvR(C60j5Lu*GTgCx!qt#2lkIzP;wY1pIHB?Th5&rht#l4 zOKeQ6hGrVuPm)CC@yQ)GY9;yIsj~FRa$YF`BY*S?zX@3*TTIm+oLK%FUuQs3 z2orO6;w0gc-YUWZr3yqM&oB=c+am#`lq*(Wg_4)gCQ2v4rIQk4`-hVRc#(Dme7dIY zRm&-_lkGT2K*#f`L_Rr6PdHacHNr^+`>=C_>L-PZzi5^z^75g%25A)ABxR+X^w5m1 zPF+xq2o^u+^0+?jrHSSuxKKmSsYJiCNkHNTQyyog`t- zPwXkAj771F>$~Q*SkO;;5msr^hk?v}D&%$#U^909>PaHYMo1Aen%%5}z%6P5@6l_n zY?LHT`b;as@KTt2^Nx0&uWwJ3t}x@+O*YrIiOKjtx6%(uQarw*b_&OAh6Yqx40eM^ zu%Ugxx3y^oLEW@Vk49`pDiUZ#wRino)dXdukmwxqUZ`N~gP<5KC|`DmCsd0{w;ePD zV!>hui&Zu*YB$9p<6~69%G}N&Eb>AdUu6(qR7pd(gZ|;RiwCW1JA;g7DbeMT`CGFN z9^IS~Kd_Ip2PxhB0wF@R91RB3Z0Q&DA=%f2OG6ITpW(pA0N9Pf!v@2e=rDIM?wlkw z%Gd@IQakH~<;m1Y0B6n&`%ZIf)ysmMV$SnGu^X9%f?+H7i%JGwGk0SA6ViKZ>p;Y~ zXf9D~WrZ1Pz|h#k`qwG!mMduH2b1^%B1%oWg4?pJ-QRP{DSmNL)A(j>h3G>D=StOf z{p`<)X$Bc?77jc@!i(szSu6e*%(hyY3rk16S#Sb4&rQ@B3B4aq76Fn)3=#CW`{bx% z*??-SxecpDbR?F=Q;E!KhJ=+_@e4|C(rTGJ_lvYkU5L&_KxC%)Vd%IxGe$;?+k4S- zY74=bBqbG_m}o&MnuJ!&e)G$Ai2Be0%Po)|n$A{Fu!+=!rzWedrL~bn5@DS7qt}Ho{gEfh|lyREiH=hc(4-nl$ZVySqiN4Mc}9SL5vC? z9*%lg%C}J{u4=dCjHRS|?8BUJvkCL)lTMQ>YR3XVKrrmKiObMp)7$oeB zf2n+^6KAy#a<&JX1_xL5E@NKkDK~REWi4wo3!;hTsPss?p*XXNb5rZJY+T)0C&3 zn^3b`J+j4w_S1-XRU{VXUrkZmWEjhGDoEk5{~kuqAP>F-ih7|Y)NJi^Rnrq{V;MTr zU^eYxY9yPY0~0gpW}rWsAFecizaBI5e=w18a#@7R$t-|_15?BO@l6ZX>0R}FBSe8Z zaKoxA?S~~NW5uIVl60>;p*R-44>va;%G0SQuB<5qB%AAxTCiz>Fh)84Ru4wz?YEH3 z7)H0DZv51CGOw_4vB5Yi;iYUNv%rFH(DuR)qaFJe)7=#cbrXw9;d=+^9cP(0aQ!uysszTG32^l+gnXn1j7ivlJsm1EbV)g1nuflND z{_mm$Gc_Embs z3XP`!tYxLdG9>7@eX02n4@i00Ir6&Qc-INvDOcb0*zy$|@bl1i2v?4u7>jw%uVk$2 zNGX3EaPDgv3@po7>oIs*YLzJ%I9~KFIQi75MaNi3pQ;_@}BixVm8?=oO`bJa#vP|qlmrbZvtfaCk>d))jgE7dpZloQ@@z?o}OFU zbP!Htd$*EpbnZ1@>-=Tv-E#`&lIFK+ZyKh=Q$tmhsO*j~szlS}Ka z=0kok;HVfB5$EE`i8cFk#+|P+P;KxOHFoQy|0PiH{r)&>zIW2bEoZyA+_U1Gm%cK* zr(X@_zw7N+w9~yNC@9k@`laCs)y%eDD*s{E#51v04q&X>cQ?y!Sr ztA!THWq0`cxv}~vIocRl?!0a88P#L!VUFhs#}F-Qj(dzR+F1(Ye20y(GXc^WO3c`` z_2SS#NDk2aUau(mZQu29_}8!-;@I*Vv{&x_a0=Q7JaSHLdp_>JtuSb>$D%fic z`1BRU4Kjvql*0Y>wY0+pEvorZ85Yuvw!Cch zI?P{2pEKkqYm)$EL!fsCEbz`drvSIPek{9qFEU?{zkSE~83eBi3RfnC9De07as~G> zxOKff%Z$(pZ^^>0p~#5t%W9pPoKl&99+-_R!Lu^=r`c;1t$mRY)EJtt;y$ZoLs_a- zFP|mP@P*VEKUGD#1;Y$Zv3wHY)3X(|~($uv+{;xsz-EXC`{5;;nuNm)-`2C}vOTc$YafWB(M?}xg^}&T_ zkycl204Ej_>;x+8963u0$lg65g2;FQDY zl#I)t&gDD;MiyOFxY7is?J8b=I$T|1tlExz`o+o$|B=#$-{Q8|khWehV8tn{LXv(y z)YI|qDScx%jl*qf6pu`}!>{K!u&dIGi%)axDI(a57KPLMR6GSVH&IHXMD{l94}!lf4HM?r7}#zMF}N_(n^yq+Ql_K z6wdZ&B$W67gY&}^reTV7Pr4w(XGV}lsLp5D99Djte(nLEv9=yAJK<}rtA%E>A3VX5 z&-WQ<@e6Q({;V&RN3fdn3s{V6$zFrIhqv4%+bZLP$D#E1y1!5NS`02pm_PS6e{X1iS!);`P z|9l>{c3flD19SlJZEy>cetpEH2@1ky zum1%yCK1p{B~b|k@NMU!@zyIhiu!tbQsVhvX}}tQbt+vyq~8L6Pk2P+`1$PLSg{Vf z@!{?HW~Tm=S~2yaCB`Ut+VhC$xMhRhU)u?>wk#oWg&c#2g+IJ<;lGBh%4d0M9+C01 zLo1W9%EG{ZxOERbDd4d81Yhy}b^j%Efy;$hnV!dex=;Qc6kJT6k^t~*d13sF>So$~}6S0aGdnJz$ilbx za=(3Pj=L3L+t-qUHDTAR>il{Bi7dPq245!;k-iM*vZ-GTLvF6u4=Oz+g?m)=;u_|` zA;z5^Zqf8|LK(4d32VQaK!~9x1#&g3XZS4=ZTr}-EPZpm_j*}RA?>3fB)Uxa1l*s@ zD~1bm?QRP`4Nf~_{$}*k=WJ|^60Hw0={GnG7zvoJ6{AfKol6ja+YkX>4xvR`aJ$Dq z`r@nfwUN{(GRBgE{k+6C)qd$H33~+3#eTUA3N8`Md}x6+bl^Gj5a=f;UIX|u4{V}D zv0pQ|wkaS8-XGVV9vK7~ItugcfO)Z8VJ|2jb{@WNNo&3uP@xzx&Pz!eGc{nO0gpMZ zEBxxtN2RLDwSw=PzPrP*vwK*H#|KhJ#%3&+2`poWL z#!$$9x;n~U{~g}xSxBvn36ts|4BzEC38+-<8VoY|ZxAqWj-o)uZ~-3ivGJM5>FF=S9D^~{bkFM_4}DGHrAiqbh`F}4M9kDn);S%p45~=` zw*zbKAj2E&bB?bn>|g*rW)Hz+FPFIc^@&iRUkh(5U-9~xSMMyrOLVSii_(0+H|Ezj zcj(cxSh{a@e505AF1A*V>(%i`5Sgk;`hVFJVq^QCn?mgD9320@Ny~kFTj^UI$+tf+ zX8P%V@?fYQji>*HQeq~Ya(CRkPsje=8_8VR%e$T{s`BnO?9l+FLRCxtCAjTvT_@9| zLc7ohKWX{Y67YGaBmDZgKG}FAjLml%D`hcuci~*JHm@^R-rN8_lZhXGiEU zulqxRudHuk|N3v#)8btw9oEkC5Vh%~)-(g7wbC`CAdkj;n)d{NxdKg_b*eZZox0=x z_&mUB5*<3mSi~8O7&M&xanj8*fI!&3Jru|G$JjzooH7`W0TYR7q40h?@z-eFDF4X4d~`|01^HV_m@?Xfm(K!Qh|x2*1g!6b({ z!lZ}EYeQ7)y;tO%cGLoA5Lyhy-Re0qgu%3WM8I^uM1rV?i;RXFYVp$nj~qDR*;D&R zv8&6yez*0XD4ofkd#e?WAB*Y8w<}A{B6wigrQ@8g>QFs_a3H40CFdL%@OqhGCD>aI zNAJqb3TD;1+c5sV+HrWdg`NX)GKb~;+?S}g5P>;i)2%lj;QPF#iYu0AKuA53?P*-a z)By3MM6tZ(MuH{_0vTBTh0Rbt<@l`M3=-pa50)aegl}1eLskvT5kulY+wE9|VK1ZV{cPT4kc-?BKQUmS2HS`cWABnq$a&nYpL=v)a0sJgO`8k;Od*)ZETM(? zRCY*W#NS8)@Q!|#t&lhr8pv4%BK{$5-t@oSd~hL+cx(&TfKnf ze3tVCuMIey>O2g|dztdlS)`z|{${yew5ZKPY0ir@*7y~E{eo7$u@y$IKc7iGdHM@~ z1RxCOtLQm0&*MY4Qh@DGZ-YAWryGEP4qi5tHrcz3Mdl&IhC zM&EeeX(^#$u!HeoSz(-ICw%-S-O4yZ4A#!qGPVjp5HWTq`Hy?6u-oayUr^bBA3exa zQrUqOUh-(%IPJy9oZog{TpU3@Wp`kC`INj-C|Yc&C@c7nR@2|soGz^Gu22EyzENF& z?oW#C@Anc&x3ZE^jEmbh>eba)?F-zMN^!?T;{w(7=kJv7y?+U4}C&NM)kh@#I2O!f!p36)YWMxfa0c8 zP^_w%q~t5J(sPeb_9sQ?3_eN$Fk$Ff!4@AM*7jjjyI*hDhzIiAlloVDucjVYVMpSE zZm$2PJY4Fc*)BO#(LFI+{GrfU12Qp=__p}#1NXjXnD6+=cD*mEdBh|QCWwl)_xB@E zhV0^tmV%munfTa{;vGjQ;f1}f7q{o{Fz+~868lh;wgaR0u_Z`H->k;bQwJICHb9bj zQyJ&;_xy1EnSFwwCzDbMG7$)t8aM^~AhS(-9F%qj+ju*CL|dMlJS!hlY9c5>w7wWtFR18>&)wyM5upg@EG-2_5SbdKzJWU9lL3ak$`MOBV@rRn&-ky4N}G%>(Rx|MLMa z+!u@F3wMBeRwHy7dmkoxDNoF#D3pHGrSklZIV@Z=9bb-y(^B6^S;huFq=y~`hoWHV zSkzw&?5$-^>ob7FBW=hHCioYhpdb5>9~goj3RXf-tI|kuNdl6MuzGKVY8?%{WhCFX z_u*Yr!C_|m28eWK?#Qi=cnJ`_W&1tHsWStvNc#%CuLp#jwF#nPkYM2vXAF`WK1G~- zY_FW;%!Ej^YURBayg&t4o!5hqPKWpPhxz;X2nX<8r(jfQeGs`tRN&JrKanPbnJpWd z!^qb+$j;RU56MS1{b;<{N4%zrWaAwCS2vWsuT04~>_2et>E6izT765f9X(CJSr^k*lB^JoANp zP4g5rN(&VC0+6k6@SOaR=i#r~+Zu*1$%zD&lx5N{lAVvN%3(`dMV@xNG5sb+roiUe zxtDmmpR(2{14U@Z#J8;Nn|AfEjZ`aqbgy>>ztZiw9pDpD=keL;r;o-9^VDUpII7*L$Q1!fEGw?K!$Tbk^}tzFVJ; zh3EqQ9VA0s5I3D}eM{ad1UA$~zDFqDJ%|tL$rx$aPLo@l(lJKet<6+&31kBsb_Rum zw40{TgEuND$4_HSkec&Am^0+p0+YJ0Qpred+H{wSi}2!wIP))yrqMphcG{M=%D68w zpC~3uK+VU`kc$f2vQ(G%cDwd0otTS=ReC{F9e1b|5<&ddDgp)-@b1QHz58Yj9WQk7 zpM(VdD2(IYJc#jufy<9&PxKp2R=1d5-}3W4xRZ4IKCU-Sd=tv% zpJ*U1ZPUwEh(J;Va$rJg8Jd??`g<`tv?jeCQv?3a85cm7veK}_+N=JAy8tcY|--~ctH#QG2>iU68Jo`p%zG4 zA)?mRo{2XLTzj~!uSzMp(~Zj)UkL+S7hZT&VY&W#$q5V{O5tULAe9Q&(=_`9MAgl3 zHn?5HuLD|m;^v=hgxc+*AuBA+A$@<*Q9(`6d6Y)>*=HIcYFL<}Owd4iAAj7&!mA#W z=ndcfIC3ucLb~|~%5$(P?V4f&NCLpWqGcX{bEii(pZT*)S45k zn>@GWG<}d1O=7e~=bh{MpuTE~?E!j?qYV+ypKQq^Zah;Xukf5w+icrvaHA$7z6mAQ ze{@7lkeExAGbOvSvG%)hrp z&0Fv-^`8@+>*aOnR^@0fd7lVmOr(4lH^Mcwb8`R0SmX)V+fy;gMvdl8Mes%z^yN@ca)F1&jzjt43y4hdT=NqpEK@@9 zWTjD?1lkJa>YV5k)x=FkHEY2HsjSOMp)9xn+9TRsH; zokw!o&?lTwxN^gD^>0X+r+D>dqV8M!mRdraK?9aG%4Ex6^>O&(0`3;JfSwQG5Dz>k zB?C^Dnke}Qz-*9+P)P12m?mOK6*i@k19`of@CqoU(z0}909dG!tH72pAY+&`?(I7p zoyQgIr*WO}HQ*J=Jxm!YXKQ?V$r*b2=b0!>sR#ESoPl365L0h_&g#JArj# z_oU#G-xr-1e5XLLMX`S~T|F9&owMk2d%_ptVPB7zz1_t`q7}=;XJcFuckmrQN_=lD z6&{fc2X^s_o{u<+Yp1e`tE#j;uC0qp=xO|LbU&Gdn%;~QLNO&g6aN?&=F0Jz1T_K% zC1hVxA170|nNBzQWxa|kww2L^6#U*}}apD(eUVT87L|@yv(SDgGl`QUGmY z(Ir^xh|`UD=Af4pdQ(J4Jwy*eTTN_<+t%!~mv{F1t!e(SJgVN8el(&>D)8nlbM~1YWWLvd3{^qS# zPuLI@2PT5v`8&+KUTQ=r)7>%=U*8!kw}NsQQ8C?h8S}*1Y8ptzpzdb-hz_kD9ux>% zsEZ$u1ua~7n>n73IBmMt`1`?TL506=%&a2cBr;W%OH81lj7lwfJ_zcFk%@=*w^wpA z4+RJ9r!~k({>9nT6ml4K1Dd22_Zm!CEfSoOSWg2O$J%De#sF*PRipb+>|kMLJFy{_ zYYBocPnR6d$+0|T2X402TpK?aj~JQ_1}kWz)O3r|(ou%qKX(yxo+eK5E)ssC1Z()? z$#hNCvC2+SzT%W5d?ykihiifS;bzePfE{Gpmhi%J|s?>ubTfIx38FQ5_^txzXQW zEW?OXxluGZfEJB0tzV^%&K6!yq=+|L?B*VqXT|ZAd6oU*<}1ET*<07T5wjmT;{`0B zP8_LT*kry%y;cF6Jq=q1J}4&*+byU~R%qyP2G+DmGC)>K$B*C~1^Qp#uKZXqb5{Y~LY5)8WQ=$_z$9h?8 zZyOk+Q}@sG!Z@O~Bt%e%-=8W6eLT$NH zb6s@ON`e7YwncsuNtkN9N6h192F@cqqFmF(Y6YX^dw=9AVh$j@FlJfzIFKeIQ*>T& z-77R~{zJcdT2t9Ro4;sSHc>$}H{MT2f64PM?}jS3O&i?Jx}ZAzX&e}pp6=nvVSSIL zK4EXi-Q1X$R|dj7F7JvM0wU=@bW=GV?Cymscas>=b*%1Qx_QkoelPJeo%iqGb<-xEaJy*|P)r-0vIMy|=7BLMhfMnKP-#47?_4 zl#!WPN2zSCux`^^zMgm$U=D!jh>2XgoDmQ^M|<}^l9%Bhd<@hjT1Rd}rbrqO(78bE z^b)z0nZN@X;}D*fsxuI)3z(n$=~49%joZ@-xx;<7H+_fo0c%na@**@iQPL9QVmK8J zI11C?+=ss%9>}r7WvCYHY24LtaSp)QC~&%mEc+%O-*~fhgo^$`Bak(z#Vtj-)92fc zLG(E8?h!v_bW)s;){8-y6G2!q_E_4+VZv17J^X%BB+=Q3@Jl2>lpXuf6`K^=2c+~@ zJAR%^4rmh8HP}fg2eW->OB3IG21Tm`yJ*UAkLf&iOOC zE9SAL=4)eZX6f|95<$Ly4)K$*z>ZLT<^o$C?Bnm2H&GgKa`p?9fXL?izu;g_j{k{+ zIha{E|9^2XfX^o7KODUGP5Tc~W1_0fviH9N(hKLPlpU^|WuBG}H%x~zE)8D#b(N$y5*CNqj zt2`aDjP+|Vw=Im*#r<8{&gs_;`=_ThWu|;S&*_eB)l)kcM`>zW{?D(M4c>)w14yo5 z_n+kCp{{|Mz(RMRfuX5d+#p~~`2j;_Pc$!HGFZ=ul#XF#S}UGHJ${#rFjp{-n#$Qa z)ZaAO4@H$R?V({*$>?@E8k`aBGfwO@J^d_r=Entf>YpPB7VE~K0Q=XrfW_6Z`VDnC zm1~cFvlfSJl<|sPvgTf zn-qRIlUS+*=1k+)2QY{1p?LbV@U%VNuc2P`&u#{d>yx7|=|aj=JmiOi==0jjwSHiX zryIraOXTEoRpbTi*g@CXF2enm?-$?~P#_q4*6kX*jdV8`Xd^$3ehIX6x27O&tn50O z?d0wUfX&|9OhawUByO=bi&g#oaqedCHfvlTfixuP!XII zVym_?E`RrJR?EzpmsB(MO2KSyHE<81G&db|Pf>g`@5Vp$LM=8TAx~h){q&onoCTiC@ib1_`rSaa zJN`P?xw&CVNK8mYDI{s1yjO_w!3Q-~N|vA~)Y#hW?p;9YR8MUAc*PoxfQO95tUOzI0_6pW z%kXhhhEW~mW!dnPGvSaxBLPa34RfP~b9gaK($2$_d@NMl#FFkhw;lQ(1Kt^*D{O3@Y0^P)D+af2A+9_ z*yEP7fqN4y{c?A3mLP>0N)^JEva0TBi@*FMgwRQRiAbnj6dV*qw>%|$Z6r+Bp3YLr z$EnSc?OSLi2gz@loeM}l%aXc4k(#@uWaQ}FOAC^>4Tf{kfT4E?uvR&x3=MxgN5(10 z367hgPuf&Uswh+xoJ2F{cyGs8_KGb$;a<<3@)hu;5o1ag11A(=DhuMk05A^18K<^A z+h|BOeLY?pRa*vS*)`Rk9yv$w`L?5dWg^~7y1wc4Ter<(G!82zoD|x6Be^Lo!IKb^ zI*m1jUH+n=v??-g#pZt;oQ>=?*D1HZ@)$Or5UdcX*<+AvEJ4gcaP?AXwC#8aAqjDp+4DFL z1?MAza6!up9!AmIo(jVW3Mi6be#`Bqjo<(9j0Z0m7xz+yk;9+Un_Y>pu^UQ@nRP z53`@uo@nnX)Gl=?PlV8_^UWf82_uH1*O%BzMLzgkr!#@_hFutRFIM9egNuf}Lx0tK zH&wS_wtstMMB3zo8QCQXDKYMgxwq*)J$*qQRX|aX&>8ODhjq}e5kp{^)mZk?-k$>h zW-pdOKCL%#I~y$$%yoNr+0S|MH+{l;>t#zB*2O@G4Px`GKJ{8-n*V9@uxF*KT@sYB zR;#a+t0pGcGG3prl0sP~`#Pd1k`YmAT(mtRqds_~l2&GC6_x#0>Lwb>mjF_F;e}R+ zK;f=ww$lLpe;7Nb;7ouv3&+V9+qP}nwr$(CZA@(2*2K=lw#`YhQ+u^l|6cr;-Phe! z-S0W)IVN8A+eFFU7{S=7pi2O6+&2;W>%3)&#)H9yxSp-4m*5F}(>)?t58QP;qDq@dmbHSF5IDX#KzP_ch2 zceQw-04S+e=L1JdHHs(5B~B4mzt$Q`JxEEbMz$!|u`DH9l&KgBw_9IugX8o(0rfT# z%imSk`QK*Aa;r~Z+{wyuWyWB%-IZ6uzovbd-X>k1JDo9!Xz zVG{m09>+QT(ah*;tZYNbN$6XUQi@>4P>|RdF<08Tp28)ju7fjga=)TelLX|PY-JPx;cD_ zMqx$T^FSb-tyx(&@aV{waPEtcaUq{}jW@NJd3Pzae@q5_l&n*(JaQ_kI4Ob{~uCydih04d% z01AQ-IsKSiBgE`X#Uf*<)1|dSRZlBQIbb|r<#-adReoXq5`eyzJ~#n_g|bA_v&^6n z>HedkZdy8wKAN4C_2huD!se%%F`WHTLBG>-j1HhtRRfc}iGpziZwIkY0PR}y4k@}e z0dgbgvWM^W%0qtK_5X%mE=RWj>F3VV9l^+x1N$LbzRb24QK|3RHz&aE?LA_+s8u=g z?op}Kn#JaJP_6=R0Z&kA{S%cDq|l0en#n&HSuo4*5+vkNyjV4LvTaGLD(_QaiMwKymdfo}&{LB~T~A;J{D*JDXMBx57w#0OMCu|DF4xril$FrU;gf)~1)JYRwiijo)2f*B zMV_hS-D8l4y64?C0D394C?9;VPOv!T7EIHq8y{1UQIoO3sc~X0rZ=eW{E*oVDXitA zEaA24>I9nRk_}bICmCI>=hso+WlbLHsgr&= zX0U^*w@$bd7CO#fseo=6)hUV8u&Q`>iBW8;Av%g$aI!t917;c4eAU<0&0w>8>3Mq( z?x;P`X$f+mn<65yxb4L0eG<0|*Fma3Ob7>Yxt)<;XJO3JCx{`^{_UVD5G*J@s!3sM zc?QS@eTgxRmzKwbRE5D%h>WxBWfZixq}YKU0CStTXheha22DEflpsMWz=|uH$Ph2a zio+j{#o87Tr)GUf#mmbzN7cR#3gRk#YWS)+)t5EdCIk)#52iB`eD9H3ZwEt-kBaLU z=t7DYEOId!Bj36_4UqW*i%_K(}Ifp#leMCCf=l$(Xej=-z zOoVM~e6J7=;!a`}lV6b=y>@WRXe>^M>gm90^q`O-Du#W}Ts}|1Hi-%J7ActYuL0{y zQOIW*OCTYGFS&-SY-#{wtkPdO{cT15=3e zmdr^o9Na<-z``!FAR}yq$xD``gw0!eIlD2YULHfYx}bMLA!ibpe1a=y2q9j#_>=nN zHo4Z?+4A8bXW&BXJuJx~iMw29q1>+bdR+12bmI=~%I=ke$(PidhsUZe7kjK6={aA5 z%O3`5#V{^(#?$X_#h~dE=xn`x7?f;t1DUNdnlP-&M&au%+XiFicUsOO9`Q~jPgq&p zCDBcO=dxsT!!S{a81g~~^0mPdsdlNrDQ^{d!6Hx3C$$o>B?mqJ=%UhfoMS5V@O5B* zMdFuPX`H!ops;40-j-AB6u7j;Mn{0?}& zPf&t~`(8z5#s9)^;I%s=V}{^kln*tFpa_SZHz`jwJ)91evh6?o^nDfgTMC<MUH(`RGhUQZx?tLDW#q`&iDL%8!5KtM^^b1uojU<>%)X_O1?#+R zL_UC00e@5*`Fm?pM9gq8kt-4AH9Va$)WZWU98*XP`?0ob+5wN(r}fSO&sG`~=l!bHLz##1kr1`tmQ3h2^4g!EUoj4+Xmk#wt@+hS76|-5>-VU^9++YwmPZe6y3a>C5UfwX%cIxv}r6A)SU{CE{X z`MYP`=)O%~?E_@`Ub^QmQ>pMZ>Uk`FA!n-;CHo(QHa=^L{Dk=W?cZB1Yl)lg(2VHz zOqZ$yGz2OCeuVrWh8cgupwG9|-c>W#;a8n2EUsVG@28+mQVIV3EUkH+!UvkkJq+HJ zT(3z{%a?Om;}3Ot3(eb}t)x9Ahi~U0JY*)?Mdm*n^!~Uma$JU=8!CLyaxOF_f<#JC zcZ<#Av}Y;55Y(PUV{N!PIgfp7)iW0~<=CdMxhq?D2v@*1Vh7UepBX>m0{f*R9c1`c z5aciAr%a-u=cIV{tP;$j$Yv2BspcNSzWgx6B4ze)rXp|D4mQnBzZ*GDFhz#MCuwFq z&;XXzt{GpzZ-4D1#ZbBcqJjf0D#2utemQ@B;>1i9g`I9=GEN})0jY$72Lhl-5m%Hz zN8~9|+91G)Ne&H8(H-hzs&s}(;dQbXFS1a{FdA=WfpXuYRUmk$T| zCxa_tXD|4VVMeobjw*wRJ6m6!shAfInZBfg@=GVhE;no$OfZBuz`Tt%9DyYo)O3Ve zc5+IzbPN||2vddmY#eb_nQLV6>`!;!F(XJQm@Da)^kYKU1aKd6->vUmBA;1u;y1Kd zmOZ+Zk&P9IL?P=FxK~gPC&kT-)4$--&K53PR-olIhZ00|Uzr*uPmr-0<^z!gT?nKl zXbey<{-TwA7@#bOP~oAI<#uiK@Q@6%G{`Z{LvN~MpIBL%UL|)M2r4vMQ)!i|%#ycG z0bvwz*-2CA&|HMMTXArRAe5~-uxPk1+CVVDWg?XpFs7~4W#XYPfFgpFE4qj>N}(`A zqd!Wdg*Gzm&yu>1HBduUl}$s}>m&c@t~qzw&Y4m)9HTrFp6-VL=Z2P>EfoBLDI_cc z(fl#*@8yaDNveQjHjDt^7aFDs16L3T1@J-B>m!V{SFKc9H^^ix{k5TmEHUl;u<9dy zu(D=DcTI}ZY>`@`P;;Ix5)_zI0BA#)mmef30m`LY?s!{VA+;-37lhC~pth>PJ${9S zQP;%N%$YtDN ziBhJ!nP;1j^plW|d02}bk7R}D^X(MLr@93=TYimcL@c8^C+z|KIQ(9W)MAdiu`j<| z&;C52{i}WpLcTbyL$9|~NF`A8qGpW7XY3w$8%B8q5r7!F|0$ke<7DRgZ}o%ifBFGVd(!SR&B+5zzlnRtJ-ZkuLh3fx3@v&aeAf?WA!1{-iq!wKk<@6wQm(D|DYxQ>DPWSP_#K- z#C_kTUndmulSZ8Z;hU;j!$&|*dnZBn+Tr$Q6Z~eh{-zM7+QIz~lrZnK*=lD_~NLHKg{x;7($#a z;|=11RQ0bOP)p7_8;6*U%GKW8u!zLVeKM=B9*h(rMvjzlZRztJaKo?l!fU#Q!*A|YPH_FAixZUf`neRJV zl6?jP-uGib2zjk$DV0&YM!51&X`Onzg8p}&`oEbZ4q&A{2G_2(qW=F(3to`l3D~2M;+J^?6Qp zAzlFE5@HTou|bC;h+~f{M}XW6YUPHAG;PX^pO;>)0x>N;Rgs39%J}lQnogRQ%!d3I zuniVIDKlQAsOOps0wAQ%QlH_r-7Z2sabA4Qb%VMX{1*vc8K``Y50k*(tCeh1-%Nf9 zWt+i{2sF%KO%l+WfNeVthdPjAO7dXIL2*>7wH_V(^me(;0_MTn9KE-v;L$^>ns^J> zj`)b@5DhENSYVs3q&m8Vvf7tdYkJCMMY zy~6@>iWUbJB;VmW$&+qH`SXz>N`yR#AyEe?6DwUCC=$i*k8YrCIExII4H5Ct=s%ar zBaSy*(0tjRotkyn_NC^D=GS)HlbDfQ#ZV)AaUmngpd(=kL}v7Z$zrLf72v?S=QrFw zu*b!FF=Hb{aJWNS18eo`gS4Od1oQj1NOm%+53-1=evsSEBX9yKQq$bkcgb1#;Gl2O z|2%z|ZQ9?o-KJnaO?H8?@_z%LRefJ=5}$>G>p$lWM9U(Ul0@})+=E0Y$T5r-ogcfg z#%hMox_folZ_K;ha60~-c3KOy5IB`nQ3S-a>=qI&;kU7%ESi=La?#9_1&FSW-B7AE zL_t~y7b0`}m+2aeX4KaQiQFUN)+bE8d^gZ)=T)r!B4)!3G;ID?Q?!A$h}j?h+OM#duI#^_HVS-C_n z>4{AG2Oaf-Da&kT#1PauE7MfUhq*x#tUb*!I*=^v;xPjn!e8%wL4gtpeq^&7x_h|) ziF&OCtUjB;75$Su$9mI6Uu%1FPAz1PqM_a)zD^=_x zui?!S=RLV#W6>$>kJXvb5eb%9NnW6)Ga{;?YP=+D@eqcxEwQ@;4NwT9UGZO-EuTc7 zxr#!xzr({u{G>MbNm=5sTEUjx$k1vsiHR17ebgCC7=IJ%Ca(pXC4`zwnaAoQ>ymM$ z(DegFEJ&l;lX{C!tbYu|P!ubwHYA%3b<-zf!e@eT9lDyMn5!J3i?Lybx+_@~f)RNR z3?H`&`&7P(dyaWw5voImq}XT&DEs2&rxKzVUSGgA$nEnpXf``t|$CIIE z{kBN~NsZPls~#-Siac$D?L(6|3F1;E9d-kzi^im;k*CG{<3HbzrXD1Picbs5fAx~m zkh?^H(g04*3m(jZaxUO~)6WE#En_+F3xth3JI$I@V2G$@frx#_C4L$~qCE>G@@Woq zT-G42(0ZzR^8&U zR+5G?aJ7Z0y8E_mom#m!5bSqn;v3|h$a1|12V)I!1sMT>&ENG5Sw{*A1Ez{CF-6_m zcAZbx6;WS(s`gA70{;bM2!x0)5>t0OBiq&2&7sdoS)2v*X}m@eX*ATZRV6~BD+5v9 zGSkh)v?tvWqzCB_j2PWe#&Pvg7i9Lw175DXuf0Lih)3kIZugV@Qce*)k!f^eZRnCi zmH9q#40G)ZhJFz7D1mLzG!v{%-VzEVL$-mD8t!5GdGvJf>VqC)U!(y9wqb8m-nPLv zItdP4@SG^?5S!6^a20|MxzikYHeonOR9_r0y$86bQPO{&idTGFW&?kZez<-oyLGb_GQK}udKodqgbo>zx>3&Ly?(<3_?FcUnoVb`r93jgrbZYKgw(1HHEpPW?vghNbWEm&j9rPN@M zlapLV{F%ji_CK$fKIq!35Gnxr5@g1Jm9RHPm-fyOh}xX%ljs#50+{hPtYa-Oswo&o zKkC8)GjyTFVJKwF?;O4|mYvbFl)RqLHb$CJ#e(G~1c1qg@%CFyJxr(B+1>q!Gr0&n zNJKH~7{SmKm1Ko5`;wc`P$D59pFa>1Q&hJ$onI8^6}ccrTa+yyAHE#43nam z3FJ(en6$-strZv8Jr4z@?}yV+2R>b@_~f9c7))*wi9w{I>A5rYlKja0D}{EK0QBmX zW5ylN8d7B!m{hCHVG4+8or{?ZS&$Bmg%+0<)^Q6OkZx-&*P%WP zHZ|Zqhkn6m_jv3ln7#P0BixdWAp|llItM21$NbnwIYhoMS~_XCiAoN{FzlneT@ZkJ z0@8VE%D%z@F385@9$JRNh{2*i;9VU0rz;RchSTFr-#Mu}fLg{EIf6;PK}bcsPvVhN zVm$LPV$##uU-5=!-uRy!4P!+-*xzjh?~=vfVGL|!Og=NZg~O`MCcSrQ*W7LI@C$vw zOj)oS8|cB%Qv!@CtelG-G_vNW6gEtiq5w)#Bu61rS2`8ON6?=?PpbT$7)|=lQAE>1 zHhi6*X)aI66;YY@_@8N8f@x`0mmqy}KBSfHjjK*RxbDPpjo?@KOHCpg%TtZf)RQj+ z25dmK)9g|N9cX&l+>)FopAPsmrul4b)85{?PR4214yqYQbJ<8$@vvj(*%RPhh#aQn z%ZQ-q{ZphO-y8cQq8t?s@?e)E_`DERlRKxqF%kIFZxAS|BvJ@N|96{o-NoU_#_3Nd z?7DLuccFfbRRtCS2Fu6@ch5WaRcE3Y>~4Se&*S%_UnZZ7BV;5*(7|6{wtOX(J;@xz zsq8bVvdft%15?GlyD$z4A0lh1HRhh-{fko52@m|5MW?AH$v>_sr=id*KG>dL4Rk$F$ERE}R_i zI0{S53HrccFtG8fN2N>bTx8w2h>}klm;Qo4IE^2bx2tT;G&?^h6<{JLl=(r;$c1H? zh{+rEMe%Ewv^bQPwqXt^r>ltvf*~C$>o8SBibEuM@wv8T2cn=a4oK+~S+e%|N3uC$ zmxPCdo8C;HZe5PxJ{FZAB$i;_w1>&6uMxey($>ww+=1`~+qnVv7>Za^$phC6KtLXv zLf@@rp%_XVMl7G^YH*!S$I0hj97G)&DQ6v88JY7!NrnE}$RKgm5<^Y=20wd9I#M}v zj-*!+#h60`4lS~!?2MFs%`eR}dWOa&h2-QMu6>6bF(g7UDu6_gd*-$aJJl(~#Jt&g zw)s0Q029M`%JP(;C8`{V0h1RV^H&45@1EA`T6hftN>pqc^a0N72tI5k6=+~DdDdtp zjrB@tpUUjyxnvOotlThI*%taYSWPJta1#u`k%bSKip>bKiC-Zw+|ooIu>j0}G}Kf` z5I91+;2|h98zw3$nJk@1$1P7G>mKQT863t$$J;V`n0>)@N3Bh-vS zIW-FxHg-NqiZRq4T7wAx=j_+p5sGzl+Y@ChSjDm-gqsn%^IL7>qj2?7P2Yi%KoA$c_a6(g5WX0PMk-s!Quu z(fdO!Q=ajE7M}6YR6>Zex)8=lG|t^*>)n$_9cLi`jEMz0oPf~B`k^@t7#;WP;cq;? zPh?_;3{6~93NQ~_eaVT40cKaWnhpyHC4EN}1JDu?uQrtR{jlGp;j05MzU@{Hlb?#m zewuQr4Hl(|MB&|ypCf00*UDiO+T0rj;U&nhJ9iTdq@kU>{t(0)7y4YV=B3zR6Reat zT#@tWBWQZwg!J=Bd>kue0+{IS4#Zg0ZUv>DKmdy@LEsMv5)MB65!6UTLC754kWf44hbqxSbN# za(jpmZ#a4dUG{5o&>*?;3^|?rtHeF~zSs7UgB7cuu5kQa1CN{+%?E~q64cgwKDW_- z^FCU-9C8&x(@f0W-@Y#=b(&-Q3})i+pl0QU+$frm;`ho}PVwlPHk!7V4ykGO&`gR} znvhz33?ElBCx342T^4Q<7rXGVv-P~MZv*1&hw)Rc9~28#6F{Pkj!npy$^_xY{yqsB zE&|e|l&mCZO%ROI-)&kzT>BuHicl%iU)%5XdVSHXS^hrT#-=i3)i3!cxMBMR&tUk! zfn>V%cy?27wO=oBXM|^?8y*MVyp$nH?)0^UV94i7pJ9&cLsLbaQ2_ap++q^i-KyOunoge``iDiuXBCXK_WuEAIjQwXvIn zfdA0Ob`zqT(K1EN1K1lc#AK@eRcP{=eD5~VY+qNK{8~Ndo1~YzlY0Fj8>V2GxSKG@ z+K&O`xz>6T=5D;nFqm9GvIR%cF(kZAJmqJ~{X``se&dkcV{%4lhVgA#Q2roV^ZzCU z#u~N85MN$5-6pAHGe$iN?Gx7x@$$SD}dX^m!O%0A?8>(5v#T7g(p?@YXE*t{^Kpl)TQv zU}tf~IKswZF`@T~GEvOXY(_pvzD(pb<2l(%P7`@g?-hrTaR1t6{E%I`$S~$}Se?+l z&t_PBlKdwdZkT6$4JGDLt7iJ)$Pk1JlL2ji;j&Dt9Efc7XgCGKvTS=qLqSxV{%<4H zAAr5X&#@6kzB>E^*N+ez?suawZP>wE&}2U8<${Q}ZIiyF3%wS@i4X&(TgqaCiSY?RuE23g-g1rc_SAMMMfzk(t8<75_Zj+fd{ zxi=RrtuP`zsaR#31xtMc+JN2b%|FRbCP3;_DaP_R!YpRmOn+2jvitk?0hGQzMfkN< z*ZQ6bCB>MZsd8>-B{Nldj+8v;FI?^qI4v0+;(XJS>y(u#)3?fsqPb}ma zDZ1-+h^56Y9kQkGFO8x4t%!@O7)W;)fg5c`tIn}`k%H!nA3J>e*;wH@K>{JmSs*Hl z(KU}EVcct#uB!zj?wBE4AT}M+kD(mR3tF z(?paU^PQ+KtEWtKLT~9aQh<|-;(0v%fphn+9s!3ayL;x_zExl};Aap2t zFH0P-u&atFr|`5%XP>XzPrq)O;Y-bp87~|`AgSv_9^Bakhj1Jn=fP2O`jc4pZdI6# z3uoq*f9&ZDVOQ*V^71T^7PML}8Qf@=<}@%3;}OR0q6I+qSowwZSVLDd7gr}a5>2%Y zxPpm`5+e(Ek2=YDbxc{4G;jhmS2SJ(I9{DTY95$Avbd5ivfY`Xx&2k(Y(vJ4Q{@)W zODZ;d;OYf5ik`J+OmM|9v^4D^HP5;fVqNVSYE9_1)KRGFd}FX5&$+=|d@_|QX^Gn8 z!3Zp^;i<9NzTRJyFlkY_kNr)sP+ykTn~1r*I6dw~n*rLLyX^p>?~ z2wEfC34HycB%U#L*5sVnxUq2fwraqlJSX&VA^B(}z@t1^o~jxQV0m(*>eWy(I5~N3 zM7LruG|i0b8NAi1#&6B({$Mefl{i+^42vhdw|w^}TBk)BBwGkoyGE`;1!EX~a1Cm-2?+b)@te=o8pVxyhoqngwe0K*A z`>(~Xo0pql0oneI?YFbh^4;#+KArtnUw^~qzWqQG)%un^(>|dU6KZ}8pX9;-eZQVf zhp*!km$@YV2)vFE$ScWN#E9nk9w9$5B0TRBO~2m?%r0JTYaa?81Oj#r`|BGb>iWW4 zj@pA?SE~VEo4ZC7b>z?BZ7wbzeqZ0DczJoAmh+@|u@ezkmzAan0nAbvbY3BDatma? zsYLSU7VlxthmEQ93vX3o#t8X`aAy|>dAvi4^T`TH{sG7t?1W{5doeO~XJ;;#cj9Sa zp4f+PERYTr5XM=E-7g6|{&auk%;ikgruffZ&g?gA?{+LB{M=pcmvcnS7|E!O5>!7| zZ_gAijc7Rj%f1HG8w}=+;cFibfC#Z>KcGL8)y0SOFSZX;mUT{^K&1gQ){d`W&OUzw{tENeL3j{ccVHV^ zxBTLWbo%n{@AhpD`x16%Fz9^Vysn3Q^}j;aci^|zKzx}o@^R@1D?Tq7LbWw|P))Lcxtm~V4K;kPTE+YwaJ^mpmp|$_`+@fla@L3&meaz4p)?RMq~!Y z{S^NV3(kH*tklDpBeVs*Vs6yYLn8#3N>Wa=}RW^0L9IelLMMJ<{Bv9Iv}Tz zjMdVGmQ^yf@UKv{L?r&mrt#4=axf8UNt57UBwG@eaCCKsic0|nh7i(@eGL^O2p@5T z3B*PLQDMK9A}YxUq<7qF1Z-Vxqp0GHh#adh4M`AdFcE2pD@011^W=YbmO5;~I>Z&P zSdD#vJQ`T_sU_T@>W_cJGtW|vuf(!NzR*UdK4KtN?vAk#CO_#Y4Bt0oFSLWYVzZ}& z`!yslwBLKppDCFJbrR0e0Mi}q?iJw308YeyL5N4pj6R2~EuxD`unxP=!3+pyAkA5j zp9jBl@}WKsK$v%d;Pa+6go6d~=@G1bU)nF-6TvhUMk@O9=}$ppRHM?)v4wN<%#8&P zuwj%0bE23;!Xb))>ckO(efWdW@I%>HNhLvns6(BUF#d(P423d8LloW5q%dtF^q>7r zo$98N1fOTtW6*6$fJCWKJW{M^oaEwCz!#)GYnGvyRl^Og27k__}yHAf=X`MKs{j#!)?P^{HP3|2$W-K5Nq@ss(^QdwEU8vq{kJzHkTeGZ-^;jUPCP%JwMo?cybA(waC~2s4zJn5b6rV z32M{xdbU}}&bl{g3ToH{%(xDeX$vIF4n&T#N;B+jrdbbriKc}txPQinurBLc7Rn;S z%XnJ^1n(BUtH;P=F*8!ohF)SrG9NMs23NNr%uRspHo>iN6E*35!)2&MD)_>aa+uK>{`S+8HnA_J7qvF-OtpTba%|=`B`P-c+SvBP{wX2 zDYTIo+(HCyB_h2Po>~S+HFH-hYHc=w8pe*Jea?SEboketj8R51$b)njGj7~ckbNFs zkZ%XIBzi66kpXSchFX#?iU2WIQ<(}D@-O$iXS0-qTz=9i^MgRk>J`g>z6n{FIQ}zf z$;QOY_TM@sMs;;uw%D8eU+Xg;$GCC|qj>h0e^HOG&rnA-TiDgA=*Uxx%_t(LbcP-M zUj6=9ii6-H1dpd*SoK;+1OvEoU~e06U{9KBG~2$+#MnN4uDr)&1k4;ym#Mq(@$~0X zaj}e*r01-^TfZ>vS4xlmJ$-cebTI#i1goU=`e%E17rry)u#jragGq03%#S(x>iR3H zaU0z>l3?(y)?}Dy+qS1wRZ_+@TR-(KOr3|D-84p%LhF$u*QmG9E)2fwU&7%BL(n#m z7*Lk3I~NF(!eqUWC7Ev58;>Z0M;}v5;5GYr)U~GXm94JUyk&Q91S9L5f{VTeco@*M zP9w&5k1+Rxk)7i0%w*WRZW?vq-_LK_QkK!Nn=GSym4o&>Wq-fN);2zpCK3YMY}={* zWA;@A9AqCor*zP`TNRbGoXSZcOe$bT9V5kT6{!4=1&o45-e%fJ)F|)*?zVlfyf5~y zLN3~ATaA;N6tKpL1`N2OPA24oaNgG-XR{nP*0ZZ>)9sB$tO&xg6V&jmVv&+{L&@JQ zd@h$2pKyzta%)X+;-bqL&R~>Kb_YH2Ei(!>)YTEYIef=+KYPN8wIO?p-grdQef@=p!pj3OC_Q&&|jov?qE>)6k?u+?x zbxMhbMZVgn4~Rwc#wf^UbfAey0XbHoc^xV1nr#E%+fUE_ITj%87g<8jAhfq>_Qy<5 zw05SRFkH{?GGhtdR=Tl>t+l%(V${3W1)^d@AJ5<)j4mX9QAUllAD`VaK^R8B8DLp! zHz|~CfAva-{T!yu+nHsPh3clJTQlW^K|-Ysa{?c>l+)9w#({pF1Y(!4Nn#tdNzxz| zon_iCRw=|Wx;O*UMb^`i6@taEVI6_aKbKr~-MbhVo)lXQBfysB8EGHlV`5yhsmz>> zp&Ckt*a$LNI-A$lwF-Lv0KIu8rIjdB#N&$Zx^i=%JR*M}53fDH_P(;%Qlbe{x2PLQ zbUDRVK~4#u9SvC-UU2)}S-Ww9FJ`Efo;0qZ^@|mrdQa`2Dx2`6ol0igNss-pQ_GN2 zGdWsCia&K-Me0}10WY~-z2;mMvm$dPUUlTGu_Q)O9*qh~@J`l4eU4t*>5dJ7KTXHt zi_IkYmT9Zb@tOt-*?uHR0qCj@&@Bm$!O|o;**!vVTVr<4#DHDnt&Uk#zs43Wc8BU!OPL zS*d;o(dcuUVc)CHXr~OG56?u+g;FO-cZnT!0{``gjr(I8J&3t^V%HsC78IHgudZK^ zPjnhg+AGk$iY{CBZx_{u$n`a!J8Z9b`dU|o(xB(8i3~acgA*qPfb|=A49XQaNg8)d z2n-pmij|O(-4TzeFdKEKY~G=T2m7Jwh1gLfH9x~_$&(Pw69nIHzqH0Q8;9!vt!%K? z1pedJJC(g~g4l@t4<2`+hvT!4RsnnMIZV5u;*O6qEF&8?*Z81c#w-^1)MXqfzb!B* zTwLx=qZ#jq5;sh7eskr#g!0*;zg|NKMoQpL42~;%Ci*~+GMs8Jf$bfap<3!iZ;YC? z54UCV*rARDt5rX5xFcf#y6j)eZ_1)Ayn(`6-;R}G5Y{3n7-Qg6Btzq6#D(z|mBHYQ zMsNWrF+q+nII+8FB#l^RBV`k>)RsGwNUrGtbU!S((g83zaYLuC`ATJPVFBT`OzC$; zRurN1*_*KzR5C{>_F-@^n>m5&2ZR>`J z$Z8r4R^bW{U5~3~-;UR~S~2Uqya|f7=c~{4r&C=k?-4hp&6sADo<8~5e2H2_zVlKZbKvh4amMt>KelsA5%weTv4o&Wk+{Ufxn zGpg40E+-2e!3mse0P_=Q3!*1~Hox!}awQ2J2sJD%wZ+?*(K=(8dSGt#ES2XrFsWXA zlgr>pdOtxfZ%)PVZ0_Y4TVj&nT-tYMroZ*A5Jq+vN74%-6{aty9br;FP5)N*IwU5vlkT350YjS7GfcEP=}V!|BI%a1$QiEZ=V(Bo?h)v;$6LK%AFrQ%Y1p5Mh>2#pks+aPk~p6ZXmv#Rk~e*{0lq55&~ z>aF&N#P){n({(we=gmn6x4@sr?1pf!$0MT&+c2Vc9WW)rWvlEp%y5O9u0pE{qe2|7 zf{`KUjMXwnHZEDuWBJ?-57{h}KbXgKc| z$qdVt(k~cJ8Thw)?_H_c5PJuk;?O z1?2soeXC z=dy1@rB0t$XKOEGkbF7VNjjK~G9zb6m@L<=f(Zk|nu(oRVfB2)#Io-RnS1u+Yl>NZ zm^jGj0e3qQUN^z*Qimbu;EZnxjENy#pfjK^SSgM(7J&`U`&k0{r|g5hWG(O1MjvP= z3Z4%Tfgs@=dc3r#Kt>jdgPHNPpML#<;lGj~_d{>~rB^}Na972Dc>ve-;p{l4`SuU1 zK9rBXA_kdch8dxbB;3jXS=8YdX|vyRW(;0Ttu_87g3zV~{plw?!v}?fFjb`81SRts zgdr3J12XEGM_`cLhsWtEu5EZ8=Y9aQN~6e)T}%I>)3D z7lD1a{PW}e(Qc5}>mM*T4l|<}VK$!X1Jy226O(RQ!i{x4Qm&5EHH@cfTlNJ{0ETfH zMb#v@xw&1Xt00RM*jRu-GD+lNd{IVJu^uG^+(u(6?P>c8D59xv3p#ow+Dq^fajfmjBT5k?rXh>GTFmSW&hV_6Df|{$hLBrg9zwhqkTM54l*OtyRDH*CEtQ=cG>L# zD^aTga)WON4^|76aY|jSAEw6@oZzpejtdZN!H-?(w@s)?(T3CE5c4o zq!s=gNl0+M+SqZH=1GzXdZF-z>Ja3h&TV`t(Qkj9E=FpRLom$!_-HMrT;96ov!h-v zXGj_16!!vC=j=|*H?R}t%zq%ptHWs`TlgK}jCdua4p&&yiRx-ogE>9&HW9^fUJB-z zsTcZVf9>TZN{c|-WyB{5N`#&4f}UU;>pL9FB`_bQ%W`B`c6X@9Vz5^SIm8}nn-VO< z;8Ei&9N%)^z}y+ubtXY-S8E`r(fhMN$CL-%*gH!_Od!`VoZh^LfR5_fz;dq}3?=^C z8iYTZ-9UW5?5LzlOGIuOiqr>^CozI9w@{3a|w%{Iw?c-yE%;z^S0*p@BPJ()XnPeZGmMmEpekAQ)zYTzULc# z)mFIDErP==E$$~PdrfBg4J04j%@~UHaxL#Ed7su-Kuz7Rmrn6c)f@aW&)$Ve3?G{c z_7G!3(?jNCSAy0bic9}4y=>ZXO;ME?Sn*&8h*k0Gul8pFKCHon@1}hjZNiTGyq>xZ z2RbAAe&n@ZPTLPzR$&5%m(BF`?iIr8t(Zid{E8qN`ie(i(8dLs$Q1wsp|TT(divVW ziI0iFHV@e53SPbLv7IXOiuUp2%(_BI>U_u{QE)7k)8(V%yd21XT}Py@cAXl@2#9-P z&d0x?y=etc0qxCw+CV%E&-QYM1do%AN#O7D?pd0ZWEwaoygqdj-&;3bU0G8VdtAo6 zdsc<5Fm$i0vlM|%QO)`4vPK~4^R%sa9*Mk*=s-f(7GHV_p-QWqg;e3*t88XmnI(Es zJ9a;@b>^u&p$HK!JZzfI!i)3KV-3Q+G-fjj9-Y`Xm1Q*4Z#KH{!206`*9+kQ4)Zf} z)3B4IR0f#=#Hc)J@LlMYAf?I=88c&|HFD8v{Z4QszjJB$O>-k+m%clfEuUpdWOHjJ z1K;WuMSkAi8S#z@sFmvVIfh5Ojf^`Gm!Xcti1WiQCL;^2OEBe3L(%-_;e1=biOYx zb6|Jh=}eQSnKC!F5z6j#2$FfaKu-H|)5Znj`|vLyt|qJJnccml>;be)*px)2LOY^B z*(@M;ZU$S)l!a{$33?9+kD3<@jOHdncM6++}!C z?pY|c8*~1h-AIxqtMRa^8lDF!IRPiK#@>R%G}j)+JUbCa$a;(ga!i~3{=yE#g`+H~ zaz6S50!jfp%LK<)m*aUmimnxJTmgRbIBTyMt!6qiTwX9>_A!dj__1nNlLs2&H+peg zSEH9pO6WgipNoSf21l`;t}xfOZ`)wH9Ve_P$}rvL8lygo{H;m?bsYwIm>8%!XPTbZ zQ+J5K@6ZYAbfy}e@K2HdVeA~5gyDkrJhpAyGtbzzZQHhO+qP}nwr!hvld5l(EK+qA zr>pQ@Ew{(_7+1N3sh9_`mxzQ(j%cEpA3=fQoeeGZo>#O8X z*cF=Lui|p&1svRaDLXk0fu?&8VQuNI8nwSd>*=t8eI-KbG!&bk1%2+pU)XlmlwUGQ zXKavp#J5thac0O=+*IdXy02s5PukD;`{p|Hf?9-MONRH+{z{QfRZ{tPv z{NVFA7)2@wHaZM>)voL)xbE<9RBV*Jz-_jMP7Q18d%1MgW0&<5*+RJ3V5x^`)wi?q z0|mJQ0!eT6(;7+}_*8&w9{#B61r6DpHzmyl8rQZ{M$~LEEf;3|Z{a>I+}tg@tS$`R z`OxltUCO(<$J>1y9a$XOZRE-ZVAxI7pO^yF?Qa4FzV#rTYd26QpvM}EJ)Jw*ZKK}s zrOU7PyJvF#_Tw9l_e^?w%43R4A;<`=xk2jfApBWa)h;a3WCke)xM0d=g;ANx;Tlc>>RGF zAu~}ZWv|ELEA-2Je3Ess3)qk=ZW%<8|M2m%G5gBy7rt@m-+;>2@UK}31CJMNkYHWu zzKV8$0fD|p2kRH2e)mws1GH9buj1gYg~$EtVVpKU>KCb7$H*!FGXtuKu&4PihayJQ znZ1{j73Zt3=Coo(n~W;Fz_(V3P>Kv7QIIFg-H^&YUJw}sEaRq6!US9 z@~$iXA}z1f(>3tXLCgt&=&HS=8w$$i-~Ol%ISR*~1yl6Nz6kL4!FjK#D2aZAm{T+f zxso4Zra1C!&rcXW<(5stsK3I0u&gE|v4cQ*16L%uNb-$4LU-N47?U=4XO*X&X?)oX{@vZix_nsU)d~ZDh{dOKJx0 zKqHL)c4%&>h>I*`bY(Sg*OY7>1d`|W53q1lKJ5QN761MFzo{ZK8{7Z)CAmgR&uNQ2 z(QmG7zE?p7E#{BJC#BNKc+7!pc6xF*BTK5Z)I74)y;K-hV&mxNc?;Gbi7=?aF@;jQ7cG|~w z^LA##XNMMZyk=?U^C{iTrcBP-jGsUkQ7S!#7Wx~scMFSO+f;yC)~@3I-D^s9<<)j% zo#ss2Wy}f%PzX}w2z8L^u(g`uJ3&(`0MS%0xmL>a2AND-m5vqsSE)khOVfM(3R?z*Go8BE5TY zMwrSix0}h7G@|$W7=G&a9hL?Q$>OM7y+EDJ4eSi5HW3l~C>73F3xfBwx}+dDDeHRO z;)j1@heagHIcZrZG69j(jz+^&7d`hTg~o$H88?yHba>=QJDD(#o*LEj!uBaIvHK*k zMu^j~(ql9)wnY?%&3}ti74wx4>MD&d#Zbk6Yh-MJd9JAthR?MDV)HLaJ4F-XgU1QG zMad+DCqFv&uk3!&>ynFzK%k{l?e+UdF;t_0Lh+)K|hUbi5xGF9=!l;h_oPItP7H|4^QXj=wK|0uIfB$Aw1n@JW8f zx+=@Xq=SL*$e2kmW*nAj>nAnt_Z3iL1WT67+1|gB!S1AHbx&|G%3>FP57e-NUI6pX zgStN@QBq=i9qW4a|HzpgUsgU?Z@dhUWxZ^-x{L?T2R1#q#kF`16)=zwN>_(` zBzkbIQsnW^nBA{-^q`{IA1f>Ac_dZFA@Af~>n~jwirng`Q{?%mb67eoy>)`J{Y6G6 zSN6{;&$Rtk63VQXzP$Ta0!Vp0TgU~DYbR>Cx$h}ZqF+WBq_$?=nSmObuE2!hJlyd$ z1sETyLV=J+jFG;plJ@G%l=%E5!-0Ye)7&1 zIDy2C-ty#Qjr*tlE%lg&&DN=k;~BcQXW&>cMYF%mMT*6strHh}`~|_eFJH%@(TfeSs>Anw!r3=3EmVFY9RHd$?nrvjSwXs`-nQkT*LT&0=)J$t zXoy9;9$e&9;&Vs7T~q?R5SQ`maRc|OjE_mt?<}TX1cw?7kre8BF`!YUe||nQVMAat zLDcYLJ(+B}CzTweTPLmLVf|WtF784QAU?EVFKs&mrq8hpbRFxH2d{h?c)PNyA3E|d zx?7qiY8U&2{&mQVXO{ZYd=B?bl=yjql1-GjxvDKFHVQHS#PCe`rN^SiB<(*cldB?! zI-m~S>6@q9&O`cY@!?q8Qxm*I`lep&fm@uL0>1Yh71G20 zlrlB|S%-q9Wwabm%>hdQjdx2Bj;w8LNR+}h@UUxs7Y?X=+UH{$y;E@$I8l=c$1cZ% zHGw2YG=qn%VEgoxU%D;^4p(* zoEc977Im zmC%PK1Ln2Q?*L7tac|2G%U8AZU;Oubp@|DOM%{0L6@oN~Qd~Rx0L?_yqK-pAg4BxI z5x355WT6+r8gXO>j=@a?*JLcyZ=pA3I>Xj_2BGX{It~#iChUTAUD^LOielwZ~}!lJO5p z>MHij0-8N9oHDYZL?DVF0Gd9$tQFm3^X;Xe{_F1{a#^$1ICNDS$}`*E^R_UL_;~Ws zcQQppP!S=1TMW=`q7miMKAzgWWee1r#Z=h2-XCjqZcs~k^|;!`lRt|;_=hCMOCwD= z(Wsg7$+mm^U~rSAJ)!S5y!neUjA?D4OcH!=6(-=l|2yj`03jIrmeyx;XWt&@_0maa zbG$zoR^{{$GSm9mK!x4~37}K*W9G<#9SAghkUuNv_-)gg(@25jws^;@2OXm1t*|6*Nab`EU|rl5xix`mLq26y zD@UtB3rGTKU?Q+c>uu?aAxZ*(d`S5(F#}XNB6q4gpsSt?5vaj2fin{9&@&!q#D8k% z2)>AI;rI+iPTLW|?pMW44?ut@zi0b@Q|Ai}wTF7Ze?d%+6{a<$kwahzK&PmsO? z#K+?S9e)V}Ab{%6y%$cP6wdA=DAl;%nf>*Musc1;OT%|sK>&3}3cbAeAjs!iu)Q8H zT*e)BPcvM&V|iF>_7FHiP_E>Krv0)BPk#-ylW|PL^L7g<2RE1X=hJ9Nl0V5@igA|W zMP%_-klA&8=f~KFSiu!gh~DEGIK=xDfWBB*;Vj_Y6YX+0{{eFRSOeGt#oHTR#o*hM zNxurc+lBN({`%xU_z*xbPw1P9m+ zGR%k4zjBUddFapNjJLd` zl-f^?$f*og!*A6;g85fDvFLIM^}KQzQ-o#Dl%M$91JjpRfFwUq;Cp&H_K|@d2RlsI zej)^%JBKrPuHx#}n$LL?&D-xqF$@b-k&5nZiKCsGV zkM$}@P}U}+awx+)BU2IQ+MN_67x&ZTw)%2;9Utvq;&1Ze)Sl5h(?y$jzM{2lrW_J- z3HTm%rmF{eva2kMK=p}^dWe3on0K7x-b5$pepojq+Fa!?zKSk|6Ea<)d$%Yl=Bo#! zXOqZ;fpABP=}y&gOXJS478`sXWpXEspoGJwFT>_5N01&w(-w=RoUAhH6ZoKzLg1960^e$X0zD+Ex8&-w=4?HMyC>U)1b>zccg^@;gbLz_WrNFRzQeL0#4s>!Y% zggB7G%fIyY)RCA<(VVxk!|`oYJA!r409bsTwSu$UZn@OR!F0E+|MY|fX|=n5Lz}Vm z(ivRQudx{i!M%kxg$IQ5e)esVVpZ5*0Ww~wmNF-_75t{EEn|N1AfHv1iuWW5&)v;h zapTCc_4)W5q5~X^1~d^4@UXO`)Gl24X8xo}WFCsRQAK02@~D*>R-)qc0vDH?0Wo7B zJ4T<|rGG!O=urze?8L`pyxvmZ_SMyO<%RyhxT{&)9vdMl=FKmbZ=e62RWZw#k6WL9 zixMPg`k%Z@pfq^^xbvY?oi*Mc!_GQ$wvD&{+fOYM!}OK_8>n4Gp|4}rC6xENt9fwX zxJGqfrz+j`A^1Ct;P*&Gw>B=z#R(?<=Dnn_8oxy^FgVIp!#8gf4x#Nn(kqk~xUgj5 zv|Dxt^$>6l@gPRpKFt%~?1l_98Ys^E01^#)zoT>HN3TUBbXSn+#~~1E@~e_=bEI)0 zqxr`hc=rR=M^uv+V4D-`s_O0^POnCdEA5)xh-1gsSFiW(591azg>Hb9o}KkFhCQ9) z{Ql+Rskwa_gW-(cp-XCK1BA88#hvziH|)_on6orAh|Fv(Tq}xu%T^yYN(t%_H%L7C z>C;Tx%dd=#+pL@?%MY|AMoW%-_;{2ebo|}xO2tKI?WCbC-U~TZmlX57qe&dq)8lyd zStNS{z(%oB>MxDnR#{e_8M2^BG<$>+l0dvrG;@Kg=T8d;95+i?fl8C?zb)n(OE0~- z?e)gG?6lM+y10(La~rO@YyXRj`cR9)!=T`Dur}x*ednDV!8K0iP57DSf8P9ICE{}i zs2v^`67&rXr1UUh=UgvhS{R8@U*0PijHU5w8>ZVfp9h80QLcu$Z00ZT3{{g%dMOt^ zy7)J6%X%Q*sNeM6+s09y{dSM)AYQ6|9iZ@ihj7eVob;+XR~o$o51x+iUL8QEk={yI z`?ouKLri|mXVa`j2$Q;?3ny)K4f)#g>i)rd>#stIqt3|t(>6(mrIbXgpo_uvLEn1E zD(5I?zuK%ZtfJQ66Rf_?eR>=TGFpBolC(Dn zM}_%!=-U*o+hv7}m-MPLL7-Ni%}w|Q1pb6i^EW-Smr_Gz4&v=VUTlrE)^aXpoV9t_ z;_z5cXBG4AP`RI7UgNIbo88=ed}8lNguB4SwMGM#692v&C|)u0kulf$EU1TNGAw{t zwQNLGIP^t-=0K`l!ezIvW_3Yi<`F=WTFtew!rt0dV6MdsUm2z4LRMxncmxa) z^d}5%*&c#~ekiHKi_EohdI@nvX(7ZfeZ?wltdID7V(@K_$_lOiYvG(_eG(i)k6+EO z`I6t53zM?1Exjtb`84+P8{=t~P)psMX4OOIB-japd3LD1`Q35Vv!>d59AI)z`SnUz zZGC9gB3akS;EOgv(cgR-p-86cR>(17;~^M_%iAatm8u+ZKd9NHCSr( zbXYh4Mv>=i=|Q*fvHNO0_1Y-5Fb(yQv||~RYOV@=_|tSrk>9bq7#Vd>(uX^*X4M(( zC-m`N1XJZpH$9r#kI^ux>uNduE90(VA;4ky?IE6O{>2wRZ|Ty=3U8^6QnAu>K7&j` zV5J}h3PywHA_`u&p3A!SDV|A(0!`KISWaVS2A}v%8~+1bP>f0Ve~>RWmj8=VhF6WQ!pIfP^rCQDA zj@;Xlrqzv%Y<00Dy)_sEV;TT^`{KWU3=aqu0MY=p03QCp{<ochOCO7+%|Fr~X1f*bvW{pp8E~ktSZ!c~FhCb+7Lyf+k3P)2- zYDNxHHUH5Uo1UWmF($j#10ek1|C-vy{tZ1$ZEuWV?eh!2{GsaH?H#<7S43pb#LNJ| zx#s>Ir~tHUW@REPL;&INWwX82f7NRk!IcIyU4=^(uB;t}Gaa2Z{nL7|Q=fRxUpj_{g3hc>?4ou=MSpv^1q-BmbGS9rVsijeX9 z;eesDCb^Og2p>X&5RD?qNQWbQz<%lQ@^OJS9~Us5A6K?ngWq4b2X&h* zL?wtHK36A=<7X?a&#rV&cA@kdtl81)7iA1y+-0BL3|ZzO{1bszR%ewE%g^ZcJVzGz zN5~?Qk15|KM$OM$A=*{3)+S!`b~q?4F(f(yOH#s3ncWH6>!|M~{0+MN^Gjsez1N)j z9>wtfllW1|Iu00ls7SK9hF+MPl4BvE-^SJO_R)jdeKTv>T6$i>@C!OcL^e1Q@5T*H zX7Yt#H@zgj*A;uqDHI2_o!B>5n#u!3%j>Dj*7bB<1qw^+-tt^Vu_~Jx{3ChaJsQfLnBR72FJNI#ld=F1j5TLr9N(%AFDbZ z97JaEZL^+}v=&Sk7Y=P+5PLav3wb4|Of?1P#>upQOjL5xMU&ae*%LVpLS%Md9*|Yf z6_4xEZ7O40GBEe>Xqo4&IA(0x*4vg>mY+I3QmccgV_h=Ykw)*q?B=B}L(sad?-JoBe;i@Hn3mH_{W6W`)^6>rJjnGDGgXsO4RGd6nzTlaP+ccN zYI!y!GAL}FI(6}VJ7)bbsr!S(E^M1gL9{gcT;EQ_9&y*duEH*jH8{av7MM9dHt1kaL=v3l-Wx&MuCQHjg{z9<;K?;N~yLo0{J*+BQ z1l?{0GtFpX(=0J_VUu=Ur@Ce-KH-tu*b*FS2k=^xF0`O#lF*FP zV1dyyVG@o+E7{GmHUz>Xz3} zJ=NbWHOP)|eQP^__S`ITr%IX_P>_2FqEp$@Zs+mxT8LFw@!$e9et$02(nM>4k1uT3 z2YPf8nvYw^iZb<;0`;T2{@rMVY%`6r5za+ub?VIU70n{aR%JV{$i>ttO0hlSaSf|M zVrN#fq_n_v&vULEN<3=1BsvJs;cQAjspJ8{=V2#lR-1J}nv*+z`(HLnd+zS;m0%L) zP5cP_jYokIztTCU+Z*_X;l@OkaTH=e(?7yEZbU~Rtkeed0%D4~oFoPlD9+cc7}NY2 zC*`nRF*cwljDf_D3=ziF?4_|qcKR^5nWJ!%kWvHJ?pncc0~Kc>tICd(8>@-o9zp|o zaa>Z(By~r;5{PK}z#krY&Hksc!d4}{UU7ae2Xinz;=gFmUm~59=bP-EArqlbe1ow8 zfto&dm;Dzsf@x3*@$LG|OT8@}R;|kcH2a33p||2ylh-k!zGlC$;>5=z+*(6X@$RIN zpVb1R%>-Wm+$QL^NOugepm4}WBe5Jo98bSry;2Mc^cT6T zI%jZ<$8QwwBRr-<;G(<@LCZbRcfwz21w8YSpVlx)ST181&-WZhMUXEE>VT|yy*0oB zgOgo0+dF~aMN@vA&fU_PCBs;t=lKGc56c9vBkk}mz(^##VLhlx*( zNiUFTW00C6EDX=<1_x~KfUbRw3%OL~9pO4?_B%IAX)ib+jU8k)nQMA0EkZbD9I{nA0Q0F&xIGM} z$Z7atH*-sL6UF2==!SpW$2y6PtsRY7CB9|<+3Szr4HIL%^5!*DSNdaHf8Wq^%>aBv zJ4k!8dH~f&fS5Mfm|>@Ov*3%Sa5;~2IaTU+1JM-afk@fV7d3S^3sv-EpLs(@+sT}) zz})32yFGfhb^tgZ^)@|`14DJf8)V^d3tY<~oaw9ldgmHv97a6e7qblI)p*%%Ox|W! zvl2F4uGR28L+RzTaJLMwu`$r0fHZCum0X!qK;~daYW!F;wXXZd6ZkkA#GP?<8J;{c ztVrHy&47emm`TF+>37>R=sO*`fm*{5KgCvu*cshyLlV|ZUtKB`+(lT{|_nG>Q zQ6~u{6|%kM)~Z{7SE4SUm3MX@85iyjm?n&kJSD5*fAiSK`+G(;F)Ch2i>|ED6IY7+<*}u(YZRCB-CV1%qoq^78h5f^QP%4F-E5h`HaPBHpt3 zfmT`!{>bd*A<5xb8?i1)W?EZXx>$IeFgdGrtDyK*8l;%Iek$QX#S`NvJy{E@oZCu< zFDa;1&@cGP?%oR<0i66UJr=Dqzd3332xbc1s={0?Wsw$F)V8b|*E`#1B)_Lg;MRKg z9)Y9Yg-Wda>4N;5h|hPVZ0-rfAb4c@vDLW-{8!9-nVgV0tr*1ElTniOD#p{7@(O}FMamI#${J$~PER0;O$o{*zkc7;_iB4{=C|A`IMF?KZMh~m z?RijvF6^y7Z>e3lb2vim`xyn<+R2%;FC3M0sqI7ut z%db+~jv7lGWWH2$i9+R#vPSxSqH){N*oeIXYqS=RWtlW96_oCC&1qWzw0i!?kG%Od zjtbN^{yMev0tpUOh_ANYX09#Ism-K|4@XG#Eb|vN z6@$n_RXe^6v69(AN(t9P#11%mziUpFadt~iKf7Y%av1^y?63rlYE*D>@)TC!>w|hU z?Zx50PxU5a=|lS~${a@?bnw|GS(29UTX!q6f+2CxE~Q;oq2ue6J`_!!(v#aUX@&Xk z@iM9o(@EAWex2R!*lnNFgMOs=Tgek=>E$g$SS3HjkgyzBC!n1xWC5KxE(T*s5Q19O zut4+BCzSy$e%T41`S3YWlt*B7GWgI(mvZiqf2N13wY!m~#tdPa2yPmbkpgVsB-M^% z&d^?tJe4$9@B_+iilX~qRs`ZZeiPvli1LP2=`NY70AtR>dR#9EI!}L%{=d`3V&-qD z>2V*M{|9W$65V>uIAs*b-)N0V0qL`->u-R6^w2y@4xD?U5SnZR493phi!`D&J-3(s zVP-z@!M22Vod^$~UDH3yhsV46y=XbkG3fmg6wHkyXS#o~=PgpN3x?l;hIfS*FF4_; zSmem}-_Q(h??=ThDwp05`fBSCmz*dwgviTeKcSWRv|@KlT6U|lq3VL2BCCpXG`&ve zb3%Zo`dZZoLX6mCE5dMM9kb)dp8_mOjJRYO|HPf_luD4yzjn-Y9dd&=>n2&b#p&G1 z2(ye&LH2N#y%S0^GWXf&-Slb6pEK0um9;n{6Lh!$khHYv%b1Y!@?+7nHBF9Uv31YY z=EEcCnIg(7>1=Npd_Ioewnkq-#@>?)DMxkU8dDGA=6CglX02mkN=f{(SGy8p8k^p~ ze(1mm+(*o}^jw)~6;j`#)$d--C)ZsWDwsDTDc5ve@i5mv?%oO=e$^)>O?pHJ;+d3>;_YxmShO#dE9GYsx`-Qx|ii;MC-=F#3ML!>X^9 z;=l8ssWH|Nbg`87&w?$4Bk8#gdaEI@_JK9R+B&qWnME%ZWqGR=tt42_8?r3|5X5aa zB3i#DV?;Q(LSckVrhM5|tW~$MPaR>eop726BxI`>;bu`!X-}!_nw4_KKLYx4)q<7H z4#bdaN+VHy3WeqiS>VSmnC1q|CeJ;7`4D?XF@HHP3KW^&c`qG7H7^&pz!LplsL{p$ z0$DFgLBk}8sJ0WSL4V8N9iO>=D8^_-=W@)r)kJJHBe zHOxhxA9LOv;?EqgpqJ_@g4D+$*KZ@?b_-WGUqNcqq!8na@(BNM9zQk;4!P5zRlMbg zG|T^iupJTI>k2dfBM=x1HKRJc)?MEFyz+rDM1>6Yrg4$tS8<|$_AW1jAINSC=`k4e zR;}Zk!jT%98yjt#P3C;&`G5`MnkRI6JM_c8AV65}>FP_~RX5|=E4r9> zC^wdd)ek*k(eg$$uJRA7b9nfeWcmq=18IoD^V#&Lho9M*t;n{iwdmm8Km~Izd-D+` z6uq&A6M$8<7fNym1_pB^e1?L=1JD3ZGA z<%v_UVUeBR;hdT|gvp#}#WB=R4Lb5;hqIC5VrT)y&`EhaB*V}EF39A(h!cT%1kKHu zhW!PP*sUE@Axya#psofMa<_y?`5BP`u<&ec#_NaePd!DtGuqR6GU~K7PoXgWi7ZI< zp7vYYxyyvzj)+q$o}p*tcxqH?N?xOj>Mm%X;0-3)_KgQZ|NVZ!u-Q|7h~FK)8s-}N zwied5KYSpm1ACU4TAQQ|Y_2W^EeRz~^L{)^?@g<*q!~+z2BGPwhKQ=1w5o(Jnw`KB zj{G1)UaMor>{!6FN`p)NS((E1nD_u*0^bl?twOL(3t=1LLogVp@?)=tBl9etbr_sr zXbu=PV(>x^tFpe4Y=$hs^_Fd3oNaz#Yzqn@mMsazUMM{)9lOBFHojiQz*e+X!fj3^ z#Z~*V<8IDML9R;Wn6W|qC(HMmC=LH?#FE^k*_5S`DARz zYn?ds7iVQikee;NjdoTZr11>YfW$(wV)0v^JVExpGP&Ow^&KOKxDD2)T0S9cMtlaK zG+|lVk%u4F)InzGzpB1KJP`HK_&(6C05DLMP)yVyK_(#gOwdugT2+!A==B#)P=%Pq zq1xzzW%33LLnrd()9QK;BzvufTD1i1^=Ujdr~MkPWB}Gf9gx)=$3D6kobnQ<$^`)u zs~b}>LP;98vEN!9yCewD4Efi64Da)Kev}PYYPYOS zGj{x-d)!PR;D_KzBb4fCp7`wXZe;Il@*#m!RZ%`67B$DWy(@+LCuef4k~9ivkq(Z7 zJ(>7=rV7ZKJS|RN*OBP#?>B-kXgotHt+6A*8vN$y{F{_`y;>MU67E3>)P6!5w1XP6 z^&7C%8Spcch8ZPe8Q!s=N}Inzq7mBDwodAMBl7HT@S;PKl?3YXqm9Z2ehkzEAOD4 zOIRn#w| zgB;vjIULHH32MXgq;^oMJO(?aSV@|0-=v$!7xX2i?7z6nItZI_&{)EQ823816Gx#x zZP_*S^5*|-fkxK79js_VWtLrO7COU18stzNbZDB{Mzpy5!9$fX^_v}fzg2l_V4{1O@>I&~P?9P?$9e0q%dsrxxDA0}>=$Htu;oVmG2q~az zI76A+;DqEfxvWd(75ztyv0UG^Eov=rI3P-n-; z6X*g?2(mafr&($+M4i)Ha7)7N2b2##Bo82LS6Bwu74e`*9bouCp( z%H2gf%~ym-Xl1zEH>*6RlS%-b33H>4!+v0_ZvG9N9-=>pF}-u$;L3>H;f9Qt5*7;i z{hyy^kl;Tu0zbOReS@hxte5-?MNAFOCT;P{`J9A?{MSD=I9CaSK!8JWIE3LvDIN3! zgY)eIm0+O677 zeXRMRBbAieaZPPF+uhM25ZFn?Q{XE;jGM3gP%T+CwuPF;Zi$C?cz%=*aRHoZMA7rQ zP7DL-I_BZ|O$uf_SPbm@PmM+OYfz62a}MCI5c?XLZLj&oC2<~C5;k_uaHGF7!=+RM z#FoN(0(AV@ko4pqN){oWKiJ=}XoG&;c&8s#P2HXsg=hr@cbKM7ppPe6b26GFE0Kibqm2p2F?6kw+=3oF3 zH$qopc6jwEB;BMZH+rk%=Rm(6w=QMuF`}hasdx=D3G-&$jY6}-36KtDSbOVVi(k0A zxo=4kJ`7;DUU=_$&R$v8o(x@190hH-78d#o~FcK#SvUIJ=eLn-2T7m|oaO zn0TB#!Sd@dfb-EzOA2oxWl(SG7>}gGv;XLulWjo8d{F;SfZS!nI0Qz#PbRyxM*VOr zuyQYS79QM-oB+Fl@^lPw zJ+RYd8T}lpiHZ4+`keMpe$!S<(vl?{WP(vN8%KbL^737S-0OOs%E+Z!GJl)OUi7tv z*jcvzNZ@c@zhqJNvTMW`*#cd03Rtgw$}btvW(s6wIXaAI5m_NAXMdIB&~qC1Hcs*D zg$K%vSHFBk>y#&8#H8DsXav6(pM&3Red+HJW|oqP7B>BO_SRwMombz8okGbMER^fi z4;pO1{fveg!?UXcd2`;$FF$*9zF?;ogdOvq8C%sPwD}-wLP8{&emmy zp+*27GLVi;!O@+`z9R376sZudI(~m=c2@mDvzfJPr&pXO$$eKN zEXqSw9}s!GPj9I&CSNX(fNhu02fx1zKETI%^KB%T9LobT&eK2{K)R1&6do+(&gE2+ zE@y!E*a=gkwwc>}N{Hb=BZt{FaX3$~5N!<(NSdh63ADe^{zLS!=ck{`=2pz*BGB-jLUzjDL8o<)?KnaCxcUbsir#pL#P|k z_q*EF7WYr$xHkCN7PayS?fu$OWCbA@vF zJIOk{G4~HwkOIV2d%0#Gv6RNt*A=C+USnqxxrObLTH3Zdz|1@hbMIvJ1BTwVxw!@8 z%ZRs^QwQ6zdqSkc9V3mRQZ?N|JUn~3(;?MHTB0GcG*_)JxF&p1)s_G;_$alS=DYKC z{Mn^BW?Zut@0eAZp<_KayT2E};>ZuEDei`;UyO&NuT!EGxct%4Cw6ONZ3!s}*r)Z9 z-HioP!pqwvta4hJE1$SB;==CXnktt{;l13K(_^LpF$L71w8^ zhRE#Xg55U~D{G*WHs-6=Lc(-LuFtliP*T8NjMY`awl)8Js2lhL{2(pB-+w zL6dCs-(WxGEFf-EV$JJ`w=vJM_mo7#S?WgC@Nks(O+b+oRixLx2}>@wz@UXF=#&$66<*kkdhl%P~M)nx7GzD$^^=a zgCthM>pfEuM#o~~{5G-gu=1{3f}_3yVcvc2d&*1j|73kn+m5*P2W;k>?CLBR7-2uJ z>_xTqZV8El2LCD-=5Kl-9t=|G<3QR{QQW#eaPu0TG=m)3@vF<&cC-b!NmX^G)M=rL z9VuS<=SE9~m%=uQC2?-2dF$|GX3Bk06yGY^9?Q**r|XLFRbQdkK#?xg&}$ zqpAo!InCNw2hb3jgmgtWos+*2nQf7;gTLWAN(2CMM{O_6#JwqD7CPNRfUNe1&t3n3 z910KVjtZ+i^QqrgEc(R=`}DHoSZTW>%RuG_f<7W|uxT#u-z1u%*`l&4X%?DB)q6Cm zQ>E|A8gTFjiGC$##BNZ&cQ(zG3Jf`r(KcKTeRs!^uWP#gaHj59c?vMOqDz^@-F7cj z+EnY+IQM{@!h@7&NE=;9asLMVDc7YlQ*%L;`YQ7pE11UxLfGrScN*=x2hq);^^x*8 zb-aIVW_vx3d#47#C8~hqhUGN+a}@qleg6$9(7G=S06X_Ab2>=|nnOoyOF!+{^vv+6 zZB417rk@l%SE}%nGbeh(O;hsuqs=$*$$GjW&v~Bms40}4EPCaqy#Wpn(Oa+@S*z)> zPBv9or%=cdeTyCw4CbFs6&VxRVgsvy#^a(!6U8ROVc5Xug%3@U`SGjA7((>Df!_i# z6diSsz?ZlkSMRORX+W?Az@R{!&~2Y`o1-b<$4=+gHbQ5^p`6Cfhh=T5*xGhVxF=ce zU5&V;qeLrq!x7OcKLS7QX+rKW3idHOSG}pwz-s}%!K_|j^r|J!6>oA%Uuf(3wHTOT zR*y`3EOW$-zyZIP1EBSO8d&A1N@WN5fzkn_xivwR4w|M0G*re9%KD1rZhyA#xPMk< zXltbd%MX%nagY02rhA&g^z!Q^c3L@Q96MP@y?nf*^2yMhZAtn%oy!8gRj)P%u?RoT zrP?LogrrK}0h7DIcEwXrcfT)udU~6iSo_GzCv&ZlGso>o@4&&0#zKlx;%D5{lH-@= z_v4mz%NY9)^L=prHg~>SbVV9balLmbcO(NCoMWr6#-GDUFpDvs(K!J~yF){xbI=SmWsn^alg#7OHsQ zAUTMV`QK&r*RIBdk80rUXLdv|Z}XntCV6k@EHg1i*~GIosxIEI=bpYD^c1K#O?};Z zY8nK}*Qfa!-U;o6eHHgQ(0Y=+o(q))z)>t*3MU|z#EwN;N~c@vxz@nNx4H!H$zQQA z3&g39z9KdD6#}9#5a2My1$nk8ONSoVU}Df;!Hs3A(AOrIbz{#PxoMCX>^L%nusLT4 z$|IL?1{0r6QMVj&^PMk(EOx-&R_+*?qD{t@ z`7sS2rYaT%tBg2WmS*F7Lc$14lJzI~beHDvj^W@eq3w{`-!Pe^Jn~~#G4L3AC?G0G zIA3Cdo>m`ZlIAwtn(~rX^Dg5g=Nzw#=t4B?`F^1`6Nm6YCKCWua;gD_gu zl11Sw4OjcFokUBHJIbfC-jgu3|E(X+we)|)HePA!M5b-aS!8j}8H91x?%Aec`?saJ zYslX}Pc~ok3%00&=7^8MB-ff5wSPfLRMGujyHd!-x=eaU2(9=2`BRAB_3j#=)Y|!@ zjo)u1mh_51~7j{F{X){;6FWu35Xh;&oQs~quqsFt7U+Z(*u{ZdGHY$Rx_Tn<(!pqn+=P}e8 zEA3uC(+C7BxD6(il^S{G)FC0k`73vC^k;hn>6k6?Vqz*mHrt$iCITMn^?a7G-BRLT zktC73rf5ovcZ*40FzirbS)C{jIO>Fjl82SC#NNtjif|+mYYq+I(370A(DGRS*9QJ) z_sO!G+C_G@S8*tc4FQlfKG+u65TD#N#HaRt70DYMDk~$pN+*FXm?NAW6)e1Rs5>)! z4QY<*FoM&kMRMMz#*GacR&=F-qFWTkiW;h*V0(=MfyWpQYMTMYDLm{mAU@i2kHR96 z4Zusl_ioMAMm$7Qp5Z|EU2R%kLzXiEZSx9=)Wdf%j8qxio#4rx_6>eDrK;q^Ssxr% zX7rZ13t}W@Ib)8%YaZaWB`B$<(O?JZZarc^H#?FSjjY|ObtV;q0g!eehIH@)>?Fjx zXto#<*^4fbt(A-y*mlun9BL912o%eG9)i*B^b~hm9oFW{xYHb@8%6d$pa#XTRcTecmMJ+WKmxQSuIOydAQ-?s`pzYUCzq3&szaW9N;$Es9k2u~M-2 znaq~=Q{1+_>X^H$6UCU-dkrG6~VNlyNz+7GruD&%=K46zheyjZ{tE0LC=kBz*pXAY_ zU~&}XJ`*;hD241s2w3M^%ce*NSD5hwI%ey}=o6BTk+V469?)fH&w(p`oc=S-sK?Cz zMc6quXTpVDHnwfswryJ-+qP}nHabSfwr$%^?qr^ts+pP(Z`J&ZbDh2ST0Vy$2ma$= zAT=xyie(*4Vl7^d#P(}FQ9hzq@5X#!U4_f+7{@-g+PPZr=wv~!?cbCj2A8n1EB&cd zLUBnPZ+rAPsejEON|czsiadsyd->#Mwd)v>sIO;&Vn1Y zba~|r?oEw_y-y_TCcY$HDaia_%4POFjD0lLUDGorXOWcuaJT1M;TE+su_m}&oV8Gz z#w1IaI$6%|Vo~8ipShXPiz@iMTF;GqS{v+L({eoTT&5wa@d804&dmj>#0}92XI)lI ziBs=FW~cNE^^-<`R~ZKz4mvdvaO3_vnC!rhR9C*sxZo8Sd+)wgo@6jL#lJ^2{l-(U z^b_X_suO2E|98}MC!0NoQO)pQ2APo}YKMD5*&9l)jA-(YzJXONDs5U>FtaE*xtaS% z9g8Ar4M)dQj))Xd^^>I8T&*TS5A%uBU5AOv6?Za@YwnG_6y0@z>6F+`1#z3lO2RVL zfc}0-f@FB67gzty-W!Bqv>yIpGk!#Ns8ib(S)`zP}xJ`eCUA_QXKPnO6dTp#s!P^`H(8l zp6l#}W?DI(y<$t8>ub&yvO=(3N8La7PU5o-TUZ4B6eOvHP}~DLTg4=b4+{Q4CzKN~ z)^fZuFY+q{)yx(Kp5EBNE|GFxi zEIS@rwz@k-)cJKH`f?HZZJmX*pXN5b?bKuyuR*NUA5{57lOx09?vs@KGFfs=eC=wQ z{isbZC#Jajc1<6_EMz1Y{stBKhtaejEU5S5U?f2xNE~9+r;w~i(|@E-xZcl<7)T(T zBi&80eNif;^^^rINwu`QE)ck5^lAMeGgz4O0`dk{qkqNO)FpmX4^l1~HomsHkZ-8t zo-Ix{!_7vQt4z`t*c8LNl_7-7R1VCg6vJf-+j-1E_E9 zKsYV!v+&EGp6mf7VX|Qj3r4}9Mn{kL_LD=#2V#LwxJZeM@68iM?6cNq` zJmLsc&2!;B>tJXCWVwSzDH~V~Ww5v`ITdEXYv)o>g3UR^0haUgKQL6_G^kfDrr3oo z#E=G{xo{EA!kYV2VY<9knS_NyNmVI``}`L4haU6W$V9wa8BFb~(* zVmB!@*F_SMSlB2TjbzZFH~0|#s0PV_**>*w^6X(^_r4zt=CK9EY$p$M589KA8n9g3 z*Vkaf#C(HZ%YF}DU9$%2NbQrzhICk*3z?(AAhieSXxpBlN_fylT1bena zS-q=83NOGlXpWX+U)tb;99gGM0C?VSB#%4y_^vG$`-L+Em#6%6x_;iP)qO)T*Y1+b zXv^Z7@;m<~B?2@2`vHVuf_@nCs`@!`t8XFOW+G%-Mq7_vNd;^s+lKERO;}iY`cUa6 z>VOY*4p99j3PmPRt?l{Df?jQgnWX=0K28g?acX5-TOt(xy zth^^~{=v?}$m>gGJSUJnwn0$me_>b{m9ZdjWV-7dIM^Cwewyc~bFgE!-D;%{H(O>k zsu(I#&suV!Y!N9w!Hkj0=~AfoL@C)wd;1Qq*!DrE$?j4}i%XVpDG#iwVsYq(*61SH zF#u&=yGWQIgO1feTJfqGEvw^pISjV*G_ByI=MqMm*!uQD z6ClBeeeSi#ADYGl)+1`s%9pq>;_Jax`Qm(F*n6Hyt?N8{N$obNo+IyT348x&O>^$; zjx%F3dVHvw=0_S>Xr>O6?5LgNZ9GwpnyJ0RUQqZOa*HXl5^-ZlXK%@VS(EffmidSe-y)P6%q z!M{YfIZsYnh0V##B%c$M%7T?2j$Q(jFr3C72en~30x=x4TMVSq17M@$KwQWTpiOP$ z{27M8?L(|7Fh4SiMaj{~o65tYp52B$-J)AMuL%jmAJ>du{Yv+}f(S_eS(2YM3XDE4 z?h$*i3~-7=P2%*;;{~-IdHL5|wn1anpWZjFur0lRSLCTnU%jN&sF%Gv9{00!?IVp= zjUlg7Q&{Si&tUIrNDNR(0DFta!?9aI)5&nRwJ0;}E<Iv8~y!sjicH`M#JeSL+ zN#R5nsCN9+VoQM5Pur zU9=>GA-=e$l+LYhRLLW8&(|0H;LlR-!Mt z`G@f<8=9dRf9wP|Gi$b0$Es?&I&1K67On6e*qW8KgJX4syKj&I>=ELvAMuZ>XgI70 zgGtp~-=l2dD$FB@yD+aeMKe? zI!Lz|-;T{P)9)*FLVZZcz=ZWslHNT{rEaM6XCTrQ`8`=h=Qso-2=d@3f>tf-ieo9I~IDtTAsc^hXEWPIU{~4Ht~@>u^=YM(2{{z-EEAA z#QNYdQdoJ0g2;Z9W{!mC9rv%%SDtAV*W;03rRtjZOw-5$f4w`laUi1Hce1T0o5KuI zIEv!6PonWP2Ua#)O!bSG-?tag%f8~D4_UC-th7<7H#6wUGaUHTbnC}UB{Gdr^;hWP z+c20Ovz8(MNrTdCX{-}WXJ*N+$I<|9r5?{76%@cBA$RZD_T4WLkg6-;k(kDx=z%q-2rGnwq{DtaiV^^OU{y3I2JJ+#-fo{lRPbu%J zXjG_XmC@VD=aD~Sn6vA@A_qw>MdX)~esJ7o!uRq^$L8o9V<_5Kp`iE3GoOn)**;^$ zfN)MGhAoj;EZUeSNJf(yrQp~_G%5%raV>EL=SrQBGs7zQLE_n@+m?Oz@BtTv{VJKv z+oVxSs7ATE4HKD%9!kbZ{M%!UIz#-0oIn#?XkA8GYu>3E+xRN@Wzj7QkACpvaaftv zW`DG6O0p6gnujw-FB%v?X0o!EF6RmT3%Io#zCXv!qYNo$-IVauw^$o+xZ`-SjE+bn z$5j0y8;*nAc+NGj8`9Q`;~)-HzSfLSgnLG0=~JJ;Dlckqs~{Q_2B&g>QF~o?7XB@q zpc~629n@t?LVvPtp|odsy+*f+S{BYm2DYOeV&$amLD<8F1!l`VO`-}ly4_UtcijCl z^M@ULkq$p9Iz|SzQHuq-z*91Ui@pl=32|C%Q)nJAycfA~fk5~4z;h&PaR+siFdqtM*nWxH6OW-nW-7HX#UgqnNHTm5SU0m zfFIv<<%l93(jYnO@#IwTmV?|AXX3<=%kNqor`iS=aQiav8la#_P2hFSo+%~jr@hL8 z;Q<>)ub_`o4icTqJJmjcrvfBqI9M>R!i#!5OTVUA4h)tE1r?IMK6a&{M}Z6s(0}jP z#?R^O58iB40{dF`yg8+q`oY^_{!=qPtA|@Or%s*_5Vpe?nD=6s0xgkm^pygCOGxQd zP!mlFgH3KNgiYeWd<0vHR-hxqdoR&HV7%#x)>1uZ5lAlHg~hrS6fP*IN(XH*EDLUjs8b?;ZO3uJOztZ4p%GZJIkm+ZO?^hk1;pq$4;um z6M$mL61X5Y9v!8^0FTiM+9pv{MK=Mf(jn_%8ZP_XF_W}~z+FW@9GG53YAJq1=`2*} zbf3dh?}kbuwuqW-Fm)UOk4N0Tb~|jnzPkKYHv&ViuW;%ByW) zb4Mp)EdY65m2Hq(eV_gH4Uh?aWfFV1w-ec{Q>XwEEGooIggim?)|pwImJwHF1Mt_{X?K%G}r5 znJ}b~PFi`j|G*gJu1sITAeM)%igw)da5qEla^0OurxAdnUT=lbSRl{f65>xehw>qN z*pWe=Q$yp@!#M*EAl$Jsw~FPWeH`)Bw;~`>B|*+;AD$_%6PnsZttSV90{2~&%k~tD z@Ph8mv@9SnVge!um}aUMwXv8nXRriNIDZK`j>#ugmzN5uy>v_lP z^hg~DxO0;m`y4j5uWt-HMC`+km#8R;%C&=?d}=+4v$|i4)#gBQteUlGkx_Yl5|NX} zQ{_)2+ptuH8Y$MH(9zam{kh2|Q6p#EFCvdn9)ZJ)0X++3agV`|chXs%`)}4am^%9H zK5C^F*OyhtwNWeOGZ+Cdjd3FD;czF0H#O0=qcK%OS4!`-c#V*G7iwOE0B=IkiW*aA ztWbmNGeAPGU7U0oGvVIY5r!N5B@;lt>MUZaO)Q9v%}a9nO0vs3R~A5vrj|3T(+3BI zeNZWS)k1XciTz;jwfuKU)SWqi{4|s=uzXx7wZp$uokC+u^xH4O#fYR)qkrQ2YOFB6 z78CTe5Z0%h3Ob^Q36BWFh@~JuiU7^+pJ9&t&_#zr@atXKbIcBIoX7R$IJ6 z7BeDeq*S%1Z871z=m=D=>y_KmoNaF)yW#(7k9BLTtmBwhubBv_!j7c82Om(huc_|p z#W)RJO$6e59yX132i!fRj;vBW5xX0vAYE}O_`7rAuK&L?BJ2t5Hl+wiiA^cZf&fml#b%TJP$7XK{e3yYDY=1mr%U+eXouO9;E5 z3Se46gl*J^Y53&;C6^WFySurr!hHR377LTHs4$&XKM^lNF;b=Z?C(UJV>QkCU%*VV z^3_w|>cG|*)W3R|jJhojMVA*o>ERZ%pe5?SQ9mYtpSN;o>WAt-x0|GS0HT8u(~bIg zEQZg!uufD`>cmHPDJIApVxkUNh<{5FD*{G}5fF*77=&&G5`JOG5rN^|lXVyAs^;BP zEq`UlaN(37tNcvw21}C(Qr)I91I3Il8hSQFN<<$=<2e3qhj0HdOMAB&)e zl+x4X2oxp0uL4?uB6iqBlXQ(6ZS^cKQ5``OS_6Zdk5|<^!*?pFu;9d+qvef$@V`|Y zlOjuP>e09C6zGtACgWAro4t*7D0%o8_cvC``|rL1u`bR?RsrC~aw^oH5-EdK=-j%# z5Hg{ttD@eJBpi)^&?BQ~>P)m~@r&9)sxh4=bl7V6ig(Kg&ex4{pn$@#C>+;UfCEgn zi$Xmpq9ei3A4D!){h{b0L?M4^g(&;h z!QP#L4|ZpXZI1ZtA<~YPIDU62&*&;g@HN*?f`F={Sa+Vmi8Ya-w`gi3b z`;IuK#P!cZEwb8wLQWyR@HltyDLlX4xBwU2L&1zNF9XYzs#!cN1}ffdS0{>yf} zSC&WU<5YVW0dlCn-_6SShH3NIGThU3{@aeBe$H3NH0|;>^=CkyGkn5rkJcNUmzqJt z{U{A=8Nrs>K=d|wS{DikiH3)hKyw{lIR?=$6XmQ~2&OW6UAK1&wka3eb@usp`dg4+_ zL!!laAkc9=ptHuZzav7SdGWI!Gp-A1N6aV1O_-^=^vhj@OgcM# zKI9cdizV;-UR1I4#4ab1=>7C({-vYFi_C8?-tOn!rzF3zf+M3U|EXC^Aw?sIGQL}{v%^f)g#})Q^YqB67LUxDbonb=l+}e|!f6>p2 zhT1&V8?EJaOHhPJu9HEg(~u*Fy*~Ap^vQA5@d>uP^Ql^XWo7$szg4=ggQQH}ijUXQ z@oAAypjcH&Xj!2bjfeFl2yFu>h1c3cJ9Vr?ld z$6p~oey~;i%v8o;V(UkDGo^++COwr_BXGRh@9>c9Jbd*Z2&_EmgU^F-RZ?eKKAuC+ z}tHKr=(Y;bT?#Oao!MpH2h|gC~94~Mq*e= z5CGZ7obasj0{r8%8oVQ(Hs=81Mlf@VZnS{fX%=S=ZD}Ob@ZFL z)3W&=M+MzwmJ&c18%cuNP3F32;%L~+Z6E%??mDnL3Ogp`}O~~ zU5S7$sy-n{wDcd>Q+$g7RVyzaDwQ0`|GusIkqax7A%?+X7ngBSy?h<>=;Bj2D=?=z zgwbzoAP8JaK$V++$#M?rG?RqO`{Pp8v{}(ZVvpx-HW$J0_Uk%FoIq>_pq_o~+x3!& z?BzMoxZIdCmAGU{9~$UCI&pS}>Kcr~^_(z6_T6?*(Xm1Fc8fowKIcAVe+fMWLRDt> zC;u;r!O^jZrV@ReuQ&H+oujmm)i#u)pH1@tx7qvpDCt zHl4`E?7CnJX-?Ex$lHnF5mx^=x$PIvc*JKuYhE_xVO43t5W6+=B^T~Y@cZQ(8S=W2 zH@+%mc^GYaek<(}LA=Q}8|dNuUYKx51+z!kjl~q5wTmJ^R?VP! z-$P@vvefhHa?IrwVH7`-xdT1W2)##NKDZSmTifSSlm&nIeS`c5{V1Mg*PH`G*Y=xv z+U)>+RJd?L3`#aZ^)Ng|z#x`acduI~k1@8xtb7GTFfsREc+01*d{SpFoq*QRZ#W0q zCgR$Iz8~&6JzG`3I=-OO0&wEn@7O(Qu)Gx{t?ScIK#iTg#AE9$?M`q4=&n`?Yr|?{ z_YY9-e=^rb$5tKUUkiK(+u|JKmCd5T_yIdJ{m?Z9hDT5mz`mQ8Do zR$F?Up`I=Z$emlSp99_Y`DSqcy&pI<1>&mG;@C)mJqs}7D-N|(XAlyx173oGWZl7o zWQ_h@M$$j|KUfNAm~o7`_lteatXOM{)+Ari)a{b}vm*$B?#Ut&40)154FzV{j8 z4L9r%H^9=y3dFK*XjgAN?Uoy7n-u7lI6Yiw`fuvbwz%Z{Nu_f#r%31k>I};5`t#0U zjK(1+s5U|8#67R_hBGZckGoY|APlUbD!dO#TsF-4{Oml=eRpz&23X7$=tE zH5%ZuT&H)ryvO-bojfccA0}y~P}BBgvm0X!Dvcx%H}yuENtm+s@f#2iwKxe`*}Ij& zOBX9B-f05X2+~Bhj7>3**Utm=-Y#LDiNMK)BxjPuq$zoTbQ1ED(8rHzZ5~;BE39B1 z6lDD9OR=DqvmX=lT=c29#GRFGvkarB@gEH+CS__{2)hy*|JJKNGnRe#;WG=tP1+La3qnn083--v;KVCu z#tBjiq&)99YfeN)C+qUH^Fe2^SFq5p2K)n4FZF7UB*_>B^ve*KRzq;MRV2N%Lj1cN zA@}XRR?7^ma-CxNxUIEOT4XjHmjyc0l0f~F%g!+t1zba3DeB>q;8UUOc5uf*e`hS1 zm4(o1(f>Xf=zs&}`i?{_E@SBKRB^g@4`L7}{y zJ*e^}2!;#O?-NjPxaZ6smn4cP&E|xiMJn%WwMinFw;%X&f6GM^Gupe*wmXIR1%Ku? zjJA}q1h(t&SOh2=M9>JOFxref4B&56o3cz8eJZaXnKH*>$zyo`6HUj=3n`d)~*~lpKOkqHU zQEzy|j_%M7Wq=M1w}W`F!lYfzS&D9fjB`)Mojb&i!^lY=-tYUl|0!uR1RWkrI-jYQ z|8*dF&LrNDR##p{{-%>P+#P_!b0`u+{hAcL{kA{i%8wRK*b}=~+v=^dFG%Q1)1YCJ zQ*grD{l%yrCl@VAyjePa#sQsymjP%&U?_AVoNYBdjRXkvzZLiFg$qgBuY?at5mL22 z6|h1&%GOW#Q0;G=_Du^CxK4(Ku6c;Zf4DI?`^>cRImfAd8Y5&Z;AGrsD9mj9G*`7=)n3qLBwHbf+`Uf`6 z?EjKHFGQ7(qmdAnZ-VfKJcZ2Mc&}?bzo_e(KxAJ;3X~U=apF#^z0;)i2Ob8~^DV57 zS^`)YQzF>(tr5EMpFw?Yyw9Wav+cjA0|VM-I;5}Vo&cNU7(x~|dQ?{jP@l3*D%-A$ z{-)nZj`xfFA&5~SZ=r)tpWowA-J>Pk{)6OevV7cd&^J>O{?o7_mGbT(P&IDx5cXlnn(d1`0*|IY zg6s&H4ccH(x?c);`U|U{wY#A2QD}a5*u21TYOQKnYblJXn}Bi8XPaVB`+xQoGt|4! zGK;8p0MKA|AaC8AdVC@NRxsmJu-ZmA{uW49Ae#A@A`#W)|z8%B60Jlr6=M) zSFFW=13A%OfBxCch9E>YW-~< zNkETA#f0(^KqtK?t*-mrhoO1Ij@|_RY=-b1hW}q5bWfRr-_py1y@Mmo^hEB+;mESY z;Xnkx8Efqo7{`JI^hHrfd)q|R(5d+eN_Fe-=1ISC6eB3+jxDTIl;@%sI2r1-$odza zHd6%-&Ln#Qu0CyP`E7$P(r`FzzO6opYzo&|E)Tf{)Ihw)5q}Qe!sXU=NI-OLz5oDS zDS;G>GK_jWFY8KZ`|!;U4@oi7L5+B4)m<=}64*j9xx$7as`!UWmi{6YMpvuwuAJO_ z*3CX9Cw}l`m)C^+bBAA9%B%f8Oap1^qa(>(F{hC8kW)tSbQ{b-D{0UeIM_Hqqo6SV z6eX+l;b@!O(>nzcmie*s$*hmlcMh6wJSwLEfrOh3J&W_d8x9}-Vyc)WN$ER&wX4-A zbB~?$u6>ArOWSs|kSXLfj2X1r6_n&_V5wQeOb=yy_X#uRZ7B2p_K^>*buI~sbl2NK zsV-HI2iM+iJ-CR-s$02b`-s0DY&<+2Uir)SdoA4yH|8z#LV`iB>90eKw+h3p#fTUm z0~_iaN9}-~3>$ETia5tyAYD-HG8KaG$kJ@J#p~O;ZRFg|$@D>Na1e|I36ft5zoU*> z)??cni+BiGH(X}L*m%|v>@mymHm}JQA`J*z%n5F0=OQo2fc4zCn%1|Lxzc{pN_u|!Z~Qy`=f5z_w@|^xg2d%#VAg%h zyK+S#W3_wwnoVpL>?>8oB5&Z~IFKJ)o?Rg01SH1uh;f%hBd{PXGgDDNXr6hIsoRPi zAoX+-tY6;~2c3Wr*BmLla^za%b#*@Jc03Qf&IO^=6mb#DNBVw4y*GIMhRoQRe8sNn zZo%fzP=r7rqB`|g>!IP-e&jwFoCu2u*%!k1H_WKWQ}(C*M`L?<^WmqaRypgXqYv1t z+z>>Q#d{U_v&=o=wV5G|J{+rqoHV?_Zd+5lXIHleFJ1m3IDp#oQms(EnfCp4t(sm8 zj;>s<%Zl873cJEDD{$~GaZn=uoq%p?X0(!)hj-kBfh7H!ufddd<8h8RY^PC&a$eUl z7?Y&A3I!tjy;szv))|_T%ShYk+Cq%yOO_KRxLNN~Dt^itdDx}*-ZZAY$7R(n5Up`B z`8qz{z^kRp6SJhMtB)T9fYU>0s#yuexgWk0Cp1RSPg+PMTrYKhE(YNr|0b>=I>-1$ zk)SdsD#)@9;P1LK>u}Dk{@S}YYQ6*Ux+>}C+C81veA48cUd`DMIC}R+uFh1VB2D*; z3=gNruIA?cuu&|sG*mg@4|f+d`P+U0;t<;0P!|-UEX?ZxtgoUAC*{;oUo>7HX(>5j#cQ;xk?m1kAZOlt*5n z>5mn*;7@f+aS$c7V%UKil2wCIJic@|-`!e(&e)~nZ5??AEm6BMrSBJ%`lf@psZ zz>sNLJ+%kjrX4o0+n+Z#Jf3?a@6e$=Y@Nr)^Pkw<`Zv#8AsYC~Q^okXp3@&Tfe3a_ zw;%KG#8l)+-(ef)1cev|KK&L4-F4KTKX$2N-f_wmE6EG%$mYQ$13yv>=ekmj^s*!8 z5MnZ@>t2MFwRO53pkn=0#5R8B#AQ+@?xRXgGgO$EHsDUcU?L)v#1MK6T2+lb-W|Y& zE7VG_&dYrFidattS=xJ4(xMzH4p3<|dPPw-_Ys@lI!t2aNAE@UclWu&X>#zr-P;W2 z-#;0tk>(VdMAK?1=$k|BwRd}Dw;OPROnu7_vY=uuyTtMoi>ymhrf;A$m2SyPKc$>(=FG%+dIS}Ep26-Bj ze{eb-;@Y{Pi&WL+6{W5=|NIw6~~Gj zjo`RA0gIx=*owgXju~{aZks5;FKm~2faX|`{%BAUkS3Db6n$*z(4 zm%qFgxn_NjU)fF1=M08|F}~Gaq+fmo{xBL4%N|8X7QoVVR(PYEG$8HEvd51xKmn8o7Bjc991eFscU?7!S$b!wK zi0x}!4ug}@1m%f5<5GmvPW~AiQys3SSlbjRnM^5xfSQ&gf+p5%(y+}}7beEBEq?;mdM;HJq_ z3O}30g}`O6+`eWwYx8^~wRwa&k+jwgp-~AnQq`(VP|V%yN|vq;bP3^%0kZ=#gJxpw@|~=`1(;fd zp|CAJvE>{~8Wgv*TVVvF)qd62gik;ib0movey24j^b%hmb|r}v1?(3v)o+;m-D-Pa z%X|zcTx@-0$|leWFd=pF-DU5KPwb#Dl{HJK$q9S&so(|U22Q1Xu!sAw4g0>SoT`o&NO^U|}!U z2!G%fYPN%?Z8hI2VEsA+-ArQiGdB(97yV9}2lU7kqlK;R?#hl<=!rJ|sf?FLjEV&d zy$=E&{Qh`GbS(XBAbPhRjdC+xmMKbx3#NvQN|6tOl3Mt>nBtWns+S9>)DxtBH4|>*Q)Prj z(e|n$O4(54%Bg0G@1_*eBH6Q$XM*$XQ1$CEK8XkMSyqbL3O2Y;ySPYn^UvG$Jf3>0FGR{vD&LnS8Iet?SUQ}l8a+$O`-`FVF?B?U`jz}dn z7?6Ek4(WMQUg@1M4&mG)v%-$#ccsm*Sdb~fFN7mjh0Svy%W=AP>1KXoh=VJ;rsYP@ ze1685KxW}u&~MIlr=bv)-wVBKq2d@3`|j4^qgouOh$#3sveQr`RBWjeJIf~RpE3i~ zpY9(4P;_1%7zL41QTCDS_BFc%7AC*X?D*<=X1Z80OE0d)HA`7sPq6`ZoA~00*H5IZ zo8t#NQO2W=@I!e}CuvbKm0R=wjL^af1>8XN3;+%FNOS^gdNw7nep?KWu47&1#;S&l zXb?lw6^c=?(D~+vcCkOaLBAKrF(^N!%}j+J;~VHDi5be&h3J(`DY)N}UG%}3Ysq~X zjFiTX4#5~znT2qh4h-io!?vrGbhWldcQ`jlPV0|_1k)J{=96jOmjS2zEYI3KL<2cW zcKslu`|(&pG?}Sp9C}yrf=zjks=mmMsB@;!)W*kFeFq1#3-HyGquE5efA>Aivie2(7R`GiB2hsDU+GEA34i%MSeo?BnQ@z|QjQvncF_<4-=^mW=!sgp zKwU?Vg%Hf8W%j{6lZ#DoH}IluRWQEz$X&&vD<%zp1yYY}7pkG2Ug&Uhlk&3^%|InA zd05vk%g*R3y!-sRQzCn=mU(y`5f)>*(m-$uq>hi&3$gj^uG0DK7XYox0Wq_4;db0m z%A;`MOARo@!$r*HMinDjVarh%*OL#kj4V)QoHWB97z58x&C>;IgKpFy^<8jdsT)HJwSkxuid5b}hOXQXak{RM3< zCY!LhkOS@0D!IB(f%cDJY0ulIX*lcVH+U#MUv7%wlw*1(y9}0PE@M z;yS?M!G{VREzO@@$&xKWX4We1+XNR$lTKk^ZeM+llgbW25G|~{-`iKUGb>9rs17Tv zwUbF}zQUm-YCk>0>Zo-~Tq{>RWsG{*?2X+ou02n5}Z-UZTM+O`6 z^nxZjA#TZx^d5*{7MH;Bl8xu%th~UBs z5(9^a%oF+}2^A)h!Cii6h7Y?9FBUVQwbKz^3maZ(lV$c2#yo>*hp=wtG!b-sXvQYy zv^|!7(!ai-NH*9f;_>&i5AaxOEa2%M@S%Rziw0t&Q)h?pwm9p<-~yBzE$rzkUc=Tw zXqL(!V_`QKEBrKQUl}y!FA3UNd9Km+#uSgzdZR5{6nHY^ANJ&RLs5e*pfZVPfyq1|_H2!l0_(Dhgjv zJ#FniYn-;q^M%bavJ&z(G8lH%s+OW@xiHk&l91V4g~W76Rl!0(%n&Lghk1>%OC>5u z3TK_c(HwG21=?zu+&;jK=Wht(*cp1J^h=E|i7GKLdCgRToB3?Svztd_Quz)>iz!I7 zHTM(3`%Ia{L$P50)+#@zLsvbrgSHOQgxz!Crp?TP(}1wzRcupUgC19Y&h7ifGOP_| zMHzd)yTE<=`>*%*JYGt?N{Lm(QAh74Hf67V|f??9n9;V&ZzfhG5B5khUQ?SxP)zP2_&<&K?gSEL*;iET09(hF&C?zz1oYVodQ2bF}| z5H0sr^xFSLjjmW7Px27ZSj?nP%5sd4v8hx+tt1vOg>M!Mcq=Bw1LgZHm!Ckg(CD;gEd?a8{zs~zfdG+mrY7x@cb8ZX+WLVbf8Yy93gfjGc8#EE9Ptc_R# zHt8Q-rf?WP^s|jo&xZY_kR(B;^_56TEMPZQ6&fGAkV^wXvq^DyWR4VVZq15frKGoM zlyo7)fQ(6D-4|@Cc6hzG`zjY@Qs#{BgUJ>_MfO%WPr-y?Mrh{-|8F6T4qv9o;S_pNQ^0L^dYg_jnT~aou zR9_AGH3&9L?ZKPmK>)k~*%LFKd>@+&TlR15XB4mWVJO7Kd^sy5j#pc<@qe-bpy7^8H2}RR_s8^h=u2U(0 z3>YGeBL<)_^I89zCOB8fK1&Z)3+%N^{Vorr8go|mQUAnzQ@e-sST3q_YGsA7l zG$QcBS}fJU+f#D*Sl-w&-<}t6Hir2kxC0<}&Z^eC^;nK1SNk)*8~LF+W_2>N|Kx2u z#)Si057q*v7vQ{}r)f9HN||%v z2SPzja5WH3YUxX7GAS=1m#CBrlASKEZWBSC{kwk4)CCwHZYX}GJb)m#n6BB8GOJX> zXhz4PBb|0TFqw0-bkO9J{k_7E3sM-hvh&AT!P<1Um3VL>6I(>Zlau+mA!~ULM^akD zauS!*pR{2*drsHeRo5gj6(#dV9QLFubvGwbdQhK&T0xz$lf3Y?c~PZZ^>3C_YhwpB zjckQ`KS&!kzoz6L+Rcg&E|C2Jmb+p0EsB-9hR>xSH2HdRA6vV&B!L`#Vb$DwySJf} zKVtmqwppeOmMrX5|w&X^7C7f1ZW04y~!|LVEsUff? zuxT$9vu3sKz3%BEv~f66GP6GUASFp^Fx?4mXiP1QsG8;ddHRECDo<`j;y7Q=+K81T z{wO?&0)E$C)T48eP&|bfw1;|p0VfZR21*R%uF3|^4A4HW-)Hm4twq|Y6b&W<+!)kz z;T}zpg%F80?>&xfc=1z7<=o-kijtozKrs-`?4NB&(pT3wtI)`}Brw=|=cu1jq_zN# zq(;su?mZMo#V%iEz-vlDTF0McqNIm@GE0j$)`K}fi`QJo(0_B>+0W++OQ$E zcCEIf`v#ur1_JM4PIJ*+;GBdH3cwi5M>@Zbz}}*z37f929%LoG>|z4rP{V~;Sg2s} zS(d;B$S?P*2?s8k>7n~2+gz!uifmjDp9Q6897ZU_ahmnl{}m|b=hU`p?LuzX+UR6N z%rUVucjW?SsKHZg7+F@N6oE*Rx;FkNnGomyAQNKdVE@0)AQmD{CU%bhV-Wg(WkOuc zOkDrlB}DZ9mkGtSfNP=J;&RA&aFQ?y@kmAlx_V@=j0Pg(#wL-LmZ4K$pp^=XbO|dX zAt9k5k(G+eWIKGm_so6u{089dd0ca~?W}njconS8Oqg4eR31Zl1Xd0iNeLGwM$!q1 zE6$)$fgm9xqoN=qlOGbsWhDEg95<-|DxU|yfp|ZtvgsCVZpuK zgAITbGFb$oF5`nRDXpQ4e3F6#5oI1Rl8o=*#=k;-1Q*^CL9!iqTo46zB_jic-rg$) z>>7eCqh8P-!@K>il-xHkoio=A;HmEu5FlYK=A3K#RV#sk#pe42-WN={g$w`;EBh1` zlrl?}8aVL$_`+rkl^6*l(=Q1THpTvHDPaeLNkxuF%1FzA5E%eP3a*R4Yl18RDULfJ zy-&s75@Ve}zkzWTZ3Tjf<`JC}lB9BuV}b~g_!9ju#?C256JT4TZ5z|JHEr9rZB65E z+qP}nwr!i!wyit4$w}_RdAN`DTuD`S_WHinU-kp>?M(~<0vw~jfU*Z>GQ*2gL-v3j2^y4=%Vdo1Z zrJ#aKNJ&jX1p*Wy9R^xdBm&(3gJ1#`@J)5to7HR}XXiJi|5O0^ z90%ry1f>9k{GKsS0SyEbwj&`P_5&aS4GK1}Bi#V^v!%z}0~1I3>QI(c^s55}0Sn2q zwAgEfr+jan2Zw%g61fTI9)f<;uUq@+^WDDZZ+rFK=gavypuMBN-K{}c@a@y*L-<*n zpZFTR_V>~HIUsz~2U>W@WjPhdIX$^@Jwi}0Kk-FM=O4S7`U&wn?Lj6S)fW;0?Y;WJ zW?O^wcMvxE+fse;4b(WIx39vH0C*WA6WpW2sbPSWr zb!7Tv+1bdtr4K_bZesUFd!YHlGy1|;ub1DVjdN$bk%Qf=52nlKX2es!BFDll$Nt!3 zqJ4Hm$h>bOxafHysIDh?Y-qbyowmJ^1+K@pU=kv_RO$nY{Y?)^!`B;`qsgpQo4iES zQ-h{93V8vHZE~OiVVilTcH))0w z8(y1&DfRjGpj+sChCkLv+W9sZhLk&rZss>hMVC{(TLj(s_=A}6{P7SA+}UyBAf2GO zfDsfBe$)okGG(u#+@@kkWKm2)4|6E$rPWw4I>GA^fEiD{{FL*2Z+9f-xS^Rz)c#d1 z-lGfHX|G&TqS-RYi;8p~5u*`{HhLmK3JGVbIPvIyp1w(0k8CMK0VOp`_e1aTPQlwd zh^#m|U$n%QFzUi=x-#Hti|*3W;E0*8TuGWhzqbl_A*iAKRN1^H{gt|Lk{8=&`44RI zLVq#6GQFltx_W3SyyMqV1m_h}d(w+k3d0>B;hqYX^>*=33jTFXFh{mkq`0m0Y)-zH z^72zmgq*N^Z%dtdsEYTX?E3xWef2vR54xi)to23@{@ThwAK~3xi&LlQX-J|7b2ld? zaFOb?v{%y@gUQ1R$n{x|L$3KtC+<~|{_DMNBn{}KJIsIs*KDYork_ODtdmdhAVKO4 z?=XaU*WOA&_L?m3O$jxO1#~6Bp>EU5JavhRRA!%R$c^1!g29E=1WpcmEH`I(D=U#R z{B8$I&$>Z_RA6R|G_D^>Z|;J{WwQviM#J-B_awcD%0Q>HGQm@8sQWc7ce=TgYX*XYh*QbGH@ns=dSd^RxN*i)zeN^@d26dK=LxVM+xNw#E>SSx4d8JB2}PGy^m4#u`B8n9TxyLXI2Tt z?nZ^WSeHN6ki8MAyRmI$wA0;t*)hHpLs-{x#zR-zY@W5V=g#qPb5!SB-`k?n{N;*8 zA)BG4@JQsX(Q<=wqPM`zgM8u^ex|r7!+4JA+V#szh8=zRL}x=jVGSA{mRrh|+htc> z*cUhZf@M|uc^CtPOvRc?HqVhhRZZ(mFk7AKB8ssurYx@_zNcR3sy3Wv*HzZtuX~iW zi-2XrMbYiiL{rmd0v=jZ6>`6#*GrMWOA?vCR?0>R7Ujw-avHxMsUc@Fpt`fE{>on? zx#Qt@xW1XE>ppPrc4c58dR^pt7J~yM8mu(w54q{4bgRbNqiPkGtqbyVPq^9ITAz>; zrU2)Ocqp)2QurbaULK6~LUo$1vZ?aB4}bspMc7OY5dSMCS~Z!hSV zZFG_L0pw4TA-L?x{wK**R&yxgSd!%EMvG#Zn2)n(-V&Fm2xNj*up9j zpRe&vGdKDzUWV^$UtSxm_@Bh%${YmkRuR0|mtTR4+sLfQ0WPw=q8B#cCnF91in3iw zTqJ~*K3#B|8CfiM-SYZ5<9uHF!_8!AQ-#<>#0w4jJvWCX15xtEs44o1d^fXcb$xU< z1lv}~74EkInAk#xvTDD?MLkuIFdS3TG(H#diXQUc)TRq5EuNEHnyyg>Lmg+5Sj3i` z`h>_+xF8P5I%yTBY3IzoJAOQWx5HCek8r~zv|3Y?b&x0d#HE9ZIq*uTV|69ACRCc1 za%FO=X!h$S;l7z%euP(uMM3ZQ&1a8?=A_A>FUD`bCb$B6vR8ulZW=3XUAW~>fGtx_ z_mnvXJ{Lk+4JTn9)Y`N6HCkyMU#)gx0n4hnGGfldE>9C&^Xghk=T9>?ap)lr3dzBQ zcS4`25Nt7hN1y1=8G96!JuH`;|A2Sviy0cpwW(_~IEO4-9OLRD+AQ=|*W2g>H+wRBiRSjcd1dJ)WlHt*I2{G9!@fw5!=5?r22r z#nE==*Rg?N7?6eZ>1T`xzU$3^d;2qwBRki&+~31osyo^GcegYIgo+$U9@Lb^jaF$HRs-?OlOFn~JE9@aInF@D1o8U$9oFWxc zFKmhQwW>Mz+*Zypj2+0$!B9+R4GbNw>gp{jBwqv0lx;7E<%cvZ)%M_DUZbY@U`VGW z__VqwhlqA4;?p z^;+Tl!qNg+&r~mvjkdOpK8SS1GlQW>u9LS0J0iPMBQnC@AL+`9_qUy`F-ks(s-xTy zTrCb>qWx0k&rYDOw+PF6w`@e1%ti(27zMw;J8vv+MILVSx@F}3;e2Ocd^xZ)tD3P{ z1>cR%NNj`#GVt&pC_*~)fUTbfa~qs9f*Yxruhojq&oapKJ)7n>Yjd|EE)0`o7xU`1 zI6ES&x#!%%vDzY$BOB^Q7=Smb5Li&P8d8M+ln3X$PR#M+(gTVe!p-V0f z9p}vA17h8A@u6!_e)rNzL`s@y6{|;DW=WN$xbw)W14tVZhJ?^*33Y~Yv;d6!*Vl7` zN(u5uCR3Ez+{DusIe8LR73MVN&v~x0>?BThp=-+sd>BVlIxK#Do1XmR>L^Yd+xt>! zM_E!RqaYEnEW*7%O3hErk#oL8t+!d zcf(IZS+nOBoDgI6$8>%9jj(O@GJ+?#1NHJvfx($d4}?(}?@@VKfG1jI(Lm%yQXtds z)`edAZs?Z_h5I}AYCk4CmFC1ho$Ef0H*H;k9v*744C8G^{zbfDa}Px^zx{k*4)TA0 zjss`})hlpv|lQ%!@ z$B58KDPH_WenZr>l`1-cDVn$NGm9sK_W78k`#ZIp2$R$R!AK}Ch9YJVpc5e4DzT!J zq!%Lx&AHk3D8u&3_SC;;sg+I8IcZ#2PNQgSy7;M?&UFApB?~1^MgR+eMDed<*h!!&(7>_B0W} zmRudL#p|0P0M@Rcu;kJ~q}~UuMAuw%OH=Hg0Ypw?K2Et>fxJ zTHJLM%-~ypj}pO0;U{;3oDalQrcc=`PhDmxwW59KNIFaf4$3cxhOcag!(ngZxh+)} ziL+A9_AcE-tU|*l2+52=F<@U#+^nz{&2elAcmG>04n{uQuK`eU})R^NtYZMH5rMPR3kW9t@LnP>>}$e%tK$XHk!0CbKv$jF~2iY zdbo`FIS8Qf4#t0r(bVgkfxhu+9~SA|#4=L>+{y`^_gw8uT$F6J(EsD65fZTCJXNK* z8vmoXs7#T&6w_J^N`x;K{3J*FwS&$7ID$Lk-WpXck_w6T2SfzV_{d&QG>Wk$Grshz zk1FWWc2*C+$w)$c_n>pKbgijl#wF`h!-9fvcV><(T+i36%K=2d68 zzptCh#tUtH1=b|M31u{j;W-K;<#>D^@QgM+HJwghzd$l7de^)hfUi=t*%*-30cn9= z7-oQ7A-~0HH_*S`8%9L%{hRB%W2*k??V=?2S^`Oylve=v}{6Dif!GIJargT zyfFO}kigWZzr_*+{nOa%9!(#PN~w0QTsY_WO72fQ#IJYtVLeUYD|gd;fX}Jm$hTKr z%u$i9+#Qg!pmcdePri-s*4!Ic;mXN6-vF_V^Ld|OTeNa6au9OmJ~lp4te*|}yiiE$ zQoIb#7!ZI`jjip#ko=xT$uXAY-$gyab7HiRoF80u#KkPO(3OYo&LLWdh#+8x(bgMk zaSJ@h6j^(*OQLSA%m|4k3NxRarn;6~n|MF82M_Z)+X@fnsss>)0j0a8z6uW0)lKT? zac#Pj(xiorz?^6cB?*Fvo}4(^K`-BV)nT}&g4z0rb=W+(bIEJ>vEb<5FF$S$ntgB-E7e3YmpkvGEjB zUDXOIEADpY0EuZ>8C(f7QsnoL>hh%CmL}A3tiY{}ns!mhv1#repH3B{#J1=v@U@yd5ggebkWIC7@aqviPMBIO>&Cu7wceq)Gk|S{v?txDn$BRozv58%#AQmpIt(Q?KFXM=!qC%q z1oT_;{I;|}Z>52whURIOJ^q&mT(&b8x`XgSv+!)PgG85WTi0)!eZ3W+fF(5gM z=rnB&m=V0SyM0DbcG+I90BlHo{`lZ_PDykBm4y8GN4CMi!ekoj;PPSx%JC+Sz(7bt z0@HBFM`9;Bt0F6*vALf9f8m_~?MDVJ6K^woX$*j9{Ek1-i>9ninyHZR72=Nh7odreB!!9G~ z?U-f9#G;2tAwyVcSFXTmlLqWgJ(czg~|4ng}*Q$=v#RDywD>tPAdB-#rEQUpWi)#tXC`c5-#j(%qm&hsJpyNwqBSpyKyTD~-G0 zx$+(se*2Uf6(c^xbrCIvI6K9qYS7$uJ2Vicpri_;NUe8@TcEo7%oCUNA{M82ViIBt z%`RB95J=m*1z#or$?w5jkrx#4kK?VpU6Hs^m`-T79^$hkK%L?bsQ9ygr zB><+%M_S~d&e*Vv1L)$CtyWFLmGH$W90koP>?3Cfa^yneag0MzLPb03yW=Cc56VQN*qs+b|#DWNXSIfLQl!VmwiaVBeDL zDo$bXxM;&rQ0>b1;_{y~WsJSPNs91KOFy)Y;G}P(_i~Kr)8?Zitk)IpHO0%CcrT_s z?Zc2E%y%x-&4VkS8xO7q1CJww$i=w%pEEU$%NQqCl`oe1HC?j{TboLI)iRk0qD+aRYEzq+^mtEDEpl`rHB*%85`NLT@f<-RiIexI zmr*U0{F_f6d_=k$_fkm#=TrOUlVyfh^4P7LY`mfMbX+cX$e@yQ0n*k7AD3mnF@E(G z^aaOQ-eZENCL_K3#0ktB|z zU!glrM_#4sULQkS$otqDt5%hp77&oJ`-ryHhTu(EYeOf^N*iM^|I#|nMfA=04dQ|( zW%+uC8kar@;=9$#qAlrBuiY@mczafV}fwv(lt>QbYbtj z-k!sI>3v6|N$|(?A@@a~6@FzwL>jcwz`A?rIp=Do#3xT8+P(Z@3EW*Nbo$?TUVfJ_ zli*b8nH)adzea3NnJ#_AJI+mlrEKcD{x`uTQ#~c*Nckv*7*j^5uiY3zsuDIuzk;3x z&ci3l3?lO&sd-%4sO^n9YLrGLX0Cte=!_#^Lbpa}=GWp7V$>b}*s5GjH%Zr#e!DH% zm%>$65I9wJUN*s#u%su=KuCM&z#f}(G0u-DjNqTI>ok`e1k>~>ZR>90uRoazl<;-@ ziN{a3e1T($>np_agRH@-RyP{xY02?@YpB-cc&g<#87);2q{_owEgU>!agHi?+96X z==ZF8MP31*U6FTcU}%oAN`$-$Si;5Mp+w;%YM#;%0NLHtjZoy^dTodSd$hU2F*8dn zy`kp=#|bHR*LNl`HL&!+K1mg1g73rG!Ukv?li-dP^d3XrZp+I`KIJ6Q)W(Q5j|J~5 z3@}$rmo`GG1gl>J6UlgU=rv2F*|72sea6@$w7mCo8)B})5|f%+<$9dIY%YlC9k@L! z9vHFu>GnI}7<*2$gOHZ9R#1_C()=59EK$DN7C%VSA=-3HOM|J%2s|P%S*>m<<&Ir% zCOccYO2kdD6e%IINA)qH{ae4PQ6p`SW;uM^#Qbiv6kcXIasdQvSBYz)Beqd;*@O8~ zJ(tU`V)x^%$h%X`E?9>f%|w;CJZ%xY$$T@Fwr&zsjyu1!jxK+R@;-5+?lnuz=CpTo z7#2ilLThpVY?$B(WAKvfJjIxbL=KQ#3lEJ_=y%G~NrX%5a=2obmS;f#OPT6?qTxeQ zp+MuQwQJCd>%U!UCNY3cVJ(MnD`zkGI9b2nKwMx3xxsgXa`WSl7?48Z@qe=T6KihQR~!&6g|Us=~mxTFY)eo*gB=RN?}CHmOi4)eUo zRprpJ{IKrcVq7wmR6^b44nr?`@U*Esy`~iJVaG9|$d*`D+?sXYv(nCCO0f8=_nad# z^*gh_tC+l@I*l#Hqvw8;&ya963K_f*Md-()vUskR)4V<`xV-7-k?F5Q)NIXBh?k$_ z5B5{fyG>5_f=Z#vrQ`C7v0mw$z3PYqZ z_zigwpTw>=2kA7Kz^^~8E)%xj?W3K!pts_=th8mPRg*Hg)*u4rzqtk(F?#<4JTKX8Qp0l{z zF9O<)z2X1H2yp&4Mu3x(GqWXiIV$eWz&s8r%P?k>}(@j+XB0nKjex7MMKm1C8wrl?$&S-21hfAkfWZcag&N9)D&!?kW;7Z}dZ7Xh+wV(4AJ!?(c~)!*hv3L+zIZG8>YX1l-T7>K!!6)KOmxDU)e zkO9cArKA_se?`uP{!IXba1Gny6XjO@LclMnj2h^bX`larGo2&gibzgI&fsE~ndzs* zozqNGTiKo)2Pcm1-)`B}7sWt4ZhW%6lljN7U9H8XrX}Y`@Sky8xpCJxjcb$HY9jzI zORxduXQ(SJ?q_XE5DAC^7LlDBkpk!t2&g?>gW>0Q-o7p3`{elh<|k8NYiV~jZ{MXU zTparv+~Ymu`Xclx2#5e~w%|_AkIN^$keMlXT7WDekZ~O6+OIdnk4P+&XLg@PYqz&Uf=JJqMrf=Wo~V&7yf-;UB39?#)mAT3WEz284o^WBLR$lOqG8{nt$dmjsYbe;L(2 zB-F@W(}z~whnE4(KpR177rDLN+O$;KUnxHap6g7gm8q*?i;-+UF2LD(Y6$nMqTeHV zsm1TRe55wAmomNp0vrkqpwm+*Cvl(lPmCexTN7_hW#za{{c{Kg=JgFhJtZJ|W>3(m zOWb&0eHIJ~Kh4M1Gr$nI!LN_NHsGnhKcZVzKxSwBVR(JCACUGyX-D6A7;bl0>{v|9JD!&Bjv#@_eyXd=tej@eMBC#xN0C%GU9|KmO`a|Qe20reL z>z_0SM^)*vKj8Y90=|2&+)(#7r>S>SKYhDUuK=!^RJCbeU$CyS6ZwV7tv%m<45dnb zEc&Nv-`z`JxJ&vX7dFBN-}oLGYec6YKmU~y1Lj8l#jYu{p9Vn}-*7VV}$u7AS0g6c9;w~MyL-`45WT?5f*bnxW*IeN@Z>b-cSG@gu$cFBw z!)N+e2O)gl{-62E@W#S}$h;qq`L3`x{@2=A1f(%|lYlM`bMpZL6HSaIA7v7$!-+QL z-$1Sz9)*J=9C`+RP*`zUY+GUKv!6|*`sB@Pv(%;r>&2wUc8SegzSr%#@*8_I_QMj? z%>cPSxZE#ZPk-3@A2QOs5DLMm)wwk<>GbOlfRbP&!OYkb>dN2zxX?pZJ3D%U!PNg` zp-|bj@3|DuTxTT6!?+D~Y4Niy6IPE5VsmF9JMjYGDI7`Z`;WF8}vtzrJbk7cfTy&d1;c+O57zV_ctUf>EUS? zJGI&+jmR*tX>WR1HrboM_h@s8KMai+!a@WY^=`|JK0;kdRK=4bHKCj~J2OnGY$QFp z3O0lUC9v)mi!*H>?TVl;}2CgCbJg12;aM>eklhb^6LxGft(#cFz$zy|jVxtYB0Nl9YX1_hVn2=W6gG z@Yaim`0uaB4(_11@bZGJclUtRO9CcqS+`ARSd-ZOK(WZp+UpVEpZ3Cv_ z>y_!CmNNq%%J#jw_5Z~fD8HAJRi&$A@;BjJtchJ-@kws^QX__*IiX@RW~P?+eh5_561guMr`NE3&WEOd zBQ5M*qk>RL!pq&}kSy{XDM5V=h84Crp;(4a4Y!ZY#Xh(ISLj5VAgW>DF_0G(@i*q3o51uEG4&1K4^PH;GB;yd)N+F1vyF+S4j%`y6CLUn<-$F zCR>-2?Jo~k>i8D&AV@V`8!?J;yr1Siic$!+NmA3m=jc8*nF`w_%;~9?J!kp+b;{$c zh;t=-oX%#v6#EL88=a4v*Ama1Uv9WU1p2-r40g$s?^Ga4-3+2Pp`2g=vd*P&E216P z!N^yN{g}mnC4Zr(yNlJyARD z7_W3nYsQ1hC6+O$ppl>FpG=&p>pP7P$R=#4@UW*+^6m{gJed~bC@Xz-^O#)J3A;hU z0kU78AnFrIF+yvPvVDEgG%*k6{mKcoF_yWyTfOSqm~&5KWArN?XS%efA)Q9-J&HUN z=H_`;mp`WiXVWTbW7a zC6d^rwt%i5)u5jedE5vLCjq)Ae{vAc8=_aX=N1XKEW(L@tf2iL&&+*MRR)lirgWx) z=X26WHA$E72fE1#XPqQ;M=x||))TVx)U$-m5df^j_rJLwrbFvyGRoKP)WjRp*dD)e z2sI(Ax#FRcWD)CBE$qs=V;nDHQKr0oVhw~cL~GY_3<+^lj=$afF^~lkGM6ALK-4gm z-@6Ici~{dvfdX9RqSWRF#l7jhmQq!bqFXZQyJVbt0drtz3U->Gh3w&k=#@)Mic1%ar^ua`i;QmZPIcwH6(=e7%%vTc~~9kx1|%u^n{8M!e|c_EL|{(N(`o zrkxMloIH({nsDMl%=pIy;nEd)&Y$TPFpm7h1B36$m|kkW10kvPr{CSIAtbjk2K>(w zxg`~koMVU8q^J9foGHo9!| z@6is0#-bdV&;1n(0my6lYl7TI^K&KI)-NIZGh(M+u;TH{jMHycdLKWY0cZ@f*3*rr z%eeS?mr^nUW*8Q}rH^;a-V=&xV(Jd=-^AU)59IaR(Lu!={_%r&$r4hYn3Gby*ZrWY zd=d=2Z6@eCs)XunnaiIHu%~mhxR_sOO|_dTmROhBp;fD}@yPrl9C+>CnIloO_4S~y zJOPK$5Kn;ol71DaLp4-qd^sMECnoojw51@e-8zwz(0Q~2y0-|Dl7(C^GFu`&uibkS zdFZ_FU})MAi@YS-$kc@J2LA%xL47)d&`Q6m@+&{)Dk{ygt*SBn;l z?Keu9@Jl@P$AV}n*k_z1ZCcy~EmiZTfOxkk^S`ztS#Y<2t)krYzd}fiF;Gdh!VA0N zSsH2p$$bmjR`~)(1rT}A-7@dmc1I>@>oIzbf3aESdZXsb;@u}vIJ`~Wz^99N9ynIM zXal5L2acOU1ox$WzJ%#n%q-bp=gYZ|oh6*xBlibAcoXwOl-~JciwFSyr?F8>#e3p9 z`<=bc>&8O6noS$cN8Gw2&1+ZRo#8yLl1zrE_pr@?J1rckOjhNQ2I$X3IbcGvd4&cw z@j74#gA4*cD`kQ41|J@!0B+$cX=+{UlhXU{VQ9g}KDH_Lu?T!nD>e*2Z;wbp zLScNfRy8&}%2)ae8Fvt@)Wz&Hsfw<=u7(iq$ZdS*9_=DAOcj>blH@>Ly|0xA{A@aj zrP6#5BIEKOUvmcEm|CiR^4IS0X!f@*GF@tlyoQ3}K>51sWMKg*^K@sH44Ir)_Pemi zCzG{mKc4fziY~k}IY(`4Zk`HEE&Z54*xN257E#qa>=Ym^^lucnPy$K{LG^XIE6?g4 zVhPF>(}-hTpivLPfb|#UVSLE|Oe+E4$AM+7>NOHTem3)5pSmEjeG?XzSVhZM8BKO} zaGW5)py`k}+XA)#2ad>J0l3y2d}v3dm2BxZjLvI+o$YFa_(9*_{3>)u1pB5G{t9vt zCy2umAkXK@!n*fcaP65T)0?2P!QamC_y`U|JW!K~OT9t_vm^ zA=4y8p(+dT^+nqTXnFdT4=tT&y@!i3IUD0bR~V=UId_&bOu*|rC{|7Zv)Wb_cktAs zIL`RYb_tv%S9X4HWbKriQm{d0CQ|IvmIMpEm2}eh%uY&2tHl6K#a&KKU#e}5_zE# zJZ%{DDb6$yk}O(|=?)iXJPg&Cu3<7W!FPyLK*wP3mrUNuDM2pSslr|Db_XY^$dXk? z|NA&xl@ek0w&SDow-b&NhgFBV@Jk%+O*uEi?w}ghg-OtS+;KT@2u&n|1RD6;V>07A z{V%>~j;}}n$Ik?K&-WN5N4GCj%J#4ME zA6dsfDw|1)Z}83JNri?newi`SE4X}+Y!%r;n6*%J3FNhkC=DOf?_j3%hy#&FAyGa)l1u;8 z)!e$#{9(aouEKw_jIGlmCx<_ugeJ!pxm`OvS}`K$>h0ByT1HF*usZ3dE% z?;KZT5@lgD_G1^U)mc|C(n_C7joCeN#Qa-z1sdZ$VHk?D?GFjU`QANo^MPamqjzQ! zSuXQvDM)!wII?`p^kZ(c9Fp&!%sobqPXZneP?Z$G2o;>Vfe!j=cYg+XL8!AKV*@qg zhbl|jBZ;-=!Y-x>aanY5!Qef!M*keraR3PP`d-=2lwB@p4y&yL7~Wey11zxW7IwY8 zdEFS=m+`L3h0iGmtADrlMpzTE{VEm%LjM}@NW8PB6gwBtdnv~lSu+Ta&7J^QuHurrG888eN{mme&%FiMItSZ;+969@z>0=+f(Fb&r^P-}EiX=V zaT68n7=yKYP~u!eV8R91hQ#w8W!U8!cdRd4X3oY1qB8NKD&_a@rer#Bm0K+ z*riJ7Tng3A|7I!*V~+viqws8BFdE0MHdcA|_ZOhw%TtCVNz^F`;pP0OD}#Ef>692{ z+&Pv3V&?H;aB^+Hdr*ZlIfswHky>~o`@+P>b6qT-3fhmZ=mQ6?j7)6Mx5^u-e7{lK zC(&FfmNksXnyRX@y2t*_oi zh&=x4QdvwHfiJm)uFgmBFC;IZgY^PUrJrDxIi$78r~;I8a3*FybQ|3xrjaq*sgn99 z!P8LH)nxK%Z@elXT4WbLNrip{8x>q;EiV!RUZd&~6*1S4miAPBRs_ZsAk=0KOH+Kd z>l$koDY{bQeaVGpEm%~D|FF9E`{eEpKDt;UTaVzfLGJvBXxN6h8t2raKCTn+`+S^e zrm#NBgKZ~&5cM%Y1cfn8hN7+u-=N->uiEnUoHTFw7)F(Gh%4JITUtsSn?w}TEoDGs z;idcg*W~J846|LEU3=F~69|Oz0{gyCPzNG~0kBV9+un$JezIg*v0?Dt=1M|GJO{N3 zP%EeeNjFffMqeA!@D9GxY8RI-==_`-8w^p-_jrHNrP$f8o0iZgns>3%V1`nor7Uqh z?9tnx;+!epnd^Z<3GUmIHOe#qlwN1{HyoR2Pfz!Sp&aOnbM4YNkxF5~BC+43{n@Py zk$;A~_KUHvrxvckWLH(5V!sL{6AzjeJ`t$wVa`|PPIw0jfxIo>4!7f4U;(R$?7r&8 zl_slkMfnF==a^aVl6QWrj)nG@Fke%-#Khg$QL!*rO_qN8xHx-P)q<3TKv2>YSf#^J z_5vl{1vDV^w`W6`CK~+}CGBM?3qslZtcxCF#nBuT`a<+Bk!G(d$I2C{bZW}h5Utyb?&A&R(F)CSe(oa=c zsd_M*?%A8O&9pbQ$9(VB^#$xEx+zo@iW5Sb5KB=(tF3ykpZN5_D2}dk{Xzj3cYiskc2OOka7fF4y zSmbwUiNTDFbQ)+f!`FRLD5{8u{A6eF9vkuddGN9p9{yeCXtC2-qjU^ zTOm&t%c{$0lnlztk7=sWz0GO526~rKIyZipu|`m5{Nj=iOu&qVqX8AX(>(j1?mkAK z<^@4TjTv)6TGNNY+P^IV_=p`aEzonQ>itXs-`0Jb^3NyaH%m_@z zld-dr4hQGqs1Y5H=&_NUXL}_Y>MJ>dW*(N&7!8V=D>tG~@mXgwFZm&;{gB$9> zegW9r1y{qbP^g12$8s%h<^e^{P9PrM zAFGy+uzycp)?YxEPkpH@G;=H&xbM!q;LS!$wT8{0cGceL);D^CJ60&)k)5Ls@%t?t zGDROLg}fVlVQ7G=c7>jR!PL^3%=cG4A#+w(T~lpyDQSz5ai7)Pt;yR;KIDRH0Ht?^ zngcYdERpLy|1b!5N}UhUl=KY6-SA_$L{i}uYJyD(O)&(m6*%#CB-j_&A94(v$PIO< zlB7s-!!R%U3r+uAW4m$iYc_R0IQBI4; zgSNc?vF7fK%B}~~C!m2?%>UO*H^wOK|L;-dc=606Q=_Mu4sr4?%$KuwSwBS7EipF! zoED1vSoFjYVP`J5jzz3$>+t7!7kEO1fI&&WyMPkC1zFSbFHgL;8;@>rxmV`qaXurd z>ThmuSW?qz-5@11)H$yb-ql-9_5zFLWft(CU$B$DD~G(**qddm>>{hV=yRP126j=Z z3abCoDBU3D%aI&0tVqq@>%2rOSA$G#m;dFD6}bE-4q<7B=2ft*2yymn%7K5arEi(C zcG9d0gCMlD-8W+Rr09K%mN;@%>E}FdH0tcTVfdGzWqVc+@G?-~0@E{ED0taK);?^^ zBA^{2LSECyr$aO-&A;}pi3Tfy0n&Ak}T zu#Re(g2}ah<}JhJE3xNhWVnfhd-iD`r|fGy9BHUk`P5?|{J_qWx@bKD?pHbS8yhJ2 zpzOR@1OL!Dv}wQ}@{c#vGfHLDHO?{+_JF423ks+X>Z(i>1Rj3Him;=n)Bg6PlHS9@ z+NB`7X7cp@6&MNOIh;CDD}3kdU%504q}xo$Iu3OojSF1bX;R`)`g1;QiP{ew#*qmE zWtC7T6sSZ6HNLGwQE#W4YTX&X%ohI@XqiqfsKA?MI#B`ENyZO|*VYok<*~+_6(Tnb zZA(w>xu_g4x8h_gJ#yJq@x2$Yv@Vy;=_b#5QS}k=50{E0Q%-9+`*f`J4=&izYiPR; znzP?oP1CslseW`?PKx1%)iTgjUYCZK)N{-TO?jT0`d_FApLJEGLp#sfr8^6-lNor`B`pHtwHAe|xie6O1E(wll2;I;s?j zlcv2|Jw&=xkl2QnFJ7t8P(dTDOsq5a7^1(4OSZOv+QwI;)@PcH}In5w@4nK z&vQ5vl3d&%1wDA&l{0ny(*|k2Fbti_t3rM@d`_Q>ET+Hmv`tWnHGQkVn(lbDh z)+G-5JmiWls(>wFE=A%HxBm#7dyZm0HOU8j8fOI+@jF2Gy%|4Nb+HVKTOGTA)Re?j z!%cy)$(!N5YXb?%J{0c^1%zI8+(wL-zD(sQl%2R6sRC{9NpBa`k3GURBaABJ66~*IkvVCM5mdJz} zSn(%;9vz!%PwC<@gc=c!w`Bcv=GtE*J=)S06{;b_>Pc#i0tB9(e7pJZ-%4hRv}7aY zo1XI)kb2}|o<(2tYkoe#Vj<-^kvTlMmY|EmL&>mDhAmSN-PdDe^kUi2NSx?CBc0M# zg`CY~#ZpihMTK)4#cBkaJRbUq+mvp4v3R4OS`uDlJiwa|$>-5zaX34A&6dJo5Sr1E zj8SB8eXRst>Il=e2%E?xL1Bfc)60b+J=on?(pBT9(Rl};*@VL>n&+@63Unxhl&823 zs1AwL^HFI$HX)$o^1cm&$<`M6&^$$VH;(r9@YWjo2@iTN1;(m1g1!i<_|W3b;A<~j z)-hO^<&BDhQ6Jxyj25U_nLP7R&rSUsXeRbPMZBQjrhgGq!`_c7f>v=7@3jcN#6a|} zQnoH)$ebV##$L``{9lMgVhbTFQM=V)+POF36(P{uXM7PR^Kz^^81#C3XGxxUC0TSP zr|$IaJm8ARZLe5HX+m%9YVSneA!Om4$Y;VY+@IRXSL=rOGEs9?$W&QgBCfe@PLoY` z3_8+&v8e8<{Tb?U%5e`Xm>_UR@GX9|Y)K%duANlvs0}^FiI5g^5O`#ox zLLf0EFhM-v^L^-vaLoM+$aHj?ctysf(OvhXQN^Q!C08%HRS_Iy9?iq#HSSzOOA(!j zRszp)DJb>lKF-`PKJPjhMSNGHY=1Uc0ZIE3_V}3pO8qc**>Mb=0jos;s@Yq<=r{LZ zhv*qoY@0kF;-Ac<7q3Ek!vJQps!_SEG-*G_r>?M}n^7vK?6-Oc|BI3+K~223mt(*r zL=p{N@~-E{QeFyC%ZedYn}hCZl;Jb!^1DTQjCjy zQBpW1dnN#kj;j-v;$3gUtE`BkkWcULFJ@b7^2i!@ajycK+ThnU{mt-@6A>^%ivqMW z(Z-$j6&Peyhu+iK@hG`MM3^tTT z=QmaD50;!TOqy+hNF(7_c10&|*lrAH9F=rx@@}eP&mtM@64UU$dY6~(xnmmVs%uMy zit>mq*?WFI`=YJ0mP|8#-yp%DNWiDqYO$@aU9uY7+{n|^=17_oMK+CX2L0eq*GM8H z)s%EEri}z{CjL4s8VX+{=S12=G>oK*9KZ*b%N1{3vunXym0?oe`k+S-u>rv}I1yb+ zoDY}n=u5y}bz?G{oku!D#$A0r9gi@k8lHGB%j!-+eJSi))0<;z86Kv%n-uX{M;RyJ zfE+K!3Pr!OtY&4@-zC#@#NagdEx313klh1klC3z(*2PHl2Q@;{nv^p%Y~M`GBD=_X zuFIY(qT-~zXlFTj_{3&jKb=^L{e=Wj%5~}z)IxmX!Gr_$r}?v_r4LDvAGe!SgB|H5 zG9@0AQ?4=lB1+m2UJQQ(o2Q!anuhO)Jfqa6T+8J5gce#AGrUROEMuP}`*GQVr)Mv< zx`$UcP)H15jrwRH*VlB69S7I4kVfs@f}Nj=5w2YXu}JRSb6nKXu{aYDy{_2h>q|Q9 z$5^K!85?(fK?|@aWGgC6l-F#id^iSmol4}vfSfVvm$-_b zf1FyH&#DfuWTlvSQ6nUc*LylC2@kfXp@;81#V~a{G2a+t5tXLf)+x6rB&N%@UnJ{ z;W79%#<8jz_{O|eiU)cgWkYvM!MO2Ph944-7zMq0gR5N#v4@YMC(bX>M_pIsj$=}C zGbopsg@vk_0JpLnWw$G44_{O`nT20UeeH@29d3(XotiX+hew!ml(oNsuMvU^+pocG zG`bOCxvt@`{Cw^EMaSyegJsq>$xJD(8pdBS!j)97>#j;h;zO}Q0&gyr6&03DKUsez z-({|Q)1<#hcgf~h{Qaz3ds-%>`hCE}@1mL8;gMjTTt^m7g{<6?`GIA->r_qvD!L9- z(pz5CEgvl{_R=vx!xUFByHmlZTbh9E+Bbe$SbsUmB;;-3Hu31YXOMM4L-tBD9iWsg z9sf01FY!CBjXR+D^I3jl1A3aHkR3fO;ZVUsGLFR-xo|i#vNV zW;A%7oyLKVPO*&o94Evet;CJ=_jh**b{*NuRHKOcVlHT`O`0xB zAYn@g>5U`o?*@w|H>c3zRv|?~^RE(ZAi?PYAvSo#wE5Kyozm@8NjL(E;VLfG&A*bn zrhM&wKkE`tka@4kw5;Tw7+IDr_EzZXCZcB{rxY2WorHgtm&L$NfE-Y>W~{+nQ6tc7 zO&xIbhraQy+{&+oUo%%6U~bY|P|LHR%QUMDuqwAtPuwvHi97x#MbrlFF(7m>q=80O zg%ZTuq16R(d+*@fYm7XKKts=L-PC5?LNydJ_wd5P&Sa(B@W*?-h5VL)hb~6L`Kqj? ziiWGm5sBKCME4qUXUQ_m{%9z~?g#;e_9xC|t8P+mcO5QEjAt@XK;mWQVt2IXe1Tmk zUHXXo$vx>lCbv~uVcUz_-L)xY@zja|33}bk6DDg878(K-9!d)Rc%5a_dtEB?^sihtOLnG?00ADWAQRB3vgK|8n4Cepjfq+8HRLS zV%Ogm$vXz3SJwhszJc9>p-ik=!V5hi(251ZsnzUI&6ud00V{`A`f?iy;K zK%|1HbEUv3H@wTj#GA~zhx1$J6|QhPlYBZLRB5!f%gQyUPLqMLXnnh+YDD%=qYUAD z9;R=v(S0_g6(qj+j%>>OVlOR&LA^sz!}yO?&9fVN13ol4*&ij+q2VIa-6=CqNqXh@ z!4V7JRQiq!l$9^fc&&RW+PqzEvzE~BLso7$jE*_RGj=7`Zvd4u2`}jH30NPqXNA+{ zxQj1Wx4sp3sEU^@&DxPhWW{({BuQE<-lDARkjV8Nmx^Uk`@}yf&Ae z&-G8!wBF!mhdRYH6HTOW+h^a1+#%}z!)GVDZ`t1IQM{ALtpT16`EDl3e^l5!pmEh* zXI5sh6B;u~5E=5yotamU40t7j?MARJ$K2Wx4asqgeQ_-7g>X|q6s0Ks1Xjvql#+}? zA?NaP@b*)?0nQH&agH!cw7}Ef?n6BzkhkIiC(`!N2gR>C{1YZ_Xi(GBw!u3DTXicxosB3y9vB_jI?+!8%VHU>x@94=d&L>uy;6z|1rF4Q& zxJ#@49c4%m5;D`r{hgOIy$YZ8Hqub$6^>iJkmX`hL*;eeL*VeR)-whS)F|(aD=)bx zE%jZc%h%(5*mD-v*#x%j7nto+ zMwr@tLTITU)=w!(eHcGVLH_UpAAjc8#U4y(NixN*R5@$owun&e65nZ0fv+mWsNcAQ zMip|;9T?lDk~L9ps+Q(#5uZY5ws1b8&doo{C9*GpYQ;GPsVp6Hw1f>h3l+KleYW||dN(xfvKq=K_ZMkK&L<+oGB!+Q=lo^&ed zs2T1Yu`>o7mN}gP>Z)E1(vnTde5zcMNVe z_zv~^>{U~#U7qH+%o@vNY$7TUnY<#V4Gghy&M#>B9kxMpaau@T_c_2%Tp`?Sg^}W| z)+fgchk8%94=nbhp(S=G0fwYju-CR(KeDc9vzsm7$Nn6DPCQA`xsZW3CNz1vhCk>R zFjV(6Y{Dpy@_Ey4luLk?Whcyi#%(0gEvd3_tJB)TVtQhGg}t84?>Q^5xpFB%9Q9%Y zbYhgKyvtI@63#=ht>;PSFzStWpF6~@1&so)*#kNZjWK`8Th~;aH%(*Vq|W5s;vYmn zW_J{Qv%LfK$f=bM8aFYPf;@TwF722#54yCkHD@TiM4{(9*0up6X48A%^%`7-?e9|E z4Gsm$4g16EjU;@-#@txOoIJ!v*Q2=wskU1*!H2M9>M*Y-2h-8L`Gsy7;hpW}Q{1*T z^$vru1mRO4b{-oFbL*9bpQnNO+xC9iD;CYVl&&X`djzh9)|qTPKdKw}Ts?i(%Wm4B z(ZwGN!`bkcU|wQWlBH6X7_6L06f=>uKm%H~Tr!~X*>@G{j1k(U`QpZimpQ$AST&~Z zI{40xeIp*zAgW9mvS=Ts#`j7wN*-0?B;O4&{b*!pKHBIbs%FD`NG~05vmVFFutN#^ z3EMpdqW*;)%(x|AmPmbWhgUnoS;6Mo!n>@~rNSD@U0RWdD{Bf*cIovl=0O(L^`@eS ziGt#vEEqu%STJ-B-0IWL^X0n-+_q(J0^G~2H!CVi*cSU|Gq z{tNDQ)s2-}mxn;AXay)lyvDF)gf9X-#ZE#u+l$x3fkDAkh{Kde^6}b_5UR1@n^E8Q zeF6c_fs%)yLMu-+Hu(t1B2FDD#p$jAL~5=arPfebN%iA=wuBPNvDk40smaB|&>e3m zUrJ6384-K@W0Wkt29uHEbO|ONdBa%46D499EvGDSa6?dUh9d)T&SHdP-``NWl_2TH zG2%U0x)HAjFUYO|Kz4%mLsdJOR9(c;K<5=~Y#h7f1ngS}H#U|&40AaI;fDD7H3}Ac zfh&(S(H&cLAi0fwo#C%x!y#?l^l)(K4gUHDU>;2tv*}3}V84u-%$m>ii(tRl5LF&@YMOHU zd^V$xXRL}YQ~|@NIZp%OYOj7QqT8U>fyt*7YGEf*@F)O-KAQDU{ z$0)38m2%RQa=<95jg)9rcvvO3=Tw@&+=BG82MdSjp_=}-0;kX@A&O}7hk1ymR|Zy`BKts zmqdu1DcN92?ql-Pk}e8#n&KTQtt-j>(A}aBrpo3{noLO+Cxgvyaj6Rz_+8u0L{mT* z=cDW+ddZ2ghB8CHH7s<6_*i4_$-7%7y_vR)rhEvut9%4hITfFTB-JE_fh zM!_&-CvqqkqG36)Dm!&M&L5K#Xg7zHBH<8{E&gpTqfpIg`zQ_-{1q#p>dGc!6&coN zd39UNeRH%=uwSts4I389UcJafZJV-4)Y{fexDCl%QN&9k0HerlDKF(>86iE;YY}W$+>xsF?9=`dhX`XLs5sdIVHYFXJ6b`=@6Y^ha@J-$%AGj|Yk- z`X%TvzV@8nuBxrsPJ z^80!F@q(ls+@&47Nnd6*Gv0ohx3#-hR-5CO*Hu4ISxYj>8{5E!U`at5!kHQy>!ARo zDJe(oKsGknG&VN4B*M8tQSBw(SugJhy-02qcg zfSV#9M?4@$Js=jYuYY9Zr%oUu2_OJ{3f%&rYy_YZJ`p2u@7Ur`I}U7oin}@?ZdURaO=b z%-l={*uDvw&@89}h;R)MHlcY!67?AHZ5D$7m<7apG!_gMtH31o<>$W_uMw0h2v;Cr z7F@fYb8P#Kb8vVY_z=`%8#n(~9jJmUVEqTJ`cWN#@-HC>RNq+t)492`-U}0W^+O6W zJx$~WFW`+(-;LIXYy$&aK0(>j?E()v7EuKf zFbn!qdSY`8>K4G&$PsMouWa(Ad;FGMaz#r@ev>U(a z>W=mR?*}G7wti;22S)%$fCgU@;4OewaMr(EZ$>~J5paG8e1G>YKkEOF0Cx)jTf>EB z0MQJ*Qv9j-wgK1t4$Y(Afx3ieF!5f&=MK1fx!IvUF!$6HxU}W{hWc^tt=^Z)JeTT- z@n`&Ql@|iuhriuF1Y>Y^Xylzti6-d#=(>Hop|TRV-O;W8N}{s2hx&JYU&~hyj9*p8 ztG!?XFE0*rAm5$o(1)7Mgn%u6Vs@hgGiLwq`X7GjFZqd|ya9jpx8BH4f1KZr4b5K% zr5^`BKW%|-2HNqxAi1I!z`pnZ)3FE4wIBO(y3@J>RdCG!-OKM)Y6LJogrK(W<;L`k z^>&T5w(mc|Ii`KrgfOfOOPlZXa-hTYq zd9H71FsGk9Hgo)|uQ;@)c6ta!)6DdekFMA%pLUaGp~Cfc-8-$(#_7!Mc9p}$s7NH z`SCOnAaqaQJpY6p(qR}YI22vAmP1pqZMSn7!2As z&v>(Q!$Qfbqe6~bB}g;(_p)74Sn_hvWl|oy)KW|zp61aVkR$WbP3+0-w+b0wmQ5tY zpmaMAR3wQW`VJ$uGF&c_j6IUo-eOHV6$^fm3{xj+Q!V+@UM@qB0xkaJ3ctXfBX9Jp zp}p1@XyTOi+FtH#;7&BM=7YT|JDy(X{}>!VRlnzfsi zh{K#t$cC){#Jp`$O0(pLHJju6SXKVT0DSa1gqKD*)54Tp*8!n58Xs9sTbooB_bY94 za$)_!IB=FrT6vOwAUMbf8(77Z;OsKY!6M$J|vEHsc>g=4m!DG0xTWI2E`A`co*?$g} zzN8-g)-gQXTI+2qo;a!>oS^Ac|W2UHl_ zrtLsc9J>Xi!T!CsSiH()Z|1{pS%wB@x8wF{+NWKXQgMW!vsgN@+w z%UXpHgrWdEiZY6cgr0_!e5uF<^vEu&J@sSgNRHI!U=7v!g{7%aca;9`7H&BN&^uik znKQ&ifTsEeMn(z=y?9AD#6pX*5V(4^oDUakJf66e@N^aj0($i3XLL|DZ@|Y?GVF)y__pO&jTJ8)0~VFtinad!2&TC{O)xra(%ktuVHM1pION_oWPSP6cHbEe{<2Goq_& ziK6`kE0qyiqYiLkznNfLeqdbD15+E05MdEhdN=^y2MrFuv0M1s?cZfNyY+^{9T)c3r)ez+Fnqf7o>nt1P5+28 zD90t>8R>C$!~9MW4+t?M5WCj_k1?hS>Lc1y6z#BDufuZnLtnh~IBzCW%!KsMd}KX4 zyCJW2qP^{^Gy{LXNsR=r;tUH}ehN1(Kbc{{OIN9f_EzI>^UU^2)44P5HJ$haf0r>+ z((x&?T<1OH?fcfId*J29FV(rxb;zT9vV^s!#Pl)r>EULKy(5Kt$Sd&`I+g|cQ$S74 zpB0W=Av@DyPbqYh^1j45=kTL@ zz7wZ_T-Gy$pabN?OZ~$`BH?VP?XaP_z;hsikyV4MAi*Ilj76%UFVI1FJpt6jH$57T z?QFGt=>jpdOS%^vk(+P*DY;D(=XmZs#l@J@-kmTCID_(S0lU7V1$^u^{U!|kZqh--@HEMIJ%Qq6^_~ww;{3Ar; zAtpGKYF;gA-;faFS2GmpMlNP*NKK<&_MT_r?+vF=wZFhxI5N=lI!?dklZIzVPlfjo zsH~D76px%WlwK0>BmbseSVY!?+eL2eOktP)sw`N7>nv1dPKOQ{}p!oK(*lpXB`<~9JPM&XAq|K}(g(K*u6N{{M9!Zl&&DSUU|NOGpU zdOY>oczW`X78uEU-)-x4fjOPtQg*pG`J%PDIfa{@cFKodR-F7H45_yg=J48R#^l8F z2^UL23M;qJb-Q@wbc#Cr?*36u4M_CdKeiYZjDLk&^EVuDK!mt%u!JMM4uXcXr@v?; zC}DHW_$?VNB-VatO5%;InQ9Q!A)-+%?;Gl%WgP}4yNqR@y2<~xYQxXP{4i;$IPjB?b*GUOj9t92`k9MsGQcJH#|qpPHnvkL)1 zD!Jt?v~VrL-Op1d?V2rN6WNZlvtVY!m6kQ$K?_~_IeNbHwtgAQxXpzGQRx|@?BE)y z*?E{p-r_p%o=j)M5t6T>T5<7ldgRg3T(`fF(uXK_EolR0 zte0{xcvkttMaQ8YcNRAJ$VtQy|5k>~_F4=YS7*aS_ji3Enj+_B*yjDX;wsTI4nIVC zqmwXRocW5}xFd7);~H8^_E@<#EpB&uM&`swmh_p+uhZos=TF7O@@{t~k+DY}Z+Id< zx$L2{#2-8kW1N+=Zzu}NudvQ96RkPQkK#i47_)-`iz-PeV()TUdx7u69;^uSwjb6O%EkmKUjHPz(o>K3AZZ z3X=b3DB_nEoVg6F&0w#M+=cNS!bw~D7beOzgcKhN>E{q7LT^83v&Y6p<#M*o7fg0I zNlsPts-NhKdBaq5Ia|-L#qD-_e8VWu;+H+uMbv**O)O5<^cGH$?=)hrx2c5G?Y#bb zLc3wg_h(cv)jcJ8{_++*yjdniJ%&t(zDr|o1fsj0stdz(lb5j~nb=+Wl!s_w^46zy z5tq#HnQ?O|b|fhhz2r7r3KL}n4}Y-k-?SGen~=Nf2QcSxTlId@YW-NZ5RTI2eN}}K z>*%V;s&R9?4RX}-d-*)`!K$+I)R=D)P#3O8Bg{>eFxIKrB$GkcOYMmYWDj8h zv`NapJQ=GX;@D>Hg3xSjXD_gd$tcyHA_r`o3k#K##WNX`wz!!~XDZF;MOP}8x)-E+U6uiHWpV-r;GTgJ2kgdGd3F6#nPBMo!=Pq!P z_Fl@ZV+_LJ&$21d(M}U9u6aD6E|`7dGO5G#J;o>&7Ou?j5mFOuRm^g^SbUASdt!x_ zuTP@ir=lVV?2}ur(vmiRr|t)JKDxZXk!~h=l5VU@896c~t z6r)B7S(=91R2U5XDqj;Eei>BKS+8BE-l%l57TL z_ivt27jr3G8_3wTn`0f4BruCb)1$=$)<{*1;Kb8kl)jBEDbyJ!=;{VO0A~Yxq{n-P zrFsOlspa;KwA+6}bT1;YgPKSB7of};H3IoK0LdB8aGB^wLI0HhIA7kcMO=O%sH+TG{&kjdbfke~ z(GV=l%fvC6EACXDt->$A1odyb_`3K)& z8+I%Eipqq_+t&-^#}MQ{l1IzA*%y=OdI&zbiigqlF@REy9}V?w2IYFq!2E=kseW4O z$i&7PebO~%bVZS3wjw0gb+g;}sQytqyv*Ug5Ttqn3 z)iNmtMxBpsf~N}I;U3*aFdL3C`vBZI|MP*caEiq z0erF@u~@g9wirQ*hkxbXr?h3V;pRo0_AA`HM#B{Q-B7Il=%v5!#!wlO(=5C{(f%tf zei4eE8u@(hUhXHMiI-dhIGKYYVex^OB|G~CPS;1e?d~cev(jnWXtXIn7X`m~# zpLw_oJCqfniwQ=BAP7^s%hx@1x_A;_Xfn6D z_b%MC`FKhatJQ%RlIZT%o*gnODh=PAw1Vj2Qy6zcST{$!E4;-SXO>}N1DZIB+1(35 zTA)`YMjT*D>137-6~;*c^OrSk7&4&{7DPMX+g)^#LcR%jH(P=pA?gPyJ^@_f2D&q3HDr_TcMU+D9Qe>A)DhPwl?s(*pH!1c~AcN*Ux zn>)S<cyu%bn+3?kVsO8?7j*ghjEgw!@?$uz4LjX0QA{-@MuLW?@pZ|W2a zmoc(`C1y0KZIoc?(1qx`V_TX7E30#C{Pk7Z5T?sQF4Lo&RXCVHBiSc04{^W3I}wVk zH&4R{_neWOp2yrh6i;KF8+#{>anO%S@YBVX11}?0?oVAS6Q7ewa-r9G{q$2RC zFOPa3_FumeNe{p1JFV}(n=L-8K5r;=6w=$reUP)+$r3-mY;QG7M0R<3Fw;wY?RsReYyt z{)R3fRi~W&HOg8K+oGJsyTJTfd$k*7Qz#h9%^6PnYP{TFrjk6oraboZo)F1iAdu?2 z-FDVswr7z4r0Bfy!w|DUU{zPSQRh#8=l2+eX!}#%Z)ae6h*Zs%K+RlL4-IS=SaM)(NR&HDjLSKIJb~UR@D?EE@+~>Pa(p>QtUn!2`?oR zu~zc%s1RaH>gAEQN}MmQY9aFewvsp`e;EPZm8sdN-`w)+Z58jkpanZkMln%c!vePh zb{<~gu8n??e(Yc}Cc0fT@Ra_FrPP(do0KWzt-_hk22F&SkK_hBgX_qndzt@+JgvtW z0)pa~@aGzc$v)+cByLH+yosF-Qqef7xZphz?DwaZY-+%S z?4>vZ5v2qPBg+O7sn>W!IZVb%Vg?juVpN|ZD}B^hL1jDAtBmjRkW6-@mfT&rqW(^o z6q@ab-Z7-q9#r#SfV1l}$+gy6O<0qe6xsmIQ+kp&5utN4s>kWk>ceDeG7j4#FCsp9 zhX_v{>QnI3cOuFi*(r%76I<9}jT1<-qhG>Zhu`ENOp6uj4MiQG5Y0nWKi?9^=Yo{YlmlLXwtd&>Z6cBeV!4Nwvk_jb z7312aiUgu0q@Yy?FCMCb{T|M^KOPv&-zQNnOvT%%`PJ&Mm(Q$AI?RqoJnwY#p+=1E zh~RXrR3eXz6Y$Kt$;Cj68FO4-^3P~sN)bBtZ_vC}nj=Ek^uDRld7>V6}LdntPtR^vkT?<3dm<^9N zt+!{_ONVW{Uza-VlA?S;tRv(Q+$bFIT%W>;>93DWO=kP3T}DP_F+|M1K7F|hTRX1M zB`VmWs()4VL!h8KqdI`VD^*mFnLg{briL0uC0qx&LqByfUBf6ui?L%S=OJ1izEuo6 zm8|Bj-I(W(pN*`c_v*kRv&oI5WUlBs6#d+sjmJrDSNg6@j3r|?gC=#LX`OU`(}24a z_{7`8p)E)gIX+=RE7}>w=6^V$G6Zwl)Kx=Djq>^^+8;3^cUgETrpHEDg<_6_`3SpH zAlW_8H0;)Z8)Qu&t1uEJiGDP&*Ka6%vL3bT<{8-MpxYAOk&)@pKhXIijJ2a1Jq*8w?=$o%aZ63d zkil(9R{GeLde8|1*!_VRZ!xjR6W)RDpFffoua@ML1~ZE5#Py0V>aICXS*S$|(OPkl zk8)eHzqq=7ANMFi-ax8xe0obycP<&IE_hcN8kxX0@9a&`ylhom-m>at>lPxgtS`yC zBtnZb)ypTYiA^EGbihZZwG5A!sdRkwL*(wPgL`hcCe~FFletV`u}8*I3O7HGDCMax zM-9!u$9OgL%<0p;9wl=ZnYRkSRGp!@ezbd=MjNSKsSQ3pxN>Rlce%uemA0s)8C&dP zZ5P0TZ;Mfrat%ODW&T4efWwF6>|8U_NJB!e3s36#D`z;7Xjm>)jI6WJ-pKRO#NQwB zUfJ*bPaP!?^2>wAgDSow1q)=NfD;nk(-rw!mHR21BXrq~DLGnS^@TNKJ)64L&foW3 zhB7NWQ!er}N?_cH8>P$-o)TNirmA&4ztMBuZUI^Hi6YzZk{shW^*uJXIm*}fM#Jb) z4y^Rk1z5;W9%2_Q^iiMQj$%q`s(t~Vy;gza&)~O!B>aAECT!X!tmf=CdFoM08Fi>? zHn;{bhxU0`x)Yxn?g`Q{HELJ9{GB79*0dhiczX~dqXB&7tylFbnGvx_!{U&lGH>R=ccQ3YR}h1-Jy4K(YqQEX-sxmh(d{URBYxag z3XI$4+u^`f1=dG7XbBy97`SJniey~45?@i{ZB|dwsL?B^XcSwQFDP`pXA-RqkWpBHRw3E@FC3APi_vyS&LdHx>mt)8o>( z^=jy$b842zPO>J)hNDyB9@VA#JCu6Ptk+yEm%U9;?bsWTS-PD{g(kdscbpd@Bq(Zw z`t!POkkST#WW7+V(H`R-S`NJ^FaOoLjE`Meu!dQ!^CW3MdBf~HYVW6RgGO{+*grUD zu@~v@d#9;4mB_3w9X;D=d&czkF2;|cd_2c%9pf!xhDVG$YfvHGPmXn##zS|SlI-RL zqG~nM&ILCFy1kAclCAgmaN14(nX5h-KTdzXPPy&AW9n8+crMC^Fu8Sq2kUK#^1ZbV zqUB5!Ub9aDJMTHy*))kRB4ET_Rv%zH6Sp%b&#sxVfPq)PV$a@ZQx5Mlm-QCVHDdJe z|3pBSI8%jmA5lE|eiSD-bW&a^EH0WFAINzNeTCup+H>-WU!MqCrES}}o!j9>vo+P& zg*SwbGma&f_qNvP-6)f+l$|QSG&>{{pwTfuZr67YWNIYnpDi0sz#X;<@-rIquie?H zXX$RT(SVYoiEH*ynyNNx9~ok}WwRf1%&S2>Ju}VcBfJJW6Mg{wYR&6}E_IK=8eYNj zy4ppeE^gy1NnG7RxAzdO`nlX*R9rVREgF2WWD(q1^J zviYU5A<8{l@j;zCqC76(cSo#CH@*}iuP-dJHfrDiK2Ym+TaHbyI#@+jYj_bSA6YEx z{#Nhp_w5l$&Y8PN_nZ*v#&NWPO&wnyMknard1~k~?SY7}b0YM8bWAah?8K{_{aT{~ zAf~Zxt4k#r^R93xBfT>{VceH=C3n*+nvYic$XNLOnR)85KkY%v20M*5p|(P4S_x;{ zk8n|4i7L^`K`B!NZ5-ir2E6Gff#@vF+YmU^m4F@F%FST=JeI^OHld-iee($d3|BN) zxWUf4+T)$=@ly%g4B?&OdK!xD`fjrxWg<-@I%^1<9?SeE_$_hi;CSQa?)((09m|ei z@IYA?ww!q<;4jvI4a1r}X9oXXh509P$-g`!PM_*jt4z}oVorst)_H7A#aUrX(b*hr zv9gnHqsYyTjdA*~sT%JE_-pWN4)tNZdOTgc(|Qamn0vDlKHa-klV)jd?4F?a>}Alb zE6uzNML<^4(u2nKu~!~?@0zz!&hFE`?J$Qd%cmAxHHJEBKS?g#=s0U-(bX>7-RPq35Z@#_o>= zJ`KrBr|V|LX!A&tY~(X|`omEkO!$Bf$h5a5YrkaCpRPb)RVFg|$GBbJqB$+eWzZ{I zZlnQA-dU-%YatdoQX4#V(@RSWDs>u*MT~TA+eUmL!=ozQ9L)e|JkEdJ-j_D<-OZ-y@KOEMS1 zlZ<_br**lgBsZrzLP)&?jKh|8QXtaRMGi&OX2*Yzza==>Y8%z`th&8Vs6K7W!I>bH zv&UeIW0=rAW&N8t9q6z4?hmq56yl9+WTw0!Ak^6&dbT#cjr9CBzqE{k6Ad~Z;{7nf zWbW`j;pl%5<~k0w{~ImzkN+XGH?o4};rS0A#7M}<#=!cY6B8jP6Fc*NPXCh@Vq{=u zV)%b8wl{&PBwwMk$w-B}FiS|gyO*&@Kuy6i48bzGNJyu#ENn@Tkl`%=|5j4^9kg&) zaN|AWIs1alNFxz*{s_PzSpU?<5*r3;6JGy_u$5!}P&$=)mkaKgc%2?E&J2`Jdv z>B&io@IkKu-+yDqEdXwgp?&#$j01EbtTH~t@;MTX(2z2Xk|M`D-A;EnA-?V`O=|xFoOEAFAKpMih zeE}Fz=h+VO9Nc}#(C>H_XS>keQ>`woAsiZ7yx^Z_o5=Z;MR4^#q_6C}>iIB3P}dV@ z(2lRNi5Kn}qra#H1f3Yf#Tihqqux6CJtJT+Upd_9YkTch@Sz_9-ap~0i4+o^?2!rC zDd>DDht}b$iGSS3il83&*>D(e3HxYihA5^$0NfybxdPg?J4a7l*?s=m{!{U=pFY_J za)1kQWCQ*P*4I0N()g+pZcKo?Bgn_w=i=QCWKuW;APBGrpst4lje5&Fk2kA-(to|X z*Cosapj?Yz-2vMCf4)8@FBSC&F_-!;{>9p_)1+4=WECajujog9p2sH!c!2og&7E$c z9Y8!h-aG+9@dSQ9x}uZF_wE>*Ka#3}T7v+dO@G%9j#7Wu%kNfz#~ zI;_E4{FrPzSX*E`KhMvs(ofR;PucB`>bE`d&rTTX7B230o96fH55JwXctr5(4p4ld zYcM}u0QjXNT>cM^7382K63hY8q zUto*BG^fIz<%gZkJ_>k(sg)`!1O{0CT*zYo6-!9Q4Na^cBweb>MCjvbu8 zcG3T(001^1(EAdnE`Vrowr_M6`sVQzFR`l#6%PXq?ES6C77&dB9n4<}z~}q`sy3uc z@2%5}KK2tKGp7>@+EjH_n^w+$3&{(YlLwLbwt0J>NHT@t(({@Y$Na2IDs zPuz2Y3V!%U{nyz+FmUgnz;qaAhJ1|(U0sO}(;COvl-9qiws>7K;%o(=p~&`#I|Ak@)}SNP|g)C*?oNJBxr-w}M;b zFb8(!(T2jx58vjkU{fC)?VY7IUzj0%s*@p)BwSeUPhG|s!VmA0wBpBQ?9b+7F1JP3 zdWJVC1~0~D*M9v81uNFOgL(voDLbCx@is;|13}hTT!K2a`MNhV$$T^da}g3UDXaV9A3qgaFHr@F<>Y{K?spFH^aY*e4cOGcsFC8S*w0=63N5qDDJXnz znJPu#JZh>D;X<@J7fnZV`>ckV1lnDCpaQA^;bizHBca*N@#%bbIKyxKIgP_*AG&G^ z9mxgP%v7nae0^NT(uaOdvNk%2#HC3Z*Cj?m|oo5VK4m<)Zw71Y0vc32(od_M3N0Q_NqsJ;M55jGaTXFkH|WuWj45ZQHhO z+qUss+qP}nwr%B2rM^X~vdHrMggMjGO_G=XWGJP{TcPvKf@0K(F)|!VS;V75Y8`9$ z8dTOTv$=2kJ3a^&15U>!rnqBBsPFzldzyhr?*dJT!QoRuFqi})thU3oqv1?Yx~Mj%UgBDku=W13ohkyOHJ#Im_ky%r8tJKG6F z@!jxpLg)Bd1;!lV^dV3vz!J6u98I{K{Xz__SLx0{&A1g#a?Ih9`T%D038NxxYq@)1 zB1I}<86`R_+N(?*t{jiIGWQicjyOoo1>NLQ9gUYy zW$5o$f<#p_H#f?SO^S*Q50<#})^<8;Lov!qweptc_bEUh>*!c~X{`1UDVj1;wR7M3 zR2_*jKDKojD>^g@2fm5P0pan}Gcq#a6_?|spm5X#%2VXTwaH6a_TPTv1Hv;yivOT^ zN`mzh{AFXI67mvI8QS|nW)}#R{|T9Fx6lOUk#$#FCw;QSXyiB)7lSI#j9!NBF-yYq)+L)=#w}j5PD+WTa;8T*zoN@xD%%JEBPh zXq=KM@2G58{(pxnd*!5dr%4+_&At$C%a00kPrI>jSG{+}64?F^KW?~^n>-Q5x%H<2 zBbjUYw~yoxB=fVxCf5}F5QopdSSK5A5nXOOfVEw8bHljs=r_~ODM<+9SSS0y zhk9((?_VFO(QMjL!3>I{a6y{5S^2PEAnA`~+b~={S8)AdQb)G89zKN^sB`c{VkaUr zyddlva>Fx^X`aQ@w4yCtQ{J!VJ-R|om`?8*rsfmeRR}>gj;_fj=9q)CfKs3fJyzdd zN2E3mG594Miw-hRgZ(M~F-EJJiboQu0}u<^Wd@>dRIse0kSxWDobih;+SDlQjnS)g zue^({>!%5xW0sB-xBn^xf$tchi_*DFD@vmj4%jEihB#mJ7H!`l4(YpYn(M(!C~a0V zXKbLr0k3;ujDCN32fCfGt!uy+U(@v2H!~k2FAI0ZDl^inU%al9NoT>p+eBA?Cc6xN zYKGQNoJtZ?oIZyk4o2Hg%AqI2h2?re4r$$J)lpt~p}4jIV?Zk75yNRXC|-QBjLb}`z*R~a`WZ3GLz!esPwC=*0RU) zfS6ywHxuH`j}qAs+e^p{7uGBZV>C$=50Xwu%a@ujE(6_B!fR6O>aYX4x9mWkKHt8M zJzuTLnzpH5NOKP;i#S+pt1d)#k51Mn4a7J}<@5dOy0@ zIe}mCMdU8+knm6(wJ<$O!g;Ro5rh;43XUK7A)HwHSM>U`@=cd9`xW~q1s_~EkSX4r zHyM{$3lv|a2bS-)GTL@xs~VPNevYEiKZKRti!Om;Qr23b2;GXmfvulyk3Z+2Z!-jk zZ284)D41ujgqyA&<1K72U|0Dv%pN&w|8ZE?{x>@~E%BQ89@Zp0x0SL&3VjY3GUaWQ z_#)gG_MN6&QY~h)QaqL!v`917!cHpVf4Z}*cBs*ANgp$l z{)#O;Qcc997NRZ3S@Y6i={!+cyB!6Ox7Wuv0N*G|5}WF+U6#cuQzgezJm`pdPZn`^ zExq8sN?{cDRx|f^UPQveo!Hd6P8{hKQ~orsypoo+ zOA0J_=8+$ZxilZ4#p>oz?A`hFA7IkTRd{>VoOv{sxKepkIH4%^zdHBY)5R5&&<6>4 z%C?y&MPA!TjYcs$&^^HVqIMRZ>~!R7y{8l_Np#$TXc~j6orJ?o-+;EwP>|yoQpev@X2k?<2jVPVdM$WWOJilk8I+s>`f3yD9OxOQzn9WqH_LZ|* zhAt?_-U1%xK_#`>qqFFPMmdy-!OSBPFC&!nYE&8drqw!zAOhH`$jWANAs0=fqZzX6 z&5cG@>)4DDk0w&aK_|u2$E1ZbSDCoHN&%_bnQu_jmxhZ4t;JDvCbdRDGqXtv`vgVk z`Q$&!i`lUk_oQ@rZ_x_VYxBt})XuFk4boe4Wr)WH74Q99o?^6)GU#P}6E>H2azWTK zp&Pna9-K{H!dcPI-D&`_B3BCTx7DKG|4JROaq0NanWN6_S{#jnl4lMV{m&A(y|_`Z zkHhXGtFULM@hS|%ZCN}uZbNb8D1L+JSz4OEXpXrQ@|+9o8{+}_&QtgsXo__IRdx~8 zmdzR;&RM+d;47>}*g>~}d1A*0D`J1LsZIPzJvM(6ri1mP0ST;U6mXrtqx!~_bT#+d zg}veDcq&mUhU5!}(mVccg3ejd*Vm#+gk5;WZDbBaa&7q@g-|3$=`NjglHP2CHigk0 ziKlHKJn>7kn$zou_Ht!1)1Uz-sv-_7^EV^u2AIaLi~WGb;`98~gHLoLRu1|MDs8&P zXlL8BSeB!2Hw=PP98WB$ESHUTv&w2+2OXj8`&jE5^`Bk>a5CV+ZCUU5)(MFv_!eT@hn@f+5&MI+s)|0yzmmr5420KG zb;`(Bk$`8do- z)bzF-3;8{^PK#_Vrgd{-+hcets1Z8eyQ`uL)znm`L$N`lW$PjQXJ`ANp*QK|@M{n7 z$%U%wV2Hi6B?zIk=4X%J1TYs|zFfDy{{_g+nM88w9(szp1J3)E+icu}rR0_Y_^cZx zo8&t$B&xO`AG$bkqC`$r08-U8bQM&57|!EGwk}JL=(EmTd|HlMP}a5OeW*JZw~E!V z;U`Xr;mBlUPLWsQ=VgsGMh;-Q2v;=_EP4D&YM7T5%%Ej}=h3~y# zEo0ce_`n8A&8xPI_zE?t=CE_}&o6M_hzky~r)b7lgQhKD50K+-;6vj?`S)6_lrgf< z9n`}&**6pN-qY_z)q{OD*rW9HK#Y~M5$)7T&A;n2x=(M^>Ln)DbwmvWx^38Pb`}g= zVzva;6Z3=6;`}+WqvuIQr~MupK)TY3{(%fh%0Fy&7?HU-_w;n>GURjaYaYf-*#%&o zz6+k>>P5Bqk$DG_phsG=Wd0=+P7}(#?Ov;)TvRPQnaE1{LLTGJ0ff=aIgj%KJy>sT z_<}Z$hv@;VIC)9|hat$}-EMZ5L^(xhU4s!UBCPO^Cn$`a)$872hRL^d;xDD-m6ML*tz)j zwct$Xzb>;=X3r__Jmig@bJERG-6SB?zh zNf;-arJQ@L!WzDNXB>b91xQ52kTi{L>KSq{3eiIO+o4(2kX6uXm3f@tt`@oPF}Q!2 z2Vm3~q%EJ+P9=pJseFnhmqM2LJZt2lCCoqTE%=b!yEYl5%UFhH*6%ei%t0SkEcvlL z;hqE>WNadF-Mo4uV#)5hbs;f661ut@I?L5Z@L>G7_OoR3p-OTTJ7~%C$*fA`iz1-& z`R#}Jo~RFTMpkW>G~?OGq>6fes+{mn8s&q&Bw05WDELD2uhO>xirBx>Bl-AmDab>u zne{tHc~(9ZUQYxLF2MfcI@LpVdfr8jW7sCST+>)7iynf?mjlGXtw=Oq^|=DZxag(; zH%PP;%XnXonAa?qO#j-BR%-cVp4}N2@P4zk=GV+3;U`?a&~2&r-j!#yD^IIwf_I zr&ZTml&mm+xsB#+%; zoimUhay{f-3IriZwL1suR5Lw!LKuw?vH(nL!!7Qgfqy}%5{`j2FS$W)xuw75D_cRZ zz*oB8Nxp$d_l%SB+uW`6@N^2F3N`NWDNl)a-?Ab@nq?RaC*0g|HL5(_F*4lDs6~=@ z{=aEoJINaP$XM1`>|IZ3{b|((NzCP0_;t<}C2w}ha$?tAK2ckny7=AJux`9WKDM_@ zI?A!JV;8^T$4B*Ppdd@Uk@E85Fvij^DS0g&L#LrZjHH;t#kN=Z_0I?!S0XY5f+Oj7 zux@@o$2t;KLUzqb5!OlJrK)~PcUGj=&L7Mqxy*nOxQ?Bk;dS*=$>9FiJld1=Rl76S zZAa6aR|6A+$0?hn#;JNbw#h>s*ZQ5h*%2hH^ap|$-D1&3GEq?IU@@*%cKX~aRyNi)=uR42kOKb!7_K&=8pBKdswvn8wvVn_AyPhb-Py>jYA)~c3 z^ac+vO0uX@j9Zh2NGhP1+n&s;_hXx}vw{BV_pKn7z;FX-e4I&Eq8?q4@ke^v{S#my+W$xv4T zLogxY_Ir^BbhC#O2?{e2I$2#j=edEyTCqCG8G7om*oXRv7Ha0~)3|cm!I^mYo|AE? zUFI^?j%V$7LX3#B4S@9WQ zwIq|4b~wsf_VVf@WK&OYCZG}L7cB~1{^ps9Um9=KXFEFF`X9ZPc$%c^4!PLKmZ|9+ zpHBLw4TR%XNc9_iOO;R=1;-YI(c-_n762m$$m9{IH2qk&~= zg1 zoFchILL9p0X-R&hy){l_rBd}_ahyY%7f_K|sGJo*plw4Rl;Y{QqsG(57V@5k4dW#^<0av~qzgwQui;)(O%Chr@E z-)2FQNMQ={@)uI8<_ypg`t(_R4Ux~^8B5LP8zC4`!{?`$B=+YufdggCN?29h;R0sH zTH+OS8SOg^ttfE$V>LBGCUPUM!$<4fQQRmulD=NAP)n#QzAe$Cx+SB-+jhD#bc*wb zmzv-hYN5`cV5 z%I2J``DK41U}uGoU2VuMsbfu15#`&sNHtn;rqJvT&?y((aL|1dXblPEaA}2`-bC`KLt9+ zE!Ap9a2uxs4gqG=8-vd=2b)+fRM6PSk*s`RgMP!=ln!`(l5RwYJi?`ko8y7fly`QJ zgWYiYLAIA8lS6><(LCq=%*Ei=sovP*E|ntM8NIAa9s@>zH1?sxmsyKe3P%Ys#|aMH zaCi}_v@FkL_BwWj3At~e1E;jQs&u-X31ax70)qr(cE6`fmG8qB{ciHEF!H*+;vxh@eOnOKVTHvIiA4cdH_ zyS3+EM8XZtOdWB>fH**vsP*+vOYDF_Mr3~0FgdbHUMaRLGocxPpZ=t(QG4x0Nb$L( zUk5Y`v{&NHL(XbUOd?w061tO5qur|w!=!LNeaGbK(yMqtz=Q`%i9e16{&?Rr;Njl)ZUhK=wgNdN_1Y_u5H1QRNDm}bVU$(cWeR4eVuPtf%E$gTVG0G3ARfwBL?WT+(`&rMHnX2{}HqU z7j9jqY^%WZV0CGiq7w<>^WN%yF85&}-W|^RA3fAQPcIV%Mvy3r9b4puziQ_DOtw)D zUqq!uu9CG8eX`BNpi&LNEVYO3(W=97(@V_ROwOo`H*A&<_yuEr^gPIPC6h8Ull zVI~#9DH7~+l2B_M6eaE$eB^~pwo7;DJ~`VspPvv7EE14lralV|w;T3oBUyUO)6>FUb8;tXCdEQmuc`Rpkf$=;>P5)dOYfM*2H+sz5Acz$z)Ii`k0tjvp~n?C6{yt z46AjEB$eY9>3V4l@q&-IAKyi|6g3Xp+**3H1r{%l@g^f1{G@ zSCzAvaFqR97v9G`kGqMjX4W>ly6g85NLT4;Z`+(oa-BTCDhQced*x-BIvCLEk8_ms z&!*%KBGuPq7POrR7PxkWg;r&`ATsXr!7}O&@xoVL57+u*CZE#mlC%25wyj z{&P6N$%*D(#@&5uEa7869%L5Pj>dONCut=G^{qJw1(xiuvIWkz3P{Pp%HE zA*}$jWN+l*jMEl0;Q?AW9r3=nhRo}gEFGmo#`s@q_}E6P)Xs+ zaK>jJ6VEtN0G>?4A0}5;hvte+bE~j=Q7-%6P8Rpt{1@sbx}L6CcDTH>8?YY^v*&+9 ze&Tx!Mhh=ym#|K0TzBwO*<^V{ul%AT52(hAM{tkJj>2>zw7?+6U_`tKC;bC!a}I&b z4~6Q;L1Ibnux;{7LJ95Rw}r?MQER+K)_Jf>CtbcolTB<73gQax7{)~{+a5_2fHE9r z>4mdR)>szqd}>5mZEiyh{p>1huUy^}j|m0-fsYAxY;zt#qsOvabKEgZ8d?!m6_cvM zeJjcHGA8(iUZ^OQpIMv%NGQ_7 z6%bQD8nPR7-7m&(!BSF#0T&MCDw^~P25qCkLLXcVgGs@oq?wg>* zH%s=CiWjz9u$6l>5W76`Lf#jJJO~e_HS3SC&Lv65d@~7vc_3sfYKlh#*x1O$Cyxhd z?6iI^DcSSnoc$=mI8t&iOJXC7!XI<5o!XL&wd_nbJXzc-G+$k1RB^ zie!eKjOyNzHrMx$LZ=yh9~D}Be42y41rI@ROXG$2>LCfUdrwt74?6oU1EW^`vVLmt z(8?CR)l;&41qpq}C9NpRi&FqDu&oJ+X#ADP2srC-bP?-L z-D&hzQ32Umg(Kl$PIil5mplZ=D~3rpSIgV!m9_bCZd<$BM?*8L^A);cK(x>zKwS{66w}N)0(*oc|)o^MHI~lQ$z@WX0EbK)iXX4zWOR zqUR3Moq&yk0`suBUHcJlEy4~a?D+m;PExy_{}|E-S2I|^mz+gC^T3Ch|E$MCe8nEJ z9Wsylg51fxJ^{_N2Wo@P$7sbNuBYQyETu*%5~I_rxmx1duf0%7wK=XL%rBsyZ4gQo_lo zW*ygBvL!Uoe8z5QKycwJAiV*UFOs3H3s1UK2B0bu8rVc8ynt zR@%dx&ipZWN{LY@DDd?VZyB~SNuGVOI7=XVBmG5wNbdhU8jm|H!IpVcPSw0hf7z;} zZbz=8jweXCMy6J|v=Dh7s>5g9d`%8U?_gtZGJ@&AcmS=u#E8*g&uHN0(c@qz4Pf-b zIL(l&8m*d+iNi|9V@lIZCc_rGmQ8qJ4+~q+>efN6t zb<(N?0TPqaw(aCCe(E5y%lerJyOd;l4eK!dUWrbpRgKZaQMq}W&ofTiISmH8TO`zf zIFTCbbmK8fE)8XX*1Nw%DXEjCe!XySt~N}}JfU{v2jRkK&cJdLdJZ**z1VIN4^~W+ z1rp7}c)6h6Ts%TRDYa&Pam0n!5wd1SMV)-Eu0Ir8xzk?2L%OUf$Cr9XN{Q9Waye^; zI;M*0YV?Ua`~3EDv^&j_F-1^&9ZwHS>!+aKHKzim7W2%b!BJeQHwASIGUHIQ`nSF; z=B|fy)T-~mkYYC@rnbM)bVGfVe$dN@_}o}LRD&BOXF=_oVeCdQwa+NAH`fY%Kk6ep z-{`KGE3M-3eDDWw6%0T6|Kb=~|6d#BpsS$DaR3R#3LKa)8Fn%pNGIwJn(8|A^00QjR_TM|?KEy!_7~0`{DqnW=^C zKk=E3J^4L-PzW*vglB^!5FtGesW*2(cQkB34s!o2asMoJ|0vkLf$`BF{KBpz00E>6 zXcl0K2EgO18-clr6{I&fJb8U6>G4U%9}m#E%mv`}_jk99e+clfji8)?az3sABt!$q zMsKzSR5PH(dRCAiLj0eUAh`)-AO}Q!V`oQ41AuLg=5-ED$VDcAok0Yt0PZo2lM`qr zpdUMo0@!BI?{y4lCRV;Bq}%UOt)V@fvQ;yGmkPeQ_!u)4G zHvqfo;rae@L{TArtDsu_g-))gZvd}t{MEjkWdADGU+4kne_C*!1ODVnZT~`N|KhPNpj#i_qwk5d(8s5M zC^q`419$$ETm}5;YLLcZu8qF>^f)2zO%aE$Y4nDtr)Nh7M_Tl=lB4ey~&UF zj86{$>!bdYp9Okw0#-Zu6Z5eFR9*5%0QXn^!XEq+^dAG8m<;!ij# z(Rt(F_m`bt9+|i>9rmZ$o$~k#{_}_p61XK$J|5xpRvs{@+T3r=vSK7+Kdpfzd_QW& z)`dHb1QHgJ=v!`Hm;;Hut|kptx)|YevlOn9Gj$xx%D#I#)63EyzvOAJ&6p^5ryZ9$ zJj<=iH%IEJjifwf_?ORjSsalJg~ItFT#+Qwb1Q_z*mS;@KYCwUbE7NaQZzu9cA5%r zseF50`Cs8>nDlgwPe(nRB@4#V&QX7V>9lrh6x3D(TcnuET!8|@b(@I1UA51;_35O`{Sg3qaZ$jLEoYskV+~dfUD43K^x{hBqPXnhfb)(K1 z;0#@j)u;=%>E=bSP?3`y{O9z3I` zTAXE^%fw95v$SuK7cG){Rhd1%c&QdnxA!8;5OT$nVShY=fBml-j|lBGKPjUX7SkvW zU0Pjn^So03+av{8S7vISJ}RGfPa@37>GHQS>{IB*mw-$0<5&w6WsP6q7+(=3kN%Tt z<27&?ca`;Z)E2yjCt~j?t;;HYwy66B0`uXE3AUT`sd!OUEMh?hO>@sE-CLkIO>U#G-|pgLY2qg1fED{1ySES!!HgInX^&k>-0-q`pr(8yxI>KHl6==8ub zX<*c=L7$JW4PrU?G|io%wqOA(>1qil^nUn|F^l&U&9Mt0?pj0V0czCQqZW);Rdf83 zm8~};JSnr?3nu=A;0&T-kUj!hv1)^-iFGQ1k&Wr@1>b*0=x$`; z#1dfLOY(6FeIoPmiD#WSqsXMK0kRaR9@{4F7^SlUboT502czCTCjBhHXojhTkDTh0 zulKDnhZ4Oe$$wVt&R_X6pYHlG+e_Fgk5_#=`U_kje^R%%n-WzrtjhQpxzka0%OI>A_|LO;jA z&VafSqJ?>MFmOxzJiM;A8?rx_-G$2w2Ys`rJVd5@-4BaoRm~eegC46sYmxIdvngtvBPvM zYGgbtkx>RxEZvS@ZqoC_S#eLeNXjv+-se5>bL_^&vLLY?J@18^p+&NMQt)gxLf|A` zRRN&#X*Y=+Lgkkk3wR*QDy5>N;yB89cLB~i6 zD~E7&WFpDgcLfddT#G+)y>w^?f3Hoee}lJY29Xew&IenKW7y3E9z-pxsS5GW7?#-* zhR9ex1r3uQ23FdOLz#}6wGXfN8jCqh_DHV{2Icf1a;9i94mpSF7G4ptWjaDV)%8>{ z#|%Ur_gMmgQsFL!EA#zCRCz2laXPooX_#Gg(<*f0!VIMql!StB@nHx}FJ>cL^w5~C z8--&7&Dg%33|Ztk*V8!2+7sYy!T^X; z`-M8baJg7gLr4EeBg{Q`p1}TPc)hSERu5^ai=(61*f)%Rn9ZdLp4d)U+1TL_311(6 zRXKK?3SeY*$K;UCGw>PN@5B=vrBr@2OMYJi-aDJ*_nRk0znaS9txMd_7T=sB4L>Z_ z#R{Dx<{!1Xk|Jn7B!gN_lZF1NZF?v&NsH1dTJ_X$B8UmeEOMWir4#y$~ zf}S;?Ryt8p+61sEkQ+dmxh{F>6)DWeIi=hY8ZX+*X7O~90AEIGgjlrbU(Cp0J!e#c(uBA6$D@FzsE!uSgT|1{r5YyrYr^mZPvaZ`n@oM6$Y`D z$L9j6b^cJtMfUQ^XMrgP4^7PM=9@;I$|gOWSA6@9q$$SKN__8Tu_h!%X@y)ldZI}? zAjl1{78s_Ux95ndJfnBJb|l>62BnjYH0EufzfGH;1s7F)cu@9NYiCNX1vZ6lVJnGkEj)rLlG@E2da~`c{p63 zmF!#EOD9_f7)B9ZxIm*33O(yg`!F^ra(a9hiGF&*_*B{_ z*`4ND?KWV)k)=8moAa6wVO_QCs5iVLa8;vM7FiCPutYMLg7=ore+dwqYkWOA&#)F$ z-uy&XQ}JMx_^IX)ucTv3QD@;P$E;Y`d%r^>RxnZP8UadoZ|3Kv51R`=b_{YsNY@b` zXKNSQaR*(3Oz7`6xn_{;Z5!>%tXDb~)zxSF=koy11MSz%BOnE|ve2j$qw ze037w?Bjqx1wmwA6%olF!c|^d$+;r^95A&Mj#jM(bl>F``N(%ZTPD)Tox6z)gNp4j;#cQ|klN?r41&o}Vu86YQSC);RZM5*MTN z=RaS($`Q+%6=itl3~oFbC4M=3|7okeH-Fruw`c+UzFU6r_AM1dJwuSC z_3ogAAVNnK=oVyoLJ5&~d`I()LElz+>rlV#3wb*w&jgrLx>N?Sg(cGdF78H39Xqh= z7v5T#nKof6qDO+v$cIo{lsZkqfodthNK|Kgu^p{+I{QSyA(HMGGlyX=lirVXG8dlY z-~*!QQZZz;5BKrA^sN?i+C8Q-@I_t>SgbU$WJnC}*k}~KXm_g};%VY}ADckGmKG;R z)~Ar>ar&*Z@)1Y3sc!I*Tq~{fJu^3=jQ}M9f`*k6zoDDTdRkZT)n7TkwPxon9d+XK zjF?1$sK9pENPmTb)0GDq9(mrz$C?Yy)y8>e8|YoG&B*ALb27OKZSDokF*K&U9l)yQ zSX@Z|Rn-Vpp40qu^wYY+sBtnx4c-cb?R)Q3IEJ`3v9v?6-&*hup))7&34~eSYNbZUS~tILcG6! z(DBy7A_Zph`28yGE24UOzaMMl<<+JnGRxu{G3yJb&a8p*L!2b*w(l-0Muv;J{lOI}njCQ&$qyIOD9^_#n#^z1D81z8Gs=s*8c&+Kd&`O?viiSduQ z7t;}I+f<5G{A!! zg)GA?Z=0^?WoWdzeHTWFw;*57)CtLGf{G{T?miYX+VY_wi5fgT`~Ovw*_iqj#A&)nmT&w>JD!#ISwQx{S&u?5VCGcgmyT1x zMb&zD71JQr1qOTFWz_H|gzg77{GijH2ylE-1I>4X5;FemxZoR)(u1*nD(j> z+wKlKZ6hKK@turZv|L6Che9^l6e_v|d~I0hjv=$@8@xjzo0bk_%ABkNakD$EuTE-9 znLX!B_Un1YiDvS>_mHswbGuFC%dZ^?iB08(9F)e+UO5_0V9GzEY$%7a1_g0wt7}B= zX3LlS_t~xX>$;#aZ-T;>}9EnjnXs}|6%E3E=A|o8GrC=)fQHAUHcEP0x zPgV;_Aq5u3rpgOfy!dkcJW2Q$NgeMR4Jv!k7>)g(CM_z3y)#lT>Dn718yPWFYE4{V zrhHBL*JrAE<*oX;j5nJh%8<^AJ`$Kd!#Vm_N%h<8K6@W~CSD3fOo)M|aZ+p15<1eb zEcw9K_h5qBX7VB`rkZWWo^4qSo*IYSrQs~mI-Yrlu5=XE6>j?zPfluuWberQ^)u@6 zW06#mrzgvAd2;mNc2*%jAonzF+2_ANO<8FN0#{=ArUbKQ4utB*#bH%um$XeY=@nFd zwz;Bz(}O&oohy#5md`(~=C5}y#7v96q6%aWGLzK}a}LjQiBN%ng+&v|{5|}vw9%~V zSP$^fEfndR^uDh1zo(}_@XL${{ zhkr&IsCj&CvN79*NvDGwo>3H~1m%$7g}u;FcZ&*}+IJwoogF7vu0)%tq@ zti35;sTd7v9agq%s3tA5L>=6pqa_fqp~9lH9zYTa-iD@?iDqJ&I_}nWf4>ohU&sCA z1$_B$z}@|VCtz^vjj@A+*h|EP|AaU^iQ!Svhk7?_a!M}hAwZQQIhb|N3=O0cW74Dz1n z$|l42aG~c8Od*g5LVAU}@Ij?U%W&zJm*<9~yy*H(xn>=FblYnp=LTW?Pp z%zCjbBZ&UniX{^0^Zv`yKN2{iLx~@8RQ9Lv9=s~;s57c8Jl!qhD_%#1Rrx06S46h& z3NKB*O`Ue%lFic;Eg=m$@no{+tlJ7ZeYt^N4u7kh7GIsA-5|=~)a1nKivuFVrT7-+ z;xN@G%(tK@Vy&v^I9A)kf_OLE23XksM#1-oRa!k9n}Cj5aMO(V?*zI}jddm*i(dLm zjMB{bX`j9Z`->WjQ9NOo?|s5LzKv8#?7TB@k1{HnS?o27HvE^x_X9DHPV&XG8gZqD zc$1~~9ZURipB{@`U+Y)2{{>Bi)ad&ww72{vKvavLuF6H&8mUk~<@+1TULp57h`3}9 zlF;q|MXBi7OA0i~`BdYTP$*C5E0o7d2{lX`%vPWOV+}`@uso+315?!0b27m(g-gE4 z{(97xp#}zxn0vW2eL+c|MoiE3X7BYP=IFnfE9Z@qV?;AY)Q0ddD@LYc}IT)#0Nujd#5yt254Ak1LxLzPEby#(l?`L zyngHeDsT9q01Iz}q7>^I_i67VK`C4rm1y;h$j_`%X>~Xyo=>dHcgk1lSS-=%9 zgFx@PZFir+bftozp%iVtoT2yCe3ev#B@gWrM~>Dwb>Qv=~=+IGPqkX{1}x2XMK;Chtnhz2Wo_#dFDo$ z)%dr;o>`TRyah!d5us>c^zeUlXxM0Tcy&ttT(rw!y&zp5l&(FyjUxILN;}#zo0Lk0 zBUOq~CvHk8>wvuFupSg(ojbpa3#m8K+w8w=ES)TB#zW;H{q{c0qo)f)FaB~r52n0q z-E*%n(R%$955KiKgzalJo?RE_L{bMS18ZIGgY4h}AJF={>8 zINJ@>>rp8}@SO>6{A%%u|3%q3_J|fNShQ{2wr$(CZQHhO+qP}nwtd^SyXR(-nPigp zWqw2DBz1PJy|#v-SFZyaOLvIBBwA4jl7zYuVk#+iM+huzXLwaTs72Pq8J77Ua3}uG z#66hJ-GN*aeUesSo8S(EZ8V9stmYXfP&wu%xK#6`IBr}oK~nd`y)oA)GWJSbBJLzc zEqNB6l5|*wJi%RQId_w%skGowq(u}<%T^d6AWuYS#;_SoNQ`>a1CMYT-1(H6yn$!hCP$_CjBcSZtp^nR{c%U0BwNwk=oTm^ zhJgL`kuC^Y+M1}}ZTDX(A^xy(6#20oAEu|ufTF=zXg3r(;Oms5P7SFmR@K_^Ep;Zu zX#7^NNv+kW)mP05!Hb<7MCbY)P)^P>_RhdftP*?Rb7l#X#jOzFm3v-rToDDZld?tT zLt^SQmZeI@ECv+X%jyr$XDYY}GgbNW7cWFEbPRNQ7&jTDYd(&44cLz~`##E*(b{QY zGF@?BL$au=%#jif(a}5PgbFDNa+qNjvo~{FdjOw=Dskhe81LUIV7~b`Kt5OXy)>0o za)_<|{uN*LisIQ@@dXS&Yo789QhqD7^?}1!$-CS(MefT#5A>vnTXg*o=lI2c-?0xY z`y+R_GZod(s)dxLG;fe2n36|Kr|x$$9(-C%$RQJnwv5VIAt`ly?Cy+u(MKu}pDObh zz=ZH`-iu;%Wci;c>mYzCLN$J0*-_17x2EUB(F1%b+iCY6O%bra_ii@hY$f||Jw;ga zEzYGOvcJR8anKFeQS`OC6>@TsT zlbN>V%Y5oQHYZ5Ztx`x1?WDsOO99Q!cT%sMQJ3g-K^*FGnCtJqg=o+b+BLO36tAW3 z9X>i)NyLK%rlnnhO6}o~%>mU5mqDayrxlZ)aoPh4K<X@PtQTTw1E2WlVs@%QUsJJTWgc6%V-ivQmB#w4O_dd-0m?l(7e#nyN z`(h^lTEu*~l?nVdjlKNZ=>%l6GWH{k^!~X)4 z`P5@$M-}fZ6u}2o<#=}!ZD!n5@4fB_`_>3rmDkS+>!)@kQSGsfV;Q8vD8igq<3 z9U!7r1QNNx{(C$T+y>aN=;u=@#=M02@v}KCs(A}oq}8y#Yex8JgUOz;ZPpx%W=JzK z8Ig%vM)@iPF%@taXp|XPReeowlp(tR3OK^lS95{hQ*8joLvf`$YNAy-s>@quQi;-8 zlqnO~J4f`Bsa6uZ&=r0s#~zCxv0w)iaP{XNI&6P!z^Aa0h$$|gf4ms0Q~WgWe==*= zsMjV!G^Ms81W0g0)rlf7*0LAoV>HfV$QXF)(k$}uEsu-Q)hL}vf>YJFf{IV#Kwoh=@m zl=~njxkkUBuE5`>zH^kfA2d|9vbkU*NQrBb8+>pEa6@mOleDQw75KJ;^R$VjU2xbe zOFLN7NbugO_3At`MwEooxPkZ@!9fg!3EDqs^dRFSRsw=a79g4+Ar`Q&SNCL9FG{tF zo0(J4=VZNys-tk#JB%_QC+ngd>(k`^U0sbG<0W_NDYUwyJR!xLq)$jLCuK~Hu`1J&5d0iv+AI`wMBO%aoq_r8ZqdkGb0hFCebq-w zMbd~lENJWRA-rkj#~2B0@3{19NE;a=wL^sh4rYVEumV0_4zVbgz7!=_1QnXJ>&vWtj^gr9beKzY?^O4HTkJtxk(>&LbF3OxISQ``_ zX^AVieHg&?7{TA>e3rWR%}H2@tr?&cS}=LiMzJ+zc$~*(fr3yhrPyi;yh(o*S239E zpt-~JZA*fLh+M8tts61xX@3>H9)A1nx4&AkCU0i%+?O+@9~zWW-TXtoq>R|mesu$q0}#I()8iZ zFI0|!aIsq%;hUTyu{j!iFZe*`aLM~3j`BYDLRy=G!|psl!eS$-%qVCywUw>2;%;)f zE0G+>G1P`-s}a}zb?JkbF7k#wmfCitPmLzQxoG95(08BOKiJ+%T{4I@3;PKWJ!pjI zZkw;x*D87l*tzZ9)_W{lLx3Q_zMsw_df36d@)YLpzu6(` z)hipHA)vDlY0hg0CvHb?yd9%t%vnRS30T<1@syzw1x=g(H$Kx$d#yeJSEAZr#Wxh*gv;^}q?U;>_Jt^4C3E7*OURP?nOvqKH)8|Q#EHdV&4o__C2TFTpI%gbxEjL&`Tqx~sj!V8pplZx96 zxzn^&-$Kr~QYNjei2L3K`g4lc2$D<9 zJ4*E7%=iVfgW$5#1*R7~i=q)PIVL8NOP+AJ36h?geiPicT&%*>C)ezl^-A9ISwVxQ zr`CTf4LA>6pH5a#Ed@E2>)p_j^0}#8k=3Me;7*8xbMkIl)$P-@f}hBs z#Oa@H=qq^_ZjUO%a^x?Ckkfx!Kjo|<(jwEe2t!S-A3QV z(N1J1y`;wItz7pqOSdnZ?voQYj+<)x1ixwyfwFa%)G&DX9s65MLrrw^M0IN2$D^wR2Omhkh*!=s-n?XZS7B+A*-YJVo{V8>@Dq440sSSKL!Lywk)5DU*%U39h5MF( z%cPiuBRsz-`Q^X6V6SlUbx%6V^TJ#`R3i zc-j1@JgRh~55VO@z$eAX0s_JwyPsr0qj`RS4p^00!C~+L@!%Xe<$Wt_pwq+*V+7-6 z_RQTwWxipCrfB*Q#J&hHiC;?yg3Gp0W4AY1zFs=5hSSKhRN{rp#39#2HTEPh#m7}2 zXm2h7B!kN%yDG0d1Sx*8QVG!JmDNOCOZ&45YqCtx%6g*$*6aDzJeqMxaHnZG`(AXc zVbOvq4JHC6{BbmdtQ3{EqZb;kpc{ka1>!j-RUpK%LQ_PQ2fFwUf`(ZjQR_qW|w)qnGi!k`e4TE7`f}GNB za^_t8E?x^Axv<~WKCDM7hoM+HSdrs&lytxG>e&r+{u7YT_Md=!b|#MhA@KhJ`3x*9 z|LYn4zd$}46T|-p$k$q;iPo}zg}GsD2X#x!56BN1`tL<@of2IN#T&|5X6wJF2+ENO%H&>$jHX-0M*r3)7911 z;>yamG}tu)zrhLR%=|BKsQOyHzxd%BK`=OaMu(xXb=;u!H2kX*m;lD$03C1s+uqgH z0IaI2yT1g1*Y5y_p|hx@0HCJ+)a$5!{e?-uTAiBO=$YKSztev`K;|)*0R6kWx1;@1 zfQ7C3Ur z7a4%Gsj+7OEAheOdzi*S?!5H<3kzV+ZYRYT{DwVX3Vt9l0doMcYk0J8c&r2F%K^k!HE>MXy}#PJ z=JzG`?;Wf`c5`3#y8OmTxI^sk6jfwZ?_m2o^AnJ!O zHZ_QP68mO>Vf@DG)9vjEq1p$&ta|kaSl#3Mle?U zhj*6KeM+Hlbz3KIYb-yb`D;EOx4gvGF$1K1XmK$~(E8?fc4whap3imRNc(`^JMu{J zmwKyU_4HeTIC@e5bxdA>(gQEzeRUa8Nd1HlTu*HO2>kFLfo%ZP20sKgE`N~|_(OpD z@gIRYN=Lu>s>m~4V6wb$60ysz6wV}^J5w@QEyV7)Rxjvum57yPDQ|2ii1FL3V?t#9ZJy$*e>_>DZr z+BMT_m}?vwm>L;sX_;SoJ+FGEUwDH3s@kEx_-mEbfx#=j&J)k;#evQFfiFHuUHJKX zxrxj5*ic!&#lD7jznBNEzh7uybWZ488(V#yr*%2LF0;S;eYy+y@l@mJ77lRM(|G0@ zSnHlzG=hHzo0(_X~Pjzbv%n-S4!zWJXdjk(w=$lKx}y*r-Jq zaa{7$18*AavQ{o>WLfqNy5IPjhs%7)*`ah8U} zp`>8cWC-Gj^6^wsyJk7`a2-njpG(yKTNpoeD!(lLnW*q|QU)Ucdt|-BkL%!syW3b4R`sRL^;)y-{JsP;L5
DKh@$rHimKXqyU4ebOQ~)a8e!7!wgZwN@ zA+?Pb7tbK*-W#~&H#CC&{m(83G|gV%PEr(afl>8)Rt*@sUVu4FKSzb3+~5zwY-Vo> zS^bjc(;%xalST#waV3J?G5Xf(i5b@eNu`gz_mw}LrtqszoDFLt1SsGX)#lP+{n5i) z7>M2MGa@1CW)KjUB27M(<}rFB7||=~s26gi&12kxW0|!Frb<831{xhOkc6~rNWQL|0HnFGd3r1Se7=L9@sN+ z9wZtC6{!H-(EmNzt6U#T!l0VJqLZvKFbCBZM;sAN|m)0^}(hrL3r~r2lGYP?e zg89w`RFaaf9ebTb4tcakM~I2u_+)p2LGRj~8^~by9z`dfGeC-uQrJ8d7yQ)}M)K zm186)0fwQ57~!>mq%VZa6 zMHn+9%p#)=uLioHGfmJBe~kzz^=yk-wY=o(sHk_v^1}NrT(E__3M3dBWEV~C+}y{T z61AYTBNxI*Hl1J%dc*5%f*~W~Rqksy;%gbb^VOj_9LoM?O4YKtAl^-H$OM`+0B?n+ zE#^f4P}};47)HfgP>)Wb5^SSFuQg+qc(a*Sowu16h271hoegp-r%fzV5xbT9z5iF! z)IL7?81LDTCPitYVsYeFeRfS?j+hV&<#G2kzw`dP;U!SEn>OO-0bhk7qV{dvRlFSc z25s(=4vCAX^HgeZ!bPCr`OYLo&@3e>Gn@3yvtkmOsC?>bDy)acPPs3pGgZdIoU_iT z<=nsH=m>+5?c>YW!=FEGbAA}Vfh;G$F0W-dbailN-u3!f#0RcP4J``1K-b_Zo5k0J zh;Y{!5{!ekB(iCExmgtJ-m}j&_HNvPZ8;Z>0T|_W=P8L3>=IPqck?1Dkil@y)J{qt z>+VPi<$-%NT7sj2wUh51LW@`-^k(fH`D#JqgwVb5E%1O%XG=- zVBa_so}FS3Tk9*SEBY6HDs2&GEV_(*fZyL$|YNS_ENa;`nvR@9)xYjfT~-}H64>`?V{55)dzwYv_TUrp2nl| zm(fbEQVlU(N}p6iu+#DK;xg*o^j;Vlbk7GR|D_vvCe!UfBovi7z1^fuHkyt1pU^P^ zh>0B&B~EO`kpdE~{+?C_qSkeQi2*EufBpl}hXKTP zQr(zgK=wn6k3(`asbq|5gr=>865_zW;`XIqvS=x-Y{{{M<4S1$@FnD3yf2sJj#9t6 zWFYuwpBpk-y2P$8zEl~E8lP^LQ$NsyG0!zK9MuD%RKIa zZKmn)@`RQ>Xo=FxH^Gd{*jx#%kmH=wRVyvCxk7(YnSZ2z@!=B537>_TeX3_ek{v~l z2Ny$NVKVry6;C+`?Jj3B@mm0O-_dWHcr$(hM#7mhB4KA4`Au?9#vE&Zu`%|8=RonE z$Xm&}^kaRZ>nE*`hcuG}J^sim6~((wIGtefV`BK%eXGI)gyO&TL~z^0Bzii*X%?VA z@)?4RcVDmK#dzg1R32Ugj@%OYEM|uZPS~UKH*LgbT$kpb5wU@?Y@+C+@~d^EqZTBLC%un#G*qNhB{{_(l(z=M$P6!{Wao+S>siu=x5YRuD+MGxw z@1qK`#cie)gVuP_;w#j+Aft~M(pUeKM^k=KM(rQ-gIr)EEPk5FaYFk9)3FGg6~OGn z(=Zu?a|6G5Ka#Oax^DNz!4Ou+h4fA1YTzgQ+`2gfHM;R{df}l|CT3`XH#m(tZUs3M z3r9nMx88Zp*?aWVTpf>NzS%gq*r}pj@gAE?O4O?{h~3YqUyxv5@`FOKcrbL- z>*bHoVB|PHb@S1AXYNW4fcEu&)voMce$7+H$r^a6orb>d9)Y2{{hrj5^h2fd2!9f? z|FhjLws4G2hJpGHG6hUs@zyKA6q*i^BIuuSqGVuV9k$zfA8+N?m_y_zpq|yup;f1STKat=)#YM%N#rE)C?TCdyGKtCB z2b^~GxER~2xuaBHL+HHF>i74?lo513Br&5(ca#Mf*mL=;=9YXf{B3^Fc>mO`CXAVm zvVDv%C=a}v2(~DRUPe+os>yJ&NfYy=5|q}@yd`v z0Hn}aKH`Y|=_AC5wjPh!d4B;~zq@;^dEicW%5AK(cBRl0lEa1qiqLH+tGu%t*~=3Y z(@gpK_s00O6l*O@WCk6M_b`X?8bnD0AhNZv*a^+arxXOtPMFHrxsCD_r)1Zm`5teUrCN)Bi7Q8gbOO@n*rlVep1^~{zn-te zlPz1hiF!k_70CHiW|`KJ!1CXd>O>0COq)YiBltKD6Zy5L;R$eA0T8I58Dh@(VV+27 z(Q+jG)KGw;OPlQ<`9bh0!9(~{%w13o5asq_Ayqz?Qe%q}w=*M42~Ya-bIb zYnLY%W~aFKtNdGK9JE>oPrzA+C!h#?mj<~(pM+&_EyHPe=GM#*z+g1Yz)pAg>QLlA z!W-lTI0qnPx}LRhKmS%wWt?L{&p_cGjy^-+sSM#X6h1Nsd69cQVLi4{B`2aqoIXQI+0lXf|6hb@KFkKjKbmlPp0040{9M#SI|0b6?~Ej8BEEwUISEkuErz za5xIVm0hh6TaLzK;8S8?(Q71)x%VNHMOpDmttNtph4(~5YEj?%rb`GQhw{Fh+)EL6 zySg~X&d?gcjWMd!6$yJhF0`x-od6Wh;v~d;hT{&PXhd%Wtg@ zkAdm6#@_gl-F>V#Y(>ja`j)qd{V-3v@9A5c*b|^@kc}YR^qykT>oMI%gLfk_-88l#C&P*BPw{>6DgMD z=nQyiU|j&GBOf!$xP>A01sR9XE?lxw?k>VPnCL6fZ#P+YIO6@&h?1F)h<5Kno>-|a zb8I#@y5W8j{3CA1kxt>`T!sr599Bpy?qf$ z-mtaQ`IJC=o|VHltvvl?a-uST9nPya1j;U35CNY-SZ4q1BIT;NgHfc|{MUH)?K z8VE9`y=S{3Xo`aUNgA>t41&X=ss@yoXefrjB~pp$+(1R>MD}rQnBuvl(ov_G%=3{7$dUH;saM2 zD%L#iPd!)2Ytjm`Z7p_4{)Lo=#zbo%fDu;@{H|_p>;p+)4^*?dC~cZPMm79zr<2Nr zfkhWm<>JBmR%<@-JGuBkNI^9%kVI{Zd-}c06M=Jv$kUMy8jZ8GaUy*s4T;DbGUjfD z^V-FvuK^*|psG)l#acGc(RM2&$hqk9$Qh%D%>Kf8S*up4a+p{FTWd&3y3KAvklW!s zdc{M63R?KhyQ5_WV(UesCiMr0iheZWg7wzbp zzGvt^&z=<+x2Rv}j!MDkP(i=DY#X#nq;+?&>niGO3aNVrL*%1JI&8{^y^|zIMlIDG z$GYcAuGTRE%g@Bv|oNGV+LgS%+16=s* zAI!ws2qbj;+hPGW!=EU`P+3i;a!aVK<(3lEI=mQ4ehpWIyC-P2ormy~{N6%IqBj%SIxutuJe!+QV}{@)#(7EK9ZI$m-y+qws;1vXCyV| z@LO2gvgePX$KnSGtygkN7eu#p9B5e+QURNfXF1{s5H-(h80ojHCB?~*oSWX1!`Ybf z^RL70qpK;TyvBVceZtczeBaVqeA)Y_77vyYT6;xDW=Xa!{^CI>Uw)bKhpH#j2Ozv4 z%tm*lJlr7Xg->2t?yH-BGnec-G3dNI22GJvs{>1ub6Nhv85Nphbj7)z_3m*fn;}C{ z_(RiwXf5jA29f3-cuq6b5}!r?HuPZ@-C6X~I<2*+-ds%X6YLGR`A*M2jW)1V=^}^U zP<)XJ-oC(JwFoaTK76L+Ebi4m*CJ(Z_g#NxcQt8jXvP2b0v2eYxem2gzBD>#R~OTV zAP(VHNn}(k=G69kasSo9{$fIOe)woZ(iGA`mF;~sCj-l}&U)vBE4|i%UpvUoK(^`X zD)6|EFm~Unf6^d@wJRPXmhE=u=$@)3H!bF_(lOjRGK=++CL@9{Al)`~6_R8z^#7O2 z5O2Xa-j4QaV~7-;8nj=*+){+28gW6g^Y5w1V19C+0;wNs$V#f`nSqz-K-{Z8YGeu7 z49->s*DqA(ZVr8xOp(Q{_D+Z)AODkN7mE;g-@8@5A}Ko{((uXWEXS@SK^oOBMLv{f zLpadVA-r74s-KNYiWw8OL6^V_?tVOh5qa15$Hwn9jZPQcPZ72Z>rRk6q)v+f{{RYt zMXHD9NNjl0b_f-GLhmN_*{hR_ZfIO9j}6)XP{x??xxpyYl{i}A`7zwTL6FK??BM} zQ>9g2s$bOxF+1%TaH~!T$@fN*Yf@XBcuA#8gj}2C{3PAT2_+rDA!uH-Re` zsev<9`5V6))16(D3yZ;MGzTG`w7LFtc6VE%XUY5ESSE%*FxpYSWg9HmUVG^jn3_dE zNpm^_O{&vH>mzJRV55Kc71B~stf_S_eK`ZG1C|%(wD)eKr8X383`8=^?b+1dZW|{l z^xUaq4;TszbyesE@v%Bis*70J_Lt}+bWy|`O0qsC`MqfLS8$3np+Brm@Hu z`#sm2_WkRb)mVE}@Qa5M9uM^$(N})ANwc5xFO&iCb~rnCtERipjJ4D(=PCbNR3la8 zF;LeZo7D%PEMY!4Bgi=86{HXje({^nlW3u#upw0NDB>ctSP4`^DRH1mzz8vMWf{7; z#@HXzar`?Bv5cDh8a(b!#Udj!1i?9Y@)qmcG6v$a!)(qbfWQx{bu3v}>QK~APiuR> z4 zUdjb&1J%3XoM8FzjNM?Ht$$y81o4g3%Kr2GCF4tmKIdXLrU+kD{DnLb)Km<$4_DND zTzVq{I4^jz^&g^@^B`*f6Ke+TA8H7(WPRF-?ZdJ(OB^kT1-m0JPCzmU^5I|-(vpwT zIYanzbxI>4+bN%KN~d+5j}jxN4cV zEUu&t6F@%opJjFBM1BvpDIdo71S;FP=7w7kSP)LQZ(Z*pCc=Xa(Qlrj#vBRCWG~wQ z-QY4FDtA>503|&>?~BJKrzHfl;Paz&YKK!oN}3c6<1CKd2?JpR|Zc$}))u&PyeYJ7eO(PH#b& zSh@R(gl}eb^l=g~1v=1`*IemKWu2yhY2?BeP*p~?nxi#>k>iFocL%$Wuiwl^bxLH| zImeUcv7I4v1y6Bby)ik7*SN!et2_3=TUN=TeZFYbgxv&2?me-v%58n;` zp2SEoJY%JBbwqPN#B4O=r=Hb{kB|X-#<3R#lH0Miui#;{6u&alvFwQ_+wn%m`Cl{S zsPOk*Fu?}UqFWHPZv!3#uVO}l9pgIJ>w(*M-&_6J1!E@B;5FW6H?j>NlsLjUE=$s? zus3_r1mDL`rcs(t@1*OMlN+b7BNa(?sGY`JD5g3ne#l&*{Nh&w3*#fEB^7%&9C@XkuG zk=_iU`BV+!e=nAzXK6*+a#>K4?f`3Mt$Nm60IeaPR ztWgJ&xpGq`Rz(B-uCe0G4p$N9v_O=hFJI}ees-D@&uM2Y_pw>f<@-JS9bfD7Fg<1c z;ARw#467q}rMH`H9q4p(n+=GCO{MIewEr9lYWGHCe&%1t2;iJ@`DRk@s2-$Dp&Aj3 zVPy&!GaytyVpL=Cpmhj9Ut{C++8Zdr+f3@9E{2$&DIr?gCW{MPzk(je=wuQ)Whlfh z+!i}h3~Cp4>@Gm{Q*Q*i5NNPYNJ<=)mSihPW;)FdenK+LdInAgJw%$i_7vX=>lDap zw@%2hIk|3oka*ivLb4) zt|+IIi?0!VsWF5R=pSi!d8%Ttp=cFu)7sTlM|%7uoNKF{Q|pWYTsi#r<9!PD`m+>| zVz65LucudUXpzX5N63)UJ`|}&!e%N1(=W<~i1_6s{N~zJg>Nb#CyvyFBaAz7(s!+^@LIet6NY@8-Tn{L4rfK*Cny^dD{*&hZ z9|>ak=mBvyqtcqHMfB%36S?gE9`~g-L}=2kJvgamj0T9k_71luCWPaDqBd9@q-fbn zspBi3x}y1dy7(GJx;VHG$Ao3hjls+J+*gUnutzEba|!WIR^z3nr#h%aS0_UgqZ?3` zI=35%zr7f@r}%t0bPeBMuE?!xYnv1UO6KN?LwLf>5f1zfQZ45#Ow3`kf`e^w8@tI7 z61x+ZQT1$B9$#R}9UaQM13@N8etP+aKx8MG*s zAzouk|By)qJ&Gl}GuOhpwaWP$D4KJ>sPNZIVGW{xcyedta@J`(qQsA^%odh0@o7~Q zFjTjiU&%hob_zb7K4>SV7cg0&Jtdzc5c?N#3@h zy`gTAFE_5qE-g_oy@?v``XKhkH$UhuaKX=ByJKtzWxuMBc7vH+0P_i=nm3 zfX!kiI&~YJV?bpcIm$^TuZKCvSk6<$)ijJw3inU(J@GHT|GcPMa?3wG8kDUAq7=e% z_g58sP(cdfjnb&4nK;P-a#Hna^5M6#Q4!sqvXghVfQyfkX=opQ>E;7X+!Pu>m_QP* z31t;Ka`r7jdjpffOz+`wCfZ{V?Oah7*)#V|cy`rLj(ZphK`N&nR{Z$3q6FL$AXNN$ z55aXgY8v4xF|$9=-#$JmF71zLa$q}lW|v7Gj0z3`-^3vF2B$*fV~@tGlsU`f7;*zS;g`>vg! zcIMVEb&kk>^QHb)T6X%|u?s7Vpi{ZE{N-Z-h_>Z1PDuNyBbY zK2jA3gkqj*W;pPE%r0Q0{1sM<0DN6_&Pq5U=YO(2?xx}<>{i3+Ulcm?%?kx)=8{dE z7`LFELX^o=Z*x1>-FxV8VW_hneZlz}LW6`2k;P6Y-~{LaTxGBSIggbtkM|Npg*uP8 zbykgpbk@AqH=8#&Kb}FE_$_?N>pA7F@Y-5E+rW5MjlI#aeTvPB&lMfoEqmq| zSC*u>vtI>+INn;F+UKn!U>lhWn^87`#D<6YQ~Cr zPYqYGzwM?sSYV9k4`AgN07;4VSxi~VX+bN@!%X#kEDET~I_!vJC*4#i7d(B=Z2G#T zRDt8QAZm3UD}C+&9Yc?;q(M2WLIk)5m8we_1iY>Cb{VL$`dG6p$_(T{hu3(>5F`5Z zmkjcfur4H5J;g|yW_cFKKi`i<7^d|IjGOmuJbQ~;kO`00btuX9YV#Ugl6*fY4<0VM zvz20++K|#2=f=iL&XRq?;jOwa_n0#OiWwJN65&;uuDU6A*asCp%#nPnu98uL2NSNV@H`u`U&Z;2J*-&x{0_7aZYHF!3$Xl>5Bw5b&dz;dT!{p*}$9U7Os)`d{|EILE#6J zokQD8ipWKZh33?)+c19!x^JJb=z9O+y(AcPF)*>>(V}gCfM)2XhCF5@`^k#=JZXh# zGWGs_0R(7a-mlSOm|F}kN0qDE1q~X6tsI30p#UPZrNYumH^{9CE_%K4rmWSa9U65h zCUxtqmhe)0H5P0t%%4#M1@7m@a(fNgrKcJH9^BFv5`xuHf+@CyAX8|FB>C&^_$sS_ zx0jFY`z~TZ%10DG!n0Ps>Y0~cq+e|!#{kVRySRUg1aS%DF@7o4wkNurv2Jbz|k;Nd1Y-ID?SNBh9I~7 z`iN0esQ-1-{x+B-YNyZQobuGvABy0BIeI3_aReSiBBcokfh2W4{2HaD*AiqpfWFc- zkgs&C-XoH=a~?BTzB6v+@KSsvVhC{F%A$@IUt4>`$ft>HfkOC(gg;Nba8L)aQt9Vw%iU$;au&20evhpcc&GWeBjA7~L)oh#CY; zMB>1{eMoC-Iw95CP%wsQq7z-BXY#L7OJvwr%sYZQD9++qP}nwr$(CZQJhNGqJG~I}!iI zT;^?7MMY$N`MhswSJdXaG+hIXVspt^#ai&?$~?*N7i1Uip4d45_KS&pT`je?afOAs zOUUV)!X@!__)t%OR;p$USgnX5VX(Fz-Sw(5?*kOUvAKp_u~Ni@o*PH?-3Q)xMQBec z9Sucq?&OLqKc)aLTkVy6zR>1j#!?4Hel>mQAmW5~cHTZPoQ}rwu z5_SSgsv&gHp9jnn5;CtvXicq@#M4WGOPQIcy3h?hTU`w~09y#k1iSFvWbq8qVu01`i|4&l9Q5#Z*4;K8MiB zK@7SYr1|1>&-1u&C@iGU)fE&XRv}17$$IV60d17nZD7_0u~Tt`5g`Zo zdJeATp|1w`Rw3IP)TB>FyjVhM&Rfa!t`Cv36lU^HWGYvT9Qfeb#DhyBof8lr}YOi zb1yFEq9M}uccFNv3uwxA zM)#l02*-akBP{>j+VIbeFfuaz_ws+35hi9v=Krl3Y57;;@Ly)66Xcct|3VRQFbq9A z*VH|{EuenFwoakBy&DAPw#(zO*PKV6UtMQgndPZQ4Hehh?vL{PKZ^3EC@jrPU=r)= zoJ=_nsHn#7fYj7i($v&e;z>zZdZw0hyC#1mY2pfz*Je^~bpa1z2=X2?Wq{7U z&`Qlrq2nCu!Ri`-)!N_G+TYdG0IaB~yMB?)PRGFG06GF>03)OUIJh+9kdcbpvb#fx z*R}@UtEc`oIG9PB0apDZBQU-tz{1x2F?z^(H2jEXR)7uem1aOqU}c->egZRizaj#} zdiz&av!hd2CnqByP4)&2j?Cx<#sF>p*)#wa1Y~iARAVT2qkRA*d9Y`-<7si^fLLgH zrM*`dE+#WJ{9*`u(|WM1fa{#y#%*gE0XTrR!+;kJPyoiBao4*2P`;q_06)Cg0944A zzGpu(KYiQAgIbEpt4 zU|7ANU+WwQ1SAzu06nm8uWo-yWAr>t$wQ5j(}N#yT1wCIRLc<08GWu z<6>8PasPo4HFdxeJ!DfqhA|wg$S3&EOiZH}be~Qyu{y3j;>pYxH9%@#ub)SwJ5oAD zE=|i=-+|vQT@y$ONKh7@k6zNBSXt?*8Qi_$u@PuJqeCOWx&}K(ptfB+ub)g&P>7#S z*c4v}W9#Xw9mXGv*XyjGQsdQLRQ|_TmpP!HOzH6(nHBwDL}ObhL-T~_}28*6y0U)M6$v)XE9 zko14L=BHJ41@o;1vY_R}*22W-SkGYpZ4%BU0jM)jTDfnA6wTNCgs$@hztaS^1+;vP z!~fT!PIL-K100+RN-FFT<@{8{*T4W#pX{6u{{s54k>KpvN4lr%`N5D1< z>sP?Ga}cJfr4``E&^?kBjN{jE?+BE>x7)+&2lYYaY8SDJ+BVz&7j*Ya<(IM_NU!A^ z-pDs|ZU)TgN0xI{Q|yIb%#?m^1I5bZMRvcH;%XPg+Vn&A0Xw;SxL2~iN7>~A-pG^c z3WW7nwNlIWkwYCgoBAjD!NqRox2Ox%9<1YQ@_Xdo_U**=dv(8yW$%aX%|$Ke-}7IH z438{4%*^}IdSqkxhW~l2h6QR5l#W9?y^?+lq_GUxyr~cl-@E2$jVN_go61u(I5$mI}X?)SUlU0mJ}p%0UXWx#GoH1?7xV zAIZrw=Z=D-4J+Ql+Hu!F>CAdVIP6Bu0=%reBv~r@S(})oU8Pqm13Pz9!2G`g2uy-g ztTs>4`*lpA1m7^V0N@@4OA`2Niqf&?e|Th44NK~1tf|*3oAtd$^nAeJ^kB>{Y9^nq z6R{V5zl~pjv!9KJ&4gXRh`Q&3QOCE2d&vcMGuydKagB0yw>v>kiJ0i*$Fbio45O3V zauaUS5S4a#{={H79NAP9F^WYQbu-xWmtW4kG=^g}L*(M!uf2n}dMSWyId302DZhh= zhGuYetdBxK_<8e+0Ci?LV!as{bjS*V{a}n8bzbNX-=PCjTRIZA1jKPUSY+m#B|5o>pTWiQDTcccZFGKg&Oq*L*Y_s5{gEQ?8Ula2Ra3tIE#0Eb2-1aj>{v zoA8}qep_KMf4CyRxA)vQ{Bqr zK@a|JN#q~YZm#$7eD6Uvvk}0-p-o1% zs3XNORoVy9_9D(vpvR} zgZP8%?uQKeKJuSxUDdHw3&1u8`-T_f%aO`?a}F#T853BkHbuosAjG#S{{{MQ-Jwtoie(kQuDytghbX! z2VOH=dTRDm9??4RB3tX zg`x9xkYN|W$qeBph|F>_4`M#tTZZj)!F}^QqH{$@ip8y;GC%x6OWO?`6K;ztajG?vNRKZDuXgxyc$tjnVJaZ*Q zKA~sK_jR#^&cme?J9tJs2NISup4^J<`*#A4p-zzMp+a! zS$iv?)_IMob;FdAtLIo}_$G58wQ)48@t=lOmQ1<~c-*H?!gTxW?W8O`!WB9Q{0ng2 z1R7_KN&Sk@GHJ^u+>b@8r7YvbW2VR}%9tlCa#(ss4h?}oNSWX{-rA1+l>o^(hGyjm zhLK>WfX316)cN>IDKxc!=az*rg7)USBSTUjmp!KUG-#~W)0+t8D~U$#F+5VrC93Mrp*w8s|0+v^4Rv4 z>}n(0GRB> zl+H}6E|Q~wHBaFv{9}0Y6|!)M-^P}ja1|VUx6`AiIIfkGbnYk+s;u_ zJPp2&4{s<}Ox7~TUOmsDD#eifdPJNj#e$r!V**wvBf?ri5K!FovmbZ%(2l9K*Hcjr zz+nupI4HUFbiBo&*daR?6(G@JV*L0`u!ns|xg8@E@?DL8(1>h*66;YatXMfLOFLKB zHYAf6QqPc4?}(SHkpr0KSgAQ2I6c1-3=_tK_RYZ! zg2=!jdTtD9g%&enJPX@3O;PJ_BjGf+sW&m1M6hZwh&dau3M)8nuE!l}FCh)|%A_y` zDX!AK0hkA@x%ZSQJW?DZA#ijmC=0C&Fnhh7#gce-b=+Zfqfi{E76L{`F5ys7jv#QV zWR52ZhWnNwiu||~yuPf@#FE!Z3Wi_s+ZR}JZJ0F<&S2F zKA^RKIrA#>*zYYj^UOrIZ#z5n)os!8O_=2nePly^vq+w8heMDqyZ)`ukl_-6ZQEJ7{AGPBs`@Z?8=(lvn?G z>0N_TU$fD<+RZ(&;AJbh(;Sr=aG^0|kp>w(U^bZ)c#($3cyKg5i*pXGQ6(zh8s<|+ z!1X!EzUVyu76q&ba)K7FU}#$lf)mhutZa?9<$+!fK~|GY`C<$g+oa-__prhE5nT7Ekw7nLkCYMWoi{2A>aIslDedD7_ zUMIy}1CDKOHiu~~SH>g@eV?%FsYB&Z!k`iL71TJ`0 zB4RE+x4_){C*K?{D|}i=1N~`&tlojK`v@g3b9|QQaCHLT;a1DD+Ouc7EK@*Kh3|(_ z95uM%}=JSIjDiYiG zIQW|`Ep8l~luLEQ?yh(eH6uqtx8I`;{bjX%W8wk&=cj&NQ!PU|r|lFwRY?;Xf8!4o z{Sj2xt5E>|aB|I1VH7Zy6DY~;xPy*-f_*7%h2aK(_8a;BiN|Ugf^TLXj$hEs2Bk17 z14L`hJ70EbXEAMBDKK;3;SX+9PaY);Q%k$w^E-weGR2PeU*>^GG$qPxG)W7{+bH#T zTr|M0_hF6vW**q4ydRQnIulUkQSoe`;0=WXuJo4*Jstp2S!|~@8S;y53;$4E27SnC zZgDnC3*$0c?l{~^t6RS{J@#XoAvDwts$KT(2ikSH`5?&L98*3zLuMDgi8^G)-w{If zg!hHVnIMOhEK$$01gcZdy(GUeq+}v(DC+M$C`1<4FyA}J?|9qo$%A};V( z(3@~n3(8WyItiXuRkv~T(wI41_HusZZT38`P`CB~+hrlwWQH$vG z5BF4V@`5IR2zBumd1nNvQ&<37vEHw{<<3ib(8{r*U&#yRTuk;MqB%^)Re0j-pQVCR zJZDQyR7lonMvXI{EpN#f8a1JOpXY*x30foLpCCG%iuR+;y18ohV?)=OzER%H7+tO1 zV~8TO(Rw2vpCly5G?A7?p#`6g-S+gz@{62c-hKV&bGO`Jh5B#GIk|y<6Ksu^Qc081 zeh#1&f(xnqKY#Zrd5e84^2}L>#Z4tiNsx{6q{2Dha0M_a;yeBvojI>~(MS4)!zlS| zL4VX_pEaf`+I>|QcEn;RyzuIjH>wNrMlbb$iG2p0kFicDUFwpyb3K>+RYM-?5gp9EIxz&kh=3nbHCl%Gb(Cx)>wu}l9DlK#+%Dv!3m$BDsF20=I z^Re)NXpP=|lQ=>3!s==53B)|4HR&Vraln&Unabk1I^Lkazz#@P5sA@Epf)E}cd(pO zQd6a9C1X!N-UCUcJsKeP<(6mv@mMeub{F?7>?@>b;LsGMs{a?!*V@gzaBJb@_>)|r zq*1V37}KL?c6=@r0S;A*f6@m^rMk)|)Cxx(Era+k2RoV=pcy0`$QB)ZlJh9(kcr z^zRcLPGr`gWLZ;l02&Tve39VNxbyA5;n7QKm;Co^)XE{c?f}tXC-r1yncsf|R44M5 znr$d6yz3MSnoF5v4EtSm-Pz(mAHr$HE+A%>0W?ktpMD3}SrO7+jd@fTT0rwYhc{?t zWNRkndaa=r2*hKOXfZp$nSNq!3(RG_$Bwm@4wUW&fw+8#Q+w*6O6uR>B?fXS4idQq z#LHxkuOSjrFkTg9Wl(3+Z&5aITIa_dT#2rgSqpnti3n~*l=~aUZ5LN?LkJ-oC zj5d&YghxZTH!AcAIvB|{J9iIcN8XKdtiv!VX~`X_lUiu$t-0h;VY_lKmKgCAIgGY~ z9JPAFk2GqBsxGm&vzD#^Ykc|oo%NsVrYtu%aEknz9%#!*yq{s`QuM2dL12CiX++AQ zWHE&zNMBMVZ7Qshw{&#$Nl(^_+FE1Ig!?r4j{s#p`2lhO@2P&oE6T6-MJAQmkzcmO zzsk$3wcAG$splf9z{j=V>s3OBHm|36_0rL12Ps*XPRRy-7TJz6$1ZNY5+B&UsvK3$X zB(CgiR3a;$O!NXqV7ye{*&z@iqC%EpvQaCctklhs*e&pRT!tS_WU&TUI8q-W;dD%! z#&Bx4U2~{Ap+2gae4CcsqIq{vyk`;<`6sZI2vQkZl=jtnf!gd1A8Hpx$IDe+;HZDJ zFW0qQ&`DYo=BSQspZhlEa|kv)7&Z2A2#q!hL_nCfs2`EBO`xlT@pNY>Y zS{&DX1TO@4W7|nJ1lNECwZ|UmXKu*Zt+ijtdn#wsW9>PSZvOhr2|#a9=9rQ2Mz;^7dXty~kq`cS3GvUgyRM*6Pc=-ZK=p1yM^RVZtuZbB_%Mp8u zT8w=l!B+!HQ3_+LE^WWnStG>BNYOaOkpeZIB7Tlilr*AFK35GtE33sLaWYIBtxNLoL{<9Y9oKUdF{lZ}%d7@+n1LjXL@4o~K&jkzzS1_pJ;*LWS%~&^!FEp7 zfZ+FXrq_e2`N{7b2qRk#Nu4F>XE~*Mzajs0G<2&>ZePL0ME?3>dh3c1AKJL|f zgRS|zbr%A;7~qBD*d%~oJZksExtq)J!{^@2sdCbUT^K&H;O%YHU<{5zWAl2=ux?km z?*Zo-WWDHDALGAcQP5bT{mfbhd}FkNpuc~8YR3Dr}H_H3w%K<lH>62j?OZYMNziOief!m13mF$ zJ~?yIYI7}_69>djby7x)Stoku>pqs?*sfUW=CXoM#J4hfbMAf2%W)t<5Pg+L=EbR3apk1N|--4j(_}a2o_|m|I5nBUa?gh1jjB0Vi%n z9$dC&!0*GPVi^*MM{^R1cL}Kp7Z#yHeh07P{TVI=l&~#f>{j!aL6cF!3RaliJu5B6 z3P(*_Ab}w+cIaZuP0l!jzuaA31@7kye54}C^`Cg0z==eZv*3Lbp$GXCHw)DGBV$5b zXw#$p>W6l?CJc`#?s^rk&0?ULi@}5p-fDwvf5o0Prh_(2QO#gXuWOAyer(VK$5KK= zkd0KHSoMjEZLi)6c7@05A3Cc9X+P}IC_76@Ec%79`zNVmjPHjdGsOUoIfJ8QISSE0 zho6|UZ`uwNF%!wx(a?$Tr@M>{R#y)J8}O~tF>=seG=`oJl>3n=myK7w3TCFJ1Gif9 zlQO#d`dzEg!09OS6>_wWB`MrQ?7GsFZah4q!+#K9`#Hp8?7$%VFsA!*IjHQnzE}CW zxQg!&GaXfb_$yZ>{bj&a<`7j!J{41q`PDt=EF+QMPZ9AV;`}{1M4IWjz>zRQOy9L)E5pEj1=4(|}F)jvo=C^0qkt%;T5Ncg{O> z2H?o87mb-Qhr5M9(&-`>5#Bs1yg|4uk|+SydE4%vi7lQIf5C{ukEK1L6+N}_6IR4^ zpwq;v<6Mac;%)^pZ(N+{hSX_^K?g}vMvO{QhW3^V0f3^*8nu%{F>13Da&&gy&zTP6 zs&~T22P4qqsFI}$bz~D%WA}%m_Ptw;KUy&2M}2D@iB%A+2_K@sUAVGNi*E zz#Jo^nOZc5m^e0kx4+b|)AUckrSKV|vb~P2Kx7ki4?<*nc5jzqDT;A+G|;*)bT3=M zc&Gfh{<7Cu?)lah@b=FYg6=(&8tuWN`#*3BZ%2jv^hd9`!TN^1adPKbo*;>d__o7tlfE44zaF$s9_&2x-~=6g%E&-`IaW4g@ z3eT~UHN(cMY>t1y5)Q6v@cey4lE%hXUQpSW8|f@F;*BL40^m2%Yi{Alx3r{||BwP* zf?YN>OVO|^v_;HtLTBhlvug}N(92`u*PxJKU=YeDt^TFfrKeGDGQFB(6#Y)b+~4 zXR$Y4I-yTM#yM3jQ&qZu484f~ zDUu;`G1S^4(nTg$@x6{+(QTZaPNX^R(H-gG6<1$dpQ<|7kbPP5Y#Qb}~lOe@X8{WB!pLPX!r6|P!*L&r>|6`_ewgE(Rim6GuXoRHw;~@7u8%QJDclI*es}~U zj!;a35~@&BK6?6XbS<1XlX@uTdbedc-(twyxM4*622Fd%8k$CG0FCv{yTmFeVXFdH zQV1Jy80d$m9E+m#q&SQ*N$v<$R@2ocW;BcX@ILYyxMa%RDV`d$f7O7X%IecgaYPHn z(!;NZ3`jELxL)&K`%rdt20xW zAP`5FG$T~fjr!R9cz}V6(Yq&gDp}0Jpq0Ei5C_~Eqoyj~`GzDgQ_UJWw|MFM7jaj*<#UnN>(T797ia{0M z8rHq3XqrhYv`nKn_rI#xfqw-0*NXTsyW*gOtmoER`UHLO$JH#>^pM)MnQ0KWGrMm% zDAT}(Bz=RCOW@?bZ|E2n&$)qX2xZe+6OqTs%rc2e(p!z8C^iy;=tTa~^7smBh&Fld z`^LJ&Oh`Xy)4vXC^g4(Df46ot?lt1?+eE2t6#X%S~vEW+& zSPI%1!M@n3ZX=q#pEL8|AVbj!TN$@_;|va_7IB`NwcT(sqggO+G;eGx+gFdVnux7z zJ{;E{SZ%Gd<%?xVSM%G8x_&!06L;Y}&g}P}EXJdoAV3RJ$Sthg3^;3MUXSMDi1IKy z>W&qB9u2r^w+6<6x2ec>#=T zD6huwtNmjuIL`ty9KL1;Wvvzyd@q2>$qu@ML*vgAO zil=J0cn<4by(DNL))e?1(bC1ngL2UB84NGei%Eoi`!)T3kqJ}tr3k_hv^*SG^eQ+m zha{2j-2~~aL~#PEa01kwA{FYgm<>;vp_b=oGdK9U91=*<5#nDMkalF$Q^5pUul44AsG>*8*AYZiqIf@gt$9Uw9BU&g{euq0;J(BoAX-oHeS5 zmZ}x20u?mIml1NeMD=u;K$sr1YiE1(&sn%>O+j<`;Tm9{CWjTvN60W?L=5{@A{p7o z5*i@}76Hv#0he0r%kHf-WPD}ueza7!Jr2z_82dPXa7U?0V6Zi>?cu3grZxke60fe| zMP&A-xP2Gl?h~H5CnDs_C@3+!jAEGllu?LxNs{k$ya?FAPfyXwNQmmz`>`|E9|kJA zT2x9*i8_ouTx_`Hr+>O;;~!XAQ#6jELPsdxUL5OprW<+tcatyUSp> z-KX8zB7_@obM>7q^vkJEO8MQ8N-^_wo?*Gq z7;^QAlcFvv^~vCw<`9)~oa5BUG8c?&sk7-9RYi8OHIDM3{&d|gFaky7SZGoAK-U~a z2zn?NWBrSX(S+62S}`uLduP&LF4?9q4%KTTx!khMSzpfF{MaExM^U1kj`y?Z1C$Ra zjU@xuAWOQ=i~I|@sjKRq5VDtcf4_y~w3e}&Y@WY&EP0utNC(ox)n-kw%rL{i;16ul zr%GOgfh{tiCeuPR%>MX<{+C%ia$3ppQIR%h-v;mg>h+6;?JJ$8T-<5E!n8~J3^Zc! za}d3LwRoueng$5|;4enLc3kSMEC)+lxn)ETHQk5;fmKYj+KZ9_{{HOz?h#a?&<8ue z;Bnz4?2Ua~dF7-0+(y>X#C74tVKuwCDzIHdSlL;9CH?3}ap+&}_Pkw&4o`%)@=XEw zITz`fyA%f|!>6Y+B9-lEmVvS2t`ap%$6652>n%ohn^_NhS)~1sX9T9u<@K4;Q!&&_ zOf*JRkeR=iP!j3HpEfg336!!jaQxNV+T}=_Pfe^%bOBW(^CQr!%P*g1k?X1dJ0)5}YLb5wgE##m`?+Zdx?u zpk%Yp2{QTVnV-{o2lFe34C$z;*I(YK75amdXKe;gJ5k;!WhLR(avjEe0)+D$nH3?7 zE3%&H2qx2tAGop`tq`cnb5(+(`xHePiZNyyiu|dQ`HdlI~SgnWhPVeCN@Yxg%-qdGX zt!A%p4Uck|5oMNbTE5pnL7tK5*}Wy*)WGJkIJVJ<(!V$=o=7XZ-02FVv_mw>J0Q4? z#a5d##K=pCiR{A%r1exPPNPz;4UWTBtUtu{YbiZg-ECF#*as#m4?6M)F`c#!kG(9JWP^m~Pt&@HjzQA;zvftudI4pdu?%75{;9(ZcH3ZM`f zTAvD=JZY+}aiPMHPeG8~l^+h%CFEQh#JDM06hVC%TcR<@^B_-C<)*==m1LOL*87lW z3UV_Eq~c`%k{(FvnET*aDP0bDs11Y#g#L)eOi?=9FfAzhE&0w~vjjyZaC>Ag8@wtk zQ~Tb+jt*#E&)K0`U^!8H+02=%+rfHTO3Nn|5wG$)0GH0pSvE$CGMVtwYCmf++@ja( zn1!|>3#-N7b!U)hc!}SeiM~+(J6W5LX>+T07Kk7M=GVZz+7mv<)_=#ptUs zekNGHcWK-=w@Yy8Tt8yH{=g7QS~Ga>ablbzOB=(^i0jE}N$w9_i93gp`aA9zvQ=u{ zj3^bTz1H&TW&K(yts?4tA%t@i(6SkD#LPI^9p)wgDX?5ch?W*VQ-X^KBiyF>6D_`QNR3fWZ}BeMt04Q5&H$kgIn_3WU-cQ3*%gI z^yw;I7SDPj`OsktF^?)EnMC*$fJ%0`X~{zsCcH2N9kPeUIQ(^3r zHqnpLJ1kU0{U~PVJ}I%IANueIx>dcWGeXz`##T;dAuZ$!h?Vd?z$1lhj<#B5a4I3H zkG|q|V^W5a!I8#&>>7m2h|j_$#V1xM*n(1bOMAaluPN{qvLe7G=ur+)IR97yd%KRe zmB*G77$)zMV)d}5Yp-8}8j_M>KNq)rZq zac3Vd=bg5;T1HOdreYO8$=8bbV+)KgGzxgxLo;xLS$zx6t{8^Tt&_6`h{sL&r#YRz zdasE;7w^Pt$?iR_5Ku86$ybv6bVmgWgT_ovsrX`P072y2`kNjQ7k*0H2!lOcC~ex8 z_iNOGuq_sb12_5*f{2#2&j8B)C@zW4Y5*XQ?xc_bZ1_Q+mk_1p^i{1e-||;2D!=mGb=p)LDChE$;2Ds}#+8RAvf zi*Z9*pF>xwz*H<#5P2YnYX*-dZuhP$>p7cQ;(LwOaZ)6#TvlPG&R#cNlB(A!2wj+& zpBaj0`pJ~hCK$~5g&x%zCq9!%lyifpob?AT?TBipEQi#bCh9zudfvLDl(rBcJzllv zc+y>{TC$V^xk+__1v+wkgT7^~>VYu~xXVHN^bS${E*&8XtwlIvlRuFH7(ESA1wNNJ zQ^R{R;1G_u(mjbW)9BuGN40x_@+hXUS%o zXI@R4Ol#!LJ@IKS#@evWEgMCiI~6?>yV?Nv9l{(6yC2$Y1F%USa+dW9DHCIgwn_F_ zeOrN=?6R*x@Z7|QZ(Ox<`s3Gh$E)!TgG?&qht`uQq_kwCTw0UejneDV zbX^O-r4+ZFqgB>z7P3FQ{LmtqnP9cVNB4ey{RkC-MNpFe-Sn8}SJoseW)vgGhM5vAQ5@WInBK-(vQ{(pc@91Q;nI6>KdQ{ z{v@-ouK`|NX;ocaWsx&8Yq%gF+}UCTi-MnBn4E&3z2So;)bKlwQIQ(Mxi>4i00E`&+0)WQ?fY$P0!?kaho zrSjWJ0PO3Zot^wD<>KE$JhL!0hXW(e6teP4E3SRGvOkxM z4SX!D5_n1%`m2+hO-hx^Gz_~k6Zb3@Y5fP{al_cWF1aYNkQf2RUg-VMOTndve{!|O zv+*6Um7-kIqY0EnRGhTnJhxBSf( zGB^-e&)5_aPz}KPs*n6v)Hm3F#do}R(6#vy2&19bAr`prXY@9g?NHT2ox@*X=Qrb* zLzh+A8&d{Uy`-1&M~sYc{|NNX*nJdEZ~w>y-0n+&ac33%_ZP0v#OC#t;U~0~du;=< z?|0Q)ck?M1+4D;V;G;J+5b$odUMWjQ9U5frSIU9d#l~fF6@2uUY5CXb{nzjfU;fAT z&+jgHf<3tZkJ93A`R{MpH10M3jxTms!V2y~2f)PNT1$YR-I9*J-V_x~Gvm{nx8CLl zgvV781n^D2AM?WO%&2ViImu}_1M^RG&7Xnh-!;ZKT#T}lBa_$BW*|Lx-09zi?wXY1 z!OP|S7s+ux<-XPF_uHfeJt?wEK9#7%OmMxM3yYiR2hJnaaO}Ow`;JHz{#rf>7(GK! zE&&~FfZF4C;Hqpb`JXg$a6cI+h=8mNi8c6UH46&HThyv@szAMoQ- zAJA-qTB@H8pWi(0+I~OUn_8x?(ai7VyIsAkEC5z9N2%3SgWr0$V#>{4qE}1SJInoF zfZI1ftG~aBJ}j<2-SvG0dC$G+yY^qvKf^7)dNryTM>j8g4c$4Xy7$R3RtTD^d%gR* zn^R!-P9JOomRw$bHNJqq%WhJxtE*mde5!}el)v%6U1xs$ba^Bb167H|d*GiP@Yk)n zL%SXgS5c6^MeFl2#(MYKcGq^jqx{k)W#ce9j@@Xoco#m6T_iHTBrW1UN1vU=fcJ)$ z1s~4IjF8Z1>a}~x(D||nz}L{uY@wLjX>YtREZ*5$ zbGjRJp1qHyi1iz}E;?DbBg$fr6{+VpF(oLOLUgoZG z9MT;@-Aj}IgwAV;^nhJvF}O1EQYC9-*=P$d34;X{)v2x#CpPsUeNsSyIQA5aOnyJ= z&8-d|Dt7e7vV=Pdx5jxu8Yp})xW9fpHz$mzNo`U$!SL1z zd3u#~udH{v2ypHjIgfK_S$wASQYmM&btn#!xp{8{yeM#eXyD;;ToytU?caHE{Q|8T z5upecuIF5xD}&N^&xVF9^o8|{M1=fRjdDK$TbN<;oifd}y~ z>w!KemGXk{1BpE3_q>84uCYewIRy`j=5bmQM&{%Oi8|SQR4#F5ovD6EQ>J>;i!#@_ zET_GEo+bi6QFEy$kFqeqwR;*s;HHdOO^%ypcuPg^V>pA}TWP4#S@^M{rPOVZNWyx= ziSOS_X$SH*0IEJP5?9seN)Ap=O*FN=LTM=IlQA>Ry7xSh#6=cwlUnQAM5#5xlLSSB z&}yj|UZ$Ir1gqv7+TGwao{YnELW)%ums{?hl_$V#vnIrZl_Jx?21z6wIU;X`o(uQV zp_1IspJ;+pi4J4j0iOy9(n88C1kSaNmi1T~SS(tKGxh*DlAi_*cjemNrW7rhQrxt_wBYh}s%mR$b0|{0r9mGioda7|67!}=ZI)@; z&Z0LXm4M7SzrMSEwyZ4CeINrW4^_{S|8=@JPJ%^cjd)l-HZat^w#7C`?6GGU+ z>g$0-$SG`0`YC|zn2opMT%5K`-{BNAH#K99D+Y4`T7T@V8fS@!w1YraL#na2vVdz8 za8;98EjKv~leF!T=3{Tx z(kl1C^R0#`Qe$nrN){E>7RBjVqiW2M$J;{{BIz{N_6vZXmZMh40!4dRkgZ-~LsQ8U zs(LCaq%SOkmnmr+uF4Y&d@6HX-1H4UA@Q?L!DyIV5%lH0lJRf(V|#{}PILy=lbxOJ z`c#5=oz0ran!o%TO3{RA%wWUFivI&|K#;%Jv*Th#x}09*`+P1GN>)?VTW_6yzu)H2VT&4+ z5EiSwyg#B~Px!8%=j3xoeRMZ*GZ0AATlH;n2+wn4b%WE`sk~<0|RM3#R{ox(3|HbxNC70Kq`Y?`hidh%&mFuNn6J9`{$RMY+0Y%QvP3r z!h>`P+w>z(?edSdD)$YuzT2wU0E|=)SS1h38Am*LyT;-!+CfU?!(WcQVoka;VO4~M zRgTLRH0%qh@Qn9n>Aw+s^Agt**{rX6vOpa)YSNr}0%SVk44M*3Y`a3`_{ZEh<&xRr z1#351Ai-+iA6=;QRIT%R@L(B)1WKexs8pyAe7+jj&Fe_S>PnFK@98ka!7XbJ zF4++==bZ$tmYir{<2U0Mb+pMu55SgpsAaM4cPPP4272AX6%(2oj_K)yV-XBoT&m~R zplhkUTzQ1#-k|`8nG>9!7l}h$18fuTn-ksf&*DBrnK6NUiIHxA>Nm4-ref9vJY2+b zYii~mlRiY+qj;uYaz=6c4JtUu7|XW@qOogYm6--CoCCuIeOJmykb|n!x$yUObOh(~ z+DF5GVxVJJtFo$QuiG)V^YK;8l7$@hgoWDV2pb8ni&4s~-z&=8YUNoxq{esZ1jT^p zK{nOjvlcC;s-jmYn)l0RQf7V_Z9LtXzfuCr1Y?5tbORIt9w7-D|wn-E6==?LT6iWgSppzsg~uvDVnSOIsgb%B)IQ2I3bOax6@pB>&PIivUPRdX zo3bb-E8No6qs#k=c-SO4rpqlUvW+(zrw4)bWa4O`CY;G*4zp!Gch(DcrsJ&z%KHlu z!Bge{pP`0H8Wg9tnzdXn*g^JGw_vQq}Nia@_Yq%B{qxZc6!}CLB~g zPDd(!fzJIYSkU!rIuR5kb5D> zs}JV`PC2YwS24d(qiig?q_kP6&OyHIddE;N4W4@9m`stAT1W5ZBvwcOb$MI|X3NXgag4Dl+s#*G1hs0hOep!$G&7R|82+aX5Qsd>b1z>LrF&J z$=z>d7|WOKwD8ZO_f1p)*H)$l%4Q*jVVX2r^(w83ys4Fqa5AidQXW1k z5`BsoO5I-WkBM}Yszw!3UqzY5wWU)NhYX9=Zt$=7_(x_+H3h~`R@`hIZ~+ua9kb5Uyj|9al893&cOz2u#9PK!euwZZda#H8Mqc=zVY6( zs7Mr&lH`%T1eQpwx&a1X4^PN;*$Kd?>lV(THg0OER@5w$M%OiC(;2i%LeAgdu$wVK zIB0$??=u?s_U~DTdQ-kKS&t3)c$a#FkZ+jQN%&L`f6NKHPo4T4LuWV5($^l>P;BrdrZ zE*^JpH{^^ce^wYG+;XIhr5#yvtK9OJJBx7gDzr&FT{OC}!5&9^wV}T(^TJbS#(;xZmnbwa|9w^Sk;akFgU}ypigopHV?+Da7}q*dU*(54(dii?^J!pWGm1#k zKIogeb9@+aMSj3fhOV%$S$B;^=WNk}h?+!nXeKRJ0^1Ncg<5;DU$|Zs^&!&q?6Sjx zW36~g>Mk%bWdlhLT68U^*>421h2mV^g6Fs0gCHO!NsdIlUI6zf@n;m5__E|LN@j-K~qz#$k-&QVmZ3WXHq#o-UhAo{bPjnqhFt-vAm9X3a26>ddd zXx%4ZgdaoC()_A&j#q}jJDlCEh!BzwpZmRJ~(H+~zaiNJ9Kpt|_kie-y2VyGUbWLvJ!vJ?DX z)AiZgoExC5rY~DIGa*RE4;M{h3|BTupbv!~87jsdn6R=oNl%P&n;9>Hn$;81G$bWw z|1tl~mK&ZcUa;443d$~Px5(neY)wtxxWnh_MNqFGZAuN5(B5MDmvdkogZ1RLj8BGH z(?FAG{+7HYl$fR*ZHmwfe0#z%9MA&{#fKA=B@;|yw53y9&eeBeUL{W9?cKg5aQ2f4 zH`|h;!0Qw~CD>wI`FZx!2Aj1%*IG)(?;phjTa>b8DOgo|bk@pllZeygdb-+A{*Tv< zP0jkfjnO8kfzxv|_M-z7a-AoUs&X$#L!&X|rgvW>1@}MJjhzR?Zz?o<@+;)UHw$+9zOI<%14l{ zNj(uBb2i^MK@FO>{jKT%YnIh?oU!G6dyiJd=kC2s?RuWFa*^zgV(o9~(!<34-TOVo z(~9v-*8SVtu0srM<|^n;NG`*!oq*hVZl`K!?-G5rDNr@&fK*1O^Vwwr0biujeSf`| zMOGku!R(B zxC#lk_$)uUKHQMnZOzGvEyo2}kB2-tG;6T_&bWH=pbH#uWW}Z%)!gcqY8zPqc|Vd9 z@+RWvqv>DGO4e8PIUY2ce65jn=QfqiK)h{S;3$5;gs??Tqdu=kk8GZztiStI%TWam zH+_&2@pndG+O;mX;kTtGy)W&9HWoWkzB8LQnQD<$ycXoHKq&Ma_}vD#E$`}bB3{a6 zEgrn4k7egx8Q8tD-E-bCd2%>~t?&BgfK=-0S|B!h(TWUFqX>2dkSGqpLi^299>yck9ynFW+?HiYKY+`j927amw}K1 zZ#a4Sl$UnyYjsAYNWL?zpK2n=F+T-1Eo^ATkOdU^^1_EOzd%fd@eVH8J z&Kp4i3&5_o1JGvfw_-5FesGnCoDm{fK!BZeTw!($Gt0+OCQb8m^)85`99&I z1azv)Ow~TRc#n2dl#npND{{!tUwDxw2ad~zX)Uc1=cPRGl?u}kV&jB7DfbOr-ns1* z2R=CP%j;bxSQnIKPQq@!j6Xdv;VcyENgtM^ycuNuwL#G}(L>3ou%yY0(Y2^gR)a*c z4}W66gicC3wIIH)fW}m@8~&}O4GDV@TlgwPC2xnC$D>Y@SW6%46V(!7wbJL8cVi{T zd!iXo<8;@DH$8U&G6YsSvP!eeOndllc$Koxh3h74&1EhPJ#a(U(6&*1n3Z>T24s~! zAEe44VrX19K54g_W0!E!rwzA@a(n~)KF!>mGE zN{3@t7GYGYqW)d}TyA>wa)P`|R7>DUY+mMsU{B=*6;V^~`wDxlP$ff~?YizPLQuy$ z>lPWQCfnF0*7MBP_U=}6EkYKfl9M9-h?FdWz7w$K5DcEJh0KHd;0p6 zt9{A{7<#3)Ryxce z9T(1r8T_bSlzH#TPgm9tlv zD1O$k-l}On@lheuXt^Grtcv6u-|}#gF!1pX(+}YXq|=^bvOhz7&-FP1uexip`k81% z!K0g5k=zd&`Me=?JXvQoE-n)#^)W;JLxbV+s*v_~JpmH9;Ke0TOQvg@^;+a3m9}LG zO>X|M+mP?|(9z}Dls&V(ne{gNS_kbb+a?xex_w9pOZ0V&)PN{Ygv#4}2E2%<#H~;h zTonq}svdoxBB7!%S_0S4g5p;4l5fq}!h`%#SnL7NJU&K0KkhHh$qZpjE62|n zrw>Ye+RUNGhB7|mq!ChY+1-4<3u|k#LAG@;a`+{nV1R0Iz}KqS*>my(DU{v4-7j({ zB!1V#QEgwa&kuhdP2&q;HPfvl{Gd;bT7Y(SEob#26%E6Zh#OU>!Y5r-o^n>J{*5YP zF^avp&->yg#MK0Qim27gl1+j%kmZrt&^9*sT}`HD-F^#0;(7^(@K`EG-6b{ywfcZH zU1kg6LgI%&y9`P#P1_pK$t=E107086{9bBiTR+^(FyYyU&@U^Uqu5O(C%n2?<&`aF zQ*GlFkfC4ps*$p?V6oIAyLZ~RY{;?lFLEAi*PWPdY}(6|l|4izabiuYUHi@A&ySLU zmA{{yi$~R`+djk1$p1)u|8rSHG!v%d3_SQ zJ^W#RQ#pl?gY6?SP1mji!&+zl?P7PYi(`~R;KlG<_^{YrtX!#$^(Os zVX0Fn1fNlC+=nJFA%3A584E#58!lTKA_kX0i7-}a8z@mcGsbrye4G4SJQ_N=Bo-|O ztgw%3sc&n335hF^OL_i^FL}tq*_->VKc{Z^GY}U=L4lj%8-Mo3VunFqH_AR+>Jv?F z`+yI}tCY<5X=eVcY3@_T_^4)Np%J8UH^D;eha_)8o zR+m@ZWWZ{TB6@q4evy7%M{93PIoc@*Q!K4;N1t_5ZhYWogcgX&qwF$E=kxK$r~5sAjca4{@%7jc=ckh( zi@=b*DAuBmTFjU5Vp9O=4mc|BSoPG|VFYH=^gwFbJ3(Af*`KUsoi{L0K=lG^hL$$U z?q1t}-k)Y)D7PlQ)Oy*{Jig%w@okQCZU>AAXOSuHjsc6&F`JABG^ReUQ*Ct<3(^6@ zh9!!p+A5A>+tb&_-+kU0a){~~u#r0F_WFwmh@&ZIPs?m*rH|LgxRm(_?X4yr{c+#% zm~P_2d|(vLY+h&^Sz8+cWiscD(8=KbqO-5O}`tTO$)!K?S!~S|{-&jRM;;Uz? z4R^S*>X;9|NRF)GGzl=-@1~M8e`#!POQPL5#&7(B zvE&c(I8zOKv2z~vH#D{=^Bsjv#KGH(CW`&KBAqEg-S2&^bq=xWSyxfSyLUR6IEl9> zg+Y;N?{N3vL>mqL;}<5|^<2)=z>^V#yU{Tn7%$?{shYHK5wrEJ(s9g!_1lHs zu5gYS)ERjN4)*R>Mslv|sOHlaA1eZ?m@CF40F=9jc1x)$hJW3*lAo_PgQyO7nb2M%rXXZEhZk~UaXv{UE?u~Hn4p7vu$YgwT(qR6U&Wm zL;9_*K`giaS?~g`d3fyg;1;LG$MM<)HuzZ|S-~IMAt;UdQm6h~pAZ|PaqQYuzU;KC zoWpI3eFIVuQ=X&)EEEW5j=ym}J=&$pPS`y%0&PgGe@l}qtz%qap_+ugdu?^#bYg>E zn-zO>@;Bd~NCWAFp8{Bv)pf%1A0k@m9DE^(`|*9Q4%SoNdn|?v4|s@&Q|w%y2qpsO z;m5bbVl3oA7Y$`-{toSG%jh?PVghOSyc|1q+XAe!i_pa*(N4QeySLe@@0~uzY9#u? zwCKddm-U1*)oaG!we;F$Tymh*D_HRp%8l@oyVWkrqAKlS^w6GkX2bN8LRN8nBG4UL z`=7iX9(qc;tU^kj|m$oToi;;2o-5j))qE)^u~&7>uR$6{#+oT9fTSj zRH^l4#B|_y4gzD_yWfc1sb`Q6Qz%qHP8JwcOnX7=BF%&J!vq^a?r+q-fmxAG^o}%P zDr0tZ$n7Jzl?Ngqt0DF+M7MQ_jlO|cAc^CNPP{q#xq0p?P7P+tF1dC|&ylxX$!!&A z899*%+h^O#NSIfv=-~u1o#w942ZFZT1!B;(B__P5%{E-!sRrv%JRod`fKHSlb^1+p zTC}KR2`5uq@6frz%=ZTqa-EeZBSb|k_cv+tqct>dq^G)dLvxHrqRdng&(dZec_!E& zm@u9eid>;e86dw-e}Z7GGNDj6*WFu<2k<7iGJ3FNsV?gaOI-Eo@?@gK6RM*>rj-$A z*JkGm%SG$CwItuM3Iq*yQgz~zcNp(;34^9lQKP@jZHk?LT5p?LlIt~85v;YHfOAo7Vi)UH1VVCJxEmYJ>R=9wc~+KNVU*e- zbX@yb6UCVtEeg;*1^dkN4i@G zX-p_;r{)&f*R#C7Xsm=BF1ytxTUYep06iULB$AbQ;KkYDOtvvXdT$o_O0_Xa2)dUV zJ?+sOzRpHsxRm1=c5ehUEMbG^2Ir`I)Z`O36OMAs-bO9iSKB?4wIGr4QPU!>q(7_N z#C#x3Nu$$`=k7k${ib6O{!k+kSh8C}G44T|vxRWUwtxKYJF@{hOJg9Nk0ToLHNw)Q z%v0f<;lWOIvn}QTrC_i-=_qi1jqebvLkP!(I=>;=F*{xt2cXPAxKI}%mT0|SJYwrA zn8XX+89m=thPfD9{kATRFr)G0#e3G+W>xiLHVv=JhrJexR<8MCL72cf*LcvCWNR)J zxQBeh=-V|NZ4v-PJi!!nXGTgk($hZ0)htuoRO_Va&um-b(qtNoj6H`yo4NmKUlv+? z6Kz_A64N}rQkcFGx)Gcq3>*~JN54ODrP8YxcXHWZxp6Gauk``Zjw$xf9VU6P7l3oa%T`3^{yewpH*xRHp?+CV_^Zh##u1Sr+K-$bMZ^Cx}d{YJ#v#W_K_c9bE}V%sFT%A51&LJUwDJd z8U2)GX={3^bz=W;z0g~<`WPLz)=V&{uu&rfr(b%?z`%Cc<+l|^Q-eow4i@>3sL+`l zntk;sTrq@CU5;9p>rDFk4bTvv-D9n; z(0Z$IhLR5EL$9~F)sVd^^U1Vc0@HIh6h2e*cET}zOZedZZDoGLVZbgf9Og~%JEOQc zx+CEjcIQmNjm3SsyQ8IL@hfAVQI)u8HOhA|Nxs{{W{h$y+e{UKV0Cp`1;1}H0YXJi zlTPl#w0&U=UHK=Q?GGAPd!NG6*72hShqRc&a*4?@w6hSr^=rhb9Lgsaur<~R-)yRWKw{<{E zdR+U#Fx!|s=jtJO5z)ED<{|a_RNMf0!(5QRM|#?YeEOD_()62iJQC@ zwin|zHN$r^48~LG#a3scy;!?y`AZK5foEoj-^n!m zC4ZDUk}O{8rOCHA6DX7>w&Fs;qs4C%Fe4Y$I;4IIEoH!Q&&M+%?;yw;>~H+xkFBP2 z{wCGu>2jQ|_~ALyukRDB0W%l@NqOuubf4y0tqU{M*A@B;be+l77*nkmpvUcTwgF{A zxrg6SC_&z2sHlG)uQOu;|AfY_PpVS0TsC9*lthrGA}dVfDJZ)tQ#}MTXpbs?t*HZ< zWINb<<`VYHE{yjZJ*W_Z8WeT^1^>mQ?q6ITr%GE9Vd^`jtSt%7vROm7vNVr(>Cm45 zan-o0X5|f!zyiSz?wM5KXVVxcgQZqYln}xRoAKIy%pUuKRlfJP9MGCaqedHV?rxe$ z9GrpQ87LB*#P6bzBLE^lR=LWs`rMjF%8Dj{mI2?ZHb#q3wp;q=~)mBuhjufV4@ zr3*x|6VLh{X~~k$kw|)E+&tm5V=8y~+Sl4%CR+Pz>c}!rD9Z>V^1{2;X;2EkbeJ^y z0uUXYiEczXlH(X`=zY(3VI|Q*hex#p&dV_O-m(4;|<)zLrQ(*Q|X-qQ01G<#C=D!vf{8oPeQcwIe zbHDkZM19-;dBjaFM6ATnNDehZufL*~77S zNl=bEGoMj7?C%YF z5gRe~YdRc5!>o|j>=!kcF&iBBZv0`1u$;*8v^p9)#+L`MqlNR;RQFlC)lfn|H%(rH~%d^njhhr zSZH&b0w(6{@lAk*Xb#e`E5)bSkOH2VY}c=&w5`fdKaVM75aLQJKQK1=duojlq{t!NVeoKix>^zp8 zc^|@Kshs)YOjj<)nm?5mGn$11Ii-uY1XhG7r)Y{~!^S^-vu`--267AyFQH0Ye1g6k zbmildWt*^$*vel(PR-!1wM%x*zNLmBLmEA#+sOvTQ=TWD7f^@<(f4`bvs~2rYP-(v z-UYmV`_@ZW{~xU3duxOPAUQBI3NK7$ZfA68AU8EIF(4oyARr(LFGgu>bY*fN zFGg%(bY(vsg2!-4T;%kAS(eR5_v(Q+7{Q1oh`mI zF36|+r6QlP^VZ2{>OvelH<~U=Tee*iG&jm;^784rY2To{}98rTM2^dERSovbWDJdnHiwj9s+8J<5j2N&Y zg4Y47D^oP3WNA&14)RkZ&Kl|Y0Fx6z!RV8SgTY1sqpdVczgbq*dg;l@?yxokPq-k9 zCu`(nYsjLpRh|%z7)XSaGFpo~;f*&3Uwm;e{P@qaWj8!HJzcF2h8LHw*8Kk6@#${| z!&j^G*UR%?N7<|6%i){h$?))3!!HNJ^X1{X`_+}uXV;}@d!Hdvw$Hjvd+)NKJ?Osp zq8nax!`G`HSKaWa`_u9tmy7lB>a@3?bzgpYApbwXFhc5s?8NADA0Zxj>}Bi4F!mv* zox`}ee1-q=ILFe5K76|Ico~nCt!i`bN6|w*l|GHoY^5*pe`7qMhtS&Kdx)}+sfUIW z5$Ur{H?Na%pKu^0VnsFsk?qunJC0)}FORxkp@_!rc{lv@^Dmuj`fNj=L<^i=o}9eg zi|4zPQ!fhfRJ_gWY_yUj@h`>N72eH zayL9&ovhAZoGlKQozrKYE!OMh`Keeoe*Mdb_17=fi}kWItT-4RuTIw#+Q-oQGBM&k z7K>+GeduvUOC|UiMnQ~r{USESn+YwzT+}Z@$%rMaUp$P^^hIn*m`KB%xTz(=KGexR zh5;J=^o$HAY)Fgw${W%$oH&h5`has%hXdO-&BtM&69EfbH%@3UiyHzm96xeFth<+U#qw-%v^^9B;4QxQZ5i5}j{u z#S<5xhwpkTp3S`-)2(=--}{npMH3&T7r)|WyjwKJZd?<3QSWy{7QeBFA9E*Uv84Sc z-KaCco^90mrfWrhwuNUqa3=rrjoNOy`L=I&?Y=!DE$}4EE>$ttHSMkyH`?TBZw5H8 zlMV>*GN%@X7jBp05%Vm8&b@Z6GHSMKo%tiXRvnEC=^Q~3hJ=_%Kb3umi1d4@-)p?ERh`{0bC!)^1nMWk;ER6nzd0BWWzS`K!W+hY+Jv5zdHZz0%IHAE>F&ei?fU2 zeEDX0efVy*UY+-E*Y8g5%}-hK5!%j)r0WzyNdHHNY{$}P==NfS!3e}*y`c1ikQ zFHfb%#o=1^&hmBt?s(lVUtfMU3qssvI&O0&W_bIZiK}Q54`h2go=vGYcc5oQVWj86<OHb$H=dplmSJ&t!iWiSx1X+AO- z*y|O{9Q(#VUvUv;&cn>y&zuLGmR9HCHVfYkXGd@T{ow;KEtwO0_@HC?%P+O*aItAR zTue=TRG%l-+4<3{?$-_bhDXcaj}Mp6zy9ha1|VLa?-RGH?X(=M@8i8-rAe4F9LN?b zPOTC1h4V#;yt^cN&hz*xcToa%(?zkeJahykCeFO17ney*LS^ryk77r`uCz#3pRaxR zjAz8hsy!$kp^?ye`zT?wr4KV!Xj$u(WZUYKSl|lV8pUbrTefIDtheYbR|H~!EfXtU zhcS6m@H|d(#j57o7R52bA+G~=$wGs8FJ)21O%dJvt7v2ya}i@Fy}+ZFIf5A_;>i-& zPslya9K^4Xnn)%Ep3#G|L@5UAke&F8wOmZ#0K~eAy(ck;0TheFe3_MH5r1Gm4wgr$ zi#TGbWvws4g4?WtL+UEg;Qh874NliL&ugtdQ|o=))X0*G<57{?2NVfMM#9$YxM*?H ziMA@kv`h$Qiq8xA(Iw)5 zzEDmb&y~xS(!M`qEksWdZ%4m@ME%%Z+tQ!6G>^?|uP9OU2II*hw77Y#^;r8bVv#w8Y3yVB{Wm z=rdN1yYlpq&7 z2>Kfvv3Tw&-GWxc*P;*0I$zsm!Ahd0+rl~Tw2nC(5irawda4nJJt>PW3yAY^E*6(P zIH;nKGoA~X$_;`bawU?HVJ^~0^QI(J-6sRmqElcdl4y3>>c|IFXhvOp{fGwHI|Hec zQYmFt(wrb#y~viYknP(RiDhC#*X1%NVu+GQ=Y9p>t|9Cp%<8hpj^;`kc6 zsL(l)4EkfR)e0NkfKeyNmqM+5%6Qwnj-qH@I0D8o|wQwwl6mr%&2O?B7^X zT69H$dH~F|)VAq-SY~9AFmU8&#g8+0{?jKErbxfDfxxKWJ$5#XP|~c1EC2(-1p5{P zW0>UVSUW08dZV&nh29n7v{V)Gh8ig=h1!c{#1*+;wHL)CEG5(Z+SWwso9(Q%q_s+M z+lCx#sc`#U+qbnXKeW~cHCwdl9NEOXO3`BX`w^k9b8L6$nXL#<3Ur5KEUD#?!_M+u zVD*Lcv5$TDPNeI{{+71ydCTn4d0iT5_6cuz$XPg5(=(ASGocwxptr~moZuh~%oQq5 zWrw3vp|F{R_FcG+Oj(FT(r3sB0i&UtU^2r}Y7nX`(1${T!p#W^NnLbK(EIQ#(`zlPAz>+#zsO@}rz=ngtIv&_ugU!--o?$4Y^g+4j zDFpu*$Qw)*N)(^2eMBF&UrJVY%JY~)w&anWXxi&jtW6yG#3YRt z@Zbhn5dw``(4#Mww{1EPmg)R&qpZN`R7jh49gRwyVdl!-1y=k@d(%hbbYwsASuZl&(JNA$NSlD~;NA{o}Zm2wfXKx53r+x=M5}wY+Vs5{EKO8_!`Q`Khq*&_!WLTv1Oq zkL9*r%ei>wS_NhXPGMpKYgM0Y@D&G17r5SpT)nncd8TDT%1x>{ zg;b&FD*izhc_D})NnwDQuaq;7OcclaxPgk(wnC@m8MXVsO4GVcFD-;tiBk=aH=$QN zcMPI#X76yBf$l}2UP%=bdL>e?z~OatJfo&>Qq>x!l2#J68(Vk)Pc@?5C?+BGOPEzb zzs$L-Wvy>Hr>#mFdL_@wOsUOqDsNa1?O<&Z;cGph*U2zevqP6bTPE?yh#oXi6@g5sJ4TXxqscPCqVKd0; zYZqDq&8As~UJ1%dU7cjO>kIXCzyJ@QJP?ldz(g=!SZo4{s2i@p`kIp>v#uEA)TDu%Q| zxDg*Bh@tZlQk6r5r@lr5FleYpZpx`EoT{D!zzY1bw?H^yEL~M#`&JTR<1#1v*+7@NyXXemTMHzMgbz~*ld!PI=+`0Ro8_3 zRt0Z&k9M;ywOH+b3yltATQj^}1Kam!uZ=>fwh>!p4_K#XH#OtzxEp!C67*_}uPm;) z#%7+PnYv;&OQ3anrl8dId_aRbrN4>)?WX(93C4D!S4#~GNUW|#Q*Ks|wX-#~TN-sK z!W>&M91kEIC95t-(^@5|32wJCF|%Lt2iJ|dD?}17@I|V&9pTODu?%0X;;SjVq`Eaw2M1hJa6wmtsahuOEM0|2cTYWP%a8)EPwI^&20FW9@@p*r zgOaz)1;KVNb=jFr;k_p0q!C8unyA!ESbAFOsPD0DsWF+JQW-scbhgk~=)+;YWJ&hc&s-g#DaMT)fJp6l#$;T^10*U{-Y zi7^!~;&o+)*m(*exx4UEo!Nww2!=d$YTa>(YpMH?C(MDm+-tf`dgiUICZpM=Pp_Bn z0?ioFX`9%*`@`2DorFYFZuGd6=`=dB#zezQJBcZD&&3z5$^=QVOtu877-y>aG)%=r zb=#|h786!aQo_3rj3f|*pE7! zLIW4+s~BDsP%;eqLtdAKG1u^cK$ox>wc0)n;0D_*4MC|<7 zkrw`>dGvkq95|Dj`BovIM^bYknj=Y;VyGugJQC-eL$2x7nl0xIz|9%rsCO$IJp~FY zw` z=!8W1fY9nuk8T~xgZK3o9yYJ1%_sdtc`Am%zN4fv`7O_KdpZrLcnMPnZkmjJq`2ud z0#jkmn#2zEQbj8OW>#M@ZQNY5Q2GihPSdKg^WCUKw@Vt;=cp<~{LPn}Mm9n)}7h0ZI`^rP@rS@C&2%L8Y+A3Vh zn6pF6THi9RPMf6b9pa{}3)$kd3ml@_r?6c-UF-&5alG-kkTES^)1uSSa>dnAToVr+ zJ6>UHXIj~DjrEm%db5ozZedFS*BR%GZrRX9oj5Hn2XUE7Z6XWJ0V_*^?WIEe{hLu+>5nbT?lqY6sldXz?FHw3!W8)aLcKwr^rV!QibqFD=NzcxMwkHHWP*FCC$w~| z^?D9z7S(IKD3WOW>$7GGRIk79J|p4a*y4K@FcOB`U6wC*mVBEiku*VG>$O9jap5F0v zfv!^0=h{A%E9778K`{xLCoZ$9NC&TFg_gCxWm=tbQq9Alu9lO&fHT8r7M5>L==ax?H<#w(Ls*iy*GZ5LUEuANZ> zXMMF}Xt>_O%rLbsc+2BP(6mOOUUOC6D{T4TsqdT*SGH8k`H9|0 z4vgL>WU@vNiIUzVi+ZVpDRIVIiJ2HJXT7Inxtw}4PjAt&Yo2yYkSZXuFHZmMx(b>SiEp4`&6K6!*u4B4vrVog5aE)Gw4d2bvC8!!}pLdjNd z+iS4N69bd#Ybv2?mKMSDko5< zFDCMPp%2U_HkjI8Gr*1-&n8Xk!J9^6?*IS(r~ZV)y3_QX8BLGs0e70#o3{~yN^q6k z>D3KW_o?D*xvkf-`q-5w=_^#6S?eYtG*P<`R5}W^mlkU1UTgex$(LstOcSN)9az0y z&jU`y!!Iyads3qZ=ns|cN=fVaeC>fZlS92LWG6gdw!U^-g^kg&LCf6B4ZE~WiF&>p z+?I9uTAbOk+AVW*nd-9*zTzNl6qz=1b>Z4pOh!uzweRa(cQ9PprY{|n zwJn@IT{P#rWpK;p7H;}!pWbkK`pZ1_=JIa&h!YT>^cn>(G-cXD!=N-XuVtA<#-W<2 z!FVpIwx?(`ky}g2>YLnpxo3mrH0a`8_O>b%$fQm5{&-nl%UZ7_+g2s)89%mV%c!=+ z(WkUNF!`f3+2|X0e0Gb^+geQRGuap2HUXsv{%K@hkqq*TIg?{jJ@fV?Q$324m?TpQ zwakSK)^@cI#Um_CJ+(D~ueV!$e~Q=hwXF3@v26uiZ&K|+OE#ybRk2xD!shA3R~-4w zmJZt;Z_+yr{#cf_m(5Sa;Ab};EiVqwkI&YtbNv~P?-%c+<@pak{q4c?KR;i+TCG=4 zSEsAbpD&LtPZmHrSsYz-Ua`O8Pf~sEOYT0Gh3QJ*RcZcK=fxp@l1f6FgW=)g?3?BB z(c3lhLk9!;MBC@qxP#%zdU0}m_~7*DWZ8`e!;AIu-GAUGI1YxN>O&H<3XQjmbNt-K zpN0p+SHr{M(eU-~Wccgw-SBkye)!w)e0VYZIQ%>;hQ-Btd47EH+wf|5b-pQvyncZ=cmupIDPPbb5h;mz^y%i(BvJG}kp+1usma6J6i@Y`@Q zyc6ZvfD-RW4k6}vmz0E-?<__Z5PBl(=G@&yIg+I&$sL2v)8NlryHJc zYLxSvs>`!E%H>&ufTmYX`Q7pB?U%Kz0Dka{NXJJ<`i)QX>-pmK@!{e`%5x5>3^$G`2kc-1v#h$?oN{iamt>TZ;Yn z!!r^3A0hB3XKxoG?Ts+67b3$)3w%F1UoK=rh#-p?FE1{J)w|`<0z6wEFHT+`zj-ss z^V!>Di1G1}Jhc(y#ifWc=v|8Jei;5WXKDEumOl9I#Se^~LzF1Nx@OC^ZQHhO+qP}n zwr$%!Ri|v*w)*tocHhApykSfuGgoA+{r&sf{U+DMa&uZLol^KQdL}otz^0|JFk!L|4DR?;Xb86(>b;1eHU?_*$?CNo72QoqS={v zDj1)ws9zbreT?ji&pf05x3y2&|L*0zyk(IG;*bR~F}+n{== z&bOU-%}2y54{rVZ9$xi-q{NP}Sr|DD$K*@NA85S}veMRyi3f^tq4vdO9Yk&Tq-y!R zzRtQ&`xE~Cg11kVs4BmyY(2Oiq3J}1X{*4RU`QD(pO0J5>3T8yr|f)f&c*BdKpX%1 z7UIUbfvTWrZPIPSCItxxF99qN5aKQn^1=YaFbcyE z5>k@pE=UARNU<+aN`$9SkV*{`yv2L~PI*BhFz%%3K=hi(d_ z>@$Gm6Ywh_-APExg8+a)Jpu;`fm~gIDTFW|#is>qLmb=yh7Obbrhg}LgMLq9k(7y)43)Ug%$37%C0LS1E;Qgt1 z-gkqrPv8a#P~s3CM92WA8bCIKb_6fD0XwU<1ZvSGY~Y{G^2hN2^p6E2n4o`i@9Hn| zCk-0%0}mcB=!m9R+n(0RGb1uLIy#fldIZzy*00-a7?+2m(O3$x8s}|FwN<7a^d4 zXb25L1i+5qMG?D`cT2%CdKDM5-NC&8W&kb`S|i}5-^-bgLC%#i2uAjhO(%r z%%I{wdEdUt_!CGZz;Gdb_ltefFZ^SF_LF-23wZZiC%(S8 z{@$K?ng0IU5TsKuEC1V6e0LEkV5korX9RH7uNzC~e^v$B80yva)vgK)DC8)BXLkGb zicp9*rGOt%3x@*e_6D5F?=`Fs;v6Uo?<+ds{K(3l>Xd z^f%G}5g+J2FaQ$mbkCrYVZ5v1Wg5#9%)aj$G86u4bBw<8{R?fIn)rMSJH!tcC2dP51M+L24;`!;VUoesVu3NjhW>mgc z{-q*-K|Ty{Tz<6iKt@IFj&{wxY^z33f!I1EhcK2a#~n&)<=o3@_EXZlb@079oSTwy z1p?{y4ao@P2)K)+12Xgob(CJ{h&t3f-QxYDT1q#{$Jv(|mgwbdZ;4DOT_5?uc7@KD zHGI1c2pn@y&FU- zR6lig-BpGD6#c}woWYl=lfcdMH%ycIHXO|+>SNKeo@|ymDhg`xm5}I?N=M&eWLYC; z1{>C=5FMF3(u`E*+>6Ps{;|P2pX?c&YvpWQVHR64{X~ThmB#6VC>Am_BA{DxAYXYN zS$GW|D$MzI#bfJQSM5R3cR%4hOF9yFPU8xDlv&VQ8UHpWNqwROz>{-;uE}PnkX)}v zVKn(TqTnevmOshZ-^Tsh$+X8ZKUldS^{ZrrC&n)fru?0d`!n!V>KU9jOgE9)OSp-K z`N63+JG;HqR3Bi4RX$AD*E{iKOPh#3M$zo4!oq9N-=_7p^Df#0RMuK31HFUfP8s9xIN?}NlpWuD~a=i?Zq$)wM#5jvjeuFdUJV0`W~sA)euKe&X8T>Vdpd&O(R=d8E;*(^zUxceYe z0PW<^5p?H1;qbghvHH2uaAJK*a~Gwr7r#)Zn1Pto!%HTCVpTtC+ui4|s-g9_!wVcW`WQ#+sB z=yi?3`zbi)*4Y-WeDf^!ofEJn_q01wz5|Q~zYOsOkooOi(*0If@KVQOcC*i+wH0rb zOuMQ2slg~54zd|K{*W-4y*37T)TwqP=QV^$(6omR$F4kO+wFGTU%cym)VKNn+l< zdY*{1Vs8mM?-~ELPnhx-m@dySn!~n7xq1m$z_33;G83Y@%KRaFY%;+*c8w zCDbWUy%^AtxuUk_*lj#HR;=0J)jS+l0Lx!lylR$;6~ECO0~HEf+i#f6!Qc2G5rpc}-!ZMsdiC>_7H@?oZ5 z`>zsiZq7m^np1At*_UzHM95ffM9DL|mqJPxx2M=Lsg#jF^!=cItJ690oW@eh-M-%` zV54Zh(MvjW%D`2>oJnyvK~)%<((f}^MfL2P@}bfFK=qi}iDNSRwsYD-)^UuMh|$C} z&}_z3<_032n#lF!W8yxev^j&b5eGMIJDqp&208wVdAB7YA+Q;iGLJ`Z=-A)sd|=0; zltXl%F-~q-j_Td7i7v4E!EU$A%v%@oyZBJg1^agkNz+%WIgzvtVzId-{UDZ=HiOx@ zXkHtR#t)##@?0d32RrNN8=QShpVyUj3nEWf6Sg@=&#GxW+1b}7K+)oC#k6uD-hWLp z8b|nVELAOLt2RFYR9lNThgH=OKYzdLHq$o(qjx!D&IrvE(ss|%@x%ob?oh)%OKP92 zWMDkFe`;UjR8Qgw0#2gFUq9J#1xBGZvYb(ABGQJZXJV{#lImBdeZG6tR8P>!osGTV zKocst?O;d2^g;WPI{D#<=1%1<%xc;>nrJ=tYdAGz1@W(nt7vE?a!vXaWreeiV;^@i zW|Lgm>2QQIIVYbp>#-|clVLm5fjlJLz#Vk8$;P0TiPvHEic_rWl^?=b+z&WUAM8nD z457k**3WqDM-#%_%Bm7h*{Q6YWG)+UMYX0tC<{^OxRG6WAp(@ zy;azL-?i)e(9oXhja%|Q`!dI*9I|4)_;g{^73OdY%F@X!ZWqfi6GdHVDwoONZ=>4m zO$&T*i6i_Rhss$;MmzRp&hH4xQSugjw_fLHgfe78s$TEjv%hg>l^gs9A5lG%WW`CA zi4dNZe?syu&rNPy_eyczuUD!xd2~zmEW6e^GMMhJbZ>Kq^IPh|wIts;%fmVu4Fb>W zi-vb|;aTktx8eL~depHwj4L!tojnI%l|GEj`SpfEY~QiSt;Cki*p7~^hZLwFY$2&H zO>?znP^=RO?S_xV`lcVq32~ZUa}?E;B^n+JR@?Jorx&YPP^Ch5M$9Lf=jY}%V{zPr ztJeBp$0}k=j8MS+2oMlb@(E7+Ql^SNPF4v1N#+I5x@PQOJ-*LiHOhM58pD6*kFI2- z{UWu;X=q`+Hay3>|DF_onUCbbjBv z9>piDL`L0}Xo9U4guH@Ic^wqhhGpJv#}z5RN6!pOg|Tu}Np58HS>hpwbo}mN*$rKZ zLTE8IgU)SSQja0S3&w$kP=qci(?MSxt#;b_rVKvVnxLq!mXOt?-n@_L9(z>{YL}iG zSxo27KR-XVexqBZwfO0Krw;S17OBWh^~D)Uia20V+h}jh?-`P8PWPaO9Xnpr)wol! z9K%UhRF!v|)rqRXhMTTBFd|OKPnjkT+36D3MP)>T?&53^C*e!%BTk|m$;w=_zbfLp zrA>Ud$!?@)o;$7mlOm?1!MAuVuj4Yq^5caVu)|(C9TSJWaVc(CR5fY7-aLE3u} z7jqrvhIW@_sI=Vu^H!-d7UiK{ZGGkx9hk$-niRA z4!qW^z7YG~tgW~}B}^m(H^k$Yn=@M;-11FDo{cT)1jd%jUAFCvu>}DJ<9(J-@`r+1 zse1Fi`|q`C{s}#NQwzS$+X+o7Yu{Kq_0Af{2^^F9aqS`Lr+wD((P)z>wQ!K~6k`2( z*c%_BErS}jFayi)v4HS*>ETxxIVtU1#*J}ivi`4gSU`L}BZGUlfrS^((KGWxOEN<2 z>CNh0{h|Oz11@z@q5{9+P)gno{{p>ubW@&Qbk(OOr9T-DcNA-<$xsobl{#$)9N=<> zuUl@y69ZINBYrP%b)rPy>Dx#{EbGDm?0Etxo%#Es6zRq=!Sdo!IGB@#o^u((`Cgcj z6B#xw2S=aVAE7B&8;*J`n`2@>A1JSwFFL6qNxU73F8c8%9m8X~RVTS0daUA!2R@fp z@R14RV`bT#mrXvzZkb(?{Ir1LL{I6Ewd6J>JC7i1ZeS0k)z?`_D4#Cv(xV=ai_@ir zTs2&jJeO(#!=&b5bj1uW*5ORuaGLK$Zq<&etKmlYc+m;)Hi8-3LKvy8`n~5`US{j z(&m94+C#y>U$SsFSAC86j#sAft9yDD2wgk!7MOD9V5VX`In$$9D= zcbe=P;x9v-mL{V2sEYjskmR6+KiSi`Nu51F{jFJ!@>(|lVcyRKTF6MML3+@kx@Kq_jyQl(n zvTgty(J_g!lEvQApY{1;NqqEj3H~dteFAQnSPnEF9u}!tO+0GypP*t1Jo{#a^{%O| zon#|de}YI9rpYjgPsu6m!nLHNfv+~1lcHKj&k1wGCt<^THe6P3SFtLW!8mp0A1tN3 zX%>$N^16!;F&90n6}Uk4xb`f;p}(&O9>LW0HW6k}k81`YV(PPH^vo7LaX}Ju_?4Km zZD{J5t(KC z+PW$LaB?>TnWvm0o|vI2AAz&l*`?!(zNV=hr5c4WwtKOfKZ#Gh<;|~VT@&&r0IPdD zpKll>OK6dEaXO}%+d*5x;o#KA^z|b+2c=Nppu2hx=PmZm7aaDT$KtN}sTdW(NZuMC z7^t@9x9NA(QTTZyPq2+PVd`uLuJY6=K${O8jV*X+*~bqnRT(zn_yl76Ah4jX;&7js zS`@H0UWZGDn7{rw=(B<;^WF9k_O`$ChKr1@zBP>9)Km~sGSCfkchwuNJr;`>FKE){ zOT~95u!VPlM7}x$eE4bLHJ90-LSxtoTOJZCHPehY9RLs6QlHr`;Rth#fCc83lV!7YgFc=lvS84(hB)9Fl}o{wba<89v!@d zRwL3kcL_8*SfzGs_)8r6XiKS`&S6B8YG!oQq8uxF;+H!V6)BEMH~ww0j_|k?Tfwfc zeA)LF3&XeHZ#O!`I+M@{wL6$wiCkB`Jr!v`9QUFZ0|k`F1gD95S&~n5)UFROdd||D zgi4KF5_jk(!DhQdlLdABz9Zkc*28LYtH&KlUv#B)>F5YletH&=;LETb0e(YECeA)z zYTAlA^K_4XoZdqFbYwL~DEsr>rmk|+%2#M!Q1Vj8;^z9}{^$d{Ru2Kr(UP+XW##`@ zs!+3$^h7W-r{5`=b&*6Rgm$O<6uwSPlg5~MrMX~zzhzu~@=xD20gMEy<);#ZTA#>< z`&SqD&*Fu{CLMe((zBG{-Jo!Iv_XG`3k<(N8GW;?eGx3_ z8k1T1`yREgpSxgbA3bsg55`TZD3T%luNE!yV%o*eT&=f2?=^I;gpSX@fn?!*BpqT4 zarQW?1wgN7KP?X>rpP{)fYTByyJ<~r0NK^yMk})7letDk1fG1>!s-3l-sH$Qq*c#_ zqz|z52U0qT1E(Fl*;eO1Q+D#NHV49WkL%KsJeMbel)kD9%9M%9MhIUkH1aAKHP63? z7j#qXPbCE&N&~eHb&1G&+m$@U<_+oG^W=aWR5h=0_mgLJ*FKhiE4BTXIaB4v0&g|2 za8+1W8uzC@PiU|l!o}H!4exQ%mpKz!pC5w-L>9@u)Bl!(6&tN_trTJ=U;RM|2A;eJ z#F2@D^IPc3Y|7{e<%~7r#dOo6c{Js-5)*}UBK?jyRjn@;(jgmrUq5ppr$%R`uuWVz z+bPfB+Y%=acSm_5)R4o@^szmCS3ynmThi%+M>h_3TNWi(+SZe`#KC>mHgN7)Lp0>@ zRQ(p*Iy$b&bA;92S*d{-=FP~dJ_zoe+dz0o`R+>3rQRma^h;#Ncd!adf)K{Sth)+v zpO=vj_WGNUZ!!>KT=K9UPtR_InMN!W^i~ z@abh9pl?dt1a}K>!g%~K_6)s_lz(Bo$Pf4Nm*|M@J3Ws~#*iq>*R4(CVEJ6@WXezH z@xQ^^FM!dA{OfyQ>!?MW9-#incTn=Y<2dgHux%afSFLi&xX5%veUf@^Mw(<0`E+tK z$5o53dKDi<#N|-q__%us@zD!kR#F~xEI#_@go^%s9m6H2b#p(&-tW&ZjepI-F^fr( zWy2NW>-za@40g-f$FpT^Kwr7;c(Sm+RGn*i5^^VR6)o*o*B_NRyW>i%q3*Axr=|PM zw|{~m;DK{yifQ2D4%I4<^dYtyyt&L0XxEj5dZ^ROPZlntq@~qc-qaPBO93@8AnLLi zJ(l6ghOBt>Nj#-A8t~A^ z@a{)aoLT2abiv|L5^G=9IYE|La^vKbLvY7oSSzTCP}`~!QfZ~6QmwgCGONcLh? z_0Kcir(x0)P=Z%{bJv*hj-l&pUfDHEA2VJ-AOw(#kyvFwvEGnO z_J=WXN(DI8XVAXXqaiMH{umFX+P|t+mYE;vT^kr>22RtlLAdW=qi@Pvb=Pq2#bx*A zmPv~`Yj^~1&Ir%oJtG^KXP-*EQDGmz1rNQA5RyX6Xw#*nSgOF+Tqd}Dn!NqZe)qzy=)|7FUpJfLBd;BKM-d*28-CeHC~N0_yxvSeE3n?mEM`*; zxeQ`VSwyz4w)v!xt2q|S!9>t^@694JFdqv9NvJZOfF7(OGbt`wQF0fW5~HnPTZlO1 z#SGEyzgRp5?N|QaL%_d{-Ur$;Rb-1E=@~hs^1NPk#CcMs6PPg*g8*S1;@NlT`y(Y^ ztOno41}S|bvtJFFsA^l5p!dGh-3sW)H8#z6pB|@5n!~{O_BBkMF}LxWc}IRc_3k=O zzSLJ`s&#hcO)0UnEYl0E^U9O6OD-K<*2;)lknyLSkDL`S>9=2Uzg?li8ybHgv0(+M z5*67(Ix*?|Tu?%u+P z^C0s<{rZ{T)b`pKZ#TvFamt&3PuU0i#@s7Iu}ZbPph_aA!@e~@|6Y*7JKG(Sm4(}a zWvL6ANa1$o3X9ic0}tu-Rqf97%9!o63{8#(8<_IA>e_gMMzA+GyH>z8M~ zvRT8;Bx!6*ICi-3q?OM!;Oz`0ycDd2DhVu^`B#%#J3{|FWTkw9)h9C=v3Iu1UGLq8 zh#8jU`Ws&kH)FzE`e{G#^2X79eAe?DDe5kBx;TPL7&2F@?uY2~{L!eh*}@%{9QKau z(Jst-&{Ho$PiSbcWjPKrKxq3${B5%5K|pdsJ->57vB-Q?6;4_xMQsj~1e4X`UD@B< zY;g(Z2u|4pa|DI}&%~y1OC)|`jzJ}Ot-K0`;sC#w?T?BKG}!x0?T%2s*&B8O@o3L* zY5Ya%_{w0Q#Z(p~?+9Sl$xPPX-Le1h`f&b&pcH;Busz%$qG_i9>MeTT8Z3xB9tdz*ZyX5={AjUM?0=nZms*V zjxEL{*i$naRknU-sL!@nc{?;e-46qkbe{Mu3+pNV9%oRkU@6Ymq@msFx{Yaay2WHt zcBl{G-L`IwC*Pbgbd=5lpi{luebHtO{pc-9&v7QGtQ?saqVIO95syqcSAFmh0fzuN zGUloVaq{eNE2;3C5X-qHLl*3TKL9J14e0;G)!F_>uFlHz|G7Fl6EnwuR{zb_IhdH( z{?}aH{U2AiTBX|#a-m>w8U7pVB9)%PF#tf!06Yx9>=Nz*y@0epNlGduoi0&L0JK0s zaR1+2-Dv0g%EO)KQx_OKINgBm^{&2>?}>7!m-1f(im83c=AKLja*} zkDmfD924UPGEmU;CtVK|4vg?`1{Fjg+hV97;D%lufT3bQLr6?R2@C=R0w7T7uQ5dM zG=PfW4gz>U9f1Hg7^pXrW5vLhqC-S7Gm!89tZiyz_8+3g8Sl0e#p8HLmyt zw{T%UjD5f30r+?7)&N66L;m5P)1T@D5KryQ6FYb(CvZVU1PbH<#3QIMf7ScxBLs-v z00}CF>4g)j$ziVHAHW241l=rxcO!!VE~LT$5W4#Ms~%gzg|`s%g7Oen^--a|p@Qw2 zu$BZtPfozXh3gK%}H3fd}va7#4Ok&(s}|Q@G#SmEO=}X88x^a8KYIMA8A^0yu)^ z^+o*h2<#aE0AGQ?-@fff`$>rm4FiBj@BkZtxdjVEenrHG4aWJIydE4NkPJXYkmWG| zzMkJdO@dF-fMG78KE}UZ9YXQWEUm4r?!M{X>{FYVU?A_1|2<3)k&!?ELqki&0Fn=c zyIYEXDFl5@fZzM&Ag&>R1^*;b_j7(o?_XBmn|`!l@Vi?LF9sIlL4X^72s>C5@QvbM zh+qH3ulDKR*zNwJFMf!h-S-Ob&aR&or(dOCe60lR5!B~&yg%ynAX%SX3{PMJzne=q zU*@{5gK!A+?Y}ElgaT){3PD_*KieV^jjCT#8&VQ2~UYx+;0?L!jg z?C<@C0?bFiU*!W>Bta7XLK1)?Jb(`Wp8ck__oCy(?QHD!2%$gYzrP{_f)NkCh(J3t zc(31$!|vmnyGptHfUu1nm2e66Hq9T6Q5U@;vwR zJk7hIq&I4bE6Jw$@n}^(aO+u+8w*o>NtXoG5EZ+Vv?})G{tKHg zIw0((>1F&Faf&NNY|i4RC%L-idZMHkj1s!soSP`jJ&;LXA7x+fxql&%i?Tw(?o-_U z>5M`X^`yzgrblY9X`(pqAmnSWh(w{1rlxSvGIWt?2S(zPp=2G`U z=fxTl#a9g$N1Qg3T`tawyoWih4I5O?aF=g|V*F#)_$w*Lg*Bvy@sZ^9W+L3Tqs6rf+=Ml2=2bbh zUZJQ>fo-5^|2(PX3}R4Hf828rQ%0D^nTRZpEOqrlcuz#!ci-j@(IhheBlLu$;>DOZ zS#9Y~VX5*h;pv;|+QvZ!){p5?Evf`augh4?3nLV=!f9W|?==@9oYpPV;v*Q($} zISE5MXQ(bE*(TRJrvc|o68%sWz;Y3!A6*+>y6$t@f7B`3T$-{Wgu_Y zKu+&IT~}9zp&U|Fxdw6W$_1N4xhvjV5KZzdK~^$-ktbX8HicbYw@KNgd>{|A)swTX zyD}9|i|ETicu>Crk?FMV&YbOqIFT2;llJ}yDNva8$o1!q;R)6a(eWIBudXsW^^M!~ zr>N*CHfQ!Y{B#DH^1hJzDJ(Ke44abL)j?>xa5$6c?StP|nNJjPJr?w=N$$wh3P~Q( ztnoDg1oy%@-zT5=d@%znLN!g~>ScatoeG17+9hi3f`YD@qLqcP!^P>7#~j~XUNp!a zl`Q4;tN3p|shn#zEJ?8|6G6;B8(d$j>q@&4RJm>Y)BxOxyH5R*}`1-s^bnov=!VkYp`M|_DjM41cc1<5gJ z=T0-VYLnjZfL03Kklx$Su*JEk(fczE>+cWymbJrYv0isc-J5Kre$|wsw3*J5Y5SvZn&PTnKTbnI% z?`zS}U$oImH(T zS9JB-XP-tU#3^cNC-7_d**l`N19dZu!7Z|Vm0?=+pz`Gq?);Dlsl55NZz4D;69${S zSSI`3fO5L(!Y=}&Rm&Gb-$FYe;pM`69&+HlgV$i|k|0JkjxFqgJ|rA^D*8W?X{s8t z*Unrt#ray&bt9*bGn4|?kR^JI@~JDeVP>n??m5-lqdY^oBGaPH%Tb1Z`g$H4kc1fi zN(%l<>doEItfaebwy=p+)YQ<(y%kX1wLN~@9BSrWQ|Uu)%f82FrmWWJ*nWIyW3GOm z{UCot-jz1*=J7#RBxmb;r#i9N{G53Yzt3O~5$=O|)&-Z~{B)Y0KCp`$(tSrlb5q4x zaS|S&U$DuO87X2anE!U);y_s3-;;%ObOUqN_dj7QrBX=ut8@y2jY}fu*4rSHIsL8u z-AkVxA1A?ICI7pg`{R~>bB+}Es!JvN!$B2V84{7)(fx%aYpGG=Q}tat?r{3)#P%lT zgOt6|yFp*tZ$sloeys=|`Q{c>`Y8%FV@?m>g3kd9XVhHT?g*pUOe&2lo<=U*XGY#y zdXu-)g-WxhG%+oWCXvk3SDQn5Ho}!IZyHgHmLSO(=)@9p&8N;}=N6xc>UB7XpT759 zXZdEToQ%wZM}q{&!?>-TWlqsvGg+2qZbHGemKB3h;moPf>D^YpicUpwkRf1_PN?@U zlzH|;$JT4cbZ@|72XgRgOJr{4y)W3Jb4-ZBU*cj(+V^#yaQhw0wwc@7Ote4F)BY6o zt@$gPN^Vi?>Cb|-O>pX*@iOi8S@;4~@O)&Ky;ZHRSeIe=9?BM`8yS|p8nuxvY&)CA z%Tg7Cn@?o0Q?Dc=()SGxbA#CI4V{Ru{ev&zm;MohFk>z4zf~|Ld~vIckNNa#ktm;y zKq`Q|Zoe8`hnys(Oa0GO?2pjr<@49+?4f8HMnK(*+^^YeeF-daD}B5JK?t|>x0Tt; zpN>z&jkgv18%ZjscYIkL^}k~-uNf8uvgO@c#vT##i~JQsvY(#x;T+#iwo`9@mBC5u zr8wV*tyy({!E^wfb9(HVs!hHgmZs1)RafmoQLW|O2WuQ0K`dD{WfkxKwnz6hf#q~} zLw#BEK}*nT4^Vc@Gna+>d~X9W4MPMfE~ywn#GMbgru70sXfPgGa4?n0h8jEZQyua0 zqkJqCF7KT>6M%VE!5e*hKC=1kW-Mi-&ufdL$8#3|h>B3xk($$~JJo-g1I;JhVRGZtGL?6ic^-fm?(JO+{!}}a&T7rf zfdZm@rJIWL=v$Z&Jb`?_fX}8e)otrrmkUWAEj5#xJA}NI#94ROLc8l_L$aNa4Ato3 zp&SM?azpJb?cFKz+~S)i+)*mpg;n&pt@?}U$P1S7eQ_znS6VS=(T|I4PK#R@Li)tx zOdH{BjYT#!!Q?>8Qh(yiryl#=Uai%M3UN`2yoY~4`*FpBNdCpUwr1vu?kr8o=Z@%Z z_i8NezXbE#q>lKT(cFtr3*@&Rc-f>L3}J6)Ip5*QpUu(rclXA~cTipglWbUbps;DN zuSvfS+EQ@~zm@M#Yt=Y6p%zzcykLoZt>U!(X!Zr3P;H8BCAcV)P2-^?YN4L|?RNe_ zxf&~bsh2x0d$7%DLHa9{gt@b|T)IJy7}d6s2;T^6%h%+%plC)1^6;fN_m4>%C>+BA zNy|a2fN2{rI|1;rKmzW~GA{7s1L2a6dQ3NavUKKTxceiSm=BI`5{7p=$PBDB4dXNZ z8O~hR(jQ#fr)>ual{eUvf{ zvJ!RQ4uv&Tg8@hpSJ~ON`7Uo})zFs;0$wkh`sLj`%F?yeQD9bsmCT+g+LSkHI&UF0l=fI%hKfx6E0# zm&b@fA4@E1+WvAdbSOg@!-Enry_cLVZ&a4u@<3_Soz5fJaM1TAa6f&~`UDQZdK*rV zZilOf!4pvLBUDi$1LLDQDzgfd#m6b`hV7bK*Zxs;UVZI0Cm@KsF5iN$6EJX471qAF z2Brfilhe1DB1A;9Vt6?I$#^7fdC$}~>hi-^Uv9Qzwfmd#m(Frd`8Nzlwb?Svjij72$JE79JDBV2znGt$m7b)BW-J|Nup`;T%0<5A+a80X{&cdxh#^x z(sLDXKDld53hRvSGm5tVq?Qaxw@0wg`Zo&nqT!3`j=BrCoRiSzyB}lfu(R!B! z2A{2}-W^2=Vp!02WT~xXju(ci=W_j30KP`kQ;cTAnc5CCgB$5^+%wEJLfcAf}FoYG%zFZn9~$+sfIz7d9Yb- zYKxVsdDZPj22BI-tAjD%uWD%JXxlJkC>R>zYhVkX^cQxDq!|&`&38;WptgVWuotP) z&h4a{Bpqk3p;FRv(F=qM(Ctg}YjbkSJc4eXRo4qA?F>t%F)yCl5^=a0pU+N@4TfA3u@L}I zmtNJfz7k#^NU4}XcV918p2zGY7bV!5RatysaBfy(d3pb6GF9Gou9jR$`?A=xaBiq6 zm`E(|X>0P?3k#lOY8y5=J22T6SRmwyN?{ZdGBf}&B&}S-?cCMwaKP>CA(Bc@?JQou z#pxGm=7z`jEP5M{h2|>}P4Hf8x)XM@9rRm8m)j@tXNUh9m`i;iCWF~=OpRFpG4Gq;q*WQ00e_6R=+nz20&R$^N2`qX)UC5s(c0@RJIk0hP zhsW<+IvFfscxk*UYI-`Rnkq8Mi5IA_57Cis9ZuBDnJs~Dh94hx5~ehIk(!qkYrjl?f*MZ^OSS8?e>ifXigSRK?}K%| zj*YgCXGy&om#?}k^gMEyvf1`k#j1Xy9ljR?Gp9!Zo4iayILwRFZ7q^tup>yB=RX%S z7^LklU>~p0qJE8)U4m3?d5oQR;(&bVDSZ@oLd!t|K-Tm|;Qw$9A#t|#*}yEKx-`o2 zJyKBRd#u@W`dK{FGS>?G-|QRSCv0+1aZc*ji4%AZHt4oR;;JqDGamSLVqe=rwyF&MUaQZ0uA_3= zpLI`3@dxPa2%Fj|Cuha8Ij=-*J}Q*RR>;Yk#hGt4S&C(;YeBy@#qNR~jOjZIW<5!O zB%kNXS@ZRtwU9`br3iP2u9gtAcr}dQ%_B8P+b5-}%jbc@e1~nO#@D7P0zPSf9`gK(v{-kJ zY--|hBBUhWT%Cj`!gn5l<5&K|D8V20oR@vy?a%um z{#LNYn+xz3#E?uZBm{Q{8QXu0Xc0lgSImUWF zNbZalgQ;k{608x+yl|d=L#4;i%xy)*Vmt!=#(~tLFZ@h@gO++JtPKgLJtTjYV8Aj` z)`042UHu{8OuJ-%$Z+95mUodm&p+(dxH)u+BP> zHLLN~azTf0xDI;~#rzBo)i}e;lyx=(KF+9G%waKsC zP0b!sf`gMP( zWcR^k&qr=OsdoK0POl72=7SH%On$C_%fRz3k)r2%49Uscru%2y$|~RaWe$j%lPuZK zA()@wOSlJe4#OIvZo%#J)78*fFP7CGq;Z|ynk@V4^t>}em-JQd_+m#FSag?f)R^mW z!=wBbRgoB1__uKHakV}@ReV)jDS7i1xbpy?-cO%qqx)vuvrOU2&Ii?HGlurLH9bbC z{IJ+jNlp7$2ZJScRJ@)#*#n2Pgy_cy$Iks0Sgh!1j&bz#^^Is|#MnG@%>AaPzLJha zDK0v}0{`a+*6jY|FY(#+u+=?_w7URwj=v;l=*zx1rcE!OElyQlSve{DfU#uhJ$be1 z-O+jz0Bd6qp?>K_EvnL^$XoXuPW%ZA`WEe`f5$ZZ`|)tl4#dlwui;@8hwak>>8=z* zr9QV;@=mxh>V-?q@$1-rUy`1bKZ0QZ?u+i7T}i@6ueMYzo10#z^Nw)6FjX{fnsRcD z$nn^yOR8*C<+dQLwL`C5wUxohMX4V{X~we=V07)mRP)LDSVI?)JllF4JYVpxcrqiSs1%u6LU7BkqT+WkpNNz$y;i2>A0fM zSsBfCt9913#u9jN;64ZC+#X^h^2%t@eDDEPM11Z#Uef#9;gyK?zZ#yzDylXgIdh`y zPW3F)vctL=&wy<~-g!hk&q+AMIJ8JHd&)kz@~=6S@uTz3tPqz+rgn|RGOmP*;Tz@1;UUAAO5qnMCVDW*Sfa6@(QVOrd$3T9Jc4L!?NmlSh6^y*eCdpIv zHg+acN%&WzO}m?&U7O#EoPL?zskds;DX}+m@JwvXA4fCaLzhnD`I}|Y^n@f#o@gys zPwGUk2=*qN1|?hFqp%0&fO-@|={S8#pD`0c*+)hrQQ%90Y>qN*HD&uwcdpw}b~&5d zi3%v~oFAoZ65v)OU)Cr0*j4P2viVI)VOPBj>yXPZ!d#pBG&@l8$hUPf2hY}?3pBpQ z(c2rU62m8RNGY5(xmBFUeFC%?IHs3xjmZFsSGC4PL%*)p1bl^RH4KF%9LMjgf{T}tiLj##XrPknNky1MoweD*bzL)SSF^#ge7e^*?IdpW$=SBO9 zGUX=HJkix!d(T2rJ4Cs-YkG3~Edc5(|5rDtw|*)gD8*7rLX&gOP2`+W!*+qhr%mST zK@gi{QWi{XPK#cC$KxI>S1@+WOpSUj4|;PQ>xnLVb97aMHn-}OxGdOu%OV+KHe9EW zrmWuae)`M_o|o@R#qcEG{e{8JctL{llE{S3h|G*ZiMJ8|ys?_`inogiHub0@xw%!D z#bs0HixQ)7Sp>BPQx89N$%$4lcEHzZ+;$>J-LYfv1LInmi=kyXj{+r~hTY-Z)TD0} z@WYx>7f$7;W*DL5Fd^ANw#ck+?-i-#&(oxi|Lj??w)1FUwm4NN{_$DyD&I$fG!&B~ z+e5oWf<5iLSO|W_$QEveXQd#H$mkJnem*YW#+4)9Wi7OUYwlb0^pZz3uIz9~R^Iu< zv2YmfT>PakgKxrnxm3dwp9{lZ38m3Wa@XKa-Vw>Hfpt@@omtL+IAHUa5_HwKkhz4c zxP{Wi60;N$?JbXIb(KTL2)kC`ev0J!Na=1GtMwfK@uvp;<)Mb}C)fRYYo*Q(Ue$0#S@JJF0QG;zPJXYXbMRPmPPJx1w#u4QbA&X;^b{b@oA)}3i^ ziYqP)-5yz#hxPrE5%~2Oeh1}c^5%YjD!&$<3kFV+il{rQY)7!SKHpLsd+s~?+o*Z z9kN2Q>7ra#&$|I4GpXzAz2V5gW07NWA$qREsf=y_%_mXtE{xgGNrfh7C}4QG?)MdR zzT8aum^Aj(+bIVXzi_!TEV;2jLg(Y8S&*j{21(W+Q&+^dg{iLdNLL{@hJt<%LfELx zV{i}#DLnJE-fcK)xQW(l2qoR~2bk=R?kTSvs$b#tBMsD1y`z{n0~(}k?8Dt2pcAk& zJwDtflDkf|VHs?&H$T4H%Sg2MM139foTml;1Hk`b?4Ftg3D`6Zmu=g&ZQJUyZQIpl z+qSxF+qP}nTQeK8u@U_ML7Kgf*hy&r+sX`OOP;^2~lrKn`WdG-N%l2x^p_#fbg zw7sqWBojFPlT2V^VEG@5z(l~o#QGmE{yCYMSpUCeLi2yi1hy#u!C69FVW}Ay*m=q= z0tr}#UIgafSNRYSPzgW@^Mr(3qy)5tgtVlTK=a;(Cp;&eGao-zK06srrq@1Kt!~;M zx&wU$R=xCgp^X9M1c(&AVE`IPWo5gHZ}&EXIBGHh8!RCacK77~KHkyKv)ilyq5vJ?7~EE1*BH_c02|kA zk1>AJBV;ehS)gE>KoA!j8w;?w^gBzpM=&r2{uoe*6Y$O;pBBnK3HREeoyWF_`drEY z2mn4ii+KHpZtdSF{wBbyFTk=3)Dl9-^Q#jexYNK9eAs6NWRc4bz=S_ds@_ro@o#UI z0SJNO{QAGHzPJ%^?nW>ztsm{DoP`40IL*@p@qmFTT3=L)SMw0c#xW-{IA_nDpH&AnfnQ2#x_o0R$E_ zMA9pe#*J`n^-CpjH)m&HpT!~HU<)7Mq3ng*0k7srdjof$86M#YdXdjzfeUy#^#uWc zl<#(s0Du73`)m{iu+xBX0AJa;u>P+7kQS2P0KS0offa^>0|xr}eRKZFYcmNIX?^{& z`ZjAN05!2u_3_!}2mL8jQV@6t0)7P@^8XAZ(kB2RqJTgV#t;1Fj-W&Nc0=3s^I1Z8 zmdG2_8!PB|%nk1P$>CesiR^>@ywO;fU@2{7<7j^YukQ2i2L1?39V=|w2^oIflK0l^qv zocV_z0EAa3?t=e!NmZ!F_h($6U=;T2m_S?q`DYmQ?`QP4Xu6-ivpqXH{fwU-GFTwb zT*qB()UzLJv*?;C`BI}pB7@hg^944UOS2wME5H^ zbDZ<2CHGRDY~qbA=`0QQ)cwg)@uilM7GqxuLr)#MgRU~LKGiX^Z;e8XhuMC~7@W)& zWtJ)q3STiEynA82gt5d0$or}}Ez>7=U4dEKDg|HvwffzO2g;uiCSk!DI89egW-PHP zj-yM5LAFAW-OIO-nP4M;37 zMDL1tRHv~UwwgDoT$NUX2y1KI0Qvg&+_-CS8RnF@71f4jy)I;Bo{!t(?Fw_1FfMH9 z?8e8jFOg&6SahfDNhYVsijl@KR2oDAB2!)>RKsfczOH8ibu!N!oLA@?88tL_2QxGQ zVv%5>zyH{8pmyj2?8IMAJ;@di@eJGOn57dA}8D?TQ+V?m$WsV`kJY z)L(RDK84NXx`Sb<*W%cP*@5GmWvU<{))oTeDHBIQt|x;J%h4xMaGi;xG(cbLrnknh z=OmZC>P(NG!ng7c)WHr#K*o3m>Lr^v!uk@U!-B^Mz;4p?KcUe%q|7zCZTPLO1pG6|Zo1d~xF(ByIgvxRU!r*jFF>&hAvuk~`Y(r#(Z`l)#cS*SJKJxx5Q)7{nK7)hJ>fQe!%eO?el2!2%Q zG#_*Clq^jw%#l!ZW$hr5X>n5`D#cJW^_lUOMq+j&r!GfN%weE^aUEWL?FIVduiLKe z1#?#1z{M@`_yW4r_yM>TN0001N-Wjq8_TnvkVNG?xgU`P4kC8YEpxL4HEGV+uEq#|qJCtCqdT?xplQo4<2A9;2oN_O#9q-oNxBWw;T3znKVG>$4Gj>UAn3|-%ZREgPY^--MgR2Y|xf1;4a6xkjQ_KJsRS|LpM2+iw<`2KF2hd z6-o|0AFH_`8}C!Dasc=H`FTy`g{?L2uRf5Qs1tlr+1Ubb%>R~wC5@UoK^$!Tr(6P- zIr|V4{MHg$TYKg2<9L*q*mmkPT=k8za*y#T*s2x`ue$M*vv9DgDfC~#Ti{^2qU74+ zEGk!q5P7nxMt(>jhV5K)kcZb`wz2I^&Jxd-rkRvaLT(_fg5c{u#$Dj%Pn~NP9ezWg zy3xGal%^WpN2(7@W}iUxInHpC@W$Qr?NR z)30UaorMyFvqh=SHKZ+@I7?hJTS+0eOxLkYyZAAPEL;TbTZB#-uT7r+FkFHdlJeh`>@B-&^H%GhMctpUn-t=%ez z>cmTga%j$cNM940;pzfBqnx0@A1CPp)NRZFD7oHb^dQ^xD9rpfv58Om>lsuid>1gSS%7Ei%)>KCb^z zYI8#ZvgF;7wrk4>4f#vj>o8b&OYfqWk(PXcs5=d-vPAn-d(fa-zYmR zjjf%8!H04dO(&#Z?KF>6^<4E9jp}3)WxkFD-+P6297@{BPHwy6HBiRe1M1yQdWaW3 z$+;^hum`(Hb>!34XCZC|xm!1%sT6SNv)GFDhbsDBocBCh$n$$P(#v0YOco3~JLzqNzjf>xKc!_?Zk)eAOL^Nu z?y`;c)8sIY^ho~oDP7v6mvtNL<`uICb~F zhVVQWI3hCgT7-sO3*4s$bK$LPMfR%uhuX*c)Q$`|2@E;ko%A}5WIcuz=^Q6@UgnG9 z-!9hy=I&1qObBm$_hM56Q}yO_z?y2&W}8*a%7zJ55WLRcXmwr;;qIHR$V$rfiIaqj ztdv+;yeQMSV-mM0OwH;#BG-W9%di}xLis;KZ<5@sLL}#Rp+S+p8KguOieV8`N1<6g z@-ZLaj(@k|UMB9kFuYb|w`jJmE>ZZ>^m@}}OLtTkHSslAl%54P?CErBxk^8js z?*L89smy6iD8jyO>;o0?MiKaEt^?^upvBxM(Pk~!63(YeL_ijTD2o~uzOh;hRV2=+sZMu#rNZdo21sA5RIZibg`VY;x z(tBg<_a^KEzD%WK401jhhp{RIi`ROAnb$J9N)6@pG6djml92BaC?(Ho1i`XtrVCRb zKbJNrrW5MX?YnzY3_bcf7L4bD8C zq1CaZfN?O#<-S`OnXLov=ppfWF#q^e!S`wAPWZ$d`@obXCkih|6HMb^!fAn!c}16^ ztGI*v1XG~ZD|I_u@DmvrSyOP#B#*v4qJQ5}L^NegnI;c{OEVKJpb-m^ige%r8x29; z_`Z*%S$Ca|xvLyd4zv+`p$~A1+;;csT5#N2d=U63>4&S*BM%WyDQOQxKTxQ*jaO3x z)R9dmdJO4D5IRp&?fN5j(i-P6C=WAbIkH@~QEUg!aMw2@2qtBL$lIiCsUcb6$P&h>I*NK0u}9w^E7{L*s1 z8HT>59E`530P~1BCP74A%C=>uum{ZtUfvG>T!H$`&%wLGBODX;!8^ygsZ|3lum9ys z#kQM|uDW(3#4mXuHLYOTc+KjekjymHK+4sz2!q8-1LPl{+=d^r32|2h54NA=l7caI zCL(ATptbvHM*GSVvy$`L!p=knB2yr$@p(8`*YFe3R;*z;X#~9h?;rN1LnGyH-veAe z4xcRoI&2{#cCsc}*9$5w@7XUFo2JS=&w?#y{*ZQI>+yMr&(53B6qk)l;_kHoWs(v< zK`g$#&Sp`Oxv3Rt97EBd0d4lov%c`>Z>@*#IsefIT`WPqQKcNh()RCw0(A0kRtd_K zYNTbh1OyIu%)uhxH)k)GK>W89vMoKc!9|8YbB4ftYvAp#uU|-If>wXDZnfdT*K0>X zW+21*JnYSmbQn3Wl`r~Fyx;*GS;-=xgL<+GL1gji63K?k>u-LC8M9=_icS~D&2RrA zG2T{8{-6--&c3#%dVLHT#{|3Edd&@9(itSA(gxvKTpuNUOOFS($_}{2O|HB3f~Et< zR^Ki>9(JAnW)F>IDi%Wcuk~6OrPN-~PS&6@wPy-o+NIi=j4r}X3Z^>@tsn^p2>`9?)3lOp{r@M*~B1C8IR;Pl)Q1z}c z$(Y#3xlU0c-*mwr(u_8;hY$8pfZ?U8LcAo1yuIlgJgV8O?S?EXH?iXkKpdkYc0?$4 z&us3c9I~D5P8w&*do`lW>{7p>{Fsc2@QaM993A>}E#tISJ7#ObIBn~?ltIZ&*-Tz; z@3svxpJ?>|n2&cH&>Rc83*6K3$}(JJ{*vz6KN~~Y1{tB#aMb1e$Hjs{)KVf6nE)`Z za`Mu6m8mCMta<1$5693d;FpUvS*9`T`K|EwR_CjHqm>abgAMo9Zd~|i&ezy{gq%PL&t;1m_#v#Ms;z)0n-pQ`hNS0;-m``qZ$kW= z=__N(CT&h~d{+L{WPtS(YX0_S1pAV`1cRwO zO2wAk4UCep)@Ndhb7I<&qp?N|o%f}C)rUF(R6E3%^8SxTw7GbrRO9&Ti`xS)KF;_+ z*ho54@ab{03!9T|7eGs@b!wg#h~d~t9(GHY>fWqwYt#-c{{ly5!hM>t|9Z4a{mI04 zO7T3Bg1go4dXjipKfxc4O_8ImFnLl?5pd^{nakzloy2fWbGK~JUln2}sliA@rM z^nHJu;nOQVA*~hr8EJdM_u=M*S3;Bv+gOi{vqhm5Rzpoz#Lw=@B&yMG%*APz3S-_n z1L617m?YF5t7QE_fIeyv2gZp!^JWYgL3O@Hx^FP60mZ6{bF(?K!ln;~*rkP%Xu|wT z2f3z`s`MA@R4VlMcB0Z?E+rxzakL&}$8zhR@pCu(SNf`2kYSp({itdkL;SLgd=(#G z;8D?p1d9n|Pg~Jz867sFS&AU9R1`uWWyQ{HLrcMTYrQ4Yu~v4oGJ%WMOiI)>Q%w?) zJeF>GQ1j35oo@MhnKV%GOnZf!6bW*!ng0sh8Yq`H zRfkAhz5+On>3nb0CD=%_DAzl03DtQL+=p=u^1%0Tz^j|L4~4-##D!&IBmNd3KfaD+ zE)R2E5>dHN_>bk*AU-r9%wBc>IggYie_px$IWBgFpZntihzZ_<)z86dyk)3oYZvs@ zwdY36XD0E7kr!rJSOO8HSDpDmMWnFyhKmwz>@6TgXed##L9~uldUxaQu**^mZVOp{ zST+t$5bQ>pD`JpPkFKf+lEd812ZU&bT@RyQQXARMPR_Ro$Cl^l{+F(!exTc+=4%!~ zQT7L_;PF!fO^2GIH6UX$nb{EREEAZqn9d-bPY_s2{<;pTiy-v{V`-;MBEf$C{Q+z zS9prf7F`F&N{m@Q^p-qXGiXUmXhNXm!FrbXwejkJUWl^%{;Csz=y1t4s}4T?@*ay4 zHo$U?}6k^Xqd9l~4`i!eg`nQ45PAMG5ZPPw7uDbeR zccCsJjdc*J88RVelHu!JQ((LXqV|75`A}ZS>Dak7R{6w zK^AmEVfm@tI-@u>s^;=loKNSu`Hlm8>fTM3)(odIwjaDgrOv+X%zsIKuEv;&Ms&=2 z`w&1z4HQ1m0Fox)e0hW{$~wV<~8JdLTc!)w6Os><)(>& zHA@HoLdjO44tYJ{p0Nck2H5`Ud)61EcEtLG9@psx{A2vuTn4z?Ub7DbOLz9u+>t+I z_PNkC3bIY%wQ;mt?ercl3SP06c)M=D2T5*CcH>=$fj4jFpH_nlPik11)^#PAM$~U? zHoB^YB`>9DRXn!tu6PlYf7ax}!Jzxn0rfE0nvTvXYzE}e?I*8S7=cQ#Wo_q4tfSjw1 z(gDfy-E5~abc`}7Mg+}tlrUJImZOPe(DRBl#MZh4X@bt%P<%G}a43(v({;2IM*kn! zB0OVN{$sM#s1ebW$9V{~F_jB<)r6O_V#$O}vKTBXr0#$veji)klC15u8Bi1VGwtdY zGI25a9ck%ek+k1qLO1gA)f)U6c0pmROqnNF^PaI5r&syJ;)#o{Qd%2lsHMELhN2AR zgDUeB@tb)6N^2=Co83E0*)+K*J9;x8OGwhIn{*1xO-6c~<2PmMv#d_!eD~#F%=84F zXfFNFJyRX@Byj@7vpOH%jaPe+*hDu2pHMhEb!d&8pJ$hjy_SdG=PRtnwYPK{&c)3o zq&nKG_V`_VG2q#guHm8RP_EUx-au5wEMb&EB3ud`I*Ma$cc|2CWRk4pm1@3++lM-{ za6eD0wa9#EO+B39(TD8TA7mM8rSQZTGhV;bVf~G<44Dp(6PYGqIbHFf#o*;w$ku2m zqo_FU5!1p@AGdi4^6uSe@*0vHJyYr9OFaOqQZ1^wVd-tk*9*iqfhBRGJX(#g4zrG^ z(`7bC4$mIQ#rk33EH>DXtDwT?W<)P_q4V9ClXcK%D1Kk}=gm>2^>|S|z>D&%{vo@l z*XpAl0LYJuyroJs;mJxw$pF7uexUSvOT-H10EF>Vwc4w@VB7W|RP}v_$&;3GKkpl} z-1Ip2>V_u_|GlBh3N*Qhjom=wiKj@0^u^_JB3ce!g8^o`-}&CdeOS^A(O7$PYZ;wh z|80yrVX|`7=BTp+!?@rh^~9PBq({u&mD_V6x+SP&mL4&o)&?RU#=O?cGu4dSVA=NQ z?GFQ?k^l!YSB=${9Xi`Taj&-}%v}8V7nNrG54B@q238kQD6t!|<~o}5z_u{?Dcqbp z-_ar&bKI7B=Q*aSQR>wBU2YfKJ!|F|sSgefl$^uWc3By9>y|7JgvVhax6P08+8lbu}{i1zgJ~IKZ`nT`N`0S>7nyp*=L*cTc2c z!KpO(`!_N)?bV=3kT=0vZ6cGjiGT11B|f5($zhe=!}IgLQ;O-y{r6YOXfXR+0?-97 zZ@60%g2NDm%@t^EbkTxfKC<3TFB+i6>K}L5t?T39x$xbK=k&HuFfbD^ygEKlMYYt_ zHqVY-pYy6&g3k?ae9AMx;MMZ;M!XH;A|)%Ln3ZXu7Jwh+y4I7NU*4#HymAlZgJ7fZ z4imuYOFK=y(~8@=>KRPVzUTI~YLDw*f*;OedTDV1kGFo;3WWG!baAF1VI^TUN1FaX z8&owCMV`UJlwfjd zmG}c|yL=_B5F5>)MKN(NR$N#SMgoOR;ao*~mt*~tyhdqOe%c5dU@r4-2pO}1#(x;K zj`}aehl^YDZYVX06ZseK?T}*r1PoqJQ=HbvcXyp_L>2E{>0GceK5B9LH5CS?jubUS z-T)HeuLm`$aBe0O)=s<^%)E|=rtV{6kxf}JM{yojXYDe1wU`~XA{fHmCX1~uGt zun=68f8-C2eYHj{lGZvqz<6jAZ!gTI&2iky>HVy-bS9FEaoL;^YZkk} zHBGwLdN)+s1s>Oh+d(QyU3p1PdEf@fllK);ULrHBhF%_^N|s%C8+jA*;3+rgKM^ox z`g;#}@jmpWK$zqH7SU|Y9dB>hA#r9A&B$I{xrvBdVlGJ_u!~k)U3hpDWa0v0$9HXU z9uH)|9|&XV^~94|jQM_8rhMWl|7shi*?GyUkwLGS;`+6Ex|bB>^#%J!@JB7j^jzvS zIQOJ9X3*y=-73hg-Zu`nk;5*+9MqKNFRzkT+T}^_&9OJ=21HSJ@q#&Eo}vEmMqtfq zA}8Ujm8>@&3xg5G@NkAKa?!s(B!m9?)v>|T1CnJiE6|)^3j^p8g zc}RsV^g)b^fl%Qv5bGMLuqCI6SXr1~2Z>Gz>pZa;j4Bue^1!#tYuw1 zSShiLr?={k&7Ma(Nh)jt$gn`Oiath)+#LUyovNOgcIVT4_RnhlBu$hB0o_ zGncqdC!9YZJ|Ir5H4R*Rb ziJYIZK>qLs;kw)n#9!x{?XEVRgXAz_Zn9ILMurIp4i49d<-4rz|;;qFkFL7_+t(LPF&Uv6v(|QKPaktC;n>UeBQ}{q9?QsiBJ3 zduu6XB0AcOUFR6(94U7t4K+qRs=yX@^XvdSjC1TWj@X?mPYb(rGU82W!X_K*qhZ0Vx>$e+8>WY=~Uff5_Jr;a&_G^^pj54_vKw<=%s<0_xTPDS z!5A1ml%qVzhVpraZQB8Y>??LMNcchv=i3C8CU1Nd(j#aMT~zj2Q`3Ebuk5!I*WYsG zbscqVNgcQ48I+`VEnT#g(wF%ITHH~gn!VwRI|Lj8q0G1IKh+QEk{=-qDQg)xgULy# zj=ONBu6CssVg+}p*_sEFi=9wly%V(0RuY9qDf7Nt3AH5jd#JQ5Q>?{qhwoq#4Tx^0 zkwAug-Sy%+3$;z|UD7Jwe7%pDq0Fw9Flx`_qSyEq<8zTNtADck19)5j1oZzRCl&^V z|Ad@aIsXr9`bAER9BltXp8oIr3MUf_=l?fyY6MkDx2}@ZuJLJLzQDYv zULCOMtFXri#HS!20aci1bB91cLPGp~hKET|6K)c1O>pd$5=&<)q%YJ z1x^mmLIieiD~P=&Bd`EcQc}0Sz=@(Yke9%f07C#f*dC~Z$gUAs5b&=(Hw4DsiymPs z9w#A!tx{khA0HnOfx0|kYiJ=2JN|#=SAa15dHe9T^}Tv;909oq)Ngb9L2g$8#pDoX zdh~@`f&0M^0l^3WDF)$S9z?j@`!4(N01meRgk5O;Yp^k%Q|p%Xz`Xr?w15Gi0Df?< z?QV7g2!6*-2)MahE3qMt!2>vfa0nC_1k_cf0rnzyf&KNLdte0>W?4`BlZ1e+y( zfm0Ab1=KqLLj8XOrw9L5!e8J-IJ<)i^$izfT@N*>3weDG^_P(6&iAAg<_=(ku;ZHi z)vSt+p%tEHx3&Qo#PsI21H_A-UC;<)a0|Ms=CcJ9ss97q34|EHP)JA!DM0{W6&K*S z$qDHbcXM8@WT3hG|BKY%Zy13=HeGz0(&5<-wT0lfbbPIwUaZfHV3uN4D0y8vT738JoN zdcXY@UZ0vC^dRipojT`TiQzy%?H`0qAqkmLxHrIW{)Nxji68t9Kk>I7gipT*#iu}_ zAM-Zf${&8sIBJmdmlnLS-~LL%$GZI?Snv1BBjjtX%M#dzaHsZ5UF~pY} zGGR^yg5JL_JP?DEM|9Qi{%AjX3=|;Xi#}e$9()u4eg%nBKj+cvdM80XtQ-lxZ^eLk z>4zQlir&lJ=bjd64H+DO`&)pwJwoxQq(J~cfkUkIAfF!w3<3&(AmTVW0Ar6|)7J{V zed7TX0ub0?xG&&Ou(}1!ne9g?Kv+b0h=1osvf3l6 zDCtjd-}WIcX1Ns?3j+h?`bu6waclQTAM*)~7BA4RANwOEpaK#u*v0d6AawZk6J}T- z0og|XlM{{;0NS3;QhO zRvRkfIdrJb_hs2_2Ud|XLWJ)6=yoy0dgrG$bxV#hq7~egVMJd)m04=qCv?ladmESi ziZqk0H~Cwr+h(>Xy4?rGB@0UeO{<0`*PZ#OUBrO&dY=6^m!`;^mzx+Z>jXk^-L>d` z%a~VSJ!M2=lvCm4J-`YJv2}32GsoC^zxrb^)W=6TBq~fBCYO(g-n0D7vGj!MM3rK` zJ!Tfos`rGCRNb88-CiE&rBfzC9Pjc|e(;z%DJSo6w)ObpiJ9+#DNEZV2*I>N){W<4 z7p)QhT}ts5ieEun=exT#?#%~PV`(~G@R{)9z5q=1g`sX4HNh?_uV8n`cBM-lc|9`g z9=MmxRa$yQrQ)hf@h0Coop3<%va;JWBQj3Z$1J?4c(HfnlG)`Wf zBoXd2r>o?IY5i9G@Z|3Jz^5CTxa}zFG4mjB9eH`MDVI{l=$akudKQ}1Tq@0Bsc{Ht z>T||RZykEM0$EKEYTZOjZeuovposwaN;K7QtMhX%)=I^$LKSw`t!uo2LJyi4C%Ewe z)fK2Cds1|9&rxs-(oAP~Ao7bsGGV<;d8JxJV+_Q-aFHax;J# zMOe7uSt4q4;$GbH_TI}adf#+z9`gfB%01RlK|O!6+xwfvf$96Rad7LIQC1`00uD~r zx@3?MoLR0`P5TuOe^RiH?_jm^EHWr=%iAyLq%$B<7Kit7&lJL!+{z}gf@f}F${~0c zGN($vX`tO`P!pX=$SlE4CSL1pJfrAdfe7U+gR!B)o4Yzg_TH>{DVAmscu#jK?k(_8 zDSlj(^k|3xyH%=MfAC;8{+KL%mMDHS zSo}GRINr5T<7iHVEE}3U!|4E5Yf;c4rO8e`Dl0+e0gHAfw0OOF|CNM4%b)Z33~LWf z+OSa2PtgtX?tIyGqnS=vcvTwpt4Ft1mK_e6M54DqM8>8ZjBuLAB;7`>5@VBbD9@{w z%3RWO$SH-x;CF*h8GwYk+C>LqUjCL$jHeyb>Cx{e;)G7dBDQt;lpfYbSDPOy$?-+W za6^nVAIAEowswXYz&`OZiLG5+INL~H^0YY6HDEhf7(OIe_A!+eXhemDlzB7S5?kcb zXAn@jJFnKQ@zCo(B^|gNJCgV?c_+)wD>o@(+}mMiL&rPUofI{{P~#l_xPSK84mqsndZtS0yj z&QsBZiEri2h0Mtt2-GUEdp#V?5tm8RlQA#u+HZV_n`fDKHravv8OdQK#->dPe{0dI zLXVxgzNcrsu;Zoio{r>F67F1$t;TLpg{;2am^G1VR#bYXm_NDyOPoxE%f-C*#!uIC z;cG;&QHNO>8^b0T6~~W_d+xW0ReU)2nCSVn+GFhY5pwS=LWceZ%2D<$5P>Cbf+Q+O z%2IH#86P3=_p#B*+n#IhaCszlH0bEwVL&=aIH zqr6wWvYMLClx)>1c#f%a%hS{I44RNC(v9pU zLXxs*0o zTb`FN)`uMC1K`Z084<%#$i4LZY8+ln1?r*p=We~fZ1LbX`0#XM>}io6=Axi&2N>Dz z9bDA3mns{UFNuG>w{S%hi;<4lG0W*z4?OO3vBW`@h#_nNii`v zzuMG|$zEvG#HW$CiQ1^}J7xdX*S9i6L|4VI(!WOP9PanTdQarYqOQg@!|&$o*bgW= zd-@MhYc`}FnnXp;ENFCvHWRuWycfv4BLa7vG^|X1x|?JY!^=7idu(xt&}TJJv{+=& zJ6Ci0*G@!jb|NPcS1~0}zJJuE?>D4E&Umje?cm*U3CuM6C@Y&sxRQEan6O#+|J|>5 zCdW%+Uw2mg2AF$dC7kf;k4kV@sDdcIUz(edIRx)&?PnoO3$j_xc`?)-XH{d*a@4YF zb(_|AtEl9?+|GPZPXmF))jou`=c+Z+qdBj1=lu84y>sAb(kCdYI(Sn5Wbbz_cMxL9 zsrZ%F=U`rkAJV=kTW~AtP`zlCE$Ra*<+T`7IQsThdbTsreb!+QswCId(hb8%gNTFI z}4vXXhbb&13RZV)hATf0s{XyLUGJ$fr?p`vg zNQLT2D}q_%VhWt`FHPhoHp161EPKWJ#eC_7Zk!=A406})2-G9b(sq+cA-RozgKbji zWRImaevBL{`5a5ItUwby8Gj*|E@9K8vSieIN5(krn339C-o1NW3q!g9EEZgE zznsKZ3@bXo@=2a#BOfnA_M^0S*hlLAMaSLMC|McJV1+<1~(Q_vNgPrc%t zJ?>LDL1*AeR-QoWZ!^uKv$qL*+u^XaQ@20V#Vl5Syt?ceU(!-!wL^+t((yiP)#=LU zxYm*aq_=k&=)2{IAT?rKGjd|`7w@OH8NyFEt#S})i!acbxEVNx>wHIuvj{`Gpu2UK*0r%ScadAfZ1vaf_ z&pMfaP1#}4Bi17YVg`i2|A|5N%HK-y9?%Em0+3T++71mjtc=_fu%-qetD^=^9-tfC zg_=Yjv_NC{n=L%6?@oR6v9|y}aNnVB7z|EpskD}G^vd{sG3~;uh1|CUoHKynF&V}> z{92IL38{uqLzpe=OjO>JAU7TqUo!hso>G0dsvR%f*FgtB#DhPwVDYOm-x{Jh7_^gV zI=9#`YAOaZE*h%P%@-jhZ$LXyoW<}v<&mm0#YTTNsUODawwpZK*~D2lzXkVvi62qa z=5M+c7ZO!U*`BLa3=?XHn-FcJkCp@@TZ^pUFK3{_N8%fyFdr7Pch4Hu{j1B{`$0Gp z*A%mn_}IIk3(+fQ!-2fPN4zbb^s=qG&K!qmeh(5Gpc}@0m7_s4L)Yuwsi?hy3tW$& zy8=Gk8wDvv=0wuCnb`HPC^ANTC~a6{L;UaxT?V%`uW+<@+Xflb7~;zwmf*pZ>+veh zqc|ZV^06R#i0r5T;1XG81dSDzBh8ppIw`D!Hy1w;;j?H}j>fZPL)GvI_a;F9qPlDH z2yuK1!`x8h<=|!$V9U)?P)5mZ#p0TXQ6CwsNx19bVQm6(`r-!}+tCa8-{L0ISn{rH z7il{u>_g6=M=NgAe*InBJ@yGvhaA%UKMRPb7Co_&4(X33YgbVuruUOIYKP$kk~ zAP0qnwX8I=)+WD@%DxC*UgAed>$WV3&Q3=7OloD8BubCs`d;$9OgeX%$DQYaC#l8~ zp72N=R9GR~T=Rr)Ku!eHH)9pi)z#kp;rVl~Bgd@W!&J}`Zk1)kX&pnZ94W=|oUQ8A z#V4a%o@H&fFd>4GPaZFw&OFiAM!@GsOcuS2xiq&n@Z95++TGeC6%2;5-Sw)Lt`j1M z`RhoO%^$}76BdV(9*aRiZVz#D0COZf`3Ml+n>;KidOlw%u|@=oWhG}Hv;}>}0jFfxj2vn68-@~o~;l7orpz}@WHGURc1=NkB*XyaDdb!fFykMhCjQDP$}9AiwV zWj6b20EtzTdd1e!8h7N!kL7S<8_43&q4ai#Q@rTf3PN~%l7f1u zZtk5d;(2aGY=|z{&#Llz-9w^$@%e-?QjqazpMxgyyL9b-n&i230m9p0$;QJB+`9KT z+Dx9?kM~}}hqsT$II3b!?K-w{qfkZ6_B1aC9}5jriK!Q8s>N(R z7^B$bU;7lT0Y0I~y4fsb2n^~|#-ulkwRigy3lBWgA=sby6yFQ=sH zfoZ{N+9p~6GH=NX`T3i#5az>!ILHq06t1`ld_5YBd_tRke8b#sS7cX z3S~t-&kpSW`cP84cHB`@>1G*JbBP#GYUvQiFjM;L0j`f8lYC7jH>vf6C`OHK>xL~$ z2e(962#(QJoo`yFm~+|EA*@!f+EI2%Z+0q5n4?Us&a&YyqP8F{JJ@6OI;CjOPDWK* zu@W`QQ|N3Hce8P<)beE@JUBX=DNk}XEQZpMy=fonTG_1a-gJVz=wNU=y+$Usn zQu^7C&)?C4);d6RdK3=RUCH|so73vI&ppteKBCwEOyFRZT`XkQ?Gi91hW^FUP=!1& zaTocHe|m9L1*nU4q$R~at~ID5Z|xgKSkfoSeuyUNJUO)mn~K{r&1L^Ia6@U~oX%C12Sdam32svC=H2!B#vNiw{rxg zzhsIA?3oW25GB6A z!CT3=W_v}tR;5dAe_`v5rsU5Wc#r!~j9pP*cG-QEs3PXLo2?2{JmHdOJ?Z<+q%5GhU|)b)luc)($*9CQCVv?PuzwvDQE?r zN7lp&V~=!S{ui3{cMr$%UfRd%7DDrqlJuZciXR7ZCV#f}GM~d!JlgAdHJH9XH>Go2#C zzLHBypWN1VZOMC7vVf;L^(Ppret;n$et!kte8OrA3D?kmz1`>gBoN(jX53aIK~_*A zU4PkraB5o8+(r~+bb3g4%EFIsbbC@l?&MXjpd#TKf}&^5as4$b_8ix)vv0dPb?a=W zFlL;+S@SbFg?o&~q(1qmhj+#ZXH)lDHaVN-(=U$VR z=Xv7-sN%_d?RYdbUaPqKZz_|S-{tLcRm3zj(M>bntX5@EnlFJ5DtA|B?S71Q60H%A z`hazbGnyrRcKwD?DbfX8DS0w~r*e-GKOk8bwY_FC{6p4oX!E!4vGuCnksq102r)yhfDou_U7Km#}RiU#wd`_V5ft`G+ z3v^D1NZSdcCG2iqxad7ytBEpP`uB8Pqi{H-!us&ztg>0(D_(YI0*!6# zjm%r!zwGpYxJI17v;FxGFiHg$MGRv0{2NSB{b-EiS-i@`;S8eUsco?Ps6}hxhCM!} zrVipw8pvPfyuzP@+LD!7r1{?f^!_x%pff-e+OoTwsgVHCn&rC&u%u%n(*_6Cr13+ z+0bxdQ`7N)tH9xtF|Igw;BiY(40RB#FssKO-ku7{ljYl) zoGJr5R&62o901Q-C|mzl@N?Z0g#M)Z3hD0pl>(#=+CfoUpz zS&!y|HLHk9|6h!qQd$m^>JQ;sYpe5 zW8Zm0>l`V0J5Dz(ebQq8?$*kstKS)ut=m6Dhy!LR z8)K8DBgrn9xlc6cg3CMB%$+5)zGf`{yA?Jh@*QwnDT$lpy_qQgjb;2Wp0=fu`AK;0 zVDP3O2G$LEG?DzS6h$#rMuAx8kM!*)i^nKU`(;w@H4V-Y_Lp)U5=y9K>sQ$Qy(+me zjGr|UE!9i3l9oWpp-9e4GcGk6NnRW1=oE*ID_!8yo0bLQIIobAZrgm{llaXXLQM5O zbC%b829W`{LP!x9yp8HQP`y~&0^1oEYq0{QErZKBm+QvI1p8%xCQy6cZtO}rY`!XqgYnU97P(UtLJ{T5+_U%oPEKt-hF zi5sXV1AcJLm6Kf_{6xj}1H`)ER_ER-^DBD|tVvSm?%q%(N$-Vf$VO?ZjCW@%tMlcd z0@$|OitDZ-atob2!zYkS+vdD#`S==_M>1Q$;P&{c*6A4q$Y3@JL91pPOK^$i<)kb( z1nut2nZWn6E7B3dqA2#sg~^p?d(ey6;9IBf$eBP`33&F$BLA$%9ve}5>MGrmukBam{V=NJkDt~ z=Fi?O>p(*94z2B#bO*^2x_h@uG!tsKvh_b|EYPXh( z+`l!ty~%ilKt*(D!LEwwM+Vo2P&~PT1ZXYtZq?MZ^yo*YR{eD(_hCQJhmW8Y+R4X) ztK3pUA|-=3Qk;@{)!&%``b;D(Z=5Bge9taQTaNN?CX!TN+K{7}GEAAl67or3dd0yQ zXrLcyhLUY`yky?A-RUORa~sUQt|bg?V$58IEq^96=nk52w0E;|&AAQf-YRE^ji8{G zE+pScAOCRYKw#f#;2??CC$xcmU#^-}(RXjY#_(d*ck^EzH=RAY5mB`ImiBh))GUwR z0~U)750bdntJ5>vUr!2d_(zbTPb+5`Y}sHOielwrwMrC|#e2o37P&Y4R#3vZWaC3Y z%>*)`f9Bojs`q^6R5I+~a{rK6mlkTx(i=QVXiY0;-DWt(ll&_j8VJ3MSt5f`F%c&h zcx{O?6qM$leoj#;upj|CyN?BL$oaTLd|?ulD=iqg{eihvFT29iPC=-3_A!Ro4_XCt zZR&2cty^^UTP0jKHU4-wx84_`MSD#J)#{p)IQc-=b)9$Ipe+Npwsg^4$~;Fl3x%lq z+alOGnogGgi-iXN#c8`iOuO@@+KUwm#qh#dITC~r-|D7Spvl zW4}0U3=HN*z%Ms?kY?-3;v%qod7g?tnnZxBor#2CI}5Lnq(G8Ek+(Az$Dt=_@)+b9 zP{z8m(C4U=f~)ixl!v0Nxg-vgd0wg0v>nC~`d7I4VC%jY|}H z!&<0Z=|^^gX@p>|Ocsa_f|>st!~?yZZ+KzLs>5ii^=ZW}S9adk=-kaDfSvcl%Ft&F z*51j@5$*WB@u;lR$bH)B!=|0J{O??5yktr1c-YL7mdqZ~H}*Y0ctr$&_Ky9bDzG+{ zY)(qCCWSNWk(x7#L%pdr9VIu0w%RDDVnh>HlR>M& z17|5%l@oyr=}^^$iaG5y;h`Qqk;Y*NQxh%c@t&oA{q@5M^ySaMCl{}LT+w4%9rUTA zdRPby`=e89Y6{7Rw&I{y`ZEp=k)|7CG^ZFgX(I2eI1hYh;omQ!Z&Q1qbq z)!rIm+I|!IZsKI9sMvPR8^RAi39|EJ`w9_aSWuB7d=KsH&rWDew_4!y^%x2EUY8S3 zSKCm@!(bVGww>gFQqUcocS2DpHPOD5nt=l%V$+eC=k0iC6T6ev)JicZnl`|-$7z=N@=!& zAj*cmp&21sTcN16scotHH9I&%b&Esi<)MiAryHfdb6CNm*}+`8dVKFor;AnO?0f|V zsPGCtKM(;oKY;g;yV?H)CNcgeFo~J<|A0ws^lblo`oCZjJ3ZU~-@fz@Oj_JPW2MHK z7bj==7lzu}n)gQpfu`@B?470AqR3;8pT|d>C&4Enfk6}sAk4*oir)74dGfOCx=C;P z_rQ3#?3&p&<@=~*EZOJf}RF)sKG0MfH*0YC)4zNLk}sl@-nP@uph-wc67#6aWt zbfGx>A#k~nA%0tT>PfrR-4LMx1>on2KFI)BO&MR`z(DjFJXc>}tkhdW>!9!|^`XcZ^&7I*{5 zJLUH*m_VNo)WLu$7&?dG=8r$j-ZSntJ}9~`uoix^UMkmkACd{UC_v5>1e&E0=x?>& zs_yW*55_IvyBpINkki}qKkg;a=MfAe%ikgG{(9Z;ssEZx`@p|W%Pbz;U)^p2{PZ0= z!T4yd?BjTcVE(PXR&u|55W#_zP#%HsmArgZPi!K5TmQB`vtB(F~L7&Gf>$ynU zkt0Jt*p2*TF zc2YrBLGXb=frp4ef%Mn_t0T*CKdyE}x~jK$3cGBP<9z?{r)#KMu_OR&;A()p&+GR! zJ{^58$Q~r@;H*Tzqim@_WW9S}EIejphD@csQ| zY3w(jjDvmrPZ#mPH+8ha%%;eyeDbY$+e0oQ(hAp`qbd&EOGOS03IPZb1~MoW+4T`~ zhxY%>0=wPm#-#UP=ijd}l5csG?bq>f%{jQ~+Xek}rPgMbtkVOG^niUslLI%3c!7TB z7=Mo({m5?h{FB##e)qhU{%vpl)aUSC_`yR34QhS;o+@}eKl7XKhQ($9s{LVM0sCR8 z=G5Z~C42Nyhk=8)_=T(oOZ(F<9NIxS2yXR)zOR4rnxgdIa1vbVZ_r=f(5@k$-g1CW zVE)}7yVA0HXHg!^YjU3Nyx!8hw;Lj5y=J?PodptdLMQ+MJie}{QiMDgf^QGN0{W_# z*9RGT_avZ?M~)Vt0_-XDDe*xeTpqFo=~AIiREBtL$bTfDHm^AFRX zlO{lbS8wy{@0`76g*_RVy(=_lDCX_XQTsG=mBKSA8J!<6;gAzL!c3>>^~_n8LAapD zJ)QWYKPb_TZz921KLN|4nWZmuj~ms%)l~}6q;b}-4>{y~(#Uw^ojADz1xGCLJFu3726$vImJ)u+Or8ISWNeH8odp4%2qf*3h@%i9y8KG?o{i!&eqie*Z*X<5**FZv3Ri0M)!y~-b4+~Df1L_di5 z+uw*zXd87^fr{ftveDPqo8A9CeK-=yT-@dRop)3<}`Y8S;Wj}VbMgWkRx|o zYh>Y_ZT>EF!)dnWFCQ^xT54;@Yl67vUdo|}NLRwyO#audEKTYN)ln9;;8l38-fTWQ zR9mB#X^7OADrvbEb0dZkO)bm_frqsrpw_+m~(m;qmesg43DN7#6%A9@LBflQs~cQ|B@TJ zoI?}#*<{*=R>|ju-M{pnxH%;&%`$sIKqji@n#bY&#`f>;!|uni)x~l`2Iv`Pp}r2L zn&zatSRw!1Q)}HAJR%JjX}q}S$m2F71P9NLKLG(GBLyd-v#vELwpz_gGY(LqN|8Ci z85O~-opsB64@ZqUS%n(icC{dUN=u{E0_YFkv(W7YVGkUx?1<5ee;u^`q#o`xz-v7{ zpF~tT_1{|8@g#4E9*#-zxMx*_6SkSI5v^h~ufrM8vuzWb?7i0=8&31i&o6a)>u>3* zbECYjDK#EB`v5fo+w>b^BFb-dy;>EkF@BcK#1412Es;d*CE0J5P$ROwlShnI`qJ1YI<(FI1BWB=XK{T_RiMl20Fao zjdrN!k}o}UabibL=WhNwAI1}&6t3E`?H^FZRibeYQk|r7khYku-y402Ken}vwq^hw zzOC*_d=z`9+%z=zlhn}*90ncPOq_4dW85(W)vfT-Aw4G;ObFNh9r+n(Ug4ey>E!!Rg}}%5GSSTzd4ZU z{Cl_}N!3xReyXY>qb1-G^5M@O?4G&E0)h-akVOa8My~unEUI zksUpSLn)ALwPgyRRE6?)YuQY40{0=;8ZGeU*}4~b<7n<_JG~4_Syg2$2W@g@f98>w zPy^$E*bKg8X1r9?S*`SNagVxb_1$Auqj`Ii7LwGCr5ij)qW#4kVEwf(u3mce%new+ zc;)IIf8lxgP1I+G(Ae_J%srGo=(@V3%{@VjwyxQ);(PApnQ$Z`9X>rqMqw0WgXuT@ z$bCm$XvipLCzTUoMp6huE~}Q!(HvA(2AC8@vq)-5z*x$CQvPpfj2F-9b%Qxmn7*2q zxqgcWvpU(Unq?j1Mzlx%kRCY*`^Uav=hxJFd5p5@CSDoqQcX2#~Web( zK!xP?0bDy{LItugHZg0L?6_}(Ab!gkj_7`4YnKjprPX)-gEXde43BY?Hbk+c{+gfe z!}C4cFE*<1XZvIB7kk&NTBsDShtMS&8li{!grjUGl2EF}7m<9DVb^Mlt#}R%mgTf* z>JICeCtAnU?zIr3ye4kM=@AFndX1~eDx>oK9lYkfwM?zz%HHkGr-^YcejC^Hz-Lb6 zw$2W>R$`l_@FJD#L*~7LLJOrOU8C_u+Fg;1iR8EbKuKOXdG4vk%FCw@{TN_sNDnRW z5X;M)iVXH^M>i@BC%N{*4?kYn8E}lZ$s}wQ=Z5>mI* zgI$e;{+#RyXC5F;-c(4L{K6a6w{;q~PpUG2$szZq(D@c`59W@TVtBw@_f+l5_@Yto=TE zr(>1^QDaY0lj}Jr+Vmk33nk52z`;!l*&;Q=)!lZ#M6c6kj!}zDvBjii_2gr9me*?= z85{Z(wK0r$R0u4sDP=F!^28jgh*l!3PwKdunjhi`^9DqHW24p#S);c<3b0K)Kb207 zwMl{O?Kcge67u}|QP}^?+|?{UYe04{zBRYR)BS!{{&pgBIFBwP4Mi z=c`?kRn(|YNUqK7ZEzE_Z^(>nkZhvl#bS5v^a)XaYiJ#)Y8_JKWA&l*2I{*hf$7NH-<fQliqnq|ttLcatRddxxKxvLc(kpNgv<_YVYLw#{t@1KMlj z?V+3FCl)^NYp4EdhfS?4n?SWfD4dtkaIsj|Utg>&G1raoQ+z>u-%2*t{;oV;C&*D# z=1$I5+FyBYoA%E<6r2O@;P#q{AQ)@Y+0&gx=?VR%TD2WZ^N2U zN8FPuFin9K%rEe|U2|l0T8{`^Xi!(N4e-#auZ!6F1UJJZ_a3?_EBFBt zPD+<5^M4HAI0=R#vg&FGqv4867=Eiwilf3sL-z!hPUVFFZNk*}<5DD~EZ&AN{M*$Z zxqyB>w1cayT2NWV;5ng^wx9fpJ=MB=3Dq0w=t=4T_;BOohy#y&#Env|G{da<%4rX% zuqLp%vdvdLN}7du79G``H9S|WHwgN}Wk|s;`Y64 zR7+IK&^cRqeAjRe<6WeLf7K(A&AYa}LaG;vKXlXxu|1mIy|v;mSR*=4CFtN~pw$3f zpGk)+{=xuopBE1u!l5$yzq;cY1Y6_iw}E+2lF#w%bW?YH-9Mw!#Ha`XRX!)k1@qj6 zA#yGg&$YU_e*$rD3)(z$wi4n-K2CcM0UgDAt^4GW)@`V{ZW>OQ5*7P5!TaH=ZK;3t z-g1RV-d(1e%#t1H5rSku;Bi}Gsoio_+s-+k+1{luiNl>T?uBQ31!IgXMVZSe0P*C@(Iy$2YE=_)%a{o;) z+6NN27?HlyC5zTH1^;-&+=a#dvgYl;lC*>sGq+yt_lr5}dkI_HrM+70U5w#d>wwHq z512CYiOOg^>xUEib#l-@qs3bZzsGqrCjRCb)pJ$DJMq0rB^-G;sH3OE^<-w2(X!I$ zhTiAr@hv~=lZNCEFAje)Q#-+ch0cg1XBW%bohgOoyvfuJ!@VKg$gHn+f6neW5?!;| zu;zJzd03ygPtz~9s7wb$$Z+2KC;n$rU29EDR;71U!oYZh){kuiUlfMW6P4O!gqo-f z@j;n8ysrIQ#$~ojnP+mL%Y-&nTfE%$mYjpcnRrG|b0q*InL0vtQb|Dg7U3rj_ioF> zk}+tKoxLn(>Q)t{zoD$siU)RTFe$m=SByS()8nmxSZoyx-Qgo(s$)*hAb*T7LMXbe z4dlYtz4Clxh^MZuNZjE_T?`lZI8)3FgXThcz%fMDD7jm4Vq~K`)U-4Vz2`?zZTgQ3 zRrDP^MesqXL zWJi=LRyX*cMSnN}eN{ok0~Ay}xM`V|;+&iv?rmK2 z#X9w0p|$e3vOpNQ&8hpd3e=8XZsOKi`X?BEqeSq&b(c zmy(LD;ZHA)B5PeUkmNb&i%)R~v$Ev29uYx;SKWxM-s~?AQ#2TB%!%(fz=vn;7+;u} zHT0NfH6ym3Gxn?1ASCtPIx9z6zircf1RZWPu(*)mMdDj;u}+uy)sSweNmQ7h*!Ij{ z0I8-{pF=fF8x-SF0D-)%K8tlpikyRe20^5WhCqz zQKPs~zAp~Bz?K84@h2f8v6hIl$P1+n|7F01k;sHfo~(=exnmU({EeQ!%p=OiTy{IJ z-`sb2UYGT^ku8tn#ua~oa8lYrGnQdY8zAR9I#d6jTEBG>CjPf`z zh4i->;FNHeo=Z=x<}+X3j&BkYP59e%;z@mk^TEmVWHi(l8}Tgu zB{3|Up+m2EiKQ?ZuPEk7)#YGR7sl(j-CeBYvosK{&rE>GHs@YNM)3Y@%NxC~9djC+ zD7EtQ*S$;ZvvufNvFCs_%!Jrl%nT9WPh32Yt&zwUU^y?Gv;)p9OsjVyyZ%BGOz|q? zmd>9_Ci7x8bQ8E{kwlVl^|(6Qu3Yp4R;g8x3BK(es}lL%`z&J4ATpBv*&NRX9V)y` z%HtGU-!1Oj-2sRD2X-}4`S2I%8T{OEHn&vh2x6VQi+j7py5M~BK_gj3*yBIzJEI!y z&pm}Be!1+)IlCC;q*~MU!rXcX+kn{3r;L4 z&|nG*vl=BQh-tL<_R22${M&H#b<05(`1Ysy_lI@K5XFk_IYmB8U?FKB63+6>ru#Xc zHIZSdqleBDHO|^u2a;6_m)HW1s2YYt$lGvU(Q=Y#=IHy(f~TPzt$Qv>AVaUzh1hg8 za-1jfRD!{P_(!n#*0!{!KI7>yjnxSg%B@-03@?>Dyo=56lduYVHH0z|XeP1dPl(mu zAE8b79NJYaYttCHSM4a{e8zaDei8*)pi&mn6(>yg8JscSU8)AI@ z_;tM#Cnfs`_@$ckbDdHuREF4fyD7uAEfkF)#wKe<>W6vO*X(|2&Xkae>*GxpNWof} z_wD-(gqf0f$!_&!T{Cpse~cj~oF+pV&tBb$kJYIu%p5e#dPe(1K}Q_Tdl@YrlBiuD zF<3Z9$4kkd{wO$}cXx%03}>)uC9DlY(N2?MB?zO=NJbq{4x73ll&xo$PA?aigyw)L z>i1_K_Dn41N)0K^{!$yxDY1JBHVGMcduGi3V&vBOxr1o)O*1Oqb3c{(8tl@D{^b2C z<;T@_T{$Zu9y4I6#OzqdVQai+(nkFg0#&I?Z~a2!Bf{1YgDL~Ju(P(4PC-iui)a|r zKhq<`StceUf~f^_P)FsFxXp^(DzP78ATIvuYh6bJUHtWxc+QZ&J@Izl#r8SbGW9hS zy7sP-P{EK`(J2bYz)I$>>)Vp24I|uN_&!*SBX)b_n@!omte0?YAAb=bKqS-Pft@Rt zz(wQv?m_ZoeO#6hQ~;@ZKQ~8}^*6o^MlbKPrv%%Z9qrMgt_fY0 zxPM8}jrr;*WtMLaE?M)q4`qWMOc52%UzveQ64a4cu7)2=*U?@Ik`Z+|0!`F0Q7n_S z+WL>#UR#CXY-SU>fQSA*3H>9m4%x)0eM7FZ^(n-OGU04<(fw}08g*i)?`~2RetaygOqsryw(=h$J!Y1hSomk#>U5cXOdGl!4@<>ay&Ox!+MG!?v%z zSAY0=l{uaJ0^Uh~HzH@}w0#jXM3e@cLEs0!{h{@Pap>VT4b~QH!L2-B%G;q8R`7WF zd*SYI2CDuEi}t>2&}1^W&v$ckQxaMc_TV!Iv{FJYr&NnJ#qj0kwT8-;LW7W#4AQKeQL9WrS8}g#i&pQ zuO1U*lKwFWBe*%KkYchA>0w0amgbP|2!gHLccVxW#JJ~5;IPIogZW}056nWt$boTA zh}cdD4}`&cch%#K=92M^%gb8q7ta zH5(&f78?uI@i35N+-3%aTMY@VW~TdWLS}Ki(N=J)rEh5?I;@f>mumBWqt`g9a9_X7BwBNtkFc0aa%j^%FJpgDY>%Cv=?qGLX) z^~3ro{8}^cZY2OLS{vy~v6{G2V?R1z5PiFH;>6UF>?Xwoujy{@VxU2Ny{0&TYbKmKIk#OcRj5n0eA$XgX$Oepk zEwK=Bu(ZU>EeSb`uGBRnlp}`=$pl#H@?HzCSdx*8(fYF@f-B-wbWEbSBE3dAUM0Mv zbrTpOlOtx1zBj-(X zvWt(A&5~392R_3fDOzYUTnoGHP@&x&~?cwklxoiHU`O@@d>c#Sj5bFAW*Av)Ze94a=n3Skn6=$ zZtjP^JeLx)#3?-FQkLs14C3qcO?i|um=~qN+nkzS^cP2Q5QB&AwF5Ys9W-wV&ilG& zj=vA$Aqr=~LT9Ix_F^Bb>Ekagh=#0SWu- za#=Ys*~lxeMcwC1#@3ci%NLIVp6%W^gdFMq2=N>rNv7>?0=i^`w%8%3)~xPg2!!EDW?e;X310hITu@}lB<8YM7Cu7(NG~uA_rPOxjNe` zRH{^~k<>lipvVTwEGHQDvECW5ptGcF2^;Js(v=&n1uL}fhNcUEv@ciArez>KlHKrX zl4Lbw3D)@5{RHM@l6Q)_SmsIE>%Dotzh{|u)WaRWO=hlPQe@sLx1{PK;3A6Uu?XQn zEz4!W3Kl3=DKKcKVq*4?@c$G=IsTS#=qos}q?TYJTBwmGey)F={}6=>M>~Xcwg>8y zAE=1F`!=^XroqgD`~van!$jV(efu|%J>9Gr$bbd-hrK$9^l|y?h6ziW-`^$)X;{-% z9~*n*)UDQDZxst}WSO12nLN}5l{8m!&q0UeXTP?|JG+hI7~1n3BrD_*`7nRv4fK## zQ$5yBaD_4l;w+x<3E_f91A9anR;q=we^;Y08@>tX( zq%~aOt>(2x(Q0Xd_SyKUsQ+4e!_Z`@7H!gWr1 zl+p+9A*K=MYS00bJB#Z) zO2oG>O2mqwQqtwn-wUTG?+i;vB8oUVJnZ=0q)ZbI?MAygpb)hx8q5Ms@YeD!)2Vrt znd|h~NAp~>@NWfle9%lnre8w;O+ZnVfeB@Fniv;srUN)k;bAR%fQ?3Q0r1H}tze9gUe71iDrvKz7Vdr4_|GPLsg-{CqP62_Q1=}CnFWo;s8api}RcSl!E}37M zUzbb`!9nsXzGBm;HXtg&1F_uvI^Ooa{>gqAv3R{RbfXh9biNb)1`q;*eLe@R)WdqJ zY!KL_-@3R^)q0l1P>PMB`Pfan^uFy}$N*$eK+upM;r{0*0La_Bt_NqYdLJNS2^UJk zAAyz&`ws)KQ%Bx~uvHE10zqt=*@*$Tz=H$GNlE*^xUlh0pk0D|EA;_1mW3dz7)?Z@ zm{9b-0r+}_FFgviQk?j45ca&hva+%|mZoevtrcC&;B37}{!W0?fUVwYTs_TRx^aND zrRlBiqMN-sAQ%?Dh3^+gE6)7FItnyDkQ@XAw4+c^>wr~XHDK`?0K)Re7Zw=$y+H`p0F!+6JVKoMJHxi ziiM40d!-NKr?HWX?VVx+BbU4Y@-Id6Jv0Mz2ql2O<+g=CtBd{ST_k|hfHk^E zAPzv_&7iMOfDgC?26*^kzj|BiokjtESighiu|K^K>_B^AML;g$>Ocy3?Yy=sd3phb z6CIjxscyCroTt_ehgb&g893C?)fQJaKh4oY1l1L^?oiafWMjAHS{z? zJ05;CmSUGn&0_=B3XrEjJO?L!PT^qf6@%UUtepwOV+Zh-KKr7)r?CE8Q_{bM-(!QW zgv|HzvnnU_%Rz99(1zUno7GRA>G8%ZNe{pEszx6P9`To2N4ti{LH^$;4M-qQztXZs zFpn=(Cf^veUt;-!0P^T>09Py3bWOrq=L0L7VtA62HG344!o}q12!Os`G$oC0Disn*~-#e z@lnO^h1(6tFa3d|dzjs8?bzV+Bk#ot55$)Pf5}M) zctRv1?lqTcUaovM|J3Ea;^$u5{Kx=-=Et{cY<#O+(B0~00opp+^XETtF_bdCb$+@* z`t<1NAzZzG!Nx;4dt$8H%hQP+j5YB3w>55}B$+MaQ3M)gd0k)Rh9y-oo)P{mHYYo< z3%OP^dKwJJ1UudsEMAOlY~~I*Eddj!EBusy0?F=u;EmW`^*uJ*S$r_WMV2b(e-ZO= z`j;HLPOT|On^X@7{{>~!@uX@ybJ%dBZicb@m6(OVSiFFLaCqO*%wRByo|Y0PWFiZ3 z5T-rFv<0&iIc!63fPZ&#k5i|_*Qq*n&=B11>7K$hn?vBHR5+(eriCRVC~{sfoywQV zEiRIrSh0N`JWNUFR4kX4JSwbwjy>J(c2YLFzoazRdr>GL@M7X&9hCdQ352JyiM1!l zr0~WQA=ap3$q~%=iYwPG8SSuJJ)@UwTGk-Kg*CoegA|DRxJ^<5uE*-))xMPOpQ4(X zVQxU(IA{PC2%+JPRnqB(nUV#HTq~a79}eqHlZ>_;G*1+KC5>85vH&1nq$X~+aBalE zN!bd)b9l%`eGw}EgQ1{DmQl0q4E-dG-JAAuI!YRV_=l^Z<)b-XV<~W&7ZJme5|Rju z*<^1^1sn1WQqk`n?9J6#*R@QXI~gSd%T$scm6)8FIGts2lYe4ym;nhc!Gl#KRCd2n zRpP}UPxN)bv20gW!90KF6@)hmFPG~4iWr`?a(m_C&3=Y)ZurSp7OU>xrEI#3~|5V($W;E+JI%(kdDO<|?flP6!cmEc#Q zGK6=^i3c~hVA74Tb9QAUwQ;Lp-Ob(M!EeA+NXR>itN~SJBLQ!r`pU?UYHY--SG8t; z54F*B5}-XX2FIQI);3tsgdcch((uf?Up_9PgTOD{%Zf%y+Nx3U6sbJ$p;yn)^46Zr(F^AH?*8SxaE6}@pC-jqsTErVjXu=D zVK|F33(|P5r_?vGTmpp94$!N=4#&=MZKca(4i)e_|6V_^oXUGy2o6``@_-E{Zoe_x zzkhs9T@$mXKCnr7VD_}4j8{t?~kc!XoXw27;k={FAxF1pGk0o!8&nY-}74~GuH0)2{;^9sZoj4H^M%GpZ*bWzI zooE=!F&nr~8YIT8mbNS2+3NzP!Gt=i5zh*Cz^~AuG0w_a*5525RiazvL-wwsQkR^O zukWwiI{%H9k?)&jc5j;ttogVj;3&s*^=4VkGx#Opz8n_J%n zl^dXAe6o%QRNpB{J$zlg?ywzfC5g))I6)d~BHXF0M0so6tHSu1U;}x0 zvjKLof*RS5pn6gXpexoH)$lzPQN+|+B&4+>P}-SgA*{jUI1`t@{H{#SVgM#q^$d3K znG2#CqE_&9?0-!i3ZwJCav1USLvTF#tao;@aXg3obgn(3cBHbre7}nWiz8>I(XU#n zieh1Yq|x0U4<`|0E!g|J!*l^Yf;rJ~xw3lfK;B+)=7IF~*6&Z0Lf)Z?;33>ijjx#*<_?RI}F$2`L=eCR&g)$F1{}giAI->C2xKYWk zzp{HZc<4+SXjqtF=T<4fWahn-3aE`01_<~}-9}(Tfkro32gU1%rCvn1Hk?+W?EXCJs>B-As zGj=k{J>)QUlKutC_#ktE=dP_08xSpJQ1nFbd`uBzMTE*%$8Zb-vmtSqKUqbjrlmCFz8i?dS_@p@)mb`+P`ae#clh6}JI7)sWzD zB(|e_%^ub(3YAOp7U%o~8#w20i{e|Tq-|GLC2Qj>SIi(CiGE*zpIZ(UeqEt&H=S}B z2J+9{(Px%XmEVQt{F zNBAIHU740Q*3+e>_3%Q7)Ep%Wi(H%@3>PMnLXOA^aD{dm6$+_tbGg{SUa^KR`i$@YcwfOyale>blYzlVa4$!nJbJI;wsu-c{X#cHF5 zG{I)ww3+Hb-^^|gBcCqZtn!t)GbEP1hzU0v4LET2}E@4usw6A z7^v6zNm*;aEKuQ@&az)0m*Xz_7sazO|LDF*S)~|dbuwx(m zb{Ko^dcw@q6ez~zLC=ZQT=hK0!}EzP(A@3yyUL2%vdv_@6el^KlKd>>Xb-IE?(%uu zQb16!H~i(t3tyGu?dJv@QC?t^f-RMcxLFILwqeAh{`7 zVKJSYL;Q8`eWF;Mo9Dj6I;oo&y*Q(76EnPPoc{!Rz#*sTd~m_*0jrZ?FN%FtCf1u_ zbb+NZA7E1j+k8k00ygolPTlP~v4E8WN{<&xfzp&f#wocstTqZLXM6DExmi+2fPlb!OuoPncQPnplggC?(aoc5>%%p_lHheKfJ(muFI8_7axZf5p8Gs>V4I%S<=C z#t8TZ-b+}D_Gp~ZmKvW*=8Z7T{0>=M zh6%1-r?f6u?Vtbcf5SmTA`ui-TI zXi{IwWV-J`mdar4c9a3ZgB^<&9J~8tlSUeaqQpjbFU-d&FHN?U@Yp@N{372c$* zF}F6O$yyuZw`fCo6%h#4N$hzTwM?jvzB_f?c|e+G z{Kz8?-DkEGBwxK!XV*TgQk+#V-`L|xr-+GH&|`2IRUm@IaH%${q|r4iW};*Nl9u=^ zg8sJz<`PIdv*veD6h;U0LMQo&iS)_`LJ5>@-|tt+N3eC%-i8D6rKN0e$nb0`Ycy1n znOjbC#Ia`UoDWKp1em1fVYgpXiZrGOcZ>-bbhhkrM2<6_nWtIR_-+Tt1LIejs4 ziS#qI5+Yu!G)W*+%IMEMhK_{h_N~UFf28A0$Yq7ItU9V^g&D*I2G`@MJs&$X8M6Ot(F5!cPm!!Q(l%k;#$dkzB<5`!`Qx%{wTqPN<+{ z=Qj|rT`f7L>3xnh7>tlm?3tw-TC|T7S$9_EEk>+upuKh(cl*{A*qo|s8)-|Umh<0M ztL7HuR<_u}HmeG}D!&EU_<|$IYMSe3$w> zuAs3Dq=XWx)`<0Q0!f~eQ@5m}$QV%HABWDe2N%riqih`xR?GMQ&}GwQl+CXJ6w_68 zDq!Ec@1ZdkTbMIOi~CQ8y{0R*?ijR{?Y~D;HGbI#9`VG@iI#Va=2Jg$wwl`U^8!Cu zeOJnf7|N{VG4TI;Oe|?P5mZ!-nnJCD2OsiAB)G6+vBxW(mDQ~XoXXd;`vu^0F37q}? zSR45u&CXh4BxI-7ryk7q(HUSVaeTWY{>lW89mq2WH3>3R8!_XD^v^cl->)ixYs!S1DJAw7{x;W}o^68J&I~+$s&n=uC^S&?|0= z%;?yUNS0n zG8Fwhb8$VCBR;BmPI9VL4Do$>oY+IRioOr);T6*6EMX1sxoGZ{1V{ROI1|$fmOf^a zk`4HB9~^yreV1Hm!t9&5H$L8m?cPbRv=uEO+q75fFhtNcah#2EeE51&XR(~Pq-E&oU%WjOG&h)Y$7yuMAX1EXf$yIQkXjS!eBBX#~nZwhx@_<>Dr@Sve ziN+}?)oz)W-KBCMFZnqai5cN9e=s1Oe1XJwsqGzp$F@oVQ!9m^TIdbC=4ayWn5nes zQi2lP5};CG@8peb!$*SKPb)@j`rfTkE{9YsC%)1u-#T!jo^v7{uKeXbx=Zx%yNl`_ z5^O3V|DaIyP$w;$UAJTWv_Dao=6KW43%(b*y*QDO2l0A^@a~aTfG>LlWddwKFe~qk z*6i1U0UvrK!c<|rzVH=#i&7NEIgXd_<83;sYs-=pp{viOwaEMYY%)br`D(D`L6uDF z5O(AA1yx4v$y|BunG{&RjR?$N9P08G1Q(9W=lX0^7vo{|0iI(`KcI~|{v*7dGaAT4 zP#z1rU#de`ay;c9iIqPk`u7_Ex<{0A}sX?~62M4^VJTu;y@YSz} ztaQl+RSazp$vyniD;x^vxor{7K!L-|M-#>E(6-CAuTA!%BdN<>zn_nllJJ4vi%4WW zM>fLL1<}>IFjr7OV;WMv>oS9fHH@9lrL*4FC-53G@B>^W(><=N_nmk`?4n1?0h+H; z&d8lu!>KdOie$V+4W8Z_ZWx;?y@|pnH{fR-RL3qHTDaGRw*ONS0GKj9`xy{Fjx4If;icJ z7e)ne8%Jx0LZ?W&w6lE`hJ|S6u46OGpps1?A( zx5NA$fTZ@`njfTiapCj8eZC;6ws$K^|DaUi{)2E~YOx=5rH?+uwHpRZ7uc5rNJ|QngsslxjgQAoOe68~mGHGLl ztRmO^GbE)eX%+c&>gOz$LfeLb^O6+$`;$}7G`Az$M5y{AyKBl7YGIZfNW7k`dU*|` z#p=-tP#;gKStidyyIA6QVLvQ1*#R9vgIp_dOAGq&z95b3;RafOVwsuCk2M)6^Rm`PuX4Syi*Sh@o&HZqcB zD6B!ao*k|+I0|XXqlQj&Zx(j_pG`xH95*w9l*j$4SLVeB+@s9T2!lfHNE#rQ;f~Lfkg1tTN6(6>$d9pxx;EP1Zj8yp> z%}lwIjke3E`s6>3(?gzDaCl{C#sm^YLoqX-x zA+iGh>-$HsA7qHO`*{Zpt}M1VpVH44|S<}GDYDN)JkOMtA%mO_2`CA zv-_jjIb6S=4VWSFYhH5Ehn&P0pddeqR;*3Z!V19pX+>E4TF`?b@TtH}{6hOLV zJyx^8qk5b(#4RuHkx@?-jM6fAtD*&po2T$F2$0sgcoiCmLuPdHBtYI5`#&pYgBbG% z4+7ktrWu9}t)abkaAueESvy8AN}A@0fmGMZ)?34JdEy+zwH{6mm@Cis&!8;bl|OJn z+jl4C!4NHG#Co`p*{GQz+hr59UeI|N1H}I2UuVwDsc+hJim6o@QRPI<2;sh>zp02g ztcs?dSm%RV5iLH@qDIcEt7T+)=a3P8Iqg!(y^hpy$**A`E8Sy5p68`suC3VUoUGec zz#GBIFutWaJEUZ0hMpOdE@dxcRsM+gs$w7_J!(2Q% zb*m5S@ACRC2H@R%wbNX3?NZ-_800*R{;3!hDE_O;sg=KMq(AN)?a>tHn~~^fisc#U zSVZ&vI~8_NJLd}h;PDaNu$}xJwGX$i6i-t&NjdnujpYQRtn6%B76rQ>8SmC+MnE|$ zepG-F2*Uk7a;4i&^KPN!Fm@d^nv^tRJb|-HUQBuMIM(#V~kqn2vSnp?pP2)vBhj$N3t+fzW3R+g6A-%OO6NPyQwk=nQ;%v;8eVfVUm&0 zX%F5Z3u!<4sZor#(dBqboaejnTqwdmf?M2hxG#;XULAX_M{}3x$Wj+a-c61lMT(i* zhFk{^ot2rpJ!u?2kCV4~L*n9f_TPjLiEJFGC^~KJU(PDtbt?_j;YlkYw3OZvi`Pl2 zVWTUS1}Wu|Y)95ItxPjSf`S#nJ-`W}4DNRu#S$l)X#EA&$grPzUb2zpm$97iF zRl1_87W@4sDn2^4r-9q+)eWsa0hrBY@l!uHYVbsc$el}RX+xL)?KA(xHei`XmE@FW zgo(V^e6{WnBy9`1ewkrQ&I}hiG@tC@EVQs3*Dp@nSznRPxKre$4S;;GfTuQyHd-D; zMd|P{Fc&#GEHxj?sid_O+q^=nL{uGIeyT>pCRMMTx{U5v;0UWZ8&}|9TbtyylEGtm(pZZsz%!^TwF|BEX%;42vk*B{W&MLycedE znESQomj5t#+Hxymt%ia>=^3KSP4a{|g{nEErc-m)M@&L%`xt~kuumi0ms`s1yEKi!@B*nlL zqm1QRJj^n9G>cwIvG34nQm5tMQ}>b6=h2yF8Amb3RZy0*LJ;*A+$|+0Z}*;n-Uaka zY{xms?35~OlCf^SciuXTxtGz=7)j-gT92}f?{Z|laQ!}xJANogrXl*fw}*+Fv<~o3u=dOO3Z^wEG0)_nJ2=@BMXze!iw%V-~D?Ve+-$C!RL@vK&3D-$>6@W(fbuV#EHQEH*5hod0dAVJ2eZ z;$r;o+y6`1VCVW@)c?3b$uI7%(19*gu_DI<7Kq7ypO!!FWPySb!~7tIiJupcFUCB} zX9Yn{p%&3Bk901Eqmn7XOrU>?I@)>vyzyeWkugo&)$qFVdFC^#*$vJcTa(s?h;UCvf&rcQjtak129yi+B-s0d{K$_IAgx#au*7oXmn&|9 zi>;sOBLER(GytgU>Z)G4asP%0xfLGFH}?lYI|=-a>>4bv4KT&RLJ4?y7lR=t$Hbe} z5fB_48-s!tF$lntJGmG%1Q3$VLg+{KlkoH)+xOQ6fP~7U{hc%ZP3`O70f+Jq#WBRW z@136u4FIth8vJ2SOFRUJ@K3OU&H|fuegqM{hJGY${2~m*+Phf?^#8u?oBEvjQh|>6 z%7G6VV5D-R+CYkMh2$jO{sADB@gbVUq=fV@-0usQ=b#OZM|uh++0KszAH10@0LY-D z0+Jc`cJJyL<%)kH!VQyb`^He+Rl)Bkv(4MH!NZ39KB!mjds7ngB+(Bx0w%pUHPxC& zBXRHD48lZ&{`$lLy*{Eh2oUvjCw5-?R)qes?8j&ZjtBsmm>L(K0{uY%2xgcT`K#vP z#P2qIL-c|volf-6f^mRoPXZqREyzs_&94liUd{pP|L`aX4)Jl*&#$Je&j^SO-Uq}& zn~`On^Do0N4xY_r+P~(C>-i&u=kqcG9xonl%6@@+3^18<^nK6!?bZbsMD&J+rkv(Q zeB&#rjQk^Y43~iXy>6gy}of`%4-0sZ!)`E;B7mf7p8{ODu-+C{MN?CAJJg!_#B zCN!XeJv#=b{hqJJjLr80gCBr6{Vvrc{&HUdfQWe(eEq5n3PSPD4|Jt4*TADi0GIe4 zNKlt4W&pK~9v;yCqB=&hy_>SI6=xm93KIG9Z28gnJO4Wp`4Ym3yqm8cT;mIo82bA4 zq!-5=3-W8qH1P*EU}+KcTnvJwTml6Y1l%9SH<;Ah4-T6MMA*na7!6=BO5~3^nsmO} z5(q${BHMs_hx}}2yFJ$~+ z-@n>Qckv~i=}I3fUUX1(p6ad(oXcT18&=^wj(mVxe=M(y-bIyZKgG8ew*gP88I>C} z-YiL-#K_BGH%hBMS_M$`KlniAvLb3{OUgdrI858*T0OMqZnnbTrSaG>cTc;}@@1op z5gi~Y-!)wFfA($+^IvD=UQc@NCeXL!)B#(yOIfoca@O_ID9bo1l5)tIq}ni~$ns&= z)x0t?`=@lnVKLK*1yo`Kxwz^^`i!6o03z9LUMlekn29_sdDMD3eOjf_YLb^iF@0-K zaylB=)2>-2v7W!4IHz%3HBV9P!zRW})7(M&D_1;+LP?CmuhAl+Dcr% z=o%^7j7l>!&1b9)~z@8g)^Sr@l}c<+UWFx4sC$P<(d{<84XX z2e}vdwkby3BK@C|B^Qq%dUhsjaBrZa)W7<}hmGh~|F5@hpNsZ|qjB5vLkDrFB$Cb0 z+uZCmsy@)eC)0v`j!d@Zm?KW38cXF5j>J;jf(2T|t>xNtLvlR2DD5lTs?FT;-sk17 zt1FT24+luUGmFBr#VuW{MDG$VG171Svqo5{gmT~vn3_~``-fJuJAUJ_fLDh54szWS z8qf=V5D3AbpinVQ!s+L1i*2s$jz>HqEDb7^E;+xLu3~VzWNb@NLsmrNQ{<4y1f{!a zk&RxMK*MWzYRH$AcY!~sh86=CSi$)Zr2!%JQbF&H2vhGC5SFv$p<{XNOT58QL^Hwk zH0-P!j8vXdJ#!nwgi?HO921_NI5D~tB5?|;HK(8L&(ZKA=Q1PN~B z{%cczLGCFcf-)ViQ4ATo5xEg%caWM$?(H5|{ub(3bU8K1La$O=4t=CEOuLaH5VGa8 zk&AqNt2CkKjqBQI4j79qKjC*8l9MmL5b)5-Szn5rz}~-dQIb?oVu-P$-alXOK=L@B z3w<%CqG2vl$&>`YKNJEY&`_uic@SxwNwijOkZkNr^;=Ysj-I;u;uJopG0ocL;?{)4 zn$VnaWlHlkPFsHr^jJ2YSOGk554C21W$;N{Z;wZ|I`Cay+ZSNOi#$sRC~aA3)XAzm zAqOoFd(?AP73d|Sa4V5TbQW_vrDyg9gLxxW_uxMKL>}eY&dqBW2R00!#V8>;ddYP3 z8G0R@;?sPFxAO{<1oEokCisT$Xd*-QEjSi%d>o-p?wZc*8y+n|Wjt-Yt5G$=qBJ$K z-9GUWF~;rZZWn8?W@*nO>ZBlV{%wG%lnNOXxFVgv9@mir&$y1X=%ZD?$I_lKumed; zN*DW8?i=|Fd|kbaI9={!C+BzP1SgZWOQ+^&fmiM>f8AUN3@kA^yzd2dQ^f=1b@fiM zuOeG$NP)@PZDq9H-GEQ%>!*_!Xf988!b+&hoZ+1D9<7tkN?im}55{8pWRZk@0+MI@ z&p+)vwPci?1c38RX>G3c|0feOV#v>{m4U!T{TUSlyuO3myUy&Pv;)EnhwaEc63&quIa znki#B$o=-oA%yZ9HL!1dB2OrnOCtn=M-VrYh);ca#kWyBp!5`FdjT7`q z%+(jk{WM;1F`nQMgc%h~xdapE{coeNRbS>3$1mR#Nz zx04fEZhD)?2_C)_p(8)V_(tSUeGg`OmEqv~QLh{%cByP09_@~z7nhV8(Y4f6OTbOG zxalICUC={nYV(s=kJ!aC>GQ8&v~^Bv>^C*S{t`9<#U;%sl0&{57Xi#oA{<}@W_i6) zD%ED2?YHE=lT?4FCqRIoBddjJH(iul`|waOlN5IO)XeJAdv(R%>{X&+n(#JKMHdw1 zrdaV!@Th>Xi|SCT;$FKK6izHK(Cu!!$df6dQEM4)DR=k^bOg*f#-B2e=mx{W@k@P2 z+t}iwngx=LN#ER&oy4M)-^TKbqV>7jGwW_DLv0Z1KA-cc)pOCz^o?t%C6%+7-Cm8$ zrxaIzxG*nna#$>CQe5%UWO*Rb;F8+K3DsDV@Pa;#?v7vAnJXsWW(Qu*4)!|yY<-5i zg-FQ;BFLYjX}Iwl?UvFLE+H;vG5#^9-Z{(PqqTu2PRXM?jVRACDLg?9 z|8-l?uzmJ~G&qiHGldUScQRnBJ2CH#QQCn2$X>*88L&QIF*|E%`tyidWjlKj5Y~gL z8-%S#*LI>haa7G1GGpYZYEi4w(_rsiO(L8>J^(aYpby2*q_m1Dq?w!8KxdmRmei1- zP%F)HCN3&c^euowh-q%Hy*-t^BU+92*HPJD&2^W&heWK3x9DbQ-EayiZX&rv)lz>< zy&0H(-&hXheCYDFeU*c%n!zWD{EL$3uxgD7#{|^cULPHF%c!_|f{)`W?RjT2W>}Wz zR)xMr3l^sE3sR*08PxO0@WQ}Man~AtZGtBLme7pOeMt2H=0muQgx`se&Dn=kW3ZYD zi<6Pu?LlV-936skl%?6k&2U1SR{51nJR37@A8TO{g>{2(w{z@l%b+BE~EMdKxo-yPF{;Bt% z9L!H`jt%pQ7SlNvxMTi4^Xj8nbbC;An7dBfZk_p%2 z-Cj0+OU2*GHGU%Tt{_)AH}<8K;L}?8GaR;2ZPl`JX}MZ5aEjr+QYHPy_iAJCr^F3R zt2de4nL`lj_UWepn*!QQYc>6Z5@n=DVVOcF!7Ae$=?$5I(c?b3pXe5^%je_IMPyA; z&vkoCB;7>BGjb9Mp!-x829c#X(agX{%ZCLYNkfOD`JY9gZf0~SyWVnH4UigzJ#8clBsuFv1JSe<ZuleB*ZX^e_sx z1af`Vp^^Lo%zV*6?MkajCk#RN2-#joGYyx;nF2Us06yc}%J~<>Fw|{yO5@3JrY<+q zbCw}2;AJy=4k+`IXI$;}L($b0te6>Dj((G*9*#eyrqC@iE}`fsk=GFY;?NiD3HW0O zi-jh_k;>--Dkm}_n!yLg@&wQDU^=|12wqnCO#iON?mBfEUS zLXH1uLc6)9$Nge^A*3U97Nnqz9p4QNVpFJsp#(%t*ZFg+Lf7{frdur~J-AmS><~KN zDvq5?y2{R@YUmju$EjcVu&?`v`wPptBPhXp5~BG=HJ1HPPo9_%liGxw&zi&$tQ2p} z@w2m1`{-IzogYaSqLmlss>-PICX+H0e{ z2%A?MmwG?(k{G>+zrWeeAjXoviW9aL7SX($e?qNu#=*M?E3;O(%iKxKTnkFfZ{MQg zilfb?8RL931VPcR|*_sV&q#Hr3k4}2#xBFp!JU4pCJiTH+q!)VRMw~5*y4D{n!WnfRr&6u42@&-qZ2zc-yqv#_iVea!D^SO_ zf0MlAx^n3qL%z^J2I$yVKmPR;EU49{j(HMbFWG6b5qULGGGm2cSn|fq*U;g9g zsh;>ZyH`fozIEM?=2QdyoIZIU+!fsh3+UH&CBd3;WZl8_%~oeaKK`SzW89)a8rc)6 zN_tf^`D5ODfh(0wx(8Mah3{*TuRJCGQ8;*p3C`kp{$sf}i7*a8XKd%h9rdm)?muP( zL&SBvg3y#4fhhKsU($l_ZuLV}67lI+uh4)Q?CV+MogMSj?7e^~j>di3MNN>x>(1^+ zvPskheD(^H}Lu+(J zpxaSEy(l?t={$v=#-k~Z;t4&IJ!qe?+|VVvf{@gY_?^vO znE5@ovPWkoOE9|3MZa;0aI&$NcZpq&U(8pY_dB<@Xq;BQ=a$Q{MjRm}(YJC@;w*es z$maW4aS5@!ppLeG39VAItd5C;GkY&QG@`|pn|6{(11~?5Q;>8N)MGRhso9#vX@GFP z=II%YthW1QlFLJmj^k~TKl5yX6EW}98DcJ^R^W5S0p4|Hzx%9}P8_>?W|N@I0Im_k zdAEidkuXgIJJ0&{CJ<}Mglv-8w1_s3=7qOEV40?G`pG@^te><1>Ca{XoHXJ1TlsI zEf`r?c&#Lvj0jIz7h8olj zq{gN1O~(h88{*JiaPLX@n-KYZN;&F6n>aJ2Jvh09+4iJ}EEQqVb}XdpR?YX3CFs4J z^Zh`KCuS;8L4$a@hy?IZBz0Nf@8vrv@UG*2n~3A~HYVEJj2=Q~Dj;!_Y3|S}P1$&3 zoWh{j(X}J)k;d51%s0Tf=jWL5I%K%^w)KAQogmH6R{lw1boeA!_4Dc?wc9I5^u5ry zNEFNThQ^!fH^U?O@g{D!YA**$>J@oXMKQx}fb^8Vh0&54( zM3uMua&f&1)%p_(o=^2NtqU0?J80bW@x~4w8u@9(BCsubI)?ye2kkl z{Ilw@nK77?Jfl#e;hSj&D_VA0d3 zKP|4>)S+xFA~-4!ulF*rwQP>CrVpbRMjAkFs=O!;l&O|jm7>^OU?j5z$>^mrr&Zv8 ztoHpINR~c#`&=gS=w3I@DPAyUQKoWoN2u4hlrF3f1eY0ulYgogVe_s|X1srk3aX$O z%n*!TX_Ok7-N-d%A{jPW75^eo-G}`f_-r(t+mY2$OK#q1*HgK3W~r+`fFy%yqoHh@ zJ(arGg12^itG%d}orvQU)lqtEXT5O^j#3f{&or}6zI%abYLZD<(?iO&KHgWsD35R8 zgtPqlpkUcQqTz!Le0*9H&GK@JeND8~=H^3p1s8r)=No%3=1{j{fU1X!&t)h)cVHkYUIx(F80=zGtNQMmR)^p{DNh@N{%` z;itX5)V9Xm6fAtwkNuqOE*z1hQ_O3^czw?>)YUZJr9vHVdDMGd{fDsm*3@ zra^i~nnJ$Df(usg>bex`OF`$w$#aQE7d7fC_Ne8^4MIe`!Her&z3E7N+@m$pb5Y9T zPa<-dV&vCA#|n;qxzzPxO%72|;bG-rx$vArsBm^iF$9`V?JIG9&6LBLEt+x(Rp(wa z>tHrV0Xw)+x<=C~aRmRMq0t?w1lT)@h=rCoglJ1E2DV8k;Mp%YH)Q-W`LUHHP4nziP@^7i(&kWWl?Lc|*n>JoXE1NpQ$bT^)oAMM{0N67eIZ~rE#@i;Gz79bO{tBg1gPN~kj z7EAG}%|I$@9(OQl;APVUI-B(E?ElFw`SEjgoSle3h5I`PW#L*IP?hgw^+_nzZpHkc z#5vCYfjGzhf2F$rUz}rRCi*`ImO<3Q+S$aBh(Xlaz}ZCD#K_Lr1eT8v*2&q?#J~pD zeIv#hLOJ>7nSPrprZ@-`WpH|)gm^oqdGB8Wei$BJ$c>boSckg88JgthHl1Xda~L%2 z#~A0yhu_Ig%e79^3dipIrP+|UTFSU(a$_|A6@ zV!T6e!Ja%cXg-L*<{%!^FF|5N052IDqL9~D5qMfK9nRH1%vWbu*MktA&PxIn?mxUI zz>RYQBOlHwpkPa1)n5+`jFmXHo$sX`%vrq45&s)s4fgczM!sri%2P@RB&#aBC z%7_MN;`%Kof}ei`F7!cM^-a75^}&k^NGJTbarA5QLk}eNr4-7SFD^m_--i)f161!L zGWw?$a`SKe+b{qG)IUiC@wDU9y~AV0j!hg#}}V{VBgva{sRCSPBeu2;PrlFvhRx60D~33mGmjG z0bW2UQS@^^V7_j*Z#P%}&?H2d)BP*#>)kg1CsI-rQncHT;>%5172^!#_81Hf?1cb2 z2t*%bT1Z(Mu=hU4yz%2@m;Lwmgo?nwT`tm>a}-yppUdU58bI4`W&q*Ej82A3w1p9{ z>L+Xqi2^CWgmnAew&Yj#v5WRqHTf-d_`MxpSQRvM!?=3$@{JtIIf&2mjlx*yB4Pxe zA27TIxZ710S|ePv?62Odjs3<~5fEY&jTczcAD)K7_zMN|Ij~P3X^)hi!j6Z~@=2P( z|8k(G>f}$VzaftKQfCK80SEq8hc&7v?==k^f+(8WCB_=%``%Im0TI#sywC8jhXw=+ z3h+q$#^Qp3h6C#!z!K6N;PIuw1{{hJ{Yggyn2PH8v!RIGUuX^xAp_=70^dTuAb$h$ zN%8j2?{QzoROQvsV2wIYnWUhV%&-sV0`lP~I4aYpILxyh7lUe(kOb1csCx zn>mc;AP<_)&N1}ZEW<3OjAonUX{PL)%l-Mfr?7Tj2#YYgd5;fUADixfH9o>)O%*(e zyad^;+IcvLV`o&QAuF1xH=k@mpZs*kPtB5(xs$#j$C3SlN4G)MX3(1IvUb(r@TfYmE5=f09d6+F;@1 zZmAN4QKTz!TtvzY6*c>A?A2{bF?O3Bd7iq3A0|}PGIu5! zwt2A@kkm*3n8Xa8-VDWlIRe(Bt;nRY($Xih#ge>jitOA9R;1CE`_~T=NJS_{SI;wi#fc2ogg_`xp(>yizFKrN zrcAfZvn6j{>+HbL>Z`<`%(-8z2<8u7m7ZomjG(u737JHwVk8-NYPP2#Gh@qOd@QJ% zA1v=3b@5Dk#U zgcm2q$arO@%x7QAJvFeaOGVLEXM(j?+H%zAFTiEEgOlH`WjXV-X|ozj_QN*39ZqB? zUK@9{1F#xNzALrrf#V{}qv#YtAB$OAlReA3ayi5j(iICe?KVjKsJpQ-H1%F_N>821 zGM5@SCR*FP^bE;p&{tzvU(jNz&X>$xrOgVJg-9`a-tsGUhZJ1v@ofIb5+NwdF7=88 zt_%#wORRc5tv!A6IGOC1*t;VRi}z0+J?{%ZWGQohfBp#gH+fFrMP<{04xNL_ufUGr z>RZIqiiz=fzpF=>u6W0yRxNtu-GG3fm_v}l&0e}|IH}cq$Sg|hMEd&%O^66A42s`} zEOr94)NtDtu;6`~r|l>11YTAoEkbgjLZi)*njPNEAFE zb$7l{jd4vR3}!++-fU(xT}^FeP>BV$r5DErVFe#zZuYZv`R=y!z{>}>#ab2$AYrKx z)j9-2rYxL~VR^734~86O=6LOU!%iYvsC@4}b-08NhzI!gGJd*1IOn>r1cyQW^F4QB ztn!5sNlAubyM7!aU!95wC}~dI8zhyc?q%4%NF4$S{?#2}Pz{pSEMIWE8F~4_xMFPA zE~@7@z5H9^pxPI7`skS)#)1)vMSNnmay`$Bt4Oo#fF9`amC+jryj36Ikrv84=A3p8 zvzx^W7g;nuJxjBOmUfAWu6aNTA^fg3JzLTtunO${^F?DpR@cXkN@A5rH%UO=pz;gX1I^cChd~%5~~2 z)reunM|E#ur6>!JVNG)2plTO0`UZdetGCyDgy|1-P{=tfdF2Qh5pGku=KHyH2}X= zq6mrv7D&dfk<_|@7s1&9G*rp@3Hh{j7Q6-?X6foGl?utvh{<58hP00WuND?sg0*B< zLE-1pONjU0zEsR>Jj*Kdq?8nk1Lok&9_JaU{_1G{An3oGX zGFkQ}rFR8PD$k>YD>jWWVfBtZgXO2{!S(R+XRC5s9BO>9sCU>S{rh*v8hqiGmtl6T z6}sc`^AI$F8n)$J?y`^y2<31ZSvMun@C+)qb+C47hE|rOqRZ+G2TYWt1QyIX`EHVO zP&g~QQGVyxOC-H#@Et1{>c#BK(2FIdWqA++IcI zsf~z|?sTC;kSf6*P{mbXNZZp~=O$Us`}i*hocvak?kE??cQ5K~+kN-92c6Up9y+DK zz+TFALv_rm82hB)^lXDZ_0NwX#Oq&DC1%?ojKqf_V`l+0?@1orr(PxX6Jx}K#wB2G^fQ*boyGx2TN{sR2-CbE&KcVEwk@keM__3cL9`R1=6rf!&jk&X7R+MK)_(7IUd9dej|_+9;c6GNdGPu$YB}aV31kU z2iQ-Zt={4y$U|A@LCY}w zwSJnwU!nS?+I#*i0mP_qsVQ^2b`jlj;7*$+jw#U9r~y72dU1WhW=U7(cz{`a&*&0T zu$YgwwSA{kr{?zer9G$8lw^xnnkcG#E?oJ91*~Yrvsi!Fmm$F)9nwYpET*+qkt2~) zq6y&Ne|pxMwTV-zg;0G6)4S@mJG1xQ6+%}aC{adBE?uIP4qzZw^6UK6C{bT@*qQss zOgU}QxTQdD|Gf}gW8$Xkr_9Z#S0oPBq&KBdvRYu-ynd20l-n&Og0~u2Y``l&j z9#goqViMz!GAm>b0V6Gbr{us=iBbB&?9sj;Jm(R@%LQXqn0A{i(oeVslis5%rUU31 zzPfn**VIT>hMLgG4qjdjdkSL~7Rkz_XO^_d0XbYZxyy(0de5zsh*$zh6$Q51_(0}p zmZS1?{Y(y-5gPCsDMY0&YHEWpMCpIZkchR>_tbh94w*IoXsN=MMbLu-S9lK-0cNaHp z@tHAQ%o9^gDHR!F65BTA#!F@!QpR_X$+M$uT(EC%!_G;s99@+yGc?~ z+hicXGZMU;;sc=zu5NU)+NT7UV!n-Yy&2XjNRp!uHe;Y)kD?o$PMdr`kTAs=3&8O)F$=|?9 zC5gRxzt3xsDhE||L^xXEMmZk5Y0O%C-O3QXmmGI$aIxM>S8}XMo($M3#L0d=L>`)^ z=s@aIm0N|Z_9pbLx`T;h%fBm07$Fx)nKW1FO;z&jwZEVdNA^g+WEWdagA(C)%(Kp@ zpIP+H@P)STdi2YXiBLl>#~>fhzY;g+3S|)-V^W$wlj)xkyi`nTywtAyD11cYIA0#p z@4~b#YV+&?&bnfBo3XzPUU{UnP(9z#V9+B#!PSz+HA4=kfS*3|7L`n9OSRvg93v4h z!Xod*HQ29n983}D31_YnyNvZ-%_60Peo|)~eLx(KSH`m2yK!AcuvrFS_83{^1or7~ z8zTOEJ?x|ZfIim6oT2rZU2sojDmi-SJ>R#~cXG6HTPrO4J3Wi>vPbtCKyk~mm|*j- zo^rl5sK$$=s8{sLQ74OwPD)iX&jWauKngGRYGOq_3&~FQI7J#Z<`%jx-TS!K7AAi8 zM}ir-SNW_&Rp($aW&e+_^4Rgl{bjyuT9;j^&jP{?3@sCI`-r@26)9?dDYd;ZER_F2 zfjmQPe;H-h;vIcF{MX1bxz5@1VZ^F3hOEGhL`ncZ$C0g@Nv)@hTbnhSTqGjltC(C{ z(3Yw*woKm2YQ5+;PuBHe?7qzpV2Aq``SuONB1;9xs62y33HC%5_(E#bXsR$G~k-U#W(-ziDM!KPz#`kGG& z*Ddu7Mp6~xPnUz}HaE`Kj3ABZo<6Rk!OP4^v7^$PLQXBLG&{6VUwJXxs$@pCNh{-A z05g)RvVP-)9;5vM{@W72QVJO5M-) z3tTdtdT^`rwA|W#fxXgYT<2zuOx3yts#5X#J%yaso<2QK?M2FPBaL?{42d`IUskLV z?9EXJ9{I$zwaw+Nh${96Y6VlxZk;a*!#F1-0v<9-)u_7a1#N*rGzKfcE0)Y-?ReeS8|+FKu0}m#L>B0c(uA~Z9)@n2Sf7>Bh3}OzC#vG0!djqHxOLdvtHDbcVb0ns z0~N}|rX1{~lD<}D)5TNwqsAtDA(H^214SL{XzW8-m550&-#MkF8MlhDsTF*WNA;#= z!j=G*eh;bu=g?+6+Abc#SLV{f%;FxOGTzdShtd3J)00m3U}XX0H951MkFcc%dspWl z$+lL0*Cq!N+t#%!{rH0<;tqI`H84eRz%iI=oI4X7%a#M8Uc%COt^K$mSnrfe; zi!X~UBEMY~;a%zXr?Gn}O)=T4ET>)U7#U2M_II3)olk__k%O+$FQ|!U7S2t}3z1sa z`T(;?7zBhPiry(Qmq415YelQ#o1?E+G)y))?rX3i)c|Kl@GV0lj6r#wYgBAS$+wBt zR6gfXug=~}<)={x^xd@coU#al6v%siDNculT<)tR<_7|>SUYEBdX^1~I}6yf>;ofQ zWol}+I%78SmP>7P3X%Jls+o_|aRiQwR=uOTHC}Q(Lx*ve%9Wtnm;Bn?EWQFP>Fs}< zT5MgE5q1QRZ6oDS`WYS@ZdQ?!Dy9p_6)osxS(o$J1kV$S#%+TAAH|tP2W3dlIz+G2 zl$&F?%=3JCu08jE#W20!nB$lGwr`Z|dxAqxyZkHa3BoXKx=@E41L>>O>HmWDuJ_8| z2ZC+oLI<9_*^LC_lsibNeuDpc5}}HOg|p-Kp2F)R_OGrno7$(NJZrR%JubGcTP&=f zz`Fe4?5x1o74jaL78Xt&qPCJxe7zee)B)X&i5dp9QypaK7^pS?GU1aiEyWTiqJzz3 zWQI)LQBsr_Qm%;l{p7C9b56cE@@-sNTDLWufagl1`U@By9jI<_mRfH~zv@gVTw0Kap!QHF&F$Y)KZPY@+9Qj2#{Yvhaqq7?NN@mcv43?M>`D1uev-=wQZ=84$VYUFG`P3}3h za~`GX@2<;jxFfnQnf}bo8dk9`-D2k5ud8wKWyDz^&pNAd?cW zvwV1e4w38SaVopCJQAct_H<%Mjun=4Bw{I!tRE{;<6XHQiYz&`x` zuwzW(m#AEAe|jPG7EEkvk>O115W@rcTVl%(=T(;rXft3;WxZ!tY?y;SFSE0ehu6;7 zXp9Ej!NnwqEmWMq{)+;-d36#5gm~?N91IvXm!>k_8f7rRPc*aHY zVqAGq4M-M7`+AF#jjhNThsX!oZ8c4t^untl?$g0PermJA;;snhhx<(d7n2Md)xl@qYcEV}GQlVCjN8;Hj77axWn8{MV}c9>R=E?1E?*Xe*BHO-jxPbH0?(jIpCkW^v2$$FG-}jq z+t##gb9&mgZQC}Uwr$(CZQFKF+g7HM%E?L9Ta|pf_fNR@+SgiMHh9!BpJuTRIq!d4 zeU|8NL}>odJcStNdpYJfV+K>9QBjHe*~ze$-wJUycr{ba?ey#R3l7qrzh#el=(GI1 zl!?bC6a*kw3#-TgEH&|JxOOkhCq$#BWT;wdbVM6x|`XI;)kr3A{qJR4$qWSX@WMJ;JmsO|wk zU5{3v|AD|UG5#k4$I8m|zXx&5guhw-Pm=n-6F7DjHunE-0_Os$oT#-(uMK5<5^2A0;UjGkVo15F2-}p_49VEbFuSO zll-zh%fjMiI?KYvjVtOVth{dyIYh?_LqQAyG6}j2f2}?a(ytGVgh`9Ax6dSUpp6XZ z2f}v(gNPI^Jo-uX=Y#+r-e-cRM6fgS69snR(eXD#;b*e&V8ZfX#H2)knEbK{8yN#- z%hMJAGY15=BvOc^4iw9|vHdI1*?C~U_3Z<73=tJ5VtjlY>cK6#Y zrwZfk?5d`vJT#5Q6BC^U?l{qO)1M*h!}J}%C8ejzuo z^Vs@t0#JiJJu^@=(C{ycExnyy5F&-%?0iPdjcL4ydyxWM|ChKLsJwkTAEX&b3b>$x zy@uibhgeLUMD)AHhoazQa8dgSg< zw}>ZUeZa+ijtdJ3j{HH8Nc*5pC|~G1ORIQynP?y2hcr-+ZepFFH~W$NV8k~>w+w!K z80W!22aGvk{69Z3pI+cdsGtM!jJR{a`(eVs-{CQ#$S>bjchjSUK7L4yx?YC-x3#By zdM)lA{qvM+yMQUci<X`DF$fwV z25i&0GcdmP{`vIo*S>v$#HgLzMFZjE{n=4N1s56wOsBUcu_8tE^ZNkncV!LTeLepg zL?p%Cf6yA|S3rV^9_$3VV}k&>$!GG@vq4V`)cfV&xfQaZIQpj2R z0XlH+TSO0a9<}>qCx<|D8GS*9AJqeTbsBU#{Pj@#sh@)hm-9(S;(ZUO1?a&*1tEe! zq~!_>q77tU3~0q#DrFXz&+)w$?9Lg)DT)7s#!+WOx-2c!-33 ziwdLPzOrbc(tnh#qRlv;b^J+<5$NaOqYR=>@HcfZDU#uHy)`e)hm(72( zzG(B$xVB7OUoUQxS&Mn+5ou z#~f`BKk<>4d1!elFG1x%zIa1Ut=u@G@5SpMe7|;nDI%juVV@_F?o?J$eyAjsDSH3A z)GdBq)Op(9cnN&Id*uy#b1OX>(1q3t#whi?`HB>FeHGmdG-{d_re{c8T_K}9ZpJMIL85Px0Uz;kgu4W7I#i_BqarYI#5a(^9QzTo{{Y%2!gm`_7d$O3Z8Fxt4mtIw^*=`2eGOtqFAwI_Cj?GO zRh_A|p*TtL66w*NFsPKHgq_etF+`@+vLuR7aWW6gd-*#|{torwtS1pqo$57>-}?-{ zJ`^R}Ubqno0X=XnmQEhCKC+?xWGU9wL-<=OSA}Ai?R7l&!3PN;14D?#x~a{9jYD9l z;hc$#xif_+18;(SNSnX1Y$hdGe+@W8=VnqZ_OHzO4X5Vv3 z9q;1rqK`b9eUZAwZgTjES;)0jP~^U!&gf^l<+b6!k7-+QI#1$v1I;p*+~Vy4?M_#W zlg_C$RVFDV2W{|O8Jq*&*XnEf`XAw=KOrPGUJ@b8s3q+2Ci3C2L+ec#91I~JyFYX8 znw&IKTBk$v>;$UVsC?}GT~~LD2@sxAr*YiQmT^P; zLfW1QPtMJ8>#Y#kZ-Tt+hEXKcIT=bu;AQ+5BTq)`m65S|WQ&4$eddRe$K6c(%qZqM z7)2G$ohYMgm!H>vj9si;IGDGO2!pyiEKh?s5$p_gbaJm!%ZQUi)c-#Ds`sC>Q0=T( zEo7bYnGx9Hn@_3YWd%ecoRV8r_weQKUuWWImrZ=-_vc;m*g0Y>5Ln}Z`=tMK3$>UP zFG6WG5Hi@K&Aursc`9dI1iPt)8(n-AYG3z(pRwT4N9JGW#DLm)9_U zN7ATr741+HbZ(Mm5%QQ)`AplSwi9zb41 zcw07inO-f!_Ks*}Rl(9_!OgysPm>Arc9PuSx`siS$!bFZYFptEO(Y`mkM)QYFdmo< z57FJ|jHAEyX9#Ns?S*c*<)ZESwP>nhInHfS$weq!Y+~shKpfwjt<`ql;D1Kc^oW@u zJ%v>=6jqD02!2{SQk=nsFO0OZtY@%8vD}tSohi_&XOQf%Lx`fg&S;lUj>viGt^RV% zLov8LR5Z)|=nk}zuDo>NSEOr*oichAW6)^3K_yCrKW4KSmSl5DPXuYwx8jsy@$Q>N z;`HQAJ#d6Mojv+BVou@?cJ+Wt;xk{zZ??Smiq|M>wj1x0eU8~zeo`ltT z7z!4M3V5R)MN~XL}L5g!Fqit$M&?RyWAZgzRCMsXTq) z(za`%QbQK#S^Oyozkc!9I;F0^M7MZZ^4$8=hk5xPXFqa7*_S0{0N!X2xb8$!#y{g z)XSnLW2BX!RsCDoPUt=DOp}d(h?4vp>V`74eKy##25qE4L5jRL+qU@>C5V#yyRD~? zhq~MSMBPKx*i&%~qm8p2@0cs$vSTVD#w#VO=#{CaJeuFEU0;A|5tHXPh6*+i!#wxd z#zCYWvUE+IOBWYz+5HlfbOxrjDYomGrKL%DmC*u6Jhwfcfnl>liL9`Z0{M@-Wm1`| zZtM;A=xKT`)Jf2B6lF38QQ0)kU4C@XlTed?%Re+~?wt>8pZ7+m z+LNX;?Ie4Q*Z91`yB~RbaWG{mwaFC;V$S7c9yL)p7LU`2Ag&L`z#*ZZl?exzjg(Tq zI9_ftTqHxrOy))PsEF2|a4mRXJIyU>8}L*vN7+u&!&&F=uTT6Bq98C(bNiu4ekIpp z-@oksjc?chEp~buM1Eje3{M(C^RV;{+6T4i%E>!pL$E{RFzzk06!HoC9&$g_ z7+B&Zn*3Pb*=gW`P-1&o)dIRhVqIAk7LAYy^mNplm47aZFx%`K$+xp8e6q3Yv{zk3 zHavt0Emh46H;Wd_np~hxsvc9T8UDS*rEx#vzRTR%VvZ!9`8AKZF)_5rk##|`#6uLcFoLV z>l4P#Z{X;VkOa|r%oE%Sw#?~sN7shL$h*Q;FD>)4r#?4o(uP890%z=crOhq{% z0yzp9A+1K^zm{}ft-L*{uO8G2nJ@gwBhC<>)f+SHI^XyktI}g$;sy!K4%L4pjVZ_% zSkW985~w5q;**xMW5L`UFAa*F2IY*6E}I;ym8-`U-a%&*1NKDo&AD*(_anbSfnm{T z=Iy2jNGrtdqvL~s(c&jN=HF;CIKQIh3&=)I55gqM_#xTgXMfFaH_KtK=4`{^wvu;m z+2?I#o}`-*Y*2p(zez6 z6m6|ie33E_Y_Y<|F?JyreBvvqY0`yviW=Wk-zm}N&OqmzcrcfqbzvYGS=kD(ET8{V z4c;-&K*138$`s(HS1i}&7tnaI$9+PLKK;iOrwR_Y@l<2 zoCtP;@MQT?s?ws{&Ij3>0u${3sbN{Wvl>J`G3PlRUcvH+ds0d^2NoYXE-{%Eu>K{~ z`zau8kN`>M6?A%MvDnkyHa@(5t7AMF0Al}!R<&^KK7@#InI2|Jzi~MRa7DrK_0Os|8kE7@K9mp3lN>Qs5AJi+ z4@}Jlthk`?3)rcNT_2Wo&Pn9DNSc5RP#;Lik_H1PV)NTi_9cxMLOyyJGl0qPc&fI1 zZ9=Kzl8Ul+Y*!1jH?}|D+W)~bzmgwE!Jj_7>2L$VDVqD*+?%}0uv$?`le6b*Xrz+( zGraL3vWc^>M>d#_oG1I!%#;y5{)^S%1sc>sc$9#xB zL$9{Kls(b4K1Q~NuIwHxZ5&m_EjD%8XU>XX8h-nwWT+4C9Zx zX_x}|gHDoDJNL;Bg=GEUms&rs)?9n9Mo5|`;+=!Fm`} zx)U0m**fJb6{Z7s*rNSu{a~;GhC0dnj)z`BD&ERbK2>{w191sbmcZYa`Y;X#UKd+D_AT)=8TJ23*a@{C`ZeJj%6%rP@ zzn0ATWZ4L|JE8uyX~~D5b}%#0ARM6DXO7dP5xPJF<_bz0`p+^IM)>5ACw!3*GR53j zRE>3{mm?DbTQ9HqK%bgcPVubqFpT)w>L{9)Xe zsR#VYvxt&ZzVRX-zX#}jkl(it=-_vq5vSm3ha{+=T+~!hZ@{?Kc|?RSzy`Gl~1W>l@5+YV3&t$%mcN3yzsahPZ%vDag$Y?E1fb#db|T19Q!qCg3j zf|=HwV8L`hv=mz!7G?JyXz1KE6$!8sfI$Eq9L`>=00zoqoEmdzIIUNnif0n*;Q>_4DyM zy%a$3iXz#X%@*YQAj4oZc7}c!e=G6O)N__vxsZA(x~zwAeOF6D{LX^cW?w!e#Fbgh z{JL^pW1DYi4s5R3!ZqBym}l+H^uiID7sY!^5d07oz2l3W zBSg!6-O?BkR3+k(a()7sbfVZ0xKqA=k7Ox^B>H5IG#_7fT>bH^B0;P@N6jfMJ@1fQ&X&L>pisB)i7o9fR48d z1L$5kHl1fnIk)117WgnBR-Ws-)XH1yi*p%va^%%JbpSg{P(eBU7>QNwHEbdTS_%zp zT&+tCN19u=^=nn(?qPW|*>3VB>_nL)cv1Q%- zLF=>>LPd3F^sq5lx|;F;?dG5P?`C+HTGi83HICqQ5qLXZSwLRwYR?NBChSw@BwimrYwD&_| zBo`h=IR%VvB?pXr1NlX?zu>XU3WZJdVI|t@0IxymAJ{g_3T;>K3G`e(8|^fiFlU;B4I#;7wbZr;^`;o|U=;NY?;)-Q# z%`VBtkHGwIOt#SPE%E;8;#R2MD$yhSTzTc)S4J?g%^r6;TrUT@)kAX(QU9X0p{Atx z+LMw#Lbk{Ri-HDjO9cHG385F-8smYFAz%#?};xAeFfZa`+c9r2hiZP=7VrMtELhnrDYz;hj z+v37+sWEuPH_s}xZtNcf;T>%Hyrf8`DFLB|P4%(DYn4aeB9)*|IAU)Xd^O+l3BO%F zSP!mmIk)+_SZ65;$oE2Ri=7?z~0Va#tF{ zazPtONk$Yb8Ao!(p%_8VVVKq%G&Y%H4Z~d}U_{(11SKU`6J2;WCcHg)4A4s7Ql7Ic zQbGR8U-ur8{ncJA(PSFe{#5f;t$c2PD@GCLz#NI)&;2U#jo!v`N$dH_F}o<8AB6&D!Y$BeI8-XPo-@bCyE zC^idbh|u37rzz9w>2Z>)!M_A8VZAOA5~HbmtPJrdPB8_as$Bj>;2#`p(h%~bV>ax{ou zEbi=5p`cxe*j+RN8%jwoCcjOpoyUO6C#$sa87f&a=G(S>Am(=@T^)S2FK>Fv4Zx8y z^IN_0DvU(Yv9Od(0jeyLM3*$8dlYJ0``LD?+8-)!V_zyQ$WI?~HIr=yIL^6$NO`U7 z_>bnc)*jA-9pVvg-y?It2gB%CG>Wy;7mOfVi7OMKvb6ihl(sF)QOo_@G>3^|ZDe`VI2&Mso8~+KKGXF2xB*6L$j8 z1kSo8Zt#elDqRlZ--vfGC0&RfbV^|2k6y`}!B}-Tv{tneJjw zmvCI!@XGC;(fGJ^OizO@7mSmYry~WSAtk4!MwDk~ZA<|Ti;#qhjEG2lxX&1@v;V8d z$WaT97VS@jeE&nkj|UHK;E)CZ6&cAQObq0yYwwo^#xJd@D=n(4j7CULP5t6Vh(d=r z2jnqG2qa|ShmQh09QRN`qVP|H5J`j4I=u@3{?rHdOUufN+PiV~tAIp<2Mt655<*%* zKlk56h6@0liqpqLyLhQZ`HZMhr~fL`A;<&;ym*1HzyB-;5XPO4+fFCAMD3J543dx#G(W0#{?7c zifVmJ!H;yix(+lHH2ke}U3;k)tRJ>JPRNLP<_|*jsSowbj|C0^9McL5%E+fa00;@> zo(2hQl+fXg&<+U>!Epfm^BNayPLU1V&mQC_Ar!dKfRa`e6EyHAf%3i*?mCGL8Ofz6 z6cWvUIPT|M4m2br;GYqj;{Mch8(Ew{xK9t(3=`zwQwnSdmJCF=kGngs0~-*$M|cll z{6&dDNJdRTM@I(Z=LwS6qpudPcRc+iKet=7#(-H$NC^}y44maaoF~9&MtL2|4Fq~0 z0#awrU@qVHL;l@O{0kM7UtgcL5BeNLDDFoI4^k-mr^+EUHslS&TEt-o6*TPo{qr=J zXGRx^6y*9v?)Byao$2@D65Cwwm)ITP4;xz%10lb-HUc3j1sN<7YGM+^*w`3Qz_-o_ z7}!T8{5-(X1$jo?Z!6c z{$$?+oL+0Ge;5*f+=MiB?O*LOe%OBibA*VPTQ{{pSr*-OZNQAe2P6N6i}@*Yd_q=G z!F^ra-dN8Guqs##Ny;_C9K_q!pQi$gZ zn|_nHUMo4{F;p9Iq)dz||$Vf23qVIw{-S%pJBtLtGK**_ju~=wd zc7kYx2f#w^2H)XBz`FLdaKRUU?ch?Q26Pxv0=%$%Rs23fLwzl+~$JAM?w zo&^gXGw>%5iP zfomYtaxCTCD>wuz(n1!bc~eB908qx=PfyN6tJhDBbL-04xU@N@Hwha#@;iO&`&Pdw zA;HEEn5h#nnuQxP%_n4!&83XJSk~_cMcSlOhcuro_f5D=r$udH_kbzCMW>S|B7CYc zwtGa7jHZVh{qUMv3S$aIHcPzHFF?c?pbhmMC@gL2r_6k%S{$$f^?JHIR#r2~9pY+t zbx3s31xCHM^}^H6#b%Pl#iodNc&Z(${Yi-+ZdWzfg3e z=|#`;(MJ01^xWC{3R#MS1LLUMr9ZP|f^-t$*_(5dN8TTz;VEPWBmW)U}n z#|jzBQW9ww(>-27-I?JW@nNAbxPK$Ot)bK6x^Uk}v)<8})f>oyIcpx1?4CGw*|+dp zlD4bmK2o=~IiCB*b|H8v{WBikNevV!yRMVB#*&5V^}@R7{osxlp|=*dY$c8L_NLDq z5D=cGDb|FD`34S!sh6onB@p>8;s5#9S$`X_TkwbIB`>K=4e_<@7}s~!09WUItnNu( ze66N_>Q|G1j(tq(+tkzi$Ts#E-fH;#Qv`a`2+tn8zq*32Y&n}e4@GIDh=R#d3>(`s z;~Z(@=^r%ZT$7WmHUsZ+`G~U%*!h_7xE`~*FqzowVidNqrVa+K;N5b=uQ40TUr<=Y zjk7-NH4RtmQ!C-WJ_N9D-X7g~eG0{S*YP??T>UyQ$RS+`5C{9M!U{}V5ym^|5x4{Rpjo8!Xz@m$QAHR71;d>=J06|+U2jE z>-TB7L@TMTcUeg*Us!7Y*LB7UI((OA`b;G}?r)f^P;07=Sxw?#-SWU;f3rl{GUU^X z#gTX9vt)-hUMtVea5w&_zcVg%N(x?qt!dMb$NjJc!FrU;GFj|q4gN|90esSU*^r4%uv&Y1RQjidIH8M98W1?vD8fM8{V9-m)= zG0ib+ZG$|~P}9gR?V-jdqv^KW3EVhB^PCI6ONSEQ?@<4)tmV?6eOl8Fn<{IOCbhEU z!!CMUK)YPRxim*J5m#XL6HKnT;Ry}Wk?N747r$yhC1Y{Ak=~F(NzP*Ji7{jLJvQ|u;J(FK=fPKMF_EozR)QBaLZVq~oFQzRS zzf4$^x8`kx!>#H7kt7bNc0+6`qWmO&S=jgXC;SS1EX;v zwM?iuEg*9zN?Wq{yL-x#Bgw0CCasG7Po4Q)i80tGY3=+unmi1$%@sNs;E%hK6;Bl8 zhPvoBke+dcsqu=poZ)e*lPL`b-_i~(MLE=7O8&Q^{rc79#cZ7GM;i$ptw_o)Ho3uW zrxa>Nz;d}i(x!}u<|7y>>N~#Jk&X9J4tN!bmJi0l2IY3?{VU?;vn+>)oXPXka%a3( ziokH_iDVI}>~`!kl4o<>T{eQNF|YZL9SLffwIh0z`%=Ks`Zn$f>3FPmy6&BJ_2gJI z6{oLZtJ{4-Wjj(IiX%l|(;508tP8q(zI3Qbl(liZ$UwFFAzqZ&^T4NLGoR6nPj4cH zfCy?4jPJ&rtcF{)KAGK6|6v#Vj$RR}LJmyyZ~rtg9`2Vlr3*#l4i0*q>v3BqJcP|z z@VCAi?rkqOx5DDsVNQ2YnJ@KJ$M~l_%ArlqXJTc`UAwKCM}1pf`%c(&a)Z;A_9+AU zr`z@Tx-ilHz3l~s$AK2Tk4pB~P^gm=+b%rlY#s_bFzl#h&Bn*yIF|oP?da<#7Vw_0 z+MT~Bd1z2ERI8i5p7;EBHr_M$Ch%w~83}_kuulh5hp)&}j7#4c9wq5m28x~AVx5n* z&)@Bg3;uXg`h>ACJAO znS?RMT_8v|oj;d-<0DLIqoGP;5z)JI4=7{&INpU6UYbbqgT*N2_UA}8q-~iGM`aBK z6>4iovH;kOJdUPU09E+6TSx4%GIqI)tcD)NF5RlrnX~=q>L~6~;_ezVVu&08w~y!J z--mYC$!Q3Y-8pG&e523Lx)}0qIji*s5TtA>SLk@@2uCWO;3axb8B5nGx_690FHD+> z{nQF8PeBAlI6;Ap%G~LZtO; z^C~Xa7nAG zFw3bYjv0z*kvQt3w>+qsuo8tDSHB1H$RLj(R^lo>C!gLWvB*U$|m8tjLRi}~f=$HvcK zw8I3Mv$rQ%`)n?K_TScv^(V5sxSjd+ovwv9nx_K4VX!E*Xdx$Yie5J`qje=L$?dj{ zGth)%WGH19XXE3}MS=0yg!9rCal=!+9LDx3qOHfY!hb*~qjtOtCwfMsd~j#(KLZ#r z!||xL3wAh)<7NjX*3!w_9OQu-|G;7np%pL+UULBE`oqzT~J z-UGG{1h2C?45a8%a@YU0iP2U&aVjk~-j8L0+jE+s3pv|P zX|_{=OPGwvEaNNgDuc5ZUFV3A&2k41;$>pPw%>#j&j+B4;-H)$21#q+<}$Qg({k6O z@ia4fu_70^k+F)gn&mrM8vv^M=3H>|D0u57+k%PqH8c&w%Ga`+p8jecQ=)=dB2=(yiJtCUYmf8rSQ;R zRxh!7a49@pOU72|d{m^COf@&Rs>cmIz{Q1JRo*?H71O!u7~$j%!%Wi!Ip~&Vtlsew zlb#OF2cT~B@oeQk_+!MILYV}6u>Lgl`ElUb7_QXu5z*Q=Jm?MMeVI2gCMq)45IEeb z7N}}HFi9wCb2TkFtj+q3ETZ3I>9xkwd93SH=K3_XbSpaWSF_zU8IkGKF+Wm^%je(p zO?1)3k3z2X8%?82<%}<4M@-NidvVwG7xzSM3+>2kqe0Nz=A2Wp7ArPnqPiXH-2AjX zMz&5@MTv7+futIvwSiM~w}Dd?Uemdk3wJcSN|C^iGJ}w6s^3^a(CF9V2`Mi)yocNJ zojc-U61M}ZQ6`4V+xs#&7aZ73lHITeZ)YC&m1=0qmueHFRrAgW_gPzU*X9ncd3mW} z`%ns3ww^+gTs2;4kC=+MT-+Auk@4aQw)}mG{8k>c{kfO#phKVjRR$H{8!HxPxfK`` zc{i}L`DG9WFB%^La@5{Q$~`mZ4_{v=D}H5;WS5J#0!Iet!Kw(1waS1alw=d3Su_qR zgb~wR!9h(Zky`(x>{H}@KTpmu)@eXULBmD9!d+A|sG8t&GR4!oqw7D5A;2$ou}06C zy>oFY65qf(dF0dwVkjGJ0}m?^Fs2H)y(4a(Mxut!t6LZ4Eidn2mijA3kgYZKnGG3# z>#zTaAXdcixx7kQvSJmiCw_A+o3{Ma#yE{js-Fpp9F=lE|}H7(Xk>S8JS55(&*@m`y(VAn`G?5Tn&VU z5w5_mdxdeHuMDSNeuq!tUY8s#i2VF3My%CCVshzsM?@NV3EXw?EETG2W#2_53oj}Y zc8aXEn>%@#7{7oNj#+|67!QM^6`p?o6PQ#J10X>sh%WtONJlJ=Vq z0BCnz3#F$-G--qMsan_7}J}|I{0gm9{L+Qb7Epa0DG3Ms_@$g{XwZV7{WrB|%vm#-%+UyI{YzLxN#DobL~^gpeqwW9h3 zsW2vVW>09N^~0l;m6MEu;I5N#cdw<#Fh-ra(DZrXqY^LoL3|yf(MV~XXuJwn(8H;D zN*vi9U5T8a=^bsUXS}VfWh>(8NX_ED+(Oy3Qo2uiogRn+U-7w)fB};2g6$ok!>`D-;Z>sXJ`OQyj_K`Ca3 zRYqjG^Ag=!^ zp(^WR3Q7zSy=;;fj`+NVe^+A)b6IRi5rAHIt_0i{Er z?Kj|cYjVNpr;@wMiLD|}&vUuON->1Myw#qy%R9XE%AV*G&9ye`!pqDQdOhbRjSdfC z#EUEkF7)pkCKs4=aXtyBk-O?SuK7(7StE)d5`|3^Py7UQ^*34!xoSDHUHzN*1d=3g zD!sbtTYq@GYKo-322M;A`{~jfk3tDiH;-se`Nr~U2RexTVyC@*T+M*79-|ujUMda+ zuW`1sB&Bu{BB)$sf+bgSWwQL4yw1lRD{n#SgH9v6#(#ZM+2o7bG>IZ$DA}AdOuvwQ zfO#TO)OF*nig(l&eMv&KBk^qy(Xm~UaB_fHz)Ceo+Avg#r~K0O@})6INI0m#K*N(f zX<#KaW}c3CC%A7I?lndj-Lt6Oq!U(Wo*#lQKaK&Qt*V)G^Q$6nMPQXqb+%OPRzYR( zw$@^;VL!2JnUtZ$`sx$~Dm$Ku-Un4Ie;~nbrzjG6)!10Ym2-eF@^PXvuxp${GG;~ z!%$f+6$SY80a2h4iRCrVbPqhJc1yU%lleIz%}nHSWJhS}s?7!CQeW}15srRTw+mIY z9vxWIYa|~BArN1Tc8IZYeLfyWUy93$xU*aa+yQ(@KZnvJ0`Ifl-Y2{7PBV^m8ikU$ zDu_?mF6wz@_aH#zIGw4q1+E3{qk*7!sZ661r<2>3dTzYm+PJ0q3DEWWr!r-`mn@}3 z$Dct)er)e-TtJUZFcGbkcAZ{mteWeMM_(cbi4c{peGb|6O~7W60IzR#TV7vXM2jdI zk0`P6W64n@Fhj=t=E3Ln*&YBk4@J+;8jBF3@ULEHot$jEYjT zH^*PUcleEJIO@TvA^voCSLQ78;+s422OnIacTXz+zWsM!6t!s1EH;G>#QRO%_bl9S z#l`YaR%$7_joyP9L7T|t8mKnG@GQ6fWY=f=-Q?m;fzgAPj*oYupV_qfc({l7j1G4hS{wPsV3cw{GN)HM~TKY0Pux7o5sGoMGewP7m!nW8$3V zp|InOGicXdDG9KL=ZZjck-`U2uw`QQ#sEXL7$!s7cJ_4Io0n(p+MqJz_TGwtA=znW zu)?`pvJca{gR?kjTP-Rt;;g1OyNozElVXDIi01bu_D{2VL4y~kOM!y8;?BOfiLXNb z%j#m$QuwP+G6mUl#md!O4(Ku5h@z+0e4k9!AN9X@dX=taZP>MO#=NCxfN}i{^ie!|R2|WeW~_!y8wcuSyb~ zw^X<|94T{^$FjM)|LU;?O&o4bBxORUYA4yQzx)obJ=&z;JP9#DlJ>R&$wxU|r$iX_?rrsw}Nt+hV;S{kZ4*;Gj2v zS253K!_XJv?jVMT?QAIIK`+G0uIvBO={ya|5B$|3K5F$mhn(ci$#y-a@A=-eXdvG- zX1eiLrNnl${siyDe=vwk!fkw<;i|q=O}1^1$I628X)iZ*x$57Z4FVzogot8AgFlrx zB=3ejeQc{-2`iEC5>@QDQqV(}%S#Nn`@V>gUNI-T@5>+d+$vLoL@h5Ix;ck}%O*>N zJBlfH0B)Or72VqqY)5;VhOW(}U#1N=LngCU@CU0BvlHdx3c8J2b-M6ot{Y$I%i?Ea zs&6zE(NPTt1N|MVhhieu{)u<+)Ge2INwH8h-`Sb3wTp+HW@ zL)F&X`O3#&SBS5~y-Dgws~5dWka06EY;6lx;^&NLN|l3jSAklH!c;myzq-O%9}A0) zn3IwOW(vfchlpWq+%uVfo;-`bRT4;OVaiT-imb;(zt|6!lwBW?aXZ>tB~z=#5;+}* zG6qU*G^Kt*wUz?=!n?=|Aui&y*nhfvpIAeySmUN@2F6s<>=N_RIb4in_E#n0`4>S6 z&E(QdP*Z)7NvDZm%0cOtxlb7V4tv{H+pguWN;;2)CAN>~+KS6j)Sjx%jPgrbsf35F zD)JY{Dlakjh9VN>%3NYbz44xl)3jD^II1uX1vCbowiKxyh=fI*BsklN?rn1>on#JG#{st zDY^vYZYXr3nIovovFU8{j9~C7+F`=VofGt`fS94{R!=k@_}u9#EZu<4a2p1$^*f7fdfu^$ZQ z&J>A|u6}JCIl5Z;w;5GP(QNevg~#tfDLHS-nB$A})|7pWHz6>KO3M3zSiWs@5a z1j|M8-%5^!&RNK&GqoKNWjO1#qx+kN*RfG_%FLdCGm;sy5Un%7N%A<}EMq$xO*T8# zqvqMsOI*kSdmOU5mGWr&2Vs+et z4b^z>m^$j5`CyYS;s*FcDZS&!v^uxSiAOP8d|@l;N{6Q7r|M$MeBq?x^_}FBg#=4h zz7`c&BJ^(74^eB!pn9sq74gc|Q+zXouY9u~*_bARI6Sr-QrXls(-=Y2jwdXDFN*9> z^@$w}PukKY7MaD)$fPyu{xgd=o^U?tlYbzKULHTyMHjUH8dHvy;#@6Vz6nNc4SjPu z*UO{O?nz^RF5#5k6;39{Ti({k@^KC^{!{DQq{}`?FRB(keBIoM$yc^meHF5$2Dgs= z(KWdLO&n=T6_<|r!)1X6e_|PxYm5)DHL!Al#nX!N_ztP+Svr?3&J|pslk8%m=hd>$ zseW#G4_WErnK4SWAnLwCC=|2DTf@FZFH-L*w0qivAvA4RMku{+1%W+2Rf8U5NOA4MrZ|MI#>0SvO=j&5uRn_q4BZ&?GS_ngqs0pXDWD6L(5 zptO_CFR;^^kF1mXSsqmwGp>OWEnqLIN~BUELGGV87}J3KK{Lp^*t zf_=}k0|9Vw?=IELS{1;XL3l|CxXL*u^Gt(P)j6Uy?Acw6?YYzZH>$78vwOxEt8gCk(A^; zsB^+xKFq%4ES^G5w)&ZF2jE05xQ6l&0tu<&&D0=uXryPV$g=%jUQM|9PtP?`{a96N z6-~`Xp0Tm*O>!&pH*tICI2G%XZ&c@QKcJrgFX{h;;936zf@f!D|39;3W;SNF|2qD6 z1kcR&hv|PY`2V*#*t)|g&UU(iZmZ?WccaB-v&E*SrTu??Y**TJs$F=!?3m@^yo~&M zkW(dgJSxjVS7{{2if;?-uB=Ec#6rbGft6ANDu!O}O$Kt&B2l8@qA~O92&@k5 zOb#dWTixE)ncE(Y-dT^%{mvysXJPTHEXL~FnplA&EXXS(BVz#0ONvti6;E5w~27HKVmlLC$D;oe16*B{g8Q|p+eMt<>_yHJGLz? z_oA+*K$6IiTw?5MEM|ULf#vCqJp60@D`!1AdT~GMNpQ+hilrM_+TxCDN$?{VF&dLP< zr}-r*}$fy8X_N9A*~%G~%V7yq3h>E{{E z@9Zb)I7&Oanj4ri>hZ@CWdy+Ull=G{n_5>Fc*?+Os_F`a2a7}E7oHdm-Zwn*_XE(+ z=HTS|yV327{px4we)q0-b9`!QaSF`TXt_U#Vu-Vvt!hB7%De|9%hY#8KH4-H+TEt) zMHO=0a&2L0?=SYL06N7kC<1_fF3s-dJa$P4wq79%YioST8BQT+hk8Z}TB0~u?`lS` zIN&7&>I;X_#hMG%qi%D|Yc*;*no#82Vx?*;Qq!W8nb#P)1at6y2H3u6`}L`q!V8+3 z2PR4S%I;4iXa;tX$K>1qj*)-s7ku39=N?a+7PB?z8s|7j2hv@uWR4a-7a`k0R4uP~`& zcZUzeFUvG%_rwQjAKA3^57Sgcen#$v_9^jIavppCFxPSZ7f=FE>X{YrWAOcjRa zcrYI(cClktEtRl{+lh|;@KmHfVMqTD06##$zatM1k~%_UyO1a@uK}m?rZA6@+xA63 z@R380Ku;sMgO|?gJeOQh>$P?L^WZ^=%V=e>}M5AvB zGleAIV>1vJi#bFpU@d+1o+qWRX`E>uHyGlFP`o&7``?1s^sFo z9!J+)nGdHO7HzqRUq1=t@w?6)e|syKyvqDfS4<@DXLJ*#qA(dzg}KZp?G?2)cm&;} z_j|bhOd*TxG162C{_h|Cn6-9;G`%jMw%b$v%*T2t2CY`_`v8-tPSk}GkzF-4)>AO&Gprv1<@%K)jxB;3e=#vywFypZ< z_nkkSX{gxvh|Y`GQGX9&l}SIoT?pq;f6m<4ECtrEdSBDBi@5AcX+|DBbZa*^LFAh3 z{g#rT_%gFxiuWXH!)UL1$%J6seam_r#_UwFOHN=VOqFu3)49~|&b-CC`jnFVj9C@P&eT+7$p#B~_?RK^ioM zdt>)Dr1I|Rm6x}?xL~hcZhk$V-8@aJb#ZqgaG1mSAn^&;PYtE}ezuT}&OFd}1~7@Z z*llj_V|l2d-)g`1F}+x^|FtWF|5e9xwNeIowizsJhhc5}x@MG(%O!|V9hIS`YTE)i z{M$Msb_^=e=?e(zVDZ)SSvFW7pLj?tkuo>vITMMuxd!Z)YzwD#-mbbcCLhW3jNurZ z$6MGENO$@=!HzLaQ^)3;$dFsyDFhiHkIu6+Ory_!;i+$n0>}$=_fjFWm!mo?)Alob z|FdM$=^j!`mur7+?S9&RRXA~!E+{TVr=x_Zf5KDna?R19(mFO!YCt=DoY%F=?LN~B z_v7Vt&s3T8cq%-M3Tb$e;oN2g`LvF3EzoK^)IV$Fv5;y^I-M<3?u75aO|jJV;>0^> z)|OT#e7J+hMHa7=X`SK|twm9xXflX*)?jW~Txy|QK*nln(MB6_HDIUc?*m#>hcx*{ zf-I|~^VTO7uKdtfOo_05G0Ztqz+_oTLw0x9%YhC51Z#;-6F-y_4SP=6$c4Dd8}BS0 zmXOIxPW?w{_zdoiP;U8Bmwa-D+*sEp)Fr;ae!jv(^zAtqf-E`s1UQO(b2p!E)V4KU zdcp}I=K{k@6zDf_H)Ox0mCUcax7e^7BEpc?9R!O!3mY;gwgsYNKZfY0RZu*q2+XeiMSUG{CEE$LI*8w=053}m6HoBD z8JH?3p2KZ5w*qvA4RvLONDrFo<+%N95XQyOaZrYVm(O$Z)&$0ug#K3@avHY`f^BmGKhZZJ5)1elhz& zS&^=-KP|Roi5PH8pjOUW6gZbmY>I3Z31yoIQC6}RQ&f#(<1Xi>=wqfHWt z_;W+A$U4$dlxdTdpser4&r6<*5QUQQwyKE5&Rq{2A`TMv!`^=+_TnI>wvk6qbx09q zPQ9L4qMi&l&u#GZm340d+n!OMbAJee-dzku=Rez;Q6Ex0BMRc}7Zn040bhOsP4EQV zGo;Kx8F5d5Fu&%BP%@~qh2U#Wv$?q=x_hD64zs`u5d-?}TK5?Uo1?Nl+HfDFW_ON( zHa%Qr<*#ecB^~M9aGqZm{;cu`%_^=(nbBwUC*iDh$HG&#fYP`h;{|^nuLqdtCC5;oDlnPYJ4piWs5W2Pl;)L&h$m zpEFFHM(FD!&X?&0Lh1&!@=$+|Pg>GMMX7CBe)SLrN61XL#w9>||Be~=AnGv8ACSxG zA~x|UNN*fdRtTILI!^lAJ>?II&dTNn3Y;_P;ON!0vUE!S;L)4NEuV!WALso5Hp~mI zkJXUQ-O0d(bY`eE6Vtx7+Xt2l3OXS!$LQ9t)rfUhY{+g&^@f7pjE0U-?r31ne|8j4 zuJLu8DMy?r0(qK)k>DBmT?ZJ)%-q+y5|{4KPOFJU#MW<4Xo{852>AYXhG*=eA5%9 zUqJeoX-|n?^dn zJdB|mCWgYrcT{=eWwAp?ZbzOSuyOfvIfVZQ<#sylr52VzH`=N@Ul2I$pC-JG`YH2hye1EIg; za!uCLI-yxh;ZcqVM$lj`=tSxgd}G7!SZa}OE&%R1m9eWV#0C4+SnqCJ(@1P#K*1ljtX5CgGKxKcMD_AgDp{#_L-*r^nw8X#e zCC+(r%l||_mK@&s-f^89OyL)BxtWz2?5*#y$4$?*6){CZ8%X%gaGc~lV5^|fBxU&l z$-r=yDaj#mvr9{wbH5hIDGH_RLKU;xJH_GD@x9RYV)5?WQP}q~T|kMdg9#EKHuPDx zyXRUqFnSVT7~YfBeH{Qs@*f1%$PuxNjR>AbC9Dz@9Km7OgEY112EMrj52SXti%Xas zWZz0ZLGU4mDEzgG2~pC`y`elR#XQ~7%%&yWOAk1$VOMueje%kDBn$Vn_$6pGR#H7( zy3Q!Luse{x?%8yCR%fh0NxJ|xU|6w8m5moKQt*w%PgyBn@tx8xszS)JuMBRAbBSh-EZVcYzMn2 z2dZK%^__5(k`verFF&-4Tu4m;>2nL}F)~Dum;C|-6bYeKwnuGy`H}^-Meub%FTsTJ z_R@wVuO%}=qTtn?u9YD$ZIVc}&2swEd#d2QWAltzF00^pgeaxYRa0tXmn;CKNNaI8@b#8k>RVx`1vgH_pj}5 z;YKn3OkUWdWZE_|{r#u}?Ha8IP>r4oMy|D>*v7~^ zw-w~&F#L3w_NM4Xa_i#-9oD0Bn@4m4>9aG(s~G_B*80RqLybIB_DOT_NBd98{ib3Q ztSxEq==8axhK0i0GvRRYzISjPVH>=ghP6Uzd){urqV0hNY`ugV;nmFB?@ox##X9fF zMfmXU0Xw+716jftHqpIdXStN7C;oo+Tc2uMhcHL3sA!o`-MeBCefsnu@-colFKzj5 zd0a)Z8hhILDLBld`Ed(%t8!^`X2eq4)BN#Zn^6r}I7k)G(^wLRyQduP$saMEG@dWZ z9Y!(}(96kR6&Bp|ad+8=jjh9&>p{fh_2+7MZN)}KEv~k+Q77em?+E?^<+@**Z<55$ zgVLNQAzRHdKf?`EIzMp6@I25KF@4DC8?Rb-m9e89_P9Qdk#-cIoPh`Lb^B z3o|%Io1^hh;1p*R`e)Nmr)(`@84qGl);M4%&S3MDBs(V27+!H$Zg4!5r|F>cMnJ8q zZ-WbANH=fR=%-jtS;7IUPj5;)8m4)j122gmb~R=0ZW@*`LzH(;d`fOu>3xJei%5f8 zW$SV9$xB)s>N{E7fR72giUaITYLLkxrC0baR=6y#ib2yyhMbH_u+*m(>;@eul`xF^;s~Vw+5w|e8V%}l*yb40=D__vOou-i{-qy;& zN>eq2j@(i93S6r#tc&78KLycof&f064He&t)EtWsLeU`HtEQ^a&Y}UZ7FCjI|KV4cywjb#&P)ClT{Bdj{TMvPOHJLasAUU6&lpy0 z_pKyPw0jqUJZiHPdFM?;>l>m*hJ>GZ8esNiF)SJ=c@y`u-Ll8c^l%aIhrYt28_ycW z+Sa{G%a@?GQmxqx240y!g~H8=X8C!G&?^igK1?2c1!Q^^QoCgpCjb+8NUjzytRq?Q zSkkSZU)=f(w7FR82_~%DVT2N%0QxLJnH0!`K{H<<(|3_YKhGzPQc7@DX{d#>r`VNZ zelMuS5e<+>OPDxB$mgt5NPp4VV!UlQ`&TUu#__b|FsP{s!y!qa@WLvg^Ovrj5wjB^ zUKP)^u{;kLWfR<;Tk&?Q_!G46Qw3%jx(Q3k6>5pvJi4m~rsk;~sO4JsC*rL`-0>0gKJ>d+8MFL_-=$(qMvtR~rV%be zlkz1vN2_GoejZTEV)|Xo<1>W0&cALV4({0{7Jryg(ZYmB?*=v}&}MTWyLx@-6P>Q- zDX==HK4tfA zGnGFTRl{-iV89#mto#%S33$2S#DKN1ZMTp@6n-N6Q>Zs+mohCdOz))!7D4N}>C5kp z=pUL0CsWtOV;I$Vc8a3JgE^~3$!GJj^OWIE%Uj>?S$cVWjIc_2ia3Dx((2qoarFgw z%7=4LIFnnSY_>wksUVZtzlX2;9u83=o(#2f`W?Gauh``0irg&OCJ$?XUnC0me^2e& zW|Z)wtu{_LwVV=a?6}IpPKY&((~jM-D%!g=A13@oo7*L2spy@^yJThSj|59v{z%jjMpuq}y?w_wsV;X7nC1@Fp$tRb-o0r_eNswOvC+gx zuM=V~dDadlwJ|xmtqd(+1I=y3NE4hcAjQzkz*M{ow{o8ayqPCt;k_WaWVIMT*64Y*K~eDRp~oQU z>2eEF6zD{Bt6o|2ji@@KxuFARLU^k@)u_4)8FA9`Ko{w+RX=(kchwK1wnu<|Z+9ZZ zHT7qbKa)*95#=gb)hVfCr;jR-X7rbdr0ZnvZc!JWJ<{g`iJMbyDy5wV!UY&nD=fO$ zmTM92@34~z(Bzr$KPto`RfGJ<_}hUrRKh?c0n;yd_#5v z<#_RST}3%dZ_Ts^hq(^@&(5Pj0f-vL&LohGt~t`;#!nKQ`KN|(czC@1OGz{j<&*ZZ zNnQ;4L3@`+ceaIrgI)}K- zzduE@Im&Bw|A3}hN`>$!#`=4V4+#}~DCh2zQRA5!rmgDbam7c=h7NE)(=$WGuK|%_y_A#HA1#RuT#Xijeyr6mT`Z9TVN>lMHcvK8Wi>5v z$E?XJnn7Ie)g0ncmUBT@*Yu~lXm9+M_7dTY@WktvPhhe#zzT;~-tac{wi%+L3iH3Z zfH-B2Kgfi;Di$fyhNCgQU(OQr-OE`%BOcI0KzLOfAY*Ij|2Q(lTrp9KrdSu%%&m;1`#_Q0^Q&OoOa5Jwtzx2W`>Mn*Hmb z%NFkbu)E7F#+|H_n`ET}Ul!)??aS$rc+6o0plg!pNIKrD5&Sa<9Bq`2Hxno-Zq$@{ zSZKT92bI^W5hxK<+_N{`6b)?hgY>CiP-}_Vb#BEvwOtA{;66*sLBA1MmuCBboTDgISr=A4QECI9Oj$w8hx+v%XODo!H#X z-5Jhuwno}e@mKClZ%dQ=hDYv=RQIQ@<*pGrN<6_qeG^fHh6lMy=Taom(?ZHi%7lZ& zxd{Uu*>{EtRDt53yJ+lo&ID{KjyJ}jW}2zbY{U%`X53CkSJ)L!;mNKX2E64qos z931O2R%nQ+c4R<|#4C}qV&4~qAm{T+-!>n>+TyXOcEWxU%ao>+wP0!|J)VJi>34;w z1}n}TSb7F6fz9m~ljrISg%NL&d;d-K4;Uv073}(D-61~wjA^Ag;G&?Xdh+Dc@D2F-I) z*CO?!Y+#&NAeR-+LK<#*IP<%GCXV~+)2`bE)Pnj1?sQ6m7#aoc^2FvWd0fztCB!$h z7O)8w&Svd(uQdnriACQD>o>Q<9D%xvFG^+f_`tETKvnPjGm;5ZLLDvn$OoM?bZhx% zuwBDl-IIM17T7h7XSF9MOoTl<5sk_w+7i}ZSa3-v8B#tlD4OqTRlfvSEr5h({bFo&g8se8Ol$twuOl_@T1*J?as zKmV@pb*yE9Y}Z}&Ulj^8W1jvtwIFF&<%w`tt$qLWB5UKg&$gMeGoA>|Uo^l&W-{2? zCy?P=Jg%Ztf4lRn{7*thnX{XOfdDK5&>pz3{h+UH0tNLYimHtfP5;jY2eo_b^zjrg za9Rfqfe_#BxYG6!lP2r{rb(!*bUC&anRe42n8QP@(XaUsa7-vKYQ;zbd;MfyzbVwZ z3+WCPj9{=p*$7nk=jENA6aA18Owvb`i*x!#dXX=U7>hS;cW%}9s31y{eq16qpMt>& zepi+bA(k&)Q}d+|3;jh0itDze_ML>~?9Z`0b6W9RrsH2SR&sBhmF*lR<=2kl0Y~$0 z$<05XP`3 zy}OUkW}4NnI`kLUUnc1!9$wgLA#r!Ri$um|B7pbk(hgxWY%rv*yoreoi{UDkScyr) z#@<4HC+{xrT5L**&&6H3E!>-vioLXI7=vt2)>m*z(nVU26{V^NC1CcLZ&d35H`e1) z=A`hRJu0PElwLhn|ZyG&WUR&$1a9x1bD&77v7|W9$=>nf(t;l}sEB3&n?GrR#|I!tAbXR7wkU^u`-Vfv%UF>}?DCoVPHweO@^Q3qcc+RPa_Q z>~-o5N_d^bKH;fC&cPH6zB7}TNb)1?6Vk1Loyb?@azh=6)n5(FAyu9y%KT0-n~yl@W*`Dobf z)uxnHCTPG_mxUBCe!bsc+?w+?&0Ku@bE zE8Uxu)z4?xo7jQsniaQDeuj2(dK&@>xq%{jEWh#U=~tw#q{&T-mKo!pfAaNE`{%1615FV3t+3k|~W0bY45 zinV%Mu8RGWJ65H$f6nguLgJFBvZ;gXW9)5xpLeUl9N0c)PBdnJOezzZ)U5L9x-*&v zC1oJ-3c>*D4@PEmW8A0#zgnIo0k54uf&*zal4?>|Wh*a*yGbUWz9Sc;w7%-hQVMuE zpH}3+{K68*&DE9#AX%$pT5$)38Xm6QL%=adp4dff69^RH>o>m2gI?2WaG66|_}!8^ zC(nxSTqL;&?a%&LWN8Y$aG1h0PWRuUxSHE7IgCN zqeT@4RlElqvnaYO84+NSQzUKOw$L@8BQA|0U*vqB1@lAY!lzh*xV@qU8;lrT^o@7w z9I7~K5}Ww{?Jrtara-U{}l4+^Z2@;8C1gkYyn#Ei&G;J_+VQScjs(W6*b zp-kkBD>x^=T81l0VSsV5qK+nZJI%^_=zAigxt&q=t|)<{GA8Q$TM3}*!&QHQiduiT zNUCP9X01igLfwG>9huFv83%B1K_uN@F{faY#n<;@Rh%201D}X#*?`MJ2;;s+!H2su zK;@5ZP=|2tkU)zBBW1Uj3B=ZI(6n#{b;C_3oT~O;e88g_@W(u=u9;3Gg-qFJUiK`Q zBs?AT!4gbKUGR#>NmQtwJ425{fXRTP_uM$Gs>!$^rK~Q1jXBFo0@}lGm2!sWI2Z~N zxi`L}*DLULI+I%DkI}Y@Y%t-gq42z3Tl+X^AT6{D!lG#r%EXFN8b1&BeX6#Y^Iz#Z zla_3DNoc4tpSytlv*{(6e_r89N#1fLIwGL&CUCYQp&l9aQzJYG`#K@GaajwibIdztUM7B`wjZv!t572mXn{x8kQ#C53hF^I$eUjae8W zSA(^83H@^i24t*X^i&@V2}QLS<;Ottfyl%3f8Q{8+!f3;?r?*z+@8Wk6Rz?AQTa4h zVxsiavuFd9h!2RNt{cC6GRui!fICh{@=NX8e_urG&wgu4rdg*obE z7Al{QW@rJsXiPluy;frSqelwu7)U2P{%62Gt%&H?w7+S_%(%Y{5oBkkxi!!L{j6~4 zB=vp>OSV1;;3ICMg6vv@Cd@BB(~jolKb|78(5G->cYzc&+}m2LC!w~ij`IAcJgFFmVXWe{ne++h3ANc z=VQN#=+~YQK0^p(EJp6pw5|{iq62&TrOonpwWiSkD-=9QXs*ZhRy_#wW=Hnn7t5&| zv$lguE=aPELH4?_HZx^Pp78GuOx|BFd=|rz>P;xP_(34GOvHH2%fN`OOS~&?8B}DK zJH}f!1JgYSe>^`UiHKN9rK)*xG!#%w>}91_&ndclA;b#8OID@^^u3Kjo2bTL!q&8N z;&|4hEKpt()l}d<)12j9^~!ie{h&&kH7f6YVB>k!79e%(HW2WG{q(1-8XYMl)m*hM z5nxLRCsimv^%8FaVK~f0+*qb}-=PEilx8ZXO*r>+$jd|LRHS424Cm z64QXMl>ICLRZ`;jQb2Tgw%kY=y2V>mDpcBIP%@_-Jwp^;_ozQhQH%(DRX*04IA^kH zqkB%Ca*rAL8v$fTR|&RlbHezsJ8VrD8CFpHu!u@+!;G=!O-{t=9~|cGNq&MY$JL6* z5a+Cp)BCxu@{jGNRs3Cm)QQi!Aw!H-;&AR&<+UZDrMsHae|QoK>)gt&w` z&%bpa9vTiszw3;64!O`~5BHCWJ6iStD@N9GGU1xoB7lTQ4(zM-yTR;Y4}pJ+Q~-{!;vSEj4R zW?y&BX>+aX9rO^-o&PJ-f;3FY_>+bCH@0g|s?-1TlMpE+0X^QGaQp?(GLUCT%tix0 z(B!>zhPBpF)Dx`3K% zSpP6*!j_jy_T-oRtUqEo3Pl4k^QcI}i$h0FDfXOHPvV!%@vlq zSMAM?WoVXnY*f;0kn)8Tv-RC6|DGMpN2!1Ztn;11(DbbL$uFGkE z0m{X-`=~f$w+YfYj*gwJl#;eqb6#=)(ew6s2wQD1!pb_LMONaDxa9*%Z4;3#s3$O` zs>Ke64Dve~e0?$jr;22FG%+9wO+X_Q#BYVeyiC2tt*e!jmzbs%I`<85KX>dz8X2=M zg6c1e=jg?5j;DOm?De<|oIp}yU{Sd}BFneO2Z;|mu8q#Md_<&JJMx&~jR>w-?V0tkdpeYKAT%2$bV*2yJLJ^ijy|=lSE7q{!SGgUbktg)`GPsp2(74 zFmlhPdu(NF2Pw`AT=3OZeUVA}+BG!Ld~%!I2^=VOVu`-hnn(`_!U;19>+XfJ*GNI64|)8&yq+%7D|r z%|?V*M)fi&5{g_pT!td6&LGPo;1se)<=Um+Xr9}a(yc{nzmQ27%-^<>ZRbUuy~$S> zbI{^e#Sc8p4FrAD=+29IPs@A60s9cJp>_VL5+4^2?hbkjlZloa825f4iDZ_-l>r%G z-___Or1F^e;==x##EqwV?@^evGd;u}j0Yih2^2f{P+e|pa!2*} zV{%}|O!pImZWMT|JaXrV!i{okMKWV#>h!ku##81^tVby0v;tn|nq0?35fahyoN>(K z(+gDS{mU-Up(c@r*WV=D2X*}v1!Snpu6~NJEGA80{;)_g(Wr0{F_f@LtQAAEZUgjdPaw>lpDl zJS3TLi^W@3x4^cUg4#Kku$3nY0sbmi(eMlKdCf3S)ILSXMMeteqCbWLMv5-;)RPSK z;rs@7HssV({cHN+`>me{h8zd-CHjvq{|@G8LdptN;9oEoW+}q>!aOHl9fUH>gbiUX zGwH+QdN#SxX`)$;R-5lQ?Ppt!JRBWc%3W^u%_Q4Qm{A?prLqxT^kFnz1PVUzvc_*O#_Q0zo}Xuj{{eW zRS4t*QuT=w2FTC@kIfStSZ-?l_NABrY{ zh%m+{gA}8X#EtRU#f^Jn)gJ}ZnRYutJWfgtNpGmrN0eXqnitetFYCS0;hof|2#4uU z;SpO;#29xAwkQFOIo`lv7pMGvrukr>W)xL4sAmX>?x~M@9W#5YXn5N2OFEj>Y+6xr z>ip(Wn1TEuI7~H4JYVURf5W@~IA^I3($njh1Lw|M&Apo%DJVrgL8qg17rh$*_tSrU zcwlSy+AWzD%$FLFj2{#Zrrvvw`L+{HisKa{P_a@C`c5&`Xw&C zOb!NlsB%Yw)-r8XvNCFi(A!sSR;PCT1S)j83oGXXc9uL|B*1EmxFB_n=OdfKTHtl!FrQ{`52)f=3B{AM23{pkxZJ-S$yp zE@a)QHt;KtmCI#Bm1^d;)?f>fuD6>0%OnU6NNXa6S89wu_iGwcs; z9UV&$c6Oc8CT0klT#%|rhr_=IfyO?*U>0Zy4m|zlBGm+M&2di^ER_LvZ-n9Iu7nZi zcfO7F5{SdlVFkBcmPslqIZNu=6LSY~wC9Nl>P|ickrbPo)Jz?5^FvA0`--tOt^@|dpLn$jMN}xWNOiSOe!^qJXrZ{!aFoT zG6z(S0Mlg0QdQ|$EmH&=OQ*LCgRp0iaMKG@F+&ic_l+yL@A75&XOkaxlvg>Bk$-CW z#3jpcXFUjE%Dj7-7cX^}5@^{Xyj&fYfa|#&%Y)!595uJJafU6l4?RUN;~(6)`Te%E zo&hpbkG+0uzRh;KZh*Ji3&W^?a%e2Tnvf-TbRs0bc*TgTi!|sZ!w4Mz$Ty$DybhFO z2)OCc<*shPJGMBAm1rC?RBZsmF^ zG!YLW~XX7yj-hY1vJ35Ewpo)vL0kIv&_M`-k^EfVa z&z=dpbe6ny*sgEMAt&|8*4EfLHq6&sgm4DsI)R)dz%PPbcsZ}1Bt;u+D4vMrXOd;n zn1ONQ2_pQJSC^giIZ?saHEWgB`B}SuC8TZLGWs>^cobr`EB&W&P_uQ3c@Uhy9a>~; zZ7b~BnEpQAmX|p3acHuAz3tc}>T^^TMkz7F5<cFcM640kv@CBp6?F zPu%71_BSe@S_OBJ{5~;VQ>ZdD7SoQU?9HG)oVKJYJ>Sv8*Q14M5lAZm0sI~#6jMfzy{jv+#eH^mCD2Ym4nYQ=GBb|fHc z44YCvbHH-_@opiiMsGb8H3JY}3YApg7vKCX1hVuY(fRRC!m|4PySLzH@d%VAzP{C& zW)DBTw>6lw)IqwQ>TJwOsqdSDHhDCEyJJtL*0@&veJgQL6X{p&!;MC`w2PRdj$@Ap zwPbSlY~Vb@)Ca!shDXDQGtBQ;fxqF?Pu>bzNM?HqVsrVi#AzFBDj{X++NG#^7}6Tp zLh0DQ{m~7~$`T}F3l2WGAr^t|&$ql&_^zRv@*d1dta?HIwwNuECOyGw`L}WlEKk`D z20@YJuT-w%AAw=-i;j7apkD?>Dz43yACddf)9h8!wo~$AxCyQ3V$HdNcTM+Q3@S4F zX`iJ5!3BuU7KV1EmE9}Bnq>}0P58<8|FNU!w-CB}1$D5P{aR{n8*}5Qw<}k?f1ksx z%+7z#NyL(+>Ck$oK=f)v-V<)LC5oOBX1Dgl56d6vv&oarS$OEr<(BgD8{s&xOcZhN zK1YD|{DYxZ+3+z#wF`@}*dD-9v-6idep$6%N)H1;HjS@-l)LBf{VeXcBF}n9!J%rDc7-~&nc#|prXT+f=w}>IYFE=KaYW~!KS~&P z;dS|y@Ue2jvoo)l%D}mT-h(aeRSD!|`9U3;34j%1@s;ks3lt!2fJZ!)!@d5(V%_B(l~D=Fg z5DLiJkz9&jtMGV>=!uaDz6mSZ@6tZ?daM zykBLZZ~r_Z>N!di_V!1%e2Yo$jy?%<$U49dNI%~LKWjE**?prirC3IQ1INK9$0`o2 zq^f-WhoR!?;BvS|2Z|5XRnLgpAZ7xro8^H|)~p~H2dPIEZ`yRpjV-jz-}Z~I_g%Sn zRXcJ^4YkpH9?>aD?^?sbn4?kSdFy3j)RRIV`viCS@N6YOgUdAz&55QRt{ltZ;09F=TvAT!c=@Bp0Sn6@7o1YmLk(M!XbLz zw9VMR0*+5G`8{rB;$=M6Q3NPtuOBt5Y@fhH+NNb;warh_-Xw7O2KyKEOl(Z@G013c z*md|bxwD|;cV>D?jq&y@NCsnvDLQyUgFKG9TuF7D4Iw1um@W}HK5Zb5xZ$L@2#WDn z?G1?oC7NNGoyl?kz%vBax#f_t$vpT2s7RxpkUg{8dsu_v$ZJ)L^ZyqeB;wn9-x#=C zQ1CNWlOqz9vi^c5!yyGLA5Om(Icih!P9~LjUTx>al6p7}Hm*hTqHX*WS9h$e-||X0aDEu{}IiU&-b3)2Crqv(m7`i>)`r#5v$=@T`*0m zTb_&*DS@=e$jD=jWR;Vj$>a0L=ZtC-&2ep!ZsR9lxPf|^$B>Qh_j&>d#zRfnd21Zo zAtlu}anQeqogA8(TUT;)X~h4KXdLIe!fu^?X%NCBk3(5x@#U(ZS1IS2H^!8Kra_?2 zHa!&Dv}i;QN11$O+Tf&zf&J-wFF8(B%Cw1Lz092Sdq08J5M3Gc@_`3pqg#4kjOB!X ze>^t1Ou7s~r}!vJj(`8mA%5TO!^4DiPy#YpV1Qas4l8Zx1qad=1d%E47#?k+<8rbG z+c+@re=_Q0cXhnMvc!f&PHEkn1HT}3$gXnd`%C#}juYwuK}_}jO~!NPR&3u!5&`?> zf=aYU+4G&hSQ+vCji6t#Nry<~P@jEcncp+0;SduLEIf3hIApX*y5>W>$m)q4)D{75*~|IA zryZ5Nig8)>=T~ZLQ$J>1^d22}9!=E!H5A7Qr0AcOxW8V#iZ#b79F@NaB|dx8nmJNi z)>5nf-H&qoA(lqD5J{zvVYLSLzyFSe?r_AVf4f2aO-3Iqel<+?cXH;&bhB4jdPyVw5>g7WVHt6FokN@8Fy zz-@dt|Lr!pTW}W6@PhFF9h@0&o-3W<_n}v}T%4}XU;=kaKx&4k{*HC%gZ*09;uZqp zC-Qp`KHfR23wKUZT>e0idcPQLO^a^*2*xWvdgC(_E><$Nd=;C&sJvA!O?%jG6TR5+I=Ow8_vBc5EB8V#Ekj*S=)KGu9|DW0uibw6f4}E$dyP@wYQ*q zb?gjJwHZ5LsS4%E>;X>gQpRd0>!UQegc;ZH?XWA+H1thiP9dvZfZhBiz0jhIe?}ht zvf%R(ZU~TeKn3fQ8rd3&vyFq)Z$}4W_d-Dhv7nMCgDrX-*bZ+J0Qd?pq%^A`j0hJ? z*>#od+ii}&g{4Q38YRAZz6pqZX)~=!rwF4iRaygmBQ!v@ciA381`+>-2m>RbV^=Oy zvBTEhe^HhHPDf#|5$mGyXa4J&1=`sWM54cNRUpn=vF(=w)ZSDSC2`Cf0v06Qw;;}D zI~dG(3o>bcmGLcUXjZ)NUZq4pJCI2#ASHZ|mzpT^uK}W5GboT&;#IwbSp7w!0fZcw;#`56R|e+U%OBogj{=)G&*qx0v$1`9+E@{O?p;$0Q~A4{FGn*xWLX|7{8Fx z^rD2Z{thF8-JA|b7>yuKjSg#=)XWGFo%&qC&P=v%Mc5G}C)#Qk8?20iTYNr<1XB6V zt{a?N&eEecyggTTpi_yvpQv$|yc%Ag#6`kU1t)U-xnCcGcnEK#SO0R!%Q{Vy+Zn{~ z+f$CIg)TNt*h>YXYPSS0&QiM+{~jI75v6%2RjWmnMCDMb@H9Nye-eY+cU&B=vWJVk zXrYbhehtws9|Vl_SFk|+ZnHxe0+qb7cw>KbAvT*^_}1)bF9VkL2Aqfv|I+CYl|?q| z$A2!ZF7~;BD+G6HDXL*}IYiY*2&-kKId^DdMs~Y$MO7a|#QKktABdSy#RuRmcop;7 zqijc_e~ZYMiHtK%iG2GtfmRNe>c4r5bwI8K$U6sC*sfNgs_I)$%@Jj!5Qi?sn2meH z5acLqFvN|Od`@*J5+13XovS#_@O^&#Qc8lUMjtb$`=uTSU(z@VTw8Xh_osDbE)(@w zS2RTWf3UO`I>LL8m^4)As{U2Y--yjLEDj&1bQczewhvyF>TpbZw-)LjyYqfOw?ddYUA8psSoz` zc*)|?O`~!@oLXr5QWm{UzPcl9Md~pkJcz;8B$7b$-;#@CJW*wCCoP3pC!OgWGn}1*g3o$jLFnzus?3-S?R7-JyUiRcCE7dS(o&dYzI4Mk=bJLf zOs+X1$sI0hwG{eu&7xaFc=4jRHia&0$mPNVO=tyg#8W0ri9*Ox zQ%OCH0dD`?Rc1U7uW(1HF(Jfo@nY>)V4 z6!^{;;@)FutGg96dm4?1!d6*0JSTHvXVie}=j>$N$H>)Rp*`M%E|lKQX&}aa&;Fh8 zx3<^5XQoKgZVMr;qNZDjCTfzTx^AENw#Nl!jBXGB?i%d^)EaT~O_SZTI~aKRk`jl* z3~BLM9gyKJH-G{MjgM(^anCn@S?=GGf7eePAx8Z^`XiO74nBGUb4l0d{H+majYD|f zx3;Hawa}&MWS3$eT%QB~3^)Rz{L$#ZHG_DMcX?Xcik1{ApDm7sq|@v!i(1cz6l;4N zI-FtJ1l-wFO&ciaTS1h6twsT0>_r2?C9Z`(_O;6|>t1PkLjWnfG$&HZu-h(VQc)o zBA^J(KfF~pHp}BMsfIfY0*D8ocaEIz7QfrL#@eb#tAalZ+HWUZgp&pE)bug4+7dRB zEb-Gk?b+tD`IKWP?!FAORy20mTMEZyLmA|Pkwr~7yz*LJNLlDFkuE`Bf68uwm8VE) z6G1aH-JP`gMethcc#fiieucUE2@m1vCBOu|h@D-XNVUplJORqL95glz?5t&{D2PMw zVpXp=cF#I5)h~2zItO0%_N;YXpJ&eA)0R&BOxAn(W=k`8jVy5dL39s)6$iMWnXDmX zr$1VBBMX@D)w8(`bT$G6WagoP70(&?$-q;5M(<sSI_n9x?G9Jctg(hYOfA*DBdbyjmcBj&w#5i)e+>- z)`~k;PBB3XFf^D^yAIEdSAGg_#v@RJVi8a)gu*!3Bk<QAj%s?N(@)@eMf23fuzZcMqVCRklRp`3waB3+X%5(SiFX6_qi#yRJ+D z0L9fwpEZaX&B^thSR$w*%RsW_;6DF0ok&JQQD-T5rF6^k`Vp`)*TZ>tA3ifzs(Zx; z)e;1_p8o>8DQx@n%(c6{7Hg*>GaU{5 z7N=Q(Wxn%Oo7{%bj{B6FFUU67e1m5i^m*vnpp8a^R8^>3!zd*{s*^|aols&OO zy>qk$z(ztp;F`r*rr1W0sE#6;3yndhu2E+I=ny3JPeQXh^XU9r(MdVEwH|61LkjQ*7a0TQi#a3gXh;M+c=UnFWFw$t}pu8I%X^4WdUkMV~z zrsoojT|uBCr)bjaF-3m_~VH{HtjBBEkjid zZGgl3jFj*{14087xmJ2$kkW^%TUqwbbQ-ou%CCy_{T(0hd3uUh>}fV1kfCNDu*)Lt zn{{&wI!SSY8z@dvy;)1r62h#C*<8$Nq5Uj>;+}0@X5}$<DW?XOEmVD|?sL~o)u$906oz73iIK`%0=I-X zgxo9w@O-3g8M~ZXGc<6Ic~*>Q^G+3nhf|-U5cj#RiVtE6?(R_o$`xYG8Bd6hh1Hja zQ0~z@Fb=#9FAYFY=X_&{?0@m=<4cdM;I2{U%t6n!(ZH`R`^T9(w+5i!*F*nExG0Tq z^JYEjy7g%qYynuRvwf#H&Iwpnb8>c|?=rcTD}?yMN33?1Vj+-l1SSsYr-G{|AS@~9 zl7ulP?6UoVah+p!4{j1a>};xPJ)lYX)rljC7;&X(o7P<`OTgh=cJbawf)Z68^PrO( zp3EEgYh@-Qc*Ard3VDyhYaYX&6hF8)MF6^QTB)Jy87XSm5n%Jp%__=!Z>Q(=_k+Sp zrh>HK<4zAXNMBi?oUF^CIK#lpGZrPPXAZS`+$B--v^80iDEXa-$QEQeNAwLoX;p1tZk%EUrO()okEFUtsgs&(w4=4m^9+kGP?_iAUvvNgG^Xc$3Z zQ_YaQ?qVJPYa1(QS*3Dp1Y&dP{3eXd!HSENKoZ5)AzDW@u~q^PS9;COBPs1d*P5yu?u zL_G3UG%j49YS?YDZxR!3&aY-?W7r3SI5A~gLKUwHJmq4z1ULB2YQq?06yDM{YN;7! zVDwaQV6OxH-=>x0p&^s%PpR8WxX9>8+_=d`cK!A;lh1F4F7nNzib)QpN+Xo_uM@A( z4QXbQ82)VkU;IzQy>}oRinAhLOZkN+MDLf!aFiUsU$g7w=yyV}E1Ew}xal2e?}5d^ z>|?U-v$aJiPdsBDBa}GdNK8WAtPKHiL67Ep!0}7W+$*O)ZfH-b+%!eJ9_LE%qcjvA z<=ns@j2#z+lX|N;Oj#zz>k$ubx~vS`9~@;jHXl`7IFcc!&})UWBJqbX|CSAYtRdyu z$oV)8*C<$13j7Tkd}UK$W30weM=bGR_QJVrR@}zsZ_Gli5%`z-dRjluc|6;ZiIB&U z(v2q>-Ja_NPGfP0i829pJ3{dIMYSZlE1AwjQu$8Lt`O7?4sLvJM~)PxF46H$FBe zT>kELJHv_z^yuS<9iZTci2BT+=P7{&#=BCcZ9XOGl*9Q)E<3bRlmA}pFu#K8zr^== zp_D=#EBbY36Rg)n!5;*z<-@pcgwb^RUmT0s^T2r)!}AsHZ3z-YmbR=U+Quy^gAk9& z)~)5O*scI@U0?(3roboC3R-K)w7X?}cZJf#@j>0Fa%~UJkKs_tINS1a%@o<*ET>)MW0So3TZ2%rz znxGuZKUR)$NmeJ{aFybv+tj@(Ab{29b#)mmn%VQia z;#vC;pLLc=5>KKF^@jlL=gf+d>Or9sf#vRb`$PqTY)VxXBxeKdbru0*hUwnQJW9$u ztT=3R+d>^md-nbH>4d?4>bpnRNy68I7!*vRl$KK_YFfaP#3I39efVzURNjpiMS9(yRS4JJuh(#S)YygKPR$h%HRYi~#X%koln zRwld>s0Iu$MXFS6gXRv~1d~~eZOyxD8KjE{e5ET zoL)WshsESK5TL^B&x|mjpaL1W4S|={1CiP0yKY_0yd9~0D??N=>L}7Rys@L{UnZ%X zuG*CLISpA->MM~nH0fp`yja6$c~Y!XrKDVaFEijywF#=%1FH&2-`xRtpAJmF@aF@X z2rl9Zl!hhb4KTd6Af}Lk`WCM^{eR3eUW>W5T}NCjGN(MjVpA4~uFVS+TH*>yw4Fq?~o6w*{hGLH z6;kZtM6X+oXISMg^7=gEm;Nr_Nk^Mbo(cCbKpRPIe+~1byrruN-rGu z&uFQ(r1kWWA@9L_q@tj5+#{aUW0bDQH1AbRbU;3UlF}jRCVOFFO+0z>AO-~NkL8?| zP`)1SfR?qw+(uhkwi0l`@Tsv7527x)*A57{eolsz<_Cp>RX?W7h z=$O+c1q%Z|*PHL2A0$9&=L-8Q%o?f~;}J0CD&e`)Rqj=ZA#IropSz&h6uzxDybD`{ z_l0?>)#PB8Z!oQ+tD0@aEFOt*mQuaP!8(H?xOd|+m4k>MG1P>0#Ch|{!AxKSIEc;L z#-W1WKhS$wvgikzLrhJ)^xvc(Mgq<~k{TM^nYTA!TT)HeB5dPg`%;#z^Qir; z4D2^J+NmNrk6^hGcU;!_*wKe#Fmm%C+@UcA!-)Z?s1kk(6oA{2cb{nHebZMA_$9js zGhMswc-XWZUnc+FJug%BocRN%s zA50Rhj%$4^aUP!6jNPNI@ItdW_WGx=bVjMy>$17{M$lBEL=?f$D8J;I2`=Efcv+|# zNa>eCA_LF|1=G^bW@k%U4E0TkqSnMIz1C54YiVdv3Njage(Qpt%~#^k)Mv}8jP;h! zGIwUu)O_Fi`N43lb7|1gp*aU0B%^ZOfj9>FUCnJRl!a&As0<2Pau%SkZx(h0m}#>*47-zs02i zCpleB*fxnH8SZ=<6O(;Wysv2>SYJSF+-g}1%mXCbOJ^);913!&M| z&716?LRr^&sG8%u`Q-2PS@!T|K|!C;3A9FQLB*+&e;vY}^u4=VtnjP%&sT=yDX=FO zQIvs27V0XlcKi;3)j0DB)+fvb`HWTD5((kJsy>2b-#S7AWpSjB4zs?uzd_YdlUbvl znWj&=fpM{%k4>?y%rm0bgnDO))PXZCe45r?Sdc;^TR= z)$GSD8Lh>k{NdOoibDRYBpP{m+5BX7lfcLZSLVQEOu{B#n$}4hj5<1T@l3Vf2ZC&b z5G3JeM$r{V>)(UcA3I)}H}<_s;gVvAMY2n#e%YWx&Fa9kWlwh7J4|U()2Rr6h;)D< zs}<4oe*?i5D+lEM7}KzrepYOW2%oMgGT{;|w}Yhw|7^neBKuZa#|GLI7>AdMaGr@D zaM_ET%WkR?6Rvj9=n(17IcOrtK9ulKxkbP5V*bEV^hz*$z?g;2Y3u4%03forF?mb9 z{TaC_(uQPkLwshD+!yOx?{k|Ego6CSM-VM#Bh0 z^Y&TZ2EUgZSvP!BT3hAfzS9`0_O*X^<;B?dn?8g zwwIP}>5qobx~K*}N2Kch31Mqh+Mbtv7sfUqz$%>j)5~SYsUtd!3Q6J&Ct7vxi~OS* zS$ScRW`cu7@Zi@9qQdy;DV{~dcX$V59PTDUkf(UpTl@6PfV(M-qK;a` zPHv6`{fYHVva6Y(dR5s-m#`N0!D1oKe-vu)0w2C3}fR!{XjrPAq4OV(M}*jh$|qo z^?v5nMdsuC!PH*w7o42!rVU7g@3hd3^__W);UDq_&Z!YkUc${btG)J+I1paGY{*hs z_qgu%>9^Teckuw|#XtL+!QuCWSJ6{Jzg{(mC|PMNvX~0}JmF-?KxPQ6AKBf}RI&wF z%A%V0UiHk!qT=VT;h;*yr!70x^iO&~ zo`O67rITcwD(%lRhDyAQxd`4n_VSu?qu{3Ecz(mW>RK`uMq8OIPyytl1+%k-_sNFh za~wpEmNx1ZeoJ4%doXTH6k;s5!6FglwNrFN?8sxSMegbNN1U}V(T3-q49{4Y^(`MG z6!*od3_m&dcL7Fy2*M3jzl2{YJRo3`YP4e0;<-wU>unuB8n&NX+Sdaj>PLdginH$~ zTNYWw@5zcj19G#f=u`ZW*7EjGLCA;O&jCTzt2)8v-`V=w^0O8oy+oYWt-zPn11QOO9)4Lr)KDmb(&ga%l)p(Tb0*99l3GAlI8J}f&@JG}pTxTov*SfMWJJiOr8J5ol=s? zZ4iZ>C8K$;!YKg+9^xZbTYWLxwhGxKsmoM%5#)pF?(`yEgLJ*urcV|4c@V z&a17NTv@E@paYJ!hO|DEIx>XUtkrgas6PxlV}J|h$sI?{Cq4zh_=n`5vpLaV>+lY` zEi4H6gChj%QxtdM!9wYZfrO1e&adVZ2Q}n7SiroO$)nuR4%sih( zRm&3Td)&P;+69|OSlF<#qd-DBXX*U6fRasa_xnWxh1cbH}A{C zcv2hdqcfq6e3yO$vJ%OLPvuH#S$U6x-$!avJT5b^cCvi8hsVBH*sR-CsB^Ln*1-8C zhWxBBdMzoGKEiw9@{vAnStp$?P8!>qhVqsxJTwCnoe46kcU}G)y!V9V64Q;bV@chc z?LPiDLImda=vB0tQ_C(q-s+dj4SF|53~bx}-ZzLDTLvIFq^u30zs%panTM>TkSQwc zI0>`xhUQ>0RD)G|UT0NEx>zBY(C>7z=!&JZiNd*OeCMc1lbc-?>4l9wmzuSdIPfzK zSlbt~$ZvHL&=xT`w*LCZ6b7lWOcJlAt`~9=3K<F81m5(e`#HiRgOn;){TW6=UzX!D*IHxiKN!4wnhqL0SMZu|EU; zU#R=Y&}AFEllIXu?F1Kq7l+}*YQU0;IFWD(W7h?yuK@edlFBwT%l{2jVf{Zr6()KX zmjCuu{s*YS#LCEl|DV_Y161)ejHA_FF!?DT0Ah`-yg7#e-v6slJd&P;>^I@ zmdxPHU;+lM89m*WRJzlMRrIO_40sH!PiFeG|w? zL>XlG=vWD$VBm>z06>Tf$uDY%Z}M6ztNlkH6BEn*tC9=TZ}L}-HRRV2<2&}5Pn|6G zSIvn&DhdEe$`H*jzP!B{2l~6$DY1#E5v=Z;%70EbmQBNYXF7e4Wos z6$^_)NK+9YxURc8urjh7iGO8hXlWc4-^kVw2>#iQp_z$+<(+P0Ze?NQ@i+2!7hD2u z(Dw=c{tgSLIg`@@}>#W9=W` z{&O4f%lvrj@MK`1P#|cr^vm?;S2l73V>452BON$N`$v9hZv4m>=vS5sUE{|zpra0%G0MZIO||9>^>mV;vysYU&C=)0(l;}@4qQ4vnjFfCfy)9?nq@@b_`l5hUtozm{&T^(5YLRoaL zIekad+GtWA3hDdMG7AhP3&@qdtDT={=ev?;JQS61&`q&zvHTo;0DN0Ht5xBE(y@0V z8*X^upQh4{MbBcTX~^-Pno8=_KTQR+!aU`wD^i1E)IrC{egr)*;=eSNpnsYQm%XI6 zLlSlMzps$c#Xn6Ya;^Z2=%+RCpQaM9=iR2MVC5ZIFu@;sXjeTDVo9Zs6G!lj^1yq& z;U#+5Dkn_v=;>+7KWpIu>m8MjiPSLFzp=_a_jtO{6C$jTlI_Ncg&T;=vb&8a0yK2t zK+;U?;UBtx&88hq6EL23tn{HP-q3|yQ6dY7!wj?~-xrrjXe4Sb}B z%M#6w&qnlw>Ns91wnTS$AcJz%30bs;iG~$8w~%(b!qFjWC-{m=qU@oYk#W#s?^>px zul(N}XTDU|qPy#8-F_?_mAMI^>sjA=6CL3!tU)xAx-lvGHI~#aG)gYm>Ue&n^{ffF zYcERJUhiStQzneTV9kPZ(Q4M1wht`~CHm5c0mGrlHj9Fr;0!K3w-$-jYj(5aG-%0e zGTn!C-P65>eTBz-I13Am>cWn0;WRV^NsxiSuGo7eegZX!e2`QCOWFj|B6$jbgJ2}_ z?sCgbga6B`wSeQOv+PwxoUH2WK|<(N=2`PIhYJBoERPtx!fS>>@p@058X6_B72ZQF zAvS(^VKpK{PQ@|uwHRpz%CQ~sYdA%pb?dUo^dS_BQz08Q*sGUvzlv|TZmp%&rTW2f z8c{!Ee{9pdp_hp~qc9@sa9@LyEow4@3&BCf8{WVqhB9Q*P4q3*mF3=Lo@&T_L0}$|r|sv&%P!7~4=8_SzmEQD z&9G?5uP^VdKO zBLPi7Luv4sRM4F*8W&QV;u)_VY-G8m8dD}rEpG^OvKm^->J`cX*%qACV_{c`w*tPt z(Pf3QNUU^M1J-S2<+%0)m&(EV_%?9=6I7O;Cop)$i!uj$-(Jl4tcz8}QQw;|TJbk( zMtQ1@S%Qqtf=7Wwlzo}p0FMS??IT|6rIHX{8fFqwKyTmzuQx5RUuQ4m6PQ#E1~QQK z1a}+ei>~^}|NRQYuHP z0(40enuQ?Wrr)0X1o8Yel!fOlm zqK$4ucV5fq`{90Z&toCN+m%?{yU3`cmYnv!(C)aMz1Y2rJ|mD`>5=trL^X=Rpw3mV z<=PhRd=8lAfPbkZLiCCbOJQmd?C-Obvb0c8I%7n?M6}C@Y3tD3`~=j{GdgDR^bt-2 zbpm^*3;03glSFZCr%LOB({(g!lqT3Nq~arl8CA`7&*|w(eD?<((q_*?6~_d()KorC zl+I8woMk4B&Ry}oEnBCSONXxE0R`5SJaQdAt;suKU+V+UvXuPI7E@q@U@B^8(v z(byq0?tR%luB04U`j|lMU|+nAK(qsck{wKd8_3vKlglBuoTGn{HM#?uE}^wX+eKzJ zD<&^OYF!U;2LhKV#7V)!yBzV#SoUk5lY3hC{rtHt4VIVxdzkfWe)u+g9yv^OqShBr zb}UW)yJguc=F^;rZhMLI)HM=xx7ui_0hXbko%{uX()Jy0>)l<%Di`V%hpQHSTmbaT z&ZM5TnrQj{w@=vLfOnxkSvo_0BLDix*u=S8<52Xt$?^Ix)#U?*#i|-6f0q$XY#?fu z&J8w!gro0tkK4_x@%i~a@Q|+HyT6qyJNRbrk4UZrN*J|$g^LPNqQON{=e_#jbsR$l z%#yyKEqK5ljN#DT*&BvXG;BM#2cfZP1y65f_s6N<8u%-tb0o^MU$HdxDPyGi(ezEp z4WEb5gy~%O#9%6j&1%L^OwQxBu`Wl_qW2ftw`~Bb8E2brSkvg#$6yaEPxiM0|FWVX z`3oDLh1{56RrQkdk$w3#qQV45e0Ba9&ug2o)~EvkZ&C&Qhz_R6@iR)9-?j_gY0BK& zDjW%=au!1{38m^Lz#bcmFj7Ln0>`3TVXXVU7f*0PB5Lr}Zgo7;cP-hlWCmh9ySVVpAzY7jjIGa6KOZ~exk{p5MdaF1- z^gKeR2DLTl9BFtN0?~Yw7nUCRE*R}A3l7pYBRO_}W9f@tD1?d8j(pPj&H&wv znNM0|ayX00jRc?MZxVkE7MEy?tu#+6bxi{Ns<7{rx2VlU^-KF?8AywPUIU9B*Z7|K zK|0%I(bNvNTiPqMMPHQAtOV>@xK&_iDcU4+a^X*@U*^@Eh-LV2LoqCYiaVGFppfPb z-~BMw=PE%+wlBZfPejbeN0OWq0vVBKhHYbDt1XCwm9a-yGjw&IJ}n|=P8P3!e*C4_ zE>{T%;iasCRZt&q5vtElcQ|s-sWeiu9E{she@-E1r*keKu;M_%I8du}O~7G~Qe?*I z+2Zoqd^N+~_2!JA_9t*M@&16EO0DU1)2Th84 zyDf8kFtb<@3-b<1llD~;G7#>m{p11?WjTEDbB@VHxC%MGMk7rrCWn)`C9~P25Fhl` zZMZiEorIgpGEk0@s;l>pj8`g_y1LcLN|k!y*Ow-TD5c$bccoZ}wq`DB!8a$f%G>ag znjp*Yc#A6H6)rPOk$wbpr zo&$RpXoeD0;dWJH`78pad;PCye4K0Nl)H3-%>EtvR)uINN84(2eT6Vj9 zY^mff0SC_yT(`@{-?NSlj^}T%Kb8n-lykWhj*zAeVh2ud#L8?&IM3anQ)VeQ!=*tf zm>C2v-E-34AB}Es3f`gFa!T-pgjBOtxh7B})s5vPqTVR^J(z^g&&+iVLEAF>1*a(~fJk+Jz}jCLhD5TAS(H)9@YcW>dU zLnHPI+bkl7bi?+2jtY}k#EI$Wzw+klghXO zsl*|tMFluEy~o-dv;ImTWs?E*&P?Hi7Y8*T;3EjYy=s^hcev3OqgZ!R=G8O@tU5f}{5V;eK_B`%PitsdYx9k>{zSoYOx1LcL> zwk(bbGu#yp9Gv5xN&6oxq43SK$T;!NVN8AAjV{Itj^j6QGg5zKOHOR2FMUJTr+tww z>U|VS%Am0#tiYd56Nk_YK>p#S)5pf&&x>bVPe+4^*+uY%HFr&02KC#4$WV-+KzTOU zRk=NEj{~3Q!ntUE9wb(?A9v4C|CTpXLHffyvJ2RLFq%6mFUJi)9Cw|ynutW=b# z)=R)(97IhXK}Wf1R#*o_`?@GKjS5ox(oju6q11hy0E~5#xZdMuuC)LQ!y;xPh@Zb}$W7o%My(x6w-+(>xYThj>g5tSsm!nt5MLk9yKK)qc2vzxDujt^#VOK3O#dL#t}#i+D( zFuQ{xpC=L(S{4-N28O+~GY*BrSH*cZZ)*l)j+`gb3$=+?3d&%Bu4z|LGd2hKTyaDK zg>58x0=Kkl_?h94spK=68OM}uF=-osB*k)ODVr-HZynXIbBCEdG35^!)i4-0Xe0YH zbPzLy@ZWp2Aas+J1aw1N*4SRhzc$);nz8Tr^kMCYbb{3k^#%pC{97hIlQ zHyjYBHLjO3Zb(wSbzumAEJ0AZAnb+oaaJ)|qJQ;P$t)@pbi)(8Th$P9^+~;A32Leb z!%4JtPNYcGdIDnSo+)#w-EXqkX~i#{>Kqg1-c)|9w2A*E1iLH&>#{ZZdRt4qnTHiT z0IbK(gTJrZSCm3Dt9)(HSsD^CAQf?J{?SB&1909hbd8Y!$azDe)jcA0J4K9d?Q0KC z`%8o>Z76r*?+_+SsU|~7N3h?2qm#Fwh!FqKe2V^&GVWOc#k*AGpcJ4YQ*gK*YI$`D z`tqcN-`u1KtSS-!H&faZ1oBl6LAQ#txb3@Q97A z(u}mM9F8DX7s0igz zC_AlK$|14tYAbainV{dfg@azUL@>K{Uy>vG4=@J^4pLA7Iq6a2-fMX$z^PJPH$TN9 zFIf!V=q$-M9M?dsx69@_?E#2yrvr@<)xs=*in;7sHl~06gN2(xaJZ-!Ua3G2@8B$; z`DY}3Z9z>4sxM={MU_yb$DY8rT3lYFw=xd@IGDnA?&cys7JTFpQe!pm;Z|ND2=aDX zxHv+Zg|IzE$D>wV8+1w6^v9jbQ5DQd@+3pEmKBypQRN=``$mUp<^rn;a?N zq@KfJ$;Rb{Nq=T!5^Z@UC=nML?WY`s@C>^Sw`UYB9ix*uA8N17r@=^Xrd~>ycfyTJ ztXpGmZT}p^(rU4fLCZ{Z)F z2hWAQ?QS0%g|0K%yL(xfooK?@*(8xJWE~SoaS#K}Z&465{ZfzOI|8aVS!)xh)X-0}4d#apDCETPZPDgeI7JIA_^B+4%?l)JXagPOOBwL!E=*q+B^K%+lYC^%*~`@lg!r7aRG(yw>T3RD>}UG2#!MTueAB%sH4c?F~C% z=_Sbo8{SZj_==tH6cx0Y79;C*Gip8?*{C!ma)!Je(M4;%ctIvZl2w?c-}yA|37JZQFnMRY#QTB9 zP})*CpuZ<+8(Hn=p5gpzbd7{sR@vsheEf$d60fZX7`~6j2bsCRV(Sx7@XM$$PVFmg z_rU5;Bdg54L*fxIlVM8wmRjjuz|F@ab9cGUVJRSMkr*qg4;jk88Kr72P5yqKg{esf z7r7Hvz7p2SJ&wb4^biwoCPJ0x*+EK&{3#v*aOv16Q0W-pPNTkoedWhOB-9LXUA~et zz%ML!b!`hc=((8s6Ed}%2KOZul9lq>qte;s=tIu1D%^MBGAAGSfONBU3%$%+Z^y=)=P9QLq&_l*3^$0* zw!*|LKELfKwGN2(XJ-`;Iket{CMhcAKx8_nJ0VUD@O5P5-|i&@ z1T+^yrbZvmO)Y|qA@%EeH64{cOWdY+Z*@V?*UaxTpFilp;OaA}AXw=R0(r$VRHU$Uh6A zdb1cIxn3(>eCYUm{PAWnS_oh==*Ne4UgpjGF1?1FI6W|xuIOi;VJ+Em${Ouahx7jC z+5RN=r4E@uDWSa~TKk&E8;1@z3=GVS7wq?SLEdBhd}T!9^biW_V8;|Uu~i9TYPW^s zQF%IEw9z#KMHijxk(eic6_Bm1qGnQBNS3K!k}&H@1_fG)hw=t`%0LwSfQ1Ufj%=Gs zkC3LWw!v=uo3(nJ*zacM>4=`loo5>WI5Wz>9c;&IWs3og9_U}3461^W<(LI5=wO@N z^)?^lb+Gv^Pw;PR84fu*q0N#|04*a;T-Zn-t2LvZnbKgzuL(4Pu)2l=*$APN1PB!9 zi-G84l)AbhbH=`VrY^``ehv$tQ|eGUj@Jj&n(J@c$U#v_&$nqHq@#j|ngs%~IeK3% zW(yf?aEd4A@Px!Z$aXhb3Dg}CixGFw1?I_c$wv&50CX(eJdS7jL#DPBj5dAHjc>S5 zbUEYz$Q^2nysdh(=eTRMVB}?V zl>k5!7rMemlsB7SW{T4hX4){3d>)&ijfx*mD*Yhj_ey&b>xVHGa>BWDRoyLeXKGbe zf#c9=4Dx`61rLX7!V&LPRf=7np41(q0atPA)H}h@WCgG=v5S;ot(2u+LjL8$3v>pF zz6p^3@eYJ&(!>}5iI^HHZ7s$z9gs)W!iXsl{~<&h@%VeyafJ^vA8g7iZ;}GM@a<&Olyo znHN947bOpFqavRxz6;cnBp7I_{jB0e%`Pvy%O$iRPieK%8(4c`LYSNkekzS8y|{r^ z;y22r-T=LEZ3)w7k*)qA&`7KuDvxp1%kn4U3Cl1de?YJ<_6@pr;bWJ@ZYaVQ;-$EW z=y&)2>mE5=GQMY*qS0VW92&T}3$%-E!^rT*LKw|(s;r@@FuJ%b{5>Lv1&FJ|c{-o9 z^nNoSolmiu9l?m^l}(<0%c1o(4OG;inA^#sI@QR#eC!x`C~Pn$%(O`~IeFP(B$072Z1Q&CAginhmZH_> ztc@ZcX$e3yqt;UZ4YXOTqbO)d4%6`Yt!og=3e*QQ5cDf~lVyTg?ms)Ppu5HOCB;#i zvlqprnaYK^b_=o7lgv9lV6(0hk^8H{=P;ewSrX2~aBrv2#x_H7!)~g?pNZkY@nP$- zGu;|&y%clnoZn5GS>l8pbGlt^w(46?;UdV}iW^X`gww+>*VZ}L=5$zph|o`xq%gCaZ6pS zZe~c}(Vbz;>!N_Pxw}XYqqDL{1D{WeEp8SyS%iQw-L{EuKjq#SI+T@3;kF<-?BI=MpMb5wufz?hXsL1c`RCld zF3q{nzzs##g0ZDvC>3YSoM0_=ICj#(qT{iQ@uV%q#Ev7Ny&;ht4OsLX)Ww#{r9p3>vTM{Id;8gHbqWOs)}bg=W{)EP&|aZEv)xP2B7pCu zag%()YHJvD{Jy)ml$h#?0b|ft@s?qLwnB$9jsoW!49>4#g7+BNmT&@)u0Jm@_MlyH z!M$4l!Pq_a2o`l&0503MjV{}^ZC4k&Y}>YN+qP}nwx=dDAKzr=H{6`$?0wgHR+fRD zpRT&zwAyu&Yw;)}hF0F5HI^q-8yEWQ>18F!fRqUA{wgNYCqH1N^lY^9TM8VMA3QMP zT3%!)*ew??`^lIo5flp4MK|3cF@2y+R#xMb~&s;P6X}smz6bA?t@0!6B%3-2g(t>h3(t3BO-Sk`Xmza&x`7nwpZWIefRJ+ zbgJVTK%FlM%I5W2KVf-$3vVSOu>I)xtgQXr=L@fxtMrfFm2i_lX3!!d8qcF(dMyx0 zy6k+BE6fOC2oneUIbz9ODL5B9q(di!Wq{(9;w)ijD=j$1);?fPaq#m1O@V>Qy;TIF zt`qp=u3bz4&Q^Xd>|pV-dQtcH;}0C^WMmk&Mi3hzmZ+Vsb=D=#Q|H(J#uPTDINrI~ z8^uQ6N)N8ANTftkDdfjE+MvYIro|0*J4Kx62XYZqE64075%geKCNdlzSoBz=7(RE^ z9v*ifj$uJ}YgHz5$swZgKf0+I$t(V-+afJ(=2t!fHb=|A|}no^hy z{`>85mTIcZubHmt0?HK8Z9(P!KJsR4v3U{OKCn=;*Sh*Vg&{~qgPfqPyHS^`$myp_ zSENTq|CA=Z@zgr(+JY5Z++Cwg9Eh_i>!x12b)%9!vXm3-^;4v$m-0T)_)E~QW$1(H zkU13f36WBOEyC?r{KS%aOR|0;8OLd%{8<($t~zP+W6xYY+SRGV0)iI#3Tyv?;#LI^ zKt|f$AfXR3eXs<`KcKOu6mK9!^V1U9D~rl<)@=4@67)6ImT)}06WOxt<>dL(o>xbt zfN2#t{$%I03KsB$wYgt18VSerM$Zy{d0xI6veE3Uz%cG+Z)eJ(`Db!lq~JQIs<&@o zDuPu)U~R&KwdPMVdkdt_ZN%+G)Jx0|oU~}Hn6$&m#^4k_ys{d;CBz^1QV$Acr2neMNaK2oCgd)m6{lJ zXH|{#ku$fiaNsU+mpT~k1+K&H$2vK&aqgjVazVyl4N1IwLUB!gjcfH-?|SqMso|$$?EvOnQY;pAkY`uY$5uw=>u*A%GTUiDP$|= zj#r95txSS~Hqlp<-dgWPi>RfjBO|SO2LfzLTb6$mOda&@bB?jeMeEPT9)T?R^0hOS zZtci8?IFh~5$?H8$Ft1`WdcVIXGDJ%KgtFnl8bfQqX@P>yqapX>IdZ7UttWp`F;!k zC~uxvt0XOO|1QX;uW&1c7|9vy6F6@|<3ZQMd9EbEgN8a1`}>JnV^1tr1I)^^f4T}c zBd=;n=zU@$ix(KK8EmtF$@Mc^XNLb-X8eDS>8>qbD<0ke8z#_DGWr7nV^UgCrU7riq^ zE|nH8%^h=o=zuQ-xcyx|qUdI3&>9sj5h19(|7G|C zTd{=Y1YdMj93L~~5S^_Xc;wRS9tVi$J)*M{<;U`tWh43VSMl^*YJk$Z83&$EE9j0h z5jlM}j>}(>AHXBJ^V%F5MY*_rb86=n-!>L={-*I^ljvdzXQt7=&VG4C<#eAcz76Z+ zg_Q&@{+Bse#({*O!f~?Lf@|ky9GLB? zm54S$j8j*^u(p_=+&Q)h;FtwBBW)pOQtzGe%THl)uT#TmN?~mUpbr*MXR8H`kNNYD zHfk!4R&krJMZL@G<+0pvkTYL=*QsjFlMzMe);7K#89e0|vgJ_FWvuaPk}p*FDG38P z^w+}0Q1R!qol_al_#D+*A>~0Ro+?`04-OuZh4@psTu;qAOm*^Y94Wj|&q-gz<`WD< z09hJB>@Qv`62mc@ngWYvlG>Lmj)>!LA}_Kx#x4B%1jcO)@Q0rW&rnaxOg^ z7u3}~eyRAstU|R>=hXcTUbB$wUmRO!bemJ6_^4JxT=a6V-E0X|PME<`@T5}h(TcJg zf3r7BQ}#605W!iiwn z6`Qk+v0kFIRR)r#$ek6zT%| z?_MNPEGJL0hqfc#4Tg%V%F&08KJ0W;LIT=;!6I`K193~0YYivoI!rlCby9*mPW357 zLtxRG8D4%Y^_lp`@zH)p*+)WLsfJi(&Pg2RrUQ~wBeFI%K^b4GOzw9VwS(5op&w6L zZnxPy|8MJgTXrK|h`s~UY4lBSbf3Dnc@6ejlnsbTqO-vum|<$nV}#Wl{_Hsad|XeT z$zJk7TYyrqOh$&lwPhUsnjHn`%qNvE6Ql6EK1HY1Q)koj5z|SUvM|TaR0Lu$ako{s zMWl>JjK7YX$fNn8DlN|)_8u8_E{61h<5c<1r^I5v@MrQ6$?aDkB3|{Xg)%;!U*jxGYeKL1_qz; zEMR=B$s@pj`Gbg1_LmdwjKibe>gp3)SMfezlZ55=8%J!?F(Rl^B!@1fPY4&21pH9B z!%TFF=&1iLuDx@y+i>pTN2 zk$y0cAJYfS%NgX4dTXMCPZzEOn++cL-ed~f?CGjuXY;|A_CMxNv zgMwrit6ZY;*TcK*V!I{u3P_4maH=np#ja=t(EvW0^AL1JVR8@`nD}0h#}D-WO>L~7 zpJ)aa7v*&f54Px$>I#|2lRv(O9UiYG_yJhL4?;h#c;cns07`*t`{xN_EJ|JF5+4^y zd0^9Ve$U->mL=8Cgjd={f|eyrf_|@Zknj}QfLBF=b}3(V<#-kxwwuPfz;ENN`pazM z))-i-)!%zB&DLUflj!Z~BH=sVag&fD{ZF3QnMcJ!sLs0r-&AFcVG36pua?}>2**i4 zL`a*;=h5kfnQuvA?p;Ue-D=GruD1~RexRspKVQLGW?GHeb?pLzC|2$TGF2|DWLouv zU69JrO|44xg>3!FzbJG!Z%Lo9J77|$)i@EHpEzZN+Ldc9P+lvQIZVkkN_6`JCod$8 zjmM~91a!Vf1E|0M(Js#a-a1$YxN^IrxO$${AS_Gn zgG~jrH7=GDw*N{l$3|pX=08;^x8={q*}G=X>Ktg&`(WpB$Rh`T2|BxReg zAdDfikbOA_t&Z3ryMs1jd;Xh3am#4YiL9LQAyXpdsXDSe4zRDpHMXFnwjs@zhX`cu z$J9i>c@*YlPp#ly2Wg2IM|T}TiyM3rW0O8XQGQCh!2E1qju*mr21MQE|F_1 zjmQ9vrpz}-kKQS+)qtX%1;mv-gmzPi%KSEH|B1x{632=r=g?suYW9jt4AA-~m#WKbkl2f7!W5hVyubRZ9$2`eF%wbL4hS#}Px zRHfAouE*JZ7%V0NR1Day8mgLk2{e6`4mW4~9Y&QRW$08fS z;BY%}GD1^}~ug8M;u4SI;J3*LTd98f@ad zH)au=F-L1P!$G~23ye->e4TAHR)@Ho(t}lU2h@i4@f;;a2(6*eV8=>-C1|g5-1ka) z2+_IUc%v0mUR$0yBim_PqGwBjhsBOCjjg)3_~2ou=19`j%+jte?+XH&~stpEtRoJ zC`AAYgx3qb6cC3NoJ*G|4pdgCgqDV46-SDwU6L{bQ63jOaE8hMIk^6loTf8r{p^Ju z?m3cLAeOB^e83bFTkrL32Yt@fQP*wlBV)ZQX)$o?{F139TythJ(_c!x0AB9qV#8cO z%1Z0~484Hf`l-$|lyaup^dJX&^fqmRsHQ_;zkE$e9nGDU-1aj0NhQ|egF6H+98+@Puy}w8APW2m&c8~MNNL~|( z+;b+lMMN{uLz#o#ykULbW>)+((yt}uO2rH9jqFraO)HB~1jfO>&Ker1Vm@^-o%rZ9_Vy2IOkc29-S*(Qle?o1cSw;M5NWR2ZQoObw`$zdYl0$$C>dX@NB za~_{SPcZ~-$7Li>AAUj+6FFKGO6q0t(av!f zH_~5Vs{)F=1m*H;OMxKyH{UVB7pW$kHpcwC)D_P#QK{)SL|++J_-K&Vi)?wt`Mb1e z3`73X1GS>6r-&Q4p$ib}wK{G45$2`nsu4(rD;-3P-=sk$X01uLYn0#XbCgRgUg07& zH;-9y*Xa^OjTLqG`m4`4-8qXqqd6SR+&ol;-Bc4R;Tlaf_ePEe4?_~;DEXj7h~WZ@kT?*rsJ!pMsIRsK(3uhNtU-3-@a1sqPT`S?cUaNruZIc;-Ef_ zod6@E-d)#%k#}M$eLG%Zd$yVyqS9k7Vi4dBjyk9#cs@?;1R~6SyC5$_y<2jX;(~lJ zbw0e#EUIXkov+9lK?6}Vcp0O@9SW`ZqNX@VX<^FiRWH-i4sDTvDF0WLJgY zY8!3{3I>8j$qmq8Q@!)(zVD=wwQj>4cxyvb8`%b|b|`(-{iCaF8;B8ve^EA0Tl)6F zm+c`>5w_)6S0wUb!gUw!oPI5TA3qe8{U?v1Y#e-3b+DWrp29pExg(nQXqUIn{0Q>8 zoahdnNien-cwx4!L_xsBVaAO3SFJNQrA18?O^uS**t!eT-Z(^!C;Xc#ZyK+!&FD;G zp+vMx3%$n+-yomgJshdZIix?)vB%_)k!tt+>0^oj>h7$)bZeOEvR~I>%GIP8#4a-E zsu)Y3s5)_k(kh!ZBMySnD~+=pnRoBm5vDcb#O&DF*b|0WUrOTq;&6A8s_vdgf~dICbgjGiI`WgeYJreqgwPjW@x!-9lb zGj0gloNo;+A7>LL)%|bBGn-5oUR2nh(DQ1)4|xoU{(5Fq(AX==QlNw zD6`I~@-FpBw$TqocYa&1=%TclW5$HGt5AN^JZ0RtX@3>HRSwB*lzSjZ3BQJHPlr%5 zh`g!fMNF?e!yfHcJy7hbxIG*Z4iHbk%Q;=1POce9JQH}D2lsB$w8rS(<{Y$jCPq2$ zhgzw?L*kdR8R#aZ2Orf(5zn^EIw@6x>t>l2EhWUUze4&B_B0=UwC|3_K(Le_V<1rc*a50`G0;r$k@r%IKGfv$Ai(@-edy1WAkLR#l!j3DLsxE?a1 zc5SOV(rJzE2?i3%#6_K6nDfW3ZGR>k$g|ZKb0q!tGQ&tQ0_8E7tjrjJktY$WHsfr3 z-Zil9%wInxZaPpSoc+B06#m*%5?X5*y5>Lyah^FXKnk`0rkcuPM0S}W70B6JPBs}_ zf0Q;%WrfLI^BTPj4TfXp+s_?oR9vBY$M2Dsz)DdZic!kBHXj`wYDttp#xSItba9R9gWkXf+46Q(91)aSHwzOWN;lx zGdRMb^pV(^C$K4Gh!Yt}`HNLgs8KUtaD3^*9W1^hk76arsMYrv2x7DuL!;}FP|Z#4 zygnVo80LfqIqASS6Tqol*U~i=o{f$nI@3T{=uk&NlpB0zzyihxG}dE;qQexr<*Ce` z8o>1X&MtMM+tZb5W|-otu}6d6v;joypTRM;et$C{)#o#H2g>;HNU4ABKV=46A9k2b zb9-!1rKdg)ETfgKyD2s4KFh|NgzOMdDx2LqX9%=1ha1`l6^9-;6AS%Gd~C*OJ;O8w z?}O&{iIpxQid1EsbPywt?OsIr#-0SKC71WN*F+{$UTr4CZw!11-CB_4-A&h`sw207 z1Q%t_pN-YG3EzGJ|4nAt;xOu_#`A?9AEl^qr2Lt&$(&$KX=DJipXHi3ramAG;p6Li|qt=Noc?jo#LNq4dPW>kFR(V3}5 zaIL`MNl)%^m;{${s+G_uKF8I|FqX9y(WkTC1lfi^MX*J8wi+vbXhy}+A zLUX`9S~4RcoW>h(32|P;#*C`b&Q}t1P?ykU9R6~M=sITm?IV~IOM>6~BGI}%oSsGeXrbGgs1ul`V@;uR3oog7< zliJ0O%5>c6XN-TMCt-Q^Jp}$94nO&KkPK@%Kap_a5si|1U)YfV*4rl17;gAR>@(m; z^Dqi-o@~SAMEwK%j-nl*0HCK(=^bKORyrHLH^bcF_q_b~mbaXnR}8e^^R!`bC_}wG zQ|j_%SGQxVYZNp#q;!ZjWl=bx_cY)W&*kw1_oLnaFW#z=-1i%`FVkMkzPWGxa`W1O zzoNt4(%)zhoy_-e7Zsm895ftQJ5O~+Q9d1?VTf{jhACKlKqp~s>t%yg6hSZ&+$vI! z_Dl?Oc+0>df%{@C_?NGhNjePgs5W9o;kh zkLJV_$&RM00=Wsme116fDA61&H>FJ!XYD>^*HRnb3-QycGf*Gg#Yko6fl?nnPEJlAF__{a;(on@5!HA?U7~-hbmf$vE;YBA1`@myy2g zHTo)t@jPC8Z!_VuZ|X_+uZt#Ftu8{0tCV7e1#9flaZN0Lk_3ekXjGhOKYHnk^$!Pn zEdv+Em@QXaWk)Q?u$mEn5D9XyV zS_Ihli-+J4o=teJfz+AqPcVE+jX(!MTyBm*wd)Si)nvR3RBhA-2vAS>8Xj%z)@+h* z$a?srLy1xuNpQiAb6=H8%>!3^eC-PW<7D7p6BQ0! zCt}upug1Fe$*5b}O>hvi;l#Z%sgL?l{;42Z2maSB9t0X&Xr@;NqS7E#!MwKWSm%0# zrc(a#4e^=Yuc)uD4)GitYyLzD1M-c%S`1e|(5ZQ2<9PM33e1eQK23pW0KOl~ZC`$oq`rjoK`U9n z1m#dXq5Y30rBCGTP$F`uBzN258P2 z*n%V5OWDXp3!|~exO^TcaSfz}Fw^WlurDjz#@02F$~pX?aS$BEE=!j{EF%A*ykrdI za(It^uimSu3&$L%ksi>lv|8Y5A&hqT>>e9;MCDIL{Q5dPH2evt4OTPHh3C z*`8|q$d$_L^BCb6X7vH|ul^-#OK307a?*x3Ri5}r?hHmUjZe3wDRFr*UkmlaU}nJ- zgKHyapFQFp$zQERQrOdxh`dS%L`yV{5%@FgiCH3{qh+KBi$G>#pS#r)|Mv&WBGA{vIq78;uZ&>=MH8Mn1KC}XF_kHMN+Yc z%ny{vaVL%VsvL(1nNfo6#{iU0pE7IwX-p)c^4M!olf6ReH`gI zG?J^v1Lz&QumzYOa$|I6wTu|nLAxCLF z@2YVV6JGutzrq$dF5X6+GU)A6fcB~!ttn^4jzrE3W{Te8Jtz!{*!#B+zP zl~*ttiYK3i_;e2W;E!nnQd?NhYMU^_C}8T2tCRFi%=y?OR#U0vwf}TFfZSt|D!ekV z5T!a>nRv{rlS~ieF()T>ZHoTJDZ4P1-5$>03a~=mNXI|mID-s~=9+y(lM#fco5Z!4 zmp@>2bOGVJ0W>H-bh%Cl8OHkrY3cC#EM#9?a_(*dm5XIQKGHjA-rm_FbZ_hlZS9bL z)tup^%QCtlT%`cDm!Slz*f8T-i~vqc7N_FRyfP9`r^dEC8Ad0A2qV&KudejcXm-PDG-j|4Hb ziX7_#-u~l56KWMCCrE^OlwoFFcZfc1MXL|+k~FQ~D@nwGj4pO1 zu5uGnI^&c!#ZDvP>-;;;cF^A-ciE&VG|PJcGn)Zy9^Iv+-Uhl82OykjTnicUlm)fX zNME$Ad3MJl4z7iKhO^m0>c(%Lf~vb5oArda0bL4_&*F2b_bo7k&W;X6rFR=eVDz*9 zG)4xKu7q3EG({HHOJ_6e@PQkht++o%wYE7& za}hpkCbwq{Es*V3WIaZ+v+%g_;nP!=(+MX%Jv#Km5Xuq4$WaRq!Qh$B(#Y+uxJv6~ zK^<4<)4XXNRqlsGMb@gNot|^GjMLD|EA`CRV1JR6xXV+D_p++W-+QV{WQRznekR$< zxy^kS_siAteS5;VJ_t6$mWqGTQ1+#m!W+uIXeV5-E@>khXP;MxA}g(e{$m4qHbd|M zJxbqS`-ud^`6okE$n+jJN#9miqD~2XN$SLeaPxCJ>ot8A?tlH^h{Spf!+m~xu6#IU zpR{e5x*fpGJHJ^r7zFu*#|7JXiqvGDe5cn+0Yu;H*X}6yuM{N%L6$z9n>e84uJZsx zrEuWp@>NN?^)tg%%JEZI{0(zC?im^;U4I*3u#}~(vg#ww5omfE;JzoQ(AU*>z7Xk! zNf^IFZtcuEU!4LA7_Q?B%R1oMCYsrp9aS-7sS>M?1T1P~rbLI$un^_Z-t~Prax84F zAVN)?jK@8+l=fJoEv7aI)~TTLmG1fXQ8PvS26=S6Sh%hlAng`X2%X7CM=h+{dUJVN z#nbEtj^uq79O7^=KS3aaPNF5HUwUj%%6r$hQr?X<8h?WGs}qw$J2f-MZ9D$&BX-dE zG2XG{uKvHw?Al+$#4m7}x5G`g#H2>H@L_1X-a5NvPfR2GgYZceb}W4nB7G=^)J6b( zWdIUU4d@o#!lJDcVyPbcKdWS@`1!>a)GGGI2Y+Srh$2^r?qPb0Vl9VVxYrYCVEJ2n za%i+Pcl>06vC^%ajH@Mkp7LPOA2O~biVsRHjguiXX}q4OC^rUe`M2(f6RcVH$NixS zE&BvOMDt86sayZ?JB+u|EsY*D?|a2a-M~>h*^4Q!az#t_e__-GRDRlHn8zPzjQ<=_ z2J4$vmlf?oMo5( zHof#XgxOJOyha_bHQ1)O+`ww!r3h3t^fW*bs_jjovj0XIdV4)biQA8@PlvW;pn1v- zbteU&cjKkU)qt%=2;Eho`UxI{Ywx(qL}PFljfU8#@&i82MxZ_*K%;T3%Zz{Q*OA`7 z`f>gGo0oGZ=A2bYLH_VTW4e8c-5(mh82Dn1rzR#Ip7z2J3Hm&CEi|1!2fJq6e*##y zwD;&_(oB7wi7L!*kvc5i;c`j=3px*EEo{dY0>@;mShNZXS zOSUU{QL2|4u_*&?)@d|kax^47%$#-S=FrZxCWR9KMk7#Y4q1TGTna1(`)7i`o%Wl^ z(!5pKG<1Cj7tkM#?QA;fCU!;#nL&{L-*@Vi>U?yD!d0B2{fBIeV_cu7k-4)Bm6Q?1 z{Bi~Fw=$2vrA3l*9+tz7?)S$Gi&&k2z#TvK~EZ3gHYra^a*Xxl_P;vY8D zt`$?@Kl)jpGCO?{V)kHS;N*}j?wa6WYo6WE$q29Ki)HR82?clCH}w@9SjK4ucHtFe zH=I~F+rO1YOrH#EF{d|rxy~IjrBHl^UKub~1O^0Wz*W`-1jt2&2U(#mI5cZibHdLQ zG=GQrw2?~py+$lJ?Vp%~!DAPgR;)jHSW-og<1RKozjtg4Z9jYKZ=GchyhD(D zxKS{g5N71f8dkYClPNT0te$;RUD7+d=|xdl2V9PuX{Gn09py6t3oYcm#Z-aB_VErc}n-qlu9z~l^jt%%H06~i=5$Gf8LAUJuS&E9YWP+;0wFB9y{JAh*)W(q>RgRXjU;DdlF}j*-VEhx^xw=z)}3sAa@ev34zHagFUSY+IZWgCFmZN`%@I(U zJTZ+@Z357Hk|AyplacL4ku) z^S3B4YmY2;Hh#3B4ZPd!wICxhM~XSNvQ}1yc2M9}L&k!pK?Bij@zk;w;+YW2hghBE#Lb76epRG8pbaMcuvYL_V zC1;}XV&0n%a$3gNObQ%q5Q8|3@#^}9)|I0GiRi&xxGlpUTMIe9qm2zT>w6eiH{Th0 zR%Gme;M1w5(0v-wb-zt?O-2RSnLYN&7>) zqWS@P=|l>)0(3E4yVF>o)6kxlPCWFo4?;S)in($gx}ck~VuwP^=ma}RBuH)s(N=BA zj9bJJX6V-OkH}e?B($*ci1W8Cyb)w&mmAGt9#uw{P}`jD8y z?ATde=^l6#k)dBU+vj*kYdP`REqEFOib|-3V1Ok#D|go|m80KK(b3lhGV7}yJD{%! z#N8Bsb;LZQCLn=t3HPL^-l6~H3{Rl{=oXo{M3+-?aqi~G-r z$=9Jd$OUgT@o4jcPwntgRSBk}fL455J1`N3`KP5hPpA(7hyjb8V65y(I_~71v!AZP zceRG>_mxp!)zckEEO1+R*9|6`h|VGN6vSI)|x`e!4>npv&a{+ z{H|Fm?SNk^Ffx7Y$v5T_?A)A0Fepd=S&2a;wwtaH44cD)Zduqkb9p994r-de6qOo&h6TM zPv+|sM(a&(LgTvz1kN|xwkS&IMz$qppbR%I1hSe5lMGDCe)~a;!;c)IPK&bUPG*^Y z7^z8_kYjRL&Tm3md9tiM?#E0J=6lmxW@yh|Wjpa!PQTT7vECeH@!NR zYI_e6>l}!zPyZ%Gh@fZ+XeNn;-($mPWG)Yc&$-I;JCjMpkWC^P;HuzMNiCzoP@hOI zeKtD!_zSkyoTm5pc;_rP$yDdTHwAm;BR-ISha1S_qst;Tr@!sW`uZE70G1}yh;)*V zY&h}RGm#D_UZl~Jmo{0Bll58w%0(Bhx^MfA?pIymqj}nXOJ|slLX8^YYEH75JeSRMn~s=B!sHMz$;LDF(M1)y?oFu~5REAQ&ytUPlbRVr5T}N9ADG4_(`@p(U zxc4N6c|J6vfglv*#4jNe|8Vs}KQ~@Kv%wwl2W9h5rByNWVU-DCMFc|9$B)J?w7A&42P?XKp4n8 z^CIa;IJA}Mfhv`L@kTwIsk$c9IcfYj3=K~k2iU{w3LGHlV7P+M7{3*Mz=?tJ0vMy!lp8-3O;nr-BTi*>*#id?`Z58f6l)Cr9E^r%G z=p^2(ZYy#TTx~;Bw}Os2KgTl;M}arRs6a0SNRCxPW6bQ|t*s-r4!T&xB(LfH6_PW{ z&UTX#j$AN>opWW{J|Lwc-ZXwRlqPggws5~bmP)(Sc4bn63iBAxj8F^~_~rFb&Wez} z--3DGh2ZM6hA?Deh{Od2Z(f@T{;`@ChRbWhr6#E z9M(f^N}?5&3fz6!zWC`o6|%K4GQ|5Ms8$J|!N9D8mlW^vNA7jHB!*$M%=5WIOeI|7 z>i$~3bV5HdSM?<)!~-^KBz%p{1K!o&AbZ43wW7K7bX0BcQFbaQ|Dak5ckLl96 z@a|yuxvM$HC~&#r1#N(f#l|YaS^)Oh_CX3HSjoRoXaa-%E8^O!^`YmxjfbFkGAKkk!MCBV^(smMQH z@9`%?>>;~XiwI^*lwM%l%!Z(hv!O1|kHDw#)FAICG_A@Q)LN5!>34e`1gx)D-RNrneeY>3EJ~`Vu z`0v~SJ(FfA_!2ot{m-CS2AC}(tKNbaz5FeYrISkEm6kd(ayML{ip6aVrO#`CNZAR0 zE~S!D6_3g8S5hn=3&0A<1v2A3bfb#Bdme*!y6v_4Q@15Yf<%jU{Lvhr9#!0WWQvJX z?1#{5iCsoHUqW|@p5-O|bss~wcvkEJM!-MOs))4nc-6L>!- zy$ic`LsVuOllP=oCZV+;BVK`qIC?bZZiBUpp&}z$dcFBUwFTWQzXOxSVy5Q0xOac^Vz_QOUxOdJ z8E^BT1^{%q4X4jx!<${3-`VYX1pauJK39yR4h>eX49kYp>~7`&KX*BCNv_t4t+!et zX)x4L`RhJ$p1DJB0e~rTa7n*^GiebrGe&x? zk_u1Ah=@sB4D}Xl?DcT)xJ%Onn|wV=m6B#iZ0Xs+|AtgjxFNl=A_MW4dyb6(GXZaJ zTX2isyk0l4(AMJjl1Xf2uBEk~^H4P=_Ovj`7kP^yW80~?f42B9&%ZL%Cg%hgkUuuG z{)=9>-mdcghyR0>SMjl^NUy0{;3h%}LcOR*`u(L_#|Ao;t9&cqJ`}Eu1%3r zZsR`O$Z<|=EYF)f=w%Ww)5&ONMuPiskkijHiR!kALx`mFAGF}6azPtc3|j4lctUqS zP7()WolP)>9Bj5)boFVCCLdfD7J7bC47b{N9Fy6zrPp^&j; zax>Xpzbn?xC;PG*_o9&#G?eHAda~e=Rs> zn9USN%*~!)BCO32Y6w~eT~Ye zaFZZETPq^vDEW|`9!PiYA~QWtj_4R@S0DLYydFZmMz!;Tmr^k`7MzZs9-%6}7xJ9? zCQ^Ke3l_f?J7U&M-?W#nggE!~crlvsbQfI?Gqu;PZawoh2ZqD519_N|3=u_|iocBF zYz_-8RpE_*^hp)g1^IJ=nNP}X7y8MG53=;(-1peDs@Sj~@DlH_P+SwMd^UmMgq3PEC-A?dHGNgDK4i9XD~YZU zsSqw%iIOoyYT!WT-`Y?$XS4g$m>A0z@&GggkmS4Ydcxj$9^{VI8*Gt(+?oG!(7hXC zma}^dus&#Tdy!yhgmvUU#b^LAe&-yZy^K=?r^4M_hw`qX>p5{v1<-)3T!&>9E4&4? zYa1H`8_QuIb!Zo7e^)FA+kGi90;9EI!J_VM0zj<0c4Ux#%-QMBs&b}|2wRUXWkk?x zhX#_3`vz2Y4IHZBzZ;CnRg|`Yf^$6ET345wNfPcBOE=CVIX!hIg{mm2zajppS$r2mHL$6qeC3^5{)&snx1X0emHBBDQ% zA3~7P2dRw%TPVRUq}$!~0(@o#RTzXjV>Yoc0U%$QY)&F1t)q!{*@O6k#o~xqG&LS+ zwP03CRH1+=_td8^CEzpO>Y?%GmrH)21L}zh&Ur-XLL;FE622hMrG`w!{f-9Q5`u%k z=QQEn7pskf!po(3o@fkMxtYY1_WJxR;FPu0wSWY3bSb2cGZZt)tMj2OtF@0ukOmih z4by!V4^9Gyim_q~!>ua2%S+Av{L!j0mM5FHtVgM-%hR%N3wq*=c3Oc}1=e9GrLI64 z&_R|Zo))Q245mx9E`&!~V6{x?ljH0;Ol^O%YT*LW_AdAdbI~|3Oe)Xho&l#4a zW4r$Z7ZH*W8bqY656T5IB2`&*nhJ}u7AqQS7!e;KtahjiOlok@)w^krZVdol6}3pE zYGu+6S_rwMDsA!y^)$7d*#kM~+CNpg)oq_=i9AHR&HE*pR+z5Q*}rBco!$I~k@M}l ze=a(l6}b;q+1!@Uoc!1k`V@|w)OSOA6FcT|&nNNI)fgx-H<7yl_&*`$?Ee>H&cMdP z%JH8Na|U(>X14z?V&0D?MB=wC$U&8 ztKHQjAoNy;*A?cbmP4b%qtlada|tGEB*o;UpmU2%eaVQBYT4!;m>cTQ79E%k!AmhP zflwl?039EFeFz+9g#iGuwb(qEkd?#$$^h4X33C%E9w8=bCJ*2GsG_KbG(Sq;g|)S` zm9@UG_3f~X_Z%WnVq)KdGQ{51$$1C@0^$lXGFl)4)Hqe3Pz(+1wF?a(Ywyzq}^A(3B?>l3}GZ>29e>R!GV3o|3@vu7WyZ>GtfjX4U) ziHPtBrB55!?<}->)>;PU239}{_V0qy?5@3^U%`ChJnidm$bSpwT%frhJ>7}f1zl;> z`~k`DdQyTPH4l1H7eD#0Z`Rs8rKe|@0Ln0qlK`BitfBzeJP9kmg1KKpS65uq!LMNc zH~CjE*Ci(=RwD*JZ8E_Ij^=+zJlcw+&x2*C7oz;oMCsyT*nUZV*rs${L`a z^bncUMpw_o7t+Oo+r>J6YOyCe;ola*S&)y7JoXX)#hm$Ey+;}sS4Jy=4n(lI4m#Ow=|63$r3@Cv2@5mET@#pHO(0| zm#pu&H2Q;VCDnS&pj*rl89ds8je#Ba#Zn<_h={qcB+f9g&^+;Ml584{hWEt{CYtWy zek0GQXL2|+my$GNFk(T+Ai-><;^x@+UjQ>e%)eCK6<7bmV>QD`rC$ZsX8ZAh9g4fT6!zs^%c4*`{mnQ z=nv`s4;Vx>eb$p=cW?y`|5zEEn}8%-5PSe~gyAgB|ru2b~W3^IZ-4V1T@c>@7 z?3FV_W4IQ+9>(-E8Ite1wO8`=dC4A)3;i_$uHdKrU;*Vunxu})`w~C{J1S>L zD=TV&735YL(wN?2`;hq*)r3K2^Vfxw5OMLTgbKAIAc|K#L#(xrQc}Rj{+{EOD3& z-NBB@wv4s2RM=qo0eMt2DbiY}ORT5*c57B!)>txjb6x(DX?u+^{f zmlvni2Vt?Um34^fl!lzp9z@>&hK&?+B~_lSv%09yVAg`5bvek^#57HV?v~tCf#}YT z^EpTQsC|HSW{6zg=8J0dXY@eh%zPSI>T??b964MZSKr}HJ~Bv>3L4xu;nWcC>*t%T z2utE-UM3593gfJ335H;ARcbR~qagak4HnOSZzns&G==x^Ul!msaB>GK2`4yK2xIc2 zJMJ;i)8N_XvT~49bC`uVnWQcqtAgLMV)L$plRzJJl5pesh8Kjum|dIAa*meSyi^Fi z{3Y8w82^k%SLWNCt6{LB5vT!ThMY`hSP>u1a={|Sh;?4oW-ZVmJq`}!xKc8Y3Az~> zLj#tgFM`idF>+-eZr>A{i)h0YGx~vn>f8(jipU*dir(W1+S8&mx->P%Qp%c4E$TL7 zzUfS3ZV!Y-=%ba{#>Z-QomODp$vZ8Fz8iD~v3rmYA*^P^`AqW?z(xOVRRZTx!*RJ( z0&%(?AUIf0x`!>@73lNr6K--6E{p;p41}W=TEqz!u>BhCrSaywj|+~3r-q+e=X5_P zbqmq)Fx~1?7TAv~gFtJ>m){JrLG!VX?o?t7MlG*Yx2cUckPhvKKoVC7Fr7S5?>mN= z{_W(vo_uP5aI;Q40!u5w7mmf)MK&%PVXDEK?$SM1R4XkuURXLHHd|ZCkVjSvTR;XH zlj!|o2W2om+j;qCoaw2Jh~WhZ$wXYCA=hBHZkTxxUWTPgm?O(CUy8Wc4av+k;O7UT z)@wbD4fZdk&w57v)?5Du7&m(5mL zv0gXXLZ@Fnz#5VU2a#tq*N~?(CwR#SlY5q8OZ6yLb9rQcDUN(%&gJPovjuI3xr5j| z%~m^h|`i98w~5JG@0s#V-(6tL(_4)H)Hy!(JKB?Hb79i?$TS{Lclmw@dvTnZy_ zh{u7s69d(&bF^l78h)eyV}*wt8W$7$ z3(&~s_f6VWw6C${rp_U%DIJm5tXB3ceUmcwWA6ib-!nkE@e5`kU`*bu>oanIZxK8n zfp;cNHQB;C9wB$RLr<_e)EZ+EAkkz39_`v;HvN!TAN&_#{>sBy^TwL@6aSWsNS`0~ z3SF=-tmJO4=7g9(NWf~l&S^i_PyW~#F=@oyd|Q(M^gv7NY9}sBP|pY>vPRh?lliuY zySgN6uVuGa^pe0I$-;jk^+Q8LCZ`#V_C;Az;uzxTD>dRfM3BtF|J->!BzhF2DTSfb z?%=kulQZVphPPcAhTIY|X^GG)#0lt`w*Jgj^8l=E{Rpy86LREB_x4)WmUPP}wUp3^ zz;|uKcM?vcsINT3U_kK{*qOk*`Fo%UHXF$k#v`_{k72q;N#PZ73-w`^ZVna_WyVA7 zTZ4HwzuKe9;LSsBoAg@NZ!|}CiB8R#gU0{+y9m6Rl^-WfTyv`qkAqp112Z*7UGht+VxZcOJsDu+8P4S$40nkgO_z6 zCck91vnkL}#0Pp-7{}p@n0}J>S>9ozaAGt|)rBS5Af0w4tD7YbmT3Pj5suY(p8$#9 zGjReBdAXkkI_4gB8DuKJ5;gRl-N}k>x8ARx2;R`;(0dQuqyAX3BojZ4uXCpum6fPn z%rzmb5_CHtvm0jVc!3MJN2}p89yI_QMi;|mWLxE_t5B*2>EFRugKV?bJx*LK%cqBx zmzDd1IZJ7imQ)rzPD>vEePg6cHnNa~>sPE#kZ1l;fa&cSU z_ZvA7RXJFqUM8)XfGyS)EH?@jViYi#nQ_1A6MKjwYrBEt>BVX0%`h;8En2AK&CKkg zM^vzb8@w~-naR?DZ$ii8$*<|?8dg%`z2H;x)xXkV@*5y}LI6~&p{)UlBJIsRb>TNQ z`7#5#o)e-iDIZ1Fl~(VM5}qU5#bda(paGd9x`rhLl2yHC=MvVAHG|CT{+?NcKw)_bevS{168FPWEp+%-gp$6NH zt1Z-WG8g*!2jV_atfAUc?4z6Lirs;X`n7$C$(eVPx(ZF{9!k}etXDUMlZ}wk%6qmy z7R~cBg_k$b-HYH8cPqNKI^%vQOKGy~>J5RYsrC+4@)=ku)^fu5>$7se4m|y>!LSoL zc8u{DX8E?{CPfyC6RVgn&e~PZ-(|`uFLxIHd0$d~$5{$iY2e;W$t{yy-L3*gHajs! zT2L#Q}2-iz}_^jQa2F(}*&{A1DPO7n^-q0}sz$s??t zOS=}p*Ca+q4zk(yn+w3I3Nve#@%XFmrXo3H;+jHP??ULBlY@zyA5a*-g?z<8QLma- z*##%ENm+rLd*G@w?Ht%eceG(W1}xo>5LL$xH0Kn5D?6x0Vxqx*RxKh?V+Z0?k1!m_ zc#psky)#;@|Lp#>oNJ8BH^)BC1+tgOfY~(3IJ#-zK?1+D#5Y(B=7Sg2o;Kg7`(eL!*23Mb!at6hhpKTfxBb+J6$Y2)FmG;l%P^ zrsu36dX*o3!~}!Kw|Zk$>5w4KO>0-I1D`!A8=*7otv81Q@-&=0Gt51 zvr4ROVIpD0^+mCrBXo)lQ`PQGJ3fm8e^^}YP}|)5NTqlw{Dg6c?Tfv}x_<#T8vLbO zM*xx(tmQMLckra+kTEo+vhjQlW(5fW4U^1BJFm0*iWoz{LtW&v0@P%812HNXMMXJb z_nuq$>Q3o67W>AEf{jI3;|G;3H>{riNshstps7FcPSSqZ1fhdqZ`(WeJ` zJC)1ZP#q{d_tPE&?XkjmKWrcnrLgS$v{VDVcx5aq)h`rIRt`J)u4X;M=!ZB6##X{i zT($H{pVSp-C!uNv67EB?IBK+BOx?ejq7I@?U6Cu#mdJ}+)lMFYAd;SCK%eP^lU_N; z;?H@*{ZP>}q?laToH;kM7D^da!vlO$LHfHo#`Tu%#Rl=YGR+@C@8$3*Wk2`#!HZu% zmSB%Y{-L^GCvXZL8RzQ?wWPTt@1o8RL(&NIh84IGB`6T;SI&-sc7uACgd5Vo7yUAdzz;{HxY7f8?zRPVK(+x@Cp$%tlxJdq zvh%BC_{g9f$y86zAZ_BV>U_NNMrXVa7rPvrYLaJ|jr6dBIb-?aJvnB74_4?Fi7saS z6z6BUWgtc>h8eBJFdk60kWT}_o|}o$9)H}j$F~?6_3he12F7cUV&xs&ZXmMzgW9BP zB7CP!09W5&Q$uV7nHq&jG;354uf3_u+q924;ZiDoW&Jqi zLi$V*GeNtYpSKcNJMRc|+o{0s@Jea=J~B4Wc~&(mu@tn#c^ z{8Bov?(%Qug&eL_vd#p@o8;@)Uu7nBem*=25~hJ42|A{-B)L);3EE}^#1R&u@3m#L*|dl7G)_ka3y=z zA$qC%3JDGoCDIqu__yD-mno?LID0gW8I)cFlwQH$#KcCB2tM7EL@=Lg0c%=VqDYeY zG)RJ8Tp9({$EH--h6~XZ5Ls^vuzIT4@P2Z@K?=ng+#{4|j2wJy+7ilsA`=_vVe6j+ zPKii_l<)#_yv9x6iL{dr;#Z>A0^3N40`fle8_+G_)KPT4{q5LjJr+0y8jI2j>;sZU zcf$obizoA1`smrIzZBv!UJYpH^dx!UydVjzI|qM>{Q-`Nr+9flkjU~k)7Ema*EeV~sOmv(F6%q-ANyec`R8VwfE?wON2IW#= zT#0UmPhCk}&Q!C~Yy1a|*&qa$dL#6^?LYg>8jaWY z<9C8vAnCu^yN#>63pk=^=>Vcl2$~@!ZAGaH-PTl2w&VRYgZsipjh8~k?^-LUxHuM8 zEoAfX!PxQ4g{JBPdG!Fd4rVmP;NfQ*P%yu!R=Rb!8wKa3yl(U=Fa0q;>>-hjM+x(4 zkMJ%5x{e&}tzn6MrB}!*3PC4?!C%H}c(~Ca42L4MoLK10{z47TI8nG7CEQ)WD&gOzZpkaUEHG8-l42 z`fuXs?h6Q3dEw@>^}yDcq+Be$fTElp<*bZ=(O>LmzFX7s1yW!OMI$kmqx5dLY!1r# z!=NFz1O30bG!Ker!jFU_PD!ujNgS2XS_;^5agV>5!V#$ZUWL;i^@%Kk$^#(}7Au6y zvgwW-OQ_aQYQ^dXY5!RF91m%bL7uhL7IVw~fC>S**T~ zvJrY}CH#x#*$Lj92?UNvZ$qul?(l$)%*ENq7rqpKQS%&Yv%aCeXgcU*U}K= zm#xEtXX#fcqVczT4|f6DIL;y7^~pm-XBFdIAqmY4Vw^3^Ap~s#N@OH-PWz;lT)4Ai zr!NJVitXZWQKp*3xVNaF2{p>w9!KFiPdR$1Lok^La+NP%`ZzQU~FwSu#2*TM>!;Ivba-arjTO;ZUQReC2l>-P;ptoHzCC0DI zw%lY{s&nT_5v`T*Z7MROv`>60CU3btgO)mY%r~L19Bqt{6+3i2Pt|bBm3apslgv%5l&^;eu+A0edc~UZJW1YrVvV(Tvab&EQ3> z1}seR2oixa#c8K;MBs$W8uhA4LBCt67y_nsly^m`_lbxDkJC(+a!FDm`Fi-@Az6)x zQ>BV`_7#c*3dOx;_SKv~b@5KYc2+XqPDkq`g+~lj1G!XjXQbevQ0MvH4SR4;V;0d$ z?cT;svT^al;Sa`->~&T|#LdqUHcVZrFZ!cao2H_J(EMI$%&%|L-{Fp-YPj4aSzHSY z!G67o3K8RlNd*cpwh@6@eWXzA4$!|4$>jc0A~j#g&duI`qf*W_IN-MzUaIZrc_z!- z`1}omV@V3kixOOBXolFT5x`uBx5$Qt5mtXeVv=Cgu5KMk25bu#}jP5nx&4b39_P0j80;bWCzfS9=LqGFpLUQ4Q5^W;b(@Qo;) zit#GEWBr5X07au_$}9W0>?DI_4D0N!j%U5X;%P@*x%mmw(q~!oS0{Ex)Q(8i&$v^l z8l~TSrD}E$vkP46CB~kiD-D^5(aRAtra|>-Nr}M-#|=*_Fd%O>DIsYnyn)88E3>>< zC6B5*vNV+;7v#Zm`%&qK=xMizun8reY8V;sAZa%2_(e1fNVG0aP@0%>x#ff$81;Z9 zy^JDfj9LXcLQEhmiQ_K6Z|1=txYp-x3avFu2e}bRFaBG;BPaK2_P5@?{OSJdOo5c~ zrT4nA#{3d#i`%fpTM~!$&{ZbD;d9DTzj{4wk^bGPzSTXBl+9f<_1sx)%nOpd4#eFp znpSC}j}3X&LYo=sEs_&(j<=aOA8)*Hn5Q9me!APYta!t#Rm+WW`iEl7=csVP(z*I{ zZ@9BE`VB4Ii6CD75818opRE#z$N}Z#e5v+OWvyRC8=dLcU+(23Q|XloMG3La?|4`# zP@PP&dlOeuSMY5`uHc|v@ko3_pQ_(4i-wC16Up`8oAlT&lW!PbB0A|STF48J%)U;U z%K1z4zdz*0FG7H5OZ`u% z08g2%KjZ5IkJAA*-Klp&>n^hcKUfpu(h#l_Glq3JW>!}ZU;NwAOzq>)5$frYo-}ch z?*x5j4c5eNh8f^1k5-AkFQq71D`E8W2fZTU&-;$z*fb#4Yiu8lSd59wO3Ry*6mZ>gh z5GS)KLl%KK76o|w#+3{6bD6hRa`?mv}7D6}T4UDqRinW)|- zh*I9`)Yr6}gZCb%JMy$LNG}r@t;dC2MQBa$H7Nw+J2Zxf-f!6(KiZ06Us$57tb4If za3oH%33R5q0!1xZ6yhI8ATK#y4c#@twyO`p>1In4wZ8!%GkWfYV9_1$6&_lnCh*P5 z?&H)m-TiUZi^?WDj#Wq_LbIdKI8EBFPq?pF~52DKIJ6d=Vhc~#{0kh2?1q=ebP|s4ao*Ou-gt^C=8@z{%z?2)~T1BH@NJuGPsr06Em?EG0qcR zYj#c{8z*8NQrY+613W$?`g?-Y z=JL3`RJdnj9BN4dnAdZK=R@nGaS{e=7;Z?T}{m*GHm zH<_nf>CMS-TpSk!MM7gwAewlDeNwdz^gwv*WW;Pb9~9FDx$5jRvsx8Gx)S;NGwCP> zQ$nZ{#7`$ZPwqpEa1Ei&Lj!iB=!H_tFv9RDP(k^+Fm)Lg_8P&hBj^Mq{0xfjJyq5~ z7dE}T40@VKQwLUu7=2`=E(uL5Y<##XcpsHFN}UmYyV$$z$LQoSzLi})0G2ubj>0p8 z$HIA%ISBmn2Z9zCXYKRM%|McyrDMpwVYp`WmQrx)t5dA0sOlwZn+as=J8`P6jBW3O zSBpcQ;SY19ph%%VF09|%3>YEV!b)>f^TZV5)rrC@a|Mp4UGGZISlGwSlF|FJsL-Uf2 z#_Zq68wm{x1g&sh^T6vCb<7w{GViRS4J$qOauwg~tA*{xTR#hU;Mds7#Yg$G%V4O7 zGDTh>2$}FPo3>w4jXLb$m;Pugl4r6)j7+XdOi@#Va`3%&M7@TPg4RxpH999llnZ>Y zD>`WUf|3fOyTfxaSrYHIyrKU2;X0JM1R*W^XGoFdXPAM1F1|~l=ILX5J8k~3C-UVq zTd?OZm0u~`4bI@KM#|h0YYuiPu7iGSj`RPj3B1?-%+z@ zW{G#xLs-uwPCd%Xf2$JI2JN}UG5XnlezOKYzus$DpXkECM#IbWVvzn8>W>KLX|P~D zw(&dYsNFBK*Ch<0CjEYA`9)S_7#VBGR%-3mgXNfyRJNKv=Hmo^H4(uBMvf5zS7WzU z%gxeoJ%wYlFOIf(&m-|&8E&@WsVQ(t8wwM#w^3^%yJ!3guVWz0phJX|H6rKIqXQsE$a z7-g@u#nH;ML{>Ub=n zbRvwU3}n47%^zn<(9{|Ac|JOsBAQ>jX|+gM`Em!Zks5Q6^v%gk7AOz0Fu1AH?&rJvgI zxja+Ux=zfTxy!|Kowk6zG-D7DdTQLGu?Dl_NZmb336ok7 zDH0h(gGd?8fYxBO!s7ox!uDaCk&W@BlaZXP4iER`l~PspLYFhGj{DVzY{Nu_zlxnA zjiJ4$WncsG^V8z71GrTIwqaW5S#^n^@HNpA7 zty=5{O2*VJ{;qPxf}Aae!_=+0j~C!344cFp`@U66%_T>_1RnG+m|JA)vR@{1qr?R` zHXkY}?bgC{B}-ixWi5po9D$y5gtwfN@r1zgIUjY^;=C#JNsO7DLKd;Zo)|TlX!p{} zIgvJZd)H*sWASjA_*0{c&J9WtI+3clvLE`q)wANhHzW)qo*Ys1ZA?xbOAp4t@rn$g zI?9#90YOJ{N+<#R?X7hPLl?jwlXx-8%S%~yeZ-o-#X|`q{iqE3_ zuoltd8;aa1z@yMR=!xy5Oc$qSGaS(0ojFPKNrG_^?WRBs@N!WFPF50S3%kB6^{7ol zaVgs9qM+T)%yRQa*LrEc5C6V<5ip%dRX^?=%cW%{Q>0v(_@jk>Lm14|3H5GR%YCG4 zPhRdGfuo?+|AAlqenbW=eMZt5HMedK!xup zs_^S(JEc$zjX5rTYwPzp^IPZ8W{pT2zZ|rpav~mjQiZhuB|9uzXIp%}3uw{c>oOhOa$;oxfDPq>P^=~ z)P^Q6(k(=|_*pzP+}7@n77Dlr72C;!DH|GPi}s4=RAC#@)qL!+Z$9O<<7`>?JoZAu z-xYcb|LOKjqBU21&_OH^$v}jGhzaR&M~q&E3rO%dV@>3(47?7nut900KA1}#xc72~ z<|6K8<^#@h7obvdXdxd2C6P3f)`H6o=IspOFCt?44Tz9{^LlPDFv-Wy^mHwTW_F&|3=*mI~}XoFwUCTQ-{ z6vD;)11=6KJ%X~NnOf$kU2QI}SbLOxLoIuJa*!O9&;~i_a0>Ln`u{2U<)Q0#mg4Rh zFVRCqxg*%kVC8cll6xFpr4DUSj^r*mW}>l+AbeGH;uZ#-zVgxJcc(5rCJ3bdwwGV? z*^JzN`Mz~~ugAKzfI@1Am3#2*6BxzRc|KR%?vfJ}k2WLv16!t#vVQ^4K8Zt(t&`AF zVfv)+lfJ)GN9?wyr6r*OVXm0~kjP#=+{pYf$oNP%Dp?x6wf1@wLdp+;he%y44kp9vKRaqwfOxh5K#$*buX?L!%XlA6C@QIVwVWc zUg8GSNsL88*;6WBP~Sp9KB-ltcbkwd+Vf(honx!ZCSzyZ)9>=~zb z1*J@l|At_EQg@gV;qj%&;O_IAG3kx+n9S z3@^!hu)>a6-1faJ3$2C%U5<|r(*Ocb*YSy_sRT;!{O?Ayz)p-FYsMpeE*o=GS5^c} zP}Nc#<*v_P2>mL~QUZPJW~?8tkzL9u0H11wxA;A6dsV$Wh9zW0rMnY<3+0I&UjQoE z8<7ucmN%q)fNDb)YS|ec#(67NL0@X$m&Wp>%`I5P%OahQJ*f{u*kp)`2CQmTY>{ZC zb=$Ji?2U3;k+j;4TfNOVzzhXVJUGc-L`Z3Fm$tGOx+d-0#?i)Kj)J{!2ykuEH&*twWgU!tV19uWWzS7W{^~F+tFk7J^dZq;cw~uWP3QCJm@+ZE(6Mhp|;{mDlMg^ z4-HfR%IzUiAz_qODF@dRFpaDhu>51?P^X$2AFj~3Pa&Z?BD9TGl3XqHAkf?^P{Ml} zWELoJ+!hA@wZU?$iM+W`OOQr`BGTHuoB&WOFOI51p3i3yvXhlNk=)&-E|iQU`Bc^x zYU@57~INbM@^>Ke#J z7|WCXv5>R@=9UvOVACxLxFXK#4e!H-{1glt$A``axar=ml?c%jMHJDe-hsVAi1Hpe+ zlnKMEIxK&G9Xjrf5jTzZ3f8|j_apUFd@CljKZZEIFL+uC)WhaR2?K8MH8^Ua3V|wK zz?Z$PQNchcmzl`l|Lm-Kh}%nf;J&fVLJK;ckDe;rH(Nz+v$K*QyBf`jr#=`ZFO)4d z?Ph{H<}McEy6#Q-!R||Ii4qe8Zfph#y1Nxb-f#+>59pA}-VVx(PD!i1AHQZi9}y&qT{o~u_unSnU?n>agMbvj>n09$ zI+4>1tX;B7hAXSaKkZ*Og~e&XfqCI*e`Ykjon<-CJ#T!+9RRwhFV^^6h2Oi9%4`?f zKg-(rR_Fc>km}EoF5rZL|k%~kJkcS4bfMeteGDqSQXmT%{_~DP- z5|ky3e;+JKq!HaAWHBx=CNd^?$xBM+9IGF*B|D|E0ooN^80SjrpPF~KLUaTXeZTNb zNJJ^ewj5LhfKBid%gS1rPhV|nStfLIO8M__f7aVHW_@H81sMThNb*#?efD3LNy`%` zTtPHFPv-^CD2@zXGrp&@_vsCRaA7#QRoH2!ag+?2OH6wayFLEm1y2cC&#tU(balw6 zxqvD1{yq`hMojOvT*(+9P%5RC{$#~#l-DbXW* zMSepr(4mKZ;(Gr*bY&Q@(p`PY^LH{&PSt)-g*Zzl9qbG@&Grb&WCH${lfDC^B+Z93 zrx-hRPR3fy^0n?HANU3T4(PYFOv^VlK4q@Y10HHGM?)>TbR#k7f#+JrO7XVI8up)K z=b}MFv}-{Xyy%f$7i^2PFXzW+!gUrNsj5NbE`FoFTutk<_RrDwW0a9WYSZ}Iy$Oeu zxdNxO5sCJDwqsC(Wwz;;T?=~0Y58Ts}G;kr3 z*bTWsSulzTC_dvS%gDW={cFAg;`Xja81DMbOjONx=tNT#NX>-ndN2D6Bm4R=QB#cq zyy`I4Uvh50%LwFgOI9cFETU+%s?eWu{Bx2PHdQOtziCrVf`HwD&l8uAFm zoKr38MIn+a6vVqVI$Ej1aorzg>>X1j4R%sA}ecMccR7ir{%@Iwe z?d1^y%_qt$52ou}XPTF;Rw+AD^sD5b`1LJ*$Ps3kv|H3Q&U_8-9os7dZ?(d?z&0o>!c2Tg#<*BBt^s%gKB-OSEcSDPLZP&GRft? z{MBuC%p3-9CSjXt7O_-?Qg^ol5z|qQKeeu?dk{X8#?F>_c!!2F=xF)%!!`Bi#B{_? z9X)Jz&`*zL>V{@Bc;`i@mb{gA(BHHH57s*=PWcKGS8T*dq&*Rm7uymP3F~WK>;QZU zUNCg|Pu8j;@XDrdy0WwajRo2pxv&C(9dq;Xn-SCqc43XR85^1A_r0Cf;UTuh#-@b? z(O|S7i?rAkv$ib<>54pa(}O*UmHzrFjm=cLG>_)%V8SqkQ?G(~8% zMl}sB&)ZB!0)k%F8|fMQ$&bA%evd{JJlo>mz-z5hW>>35nYJ=`Ysm{72v8#80v zlsz^pRCmae*NY2PglP9Age)u@&_>LKA1KG@s z{&Q#TdF&l#yp;u^jQMicmsW<2Q`dcmr$msp?MUuox(N#wj;2KxBTizxPwA(|v;xbe zH(>w5YHf`Cp11dE?5Bsv!$*5L*#iZBjiMe^r3%YZlan;5u@*o_eutA01?Nlr8I z!4H46Qqkltu6ow~A}gVq+jRdH192!rF?=`SBbAph`HkLLBhn$P%$wBJzeoI^O?d1>EHR`J+5Uv!77gN5BUMggNQVQ}WlW`VV1*^< zQV#-=8OBJ~gsoT}1h#rg0mrkpjI)=pJDVbltV|t(TTMw_bP<*m*sMdUjD)p`iyL}Y z^##Xc(P8$frQOvnX8NPy`)Vu{5PneH%?PFS8oIM4wzIw95(tl~;g#ut`)L-)&|nC&VKQz?`1Z!1?Hi zZ4PeBYV8b@rK5iJ#fS*{DQLY`Sbf);#*~oPjc&ZBexe=%=J!#X!dd1)AAd^mUES&W z<&O&1$mnidlWcD9Y^bX2$k_6#E02~~)JKP+n z5Aee_&p+M7@e0<{3eriF_^zfc`=K>ex0=W(m?bcRxGC5mu)vEC8MZT?eLE__QriLD zNyHyr+^S2QF*!vzQO}Ul1ur+^wr5dm{YvecayAKEOA7Uz& z^^KX;r>>AgmRSnsEkLoUZsw`};ff`wVZa&(+iy{dTzB)NT&nuI+5|+(G^Z(ZRAb-- ziBz;$d~EKTlmq%lPNCn{V+uSWy?i~NdZt% zpQnx3OkNqq@TlwC_)Tvd+g;k1=U&9Df!y0Be1_6QZ1u#MHd4j9>wcbtxgqAhL4DRj zb2U1Rz1fr^QfxYb$2r93M7Ao3a|Pq5CKkqw>LlF;#qgkNRyF5r7#~!Y0fbbexHIVf z^Q*#k6?mG_p&r`*Pq;zqG)$KTD$G)0NLTl&_KRTIiV1bjlhCb))xzX2Kwyf;r*!4R zM`uJ1;_8&@ecvl)VotwL+CVYQh)pz1uCAV-zYAo0W0@md3ytd$t+XN`%#?!Bki|q- z4RfFR%&0c_jJ^M>0k0R%3@Fw8ehiw83OEgYhtfbm9KgkSp_(Ql#Q@){1O*so;tu%e zVmhiErMvLWlE@-)eIUOTGQh^ZYUqQcNs}}9*t9b-OyRzP z);3BWBPiGx3=#i)^=%v;g7Md9Jxd8S(|HSRE0i&)l z!JE>WNUp6S{c$mVoadyFC-=YY1dp5dPI983ps{f8eV`6R^doeR@*lDD)H{=(DW$Q| zd%8|y#3c(w&7^|VmaDy>Y_`{n5#?rX)Qw?|NmrW7v7;xrjlq+^6sSxQDW3q?l z9Bw@3_M5NV?t4f_N<0FRe^B!&U7C4)q$qATgXC+~Tui9lB}(Pz0=B3n@$=V>yCYs! zx!cI=w(`;8t~7HLb{z z(yp)*X@16KiZaWnCgzn7YV+u~-~H~cynAXJyzUd!mGT8@;3nRKA`-Z`$geC@M{`U5 zWW}J_V5Eu_c5{l+Db^!q=_$V`eOS|$=WW>KyvU!YhHpThughm(F4Xgs^|2lugt1`A98jCvN8;xAW7nq~0<lAsm;z@(mZ zTvS9HV$Z92btfFpzLV)8{SHnyZjx+MC|z#^{@6ZGJKX8K*=b?_ywJ!rx}y$9vjR^E@3p@sIt59P0dhF;H#0uj)6GNPm94 z%-!6*iD=kI)B?rau|foty{K)Iaf*j$;+l?BPK=2w*$ zm)E{5BG-nira70)JUjBm{g@9(O)X2^X|@z$03vfuk3A@0-(n2lfBQ^GMGMxR}@ z&C7wMb?Pg}Ke1J`^M2086HY9iL!C0;mtSAeTM z7Zl0xPe^!*;a4W8elt<}Lg^kG-c~R2%0CdKN#o z^Xx7GWG7X}l8KK^S`utx@5txKlb_1T0`;D|fm)bYhH|h^$PVpIqALX)d{I|2kO!^F zhM6#810Cd(7!=@cei`#o7exvUX_FtZOA_C2h7b}`f2pSx=iB<<5+9&D2qVOb3NS>Y zy|?4<4}wk5q?$@%H62Turc+a-5>$i0)6ttu+uYn_gd-t@H8$v>lKtzw|*^c#u* zrL{$qi{UxLp3=nb5UeEyIaLac#;}mQ`dQlRqlBlLGVd%AT<#%;z~8uIxo@`v!p($( z!2NSD+v{^I%t#;-=|I|h1D4n~uL4Tz@Vf`U1HSok*P3s;!3}fWU0+=i9ote{Ji`JW z_SctF4E0W6xE=^rvsZ+Fp&|d-YTA&ExS_-NMR7?I>-dfAb%X@FM@L|b1J9g0%su+Y z`El2qMI#<5J|G8Mb*!Gty+^b1Og_6J*DnAoScr00@fRekYjXQ!jb>roWn-9;x|3`&ITw zxVlTQPYZcK$Jkp~JU{9;2Rw*-z=7x5q%Ddk5|8pvMBm;v9_(uWS?-6i4YN+qP}nuIkq_)4M-ndXW(~ce%=|INy<2^0oW3=J=I3A)O}Ycrf3p`-kQUEbRki zQwoQCaCJvjGYMjA9unLTFf>seM7Ia#rhNlq3@#cbfmW_op~# zX}9~-vlHl$IRdfx{UwgWN(tBy8KO>9t;K21h8V>~scT%(CZ|Fy=T)~#p$?CRlole3 zG%}{tiu&OaTIJs5Qu4Vs_0}kdL}>COC&gBPRD&Bh^CSsX&Is=!dWeANjbZJvlM*@#{CqaT zTtx!;p&pPlnC^Xh7PXl)v_j}GXXMZX#cUp8C?pHEXeUL%n-f{OtU92&A;`67oNDMo zp{fQF59bnYJ5dS{l&_bKp?94LFqSsC30qY~s9O=MT4q0qm=TbI+X}b_RSTj5-L?rF zf0E>vWi(2!|894R?qJb3f*-k>WuNZ`_sgm$i&>KaJ@}U=RuR#XWZ+T}97aX~Oh~d%3Cw zO!|}b*@yg-6Y^V$UvDp@;DDn=a}d(K^+;_A4WOir*O`p-Nv4ujcAMDP+|p$~3Yw9E z&1zJ6)#uO1G#gy^Ru_|qwnt!I+cTb5ImyQ4*wi4kY%ZQHUq_UJ*jk+%M_23FX2ABJ z0#h!N*Di|0NKYpI;~|h0Kn_Du9WTY3C`*kldGBPjnvkvs@mI(^Oqv}BZE>CW>9-WM zIuv|r6&w9RFv#Q7Gii8v3?8eQ#WOeb@|_txdqixNp7bV6*~jhm?x+n3mlXYdW&s^n z+j3dabKpqvE;N$>{>*0m%;Wis;fRVBfFC5?Nfkq2p^)h>K{z`v^<*sPEA+8$%YyF> z{}Cc9IEg*38ULGonZpM2GT%DARb#d{AlyDzzq7aMrveS)FQG=R#B|#kOy~9*#C3WZ zd&u2kIg^uL1H#AiLE-_h19jqteOw6^jOY#pZv=Mlki0i*(f#D+-GF6`c`d;xT=6n* z=HvfB>o76=e`y_NW+wLkNb4|jGIIPcX&swZBYV;-Bw8FRLvHRC=J}zKp%W19wR#&Z z_~I$dd{?;Zgr^W@%my_R;tzB@WYdq2USNWR&-`VQWH}nL(>b03W+2p2L^)n z4GhHyNKjtr8=iqQG$S%QGwp+#rvU&`k+}k>vH@U2LsJ8h3M{S6Yy%pb-hkvWmVCP* z@Fw$Kq6IRWHGMn=X(7BVLWHUcd2i&Bx10gyW(`%#WffDlyVm66iY zL*~UNsR4^;tfg^aXab^SM^_1q_~jB9A!CanYxY@_RO;k}qF*E@J`AB{%*&ZH^eZp@Ht&LslEiwXR{g`QB z{>(Ccocaz3n05Y^{16_!!Se%5 z^J6Ul`H_|U({-w=j$o^009XoIR$OOf$>0DuwerFPVFiX(pdb+3oS5nXGXQ=g*_yyK z+JAo!e&K~S#_#ZXgKVaL6ylEg3F8Vu#->5aARIr$9F>GCeTzQav2~`YT2tQ`7#JSC*!%x%9X@hoWOihupk@=TLwrFr%4tFqz5~$4FkEOHo4ZSe3&nK0##*_Z{)jt8gFDnE>PQY%6b`8Gjho46sC|7;zs7NcCd zI?#Eo!00tE3@AJe2hp{|6-!eHobWPB)Qv`R(TWs6XHxnJ7JOXa-4Q@e{wq*nIw`cVL}Z_J2jF-Pw5CJBs@O&{q~Suk(n6#+iJ|awY0U z;rdoZAAc9SAr27^_q8Y^rnQNz+_sz~*DB1{zelXy&nnAzAKLFzziZCLg*caXuqWYF zP6-K-e-^^-xG+x?JwUPWVXj+$AZ30^jc1Q&4{oiN|Dvq+gd;SxI6TonGKc4MYtp9c zC&;|sX<=u@?FGh+IRQbSI~gCMZ|t|%&Vj`zG*6uk!mYf=x}ImMdj#57A*Ha}I7(X& zO;VcTy4CW0@RxjajF~t{^{a!Yg>Y6|R{5;OFb9_zn?nkU|Vh*yC40>oS(Yo7;ZP4bAHqNWSv8(?#j_A@Q&%2N%BysMUiyQJz z?$gp`*$F}k1uVmBXq<^C-iCRMh{I+M%TI8MA*xqp7O8?n5iH#x8%0lyqT8Sm4QhL+ z3xRHZ>~pBGJ)p_9$rUTd8mUO?UL1hG8A55LhMp0IbySVQ`u8pVc~cZI5#1ZavTITN zB^*wcp#s8XaX-8z!??N%pvz*b0>6*gy`UM&scg1+oyQF#iCdWv95{N$4Bf06cBP{K z1W6hl?h&V)#&iJ(8;rRQ4}ojL!z=yW=W5{45=l9Ma!B7fqF16;N>#eFDz-7ef&qfr zzdRwVYhnby{2WpGRn3WesrWf3vA9WKly48X1~W%nOLHD!`ZK-Gphyn^)3wYEaS4t} zpLyB(@1os(J@kfDGxNvptrrm+OpfMU_lQ7ZESLnH8voJ +{NS?aSVa>-LKdCBnn zj`iFou2kC>Bx7IrQ_y2iEsu}+h6qY}c@PUa>RkE z>5T_^YSnf1i;`?we@{2apTaApL!!sg9afI$C|m+V6T*4||k}Q0M|s3+b^Q!xB3B+3P5*u?_TC zI_}~~-<|@^vA`ZXk^@A2dK@k-Q)R+0b!rrAAcv5XJm;PVXNy=FQX7h)0BSSuQ8>`P1jtSdn5wFW|Q3F<)kFa#g?2o!h@peXf3LB zRQ5Y??DifGpx$n69I%)L`yJj-+1|+=t=bP}{^iwuL6f1La@`zmQGX z>@3Y?=jP*EM)QYkh*<8^#@$&E)yEA@w6J5HASkGf#{Ilr@7cZW?4(sN;H8JRAxPeua_P|8vD-831Kr~2i6{E2P$rIB5%(d|W zE90R2?{K%cAineb7Zn?xnM;hdOcmcnRh%S$bp#S(^^M=_UEg?dqDo{y`@fx2txItl zK|$q#md8JUlbdqaqD+vaWP)uo!6es)QRaTp{=hD|H&}hhf0(7v^?N$W247$rg;VdR z(9*L9aF&%=4W))N0Wlu681Z1yLd+Z;rU7(QS||dm7{&4}l(bBCnH3)@l08Hamn*MR z8!khSp$+)ur)}224Ep!I>3M_65hmDK$>N;nnMi<*gm@n*6>AwaIvf-Ed%oxTOuJMrRT)aL|Szh{*dK|Kw;Dv`Q za>+>ruHmkDZ>M|63hGiTydWHL@dH!f`C z)yqNjkV1A=`&k)7G{T#^Ze=O(>FZ}!+f~igCe)}6SEqYL-PyH`A1@^CdeS%uya?P% zVESZ(d?o$%CaA!olayMo8%b6+OAiMzQ@5)4Q6mAoMc?Z#XDf4S(Q$Rr&>kza1;dBW zg-gD7!LdIzF+@ayH2eNad-`YT)haEmCPKT}mfR`BNW`XFn?sLO?f$Aa;Z)>3Ed&evbNiVdY%!26CY$?|LKAQ}bt#%~tK)l+e;?o9 z&QMeP+*4y_(y3ma3=sMaOGzCo^ux#e$hG0vnL*{6=81cd&m}lbzn%6LKHgR5GdlR~=6^;Byc5U~NK>f;7 z+(jC51If*v=Y0CL8Q_XJvNG)gMN_PX<`IgDz(S|;v@R`*URJ4xDgDkpmL;XF4|pw} z;tur=R#lWPc_`caHX%Ki%z${J9a-7lCU;#CLC+8t9H$Taq#zqvwV=^cG@@~cij1Bc zN|$8gT%vfjUAdEHk(T!^Fxs-$iZ{zyv-r)032&VGv-$384|<&Zt{SE@l}ApamD6mq zSzhCx*XWr%aH2%eqp-|Qif8cvh*rRu5;*M~RTi)X2Spa0rg*c2`eyp1n=0c0%DKIi zkP-sJuN%TH@t+#0)Y$N-8byjA3}fUJ&Rs(%@%&byLX&eV2HqA)&{vwVVV zE8S`?VPj%?MZK3=)>AdSX^FT|u}EF&^H_|d;aUmjNii8kAR<~?_zhrn7?c*ZUbb(M za!!bnG!PZBjxAViBw?R})}e=(Wl#4`EGIJoUV-pJkHt5&mtUynEsPsi5!`gUq8DUd zha3gP_XU50IvQFHTyykGL=2}RWrdLGK$9QM|4kL%6-)rS+t=Q%>{z-w08{FRSE zheWYJmZ)-wCFHxgwWKVx1GTB8Beu<^E46IjdqEJ@1e0tlUEoT0#ZPzotKY}5F%_7~ zdvU(F(Hb&I(B0X=qBy4edYtz9e`Rv8x9(qu^4jy;RxFOC9YtUmCtc|O1^~wqp#C9@ zsHAo{VpLa0DY)t==6<=-s|K%vyzgs0EM`ZxW|^;W{B1isoZtkbW6O*xK+r_+Qz|hu zb*k+&uAmI!*e+>rs$D@_4Q@|2D)7{QE`Q!5Www>>i`>4ZEcCRlym_wUB4a2tAwnIr z<9p}f!$-O`M5zkrvjC>#BP8ZWjSNii5rQ2nRezMCnxgpS%@$V7sWbq=S>A!3IlT}9 zIGSWz<+5Ce0`!OOjSzP00@t*ziCE!eQSM3M5v*36K~Snyt%MhW!17?7t4c+4^tU@6 zUVwa3DAqV~Vc%QGq(q_J+N2HDzC*!jcCN^jyyPOIEm0ruh~{(8sNH7ib`Kr*&3!b-2F#9maLpYNX8K8WPa4 z^eYRMXHs*0Edq~?rA&Pi{^Esw4yi^q1a@*Vq~RI)Z^_YP`R$-ry}}+)ZP%~LeO!6q z7v*Wjt&5@mY%FGgFH`31EsFs;hc{QuJb0~}0HQRyK}3CVP891f5)6IZ-CWijNg1dQ zW^#}rj^%lTlX|PQ<#PTDCEo9;fJ%eVuyv6d@@gaMen`8qmNQ8?5kPeU5i+6x)9K9t zde>i@UZ=490a$03r&ZQRNSUZq4|iI|yUP8!IS+n1Ad2g{e@`t2$BKY$yR?Y|>^r~A zeA=s?G0gCQPYXS2g~vNZG;@Cxj3y}^mH@iW_|?GUkvS7EXJLieMVxX`x1vW4)(eU; zzi*0-(L46nc#kXg;Do{m&+95tBymXdp(rT9LpJ6L*iqcK%35q%w7ry+X2^LQ=C6N% z?4vs3BL1(WXC>I!@A+QCq_}oAD<|n~mqNjBeP9QKtR17leL%oZsvFbU3&Ttu*YS)J zl0Rwa9CrHT`fxmBcFxiArhJOvkXp=GAxi(WDmbXp z6UOO;N7cXh;gyRJAO~=RZO9J-?ATnvz)TX}(P%q@LGHc| zaqgq*rn42gyJN{IegDVQK_ghYT4Zhwo+_!g-A_JpLoW>G{E6kTDN;nVcF8?Br3DG zTg}%?*_H>=CkJcaK!Jl+&^EAYYG$jZc9Ea8A_c7|!3p1hu+@MHJ;;*4@SXH^kga`C zk2tE5PfYHBHfi${1cKO29a?&Ag?%&xWmyW7SYsiyDXH^wkw?C~egpUV2Id5ZftD&1 zy~0*@W7Bya_^E(_=MscmcGMr|Jo7+Ey9s(LL!c0O{Xh!7@8 z=SD@;M*z3oH-if>j*;8+#8*0a(P^jOL0m0*T>+&AgMC&PtF|)G#9A;auJ+$4P-Da9Vx!jAgK)FwDI%)I})$ZH>Pb z$G(9kem$H;|LV_Epgp>>gQaWUAcjmkW9l+Pb%)p!-N=z*aLExnhn9`NX(?EKQbetO zrDnI#5XkZsJ+5*|N8Tv3!0(lMnHB|&Ow7pO@698LX@CM?9oh>H{rkK3uewU;j)+Qj z(yv0=^jy(bOYXUNpb9d6Xx_Mdx<*Q3Bi&96R5N?vf$jXB`>5jX_HDE#ArZ^!GHO?` za6kc<-wGHeNhKHPPe3$Xaeo70{|u3iVJd)4qWMp~oB(mx8B(s$tym)(11XUAq|)fL z=LC)5j=VXYSSy&ar~}x6f!4@2(6fUi7HbW9Edo^D-Zq?=J_M-{nRXD=r!}0$?w2k6 zrE*UZ|4a#no$Yc^69ZJHI8MkJ- z)|V-&vD_InB;N)$l>WX32B@AEgFwn2acrtx6G{6ns+4=cghq|akTeZ?Q*zjOl3pn$ zcj^Hv(^)La>v(uw#pY)8eJRPgbiu<}EQ94+ex1Q)?X1{Y#kwSdua<}}GAfvX@Uft7 zmOC8CW}4oYNP6kP2!)r#-Z(dU@8_5vf;d3@T27m*!x=YlV(ZqZITd?jEF$$=wJXXM&ZU2Vx~n-Rf*m0*$}UGyX=YaP{`%Po_#_lGm^h>HB&lsEk)8 z@x*>=;~PnMLs;ln)HKj?D>hLyd4!9(dcs> zNrkb`+CxwTGTfL}2t&Um0p+*foIjm@FZupCm*4x0(9r@>Od5y1#*_LhV)myyb7s74 z{6s*8^o~?#Q^Q6eC8b$kNW!N$!&>Iw)4}*}sN09kS-wz#8kCEkRk!Kvg=gj1!zkeqsT2r2Yq^{}|*#mUV zLYr~gvvW)C$|}{Atmm39#NJkeXxw}Q&<2zs06C>CjS>kb=z-*oJkduA6I!fwChtz0 zwhiLOTc%{adrco3t}LCc_EDxiaS)w*qO?5B-RZRvYWZkJxR!|MhH&-^e5oabv`)aO z3jsnga?hq|cF3cn#(qD8p^&+^T|d=LLA45Kj3lU`c~6b3w$EyW;@dL8rpUAsPZ_6V z(s4F|H{{AeE)xygmLh^tawEiP(lM+cVY@()OYmEejYr-gH9&M)%@0Qp7C($pZPcrr zTH8?QnaD;L|9!S@@9@pm%G6E6l^67u{PR+dcrxZXgzo6_klELCI4GN`w@0Ie*)~E+ zK9nS$D%dUf2ohM!vD6_KQ2`8xLdDqEnn+2G-)P;IM~W4a&rU1zQWYaBoCRRqEQ

gTe;W#)Ce;0 zS<&5lpiZN-mhitkm-2VSnLZqvR(EK6=~~t@4>wfPa(ux92nO9446_YOokhW~N|I-q z;#b0}yVf#bGOa2vG6nn5tBDBa&{l3o`kEvA#_(gZlQ->75iC+dfPtv2WWs+aAL0gq zPpC8_nrY1wRLIF`Hte&62Qh6`nL;vDleH7pxq|XhamTMJ%XT2hnJ<(b3PQqqPKu*; zL&~O7bc%j0k;WELI}%xL;+mf0+z z)C&(g>dMuN4xysQ-@Q8P7!3HSb0@n_FNbKHE>ORhV-fObr6hV@+n`!z z#@6HX6h$jaPj$ZIENN@H=gSkVWzR23rF|X~Yb$)^T&EUn2uaMHO^jYJ5}^Dyn1zFO z{!Z=miB%3GC!mYH(=T!O%-^$1dl6glp5-2 zO2~~s2Nx8#hkRQjX87R}!Sz?o)cB-0Pzu3SKRa-n2va{*2bGjoVuU`q{Gq5+$Pjca z3{F?Q^z;svW-ft*efssJbJ((QO2i{Sog>NKs?i-sTn~KV;;|-5Ug*c5cW;{WJA8Qc zQ4}&Ooi8%8+Ogoz$e7Aa_Az73b}7Ozhpw>u%3x(1f!9Bk7QnV&G2A-~dNvwpecuqL zOHz~b@lAEi_mm?QC4(q31N{lyWb~fw=S0f#rKD1l$i+6Ft<m=JYlE2gf z<0Mj8xy$Ryv0PX+6#HU5z8$SkPi-oLMdJ2hRux%-=jmW}B4SjAQv`SNxO^bZs!>Vs zL?__IY55ckye-LFw;FcI2nQxjyyH3OR-d?CBX` z_G^chcK^WD+ze49w{6usMEnU}*;1K{rlx|oWD-!edNa^9U*(|&^%cH2{fkfqV`$0J zl7MAXEL$ner?H@petxb8;|1SAwW~sSlZjsssY<+ zQYTF&XJzSWh$63oj7-c9L^q4<{^aLfa)YNs!A&xz_jLN8CJ$?Y`QsQtw*Mp|2|ZPP zFU%lfXJ0!pa`Gd9BR%lV!c{jbODB$VOmyC_J{#eo0dOS82*J5}b>mdy-^EVh@2~qK zNlV`+oqhnka0EC!f)wcmXhST>o)+v?n&t-01$HsWsWrthp6_|+uX?u{oFn}KhHJ7! zZaI1naoiDj5@Wtdv$lYV*`&eD_8HrEG<5K18Hsgm2bz9NWL}O%A18qF ze)~HSB#~CaNt{**27!2e9}u?B#H^_CIssYbiL;jV4)?;`%pS{6=of3hob_?a}x_K)%AQ9AJ5auD;}1+yKZo<*D+tBW&r*MK$F| zqV?(kD8Bd=IiKG2(%}Nt`Iu&|osAm;1T(79NC`cDOggRB?;UQ9qVJcfjGtY8yPxU+ zX*4apY?Y+`ikpI&q^I1z=dgg{mmL=bY1h(qN#Z}JZ&eamW5c$(Je0x-2N&3Zhl0dh zl&5A7S%@HCE*Q$Ob@~hLK)4E9*m3@Lt5QRG(EE7to1Yofu>>9J0u1a?i&u*?#_X&y zQ^D6N`W6jqe#8HDsb*XumUFgA60PWkhX^C$+WLugFj7Z-S$1d1 z4MyQWLPKsDiRwPE`cPXmSG%@Ilo2jRCN0hP0>n^PLIH(0I0N0DekQsA&$Px?30q|a zFhZ>ADc94dAJyT$o&`4MKi%Cy%788LT-t0)NNbuuO^O4+3gIW8&<=)3?>A>F)i>D z%$#t|8X&t#Xk|xJPR_SqeK^TVbXVT*v6lkv3j_i~+QpE-KDu>7P@LbJ+_4!PCtfNGvbTT@1yQ zo;aMZRE%9ci~!Tl>brD%f8)5dkCp_StODQ(>pLKDJ}nl7iULwDn#I^Tj%q2fKow(F zA`)_+OiMyH%bs4Mb+Vh!hU-a@%yD76=oasVL<||r z3@VX-A2#Iux4l6_Cux75jM`DSY=7KW_ZmglF=>Sj*r->zA4AR%OYWf`4m+DQf*(!| z6zy#AZipo}KGe;i{waW!S~%Qq6{|!NM%Q1L3PX^r-7_BUT% z-qHDhbux=114zPRU2M>?n%kX{OF7b!M8LfLDMx8lVUd&I8;t85PS?%b&gw!|~JNRDhGOKYrcV zhnWm`&*Ckay+)*QMY>4+@Oej?9CO1K1LMkKqKE{-!}r4O;4~1Sw8q~XyB?_p=t*s( zmTPgL)h)JYAG0kbvOlq(;^#Q0kBf~a8qSHL-N5s?bKf+RaI1q2hTNB*Rv-M0-IIzk zPV*j2gM2}JP7boO|FbY=-rreX2UFm0b=LrF4$Pr4(W1Iq30~qMfUfnr9B?s-brNxq zKiy=D7$JhErHx|DOKjrGbwh88Wjkz-^a5L-o z1~kmB8A6?rId-Ls&8knvHOrkMM3U&osJRPRlU3frWjvs!622zKCl%z440#=%ZJBbv zPIkWQZ#OpDq*mWi;G2SrF4agG>!@sQ)7g}G*k}yu+rw&DLm4q^wI0E6*mzaj6)r*? zX+lcQVb35xI-08VaChAuA6pmyycH+uBo?=JkbWaH&1~bwvWR65|Hk!=Ta`(VQ?ZHN zCFECf6hayBjuZOqEVZVn;qR@DCJ&KsZ_TU$8Z30yA1~jBq`)?(6Qt z$u|Jy4)~+dJH>{bmTk)Y7lZU;zD&FOZ{Yb*@Zs790lsiw@=IZgJLMWh zj{zXGWuk`tM)q9;*E)W2&THGUElZ=T;K?2klI9jv=q%@iZKEaw5IRsDiyZ}+m}!xnccIO*@1$Y6=@^?Y{?m;IpY=VGfDZ#D zUM^0Z!ZY2ygmwZEoIy{Vlk2-LYG+U0=ll*`H}xmLYnpYXqp&+4`>lH1fooyMr>O&y zNg$k2SJ;uws!H3(!1i!RtlzP+u~CLZ>{0avZskcl#_ov+zqZANKFq}WF;JVgTN_(x zQ^4>A$d`s}!asF9B|b6NdQNA(*<<17jw)A1u`$z5*ylBu`?f+sN}IfqFl zQ4IC@Ye@wipva4~9_J)Y&4$`iEpTedtD8_xOT%D@oG+jVn^XQFT@ouB8)_m&CKB^9EQ=7~BFf@rOkDk)~HV43#?~ckQ8ZS12+|?3U`}!Z!;MPUVSxlEn#y zhttXh{Q_AjT{1w0BQeKTPOpdK#eNFbqi#Xb9HNM`#Gx&ov_^x;CdpIS>aAc3NTf!QhVxdq zE6IF{&*6}M6{zMpx`mj}2t44i8f&6agoeKWx85Z<9Bt;s+`Iffo0aeqbKb@fD^?&jbAXyk1Sgwt#J} zP0#3bOm*!H@aFdQV>+w46lCDIvb^3u2I1MfTAI7W zl8#+mfTMkS%sjI@Qv(eTXeSlJcs)of4J2+Bv}Xq{nIar@9L?U+rzuXAYj*JA0`ADQ z#dr}ry>g(mU0ofG;@HczPSD4@>E;1G3)y@@|lz0 zurc`&@H?G6z2iKiOFp!6F@4h%hM6nXjW9mFxe{E-cmSHftuO1`1tPsLlL0=V3vhBl z@R70!+gx-TZ+=M^}R$I?QC4JrBPjnp{A(psWuj}*O!5p z6TkMPCYh(TBs7Hg0jTSaPwh59Mo3%atr1B{@;(SaEvMZ7H?!e|;fImIsQm)Fmgh|u&lJ?Pum#QE*} zquiq)uNih;*}-T&s( zg!CVQb?2c6OfcG!Ch#!@&eIhK+!5m27gu>303`$vi$ZvFZO_5o;YVNPxhJg3V!Wij z(+FPru+RP^JUt;v=bUbf%Gckv0(+7#>4uP^<6RJV0piO&)zL@=@!XrMw*s}C>DW4o zb)z^Tmm26Bk+hhgyjr$pRKY>==`l{F6~AGdGKJiSASiaFm{Zn1ki5&OMbyLqa<^`+ z3L&+~JuSzO!eUhJ+g_$`HNTk6x`2sg-)t>f=Rh{Nh&uDZ^~gs$_<|B@!2<6m{q;53 zC=EUGvRv$~iAJ_Esr=Ixkz6n?+hC9NY0S%#|(uAr@r+O`GVv~BdG;!@H{Kb^^?y0n{mRCc+UVod+gj_r-w{R z6ks6OfQyUz62xh>+%kS+#6%hXZR%L%VW|bW^rYzD4Q^y3UR1taC4*A9h4~|gynX=6 z1k5zXI*Cfd#=EI`)J)FgUJb(YGY`cEg1%U@H3^O$C$uOf;;u1QzA(dw+H_-F&rI9E z(<@g3Pn8hoV{!<)_8!&&r|8m?5I&4HNg9TCdzv!=$zbUDAYUbypB-~h2|CSUZi$LnSho$=w0D;O&x$ad!R0$jk?j#RhL9;y~6 zpjD^199B#WYOpN6R87t4mS60eiw9v;W9Fw#)g4<6c1r^Fa83Q7;Ei$pP!8k5X8{6n zJU)Jn^rEmlq=8_b(YFtd2B7Jk_xiq8MKzxid~2g@@?X0o^>bVzGrc$r6-@>yBf-pa zH>0O#M1&(8cg2Qck!M@}k*gxsPeD|F(TqkEPuezYNIvVFd9q2zi+q{J(rVhVim;au zDi{G9jqnWmHxSN*r(Z%MgTgK9)Z^rN>Bl}cBvRS0MtJ$*ZQz3|&zS~VS=aYI(=iuo z7;!Wmi?y zw`w7wEg|;n@`Y4r9&>F&!{`Y~J4N#}gK&#^{aiultIzFnyM4UF*NLUP!o+sZMfOQ# zHNs}f{WWv(Y4Lt6bE({_;oouP9-09gO%Rx59dB5pX_BQRB4~}*?d;2tvqZqZ5G;DJ z8>)yzMPyFp3Uu-Yi%Kmi(N(;_^X4T}okT;X1MEd>QiG1CH2*5A7NMb)831nWxHoAF z-l|nBKdJYkz}77W&qM-fNq>Aup?cFGX`|^E>ff=l;WA$C)sMilh0?DP>VQyUf||wh ztOWv>blvXphA__h|IXB5m8JI4+8+HL0hUhXMeSUyTjqa~g?VL@%xEr)g@0%B`Fk+_85+Z+TfHe`e?@df60fT( zPd&{~WbT;oS73|2}~dlNDFd+eOZ9Guh59PBcjNt{i_H@X?=*9Jh9Z4D!%xCA58 zx+C{@X>w1Z5C?|aI0)@@@WbT&tA^1*J-$u=reElgg?>s8V#gKg{48Md4{6*93oJOo zGJmpT#Zg4rJV%#&@nldsFUD#5M-?S|swY$^BKoLuN=!_21t;(Rp==0O3h_*Naitt5 zdSxuc`l+)e2AVvtn`Mqqy)2eN&KE_$uO9qxzckX9z8pnD+2(mJZy<9!OPQ#sB0`S) z^R(B)qLdv{WwRVdnABH=ZUb)TDFn#&>7w|CnIAd8-yV|)+`3~h587;%?!>CaKIkj`sHUJxK}=C4GChkB zuDb4H$<827ajJ0DMvNgJm`(WVke_QmxXMAa-3`i)h?4DSl4?k4V**;rh;Nc>VOJ7h zmggEWF6ao)wH;iOh&LEBoV8MCi?$aN3m#DqYeD54VPOunVw?gB<{vr8HF?W?_6dJh zK>y=QXjH!ZpHgj1|1s6Z_&+f=CIUuI4o24hd1WSG_@5yG69Y2~+y64vX5DGzOgFtk zhqE~%={Wi?$2Jr3f1j2_)PJ!y^4-T9?;YSVr0K?e}BXf z6as6r+kJBb(;JvPhJvqDq&!m_eQN`weF*u+#ule$1}5;kwvG;{Hm>$L&W^kJZ{#9t zLm+!LcEB`E^-O@EV^kK>;~_wl1IG#g2V^Z|F<>jdfXMvjU_^nDfw7^L@gO99%TtKD zA2(oqV>2u3d;YYnvmemDmF1!NcYOVl7*{p`4Fyy|Mt4qBzcPmI2!_7C6$}F6d%3pi zM&wog9kM?$Y`@H0SbmSbBvygH;vv8BKSnOjzpWzRgJTd3Eewue7#V3AYXft?@OB9; zC9VK1zLn8BSHG)I@C2p4cYqW>`*BG6)`lOkt&z>mS^m+GenM+ABRj+ELx==cX4VJh z(DDt9jCJ37^uXBARycFDnsX~CW0^#Q?;{K}bF>c7JC zy-ycpzcZ0l;q}3h)4hi>Kl_KdQCOSnnx0>|bibG*y+PQNOpHtb=IQAe08v*_Spc$VXt}=uR!0YC=7yJF^aH<`lfNy$D&k~hH)MYp ztR`LmZ1ty<)8-%d6a+V-?kzPgepDp$VUi^upKL->BF>Z0+5d0>QYu z(Yuk{W`@HU%Iw9Ell>xuT+$tOp&_fKy{}kzQ=jxMhhFLgmK$thjQekkY;l&>)`C*W z%QP6DWUY#GCOr^EGXKoeU!H-BZNzBl{B1%lT_P15pZf@5UN5crlfep4*~WI{yR8Kb zISIWH+(|h6W1}mNr|*kV;%@gc4O7`;WWISBO}5Cau&PR01p15scr_c|J|N$JoI*sT z`_RnR+Qiedlfq}2QTr7ZDx>`CQ9%{m+&sK0T48qPV3K{3b{sE*&QGTC`vE%sLY+P`Tez?E8&jwy{DlCi)5_1!V6or= z`B9i$UveFbu;oaVa8KCmS;hYz7msr}^zEvO=Eun217q6z&r=bD*@^VWzC5=nZB$VQ zdtV@vw&WtsO5F*NH&0HWuxP)9Q{WcUQ0(_YnlI=nQ#=!9sb|gTs9U~U)iF;Tr80AZ zX1x4IlNT{uwk?h{G(1TeLq*#WhWFNhy_4kmdYV|-pb}FUSlpRg452ym@@QGZy$nLy~y3O=nL{<@{ejh;-X?xN|-1hEA%Hs8gfTY&!-;uXn3 zV?ljjqc-kCKuKWJ!faadEzibhKidK2;)cV9U`8*fqn3wsSi&s!06$9M_fv=1fj@`g z-08)wy5h-EU9#2?3VC1$V)=Ko7(HHWDX}|0G^tu+mP!0*{o8&&a9{3iDyMspk5VKV z@2Q199+P080W%RBW`)DHo;M%&}&6O$3;NGi26qT1YiTB1W!hsiR_=JQk~K069xf-jjGZn>@pK z?P=gR(&}30X}Sl^q668u4<)Ip5Y2P&Nk?d3sprj>0VKy?`-`fJh4UupUq221va8b8 zKG;h}U#>#Oeu<~f!nGBo~fBN^RJ_ineW^I%p!Tlt?wwSEEr1x-M*zy9Mw z`!z0ukTI7FbQI~HURh^oC9(cr29RUoDB9Bc1>Gui;jFHYr^PMc2C z58X?XqwN!s9Scd2a*4Ck7m|!Jn%F}V?1U05G>-CFWX49BN0is;pn9q>oCFn2iM%1f zGc^uWP|yG;0Sk~%V)^}HKuD3fG*8mv$oSLvSa2#6ztBRdrw3h%Y*(IKxdFeC-J4`I z;6zsC?RS$QTIMrAbYR$yQ=w3B6;15+OrH2~!-x{7jW+P@HBHgB^8G1etRSEQx2j8@ z)x*#%wFF9CrodK@j{K=&)`2hxx{|yK4dWmLrl)h=hP@3C^&BRzL5?hy%-n_mOAuOQ zk?3ALa~*3Q57_L1#IDh;bH!)dw$wGGg@HT`R$@)($Q81c>zJ`~o@Jnjb7RNGp>X01 zF7Fx3y1v?!HUG$;OvQ_lY^XfWDCJ~2VsqnHNqNPzY+_}Kpm1@&gU_JmKZJQH{lU0(qAdBe zrcV__#Z@jCGnl2P<85m?gWaZW9O3deSI)<-ff{426-3BzCIe{Q%ROtx8j{8ze*1TVkG9{NKmzl|tuoAq zk(w$;)1%|sj}T3vbo>qkQ#SfN;-;#SDpg)@(Fmy4Z&B<0dtGq`)z)KdaLw~X#sQVu zzA>BahKY;ttQHJ>6qB5hbjwnM8jl%5EwhU*nV#VRk}_lEcQyHI89!&%$g|)B@SV zh^e2sGz92pG~xDGGP#OJIo&0Zbt$iTNvmkS^jQds%_QY_tsM40^x9M_4}>J0DJ}=t zTCTcgw_Ci0?C}oR{UL3Vfl5z`&sWvRUZOi4t<&guqAJ_3%DPQQ^wzxQL=`Ea#$ow~I$fDY;|GYjHkLBb}6P{vL%u~L6+4|jM zHrM!%D0eFNm-!Y0sS!;3J+FnZG05+BL4d>t1ht`teMyFOqbvYycJcm=J@piS{tIsj z@#{%~Gx7Ov-NRvV%1sC7+fuyx+L!+=kL7jIDM}86{p@ylrJba=F+pqe_g8%lSasI# zw_}&dXCbInqux=CXOnA0G)8k~@j#)sKBuKIB!WwbLPmo^UL`!oR>4xc*UP1FwY3Na zhj9U`k#p4jhTO_f-tY*1#y+*Xp3O=G?h~=1I-WR?GkNS;n5~oTvo!O<30sdd-Z7i4 zws7I#C~8L}{*-8d&+8YG&De!N%SU_ruO_mNgomq?%Y^Q9CHQ$`@l;veGvez5Dtp@3 z=gro=>O@m}EK#FdF>j?S6ftn`8c7yZ-Da3w$vaV0JC-Ads674qT0z$9~909uz)vq)aU0(|1KWYj!v2YZta& z)@BVO@US|(4|ofT%`md;88rW?cL(6bYhi^h&$-fx$x!ddKKd_1(!4R~q3HfKt6O&@ z`0rFw=)T{u!Zkv_ba+jdW_r0K>1k%?n0RM_(N7H$e72uv1|c7gEfg2O>V6N_M=>7{ zCO!r^!!9QR2URqhxQe9@-faiBTMOyu${iXvyP(?n8pRf2qeJ=~+F>|mU&g_=?1 zda5N-$A6k}>~H{$$+#+H+AtQVr-!^**M;vGERS0I&TqK`n+?#PypfKi!QpgjoY0)O zLqUg>KWamlh1=j0M0B{MxgZarX3XO@D=$5ftvLrZ?ApD~W^_;g0-5ZcGOfZ#_hnpV zEZor`f}#1)!nQLPvqK_+c4s>ft^>i9W-yvTwq}w{C-G+%76le^6-4x2@=}Q~6&7Ss zu1;z*Zu7(JuEip~`V9C=K+4&_La|hDI6B-4N-6#pR^}U$SU|4$kHg343_PkX$@-Fy zZ^AiW@-XJ7TE|4foiPHw^iHm$!Wv>XrDF2QjLKx=^)Cbi)c(9z3|(R86mAdy><*cU+YK9X6fKJ#I}`H0ltpq8!U==~F-X)Afjb%p? zhu7cI;(#~-WkEb-wEj~&UDQmtC{$Utc zr&kBPE@k*$n#sqBTk@^pmE;F?t@~+V(RSx)e0#B%9WRaDh?nxY4VU*k8Mir-jL?pV zgRi7~7+EG@i&Cyr_!uf_lT>Svtfr*EA$3(W8dzU_D^_sFd)8{=^#SjDjjZz`cBm;c zgL+Rl!7n=X2kV%c^5wvgrbM6a-AZ6cH|T6P`#`b-5<6Go-w|cY3qRC_X~-a*K1&#$ z>-grO68+PjnPA@$^6BnujFDWxFw^Y0tPO3I8u zXux<`4qe1G|E)7y;u9O|()#6#FVUG6rEs6pnEVIJSS3=ypLy))(Af)2qh@WeE4#9) zq1d1LCWUP&_11uS#!f}@XZNE*5FDm`)KO@aZsJ|azZ|B^T$>G&f4pZ zwD_WBLuvoCvDxO}cp#`>I(;2t>b2{8BlEG@JR&#dhr1aFq&1-UI%dvo2W@BqN2bk& zh|8v(Cp*GoRBdJy_*y45&(R^J0k=zn6_3NslEPY*4=3Q9o#g7W-k^R#ltx*0=$V~o zdBARo4&2yqwmxM^*k(tOy=wM{3p6?s##4VH`|10#6xcOl zy>P*}hqPoj4QW#039evuJJqsio7+CDeW#FCIT7NE!!~mRTY8|fQK5abZHt~bQ3@?q zthqad($UV5U_|T+Sg@2$!DCv}Hf!k3TqqO!NW4;x_^%Q$E9$BxlUJZR3V zj$Zv8=x{g|5F2+^PpnkmsTy0T>>8Aqlrh)ow4dLf_Uh{NLOr9+0Cv6DCM7oL{te$t zl`}})j?JjRSC%n`La*C&J+~GkZSd4Zi6w#)`o4gOw^|ofeNCL3uuUd@We_LjySNzBz6Nv? zm*O{s+m2}5OIm~%9VmwOY}R6-(PnQbNymkUa*TAJ%JYMY6)eXB$I8_smx}{^*27W~$}1^c3s*YnX&-2yPJ({E<+sciO^sA~u4{ zB20=OBylGg{Zu+2Zz_W^JK8` zWnU{|~B<&jRtfj5Q4^xNz))sInpq6%8v@e{T9 z#=;7JvxY<8eW|ZEG<`0g&qtkaHrA<*JaD${Q*C%Da(W{w+9R!}Izv88501?;?*)y* zsIb-b;{4ijjk}}#3TYfD6dTLk*-hJyDn3&21+^G?WLP4z$G(rAYQf{l4?^Khh>t=6o$jw{>352MDO&6aIf?@PUH!7 zcNE4A^s@=DR_xWk|LDtkDGlkJUR&8oN~@2h3l6b)V@o_fR$&GI-Nqtib^wLFcCrIh zFIN4+;COu!d7Ri)jtyk{3#VykO);@6C#mmxVv5xhXa`k#;Ny!fo;h&^pe&3Q9`h#nY^k#6Qc;~wQRLnpvWuBhL}Q8kDBy( zy#=#nKtU_gW^K^E!&9pFWgEA3!Z&?1XkYH>Ub3?pMlS5ur_3dX@x2cz&7W@W=(;O7mJgL2>$ zm-PP1WtZ7~=!FsSLd*hOm)Dg%WGZ@Dj+uH~_XGaR>>E`}Y}of4Ez;!Co*`Bo<{7)L{&ldtcy#Z-RJdh|=4OFm%+Jk5pxD%1yIRnLs1yTmjIX+x( zqr}Z6>ZbK7R+q?{vGfW}%;G0}qUSO-MtE)Pu&Q747yLU$9VDPrHC1 zWGe)CBI6Aq#^CT1IvGCgiihh~VQ;LY+_hdK;G-7WjhmjENgFc=W>>>@NrcD@a(TKz z`o7u$?=ytc!Pcbyza>8s)4$eoj|HT%=sCmQc!pc6HA_wrB(yLJ8TS0ssCsQhc>1L|nx^K6`q80BFlA`a=o=uGEtF-W-okoz z1I;$* z5{}Px)R7?`SGu{MY;I5k9nao4iv8p)&H({SL@55k3sM&Hf4yx4u=Lum=EBfM9&tl3 z=N4Y=Pbh-zIkSt}Rl=P;WCksUu=veLgINWgUH;UoDYx4B$i}M~9oSREFJF=cuX;h) zXNlY*FRpq3q|}s638^q53)ia|*rlo($%l=g

g(D~FVX@JF3Pc>7s`@>KYnbWRP`|ujl?&KQ<^?=ELJ0%5ET$6!cu>1LTlOAoEWfGy_22si(zpEU^ zg?hDL(Q|(%gjDVzwiI{Gt-Sm6DL~#~*-4FY>4+!Kcr2H999BOaVJowWX4ys#a4Y10 z&gX`o-h~t13VNi% z?%2&EHiPgRZ?cK-MN&S8VxI%yembWU2zt1hlm(zT_(pL*;QaSPz`%zvDQ-i)V}>Z^ zSCi9`D9|Ky@Qc-6@|+*jnW@^tNF&mI3{SzNg|GTfmQE=hS)}b%Ig-x%D*^I7gC!zn51^ zQptUzA*Yd7qy&^XzuKh{DJzSS{02X0|##)>QGs6sSznUzjU7Ui)XJ9hH?g zsTHoyM)gQC#Axo`iQ(c{pB$QZJpkwVyk^9`p2av~#rMq%q9LHfS(!h81U75fS7Hya zC^SMfSD@P~RizVDmO)1bl6|hKtqb{(9`>rx4C~LZz0cxt7Y=124s65#!y~5>U)<){ z^ddv&M3d>g?*kK3)vMo)bTNcJxF=1`tsEc=j2_}!TuH{{VAJw64B0&FUFDeBH4baw z&7gc(;$!LVNx)kdjj*~~`kF1OfO^Vd18T79Lh}=yNlz`tW?}$aS8kZ48^c=gM{R}$ z9!n>7Cm}BS zq`#<5rIYDp@nam#g?tC0GA6azYJB9{ONNpjsG8s?axZ){X^&JnRuCcOo zYf4MUF;*B|u{U+BH}1^NZ?5HF*aE4JBF5H0V4taOgSVZ5ktYK_BgK>!+un0e8Tzs~ z3;UJEVeG7l{;A@oeQ1zS7kHt9l#NM9JneXIg+HAIDQOT@fux`3Gu|P>Q0E(A7EGBA zXKeYS+O631VXahCmY{fs8s67axvRpfQ(3PyH?z(D8qu^cG|q72GnU1B*2U|jVs}ji zrTV5!)~3^9a|UR>k_@NltRU`1C7PzD*W{s4tksqPUznssv$=t z1{)-1XW?wlFlC|0u8#Kn>`+yrKCJm=IC&;O#ri%-bJph0n)piZB%zo0cP;YH%!2OY zC6t_PxgVN+E#oioR^L8OHm);bwheDQ()Qmw=dUPQl|KAG7zu&|Ri~*EJQQ38EjN=r z*`~B_RT>8UEKu;oAvvxzx(!8xmD!V2->~lRD%Hrgz!iWUkjZHgxV%O30~~!MPm4mz z)C-JuJ&n8gC-& zR64DAtPEwz*ur(8`6uz;q}n(amgtiB?%W(H?wzg|0(ZlLT_t=T*pk_-#W}t7O$Z}D zAK}SO;2q59wcFhOX9EI~LnFd!>)V^Sv5l|q;JM_6w||_|(r%~~*EVrU4M_B~d9fIEq@u5trfhz*P#VY3V0PXHApz+%YU+lKt^Ruc&U5KtH< z+Vu8Hz?L$OB}G~Zw{QrzIpyQba#+Y#}>|4 za)HE;!Wv^Ly0&ur7rO+x)j>_8U>;3-7k#hAyk0JoeL+l$YldtWZkf|sOAYHwW~ep8 zq_8Eic?ioi>;^gGal2c+X4AgEFJn2TC#hegDRztGx7c*y{q(Q!)RhdBwXGS2+Z#AS z3DF?(qFAPKFr)lFT2ygBh2VVI;W5)xJP3=F6@|P zYzj;qrB!Bi>>Up$xO@J^Zbj#3mQ}{*QwP|bTYK)H$`r`YnFY)D;%QwFM~+;hq2Yu4 z6`KovZkN!VD`e2Vho9OZdz$p0<@`WX=l1W0 z{K(OvSVs<&^!|Z!8td|v(C1Hs*0F0t2XrnN{(6eH{u?sEBH7RN$`7j z;m-t&Jlk%%qnwMkyM3#{0{BR-nnH23L5B)8es^Q>ghQsmd^e+%50tgKwu>-}o%AN% z$cF;d80v~4_x11`VM%jknROOwv>qDUo&@-nc(uBV33#nepp6JMMxTvc(Gi?R+zHd6 zVPIhOH8~VpgIg`2h9{;{JP7g_|7BoUPX9TJi^U{DVn4m$AU%=1M&nhne#t--V?t^`I0k)8$AaE`Z2t~kX^Rh&s9vBD{QhNi2^0=aHb&CnWX2PD-sTfLI5OXJYPKPX^JQ$@UecTN_=pysIa zug1g7jXvmSxV|foU&K`$&u^>q1MfF7{~B!D#soi$V<11WV~`pu*|#U#6J2sCR$H6? zz__$mSAO?*+yQii4*I%JPHb$IlIs)Hc`7ttK4Cs&J1xGUYaaLxu1&YrfE&=WcjFuB z3JI``GcX*lP$4w2B+~)!AJeA>MT{WOC6zEVf3zjig-JZiAkw<9D9RVeV=RK!|M#S5 z87sAnAM%;3^Mv_|7Exj0KJD-`=Ek|uGBb!Sr-{Fp+gd>{*88+or4KJTO!daiVKKa8 z<#=J;C+2V$9%kqGb`R#cCW~KPz+-GU`!Am04AzR|AqZg9PJz`-`H9J4EH>?+{#n99 z&g1l0TFr;oJ?UeDr~idJ(Cs-@l%h8(^we;AflbwvCp6jXLvz_A5*y+v2;KR{>9%Wc zOb36Arf;8tPYlY~o4i0g86()ORK^2ja=X@1&abyt>opw(0^zR&{K_5j9R}^L@|Z{b zbe4;syY6$YcQ#aT6%T$jv9!B#!lo{N#jNhe8;=-D-X6MteBLpn@V}5kqcWScT~%ip zv&So4l0*3I{kAF-Pt%b6tK2qcBr{liodXs${X^GC3|P&(R!u-*^o)g#!>tt__bkn$ z{JrB1tKe3!eU6IDB#8P32{+X#(tJ47L(?D-j4e<@leYhxQ(#|md$TD2Gn?q(ath_h zFLfG9CwMq9{0oc<&sr;X?6$qhhmqsObU>njBAkxL3k~hNCz_^h{JAkrxl@8aNJE_N z@OeSJ!N^pSINjb#_kF*U?!mgV5sT!Fsrbo!^%TfB#kic*bwwp)Q|8yK-Xj>jifU*a zei$NEx}!%bGo?Y|2+%Gq$#Xi498I-qaqnI2?~5E2(7){3cdw^2fu+H}ZnvX}#(tp@ z!k|2nNlLipop?G&QSg1(G+6@VVI>|a33YWb(!KU3gQ|LL5knLYsVFSI_OT#X4_{o% z{PY^;HHN4BhSLmnHj@X7+H!O~$^`Za)SG_=E>@vGn%^U8T*w-5eP)?@1&}8%r#3vV zlZ>jZVZk_zG-B^ouuGceS@Xw<|GYc;wBsE;I-s+oA!e6)t$WPr_$y@!pWXWj;uW6H z-#B?%VDG_nED{=Bd$0HAeV{f`ICSF{j0O}|)2LixwwcKiu`jVicM;6Zlx#Aopbg>j zXuJCDi+nbr;R{wukbID= zApx}p0^_3Ayp!@>WzeJ1SBUGNkQ=p8M!(!`Xc8}G;g6Q0kBME5dnq4)i$21b^T%Wg zBKA2;{<^Uz3~`8Tm2-U!28{040pC4a?5#C^y>h5&G2R)R>js)PK)fm_uyVXzn2Tm> zkdstciaAdil;kCugU=;P1)Pl=K=6f6FwP`r1|TaS7Tk?5X45~ZGX<5cHG%a>3m@^R zKfvv^9||Bs)dDN>`eU3J2GLqlL^sY|kXUA}b&Z8x*)CG*Pi~wRo=&_yPx-LN`>U|U z8}yY%5zM)xM4;*D1|{8~;1A=t`IQv!r0+@3xGE#A8XS~y;LFnHNLiO* z2=sXKjB>W@2{Akwut66wWA}-<9Nuvm z)GGMn`SM16YiN?NqiTk9)E_B9K8(?0$0z2reD~R5q;5%ywE7KQ$Q!2!{rl$ zgvLSWET<>Ug<%U|Q*r>uzsppHXVh&cnW;2=1i{`xl}Cd63gss#%HoN;(4TyxV)zmU zjBT2_v(R9v5vefiCWDWa8~$+li2k^$Kz6DaR=jqX1sUW3c+q1UKGS$%c29DvKYJ1p zkECME+IBWKW#R&AllNno=a!ne-|QNIm~Aa4u%cE7Fe=WOoW-S`@@imn&&@ENfjbyi z^Y%8NZx~`G9`$%?(!rW?tdpquW0B`Li`)+l6y6us4aR1TZK}Kzr!HQ|q=cNgm?4<75JMBgEzEnq z*URpOJ^l`7I0e={G@EEID1ph@GG~6=?Vx)ekw7|!c~!CU6LIRTKBKZTr-CPhExsTy z454WGZ4YL6qWyIbjhi9e*MevC#g~rl)NFtPy^zhpA@xYIoZ%=d-otHFhsVBPgqjrB z!f-8Ho@{3i$MkV#V6hNOCQuS1p1%8r*50y+?S>@vMrC`F7|AvO%0OD)6^kfKT5xsB z6+xZK{06RoD%b14k@GGufeSnOk_1JxOZ_h4+YEn{X!Li~o4SFCJ+C^yDVp;Wl<9;U z^h5MD_15;P&&WgiC7yFy*n!m^9<~5|>dNUDN`U+_6$-OUKsCZlHE>M%yroj3cjNsJ zPvWZD_#+R?;ZD#xBT>&eIsF*pam*yGk{>g7pR`&5tI z)cXmW{;r>Z8Bl(%$Uv)`+Ud;NAh6)t6p!&*in0>TG^=ltqbi=8jj}r!H|{ zm7hfR){+}G2PPOlQu+xf&$8m=cO0*_vIU63d#l6bZd4oz24diCIc|>cQStw6E~z(zzPh+ z@MH*oGiNk#j*STo@A!t<1M{wvolg1p&XtN1Mv`@>1vo4rZ!?G#MHQxiDG?(T%hPvb=dt+J`g|8mceg@_GQ{2*7IsJttsvss$kqu>?3 zAp(bQi77PrVzCc&lxZ%Zl3p3^77+cOn?x+>s`%h;1L=aC&{Hv;9QXebY`dAdHWxdo8AQOnM{`Ytk~Tux~7A{fj5) z6T#*Yj5(GyqCEQ&ZW6fYlch0)aA^Zv4Ol|#3OVXN>|~d)%jdxzVcv=E;~OaT73)Y z7H4OmZB0fm(j4a7!fsehnfUx2Veu!ZN2+_hXXrBaH<0)(q@i5Qb&_Sx`+9-((vs1= z^ypv1R5?)Zd2Sn$GzTZ~*+#y>)90xUUSz1m8B7TSo-`e1nhXt=Q8b+_daObNAZU&T zy|1iLQ5Y95DZ_@TBszN+x)2t9T zy4Zlf3jb%x5xNHs>(wk{#d>~5+Kdz`2fj?AXy)aB(z}IS1Jx!LGNUVrsavf;wh|{E zmw|8IM!*uvh`zR;4MW8tR+iL>{>y*Zg0EoSCCAf(Rg#KWVRrLEXDIP0WyIUi`v#q zk_PHGKFm)8g(e;W{Sy%`d$+NISDOndXrvDB-J3?8n540`-Sc@o=(xy#W#HbO<*qve z9MVpO*weLYM~0DawFcf3b8&4ruBC1QizX<7R%6D~2N%sHN6@C7c9Pujy-krmULlyP zh=O&xCO=RN>ktu16Unct_D8kA#via0AiU|@!YVUyUaXJzfh`e#A`WBqYjr@Ry;Rdt ztvte57d*9&4pO%r8+2D5m)f;P6o-M=Qjy2M$S%PiH;MQlW_L>b5FVBeV4C1p<)+vFwCyiGq%ZUdbxO7m8lkMAoBoxHlAE~IX6g{YOYb0 zY(n*t?i>QnlH@uOjQ>xxa_7UE5c3D5W1xe;m1%q|X|zCm#csCh9)1Ezx1zY=KJMI? zdO?fqKHbW&lH|28aYC2^!X$0b^cOS60g*KIRy~){{vFj~+(o&dgv7v2%vE$#XjaRP z!7KbO$Lp`GlZpB0P!8a=BW*galQ+?mWewqWgr)T!7W^3`%InrGeloMVgax^7Z;yyH zd-o=JYH}h>k85rbHH9Ba6_9W<3#of1t~~xlo;STXkRgvuL*--R!LxqyEa-oKFZ#qv ztJrR?CaBC&7twoaYx^A6p58os?4hCmdSPg3ElRtkT;P4Qkhb(B%@hA>~;@dgQMDhw(4((jH;B2RUMwKR0EvP^*6oFMb-Z=DFbUMzlyQ zalP6XV_i0x*`hxeP&7&ubVRg!#6k+Z$+IO+ou450jc%L|7n2J}+C(c8Zu^vG^>Lqu z)M+gW=&H2%vs0)^6T_sEL~&n6_AfaRbBN9qLQ1cC;EM;&KIwjQ={OKi0WeuDF9jc$ zsP>hIKAJ+%afHuyuQHOWvqGgLpFVXlVuk?8#1o~D3^!%#i9+t#=H(TDo_eYy1k)Rr`-on$ z4sJW+KMi21TJ=f65H>h^Q{rkouUxOJ3%XsiuBR{D=v`arP!8kwl}eg0nf$$VRPrZ4 zzNuT&LWtsVh2V&D!5II3Y6cgO&aqBnR3Pu;@h$>SS4>{L4AyvJOEkW74RUs{%(17p zl-^#nSN06kd;H}(vt>I~2YJG~|F@H7PNsVU;W*_TIl90zmkC#7P}vbFn59xeY)1EC z!XOyu>Cm*Q)8rh!A;kTmlgYYue7-*umwN_MS)+Ezse3M~REvX@-D7YyIxr{IG3jQx8=9dZ|)O!hj-HgrZ z!zr$~SvFeB0>APq>KLkA{IwgYGOkSGAEqQEyyagFh*;a`wDi$r^bt4;+NGFbY7%`P z{b+*e?(PeA$G@GeZYmN>fS7^VH1Dj<-`&K55!PGma#wWlKvCP7*b-d(7{WPvi1kR< zd5J9FaA?^2_XrVBSSv@2a5B#P#L=cvSdr=QP)ToCC~dZ%35>)CkrN+LQ5wveI@in@ z@{wnrx%&K!3mkIV?#K5CxJ-MXAaipcS86spFApRm_tRF1@{ zp+3*Oty>gzCIfUB2(7}LBkAWM(uAK-Jh6;Xt68D;&B@H73QIGOe==u|T0E11U?lr2 zHy)L*N6GVfs?h$K>;38Ry-h-L>loTXP~B=Lyejzt3Lv^G{=Ug5ZZlvU65Mx#YGE^i z2Ph-M1ibv{U{QR#F1x#_@5%o2>2@wsQ+L?GOxR+-fG-U5c|*aU+EDx~%r&x-AeeN5 z=lFxCtR?C%sUt)v-aT)?%$(lC=8X10BUCqPtjyu365O#0L)n*9p$LWF+}9YJ%P=c^ z*ny?sWWH#Y5s2!*w#LX=9XOqY*XY|NcbgbEP0K)pkmOv8#E%>?caxQ+O|9ykYca~c z;fKThqm>?^bh7yeVsiE2e*$Viz!Xk46r~uAxerZQ-PQH)!kK;Ps0Z-Nxv#}Cq0mAI z?(nTdJp5b;Sherx8mgtmOxkM{43Ofyxh&<+c0ceR>HyJ0a9zRgy(Pekzx?VOx)0kw z`M*;P9Cq(|58@p-rC~3^~HI2 zh^r+xcXX;6^tHd&kCL6Dps1N%0mt0ppm-f3^T!Z4geHU_DDX0B;tk^llpyS2D}*J( z0!JjkXb~?!qUHyAUr8x_zmF6eOPfi+JpjdVTi84P+ZM zOqMg#q(;MNumLCZabDS_W3KVIOhN!)0(NZBVnP9U;ncPcbVJ9lh`k3>5-A5G{XU?RVo+Sh=w7YQ% zromx`J1T_7gbeT@+}Sy|Xs~cL2^*2E>Hzb`XjA#f#w~X2JbZeBbuStgcU<& zxBN$537Q?>_gf@H2NOMiMPu2M3f95rb=eR7qYU=>cN5QFj;#-T5q2$ace;m&w(OW# zU~pbUwwq9Im4ZSzVMEYRaY$B-8*NugLY-4;MI{CMk4{V;f12U_s5aF8q>CUk9ur9R zv{R>Et6P9Sm9)^&8v;w3N>F40N)+*Tw8afEhuX@C_f4pe$xTdpt%P#X}wM46tL_Z(|Q?gocl zni?du)<*)JFg7oSH*EENT`$rcY$R$L3l^XU8<31Vw^3ry39Z4Swjq71~{gUx+joVQ1g`CSWtd_*r)atlx!_dXq73z_R9hD;*#``&*gS5hYpt4=sW z^0%Ded&LO7CVOd{?+%6PXf2NbB2HBC5DB1jIo?CGlUA1TPPW>{zUV7sz=d+Ty3w68 zR3bPTw{zmiQ&MQQ%|IN9sKpwOyoh8&ddG(@eR?jh@O+ftH}wMMcD)A++f|m>F$tVG zo17bDbEMmVvySji;)eJ~Hw}0})uPKLCJ>*il-sAnstAbrA`MDV=er`p@D9?!XLkD* zAT_b$Sn-Amv}WS7$exJpMI^2GP&MUj`~}FN?NVfZr+SWAJxt_G^s`e2na&{2Hf{!0 zDXVmWS9BSXbhqK9J+9=erR18kC*}PF{D@F_Ob({=FqD-i=W^28gT|I$+yfEmJ-1TP zATzDWR!IPh%2v?fxVJ3#>9c?(TzGNVf7LF?v}BhdXw?wcVXc z#Rnd^qrs$Nk*Mk^80ff_g3HbR1A+|vG3Siv#kW?Pd;Eog8|E^*L0OI~z}%*w{=Q8l z4qcBVTLDz+#f^qYyUp134rij~n5OW^f%&qVG;L%3XM`8j$bgAxf zXXGF-{%y%qW=VJ(u8K3FB^H{0ps}`U`~M5M4o2}&m)D2jN{iHpQvx44uE}YY@H+!Egp3+dbyu+`#bXIhACfbx#8N^Y z$bYvpwNz$qwirirG3b$-pIT6`ZiiI?Z5`9EdFCPAI4Ng1yQ~hZ8A*tx(MgNne@^Do zVU;d%a*zM!c3Zv~YDeh-ty2t(Fn1Gz(kwL}@E%qzuU2V!f@4-BwlUoiwow6VAh>7r z_TsGzpE-LGr-d@ll&3C-EWN(wvVKuz#^wwMurW)c_;;u^FN49x`jMm=Cb~B4dahh#2Lk}d{Cgiwyoxa0kE8dHy zHTbUnC$eQ!1Jit-r~E!utQ2}UOYYp_0#`HXHzU#HPSNyB0yYL!}Z(-T!hO zy0z-?sJSAHEN~iRz0ku?;h?Cfu7d>d0lTer;FIXQ+%QFITz)YG_6HRKpugNHQse-% zV?EqVFZVFa=nP7`8P{%m`62fXDC?;9gkalKDA-RTWO>CM1h8;%Faib^mqFcp+YHI zTssVf9E!=dzdNUhk+Q)-)Ssq*Ed>>iU_!_-%D(%{*#?xHc8C7)!@IfjxwZOIj+t$4 zva*R3VnGvWgMA}DRu6^tfI_OxtmO;=ZhOc+faY0X(BE+GUD(<6fdDkx>yL|ibhYZ6 z`Abt+m1t1gKGkqg9fH;s(J0KfL>}{D`*l_~zK6^gr%s@)O}O2yyTxET^lE%VK;DRq zS30fXzjiK4=cJVqVs^1lOdgj^zOj6nO40|4YX~ObB|Rhsyh^iD63T6S_jhm2G)afO z&;d2aod2c9x01~mktBF``Rh-J ze+j9yZ}9`65`@be8~57w87wRL{uFMuyJ`d7pkEDeMnp)?ejf-y;|kVG{G|)r?r3?l zXy`AUrpE;VW2G|y3PJWpu#s(^tBuIK?qWvF%`0kn6b2t-t_hG5z(ee^f)CBPolRYS zo+EAPipTGU5&+(3*2elG-Nq%+jO=zvFlK>V_v`^cgmd^;DAEpIdyXc7Yee%BGBr0h zogSi4v!Z1BZ#i%vz0<`QzWXK4fQwbV3)AnyY`p#@c1^q8X)ea8 z)~C^!q%r~^eLc&n4|%!eB_m9J=l@%mqk^$&a79ap08rLN z#OYY7mhmJPBg%F4(ef25y><4O7%w?!txZ_J)x_lxN}tiPW!~s|^h^NuZ$Q|^{W@=9 z#V2&mJ=!opUV6x%$vjDM+?;l(t?yiurok=qLKxhYN`!mYD0t&V0kz40uqFFERn%rV z;UpXPPoel{sY2d1Vs=R{GjQ&UtpC$Ysa{f|C^W-?-% zM#|K?d+=+NQeiIy@jR!3kd5>1%z(}zSl*1?W0NRQkeK1IZQHhO-LY-kwr$(CjXSn& z+g5Hi6>V)*^6C7BKHc@aZ!_yy!$V*4z|-StWeL-GL;{cY$|!SQ(F6z?fX8A2Z*sQ) zbnP|Wen}zlhaqk4FYXlG)`rqfQIU`Xe`jgVM1s0eleR=HkxvxG;+w}JsJ8^5JIPN} z{G%=)abucwRmk;+dvSI+A+Nw<3T4E0V;+Y6Oen53$DxWs^k=a2i{wnJ4`c)iIN~r% zgKl0B`KGsm(lJP=Hr@9*9d~?Yk(v$!YL(>TKK1YiY0GrLA1Vk@mZ>@*k~|~Z`*lQ= zT1bN_l0dSDu>jhQS0)ss#iJsH7Ow6ypVT-`ED8KdQ<1Z?dP})Cy8d82A@H!MNrDDM zlZwnt4j*DDLfinbF-)xHVAZPrZg=cJRdFh~jX!`G^Wpf%VrzZfF2}7`8h16elUQgh zc415Sh<2`{moorYICgftJD}KV?9Ku;+x?i9l0N58Ty3WQ^1nDQe!^Pe$brS@Zth}t z$Kf5#ZK;hy@rYrz8U_z*wB=!b8<9G3`4<_4RZEJanNP5tnpP7t5bKxRuANn%CGzznV1(7keq-(#$6*_>js4pFt+12R#Oc4SetB&WVMOY%yi`e zDu|3W=e0_7)e8a37qQAOq{<}URVqx90$u5u=-qX+$=ke_zN_EyC3|a6JUT8MI5Ysr znS2fisI%HYR+MgtN5vFOkWH(7mhVlol)CO;osWpmk22?b5{MVTqg=^D)oh;b%Mv1ZtFv ztacBR5<~4VJNB5`zkr0JBWqtE>cMkkaO@%GSNigAE^b~0q;;0LSqD#XwYUP)cC6kQ z7MaZQFFes~5^CU;J=6svKfbzHEv;R)pCm49-8W3k^hj4^khnyr-O7EKBsK-JiDcTk zf?STb$D2GDuRVL43*YeuFb<+Pj*n50Fcd)M+0vAJ@!`%9b*$!R7P4E>aP-(3>0w%E zgKzPYeUntn7Ly{g9%0dx+IEAC+K`L2(w)Yz*ojGnDs5PiR6G_`mDKn?P=HHbl=Z*m z@N|{_m6C+Wq5DOz=A)S)NTh1dU*|n6dR?Af2l#u!{V1*m%5M%x9@e8$V1jVoQro)1 zg28jKnx4Nuvw818aQ5_ZV8A?@?cz8Wu&{pNm4{t{&JlGRqV~Y54DyMhaWxUYxnOBJ znYg0A{uoSoz63VH=m|{I&RQKNJ zB7{Z@m}@~K4j;?uQX6r7rrgdCQ~HnVx0&RitPcOOo`24yg(5785I5!NP%Q}s$cIz{ z{7db6K%B8h37~9x>7pf}-^qdA^vT-M-a&)1PrQJ!y?~(^ zFHNH=1kso76}#Dpmd8gObq-sO&ejoU+@@ks=|w!TT}fNMclBU z1L@PFG?i^AQ(W4nE!0uP-qW-B1bKRt?>n)=w3ARzpU3(Hwrv_2Ziy9+HZo^VFDOK? zScBR@(PfYj2bZXLy+@j)>XALe%@ifBqb>qtAo63Ej!u2vTZJF`E4xoxYnj#w1%E2j z7QfZT0*^T@Oat%K%4m=cPOZ3J7C~E}g)jKIcXA+wxhvUYwa|L_EYBb;RlO7&-e;Kd z-jtwfn@)@o@G~kt@zH}J#w;cW=_I09Gj8}ImRY}79+8wS-{jQvI^i>e2Be0B#S9N; z!!%V{=TMBXGv*D`h*QG3gDO;cS9nPi29E7lXeW|!tBM^FBC_>2RxY(SPrMWDOxDH` z7%87A8mt7IRYbT*s5~5f|6$}hCShJMbc$=U3JI1+N~8^j@k5-x|LbIx-3q$*NYoL# zq@{(Bun}Fk7lzYOhJ#I|qMz~K*4_uF|FT0T;r(z=O!f8!2$zR)tv8}>pEBmZ;T-R4y&R~G?*`;pMN?Oar0-J$Z6HGqs2ssvZJYGr@!}y&xU+?@F zXSbnx4x@+yp@50;dDs{7NkO~T$w6e=mR4CSMZm9?h0f@X#zN>;)y#LQnfsfAz^l>4 ze$^8AF;vXX?I~()=ulUz#WM3zTMvIfX_Co(p_#^*^{{46N4^qzh z-+TEzJC)PnQnlmRYhA6=^Kzl{{S-#_KfJ$}|Ly%nP);z^GyjM8S0MSHyg$o--XH1z z_Wm;ellSL_kZ<`vy}y?K()&{?HvYf7zav%>svKl|YMZ{osI|5_gB{O0IZRvN(A$N(YrIWav^O%cQbBQtA%=h5=b&5ZOv z^^yZ(LRv2H1sNU~vn<^|_guJDgBevljOk(%t;8B^Qrf5a`7fG_<>KHRjYkN7`{ zj0~;!4IlG7ll-3E%*11^t!sFHW)pv#?{7iRSxr!iSwkv**#UlKqc*VCF}2n*0h6lV*Q~SI36u=l`y}?*{%ZPWpZO!6!yUbVVROZ8YW}y49O# zN}Gq&na^93ay(x(_d-205#LQHzs8^?4(E3??Chhy^f0;hV$##=uf}8ipq22?Y7c9C{a3!u8nz-0 z@8e)3z~>M{47h4!Up9A?O>Ha2 z%NwVs#_fT#D}lPktl+yxXNx+$)Xsi?h{sGf0IA`OOW}<0M*GB~I`pud@XyhPTt_!TQZXtdT{_mZqJPxh;V_i=w|A|?c22zNuC&BA z%#{0?3Q;Nd7!kY9x-w!=cplRZk7fzgwPnk!8&G#`ncP1bGO$*>U#so+dOwPBR6;ls zSU0SBdD{(@m|3EqB2f`XT?8eD-eMd&fRN?!d2A?yd`eQXex73#E6%P;EHO>;RIuzLhh} z4&c=X;8V4%`ybNAna!KXlg~A6 z3)~Z3Rp^#9ujhger5p4ebCQ(!`13}-SPL1sl|bsp$)sh)|?^1UvHs*Px zNe2OyzBMNj%l07(u8ST#s?^Psz9m)`=1rz(^V#Lq*X|X315Ys7e2OFiRI+0wtw9Ce zOmNGfqp&o39{1bguv2SMJUx+s!+lPT= zbD3-8L0g6S<_wj@@6fJM9#&>P97(E)UNH6EEy@5Jo%8Qgrqrwn{kXb<{Okjh+1;Y& z5Ga?7ul4Yp75TNZ90-^BAjdQV&)wS<4O3M0a>w#sYP8U}6So2Ob0{9a`&AQ6@>1BJ z5X5?0k`gO4G)NnZryih#6W^P2ym<53WxmBlCcN+B+@Rb)6OpgIW1U7$0n${lps3E2TT1^t!T*U3e+HHDW@_s2Z@QuvCjp=1vtEa zSo{d>C4gH?e7K`>a*{k_od~_8$qN7FaK#OEuou;@+dQP&a6pdqc^>w%hNoY-L2U(R z!{fblry809&ls@28)< za|yXaLCcO5HoQG0lajDZx3;Vxm5P>;Lz^*lq!Yht-d@>><7vklE+d-w5NI+0>kMJa z+`nL!Pp|yRK=2V&h41;3MfeCB4%u90gHbRZS(0nf;dBT;WC9SjZ390;){4D<&`y0% zEJ{}e+7}KP#gf`JG2k3zZPnAgRaInB_wY`E2c?q|>qZO?5~qwPlL0&1j0!YZ%+6T1 z-o;gQP@D`(Zm`RKA~Zw1<{TujONd>4U2}!btCx@W8lQ{$#DZIx?;8=Mbv4|9KSa&_ zMj6|`r_1N`c!KiK$$ni&wEm|v`Jrex<2$_qYWar7D&wsaVOF`KmOzz}JJHg=dh)x$ zsRoF_D1diOkB}hHFCRS;05oLIW$l6JiK_AA1}`fNI|v1K#Nyfs&IQV>+gM8$N5Dr8 zhvC>8)|y|8*zq=8G>QwpDfe;DVnF4!L;M&^mgb?#%J{K?--&D}jY92>>Ln7aX5lM8rTyCmk6t9|685##KD~WQz=+b-Lm?n_QJcckMk&PdCZefsGHIn`QA+Kvm#Oiq z%!1H+{G^d8rgKm`P&Og7zc1p@hj@!QCj4uRC*Ojd-`{90TVFjBo0afa@D=x5JD2#m z;b_E}?9jd11b9@l8H9uPb`nqFu`6d6D|rJQr=51L;=(c+azdyAi|c?4SppeS-*bRc zm8wKqJ^Ap}QK+{c9E)6NmfJNJ_cE>caRqSQz<`Hj$B?39OzF7VQupZNN}{0sjw3i} zrd7S!^6g&hsdUSsU~jLcuKbnj{6%0aI-qXE!xc}d0;34ukotrh%}{=^Y+8pfzq|owgM^0`U3}H3AT(* zdwyU0P*Y|!vgL&? zfTzy3ZG$}m$=gs@7@eBWaH>m|1ceG|mC-8sU^Gz68ODaYRuP;C&&=h;`M^-IvyK#- ze&Z_>*2C|(aS|!xZ>_oo+tkfG%(#j^eW+n-09ex~e1b#L3RU{0YI<`QSD2Ox#xYKX zpZ@D1$`7p%- zX|W=a+se;=KGf^bK>MT7D9_=CZgM?Cfi;p+gj9Oe@HgR zpHf?RK8vklEI=y+Hz-+>Pj3Pz_UY3*@)nyggo-43*@?$%#jTrQq4B1oVS=QlII?6p zbB;XEO7`ZSOpiOrEdsD3PZ#iS+U6&JoO#V4P@=>A@gar6+cBX=>8{;f8i*nfe188a z;`5gL;7GyvrfX1F?s4rqPpK>;h16+4O8!~0s)pWdR$pdH^bV^I1f7Z0&qL?j4U`6J+oEVOZ)LfF7VNdJ+7IVrnd~_YZyE%>P5BGvZh@YHIP#}+vpMP}hM|-`+ z5P*DLo|qq((44_nXMj?pE){y5-c^lYSDUuahD8!x!WKd{fJ$aJ4`3nuN@&5E*FgBw z&4KcQjZTiV{i<~S{B-Jh$X7-h*)W|;qc2o1VW=**4xM*aXw)(HJziR}~I zR%M(Q06FkK-~foTXIl2ACaXvYKb)zJ6F*2>drTu?kooGW7G2iWE1f_K*Px!`tpE0g zLI66lCm|ccro#u45NO($xIM)(%(hK}Dg6q@f7uuqQd+$z2qvC7F^bO6gi7w3iAB_s zR3q(BF&gy0g1|7R*vN?YEl%~HGydrz`7EV_)m@uENrzi{=30tRx@bIQqH#)wb{3mR z3V{mw*9KE+t3|q<5?ytyamHW5i3&mZ!8kkzcNahlz7hVK8z4}UX$85(lDYF zS{+#;1$|xJsK&i*^o}7tF3Iav2_u?q9RFTbLJ}f6>^GZL3P~cU;A!4T#V)qyh$T=Dd}Cju(eB~-C;4^H6yu9G?O9@L`edm zln@%pi&8$;HASmWoBrJz4o^wU@*0iFW&5{#^~K@ym0xDhOuM%hYc9udq@y)TqB)zj zRkBIHcQE5Fz{dsE(3YCWH;`n&X9Zv8Nd?``+;ce?a+cxxh>&k5AEZM;3N62m2@@Cf zMBWwT!0lMGPk0agY4`a~BG3hpev!gWmuK%9hx3u*G3JGD zXNa38jClO1@2Xp`FK1}i*Aqu9X(XJa_mSTMtxLCRS;V3~Qz@O%GDyAl2o<>O zocq7shF)YzA}qlQlRviZBP!#CQH13#1XL zif^T>vw0EgE-)r>Gle24(nEMT_uke56tzU~s~&&)uzr0)bs5qqBJ1KfYo;b#Pop9* zM@kee$YzE*VsPI}uHseW*55e^0%yrSeVMb|+4)~Z+lsv#t9t@FbzKyUQ)PJ?!k8k0 ztXUtpYMY}&LWN6_4Wd?o>gCVK?@U+A2d&wxz#u5HGO0XbR8TLxN1tQk~IzS5L0IKO~U%XOlc^~J`Q^{hVA&=0eW`r ztPWQfaRt+{(nvi7reN&I!agD(BbLpxZ~YCSom7qqj>#0$Bm0oC#aAQcxN@DIw>ee# zw@JSo{@SQ;GLZqo$_;g*q?6PyEN+`3cnc8XQ!1x*W*a$497_Z;{L!@|^41eqh07lt zLM|p8e$&p}M>DGkgLyj6_7l_zeP*B(NteKBJjX2bw*5DsySc}s;}le>{X+33P}IU$ zfevMzA=g5GdH=qvD-bA{6Sf{zsaF)GXn`bSJU(0D-wM=quPMU^1^(gZNF%TtY}8*a zl!Y~qCzn*%K1O=KR$OaN9I|!a!}o>Dv4XUMid&J}fEbB-qGQKLw^+Oef>0Xcjinl< zZ^5}w94c_(B77XQSb4U9_Ksa~vLApDAm|AK7EJ&3+Mf1Ia^L%3lWbU z$&`pv4XZn5ac;SY2ovZ_4@go!^&}%`-H_`vZFh;LWFmS0igQK^T|yzshOs=S6NZ;& zNL2v00UzD`ES9@o9TiCpaUDE0FI_(3Y>nz5q=osxiw`e@U+bV!LL>rRJ%4jg==It) zL4ay8hX@+yJSK+LdYE%QzM+80!^EYG1uo*w^;S>U+dJXz(N@Xe$rmO>NB-n?MWZb! zt7r^(#cr=!DwV2M?zUD~RdzwLeVcWTHE?j%v+Q+(7m$*&-k2jpuW90XbR=GGb(JNS z02l@7rrhRK0!0P!yG)=%NYhVrk1fx~rFlS?TvyeV46m2ZFPw}Sw$bHy^)#CIdY9I^ zK(<2T|DAA>N+EGAM~`?b)x76KmOpfHPGb@|mdSm><d?;e`P zp{eJE9jBwj(-4NnfL^TyiGinAv4^2=vy)!1di^0$_FSpgB`_PdSJKAsgqYzg^uzN= z{aBeAasiAev1fZuYp9mEx_(hSyL*abx-1s@*nDR1*g{@qJZfoiCSt<_f+5fSJQk@n zTa3<6ic#I<6F! z;w%r~?D-@AG;F`cU%hCnK~43Jx4Ip!gH_ zIin_CCnLRR1Ek=B9+&q|R0t6U78hecJGCBlWY;Q?P?pPD29v$i2;!5pAe0Qv(r}!x zF88Po!q?y0u)bZ5-RUgxbDmH4?#SpC4Xn||!=jv!H+_?^m)oSp;2jt3Br+z&k&{*< z^!#FQh79gHDKcY7Q%aShxk~EP~e?6HP>XyPFChv)*^xmgW==iG6f=I}K9;{1OG0q;~u8R%JKfgt)?F=lZ$$5+Q zoAAw>@Z+uxMlfBgG=cPTechLpTx&YA7&q2DTqk})a|s;mDyCMwiUkul$E$umD>F5Z z-@19ms}RcHqAyN0Zq8A8<^#u#;dI+{A*@u*5seK**&X#f>jPkuMvhSlYw=Y_dDlrH zFrbfx4IyPOT$Je2-Hh4ftGeDqqbg*QW!GhvvdmUkeB%`OoXx16;$)ul8;?AAxxbB6 ztG~?HnW42>ry;~%C?Sl=!?0ZT4ZqG-cB{~3fN2=Bo+Unev#&h@V>zK zo#1;I+Smw1(Y6YA31Ft{7n{%9GG zU{=lGb-jAatxm6Wmg(<1u(*6`ANXA|QB4{LtQs|>LGaEaQ(FaEtN+;_8b*cfWd8FQ z9YEHrbwACcRz7g6c+yjsK4r9>OuqixBB*pNqNR{E#9o6<$5+Ow-GPF4;K5B$HWeb= zapTrxAn=1oaJj;{>$OHeyWj}HCVc2lZa?tVAk{{#XpmUlh#1mgxNKr)UHH*)W_ zwk$Ckd-r?|c7o?oFALhJ8qIv?<|V?3ya*wm%PGBKGyd#F_rf{J7^lFxqzPk2H=o|- zr5q|$bt(XbmgL`9`kBS>WL3rJS)mk@|xasndEZeqo|&w zdFBgH{dvIwl!ZGEu+3L|5sZBEJVLN91KQB@AjS`Yio%t-GVw8;)+2b0?~Ho`*ZcAP zoGuVHuhw2Qu(bfM#HriRaI99eqJaGvO(VZKg>|R2iCWvkQZ~k*W2wG+8|u%Vr$S&Y zaO0?ijZ`%)JLRxl;*T`C+8m>zrnySDBF-@A!}3@5Gtf2P*}(Bk3lb~cJ>D5ZBJ=nt zDjn10_;G}V(07kMlC*}|OWrqtT3p{4(uimQH&OxVIsTSS!lZ)5Se(hSh-oChhzk~_ zHGFLrwRbDckQxA7zdi&4LrM4AEtW$!b&f=q#!vRAWkaBO%v)n7~xx z-4r1jroPGr&T)uDe1`BNb~_5&ti@w&dfz(#u}{qa*16JD^;}>-{+x3bkj03%En;X_ zD2IQxBotRBd_($tiIr@rrEA$iAb*0I2AVo{o7uXEh>Po~qE08U2+0KKY&xyjC6#;G za_|lME<-DfU&hl&FL%i_l808BbyQV>OiY7n%A=D}k~-0G(P~Pox*@ z$#*8naKh^3B3na>^GFLfg$p2g*84!KX#?nyb#0_;ChxJ--`ZZ_xQZ4b z(O0lWrQx61nMEz+WB`J4i#cppyjDqO)xnrD3xsdX8kt`}6&lvTq`qI!=WJoG(s~ zvnZB>G`qpLhqG(+Z`qe|kuY42*w1Z<3P@Qo_VnyAfYC!~++mNXiYiLu>zfcD{8Sy! zZc7Vz?sitAT+7OsgG2@Tjssl}8N!v%AQLyL>CK-bDr%=q{GS^wB zvR43lgjfOe;WDW+*g4VhX}4nfF*$5wg$_wJ%kGB-41GFm2Ft_Yum2MAz zWes`qz|p|XxG37gUJeG_b6BswaKFEhOujew(C0P76e>r>Z?S@6SZW7Dkgjh_Qtx{plcuOxonvd6zA^vcQ8 zJfZIiJ?UY+$Z2-p#w7dloOKdPE1>dXE5pRs8S=U(ZLSj4=$pT_p+Q)|# zIU4|SIbizfpOrmUL|klMrbkr+GD%9WIW(FTmhizMtQb0dz%MEWzB0f`cpI`8`BJ_aCoaeW8gu~%`WM6`WpAM{FMla+p?!b2>do6KR zGNMn}dd{@$YLCI<$A4T(Yt6pGm6&29@%m&{X63^Ox$lhRtwMu<6FQNR2WlcTxgG<`{E%Ah%%*is zAJu;7vHs7nemasQF@z~%L{TN}I2f2%tkrbAfzbso^Vyh?1XEnn8v293%K4NDbR*4v z)6nkZq2}$VmY}0&?*zuq7w)7a`m*nP`m~B%*?Z*)Vz7a$~*GM{yf(*YR>5H z?FE9K{W(SUu(umb5AY32i*pZFojcB_3cKSoiunuuO@b!kEdlP9vN^ZCT8>Y|Ve?uX z{$p}+appQD1MyaJuzYtB+p$*KQ_gU%dZfa}G^b$OZcXilR;~3%Pps1;b(RoisvO(7 zfd-|#`B+GE$qra%Fu0?Uw)o&z;j?!46~8vQ*vtUd2a;0W1vM;Xf@K$CN8wGvBPxN7 zB>D)nr&xq(hkJ!XMQ~)r#vuzh;*k;W5lFLBG#d{tPq+3^vxs-8vc~chq;r=o_CQU? z*(lc(Yp~PVWj~MLj&_GGd4XLN^L;bQY-(k==o;}5l3YM_evBP6Z~De#gdu(^-4-*1 z{!Bc1Scdnv{W4O)ORw^w|1cIv zZxAx*KsHnBme}v#>1NkuI~(%Ui!38nlk=H<3POQ>re0IQKTrM6ok?$|LGaEF?F%zy z{hwI2?|-AGW^ZMjrzP-F2?p@TPTw&Q#a;;)i%{{knV`8+d^IivCUoN}*Xt~^oN%Y@ z48q%EAG_glPIgZc&->f!hn-y~-9#l37apUi=|6$CNl`N!#ikf&oLzFANV%o` zfov3EryQ86ueL~A!iLqI6fnOB0$lgLC#a?wh&9GKn@B&KpApkY3L7cB{;Jyv@h{11 zy{z~&UsPV72KEu{H5C_W2|OZhAoUTkP7YvUuQ*#Y+Ox5Zrs62*|Lwchqb z0?y`gD-56Oir-b%keSZULN?(O-d}qB`OWWAB2qPrvr4rduXljwx5or=Ygkql9XQX$ zMj*tBfPqm*wv2AeZ(BF!{tBR|w``T%cV(X>xw&SV+ql|a`Q&QRDPGFk;-KZqarer7 zFo@Gb8en=g&G_A8nNkqK(01>tPXzW@ZW?Wk3(QWU6;ELx`#Qc!shb#Da69E9 zzs=;dsq(65$jNC=q~pbjxcp%M)uuM5cO_?pN4z=m)x}JBs{TZWor1PPm;I=tbTM3y zn@Bt4&1%@rM-9H%Ljqv(o<*4cF%Q6@k>U|O(r)Ct@`k;Yw(4r(^XSf1cRHObNE0=^ zvE_mbY!N?A)X2;$@C`IQYc-=r=Mi+~tVlk0J{B-fw#$>4DJr11w+y zKwZI?!s#w;wvB3V@ILNXDuwT>=7EgKf2YPUO@Zvs4})>2(>wILg4MQKUTCM7gM5_F zSuA!uXFcS`)#w~u7edbHyj!jsKhL@Xr}PRS^3tdJZykwROto<=t;@!>M!u%G*;?gAGDj<8%bv~ATfE!@Oyj8$6c65xQw7RE zi9XC)sGuPy9tA)j%V?!Fa(miAr8_$;u-^{HrAg=Rsw>Dnaf|@N3k*}KM6=tbMz)~; zLa!^V-0L>;0z^^Uz>kzGk_Dzo_wOy!bxh}(6g)c%FdJ;V!Z#_>fdN&FJ_lQJ``Fz+ zw4zf<(EDi_Fo;J}W%puCFaU1tE7!?gB|r_lm8+;WNZtgAWX;#WU^6D9P|Ghl*BF%y zJuKLbO*2XM)gahZ*EAEoCoQ>m>{R84=1)&^H7dXMFLKw0PDT1CPtlM206p-FriPM{ zp?z>Zb#>6Y(1d?=bO+@Z=$R7rlJBL!5iSl}VA-Ov8$YZzg#~4B0;s(D;E3y+fS;KityEh>{8Jvk!}Z(_9jFqDagOE~hG7%4X}ApEzUmT4CsmVdl~c)N zNe`iemV7~0(!#%(E`T)97P!n&8Na-mkhz)%D!xb)5tD5)3_`^;g^RHn{MQh-+%z5# z;Cx}oVM?B63rrfO*(6+wAY4QeT#w}s8VS2%@NqP+CVlOq_7;J})}}`BA@Pp|BFf#b zzkRwbroT0OSl74NkYi%lxgtT$4XDDkA%nM=%MTW!WS#q@fOR@u= zygUE+In<|Rx{~*Y<5b}OEa@Vc61a4JoiQ`5w-e^j|zs#Q?Ef%=Pp`imt;rSI3xBhe?iNmCua5S^(`bN;7B z1>ZN&b3G!=ptF1O8^F*c|EeEZ>GKd`)mYAFlWJH>4J5{Z6*G99xOe(YJva{=6kj*` z%^G8)nX3belOZ96V^{xc)oiSxPCck?P%8A^+2i~PlChAGJ>u<@$B-vmtS2aO=`mSebW?8cD8S)>5RWA#Lr#O~h`t|FysejUu~M(XQjJE< zC|wTP5PZwNOsK$wJT@cVW~;4V$_*>bn_g*Al=)y?w}zTBara$+)%AU!z!0JiY=*S> z(pu0?d6c~)wqDj}HG4x6h?;&Y^^1%812bz6;Pd6g=iq#M=4A=U>Q{1pbnUf=^B64V zaZ~)Y!J9`B9nuZzvFu}pMF$a!33%8ghDiw2)F-S)MuCu7eH^4D=IjO;tK5pke_rx7 zc26Xiz+C%^YZGrzc)7D*k;Bdw%dgm~tGT_`FDcZg1lKvbW*ylf$f52Y&v}==0zsqu zXtcAuI{Y@@h4CQ&)+B4wbG9r#>;ZMyjvqYOz>i~#w}E&>@of`J6^-6G#FUYz9Z6cB zjAGORgIMc_NdeNYo52yqeSE9tM%VE+7ECV!MRD}IJL8!_%9kD*U&S>@{_uM z4-Ks#s9Oxa=#{1u89&vZoPui>MV>Z>5^3&>G`ZG3{&PWe@;W6{M2WeTl{J}~;La?}MQ{w`JIF7l-P zBUyFQ$Y2kG+do(D!-kuag-%9Rk--&EXI*2A}Uoh2+ zgr?N#Awes?5-gyLWWKSbsy!6S)kWPY1uuXx3+^YiRrYec&{i&Sh;vO;bNiVO&<6J7Ros4}J^5*(Zf$6v&X!95S*>IT3S~F)f9;?;$L~`GR{)avLE> zFmnYmR!3YNruR89>-+Brjv}ah%t1h$47)2VsM&c!))y8THntaoU8hZ54qB_klPI54 zZuh*tZR9GOw9l1bjT2VGX)02ETie4legBkvs(NokFp~hyl|+Nz#0hKTsORSHtG}>(pRo7re>{H*#cf+tJ?Hoi1 z1N&b|H4An21(Cg0ud=oex`)@;S(>c=T`^ZCQKFrc2m&f9{I%63!Bn+`kSKMDOQ#+j#wbV06D#Fv zk(f>lx|i76q^oD_-n{apFw}dA3GN32Ub!E(Pgp1_TrUASAK0h(tyJ-0!Pf2<4huCA zILrm4YR-y|nU0lq$|1YC%&BLyvZZ^=w=R+984hmozu~c#a=kP39UvSnwBc4}+~%P0>(O34_c13!zS_BOxR&tN%OT&y=5lL=x- z-N=!<^_uq8>9%EAOFePWoOki5}RW&@d9g5x2CJ4-JLu1?(&E?v1M)T}Hn3$%O? z@Y>S%+Lap%i3zx%7=XFS5$sugr@bI88^Jqlz zd=|lHI-R6nx?5kxXTdr_W9Lj!vk0bo&O5We3w53r>}ben(_>SftTlg1`uCy9y4uJd7~gH7dp@R3aZ7o*Qb z`e{-$j|iVsMu+X~f+;H`%+gu1@YV<;IulS)bHJVA2`-#O3@<>;iC`ty=ku`jsJ z;m&kg;tvJB=$7cN7r?dNbQ21qlI{32@ZLWFXJp>sKJ(enSs*#U z7U+X9XI+AJzOcW(K?g$ee7Wy$reEju-d~MU6=(bAeP9>wa(S+1XX0Z_69*?;bU>I| zyP~PEHh-59R%A%pu+SDL)xp&WZA=lKDEgU>BS_kIRwP*_Sc-(0($#v!V}hB6!Klin zEj3Svx=j*NxMiZvJ@SwXST0aLt!j#HgA?Z@a!3Mes5n1z=Cb$ffEU^mB(v+B?21c~ z_(xDOVBm3+-cK`Pd#yvOmz`ed4AKk^(G|Q84!i2tmVMM|rB0^nDwkxig&G^G3Z)AeS+nxnSm{9k)WJ9>0>`oXXZYqNjtSReCMA()*(#6Acc(!_T27fH`S}9u(SBiHJH*M2$bAFm< z`#crOSkAro>sDl(<5&9Fgbe`WK^aCsOP?Mz zCGx7j?OY=_T4oqD4&rTqKarqMS#E~b&^rwV4F$gNhM$JfrqH?4)mC-{nOy-&U(mIJ zDF|itL_;=^-8VO^{dKcBPALBSpc2khliyu4$RR_cd|}?jot=Cn0 z^w9J^L}tw=VzXY35D>SmmAuofFJeivW%#v2+$hp^F|j;hC^)=tDIPcVMdx0Onx&4-tKVV9ynH8m&V`8%Rsq za)RQMMI^s{78Z-ZH}#f&2cLqs#7%Npnls1bTv?AV48eZ7h!|oi5Rx}cjB^$D2Xg~D z=M1x&S1>|dcZS5{a*8I*XS4UdYmujA=2sx6tj^$hz80vc58hnFZgVZyqTgRZO zBRr((w0cu<4a?u^%2B4P&rS!?H!!erk%CQT{UZ_hxzsP17qOTbs#T% zGbrmt6*Q{yE(-oXo_ZV?xGO`}9s>_adAMrk{Gx>*s$@lw1?v!pzUAG??()9swF4SD z<7YtLAll5L=@YXb_sg!EE<=V|!blyW20Wp4ZXL`lJ*!SRmt!nCdb^E=`w`EXAlC>F z=%M9}bF#NgdvtdcPH)CdagrVY6W0V9B>*GzQ<89`nza)tInv{X(}K2wN+ML~7eod8 z)()F96v=%LZT55$vP+W8p^tx>2?=)zISx)er;sd6uAAp|%C;;WzPIo|Fdm?kuWE^V z7&FeHrZs;D0qgKZqS!x!s6nS2LammG%zgtR(x-?uqbk)qe(7SN?H{RYopuH+1S|~d z7X}E$W2&F|@_DF**qVO4YWEF!EwF5Lk7>Ut60X0ppC7Fl;!1-I%7@U{k?W!YO!M z&?>pq)<=%T8F0n>9G)HeegxXMU#@;)0SB78W~H5y)=o}lmbnPfpBG}=;om5~?%3HB zXA&la1u}{&-dL7U6)rFvmzxR1QZWMBM@cRwY^1R<=gfq3tp;t8#VPn55xt24-tO(m zd73_~7H}}PxH`j##$}*0C^RKPR3jH}9g}-vx zV4rGMklTbA?KkR&++@f)ODi){ArZ0Pgx_RLvhyNJA9?EXowZ_jN z1)~e&s-_a(?j3^F{X4RKTrgzh9q+)|b#&D(qG6vBZ#|}COesLbQc?8tgC^zQw2-%wC4DbX8%=ZBgpANx=<=f z44}5bhBQgUQ@$k}xR8d%b6dFkYNUr(RVxoq>7@n7yfDMI7h z28jJbs*G*9*9-_v=t~BzUyjs-iBehYH(PoY%b=WaT2cM=)vR})UzzC{M3fgc|At^mp_<=(7B2rL0g z!8j=IRZyJ#YGfdX%gD;F(oqgtoG+d3GLAVbnp_(Cp4MM^^;{=^Nq+Eiz^|=@;HF>WkZ-Nmc zfU_6!R{nWYI_`Ch4=@`5NwI+ShpxL`CkJ-lG)SnK)ZZ{-`;aUe?9R0s zKYf@RxcfVXi29TGIpGEZW|%ff&KUvq^*s^d%btht-q?M_zS%yQn|yInS`d z=jeZ@@9wAgl862HzylbpNL%7)!bDyLH1r>vWq1~ThQlaMZ*!mmjFE0WOoJb0yr}u> z#E~Q8jkD66#@TIaM$vLVn6h=hf7WL*%?!9aXBV4#(=hKEZV>c5_mM^W6TZ*npi5wnc6JXp2PqB;PW zJ+ky+R|?uJtA0f@kg}5>A-0+GDFcPl zNSilhGEFeIE!(OKyX`VU6F@5Auog8}9yL5C{)HKIG!hE{ToSmhq+yv+X9zF5fyG`? z)BiU3z2UZ&&ZmeC%8Xm?=NVj&SMLBQWqFbwnN-}Y7j3j{=c$j{9-&L;Dc7Y^sSl$u z0YE$5&iJ1FCS<+7Eork4&`mB$SHJ0O-a-(G5Z8Mw_0)r`z3sITEGrkkEQ{}Ro@2!n z1d3>thkwVpT&Fk)@X&`f8i@J~y2Xom@`}g+!?AvhbvK@*wtSo%YpHKspnQ2|gNF^; z!^zUw&-;bj_%Z3Ey+Cy+G`4O2XWkmg1%ig(OVl;iR*E?8(s($AN zrewV|p_Tl+FrvXqTs0@V+W4ta-g+VXH5A16Hnxxq(Yodt@JH@TSyTZkD}j<)8^H0L zb+1wt>Ejzz8@{0O&p~)!#E~f9FD$+`pkt*`5OsnFiz%E}0F^Xfag7f_mFj8ZS}Eu8y^ z$kkqlo2BuZp;q)Cn98(h=uR^>2RW2O{`*qCK8i+N^D9)b!;6 z0YA4_G+j1rfAYYkIsgQKDu&#r3#}l=@Nof0rF5WF?5!+kbP( zGSfyKZL|8wWJ42W^wNep!F?%(T!|t&!?U-NjTr83OhgcC6UzR2Jh#t0G2)R|aqm6L ze8>f{k1zv|Qx+@12bf7knf7DVm?9s3A6ss~Lrb zbdiUMRTv-lvN1AZ0Vo-R>>@dbw8Ov1OSApY?QhpIn7L|)AGZViwwZCOYS~Qnl4oA_JTsHrG1gOv?_gYv>{b|B zZ+A=SF`q)27vtzgi$0Z}66>AZkWG7oZXEV)g)BZfZ&Zlu$z~h3X6E;A?d z;S>_>;8rZvj5T9*DXYn^^=QaN7`LDah77}R|n#U zCiJ7Kz&wn2xp3^Bbn$|#RYl!fYcZfA0eYZXR@RWcaj-I=I6=X)cRES+f2Tiq6{RC$ z041eiT4c6#E;Ppl=#UaVDjN0F<|N@>93b?%VWAKbU}bTDPEO{b*d?X)MIcPifJw** zM0@(8&5#I8>9;D4i$*ub<)pYG97qx{f3OSy?&$ z6RM4gje&#pe?YaFZm-Z{Z@S*BnS$eNwm6OcKdmi0CNYLn_1}_gxAB$tljk|-!}1%W z$|Kq;rrC5EGcizjYiCAec@c1Ipl@Jo3RoUNEyd`>)D(=Ksi9v%{x3UVv^MqiwQ#hy zKQ{nO9tA(>8Gyb4XdNR{W1j+m@D&bsHuW@(Hoy`o^DisVl1-E}jSWpUz{Lyei){)i zDgZAV8=C-|nVKlsYHrjoaXCN$02&m2-_!!wA9w}%rG$i30C}mgNKW0GD?{2g$#W$J~(FXy4#E=phh!H%c-06yqRZe_eGVLToYNHW8)Zj1L zT1X$@SL0i0)2%86BAODf);-Kz7GjRS$`Gj;LQ%56S4wZT=D z#i6O=P&YuQVeuKI{PorpvHODa!FjABE9j!H3T22;0V1_Y6xu^K^#o1-PUkJ;jZ-$8 zUg7O)`c-u#u0(KjOhX0?w2(%wQp4xpdX_pu!)=V;<{qXv=_oa|wN)YMrT5vQ^P)fm zQV;~|R(LL7ZIPk8;Mev{4Pg*%3M}L;apBkD4zV!|rLlqA-8QWhBp@KV#dmc9js|^D zdU(E+ZW^KtR2H$2dysRr_GRS12BD8%))UJ_G1CFTs|SbRh2M+)v{PGbqXDO!@!NN8 z^>!ExX2tBxVd0bvlr#6EB==di%O+44_O(I1o8rPiKoF-J+E{sYrYES9B}U00$_D^> zj|r@w{1WZkjcQ)gg?O(tBzrX_xDXiEZaeU^wt~VlRp`x9-*JM@_XZ=XntGsa^KT-3 z7kUxc$bd0xohNj1-|-)kE1_hmeU^yjAzRdo=i?GT)6e(&e)@UTe`?Jg>(W5H2R4`h zAq1E@sp&kt{OAf2yb?V#FNZ~lwwYq8(=`OnZ(%||^kn2z%aXjay^ArLb9K6UgHtup zNT~{Y1?MX@jiaZ;S;FtUJG8{4wnv^GH>Dq)5fuvtT-@K2yB%s__lhrq3FJ0V?`>yD z+R7GSNIt2RTDl^aRmbBcBH$6*us)k4sxdK}$W)%gyMY$*kl~vpnDB%<2<3FyT9Kzw zu??{v>OmWPl73V|Asw{3<-?x0x78zLMU*Ng2A^(U$hnDA5!McYTLD^`X{>0=;5~NM zKmySaeQOEybR@K`G)<-{=8VaJgbcgWtl+a9Ap99gk6h*!SpJ(BE(OAOPaMiIr#W{Z z4ZrIH8;9oIMSy#QG%Kf4-JYOqs;QLo0z@QAU4Krg6vbNMn8VeYDKsNRHuEc-g=XDF zZIwJI``tgGex*?w_ca*xN5dhZ=Biw3tC;G_8>#rfPvuk+@kLE%sE`9#YW7c;r+@(j zel>B27adW!z_jT$?rf}C!@PA_B$+dK>#fVu+UESVEbj-?)l%J$AzkcAvErGKgo~Bl zJ#sZdQ3kq^5w?#)z=WXON$``q&aOHXx8VKo^qlrMJtkqnZ*hA+YMwlgn19q(Lvb+i zU~3ux8_$$L8#R~u9ZGn{i>4)`9-kSN00b+)O1phen_R86yjf0R-F@n$Qw(ICul^Iu z5?$Y5*Z~(65w%(~kz(HTyei}Fn*@0G2U-J} zS(sh~-VYuk;ShX8c4SHSaz$Xs15=2PF2qu+PsNES5(NvWmYUNX=i~z0bC+uDK z&iwM7y74XPI^$|Tbr6hvz<)g|OmbRH4et7Jb4 zh=H}#_Gx?m^2n3g%3cT;S-5SXdE(N|s1OFdK)iCY3>}JN z)xulmsM^&gPJMsn@u=o16hi|DRuMr&DuVg>-|h?kGt=rW@RwZJmZIyubYhRV=#c3% zE9IFFSH1_rw7uIq8L3$1XUWtT0K6c869*X-dFJ-AreV?mIcz~DXmm7b)F@Z(<6>bY z`Ey-nEkXz<6EcwC$z2sMvrlv;AZOM zAxY-BR8FEykEtvuzPobr2-wuyFeMGBQWB_Lm8adH&5TbyAIS?*DZmf|Ja-lCuW#?M zm!+6A5;els6ZPsMx9Dmc7)IOP#gauie(7(AC`u5Oy2lz-mQtoKd&}7r;=!gp^>MuT zwI$rC49n%)ppY8fhf9S{L5k3A|B&1OHxK8F8J|8)RB)Q=Fs+Dt358MWPwg6b_}-Fs zQm18oq^ggdP8#L|v?)gyt3CeNTU2zbHTPTvKFpQ={YhqAEKzzC%3sQ@GDXjsVPeKn z<&KE@VGW8(SpcyD;~wTrRRJx+e!+&2jdNOZI3Un#PlbA#hJUPhOAv+}OL~ncwS^a? zqJ9hdm9S-}UspX6a5RBooQD|7|ANUxCef?WAlYtZYrTZ86FeL9NNJ|TlnlIkInNOY zC~KeMWslfVCNr$4HU_ zGiiS*UU4i;oBB9RWI1u|TrnJ;^7ypvS*NsF>ecB6kcn)7cb2%{ux0c?lO<=SfchqN z%G1@-vfgW<*~V5&CpcVE-c|5%b{+;f870S@m)mH6VhU97k72KosrFQ&lzp4R;RLV&~J|@eg>+ol_}LYk8Ay z0`6_PjEAS%sh@TBhdZ&crr)2Qt?AOv40Yv&w*)0mG#G4fMpaX|HFbBKA!DQ*F45rAzX(WYx;5*l zXg_rt_J(K4eKyRF_nKqlAjbPwt|j2IMjH}uS`;2T1)Un|G`KQ#Hop)>MDI2C1LXvG zP^{wkxV0~69z7%SPm6b7(om$V2C%Eba7^xI^Viok!CHX-Rei`G`;j@NdGUVOvo9nL z!<#@(rGxsia{}d2o*DC1vyQS5EhFiosJtFCk3o2M@fhRmSe&aIy7%n#rOiqIbg|=0 zbce?h_hdtI?{C14BSsyc*y?ghMqdY?&`5m?K`y{yUN8wkIU&I{NJFZakcdfD<>Tm- zPCb$_^c@j8FfPL+hO!kKh{Ncr;=?@R@>;b=K9S15w^AFS73&gzwwuWTV3bS4*mg!^>{;rEh+ z-Gglw@Rq~H{v}asRBBuxv@EtE{)(7#_W{xSF23G*ujfU=iie^=_FBx$0!+lbtzbBJ z;-RDRDdeo%4lNHeyFXb(o8D6U+PB_Z_6{-Ys2otN1R&N(1_r(8h|)w^PTx$xtN z=H;iNKH?XsLsd=;W|nL-QG!+dI;5&`xN>W1wJZd$96dj9b-JJdqNjO`>Gis*G{(H( zRP|hyHjfC2SsZ-Z@rb;W|4XKs?C39Ri~hr+`0Lp`l~H)AmvAaVRH?wus)Xc~GLf~% z#1LO#tHnM-Lk@KmuJR|Hv#QAcIrb`B!A8B}T#4PQV}Nx`q8tWNnMO!FvAuS~7dcmg zhuvND$N^>}k|7o2(@h>Ij6da-=qi_+8`|WlZEE=x50rCJFSG%EQw~QWIG|`lhY{3S zmhJ&WbWwZ&?ZI9i+C0jH_+}9Ip~69;nFLNaZ}fv}CA1cCC(qK>5Gy$kI(Z-hZ=Vv${Nj4Mvt zJ^XHcFkv>9rl!pclQABDs;HKdXznic1r3xxsXWVmHxQh?D7GY&M-DZoF#R%yP4O7P z>E4i#KnqGu-+sWuaOkBl)Nk2}-8u^OF(ll9*f5J0DH3!&)Ogj*OiWX#@yXAKI6FFY zl&0^uOfq4bUtf_SyvWUt9hG<-*2h70ezqbFsZQ|dvJGK_EDL+0dY?ZM(C-Zt!PBz> zVHk1fakIblhh05t#Z4Cj_Z-od*DNTk^GwC)>EHi<;LX zGP5Wm5K!5|BXX;-EPRjtUo4vuVNsG${7*UR&Qxppty}7Jo$HamV+s(-4rI5t#L{rQ zh9dIB4%C-AdjDVXm96jHGweYjhLa#c7B-pZ)&1i0(I9JS*J_ob^etZ1%%jP=B{HCk z6xsV5rvmOjGc3jnQ`74gNaLU=kUsZvWtEG{ZTWp8Kxeu`nqhj@_hIDfB*C+j4G_KJ z95z$oL;mD4Dhps2Ju^59SJ@yfJ=LZNIRK2abR>eF&`*Dik*Ihvm?}1+qLqIx0VNyB z>0;{sA-7&fmJ1pI{^l*1GlskBChD4DM=r;C*oSpRhx_M{u}Y;p*hDe|`~!F^t`kJA z73pbg=cQ;fR2O+cHh&#dLf`Kq+8d4oWzlQ);Mn2(c7Ta8#>8ii>3?~;8HwhX*G~TgSRGa1d`^JsgC{j+zB`W5Gi;&=9*MoEJR;#mqd#Vc;btwURMf_EKK8tiP0fnzQr30H3o8GG*a!eBI{~qQbo?6%_t~)>us8k_D1j9(XQ@ zRL{#sS;0*1U_~aCyXTd;_qed8A3EyhVrXi_3|r^;u)$GhMofNP+AmSn3I9TJd(x%V zf?h$fMQA!bm&6;t(wsoE;xX8{AOjBmYl`Ai8!j>A7FaRuwdKIQZA-A#aJuQ!c$3}Q zNlLPsc6m5)tL^?~D9OAjw4Wy;1&DPP=jh~Eg$3RTO!n5e# z3-^)Gc9GTNama`aS;%{6RoJhLty2&{D1wq!^qO|zg2SEfwy$fEtMKTex<9 zRt`4K1qLBUaw&L+;yC4{3_B-Sn!J+HA^7IdH*t*iUb!p+BC9|7<~hl_yM!8NxB@T+ z?TxhcXLpjg7Yg!Sw^(6qeT3a}4Wo2{1+e>8B4+H!J7a<-G@I+Wep{p_9e7|UiTu77 zO1l?d<$>NkGSwwg7^<1}0jO^-(nmPH1-dZfT7;jfymMCovy&+F)zgIq$cA+o_xt^= zP-$sg7ZCabjbl9}&p#wD4Pq?|hHl?2)3RVn7u=OQtVUMNl-qmn>{Ml?cFdX7J>i|S zpluet?r9^M;|`eA%BQHTXsUpAVUFO-aj%miy`emzL4!~o*w&%%1RJ2DCvLAY!dP^R zEhnnyEXEt)iHTOPK){v10OaS$C6*c)8xyGLq~GQJ!twIbARf$+Hd1KE z+(lt)MnGRJ2Ucs#knfxL8{0V0FC7Ad9qXXVfQ9u*u=h*neY*#j1s*$c#eU<2;#Bz; z_@YvWD0&PqUcpb|Z*NEn;07$~sbx3ICcySX#=Zz5G5 z_Z5Wh|BYm7fo>%t!D6YyeLg_U!NpF}6%~_h0Dl)WyLgn3I2^NT#md zeMCOoB&eP{_ybvXoyZpv+kjem;V=+m@YF=H*_z?>NH<1_F`avMFRF0h_(D=83aK>@ zC_0V#RVBl0)NoY)9J|8iNAyhP7+yNMW;>8!kkNoOX{eU<`|hG|&EJws*&+bU30WHU z^NQ1$HZ!u(3|QRQNqKA)bh&jIh85c4=`N{QB2{3hb#6Ny3_h2NN-x*(V$lf@Sz>L8 zZXIAkQ2!BGbmP$_#=}Gw<(VA1*J~2lXOfN`#xs|=^geB+E(Z60=J~=gh0VcW(VTaGhv>6dV4!`$h;rG%2#ps`LH#e%Q*+!F7IeX0ffw%0}iDVJc9Z6NbGws=^pExf1El~Nx zTMr#}%*AKIB}(_=yFUp*nlan$PQNOgGiyv=I|o{ZW{K*olR}E!?($IDVJ6c}*Wa3s$-4;9cK;~K5mc?F_6n>*VJ_I@!>R-9X$UTJbBKXNK zMO;De5tVL6qC1NwwpUS&7M}Pu)PX9B58bzkqc-tD5C-#{;@@zL z=x{Ll*k*zl{4qM3yw8Jj&tS};zU#ce^ZuiV=;z<0=PjBVG-{Wt#)ium4rUGV@le0y z^{@D`q;NjCQ}xL)p1IMZETztQk%OrP>bsl1Zj?;4?TJgk>SwBxKyt?Ml8CcOIj`DM zfV_N|<~wfDVWfxAQR=lk83H5OyHjJDWcBP4p6;+@XT@cU&$4;J541t5gP{N5`_$(S zN@=Z)>!=gc)F%^9>`~W05}R;T{zkN^Id`Qh*1jr)?k=kkeDSjqhFpeNFR4PRPAo%-W)}=;7}ri*b98c z8Q~i!&iN#Tvc8?zYMwnpOeIHKt_~d*ENdUqlT;9ia|YRqB4Sn^Zbws81L3HboCcFa z>qg(xEr5qVrI-UV&Z}F1j4O7ZhDhi4?X&!I0?YLWJ!QSD^xtszA!HxgeG9`&0JB`O zJCdmjGJV;ZXh-Lm>@; zN{qfDI1aJFXQSjDC=J+OkYmHo`6p9d1pV=@FI%AUYR4;SQTwc);H#GJM|<s3e~N}?!L1DJ~yv<_aY568McaF;0x{@#iEr@djz{0oK!Bs(H)6l ztpqPc@b*DfeNs7}_we(`BU6FLOxEg*b5$6~^7|i;&(7F2(r+Pl_yM$eXaaO&NxSnK zhV!`H|82}aX&mXt;U`D)A?cs}^NE8=(J=0Py(s8OQ4(v%L9e@VTlCPUKa>~`izynA zk$+T;_V)-JBEO)Q!aoYi20#s7QvvetB~EUG58?1U!;$Zy*{i4wV3Qw=r|$ExBxxqW zd30C6#?~dh6X;#ryG>jZX-9UXD0UPO9Q=vbO?Zl_5|0NqpZY^PeBtNX zO^V%{Il2rQ*P5z67%eSXH8B@(+mbBDo2%M@Gl;Et7lpRAp~V+JN`{d9hwWbv@_g-6 zFPlKL=ot#7ojgv`$hkt*Fw#QHtD;%a^fFjlTQDL)$J14br4`v5nHOdflOb>P=m5OW z$;cz3wfZS&@7I7^r+c$-WQ9!_n(fcN-CGS$dgL4lOJHRyhpMRr=gA0p*9{JQj0~J6!4y*c_1bKq+~AI# zVL**|u&|ldbsDLzZCN~KG&1?#DeflgFr3LMtc>TrOrwTSX~}5heu6iyKAXORd7K_V zVofZT9L)HsN*DP)TaO30w8fHTpyHe}*>0rqj-H;DBY`|EW|XE&;nT@JUqd2Z z9&4WyEP^6BKpe5&HT*E#CwY&bC$=l%IX(0Y@4_As*tUJKx?Vdv<1k65m8&|zPR!7v zHeekih2)eMF&cv5QRttp1HL))n|xs05*$H^c-x#&TlvSnAEX?Lwgrb#EZov9oj$1)h?$2{`I6K@YIBgNN(hTr!Vai zWC=jmbop^*3=Y&q=IvF8Wp=8MoF}s}LsJJUP6%rS4&5awZ{Eo1%nHvRx$yVf@2-fk$DC9 zIF`DSkI_$dKt-X|?5vbPn&eHOc+u6&N;*ZJShBJtLlq2`r$Ig;c)xvF4%KG0)vIDr zX5mHffK9%gb5?PVr;5q!6SuZIj;Yt#T_BU)5%{6`|6q!Pea}MUSND!+%YB0ugXTMM zPKysy8Jik8O*5@{-=wI!`w%lQm|Gq-5TlS1p@`mKd&zi-J9BD4Ns%|hj4wc8GPw+_ z2x=fw!GyiuNV4Yq0fmTwVX$P|ym&J^S36#l7`q}7<6!t>skkOK?TvuMEGGbKYqp@U zSu-bSZD#KhNP_|U=`ueMdBKE~)T2k9$T zJ=IL>dE1aJB)OhSz8odw$AVmP>S_UUK7q2Yzrw|2qsiq`?4$g_p5*m)iF5%Pa0dx0 zSyQ4Z><1@iV4;!8^l}9?5=J%)0%Ct`%YY;)=1Lq`L`@1aEN$#7$AeStkU#q{7qaa; zTw?O1en`lIevjP-#Kgh$!(d5WabZ2FF3daA!V+fvXoq&m{n1Vd0(Nmd>5!Mm6x$Y~ z$w)~Gj+4A`;XCqO#cbw4Zr8C!-wyo)V$F#TcMifQJq4A|caY=}^7i8r*A#BeUb&ie zubVt zY0@`8q~HJ^Yp&Hi)=rlo$h`-7-{KNbvKp&M$G};O0Or+fawX_pAuEjw;RIY=PCvMT zP!*zsUIGLuf}{4>9CcPRuOa~b z$JKxeIV?iYDE%xVsKZ1?atkgYtXbBA_qfc37Lwlc>0cW1_ciQY9f8Ix-hK7OCUZ~e zM^i1%;&G1wG6kVX#L!2`)nifhkXZEcFL;vYTK|#{o;GDIh2Xo*b9W0GiFCXpGND0M zk<~fo>_JOVh#Nlh`1M;d8s%BXiQinfOj|)t9?3Oz20rIed9R~y-XZqoA2{H=WlKs- zKv2gP7z7a@-W-n(a4yIdeft##zxj8jF#N4%@-jm5#ZsbJ=ZLfy1BycQ&0?);iiJ{(!>$&hRVtFuvx==Ce=BPe}j%zHDBM{uYX9DdSY9&?f9QvRd zr$wqdp@}tYy8}-Hi-)SPw;jh1>tlj=Ju>-*aptH)bjg5D+snron}~|rx%fK`L)o;- zB~LIR#SE}3Zl%P;xyUQis?I+f;ioPHt{1@2jXA?~#H#Fmn?MPmw6mUdGPHfWuVY4* zh7I7m@7MT^a9(p^%ln{JOe=>R2O>c?xY4A%EW>S*)@6DM1RY9pVYPPtQmhpsvw+E5$ih zbk?QN6vmiODQX>}lhUCHK6T5eRF94|zufssteO*!*6MGT55U5HLwrmC)B|Dr3$qeJ zTthw`2L~x{#2TouxS~=lobMx19KAL0I7FEe(^|IYZ-+|IOXUdeT97~0WQBeUOJeDp zZv*|^#H&x$EYLI#(cewJuC|m2Z44@7R+x~v0ycAx(@_hJIe<^aeVn9n3bMoIc?hX> z-aj(^j9->?^(`^X3$9%EkdTT_hlA@z=Hu%jL^dD87uhZp-4HAQBO=guEqYzc>%0HL zeq+~J(TZs=H@9KfD9dgEndPd>Alpj|AilHPT9vl zRHMOfsELwT_IGpU9ej_RH85Rxr#@6tzTWqkIjOGR(mJN(Mp{9#@M!mn(B|Cmlb1k5 z!%6UF-BtQE_B$Sk?kcVgQ4rip%6vwP5~7X&S&Ij&MgE~=TjSY&d;_t=SZv34VbSWV z$eWw{DyfidA7gVxclp|wNxW>|$Nmv>D2$`7pkCm8(~gNylVr}La(`@87_48rBo1|98fCJaBt zHVc6$6W-E092hsflQ+_ac3FuVW@KmltQfJ?<8mDqBgTjK;oKE5P4?3uzfx@LUK)b< zG`#QD&P}Z_PmQJNj&U@@5*sZ=j|$nt$@bDN4rOBlKcU}me$Hh#gn;#PK`qb|f)D7- zYcQ+aym_YxU2=!NGlWEobze{bRK*ut-*n6qH*E+A|9Fw~pNj*}nX9lZxzo~6Yh?-T znXcKyN3T(xJH*u}f;+oc0?Krc|VSxLYX0GeGLKTq7U$sBx{L_tZQAo(#!Pquz##z@*+r{y!6fihG zg|)_r@AiQvzy6Yz2Bn(jbeTKDJjU^L+sbTUR!is?(LA1DA#t?;2c0cqNOlz>U_CX)E&&CFusw%+&_DZoh&Z|4cWbUh5n@lxrW({EQ z;C+()y9Ep#9jvIz#)$Nj79eo)?%XGXR_+N_Qii(~g*2*3#~e+cRqat;2#ziKy9JHo zMyr$siqSGtEgJ42({Gc|i1I>K;+LGaW(UV;WmIC8_2UtiJZ^k=ZaO!s{&>nevLENux2_WEj{yj)G#Qdrd!U$>cKS0JenBEuXR76)w>f!M^&7j zqcTMjn)2=)XVy8Ip0kZ6`(yVchx18Yn-Y8rOd_SaUvDTp?aFVbF=7S9I0CYQEq>MA zSF-{-arBDQxhf^Q!92^|lLGo{v~vZuPs?B)jxB$uHb}-Jl4?`<6rxAhekfcVQ_fj* z4&r@rWj}|^htKP57i}_a z$@}FvJ+L=fU&@ywA%-RMW}mqATJd`aqfx3{?=+m($t(BQjdB6#Z61 zBjpk%8#!h6B8KTvVj|pM+1KIjPB0}aN$A|dll@{&d+xOmALg(f zRd{H9`UqSQ&>kKxC68N!gcycaA%k1JXr~ahFO3N~T#mWJx$U#TUp;HbGo1$(np0OvPF}O@L(T{U;kT%wofIvPQ)RI#+2$mUt;`HW#_uBk3S}>c+n~*e zc5BKszNNMbXf6s-104cz8ngd>01NwcNPeYDv4i<^IMNX7bo&YESE^}xyc%^*%FU&0 zQSi?_CT{a1IVNspOI1t$;pQy(M{@^>zMqX|pMut(%Zd_)LMy+sNx~z)Fv9^J-veGg zcHRaR``78YtwIK`KWreq&5wX%@_ztFK)An~1Wo&erL@p89NeOq?u#0_`MP@AIv{-* z3mp8jeUxBM5@#?SugiA}ERJW3n>toenM#flp)NE_QfT^QF5RrJ3C{pSz4+SYRNH5Y z?QnB^P%+@IF7+RQPzl5LsUbJcskLJu5Cs$&wR6#~GG;AFgCQ@IDq6iQaQCu|j!%3? z4wAd{MR|I7IQ5qajlLNalQt%i%!p&^^pgM#Vb|6bXL+E@DBlt8bY}sl*M^l4q-nS` zL?A1sGLq;=>2q#@7u*YPdSheI`tIco;0gjbFg6Xy;o~cA@ThHi#ENaY!#|!5yH>rU znWTdi<5DTH)wf3nNX#&1`ud{gKz-raqE9d^>R?dsONMsIlck)FK$es2HvBNy{^KPn zq%dZh_I|m~t7SvgmsCB1!3dcqA!&#zqp0AK92-vmHogy&eX5~f*F0W*e) zdMa#1RIVpKOUl_KnQzn&T-?UZJo9Y0N~3~^l1vip%R5PM0{w?F>0$-V#4{G;-jmoy zG`TMpMfjS2hSi^mCtri*!q0>cX@DQ$pfSvspE??M=6_PWTzjFuS7p8>@FihC{*G;JQ$c6j#UPR`8Rq;Z+*Pb#TDi===Z5YzO1= zsHKbLd=7Eh^Maz7YZT!j`++JtmbIIp8str-*NEuLb?n zeX6V>A-S}nSPYE{jGn4_Nw;72lQOP9fia2#2sa9=4*A|={`J#|1)|Le173q!PWSO= zQ5cA-y}J^^tQ15<)I?ut(#_BkeWbV3E^Xx*Y(v#C$`<%_yF2R61zgy<1Bj4s;BdaL zSoWT^PZ2U9kv68d&c2gsXEr`X(i?ED=bk@4Ik)O z_3{kyMZZSDa3qG=Rka3%?7;8)Efw0AjtPF~pHG$z$u5Z)_ zJ$SiF5FWE;ymArmKA4M$Rsvnft8Bxer7iBNTV7QQE*WiiWXQs&bo-~qUwP7k#qE%o z%rF+gzpDWR_x8DhM)NJd8eTC~1YNG|MyXT|8TC0lKkQX(P$mV0w_+9}`bwMbt;R-~ zhK1kib37=O;3M%pH!3U`as_T^L1*$WLDsvr_w`f5czq3w`_`lPg^-~UVl6GG9VmQS zd`;NHhLw;x8b8CTzN2AVxj#>K2ug)S=0WP5UWK8;U>t{wMHP6?Fu$f?+1s${BT5`e zf3p&W6b>azy8DN680Q;&-We!oU3yVBSZNrrT;#?$5~<*-xg?v?THrw@9&N%v!tX5S zhbsceh>dBzliGI!LP@o49kK1e^lG>vuIadg#%q_u6g(GhMCeq-(4#$ry&O}lO}Gb$ zMQAP83AXyc#}?l4{Do1Tdw!7`#UyI3kPR4GB~UCxtFw1!LOMKbP#F#Ja-x!;ip`lw z?_8_EfY=}#RWEc;#62D6A`e{P!wTy!2*Y6RRl!njd=Dv-rtwTzkpitUr@e{G`7)-O zRtfuj+wP1itsWM3XHVsJDb*)}Z59{1+d?2=CX>!UfAW&HdR`}bwae8rn5w>5q(ZQH zmhRPeeyb&Qw6zk6F8-b9% z{9>~AE9bCB0Vwt1OFj;@jqcEqhTUhsGG{&OLoIW?YWb?b?eR0Gk-r246+PQ#R+L76 zgVAL~_5G-&k_mQ52B}(uI2eN#%UnuvQA+L)&Df9>EqNtYU$#dJo_X60i3p^xdi?&z z!NhpDYV-{lX|3Djv|_Y6Ss$JT)4Ays@23(ZKEPFW2aFldPN}Rl=(8w5@?F$(sNkeVUA$)( z#9BE=?2Ait&+^bI8x_;#9&(0EdXq#HB|pAE&r~qq=bhCW$=^x9bNt(4Zu<@y5uW1wEXGigg4htO{ zQHeq&B`ojH?$$Mt^lWW(Rj__>mmbd)@^b|ziGEd?PSsgLI^pIF7{p1AQ}Ik-*hpg9 z4HxEz`c>&TSXW?psk9eg&Y=lwam;yn5?C*ef~CG+vh!RG?A;V>^eDfo$jJ3Fu8^tZNQA+ z`?j*ly03pKw?PkOVt~w7`f69q${l*>9%Sy<=huS}iGLaG$(y@pc86QCoS6Z|>KIo9$=nKOL&Ssa0wf)4?Bwa~_2;NPR+ zQuR&>ZZ)?@I3>>g>5gnL_h?91eamKe`L9#^HzAZ&%ZA%aW z7Xlm(Ls*gld=D}OAF^nFERv9Zn0kPD+&>oy-R5zSlN@L1!q?)Slqc%_4!WQD25cH6 zjK!W`(U_yp$0YBbOrov#=A=6t^p0TapEJzr%l3hd(oL`h+6z+BOs!;$VoPovF|XMg zWA-xUXJ~5O+3Kys_RO%K8{F&mgJJVM&9HXv-x8p}_I`jGb16zdhdr{Kb-1_Ii8{~HxNoL}y24P^D+$s+IQ z@vl_}0lZ}M-?8QWD2J1sB*lbeVsK-5!ryN$>Ivz<@}@^e+W=^N?+Q<7ygzbfNSH~B z`==5*`ZDIorPHZ&8aK#CDh?|nNPCv?a>3eVIJ%hpswBIM$3XTb+{Z;-=Vmjc`i^k0 zx$;X_%gCrbye)t83?IdXj+RK%y@%=<19X}lxNlyj8pCrsZeqy*S_ zYo3fK+RWBtyj)?EqzV&4bZnMv_9^~U(7F`SVqvb8FcGx=coQNG7q4(zuMav$sQU3@ zRJ4$ZR-Q*^u131}1Mi(mCefy=JXQA4zig;F;$0jQ-Z0S! zgDOmVW2o7#n{d&gs1CKYiEk8iEzRv;mU``_zi6@8p+kMp0KM#Ep4D<45<7#JDWBQQ zBP@KVDgyNo*D&Ct1E?$+@L4~XhiDVk@{y{}Knjax*Lq^cS^%VXO6lr>InAtEqCv)~ zuUZzhB38||l^@^YWHj#t{1TQo5*FU?^pybFNV}ogl*=4H55Bqg(ZK~AHblzbJ>)V` z+fs;>`pbaTY*~BP!a9J6)KwB~D)pS6C_!npyr*fhk77`jGg{SE-3eH#v=mC=@aAW3$o(cZ+_K?+ZXE0#RNvGDUAmta zN{{mrSwk279Be2^H*XQ(?-sf`{oO)X^|}oaUBa@vU@De1d!mM zDZGe}wtLi<`~vLLKSzt331xDr;{a*0dka40CA(_Fa=ZKLkXobB4R+ZjE~56;dkpFo z1nBt1@UzSvhy-9_7Qnw(6O&2ij~KWWa2@zKR2n&?@Spbi&lmR%JHJG7hYD}5 zdH&{z%yNoU$}l>-560T^l!gc3BBN_h_U|gJIzhMa7d`N~9MN?Yi*ueDtr}G+E!jPl zQEeax4R^xSIE+q2_^zrF!eF-Bh6osWc~4^=lXe)eGPL$>rRlmaO=V1iM+>;9r=Vr@Fa*_HIddYn}M zH-7x=Gy1vy&49GMYXDl=)FzW4>u&Mo_d$%-;JIHq^1WTRxC4iv;oMhiBgXI*xhYBx zL6SYO&0^Zu1h?ifInq`kR-K3bGb?0V2Q7P z(~(D;0b4co?Ff7y8-nQi+4ewtUd4YBM@hOe9e<%?`H=*UZJ2qmGe&H1g`yM-ie#qB zdm@&3gjap?qdP?{C2Jtn5&mjPixStryC>G(2V?C2?vzT_x?%j(Z0TV1NIgm0jeCI+IkTQ75CcEM*9W0`~Nd2nG&up z@8Q*A95$+FlLhkh|GGmNX+lzo{~}UR&C*#U*v$YU;q<{#eT)7kT=Qg}!5l*{A|edW zAzR1W1KE2d5|gV*iWU5@<*9E!@KZMsdBN(%Vd)PzFf2i$YouNJ$g~7_+w9sa-gTT} z`48!bcwb^Oe!GfhojX~EwGR`sD?}MDnba9%PN>|dyVBC*{c(no{@$1ZhUvkP*U~}L zP44G7;f(%WoU(Q~)n=)ISaX;MA9JVF3GDUQeGAP3>TDg+OP|)tg%x;~S3{!(JY2up zaFMBK>EjHwmxFf@G(qD^O{BN-pQ~n|x#8nZe;O_LwaU<~<|GPgorr7?GW&%Zh0WlX zoqM+^i882dMcS{*^TlUnA)dfr$Oit4=vjUNlRDs|8a2+O2PoTk{;W(ZQHhO z+qP~0W81cE+qUi5bM{tk)!lbUQk_ovlWFG&R@vzrX4ia!V<&X#Ea*L#O56nX38qsM zPIA+}G0c`(-lt}E#IP#3p&DnGdDBOHzY%SZA@I#p0U)z{1Nnt64 zczM26qImC)o1sxpVWUzxjeteBUgzO+TIF$sE#EHEzUzV&)51|7h_ z8CmvG{=Yg^6Z)9kZH0iY@i_=1?lK(werO`!XEyV^)*nPT=^XVLj~?N+eM#n*mdr!sno=+#um}+xbJp{jNiQjPd8i^1aKn$YADYHxquBod51 zYDm9*gviq`>4>-SE^Z`E1ZlQ>y$La2-tCcpI;__eem-i%Qr$M@$@*b{qDOO^eU}th%E3Jv03GJ^jmPm|8-pC8|zkdQjNa0A;@J#DG zmvdihTpOiaw-1E}C{{)NE2=aq`;R9h1-zHE!C9;u85W|y#hc$@#r5tJfME|fTBo2g z&n0)~vDz8b)0w+J;yAnf;JW*GOaEXG!m31g?e{HxY(lP&>9{G=EpwQU#>QOGYCj5U-m zpCA6CDhGMPeqd&~rmiU%k5pfzfRv`0g@acuOT=dB94>(ONNt7+sLIuY-QEIi+Geon z9*F6SQ1QAx*-(>b?S$(RuQEjPMP*tOclttjZBUAN@5l`0^MLohhtZTg%Rj{+b)T>! zt~rPam=n>%pN@u$^+yklpSe|hGY@?tbB$#QM0x(e8FIBnw5L)hz%amh=jo01_Oi@{ z;3&3tn{hEfENmS13*4#B0iwlY$*B|#Q|D-_BdQVLFpF-KEzS^|icj%D_VXGId9bN@ zV`qjHe~qX>H`c^VDjE_Ye*k`9*2<8r_X`GleWue%A=;ubH_$9csF$3=kpxL9(qEV@RSHc2`Tg-ou2m zko%`eoDj|wd>%n?HiK@g4Tw+>sq6Hj@-w~QJoQTKhx?y^t95Tvnz(ok(-8uLndz zJ5sLZg?zjImare$-fzyU`U_d}oIUBql&3*aM0e94zM{#bHN7~C=*_da07z|4V&!yE z&3r-BU3}bXXR$$daw>NQ8ZyVGq7AO+P`UV03CDw`6jQBUIsPq|M06XW8lDz$4(KSlLm|e(nMSQ=Jt0x>QhFW%=`Esx<^zYwBw(_^7D(=4Oo+jAVp*WtAzgeM{8@f z2o>^$0gQ~@+Y{2$5h(3diU?0$mtKw*?g)aN3$mTBsROMDy0&rhoz$S^`0}5vU1_dq zOF2uhd;${Fgn9F6#r5!>cphrd5Ii^8b`n-(%M?E>DgmHE*UkA5X4I(y4Xk;BvI`ib zZ(O^lgil+d1MytU7G3aVEJ zY5l^}EB()}|6Gd-vI;sujyvrCY8wN;vGgMA9eFI;Q)F}0@ikI(1@T6<$-IFqMTkek&Uj+*7u-cm)4qZ zm|ZoVwrUrWVz(@)T9;)0>wYKd;{#wTFgPPhG3ellsML!0j#Nq<+t6{g&;l=XEvHjnw==oK^t&0CMfVEQ0 zpu$qUFzrL)&!ySV+AtEzUm@SE$k4dQ1h8l^D%IC`8j#nlwU9eV%pV)#Uv;AH z=+ujpKNu^U-@Zn#rWF~_bz^(9@3Vd>R#t(2sk|B-Q^vZl1Fr3tg4XRme`)T$HA{sq z7*v581R5Ri2w+&%Q1g7@6xP>G49#1B;# z@$defwcYfY&;WN(4neij9_by*7dFYnMi2e%rgPE0yJqqr(BMsDN|Ozi!&@%CXr2}s z@0evzr-FUmEhEC}qkLEuv*F^N5}}BAM3lh|qZF`~J6<-{=!}hR+=lz_@5zVp=&`Ld z(qnHS1YN-e+wGw(*7B2XqyrK+MZe3{+T`FoXqC|crMQo(lubHK0X#=^ir2DTCVe7M zecM%`rY8}qe zH|pYv;?4lLpaYfVeDRlt0^4~$@adw~SD$2oldPByOuJ5apt~+Lw*+^}uE-QeiJgEV zT)_pzU19_#^KBs>JqM2J^Jlj7oTeOA-D+5Zagt_K&VSdN%&CSx6Ick=XFY+YveLfu_Hyr`EC8yBhCW<*X>71(b%;CmgzbSZ?0SR>6ODrIZf(J7D^EGh7#Fl*|B7D4A3bHTBSr}f0tfTI5O*$4z$^eT}lIhJ;LSjTc|0tAgWrXQ(Ze$)=P2kBc!Q_4!Gh|e z*BsdBzM9S}vT`ox{8YIpKbskcUdIokb;+^O9RT!_s71KeAZ9avpY-`oaQi1I#M^-Y z^`uku9GNfBXNp|N0!bKxok=9rik%18ff1Y@P2aV{MI;TW&WC@Y+ut0*(tbZsLH1gp z6h0L~UON&OMKw5#Sd7H9#fhF7D>%1`V{ulFy8u>W*W(?ZA+x|B0%oz~?638!NaY5g zP3XL4&50KnO0Avo!L+9v!W0BIEU2XRM1z=wXVgG-RXFY18YF~WRFm1NcGZaiNZo<-m|bCPzo|tccB-8%nS)la$mxAKPC2Nr>I@vxe2MJ^DuxB=820bF(?e*vXR@o3=LknMv{BmCg74I(>NH~{DTL2J;xhF`rS863TmD)F&lWLKtnV>6 zYk=4boo9%V%I=9XwE*zz;=VWdEnt>81Aik4iYrXa1OiV+hnxmhC5?o*XVI8f;y5*V z5jzv_m`=)@x^&?IHr=GIv036qh_@&qqBZP-cYPSO9s6d~&bn?RZ}%NI3(8Vs0PJXn zV=>6}OvKN301-7R5g5T;6@u02Dts6S&KCk*MbUyUFU@qPoh7K zCXA9(y-4=;3Y6RCU9M}9XVkIkvWP*;G@g@inugZ7C?I!d$7`6@tH0^xls40R-IZ6z z%VjIJ$-3x1fA%h#hT_3DM3`td%2vcSmnNCzy)iAGZu|nf%WbgIEU1JG2u&G@7l47b z`YS*NKN@0E{tU*DN1k-K=95O85K)1n!BPx!5GD?=V#m_%HrStAnzSxr+-fC0RP$ki zx~6&sf5=sZh65wIOf!2<2W;fg)~wV71#f3BjAJKva=-a~4XUDmQ}f#3qD;UwZdW(g zrC%(~)E%j(cR;*A`1HjGDXaHV2GqxFc~h*QKe@qee!zM$EIH?CA}IM}(p=?d(x}>R zyEp>Es?V&=vt>ZHkf3ALjeHJKbq?l0*|P(yMqDcDwp~%k^H%ogD=zd!sAxxoR7~Xk z0}r85DN;-%@}I$sCUp4cp^F4lqo7K`E^@!y*Z+5+#-$z{|K?>GHA zG6Lk%+%R@p9E-7QcNhR`y|SI#ah$lCsmu9w50AY9!0b+HJj05AMy9U7gs3*BbnC?# z>81Rn3((pyg5Sd|b#*rBQ5HSac}{Zg0>ytbeo{aN2b7F^njKu?6r2I8Ipee|4o3@_ z$MsBWByQE5IQ(({Yt3Kf8{GvAzBv=tY;%j)D<=DnXtn478V> zBhkjasQ2n)eaJ1uw0ieXrxO27KqGdX_|$z}-#%LwUf17k5d0n64b8xmQJ3PG<`jSK zw^owL>nTgYpSzp)M7j!Sx8Y*VA>9NVubpb8F5di8=enQUO9(62ey|Yj?I^~W3$|+s zP44UQvkA2Z-xdH^G_(Cw*upWvU6p=PX`|AS>QnNsc3A0RQ!H8;{Ivq@@uL}1fFQUV zs|cIt6Opba{7yc#abW@t2FHm`mOu0cHRCUyF9tPXxI^Iokui+F%Uczx7uLOP$pB`k zoDgK*AuCBqM@;g|YURk!uj{b)B>OP}(L6O-5a~UQ{}$2j(RPDa`6z08Vzzt?lD1}j zvTuZh$#E;=AUI7dD}nL52ExcG{q_S$@(jHmKvG4n!a?A)rlI)b=_IP-WX#G-Z)`$$ ztOcIp$JOZWGT#UI;yl3n0fKaII6Xn{nqvH+eO8JH>~p8!t9q(8Cb@Fiy^nXDW}bY8 zaZ2pjV8nHCK^!acE{jvV+$*_Z#9wm z9}wzMOKSNf=tqjqRkTDGy24x}Vd10B6m<}IZhLX8Dz;!51Spsn0qw+m3}=2P4eg@b zl>&dfq6L{&hIGVNIe&v}OgK4Io4MY2XC{^IT=)qG`xLXpxs4U;g2xeVnTUZOAGz|4 z2IWB6@*bjDy3jCXVL^uu?)D-9S;xVJ&Sn61@|0lhMJgUrer)5Xf-RbeVs~u5H|9*M zx>x@{-z|Yci%j0skjI)r698F!$WB$HgjMA+f8kdiMi4EWvCwyIGvK!)>H<3xrCwjO za*U`sO7k^tgF{3BJ!gLOTf8js5l+BI1z_XZBdwV0rY`_;N|?+9H(>- zv^KM?N4s>7-fwFBNbdpxx}qHfGktAIu*L{OOd7RX^?ejWiDWd8Mab*WK{+%zd=F?5 zxT!I0Z*Oc6`RfwwLa35@wI&WMI4XLnbKaUQnP%5F21r>vU30+2rD=Y+{{C0+aYI-@ zFuLO8$a@EoMzIYg1qMK8GYaP^jSAUxoaXKfkKlA8R4hq=MhE?{Dv=;wGvXEG!l~s< z$xRE(Z&XSLSs4!%4;dg)`ys#Wf2T8Uv)$i{s}tmHP|89wPKBc8*&6IUDgs1XdSmPb zXF9MHA4SO?(c+GWb5!-5bhr91(oBG@Y6+>3Ix{MY5o?l~0>ui1H`&6uWY5jK=h80F z)cEr~9lzaR|7@7`2abXgKUTbCR)lw9syQS3ra19-QXZ4Qx-t&_#}$;xyFCZw`MqfL z>=Y*imF>yhz>c`TM=zO&D|Kr_7pRn4$Jg_MP7AuG%^cnQ^rPHisY^me)tV4zCUt-1IPx&&E#C|t-s@a zA>pf#;164Pc7I%HqlHbO)od#po+7!}cn}dlA}P$l#Lc`8a;JUA5m@kc3mXI*e>>6z zVHJ>jiiwh&z2~5ekGBT$EE?)c0$xH89gL3RZ~dB1JC22FlRyA5$2+fnna~IIJ{%`5 zM(lOD+H4V#c-EWoeCssHESX0%;Jy@eZamPQkdj&k-~W{idSD>IYNAhel7XDP(w!~I zO+osvjZB_5*3gO#sF$o$tWI%3LX70OI?Tr)j^`uYU*Cn!xb3$)-gv>buRF`zoXcG3 zw`M3pv<9U}LGbiW{UFKMWdjY8V$rmYgXQY>PBWy9X`nkPhMS8jNMljMMmmORem2X8$IVYb^)>>l=cV5; z$&SrP``}scWf?S!GD|iSTfR@!ojTbq$gQG z*+A$~de=NDqaXy!H{JS?d^NwP2BqnuBWkc^Za(h+Sziyu zk)StaiX8QD?~j4z(2Mn40u<>l!mZK3(;ceNNvJ(t0(`>N3-MVlF6_^AYP?^10T~~L zGLzbq_pgzBx3ydNu~>AGUHH75=6IClMtIGKI;LCtu@=NJTHGChAAMC9Q9U`6(+Xf| zVl&-}=4e4Bk5Oh>_;y^F?O9u!UT9j_8KeHS!#80cN>Vr|+8R9Q15+?E<24ji7RlQB zF}>k;@6mcOQ+eHHUZ-Y;0Dj$6yCE_W_3t5E>qzl#d$8HpP@UBUMUH$HlJ&m@NO~V~ zAwuYp)x0;`nYDfRVw!M;;PtC@o&AjNJ!S5en&!8CG3#v1!Fh1H(p zB-zS1p?bfJQ{PcJ!Bz8#K`c z;EraJmJZEIKBRRM7Ng^Wii~mKdTVyXJ{<#w3D?=``nR^)749_9JzQOA%Fbr?CARqx z9;aUu%oEc${K_*nvG4m0uiS9p7gc~c4pc=bV)#9WOEy64#T>Sx0{6DSF)CUEZ0$g+ zcso%V7Q(OK@7;1L>w0cjN#5SOk-80=$|N5&U)fsjEBZ4dXTp@eZ-!`#O1zmG)0O9h z*%fm5==WE-@w|tu!2$=wp#Son!sxzA%;&@Sw9hVMc;F3PoSe+T)q|R7)Uv@mji0Hw zrl=0aN1T`@VC05@n`-F2qL*D+n_`fuMwLWVA8w(Ui0%KAC-5a~H!yzE|4GJ>80sKh zU#c!pbw9)%P&SQ&>hho?&<(`1`_MErRSIAilFCbJ^RM+P5lOwn@Zp9r-hW1#=8NLm zsaZ_lok>`rwXNYkpys67A&2pZ^13P{31MZ(mnJ-YL}vm+4fU?EIfsh&pB`T3XUhMT zN31}euctl!=xaaX?faDq)z)xAv=2h zD`9*59Q~*4f0_n#QJLbSdC7J;3y-{@RjqC!P1UF{?me-uixKbvu-bDDa1?$jxhHBn*nUY@cJd&4_X$9*HL_qsU`#Z5Xrn zwoVJ&*M=<*A94%Lfvwa8CRr!zXEdJ1HHg7$Yvl_idgmzp-sNp%Mwv5FpX zJ_%Sa{ z;Dkx1NMf+tWekIMhUafJJtk8{Oy`0P9Ejq?Xc@)tuiQ<8U>dqYZmVfaVbGrlreUsTh}KmQ)E zx)vaqBLN!)ruf&(=-)x;HFo_6?lqZg4dS_bc_AAxWAFkn)=L4`M6m}nJ9`h?$I z5`-T7?!P=91+=jJ?{JoKfT?MgQb58*#{rmZrblg#PWE;PHL=gh2ChQxdGd+OFu(HP zWbs^8L{25c#7!LE-)*70Iv-u^LVZ0OEJT*rcXu!R%C=o2nXuA{rMoidXIAy4c-ZS@ zL1}ny25C**vni`#8anD~gQn)%T1!p7z9d}p|4jYt3(aOgQ3!YGI$X|b6jd_{#&b4k zSlH()xt5;sU@-BJtw<5rEhtc+2XRiM`Xy_bb=InE_{)t|hP^;cAkX2+*P^_RQyhf0 z(QY1NlR?b5x;B15R?qZSSNR^f511<>4_Y<#d4-xR7%Ko#2{PnG*+RMv)u2huDbaf6 zN}$-=KfRjAHLr&$8xLh<7eE-s%Q= z{8}X4l%xp{$-TFW*Q^P9eA~~>t;E?v#`v6Iz@7IxLrEGqX5aO-c~rQ(XBjN`zGgtT zH+kqHJ^e!(?XM%z_N_ly_`$Y5o2v^~mMHj;%l11+n~Sh?A9TYtLFCcAEkEXQY@w_Kq* zC?2e(5WJjHxrG5K$%+Kfl-SplhWV>D_xL{ELroY1J?d5o>BvK)OheD+uZ(s0sch8N zg`m)7Is|*$)*MmaQ&b_H$oQ_^#ecnd4?%9S6vR=I{*G^&SP&nEg|}4+*&V0hbe4z< zAyXR7Q$%4lUKiXVM8}Fg5?J}fC)ZS4H16V0w;rx0q73-#MOwq(FrXU!v;4NV?Y=AL zfLUjW?vq~)8Cr1$_Rb$qvGBO2coxpoL@D!xeq0>Ue2Rz@>*dg-Sl@2#vpjhqzL>XLfK7&2_lajHh*| z*XP%zImwc%w)GW$ZwG z4a9)==K#F<^o&o_c)@#fbck4#INyW?@vp;~@$n5*xYeNBN4<3h&5Ke`S#F)qxmT=> zBCPa(2~ghgZx*ZCjJ^y*z$ZtwEE6==4r~Z{xXcEoCa`C%eN2%8ZoYa`Wz=0ah`3Er zKu_MSWbXo7VJ}4s$(-GUw z0qchNATv z&l_4+Jz>RWR@jOtx8&6*D03l$5HTS~ZrfxG7egj*E$#{xqMDTCF&U@$^^5bC@~wJ$x#<0T^z_OZRc-xk#N zEWq29`~3@Pt-8mWjk7yX_}&9juEcp<2%yF_0|tM+>*nf)Ymn>b-j6<+k`VIqS3Bkj za|n1jSu`td15m5+BA7F%w^32pkQ(X>yK-vg*^kzZTxe?j29$_z3t=~|G!N7L2RJqJ z_kvq^3AFhIbyZxWm>IXE{Z3@L+ z_)47ut?A}^Xu}HYW5Lk!Es;5=a<6QI;diJ^@QWWw$x`54GUC(F)|WB#Y^~MRmf_^0 zc6KF(xKZ#Ey86-~ZfRRKC3LVC z{8>C--IMY%F!~3F3qFD0njF*Vwm|Vxzn;gMhB~~$j(PNimv^yDBDCJv-3mgI1$gq@ zD+ZWor2HyrRr+};n?dI>hmei-=I)Sb5f|9>_vig8k7^Hc)&%-%~J@BWqx$DgD6eO7RF?SE*MBLre(qJ@DIQr=Y0wfj$FW1?458s zB;OU&z;GtxG~_2_BIFrHL3ZHBK%|6M zK8NAfbjHv~Ki|PPa}V6Dq-Sv&=^#|_FQ|fN(SO_bty;==w~30KI_R2%3~&XMtt>M1 z=OD+|+$bd6*Po#rl-1#^B*6;>Bzmtk(*AnVBO%XB=T6u%q8$tg4;cO8pVn2EVNRf7 zF=?kj_p3WSZp<+!+&|jT>@QVvxL}F$JPw$oI4^8NZ@(3-Q9RTF=}Q!iKA>1bT1vHf zabImc3wJaB-1NfN>w5l@GUoh!Q*WHV)qo9s!%M7uZTh`T7f3V*VF|Rqs5JZ{h00fA z;ar3_GM?2u*)~KiGm_s^ZS?}g`bT1L9nc7(T+14#9hR2htse8dk&cW#z>AZmGp&jB zn#(wad2kV@VRg0}wa?`YZJw2**qBt#{aq+X4eu>zW^!@M(L=bteUCmC;;+&Uf6`7C zPr}T5YGSj`XBneD_;GkP1z>}*lGR4t8ta?c_SHEAcA;0*h>?Gq7Up4Y!f0@If zYLlGSZ3Q3s>fsz`+QA&gpCz6mdsA{WPYgbs45>B=ULu`elhm)`^STfR!9&M$8X$f1 z^#=M~wHvzQp5USE1b~_hvH+HsYiC64NY+>myXQ8Qe7uOE`qD z3YMZ#q%;sgo9JS&*D}9(TG1Hgf|7q^UI0vZE&~&91#=@f)#e2zhrvACA6Lo%n=RgHT+4s z1{tz(RH_*+cVw1|1Yh;|tD2TSDgzobtVW{cW z`~wx0vJ#aAXv7MXF$F3ibDM21iw<2y=Qpr6e0npabsdXL6;ItrGdKTX!DOdV%~gh4 zgJpbAdEf2kfgR#X81G^FZEqe2q0e$G$QQ~YP&WPUh%fsb@^PX7&S?Eqc{1L6wYm?+ zC*%j9U=1X`P#*ug1d}8{+}6mFZclnG!~ln;QCHWBSq1ZzM=t^N$4)!|xeX{h9+3J< ztd+8){vg6qNs4E|%1Qs{A-Gke)J&Lq$+h@$AqV;tExxGVsY1(-?7Oy6z3gXZ#m8M7 z4{T`9r9;Ws)|SXJV#A=p(*Q+)&?n+0K#=YJove0ZY;}uc1sGI(e!7|rR{~?H;8~u|twztjP|tzAkh^?G)T)4elLUXjzX`rbD__HMMy(lwgFq0Z zwVIyA-_h`?*36j&9V%J;MYz9=&dYu6QGW-6Kw)g^)*}B&f281&XGLbIJtjjLCW12! zS^9h%!OInc66B}i*f>BL+D3-<6!R;EJMP3CdYtLuRSz?MihPW=YY`l=2hxvQ2y%&u zAyw3qbX^9VKRc?>Vv?`8NVL}=vt0cE2X_WCE%M~c%}{6~l{fJ1*wB?NBb_2#xNo)) z>l~(WCjxn%AfWA+<7DpD#ghW7L~oVqqSx8Kk+1}WZ-=na_ZN~nU;kVG=*0sqff^XV z!z{1<5bf;g6az$U^pU%RwjrtRu`@HNSRfS{$s2yQgmrbRs61>WTyR;ifaL(a(2RIg zz18&06`Y=zRM~5>6}(^s8@OUljs?X@ns82(+M7u^5;Rab53^=(&_?ifH0bRFWZHts z)7Cu*dbt~U2m%_%?nP$%qh3)MdEW45pf z7Q={qBF>1tXJzSlHtli(&z^=p#K4~x$wa>3KpfIS`sE;9*vKh%gW+v+S4<3wc`c2v zyEVI%k#he-zvnS&FP@YX&nY{Y7Bcm$Oxf%%e5dm@w+6Eq5!EQ@M zPOH$*0oF2AVS*U@axo)lmqmsqP1HoCOs=f_vHGWbQ^32xSX3sxm`-h55A9RW$iE|# z&KiySu#?!Up@(FHJ(wc4x`@qD&$&YGpzm~w8U6*o=pJ%iXq^(^M0+qA(h4{T`P><< zwiUKJD%88hFvUyL1K0>}5!};iXIAtsuo77$gYJ(&A&f=sMjxcBpv=bW@!H;$TwYV@ zJtctHP@I4b=6+Bf4%PAShO4sh`o&?Ap0Q>0h%Q(Tic?L*kIu2{IMMV`fUvd5tv5Kn z^R4UpJ9DwEo=Lq_Q8>T=UbxDRBB=P%h~jQFU1l_JO@-pm9@@%~b6eY(e`7{J2J;HN zc-`WotooxI8SqOjXPs<65BCNf-KF)=t5FV=Jnp0O;1~(qg{xV%4E2u2|uVpln(DdF7@x#I5XI!Zz#Gl(b z!xw0V#-1UX#3JKLnz)62%JguPyu^n~vwOG;*Vx2y)_h~Q8%3>k0zK#FE|0d9$P=HY zk_J4aesy1Gius}?KC=4Ne%wr+py`3)B1<%mV(vQ7>%@=aCpuO6{Yg06g`KuG91nlP z(aB#BfM%3cu;HMW0jlIDb!=42IW2wYkbCMj!$9`%hbR=XCK$SrO!uWp=T}cOLW&Z0 zz-dC(L~YYF#%%w)X{$RD?irK|Y$ul*OjWIs2@0xb1vc8B-bdFLE+EN)iem`JjeC@c z&UE_3lIpJv_abO2Z19zPW>u;xwH>MKCh!H23BZ_s+pRzolRk8in(V#lhw#t7hRb6{QZZ@x zSW2RTJ&xu~N?$F;IfQe8R3R^?V9S|ZuD>`8R7JE^+71j~Gvtl;YEB{X*t_^tH3K*z zJlKt>dy(G7!k7bK-G_=>x|v|e-Q8pWRC2m3f*&JcIN&c}ibAb(`PO+ZI!kvtSRY-J z55BD0=O~na`*%?ndi+Pwuif5idL24+?iggww59a$xc(j`aNGL_LK-QEz_k^YTFY*g zLU#J@UcAA*Hp}NzcG}eMET|w@Ttp)_SKy+36Lkc@kk*I`Xq_?2*DoACJms#tS$xgl zQUy~o@hnz?jI(Lo3~|GQ?S;ajv0>VT#@UR=42_|d=_7mD_}qlk*~9YeSivA;hz(9O z+MQOZu%@}tW3HK2>ztyNnKpGQ879O3sBH%~u-`bP3fRs)tXeA#ofb8aXfJ2^4&2Uh zuVrP=sCVdojq0AK^M$dwVvdF|{+H=9Wj${K#H=||*dusR6_yhhi<32Le1dA;LX$jL zYm799<>U(!-f2^#MD7GafS1qgl|U*6GsTiSQ}BdL+E*`BiZT4z^1&Aivq?dN;DQEM zG?GQ_6pH4(K&(=Lp+#4-SAGpcoB2y1yL%mqXTy&#_ZaUG<&DyJ0Gdz3L&Vfy+K{b( zY^1}|J)aqW7A9?0Q^>q^H61&wsA7UWP8V(B7BKpKTR2bkZcbEa>bupo>Bj?cn=N!d5pJS1!gQn-z9lS8;8(`em$rT& zH{*#!9><}RWJ;u$Y;K&;awqt2rH9dP0qR_zmzyP7UWl@+sq9LaxGWBQ^IVNwOKcmu zX3<15AYMtGXdGAA*TZ<&pekC*h~q?&)wBZ}3sFaITD;W5xdsCU2R+#F-N~` zh|~_AqFZ4jy^Ps_W-JPKEnGc1@4$%iD%V9JkLsj;xB`WG}}KG+ZPlFLXQIV5?GHZ7?@Yg^syW0a9BZl>r!)!uw#ig{@D}t#i*{?Hf|qE}*+2i6U`x)Ki5^n5y}&pPZcLC9 z`PLq!jpeKm^gw1F3OQYlz1&?8-2L|*w3h|k(E(=e=`||3y(S7sm+)Es7q;_QaEJ)2 z5v3S6U*D4O_h?5Dl}dVB;(-L+DU67+LcYB>P@IIs?1U!?$;pOgDqn<)R5nvUdZabH zgMwayr+^%WSc~tBSL)#gJaJ^U)PjYNd`d_3tWKv|;}qycgN*(VjXG7HEXx!v77#7y zByEtPMh;?hoEUj);4858(JslPnM_LRLjzZBp-Uu&ETZ=4nYd*|hd->n${Svlolod8 z!+gk@l*P%`+{%rA~7x^MIDRgH&pE0N_DcSffv2ZtauE86p}l)UJ|PEMIrGaIeq1-Xq3lUW9;M=u-0{a_oMM3TEL$sOC>olN{3Jb=IdIs4&7W7}rv-*lm%fMcdd|nf-%8P7iN_ z9OT?FTN@F-Ez0FE2DBXAQ0Uo7P52Ldo8_M|UFXV=Fn^3>efouq5V#l6cxe-HBPF&U z`kN`gi15G74j2#%D6{uzyHmMLg-`OUr)(=^X6XRL(3whLL9Rn!!Ow5RLOg`f{3pGo zCfbc+(~c@ajwkcKgm>MtLeZ%%8I0&U=wy2!Go*)wI!s3i(3CmQVDV=*V`!!5rI)z= z&F!gXKsss7v?E;#vqoZY-8L1@j~*}(KmkN~kJ*YH!NUt1iJlm*B_qgPoQ8QZ_fy|m z4ktFJ$5>8KbU6gyRCthO(sdmbXj}(U&v81ERzoYjlSw~;_h#Q`Yz$m z#4WA8U)FMH$mz;3)k`!qFeYwNZ)yH(U6SO?Nd=apa)ve~dJvP#E(Dm3S6(ZLYRPuy>|lm<=CMG(+|GnnaLqhGc$8`<0hi z(}F2`88w8&I;O7*$p>B2*nUem6jP}HIQ{^?;x?`j9X6XO~-|ldC?#X{~=G)?m{X^lbD2bg{bSGC=AI^^G!i%b*!n9kQQ&f)7J5b z*m*kLYlU8Qyw7-c0|&UC!I&*SOh={z<4PR&4f@c01GSMapUM}-oD648dgmCBuhQEE z7MrDCEx;x1y38oawXFIhBDQhVc5;Hp?;eo@O3Dm##*qQ`q*XC(-C6z-xu%h0;dsk@ z`9M*mf&$f7Qn7}-Q0e$Xu-1iH($q&F0H%4s@v_*%C)?v5MKiz0w)WbM)w~~tH*44B z{1qK*Qr!9L`(9Hr!58v<2T)}gyrNxvbCH=cxL$lqiSaUA9`|zQ8B?*ymOY-V=CEz= zsZ?_XkMSGVjHk6!f5VOQf>!_Kn=$fvTRzY(&D?bQgLUkAo`dSJi;2;4Bvgg=9mCQI z@+^lmK+gWa0Ojdh#EJZgQlw@(Qrz8QS?WEb1_HSl680(+y0IsDUC9XmC4HibdpsX4 z%}&cfEtoGidkAZRAIJt8cllu1!kDhxbZI{LFR}_Z$L+SQr|dLoWOrU_!c=hiU&~_+aiEI){!&u;xtb1 z@gFVEx3vA~x&J2G(K2B4+%-Qi)^+hR-5+)sS&T7qUyg$b8>)kjOFeA>yhA)o zx1=}{#YKlVf`$QV?Q{Fto#Qm{p>WX z6d@<%;orMtp8duN4l?p7|ELpRQX7yo^N#7eFJd&Nn0a|XAuYdTqqr)f%ivV1tg>SV z*>ykZ_-uIjK@8T9G8$aOk+t;iyeANk&qX{>9^yko+GRnOB5yUrF;YylF>K-B_-s^; z8F*VIUdFz9R5KtL2HM$BXXHg?&*FGa&UH-R{!aiu5Ww$h2{4yCuvGNCS%JG*UZXY} z<5Ic~xGP_J)ul85pP+WRiR}p2+XJ=!Y_Q82!5LxSD*=Ge!!0TQ~qnQz3i$wPOunyIZ~$R@laSLl5$ejW5$uYY@+*EK{>LC;qu39z{TCO3-! z39X^Wc!Wtk4sLpOO$2r~zqsRpbUr}MeldVEaO3OoPgl_GWbNS>k^{cphcE*HI41Lw zTvP6z=7)23Nol3IC-+>Q5SUfBZ zoXYFzbs)h=XNmLd)Jsm@ol>xMkqwd@6N?+NHsW3jzH)fVRn&Sd-IAxKEfta;Tj$|>j$C4jX-6D zi2|2b!+Rdwlbwu ziG_lIPP#>dwYXK-3=X-zX?1CdcOJap-9Rmuhy>}perk+sz!b@gOiqyW!s{dc!-uWI zi>Gq9shN1HDnr&XlTd##vGNZkcO9zM`?H`HwYn7Utqs^X+Pn5jUMp%L!lrz2)Z$B% zAa&Gujsm+^tK)Rm#`y?i2>B~E#ZZ0?syv*n>zM`V@*8hyEWEcxpB zNfG$NY-!3RljMjEs_Ef`m7k~&QY-ptC5co9zc0%Ps$UNt8u45ZP88PF0tR34Z6FXP@kx?rRjO!-o8;o{VH+<2v3kp(z(~XiZ%d7 z%Pyk^f2H4cZgFu7<~#HT0fRi>p!nupF+M>_WqM=ne@)r9T^87?Pf;-po2tST46Jv$ zxrTu>A(}6UKapt)J~}=@Du0${;9#0z@r=JbI$W?>Lj>k)d-Wh=u}q$kkmTXgGa&Z4 zNV=)o{e&ZGGqQx`D)R0r^0@mYWp8ztBm3c_DGjf`jo47JBIMryXq@Am3f2?<7ewfs znm$bjrs#*>h@0fQS}{x<8?>5S-sWIUDv&#w{hRai0N4wBmj>&0vwzMo?z3juY(6R@ zP&RLv$ z1tqcvW^s09tNT9@U+lAqC*dnw$6~xn@DOV*K6;HxWm7tr-G$T(7n|*Z7C9n@8pNxu zL1v*766PR`^u2W|Cbej<_G43{j=taw@5d_+G7}hwtinF1!pIX6V?Etd36;H?Y?6*390mKSU>8K5z z8J0PSv^K|t(zPh&n>j5jQciR`Ay15~Pb%TQIK<|o_8RP6Zm7tjsF8SO+|KGR3M9I= zgQM@iWv8WSBqL|t!Hhl|glP>?mEnKXA2vI0#e!$ZpD(%Z*IPBnQoPn~x%x59eB%2yp>}?ICB#cR? z=5?*JiY3TpITo5CTV87HN(TbhkLEIrx8Td{#~n`xDzj!7t|SMTQCZ!Su-_pLJ$eo5 z^SnkiwaUT78@h9rP4y2iZuqsc=uHwB`Du|#2@&*G4#=XGNYDXeqT(&kJ+QVFNRj7V zxV~Y?`t@_|i5W-hu6vBb^tHB!!8wtIPI)4JXWmD48ZfE9N8JkR0Q@s>M*65yIRvVa z$h!EEbWxGW9ovQeK*TD@e$WygsBR0w;_R7)L%}TZ;k^m}!Thuuv}MGnog1JB+pPt1 zKixI!SuKldMb#DTQ#R@FC4fnP z@`0C#&Uw-sEtwbsR8{ADYWA3)WJ6qI$iBOwpAhl=b~4PFb77?pvVU~wVR@}fb}TAP z(NdTa+vrA;2rb#*DXEm~D3C_&j zNnQ#9%&ix%w_9$}d%-VOL6k3Ak#}j}!}lNWM(Zau>3o2659~n5y%Jju@g18G9U$x0 zS6ay<{cni-0z@G9Ya()<|N50maJxMp7_(?t$;+*V=0U6go5<$0>0h z?0Ya9`KsPLyx0mpCC6q6@K+qg&@1?WExP!x8$b}0d$P|l6R#En;C%G*`j(&tv++b)jaN$D zFHJzVm>jDRS3V%ntjtc2h{=9wU=2B(Ezjtw6g$tb@DnVWRv6(Cz>T%xc$;jN!ZH@j!*GfIH zY;n1G4e)w6eQa3kOR-0zCu_)XtHb=SqOU7WEHI!-O@xLcVD^BY4()_okjL0b(mpz<=CaDQHoWexd3+{1=CfLD_bZPM5$WF3^&USqc( z%5cF$x{jGQS=Nj_*uo`6B8mtuzWD*KOvBsNFYZ4Z_K!k_oOC}z(T=3)B8}|r8Wo@x zHcc{RYS?$}YA1=Y9mDW!Plc7l*y^0(RHl`Y@5(oc;gY^@?d7kC2w0-Co~iCix1W4@ z4K}Z#)&7UekYla!UXO}ZK%YJv_C)`hleHK(T-7?9VJ`?YWx^$xIXAd)PhE%yu#qsz z9myu5OC04}ReEp&_0(#hg0*N@v9t6sHKXwn05C{5%c(Sgz37{iI?(HIohb{5_E-AA zS7IvRPKM|aOdd?#7`6Y)$fI3WnMb#7>lh1Ld;6X^s2DwA!;Inn%-@D&5L(^^4q0g= zTgjBET_0J`f5c2@?R&%k>K$(BT~$zqoEj3MJJp_nO99ZDM_nF~$EuP4zAta@vKj;? zhWP<(%aP`ZQKRy-aS+Uc$g!)z)D!xjH-lji0dGHv3)1y7Ni&$^;t)8bt_n?WWu0$S zuk$9`U?uHV7@1o{5mS7CLt2aFjJ}$o0o(wozUXSHFN_S8g&%di;U zPUH-r#9X$J@Oc;I5QI?{pirutGq6AxO=d50@TVL4jhyNYZlMt@T?bjb{4n7+3zhjU zk`byt2j`#NE7Yt<FtPgQ0?Jt+BPhZaIE1aT>+d@mE5G+%sJlABN?fgqTADhyTZ8<|~5@1jU1Bbc25~ ziQwt9A0&%-+)d9Mn`R-Z+$Kg zejKpHElHVRcWJe|E*Z1l>eLBHK|Q(mNhHkYk&Qa8f0UNwm>;>8~<4!_|FEeov zn58VP)u#w84mBrkH|{R_w@7EUNUd4AZt$w1gvb0TZ&*eSlDcQkFYML{#vWg0o^^9* zqf*76)F4ZOuNLHoDq)d^TRt%K2qV(Ks=BUnW`l>bdC!j7+~F8^>a&il7_a6;m>Wkp zTC@`PGEZvy*G3Du6liNh{odlBJ=I$!uWhMQG(bmKINGmC^PohEZX(G@jon`}kti}x z5QSQXItARh`xylwv-HiqGv_{g00JckF}D0>C{Lb19hnn>l0sZ1NPX)X)KR}%fWgw4 z(^lm<2~B~SK@gkB!cN**yCVBC;ATKk3IWgayMvjl-R%^Jz-1+Qxl$Z0mw*81bA)&A zOx5AZ%rL+ROiy($;uYGa0J$@9i)J+b8zM%33Im%Am;;lN&T^$ezDDm)%y&wwF6;_rZe(+Ga%Ev{3T19& zZ(?c+G%`3KFd%PYY6?6&3NK7$ZfA68F(5ZGF$ynCWo~D5Xfhx(FfuYS3NK7$ZfA68 zGaxVuFHB`_XLM*FGcY$aG$0@#ARr1aMrmwxWpW@dMr>hpWkh9TZ)9Z(K0XR_baG{3 zZ3=kWY`bG{Zr#!@9NTv8*j}-1+qRt*+qP{xS+Sk0*tTtc&))m&_tbmptNEu#qsGPDVxmBRvZY8JVz?siBLdy`6}mizydC z-P8o2Z0Z1DVg@iXGBUxC0fg-xemhy3Tetuyjj8^v1gO~}_Fvi20$lz_vN8N0$>@Ki|FJrG{70gPVPXQ9 zSQ@(kj7-ff?O+)G#hav^nLU8*KWYLD0_pALbbUqq1}s zv-B`EQLuFRhiWrJn|~Sz{JYP4B%v8_xWFNja{9bOzm9$ zP53{#{CEBDLNYb=Fg1o*Tedgm4z@~b4XyAMEt;(armu6GTl zhHYys2X+&2=aR9$7N|fy^(!m)yjIGwj2Bs)DcYdCqaPe;JAQTH(N3I7mjWeI>l@B= zpQZ+B)*__v+KB&kZw(|YKWG>3h74QZqOc;s;V?||-u#;dR(;3FuGe|qJyXhBjQaQp z`zS1evpD?z8!5TWl5CoEeNmlj;qR|Qa);weS<*x4oHe zsM-y#Vql$`N5_|{wffsS=W+`9MZqrY@KVf-5Ib=PbY;P1MVpC%+V$!!`DcHL{NwmW z2Ml%jR#=zGowLV-9Hz;Kd}yUIMd!zSuSH0J392FRw#FiL@~9CpOcU&VQ)Qv`g%3LV z0cx@_ZEGWeD#^PZLQnOUCM-<9Wsjcnjx^9p->X?|o)I@w3^4<<$5;kBcX<4Ji~WQE z^hQ%e6s~Ooy*6hiZ^wtg+@^gHRVcyhNEF*oc%1+FS*T?JUfj`y`urV+RaP`_P7W)S zHeUZ?#6?XQHKgMkeQitc;N^t83WFIKPXgku6XQI{nVd_s-E710cvtu;I+j_E_w#b} zNA{G;kSz+0?FiikKcHud+MG9m)-x)={UsA>QdMq{VI{_`Av?X6HoEUJxuUnTQ(PudS~Qcc3r{S7zgJZl zJe@C%Lw88C=F4}>I*n$BdCmL$LSk>Z+-3$eZ6NL!+PfDmonh}pMq=sh{l=1nm^#mg z@ZSq9HwcnK{E&}X-{yMh5_XN@J9cApKhcx^@b%F|*PBzaQtEA6=&M}URwnV3Hktdx z9adTFz~zJcF*qI@yQ6#6HWWrHM4vhR@e7OJAf9II;HCw@o?}Fpi)d3{AlvjqV`NRt+8UzV8Usev;7gE2F@_0;M?Wi_R8X4}ARmjtASBDI08? z>GI4|*FgCVmurS$SbPN2 z)|IoX+u&s&jDP2G%3?yi$0KdKgrE9yJ#Cv$lWL5|+HywWRsT#Mb9IZ56F7--Mz+Om zbK=Rr*9cUR&r7jA05s-c`!Sg~d^#cnT#<-`S5k>Al1r4dg zF9g`2XWSVNWAM8X4x&SO?Z<)Nd>OX;6Id?%cDaEgb5!t(MWVYCicYsr4D@~$rF02{ z4cm>}lB(jcOS!;^-H465b*(F!<+uJ$4;GlS@-JU58|n;bzC%jh8Mw9f=b0|G64TJGGi=_@zI&{?`G4$Pcf?wH44aQ?snPg(w%0Wf6=HR z#hDlHPWzH~43@D44|$!mfZ`*-O3RvsQDUHlU0YAYpE>>E-`V|n+UM}<#1zSuB z$s$?n1ojzALNew|Ol@S}!O*zj((>eJ>;IHZfdi6}E~`(N1O5$8(E#!G3!dys@7^hV zyYDyIr9CZq_xdiC^pLz<$kI-d)4;1|bDOO%XgwbpM^h8!5ASq4RVFQBziw@uHC~b5 z5(Z~rLm)&)L~u%yKpt2qjVx{MoLEkJD!KqSF7<8tUl}b`QVFYxTsjDV}1wu3rsEt0#Dg~euji(@3$?dJXaXc+%@+NC?5!4L|z9W@<-1Fs{Bs}zVzsn36k8spS6X^#+iqpSb zK_Gk(Ac=416BQdVqIXwtFJGohW^m_i!X1^b6bN90a-iSZ8?Ti@In11NOrD4v;4Nwo z<4p_N#=TM3woS7z7fT}Gc{{~Scur~b;9v4{=gb)w3COk0d85lP>Voob6f35HxONH59HN~or3X}b*Q`?=yoSw^k*e?Xs8Mu}{k zY^$`9tfJ<17@_S2ouxkQb2h#`T#vAdw2jmXdnKDi$9cA}5gs-KZsxK=)H{_J^cx4K zztxy>R^z7RM5y)_3K)(hW(UAfCd(G{3R8GHDU3VNAr&tt65jq%S9_`5mV_&d5X5~qZ&+PP;| zlEZi1HS-O(YHXl|Z9dao%?CvfJc{?LYUQC;COK}6w$q`uXxO}R(q^f1C!hzYqh!S$ zoG4}sRjS6DxiagBo~c%SW>I-(NmZlf6P*N!shk^h-`(X01Pe?9*AfjQder8~<+upk5lL*v^Q&>nP z*ih7wfiTsxNG?n~PRT_2Z7@D2d&AG`mPHk)GVLaVxK%HW=kd26SsJv`?#~H~>OmD` zwt~9_Q6h&LnbHUV**e_tX4Ari1Z}>k(VCW$OMe9UO60EeQm|8rW!NHZbFsn^Bp}^D zdnenT!bXd4xhys3`&S~NC_1d-dORd3_aE5hlU>YWYTF7+HZtG#@YWfVr)x34omQ<@ z2RFQdHq##5w%vS*g4!bZ@>XA7ZYZYPZTK@AePl#z#bwv-Gzst=Z634o0NG%w$7`*+ zjdHo9vR<#&o!oM=<1Li9_U|op9m8ViO<50W_5z|0LFlv^LkPJUGi&|i$|s8yrANB( zdy?>qbHOuEam+*0AKi@he;C{aokOpF*4vJoQO_9+ zeLPF}p#%Ktg421VWGWWj@`VcTGtQZnyt z{o9yxeJ?ao#do7TA6Q%Hdi)GvH^l&|;=@(ZjN=_48uJj4li`9JSUe$&#ISXtsHCu_ z^Wpos{L29cuM3F}XKsP-YiS}z4!P^PnqGbbmkaejPG{u3r04)l(LwuVpRo}IQR?BT zh_f5feRQSB?Sg?Y-RhKzq}*fW^^up#IdYy1>A6wPky!V*ZH-0=FfDNHRN$bUyd+0} z1@P^J2C<1^3F|wdZnjxWgaBTKJg(kT+NcWTCg9ghee9V+x%(^r9YO8o7g|>& ztIGm|U{mJ9wP5^|D&}+!ijTRi1mRoJlNlB$&sRvAWGh3XN2mTvqmRb{rk@Wk9xR#2 zZUV2UiZfaaZ8HlzRi;Ls9@QpnGku35&#RcF!ynVxHV%S<8#vPx)gS7c8mlA~8uEe2 zoE&Fcq1+U1aYCc>*p~cKXs6847S1*=xKhK<;Gu*(yoCplw9SV3b(PYPSnO@L=a=bJ zBlgjRW|<&jJSpsV@Ba7T+eLcMq$Yv-^!oIl(`RlpYfvLo>lZ*+XJPh3aWfA;%^fwc zUR5Zvqw*zRH~M9)2{e2&N)t^)<+(8XpGQU*_h;KQ|r=36qk-e^V`_1FM1HcZJgml}PGXG)oC$!7V$7 zx1Bc4W&cB#=p@sL$(xb}JWISp|N8^CTJ_|RpumD`*TgqPUjCX6 zGv@TLL`O48=rGNGh+HUUp$%6Bf(9W;46Z;C4aW;L`5--Jk8(5cO}>@I^j02KXVF*^toeei}SmmUw35^3-yeXHVB z;A*ApAvU6(!lCmXtysp)r{J^c4)5D?f-e&J{qvch%JH{k z=dhkfB%$n_B9PfmQ=_54$knHSLdegpf*GQsnO8#c535$@Vh>5eVKXbR1dXl@xc3$X z<=ps*%UEv7)5!bm<$DHwmWt1~#i{lLu=Ogc=WUZfjI7E?H#n{tOwrw&8XTv-i+RYX z6LD=P{gb#An6$a@TeMWxad(+AoB8f%$Ylw3HX9Cs{`ZYzVJwj}_ZN1INz21Q27T3T z<5~Wj0;+pns6-E`LE)(3=BTQ);dXxf6sZ3U9bm7PK^20qv_*o2a&ZiVcE1Bd`HKMMNn;ZNK2PFLn=kj7*J6tVMoLWp_K&`J)(+?yQbRIGBGtiW%4kkE%XJx)h8 zv*@`|i$z5D7blTG@m}cV{Oj1Nvd))oQU>JNeVv@keNSfgAfW2+r7)-N#k8otLLbDX z$Uhue0JB(`lVZR_WOiUTP;RgY==rAT6lpMKM`x#O1w2=3gp{kif_nx#)AHqtk ziDUY25@Jmpyt?*X6dTgAAkDsS*ki+p16>EEvW0nEB2m+p+rPKb->m2xxB$tYoD3r; zmn9e`-3q3PsE-!WUdOTuB0g=4$5sB$y6cVfgyO}R0lMR4+I~u1gyZRuK^>4*I9Asv z?#z3}h5=&Zw71yZ@_v7a&$#RggG>r6AXJRoPsIQT!V1ey>Z6#v9T&A59SnUdOiUT_ z{V0kBpOQxMZ~-Yb0)rw>ZS2iUf+>xMh7avP#VPe$S(+-vm75>6U2U3^?jTt2+vM8c znzGv4UNJ+%t43&c|GPE1SO2kO{qsfE22*iMz8r##47&FS_?6K5VPS4SX4x^qC{od< z8wUpXQX4;*{EJlj^nQnwsma!$+k$+Ncq$2sNby1(P}AYQl=3a^ALMcRMw^R8=XCFrAZ0~K1?u1#ytqHIbW z*)l$Xqv;;}sSM{wd>LImWKWk|kv3agt+3bF6Ma}F5_q#*M0hbNFLzU5`?%xpsa~w_7XT(b9;V5_jgaxjzTd@`2m3v ziB7`w{b9x=mpYkyDHSnC#mB)XflAk{{#VP^zBywUW{L^rr_f1tmNldSuK6~nIg6^d zpQ^-+W}knPN&B3)urA9mnn^7ziB&07=8=1gx#WKDyXFd{d+JVI&ghmK=w)qQvR9V* zY42h_I@0;M&7c~p>aBW0!8|1KzlSfSmP%6yiLi)lWEhh9G*}S=I!%Fruux&aGqn9p z{puS@ZrFZ*MU|hpOWhyML>`r|eh55jjz#X1BS#2%smz==ReZIR0Fki~qck~C?|E)p z(sV~>;Vdh+mQqo#2u{8x9y}p+7x);7l40KH8$PutXmXB2nY~JO(X4wf^|oK4Js9Kr z!Y;IZT+S&^qWK(4--^&C)hkIl4*M|^C2x94c}`xQT)2vTlWa4nD`D~Cn|+b_wY)Dt zhj8?%(w6Y=?DxaswUBRa)L?n7QkiOwe$M7%?qEDCg`jJo^B^TM7c5Phsv$a>6LVbg zB3NWhsKf62WzmRt`oAr+*3-cTjG_6Mz>P{{%oq?O_asPAZOEifBX0b0dIJR7bnb<( zTek80!a`H4b2e-u|M(K>e~zRo3KZWqB>7|^ih)pG7WabNR<_s1j z*n0+$kIaXQh5!dl3*FFUFC`p#I+NnVcFeKmUE9-k%} z%j|SXQjjOO6h%vFe>I24P_J-UX2tjD4diDh!oIN$Uz1UzJIS5>%DyB9ELBk#mktG^ zPi;`M_Fd6IM751Zx)r{YGprJx(DMqp*fS$QXaz<4Z;l9!cyU3AieL`qD5B{@B)uYSl}d8I)eW^pL~BWsHKS4r zt=h(bc(qMaaw@~3(}y>6>A}Hu9n*YsC5jQ7IIqz`-Uc>;}Wn{9bruQP8yzsMkmrjw8yP(rXbiJ zoTd_$b{|%S)2(^bBX7_&B;-wrv!mymJS5;=D2@Jcetm!ODCKveZz`V32HUp)`2S7Y-X8#pfku#2HfJD?q#x#b?rZkJ%!^@^o=J-u9UiY7Ey%qh?RpNRVo@`vNh8Y zsx$CM(T@i{GNLM4xr^X?#sLh(X<(;P)Ie8_e<7yvN z92GCt0d;#8h5)7$sudeM&_97~aE4d!qnsVif6OVkYUwglN1#-oA;K6t2=3)T4K#q| zVi1I9Fw7kRpqIpk;TiGL^!a;y0hdKc=<=~V0{S7Srj0?~dmQ&VkNK~&_G4Y`GM1qVvv(Q7*e^~LUC zQziuCYm#xfiagtKQou&*`X?hp{|(zzE$TRD<9Zc*J6OgtX@tR5JN*?xm)KRzSPQT! zK%DzvRf$46`0Y{Fgj22%)XlrK18S~PH~q`?hkfYr_()iTz2h}Bwr=6v|H?3oKI%XX zN`_cRc!nyR{SfUwy^%2P_fO`X9_;B-3An9$cw{{`;7M8@4tRq3GQtWG3`L+nDb3=& z)=lAPFz7z2ej5Esa1Bv8NZ~#>CSY%+VK)Ml6M`6xqwuqh6=-J|+JszUW-OITQMgb^ zBV5(b8q$r~kcHK%LziJnYNl95yBvBU8CHqZ+H>l@2~B=0jI#asD%OKFI@6u^_^T=vF(`m&vUsI0L*{EB!@9}YeHTv3!Rq4aVjG1o5 zAZQN@2zU*>>j&(g$6&$LG@y5!eB&IK?EUT&!Z0Kd{Ev3>GH-aDj zM@UFqRloC}&3I@=H|x5q!Hfx{^>H|$wlFW&uv56-S=`LLeo6v;*G~BAHTKp1h#Alk z)wKHqTRmM^425uYseypieFDa3_onPMM9@uPjDhB|;Cf5C>PXcU?TK)R4?|9_PG%QP z;?!S0t z4Rg&zz!|Qp>DrhH-qqFSpS~%wPab>wmGdRNiFUR_WghGCDKhc-4!$%CkZa1;{2rH< zfO$P9O;-vrrT6e_JdPf8ugxNcucu~Dw}j&^lGBCeeMavl4~zH>jq5#9SKb<^MP_W= zBw-D#VcL*ZGCm3M`12F|45liKIo)6e$C9bqFj2kc{qCX>@=JC1(|C}@o_sr{YG5H@f#Jb-a}Dh1OBj82qK z%x-o~B#~H1vsJELz7q~T)!|!nB*Sq>5fj`@*vv%T+@(GatCy$pawV@l36CuxxA{Lb z_BI;_DjAm6P$%W@GeX|veh>NyCx0Pa71O%G3p>Qh8u((n#}>i9K#rKzQd}7R_35vu zjQ2uod2<57JZt;5b^whuKJ8K&(GZO%yZqIF(QZ@z@nlMZ2Jcy7w5)bRO#_eF!u6Y^ z#tS&1d7oh_tar3wfkIJAIN1|_)$@)n0{BeaBjVb@7tr}_RrKhq^5;7{jd&{iCK*FN z{N?q`L-?hPals1a;%|{2Rxo7)SOaqGF;637Wp{W8fq)1;)vh?Z;hH*mk*WQqx~PRi zx39^ipOwERKfdl5*2Fq>$%&)!um6s^{}C}2a(&0FLhy+dZ_ zXR=fS-H@)+*7|h4%)%5$;`@>Q)w{}KBRrC$<+8; zlzQq9{Ynfu*nqVxuP4yCQSSb>0x-uKaQW*WiaCuohaC2|Uky(_QY4b>W7i*dW4!13 z^>Uc*6a1&;DMfad^OEra16N<=0?Vs_;fd{3o{@E6iNNp6=)^by)<6OA70*30HS%W3 zk5aj27DK{26HlNn9A}Yf)xy2?lM!{AteoUM|B?0Yk!1v2Lhe+n*o>YrXyQRz8}MfF zmUgyTK@uydl0_7HJbF;qFy;GNs|W!n)+CYg0(%TbXp5lHJBg*7$3GZ@)b^^VFt z1#7MchpBxaxk`u{cln3FpR!05DM&8*-3H}oT?pf-q(o<{P8Ne89rAE!%}C4xNifA( z^Ha^?FUwt+)}Bh%?y1_<%CbesWJGnRW(I+(gF+e$@@#TS_~k*Nk(txV(Iix=7c5cH z$7k(bx1}M9B`KwEvddx9ei!U;_AXKgO*FYIk9RHak$J{(a|b&Zb&%Azfb0}YcTke- ze3H-_yagvbLGk-&z)Wk!Bv*j+_e>j-YQ=kSRD8!^2-Ma^MBY1Knvn&fmZ!}Jj8X`i zlhLV%*VU8;dywB!#crIWj*xjJIB@9$=GI`~D_i#hP9^!JYPSz9@E%p*T?<3}$k_sAI=+QX7xoQ z?`C1-(n&!xF*LmCg25(WI=Cfp&^z6u^sUoniotp2E6}a-lLC%GRCP7d-3I@jXW?7m z5E#N27fc&7TiO{jJJOvaU$DPpk#4PfpH)*>7N%?;_AM7ti^gg%ZrIQ%47DSQ+m@7! zGgm#RFxurFuswkzGb#0M1&q2qI+aS1X}}Tlfqtb*1%3!RcEEV3)=?dlG94~e2h7B) z@*56;)28%fazW$dvgdPB8rltygLpwD<%4ZV2zmPImG0y2bM4U7Y9yW3fbYtW%U`8$ z7a5O9nz%*Dd)%Lg{W8A>jiY0m2@Mv=5v#skllHI$Zl0xlJ4qQAELjP|1C*IgL!!QQ z#aB-5NY-!&gHoQXQ?va!Ui;|z(BYF&5dg1wyhA!B#fooOPEmc$1Vvt7IxgDovSE1! zS2a)+chi56)y5RMWP-;*cj43?R5+ud#whyC0!jsi;+_a>o-9V^Vju;%IoTYkEkUQ$ zG1!)S?9SSfi0EXC%>}D*LdfQMq%hHz#;&f|Gr(s8 zz#q)$8-z?oGKrMa&+pSMHW-I<9BS{GMLx2Ft~-d2)XIh*=?9t9y#w z88M!aL}JJ(xUF(P$SeM4DjOV3zNgfB3LW+gaSOs#%DeHMN~A5M-;8Kuzs~T0?8Nwc{OpPkwi<1@JG1i7C#QnzM zCjf7W?N|& z!{Z%u-!@24VhstNz|wf0ObM|DzH&nRXtBt;GQL$ANZ#h_(HQkzIXy+X->Z{(2iz^b zgH~Fx%*KpmGpt3eh(fQI1>S44ceS=t@OwBHm|TtOED#U%7@3w)%K;^9g8R^i6uxE9 z=w72#XS0xfJxAbN0a#vN?*y>%r8YQm1?QEuB)2d{ODc8ShUlIFvwlW zdDXo*-l|(xA@0~((DtLXvc|At7oId(9>HMH)rAcs&l5SidL{88zB-@Q+;N7ik>rlx z*8yx;G|np6>l>8ytioDY7P2oA+$prx9F}Z46uL9DS&5m1IbrxPoK;r`m<;)aDDH|E zh4|f$2kg=-7%ymodhfJkoJf9{)c2SbmR|hpvTZ&)tufw9L9*HRyN6BdYJ%m$3TCJm z-alKTl&r<}GUe|u)am2xq74s+!Df_5i+K{wKX%eBy#ls2AD8aFNN??XEJf;b5X|ZW z{)9^T7c^mUPswl>kVA)`o1)-sJ1Vg2$6SRp7Z_qm@jnZMd{UV2@zDwQ*O*x#^x2um zeRQU!wW1jkRR>_3;>NSGraXwQDhG3sfoTF$sSi=P;RXbQ9H`TCw_8}l1u0!F*oS(P zOX8jT0@9-1^kYV7-o}ldsGY}QZh)ec#?-Ptefi7~s92$>(oJp|xBO%VwBvDv06KHt zehqAZC!B>~^A4x<`VwZ^<<8CEm%;6|f&=B2UwS#IDM@riW2q)2eqEIuWk7Szz)uZk zo?ics^xqo%H~YY!DM3*KSjn|HlcBmp>R|Z160Z`;kNWtsjVcJJS>mh~A)$>s*SQET zBisMH`WA>1n7JfT95T>JGRm>(QGFO3 z2-2V>wSkf86nCPDI)PHb_ge^o3KAxfd{t=sX8pT^t*MF{do}#6Tprmrtpm}6$b**H zKar9%*qjXo1s*?UPmN26G9ftkNyF6aG>Oa8t7|Q6UzpA%V$A9cI`f)sDnRvlpJQ8V zQQIDl1~!l+uhCV92$8#^>O3f*@vd(HQVeoG+>rIGN-(;cSLqccu493?&8%<&CFw~F z+CD?rFkE++ws#yVjtm4@ zLnq`_{z3UYOVqoD|`JoKW;l}N7lJ521jiy^|HC4li09pz+l}T{o zExs8eZ4p}_?VQ0CdE_H5ob%MbX@@6i+_z(*%tsG}?M8dOX@j8nBvMG-Ok&Z0JIu+e z7`}YB2Cc6nc=xaNSlHCK96w@S(LT8C&x3V57eqIIP(`(;ie4&p?{k22k6 zQ=0u4wKpGy3#;VN+3tpgv%FUWCS|v}UW2?OAE>k0WjxRWhI@k&qY9eTm}?*~@XEJ# zgemLOH|N~d-;zjmY%idSZ|jy5$rYQdXig$jP>!da6Z>~EV8l~`*MEN~k^hF!c(sU; z>_C|=^16RX4+|yd>k{{NEi8)|k1grI!{4m>vsH7Sp+_IH3+fix9qZ7ZP{S7W9-mMN zBQ6wsLmD8-nft?8gZ0rfv<^K-!gvs9vD)F-9Y>aq|GJW)v6eG%Vq{xBeyW(;f6a48 z*C+f<%rKZL*;31QdapC&_7@Q=s2Cs0Z}|=NZWW_2#%sF-mf$_A!EktJbWw1C+szg? zs1iKt@*Ssa}n~H!Qd}H6%0lM`06%m#l}Q6-&P|R$BvUOMeXbHrxO(cC|Ap_=x0nodZptc zAmoXfE+y>i?E_l-+mLMJJIl?>L;hqjH8rFJg)ncn=zKWQJ{;sI2Bt~e;B}5|11aQm z&hilmn)F~5C34~+flv_GmdVkefdy6EqmL6;JsYFF`Ob>IW1B8h_sE+wmfGM6w^y$p zo{EQb>_RYOc!Ep#Kvck!0EHi8j(5|w7VAn$8bUEZSioHC95El;z>#w zq1%9;B3qMUkw6EBO=5?vE9F5i1ufN>p6Sa%_=7Gmb`h4dGEpWSjzQTtg4WsLP@)s4 zIJ*!M(`4`?f3E| z&w=W^(FQ&nU(3eWj=;kwi>=2zpUO|_fWco@x^`w;S$869HIe((++?gdNqNtyGNrcO zI8Y2#&UEaD2#C9hxH*KROUhmCL08bT*1`mlhhoU1ppe8?6`R#&uU8oT2jj3CB%ykjN)@H>0rdPW|mG-96YPgU&HY-m^*cY3&m&M=0uaK2Qmb+!x!p$~=^ADfnUg{#07lwVW8oC1fy32sE;>J>Hrn z{ywC9zZR{YIWE`#j+~xy9)noAv89e~4S+r`J4uYVrxYj}%nHkdx7c9%oaZ+e#Ri#Y z;MA%(xhnSJwym6(k8mEHpIQ!$Cy=3o+n>Luz*p9v_8vS|o{ya?(qch*f;N-%Y53c& zaM&HyDj*oGA1}f$PV8UZHiJ3Y!*#m5m<(mLWJ~RUx|ZEu%aZT=sQ*cW1pkQ)YVtb+ z$Gv`0`V#ZAMk6L~u7QLQX+&6VOv@I99(7ib%bp9g<_ z3#M;~3Y&MKXW*pm$gqM>{tL^5GdE<0?6_cQoP)mM)ZR=;eUYqEG5UN+`h#e~F+rk6k zE{UGSt39OvNk&I`lhm*U$uW6TGo*yN6pY(A)sg*5b9wD>CcohRSlfAPXLv;;cDx47 z?ztIvMyJe`L%lFQbG;^0<)paF%+vp#NUIlaWWo!zar(GCq$bCAkts z6<1@*E2MYo8>V1QV6W(Zappz`u>ig;Q*gA>dxDgz&VtXSv0F3lW@JtTMmb5BLRKeQS0@*G@Vh0kraemj3jZ z-D(OMxY@Nlad>PHEaS5TRrQ9pQHLQ8-a|}noZ?&f>3XD5&m75L{y&|EKr|Vai1Dm) zjHaJW1XFa1y8e1=(c58yS$1he-))+h$GN^glrw?EoK-&ys0%-nR{Ma=V?rby#mD*& zx@{Sygo}=JIZwm?8v^f@wxyh8FCbH|YDln9!Ww%*Vwe^g9+Ze`I=V3*v%zbn1=;jB zsJ)DwhPy4SD0*Iw#^ckkQ@Ai~l$K}Px2)k;*qlX}iho9B-8wl}?LBUeWabLRqsSBO zdR>+7$sI4bt!9?tFyVV-b-9J!d9k`T&M%TL`+Sm;0Wn}=S3BI{`a8ertRvW!$em(_ zLu4%AQYs{Nf8QjUg4q}6jF`hp-b|5QlWYUsuf>?@&pYDwQ;^_`q|?e6;5#S$|6I7% zoc3b#>SF$&VrYSPo|BtX;;-n7Fny;AEAzeM_cU+4)rkR+u080i~Rtt!gHsDbCYMU zpw%P~y-<>-OjhPlaq{9)S{rRv>5Bd5FO)VRL8h{OP4uQJZ7YyC)SPT9Wetc&p?>g2 z@>E$}5{>jX#7DFgB7M8ZGv4~x9VnHm_4O0=DNAr~(0t>D@(L6Wmk1QcKt=M$%e9O@ zR5CqM(;dC0%=-Y_so;+Noc_2*5-yZnP6V!K$yhCmhc4?kfkrW>7yeX+id8h%1ksD< zo5dY$8!<}JyGgs@*@0yvJUYsOT)#Z!19&4;?=;bN+x>xPsae*a@~(pa{gDH_aKt+U z^-C-#YA0mSq{z|oag9H$X5=+hke0>9kSQcj(`$;gEYrF+S-33K9fTCRt)Z$Jm0=>j z+_af6Y*XZG68-ZR9nDx9!I(G3_ZL5QzUxmIN{!(RFsf@}f!Fub%u}TyX9X6r3h1S zayBLf8^l$e(Czo-NiqvMn&D7wH?lN#I`;~|w5f&Ii|uKo`p!z=lwLXEK0BI%1d+Ux zFS+hZ1XOn+*~y%Y;iHn<4=arQG*0g3oa1llSSjHFNY+YzWc<4l3?~7qURIlfpIC!2 z28jYZr16OYnA#fw$=ZaIH4_;xC~gj_D1~h%fhX??4NHezi8rdyC!~Rt`gcWPp!RoT z6G)^5%OXdNiF*ia>lDWB@~w6~bn!Qj!clgG_;9Cpyeceag^rY8N_%Vp*)Nutw9I7w zbzg!N*JSCQ6R(2Rqcd30zc7(4SIshCU`4|V{FZxFu6a1P{EwFY40AR)o~_BCnuPhtn_D=Nlgs7qXHM|9tYVjc!TtG zet;AT-M8uww%px)d zYY_x9N=~x|J_Hq4gM6K?J+qIqEP#|x>mLP-N)@gT_)&%sfQP<(wg;wx__GCXm zUhvnM&(nrz`Qcuzt3~5b6`k8^63kyq3n^9WlDtbEYK~+WSO#Bd7-H$cCq}vLo4nJGk@kpw z9xXcecnh^INo)wuQe|V=L!OW4PuJ2Qs-KaJ2<9b=mmfl=*a2x31KA$Kz{m-e1*ElC zu$mIt+&fwf1W4p6ne#A8cdMPoC*yY1B61N)e}+OMfs{o6$e{taE!cm)E};X~MUR53#4NtCt^a$201lbIq1}JUP$HX9Zt4(hOT(07Co3o z)g)5~dluOL9lzEl!Gb?jsP#7?oBWByK}6D;DNjDrildb@^M)!?7=v^crv0mrD-cTZ z_~n*Rw-qw_XSMP)wOel)VTef&jo*j zaj_@eO(OC=Lns__BrYxJC*50BqjYn*(VVPOcvu3Zv4}POpJ5ofKnVeP<~Q?lENK#!TtN|(If-Pq$mJKtT*_Wqufb=o^qjg}<4z#RNVlD1HK_BALafEo z30UTWz8siM*P`R3!@drgf0^C_v<7Q)yjBDKfXd%4ardr*;0;B?L)Gt z()P0~JS@kJ53nd~--{sf?J*%QD#)Lac5p`ZUf`$l)*2r6z&j5ChYLWl4u#&K;i=-y z$UD?|PNHREMpI}j;OZ*x>Ju$$`CS|{Iw(tB9;2PBdq6H(947+6v?0azo={8`r){~V zZ%zbWH=-pZJ^eJUv2B!|BB@Ru5oBY{C1#ejP>@J!($u=V&l|S;R8L^chl*WQo^A#3-!}!A(^YvSVBetvAi8`*3FB}y8;0_2YBE~8Ze+@Rl zAZ76d(^Wmk_1`<0R_{HJv#^8dT+W~7oc9S9@^ui3nGeguTc+e!e7vP_&;!Zq_`7;W z2f|9^hz~zkxuQ4UUfJdZgFAFrUvl@ykgH`DCXE(9OePD7#i#d1CSp5~rAXztMg%jD>!wQold^@}h9nhDYJ(J!vBUtLWvs4{M*Ue|p-T zdrw<_i-<0)7kbB+*fYf3D+cq<<~SAp5klxeMt{T1RTCN*og+Fq=&RYV9zP%+S04ik zI~#cUK7^+)Nh)I$<_osMWr;N{Xu3MVr{4#<(p$0f*X5EfgiVY2PU(8GEkF0t3m-J< zn&f%D<=2|4>evr1R1(aw4v*aO+6GltW>I&S{tYw3xBh3)Q0!}a5CC0d(~}=4B1b*m zgYg(Hz*7vS>={r{UKe5B)MdW;nHew64<&^5dfc}A*Jw6jiM%aa%hwFuY~*Caoj!fg zxBn;BodP=PZyLmlx%?Psd20225FxEePr~>&uA|uh7hX4^$mw8H&L@h+`Qu>eN>Keo zeeHMY0vTPn5$wheoDO7nEyzY0a}1aDM)GHN<~j0mxDQyFfYK>DCiGLN;Wp^V`ex%p zYY~^6pXyoa(15)*SRIvStcGHNpfbIG^!;%ilz)4_+Ci_D-#aew7<@PrcgVQA@;sbo zw$MN>HNgeZU2FcJz+w8Jo3_Ch`q8Syc8m5;7s)2_bohj3CDgf6`{q`@3eQ3+;&w|I-T zFIxvYrW6y6V(hJYWt2IB&|eVNBQpFlM)r2O{0_OgghAsma~7|lP~&EV5zc`$D0!H0 z+6^c+aw#6WMzVbA?5#u%Sn5+$NgfE6UAc_GNU4^LQ+<9)+YY`W3GKXss!P=O?PE70 zLUXF%-m40^mX&+0hmJwdU5kF~gc3GqPA(AV*D3Cp@-3y;tw`y5-l$!HrO*0Y(l0f< z*IP*$LEPU|z6-3DMw#r&Z__RMXHzV&NMQ1uZGrUR4tW2bfqx}t@WaePA9$$+( z-%5fX=Cz!wbRkxo&}rf>$XxcNSVw(gJxQIzOv0EZF%JD#yk!-zfhBOknVFelV{_6Y zCI-Fd8$fePXo_B+p+HlaMB}8yABfG6W)09 zZPPEBr(crRS4pw>`4)jLO=bCpo-SoMszIou9b-qO~O-B3WScLI|MAngGZ&FXZ7xA+hhT?Vpaog^s@ z@f1j=3Z)@6uAbv@TYJyJN)<6#m`4dq^jAyR$`sRXJ(TK*lZQIX$ur>Bn5+74ZhV#{ zGoWX2CqM`>@z`mal&{9g$->;4T3{9O-L?My}TVW9a*;V7S-zgY2{_ARZ69h+fnrwINc~~H-{K6HU@uyAX=uC1#mBz1MWf{hf)E35 zlB`|e6C}c2dVL|*?CSQdMbrwCP`iXJjK1+|g?4a)8DHt?`?_k#ms^0l2A1RH(BCSF zSK-riJ|Bz!|Ge`iX90m0Pc&gzjo9xCs7<|^7TBlCbNnitSLok-f9ik2(Qnn)X>RZ< zY>=EP)MZ)T$BB$Pd8kJGlAJIJ!b;*#38M!cpIRd!70&w+GUR*xd_6qCFFfCcLBz`R zK$urY5n}fmZ&fhi7~u7L6A?w9QYwd1la$nJGnNW!NVu`Z^KH^A{i%cE2tda*lY*t> zMq6+(53jA09AE6oleNYQSUI^l*0GM6vBhL zZDx8(Dm&<-G*_RvPDOYWza0HyX#ba{HdEDv)delBhWn&_31omzZMzEEJr0jl29^AUGje{Q z04<0G179C$Veyx}0+EHGj#>Z%>_7U;F>9e02bH3ToInI0VRaCy_L4kiw)?p!+)=$6 zQ$``Kqd?cp7#9p&{IQ?R$gx4#qP76$+&h#7Py$4jCv{I}bclGSi@RL%o403zBN?G z{PrioxwWZ6m<>R;$D2^r0(Poag*AhyAg!YPSi;NAWAbk_hyC>Zb(XGX**>GW1t*|N zqBQdbXAJ|Gy)gIPA0{|s!!5mdeV3oegifZ4O0-%=!qi$W$*#S1;EA2zIKs)jJFF_E zD+ayCYLc!vwmk){yCh;GSjeQeq8D54rXU3BGayqf-3YUkuQj$-;r(kctYl^Ng%NRZVpB{zDD!w`2$6^c~!&Y zZw+9~ooV}$XvY%b%8|cG+F_r^iC@L2td878(jYM=+M|o&q(h|b_hq9exT3l_;i!4X z$rB)J<7HtGLWD$0wtScQ6bzZtG)OSaDw~*LI$kN7dDP7O-Z7fQLL&&@5XoGGs{*ja{=^^1y%8Tg_TqH!_WZipWuvTlo^+0COyPX;G^<02BF8 z*?G*d)pKU3m8}q6O?60_(XA!JTe@@*nxCHW#wldCU#pOl!(q zOTu<|t-dehmQafZ{LdZ; z)wqqDU0;rND^l=1Q0MKX(yuc;eH#bHy#X3>OO=S>%~c+#e?;FSN~6eDgcr|}(kg}o zCHw=cQfF?sv7{4SFkY8ZgR!Dk5Z&z#CvszXU|#YT=R0wvGHKec3f%LsRZ^vL81U4Y zy=nYz#UsW;fkNfP4*{(G=G5v)P@2x*HFm%ZeYB#r7yN-Xy zeFNAN(#_?&%>m=He7p|i6(y96w478+Pj}ok)3q|f8vcZ03Xo-65fao4bQ_5_v2K&x zLJZ}2t;ymM4?@tp@+(~)g=D6E7{dc0S@a2rs`)0Z-NJ4wZakt>xnzy_7Zp9C9KO3X zpB@0fQ_QX1oF$gEfwqw2+v`2JA(9TM=Vq{A`$*qaW1|vz0S{dT;lya^I9oVQo~SKf zsVpUXF32w0^V=$ua8X^gWYA-U5+NrP%&wrEsjfElPFboG`}(3myNdvva~|3+Z{2o4 zHlZ$5ssq0k)ZOb+2kk#lYrRi%MOhzOba)yU@)#WForD zi3qdYbH}#>--A>^69j1V00!~&%Bg#1gcd6G`xAO4Mg(6Pv}7H85cv+l6~uB#xioC7 z{(#;p4LD4Y!FlM&!RO*q3Bl{e7~$rf(Yz+Zm1`cx^v!iZJHkeK9kaElrcw zACRn#7ck`{e=9=ysirnSfbRQs{lyWDZuskj5sg%CT5bYQ2u=U^$_Hdm!Dl7FcR6ap z-ZUc7s24w({iYVA&^{I(-5PDc1+K5?vCPN6-Y}s-k{hH8$x{)Oa{ruwjCs&OQ>N27 z5}{~6*T17$?rnezJmr%FQk=1JyffAMKEKL(EQ~8D1gjmw%RtO~^5}U?2^E@3WWkl+ zw!s~kgztL_Xd>le5aQz%+XKm)V1Jxa|Gjmy_NJ^fk?Iaq{gmWrtbT_?_SzqM zTxjMyUvNoOPgxm@pr{#Zc+bJZcb#v9a-=eGP~jksol@ut#SbqTzvEe14Oi+Cf5jO- zkXz;0yHuGP2kRBP)Qy)j@qB0V!hK=3g8fw5QOxOq(b)-Jt0V`$iuoG7x+a%co(ZeG zU|gK=78USE;sHlum#nrF7v%5TbetIzP*m41skuo|vN;?2Z*L$c5lNDtyYaDNiAcm= z>$7gxt=#8|aY&v_&@Vz_Qq(1Lo;DFT=$jLvQs8e(q0+SSm$KnZAAaqj1M7)7DOal7ym4+ILmQgt}Ta|4X-2p~o8K(`0FD zoC=l0HtVSb=-b->&_B?Mn-Usl9-<7=dh?V5dQzQ>ONX~m#H&3UCgbud`kt+Sxt<1Jtin~`5lY8Y`f>rdA? zr84*Qau?=C*loiNq_~{zsp%Gag4&N{Qtg{ev7poI?8!blaey=WPP|Fb*proXp<<%j zN?Om*ztgsk;kgcJ3KV@SUz?5xO_bG3kP{$53?}Nf_`ATGJ0D zf5o$lNL_uU_EO+u6Z1k`Dmz2umGID^RKlx7REQBo6!=jM&^ibWb(fixuH?HbkLeuT zvc0uZ(0h?_tF7n0pkSTarqGtPQA72sul^#I{5oMttdo?v$UuTC7Ws!6o-^gqW+v7w zYB~}vpnAjeY-(be(`Ao!FCIV}20Mk!S7I}Vb<bnWt|YHM>QW z$w*ifCRU>l)!w1>vLZ$qKQsym7S6-dl`uqz>5@~m$DvUSb7)~nIE69XwiT_W;|TNg z0%jMdDkHIGQI^<(@;PGC(aOIqb%Mh2Mr>4#5veCRk{7U4%8?fuIyynutRR}+9Fdhr z@A~@zEC!t2>C~Uq((BQy3XWwqfw(%SBP2*A#y}Yf2y?=Cg{bY-;Ek3n8(Gj&lw!EP zU27~@CE6Gc%fuVDJLv?Fq^}JBt?e@yt7|86Wuls|k553Z9<`hA>{7GP6>Y?o24mQn z5;a0sy8*^=3;n$h3-mJh#5ylS?E)2g>TpC`qNAw}?h0nn7K!wSK@1rJ^JKp}e63EJ zN1Lu*4Z)d2Dot#3Uhd6c2lS8vYiP9ZdVf>V4)ylL35pU$5O?qoasxRmo8;0t@+-Ar zKA7-^YWbw4Ig}k^rk;oQt@1DY6&pft;lS1$Nj$BcwsDm}I6;=VKoMle{{cQ6(2)w& zni-0v-A5*V+HNQ>dhoCHssx2X7eugM^2Lhs>JxZJUPp!19D>vlNs6 zraFeH#X1G=;5r6l&|X@9D$3Bo$J|~r>*@_`fKu_s3Pg7<4?+RCATPE~l4vrVqY51; zQAd!!IBmp(Ww`k6i=c%APi6sJU`_&Y0{p5a+wwo)zSutrR!`B?L~#Jqavm+0Lpm>g z@$Yj!X?hN<>Q|hY?g$et-rX77UC4?RvyDja%fizgjDeA2S^%J5%gb-ktk_xQMd zrfpF@0QkqCe8?PHbf(lTmOI?Vl0J#<3E6vh24^hJu!Pb?GC_M>gccT-ZGoZKzrwurHG@_-J<(kEBaF!kM52wW95*B_I!am zAb}{GsQK5M#LEv$&k3zFYC(8g;F`@?oyHeb^KBa{VW!YrTw%em4gOBEGV(#}kau5!yD6x~K7F)%X9_=B6^J%@D&ffL>b%bLltZURxIX|dvau!UCnF=)ThDTyCP)rR#~n zxM1(sjBto3Zo47%3P_*#zxp;xQ)h)dffPzW_n7pFw2PG-1$Xs1*Z^k)P-_`f7bl3E zkHmxYS20}iuoTOt`^?Cju#2{CfuE3wc|teVT`wZ{2^-m?ZuX1EYtJl+cWJl%Y{0NM zegwHKlK5Bw5QuXizB;Ix>$@UCM6Yqy=|rtqt7ceQX4Wz?J6bg>A4{Hv@%P8BrT>d1 zT)$-xX?)zN0gtRg-VhrX zMjzO`4g^3`^Nc(fy{ZcRuj+tU5P_braY&E=QpGOGllu*z~O9tT|~+(@Z592JaaPonb55CR8Z==Ad(>e z=0nebPb~?f#h4i)^C4N*u74RgzECQMz5Z`#e`k`o;8Y50D`kaw2D< z2)Jgn*@erI;da@`d1&x@|Kb?6l^v-uLgwKrs)Nee$Qv+N$ik=`e3cThI_xsMO5#K) zADb{sMV98cTVxcy`(7CvL3P3^iqh;-cJ;IBii{|KCquO0o9Pi<{HlI1e*1Dd_w7Ej z_X)_2c>^8`c)|f1UWMlH9YHluu;xrxdQjP&C&LFe7z?n3Lt01OAUhVY2hJuFv_Vvf zySZ&zPvgzZCh&nub%vuDzz+E`n$f!zZ2rBQ9gUCRjNL;I?hswr$(C-M4Mq zwr$(CZJW2v|7Ma&W--akrWUoXvp8SX`#z=>*0c`&h+kZ z+xk7{9J;a7h{)|vh;S5;hELR_)}`;*>7xA4fAShuiW zsMZ`ey9c;5{Mu!8nr);F@|kbik{k4Bn?5BpT6I#@;wEuEkf@g=YUCFInPaq;m;x-M zA+q5Oi$?R9`oj{ETJi9A$L|hs?GCBo+{KpBP8=l1ZjyB%R{*G3C@*YYx-o}?N*T^X<;V-@R$bQapn|P_`|re?p4yxw{v!DM+R_oEfw((*XU> zUPqbF;-pWoyB^uc>ZR6q^Nf$A;a)4y(>*Plzu z4{e(lh9?jj*R5m@0_%5k>L=S|3T6c#x_Ids zwGBgYg24xnf2gn4{x0n+(PG0s?l{OrPwT);Gw~tzwWsKeOEuc*0efVRvNaTxCR5@@ z<*DABve$y(3*#uyjt6N9n>ii4zTVu;#$=S5fV-z-ct6-PyJUmmPn2B0DGCY~Q)@0?cSMTwJocm{az)TXLSYsJL|! zMSNHCt#VZ5mt%LKeN@AYR)Hn>3dWfJS=oebT3Zin z^@&ZAV8;!maXHkS086^)XjNifxH;bt+bP>kF^}e?u;v}p?`8+u=Y2!U;Zxw7!%m1* zvB#B)$}nj|=a?-9ASU(RiXlQ+WJW4{!^*aL)TQ@`I4BaN=gu#$U_gT)3nwHg@dj+2 zC3fG~_r6oRC8`uE{drY2D~Xu^rsru0&G5H2&Ys4E(STbPd&*)b|MW*Y>;xUu!Z~TD zRRoKKxxPJ=l>mKNx5Tg)H@_mqlhIZ3J?QJ40|qJ|r#M44*TOWu*DBcRpfk(q&FTu$ zZ#67@hSAc2UZHg*KQcrBH)a8CNJ-|z@4UV?_vqftuGq4SNWD?-ld$SZHrCGjtWnOP*-#wHup1fddvWl$?ILrzj~Vi{wCB_3}-@>4QXpi;`zPI8$H>zSqB0NvUD z>-`vJbj3G;M4JKSqc#sC?m`njNG@2;TT%c{-GZH2PJa|#QLwS#gIhQD=hd3Pe z5mYM2_F5+ELTNg013H5D_2g?_ZY)A={UwzVIYBs!Nh}6qxWH@*&mU$}1v^htf+-c&Cl+!d1pl9x-lg?7H8!u#DOzKI@|k zmv%*6phY0(RFyzu5*APPx|FN4@>Op7?N}-WZ5OULL0SbTc%Z?iGgITO=K(0-!}uL#WeH=1z8%%kXM>WHQ?2H$6Sfu^38wlo z3K6V-t6lP`OsAc{XyYS6Bf{%w%TQ(a5T zeq(Y$0%dn0`_!$;bEITymvk7N>GiREE@hGd%xZP?IGwZKz9QZ2c~h0bt64+y_=9Da z^LEFP=#tBF!`;Ih6L+l>abmamRRPT1cEbmJq4}NT=IuvvBibk-JhzAxmUhhSP%K?%BmEBDPQm>EP}isjem^< zg$r}_&Ss%hX(Pz3s6GZ0f7%>2{+HP)LK5-=1Cy%3o&%s#MF$*mL^GPMAff?shL#-2 zYWWb$#&5Ses*!ut-=g)G7IpqPF2j_c!BT@&3WL0SHF5n488oq+AQ%nzUApP9-3;!; zw_4EiHO_~%-8h?1!-DzQW92qW@FCpL8HIabq+SBYjIM`k>qdNHhcV&FhoI(4dsl4% z7<(`l2qzcz!U+|dN#oy1xlx=nthP>C3&ij%4)#FEAU}1-Ueo9?XcXSuovRRBi#4AW z0X$I96^MGaKnWS@K)|4iCMCn5wAT6sdT!!E`auT9{+ER9;%lTtRBzw=+C@8JVrH1{ zBMMnK)D~{WL0x+>lYZ`4G>){I)&1dUTKF!Jfj_SU*=;E~s=IIgYs@>cM3s1>>X~wq zqo(`Hpf=BPRqL#Ch)A@>FG8YQK1Nd1IC>SodD-*S3?S{Ke3m-Wr7JTgY5;N2r30ow zoV7Ms#-Vqo)@dW~Zj)=ZDHW4;XRRlOy=sH4G2$(CqzOoJ1UE z-6l_YcYvXDe8GOVP}<378OWO{0x~uFr*K=9Y7KiNwvD(bNZRX~8T4Mxb^}nx=!je= z>L-km?TpwSUeRS8)pc6}us0hhJ(U6yq6C5x(C0dG&*j1;L8^< zI%I6;S@6wprW^aows5+}BZmSlx-2j1vM275TL_1YzqH90kNsC>`Gk3g`6q2a*>OzE z4{i#Kk?Zl-W-S4IiAuta?)A~z!x#|=f-Zh$(8wDt)Agi5!3484CJ5qI9uMgiw3b<* zRtY!?37K+O!}DK=9SJ~c4<%?Jd|@msB^F=@&P=2|DJO#Ho4&T?!5v~<+x;qWC0y`s zP3qj`^sOE8=jQiv7AUaOD`1n(5c_uAE8y%7&$yvL{5YJgC%!sxfIv&)0(-i1~tM`$^4s~vPPI)q}oIiI!8Xoct08e04_~{l zl{+lt_}zJ|l0x?YnMC^>9+~>@ihcS#?`O~}$Q$$*6h7VfzrhwL^aq}G>QEbxr7X4pPSM{d z$2+tH*V{!>^N_08czK_l=?B);ra4G;*r&iLiEBp9pR10uH9(!q59&xoRfNQjI4PP_ z1B))4P@J!UygA|Mvk4x-)F@Cgdq^Y^W?6Rio@<F5?g;L*CdmbbH- zT3P*_x1W$;EXpYjn+8KgE7qMa@~7L8R|Z2NQzZ=L3Za^<{_$}rEtR4no}qI)twqYX zl*THUzayQ=rd#c`y<$d17cbWEe_mD2ZP=Wd-xArE)+h+2>5Zi_f1*$0<_thTl~qaS z_~UH+5z_GPs+fZ=qWEcfLoJ-ekaeOJF`?m#qwhw5H^7=FI%rdavZ@PRn+_))T-==_ z$=fg;0YUJfW?2;C+DD9$4NN~ZIs}GZkp0sq1duJS1FZy46XNH=5_MIptE?f3<(=td zf^gXFE{?naI(KOLDz|VHzrPdy!Y+Ba^5<3Ztql#)?zspogotCeuGZUn>l^h%E5dqj zzK*NS*Ri&31~&&uyraDVk~ zt!r9r(hSn@tk9esErFmpV;+%_fU?U^}7 zaG^#GNOvA8s-ASoBY>(0JMWBi<|NBmZS0!eHCairb_^XR)6?2h2+eK8oF^1^QLCv2 z!76!ttI0ha*2D}7)~1M)`WIpYH!-_dY}e7;g7fNVYC@7J#eU+8p!0v9g%P79?;RW{ z@bA67ZbLL&KtlNM9>TZe zlVCQ%OZM)Tfn8!D`h|zyk=T-eJdtrZSv_JQ0ZP?kaHe|&(a5OSml@`sjy9yNlFA5f z_i5-~%dVrT**WgKamdQxHVIEh*%TDbD%O!K%XK+QXJGNv`dUUUc(lCXra_cW#_HN4 zXcT8lSRO{*mFAASrVt_C2x375F>wH&c?K5lbg>4N98c;^rsm;LKDQ0|8m@vJ&j|!o zWyHgr3nv92JV$UiA_1Ou?i6@_FO&ABlYq#}I1DW~mTqA?v}@U7t0d0q&eXZLB3=~H zAdHC)EouV(dY2b3!H=xSV1~Ipf-(hHt(zaZR~h?e?<;MR1aCLe^oXNB*vu#OQO79f z4@M{o24E1KI1_3}UFN6}^gSv9Q5&pSw{EpTaldY(M4{>xF)X94;3OQNJz4JO7>UC(Ea4F|r@sbhix!qr>l|F~SF z5$m()kIAZx2#BWbc^$U%`1WrU_Znju+&X2j5LwUKno6&bd>Zv|HHs9T4$x0p6ATEY zqb(UI+;t(@^8Rb?E;ph9bulW=%!4aoF<>`x-KDcg_P)&27%UEV3>(Xf$+^KuX!l1s z1Fyv+cEQoJ9<;NgQYdb~+4HdUT7$b9A4WTUtT7dwoa7kypz!2|67jpqvxS1c3GOk= zXoj#Azqn5l}5uBcdKQ^)s`m=NcLC zJ!(Ey{Pf@?F1WJnAPFM)>su&cFMeCumGrDw(~VeE#e@erFGO~=`bs?QQ)l6Y$|%-Z z1aOA4mAv3uCT}zRF*6>c+D_lOwoo&R6 zu!5&HH6@EAujtxJ2uPd^_PamBP1l4}XDhWI>`8+1wI_t*KngaYn!3DjOoUD33>u}I zEMC97NVll(aVp-{jc~j!--T}zcs%4NkX02r`~$4z8hYon69@+s2gt^7G~@melgG`8 zM=bbZn%6&kcg}bwe2bH(i_JSbA|(D;8-2Av+KGJtH*3qxO?LsMD;~Ppog8AN9^%$c z%9WmU(A6h**apPXSA61jf2o%R@C7bGBq+=QEO+8jyOg-~%xu_8a(N!_9` zAs~ppca=Il_f>XpltY=>X_|>uupzz|qtDpD0PEsc+6AQFU$4p~N~*5;djrrFAzRCS z+2I(JSlW!6+@Ji@mitlL)$<<49S( z9N?)A^=ZzBNg8TfCJ}emj)248&91KGSS2}?>CV)?G1RpA6m;zm6_9(!Rx&{D~bZVQ$xeg*260Pvi3us1N~%*DnBStf_%=EVR)yXtb7fr2*L z*Kn}F$65?)qe_p?6|T!0V(M#Bv97;vI&i8jYpR#pjL85vIua$7I~}EK1&AKmeybpz z4Z9npeocRyla&iDbKIYl0M+puWP&{ z1_HAWOgs+R02X84hndrgXauD!T%!jhqSmFPMTYR%k^Nkc?QPBG%-dRhm%hF??u4&w z4omd@T1ozY9$)lqtskZM)ut4fjIHeS_&4t4S3#%9A{m%imCryH^Ap|m*EEBPg{zfU zFBFYft$wF3BklycoDZT4qf!JFwihNi8T5+P@Si*+99vfF_)LPkjvVuUt=IWg_{7gE z+eP}Kj4@vrbHYWbs4>e_ZSc!4j9(|WP{kDLci69xmdbkI;J+o@3KB!;_8eM|G`xWB z1R(GWDRKsZ3f=Nl(q@8>(*O9|s5WDxN&AlD6oKo21K0tfc#tw%FZvlyg=~hr^4xIz zzn3F6pTO;BX@@fKuXjLZ>bb-$Eazdy#NxD}uFC-jvEvC2(hk#=h7zv&80HAIv?cOq zCH^+Yob!Xva-3asI>U=!#}^ThOCm+14i=EYhVK~`b}Y}MV%^s_LTdNCxJ#=(-Sc>; zaFN4Np6YFX=YbK^)?4C^X~i2;g6O0@@W`JEGi_g%j@9momNv|_F)*`ktT#Okq)&Vk zr+e4m>Gnh%OtNA$TCR=2;xjWE;%)W&5K$0v$bcT9HAEYIYTXxX$VRzew4o@{a?Zzb z?ulS^>JO

ZnwtSqv9-!#bPLkDe`HRP(&sE~v4*&^~xv=AS9CYABp2&xATv54}_O znxFv4t0BHTO=bPDP;t7$rr*cJ*!V&E~Hz&06K{qFF z&sQia(%+NvSVtX^wVy;C!L4%ltGftt-05erEwFrze}gu7!lhQ1 zok{0(EE-E#sws)O0u?s4;2(zX2Nhl96WD?JVy|a%I}(iXQt8Du$Oz8Qw~CYAKynng zpKGHMEP=b3Pn(vw)S5MDRiOZmh9W^Y;&`-y&2@lJmv(*hR?&j-VRnT!jRu-X0|5*z z1j3yo_wTwtIzOQ__&m#!;-R7Bs%LO5@o1CL%YL3MnaTRAFK0#(nMcUzY-T`mJ8()& z^a*{X#}IbFLHBpwyJb$T=>cXX(Tb5HKB2y_^-;ZZF&e7dszw^r5q=e*3MGgiJfd?8 zM}Qoj4~52M$h?y+P7%;)UM>0t>|qMI6V@IIdxsF9Ulf0@#(03gn1;d;Dr z&_^=cXa41%HXt?JTdZFCXjp;A=XT>7T2~1~&oXr8XtNIe;i|w0=MC|0iMreW?i~}9!7ma{QRLi}ZQWL4eLP`3 zU_66`*nMhCEWSGr&XXBW{{%`!jZ-R0Wx$S>djd8{dXycNFH3FRGIhz17Ae|y7E7U$ zgJ4HZKT$=3qOStD_(FA`Qqhnxxo$?&S9p&hHv!UpNbFw3iI9p*n%NDijxYJtaB{@8U_^G z1JY?_wfOzRO)v%9{{1Ifr64kww{{TFCExbQaf(;XgECVydfGgHhegENs0e;c`kKLD zxJNC_ATF=U^>dxNn>@?%?F8VnY&$RwvlFCMsQGG}W3#PhRUd`WBh&pgjBt+VBEK+` zgWj-2@iCp&Lx)IiZ%JIWHPUig4f_Di;y>S>(d4i+%gNv|#@=$yf*V5>`?ScJqP8NE_d7hUgWB2B zjNct-;@TneJW!BXvYIVq46_IMooVePy=pshJQrJoxsY&G5|#RwF#KH7)XVv2OJg>!+$GqpYD)EpI@w)0KOA5W`(rqzH{^g0JS5!pJ zD(GCTuq^6E&|LH^dGCu577f<6W07s{bO7pnG>41tLbJ06eXgPg7CBKs6!H8g zJS51(6LcJ5duvU%qt@c&P_c&l*_pq71BT+&MKkU=X=d`P$@{z;v+tUZ)<5EJT28{F zQni0}lB3>vGb6kU>K7>n`jsYcpfs6syW5jE0R~}ifhmLfPt71{bG|p%6(0Y@%IqSk zImTcYup@R*4o?`yY?_Lnb`b-x>WL34T<)|ZaS~=dcm{!VF>vTWB9pK8XTsd4*WHG| zp(L8a0*d%I$|>BZ+5wf;v=^zu;vmW}fp=Mjog6j&&M&wv* zsm{>gbgMn6#3-MGVcP)Xk_H;T6)(mZjNkL>m3#2C>zIyDP}+85nu!NG(N z(R92aY^5h^ybNChC43;24LT~=e6kxHUMoWP=R6{T?HFF7w^VCenFju7+Re@}lJ{;i zSDJGjoTZ26t=>o1c}JJc)y1crfGOmJHh&S`7u8LzI*A&ihR~CsKPjbrnE;x2qj^IT z1j$5S!a&F)-C1(EfoCfCM2HMWUT|MpBSn!DOR^Z=OP_C+Jf1i+=-(91kceg~(lPmN z>tJyHF1hD_eoH#RYC&OF&UEvE1t{I-&jyI9hS9riv#|}Q?tfbaVoFxe;VBq%P)PvZ z0J0nvAI5z=A$4Vad?EN-4BJHVhs7B%BUm76DzU&0)cpZBw472o>nskwoe`Pv)M@^? z$#7m#*Tarn^Kee(zuwE#XELDsf~MAVRe*`n@^hog52vomo2oX>x0F-L&f>|TR_B5> zOf23EWm2Wu;JpX}hSP}O%lcnDVrGW_%OhrDVPfX^Z-n@Ndc;gD9E@!LUyrz5Ii5iV ziT0m8{Es2V*8jI`?flo;YJ11e>~=b8XR|DQXtF$!g!!yc9Bky;I!lH3LKfW|{Rc(| zWYS2gF&YL21_b8Mv_f(5U@RHfEShU85!fu54#*|hD99w)3rMsKhztx&v_dh+pz8}> zESlNrEC`BN@-G}xxh6J-_PPcKQu&7FR)-cw2J(G-XD3uUSI0bO=l#OBa>dNrKJHAnAtScKxVnI2l(r@w>7q{bg z5F4XY^Sinm3&`wmQ#I_5Ip)vYhyLoqH$NIK3Ndj&Lr4O1KtV)C2rbHY-=Cyr))r!e zPxXMzyAumhH18Cqfu$He-k#B`#8K%e|zKq4NV1gRD(bKpzrUR%WD#H8bWF? z0~-JGYhiI<{nG#23=A0YYdz~T`RS4ciT+(nV|8$Cb|9ZO%l};SWca%JT3h_?qaY;Y zcB9u4gOdZ&G9wcM5<^pC1Cldy&HF{yB}Nw)N7g>`gZ`1H|9$@Lksu?xB@%5s;*5#mgnx&{R0n z&NkvCY>4k|e7yKR^H^635UeAK@fc2f(GlxdYg3Jg0jN4$4Dc)C-l_yL?`UG;inM?# z-A3?!i#2Mw+=1LG8GbAz$O6zYeAz(8i$1>$STCoqK2ZO`;lLWM?{Xj(9E)5f#mlUa8WIaBEAuKX?%$FK4iHrBUV8c!b(6NDm81j$LDx1le*m|I3=rctZ z!Hj%mLmGL(#yj(K#Is6>Lw4gS=yixQ%RrsUN*aPHh*|1Gan!^W1HHYMQ(xNVc4sB5 z6S9XIOakaT8?OiWLwAN_%dPImw2Rss8fln$^xcT`oxxkeWD+D|%PizIAwSP7YC4)F zaD<6!aP|<^19jX+td+1`RLY>y^~2N;29lS24THoGX&jAo&IE)1YR%?1EyfyZyQvUC zw#AF^OwAogV2zTbhIlVLj|0SB(K??4bZQcSNP(Ge{@@fs?GNcuPzXjRcD7ug!>XDr z1d>O&&Wyz}cC_wzRnOkvou;$Ty;cU~GS(bdC4+KDd1&VHP=yDSATU+&Y{F7c{b9qZEb8gB&1TAK>f)oe_L*poEX2rqXkij zp8x}7E@*-c&mj$cC@UZ7iplq|`?+;=*AS+RM^bFJg=l_JImLoi(jiSG>8%(ihGxul z646sCqwD?mmY_nNuF$Q#DnQ1uT?ckK>`1Qd3cPWqlXpjuqY9pF@^&*J6bFO3iG?*#Q;gjKA z8GHy$E74!M-9}?k#ZdkxTOIHgGK^ezn?E*9xVCM2B-uW$+DbjzW^cMS&0Mn{9br2u zYFRL)abMF7_iYcp*t(jx47ZZl+d}rT1Og5H*S>u22azz(j%*hXX1&aYR`eZ;ViKn7 z92O1~!jP1j1uWzVK&&hHlLy+ah-tNZG-QP<8z^PviGgXip}wktt=xttW<2Z5B&KWM zO3`f<%QKk^={DRt(08f#_>(_+*kif1R+AyC~ zcE&`H#w6ZWXmG4c+DYMYS;}P^?f?ZsZ%ddZKqPzoH_BtNIz5&c$l-mNNtt_6nWcRPnGnHLr$HJeFhdDVCt z*7(Dg*@&SSr96CwnZ>6uP;*I%cr6d+Es4S)It`&gdb2K>o$SgB<+wB?eIZ#T^bRQ$ z;eKON8_ZPC>-J0ek6xt)p9GJ4wG>9t4L!Qj1R6SK&q2aQf1Jcscwifw!-7KqBcsS# ziOMD6DE@(m7DU{xHa|#Xc__t@i)U1BK2XbC}-7b+~;|(vMBtZh|y-`jkntt~%Vn=^)EwpL^+|&+$Jl)Ti z>beF=Drf2LnxnGvJXY}3#6j=T^354pGZ1@4){7^7-F}~hyg-6tck5MV>cmMxwOJhg zG%swP)L!lb5r@DBa*EDDVX>=;tT&3=mihwxI>atDDuYm&D+gXgN~qRsZ%a(@Q$qVE z7rh9hm_Nw*iR!;=1czVy1mYZDiss==-sx8J9(v4!lryKdvHQju%7>IhAs zvU-QrS>Hz_TP#EWvTug)dUh1J?z0ebRX#)=%QMT9@Woh+>K1=@<@B2hwhlp+31qQ4 z?JDo!s{(&&9;^KC^X9`_y@Jw^D_-3OBysTqvI%0ACczsa^f`atXq3z8XLrOB@W4Tt z247?x&UZ*HSLw~rP?BUhk%^7m03pd?=ssU)q=yq0n?5 zQ1<9j0HA*$j%^rhD6$FTd1^p?bs^sUZ@F5u-5 z&TYeCHwBubG)^6*@PR;fN=r9kS0iD{KvqRJzZ->RGc7p_%*2g7!E=F-Naj%2F@1;Y z6VD!7AeT0oSHl zHqWkh$ShAus)NJi8hs>f$y5IqNEo$Y+;Gn$R@Ft^y{^Vp^ES*9@04e+%fFzq;GQ%7 z!|H{n;pQ0#L4=9-(1mZ7uCSx9x|$YH6cj4Y9yHHW(zY~MuU6L3{8BNp*^iJ$ld28j z+Fvx`mMq)LYrEvid3fY6YVB#U@?*Brps6;BxAd{a_%ppkC|L>>G#U_C2qhO5uCLz9 zW@EKAsHZFQCKc`=m_1ZVh9X0443fL1+h?7`cNg5-q+K#P=M%Nrn5JPe`i)V|>Xq_H zVj(x4QlFL{|DAyXG^QHGZGf8~2lggWs8ly}akIVK+gnrBcI0g-*dVV;>8mnvvBFCQ z{p4#9+)-K7^6EA^no=)*l(0RWA+glPbO<^)No?-JH=ju9O)o5{TY!uJLjVn4~93?l`&TYn;2CN&b?kM6=QFh}5 z(0dcfHcc6{M3#7*T|Is)67^V%-{GXUCD~057z;>iJcn7^TPhn*5afm;wx4kQ=49m> zm3adfJx;`Ys1t6vm9)@Vm2vrFsO&>A9u5XK?x)EMJWs?FS(l8M!$3jEj-Z1(qCrCI z0@6M`gdyE{;#J4mDhzLnr%biPP{kTkp+AM^qAwTFSK|ti`uYxg@!bjf!s&qk7*HQJ zR$#cL!VW;8BUz`dwO38$U6kHAc`UEDl_5gpFqHzVKrxfZUieK9=PJ+pe}q~mXG3LC z#vP@cJW1<&VOqO&X;S0kzEnS~F+o4!7Ho`n-uChMnfRNz>c@*EJMBFas;6T>?x!;W z0s5-aUUP7Q5C5_|b=v!T#CgvJfVroEMA^mR4olhD@W1bYni5qs--tuZctvDA?)K?i zxLNBGz4KzV0V;rXV>1YujN))73|$X{8L21b#S&M0$y`LS!nE~R&*{_F z_A!wf!PkkhFb1v7&4LwZfFLH$Ftws-j^uA3Cm_}prEo-hIRjUkT}kG}p-(H{eaVlU zH?5`RIE*lZ5OBaql9J@w4nP98qd^VcChFF?N)60|HZfvi{DUz%eB}4&Zd9Z!;8<1S z-+u|&lK4{>IgE$W5Q+@-30e#)VUN`3 zdAaUnkPdv<^Uu`W!%^faj#$K;89*eR5Nd{h6P{>o+bLsvSz1i#*+;S!HeS5`^sY7% z-2a~70>OQ+^4p?{n#I?P{Q8mPNetf%fnB%W`E~5``n%4`W||*TG%B22Tp~ISP{M<9^MFD* z%S77y*90^(>zQ7cx%@3F-fwmX*Dv%ev;7an~0%~*5)3fe329iu${=b%Z zugh+(+NfgU$o(PyRL33Da`u|7bv8Opa}mgjeu+@9SgtSU|FGFliY2@5wc%?+$Rmdx zQtV>2v=ALCe5p|OHrxdS@9+vy?IAM>c`F7!-DO`+|}HCbPU6-qKmKuK4ZAFhg<3uO=@es_VUsccH|>( z<0k&JIS)PWpXs_xTwf3eBI|^Ez*Rm~f1D0J7F11BYL|Dd4L;=HiG;K=W&e#c_2p*o zf&kbi+;o6}4zqJ;45!ZQ13=GHuI}FaNk8TK^5!(ZMrpKJ%4hSKuhUw<0rF%J@yw=Q zCko~;fthcBEyVbZ0G_WbtUr%L!n>&5>u=U~c%Sx+q%CfDsOdZJ`t#ADg&UePZ=? zw~*0+EldfgE?n70{TvuiFKp-Sr0#DH(Q(Ja;W2_l;m?AET1KozhH)U8v#L0(Am1Tc z%kiLjwO*4THPAaEP={Olwy-={a~cg%4BNF~t+$oYl!o_RGG?Y?9uR4!;fT0e9S`Tm z&q=;69#5Cll)EfuK7fnpi)-^xF1AW@=GR6kq>xm>d%M1fG%P2F*Fh8RX zVQp&Wy**M|viqaoE(s{;XFWOaqAV&)2?p>TVD1r5=@556aQ*jwZjk?4Cd&uNAat8i zY*I|zD3TdqI=+YF&b6u<9qc3?GI^pj5DWrwR-W81d<{%sl#wy~v~+vgZ0jANY39ia z!b+?&ND+-&k+OHV07MZ;HTGl$=W>wj@nTHZ8N1}N5QP1bHdl%>^V~M1PTRJGwiN1@_z0*SI_FWT;x zWw;QkR#7tTUj?zv-Ywur;jMJ0?!M;9Q;zE2@f$$st-R=1VT%=@X+V`8F$0&uE}Z>Q z`~6wPqe8;23Wl(LOp)4^lCu^{83*M*9E`jb{T078Vi>IzX{TU+wFiwEDik7I>i`C} zq#4h)?-Rb@e8zL+enKq;`Rb6dT}nxZe?GeaHfIE^zKg=fen z!cPE$^r>f`ug}4}uXsrM@2}($t+LO!c!tq#NUaP;u-ppYR|IJjX)IU9kWfEmNGs^w zQlH2T6R->+ID6(EpAu0U5mziLAKH@0h8WC_W9XR8`oUn}d1yx#jXW4=L(%ENA&6Lu zV?};S4E5HbxZ}o$bdCuP5z|y8SLAop61$n*;5SD*20qhh$t@`ye(I(E386iFWQ!^z z+lJ<2!LsJE8{_Ju;>ZZ3U8=akKSYvg=p69_wfl%%LZf(+>u0O)I2J(qz@))Xc*VE%p}Sb!1+&i^i4xk4`wFdok}wx;|}wKj48YxuV}V~dM_!gi`!)i(tvk> zLM?-o<>w1q_}D*F2a)b|_r^Ok{W#=#G>6T)iTT)D}$ zA(YHEne1hv#}}gO6c1OeK{~?&SxtBi)lQsG&To}+%_=J5DPb2-HXZZb>&a^cq5Y7G z0&B@d%Iau<;yZU-6c5$jlmZ{`{ziHoZYj7@y4_ReLvR{ju;>Zqb!5Q;HRVGe%)kWK zQotf&lJ1K&SqHiO5R&h@0oq@}+sJD>Xr5DNODG((WdyxaD0 z_)@ehAn-84YH(nGLnTjVHLig{&cDv}09+)YXfg{JU8rj2W0ELWaPR%Tmp0!kodBXF z6v_5gaxCLg)XjQ+15ou5%_h~8#;`ls?J|;DfypvF=qc53);nDYD!8vK=4`fVPM$4< z5b93`L{r>qz%rerUh2$jbut3Ve4zZ%?V8u7!R9AP^QtP1JGG+;=>GYtnq3 zl*@W7r*BUaB2_&I#z1{*=xtWnqww2=3U=FDrA3sKuzfjrD5+dn-> zVQfI7={Ll_g$4IEFDudIEU{GkzLYY+FtIHa6?Q-+{$7ZmE^5#xkLpir6T9`K`vowt zkXS!p_G;N6yN^?MwI$-R=qWIDxX0cQUfWGn`N>&|Ys_zxtOX-KxVMb>QkcTd^_y%Y z?gPbhwn{HP2|TSCUxno}_>xNB2WrKwC*YBH-@Zls!MGcZgZ6lZN7SOT%B8yDdc0E? zjE^A5#J=kw(*vIG8C}DFHD;A6T=xyw?7Sf<$3=Q;0Ho7D$UN_;tN;UL-3V++wat9$ zvo%++TMMO|poNS31gi5ZMpaoMt?C8BDm4{VoU!3T!?hXRn@Rj+dJb$C&t!pZp zE4^yIgq8nJ>WSv8cI{Hqsw9yHjh_0O$`-=!DNu}s zPorCdPeW@f zZ^?i$%6Lv87BJL`e6+fDA@gn$Yhoekm8*H?90V972!DV~zX<`F``;LwhAA3S8F=BmKiBv~G)- zkO7*-e)X41lv6ou#wcnDuKyc0;mzS{=ACn$dk%vNGLu^~tR@zUWD(B=ef}sippXs+ zwlbV~)dH^y=~k z(XG%S{xM>*ns6qEl`@8~+Id9NifX8p)omoP&aCY-&vaLN+fr+*R_tFxZ_qaV z0DO|ReNk{hi2JjokpCq5;%?~cs8y^M2DMoU;;MSmxhmeoc;4KjP)lAnt_6DcOP| zgv;6GlnN(VMFw=?H_4Lal9*Mb5o55HM(!xgv*^mi!sAj@)ptqfa>#q^Sfg|A64N|W zpSdg{N$SeFzz;J5kD=3&Zp3GyMuI_iqTaFEsvxOR5~2{?YoI=KK%tDOF*dE>*?&)6e?c^~ zty3e>#{CyRKzL=QsL?{kam`W^Ye~d%(n!lLnkFz*&UH z)!PIQqY8+*V+xTqtSJGR4xSD;>)NvKG-9q>-mDKIy4lpM$mr`23huL@l|xOSNwU-d z*lY#!5S-+I4r4E0~%@-b@ks0OHHCC?1~t=4TG-;Jew7=GOkDOkg!CJPLj)${XFCACr_1W4BJm%>&S_Z`Ey|(Wwr%5V+qP}nwr$(C zZQHhOTYamayQ+U;t;|%Ck!`G41UFM7pf$m$kYMaN?hF*CE72b)UDt}*a45Jgc3Hq; zOVuw6aW}7nGX>c1Iil7EWvS5s>H%*kG`#MO$nisCSm|}{2xsM~&UeLx_(D>H)-0XL zHSsU*Bn4>lZ<>j@kf9CL?)@kNoy^z&S z3C1YY2dk@33nTc;f8M`9Od(q$3VKg7S2T=uR3*TAL*hkU2f@E7lG;Q$eL8r;mg(}}JlAp6)e8P~oGUm;)S zVu1T@C&U;*Vov_^zzTPkrD&Q4rywvq@t2WZEmw<2;@@NCGL3r9d17m%Lkp2Frvyn0iqdwNKztQp~B0f3Wfup7<0n=}!+%t7S281sV>0*_eB|Rk` zVatQzDeeE;D8_U#kl{ypGZrnc8Ns+cXCJ{ARcO9AcFI*66m^@~4P1bQ7de*N zY(%)rtKbLtZ0SZMni(QV7)hBH7~UO>!Zswd6Hx}6!YUX1{`Qp7LPn)%wY9y|7b3+M zGzzQGzjy0s?nu!lmgs$c$OAPC z=J}vjJKx8396DDpfSIb>v}jeW>u65vIXCmsRcyWOgPWA@Y2kT!g53K+6pAi_O|&N( z`fQbZF=^!*JrBZfAL#SCfDfzKRddFecklJTh3wH8O(tx*vhj1dOvnmYT)&M&vH5kR}4*Zf>c%DpW(>t^+SyR;k=z@IK0uZ1k8tlV6%J-n6Wu!kz3abs8VP!7m>dv zdOIt8?qN@nN;ODN$*F}_U_7V2imu=2Sq8vWFA@~EDDriy@7zZP7z) zv8%`b62Q0RW*CSAA3G{*1n6Sp-+g|##3h zlcfG)aY57wRBCfy{p_v%hbIGqja6Zv__-5?wB9^)O*_er6_ZD8f&?U(5AIZfqHSdV zBKPs~TYPsg1iScDq|@Kt7Awy($bYX7W(UDi_n!?DAGL>4!huWS~n~u6^DL7eW53v5*7?ZmB#BBQ5atYWE98rMRU^sqg zKrmmh!WHwCV!%f4_{>`ntO0XGxaWf!nNk0mZ=l#6^mra3peSZ4&84pU>D+qqORT38 zvukUO2lH*dZQxe2a(S`tQuv;CsD;uDGM2==2J@>?317naT{s}dr0mX%tns5)p68Wu zST*kRYH_{;xe;6P$sZ<3#fdZJj8q8YTPI&MLk91@PZyTscqEe6NN7ad*IQ&0k2JMV zVns;DWV40uDyC>QXS|I&BX@+!cGw+J7iYs#VZm03Am}6|XdW&A3j=+MTJo{tH;Qxy z0fa~1M9&I`*!Y_;n3O_@_|OVc$bgV}xUw(_Dbyn5_0(99Z0YnGgy%1F>H(=yKhGaY zqPG7h3N~O=J_>M0V9>$4=#l}-DOx~@UMOuEzNTZLZq25hp#YZxWbdM1^CAMQD7-2r z4pWXOnmXEZa9Aw!$Eaq{mI4-wgAFurt8uN?-DW(vkG=qcg9}3wMBNhbBREGP4c4)6{cOhn^sh)55^!Tw^BoLYCh6952CQ-zWd~hn~%lM4SN#>-- z-{p17#0i{I(}Fnh^oLz8+2j}Y?d3`m5a0r#6C2xM2TFBy9pM{;;9ey4Ny;U~W0 z>)O8QklNh+V4I0k&zX;KgbVH1HJh9oCb4L+} z+IP%g#S^UKt%`%0!PGdRl9l=8^Q|JzdbXAvc>z90?-FKX@st>XE@8n=N?Bt_s~Zzn z;7O#4V1_pczHE7VdewgRV5<~*4> z7bx_M^#VoY0m3JY!Q;LgL5F5St$Rt@F%TvZ{8GRm zTGVDvmNIhe0{(&(C&$+cfNHB0&c*m(OvkfKb_X}8|Juh=GgUNa+Lrzo7F0ymvAKKi zfIyx3YJiw_6MA0Z{bStSm!l7vLHjR)5rGBY1EXaM!D#RMi4LBi%6J;kTpKziJ{#3sma^VzQWy8oi#$=n-x~{ORYHbeANBP%wct||*ReD0a zJ>;w_(~@EYNkhpfh8&>IQ`bSa+?BY`<4b)VmcDZRew}Msq;qH?^5oe8fm=$-l!Oxl zrS`}q+I2(cvh3?Iz5FEMasy^@{v^K4zvo47>-? zBu;xH7yrY$RTPSPp5Ey(aT38vG*$h@9FN?%QScS-eHnF3b% z+@uT~vbWYz!ID_Dk4wf&N)z$(BuB>v%O~tqo~Kc9i4wpyZH1@Zd+w&T0UC)n8bup! zi%Dh1OrtEJg(My@uq`Zng}KWK$_I#pX6Ct5a%qbQ;B=hZDeTjnla7DWH8PE!8Rvw& z5$Dq!Zj~zvux|Ewbxqs(gdTF>BI6*FSAbq2S12;>kJ;C2g8S`pHjX8D=E@-#(Jpz6 z%4;9<@C}%32bt};PzDJXbc1s9ZV4y1+tVkGGTn*s;esm*@vx@cA95GCd36vejyf6O zGFFBO;ogW8qxD%O;E1fjFbl5x1~+(@9D-NWcVq5Sx)9^S2^Cq$CrPfi%D~{OqPw_M_s(slt{T?V|_~ z4uod>tw>gj>slYHAmIz6JbN&FJlbse1^X4rwA~n|^!0f{D_{eiyI|+YK1OqW1%1oR}HjnC)5MZuTilm7*7Iu^h=L>WM?E?Ix~M?(?QKl7$K9T z=T_fcLRfI+M``E;3exXk<@0h&Y|{Y>wZyU%Jp)k|lbKs6l-CzE^*GvjzC?f7?O&Ob z=Vg!xc5FEZEqCfN;lvG7GC}P|`}IGSj0CQ&J?!28?B6zY@wBR?FWF^jJD6y3Z8j4~ zT%5V_|Kuvzn4<9P+)EQW3!$o6R2@HaR(Cg4K{B@X?(Oyyst#|8lP88Cu0Q)CP6Yrl zC-^)+&*aJMPva?CcE*t0N7xR?bw#Na1&E@SS8#i6VelV<@=8K8fw6PU3POpQ%DdZ9 z(g`(Ll(3?YK-?LSg7iefpKsCw@3^E9U-#6Qla4g?B5;D%RR8jYbT#>Ze(31agD+RUA|TJk6%fp1RtK4z*u4tLz- zR}kS-;C7Z6K6hh`@$PLvn0M=~?-&G6EdIr^Zm|kU2Xo=7iA7P>&TY9h8Pd(7>N*MiI;W8#ntVh(wxJAY!ObIxO9TyGD#xJ_bDdS-6;l zwQlU4Mk}V+*V~FukV8F(l%2|M)TDij-fF`-dH^NzmZ^Dm;v})_q_2E$jg_Z(QTqFc zJ^)dEMuz=TD)5rzrV0s(4y!4*X~M#@;-`-C2S1Apy-9x#oC5=OYy$nd*TvImhY{h# zcy3gLDZ%MOMJe9RW3RKfa_v+;Bg2}%95$4VY0Bc z`6-A@;jtoip11FPXY^0R?^hZ%6*2MCO{;LKmpm%N+6yrBS%X@Y3~j}oXiIOh)rV6r zrv>(JB@R<^%`}&irIwxKWo8U!_JxnBSf0u(9%l}og8~Tk1xrLL*0TSiJZKN|4}cb| zD|Wl@l^8OsE-Vp)>VBvgutKJz92;L?7s1GHKAuLc9&w6sM5 zd%j6#IB`YDTt3XoCh_Ka2lE_qe|(CQ%#A%$sfbQ)Onmv^N#CD$s8^scRB#9)jOcPErMoaS%tH* zow6yL3J2s8jvccZ5aO1ynoBFrs~uS^Qu^EeO9dsK5)L0*IKGG-AS`P22lJnXLA&p- zy8~&1IUpn5mMc=aOCq}KbViEWzw@p1%3%E?4)mp(Krc(kO(9OHrl}b>Xu{E5#<|sA z{V1Kr73zSgO;25H4#_olCb}slM%@7kxeoLtS@M(KUdSr6LfnIz8904=FzaXv6so=J z?n-plOH^3B?cmb{;AtY|+RBoR{fgWtDTDtWd+3KcPynym2Sr=8rT3&`;M+%1(RiBA zyhEAAs|VhM<*AZ9wm`Ihkq~8mS=06;bgxMjEz_8-9oD#bFWf@~VZY!j@7iKbT5{i$ zxJk@FSmfk!YD ze6@@2&kEW(rRg*(lQ#@KT?6;v{RZ_WEDMOeKSWPZTv`A z^-fGZ;H@fnK?Y^Q>Pw;C4&= zK%1`2l5ROz1^96NQAgY~m32@l*OX0;Pw(ytP~f(bA**Xt4oM2YJwcHxnT4sBLuz^Z zw*tJCv>M@ja*nXn4h~=kdGEs>{6us+d#5{1UbIFka3yhmo{Zk*irDyjc-;HhqdtFf z5+0j?Kv#LwGGE?bj(g)t@cUg~9D;stmLtt&zpIit`%Ge^znKe|>_v4fn&AkRQ-w%-VVDm4cax98SYNS^SRv(T zFfdABv-d?GfOggmTWe=8&^Oo^C)sPb{s~>7@fs*Idx~TTE>#aEE*FsR%cU*%Ekjry zBY2#j&*$HREdvcT+FA)e2C<(a{;+M1UokfBmk8rV;a=N6KELft&0m}6o_o6~LbOc| zM4T1^4yce|olzU!!C_yWH2@?RJmJo@t`df5wt5in5rF!>>+v}&J@yt{kgJ5Nqg!b- z;YKTUqZ@YLj<7VS7=r;Qf@X0GKQu>`GQ8OUh{O2PV4z&@o2^yeKl5TuqvCkcb|MU& znXCol$N)&_X*LwA5LEXF9!fyVK(^#4_+~^6C;4!p12Exdda1>!@2D+mck;e8$248M zp@6Xe6#3w$+5Yn4D(G5k1Lt}S&;O3LSn`K+R zVUK>IsD5#g(nB9nlOHJkOZJb*UxW4OH&@MHbtb_hc|VpUma0qPc%l;lUBnT{V4YFj zd!mBzDw3VH*fKHkn& zw}q|O!4Qu5edGA1E8Ky^))Ku@nX4xVzd#k?f_2&B_2SCH8K4(Q9tSOS9=iHzD&5zX z*8d6gJ|CXhAnp4jZ@2sqc?X?GnI@@hCR2>Jb88NCPP7LOepSUvP=CCzKpB&iWfU;+ zzpv=DK%%W*(pnOi0R49rqe@U2eDAeVzl;M+q~q)` zL4U}_VcA1zdq+D`I#L!B&DD!^qI4?l>|Dv?u}%Ad=a>yTw6=X}>~`ol($E8RPJ*!q zWiP1nipfq@!;MI3w~17HTRT@zYraeCmo~(m%1%Q33U1~U5)Z0bm%#P@h26~5A>3jg zF*TuzF~WR&FyHN~YZ7NYj^1-`3jGXrnFpKsr9n3{mk6g7GyPYD0Gki(kWW~p0lLiT zw|l#Y=CQc+FcgNa#)~Dp>#TGsPzs2~K}UkVsDae_1D7OktN%)nizi?z@kUN-xOD?l z2`Gqr)URH!1%Y)MLS=~YTbJZGjW%`X2-n%EY~-T0Okb361d5}NI}Q^1FdSxQdcU*a zHn?Y)8RzoZG0kn2y0*l}V%nUMA(fyu%kR{$P`MX8>f=_(fwq>At(o8rE(vp6H;MF^ zSaUniSzd~+5+DwOR8RhD0A<`(G4F^dKWv1Sza#C0reL2tspV$le$DJI90+_cg#$b= zI@=%loCM<%Td096bf6+WM+Wn#G~u~8gZk*II8@Kv6fK-CqXmp1(^?oIlZ7pDZ%`*% zK&-9fM1}>pGH37uEZAtuVEm50OF;%k`AD?Ks>3^=$HZzxH?QoYu~i}0;Lbg6DQ*-M z_YK-|HNR&mY^ue5Y)tX&uo}O=NMVD28pgDcWVMI8(fHr-N#G7>yqh9+U~8&to;DbQ zp6)mZi+A-F9y&3&i!JF%FjD%#OFWs~_}?hu!G5H=FzYHV*MfVXLJIPIBLFJ2FVuu_ zPJI{2+j&c+)4c0=LGr@%{rV%I^ zep?odr>^Y@bwC^g2$md)`w30#mEQhS09MD$XN>yl>)eBKeoY2m+EiK8b}Xc*gM6nS zZYL`Pm7^U6Juc*CgA&kmN#h5RMpO!>W#Z&WQWXKzUfQ<7jSioi9$%3AKDzoC_9ZVN zx!<`b%0XyzE0S@n#<5R5yeR~ww=IVdgYl`t(FOr~&+8skGYW=YXtRroQ6vUhZB3~A zg&7JlKnBu^Sec(mld|>?zy(>Z&FP!hb-W2Astfxqg9Yl~G?T{AYQu>fCBQt~S6C=>$#aWim+2 z4j_NConB>AQ#@~O_m7m&`6Sy0zQcF50cW@8RS#j1rMs}-h0i+h+1dx`;hPSO%U1(i zwK2AfpN3ShNvL)YoK=L_`(y$qJQQX;V9M-&7&4)e9A4UhDS&?XS^B+pt?nkj03iNX zm0<-TSDhUq7q`oE$=+6lQO!mI=!@wYK8YlZx0WYAk6n@qL%*2KAaFMBVaFVA+JQu! z;%$~|czo_03~g>LI9PnpcK3+hirUz;#Pm_;1r@bL=o}yRf)^FSQS8!QiJ^x@FE%|# zicfwBW%~W!<~myc2`r;wR=QW3>IRK$Xk+FiEnP23<45h2IPCoYl=~KEpi-_zp{=^< zih%?^#UgP(ggOr}jbnUGVLUg?SXlNhaYV!YW6Utg%8RK1=y#XZ)wmSp@RzQqeS7?E zx?EL5Z>**?YsPhXOjAA`dXpYhqW!JyP_T2qZ3o-i>6nq6cY+ke=DM)lom&<1;#Wu< zG_L`*PAZn{RIwBHF|YAPeN6++u53?E2h0)yjS17;+aHJ`LCqe1?=P!_Cbq{*4|)kA zl4BOXP#v3NYU%08K}LfXqyClYT%ZSwpsVc9r^BT<+zd728p8l^D5uVYm6eUy9qnsa z7e8z4_(RZIGUw zX|-;cvDbYgb%5zF`bjZYKdEu#f_;GvL` zES}O)xLf>;h;9D5u5Ain0dFnK1W<^j524Es{P=iaU5}JKia>{mXykOo9m&C5KK$V@ zS22OahR$1ra##|I32+Lux8r-POkHf!?ky42&o3{rM-e+)@u^~ZP@|zP4^*w%MVNj* z1bn1m!3r_UtkKD#d3Yg0r?i3<=XZoSHvhj)igUFYaTnj_BO-F@DBSfUkN69UBKKPm zSD)@z>dNH(?T_2~fIYIj%ND2?B=Quo+ri^!d{PfoOeJMiwV^@Jz)J9OJ$Z5N@L$2N z(d9?<3F);6W&qe~;p)!Ms-A{4)p8^0U3zi0Y2}Nd2m>j&wfh4t zd)a^^k!D?gEt2Il2v@kKDMIC0*o+1XOl1n`1pn-syD2!%J=&1gL>oqjk;Vy2L8_M@#*qcGn z=0@g&=%YcxL@Zg609Q8t?h5p2xc#yRCnTxT9&zqsjOKzp{m~-`QhKuSl(mW^#-cUw zcw7Tcer;|5z;<_q*A4_d>_}&Q%s6*bmaBvPkE)^4L%$T1cC^2w_AzXuT^e{5Zc_mGsg-V?BFWXT46Y zU)z}$O;G~~ow7i~5$i`nMS7B~kW(2BaUOevj+zE+BD9=Tp-m@+J>x3g>)aFcyG4|O z`TwdY3H>~Fy5oYt{o2DIqMQENB8Eq(e?CTQW10IgSWU~P+3fgdy4`Gdwvr_($(h>1 z%~1B=J)`F#gn?UEh|nOHJ-5w>=pF`o4g%Cgm`i$&2}dt_;z=D!5r7dZD;Y7}6>rU8 zIX*BLqk!szRx4Ga%{F(vqCRfh1n00XXSIs7oz~g{Jkhr!vB64VOg~MN?cV3KV`R2U zCw^j|2MUVI70F28smB_+)ew5s?Bc`Hz0y z?$>QXTA8QVFiJhbE1l`(7lDvmxt-{9YPR|9S5lvvv>rJ18K~rgAN7pQJx8!Kyc8^{ zs`*54`ARHw7Q#&V+U9pC6)l0Om5C(DOaCmCb?Dusz^eqqipmP-Fqfp3l`#+Gs#QEj zBH?BJx6u|1g)ZdrCcUd(Wb%>*3LH-c3BprD){6zlt4=S9)`RdWNh8j*Vx@_|wm zB-Z2N;&;}Tb}(SS%o#~uw)Pem!Qs0T1fc$)X(0ne2?ArVO%&N!kmL~FT} z&;C+!{F$^xO^b05hs0q8#JIKb3FJ40#SpE7_$w(qSKXPH35SMvv{{jTl6_(VRx#we z9xR#B)?I_twwUBZ9z#y#mL->N3BE$=5PE$Q&k)y#?YU)C*Cl*zSCyjf<&f8^5L-!Q zcCW}kLHO{g3>d;RnQfcijV^Ry0UPH)x;U@`8F_YZww^qxazMZr81tKGfZ zn(G{ETQoQ~*@FNbU`@Y&c z7Bbls_TB9hVbhuHW<)l#`dUGu+b`;H5QnVA9e98I@^!fFJbdV~g;9AQ(Me1bUWw3X zB*uxv@w9iJdTjK-TDHh+X>muUXf!s^c1P9Ems;kbS%sP=SG{dEwnwmK>Y=a)RkXvD;n07a5aL&-8zQ%5F(?0aL$ky3t$wy?kgaCq) zTU)S)!chX1-V``1)-+7)PefxC{)}6lmC|K5DO)eFnt=$9#`$N9UCMb9)5OV=Vh$v| z`zQ)PjXo+dsO9g!-DR)pT215IdxQ(b)?nWr2n^n1CKjEM&pnaLssmLF+IxtpPI*Dl zXl$vhF59;LdywVjqjMQ|)qM5eLMm2+MeJ;U@>%=oQ9=62nCg$63hPdSW>~06$5xdu za+0!b?G9HvT}Mo7_1R#|g2?k)hDT;`!E5bLVa@nA`Sjj35fzN{dH;gHkYSxEp07u@ z6+q58{B;~J{>)A$l!l&+lnz+B)4bMuFS5_92fv|LKI8SmcKSz1G`JqtlJJh>-!O94~N*BmFW8O z`FzJRCksdf$1Rx`$QX*c6`RWq&ZcSe$dG`d5u%P+JCq;d8NjW6f+1cT{qv4?GzHTd zT9p(`t&JqcsE~S>(a_{zvXz558+q+mT^S2yex=xwFULuAEuC^%dhyhF4@_somBeV^ zOz~>oYm9y>b?2ot)5`qNfU)xFOHkpuPtX?|f6ZT`p?8g<{{Lp=Q}zf#J+FHJNq01B zDfxSl#DYh;KlkC$V$ZQ7{*p8{&WAi|byhWYFexbqx8}+BPv?8t`dC~#M(^x57ff<2 z3oTRh(;dc*-E-nEy=JVMWYKhsm7Zrlp9Uj`-rb0sPDDRSHEvT=<(?E`TDpi_-lB7| zX)k`L2SpFSn(l%khfwaP5Bm6ur7tSYT$Q5kF9$R|Z)z1a3=diUx`~u>YNZY+T{8NH zV`UgX5k>ODGZWDW2M}G9)3_zeEnn>Oe;*Ud~ZKW~;LhtJxW>g@+ zLD_We?W^m}-jp(m1d7Q2y9@jD%Z|RwTcSM6VF=Yi&4!wKjOhUWFXAgKJUGs8Lhz!! z%Qmh9Q3D73?;YJS5a^KcM7n4B#Wvi;o0PZ9;DC7^QZ4@N;*|pTR~}S zxQ=W(2?(=K3*7L~lC=Vi>l{Gt8rRFu@j*I~`1r*S5xf3RG}ojA*VbaEn^r^AK& zlzv+gsN9jurr8eYD4ORD}+T1SN(%8N8&Cva%v_$`zK+W_0h!-Tif*EeJEf{ zvfZw%u+{5x&haHVN=u+DO#IYnhq2O|*+g?o##x4&QJ?sN7(`sOYH1t1bc}(a^S=zQxFS-x z4|Rc8@_MlbvR$mfu_eH9s{RO+JT$nrVGp;yuGVu~MzYf((~Ey&-+)2&j2U-CLv)!H z`&A1vuBGJYKRt{gJ%zGmk3K-YS($iifR$V0;CB1I(D3Yd_XEfD?fkC+4< z8rhYyR1@fQ;SerSQ?`d%@im?a@FdJ5$U%RHf{!#-as}k8CI#>IsgJ%L_(Am?ampQ9 zop5h8wbjssg2WO-&YLmQ2$fK?QlCA1Tt0OxpMM5djCxr=KLqRN%d6>j7v)+YNntn8 zQh*!9jD1L?7RW*g+BLxHjV5&}KzF+S|d4(K!fbm((ZZ934^6q_VHhaP~r_b)l!S3K9cL1jyfQ zTjKeqQ7hnHmUvGBj!1|$$}yV|8gCKQ!;yiwlq2ozOnT{%z`64h=d0mF#6 zDJt=!+_5wh;OxdmhQ@kxm~h>Hb0T&8-gAPDM*b_$uM90`Z<5yhJW8<(0|J~5p|niR z^zg-{pFucge+B5pzqe5hC%}7SQSG38SwIXb$wm~D=vD#V>gsRW8X@6_!9l5lgO&J! zglIPgW+Y5y?9A5(@<%^2huFza1C}_te9QvHz}_zxY%(kL+K73BQK}(JuT0{U5b{)) z_zRwa<1wMAatn1fK3n>-mqPapBg;spSTJsXABGQXdqr~Zx3TekOErNoG7RtG`_h(^ z4h9Ow*LaXNe#48!+b@>X$&5OFzwSBpwB<(41n4+8CZtN8p{-zMy!S|lhWrD4urBIw3WuC(35OmM zd8VwPJmN5-vih3fJo$SeqRx40DI=*u3x&>t<@j52A0wD=uSa* zAFp|C>fYXlgcs=PBDv^}okZHx-}FjkA-&ovMo|yP#%VIq4?H$5%8Pl=3(K$<-QqKu z+3$rSV%6WQ$?o%Snk%6@`_RE(JK|>q8k5;o{KzPeBjIW46t9-yOhAtlGj`s{ zPH>Fv9l~E!o43@zp0O}xY;2_YxAyY4)5d@UUPU7&PP=!|5aaA>89FP-ylYzXcjm?Gjh2QUa$0g%$zR%O1ps?Ej58S zl_rp|QG@ZsqbxJmO1rGcMJi-^ed)tvD417~7p(^#x;v4N=LBBCg5=`8>fRjGpXqBn zptbd*G0?P?8$4sPnit1mvU2^proOS zph7$9#+k|mChm3dZTqTX9aO9lI3!?9zF4br;COj(!S^*)2wyRBZLfC_TPY3tw;I_e zG;+DQ@4YZXSOvw)Kx`|D7bR6eUCWpGG8p7{Qfwg?6fip4wmK-0T(g-T=mQ9={Yjj( zs?M)uqKGCBvVw?K%qzE3-Amq#J^Zy&6H>@T-~UpCm;P zRTgbiPl9V_(_a$JH5y&onJqLs)HKM;@zgL>Sw~|sdWP>= zDF4*P%hNH|!_8DPlRN0jqS63TCM5{?2}fX~+V22lDnkN(3me5cUf>oa{=WT1gBBf; zC9bq^)|(TwccZ_Spo+-<9Hb}I!tb9bI-J2!RA*RptTF?CC-0)a_xT^In&xyM<#Dzc z+@mXH!n6YDUY{xv34TdW(Prx%UGNmdhhQzb-^A&wwkrXK3aSq$bJ=>gsK-uXa+#IW zg8N}ktOQB)DpUt!pr#aYCI4ajBwn?CTE15yPO!4v#ZkOr`{?^XV*2WIHm;|%ZA>80 z8rp0aX$rE<`}M+A64N+kOQg z&?;Ez;lv#UC*%w+DTQ?OUFMK$E`Cf)6RMo|{^g(17OK4qIzKOx(C!0kl)XY{ukdfi zoxvN^VGqmlkM~}zK^r+c*VN>a?2Z4(b>}~*igDWQ-h>WnlM|A`6j-P3$Ri%!ur{#9 z`(qBQv*z|FlL|0S`)G|SI9Y3`;dZXy;p_>L+^TBJ%Z;SuBmREwVb2k<=Uw_jUzbV^ z6tO$(7_7a$@0vkKz=JxFI#O{52krs*`sB-zubH`)@Z|b;e$og`rm%FHUgU8FTFA{O z|GuL}_K(bpJINNOCn%lRC)ht~CB%ysjfW%Oy=YS0xXL!o+-5&tlxQf_Gsi7XiyaID zGlR*OIj<@Rdx8z-8M~?)X-4@fJl|H^rb!~|gs0iG_dR~ig1UNdFt<$QgrK=k$o+el zhT2}$Lp)Me)6S^_Pf*egaX!<0w$qG;yTe{FgOd-%U{Wh%P8Bi~U&R@cOpCP?KGC~U zffcRpLr$<=&O%JdMs1Mn37XMw3Hg30qE_Mr2t3lmnp#DPP&O z7OQL55fDD<1nVeY_rHJ%cF&{#T8}%=Ga=+ly@%9th~(S-c@ z*|Z+tl$6JEhgSUD$lR=iM%S{ApF#Wa;!1ucr<8ld;#F=4(JPLS{EmzbwsDsom|px1 zvlzZKNT9pU!QOOg}bGHgW+C$wExsga<>w^ytPsyx)xNh?F zEyHwx-ArZqov$wGoKBEgP3t@zryGVdTgXD1HNT7_)^d`ELBkhk#bOCTx{(wpQ1w4c z($eE{!kSKG(EB|m9oD{di=MZ^18jzL3?F_iL?}p2uJl?OK50Fqv6hpqVyz?qHQufu zIua8I*UXR03{b%y?uHz#sG{;#)tcz)z!Jb(eVF!TTH1kF7w!~P>AmBSikZu3@HUTR zdLIMPCjRIW0cjl_Nl`0n+3vy|wyW9rW)iFsBDo(@uQVn8HE3-4O5PZiT4a1>9$9P1 zhAo{cC905;ol7e3&sTQT3p#>@XXDo4BaNn@f?Trqk*T*glZ!zFggxirMZ#`V|ovK+PqrMY*(C42lEP;X3_aPFoL2!`z92kDip zSY~}Hu29%bnkBCkZVa+K%1E*WZ2fH#a{vfS%`VJRfH>(fI+6u0)rV{!nPgU=869tr z&hbQ)2K`7cPg^iPTc{34j$)o2c@35d2d~ zqT3U@-_DI{4jqsqz8dl;OY`!C?dVV$@i&_*QO)uo2<^4a>Sh^jHvUa5nQQJk{H5&w z$wdBZ56x-kr<)X1L`A|FM9BdbX4ZohjsB^nl0`D>?^g&tyuGM#Tm~pNg|U~UZo~E$ zErt!+aZ~()=m4&qeB)nZCGN2V*ZQh}0mzJX+`+;t>V%$PLb&MkJW?=&we$C`^x#&1kLG?<{8;B<@O7d_Xm*{ zxA>)Kd8}B8KOCi|`E+{iUw=hnj3osjqGg0Vde8}<6W*(BEntLf#DnbqpK}wdY|N;H z%3sd3xbFrk!M5>IGpLVN-8S=(5?|MoYwE>mEVWC?p7RXUD`Pf(w_EiR=vkEW|4u00 zGRfxd&DH2m_PjL&kJPhdym^u7nlwic%5jz`7YSdiet5v8rx~L)!N}O|XJ!T6#*kI^ z=~@|tv}8)y5_j>VS6N-O+kwn~*s)wH&8ug(t#u3Fio#cyER-O;Hw6*sOk=<`wZo2$ zf0Z3$!+!dO=*Y~HjWpTmYu0n9)E{NB2mjpSkK3Gg-QwxD-$tdaxLST5IdupFZ9c5@ zW_jc_v1_!FiNGxPSi(40v|FD|6<@Qq&TO=UyA}_oZtrIN(En0%tx5j`J$6&dE*PxMlg)StjR!A@z%VbLCwqUpt5gud7aL$0HkZE@F2{V$_MWJR zeUyI`qq1Fb=sc|=a}eWt8KG$h8DF8@=e21zm3);Ai!C4XlH7M&T*dY3oW{S;Dkdjv zcvkdl&>gs%*n2$I*AVYqp_N-qPL-WSiF?PdT04h_xo~jpu(@ZSOzX;%i)j?*=?#a7DF;yeaoX{dVAJBzvgpvZcxs z{+CbXb=@bKcLS(|2Mz6AMDOf#uoLQZq)YI(;?^3wJW7kC@u!0rSdoTqol9|g)PMniSW)dS`(Nc_+&VN^LibqwWwuwavzrT9)}fYyp}R_ zx_Njibyt1{1|;hxUi)A%?oI)lJ;J|RKL*v5s#m%qQ(#VgA4J?ic>_@d;zAL2?fz8- zwQT;`+1PR8u`nyi2M5h85>SXXVd8^=@OH5anVch*oko{RdK{b?T3tg{Kxay)6>@v7 zLc-<+X<~1@-Bn@GnreYDm{ZC`YRswR?r0@E7WQ`^@)aVRjXOn6Xo0M#txgJNO?A%Ue&3}O(yQL}kPmE0xN z_kVE~82>-c0tX}G|A|@PU}IH>6rVUn+0JhiIXspeaYopQay3OKxD^^p(*1D?^3#)7N))U`bHrF1T zn=jKHxB6W7mBwA>heCn`2S=xcJ6DjuG{31TuYYH9CQ*D$3Tpr4bgZ-pDI8cv=LS26 zWO`@7JHRFZv_I!KCV(;UcL+d21;C-~?5;jGwG|sc4aW|jh<|ENMMz8#EJpr`@!he3 zku5C3{rx?{gT0FxSQqqvHkTrJ0D$c(fqzL14+KGNbs=>X32c6f>LQ4E5RQPpl9zvE zS4K|^2min*&ha@^A+S1U0O0GNEWoL3EE?RY#|{Wy-PCy#rQX_o3qo&R}@%A5bkdaE^+@&Uqa7?A92&4=-*9X&o3?mF>n9? zG`16)Kj!%4C}PHmUgrd-CMRI~uTHDI+ppXe9pPHvH$bx=OEIvzKh0m^&dp7VG%i1Y zrUN;+nHw7dM*g`mAP^VOayE`M?q3k2OMS!J@897so#5)sa~)h^tg2s%(U052sLJFh zv~hvq(W{!Jl+srp(vQ*_*w2>^iU1nxE!jpFJqP*?-PpV#fGLgsAzsZ;6hme@S35X)$@%lMlU(?Qsc z$$9IIL%c@m5g?}G-qC53koPM;Y~63e_=gibj##uxPOrqS+}CI=se0CIEiu(kLpK2zmiw$0B$J+$eCrDGQh>?_P;L^@wYOayK+J9eBztlyZ52@~!% zwosmvLvP%BRlQ0zNA?yVu}RmdqF%1bN3kqkel;B zo5bdEYos4XjBG!XZRlfb@)vpSO`0LJ%DYFUrvd#wPg(v2gkD+ft$QnPi#o)*;XYy% z&Op?Cv@up~bxE+*n3fP?N7fDJIEC#Q2K1!mm4Dsk*MoJV0eL_@ge9wrDNleQ+XWv< zgk7ZrCo(VXJVJJdtv)q?U9d~eLc?jm4ub7-q6@d|)4$KhGeQWJ^0THOfweb^`LdT# zQWv_>AZyfN_ZU`rV1+(&Bq^4r|ITi{?Z7c_1C8lPuarU#^$yIAi}5BVH9?Ge55q>& zOdKBVrTo=1Qnh+-$M7G0+cys#zMC@MLmI->cvD{iXt*W|70z!4hgf;FJkOJ^Y;E30 zN0E_}Z_}ej*T_TGRS6Dnf5Lf$FHXqOtAGu%JY^Jac-ewgI0joTADc&*NFzkH%)Zw1 z6dB|#_2^hAyvejiD->)`>sj?--|RAgj?ewRuthZNz(iRG>=j?8L+2T>rfZga*k_OAk4sI4%jzdEoTcRg&$ET$~8MVv^1BWem`d7kJb3f(>Mb!6U<*GD*^M!nZH zeS$9%cOnR_pYcApQ`q2ZyYq2}ae(#U=!Q|rWW~u8-Rq1Z5j^<#U+S3HA@N$UFFJcR zDTo4E9D;&xqU)tF@Gfnj5Lj@9Da!P{oOOz?U+6ccP=FuG49-4!XWjpfnsE^@kz{E1 z&kb`DG5zH{03D*~JxZXE;h-js;q^guuq?msLAwkxi5PHQJf?Y?C9WHmwoiH3AoMjt zvLtr5mts%)^$r@k110Dv^J$g_4C;k_9LnRRkn{#SAIDhuPG2vP+$JyVg_UzZDoXZD z;$t2pJ|Xkzms$E^^5mgE;nAv{wGM^ly%jxbr&8Fb;toDZZK!|HZU8uQry~*?+iR_s zO(F;+thqPQntA2irNhX=28%(!tn8{!!u=hJ_7)XcB`x%#n@kM6pSjfM&=ti}Z$)}@ zU8;$|2z?>X`0~+2oAoWXp)uaF-@GuW4R%Cj@&VkW%SNTdnq^1}@j$DI&uDrc zk$KOS;qm^~pf~YzHZkL&H>VRSc*ep}vCK*j&6~t$mFDkhGrpwJiBr>ymQn79+v!kV zebfNXB-WS~pqQVMPL$Qhz5#$4e16X02+MZ8IZ6*)NTMl+-L}hQH${%-+@86e7vDx| zy^H0non6Co=+C*XsjgyFA3!|1>tX8hSR2ug-kl`orD9g}7*g?f9$zl-9ak4~KH=YS*`w(%@(j~% zd*^a6$bWi0c`2DHC#>+9mHmk)TM5`OSY5N3A0;NM+O0c+ zji_DER)%tO;CwJEpVeli6>*F)pS`ABHFSY)*|CwB<7Nbz41?DcyP$$2k3J|d(kgT6 z(QuJi)qCZf7ME0>e*2`pP7*;d_1armckRdPMrzvh@O4((rp+A_1lf8}vC&o>v|En< z=d4;epG+C#p?l`kd%>u665s^upS39bk5(alZG%g^3+y8<_+E7Z{WnZH0_Un)s#U3^ zqWSIc8S2sWqBB=yttjJ|I)yMx7ShsXbhKoQ2}9Cld~=vvMAEv;wvuA4hysIKb74e~ zROpVLqL}Y=Q?zmaJFqHI!(r!A%>(TVHY;JQYW_R*y<-H7`k1{_5}(Gg`;ltGXi|rJ ziONjbBB5KQ3~bX^9Aa;Fm5!0-WALaK7UxCjivb%6D*W>rU!TLzez15nBQn=}=9I>h)ylC!R+?4$+Ga zJJ=>D;|a3~oq^%Swe1z(QmBFSu>+h3O$P!bg54hO+#~`$>k__(_L`bmdEkobcS0BW zeW{%~QYUm)cE3hL(taaEP%OZh@z5KN~tK1fN;B17D}@8luzxV+#@RxVMc530ZE zOco<+*}MuAe2-alc_EJZ^xeFIkkord1XUp$%TQcN#yj7N-w#Y=nJ|V~xlQ(dk*K*% z?h3f$`)nw&!K})gIoiFm7E#)m6)!CCxNiP!#+o1EX0ZPyUa@Mk-J&_nYCD&*r@UDC zX9z~bcgN^%G>WWvb94f-#pN%NbH}P&y-&*%uDb-KV(n@SHWeY`<_avp-1#7ZZ+V9i zTPIj!EVy4zy~-C~rNHmh7vA+L7z}oFl-Y~@u(<1rDW+n-*&oSHC~j@46&OY6%v5H4%~eg?gl9erB>2i0v?|Y4TTcBIwX1SmhOv`7K7Wo0uaOeF*y9wA*I~fUS@|G8?p^)$L zWqIrVSh&?!dRttQGoz%&IDVK%@lDAUzXkJ!D|B(!iiX~Z4wSJ%QW8+?k>#bAJt)pEA%JFO9D|)n-CT94>=7#| zO^Ej6ut&H?%y~s}fpK_TcSdCTeiSA|NRWez6;GCF^wFmK6-Gh?+4DEpz}G3-1f?CB zdak{M&R#eB&hC_M>*^94Do2xe704PzLR`jj5R7ti^Jw3W{#tDH3HSW65?z(q!{Fp) zW8$O$386UPim@dI!lD*P0ZOwCo(a<%0#OE%q`y&r&=Pcj*NFN;x=;-2Gz<+kE0B2o z`^fV!WBN0M`j)nU>05CfTO+$HGi51oc0g63axd(*pHP;xkQrK=k$zf-)g`)GAEBw(_;t&YL(}qUUZqv z@$=h}S~exdt+eJG1p~>4hFuouss|_K3X0LJBIF7(n^7l|3vjAYthKZ$&lx_d>k4mRNMTW}fZjc3^xP8wx)X8MUT%oHprGaltZ=cM>^Y;TB&QI_hDIL@gfVT^vRHH2ZB%08e zZ6W{d_!#f(jl7!|pzL43n`h|*o-eu4T1rXzAT+nqyl^z#FK;ODWHZ1e9Pcm8OOF;k zK|6>H!LJ6`rhpIjUkMj`Lk(uhqU^H5Fe$+hf0tsKg%({xiWprH>DvKXI-R*<8hf+^ z9>eK5cV;kgR93yCDyK?7Hbf>?m|xii#Wp{DeU~YIY(QihW17Z@3=h9&&Y89r248D=xVQ3{$?!tJ+v^!Bn!24jzMPFrY2i^BX}h=-Aa>y-=4Mue zzmTj;yz!oaFj_-TQ@cjHDIsFw?Lx+{c2&crkD$G=do^C~bhOafcRhr?lJtna<#R!U zC`?B8o$utEs81K_yo`@RsnD|?fdIS9L)G%83MARMIGVU2%`3qhaIUPsaB;r3f|YGX z-@L?h+DJ$94ReDG3$IQZpx)+zxG~oky~h&6#jgH2zxf+_KEbg3O z<8zrp=FA2T6B|qgr5fOzLmPWJ8&N}2Kw2xYw)b$_K1A?lMptNM*PJi1HTIa*X;uX% z9x8l2NnxoSY?cTuj->_^fO2>2qFY?u?v^>5TD+&(6vweaZ1H^x1V60kudY2Xw^lek z$;Sp^dD`I6(N^m;OAZ0SgJ$eqb9yDZ9@2xR9?iB~Vd9|~UWGNZo~)vlUhoGNVv%9i zCNdzEx*GuK4waXQ&d_l%TN_IVOZxehbgYDS)$ZF#fI$Q87 z_M~WvCW)T@;yOk5+eiLLCfyba?MCU%kunyc8Cl zQ&lUOwk$4-7zf}4XRnMOWXH^As?w3qIOvY16tGtu=zDqubE59#g|{}xs(SKw6lTiYk8`UFY&qZ)3BUMfmc(V#1hpQxVCVs_wg)kes@f^B_!_rGBDSI?>c~} z?c2m;0&aH~NE!9HakCE(CVr9;rma-Id2+G!18ZL={Y-l;wE$*I4&iLa#S5;&7n9je z$hN2Ykm$f|9s<%}@{@dMh~#&wu>7QK(i~UH^2j>(ibLl!pR~6_eELeCG$F*?<&@qU zOHfd%S6Jlh_=IcO#C_0KPS%-F`}IIc^j>c$EWRX`%}8|_Z78gFSv&C3FEmyy9z6GX zB;r7mu;`?Aj>w0+NigXoNK9;R^DTv}S9}(a>0N*`hw8O&*ruyEgvOA_`sCA4`GOLQaN&0Ujc=27@wyO5#{@n>d68&75Z0*&mq zR*oLnjx%0N;cfA@y*8sA2ebz%<(6{n-B+7ih?Wr`A-8u}I&kM+QY-mz0Qq(+sor-~N1}mZ&DuqU4R5+h zfe99!D@PgQQjM@ywFMUC*ITI*E!?rrB&w}^%8OFVRue75O;@yj>73r{Y)8s`tgTx6QyA*(i!qQX; zd83|gONKRdu%UFjrCEt{<~(D;hh`*;=W=zIjgk2)jC@9a?;ILtdNuKyKtLRU#BwHN zueO%p2f%9&=PA&HtI7H>Uc%n&QG1kZNowt;;n2BzdFn_E#uoz8iK4{t58l8Fx(tYp zW0txpx5Qp1L{V1zrFEz}(m6so*p;dr5&Tm~5(Qp~U6(YoTxPzf z*i6x53%oyL^j5R#g&r7%8-aGBk7K00mO>Zc5K76&^3}D3q*0*k27aeaJpW`2@Ybsf zw=Qg(coFnyPf!aL$0=!-CGFLwoA)EaGZLwShz{ZE%a%FhGc0C|es-B6Da*|Fh8>n& zu&2|X;eNV2o!jQKlBR56Inyep$3$;1Zj=vU5G`=T74(@&1=q(3y8ZQ_7v(s&K9R9=kjHw2X#pp++FX>B7#@5V#Eqdf43n> zs2APqnhXiJXdK%Bky2kNI`{~?wP6rrS&+RBp+8<$pe@iI3a&CR#I9jYl*{kx++6~! zAIV|*(s_i}P+@}y!K)L8a#+{1KH~QlB?wl2_POW4cM3FFdN$XU=N%;}z`PqBQ^=l2 z!Qg@@breN+)3?5*96z9+KAeepOLJ5k`*uBN^)gI$Lfwm7=IueW{$uMdc9;GVn{-Dt z+Oyp``YN}GSYOI6Ee5{9T6d~jZdXXS-BYbpzq_A-{TMWAu~Z5T+h)}6jNy9DeXk@k zQv_PwY&49$0K)yd>d4LJ;dzg(3+ZbrPbXO(iBBlJ{dlpa-yyqSpDB1O@-IVjrStvI z$ViGr+WbB1*uD~y9jp=Cz2cgAS5K>t*Ij2=0CtlIz(Saj69?UCK{qWlqp;r3v0nAG zM--u_ge?TH-TzpR<6;_`KkkE;1F@(9%p(Q(>Q?EF#zi=ka0hqEU0BV`yBrg%01}vz z>d^@?6#rQRgU=C~M+4i6F+}(rD|AKFEowxy>i5b{~HiOcE*?SC&bAb zM3XRnRs#i0y*jt8OgDD-~%x zbU;$@uoV^5nzAHDt3d&#>8x3EsDMH<3lwzEC8DkZZd59w7X+bhk;RbhSCFKSt#b;O zK!{iV^rOLvVl?fLs@KrT_;>{1kgZq1Xhh)K^~&>hOZeNX&5Fm@oH@u5x2;*M+>Ep7 zLC!14ebAnw$m+aQr|;1P;f;sa(mU!YKXax23q>mar)H$h9nanw=wk66hGKJI_PGvO3DqhK+U1Dq(ZG4Z?>4S~n#a%$NI}Y`vcOR6kNnr z;2jkMI`hFabSGTIq)l_>$jRd&P4n|`*=PiWG~5WnuoeMcMD{CEf3U=JZ&@elORr#< z@^)6C-dKRmwAKkAtgb`*Q12(jQxP=7kJ2&<>hpTpv`V*~9$@y25iEVV!#>4XiTg6Y7xs;2J;<2d85qfI z|55EE`55oR#_7zk;WkQ~TIqKLb9`wJpR)icdaCA_1UqN&%a$WS+de7vg2NIy;qWT2 z;l|_UT`p4};O2Ix54>A`ojFn4G?O?}z*eolYYWamO$?DbNHUBg}M;VhsFHoStnEziSz8Rc{7j`$ugMg4TqI4_?c?YNfgX?OCi?PmHgCLNjPtx+auzVcwMk zg^<0RerxU~FgixW|7`+5<8rftc=#N-duRftlMxY3enosxvkGe*BEop;?Fy)a=w9`S zCr94S)zdjayk@-Q z8^rg^#QBc!M$c=05H`69dw7O7sBzM_+}yK!-%Dx%34xPHeFe>&sdNpk=c9+CLH}Cz zr}Uq^)ORJpa!c*HX!Cp_I{x8hR#ijZep~=;fCd5}IDYBV9pe5>D*8ZQ-MbjfSo;7~5<|>k_5AnI(;^5gj$M8Sr0||;ECP(@In$VaZnT4>7!jP09p;}*$0>|S!SFMDGy#Nr)ZTQKZ(``}8!=Mnf==t%DG>z6q6{bdIQSk7B+`!q`&Pbf zJ|=|KXKEs>E;C0NY4cTrl*dM_4~bVjzvba?-o?*i980y67YeU{GF%FjWFUsYe@%fm z-AjliEvIMG6+ur4D_+JGi$}Y*E8wv!#*4!(HQ~05&>m`rU|*|=p~}_XrMkhc>xm$C z>2Zut#*hGvRKYH_RS5c9-y!PPZ?TE#G4O)wUdePu(H(7Ns zRA1V%D2h13s&et1!Uz)*kzEU4T6HFo%kVpuSl+q**cZ*Nhv_iqumcYaR1&vKu3Ydl;2q%oRS z(~V|ytZp`wixXYp%R9Sx!b1W=5;D;8Nh>P*2S+Ah^i7S0N{f-5fHU|9T;Uy%$iva{ zK>z`l`CkE0RRO4&n4lO*_@`%gcMz-%AVB1^7QT^y%{TwEZEe*T^DoVb$$?s*M*OcN zb4Yl7cszZ6PCfqKCJqA($WJ2x;8YLu-_2KCO;kh8w;KU{7KhuZfpMe7m{)8CpiM$`# z_+KbK(~lGzMtBE~|I?m9e$$Viq8|ju&k0!mZxsOy0661clxu5)0&@e%zw(i5>zkYc zd@=t(9Qb4Kf90DSnf$*&j13@L?SFm_e?jp9ncph{z=jWM9H)L^{j!PrZ1D0J&_7fy z9T78n5x)^u0lz(TkofrP8?SpW`F)<>I|}(Y!=pcbsoz_pd{B%PmX?v`5=%dB0KXDa z{L|O|`r#n_FrR8tNMiebK!3$4GW~zKxqtB=bIkwLf9vY>0bPK*{ynE-{ITFp|9Soz zVSMW)mzU>kv4Nt}x&i=UL9zLV{C5uhL$gOOedn1RT%3XU^M1g8TT_1DexMM*jzAd% zbh8^741fgZ%neMHok>F+pX!FY@BX^U{ zMAsG=VJMBTA%N+M)Ci*@|FzblTQNW_t-(`Z0#(ZKIqNC#XE~psBc%p#l$8p;VMW>h zXVvN_KTG})P3;uxeb@`QO!g`mX2w7VwoV$gE){VNpFQ1Ysf^ykls|6m)Caft!Z#RJ=)6HckwhGyO3Y zLAme^zHq@(BE9rcB==~d@&j%gyBP46ViUYN`c;eE{7!yk-z!Q;q$ubi-&Q$^1{!kj zd&qQZq?)cFPsu8NhsZDEeo9kkNHVh}hlviqleelL;O@xoS{%k9@L!qDZ>xx%5s5Q0u-_JYv1oJXixDsjhhCJh5WLQgR}^Z=(YqvB>1JN=Q{;$x>8 z*NXSSsRf?c=DPsW!0$}(LkKI-lHU++yP2>zEBGy}-k>Y-$q0L}V%G>`9pru7z)~ae zP?b!JqQV|7s$-t%{cd^mzsJ&s^CP4>pKIplHqy4=N>`9B$1`}N5v=n!g=z1kLu>wL z#5}7n);4%*Utxi}-{>|L+(~i|wB~6?Bc?SWHduCxhK30*?#f%OzDku&pm6sVbOHb> zV;gM0)2v+MRuNB~L2g&6zOhfaIikS)BeE7P&B45I=6O*5wPT9;inN9H+UFeq%kr4B zK!HRwiGCPpm17C`vza4w`d-TNjuP>y*RFUEX8b^C0OYPgxLq7WE_ zbX#IZ!xsiknTt$eQPNhKRA)=6x;SPKl|-#g4B%X!B_b9cx9Bov9AIsy@f-R2SSFMe z32u~{G|FkSU=2f{u{jE`=s``#_HGPmh9k@{>umnT@D4Tbf4Y>iD7h154*9}11e2@?C!+yPh(E8YjUPaql`Q`k_72)FWKg+{ z!iPE5Ex5I;Ih~Wu{SXRj$K;%;3I~a?-nMrmlsLJbHc_O#Z4;X#iHT(Hw^HI$K#N|Q zDm8`mg{X*Vsk9iFkKbDj`)bd{6EzCInT>10L$*n8|aS>M#n5bsV z$MzP=2?l7a0M?H3?6u8pSw|_Q%jCXx>YxWJ(t!! zgzzrT&nitYtG@bSmO8>vs_6CTlx6)AXvwU!WSMiN_PL{fp07qcXc0ii`D~WLSD=|t zw0Jjg3w0GtI|r!7@Zx;rIlp(RkWiFZv>%9_ouz(I${R|N(uSIqWY4Zcib33JyixkG1b?0V7@346$o*6cN8N z-HZ>%o%gLb*BynZy=*^^uI8L*kxXIG@iP^YgK~rP5HcJ3t%xR1H zA|;7Tey>qu_AEVNP@yJ-8Cc5fW7>VfNr$FuhcEdIXW9xUNy|N?0ff>giTA;W>!?mf z*ESJ?1g)zWYz!4c6;HSM|QMd-JX7Kta?90jsmc3BJ~iIhgIO`wyp2?oa1 zz}oma^SG|UD+`@k9fw3XI+g}%VOi?B#SsL=;f)=KQd+4t9WMmU*gC{oJn?d0YY&KN zP@pA)EPP21YR*!Z!&^o6PMv!9+w51$8imXx^o{}%D&t5aG&-qZ@h_gklj~dLOe6#I z$oXP}^g33|S4=BdfCkh1m?gVDHFW(t{32Ct`=~7`hL|z!;3?H?D?USvT2_qXi*ckt z!pgztv|PZ*!nI2`~-HswUoa1j) zHgHrPl>z(b@&2ivUpc+^wL0;Fm10jIizYtwHMAEzgN0`u&l`jO#hvfI z_=QL$U6);uOE-y_(B&;m8|-OYk60+R`1V=yGN4a4ATFDJ(TzjGRmZbo4EL@&{w|M> zlq25!A+8?hsT-2z@emR7=7xTFBA9UMdmJQz(=POvqgqOSZy%%G$0_E$wLn!o&Ni4I85 z-h$oHvhFam%1&wyBxEZqGAAveaSj+erF%*g_ZxiDTyk_xW8GjA(R8DUfb*g-Q||PJP#RCOd+SxmDxAKG!!}DvOg~2asycLsF;;;qVG%DFxxnfUxGD=;Vc8I9 z-3B2BRcAhypA7Y#O9rwGf!OfR)9Dx9#Oqe5g-&d@f!Hkf$~81>G40nZBsA3}3Y_F` z!YlwtYy-#;o_I>sw$btw)p&3dH6snSeE!BaBX}I`yFg;J&Ko3`OHI;(=S{!g9uI`X z--*Nc0nK(<_ahbOnd|8oTou!$Nw}rOiVi`lYb$H?%l)Jrejt_(0qAWq!$>!C%k?&P zg9nrik}pO~00J@`8jj8u3YI@Tr#l0&|Mr!2WOPmmv-J5GXB~^{%MkiMfrL;HuMy8Z zdOLdfT?M#^`y}J9wjA^5kMNEvB1?&}t zMe<{$V|uC05d>=nyB@>Xs#eJ+Aa5KKBbW*Ln;Q|rE7D>xiNvVEx1D%8WUaf&JsLjD zqJ2eAMD5Yi^TaKRZ_O7z2cGAm?#`Xjl~aA(>@UN|evyJ-vrH}tr@TRH+4Zy@d2Y|u z+%&gM+(O&HG{v=_`T)$J=LBG18&evK=#iAsuRzLwcz@N(sjZptYHlm%#D5ldmPTk&9Z8W0guScfDFTPWWA1io!kDP%;{>s zB$?kdJVZgnpGm8bev99T8@#!vOZDar=bXf=529)MD}kxt9W@WwzOJaqfMqqMN`{vF zw6T}e`tL>F2>GN*4`g;^u)@G7XOD@~6M`-P0!#P}tL5{Dhk_mp8V#%d2$@razq~BN z=xT=6JQk4j#I04PW{b?7fh%V-Vwh>@NDAhSGj2JfiY-LnvN%1H!?L$%YG8FPx3EEZ zd&Xc(Eyt=f3<+Hx>75ctYc@~VB?potWDvSPrOVFd}yQLc$Q~8Mn%}y;F&enOY*Iht2gI6 z=VFqrAJYyZtM%8(+YXO-feLTCZ5?qG48$YxKmnKC1{V{w*1StV&V_2{xq+Dx-|tgf zO~$jS`Ga&Tr4){A;*eXhA@W%kb@!yxN|hRkGmaeBiyDIkGNd;2(ZawVDwA?Z4p?!A zO!{QaiT>jzbLpZH@?j#OmwiDVs|7sraJJar0?kS)s&gi%uvDD}Fek?>0b8wIR&S~$ zh!WushVCVhfs^nbxVe|cPVH44i@>&x=N-C$345Dk8=&>Z#lfODj|%@e!R4|X;4<-tB?U2i-SBy;iNXm=VJZ^S8Zy1j^JFbT z!EN(p&0B`i8d1S(M2lO{!Bt^uXAlLSBKzflX_b}w?0V-2RKM|dxV{CosXO&>WHBT> z=aE!<+v7W}L*GmHzzI{fF>${KiN_IXT#Mz%+U$Il7%MXy@dzVzo>o2g&D>IpN1B*y zqOR3Knd2vq2s@1BdneDnsH49LoXnOb$r20gb=y$G8Wj>wG|m-tYspj z2clPMcY|ACt9ZXL3wM13p9#RFSQF%aG{xVUD%knQ7*IM_qy5JFbRi=tr(_CYeex%B zmX%0?xkzW&&P(-ED|6x2f6;PX7b=H|U%YKTxoSZ#y=xO2j7SjyM(|33c#L8e^@}OS zP93mO$6E&kRBugrLO(yg1k)Hv19GY`G>(^V^UchJAzsKKibuXxK_JaX-N|e=s4_&gfkB2TNh>NoE~FcwGi5ScxE`9 zHrOpip;ldLt?nt5(&-fvmgM?K72IE^0Q9|nZV{&(0Ey0~3&g(;f3n=R25p@D?#jsO zLT?*;xnvL#;rVQXtIXA4SUe`Cr5T@Q-+^>?`((?hldLva7&E+>WbF1(n+9lMzTVJ# zSPLNLrKTy0KA5?gSrAv`KTKca9HQoecfdL4z}dRMQ}rK^G<1z~t`?}t5WHPJI6l&t zA|7ZRk{g_H7j>p1TsG!A^TZw5bQg{>mdF(WxU7V4VwdIWt|QYf;*VvWn6I)&`sYJH z6gRRo_`HieM=nV&pm6ycBkzI<#O9#=_!^EiDGO(m4sqkL9|TAFouSXbXkq%gxM4*6 zzD$a2HPVsUp~GWO03<6u3(# z6HqWqZ$IdTemGuh%I^Wq$6wbT>|gsGw{Op&OTCGB7lK;I8=kBq#$nluKA^TJ!Y^=u zt;Cx#MC4=jdXPGca9~`ZL=USfAhMefpOUxE$)NcqTyFn>V%-W@IL+1W+F*xD?~|2b zY$&}?Ux?P-vJj(G|6P79!+oMP9f9*IgKeg8g>KHog6VNl)cg1;t+=cq-n%n=6DaiBEl}S=?Bqg1*NNcNIS`d*2VNpNGEsR<;@RUM2 z`FNsv50WP&MInV2E_14d{?5l}cPZ0l=FPD%v|5oh_Y*xfx*vRRF86G9>^LhM=n}u`VO4kP@0hS*<;DD(DtHE3 z{5;p@4ImA5vsmyyN*qjW+2-$5ATp5@78#BVK#SF-p#G&^A*CSXG%JO4Z}lE`njbD} zHUHMCS}3SUzo+Y&z(UUoZ4sNc@+8)rT8`5NU(V1U@*bwNK`OF}hp3kX^+ zV?^5xxQlBUT_yE%wGJ!h+1GP3rUep(b)t8pZ0Ukm>K}7Nr-ucbEWTkhQ}}csQ(mX_ zx~U8uOa`#cS34bfsx9NIV_LVVC8y|vz?LN+wfxWkVG<#}yqGg_PmlM|HCbBusO0%i zlj2kX+3L#l%!AIXkLr28$c+g<4EP21T0aawUu$CEh2ZbowQ&ZwPiQ}W!W;q~;i`Y9 zPQAUY46XR9v2TBEeSVNcUPod1GGCf;m#sGJ7mS=ah@^VxAb-?D2{?t1yx?6jwKFQkcn z2lImm*MaK%5%Z6IqL_bQKeR;)#v4vkKz2RdtViw~2MuOKNN<(4$t9C7J3U#uZ~Z?Z zeHL-CFIWnItC8qORP~qBJ^&vJb|siOl1jJ5A()%(@Fob=%?dr%G6^ zvVNS+!(HN#BMUbLL;U{;LDoaZLzYFz6`ZGXfMI;#bT%;LcD|h3!yiYxHmdvFy~t>o zAl}qYYB;Qm)~=1IwHVI$jXRHNmFX8R{vy;=d+VWcW%|L5c8tHKc95pIUHDg6J4$>gQ2&z>+_McHxf9niZp_yXS@A?oU$#IZ zEViF6yxc6C4iZ4i$vO(@1}6RqfNG25JI(f_^j;e;h|k?7@fTsTjK*O(K47fnw?KgM zvGi1vYa;l;dM3WcMLweo*iNiV!)YV2Ry-O{4p+`Xmain~#i(hB|8!(0J7$QdJ==6`dLR_v( z#8tBX+h=UU?b;tbod4qnP4p?X(0T}X*1?u3W4*4o^wBAcS9?jR?GjqI!5Ywr8L_H6 zGRG}VswJ&Df9%?d!s9@D59X~ zD(bi}dWPZj98VhyZl5o)rK<(<>UJ@7BFegKKt2_Ud)Aa}DR+Xv;yG1*Sg7ifP7-#q zT4%{M6(vuO4W#i1%|Ml#fbt2~5(cxiwm9UQV-SrO%{Lmz&)ag)0-I?+s}qX%u_!{v_G0eEAl) z%Zqgq7E(m{HVIlUYS&tpb-1Wc_9nC4FGb89wHgF&XFpbT%gBf}Rwxaat6MYk{_Nw+ zT7~h&8oGWQ%U{wQ0S^<@K>M!)%{4T+B&n;1hhAs72cle$^qG=9ac zM?^C_2^4pD=d+O4$15CA{iigiMNMvI;w(Q4|50_CJdyL@HNl1W`p7J}g8CKdwAbDo z;0DD_R@fDzkS8dcsm2}CBWmpqTw0c0hIf?tqoK0Xv}sy2OX_nuw*C8dpg}oIR@CV| z5R6lMlW!aJ4Vxt%ihwNY>4-yLsqeKw{`07bJ$W)N*j%4`&Dm|n_iQQ%`C|MOp%fd6 z-``Ru-)5&Fop|8Loa&2%*8=18PCUfdRg#!0!u{$R(L1Z7&}k5PG!i|$ZIKTPchmCv zH|c1j7`U%`@1Pj@ZT<82A*1r9*`sricA_F8peytp4v1kV>XaYR%(=R@dVwz?*&lcC z11VN#Daym5=INjIQd7p#TPfqVbprm0sTTfSHos!$a?A%2u?GgE zI*8%LGM!`Ck_Qi~tGNnkvzDnc_lF*?;&vVu7B!J9EBqi>-+<%C%U628jn5Gr4Bxm( z-vy&?vsgp1W4VFH{^h3Kych5A#-{zR-X!v_1V8Lc|9RV11O$CLGo?y%Uu=mx+q-#x zDg9kmwxc&QW)kOPfLx-ftMd8D+f=6t8nwLh{9`(I;pUSz=G<)}`QmJ@@02Uwfke;^ zUbj)qo7AHo{AI#I=bR6c2v{!_y8)HlnF z?O{c57hsgrA`NpspV%mh(fQTS{VtZnLfcm~(n_u&UG8}M!z2QK(zinc;b9@%CMa}G z5!3g^gIU?ZkW6kTyDl2c8=@Ke0-966Vp^fj@#=ez87$)T=65)5AV?OuX<&?W7E~Kx ztqVs?_=VKi=9ISnFhukX$M2#v2P$CN8_^cj}UXms*H1i`Sd>N3e zChcg8wcuUGMK5burBNf>Q#lhRD)9;u$)tm_9ft-(j5@2J zm2Pl@4d*wJbxn`9E2g!CbVw^;qLLAIa0|N7=b3HuMn~OBoJpQ&kweIe@-NxADX|Yc zQ$d`do0}pA+X~br1n#Ia+6oGb;Lp(o?Mv+ia^kG1J-^t~Z_C2b@O#ncB-zUl2Y-|m z62pSsj#`PtUE;!h^Y7R7Z7*0o%dBmQ%v%47_EM{h2Z8Ku7*)Olb=obS|FjBya}Uhd zUPB((K=H&Txa1oWoaBdT-ihX2-Q#kS9(`NUKZH%wQ+^PnUv<=I&enFNy9D><_E1k) z!yQPOT#U+An2i7yB3w_G&*Z$< zgUV_{8F(3PavF`1W?P?(xkTslno}?gH$-yh)6`C)GGf0Z zBxMsP*E=3?y;%wp$)68-I>2Ru1;Y<%i)d#W)@&q9jy#UVdTjo*ZG;$;>?8V>^ozf+ zhSPT(B=Wwd40FVk|f6AywhkUA9tRWnmsF zH4dEWjlBTyiIBovBMG6xGv7el4iI9-0~bU<|(2 zYSCVf9yILf$P`SLi)*v$xnR}=>qQ+bHH$^XX3w6?1vZaYhEsN-iP3#Y! z1Y9x1|7P`L{a>wqZ2x2KW5j1 zrrC5&o$8!7+B8kI+HAFMm=sNozD;x7Wk+PZAAUA9FurY4y~TACCI$)*&aP^#4B?Is z_79Fv1Is7Kr6@NG)c>{t z(>FA+w0zUa%0~YY>R&-QJO75IXB6eY^6P4=s0%6xpy1O^7E?meH?n|^OZ+r#uWm$~ z;W5t-Z#?#O7J;+>jWx1<#+iKMUGZs%z4>WH+f!2mC`loi`7_LKFQUd?^f@QBvNQqP ze{)%18vgB7_(5>|48imN#Dy{gDCKYPnWI@*ff8Zi`)e-Gj_!_NoI}L7F@bV!0u|@P z{9au58Hlo`uHydn_5X#vvuUrde&b!BtFNPHZ2aQ1IVaXTgJ)7|aQ+dsXC%Dv3I0&k z0)D?;j04Ni=ve>uK;Gl2{Bo!%acNn_{i(g>{YPF&k&qS8RaTF>R51t|Kt`t-I@^>G>0g z2w(5;VyORCN)LDehyK5gKb-!V&!IEF1r`SfXOK7OY^vZ>rdjDG4xBo1RE!d6pEbQHbT}mTUPD1$Eu=J;UJ=+Pr zR-`PSu)^%Q*^#7>Ygz+d6r@%3Z#5gfO0#}7@N0dc@?$Lw3jXg{4p-T|4Y=k1z5A&d zPP#-#@{@6t6NonIs|)ZEk7Tws4+E-+qH$0(LSN8;?Q&`{jAs87?QN*v8{6PeQ_-5E z?$dzQ`U~^9`+w=BuV%l!{PXYcKuL)|MlQW3!-P+E(YRjSuSmt~?JcY_a0*y>R+EF#;R?6iuI+Jgxl630P%VjaWGnq-jk za%8|z+_b5=G)M|Qs6;L<*d}5XI2?0e6Oe1XD+50bp_^efY-DCCP&|2(vWI&F>;3L* zwSVFdQEomW+E|8J-w_(ZsbJD{nV^unV(2*>&ue!?0r=WW(mtZ%Q#cYpzM6F)XpL*c zZu)!iY+XsU*@IFWQpOO`%uL!s68>JAWYBOxqRIeFULcD&1qRQ2^xDOQ80QwZ;FK9y zMB0b-EO2o}P$_uo7x^yi$kKZckaW!1K~&_16=$!Bn|o*P5#FE&!K9qjYdQVr&X0 zPtK;C4a;Tb36IW+6V90*fQ+6zkFMZbzEY>!$`CQo}=DD*fXXTL6lS$yiL54v3qA*MQ6h%Rh12T za1>vgBGS)jT#!cLeVNbHaU@0H1+B8U~ilEICSkZ$P-4qWz!6wCo7deY5;h)EkVnID$gDQC!&y0gdeDJ@>4J|&kt zY(xI0Cq*=iiDjrFp7r;t0*GKNRi5=KJEM^At#T3UNTg83!->9R+WJol_KRcC&YqXl2t+ zK~Fbn;U81^ff0h~44jB`%eSwSWDe0wEA4NPLKU#gf8;pmp?~NJw*YyRLG>{qek)4C z#VMFv7B6dHS5<3z=+unpp(V!6M%ma`(+OFA{l-tp_g9@d@Y<#0Og8Ngxc~JH&5!!%FOw1B|HTA0>@%?d&uxckiTt9BV%_KOZ!!8~O{_a28#K#WU=f?} zFM7_#(c$Z$xZ2-1slLpTlRLU7!>PcxSJ|SA#eP1DJfMvs-TW8{)~^9JyeFoeO6R{m z5#`d*2}uyEi@(tcNLRC00o@;{kaU9{uCp6=uC#rdAU}I zJED93t|^$V&O?cTTc?w}0%)LyqOAp-*kK8+=5hn|`UuKeaM7S6nfTE*Kz#-$T@%?e z90PIdGgm2_@swYMOo}1Jxi_gso2-BRUAJPw%xQlxO4ze{mOfCxfxLuv6fg#F6FOQlV`k?4_}zfL`G2@{$w{i2tiYv?R%Ma>i7tPDBn*WP}v zAQi$t-yg=&NSL~*wcYA2nlWy~Mn005#goK;oKdHHz`h*8HyDcQq*Z`g;WDGCFw5Eh z1Km7X2N@}T`C8W@^~!T95>lyMvJP-U9pj9*@_Z3TMy_ zjiyG1CP{ctvorE$hvXK{8oEI7$S@8)eV+k~SY$o65pS^-I!$QFWhn;%b;n<#5&h=Q zKZb9A0maY}n05!fCO-sLNCdm3-rod~q#Iwf8@l*l<* zeKCd^PHjVOM;2BA(uy=k#?!LcT81`sXTz&Tf546b%94B#Q$=B$Lv1m%l?3P2*W8itQ z+x$$m%Sr}D_LwX&wRpOC{B0SAU7sj4+eIr_oJ;7z>x`E)_FiPuY#kR3|dJ zcNK8$PSbwhd$$i8}B`e{1lpq zK-Gi6FOIk!nXV#mEH(TiW)ooqC9Vda6;K7xq>d26EFawJB0K8^6E7ste9L0FnI!ckzksF}70c(+qX-pUzFWan)rXaznNV zTm>KNr&BWjgom&G${9Hgd4#!?v4w8{(MT<%*@;AJImUX!u0=YPuFaak@ZrdSKlJ0E zurKqrD%P8G&t^oo;6?XXpTSE|W1n=nE*4%(7Br&}oDJxyP_j&Xd9oQF-Dj4B!qtX; ztkD$$@h2z8NFsQnh?vZUMX1kvbmQyb2}$SlO5n<)Z&qULSkaJOUz}xF6r=PYH(LGVX&y?WUdfl&wB5MUB+|jYO zP-b@9YoT^JRAOHqMK#@G*!8rf?=PShyIwzuhs#9|FsV001FQ*oQz-gpa$GoE<>nz> zX%r@%gcU}X_xU-9VuW$pWLYOk&+zOz`e1rLRVSsfJp!DrtKg$SG6CdDFHOcSEPQYyvJKKQ+Y2DB#$=M?2SO(Y$7L4_cr5JXW)@OIL3L(pz!b=E1BjC zM3PratiR$WZSxSWSY1^5Fo;tAS%g^u*e|s{+F8u1@V_g0SQ09n)~)YRGyG=0sof-Lv z7K;#g_Nuko=WOhH-i)d*DNR-JODU^YTy%|^s>Fj7@Eeq^Mi3R80Re;2d^GxL$rpS1 zjb#1!FIF^3=au2U?8Lx|Itl_h_8;#q3Rz4h*TO?$`L7q6^5X5N2SI4k-ct^0@xlCh zN7IggY)(ec6-2px`a!C@smfVTbPF0BWT|?n6&B= zI4t?E3D2KdWJQe#b?;EbiPHRUmtWD>?>jXvEZNpa8JX`s-FJ1oVfkji%GzLC+WCD` zH7*u~0c;xLB@95f-_~=&?CngK50V13^1?t8jMGebPDX_2!X-us6=qkkl%CixY7M6p zC=v3Lb`{T`tBP6qZ_f=n>PyVGt9V7~=pXam9i~N5&zhrxWXewel#^J7lX-8Dd3r03uVw%F^Wx&6|~ zY9!`eCBZcZE`Tdo&1h2Px#z6m5@6nhf{sIS%>-a)zgz;wZLR>Vc2V6vbnvUN^;N@U z$cn@i{U)|QF4p=O<6idZC(g^ z+}GOkHcwI!w++JCrBPJ?L^UR7g-g$(-`%@Po&QJ@3Z0g-sunhI;-_4P4Zz$md6i=r zTbUWTu2WHC!Aw8V1klf+8WAH))lm$p;4`^K>z|K~P6xs4*wZbk?SrV%s_nB*L#sF8 zc!gggv!;i{D4|}L;o`zsXt?@wPb=q?I;z2Ng~f21L#CE)0%#AKc+`lh7!`})ah|Yn zz7FNoZK6&HWq+;Yb+q6|81kNq4OAUBp*{@8-eTA~3}5>KsZ3)=YynV#60ZYQN@jJi z6;HMAB11Pw9YTvyn*978RRF*WW$qW|Rb@8?`D-D~Opvl1AL6i<{Bf$#+tC$V{W*Lb z%Wk)xI6g)0i6QiP+i~wna*SE<1)f%v8Ys1xQ==Xpz60j!}WiJ9w?Wd zhEN#yZ1L;A3d8!OoHe_RnP&@W61Id%jH#{rWqU1Y0)K6hMUSi%$imB z6og{Kp}`n7Um25bd;~E8P%@_@2Vf!2uppw^5affKtko8$j^d##}LM!3-E3+(#b{Z9r$=NF^g|`q*FHF%XZ*1c)BLnV9 z65J+5!p{aImU&)A`M|BRQSjv_u&`e4s-SU~rU?by&SYP@cN#$%iiaH%P2?vidOJ}H zF`s&W#7Tf;lazDUZ!5jzrT#znMS9MpgH+ilhkFHX$7w5Q}QG`$9Q`EixauTtVUu;H0#1!(E5%g6kW6 zKj$>@EYVw)xqs49<`h}sc~~JC8zG~{>nHK_WhG&c6Zlk4&#>c0syptzt8MnNK(^x# z^jd)l>{4l_5D(E7CJQ26z?5+Bns(?611uz37^RgrZ;mg{p>YHA5CF`Osh;vYF0hV8OXh>Gy2Q4Q!iILTpTrJvAKjq0eRwsdq_11$0+{ z*>-B2EaxoCN$Xr@^Q31m59EK@9|Yn%OAbPoYB4*?{KIKOnQ+I*L}o{SHEh zLcYv25Au^}OPtF4$H)K*!3#lUQX{()y}?HOY84H3Y%r0hq0g`>{c*c3*4%ClN#y)m zrs*|DF_L=*@GG7!8)HL^mOT<;iuP@tJhNYHiqE{FRE_oCW*t2_+sVJHt{(OV*~|#a zs*4KFN_g;*TQdN91Bql~wJTrzT41%YXm@5s6vr)lxxbmXg0P;IBR_6W-E?Y{cclqZ z)7J z*3`s7Wr{h}Y|~sw;f3Qum@Rh|1(1{@KChaP`PwMgRX>sxcJk*AcV>g~|31L@Xy7O)`7iMv|*Mj}^rD;p=(a;%&Np@G9 zg^(m<5>UD#8Ixx=CX?-!eh*~Z_^|XYDvUYB5*a>GLT8Gg*M+WaQbvkfXNK-tApK8s zz|<{4)pIw;T+moY83(d27g<$hSaA|sZlZTSFrAL0(0>Xb=h2HS3}b4RZ`9M6VA4;m zo1cgn&4)C{#KK~NR4SG=x+!|IkIjp}@IoqI*SDRr`lO)f?yD$^wT=fc<$ew7rb_Q$L_2vBv_L6?6Om z?EDxBy;%|ImPn`#*#uQ4lc>8Omty146T304<#Ka*#RMnIib4*cqi zJ9Ce^7f-8J@NBT;h3q(-#+G#OiY*2P6wO!++_l+e4Wn&5+V__oC%zaNVcSroonPNO z`Or2L44vq z?;QSGvwoHN-exF~JUb-M&gS9Dl(zD*ij&=*41S&iLJA4OC*4@^%Ebx#Q^J7|D}A=H zM?=bSG4ZfBd(3y0OgQ;zM*pSxh$%EOdAr3(`TC>j5yXoFk;uKi$%C*gOIQ6L7{2E? zj`g>C6X4o4x}W$A{aBc)oUCbreq0DvEuSr}@hQ|Cgy#M2DBWlpFd+2{3_C$>QZ{B}6n8Nw=aP3OE3=&fD zSp&7#MpGHcpaD81N=+Ua`?p3o3cA)5cw3Eeh`=fjMs@-0u>8F44a&tBc$dg04dMVt zrcnSvA{NgKa8slILWaQ>RY|yowC&IULXeh_+1Q-e_L1nbC7Vfch@&Mmadp5rrq+kZ zu_4bYV%h8b?|qbJPuLdpJCwSa)iSsk#XIlXl+f|ad=q6}Nt}zr9hwTOw<-+&n2aFf z#{IhIget~CE76q2W{>gzo5-s>LvnS0g39iFJP$WN7-I-hufJSimR4HLEZ)O%OeJIu zZ(g9_A-MK}|Hf0g`gPG5&Qc#*(JDPgy9o)NY zGLyQ=Oq7p$nRf8xXF)}B?rFFgzj>SXy;>bud~ZGy7@2E2MVn*gZszW)-8xHOtepm5&sSfQW=X9l zTScOVsgT(wJ2WL$q9Gc$ZsKkqSrI_$Q-DRE1D;zfulRgN14sQjsR7HE*}!H{&h-9n zx)>03B(-npiFD>NhK$8>RfB9;3-ejL^p56#OB-u%fap{kwu%o{Obi+spp_JMo$izo zSe9xpa{Xv~U?Y9lx4Fj%7Wxq0oadD@7#kZ+!Xx-D zfq(5n4s;LU+?oET2lY+$@1GOMTWJ6fJxA_BT4*FAc(VU!6J$p^Jwj3b66}~PG$_dB zl}<6c$aqQ}uD{zdWv0iaIwnN`3DO52XLBTxq(^7G`&U}G*Dm_e46~joVv*I%jwCf3 zs^dN`1&hZmz&|JxHZ(_4aU9KuNA5q*w(63D?BeotjYM||(~}6oudjUg!7VIL-GPD^ z8i!e-1{;~xG}M9D6i9t35^nmFow)Ai;XB!}f}a3iJTAyMY` zAExsg_Lp1I(dZ$A@Xl&FpGg&8v_bzs;z1S^{d2Te<_#4nEffQ~Oj}1z+rVlu*`)IH z2jj0Ov6@{e?sxhuy*3y>;M)Ov^+CJ1b*&iDC7z3cX~&BVP>U?hVxJdsbSeM6UqhnH zMv@}W*>c=^OpR=VuUoCG=AD$L(LTfJw1PEq{&hsE)(fM!CCmihkvk%OX~Qv2u_6iz zrCJ-N3WPtXgdhg(z6ll&3flh?;tJ!>)w1Dxm{Y=)Cp`ysy(cQ|iRq>6V`hQ_@Lf0V zxY$LsaAM6<3@&H_)+$V4*>JMnEXv(NRayu}I#vJ3sYQGn`Jp0hRa4!8ho=b#spKIftxA>jonV=SeuH`21crrL!(=$*( z)q28x`EGWF#RFL}ho&rgV6>H?Z#3Ys6{0?=e!{DmeNbIx%s{rF13L_t|G|>Xi-rGt zi$ot@y@_kwvDUV$=TeXyN&=I!lCUEeg|AJlC7>j%a(M!>uKgFE&xTP|R9mzAwo(b3 za~^_BuBQ>-uj3_D?@&ieQ=T5{=JJ`L>Hw&O+7Smg zLR#G5J1G4M#{%B8pOReH6QwJSlwSIC#1R&$eEMH-P1sRlmsaZg=WfH&U;$kTk9BZ)20}*TL2RhL`J;7F?BjF`TAT9V){;5XG zFJ|O54ytu^^iiAbOVJj66~z?xPYPZ^1eRed1`<8#IH6*SOr8f?Y948y@qN@SdJ9tI zwY{~cT~gaBRu~Hr*q^b@LkMD417!hd`x#GJPXqx7D)D}#?~E7X12dm0p`khc%P!_< ze5fH%Z4DznhL~2B#+gRD`m#N2>~~`@f+ReT=zGjt#jMBGy^wkwBwU>*rIOSZP63Wc zj#(^AV%YRB-Bg28+*L0DyhMWGAn zV;XrvNjM-+2Ht}cR2=-mFPHN+0?8F>m3bwn(bdRr=j0xlx-QxQrra9*-zps^1nRGoRU_ubIZzJjJ8h z1SDJt8{uDB>P&Y3{-VEE2>Y;Lg6<|A-dsLk{%VT^1bpwc2*6ameSr@6!G84*3y)D-SJa%scJhNF#gO+tGZW(CP zVJrvp5Z|gkSF_|}#(cL*1Z8hzXk;0p;<`G`YAlnqwQgJN6CZq%y80}FMzN{SoJYlNEAGDGxa%u@&{W!#kL2qBuH4Rx&{wjcMleprab?w{ zr@N@(&-d@pxjf^@W!b}ph$`?0(Ze;=>jj%+mKyh158L)z>7N^K-7Qi}lJIZwfXH?~ zz?s42d~`yfIghD(pbBht8h%)B@FY6!zAvf22l|y$h+Fvfh{lTfBkRC-1(i-$ld;0q zevSE@ zH|CtH^=OwLfpMuvrYk9k1V_=5QsF+-mBrGkTQn~bnGqDCa8grx~rqpD{JCWOnln#Y~}G^9430}P+e{InLwuk=^PsdP*AAs0b9n3q=H7o?)r&q*Tj z@nd2AF^h;q=j9Vc4p)h|0&w_D?2MeIZRH<0Wx|dmg~Bk4pVogdoFeQmH;uV5)59ja zA`^WQGyD8Ni!-oICk7DnF>zOiDT()fK0nImDcB8J2;CX^MMn zuBdJ0ldc4vg}&I zSJs31yUhC^tG4oTGF(a7xc!c@08}?P8(1L-+gV493R63)g{fu`2ZvQ9An9Bzi(_() z*&X;70o2N-9$*teq1(1DO^V}mdy%V~5U}pR%0f8aGLqXhpi;a%&X3bqM5G9)8>SXY z$=BFfBwPRpy*yXnp5^E*(_AI`O_&jik|`#JS8J13Ow%QPzYy5dmJ9Xsex!H~7*2nu z^;SWX#}fZ-h6vZI$sjYz;HOC0iC29tfwO%gUH|CFccXrT6^ekvpvLbe9M%%FI`uj`V#cu%Zl8rGa>>qNu)zBHqkIUv!|F%vVSz}dChii z;A7mZi;3jI|eW{2o+BU$d^%r6lP1GpCC;wLc74Oq_g`r z^w8!m{;t<>5H@9Ada!N7i6Zb*KIaqX-XM6GP!WK71|AZ^+1=dZvIOmPOL!ed`dDd~X{B$~DQ+^rGPdu`eQ7Q^ImH>%x zXCK(YJ4q6fi>v+)Ynh3@{D{9Ns0I0~J-CH3v&GKQGZ*oSD?HEr#zTU&v)bp$e{!F4 zKxi+GB|DUCuh2Aa;x%6a%jqe3M0O6su?eogN1`_Vr3%HTUlXiip2;$-BciU@n^jO^ zWb~l><%^VXH3}rzW;Gai!L2$|V(T-C{&zF|x%>lH2brrUWKUt=ALG4QDy)HFw|1_vZ3Etkr(0r$akyxTMllEln-*DjJ4Lt|G7foL?&~^;f zyhr`6FH#|#SZqHNEQai{I#@r<@>5+3c4H?z*xLl!1P8g9^3!p5U=Q#HJx0U8+SXx7 zamA?*?Q-350w_Np4vh&R=AkJQ^Hd+tRj_Beu!$6Uo?FrHm$4+IjI9NnNBmU;^hj$G zxs=V;`@%wO2i<~R9OlZkw+p1(5JqgVc^#X_@copoL`zTc0X7sJlZR$Ve0r?O%1vq| zaq62F%Z&+&BKF-;On5_Do^w-&c++|9caM88`%Tl3`L$vgdBNu=F}&Iw7e1TTkG(mx z$aaPY5lAF0;X&OO*AUNw*m7>*zqz15=y>ryv}LAJzMQcQuh?KH)ZXqngDXTk@7L{C z&X@yQMlnA{Uow-87Czd-K8t#2X07Ysvhm|x7`j5>g@yIrx(eOa%Yj;FmkyY;G=7|f z?VpXI_A#|MvN@#7m~wn&`-G6ep&H`SCGX#UFK`BupL{BqJ$^?UxY&Q($k=Dwo zPWJbYlwp!VutcU~qL3YpWLZpdB11s}GcoP8dgN0$D>h9cJ0Lo+k}(jyi#jz{8V1_L zs4?6)!!k!z2II357D4?Dqkjn+|9n~VXh{W%qGCXX0n{Cv$TUdDT2XRoTJo#4B}uTp z0?K>B*Yf5&Id0ub>x5clxJIc<#twL^pirSSY~aO*K3fR<2)sDabqENucrHjG%DoaU zx1P*+4|GYIAUDNOJEg3}7N>*O3`I|7CGymRinu+=O{Y+PcU7A2u&I5PCT;0L-~q)*WSck){KQdCTZ21}pS?_M=a<#-7o zx`Q_s6e|^|CVIQ0aqHQXUw|r^j`Bd6f^=!5#$)Z2kU)}EY-R4nhDyK%03USD*xhWW z2F46APQV+m@d}s;4Qd2`8-hR0Z~XJZ!8D~Tl9M0c*lZ$=xJK{sUM|sKH~9IuS6a!b+S>RFR}u@+eco` zt6H0!hd~$hJ#y&SaW|(Z7r9}K;x1b+aE3f}b4k>y3pV-8Ye>aqFlcJbNon9D2B7A2 z`zI;ns_(ljXCW5*en7`zZXMf?Rfc>h!1wJ#9c#OwMM^;n2i3C z#?x$nEgEPV%25d@3^4p`x>qlWksffzo?V3@;DU$h1BT-T*HU(7khDSxsLX>@N692# z&q0EI0-Gv|TZ;2|?qGl*R;>gJRYw7_k2TiS52Umg^Eh;lWK)}&^;_HM1rj7)=JeOp zVcn}vrE-W|$l+RykSa0c>J}Zw`Hzd!m5pJtT!oi+SNEYIMf6J4ZTZB<3+Pqfv#e{o z!KeRY7hwTo@a9ODi7X2OFK<2qZhFB5hAO<_MSZ9Be6hYl6B~7YZ7?@#ob*UCLRg9B zckh++_w8${!BU$Zqp^}}N;rH6G%GjBbL8jf&4G@~nojDYce1$#P5TUffmQGD%;qa8 zw%R$vnEfFxV#UBr(uH;BJE|Zg9NG{9iFTf?BN*Qbb}NppUGtF`rG;N;6%>r0J*?pr z>rZIzO)u0NbA9vbGdd_=6$l=MC^!!_%awFm5;bxPDOZs@iY#eVG(^6m9sJHS-=n&|hL9puf*j88V`G3{MhUlHwY9;fsdH8qTVQ%~u zN2&mw4)AABIB{cXi%Nah&ZnT!3}52AaBo>X8<2;lB4KMzG_{&ycQZSlkf%}j!SuI_ ztm^5kU$NeB(JJ#vY;C{8Xp=mDaUQw3d1&JHh_#)DL(?T#LHmqNv-KHuu#;&FG8ShK zQ!#?CL}d1}^uPY!!6qY6KUy=xmV8};>iS2a^YiY^j+XF1sudWDR24B(*A6O7TIm$Y zRn|!2vsEZ4_x*0A=RBzu>qu}f@pQF_l=_GO4DtCckV-yRrccm4G)?n8YhFzfzgM@Y zqXi{`lg%KK=l4>3<+$bqL1Q$`8=vF;{r>-6&$}T1*aKa?W3On{GFU-JF5w~f3R32K z%$weB;@iK`Sc`%*9G`?LAxDCbmhZy_Dw@MXBO+_OH;&!J5TNp~SL&%mq*pJRD`)p_ zq2YFWaD}m#LPnHM27ZuWTg_u`wX(FAyawkbDhw##_>1IR*(ekbx%f`tTz=TN#PYs2so4(5UG#dxoNHxyAwr5>h z95%62;uUl^XbuIegBXTg5)7xcBh5E7(RuSGx{@*KcXJc)jSC?)yTzayaUQ`f9XtQ{ zKo?wlfa!!x9!&ip9IZFl9+U)Sz51Edo&YCJKsY@m;x>-VLEMW_T5ryi;2mhj7LaL~ z%^BC#gep}!Sc1fqfce%{7~U0WLS02mNO`;rWKN4%D79V5F5&bx*0I6uBrYT%jO#}I z`YYK-d}q_V3X}qtl=rvgqnn8}iIFv|qe`FZkDN1~sdYrza1v5Ac!FPXO=1RJ1{+gH zX#U#XK)OC{mbe^j>lK&8@O{vzb8+|El?Qk6x0(`4F*{jlpF_{hR9ue+$H*dIc-(L| zb6kSE>@t(*9=t>tEnhLGdbrf!#J)k>ThUB3?w9sG4e{F9&y6`KO}^ZmyIX{IzpCoO ztYinmHZ&5A_;~lyjg=`mIHdBwlqjWFpNbYzG*d7xDH3YmBW>R#Nw=fqrYQVwPCJBi zCn&l(Ftx|`r|D7TZ_;Z00<1fqy3?t5P79X^0Q>N*j{=_x`z^OTYO;WS-vibc_DpwU zezL(V-7}n~ixp&e0gtbhUpK7_ASF3qz2fu{e@vp<<-chB)e>*c&XC`b->Pd9r_72V zchZjGKtl+Nj3(3Vy0cY0-ngk+@Q#$?f*!ki8tjrJo2V!oPed!CC!wY9@noACWq1ocUt60men zRSGG+>-ftmd-~`-R-G|p?6-(wHk-W!E9V@)OWUn5Ql5g%#P{m1oaIYduyLXJq=y(2 zKVQ!hRUU;|1$ruwJA1c~A14LEPw&F>Q`xwxPLLjk^IC$$b1LmFg~NcEJ2Zz$RwfxR$|7uB^!Ma2PnUQ^rU zNrRTzawe)gC#9$BaRn{vj{u8QH`$YM9?R918|BJ`g26F2DC?yNM|%7 zORN_(jrqQcHcFXf-w0MsZ*tIT8j+7@gVoRuB|HSEU}nR?s-ik68!w<*Kc=LOd=)qO zZnHL9ym*(Ie4B=?h)~9OZ+sI`#5_-4(;AoR2t8JwX6FO@G)-w!&l_?Ur7OAx{=V|n zRG`1@xADmQsO&I>GyRATN+<+I*Oj0o(_OgQoNH9?c7|ZY!*#dTl0Ck+P78gLzkG5| zX|53)8GHoUx5e?LvEBbBB5NsW7D5Z=rCz!sP*ZZNCfHH3qRP>?qLeZfr^p2gG;dK} z@JaPf=&r%a@Mn%N)tWs4Cm?V>MG%;vaGnln4& z*VSj=LkL9Y;wq`~M}$0pTT~_HkO&Q3vMwt(W#JiwHUaAC;f*$z25<>& zg7dm;!v|liElVQh*{EOXbsEll(yi6i%7fzEXHmbCSpMYDy*oia3%%}<@AJ&`H`T`Q z2M&?*1XHx{3{A!twtO^u=m$ms`duoAUAbgz$)vT%E$V9$eh4SRuxT%i+uE0$>nOx- zkH+B8y;vr>fj=_D<{s#RYvh19lWuT90ibr zLJ2`kROcPchHZ)4Ll}q6`t%36I|)d+jz&#h4Ez9=agoKYs0gsrFW42CA_~&XpT_`p zCg;6Z;r#|=HEVaVh;~ANK`$@et)~9vL5s>jC0iTMUMz6RRUt9t#A9(d@$?jJI)Kca z%Zt}q4JjShUb6s{E3F^FH+^nlr5EumC?_mLGfgtKQ>g`0frS^A)_r4Qo;&t;8aKy= zcW$*Ioj2-KqpFYp?wgDdcOKCJN=fk2qV7K{I0aAMuy)aR$5D zZ;|kbgjyi?Pic*`56os_r!??go4A=QHsDNPJ>X@pCQNhpwE9IkR<*Iv?h+isj1UN> z*bF-wF}Pvuz;G1__G58>G~kzYB7OnvEFx{G#Fyl77IFiMprZ3umU|4>sw+3kOaQT$ zw;ZAC%u>L1lF;MV=jHrYiH| zWu!H>^#x&0A?;y#3%SWX5O3Z$lC?T{f&h4~>ZF-=^zY?kq+ty+A(!^-8Z)kq2=S(v8~z8X&GZvjpBp`pqW zCKh@~M#x-RQq%w;it)8CwRCAkOlOZn_~2Xqt)34GF}PjTF$;?aeI!!^0;bkKy?f@q zo=P=G?k1#SCN%5?OSXO74YBbWE>rh*k@64yOKVyceGQfk(8+&`G*Gzk6iqxwyNa~A-t;R~gA zeo8h=A&o2UPtRRB-(Y))V0(p3K=eaaemirRpAtie9U`vr04HQ&P~GPp^JOrwB{5>xDV{Rd={NU?VBl z)?rkk6cg>F#g+l(s?c7%Hz2OyJe>z739YA+$9$o5oExXMuJZeinT&Yn;C3+DRy?9z zlLTahL==Whg^AlJa7f#h7vG$1 z%kKwK(6p%eYm;PN;#`$+ZG@Rvz6!G*WmbM69Y_@8SE7(oB9P%3U*7V(zb0&x<#=)+ z5h`(1lF|$kc~SqWO*&jmL6`HAeDz{P0BtbEtLQ8R+2NUZPIqn(2~*LpaG$mX1`&nHu9(x_dNQ z3FDT8cV7X5L=JRMhKjG{VeXviA;n|z;~g=@0>6bW_81X_e+;#y+PBVaMN*@woUxs& zU=hxV+pnZVj}Aq}&11RV4&^KoG}uW|DEVJS6sH{s?w150yL-}P1&eT^Tzne3SU#r= zZ901O^wOv>0!(fMO-%C4G=#epU()*c=D;au%A;hoORD+#WQi_z{nbgp5Nu9LH0}S5 zhi51u!_-^u^|IPR%e&<8NsD!9hR?>JDqm(~{%~IFT0zi%9dp+#N6u4OFl=7YH9l6D zU|MjP%m~$l(OHp*nWM1hW#znA9UKUgY*QFv7QN#a8rYa$K}SBV-?k2Jlik9_@b zj*cs`-IkB<7OY)}UDb(BQ@zwH6dxbatRcme^>K)j5d6`h_jJbbZ(qj{`v8mbLtdYR z3x&|L#S|Gojv!=!OZG%wOq8D26@kG39hfB46Kg{PUdtlK`hFSa%=}F+4yHQ<<^Tl-AEA^&lfCe&}nk{zZK4lhuiynthrX8CL zGR5d-Vu@H+~i(fk04grfvo(PbfC@PIav~-Niqk1+ED)aUpnf)k!Xi6`4 z0h6MBWRuc#CaSJQi85HK6Xa350~0#=^7-3_63nnu5Wzq4Rupq1cJxh*Ev@b^wS2&B zh1XUE)O81z!psCAtB}l@C)>KG-b1AT2EI*Cs*%@|oc44FTev?k;+K;f_%pJWzxoP7 zUu8dBB?K7h+>ZC# zYrY>o9$I-i^fSHmjs`ZoQ=@2`J8gt3%ksT^3#OREdeNr;lEYt?j9fe*p^kYkJ6srM zrW~m_922MA7pG~Y6&mjmYoZ%F{H$~N-LJD^(<-xL!r|4QW!5=JMGmpwu4-!v3ZS3J zKtE_wc4!LxzX;iJCO76qMycR*X+hiH2>cia)U)o*MD9$$Ul1rycGNCZvp*z6u?fgdqzB*&1Oiyeq?rCeq>YnQ&OKlcKVd@nud0$P zn*KKM>HD#M_d$@FRD2_SJ4DobDk1P~$@Qz##HO%>($EUJP0_R;lQzZ$c&Iaj*tnUE zSd|?QBv=p1u)+T_k(4R^l?FWLQ6nV|w;I*OsG%5G0Fce?6zVOjJZx{FDL^A)b?#5c zFAi~t<>XJCqTE`>+XIz}=Oary_stg_J)a`so@Y z>7p@MOusheyRQ)gyRAC)!_KtKbo;N`hUj;D+k&Dp5<%E_Px$FTgNYGkfmQe>hHWreHT(Sppea>E zZzV)u0;_Y2=^r{+GX+Znz{zPerT2)SuxL1R4Uohiy0fVnXoqo#&!SgZ9@ZQbL*>W< zH7<$HzFCKetUr8)7AK^zEY~xh$mf7nCxr+@E9@PAg<-7&MLE{jIM~Y5T)b6|1uQ`m z1_Ducdj7OnJzAkb#WKj}gR!%FrEI{37KW{OjTOAZ0tY%Um&y=&zO;zH1}vC4=O^En zi2C>>lcg~`R4ZBRA`ikq5on|AxS^fGdDiYC*sNM@RC1X>#4JD?@SpvNxThjQVCXh` zO6mtk$gqvg9PC!b*uNJP8Yu?=#r>1$Q^0F(br5wJc!R7rPLry#ut4g-OFEraa?w(x zJp9)BL1ia|(^}D_V=Jy{o0>Jh1j#;)yh&Id9(gJ z7w80uG)jO-feL0A!MT+-jzy z*PKSZ3R+J+qWMv82MMV?df&I`pbJjKTpbCn z7N#z&jC>~qWKwj>(+d>i8JOehB(QLQ7eWYKX_0d}NwtjpxcBpMU`*S-APXD{r>Mc> zSEiZ31O#l0TQu3QX$tVllkaA{Rd1cB3M#0EZx?sqYWaMi%s)ng9k81`ze!j_GIESI zh6z{&B_7nXAJowC3!!FjH|Q|cVKU~0;Vs?_!+tcRt81Uw<0<_Cr=)sdtnV(w^{asg z$TzlNN)O6wOc=zWGPJ^7PV-6%f)8QiA>wtGAocRdwp@dK0j@0iOI^65hPsp8g2|*| zfVHC)DmrZiHbZ51{{RE`yGU4IB}|Mzkc3}8zeWS};R z`O&c_Dc+*RG?l=$Cd9q@&fAha*1c0LzVa<*cA#ZAR-;+D^+q;zrL@1-3|TaFu45BQ zFq>iS)l>&OSNoD~Xm3MT&tl+{pR%kKPGJc^qRh2u2Qp+|U0BUiN@M|nK0$4;)M~78 zMKdPTY;33mFh zYIr-_2+=-Ci%nVh%h;{q%eAU*`Uc-3+IxfuWhfZR3^{pBFNX)k2|8zkWl}1?cSF=` zgXb7B5|V%xFZ(bkR%+MZAYGQ@uu~o=Ia zUsn;I&7WkF1EaN^`4PICR%+qduA@oZXmU1nvI)Dd3pZw-Ef_z-bOfxQZo#e;LtY38 zJrHlg7{wMzr20c4t=TC!6m!ZY-agFkT)#-UU6!ar*OS^l6r3K27;Ry7mUocFAA1PQ zB#&Pvlu*nFJg}zGEiV3iCnRKkX=-YGXBJ=4z!5kgLb&W-?~%uJ53+VccWz+QBsUrt z*cG)32(92Ig*lar;|I`!$kwN9-qvbX>J@N zx!CcC*sShpbSr(%)7?TTV@49T-7slRly{0rCzMwhLyL0)IMsEpGSot)nX0Wg!L)_F zW7oAeM=K2x0!XjiCs;F33V&1;vfFx@#JJgA!Jt4ar%Bo~GeUI%biq%H9FdX)A<}QF zcoIL~u3JzWTLGoz_{kw{Lf6qm1FWOoZDx68uzgQOJ+u{0W)7d;*H$cww2Mf0$d!6D zGA5dk`rikyG2+J~^Y5nM(cR#<1kc^8xCn=fUm_ovEHP^mm#b9%wvGr zmUe05Gvm`Mg#3u7DZTDlO=1yCwkYm7J9jnWj&hehJSU`iyz;p1NC{a-M0NeTSu6MJ z?ebLS0pF!Uz&!{a4B3!1(N>w+#QaKOiOeGQYL78OX3kWI2_8=>1pRMP$|z7m{!VgC zlDz4kysO%X^=vu?L&rV!v$r%od$Q@LfJIWM>U?4`;3QF9Fxh%1u80?{-bKp-&0s>2 z!TG|D>-0F0D}#5%!0zBw&Ha8qk-nf7%oW%vmB||39IH@)Iv&+r$JIWQaqlkvrmkTT z7GWs^Jw#2|mEe5bg}tVvL%&BFzIQtzyQ zCJPGPX$C?D+tb6UTNx5>z|)RE6bx3L6s_EnsfDOa2^fN_J(wQpu19gjGa-8KOY}g# zp2RRDHEC?_r^{Ij>}f5QJ75LOeyS@SsF4rk|B`vEo7wl!Lna zb%J&Q9}<{=77ZA9x3$dX2stmlsOrtPcEdf!#U-(ArJ;I{&$Qpqlt9}_l08@tilHFW zY+UxZH9Zf}Y}P@FzyCWXoJhx{u)%o+wm>Kbny-};yWexfN=GIu(|TW-&)ejB0T46D z0*PqsSn|m=r?&+Uxgj{w51O3@5LJJdOjQuiFbP@jlQ28zxTDvzND`P_8fV;{TbJG1 zXnA(;mYAb5h{2U86B@;oHJMPwaS0t>@ePu67Izlf?|S;qeXSTrP)`|fz;Hl{b*5HX z_`VNu&-o1M1*9^FhUSK!L=YAXS|1iCh#oUl^~uq$uCz!9e{HTIO7k5 zl_xc_9rlAyxiC zO5bCn=BqP1c$2X5hRzpDXYj7-#;X-?AOS0bckFZIYp98X4(lxUaP4a4kkqGVGbW#C zI(Rq9N$(bb2c`Lcq3^kH%CPSQrft%;&v0uJ=?%k|YK$s%7elnIqP>>$C=V%9-cVZ8 zcs>coiyl>W*oD~5ZxbClp=!Ei9i^K<2^pN`m>pTR5`ZLb(oC4>@smL1iMWc)dJqHo@WA$0)!T~p#A{W ziL*4&IQQ@!|FP^W3drmwi(jA4R0A6wOo3#r;?|$O(d)-*9PYOF=1L)DJ3)Yap*Gpj zoGI3EtqiW$hkyCgms^$W@oRkwb57%FM~TICItvFTuHq{fE^|!>V(OAhmn@SG`pKE` zZw|WkcSeOHL6`jn)2Aj+=Xbzn3!*AKV1t?(2X9$&)2G|fkovD$Ml8~lf3gdMr7WV^ z>CK@`P{!4Psd}VJP1f2_qI=8Oa-ULYBA{(ORoA2#VwsE*%MwR(dUS*_nb`4(Ls2yN zH8>Te?L6+e#m*Nci>TlFy?(6sW8u8^o}s!VXI!Eag%o;AB(LlSJ7;}A-GZj3;hYmP z@hvmq|0?4qk0k?w^{>uNioxXIGj?wX^3$3{`|_%52Zz1%rH<&V7XbIFWpp8>O7 z*WkO_=V^4p8bzeG+g^6#BqvVRN);yjLwq~yEf_5NIe()rHu9Vru97Cbqm+*4_RwBP zCa8T>BBdOPZF^SPxzO3M%Vo**ud6fm*|s^=->m+4r zNItuG>>-DhgbAIn7r*t#2mgt6n`8C^Vr1&~5|)N+!US{gGvjpQ(e3^85OFs(?uzSE zY>?{t*#aCi7Q*}aLFsiMlGb3YUJrK2{^)ew^6pp7>Cw!GY14W_~@RgZs1ihL>VG5YnIXNjVV zcghTO4(FcUoCJ+A52>>#qXNzUXk2=QDs-uMHtBZuO4_w)m9@D{Rj=;37Iy1OwlZwS z-(#E4R0O9*LvMdNpiNvEOtGfs^~n`(3}eewBV-xrRwN*ei6MJND)8w3gi?7C15Le=iF;2ocgPpf%Y;V8MoyQtG0GoDZS^{d~!#=zbwl*mm zL0Jj;__#^*?zbGd#EbPR&Cb@dG#E0>6^&ScHMz!?{)8`i!t^vx+HxBESFNHHfh!){ z$m|g%g`SP~XsHq=Vlj=V|8{_s;Lq z!s3H|MV3hK3}amOFaZh4(*LIa;7OaPU$E!aywRu(OIPPX=BwA$LsdXXF_;V#i90X@V%pIl|z>HnNSc0IQKo!``(>11qxt)R%j)QeQf_ z35p8o!o}4}Ir*LMh0KfF1`|H;u9NZ{hxz9d$krPvfh@a$yXV*==uS_z61REi+lFCU zxYU)ZHW8Qlmm9)O#Xs(?DB)()wwNH5Im9l2LnW>=|4+G+&@$u`?<2-J z8>%+B1|wtw0-Y+?JEnG{F&JKRJ-fxyY}}6kFA&7SLzr`*56dU0Hw@xEV!(|M(V+l0 zLB*;OY9SuZ8a*l-_on0!2eyPV_Wk2KaZ{W^1bAoemXLkIL|MKWX_NkW0PkI_&*BohIzL{VaLM)8RnUC` zI7C-{4}lvns$TnbOI!40G*}c_-GWn`-%8lc} zf2{vpRQfmpu-c{1GVIle?m`sl-_vKA7#UYflkN@frlIR&XA*(j0{(oOfGD zTH9W9yXcLkMIB&Cpzfd1;((*1P`&%pt=_vA?{(9KqPx^AqcwJE_-j0kh>J+-haQ)5 z2f92l zX|1m}S*`Hg%Tulwu#FT;Q6d_wDW`$%UP8*|G!6gxe9Vz9YQ&wcRhwFXUaV`nDy=WB zFef{UR{UT!#*E;9fJi;H%xD{=x-6ysurBeV zlW?w;tvSu(v_@J)Hs@@ICI9xBN**2)xV0Gz?izM9!=8-9cMiklTx0=80Q|| zDX1W~CO(LT5jR!OAo^f0t@vT_p87&y?Ik!7KB!GF1l zWR2~2fZb0c7=naGyvn+R)oF~EZ@`I~^tmBQ*li8{@%1s0d;u-RP+HB~<^eoi>Eatu&ciH{nZI$KSU~~j4uPCx6{2Gx4 zKq1i&CdH!MX?PUs6^KGLh}%Fa^`^--bj+g+iV1~;xfF@!2>zlm9@JOFC_*=sOla)A zVoFUD6~ZX^rMh&pDTH00K>W5)$Z<6|}G<}5&?#APDZ_C-!9J8QNVA>PTc@{{zNy}qkk)qg~1R`8|>*Q z{eD6vwsfZ6>U5xUphtf3+>4r3cgUQUJ4FP9Pkoro5UH)mYC-q=5)b=p3r4td9Dn-5 zbC+!Y-TjE8E9J78=;W1ghS#G8%y6D4U-#Vw`8QKXzL1w4@+gXCfb-fSeR4_d>Z^Nm zpTu`puaUcozqj+`H4~gIDiR`W( z3W?_xH1WRZQfv-(t^UU$h z&pP5}4&po+lX~BHVr}{ls%^8 zs1YUjFF!)ZSvo;yhU-Pa@{~OAp^Vo*CTQ3hu#IIADF+?xz4|Bs^Hs_dF3iK}oF1-d zd+7@E*+9@c+jQ#FN^8NQU{qj?I>z*@_(i(ICh`gGUv#vL>p4iX`*R_Qe?=Km32|yxhrW#XFdU(ZE}#a>2u`{*jiUHkGS#2UP9V+Y)H=wQU5F(O?(SaxX1g2&W8j zgIeOytzX{&!2=<$F!j^lgF*^19W+ER1ENtbm4ou55ur~52SU|1hA|Bykk+&>k!rVJ zO7&GP(jir`L3Wp+?|HfH9xAM%)MeN&HhG`A0loM~2at!~&5&TaH(2JtI`0(xl!szd_ZJv;;gVE_P04^GsEq%3cn%sykv+xLO`{ZZ+6z z>3A92Vt_<^`GqI z7m=vrBowwzHNYDR8YFZ=1)~i-7&DCPAU==zExzrT&x)p9PGi*d7`vD zg~@1UDZl+&enp~V&)BZpDKZWCo;6c^q0(JOB>y_*Wy(d^QxZC}*yPgK%YyeO* zL|#&-4CBlYTT6Qu3e!!$A#I`WdIh;FHXS&)BwmPT{6ip0KFRV{*PV#~m@U$u4pyJTmf*_#_k6tM+^Z4UYO5>j^Ms2Z-=?>NihOYPgu$*Mp2yxq7+|Wgjd{|$}J_N zN5Gs(yGzMCa<MX>{UC6kH0c<`dS@OTuNSAP_#HXAZqFw;aF^hV5NQ*7)EC0Cy*o4hBqmG z!IA9!aJwfoQfK`nHuS97A8i9t^@**b$b&$)kMq6mIaa%|X5Rnma-Ji0CuPIO7Jla5 z7Akv+hOj`0Lvl7(t$U;=-Vs`1uPgFkC3%WcEmvIA|E`{NiA}<#neK@P1v>VyGx|0@ zE5JTqz;qs@OU=M!C5w9v%Nm(_q%E}~@Zegu>Gy4p9VxaoWtBZoIOyz3_1Y_F`4zW|nK zt*DFWEczzkA)Oh;Ej@4EFSUii_uP>xWUjq!k6%il#NUWUm)bIy z0;G{f5N#Sw7KcY7GOY>?Y4i0F#^GxSo?0y)Cu7cBa2$EQ3fxxzHjG!{oEHzCzu{Q8 zcev=<9~VuenSc$P6f$}K_OJp2|El(n2OIiIzOwO#lHovoQf~$gt>fTSFork5Kkj13 zi7ommV=YqV2Z50kM;2i~Fu+D*-pn7Lce3vL`VatD&L-L1gzUfYbI$SR3Y}dZ_Hyp` za=YG>IqmUq1`u4Va7(E!zK;*bX6Y{!#Q%ajRB1}J7C zv-wCNEDdm+{WGnZYWbp09{^)U@@ZW}Y-gv|s; zIN!>%IkXN+P?u>%;M47c!G4%m+NQtcd8+O^&MGVSL{W}`3efP*?(o8n2zfMn3e)9LvEJMXTEpRS;qF? zA6PYqO;bAI&#$pp;XRLxAGQsLsVo_ZdG3;|T8 zY*{_uauDtmd$J7CFn|*XNd3i1D#W7s?JC|T^8eH|jkWV(@r<4g4jj$$!j_!;qrStr{n&@}?zrdK6W2&~8S1xwEWSv!xT2QPzZ0D4Qz6IG% zv;)T)`PKfe;hs^E#w9WQwC6!uLhl(mAq?*1K`0JKnCX5mxU%&TKa{wRj(UW7TL@zL zVDz?Z4Ijshg%8QU(k9+bGQl;U!jq#MxaSmrE9KVOQwJ_L^*_pdng_Sfip=n~?0@qr{ug2x{V zUlekZ9`0pEOH@mv|JsAh4+?opAVRY(AEvI^w6Br=iN_hRoTE|qM;wG=2r_41BDl;P z1|2P5eqy=io7_7;Z3+jJSFO71O3(eah)fg0x!$<7A8F?s<(`(9B18nt(kW-8dN8z% z5bwBh+}P;Bf+>cL6_&+H)BFat9nCwOexXV$Fs^#`ZD=&%B7x?L01(ohv5~MT7DM8R zQt4Fs@XdXthcSY{@zRBwRvi?Sm(lw4vn?VO*p8(IEqsLOzkin-^D@7aF|Oa?1;P^? z(Ss#kfwTQKylCCr3r?^xcS|TnCV-P=H2tDIEF@qWQqrsvMS92?Z$*sTC2&-R5n*J4 z;pL1CzFI3Ed*vNI7yn$U44NcQlS8akW3#6rj4XYZrLy#8@~OYyD+4PF?{Wjnh&Nk5 zH$J{D+6u8{ODDzG-U%(lTV_7x1bb%kyEeKPo9k4pkmgKXs1Un4SY+#TzNn5Go+Ale zGBQQe_Xu;pidK4MG||VUpLJwcvZ6%Aoj$GCy_i%rcNX;YP;;q)KJ4RISVi-UfSB<> zDN3L+DNE<_wK!>DwYMQ9L7^==)RreN^si*kRA@deV`OVlJju83y@+>AHL_@zk{v9p zQlrM`%4!(VaDjGI$lg@!xKny~G8@%Fu^Y{CYcyCEAJb)IH4&y)-0_NY@NS27B=Zay z_i3E0CK&#O_e}ZjneHWvmh#QaUNYLt^h#pV%H>R@1a^=v&*#pa&fO0mHt0@s+_Ag3GjZ@~_} z5qk{f!(bhe#8201vKFIJVf4CuK}k#Ci}%)?v4H7U*jQcQWJ{WY3fvLPvv2>jhNHM*1(D{#XU6wdIMQ&s4;1jAiKM0 zRw1#>MJzQ^81z~#bc zogI_&kQhPTkT3M6W2_|kk zi2-(u8p~Dx4VsOT2viKMCGueB#GaUL7;eu0+Rk1-Xi)|p%p^ytwu)3FGS98@a*D=L z1Nbbv;n(U;H2Kvyzw%eg>MOYF9l!EHt8Z*=zf;106w^o*H^Wh-m^tZT1XZEk%g^tI z{waQtFZO2t2*e89?|0@5ifc5!Nsa{r?c3bauCJfV*0U(0ydxg6Ln?eY(J2p3!Ak9K zoJ-loo4Tz3If#~lrsuDmgAzW1zXdAlYW<3FxF%IQ1`|*mz;KhBI{~+9P|ErpLcM>Q zdyaa5$8XtM!`A43^SLMPs@Jc+4Jr=U{~IV2$`8#CiP25XsG$D`W}Oa#M8v(9Ax^#0%6RviDU+luXfWL8WBjO>O-N|G#dl4r2$qxmI4BE$&m~t^X^u%E8@gy}4fhZ|T;U)ZOPi`3-ro;kkZ&ADNQ&yl#Ol(}+=k5P z!gvByo|OrZEQ<>eEdxLX1}27~6j0w2T$WVQ5egkJnEtpY7?2rL@UNt%1X*BacXW4V zaUH1t==79q|MXhw?08cAZH~ZHSvepol(K(%aTJAsrY@h7k{m7{Ax#-HB5N~)3v&|y zMJL9FCI*lxb+puNwlo=l*_%9I#vdo3p(p3xNgoI2(oa@EPF6+I6#wva_O&fwV_A4A~a-$6g7q7UmNUi4q6LyD|k}ogEYF3j=GIrhWtaDO{_RHDt{_Ds5&JNLg+cqe6+EA&6eiZO4jO z>wj*m1^&)aKZSu=_wb=CScb!$uFVu53o@o?=1}g8|J+tEYq|q|)P%JxZ#6r=3N#*d z@M~Rw%0rE;slZ(^?d}un8t^LlnMM@A*G)JOn=Fw~vcjw^ZoB#B+eHxL=mPy4Y zmOsOodGl)pGn*hNyBQDtxAegwr{Fh(yGTbC+bF6b95|qsy4SWtx=?`JOmA&MI2g;~ zHhTXZx6T?J+D|@ja>Al%GwCF4rjhbGV`6c7@bU@q9*b#IgsFS%F#`H_K4lX5p6F-9 zz%Gc$!Vn#A$_=qfX(;q6{=I(lev2Ee&@YaDoAyzQiR8wo@Ja38+~HT;XbY>>?*3C8 zL`d)fROjNEoZofgV}7}C6={zX{`-6f;X3|`K?+`;v|F^Yx-2mKQBq;y0L+&%tuR9z z=NjCQf#vO)BUtld8ZHYiK${Qf8jhdO^0+%Cn^fd)3?yHAF=`NpPj0qpH?Fd#8GL7N z;5C!W5d5pwP5E;DTO&Xyk+_mw(r#oyO#ZP$mak3wALU zP}mHRaj9X#@fO4Y{`!LQzEL|_zsgbYvlR$oC|0owpwgMO5`(6X@Qs`%`N!p!rlMqw zkwx9(Z}`yKXOib@-r-HcHx4HrO%Ju!)}t(?{M@xbce4hCb$q4S(98Wh8tAE9$xU*H zi+9c5zXSeLACDx^MW%9BYC!2vnhGgT;>!fVM0osAz{wRyRFv0OPXg1K+)YzMbvO6z zGyl6lvOYAi7E2`|wyFw9Zb;o&>jw7EL8Y z>m)>B5v^u(vK7I@70U}TgDOy$y2m0yYg-FJ%%5*aIkTTTec{rrT^Q|{fdvwu$|qLt zwpr^R^d)3SdpdCNmZxIvW{^J$;T|<75!usnd<}{Jx~`Bk{AurTTS;tE-L2;Rl!g84 zgGd;UtWvt0+XJlTxprEQ-8?{1W~#Cpev3Q$r7=6On|B{VTN}C7mm;U1Po>G$rHWMx z7*pb!B~!|sCLfjw`E#3O*TRS)U(4GZGT^kweDRia={2gvT6A^AIQr_fWZEeC2AsH_S(lUFeux%^c*h?w+4s9c^pM?!Ojd9Ua)$D7@sZWD4s7H~a5x3h9FesD#|V zgy;4Gh8R~UrvR6J1N!Luk8Rr^)Y{Q>bX7=KvCP$s7j7)hING%JreGhSBQoUTFFha) z=Zwwynvb%b2&m7&lETFjBFQ1s0C=VDh8fYuC@$=>RZe*bPPbQ*|{+ zd2cGkDkbyH*lFG}zFrb)A{fTnblLhs-r$QgKxJn## zb>K6$AM9w0?Rm&agiF;!K;SBmMb+mv=5pdcMrS?LHpHV<9h?dsq=%qZ@U3q(8-K7= zfXt9FYshTCLca!0Ts68qVZ8m1zBKfkgd;!p!Mr@is1leLv#Tn6UNZLoM$ayx`%W%~ z#(FZ7HtDS3h`MJeQ8H+T1&2zv&`s$@-14O1aHf;iJE|B(CDRX@!gJ|JJ>u{;lVE;A_5V2BP;$3KH-qjS0So=i1)LpjgswK=vX!+ zbS-wKg~<^}OJNebVHRCOyjPpRk%)`P&x;&Cz(i)eqB_{nLzHI=%Q2gE4UWJW^oBD#!loLL=H#y<*=DsU3EJ=vfXPYO#|EZ^%fHkY%OJ z4LS}Qn@5>9?}7OiDVV@J4=D4-Ui~~q zP+j4>3((7w&_5jcdHlIc(^L0RdOVx}#T%qmf%F1SQH1_1&b?*%Om*Ie4*jAXB zDm;>5w@l&cd9jMc#EG=4YOjYqX8)&oI0l-6rxWrrPc{HPZO5W2%m2bG(j@psIs5B= z#e8~a!m*`2M59`gBO2cyft^JHd_4OrtQ?K%jaZVRST6-}mE`*}g!u2nawbj6R+*j9 z{Jl6;YLrr%nnyWmq>5Z}-`at^8;jzh_*GZd7B$-D0=ek9QdA6FF?hzVcq+ieAox$E z`wnh@Ih`ac530QnwTE9ZN}Cb`0uSX4qNpM%enn^qL6%I5lC995UYT!Q?|cF$Lbw+) zN@)y=Ri!_6CjUg(l_tm8B@#EU&Gh!7>Y+Y&ctkHGSGL>^M6@VN7I&-@YGBY`ADvxo zx5$B9&83d%Fox0ucB5A%nu^rA(R{c*84@B!CMb!oDCnavIcP6Tw45HTN&bT5V+_`Q zQPcLNf3^MO0!+(n(e48WfBL}jeGPz`so5$%$0(T+u;vyJp=&A1=4AEs==_e_N}c6O zpk-q9@=+>>Ku3gW@uA$FDb&_9D1txY&76HhS{z$yea5XtFEq_zMoXT(jU7obUg>JE z40ufFci8HfkasELNOpi%tvb^9aXMIfJ7&N(`>}m=O9WZUFGuiu4J;YqM~-InB_-Jr zGX!5{npcHii754i>ZYkMi9a;_AzwW?hF^0Q(?c+fh@(-8O!fk;5<;>BP#1Uq?w9`9 zkR6l!!ttHje*oMFULj^W+(u&vX@>8BwZWfOOJE=(CBnVNDO;NmiS-62)t*|tQ)(+h zl(JRBcd?k;fuc;&(N8C#WvF``T(ggzF=|NNsoCJ7Q>0F`40vQBLJ^H%`k*viD4 z<~L-pBP^g5BE?L<=`97UorTJsscF=H(CSClr2)kPrVBh~$Z?rSx+$OTB@XbLuoIF% z`CUMP-qc@uQ&I+v1V24rNrRsblr$XUg*n6TXM0a^=Eel|rrx7o0q;T?;EMA2l?beJ z&Ibf~_Kyc41NhJ>V%v0582aZPts979x_1@h$Taxk&1H#vw^XK|kMaeU!vTd{q{* zbXaL0WY66jL^=iMO)YnimAOCsT4b{?o@seWq)R$u29}JE@1{wZRs9{ z{3ku$G?FK~4D0i(Qjslrv7!a zT(UD9-k1jqgy2@}CU7sFaX}Ta47*hR%5+Z*J~Vw3mjYmuWbW(5BRQImkEsal9m3M^ z5g$y>A>nFuj%$O53DQ2=JMARM)evArSIz5Yw3hmSMFtjj=@Niu&-Mq@}_?5{Yg%m;vK_Lk4T?FY;@>gHMVgZz0OMznU~!{AG$I73ac^=yuPTG-fu zm+}`;LmlW;`Lphv|6v53Mi%OyfK+eu?^FS}ynsvIgm|<2kc%uJ?C>{1qZiya3x2)9 zWb1kMjlSXDRV&aT+;e)#!bf%kM(2zoX+cHD7f00(G{X&_sm;d;ge%Kj${o$Tal9=E z(F=JPp190(a$!uC>Mi#c96fZQy5a)H{n}pi9YDsJK$ELLe}vUyY%*@aqlMB~C3<(L z9d`o`$^H!TFiAy@2Ema8kP)@6lXYO}iXqU0#WtZ; z@5dq{u7Z~MFfoRZ8PdvbAm>GjK*--VWDxNKWgh6wvQE2_?YlKDg z=Bv;rN2)S(A(~rajc%L+a)J%3Dx!muM&>T>v^UGYTwVzTwdK}i52!c*sJW%iXiKI& z2bcu<3F@rfgofcQYd$zy9m}fXeWf=|Cc8RkV_91o9M3FmMH(a#5w=EHIxZ+G>A9@l zD~Nj53^Cxwg=RFEkTQgrgoqmC%g`euCk&y_~onrZ7Sgpe{<#ES8vl{=J2@) z%XygGBl4Q!B%`0dj!0n{iODqM_EEE9kdTs32jat~W+*Roav;s+J4lq#Y5996>S9V}wpE|cxc4YoXK8~YLitwsYqR4!Z(;y&#=r-*2qtF5Dck~2{l{P) z-ye>1p@vCaeg#jYN{4Y8L7zd&3>lYMorK+F1x*lrshc>c1srg3F(LCy9h=o0D~P0%w*ur0 zL4QHa58vb_WXQZnYX0clfr6e~_dRd-0f)J>EDnADwNzzMU=NOeF0k4p*0WQm2iIMn)9t za8xRzO1b-_2gGEG-Vf#eZP+Ket12Ru*3APyBjbapnvGsIY5@di9KxQO(VUkPu;Ry&Dgc zT^K7x-dw!gh0##y5+6Z)G)EkAy#JmSJ5M=1j^FA(11co92xBS{O#ojKTrK375lz3TA3G?mYJlb@Y9sHdx}Ub<=@ZFLB}OOd;kq&i$h5 zxgZkg*fx2`89K2BgG^I&;KJaoQ(*X&U`NwhkKopyMEe%ikpe+?{6YU6>GZy%I^8vI z2l#j&0Mqf;7p0`oa`ED?AJbD#=JvTs;2W7boitOd=CJ@H|xs4-{iE2EbyeBafVq1 zH4V|R;4u95zHqSt(#aT&b;!aH>oT`%8Nbu`?O%tg+m~#YU4GMwBGoTkH zHZ>lly@8+0MZb|EYh0Fd+Ea{E-Xmcc3kl|@umWnusx%zCLTw3WB z{hLy(#usz^sGFG2jZ7?!5toe`oe5sFjV8F4)@yE;!lZ9cs_y+l(;@e|khu|9$}%do zMnFvreBn!?41;r|J?67Azu%wfD|tRd9Q;O5vf`*E|3arM;fJ$;a*TlQaN~n7TBAwn zQ0Pch<{0~u%xG072{9DJjMiZ&q3i=ODBx&kAEt(nw!i`L;@0)1#84;h%#?-x_Z4N6 z3L;O$ai?7_+hcYlwjqyF7OtAoncqy1d4rdgmNHg$CvUdXcYp*g@XTmBh%;#~W_h7R z*Znp)+biCGDzhQ{=caev{iYjeQ7!JHPbXql3WfGdqH)n=m=-^~1kD^O){W z{%ToC7+YCq(NnuRXTEYDQr=FEYAsjk2V>e}AWUSxI0fkbnp3vsnrNHv&ru+10Lb88 zO>_9}P4O_^ylnUluHJUU$6^3RSZ<4TZ41a-GgepefdIkh62N9B;f{fwRRKCFt+wU4 zD^MxEJx0MmXh$%B-R&I05X^9@6N`m(s7F15m4~}>`K<% z%On!`V(+K4NqI)flsku`CBnW1q)E@ymWuH&)13ekxD^d*;5vS*x;Yq;=0_UwVH#$! z8q}6Eu;uck$hZT6+8C^kS{MA8-Xi$o6?_h{%7R7?;1J{Gd1DbjoRHkm`+6Fr;#Civ z$$xv>rg#Fy+Xu43b&z#b>V7(p(3YBm-F)Ak;A1WQ05iIdcmF0cY=A5J)%q|&F+AQr zpsd6)r&9K^Th8J`ft>4z`$ByeLy;j{arm4`IGN+-%d3f`CLG%gPGiA4Eb0+wE4))FF)vpYk`H)<)lN#0!nG=h~< zD*kdX%YpY4l>^oNO+G%&)KBm?X8j=&k1v(_fCDUTEouK}S#~jj{j-B3=<=MzL<-yfzYPfa&=C>Y&U3$SJaHsa z<5JQ!s(5Dxre(||hlj00b?CFUB2)s{vDP>Tl&G=lQr%%%-qb^XiJtW1DUle_j)$J2R^4JE?Dk3vIh{fx*T_Go48&zwNm z0iOkcn6EzCLFVpnbDkFqS9*#Csi;hC1>8Z|ah-QjIkHuF@(Mr?vV4<-;qD=uDlFT- zC1huRtjdp&G}*9FD8W5;o6Y#8=ULwHIA93>j(oWGVbqP)W`VL^^}dABX=dU|?`T`b zLR^|`?Lx#ba@{y98ZRg`oVc`UeqA1UZ}SFLa+N>EaH+s4O*2G#*oyCr`$tlKI}r@Z zR0$zIH*$RGX8kLbWN-fQtVTf(4$O!>-4JL&nqbxoim}>S`}8x#vl zj~4(s`~tzJ)JHaHD<#BR5hQ}gV>#2sg(?XA#VZKgi5Vw@8Jl%Gz&|rsG)vU{`PIZi zUQV@pPwqB>7B$h-ToT&zvRuaK#hLeJeU*vB=vLc1un>&lOoFQa3O1AI-u)8x^BQ7o zZL7L?Vy7=&Z;A#Y6`wKsjM1XK57KWUzy3-wwIW@i3cO9KZB->xgRFBn-RGyS;55E= z$&moVIe*aI#rfyifzl@NeG&nR9$mJ{!%-|(CXEWPR%sd_(F#QDdpGi2%ZC)B(uxaq zFka|>iAb7sgd}!9s2~m6L8c^>bPt>4p^+vB4-8Cxo!ZMT_RoInG7PcRfRE8k%|v-$ znz9=RB`>o#(i8l$nAKn&>f_)<3#A|QUuPNe<6Y_t@QYZw06FyXYP^P4kka_;pCy>XMVFe?x5*fOXGFyE@DEe33K8@Rnxts zeP^;OuE9x9oCo7#Ke|rT6`gOEhPW_X_Y}#1F(>A0yB=Q(k@kaX+$HW2&YiIOPWls8 z{UELjSJ6cP>#+^`J@ZAtW1KNMHr0pVs4$|GBs4chK>cDm$U;BZHkYFjB$)}{0l^}d z>xwD=pQJjEd3ji13=k$;=fM;uAcmtSHf!9Rw_e7X)#42!ERI5Y^Z)yw>b5qFdTPaNQLp0LrB)qmHEPx;1sUq zU_x6lRTIY^Idld(%=@g1J+O(e-2prYiZL#V2biwFD2Z{;{KB<+;`VAlgnQTx%e>0V z{CBv4u7Aa_y$Y0Vq-XPb=S(AQ69MeX(dcMQNEX8s! zPE8)P!R>yVqA$320;T<;tt1I!bkl_eF1t`!O$lmrFqe~j0=lTyXH1rCWBjLViLqAc z(ODcLl2xb6yoA14mK?`e9}8}bcE!dRE@R7%+!XorUYjGY(~5b0n&j9wG}Qs$5UG1+ zZJ}%<&0><%`a}X0b)>}~D{BqDY`!zBW^$NgbcKQJYM?#i__{qfA|`BNTS;09AlkyC z^2`ITP;jTqi&vK+Ylab3o9Cjf+#^- z1q*YKKuh<9c)22fxmzZK3Ux2 zOP-1xAho4rsh!VY@3Dh7Qv4xez;{oZ!2Wd{^!X+Si-N*_t~(nJCT?HLv$JZ=g{ohW zCkYi~<4Nd|&d!&uIv~~X7F08mBxf6G0kAZY$&p&%|KiD?q=X>q)$$H>W@}8aGi#UU z3+Zbl9_`*!$jZ1@BYOSKK0T1v8tzMnB!S66vq#{SRI(>u(-RArKqj6~BSHi~84JX_ z!_m?z@~Ww*{V?m%#XG?;%aOLdP@L+F1oc=HR| zL#9B%h*n>eQNg^)QxS}wFXUxwC(b5mgnQKHh~L4xt6RT`{mECK6%Q}Sh9N@)=IMrF z^;rx8X4`VsCh4H=>!Vh#hR+%9Sw7+G6>0Bb@tjbC_p=X zHkyCiyN8Iy0mJu4w^&aL9oa^s1&?*=i$3A3*>(d`v0?RBjh3UBiEz%Nt)$?<3fT4t zcJ`!qPfXpO)PFI0Z&kP)IAO$RY&k~Y*EsIoyn-LgJFyn`O{x1U`AMkWy%6gu*HCu@ zG6`apSXlo`?PY!VL{yY&Qz`f2vzP*mthnVmco(>z<(p!s=Q)oi_O^Jta9)frhy5zN zAGPBm-|>}4py2$=dX;+15He$7-DJF3v-7|@N@~G@qF(-z)fW*#F@p%P7oz}Sc}xic zSb51%&AW?aAdLk=gbr9Z8c#;=h-qDqjNn+dSR}0BG2Xc4>QMt{A@dA^GfrvbeZH%~ z47ZGMR%2BG2r0T|nigb4Y`HM^_j#d}wbEZ@PJy%$9`gIU_dN`WHI^6v-nB?vo@st~ z0t;btbCXcLCh4 zpfsCV8jN+L8X<0$sWoTXu@DY7RrN3)mW_ zFYB+-+SL5LykaInGKuUpPhHKgYR+I8S=UZe*y7^5lEW~A5t`_WNHZ;NPWvxxsRbc0 zohIUEJew|WBG|Cq6u$2_t38q0oa%D>451rE5$dFfif3_1r74KmlsN^SDzRPJG}-?BC%K=etXMT2QXeefr=olQl! zN)BdTTF;C~`oO~BrEc7Y&f0+5m-So^4cB`>zc0^2WrPfJ10Ciny23fbb5vcdN0Slz z#1~}4MU?m@?wt?tzdyp_Q^&*LlGp)l23GEQ#ICG3S@h9EnSBzH7@0N3vH$!$4>n<+ zd~NTgS0>JB64HAV!l-bC<*2zDke7&G`TL8rI}cgoq+bq2%%EXs+*<@p87tGN`Ac`t z&`wh;Za(O!lgTIgK@{>ahDHHr;a4gCIeFH96bDIFmL~xp zofqixkxT4kptre;COi$6Q<}hv@eP*~T8QU3CyiFZe5c;2yQnr0max%C;rVm?}n#Ik(O*fdp& zBu@g8l{iKL_wzk)J??3v@)&_S{Ctk!8iN7c6!w&SWb>R`&yLd+UTJ(pT?tiQlbK5`82DShz|1d`sR79fe90k z9cAnkUcwaG4&wdgiNni^i=(KV*TC3X*C{0LHH zTSOXf(g#DXqknpRO-Z_MgcX~K7D6=`l==)O@2o_n)iOjwe5V*i3@L@i3PK#x;9;pm z{;N=rM=Fv;m8C?WL!Dk8+!UuPI57`*QMsrTZYC_k7=OlA=a1kuQz8=?SjYLVHsITNfc9--R?U1=J zD&c{|wiUwiu4qP~AuX@Wr{P#rw5Sg4(Xx704@aik3cW791&>UR3a~SZGs0#Dc24f3 zudPIejYx>l?A;i^vl6>q49+ZSLeCZ0aB(6Kk3~KRhMa6lqON+{K@Zy&#pe*uXiT2J(!a^~F5$3XH7;r_}AD?A% zDJbe_j64d)q?-z-)a#F4eWBNgQYsuQ7_|ngs4(83GZEk0T~=F&z$n>&WZ>}Dgx*zd zlcd?=kLF3-eZ;}PSm4%bX|hg%Liarcw>z*t{}iqI)Fe#WVCV{AkXMgO132piSl1;u z{qSKjx&%$+_fTV`cHqz~Z)6WMGqKCp%K1h>l`oyt2E!eIonSvbbW%&o9n$R7$~U@M zV{GmV*EcMv1ibjopv~BtETo0lJ>=X)6Sag>d}eG;Ir_n!xVp8>x@K?=)?u*C)>AFw z87Afy_wHAb28q%`v=NMnQG?lX^$_F|*xc3p3qasd+Wj7h`L5;$=~*KA0TaP<`sI`$ zoF)lnf3p|CKJ@d3-vuak4bfO?sXK>vp)yjlbKxHn9@sVpC=2nXKD`{-$OB?(gSOn* zZCH|&+B<`te(s*y?J2&-8JvpzxfwFw-kye0JsV1;C3(mcX}iC=wJHp4rz8FjB;xQQ z4Szh#$ATr!gIY=wHwn!|`sxAfV~~lBW^~{kQ0V;_>J|$uHmZezJE8+wY@0`lsqsZ? zYwEnRW=)u@m1RXD>gym+HBp_vmdP~7sFyTW=zw|0Q#SU_!dJ0;MQ(GXj;wc-j4Fu- zeoy5aU%P7_RlF(dxwpemOWb0?D)8k-C$~N&g~TTIl19VLhFp(!H0f~+F)rJ&p5_pU z$Cz>~nrAk+ci4z|?Z(jWM1rwvuza(452z~fU{9GXDNemF))28uoJd!AjC^ebzUIZZ)Nkqk4t)4x~*1~ z6B~8&nime6AD*iC;=68ZR&Ot4@13{R6~W$Wx2sntEnu8=f<8Cwy!Lh zs2V+*SV6wuc%7jbcZJsx7S!>eOhE&dtAzzew>fr;@&r6rQRXcMuuN41s?H1W$~GcU zkM-`6KqI)&w)|V?829BV_=ohkiOo5boxtLiV9!Y(^A5ph#qp_dawp4ZPJem=!Y%Hs z89I2^iffw49RhTk4@5Sbw#_#8;fY|1dh~1rH73b8h~4_ zQX+A(ig1y+d+=Ngg4L3&(V>c-^HVo+vR|8-hU_eVf8&uDv%GzlB$tB;9m@PN5Yx7V zLOYsnJ?-;v0$r>d{-Fb~Hw5TYN$icNv7Iz%*%gy+>Z9hS}km|%;&^jm6qeTl!Eg@}dh zBvq#0*);J_M`Fp}c{NTvsRnK8be~ihsLCCU5pm5Fhm~uoSmy9mMmJeV1d!Sa3AfD` z1}PoymFOlfrj^FDUYiu|!olFJ_uDSzv^^9;phH^!Mg|fUWPD7)GN%L*rRm7ktU8jk zyWoL|I7R|tj_ewS3$hA|&Dl4QY3VvZoE1)wyzaMjvpKNG47B@!AO$(JBA1fljqx-X ziNTd6XG347pPQ16*wJr8zIp-oTGf;+eBh?;ASd>fT2{(Oo7}|{448%iK+qLNJ2KLE zgV-ojWQaALg!*ic{^ZWl48%*&QVcoXPr5fTw2geZ#95WD_Zs8{W^YBCe+g_p!A?3| zD%UD=Po&3_W28Ggtgo9kzp`aFn;{=IT^M>LF2j9PY;qzX|0I9|T)Zn$IlYppTGc^6 z#bO@JXRIh=xq5iR_R^;#aE)FN>SW-BPj;t2e@IylzQ7S{NcPt5lSpJT4!nmrSr<9Q zE;d9l8sdGGYCzctCH=OU9#29!1X+o&ykd48d$09C`wM0i`!(*{ajmn;(JHi=-xbnJ~OunRLqaf(TO|GkJgo;Vo_C*&cis{dl-^;ky{En%+eOhZ$?0pbugtADO?` zGm`g8uQUd!)D&wL$)#>YZ}p&rhbHbAWG-vTwHRtaSd{SaMrUHudyxld>4f1*!L=N5 zkM4sVB`+8=D)n?ptq<)23}nEy0?Zjffj+z}hiCWM=WJ@6{Io|-?RN=ot0*b`BNxGv zD9AReZ-E{`1k~|MEJDslj)UuTfmv&~)m-zzg*jbh6eF)XeTL?D^gIzP%XWCpie)iH zRwdsVs^4`A&8Clw)co2-WVJwiJ4HEor4UUalE@B@HQ4_sNPbMPqxZ^NIzL^`GIiwq zQh2h1X>9q>l`by$qP&^b87!sWqMAQ37aShanIk6E=kTM;ImmUAlOvo$D5~`mVjNwP zb}~YdR&kIRR8IK?N_u5?RK?pa>U=?Bm^XKW#5O{sU3$JGB0-Q4%j~KC*v@BEIK{M| z&IORJ>#BfgTv;?5a$&%rbhEb?EYRLM@FZl0e72D^jCOCLA5*L2Z>gdsQ?{4O&?H*= zEdDj#H4=4CxKzLOVE~8$T{+}~c#;Syk3Z5MO$5BNM#?2`wcRg6Rz*?dzZ`5*a2B95 zmA!TROwM3a)oc{uMFR<@nGVSB7=k05U)1(yBrG*S$;lZBb%g%ygDMBre+!UFA+FnO zLjy*Xrb)%7*`VImCL@lKSo`{!A9Y0QmwSJohM3yG1_v_Dl=)WdPHmy&@9DO@0J~Rwd9XD*(w|lt3VkO|i)w})7d#1uzhJy@ z%Q6|MwlAq?OMWPVF>aNx@Bu6_suzB`kW6=gj!u4EzNvomtXCKxOa^BazP~zv6txjL5#!? zzczV6ZNF(U`KqT{V@gu1G-=5YJ^6H9gYC@|h%Ci(yUhXf~`-^4ydX zb~o&PEcwqAHQC9o*T{AJL-uuW_-@?`hjVU*P3BzaSane0bLUZ3nJ%o8Y@X__1WL<> zunfN50aoKIxM^inkD8}*H2EdyaItK{;r*R1)#_==T0EhHqPkZ#2ew$MP!{-0dmuQP z8_lvP!c^8gN);MgKuOfAqyl&U_9ar*7Qw!9Bke}pQaISVhFd<8&IJMs8-@4^hiMyJ zYCL+kw}NCPXTwLRQKXu+s1hG~wc?f=UBs70Z~!Zch|P}fFxK?uQBWQlL#)rU1^>f3 zL`iEhToej^A1AE_$by4F+H@qesV`hyqa9a^nvPEF0N}w7I)55bnkY$`3h zoWLs`eOV1-mIX{-M@!5$uu!WWfAAH=R&dvbKoU3Q+VWC&oj1=^0=Q_P!l+t#oC;(i zBazo0J?il>4U__=t7~%X(>q@zviht1Iof85WnAwXCGhewpMRBrYE=0617PA-xX2$eNmBi5vNVzY{OwMMRpP=2Gt^Fsp<07QfZh3 z1%yy44o4Jj5n>TlKp|tvJW|eXX7i(&h98L#7o_`cZNb!K;V%q7aV*nCSBN0|^9;|Y zWFcVPJ7*OgX?^unSheVGTERc_rUM`(c)-yI!6dm&_waAa;^)6kijkcUp|$eBF-Kb9 zeoAtOB2m@iH%R;i0Sh>9-e(xnw%)t++o(^HidNf!G!`#;eTwbsM7!KI&AF*42Z_S; z;Z^LJxp$e6Po&%1Z6qT5a;@aoH|9OGHbW$Y)TYq4)C`S>bIAqNg1;f)Sxhgr;rW7I zw&D|GWogN2F7d$OX7?b^&mSpJ$&pzua}vxC<)YjR4sXcpL0=wQN$57Ub!v&jU0L_k zB#$&itc)Na4t&+8Tj$xqrKX;r1X0Xxybhr01M-<&TQ!Za&^ql{wShsm$amoropLBHniloXckS|B)N29Flnk+ZGvt!O%!{2LFnP;gxT=t{}L51zR-ox9&{K zdRP^_pcYxVqXz^p@c!u4dMvN*h9(x#YHKfn4=Dwc9g+bzu`LWRd})_}O<=U@W$(eD zgTk|+&GfUCLI>Fl*}D#wNIV2@r{p@L1xebmfw(%b&8zhUD6t^uj5q(h&Qryjk@cKZqJV zVi+at5eFcLZ}og2&-Y@j#3I6%zpt~l3~P7`mo@Xgf$cYNtPZ9DTymi{u+@O8{oo6u zzpbL(jY2OAkAM&^*Au_borZ(?mn_CD_uuMB!0H%l1%_&4YN5pOGPKmmu|MLPa=w3lmy@Yd+QvCN`6Ilxqh>s6}N>Z z)!Q@5`K#OH(5e2$nNKzf1`Oq6Pr{{7IH^C0LHdyxv9?w#Z!;`xi@O|;U#3(esJ?T< z8D431k*m#A=nBy}Ft-x{Sk3Yq`xo6CqFZzxR6d%uKo`{LSGP2}(A)(-J7>KykwjW!Mgd=`WPf$w6JYoqA} zUC7t>A?9}1uoE-m5)t`t-2~Mo6al14bz-0W^S6^B4YAlPd&=51x7(W$ zS9UG|RqgtgQ0fbv;|eWwxYdufXa-g`=!)w(JR3BYuweGgee{6g_fKfyuXoII9xw#X z{qo-j@ZItJ_3oZ9e%-AlI$48bgYP<0waFr7tbfpwqE80t+Bc5D|IWPBJ5hlE6LM_U zK>Ox*p7dgJJri1)zAuUylb&OgfV68TF7zep;N<0ce zQN(;?GA44;R8ZY~4S7g76d+~?teaiUE0*2t0*UMsbfcoYwLwsK%{xKrk4iw)FfJ@- zTzA*_P7`A}CwwVT&)PRY!Y3;+!{Prdo0n-o?)xOr5YA)pZ932>Uu<1Sp60Wr4mNNa zZ}=qV&C2vH^e;hacY4sH1WBNJmdKDxi>I@xtv@4(IK`6ZLirNJ7!tc=(%%abPQPsK zQ1rn2Rxj4KTX<$I9~+?Y#m-m{M{L27&yT^TxwJ{hzE-?dK{OWL54Xg;NPBjCKg_d( z{xdo)mMt@mYBoWWMR+tO8?UyPlV$j!vsP&Zkj@LS;oG9$BlaHbyTGz&IvwYJN#8^~ z>|Pe!CNRhfZ=;7%C_&Gw0r+y{3fW|$Vfz=}KP4r+n<-dgjm9O#dTMC!=B(Kw$T4SG zQO1pof-4PJsc5}jK;Mc>JAYBtmnv5^etB{TA=UDlS^RG?%Hu2$SvlVjkXz%RpjSo% zXXGWscTssR9>T8ifclVS9#NjdhP1tV~odJej7 zQ7du%?D9R8+p21pa|QIB%LbL+-h;!o zQcJ^c&X~ajHr@wd?CCOzd0D(p0SQ+js2z@RjqpLEgB@4$C(*b0`CEwz?%>kOgUv~c zU;!{=P`&%?M)7CjZCDQ&brU*VD-oj(k|vpeH@;*UIu6R1F#s+++r@^m*HA*9M0)y5 z*WP>6s}yl+O49Cn{$-s9?@m9kYe1CmSv{^mO(5szRK0H!@z^$seen6otQxaI#m4li z$bnvLz0gg3SNi;x2Y1J2GnBi(aYrY2@A{ZLshBvq@re0U=;=w{^pQ6>_S3 z-s#thuMob{Z&Vop|w&Oy%^zu#D$ zYz`M0KHq^q+}UO|lrkRR-pn!`uzCoF8mnKbP9|V~4=G{UDmGPE z2t97CpvzJkHX)k3x0gNn3(6khTli>lKx8@0M*sIiu>ZG$u{C$bh5u0mwV&vQ9r%VG zs@H)pV@7k(4yEjRrf=8IoXS9>7-U3cT!sy@62jWVEVxbMkVy|icB+F{2Gh-z6(QGQ zP?GhP2}>#wat{TkDH5*nZ+4`pvV&-fQ^;op>MVS&UrnTEr`0P&{1>rS4H4<%Iv!2n zpDi~MeFGj7M78M{p$AMGB+B%b&wcXoEGMLCam}5aB4U__4w`d~ z{luB(gv5LW-?5m!eHK^|uYN8#!#vM`ri$IGmv}(mFz6(X3%GEqMA8=kb-1rOUb`VL3OE2#ss(|jLFm8Z>pNfZ8{-w?n>HovI5N{zXAbm_ zBV(6Hr5Z@e(8}tX@T9RfuJ&^7dOYpfx^v$Scr5G-5DUxR{1;DBFVy7 z(c#EvPM#9{gLC0Rz@UpwNW(mt)4V>h9lc>BtIpD}8x3)``u(!^g+WiH!KF$l(iq2i z1+q%}+S{z@wHS3ilmla4^XSN*km&(8q*3S^r=k*^HJAJp2HAJ`*eUBZG*L1%&+!nJ zg7&*6gk27cV-7=2npn6%2YO`qvzYtU%?kJE{TevXwdi>JBc%b}#_He3g<{c=Ix5#C zCKjsS&HHgC6qz>_c`Kn=_IJGY?U+nN0E6cKZu~qVDM@eVd|BWSI1h8;$fW(9#dxui zJcMKeb8ISeUw4BgMZZmfS|9y4a^{_*jQy&sWz=q585^SI)y+dam&0FlO5C4~oPmw% z!V5g!{CW$m6Fv?O!ehZ2$r&Kn`Rp3uoXeYCvA1CkmK)wPpc_jjU}j55{TwjhTf7vJ zIGcmPLdx(*t;jy-Ng42>4Bp71Asj+B5&Yoi>xKTtwO-uNg8zda!@}^t=rIHwEFAw0 zg&`n-q8GKWax!rspcl0=a551#F|su_f#TzXa&&SqF|dYmTTfDviK!4p=zO3i3rTv$ zb;||>9!M~;03g2F^EP)yJwjV!*hW~>H}G2Z%_+Nn77ce;3#iK1+0s$j-P>!?qp!Ot z35Z#!(d7NOaLVAuU)gy{Io-=kYu{LlnSj6X+}ZK%@=@wJF7v#L@Tkm5FR@qe2Dx8Y z&9dq0Y52}jerqxKO!>!qez)|4$S)USk=8ADfWrULvSGnLGI54MPZs?vFpN1FVEQ^s zc-mR;tYkcMR+lyCKgm!zmCreqZ@hNp*9)=uES^-Wa;CT(?#`YTKf$UR%+uWB#Sf{= z87k)Lbuva;g5M%id)&(UGimPQ-b45O$%0p6-k?xHV9`qVz}@zNROK_>l@oF&!|NmD zdg|GxLq`rDB`LO1b^gUz37z({n$xA}&DS0SSaXA3gVfG;S(@SD+jYs!e>1|4Z{4_E zF0)}n%>VrJHdVOkz_H!d(Ys{*D(R$YQxGNCAW^FS7ljGMfvzww7DFGU5sm}Js#jwI z6dQ^Y15{?26N41PBtRnYOK;<1k$7?l#b> zL)I(lOLK4Zg}A0#X9@@F3iWPq!?1LM1O0h0t8yUc1PUiccABBBq8(x`S)( zTD&WAC72b)K!?nK7Un>Q4)Zo(g6p4kt7(h}>%LTu9Q2(;>8IrSl-jjh5&SO&Vho2OlJMDG>&*?7tS%vnXrV+x)q7((FKX= z!MM!5Es5#b8xpfqr+DU0?lH_Z&at#(ssGukBr&_JA~CzA9>e^UbO_FtbSTyx!`!AG zOM8uHzN7t7f}B_bpier4|6MUBweg=8rURS{(%&O$EMoWIC3`xU4(6Z*+e{@EM}fJ> zfMTS&koaOwt{EqG9t)lU(>j^01E>7Dv?*u*=-y|!&%SMjmS))A!5L9M5dW_MW&Ga* z%EHO={~l251R2``287_7PpBJ0MUULn7%vrwutC9bgN^bq)h@DU5)mjc%viP`A6`81 zB^5Zq7_)869^izrnwPGeigys7-v&K+qq8l^YsSvVaE7!n?FW?83skN^)Y@}USBBItMqLJVgFf?X?Y|~q{Fi~(oeYNMiCcCKavEzd z{zD&H8-h&>C+kv5XkM$v8)+F2uZN(vqW+XmK{CFY4Q1XtENTMSk)9gED4Et`+&>p} zKxjOfch#JmSo(3?A%{}SQ0vDOeK=2`orw>TMIv$Txl6undHYUyg@y)2h8Fld(`MC% zlS8gmpIlh4GzKGB|w6^LvRZa9D)URcXye=nMl86t+mfSwdt?-{_33W zUb`1nNKLBduX)wD$GFG)JS)6EKZ9QAU|U7ntq?lP?j6ivN&fIrr?d*jS@Ss%P3vBw z;ObuT3>LWPs{h=2_x&e|dDhbxR=g9o{KjGjp>S@H>Gndt zZS>9Z4J@1tY2ZU2ANGIz_;B-b|MMRoWm&5gR;sWD`Tp2fg!Y+QqM@M@v7Fss(hO@mF}z5sp%-Sy+4$=1 zsKQ)^W#Mf>;C=@4qt7Z1yjqJ}8r%m$kqtKjnAOu?1DQ3Mr%(FHQUhkJ?;6Y*lWsw)ACGUHD`ZCMkLr3#DmF(;zTTAF zlnsAA#(RU`@vOq)bYI3ctwK0#Zp`MA6fz5hWf0BYB zLnh2OrqMI89}ZHlvtjyxoTYyI*F1}dF72sKQ(!oAE!JNyOHW!|0*A2 z(0j1-1I*;yZmXa<_Voz+MUKjm^q`L}ABM*^%|*`WQrtukv*2LKfm+xJix>xVBaq~1AqF>D4B1ZgPO1JB`N(4`4VAdI(~qg|R+(YDX9th< zl4M=CGS@%LF3v4GZwX5+i*dSdGt5rzBi7$#_NBaM$r~LOgv-mB4L`p(Sl?N( zEf2f!^e~OCm!=W5v|#UIjVulaO% zTPSsK^!#Aq3WBF#k)g7Y()4YiP@xjN!eF1He{y{e{Zxz5A5jtUWp*!o1}&_Yy(v;U zF^UBqVKM_0M|Rhk{~#qJ0>1=70?p7TuTL%#FaZ(-jYw^#eeYfn4m^cAW=$5UV|xqA z8-R8u97vbwY_m>)OSf#ReM3>|$W2jNe_gX%I$&M(oH!7Tp100+8_cV(Yx)*z(5*pY zk7;OV>GRWEzRAm{ah3`%3pB8F^ejc(ET5h&Yy{;TN`E_Xx6=v*evEx;vJ&}rxJ&!N zX2)P9JvR(pLt3Yc=%OcyDnpluqRk@4S(ko5P=`{-HTgnWQt^i9+|j>@?NsqmWtV@1 zH(fh6jMJ+|Kc9eT7b$oP(*xdbh0!}lq!-s_bk_Zi5z8;tqQTZ~AOK!~y5O1qXD0aqt%h{}czAYd-rAalrL&!9m_%9Q?(>KgB`) zvd_a~3zX=jsOnBwi!t(N0H4o*a}9b(b!vt8mo z>+oDPviE$?_{S{9bvw!gv&BU#3YY_TNd$X4wGlqoq;0lxC!Gz6_dZE#s%<+S^8EZg z(Bx`Ft-5p;^TVBZ0{^s*TwazxN@v!1h|1X#O5JptBXEO#8bX7@g0Ot7lna55%CyRv z+e}RLVig6qrfrjcZ>G9WpL@Z){-%zCnT-+oG7eQ#76EE|1nW5$?#we*kQ$WH%ZPl`G!b>Q z4`IygQt8yt)Td!^XdjtfumeWpdxKb$6`(fWf`HRkzgx?t!2SeAq>o{N%k0R{=L76& zgmezy3+XssT21PvESq_AVh}0X+OLnS+O;>0e5Xvkbow~;COxC|iGqwn$*}_Sd}gF_ zC1pd)oX^GF>%GQ)Z0W@~>ut!!H+%6AZoayXcuI7bo$fJC^I8- zcGhb2qIhg;97%Tpwg9f#p7?r%?;_`TDehA%OC^%W zyOj+Bca4)L%V{5(>}QGAa}MYEGE~IPVNf$I_brlA>ToKyB+ae0>YQBZGw;S0e9j-@ zfakxM1Hb3~k$ZkGNvn%rRn0Karb7P)}4~Na$OX6bkumGWj2d8I1yV-DnBT{S7jBNLN?`I*IzBmB>?(aO)nQW z5?+)h)#;%uo1TtY?^)wXC$6p5+r6xq5>Sv}ZM|5yyao~`;|MHgA1eCItCHsn6UmGEnP(G$=t)i8}HNl&_Q)_uc$W7P~qqx=opMTqYL(bR(P2z5YO3E6+h zP96{Y%O}H@!KEX={-Wma-XFa9n?HaD{DJAcKM?r`e~^HaYVf;1F#6peaNPR? zj(dL~_nSXh`44|U|9Aet1@H#~sniQj|KJa5BfJNL0S^4`IY7SW;Bz0q0rL1V(NV|$ zi~~W;UtZbaNZPoSh1yrw2nZ`X`dO|jwxquclf2og<_M8qtalJC44Ht#e=$Ba%e^~; z@i%|4Hte}S>vR1O2mg4SJS7hY??3O8Rqg{L+=f2_qYp1(RQwD##a7BJR0_s*sMzLT zMy6w7+??-%shy_JpooPd(v0_7=Wd*|<(20x`f1z%4wCEabQ7e`?Cj7E4t|#KQONKM zkSHa&Bnt`xfl=Xv5fB(X)2-@1((5j)o=G$3sA!@yJs$p=>f=6jQ-3L#kI}dkwO>1p zDSPjedD<3io05#GJ1x!nm@eO4DUDXi%Lv#GSY(*&7V)>V(Z}o9mu-J}Kl#XEDCv1}-F`fpkczvQ#l_?LYJW%_i85Tl0TtfSx~n)-Cp zh*2{SiDd+8YB`qINx*lzDJ$PhpAhT|Xz3A28qhnVG3H~y3(ypK8~dRy1$|g*VS78d zSyA~0HAV5|WRy%hn-HO~M?(wfd=+hVh2r)Hy_NYA@}`I%2o)Ohj0Q(fN^1*e0Cti? zZ&So5d{y8FsV2O(?{}(Dza11OIdifu=hfQPAu`;f$H_d{t<99WIjd25eKsg7FZF&d zpT+oCl3e9voA-I+d_N*nl!uplDJW(w%14bZIQ#?#6cq%*1}TVmWKccpCUEIYz8H#J zQ$`e~@(33~-nMtl2W_`IKU=oT${zfP;%+(Cw<#em#PjT|Rv~!sqoNQ`NJVD4q;>

+FU)L8 z-T&*{dBlG(irGDt{optnIW14-O5k1PZc43g6C`KV;yb^K`)PsQ;w<8@VD&*p8_wj{ zjVp4BdTQ0$2-hE7&FEn|JxeU$<1eotVutr0wS$NCUtk6ZS%Hkc@tC0@@YO!mM++Dl zyb!}kDmLHUL8(y;IONV!)~#8aCn>vY#dDPelXgxJTzL)!nHb> z(+=qAuUM=X`c~gtzhi5=f3c^vzhioOO`{W7GE=88XPo3V%Dw7KXEn3qlt15om0&n8 zIm!-yhJAi`a9G5LKK$O(G~8fxfadjL5_pSVxCn*fk**jF&h5A5Is5&n*Aa@ZD4x#n z?aEnu*|9c!?MWSH(Q7rfXLQA_{&CCRM8W4WI1GRbRgf797M4+xihIPs2onw`i4 z<;Tvk`g8+k&}AW12hAwQsI$_G6l6hI?CPcB`xs5B!so{aYwg#8qw|D^X^l9<&3L8- zbu%dt?ezd{$g+=ePu0s_Te5G}x7GMcowuwCIPx*lPX4r-i1`?T6KrtVT|!WypK@A8 z5cSt>^jVi|B5BLw4I}CBvI$?Z6~0kmF_>WuriC7Fp0|#9)`piY^|Yue4B9-TEr6Zo zJv0jvffx0R2uwJ#LCWqxJStz+i2CSb9dDxqYzYs!Kxrs~AavVT?Gw6&1+Vi^s7jva zHIpte$eu$HE5E#k*Bi^{SWA~n@g*1MF)uR`wSje#s{J8y!!C(2ZwGya$1NhBuJMkL z<&*ps#~NNWwocYR5VSWJq!*J%ywTEavjmd2wJe7Q- zhkEAAG>w^HC~O#V6KQ%TR{iZ1{!Z?iMXWEz= z7fXgI0U)===vGhbSD!o|pn~gx)$)IhlFSwWREX-=eWDbhx~F1Q()vFsiN@cQWc(kM znmbBdZS8UHdZ9Na;gyI zfn=D{k&e|EO}**isqBlL+I(b(>XCFiX8)VBcymt$2CFqnE&=v?zMdH_i+SDXGA4U? z(TckKfZiYOYKX3n)EjO==o@8bj@psA@2|}0)lF0r|Nt%-W%avsQ|F4qZc&v0+h9%SxI z`=$ha8i}}u!cZ3T0-5l+vtoxus^=R?cE=9+fyA0y_m3+K87S677*BnICdVFm`|IX}eA1KVC6pri5I*Z;bX zVet>s7uxUB*MBe*EDAbVEM0U(W}YQ~zPbrM4ik*8r(l^)D4A(Y@sCJuoUwD=mgblC zJg8ebU^egV?dEP{F=u!qTe(ShHd3=bfw3nvZ!Rj&97q81p(7nsm43ZlCj5hWF7{@6*J$6oI^*T{?5UE1zGG(xT z+baX$w*qUvqDeda$WpZvrSqgx1s+f$vcNP3BVD8ZRG~~0 z$obJ7Obs1{Ij4qFXEDU`MB4yC9UEbh>U69HIo}v5Idb5KSE@-E)Gug23UVGIV#<*$ zTSRs%z6dc2WEUY9L?;T_IERXMBgmWi{(~Q5g?RW*YKKwNNXRbdwx%{E7}7|J-7Pua ziTjcZx=4C#9A_WT3T&@%%LgoObO)6>vjgLo%+|3XzK}faPiJ;u{8A8-H`+cnR27n! zfCvz+9vdpA(M4SfB+PAg;|j^spF6+uA5?ngo;uQTI&c&h(QNOOv~&;6y>WL9l4laK zKWZQU*x1ymJ=^;1dWl2m#-L@Z7U>o`TP)AxA!r`hasLU0=F-sB@=R=04)n#UzpN(O z$|!-UeDKKz{x$ba{`L$VEjjOI_3N(=6J&ZzxH2q{r|hGv%xCJFpT2#wh2m%EpoAXx z2~vV_MpNc=V?*5ccKo1@Qg-@eTrM@>wr$%LDRcOH?R%g;IQYSCog+yRe*hwNKs)5i8}UZALa zMN=fwWHiA2)=}-%{aWd*CF)yKn-}n*zMS~@bUk>C7$a~|QUMZL>4Bn{Ro@Qu9X_Ib zzWnm#s(zbg#`#+ddw8efPeOG?G5{KN*g+(M7VP~NZ-eh8gYS%M@cD8gOYWg@V;#75 z^0qnXBI@j*`^&QYon>KgaX-1YEayXlRex9(;@>SxXO|~hSZePQMJrUE??cQyunzwd zGSl%FGk-Djuh{&5ikbPwyr74ed0=(U%lfYkXR~}*4ZCV(fl%A6T*5Sp%xJLLhOoRR zu(C%-CE@LB$U*PKsxdFmBPz|4EIx^aja%39^OFWRrg;brxHWr*a~z9@=z_lq%WEfd zEmjAv9F)vEZfcXtFB%!rkFEK$D^aIM-4XVnp4wBUs|5TLva_ z`B8qGYpp&{8TaG>u*%$>6cdwRDrR5}60s?Wld4-%>(CP;do}E$P#lsoLa!R&H3Rb$ zS{ZKYkr(_Dlv6YG5;WXGCUknLnR6&&8A2IV3-r(!R7{m7A=Fq4`x4Y^!N<9phJ<6R zT5Z0P`HET@HPQ6Uv3Tsy28O=8rSem0fNy6Es2rPpc~q9J$ADl4HPpro^^FS(QEs68 z`PNF=){v?+6V4K}Q`(c0_&E;D#3$(@Wg4SiE*S%f2xCY>=ESe5iz;)WPl*$_ELhdD ztP*qEno`Y*OY-J!YEqa3hBPz6m1)$WSn}yD={Km=aO3=}o^C{0wJ2&Xpc2tYXIgW9 z4%e(^(qGXkX3L!`d$oqV@%phO_>=mWLXnRt77GZm&b(X4>$|2+7@wwHSYsk@v|)N; zvFIggIQ^oX%q7zyJWu0>lXv{!EAOO?rV+i;x2fHY^hP<|Gyzqgq1h6?p%@N_7!-(U z5{X?3WWn*YalGm=K6dG|ytsWy_@iteZ@Evy+p{~7a7{V6^sRr$VQD8Lo^CyHGE-U{ zto$wrfv4AUj(enFwD*vbhx-)9cFl!RSSOkKcKFu79gE0{e8J57DCnF%i~ zSASLesJGuprM>*lT$X>~=e2Eru~NzVk%);6G5qJq>5p4tp%|eMTOjCA zn3PBO=uoH1y>A4(xV+GzF}!jSz7&mrhe}6?g+}z+!3G|^ZA%YWCMFkn6|;iUm?L9T zUKFWd6Ci<(N>#MNL}|p9oUy!Ohg#|XwrrAff^#GN} z@~nZ6ctaxEGsE@qWPF`kd55&6&tPyTLJY=+6Akvp`!Z-?$KT`i=vvEMrW*8EIkSA* z=ITOicT+`~LpzN@a790sIgn*zkfmiSia`tNZdaiURV7s-8|fkJ*(7@MjVM(?Q7CEB z;6vNF^(G_@cs$w zie|hP6j2pDY8z%}V;;WEX9+?`yW$yil5)5RDA@BXt-NdOzCC|;)?8X~*Vuo1&J0GKF1`OAncwJl==gK{MHGOFjhk1Eq#PFVsALLH!rh{|`a^+I?H* zA*df{X#8oRK6g2DBVUkf14aFCCzi9K-^_aVi4}}UYtk!h!-oL@7rxfbjz(6GaIai^ z8(_fJe1vt(P#O1jFAtRnjArIPl1WiFx*qCLul)!%iDEG#@0%;3?pcQ|{YLZ13YfDI zr){v~x@)Y`0ks@>Gq&Qxs z=5Q5Uw!iX~OxK`1+V)9oQhz~^!QEuyhlcgb^~5)MTGQDS#RTI8>d%0y0g%2hc|CeA z?!^SH4u#=$P69>#e2Iw|Y9~Lo#tF(CiUfufN*4XKMxj6Sb96>n45)Ex{Q~g-5%e}L z0jKC^9e6I%2`l;rx!An1Bj3U@^UFDkc z?d|4I?6~yId(Lj_BNgNAycG9=Kl+@w600v{D{doo5uh)0iNKh%-WVxTL4zVmz5V6g z>|@%2abklW^uiaZdKgb^aVdhu$vU3JBY=JE((7aQ;GY)Zb$!X=>Y6G{5R*bEn0^-B zycGXauq%4;r;hcgc{oAPWfAT-q8$eGGvbYexg>qnL>YZ`1|qDe;KJwl#h_fi>=?G_ zL5^3}90FO{Zrr6^djW?Le%@%aI@klrco%U+!6I@Um&B@HE^f{G6v0)6t67RQTV*AdSM2Yz5vnA7L26K<|dkd8XcS)&P^TBU+N?;N44OyB7k-?>KJ9QN*c z>b|>kO}aVM1~j|!&KD5pXAt47y*s^%JF$ct3ehu9lZrc%C}Y!}*)aRwZPnE@e)7!O z%Hfne`o^axn#0oGJo<#I`y%{%m{a1OCy#U%wcT!}aPMI4g0~eOg8G5F$)6YMj=f8P zhLivs+D{P9GdIM7iMGAGu10~?q z@`e*?Oo_<~3^f784F`fsBu=kTMTPYuR!1O(Dv)ef4n^}N4ls8*da4q_a1zVj(NQ5P z(LqGa#pqWSQsWihVIAv2f=rH&`@Z8jI+V*4JX8(BQcE{4K9V*mKG}>}{yP`}sKZJ> z@+j90kxDpDByIp=ma+TEyVY10*39pI64#-w%thZYJCPl=pPE!E9zTt)zpEbYz|v$G z`xF9)y7YvrgQ>%9Eo9~M5qgK)PjvNHy{=TghEllU5+e$|)#5VAgvYv-)* zhbfA((A%vi4NdhISLO7#h_Nx-st-Z^KpzIg&GS!hG2K;KEr0c4@D{{!<SlpmIYV z6Co*L&Z=5e&6vZ;@hHM#OJ(l^Tg<*};8AixuhcH9PAl@xri-&9PMi6%v#2%ALP<|T zIfGrg@$+$^x^G_s&%PetfCnH0j+@hMA6*Co&$=nb-#NTlJeyrrpC8@%0Yui0IkF%vX(jA}DI?CKtt;Z#hm>6(0gvD;vVRpVL4+s#&gVdraz zqtv2|l#OIa`cjmVy9LRF%o=IRYcv3s8;+gTwxEcE6@@hfI4{Xp;cZN!IUH%fIjfmm zeyh47P9lsnD>1GGC`V)%fnK%)I-8RVMvGeStLH&wdJmV@dfQ1)X*Ya0pXYCbbQWkIXGh!~;@3lcB z6R#f)1ZQo-mX{(6uxSZ~KK^)_D^%N<*xH4rXzRK}md?6NKy zC9++0zvF44mextqvuFumYp7;3FsVc~7^~svl-Bz>6G^~r5O&3FEJNCRBj$_8Vxs-? zd*4zh6OmYDF<~3^8;;zov1X}qy7z5_e5ey?`tv@gc}oOEX=krnuavgv)jcI@i-g4v z;dcjv2d_9*DhEgtQ6~~sNeGHKGPEWVUNi4YpU}_-)CORPl zT|O|;F7WNa2TlO3aJV?GaGdaiiTF}3!WL34&=!*~WKKJbw}6-JV^=sPm^zG)q`^ej zif#d^if*}DV4}vj6%KHR@yZ(^BBWjoKb==#ipJ;_;j_2&w@R|NYNX?9ciGu|E5abI z(wOgoiL0hQh>}r#*c-KVpKwCtXh_Gu87>_9oNULI(N*?+ z4vER$5>467c_9ynrDNQPZ~y34|5kq`qqe)Nmi+r!t9hEb@ty9JtKOk+QkU*_XeA{D zO|_(2j!ljw4*MY2I;9A)#iDr)?YKg-@%=%b)P|kJN$Fh)0NL-2d-eCoP7WSywsXxc zN|+0pVnmIuZv}Ku-y?gzsU{B~d;L4ImY{vefn35H;X~No5+(>NF+aQ#c3y7Q+%E7>+cl}wqBabj;iwp14r0X+%N#cXeqIBK`PK_W(*lz${Z z7)N1nH3DF}!+5t6k$kmC=tR|kYD~2cKk>Fjmeb%Qs>jkxlE~Sjp^BWO^N0_XNbC(h zo5aogPA6m;9g?4qr3tell zgzvWa8F8Z5k}Uog5$@_icgj(Bc+y;#!t`IlFEHgg%sY-UR-8zeTZd`v{G3Y)*8MVZ zkR`d86VB_?Ytek0X}4%I?I`GOo9qtb#><5thmzN=cZT9;1@c|M&S`E!J;dw-ZJ|FY zv$SI_Bc!x>y~F5Hl?f)<0*8a8saNpQm-MI8(U~y9bJjEG^C|2#n&uShA1tORhNkG_ zLqJ?)>j!7r{6V5s{tGRB1@k<{fXwnb1x7Y_@Tbhb9pAsw6H*(c6L#jqw8}oGjTb^I zCOMlAt-F;;rz>LVS}pV?%<=6It+L$Qb=& zSBY2JwovpAz^yw+g!>Wh8^A?B83#F~ywv*bns79(M5t5Q_>#bLZS1bI1$9{)glfuCh5>7tNtBhQ`~g;==ZE=f@#z$6 zz@)|x!kLO7wjJX?WLD!l;b~$7hv+c%5 zsDRFj0y-=Do6ZvarnAOw>{zjtNR7BF!Y=gfo_lSg*LjK8rRoLUcTyq_?Vi@pbQl6K z@w!%#FvDpRsQc_xnCNg*UV;j2x*HbZ``-~>bBXX^r$9j9^Ay+SqVOARjguiJ9-Xg* zf@7@|R4SV&4>9{dSqa4b&mXwtuJV74&Khx#Ms2Y2YKL3KC-=XgUU&BNI2~hA?~%_R z%*DmKSPmF%Iy{K39oK}kRG9bPdUeXRD!1ovKXt4*oDsn-U^+pdRo-vn?G0M!Z{_z} zJDuI$!zi?zL_Drd6uRslb#|g|b|W@e3UIbtfUV z&AgyLM&sdpz$kEWyYI?%$*u;-p0D`akH)3D;!AaB&Ad1^Js)^%<5COF?@%0~S$t|m zDv;^*Ucu?ZJ;Hb)ghi|e_w{TFkv(EH5jFvnMlV}ggHCTvZ;cm*7Cx)}8Q$a(C|*rZ zIa$%WJe!siK2~q+sU~pQMkbfy&S%({`3!L10=i5*_c+o%y7M%1`>L;XWR!9Hg%Yt6 zPrglnlJsMUPd9EaSC{A_E%n0b#0Ra<;eu8{V7q~0Z>Iav*hzaOnv}(h;eIr3|1}z8 z$^AAOW8aU)KEP-k1&7(Wem@$Y{x%v@=iTxk=pY?_*Ldmb?LcUa(@%JlT|u&>AfZEI zRon%P#si|AL$Tsv3c^WjIxta_0S!MMb$&_92@8(0xf_ZMCIBY#w?;=#7zoAeRM`V0 zH9F#Vl6=w zWB}YnkZ}t3dc{DHtwvr3MW}oAR!7L-)^NxmTiJySDVWGy155-N2^mzM3>iEc4H<05 z{w1y%iy?zUrX9v7mL0|yMjgflEJ9+!zlrP;0O|?u3dcI|evMdZ!7M^J z!mClk4i@oP)e`*o*j)8E9Dx&g|IAz_^7vg5P}l5VQ}p zr9j;O{3$vBwEyV=5A@~2UxpT=zBtjWY7#e@`QFgpKNQ8o@wL9N0CDK}qW_}5ZAxvC zyqaScW6%W!`NvQgv+kqX()K2a)Xs0U^Y3WUxs=;pTF5uMD!G`(HmK1mg{di>wd4ZqlbZd zwaUkmfX+tz(%Hp`Vv_n>acxi9A)M;CRq(h-*hs5cj%LldLnDju2gJOMUpnjZyUrS! z06J?v_ia~Kt8C0AC)S}6MQibw&N?s1&sA;i%ruLVtrzNE!fK_v|I2tjIqOP zT?|WpQYFfge;B?^GX00uAkD1$<^~cb2xdl z$%yn;>l>O@Uz655mfQCHyN})k-|MZhM|AYa5awQRNC@ZeAvi^Un(TbD zTmBZUv@(+I>pN_Nu|fQ>$(FS$DwYaAKy>L1Rt)EaISp6fi=}nq;oZ7XI){>D!j=_O z+3x*pa5-H?BAY#cYiAwrZba*#hVXs{Ah<(OmA8Jj8lFB2b`xoj(oX4s6VvE90g3J4 z8PG6(QTt14O}D!A)0kY!0mu4v#SV?}eh}`|RCViY`UXJN7ga!d z8v(s-(f}vygbof_0WUKGKIS{OS#fN<*V(q;bXFVivy-h9T%R`CA42wlveusz+5Eqd{R`QDCuG$TH+ddH z_JJzazlQ9?vGLzj)-oI6@n_PjdzCeHR8zrB-vlRtlLE_2n*@vKN=Q;~XlloIhA9G% zr>>+<-R*)(N_H{?58XYlmfbh&d-uR{f?;CyXLJtQ#XBm^{PS=>3#@bpQ<}?!=wfc) zO?tjA3$OF|*-YC+c7C54pGU1;E_HQx-$zg%^8P+GPV*2r2)G3zsNYlLGm|~S`_x!x z&C>BcH3nq%Q?N>nxjcKOV?8DYmGjL9fy5Kb?pdUmnVGHd62lA6yYQ z+$6L-S5LzXRKsL@B^m0!bIm=5-gG^+*O=%re9C{ky}zU)W6(xCn{nLLmV3II>j@s# z+VxN5>3{sw{d_T53bgL*y8Pdq8gEy|YhPJOpK) zdcr5F`9QkVS^<-_5{(kZ#g#!{hL9yOoTy}mTl`5ej`aB}ZK@V70SSmw+NJ(Z{beIb z$72=@|0*giMz7&-7`)3|(_%k#6tN?c^pqWveX%JdcU|HL?0IZJzxF)2 zz@7&J?0JHDH;EoX_JNYrpA^}Qzf|@wmHl^8*?}hy*D3y+Dp3Ciyew_Re)hb4_@*xf z9UaH;OXrys9aunSQGS=%EAY49BV($vy?V2$ta1ZRJR38YPxbSs^G{L6IZh;KRp(0t zyMq??g+TXf9`M?Ep$Gqq`!!EM_o}HIu;v*)@z}ct0^s{K5A0iaAyS<`8c?%91L~(i z<+<6F_~c<m;*iR?WE=&VBvl^(1d3C0xGkCq?#6$33#Tb`o8$zVwMtC*Ew zTXUASe&NkQFp!cpL1An)dHr=c8bO>Sn-dQ)a%2q2`lHwIZO)+nhwb{CMxu^P%3nG= z0O)LZAsUEEs|`+&I=qdYZ`}z!ne9Uh@8(8J>}op;s0RAOeV<|t=u_AQhL8=zjGnXz%+Ja6q48kX?XK?Y>WO`dgnu7t{~vtiBV$p#E<`T8>01B>!U@y@Bg*s z=}0X@H`roS_Y83WYEVA9WgatP@VoLEd3rZQ>%h$$jRZx)bV9p>L4$mDCd}%_;Z|B@ z13;E+4D4-3hax(J_)k`VOn9q(6IcD0q*f(ggt>16Iq+_~@dfZ3P~NTi;K8B8cHokY z-`Y@ab}Jm?1Zf$Iz{?uIKBxvat(E1s7L>~?aDpwc3(^7BLj-BTR+qmg#vMRn90DZ9 zIlmHPoBPE0^L=7G)}V?)VHTc;{Ci@&Bk^~M@yszO;E%-k(B;lUgDe2HVk_g#f$gxq z{o)HxsFBf4%LinZ=fMipfAN-w>i({Tq=f&^EsyixZF!P`Gfg}|lXYk8e{6ZOOL}Fi zw(GY$qj=npT`cGQPkHmLS+ws&TKRUax^EI|jdtuyPK$W^^RR%V<{GP&MC&-ECqDyl+6+VdrfS{O8Ho=reX>lCL9D_iWAR8b*?b zFkEj=qVQGsC@0WaOw#iZuD0C!l|DU}pGuABQbS~Wg;*G4Hoq`=3ZzeD$%&r7rB7SR zFM;$aDZ?RewZvw%#M7o#aW_VD;`EudJ7h6A7)YNymrDfICU5a}p6bF&1D!?1mT07Q zBpT8nH%d;Rv$$W5X-MA1g~(3vvgKudgwBkqZHv)TUNH5oTs?|2$4`5!TB`F$l&4}|6!4jYz# zRu4`7ZmkMD;R4)>v&(3RW}(pz6e!?VxmcYoeuOf<4ZG6li3bq@$R;?6?0zONg4V6@ zVdFG@V&D|55|$5?_4W6Hi;U1xfCW#vB~YR`vYU{1eF}`oCu>E*KsPZh{XF?Juif|t z8LVN9($Bjq1O90Dt4VP`9TQ)bPZ@{m6kUXo7hjOV0$UmEwDMTIv~m`pHsJuwy~4%6 zmbLeronK=xFhk$Bp{!Kha<6_bSO8P;F3_mB-|zq}C^2~|A*B*vmPQ4}VxUmL@q23# zn2PTk6=!d$ezg`E?^}!M_pL>|QoOdnkX0MM-&>0Xv47WE+z<%A{iC(0eYn6_@ZtVb`LBb215 zyp%^GKR>icZ@0VYt~Xz^ZvEUKPmg(J?E10e;_3b`kp;RG7LzhSU~G2;1jdjLiF1Fq zmrd;C8F9twJM5bHsC}1NB8;Rd$-FSm8L!0kACr+KE6*5$GP@8`xjP37}fBH4juQ`a=elb${zI8dC`w z%!|6+8`)N18M{T_ZrlUBpDUFRj;l-b$7J#cLaZ@S`P@&%ECS)W&T^hlBmHh~xSo+C#}*!@4;3IK#COIL*8BK$n6v z{8yKvS3Gpso$MO=1<+D|NM#==K=H8u^QUOI|EI$;y*s)6;-7`I=S8yOw>NgJC_I`ws>*V&s&3Fx_+9&|sPpIg&*;G6m2 z^0m79)Y-ueYfwyWe~-qp(xS;CX?x&Pjm4==!G6Li7k$;CtZR-Qg$=~a6QaaLn)7L- zH2l_SXMagP>*o_&ouutHo8~O!8rmi;9Oacbvd&y{3ctEI#k%Li6B>>$tg>q2PlG!& zz`&82Z}I6DD5sS)$7H$sbx%sd2WV&xg}S=QYxXmaC(O&8Y(iH#-GH08t-zdPA53N( zZ6Gf@p@T+MO#-p3x~1VJQOXC?Y#Oj-aE&JilrR*?>Z{Afl`v+?1Oi-%MWW32+uS_Ek6SeiFw-?>IK; zoK)Jrflqe`8-ECp2g*i&MnK*%=ID~$fX8NA( zP6{D8u;kc#XBJl;1T9EdvF+l7R<4MRPP8#9Hi%T@-JdC&*u14pmLm0X!96@m&~jF6 zvv^V;yn^3ldO_3SxUgdSWq;`F;mVn`e|S0n=}n+NF;!!~#29ccKfe|Exg-PrB9})- zlgF7Va2Q2V#f};O8KegTayIW`PS2yS9ZND;ed=k>FyO@BHwHCE`GJL=>}-P8eu{Bk zTKeO27>UfMSj^Z%NHozaWvI9qc?=l1%#oQoJ!pa2lw#>i&n4;xP%^iQzFbvO80{-c zOldwFK=KwVvGSsl-w{B}b$6(S*%12t2(+%XV%#V1ZMXB{2?j$yVWJA$U}o}B$O?1+ zTi7D~Ed@3M&f*>hNwG={u9ZXwGn0wE-~euP{;#Tj9N|jL^_k_R8;Ml5u-5kJZ*?Q> zn0FB2yE25czhz!gtuu3+eABWKfC<2aKCka1!pU3G!L{xNS%a>+?t(xHep=MiVi%@k z<91o4#&2QHDAbUR^&X!Q@S(Z(IQAW#iV6n^#!Su9s2$3o20G>B7wC>Y+uT zxyI?0#t(9dq=44kGgyoSk*^|*BVKbD2|}B7x&aPG(IT5K1}s3E*6}^KafRqPP~woFl#dY*U+fM|>rHq-#K{O)Kd@jKHI8 zVm%z$;$))fnqc8RNgYZ~9I`naR+={gL9Lw)oB2mTIDlKb{^KeyWoPSmlq~Z4W=i%J zC@gZ6Y^<#J$7I^t*;DeebN=nBFAfmz@6Vt8uMg2|d^2r3+xf_SQ*DW@i5@C0?u+HbB$1&HKTm615oi!eq>g&%S>TKX-Rm}GFrnD{8(R!CougSV z#>kC(R-wVT$jedAnyYz7NbepoM@znVxh(4X!gSe2RDk$7Ti|Up%Ej?UWNN>sX`V|=FrO?mHE`HMT^x}e#zWdX@4Yc0y_cnZu z=R$=p`Ym1O2V8W^52{J^%!b#ZVvjRkK`T?XpmMd)h&uDVe*TDZ2sb`U&)?|thoJOS zd}ztf0!+L^2%{!BZ$AupYwO{Cq-mk5XkQLd{DRA>*&-Srw;Y4@x~)uAf%S=>lrL*E zs}Pir+5c$lyQAU!zP%Y4-6+xPgb1R{3`UOsimU&pvyfv(Ncsud_evIeRmMh6pt^gINKeY6z0L zwFdxK9I}LBR5lzB#1d8qgmDsIhq5M7fvKeMKH^JqpB9IfgAy=B)hQOvgoe+N#wDGp zcA`_vw(Mf$^r}!zBQPbWE@5?tehbcb3V1NdudlPrA0-Ajtth|`L^idHs$pVF-JN&k zzlfR2W&Nm_!Y-xkv%+8@Z{d+7cBHGo)u-5}hoEy(3@+_BIxF-z!7;WW?~p==B4{>E zo~E~tc?KXtE5<02LbVifBkHvFN|(jy&V#ezsao-IyzWx}5Uks&Ua4qk$*)c9)^$kC zp{UmqipxofdF9q?Ast(%#`|+9Kro z`&Fu4A}u!DXvy9!@0Z9#Jt=7!Tw7(i=ilExcG*Go>Va9o??{F}8vjP^h<{WgN5Ni- zu?Ldgs_I4`B$%dgZGU7%217Dg?(bK{#%d;{RcF&vsHv$3@7&P#3@BO(M7p)Bs;$Kt zxKb5=DBb8o7Jnhx58=m%`})t`{>JtFO4jmI9MUnKiS3%X7lX2*Klyezz{c4z;GUpg zSyY+-{rcj15ZCDCv217r5OC_SxIold`J8VLxNbVzEPuXe$ttYYVf`*P$SAr%m?cOw zP}FexKuc#wW&}TMRzc;6pRuRF?AO_Z*wEUfH*?R#_K}0wZY$trqP@szR z&>Aa>uOk?!*fD-G*YnOfL2*n4!bqLI;hfg`Nm^3u+&ax!+W^#JDcuo_q!|+Z{%l;) ztkO79n4u3*U{bn$vHJCTsTEcxr-?jX{+dQ5>ql(xwJ|EiMy}TannL63z!~13I&)_D z!b&fZ3g0fVLmlMKQMA(a=<`=%BZiN_OcmaQ<`!`=X-T-qE={?atmh`tmFeskk$*l{ zzniI;@swgHHWp+}PI+w48Cp!2D!PM<^2BQ6mz9yhn~-Z%MVF z#2~SZ?Hv2PkL|X6(pd6qj@wxoyamXW+U?N6AJH47+v)bO!fm4SpKl4Of$Y2x2v)Qv zePS4s@+Sbib8;}~Cb3VDpG1ZvrUr4d5@-4UC{!NQRID?zuR&TT}=A% zt75++y^DFgw%Cx;kKMW}d@b(aXKTeBc+Q+%#b=Ghk^LR=XHxNR-NGVOBN`u2GaJFT zMz$ZZO3fd2gjz1nqEh-_bK>$;4<+X9M&mbro9jQ`t?fUXwwSVW?Qjp{BBxQ;h@&ENdcmH)x@ee%~)?VztF0m4HiGyX7ycqSEy@>{CM`e zeEOhZFSoTEiFf{cQAfq0d}D(zV=q5ReUDR_U0?MO4mep)xWE`cK9e|X;Ce5fz&$nXPA?)L?zc=t1bw zPZ?{UQJV9kb4i%t#Ps5@6LZA#*3W$H%XpT#Y5uJi9l1lx)ykWkocqc<9|k?~;(|`v zNBfMgu7sgvo90eTOau#ltxE>pUS6Kf=$7RywFC+eo;RXGulW3~_h_7#j=bLA%#LoO zclVN|8+^P|+G2}so-xTZYX;W#j+*Q(_A8MS9biTZ@;YznG|st93ir`9$9p z0sUR(nC3()$G@rYE+}LG#kf!686cq&{rAZ^8CfP|biP!6$hyP=eEwoqU#gPWmXxXdw(?M8lUPK$ zJ9D^I$$~U-6_YfKn#cMI1@WZYLNe=Z{a1CBL05KfKpBA{_pNy$m{0~b%u^#e4Et}7 zuGg)d2i1E#Yiz!6F8Z^n)S6o3O&VIYFUeiG%|`05wgp>a?@#;eTWAh)%879t(Wgo2 zRYuAZH)g_bYw%BFb}xsQ_N$f-C&ZX?;*{3kyDa9Au=|_KTBU)Q@m!B ztph|g1xLwKraqIcc#@lWIniAZhaX>o+cO+Y&5J|K5@AO}fhPZXc=+e42cQcN{*eJD z&H@vc2P?dkwI%Owj4Q_1Hp1h6&LR(taL6}lAgBxa6f_+m9PCLG?{a?<4lA6;Yr0E6 zCk{K1nyeIjLAC>6-N2|D9)|B7Qk(PfbeDz;OOrcBh zhc}&rlo;a7_5@N{&0f{MQl(qT;_m8fyclu~RaB`Ez|_1_q+2oKUPqSyOc<3OcATAi znmdf9PCASd%w~Tw>OfaUY)29z@FWVdu2YV6RU-4?7wDLKnynT$KJ5Bi)eo>eE?_+hYFt z$Ip5;=>vBI(GdS>_Rf=Y!6Ep|xN-%Df=`Q<2c^qOkb(+UddviF+?|p`CWpcXs~5vY zQ)~a%;VaTwe9jBQq$qBmgiJJvan+*O`c&rAH#d^Hb$~|;_iRS?J!nqW?fZjdvRC!O zLH=5Y(h@n5f9#K9ME&;`+*oxkim$J624Wj>yaTUJr~?DdU<8gCVe|4O{lB8ED9AmR zjQm2z_USjbq^G1)Yn|d_sxw!=4PVEpXXN0dqdh#2vsFZ*bPAmI1elrJgZRd`y4;fc zR$9LGxkzqRjc1JJkNXDd4g4l^ygWjg>;7bF3aJ0M%k<@P^gw9Rd*s239vQ*o^5P-c zi#P>W<_1gVwHUofC5RZQ*D*BDJ(6+?*#*qZD`Dw7eF%R-_dKF-j!c#A+!bMoI~wuj z$~%}Wokb_ZxwB1|*T!S6DjKIAY+ggt{pm+J_>l1A9v%&P~1HS&RFaCR{H)?w>>1IC+Nvpgw+77#_aQn zafVcHD@1`nIlgEdUIS?PVBT6E41e3!V?h;Z>@~_G{ZD@*<+xmUv^x?9T*8-{FC_m= z0NBA-64OzwKa-&&tN&BC#=@B4f#!?sjct~;Xp_f9M#`$MNJ)mT6DTh!W_hTM28IVn zlo$I((l|l-*}AxqxV$u$b%xxNKf~PBSdcuBRB2?XT@T+14U-|X{o%DS<1PLJ2j`xj zuA26oN)N8<8Q#PB!HL#ys;)U=I?)2;K0WX z<~@N%T#>8jIwt>nqz;Gv*Go9s?r3W-E0jB!+tu1eA7#W36A*?92#WC#L*%?X?a=Pr zJP>_*PbbvfSI^7R0cGWRN6EXRke+va>c~44u#df`9oX(JFcb!Z3W>plP3|N#(ANKbr@lMV)fHt8hA1MPJW#|C zZGD81fTEX^lO@u{<*(8DcJ?0NyXXHJ&j&U@xqIAA2Nn{53B%yxf+BDsenA0||6J9b z!kxY9_TDJ)oh5LH0?N`J>B0kcbVYjFd7zMB@Bh#j5fB!D@eoT(|J}cTl`0qeuRm4) z8@?g-pYR3@4*!4fhA3RYKZn`p3hnJLE(I_}) zl$YBamInMf-PtO4ZgwN3vnH> zN><0{z{kpkDq2AM`EWhC#|9Zcesou?X4z7wh}#-wU!LygH9v1y=muIi9SXm}hN!EM z9zLs-BSlDm)FC~*3}L6!Lwbqv7CoD0@aWK@z)wgZ2fdL9VPoFv(f%GUmIn;{(34@* z)^#)@=^R%-?Q2VFGA0x);PkzlcSo-K0gUA0JEM3s*FBC1_DG9muw&)%D81*^qp(l3 zF6T;7mx)Ok{iLT}XPUd)FGbT}(GNXB!1p z8jdy7F`9o(at_<@>WeD!D&qfLb0nPMx5gqpPCd1evDS`qAqjZ=n`lfrae}GdwTS)6 z#qBc9iD2Pc`{Fwz7tWjE(WC(P=}Z@`unRo(IRfluj&m{X!F*f5Y(++IsyAYR^7wd} zPg>@Te76|ajbZR|YU6%Nf6(HhPc+-*J~ZuH!_ve;p3CpBe+Zh|J;)6o{Qun2zXOOy zx+oiBh>D9f3IjHUg5gk+7sL=fdq31)k{DtLHidzOz_2@}E*kB5$NUS@>;BcSLEp9h z`nkU?1+b}>sSRwkB`_3tD$B z&H~wdKUsG!$Re_Fqmd}DOsefvgzP@ix-5e+sMg3g#s$}$z~*Z z#w^plLv>~!5Y0Nn!Deq$zx-~>)R9;#wP0PQouq;S)d3P=mE3z)jN<`6J-vQOV#P+C zU?_42Tn8>yy4>AmjZ(>#undqVg`*hN`(yvy+h_O7B*_aamnRLhLglJwo?fcWS4li2 z@TP#{xh|y($WrPq|LKI?9p@h~X7_*qcm!v}!fc;6lcA?{xD=cQSd?gehOva@jL6s3ABB z=g;!gnP+qPLjB6zFL{kmge50-Vl$bez{Mhu2~9f5tzW+u=>Uv_TSl1OyLuuQpJ{Uu z?_JzYChc{$dfwA|{UwSK0W3yi-KqHlq4%k{o@iENwTR1k`VDM9;F{2!eb4j#ceIdb zR>+zyu4%*`j1hH?SIc2hMmGe+d}Wi^ygk#*&9fa2k5y}>6Rg>s7OS%=jd^y7sN zdMIrSK}Inpf`X$!n5tdbmHjO%d`5U63X|nvFqeBGaj}xj{lU3VRFJ=&xX9LSV@meW zlw=zB#WH>e-)r^wfir_2(T@d1*~8ccsniBs)NJCv7e>wJ9Kw^A(TQb^?(5E__`GNQi4Ju~$0xcP25rYV z`n6MDW=V|pUO7?^rYw^=$qN;YH?FL73o4JcyGQCwHi|>_GoSgaBs(` actuator for modeling DC motors. Supports optional + electrical dynamics (inductance), cogging torque, thermal resistance variation, and LuGre friction. See the + `technical note <_static/dcmotor.pdf>`__ for more details. - Actuators with joint or tendon transmissions can now contribute :ref:`damping` and :ref:`armature` to their transmission target. These are applied during the passive force and inertia computations, respectively, and are scaled by gear\ :sup:`2` diff --git a/doc/dcmotor/buildpdf.sh b/doc/dcmotor/buildpdf.sh new file mode 100755 index 00000000..ed73a053 --- /dev/null +++ b/doc/dcmotor/buildpdf.sh @@ -0,0 +1,21 @@ +#!/bin/bash +# Copyright 2026 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +pdflatex -interaction=nonstopmode -jobname=dcmotor dcmotor.tex 2>&1 | grep -E '(Error|Output written)' && \ +bibtex dcmotor 2>&1 | grep -v '^$' && \ +pdflatex -interaction=nonstopmode -jobname=dcmotor dcmotor.tex 2>&1 | grep -E '(Error|Output written)' && \ +pdflatex -interaction=nonstopmode -jobname=dcmotor dcmotor.tex 2>&1 | grep -E '(Error|Output written)' && \ +rm -f *.{aux,log,out,bbl,blg} && \ +mv dcmotor.pdf ../_static/ diff --git a/doc/dcmotor/dcmotor.tex b/doc/dcmotor/dcmotor.tex new file mode 100644 index 00000000..c60e08b4 --- /dev/null +++ b/doc/dcmotor/dcmotor.tex @@ -0,0 +1,1416 @@ +% Copyright 2026 DeepMind Technologies Limited +% +% Licensed under the Apache License, Version 2.0 (the "License"); +% you may not use this file except in compliance with the License. +% You may obtain a copy of the License at +% +% http://www.apache.org/licenses/LICENSE-2.0 +% +% Unless required by applicable law or agreed to in writing, software +% distributed under the License is distributed on an "AS IS" BASIS, +% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +% See the License for the specific language governing permissions and +% limitations under the License. + +\documentclass[10pt, a4paper, twocolumn]{article} +\usepackage[utf8]{inputenc} +\usepackage[T1]{fontenc} +\usepackage{roboto-mono} +\usepackage{relsize} +\let\oldtexttt\texttt +\renewcommand{\texttt}[1]{{\smaller\oldtexttt{#1}}} +\usepackage{amsmath, amssymb} +\usepackage{multicol} +\usepackage{geometry} +\geometry{margin=0.75in} +\usepackage{titlesec} +\titlespacing*{\section}{0pt}{1.5ex plus 0.5ex minus 0.2ex}{1ex plus 0.2ex} +\titlespacing*{\subsection}{0pt}{1.2ex plus 0.4ex minus 0.2ex}{0.8ex plus 0.2ex} +\setlength{\parskip}{0.4ex plus 0.1ex minus 0.1ex} +\usepackage{booktabs} +\usepackage{enumitem} +\setlist[itemize]{label=\scalebox{0.8}{$\bullet$}} +\usepackage{float} +\usepackage{titling} +\setlength{\droptitle}{-4em} +\usepackage{stfloats} +\usepackage{url} +\renewcommand{\UrlFont}{\small\ttfamily} +\usepackage{tikz} +\usepackage{pgfplots} +\pgfplotsset{compat=1.18} +\usepgfplotslibrary{fillbetween} +\usepackage{hyperref} +\hypersetup{colorlinks=true, linkcolor=blue, urlcolor=blue, citecolor=blue} +\usepackage{caption} +\usepackage{subcaption} +\captionsetup{font=footnotesize, labelfont=footnotesize} +\usepackage{xcolor} +\usepackage{listings} +\lstset{ + language=C, + basicstyle=\footnotesize\ttfamily, + keywordstyle=\bfseries\color{blue!70!black}, + commentstyle=\itshape\color{gray}, + stringstyle=\color{red!60!black}, + numbers=left, + numberstyle=\tiny\color{gray}, + numbersep=5pt, + frame=single, + framerule=0.4pt, + rulecolor=\color{gray!40}, + backgroundcolor=\color{gray!5}, + breaklines=true, + columns=fullflexible, + keepspaces=true, + showstringspaces=false, + tabsize=2, + xleftmargin=1.5em, + framexleftmargin=1.5em, + aboveskip=0.8em, + belowskip=0.5em, + morekeywords={mjtNum, mjModel, mjData, mjtByte}, +} + +\newcommand{\atR}{\texttt{resistance}} +\newcommand{\atK}{\texttt{motorconst}} +\newcommand{\atKt}{\texttt{motorconst:Kt}} +\newcommand{\atKe}{\texttt{motorconst:Ke}} +\newcommand{\atVM}{\texttt{nominal:voltage}} +\newcommand{\atSTALL}{\texttt{nominal:stall\_torque}} +\newcommand{\atNLS}{\texttt{nominal:no\_load\_speed}} +\newcommand{\atTMAX}{\texttt{saturation:torque}} +\newcommand{\atIMAX}{\texttt{saturation:current}} +\newcommand{\atVMAX}{\texttt{saturation:voltage}} +\newcommand{\atCRATE}{\texttt{saturation:current\_rate}} +\newcommand{\atKP}{\texttt{controller:kp}} +\newcommand{\atKI}{\texttt{controller:ki}} +\newcommand{\atKD}{\texttt{controller:kd}} +\newcommand{\atSLEW}{\texttt{controller:slewmax}} +\newcommand{\atIMAXINT}{\texttt{controller:Imax}} +\newcommand{\atL}{\texttt{inductance:L}} +\newcommand{\atTE}{\texttt{inductance:timeconst}} +\newcommand{\atCOGA}{\texttt{cogging:amplitude}} +\newcommand{\atCOGP}{\texttt{cogging:poles}} +\newcommand{\atCOGPH}{\texttt{cogging:phase}} +\newcommand{\atRT}{\texttt{thermal:resistance}} +\newcommand{\atTC}{\texttt{thermal:capacitance}} +\newcommand{\atTT}{\texttt{thermal:timeconst}} +\newcommand{\atALPHA}{\texttt{thermal:tempcoef}} +\newcommand{\atTREF}{\texttt{thermal:reftemp}} +\newcommand{\atTAMB}{\texttt{thermal:ambient}} +\newcommand{\atSIG}{\texttt{lugre:stiffness}} +\newcommand{\atSIGD}{\texttt{lugre:damping}} +\newcommand{\atTAUC}{\texttt{lugre:coulomb}} +\newcommand{\atTAUS}{\texttt{lugre:static}} +\newcommand{\atWS}{\texttt{lugre:stribeck}} +\newcommand{\atSIGV}{\texttt{lugre:viscous}} + +\title{MuJoCo DC Motor Model} +\author{Google DeepMind} +\date{} + +\begin{document} + +\maketitle + +\noindent We review DC motors and describe MuJoCo's \texttt{dcmotor} actuator. The equations are derived for brushed motors but apply equally to brushless ones, where electronic commutation reduces to an equivalent circuit. + +%============================================================================= +% BACKGROUND +%============================================================================= +\section{Background} +\label{sec:background} + +We use SI units throughout, but any coherent system of units applies. We assume motion is rotational; for linear motion replace radians with meters as required. + +% --------------------------------------------------------------------------- +% Electromagnetic Model +% --------------------------------------------------------------------------- +\subsection{Electromagnetic Model} +\label{sec:electromagnetics} + +The key electro-mechanical variables are + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Symbol & Description & Units \\ +\midrule +$v$ & Applied voltage & Volt \\ +$i$ & Current & Ampere \\ +$\omega$ & Angular velocity & radian/second \\ +$\tau$ & Output torque & Newton $\cdot$ meter \\ +\bottomrule +\end{tabular} +\end{table} + +\noindent and the key constants are + +\begin{table}[H] +\centering +\footnotesize +\begin{tabular}{@{}lll@{}} +\toprule +Symbol & Description & Units \\ +\midrule +$R$ & Resistance & Ohm \\ +$K_t$ & Torque constant & Newton $\cdot$ meter/Ampere \\ +$K_e$ & Back-EMF constant & Volt $\cdot$ second/radian \\ +\bottomrule +\end{tabular} +\end{table} + +\noindent The quasi-static model~\cite{hughes2019, maxon_formulas, simscape_dcmotor} assumes instantaneous electrical dynamics: current and torque are direct functions of voltage and velocity. The constitutive equations are the voltage balance \eqref{eq:voltage} and the torque law \eqref{eq:torque_law}: +\begin{subequations} +\label{eq:motor_laws} +\begin{align} + v &= i \, R + K_e \, \omega \label{eq:voltage} \\ + \tau &= K_t i \label{eq:torque_law} +\end{align} +\end{subequations} + +\noindent Solving for current and substituting, we have + +\begin{equation} + \tau = \frac{K_t}{R} (v - K_e \, \omega) + \label{eq:torque_speed} +\end{equation} + +\noindent Output torque is proportional to the difference between applied and back-EMF voltage $v_{\text{back}} = K_e \, \omega$ (Figure~\ref{fig:torque_speed}). + +\paragraph{Electrical constants.} +Fundamentally, both $K_t$ and $K_e$ arise from the same physical quantity: the magnetic flux $\Phi$ of the coil. Faraday's law gives $v_{\text{back}} = \Phi \, \omega$ and the Lorentz force gives $\tau = \Phi \, i$, so in SI units: + +\begin{equation*} + K_e = K_t + \label{eq:ke_eq_kt} +\end{equation*} + +\noindent This can also be seen from energy conservation: $P_e = i \, (K_e \, \omega) = (K_t \, i) \, \omega = P_m$. + +\pagebreak +\noindent Note the dimensions match: + +\vspace*{-\abovedisplayskip} +\begin{equation*} + \frac{\text{Volt}}{\text{radian}/\text{second}} + = \frac{\text{Joule}}{\text{Coulomb}/\text{second}} + = \frac{\text{Newton} \cdot \text{meter}}{\text{Ampere}} +\end{equation*} + +\noindent If these constants are the same, why have both? Two reasons. First, datasheets typically use mixed units ($K_e$ in RPM/V, $K_t$ in mN$\cdot$m/A), giving different values for the same physical quantity. Second, $K_t$ and $K_e$ are measured differently: $K_t$ by locking the rotor and measuring torque per Ampere; $K_e$ by spinning the rotor and measuring open-circuit Volts per radian/second. In the first case, high currents can lead to magnetic field saturation in the core, causing the effective $K_t$ to drop below $K_e$. The equality $K_e = K_t$ thus assumes $\Phi$ independent of $i$. We make this assumption for now and use a single motor constant $K \equiv K_t = K_e$ throughout the remainder of this document and internally in MuJoCo, but see note at end of \S\ref{sec:dcmotor}. + +\begin{figure}[H] +\centering +\begin{tikzpicture} +\pgfmathsetmacro{\taus}{1.0} +\pgfmathsetmacro{\wz}{1.0} +\begin{axis}[ + width=0.9\columnwidth, height=0.55\columnwidth, + axis lines=left, + clip=false, + xlabel={$\omega$}, ylabel={$\tau$}, + xmin=0, xmax={\wz*1.15}, ymin=0, ymax={\taus*1.15}, + xtick={\wz}, xticklabels={$\omega_0$}, + ytick={\taus}, yticklabels={$\tau_0$}, + tick style={thick}, + every axis x label/.style={at={(ticklabel* cs:1)}, anchor=west}, + every axis y label/.style={at={(ticklabel* cs:1)}, anchor=south}, +] +\addplot[thick, blue!15] coordinates {(0,\taus*0.7) (\wz*0.7,0)}; +\addplot[thick, blue!25] coordinates {(0,\taus*0.8) (\wz*0.8,0)}; +\addplot[thick, blue!50] coordinates {(0,\taus*0.9) (\wz*0.9,0)}; +\addplot[thick, blue] coordinates {(0,\taus) (\wz,0)}; +\node[font=\scriptsize, text=gray, align=center] + at (axis cs: \wz*0.25, \taus*0.2) + {decreasing\\ voltage}; +\draw[->, thick, gray] (axis cs: \wz*0.4, \taus*0.55) + -- (axis cs: \wz*0.4, \taus*0.15); +\node[font=\scriptsize] at (axis cs: \wz*0.4, -0.08) + {Speed}; +\node[font=\scriptsize, rotate=90] at (axis cs: -0.04, \taus*0.4) + {Torque}; +\end{axis} +\end{tikzpicture} +\caption{Torque-speed relationship \eqref{eq:torque_speed} at fixed voltage. As voltage decreases, the maximum torque and speed decrease linearly.} +\label{fig:torque_speed} +\end{figure} + +\paragraph{Current Saturation.} +A maximum current rating $i_{\max}$ limits the output torque: +\begin{equation} + \tau = \text{clip}\!\left(\frac{K}{R}(v - K \, \omega),\; + \pm K \, i_{\max} \right) + \label{eq:saturation} +\end{equation} +where the maximum torque $\tau_{\max} = K \, i_{\max}$. The feasible torque-speed envelope forms a parallelogram: + +\begin{figure}[H] +\centering +\begin{tikzpicture} +\pgfmathsetmacro{\taus}{1.3} +\pgfmathsetmacro{\wz}{1.0} +\pgfmathsetmacro{\taumax}{0.7} +\pgfmathsetmacro{\slope}{\taus/\wz} +\pgfmathsetmacro{\wcu}{(\taus-\taumax)/\slope} +\pgfmathsetmacro{\wcl}{(\taus+\taumax)/\slope} +\pgfmathsetmacro{\wext}{1.5} +\pgfmathsetmacro{\dexthi}{\taus+\slope*\wext} +\pgfmathsetmacro{\dextlo}{\taus-\slope*\wext} +\begin{axis}[ + width=0.9\columnwidth, height=0.6\columnwidth, + axis lines=middle, + xlabel={$\omega$}, ylabel={$\tau$}, + xmin=-1.6, xmax=1.6, ymin=-1.6, ymax=1.6, + xtick={-\wz, \wz}, xticklabels={$-\omega_0$, {}}, + ytick={-\taus, \taus}, + yticklabels={$-\tau_0$, $\tau_0$}, + tick style={thick}, + every axis x label/.style={at={(ticklabel* cs:1)}, anchor=west}, + every axis y label/.style={at={(ticklabel* cs:1)}, anchor=south}, +] +\fill[blue, opacity=0.08] + (-\wcl, \taumax) -- (\wcu, \taumax) -- (\wcl, -\taumax) + -- (-\wcu, -\taumax) -- cycle; +\addplot[thick, dashed, gray] coordinates {(-\wext, \dexthi) (\wext, \dextlo)}; +\addplot[thick, dashed, gray] coordinates {(-\wext, -\dextlo) (\wext, -\dexthi)}; +\addplot[thick, dashed, gray] coordinates {(-1.55, \taumax) (1.55, \taumax)}; +\addplot[thick, dashed, gray] coordinates {(-1.55, -\taumax) (1.55, -\taumax)}; +\addplot[thick, blue] coordinates + {(-\wcl, \taumax) (\wcu, \taumax) (\wcl, -\taumax) (-\wcu, -\taumax) + (-\wcl, \taumax)}; +\node[font=\scriptsize, anchor=south] at (axis cs: \wz, \taumax) + {$\tau_{\max}$}; +\node[font=\scriptsize, anchor=north] at (axis cs: -\wz, -\taumax) + {$-\tau_{\max}$}; +\draw[thick, dashed, gray] (axis cs: \wz, 0) -- (axis cs: \wz, -\taumax); +\draw[thick, dashed, gray] (axis cs: -\wz, 0) -- (axis cs: -\wz, \taumax); +\node[font=\normalsize, anchor=south] at (axis cs: \wz, 0.05) {$\omega_0$}; +\end{axis} +\end{tikzpicture} +\caption{Torque-speed envelope with current saturation~\eqref{eq:saturation}.} +\label{fig:saturation} +\end{figure} + +\noindent Note that datasheets typically distinguish two current limits. The \emph{continuous} (or \emph{nominal}) current $i_{\max}$ is the thermal limit: the maximum current the motor can sustain indefinitely without exceeding its maximum winding temperature. The \emph{peak} current $i_{\text{peak}}$ is a higher short-term limit, typically 5--10$\times$ the continuous value, constrained by demagnetization or commutation limits. + +\begin{table}[H] +\centering +\footnotesize +\setlength{\tabcolsep}{3pt} +\renewcommand{\arraystretch}{1.2} +\begin{tabular}{@{}llll@{}} +\toprule +Symbol & Description & Condition & Formula/note\\ +\midrule +$\tau_0$ & Stall Torque & $\omega=0$ & $\tau_0 = Kv / R$ \\ +$\omega_0$ & No-Load Speed & $\tau_{\text{load}}=0$ & + $\omega_0 \approx v / K$ \\ +$\partial\omega / \partial\tau$ & Gradient & Slope & + $-R/K^2$ \\ +$i_{\max}$ & Maximum Current & Limit & Thermal limit \\ +$\tau_{\max}$ & Maximum Torque & Limit & $\tau_{\max} = K \, i_{\max}$ \\ +\bottomrule +\end{tabular} +\caption{Named constants derived from the motor equations.} +\label{tab:electromech_constants} +\end{table} + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Inductance} +\label{sec:inductance} + +Including the effects of winding inductance $L$ (Henry) means treating the current $i$ as a state variable: +\begin{equation} + v = L \, \frac{di}{dt} + i \, R + K \, \omega + \label{eq:inductance} +\end{equation} +The change in current is proportional to the voltage and negatively proportional to both the instantaneous current and the rotation velocity. The time constant of this ODE is $t_e = L/R$. If $t_e \ll \Delta t$ (the simulation timestep), the current equilibrates within a single step and the quasi-static approximation~\eqref{eq:torque_speed} is adequate. + +Motor drivers often impose a hard limit on $di/dt$ to protect windings and electronics, bounding the torque ramp rate to $K \cdot (di/dt)_{\max}$. + +% --------------------------------------------------------------------------- +% Mechanical Model +% --------------------------------------------------------------------------- +\subsection{Mechanical Model} +\label{sec:mechanical} + +Several purely mechanical phenomena affect the motor's behavior and the effective delivered torque. + +\paragraph{Mechanical losses.} +These reduce the net torque available at the shaft: $\tau_{\text{net}} = \tau_{\text{elec}} - \tau_{\text{loss}}$. +\begin{itemize} + \item \textbf{Coulomb:} Constant torque opposing rotation (dry friction). + $\tau_{\text{loss}} = \tau_c \, \text{sgn}(\omega)$. Discontinuous; already available in MuJoCo as \texttt{frictionloss}. + \item \textbf{Viscous:} Drag is a smooth function of speed $\tau_{\text{loss}} = b(\omega)$. The simplest model is linear, with drag proportional to speed $\tau_{\text{loss}} = B \, \omega$, but higher order terms may be needed for higher-fidelity models e.g., $\tau_{\text{loss}} = B_1 \, \omega + B_2 \, \omega |\omega| + B_3 \, \omega^3 + \dots$. +\end{itemize} + +\noindent Datasheets report the \emph{no-load current} $i_0$: the current drawn when spinning freely at no-load speed $\omega_0$. At steady state, the electromagnetic torque balances all mechanical losses: +\begin{equation} + K \, i_0 = \tau_c + B \, \omega_0 + \label{eq:noload} +\end{equation} +This provides one constraint on two unknowns ($\tau_c$ and $B$). Without additional data, the user must either assume one dominates or obtain friction measurements at multiple speeds. In MuJoCo terms, $\tau_c$ maps to \texttt{frictionloss} and $B$ to \texttt{damping}. + +\noindent Combining current saturation with both mechanical losses, the net torque is: +\begin{equation*} + \tau_{\text{net}} = \text{clip}\!\left( \frac{K}{R}(v - K \, \omega),\; + \pm K\, i_{\max} \right) - B \, \omega - \tau_c \, \text{sgn}(\omega) + \label{eq:net_torque} +\end{equation*} +The clipping applies to the electrical torque (current limit), while both friction terms are mechanical losses applied to the output post-clipping. Viscous drag $-B\omega$ tilts the envelope; Coulomb friction $-\tau_c\,\text{sgn}(\omega)$ shifts the right half ($\omega > 0$) down and the left half ($\omega < 0$) up, creating a $2\tau_c$ discontinuity at $\omega = 0$ (Figure~\ref{fig:drag_linear}). + +\begin{figure}[H] +\centering +\begin{subfigure}[t]{\columnwidth} +\centering +\begin{tikzpicture} +\pgfmathsetmacro{\taus}{1.3} +\pgfmathsetmacro{\wz}{1.0} +\pgfmathsetmacro{\taumax}{0.7} +\pgfmathsetmacro{\bvis}{0.3} +\pgfmathsetmacro{\tauf}{0.2} +\pgfmathsetmacro{\slope}{\taus/\wz} +\pgfmathsetmacro{\wcu}{(\taus-\taumax)/\slope} +\pgfmathsetmacro{\wcl}{(\taus+\taumax)/\slope} +\pgfmathsetmacro{\wext}{1.5} +\pgfmathsetmacro{\dragL}{\bvis*\wext} +\pgfmathsetmacro{\dragR}{-\bvis*\wext} +%% Right half vertices (ω ≥ 0): shifted down by τ_f +\pgfmathsetmacro{\Ra}{\taumax-\tauf} +\pgfmathsetmacro{\Rb}{\taumax-\bvis*\wcu-\tauf} +\pgfmathsetmacro{\Rc}{-\taumax-\bvis*\wcl-\tauf} +\pgfmathsetmacro{\Rd}{-\taumax-\tauf} +%% Left half vertices (ω ≤ 0): shifted up by τ_f +\pgfmathsetmacro{\La}{\taumax+\bvis*\wcl+\tauf} +\pgfmathsetmacro{\Lb}{\taumax+\tauf} +\pgfmathsetmacro{\Lc}{-\taumax+\tauf} +\pgfmathsetmacro{\Ld}{-\taumax+\bvis*\wcu+\tauf} +\begin{axis}[ + width=0.9\columnwidth, height=0.6\columnwidth, + axis lines=middle, + xlabel={$\omega$}, ylabel={$\tau$}, + xmin=-1.8, xmax=1.8, ymin=-1.8, ymax=1.8, + xtick=\empty, ytick=\empty, + tick style={thick}, + every axis x label/.style={at={(ticklabel* cs:1)}, anchor=west}, + every axis y label/.style={at={(ticklabel* cs:1)}, anchor=south}, +] +\fill[blue, opacity=0.08] + (0, \Ra) -- (\wcu, \Rb) -- (\wcl, \Rc) -- (0, \Rd) -- cycle; +\addplot[thick, blue] coordinates + {(0, \Ra) (\wcu, \Rb) (\wcl, \Rc) (0, \Rd)}; +\fill[blue, opacity=0.08] + (-\wcl, \La) -- (0, \Lb) -- (0, \Lc) -- (-\wcu, \Ld) -- cycle; +\addplot[thick, blue] coordinates + {(-\wcl, \La) (0, \Lb) (0, \Lc) (-\wcu, \Ld) (-\wcl, \La)}; +\addplot[thick, dashed, gray] coordinates {(0.01, {-\tauf-\bvis*0.01}) (\wext, {-\tauf+\dragR})}; +\addplot[thick, dashed, gray] coordinates {(-\wext, {\tauf+\dragL}) (-0.01, {\tauf+\bvis*0.01})}; +\node[font=\footnotesize, anchor=north west] at (axis cs: -1.75, -0.45) + {$-B\omega - \tau_c\,\text{sgn}(\omega)$}; +\draw[->, gray, thick] (axis cs: -1.4, -0.45) -- (axis cs: -1.3, {\tauf+\bvis}); +\pgfmathsetmacro{\gapmid}{(\Ra+\Lb)/2} +\draw[thick, <->, gray] (axis cs: 0.12, \Ra) -- (axis cs: 0.12, \Lb); +\node[font=\scriptsize, anchor=west] at (axis cs: 0.18, \gapmid) + {$2\tau_c$}; +\end{axis} +\end{tikzpicture} +\caption{Linear viscous drag and Coulomb friction.} +\label{fig:drag_linear} +\end{subfigure} + +\vspace{0.5em} + +\begin{subfigure}[t]{\columnwidth} +\centering +\begin{tikzpicture} +\pgfmathsetmacro{\taus}{1.3} +\pgfmathsetmacro{\wz}{1.0} +\pgfmathsetmacro{\taumax}{0.7} +\pgfmathsetmacro{\Bone}{0.15} +\pgfmathsetmacro{\Btwo}{0.35} +\pgfmathsetmacro{\slope}{\taus/\wz} +\pgfmathsetmacro{\wcl}{(\taus+\taumax)/\slope} +\pgfmathsetmacro{\wext}{1.7} +\begin{axis}[ + width=0.9\columnwidth, height=0.6\columnwidth, + axis lines=middle, + xlabel={$\omega$}, ylabel={$\tau$}, + xmin=-2.0, xmax=2.0, ymin=-2.0, ymax=2.0, + xtick=\empty, ytick=\empty, + tick style={thick}, + every axis x label/.style={at={(ticklabel* cs:1)}, anchor=west}, + every axis y label/.style={at={(ticklabel* cs:1)}, anchor=south}, + samples=200, +] +\addplot[name path=upper, thick, blue, domain=-\wcl:\wcl] + {min(\taus - \slope*x, \taumax) - \Bone*x - \Btwo*x*abs(x)}; +\addplot[name path=lower, thick, blue, domain=-\wcl:\wcl] + {max(-\taus - \slope*x, -\taumax) - \Bone*x - \Btwo*x*abs(x)}; +\addplot[blue, opacity=0.08] fill between[of=upper and lower]; +\addplot[thick, dashed, gray, domain=-\wext:\wext] + {-\Bone*x - \Btwo*x*abs(x)}; +\node[font=\footnotesize, anchor=north west] at (axis cs: -1.9, -0.5) + {$-b(\omega)$}; +\draw[->, gray, thick] (axis cs: -1.5, -0.5) -- (axis cs: -1.3, {0.1 + 0.35*1.3*1.3}); +\end{axis} +\end{tikzpicture} +\caption{Nonlinear viscous drag $b(\omega) = B_1\omega + B_2\omega|\omega|$, no friction.} +\label{fig:drag_nonlinear} +\end{subfigure} + +\caption{Torque-speed envelopes with mechanical losses. The dashed gray line shows the drag function; the shaded region is the achievable torque at each speed. Note that datasheet torque-speed curves typically plot the first quadrant only.} +\label{fig:drag} +\end{figure} + +\paragraph{Rotor Inertia and Gearing.} +Every DC motor datasheet lists the rotor inertia $J_r$ (kg$\cdot$m$^2$). When a gear train with ratio $N$ is attached, the effective inertia reflected to the output shaft is $J_{\text{eff}} = J_r N^2$~\cite{tedrake2024}. Note that real gearboxes also introduce efficiency losses (typically 70--90\%), which reduce the transmitted torque by a multiplicative factor $\eta$, approximated by effectively reducing the motor constant $K_{\text{eff}} = \eta K$. + +\paragraph{Cogging Torque.} +Brushless DC motors exhibit \emph{cogging torque}: a position-dependent torque ripple caused by the interaction between permanent magnets and stator slots. It can be modeled as a periodic bias: +\begin{equation} + \tau_{\text{cog}}(\theta) = A \sin(N_p \, \theta + \phi) + \label{eq:cogging} +\end{equation} +where $A$ is the amplitude, $N_p$ is the number of pole pairs times the number of slots per pole, and $\phi$ is a phase offset. Cogging torque is significant primarily at low speeds. Datasheets sometimes report peak cogging as a percentage of rated torque (typically 1--5\%). + +\begin{table}[H] +\centering +\footnotesize +\setlength{\tabcolsep}{3pt} +\renewcommand{\arraystretch}{1.2} +\begin{tabular}{@{}lll@{}} +\toprule +Symbol & Description & Formula / Note \\ +\midrule +$\tau_c$ & Coulomb friction & $\tau_c\,\text{sgn}(\omega)$ \\ +$B$ & Viscous drag (linear) & $B\,\omega$ \\ +$\omega_0$ & No-load speed & + $\omega_0 = v\,K / (K^2 + R\,B)$ \\ +$J_r$ & Rotor inertia & units: kg$\cdot$m$^2$ \\ +$N$ & Gear ratio & $J_{\text{eff}} = J_r N^2$ \\ +$\eta$ & Gearbox efficiency & $K' = \eta \, K$ \\ +$A$ & Cogging amplitude & $\tau_{\text{cog}} = A\sin(N_p\theta + \phi)$ \\ +$N_p$ & Cogging periodicity & poles $\times$ slots/pole \\ +$\phi$ & Cogging phase & offset \\ +\bottomrule +\end{tabular} +\caption{Named constants related to mechanical properties. Note that unlike in Table~\ref{tab:electromech_constants}, the non-approximate expression for $\omega_0$ takes into account the linear drag $B$ (assuming no high-order terms).} +\label{tab:key_constants} +\end{table} + +\paragraph{Backlash.} +Gearboxes introduce backlash: a small angular deadband where the motor can turn without moving the output shaft. Datasheets report this in arcminutes. MuJoCo supports backlash modeling via a dual-joint decomposition; \href{https://mujoco.readthedocs.io/en/stable/modeling.html#backlash}{see here} for details. + +% --------------------------------------------------------------------------- +% Thermal Model +% --------------------------------------------------------------------------- +\subsection{Thermal Model} +\label{sec:thermal} + +Winding temperature affects motor performance primarily through increased copper resistance, and can be modeled as a single lumped thermal state. The thermal constants are + +\begin{table}[H] +\centering +\footnotesize +\begin{tabular}{@{}lll@{}} +\toprule +Symbol & Description & Units \\ +\midrule +$R_T$ & Thermal resistance & Kelvin/Watt \\ +$C$ & Thermal capacitance & Joule/Kelvin \\ +$t_T = R_T C$ & Thermal time constant & second \\ +$\alpha$ & Resistance temp.\ coefficient & 1/Kelvin \\ +$T_0$ & Reference temperature & degree Celsius \\ +$T_a$ & Ambient temperature & degree Celsius \\ +\bottomrule +\end{tabular} +\caption{Thermal model constants. Units involving temperature differences use Kelvin (equivalent to Celsius for differences); absolute temperatures use degree Celsius, following datasheet convention.} +\end{table} + +\noindent Note that some manufacturers specify two thermal resistances: $R_{\text{th1}}$ (winding-to-housing) and $R_{\text{th2}}$ (housing-to-ambient), which sum to give the total winding-to-ambient thermal resistance $R_T = R_{\text{th1}} + R_{\text{th2}}$. The single-node model above uses $R_T$ directly. + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Lumped Thermal ODE} +\label{sec:thermal_ode} + +The winding temperature $T$ evolves according to a first-order lumped model driven by the power dissipation $P$ (Watt, detailed in \S\ref{sec:thermal_losses}): +\begin{equation} + \frac{\partial T}{\partial t} = \frac{1}{C} P - \frac{T - T_a}{t_T} + \label{eq:thermal_ode} +\end{equation} +where $t_T = R_T C$ is the thermal time constant. This produces exponential rise/decay toward a steady-state temperature $T_{ss} = T_a + R_T P$ (Figure~\ref{fig:thermal_response}). + +\begin{figure}[ht] +\centering +\begin{tikzpicture} +\pgfmathsetmacro{\Tss}{1.0} +\pgfmathsetmacro{\ttau}{1.0} +\pgfmathsetmacro{\xmax}{4.5} +\begin{axis}[ + width=0.9\columnwidth, height=0.45\columnwidth, + axis lines=left, + clip=false, + xlabel={$t$}, ylabel={$T - T_a$}, + xmin=0, xmax=\xmax, ymin=0, ymax={\Tss*1.25}, + xtick={\ttau}, xticklabels={$t_T$}, + ytick={{\Tss*(1-exp(-1))}, \Tss}, + yticklabels={$(1{-}1/e)\,R_T P$, $R_T P$}, + tick style={thick}, + every axis x label/.style={at={(ticklabel* cs:1)}, anchor=west}, + every axis y label/.style={at={(ticklabel* cs:1)}, anchor=south}, +] +\addplot[thick, blue, domain=0:\xmax, samples=100] + {\Tss*(1 - exp(-x/\ttau))}; +\addplot[thick, dashed, gray] coordinates {(0,\Tss) (\xmax,\Tss)}; +\draw[thick, dashed, gray] (axis cs:\ttau, 0) -- (axis cs:\ttau, {\Tss*(1-exp(-1))}); +\draw[thick, dashed, gray] (axis cs:0, {\Tss*(1-exp(-1))}) -- (axis cs:\ttau, {\Tss*(1-exp(-1))}); +\node[font=\scriptsize, anchor=south] at (axis cs:\xmax*0.5, \Tss) + {$T_{ss} = T_a + R_T P$}; +\end{axis} +\end{tikzpicture} +\caption{Temperature rise under constant power dissipation $P$. + At $t = t_T$, it reaches $(1-1/e) \approx 63\%$ of its steady-state value.} +\label{fig:thermal_response} +\end{figure} + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Losses} +\label{sec:thermal_losses} + +The dominant loss is copper (Joule) heating: +\begin{equation*} + P = i^2 R(T) + \label{eq:copper_loss} +\end{equation*} +Optionally, speed-dependent iron losses (eddy-current and hysteresis losses in the stator laminations) can be included~\cite{hughes2019}: +\begin{equation*} + P = i^2 R(T) + K_{\text{fe}} \omega^2 + \label{eq:total_loss} +\end{equation*} +The iron loss coefficient $K_{\text{fe}}$ is not typically listed on datasheets and must be identified from efficiency curves or manufacturer simulation tools. For most hobby and robotics motors, iron losses are small compared to copper losses and can be neglected. They become significant at high speeds in large industrial motors. + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Temperature-Dependent Resistance} +\label{sec:resistance_temperature} + +Copper resistance increases approximately linearly with temperature: +\begin{equation} + R(T) = R_0 \left(1 + \alpha (T - T_0)\right) + \label{eq:resistance_temperature} +\end{equation} +where $R_0$ is resistance at reference temperature $T_0$ and $\alpha \approx 0.0039 \, \text{K}^{-1}$ for copper. This is the dominant thermal feedback: as $T$ rises, $R$ increases, so for a given voltage the current $i = (v - K \omega) / R(T)$ drops, reducing torque. + +Note that $R(T)$ also increases heating for a given current ($P = i^2 R(T)$), creating mild positive feedback under current control. + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Magnet Flux Derating} +\label{sec:magnet_derating} + +Permanent magnet flux weakens with temperature, reducing $K$: +\begin{equation*} + K(T) = K_0 \left(1 + \alpha_m (T - T_0)\right) + \label{eq:kt_temperature} +\end{equation*} +with $\alpha_m < 0$ (motor-dependent). This effect is often small over normal operating ranges and can be ignored. + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Thermal Derating} +\label{sec:thermal_derating} + +Real actuators limit current as winding temperature approaches a maximum: +\begin{equation*} + i_{\max}(T) = \begin{cases} + i_{\text{rated}} & T \le T_1 \\ + i_{\text{safe}} + (i_{\text{rated}} - i_{\text{safe}}) \, s(T) & T_1 < T < T_2 \\ + i_{\text{safe}} & T \ge T_2 + \end{cases} +\end{equation*} +where $s(T)$ is a smooth interpolant between $T_1$ and $T_2$. This reduces the maximum available torque as the motor heats up. + +% --------------------------------------------------------------------------- +% Micro-Friction Models +% --------------------------------------------------------------------------- +\subsection{Micro-Friction Models} +\label{sec:micro_friction} + +Simple macroscopic friction models (Coulomb, viscous) cannot capture complex mechanical phenomena common in real motors with gear trains, such as pre-sliding hysteresis and stick-slip limit cycles. To capture these behaviors, a richer dynamic model is required. At the microscopic level, two surfaces in contact touch at many asperities which deform elastically under tangential load. This can be modeled as an average bristle deflection $z$, governed by a first-order ODE driven by the relative velocity $\omega$. Friction torque is then a function of $z$, $\dot{z}$, and $\omega$. + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Dahl Model} +\label{sec:dahl} + +The simplest stateful model~\cite{dahl68} treats friction as a rate-independent hysteresis operator derived from the stress-strain curve: +\begin{equation*} + \dot{z} = \omega - \frac{\sigma_0}{\tau_c} |\omega| \, z + \label{eq:dahl_state} +\end{equation*} +with output $\tau = -\sigma_0 z$. In steady state ($\dot{z}=0$), $z_{ss} = \tau_c \, \text{sgn}(\omega) / \sigma_0$ so $\tau_{ss} = -\tau_c \, \text{sgn}(\omega)$: pure Coulomb friction opposing motion. The two parameters are the bristle stiffness $\sigma_0$ (torque/radian) and the Coulomb friction torque $\tau_c$. +For small displacements the model is approximately linear ($\tau \approx -\sigma_0 \theta$), giving spring-like pre-sliding behavior with hysteresis during direction reversals (Figure~\ref{fig:hysteresis}). The Dahl model does not capture the Stribeck effect~\cite{stribeck1902} (the drop in friction at low velocity) and thus cannot predict stick-slip motion. + +\begin{figure}[H] +\centering +\begin{tikzpicture} +\begin{axis}[ + width=0.9\columnwidth, height=0.55\columnwidth, + axis lines=middle, + xlabel={$\theta$}, ylabel={$\tau$}, + xmin=-1.4, xmax=1.4, ymin=-1.4, ymax=1.4, + xtick=\empty, ytick=\empty, + every axis x label/.style={at={(ticklabel* cs:1)}, anchor=west}, + every axis y label/.style={at={(ticklabel* cs:1)}, anchor=south}, + clip=false, +] +\pgfmathsetmacro{\sig}{2.5} +\pgfmathsetmacro{\Fc}{1.0} +\pgfmathsetmacro{\xm}{1.0} +\pgfmathsetmacro{\ch}{(exp(\sig*\xm)+exp(-\sig*\xm))/2} +\addplot[thick, blue, domain=-\xm:\xm, samples=150, name path=lower] + {-\Fc*(1 - exp(-\sig*x)/\ch)}; +\addplot[thick, blue, domain=-\xm:\xm, samples=150, name path=upper] + {\Fc*(1 - exp(\sig*x)/\ch)}; +\addplot[blue, opacity=0.08] fill between[of=lower and upper]; +\draw[thick, dotted] (axis cs:-1.4, -\Fc) -- (axis cs:1.4, -\Fc) + node[right, font=\scriptsize] {$-\tau_c$}; +\draw[thick, dotted] (axis cs:-1.4, \Fc) -- (axis cs:1.4, \Fc) + node[right, font=\scriptsize] {$\tau_c$}; +\draw[->, thick, gray] (axis cs:0.05, -0.78) -- (axis cs:0.25, -0.83); +\draw[->, thick, gray] (axis cs:-0.05, 0.78) -- (axis cs:-0.25, 0.83); +\end{axis} +\end{tikzpicture} +\caption{Hysteresis loop: friction torque $\tau$ vs.\ displacement $\theta$ under slow + periodic loading (Dahl model). + The loop area represents energy dissipated per cycle. + Unlike memoryless Coulomb friction, the torque is continuous.} +\label{fig:hysteresis} +\end{figure} + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{LuGre Model} +\label{sec:lugre} + +The LuGre model~\cite{dewit95, lugre_revisited} extends Dahl by making the bristle saturation velocity-dependent and adding micro-damping and viscous terms: +\begin{subequations} +\label{eq:lugre} +\begin{align} + \dot{z} &= \omega - \sigma_0 \frac{|\omega|}{g(\omega)} z + \label{eq:lugre_state} \\ + \tau &= -(\sigma_0 z + \sigma_1 \dot{z} + \sigma_2 \omega) + \label{eq:lugre_force} +\end{align} +\end{subequations} +where the function $g(\omega)$ captures the Stribeck effect: +\begin{equation} + g(\omega) = \tau_c + (\tau_s - \tau_c) \, e^{-(\omega/\omega_s)^\gamma} + \label{eq:stribeck} +\end{equation} +with exponent $\gamma$ typically 1 or 2. In steady state, $\tau_{ss}(\omega) = -g(\omega) \, \text{sgn}(\omega) - \sigma_2 \omega$: the classic Stribeck curve (Figure~\ref{fig:stribeck}). + +\begin{figure}[H] +\centering +\begin{tikzpicture} +\begin{axis}[ + width=0.9\columnwidth, height=0.55\columnwidth, + axis lines=middle, + xlabel={$\omega$}, ylabel={$\tau_{ss}$}, + xmin=-3, xmax=3, ymin=-2.5, ymax=2.5, + xtick=\empty, ytick=\empty, + every axis x label/.style={at={(ticklabel* cs:1)}, anchor=west}, + every axis y label/.style={at={(ticklabel* cs:1)}, anchor=south}, + clip=false, +] +\pgfmathsetmacro{\Fc}{1.0} +\pgfmathsetmacro{\Fs}{1.8} +\pgfmathsetmacro{\vs}{0.5} +\pgfmathsetmacro{\sigtwo}{0.15} +\addplot[thick, blue, domain=0.01:3, samples=200] + {-(\Fc + (\Fs-\Fc)*exp(-(x/\vs)^2)) - \sigtwo*x}; +\addplot[thick, blue, domain=-3:-0.01, samples=200] + {(\Fc + (\Fs-\Fc)*exp(-(-x/\vs)^2)) - \sigtwo*x}; +\addplot[thick, dashed, gray, domain=-3:3, samples=2] {-\sigtwo*x}; +\draw[thick, dotted] (axis cs:0,-\Fs) -- (axis cs:3,-\Fs) + node[right, font=\scriptsize] {$-\tau_s$}; +\draw[thick, dotted] (axis cs:0,-\Fc) -- (axis cs:3,-\Fc) + node[right, font=\scriptsize] {$-\tau_c$}; +\draw[thick, dotted] (axis cs:0,\Fs) -- (axis cs:-3,\Fs) + node[left, font=\scriptsize] {$\tau_s$}; +\draw[thick, dotted] (axis cs:0,\Fc) -- (axis cs:-3,\Fc) + node[left, font=\scriptsize] {$\tau_c$}; +\node[font=\footnotesize, anchor=north east] at (axis cs:-0.5, -0.1) + {$-\sigma_2\omega$}; +\end{axis} +\end{tikzpicture} +\caption{Steady-state friction $\tau_{ss}(\omega) = -g(\omega)\,\text{sgn}(\omega) - \sigma_2 \omega$. + Stiction torque $\tau_s$ at $\omega\!=\!0$ drops to Coulomb level $\tau_c$ + over velocity scale $\omega_s$ (Stribeck effect). Friction opposes motion.} +\label{fig:stribeck} +\end{figure} + +\noindent The Dahl model is recovered by setting $g(\omega) = \tau_c$ and $\sigma_1 = \sigma_2 = 0$. Linearizing around $\omega = z = 0$ gives second-order dynamics $J\ddot{\theta} - (\sigma_1 + \sigma_2)\dot{\theta} - \sigma_0 \theta = \tau$ (applied torque): a spring-damper with natural frequency $\omega_n = \sqrt{\sigma_0/J}$, critically damped when $\sigma_1 = 2\sqrt{J\sigma_0}$. + +The LuGre model can be shown to be input-strictly-passive (the map $\omega \mapsto \tau$ dissipates energy) provided $\sigma_2 > \sigma_1 (\tau_s - \tau_c)/\tau_c$. This passivity condition limits $\sigma_1$ and can lead to underdamped micro-dynamics, motivating the following extension. + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Velocity-Dependent Damping} +\label{sec:vel_damping} + +The passivity constraint on $\sigma_1$ can be relaxed by making the micro-damping decrease with velocity: +\begin{equation*} + \sigma_1(\omega) = \bar{\sigma}_1\, e^{-(\omega/\omega_s)^\beta} + \label{eq:sigma1_vel} +\end{equation*} +This allows large damping in the stiction regime (good for numerical stability and physical fidelity) while satisfying passivity at higher velocities where $\sigma_1 \to 0$. + +Together with $\tau_c$ from \S\ref{sec:mechanical}, the LuGre model with velocity-dependent damping adds six parameters: +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Symbol & Description & Units \\ +\midrule +$\sigma_0$ & Bristle stiffness, pre-sliding slope & N$\cdot$m/rad \\ +$\bar{\sigma}_1$ & Peak bristle damping at $\omega = 0$ & N$\cdot$m$\cdot$s/rad \\ +$\sigma_2$ & Viscous damping coefficient & N$\cdot$m$\cdot$s/rad \\ +$\tau_s$ & Stiction torque, $\tau_s \ge \tau_c$ & N$\cdot$m \\ +$\omega_s$ & Stribeck velocity & rad/s \\ +$\beta$ & Damping decay exponent & dimensionless \\ +\bottomrule +\end{tabular} +\caption{Parameters of the LuGre friction model (\S\ref{sec:lugre}--\ref{sec:vel_damping}).} +\label{tab:lugre_params} +\end{table} + + +\newpage +%============================================================================= +% IMPLEMENTATION +%============================================================================= +\section{Implementation} +\label{sec:implementation} + +Here we describe MuJoCo's \texttt{dcmotor} actuator. Some scalars are grouped into vectors; we use a colon to denote such scalar sub-attributes, e.g.\ \texttt{cogging:phase} refers to the third element of the \texttt{cogging} attribute (see Tables \ref{tab:mjcf_attributes} and \ref{tab:cogging_impl}). + +\begin{table}[H] +\centering +\footnotesize +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Size & Description \\ +\midrule +\texttt{resistance} & 1 & Terminal resistance $R$ \\ +\texttt{motorconst} & 2 & Motor constants ($K_t, K_e$; see below) \\ +\texttt{nominal} & 3 & Nominal operating point ($v_n, \tau_0, \omega_0$) \\ +\texttt{inductance} & 2 & Electrical dynamics ($L, t_e$) \\ +\texttt{thermal} & 6 & Thermal model ($R_T, C, t_T, \alpha, T_0, T_a$) \\ +\texttt{saturation} & 4 & Limits ($\tau_{\max}, i_{\max}, v_{\max}, (di{/}dt)_{\max}$) \\ +\midrule +\texttt{cogging} & 3 & Cogging torque ($A, N_p, \phi$) \\ +\texttt{lugre} & 6 & LuGre friction ($\sigma_0, \sigma_1, \sigma_2, \tau_c, \tau_s, \omega_s$) \\ +\texttt{damping} & 3 & Viscous damping coefficients \\ +\texttt{armature} & 1 & Armature inertia \\ +\midrule +\texttt{input} & keyword & Mode (voltage/position/velocity) \\ +\texttt{controller} & 5 & Gains and slew ($k_p, k_i, k_d, s, I_{\max}$) \\ +\bottomrule +\end{tabular} +\caption{MJCF attributes for the \texttt{dcmotor} actuator, split into electrical, mechanical and control groupings.} +\label{tab:mjcf_attributes} +\end{table} + +% --------------------------------------------------------------------------- +% Stateless dcmotor +% --------------------------------------------------------------------------- +\subsection{Stateless DC Motor} +\label{sec:dcmotor} + +The output torque of the stateless motor follows Eq.~\eqref{eq:saturation}, mapping physical parameters to the underlying affine model. The three core parameters are the effective motor constant $K$, resistance $R$, and maximum torque $\tau_{\max}$. +They are stored in \texttt{mjModel} as follows: + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Symbol & Description & \texttt{mjModel} storage \\ +\midrule +$R$ & Resistance & \texttt{gainprm[0]} \\ +$K$ & Effective motor constant & \texttt{gainprm[1]} \\ +$\tau_{\max}$ & Maximum torque & \texttt{forcerange} \\ +\bottomrule +\end{tabular} +\caption{Stateless DC motor core parameters.} +\end{table} + +\noindent The gain $G = K/R$ and back-EMF bias $-GK\omega$ are computed at runtime. Storing $R$ separately allows temperature-dependent resistance $R(T)$, Eq.~\eqref{eq:resistance_temperature}, to be applied. These three core parameters can be specified with a combination of eight sub-attributes + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Symbol & Units \\ +\midrule +\atR{} & $R$ & Ohm \\ +\atKt{} & $K_t$ & N$\cdot$m/A \\ +\atKe{} & $K_e$ & V$\cdot$s/rad \\ +\atVM{} & $v_n$ & Volt \\ +\atSTALL{} & $\tau_0\!=\!Kv_n/R$ & N$\cdot$m \\ +\atNLS{} & $\omega_0\!\approx\!v_n/K$ & rad/s \\ +\atTMAX{} & $\tau_{\max}$ & N$\cdot$m \\ +\atIMAX{} & $i_{\max}$ & Ampere \\ +\bottomrule +\end{tabular} +\caption{Stateless DC motor basic attributes.} +\end{table} + +\noindent The attribute \atK{} has two sub-attributes, \texttt{Kt} and \texttt{Ke}. If both are positive, $K = \sqrt{K_t K_e}$ (preserving power balance $K^2 = K_t K_e$). If only one is positive, $K$ equals that value. + +\pagebreak +\noindent The following attribute combinations are supported: +\begin{enumerate}[itemsep=2pt, parsep=0pt, topsep=2pt] + \item Effective motor constant $K$, one of: + \begin{itemize}[itemsep=1pt, parsep=0pt, topsep=1pt] + \item \atKt{} \emph{and/or} \atKe{} + \item \atNLS{} \emph{and} \atVM{} + \end{itemize} + \item Resistance $R$, one of: + \begin{itemize}[itemsep=1pt, parsep=0pt, topsep=1pt] + \item \atR{} + \item \atSTALL{} \emph{and} \atVM{} + \end{itemize} + \item Maximum torque $\tau_{\max}$, one of: + \begin{itemize}[itemsep=1pt, parsep=0pt, topsep=1pt] + \item \atTMAX{} + \item \atIMAX{} + \end{itemize} +\end{enumerate} + +\noindent \atIMAX{} corresponds to the continuous (thermal) current limit. Peak current behavior can be approximated with the thermal model (\S\ref{sec:temperature_impl}). + +\paragraph{Rotor Inertia and Gearing.} To model rotor inertia with a gear train, set the actuator's \texttt{armature} $= J_r$ and \texttt{gear} $= N$. Actuator-level \texttt{armature} automatically scales the inertia by $N^2$ to reflect $J_{\text{eff}}$ to the output shaft. + +\paragraph{Mechanical Drag.} The full torque-speed envelope applies viscous drag \emph{outside} the current clamp: +\begin{equation*} + \tau_{\text{net}} = \text{clip}\!\left( \frac{K}{R}(v - K \, \omega),\; + \pm \tau_{\max} \right) - b(\omega) + \label{eq:drag} +\end{equation*} +Actuator-level \texttt{damping} reproduces this post-clamp behavior. It accepts an array of polynomial drag coefficients (\texttt{damping[0]} $= B_1$, \texttt{damping[1]} $= B_2$, $\dots$). As with \texttt{armature}, actuator-level \texttt{damping} is scaled by $N^2$. + +\paragraph{Cogging Torque.} The magnetic torque ripple of Eq.~\eqref{eq:cogging} is modeled as a periodic bias added to the actuator force, where $\theta$ is the \texttt{actuator\_length}, i.e.\ the transmission-transformed joint angle. + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Symbol & Units \\ +\midrule +\atCOGA{} & $A$ & N$\cdot$m \\ +\atCOGP{} & $N_p$ & dimensionless \\ +\atCOGPH{} & $\phi$ & radian \\ +\bottomrule +\end{tabular} +\caption{Cogging torque attributes.} +\label{tab:cogging_impl} +\end{table} + +\paragraph{Mapping to Isaac Lab.} +Isaac Lab~\cite{isaaclab2025} implements the stateless DC motor model. Table~\ref{tab:mapping} maps its attributes to the constants defined in this document and to the \texttt{dcmotor} attributes. + +\begin{table}[H] +\centering +\footnotesize +\begin{tabular}{@{}lll@{}} +\toprule +Symbol & MuJoCo & Isaac Lab \\ +\midrule +$\tau_0$ & \atSTALL{} & \texttt{saturation\_effort} \\ +$\omega_0$ & \atNLS{} & \texttt{velocity\_limit} \\ +$\tau_{\max}$ & \atTMAX{} & \texttt{effort\_limit} \\ +\bottomrule +\end{tabular} +\caption{Mapping of attributes to Isaac Lab.} +\label{tab:mapping} +\end{table} + +\noindent Isaac Lab does not expose electrical parameters. To reproduce the same torque-speed envelope, set \atVM{} to any positive value (e.g.,~\texttt{1}) and \texttt{ctrlrange} to $\pm$\,that value (e.g., \texttt{"-1 1"}). + +\paragraph{Gearbox Efficiency.} Gearbox efficiency $\eta$ is not a separate attribute. To account for transmission losses, reduce the motor constant: $K \rightarrow \eta K$. This correctly reduces forward torque transmission. + +\paragraph{Computed parameters.} +Several derived quantities that appear on datasheets can be computed and used to cross-check the parameterization. The torque-speed gradient $\partial\omega/\partial\tau = -R/K^2$ gives the slope of the torque-speed line (Table~\ref{tab:electromech_constants}). The mechanical time constant $t_m = R\,J/K^2$ is the time for the motor to reach 63\% of its no-load speed under a voltage step, where $J$ is the rotor inertia (\texttt{armature}). The nominal (continuous) torque is $\tau_n = K \cdot i_{\max}$. The no-load current $i_0$ can be computed from Eq.~\eqref{eq:noload} given known friction parameters. See Table~\ref{tab:datasheet}. + +\paragraph{Not modeled:} +Nonlinear torque constant $K_t(i)$. Separate $K_t$ and $K_e$ values are accepted via \atKt{} and \atKe{} but collapsed to a single effective $K = \sqrt{K_t K_e}$. + + +% --------------------------------------------------------------------------- +% Stateful Current +% --------------------------------------------------------------------------- +\subsection{Stateful Current} +\label{sec:current_impl} + +A winding current state variable governed by Eq.~\eqref{eq:inductance} is added if the electrical time constant $t_e > 0$ (derived from inductance $L > 0$ or specified directly). When enabled, the state is integrated by \texttt{mjDYN\_DCMOTOR}, and the gain switches from $K/R$ (stateless) to $K$ (stateful). +The time constant $t_e$ can be determined by either: + +\begin{itemize}[itemsep=0pt, parsep=0pt, topsep=2pt] + \item \atTE{} + \item \atL{} \emph{and} \atR{} (via $t_e = L/R$) +\end{itemize} + +\paragraph{Current rate limiting.} When the sub-attribute \atCRATE{} is set ($(di/dt)_{\max} > 0$) and the current state is enabled ($t_e > 0$), the rate of change of current is clamped: +\begin{equation*} + \frac{di}{dt} \leftarrow \text{clip}\!\left(\frac{di}{dt},\; \pm(di/dt)_{\max}\right) +\end{equation*} +This limits the torque ramp rate to $K \cdot (di/dt)_{\max}$ (N$\cdot$m/s) without requiring any additional state variables, since the current $i$ is already an activation variable and we are simply clamping its rate of change. This attribute has no effect when $t_e = 0$ (stateless current). + +\begin{table}[H] +\centering +\footnotesize +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Symbol & Units \\ +\midrule +\atL{} & $L$ & Henry \\ +\atTE{} & $t_e\!=\!L/R$ & second \\ +\atCRATE{} & $(di{/}dt)_{\max}$ & Ampere/second \\ +\bottomrule +\end{tabular} +\caption{Stateful current attributes.} +\end{table} + +% --------------------------------------------------------------------------- +% Temperature +% --------------------------------------------------------------------------- +\subsection{Temperature} +\label{sec:temperature_impl} + +A winding temperature state governed by the lumped ODE~\eqref{eq:thermal_ode} is added if any of the thermal attributes ($R_T, C, t_T$) are specified. The state $T$ is the temperature rise above ambient ($T = T_{\text{winding}} - T_a$), so the absolute temperature is $T + T_a$. Temperature modifies the winding resistance via Eq.~\eqref{eq:resistance_temperature}, which feeds back into the motor equation: higher temperature increases resistance, leading to reduced current for a given voltage. + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Symbol & Units \\ +\midrule +\atRT{} & $R_T$ & K/W \\ +\atTC{} & $C$ & J/K \\ +\atTT{} & $t_T\!=\!R_T C$ & s \\ +\atALPHA{} & $\alpha$ & 1/K \\ +\atTREF{} & $T_0$ & \textdegree C \\ +\atTAMB{} & $T_a$ & \textdegree C \\ +\bottomrule +\end{tabular} +\caption{Thermal model attributes.} +\end{table} + +\noindent The time constant $t_T$ can be determined by either: +\begin{itemize}[itemsep=0pt, parsep=0pt, topsep=2pt] + \item \atTT{} + \item \atRT{} \emph{and} \atTC{} +\end{itemize} + +\paragraph{Not modeled:} +Iron losses (\S\ref{sec:thermal_losses}), magnet flux derating (\S\ref{sec:magnet_derating}), and thermal current derating (\S\ref{sec:thermal_derating}). Only copper losses ($i^2 R$) drive the thermal model; $K$ is treated as temperature-independent. + +% --------------------------------------------------------------------------- +% Stateful Friction +% --------------------------------------------------------------------------- +\subsection{Stateful Friction} +\label{sec:friction_impl} + +A bristle deflection state governed by the LuGre model (\S\ref{sec:lugre}) is added if the bristle stiffness $\sigma_0 > 0$. + +The Stribeck function $g(\omega)$, Eq.~\eqref{eq:stribeck}, determines velocity-dependent friction, and the friction force is given by Eq.~\eqref{eq:lugre_force}. The bristle state is integrated using the exact ZOH scheme~\eqref{eq:zoh}. The viscous term $\sigma_2 \omega$ is mapped directly to the standard \texttt{actuator\_damping} attribute to leverage MuJoCo's implicit integration, while maintaining the $\sigma_2$ \texttt{lugre} sub-attribute for convenience. +\paragraph{Integration.} +The bristle stiffness $\sigma_0$ is typically very large ($10^5$--$10^6$ N$\cdot$m/rad), creating a stiff ODE. At constant velocity, the state equation~\eqref{eq:lugre_state} has the form $\dot{z} = a z + b \omega$ where $a = -\sigma_0 |\omega| / g(\omega)$ and $b = 1$. Euler integration is unstable unless $|1 + a \Delta t| < 1$, requiring impractically small timesteps ($\Delta t < 2g(\omega)/(\sigma_0 |\omega|)$, on the order of microseconds). +Under a zero-order hold assumption ($\omega$ constant over the timestep), the linear ODE $\dot{z} = az + b\omega$ can be solved exactly: +\begin{equation} + z_{k+1} = e^{a \Delta t} z_k + + \frac{b(e^{a \Delta t} - 1)}{a} \, \omega + \label{eq:zoh} +\end{equation} +reducing to $z_{k+1} = z_k + b\omega\Delta t$ in the limit $a \to 0$. This integration is unconditionally stable for any $\Delta t$. + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Symbol & Units \\ +\midrule +\atSIG{} & $\sigma_0$ & N$\cdot$m/rad \\ +\atSIGD{} & $\sigma_1$ & N$\cdot$m$\cdot$s/rad \\ +\atSIGV{} & $\sigma_2$ & N$\cdot$m$\cdot$s/rad \\ +\atTAUC{} & $\tau_c$ & N$\cdot$m \\ +\atTAUS{} & $\tau_s$ & N$\cdot$m \\ +\atWS{} & $\omega_s$ & rad/s \\ +\bottomrule +\end{tabular} +\caption{LuGre friction attributes.} +\end{table} + +\noindent\textbf{Not modeled:} +Velocity-dependent bristle damping $\sigma_1(\omega)$ (\S\ref{sec:vel_damping}), a constant $\sigma_1$ is used. The Stribeck exponent is not exposed and fixed at $\gamma = 2$. + +\newpage +% --------------------------------------------------------------------------- +% PID Controller +% --------------------------------------------------------------------------- +\subsection{PID Controller} +\label{sec:controller} + +Many actuators embed an on-board controller computing drive voltage from position or velocity commands. To model such actuators, \texttt{dcmotor} supports an optional controller layer upstream of the motor physics. Two attributes control this behavior: + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Type & Description \\ +\midrule +\texttt{input} & keyword & \texttt{voltage}, \texttt{position}, \texttt{velocity} \\ +\texttt{controller} & vector & Gains (mode-dependent) \\ +\bottomrule +\end{tabular} +\caption{Controller attributes. Default \texttt{input} is \texttt{voltage}.} +\label{tab:controller_attributes} +\end{table} + +\noindent Unlike the motor parameters in Table~\ref{tab:datasheet}, controller gains are user-specified firmware settings. Gains are in {\em voltage-space} (e.g., $k_p$ in V/rad) since the output is a voltage $v$. To convert from physical torque-space (N$\cdot$m/rad), multiply by $R/K$. + +The controller computes a target voltage $v$ from the \texttt{ctrl} command. All motor physics --- cogging, saturation, friction, etc. --- apply identically downstream of $v$. The \texttt{input} attribute selects the controller: + +\begin{figure}[H] +\centering +\begin{tikzpicture}[ + block/.style={draw, rounded corners=2pt, minimum height=1.6em, + font=\scriptsize, fill=blue!5}, + mode/.style={font=\scriptsize, text=blue!70!black}, + arr/.style={->, thick, >=stealth}, + every node/.style={inner sep=2pt}, +] +% ctrl input +\node[font=\small] (ctrl) at (0, 3.5) {Input $u = {}$\texttt{ctrl}}; + +% Mode selector box +\node[block, minimum width=5.5cm, minimum height=6.5em, align=center] + (sel) at (0, 1.8) {}; +\node[font=\footnotesize\bfseries, anchor=north] at (0, 2.7) + {Controller mode}; +\node[mode] at (0, 1.45) {$\begin{aligned} + \texttt{voltage:}\quad v &= u \\[2pt] + \texttt{position:}\quad v &= k_p(u\!-\!\theta) + k_i x_I - k_d\dot\theta \\[2pt] + \texttt{velocity:}\quad v &= k_p(u\!-\!\dot\theta) + k_i(x_I\!-\!\theta) +\end{aligned}$}; + +% arrow ctrl to mode +\draw[arr] (ctrl.south) -- (sel.north); + +% Motor block +\node[block, font=\footnotesize\bfseries, minimum width=5.5cm, minimum height=2.2em, align=center] + (motor) at (0, -0.8) {DC Motor physics}; + +% single arrow with v label +\draw[arr] (sel.south) -- (motor.north) + node[midway, fill=white, font=\small, inner sep=2pt] {Voltage $v$}; + +% output +\node[font=\small] (tau) at (0, -1.8) {Torque $\tau$}; +\draw[arr] (motor.south) -- (tau.north); + +\end{tikzpicture} +\caption{Controller pipeline. The \texttt{input} attribute selects how $v$ is derived from \texttt{ctrl}; motor physics is identical downstream.} +\label{fig:controller_pipeline} +\end{figure} + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Position Mode} +\label{sec:position_mode} + +When \texttt{input="position"}, the user command $u = {}$\texttt{ctrl} is a target position, yielding voltage: +\begin{equation} + v = k_p \, (u - \theta) + k_i \, x_I - k_d \, \dot\theta + \label{eq:position_mode} +\end{equation} +where $\theta$ is the actuator length, $\dot\theta \equiv \omega$ is the actuator velocity, $x_I$ is the integral of position error, and $k_p$, $k_i$, $k_d$ are the proportional, integral, and derivative gains. + +\noindent The signs in~\eqref{eq:position_mode} follow MuJoCo convention: $k_p > 0$ drives toward the target, $k_d > 0$ provides damping (opposing velocity), and $k_i > 0$ reduces steady-state error. + +\paragraph{Integral state.} When $k_i > 0$, one additional activation state $x_I$ is allocated, governed by: +\begin{equation*} + \dot{x}_I = u - \theta + \label{eq:position_integral} +\end{equation*} +When $k_i = 0$, no integral state is added and the controller reduces to PD. + +\paragraph{Effective torque.} Substituting~\eqref{eq:position_mode} into the stateless torque equation~\eqref{eq:torque_speed}: +\begin{equation*} + \tau = \frac{K}{R} v - \frac{K^2}{R}\dot\theta + = \underbrace{\frac{K k_p}{R}}_{\text{stiffness}} (u - \theta) + + \frac{K k_i}{R} x_I + - \underbrace{\frac{K(K + k_d)}{R}}_{\text{damping}} \dot\theta + \label{eq:position_torque} +\end{equation*} +Note that the motor's back-EMF term $K^2\dot\theta/R$ contributes {\em additional damping} beyond the controller $k_d$ term. Even with $k_d\!=\!0$, the motor provides natural damping $K^2/R$. The computed $v$ is subject to voltage saturation (\S\ref{sec:voltage_saturation}). + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Symbol & Units \\ +\midrule +\atKP{} & $k_p$ & V/rad \\ +\atKI{} & $k_i$ & V/(rad$\cdot$s) \\ +\atKD{} & $k_d$ & V$\cdot$s/rad \\ +\bottomrule +\end{tabular} +\caption{Position mode controller gains.} +\label{tab:position_params} +\end{table} + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Velocity Mode} +\label{sec:velocity_mode} + +When \texttt{input="velocity"}, the user command $u = {}$\texttt{ctrl} is a target velocity, and $k_p$, $k_i$ are the proportional and integral gains. +\begin{equation} + v = k_p \, (u - \dot\theta) + k_i \, (x_I - \theta) + \label{eq:velocity_mode} +\end{equation} + +\paragraph{Integral state.} When $k_i > 0$, one additional activation state $x_I$ is allocated, governed by the integrator: +\begin{equation*} + \dot{x}_I = u + \label{eq:velocity_integral} +\end{equation*} +The term $k_i(x_I - \theta)$ then tracks a target position $x_I$ advancing at the commanded velocity $u$. This matches MuJoCo's \texttt{intvelocity} actuator behavior. + +When $k_i = 0$, no integral state is added and the controller provides pure velocity feedback. The computed $v$ is subject to voltage saturation (\S\ref{sec:voltage_saturation}). + +\paragraph{Effective torque.} Substituting~\eqref{eq:velocity_mode} into~\eqref{eq:torque_speed}: +\begin{equation*} + \tau = \underbrace{\frac{K k_i}{R}}_{\text{stiffness}} (x_I - \theta) + - \underbrace{\frac{K(K + k_p)}{R}}_{\text{damping}} \dot\theta + + \frac{K k_p}{R} u + \label{eq:velocity_torque} +\end{equation*} +Note the role swap compared to position mode: $k_i$ provides stiffness (position tracking to $x_I$) while $k_p$ adds damping alongside the motor's natural back-EMF damping $K^2/R$. + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Symbol & Units \\ +\midrule +\atKP{} & $k_p$ & V$\cdot$s/rad \\ +\atKI{} & $k_i$ & V/rad \\ +\bottomrule +\end{tabular} +\caption{Velocity mode controller gains.} +\label{tab:velocity_params} +\end{table} + +\pagebreak + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Setpoint Slew Rate} +\label{sec:setpoint_slew} + +The user command $u = {}$\texttt{ctrl} can change discontinuously between timesteps. When \atSLEW{} is set ($s > 0$), the effective setpoint is rate-limited: +\begin{equation*} + u \leftarrow \text{clip}(u, \; u_{\text{prev}} \pm s \cdot \Delta t) +\end{equation*} +where $u_{\text{prev}}$ is the previous effective setpoint and $\Delta t$ is the timestep. This smoothly ramps the reference trajectory instead of allowing instantaneous jumps. + +\paragraph{State variable.} When $s > 0$, one activation state $u_{\text{prev}}$ is allocated, and updated each step to $u$ (post-clamping). + +\paragraph{Units.} The slew rate $s$ has mode-dependent units: rad/s for position mode (limiting setpoint velocity), rad/s\textsuperscript{2} for velocity mode (limiting setpoint acceleration), and V/s for voltage mode. + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Anti-windup} +\label{sec:anti_windup} + +When $k_i > 0$, the integrator state $x_I$ provides steady-state error correction. However, sustained saturation or large setpoint changes can cause $x_I$ to grow excessively, leading to overshoot (integral windup). To prevent this, when \atIMAXINT{} is set ($I_{\max} > 0$), the state is bounded each step: +\begin{equation*} + x_I \leftarrow \text{clip}(x_I, \pm I_{\max}) +\end{equation*} +This prevents controller windup even when drive signals are saturated. + +% - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - +\subsubsection{Voltage Saturation} +\label{sec:voltage_saturation} + +In \texttt{position} and \texttt{velocity} modes, the computed voltage $v$ can be arbitrarily large (proportional to the error). Real motor drivers are limited by their supply voltage. When \atVMAX{} is set ($v_{\max} > 0$), a voltage clamp is applied before the motor equations: +\begin{equation*} + v \leftarrow \text{clip}(v, \pm v_{\max}) + \label{eq:vlimit} +\end{equation*} +This differs from \texttt{ctrlrange} (clamping user command $u$) and \texttt{forcerange} (clamping output torque). In position and velocity modes, \texttt{ctrlrange} limits the setpoint while \atVMAX{} limits the drive signal. In voltage mode ($v = u$), both clamp the voltage; if both are set, the tighter limit wins. + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lll@{}} +\toprule +Attribute & Symbol & Units \\ +\midrule +\atKP{} & $k_p$ & mode-dependent \\ +\atKI{} & $k_i$ & mode-dependent \\ +\atKD{} & $k_d$ & V$\cdot$s/rad \\ +\atSLEW{} & $s$ & ctrl-units/s \\ +\atIMAXINT{} & $I_{\max}$ & mode-dependent \\ +\atVMAX{} & $v_{\max}$ & Volt \\ +\bottomrule +\end{tabular} +\caption{Controller attributes.} +\label{tab:controller_attrs} +\end{table} + +% --------------------------------------------------------------------------- +% Low-Level Semantics +% --------------------------------------------------------------------------- +\subsection{Low-Level Semantics} +\label{sec:array_semantics} + +The \texttt{dcmotor} actuator uses the enum value types \texttt{mjGAIN\_DCMOTOR}, \texttt{mjDYN\_DCMOTOR}, \texttt{mjBIAS\_DCMOTOR}, and populates the rows of several \texttt{mjModel} arrays (all \texttt{actuator\_*}), as follows: + +\begin{table}[H] +\centering +\footnotesize +\begin{tabular}{@{}llll@{}} +\toprule +Array & Index & Symbol & Description \\ +\midrule +\texttt{gainprm} & 0 & $R$ & Resistance ($\Omega$) \\ +& 1 & $K$ & Motor constant (N$\cdot$m/A) \\ +& 2 & $\alpha$ & Resistance coeff.\ ($\text{K}^{-1}$) \\ +& 3 & $T_0$ & Reference temperature (\textdegree C) \\ +& 4 & $k_p$ & Controller proportional gain \\ +& 5 & $k_i$ & Controller integral gain \\ +& 6 & $k_d$ & Controller derivative gain \\ +& 7 & $v_{\max}$ & Voltage saturation (V) \\ +& 8 & --- & Input mode (0:\ $v$, 1:\ $\theta$, 2:\ $\dot\theta$) \\ +\midrule +\texttt{dynprm} & 0 & $t_e$ & Electrical time constant (s) \\ +& 1 & $(di{/}dt)_{\max}$ & Current rate limit (A/s) \\ +& 2 & $R_T$ & Thermal resistance ($\text{K}$/W) \\ +& 3 & $C$ & Thermal capacitance (J/$\text{K}$) \\ +& 4 & $T_a$ & Ambient temperature (\textdegree C) \\ +& 5 & $\sigma_0$ & LuGre bristle stiffness \\ +& 6 & $\sigma_1$ & LuGre bristle damping \\ +& 7 & $s$ & Controller slew rate \\ +& 8 & $I_{\max}$ & Integral limit (anti-windup) \\ +\midrule +\texttt{biasprm} & 0 & $A$ & Cogging amplitude (N$\cdot$m) \\ +& 1 & $N_p$ & Cogging periodicity \\ +& 2 & $\phi$ & Cogging phase (rad) \\ +& 3 & $\tau_c$ & LuGre Coulomb fric. (N$\cdot$m) \\ +& 4 & $\tau_s$ & LuGre static fric. (N$\cdot$m) \\ +& 5 & $\omega_s$ & Stribeck velocity (rad/s) \\ +\midrule +\texttt{forcerange} & 0 & $-\tau_{\max}$ & Minimum torque (N$\cdot$m) \\ +& 1 & $\tau_{\max}$ & Maximum torque (N$\cdot$m) \\ +\midrule +\texttt{damping} & 0 & $B_1 (+\sigma_2)$ & Linear (+ LuGre viscous) \\ +& 1 & $B_2$ & Quadratic \\ +& 2 & $B_3$ & Cubic \\ +\midrule +\texttt{armature} & 0 & $J_r$ & Actuator armature \\ +\midrule +\texttt{gear} & 0 & $N$ & Gear ratio \\ +\bottomrule +\end{tabular} +\caption{\texttt{mjModel} array semantics for the \texttt{dcmotor} actuator.} +\label{tab:array_semantics} +\end{table} + +\paragraph{Runtime mutability.} +Most \texttt{mjModel} parameters listed above may be freely modified at runtime for system identification or gain tuning. However, five parameters control the \emph{number} of activation states, which is determined at compile time and cannot change during simulation. Toggling any of the following parameters between zero and positive after compilation is an error: +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}llcl@{}} +\toprule +Parameter & Storage & State & Semantics \\ +\midrule +$s$ & \texttt{dynprm[7]} & $u_{\text{prev}}$ & previous control \\ +$k_i$ & \texttt{gainprm[5]} & $x_I$ & controller integral \\ +$R_T, C$ & \texttt{dynprm[2,3]} & $T$ & temperature rise \\ +$\sigma_0$ & \texttt{dynprm[5]} & $z$ & bristle deflection \\ +$t_e$ & \texttt{dynprm[0]} & $i$ & winding current \\ +\bottomrule +\end{tabular} +\caption{Compile-time state switches, allocated in \texttt{act} in the order shown. Do not toggle between zero and positive at runtime.} +\end{table} + + +\onecolumn +%============================================================================= +% DATASHEET MAPPING +%============================================================================= +\section{Datasheet Mapping} +\label{sec:datasheet} + +Table~\ref{tab:datasheet} maps commercial motor datasheet specifications to attributes. The left column shows the datasheet entry as typically labeled by motor manufacturers; the right column shows the corresponding \texttt{dcmotor} MJCF attribute, when one exists. Derived quantities that are not direct attributes (gradient, mechanical time constant) are included for completeness. + +\begin{table}[H] +\centering +\small +\begin{tabular}{@{}lllll@{}} +\toprule +Specification & Symbol & Formula / Note & Datasheet Symbol & Attribute \\ +\midrule +Resistance & $R$ & Terminal resistance & R, Ra & \atR{} \\ +Torque Constant & $K_t$ & $\tau = K_t i$ & kt, km & \atKt{} \\ +Back-EMF Constant & $K_e$ & $v_{\text{back}} = K_e \omega$ & ke & \atKe{} \\ +Speed Constant & $K_v$ & $K_v = 1/K_e$ & kn, kv & $1/$\atKe{} \\ +Nominal Voltage & $v_n$ & Rated voltage (e.g., 24V) & Un, VDC & \atVM{} \\ +No-load Speed & $\omega_0$ & $\omega_0 \approx v_n / K$ & n0 & \atNLS{} \\ +Stall Torque & $\tau_0$ & $\tau_0 = K v_n / R$ & MH, Ts & \atSTALL{} \\ +Stall Current & $i_s$ & Max.\ possible: $i_s = v_n / R$ & IA & \\ +Nominal Current & $i_{\max}$ & Thermal limit (continuous) & Ic, IN & \atIMAX{} \\ +Peak Current & $i_{\text{peak}}$ & Short-term limit & Ipk & \\ +\midrule +Coulomb Friction & $\tau_c$ & Dry friction opposing motion & $T_f$ & \texttt{frictionloss} (joint)\\ +Viscous Friction & $B$ & Drag $\propto \omega$ & $C_v$ & \texttt{damping} \\ +Rotor Inertia & $J_r$ & Reflected: $J_{\text{eff}} = J_r N^2$ & J, Jm & \texttt{armature} \\ +Gear Ratio & $N$ & Reduction ratio & $N$, $i$ & \texttt{gear} \\ +Gearbox Efficiency & $\eta$ & Fold into $K$: use $\eta K$ & $\eta$ & \\ +Cogging Amplitude & $A$ & Peak cogging torque & --- & \atCOGA{} \\ +Cogging Periodicity & $N_p$ & Poles $\times$ slots/pole & --- & \atCOGP{} \\ +\midrule +Nominal Torque & $\tau_n$ & $\tau_n = K \cdot i_{\max}$ & $M_N$, $T_c$ & \\ +No-load Current & $i_0$ & Friction: Eq.~\eqref{eq:noload} & $I_0$ & \\ +Gradient & $\partial\omega/\partial\tau$ & $-R / K^2$ & $\Delta n / \Delta M$ & \\ +Mech.\ Time Const. & $t_m$ & $t_m = R\,J / K^2$ & $\tau_m$ & \\ +\midrule +Inductance & $L$ & Terminal inductance & L & \atL{} \\ +Elec.\ Time Const. & $t_e$ & $t_e = L / R$ & $\tau_e$ & \atTE{} \\ +\midrule +Thermal Resistance & $R_T$ & Winding-to-ambient & Rth & \atRT{} \\ +Thermal Capacitance & $C$ & $C = t_T / R_T$ & $C_{\text{th}}$ & \atTC{} \\ +Thermal Time Const. & $t_T$ & $t_T = R_T C$ & $\tau_{\text{th}}$ & \atTT{} \\ +Ref.\ Temperature & $T_0$ & Temperature at which $R$ is specified & $T_{\text{ref}}$ & \atTREF{} \\ +Ambient Temperature & $T_a$ & Operating environment & --- & \atTAMB{} \\ +Max.\ Winding Temp. & $T_{\max}$ & Absolute limit & $T_{\max}$ & \\ +Res.\ Temp.\ Coeff. & $\alpha$ & $\approx 0.0039\, \text{K}^{-1}$ (copper) & $\alpha_{\text{Cu}}$ & \atALPHA{} \\ +\bottomrule +\end{tabular} +\caption{Datasheet parameters and their relation to model constants. Groups: electrical, mechanical, derived, inductance, thermal.} +\label{tab:datasheet} +\end{table} + +\small +\bibliographystyle{ieeetr} +\bibliography{refs} + +\end{document} diff --git a/doc/dcmotor/refs.bib b/doc/dcmotor/refs.bib new file mode 100644 index 00000000..29708bc7 --- /dev/null +++ b/doc/dcmotor/refs.bib @@ -0,0 +1,82 @@ +@article{dewit95, + author = {Canudas de Wit, C. and Olsson, H. and {\AA}str{\"o}m, K. J. and Lischinsky, P.}, + title = {{A New Model for Control of Systems with Friction}}, + journal = {IEEE Transactions on Automatic Control}, + volume = {40}, + number = {3}, + pages = {419--425}, + year = {1995}, + month = mar, +} + +@article{lugre_revisited, + author = {{\AA}str{\"o}m, K. J. and Canudas de Wit, C.}, + title = {{Revisiting the LuGre Friction Model}}, + journal = {IEEE Control Systems Magazine}, + volume = {28}, + number = {6}, + pages = {101--114}, + year = {2008}, + month = dec, +} + +@techreport{dahl68, + author = {Dahl, P.}, + title = {{A Solid Friction Model}}, + institution = {The Aerospace Corporation}, + address = {El Segundo, CA}, + number = {TOR-0158(3107-18)-1}, + year = {1968}, +} + +@book{hughes2019, + author = {Hughes, Austin and Drury, Bill}, + title = {{Electric Motors and Drives: Fundamentals, Types and Applications}}, + edition = {5th}, + publisher = {Newnes}, + year = {2019}, +} + +@book{tedrake2024, + author = {Tedrake, Russ}, + title = {{Underactuated Robotics: Algorithms for Walking, Running, + Swimming, Flying, and Manipulation}}, + publisher = {MIT}, + year = {2024}, + note = {Course notes for MIT 6.832, \url{https://underactuated.mit.edu}}, +} + +@article{isaaclab2025, + author = {Mittal, Mayank and Yu, Calvin and Yu, Qinxi and Liu, Jingzhou + and Rudin, Nikita and Hoeller, David and Yuan, Jia Lin + and Singh, Ritvik and Guo, Yunrong and Mazhar, Hammad + and Mandlekar, Ajay and Babich, Buck and State, Gavriel + and Hutter, Marco and Garg, Animesh}, + title = {{Isaac Lab: A Unified and Modular Framework for Robot Learning}}, + journal = {arXiv preprint arXiv:2502.11048}, + year = {2025}, +} + +@article{stribeck1902, + author = {Stribeck, R.}, + title = {{Die wesentlichen Eigenschaften der Gleit- und Rollenlager}}, + journal = {Zeitschrift des Vereines Deutscher Ingenieure}, + volume = {46}, + pages = {1341--1348, 1432--1438, 1463--1470}, + year = {1902}, +} + +@misc{maxon_formulas, + author = {{Maxon Motor AG}}, + title = {{Key Information on Maxon DC Motors and Maxon EC Motors}}, + howpublished = {\url{https://www.maxongroup.com}}, + year = {2024}, + note = {{Maxon} Academy Technical Notes}, +} + +@misc{simscape_dcmotor, + author = {{MathWorks}}, + title = {{DC Motor --- Simscape Electrical Block Reference}}, + howpublished = {\url{https://www.mathworks.com/help/sps/ref/dcmotor.html}}, + year = {2024}, +} diff --git a/doc/includes/references.h b/doc/includes/references.h index bc8a548d..fd855a1c 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -635,19 +635,22 @@ typedef enum mjtDyn_ { // type of actuator dynamics mjDYN_INTEGRATOR, // integrator: da/dt = u mjDYN_FILTER, // linear filter: da/dt = (u-a) / tau mjDYN_FILTEREXACT, // linear filter: da/dt = (u-a) / tau, with exact integration - mjDYN_MUSCLE, // piece-wise linear filter with two time constants + mjDYN_MUSCLE, // piecewise linear filter with two time constants + mjDYN_DCMOTOR, // DC motor electrical dynamics mjDYN_USER // user-defined dynamics type } mjtDyn; typedef enum mjtGain_ { // type of actuator gain mjGAIN_FIXED = 0, // fixed gain mjGAIN_AFFINE, // const + kp*length + kv*velocity mjGAIN_MUSCLE, // muscle FLV curve computed by mju_muscleGain() + mjGAIN_DCMOTOR, // DC motor gain: K or K/R mjGAIN_USER // user-defined gain type } mjtGain; typedef enum mjtBias_ { // type of actuator bias mjBIAS_NONE = 0, // no bias mjBIAS_AFFINE, // const + kp*length + kv*velocity mjBIAS_MUSCLE, // muscle passive force computed by mju_muscleBias() + mjBIAS_DCMOTOR, // DC motor bias: back-EMF, cogging, LuGre friction mjBIAS_USER // user-defined bias type } mjtBias; typedef enum mjtObj_ { // type of MujoCo object @@ -3659,6 +3662,10 @@ const char* mjs_setToMuscle(mjsActuator* actuator, double timeconst[2], double t double range[2], double force, double scale, double lmin, double lmax, double vmax, double fpmax, double fvmax); const char* mjs_setToAdhesion(mjsActuator* actuator, double gain); +const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance, + double nominal[3], double saturation[4], double inductance[2], + double cogging[3], double controller[5], double thermal[6], + double lugre[6], int input_mode); mjsMesh* mjs_addMesh(mjSpec* s, const mjsDefault* def); mjsHField* mjs_addHField(mjSpec* s); mjsSkin* mjs_addSkin(mjSpec* s); diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 2f9cdd35..49cfee0b 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -244,7 +244,8 @@ typedef enum mjtDyn_ { // type of actuator dynamics mjDYN_INTEGRATOR, // integrator: da/dt = u mjDYN_FILTER, // linear filter: da/dt = (u-a) / tau mjDYN_FILTEREXACT, // linear filter: da/dt = (u-a) / tau, with exact integration - mjDYN_MUSCLE, // piece-wise linear filter with two time constants + mjDYN_MUSCLE, // piecewise linear filter with two time constants + mjDYN_DCMOTOR, // DC motor electrical dynamics mjDYN_USER // user-defined dynamics type } mjtDyn; @@ -253,6 +254,7 @@ typedef enum mjtGain_ { // type of actuator gain mjGAIN_FIXED = 0, // fixed gain mjGAIN_AFFINE, // const + kp*length + kv*velocity mjGAIN_MUSCLE, // muscle FLV curve computed by mju_muscleGain() + mjGAIN_DCMOTOR, // DC motor gain: K or K/R mjGAIN_USER // user-defined gain type } mjtGain; @@ -261,6 +263,7 @@ typedef enum mjtBias_ { // type of actuator bias mjBIAS_NONE = 0, // no bias mjBIAS_AFFINE, // const + kp*length + kv*velocity mjBIAS_MUSCLE, // muscle passive force computed by mju_muscleBias() + mjBIAS_DCMOTOR, // DC motor bias: back-EMF, cogging, LuGre friction mjBIAS_USER // user-defined bias type } mjtBias; diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 362f16e9..ba496360 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1726,6 +1726,12 @@ MJAPI const char* mjs_setToMuscle(mjsActuator* actuator, double timeconst[2], do // Set actuator to active adhesion; return error if any. MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain); +// Set actuator to DC motor; return error if any. +MJAPI const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance, + double nominal[3], double saturation[4], double inductance[2], + double cogging[3], double controller[5], double thermal[6], + double lugre[6], int input_mode); + //---------------------------------- Assets -------------------------------------------------------- diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index d4dcdf34..bc28f78d 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -263,7 +263,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjDYN_FILTER', 2), ('mjDYN_FILTEREXACT', 3), ('mjDYN_MUSCLE', 4), - ('mjDYN_USER', 5), + ('mjDYN_DCMOTOR', 5), + ('mjDYN_USER', 6), ]), )), ('mjtGain', @@ -274,7 +275,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjGAIN_FIXED', 0), ('mjGAIN_AFFINE', 1), ('mjGAIN_MUSCLE', 2), - ('mjGAIN_USER', 3), + ('mjGAIN_DCMOTOR', 3), + ('mjGAIN_USER', 4), ]), )), ('mjtBias', @@ -285,7 +287,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjBIAS_NONE', 0), ('mjBIAS_AFFINE', 1), ('mjBIAS_MUSCLE', 2), - ('mjBIAS_USER', 3), + ('mjBIAS_DCMOTOR', 3), + ('mjBIAS_USER', 4), ]), )), ('mjtObj', diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index ff1bcf12..0ab6df20 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -10786,6 +10786,86 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Set actuator to active adhesion; return error if any.', )), + ('mjs_setToDCMotor', + FunctionDecl( + name='mjs_setToDCMotor', + return_type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + parameters=( + FunctionParameterDecl( + name='actuator', + type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + ), + FunctionParameterDecl( + name='motorconst', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(2,), + ), + ), + FunctionParameterDecl( + name='resistance', + type=ValueType(name='double'), + ), + FunctionParameterDecl( + name='nominal', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(3,), + ), + ), + FunctionParameterDecl( + name='saturation', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(4,), + ), + ), + FunctionParameterDecl( + name='inductance', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(2,), + ), + ), + FunctionParameterDecl( + name='cogging', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(3,), + ), + ), + FunctionParameterDecl( + name='controller', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(5,), + ), + ), + FunctionParameterDecl( + name='thermal', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(6,), + ), + ), + FunctionParameterDecl( + name='lugre', + type=ArrayType( + inner_type=ValueType(name='double'), + extents=(6,), + ), + ), + FunctionParameterDecl( + name='input_mode', + type=ValueType(name='int'), + ), + ), + doc='Set actuator to DC motor; return error if any.', + )), ('mjs_addMesh', FunctionDecl( name='mjs_addMesh', diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index e3bfc7ef..28af8af1 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -1303,6 +1303,31 @@ PYBIND11_MODULE(_specs, m) { } }, py::arg("gain")); + mjsActuator.def( + "set_to_dcmotor", + [](raw::MjsActuator* self, std::array motorconst, + double resistance, + std::array nominal, std::array saturation, + std::array inductance, std::array cogging, + std::array controller, std::array thermal, + std::array lugre, int input_mode) { + std::string err = mjs_setToDCMotor( + self, motorconst.data(), resistance, nominal.data(), + saturation.data(), inductance.data(), cogging.data(), + controller.data(), thermal.data(), lugre.data(), input_mode); + if (!err.empty()) { + throw pybind11::value_error(err); + } + }, + py::arg("motorconst"), py::arg("resistance"), + py::arg("nominal") = std::array{0, 0, 0}, + py::arg("saturation") = std::array{0, 0, 0, 0}, + py::arg("inductance") = std::array{0, 0}, + py::arg("cogging") = std::array{0, 0, 0}, + py::arg("controller") = std::array{0, 0, 0, 0, 0}, + py::arg("thermal") = std::array{0, 0, 0, 0, 0, 0}, + py::arg("lugre") = std::array{0, 0, 0, 0, 0, 0}, + py::arg("input_mode") = 0); // ============================= MJSTENDONPATH =============================== // helper struct for tendon path indexing diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index f1c99d82..a82646dd 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -1557,6 +1557,13 @@ class SpecsTest(absltest.TestCase): self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_FIXED) self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_NONE) + actuator.set_to_dcmotor(motorconst=[0.05, 0.05], resistance=2.0) + self.assertEqual(actuator.gainprm[0], 2.0) + self.assertEqual(actuator.gainprm[1], 0.05) + self.assertEqual(actuator.dyntype, mujoco.mjtDyn.mjDYN_DCMOTOR) + self.assertEqual(actuator.gaintype, mujoco.mjtGain.mjGAIN_DCMOTOR) + self.assertEqual(actuator.biastype, mujoco.mjtBias.mjBIAS_DCMOTOR) + def test_bad_contact_sensor(self): test_cases = [ dict( diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 3f9b467a..00267a3e 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -1107,6 +1107,17 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { bias_vel = (m->actuator_biasprm + mjNBIAS*i)[2]; } + // DC motor bias (back-EMF) + else if (m->actuator_biastype[i] == mjBIAS_DCMOTOR) { + const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i; + const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i; + if (dynprm[0] <= 0) { + mjtNum R = mju_max(mjMINVAL, gainprm[0]); + mjtNum K = gainprm[1]; + bias_vel -= K * K / R; + } + } + // affine gain if (m->actuator_gaintype[i] == mjGAIN_AFFINE) { // extract bias info: prm = [const, kp, kv] @@ -1122,6 +1133,28 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { m->actuator_gainprm + mjNGAIN*i); } + // DC motor controller damping and LuGre micro-damping + else if (m->actuator_gaintype[i] == mjGAIN_DCMOTOR) { + const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i; + const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i; + int input_mode = (int)gainprm[8]; + if (input_mode > 0) { + mjtNum R = gainprm[0]; + mjtNum K = gainprm[1]; + mjtNum gain = (dynprm[0] > 0) ? K : K / mju_max(mjMINVAL, R); + mjtNum kp = gainprm[4]; + mjtNum kd = gainprm[6]; + bias_vel -= gain * (input_mode == 1 ? kd : kp); + } + + // LuGre: force includes -sigma1*z_dot, z_dot = a*z + v + // d(sigma1*z_dot)/dv = sigma1*(da/dv*z + 1), ignoring higher-order da/dv*z + mjtNum sigma1 = dynprm[6]; + if (sigma1 > 0) { + bias_vel -= sigma1; + } + } + // force = gain .* [ctrl/act] if (gain_vel != 0) { if (m->actuator_dyntype[i] == mjDYN_NONE) { diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index b28c342f..e8e08f00 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -257,6 +257,36 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) { } +// helper for DC motor: computes control voltage from PID state +static mjtNum dcmotorVoltage(mjtNum ctrl, mjtNum length, mjtNum velocity, + mjtNum x_I, const mjtNum* gainprm) { + int input_mode = (int)gainprm[8]; + mjtNum Vmax = gainprm[7]; + mjtNum voltage; + + // get voltage + if (input_mode > 0) { + mjtNum kp = gainprm[4]; // proportional gain + mjtNum ki = gainprm[5]; // integral gain + mjtNum kd = gainprm[6]; // derivative gain + + if (input_mode == 1) { + // position mode + voltage = kp * (ctrl - length) + ki * x_I - kd * velocity; + } else { + // velocity mode + voltage = kp * (ctrl - velocity) + ki * (x_I - length); + } + } else { + voltage = ctrl; + } + + // clip voltage + if (Vmax > 0) voltage = mju_clip(voltage, -Vmax, Vmax); + + return voltage; +} + // clamp vector to range static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, int n, @@ -275,7 +305,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { TM_START; int nv = m->nv, nu = m->nu, ntendon = m->ntendon; mjtNum gain, bias, tau; - mjtNum *prm, *force = d->actuator_force; + mjtNum *force = d->actuator_force; // clear actuator_force mju_zero(force, nu); @@ -327,37 +357,136 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // zero act_dot for actuator plugins - if (m->actuator_actnum[i]) { - mju_zero(d->act_dot + act_first, m->actuator_actnum[i]); + int actnum = m->actuator_actnum[i]; + if (actnum) { + mju_zero(d->act_dot + act_first, actnum); } // extract info - prm = m->actuator_dynprm + i*mjNDYN; + const mjtNum* dynprm = m->actuator_dynprm + i*mjNDYN; + mjtDyn dyntype = m->actuator_dyntype[i]; // index into the last element in act. For most actuators it's also the - // first element, but actuator plugins might store their own state in act. - int act_last = act_first + m->actuator_actnum[i] - 1; + // first element, but actuator plugins might store their own state in act + int act_last = act_first + actnum - 1; // compute act_dot according to dynamics type - switch ((mjtDyn) m->actuator_dyntype[i]) { + switch (dyntype) { case mjDYN_INTEGRATOR: // simple integrator d->act_dot[act_last] = ctrl[i]; break; - case mjDYN_FILTER: // linear filter: prm = tau + case mjDYN_FILTER: // linear filter: dynprm = tau case mjDYN_FILTEREXACT: - tau = mju_max(mjMINVAL, prm[0]); + tau = mju_max(mjMINVAL, dynprm[0]); d->act_dot[act_last] = (ctrl[i] - d->act[act_last]) / tau; break; - case mjDYN_MUSCLE: // muscle model: prm = (tau_act, tau_deact) - d->act_dot[act_last] = mju_muscleDynamics( - ctrl[i], d->act[act_last], prm); + case mjDYN_MUSCLE: // muscle model: dynprm = (tau_act, tau_deact) + d->act_dot[act_last] = mju_muscleDynamics(ctrl[i], d->act[act_last], dynprm); break; + case mjDYN_DCMOTOR: { // DC motor: up to 5 optional states + const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i; + + // verify allocated state size matches parameters; SHOULD NOT OCCUR + if (mj_dcmotorSlots(dynprm, gainprm).num_slots != actnum) { + mjERROR("inconsistent state array dimension in DC motor (actuator %d)", i); + } + + int adr = act_first; + mjtNum velocity = d->actuator_velocity[i]; + mjtNum R = gainprm[0]; // resistance + mjtNum K = gainprm[1]; // motor constant + mjtNum ki = gainprm[5]; // integral gain + mjtNum te = dynprm[0]; // electrical time constant + + // slot order: slew, integral, temperature, bristle, current + + // controller state: slew rate limiting + mjtNum slew_s = dynprm[7]; // slew rate limit + if (slew_s > 0) { + mjtNum u_prev = d->act[adr]; + mjtNum slew = slew_s * m->opt.timestep; + mjtNum u_eff = mju_clip(ctrl[i], u_prev - slew, u_prev + slew); + d->act_dot[adr] = (u_eff - u_prev) / m->opt.timestep; + ctrl[i] = u_eff; + adr++; + } + + // controller state: integral state + mjtNum x_I = 0; + if (ki > 0) { + x_I = d->act[adr]; + int input_mode = (int)gainprm[8]; + mjtNum Imax = dynprm[8]; // integral clamp + mjtNum act_dot = ctrl[i]; // default raw accumulator for voltage and velocity modes + + // position mode + if (input_mode == 1) { + act_dot = ctrl[i] - d->actuator_length[i]; + } + + // clamp act_dot based on integral state + if (Imax > 0) { + if (x_I >= Imax) { + act_dot = mju_min(act_dot, 0); + } else if (x_I <= -Imax) { + act_dot = mju_max(act_dot, 0); + } + } + d->act_dot[adr] = act_dot; + adr++; + } + + // compute physical voltage to feed into current and temperature equations + mjtNum V = dcmotorVoltage(ctrl[i], d->actuator_length[i], velocity, x_I, gainprm); + + // temperature: dT/dt = (R*i^2 - T/RT) / C, where T = delta above ambient + mjtNum RT = dynprm[2]; // thermal resistance + if (RT > 0) { + mjtNum C = dynprm[3]; // thermal capacitance + mjtNum Ta = dynprm[4]; // ambient temperature + mjtNum alpha = gainprm[2]; // temperature coefficient + mjtNum T0 = gainprm[3]; // reference temperature + mjtNum T = d->act[adr]; // temperature rise above ambient + R *= 1 + alpha * (T + Ta - T0); + + // get current: from act_last if stateful, from (V - K*omega)/R if stateless + mjtNum current = (te > 0) ? d->act[act_last] : (V - K * velocity) / R; + d->act_dot[adr] = (R*current*current - T / RT) / C; + adr++; + } + + // LuGre bristle state: dz/dt = v - sigma0 * |v| / g(v) * z + mjtNum sigma0 = dynprm[5]; // bristle stiffness + if (sigma0 > 0) { + const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*i; + mjtNum F_C = biasprm[3]; // Coulomb friction + mjtNum F_S = biasprm[4]; // static friction + mjtNum v_S = biasprm[5]; // Stribeck velocity + mjtNum z = d->act[adr]; // bristle state + mjtNum g = mj_lugreStribeck(velocity, F_C, F_S, v_S); + mjtNum a = -sigma0 * mju_abs(velocity) / mju_max(mjMINVAL, g); + d->act_dot[adr] = a * z + velocity; + adr++; + } + + // current state: di/dt = (V/R - K/R*omega - i) / te + if (te > 0) { + mjtNum dimax = dynprm[1]; // current rate limit (di/dt)_max + mjtNum i_dot = (V/R - K/R*velocity - d->act[act_last]) / te; + if (dimax > 0) { + i_dot = mju_clip(i_dot, -dimax, dimax); + } + d->act_dot[act_last] = i_dot; + } + break; + } + default: // user dynamics if (mjcb_act_dyn) { - if (m->actuator_actnum[i] == 1) { + if (actnum == 1) { // scalar activation dynamics, get act_dot d->act_dot[act_last] = mjcb_act_dyn(m, d, i); } else { @@ -407,17 +536,20 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { tendon_frclimited = m->tendon_actfrclimited[m->actuator_trnid[2*i]]; } - // extract gain info - prm = m->actuator_gainprm + mjNGAIN*i; + // extract info + const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i; + const mjtNum* gainprm = m->actuator_gainprm + mjNGAIN*i; + mjtGain gaintype = m->actuator_gaintype[i]; + int actnum = m->actuator_actnum[i]; // handle according to gain type - switch ((mjtGain) m->actuator_gaintype[i]) { + switch (gaintype) { case mjGAIN_FIXED: // fixed gain: prm = gain - gain = prm[0]; + gain = gainprm[0]; break; case mjGAIN_AFFINE: // affine: prm = [const, kp, kv] - gain = prm[0] + prm[1]*d->actuator_length[i] + prm[2]*d->actuator_velocity[i]; + gain = gainprm[0] + gainprm[1]*d->actuator_length[i] + gainprm[2]*d->actuator_velocity[i]; break; case mjGAIN_MUSCLE: // muscle gain @@ -425,9 +557,43 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { d->actuator_velocity[i], m->actuator_lengthrange+2*i, m->actuator_acc0[i], - prm); + gainprm); break; + case mjGAIN_DCMOTOR: { // DC motor: gain = K or K/R + mjtNum R = gainprm[0]; // resistance + mjtNum K = gainprm[1]; // motor constant + mjDCMotorSlots slots = mj_dcmotorSlots(dynprm, gainprm); + + // verify allocated state size matches parameters; SHOULD NOT OCCUR + if (slots.num_slots != actnum) { + mjERROR("inconsistent state array dimension in DC motor (actuator %d)", i); + } + + int adr = m->actuator_actadr[i]; + + // adjust R for temperature if enabled + if (slots.temperature >= 0) { + mjtNum T = d->act[adr + slots.temperature]; + mjtNum alpha = gainprm[2]; // temperature coefficient + mjtNum T0 = gainprm[3]; // reference temperature + mjtNum Ta = dynprm[4]; // ambient temperature + R *= 1 + alpha * (T + Ta - T0); + } + + // stateful current: gain = K, force = K * act[last] (generic path) + // stateless: gain = K/R, force = K/R * ctrl (condition below) + gain = (dynprm[0] > 0) ? K : K / mju_max(mjMINVAL, R); + + // controller: compute voltage, override ctrl[i] for force computation + if ((int)gainprm[8] > 0) { + mjtNum x_I = (slots.integral >= 0) ? d->act[adr + slots.integral] : 0; + ctrl[i] = dcmotorVoltage(ctrl[i], d->actuator_length[i], + d->actuator_velocity[i], x_I, gainprm); + } + break; + } + default: // user gain if (mjcb_act_gain) { gain = mjcb_act_gain(m, d, i); @@ -437,11 +603,14 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // set force = gain .* [ctrl/act] - if (m->actuator_actadr[i] == -1) { + + // DC motor without current state: use ctrl even if other activations exist + int dcmotor_no_current = (gaintype == mjGAIN_DCMOTOR && dynprm[0] <= 0); + if (actnum == 0 || dcmotor_no_current) { force[i] = gain * ctrl[i]; } else { // use last activation variable associated with actuator i - int act_adr = m->actuator_actadr[i] + m->actuator_actnum[i] - 1; + int act_adr = m->actuator_actadr[i] + actnum - 1; mjtNum act; if (m->actuator_actearly[i]) { @@ -453,25 +622,38 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // extract bias info - prm = m->actuator_biasprm + mjNBIAS*i; + const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*i; + mjtBias biastype = m->actuator_biastype[i]; // handle according to bias type - switch ((mjtBias) m->actuator_biastype[i]) { + switch (biastype) { case mjBIAS_NONE: // none bias = 0.0; break; - case mjBIAS_AFFINE: // affine: prm = [const, kp, kv] - bias = prm[0] + prm[1]*d->actuator_length[i] + prm[2]*d->actuator_velocity[i]; + case mjBIAS_AFFINE: // affine: biasprm = [const, kp, kv] + bias = biasprm[0] + biasprm[1]*d->actuator_length[i] + biasprm[2]*d->actuator_velocity[i]; break; case mjBIAS_MUSCLE: // muscle passive force bias = mju_muscleBias(d->actuator_length[i], m->actuator_lengthrange+2*i, m->actuator_acc0[i], - prm); + biasprm); break; + case mjBIAS_DCMOTOR: { // DC motor: back-EMF only (current-limited) + bias = 0; + + // back-EMF (stateless only; for stateful current it's in the ODE) + mjtNum te = m->actuator_dynprm[mjNDYN*i]; // electrical time constant + if (te <= 0) { + mjtNum K = gainprm[1]; // motor constant + bias -= gain * K * d->actuator_velocity[i]; + } + break; + } + default: // user bias if (mjcb_act_bias) { bias = mjcb_act_bias(m, d, i); @@ -537,6 +719,41 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // clamp actuator_force clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL); + // add DC motor mechanical forces (not subject to current limits) + for (int i=0; i < nu; i++) { + if (m->actuator_biastype[i] != mjBIAS_DCMOTOR) { + continue; + } + if (sleep_filter && mj_sleepState(m, d, mjOBJ_ACTUATOR, i) == mjS_ASLEEP) { + continue; + } + if (mj_actuatorDisabled(m, i) || m->actuator_plugin[i] >= 0) { + continue; + } + + const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*i; + const mjtNum* dynprm = m->actuator_dynprm + mjNDYN*i; + + // cogging torque + mjtNum A = biasprm[0]; + if (A != 0) { + mjtNum Np = biasprm[1]; + mjtNum phi = biasprm[2]; + force[i] += A * mju_sin(Np*d->actuator_length[i] + phi); + } + + // LuGre friction + mjtNum sigma0 = dynprm[5]; + if (sigma0 > 0) { + mjtNum sigma1 = dynprm[6]; + mjDCMotorSlots slots = mj_dcmotorSlots(dynprm, m->actuator_gainprm + mjNGAIN*i); + int adr = m->actuator_actadr[i] + slots.bristle; + mjtNum z = d->act[adr]; + mjtNum z_dot = d->act_dot[adr]; + force[i] -= sigma0 * z + sigma1 * z_dot; + } + } + // qfrc_actuator = moment' * force mju_mulMatTVecSparse(d->qfrc_actuator, d->actuator_moment, force, nu, nv, d->moment_rownnz, d->moment_rowadr, d->moment_colind); diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index a4827ddb..81c3b6ca 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -709,22 +709,69 @@ int mj_actuatorDisabled(const mjModel* m, int i) { mjtNum mj_nextActivation(const mjModel* m, const mjData* d, int actuator_id, int act_adr, mjtNum act_dot) { mjtNum act = d->act[act_adr]; + int dyntype = m->actuator_dyntype[actuator_id]; - if (m->actuator_dyntype[actuator_id] == mjDYN_FILTEREXACT) { + if (dyntype == mjDYN_FILTEREXACT) { // exact filter integration // act_dot(0) = (ctrl-act(0)) / tau // act(h) = act(0) + (ctrl-act(0)) (1 - exp(-h / tau)) // = act(0) + act_dot(0) * tau * (1 - exp(-h / tau)) mjtNum tau = mju_max(mjMINVAL, m->actuator_dynprm[actuator_id*mjNDYN]); act = act + act_dot * tau * (1 - mju_exp(-m->opt.timestep / tau)); - } else { - // Euler integration + } else if (dyntype == mjDYN_DCMOTOR) { + const mjtNum* dynprm = m->actuator_dynprm + actuator_id * mjNDYN; + const mjtNum* gainprm = m->actuator_gainprm + actuator_id * mjNGAIN; + mjDCMotorSlots slots = mj_dcmotorSlots(dynprm, gainprm); + + int offset = act_adr - m->actuator_actadr[actuator_id]; + + // current filter: exact integration + if (offset == slots.current) { + mjtNum te = mju_max(mjMINVAL, dynprm[0]); + act = act + act_dot * te * (1 - mju_exp(-m->opt.timestep / te)); + } + + // LuGre bristle: dz/dt = a*z + v where a = -sigma0*|v|/g(v) + else if (offset == slots.bristle) { + const mjtNum* biasprm = m->actuator_biasprm + mjNBIAS*actuator_id; + mjtNum F_C = biasprm[3]; // Coulomb friction + mjtNum F_S = biasprm[4]; // static friction + mjtNum v_S = biasprm[5]; // Stribeck velocity + mjtNum sigma0 = dynprm[5]; // bristle stiffness + mjtNum velocity = d->actuator_velocity[actuator_id]; + mjtNum g = mj_lugreStribeck(velocity, F_C, F_S, v_S); + + // ZOH exact ZOH integration: z(h) = exp(ah)*z(0) + ((exp(ah)-1)/a)*v + mjtNum a = -sigma0 * mju_abs(velocity) / mju_max(mjMINVAL, g); // decay rate + mjtNum h = m->opt.timestep; + mjtNum exp_ah = mju_exp(a * h); // state transition + mjtNum int_h = mju_abs(a) > mjMINVAL ? (exp_ah - 1) / a : h; // input integral + act = exp_ah * act + int_h * velocity; + } + + // integral state: Euler integration with anti-windup clamp + else if (offset == slots.integral) { + act = act + act_dot * m->opt.timestep; + mjtNum Imax = dynprm[8]; + if (Imax > 0) { + act = mju_clip(act, -Imax, Imax); + } + } + + // temperature and slew: Euler integration + else { + act = act + act_dot * m->opt.timestep; + } + } + + // otherwise Euler integration + else { act = act + act_dot * m->opt.timestep; } - // clamp to actrange - if (m->actuator_actlimited[actuator_id]) { - mjtNum* actrange = m->actuator_actrange + 2*actuator_id; + // clamp to actrange unless DC motor + if (dyntype != mjDYN_DCMOTOR && m->actuator_actlimited[actuator_id]) { + const mjtNum* actrange = m->actuator_actrange + 2*actuator_id; act = mju_clip(act, actrange[0], actrange[1]); } diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 0dd2ca97..65057a51 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -769,6 +769,26 @@ mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]) { } +// LuGre Stribeck function: g(v) = F_C + (F_S - F_C) * exp(-(v/v_S)^2) +mjtNum mj_lugreStribeck(mjtNum velocity, mjtNum F_C, mjtNum F_S, mjtNum v_S) { + mjtNum ratio = velocity / mju_max(mjMINVAL, v_S); + return F_C + (F_S - F_C) * mju_exp(-ratio*ratio); +} + + +// compute DC motor activation slot indices from parameter arrays +mjDCMotorSlots mj_dcmotorSlots(const mjtNum* dynprm, const mjtNum* gainprm) { + mjDCMotorSlots s = {-1, -1, -1, -1, -1, 0}; + if (dynprm[7] > 0) s.slew = s.num_slots++; // slew rate limiting + if (gainprm[5] > 0) s.integral = s.num_slots++; // PI integral + if (dynprm[2] > 0) s.temperature = s.num_slots++; // thermal model + if (dynprm[5] > 0) s.bristle = s.num_slots++; // LuGre bristle + if (dynprm[0] > 0) s.current = s.num_slots++; // current filter + + return s; +} + + //---------------------------------------- Base64 -------------------------------------------------- // decoding function for Base64 diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 756b3b8e..cac5bed5 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -50,6 +50,23 @@ MJAPI mjtNum mju_muscleDynamicsTimescale(mjtNum dctrl, mjtNum tau_act, mjtNum ta // muscle activation dynamics, prm = (tau_act, tau_deact, smoothing_width) MJAPI mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]); +// LuGre Stribeck function: g(v) = F_C + (F_S - F_C) * exp(-(v/v_S)^2) +mjtNum mj_lugreStribeck(mjtNum velocity, mjtNum F_C, mjtNum F_S, mjtNum v_S); + +// DC motor activation slot indices (-1 = slot not active) +typedef struct { + int slew; // slew rate state + int integral; // integral state + int temperature; // temperature state + int bristle; // LuGre bristle state + int current; // current state + int num_slots; // number of DC motor states +} mjDCMotorSlots; + +// compute activation slot indices for a DC motor actuator +// dynprm = actuator_dynprm row, gainprm = actuator_gainprm row +mjDCMotorSlots mj_dcmotorSlots(const mjtNum* dynprm, const mjtNum* gainprm); + // all 3 semi-axes of a geom MJAPI void mju_geomSemiAxes(mjtNum semiaxes[3], const mjtNum size[3], mjtGeom type); diff --git a/src/user/user_api.cc b/src/user/user_api.cc index f5b569d9..2317cc3a 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -15,6 +15,7 @@ #include "user/user_api.h" #include +#include #include #include #include @@ -1120,6 +1121,166 @@ const char* mjs_setToAdhesion(mjsActuator* actuator, double gain) { +const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance, + double nominal[3], double saturation[4], double inductance[2], + double cogging[3], double controller[5], double thermal[6], + double lugre[6], int input_mode) { + double Kt = motorconst[0]; // torque constant + double Ke = motorconst[1]; // back-EMF constant + double R = resistance; // electrical resistance + double vn = nominal[0]; // nominal voltage + double tau0 = nominal[1]; // stall torque + double omega0 = nominal[2]; // no-load speed + + // derive Ke from nominal: omega0 = vn*Ke / (Ke^2 + R*B) + if (vn > 0 && Ke <= 0 && omega0 > 0) { + // viscous damping (linear), add lugre sigma2 contribution if any + double B = actuator->damping[0]; + if (lugre[0] > 0) B += lugre[2]; + + if (B > 0 && R > 0) { + // R known: solve quadratic Ke^2*omega0 - Ke*vn + R*B*omega0 = 0 + double disc = vn*vn - 4*R*B*omega0*omega0; + Ke = disc > 0 ? (vn + sqrt(disc)) / (2*omega0) : vn / omega0; + } else if (B > 0 && tau0 > 0) { + // R from nominal (tau0 = Ke*vn/R, so R = Ke*vn/tau0) + // substituting into omega0 = vn*Ke/(Ke^2 + R*B): + // omega0 = vn/(Ke + vn*B/tau0) => Ke = vn/omega0 - vn*B/tau0 + double Ke_exact = vn / omega0 - vn*B / tau0; + Ke = Ke_exact > 0 ? Ke_exact : vn / omega0; + } else { + // B = 0 or insufficient data for B-correction: omega0 = vn*Ke/Ke^2 = vn/Ke + Ke = vn / omega0; + } + } + + // resolve effective motor constant K from [Kt, Ke] + double K = (Kt > 0 && Ke > 0) ? sqrt(Kt * Ke) : + (Kt > 0) ? Kt : Ke; + + // derive R from nominal: tau0 = K*vn/R + if (R == 0 && vn > 0 && tau0 > 0 && K > 0) { + R = K * vn / tau0; + } + + if (K <= 0) return "DC motor: motor constant K must be positive"; + if (R <= 0) return "DC motor: resistance R must be positive"; + + // set types + actuator->dyntype = mjDYN_DCMOTOR; + actuator->gaintype = mjGAIN_DCMOTOR; + actuator->biastype = mjBIAS_DCMOTOR; + + // gainprm: [R, K, alpha, T0] + actuator->gainprm[0] = R; + actuator->gainprm[1] = K; + + // controller parameters: gainprm[4:6] for kp, ki, kd + actuator->gainprm[4] = controller[0]; // kp + actuator->gainprm[5] = controller[1]; // ki + actuator->gainprm[6] = controller[2]; // kd + + // controller parameters: dynprm[7,8] for slewmax, Imax + actuator->dynprm[7] = controller[3]; // slewmax + actuator->dynprm[8] = controller[4]; // Imax + + // saturation: [tau_max, i_max, (di/dt)_max, v_max] + if (saturation[2] > 0) { + actuator->dynprm[1] = saturation[2]; // (di/dt)_max + } + if (saturation[3] > 0) { + actuator->gainprm[7] = saturation[3]; // v_max + } + + // saturation -> forcerange + if (saturation[0] > 0 || saturation[1] > 0) { + double tau_max = saturation[0]; + if (tau_max == 0 && saturation[1] > 0) { + tau_max = K * saturation[1]; // tau_max = K * i_max + } + actuator->forcerange[0] = -tau_max; + actuator->forcerange[1] = tau_max; + actuator->forcelimited = 1; + } + + // cogging: [amplitude, periodicity, phase] -> biasprm[0:3] + actuator->biasprm[0] = cogging[0]; // amplitude + actuator->biasprm[1] = cogging[1]; // periodicity + actuator->biasprm[2] = cogging[2]; // phase + + // count activation variables: slot order is slew, integral, temperature, bristle, current + int actdim = 0; + + // inductance: [L, te] + if (inductance[0] < 0) return "DC motor: inductance must be non-negative"; + if (inductance[1] < 0) return "DC motor: electrical time constant must be non-negative"; + double te = inductance[0] > 0 ? inductance[0] / R : inductance[1]; + actuator->dynprm[0] = te; + if (te > 0) { + actdim++; + } + + // controller states: slew rate limiting + if (controller[3] > 0) { // slewmax + actdim++; + } + + // controller states: integral + if (controller[1] > 0) { // ki + actdim++; + } + + // thermal -> temperature activation + if (thermal[0] > 0 || thermal[1] > 0 || thermal[2] > 0) { + double RT = thermal[0]; // thermal resistance + double C = thermal[1]; // thermal capacitance + double tth = thermal[2]; // thermal time constant + double alpha = thermal[3]; // temperature coefficient + double T0 = thermal[4]; // reference temperature + double Ta = thermal[5]; // ambient temperature + + if (tth > 0 && RT > 0 && C == 0) { + C = tth / RT; + } else if (tth > 0 && C > 0 && RT == 0) { + RT = tth / C; + } else if (tth == 0 && RT > 0 && C > 0) { + tth = RT * C; + } + + if (RT <= 0) return "DC motor: thermal resistance must be positive"; + if (C <= 0) return "DC motor: thermal capacitance must be positive"; + + actuator->dynprm[2] = RT; + actuator->dynprm[3] = C; + actuator->dynprm[4] = Ta; + actuator->gainprm[2] = alpha; + actuator->gainprm[3] = T0; + actdim++; + } + + // lugre: {stiffness, damping, viscous, coulomb, static, stribeck} + if (lugre[0] > 0) { + actuator->dynprm[5] = lugre[0]; // stiffness -> sigma0 + actuator->dynprm[6] = lugre[1]; // damping -> sigma1 + actuator->damping[0] += lugre[2]; // viscous -> sigma2 + actuator->biasprm[3] = lugre[3]; // coulomb -> tau_c + actuator->biasprm[4] = lugre[4]; // static -> tau_s + actuator->biasprm[5] = lugre[5]; // stribeck -> omega_s + actdim++; + } + + // set input mode and activation dimension + actuator->gainprm[8] = input_mode; + actuator->actdim = actdim; + + // enforce actlimited = 0; homogeneous bounds are invalid across DC motor states + actuator->actlimited = 0; + + return ""; +} + + + // get spec from body mjSpec* mjs_getSpec(mjsElement* element) { return &(static_cast(element)->model->spec); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index c8395157..5198cc20 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -7222,20 +7222,20 @@ void mjCActuator::Compile(void) { // check and set actdim if (!plugin.active) { - if (actdim > 1 && dyntype != mjDYN_USER) { - throw mjCError(this, "actdim > 1 is only allowed for dyntype 'user' in actuator"); + if (actdim > 1 && dyntype != mjDYN_USER && dyntype != mjDYN_DCMOTOR) { + throw mjCError(this, "actdim > 1 is only allowed for dyntype 'user' and 'dcmotor'"); } if (actdim == 1 && dyntype == mjDYN_NONE) { throw mjCError(this, "invalid actdim 1 in stateless actuator"); } - if (actdim == 0 && dyntype != mjDYN_NONE) { + if (actdim == 0 && dyntype != mjDYN_NONE && dyntype != mjDYN_DCMOTOR) { throw mjCError(this, "invalid actdim 0 in stateful actuator"); } } - // set actdim + // set actdim to 1 if it is unset and type is standard one-activation dyntype if (actdim < 0) { - actdim = (dyntype != mjDYN_NONE); + actdim = (dyntype != mjDYN_NONE && dyntype != mjDYN_DCMOTOR); } // check muscle parameters diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index d40e6a9a..352790b5 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -206,6 +206,10 @@ std::vector MJCF[nMJCF] = { "lmin", "lmax", "vmax", "fpmax", "fvmax"}, {"adhesion", "?", "forcelimited", "ctrlrange", "forcerange", "gain", "user", "group", "nsample", "interp", "delay"}, + {"dcmotor", "?", "ctrllimited", "ctrlrange", + "gear", "damping", "armature", "cranklength", "user", "group", "nsample", "interp", "delay", + "motorconst", "resistance", "nominal", "saturation", + "inductance", "cogging", "controller", "input", "thermal", "lugre"}, {">"}, {"extension", "*"}, @@ -436,6 +440,12 @@ std::vector MJCF[nMJCF] = { "lmin", "lmax", "vmax", "fpmax", "fvmax"}, {"adhesion", "*", "name", "class", "group", "nsample", "interp", "delay", "forcelimited", "ctrlrange", "forcerange", "user", "body", "gain"}, + {"dcmotor", "*", "name", "class", "group", "nsample", "interp", "delay", + "ctrllimited", "ctrlrange", + "lengthrange", "gear", "damping", "armature", "cranklength", "user", + "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", + "motorconst", "resistance", "nominal", "saturation", + "inductance", "cogging", "controller", "thermal", "lugre", "input"}, {"plugin", "*", "name", "class", "plugin", "instance", "group", "nsample", "interp", "delay", "ctrllimited", "forcelimited", "actlimited", "ctrlrange", "forcerange", "actrange", "lengthrange", "gear", "damping", "armature", "cranklength", "joint", "jointinparent", @@ -724,33 +734,45 @@ const mjMap mark_map[mark_sz] = { // dyn type -const int dyn_sz = 6; +const int dyn_sz = 7; const mjMap dyn_map[dyn_sz] = { {"none", mjDYN_NONE}, {"integrator", mjDYN_INTEGRATOR}, {"filter", mjDYN_FILTER}, {"filterexact", mjDYN_FILTEREXACT}, {"muscle", mjDYN_MUSCLE}, + {"dcmotor", mjDYN_DCMOTOR}, {"user", mjDYN_USER} }; +// dcmotor controller input mode +const int dcmotorinput_sz = 3; +const mjMap dcmotorinput_map[dcmotorinput_sz] = { + {"voltage", 0}, + {"position", 1}, + {"velocity", 2} +}; + + // gain type -const int gain_sz = 4; +const int gain_sz = 5; const mjMap gain_map[gain_sz] = { {"fixed", mjGAIN_FIXED}, {"affine", mjGAIN_AFFINE}, {"muscle", mjGAIN_MUSCLE}, + {"dcmotor", mjGAIN_DCMOTOR}, {"user", mjGAIN_USER} }; // bias type -const int bias_sz = 4; +const int bias_sz = 5; const mjMap bias_map[bias_sz] = { {"none", mjBIAS_NONE}, {"affine", mjBIAS_AFFINE}, {"muscle", mjBIAS_MUSCLE}, + {"dcmotor", mjBIAS_DCMOTOR}, {"user", mjBIAS_USER} }; @@ -2498,6 +2520,54 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { err = mjs_setToAdhesion(actuator, gain); } + // DC motor + else if (type == "dcmotor") { + bool inherited = (actuator->gaintype == mjGAIN_DCMOTOR); + double motorconst[2] = {inherited ? actuator->gainprm[1] : 0, 0}; + double resistance = inherited ? actuator->gainprm[0] : 0; + double nominal[3] = {0, 0, 0}; + double saturation[4] = {0, 0, + inherited ? actuator->dynprm[1] : 0, + inherited ? actuator->gainprm[8] : 0}; + double controller[5] = {inherited ? actuator->gainprm[5] : 0, + inherited ? actuator->gainprm[6] : 0, + inherited ? actuator->gainprm[7] : 0, + inherited ? actuator->dynprm[7] : 0, + inherited ? actuator->dynprm[8] : 0}; + double inductance[2] = {0, inherited ? actuator->dynprm[0] : 0}; + double cogging[3] = {inherited ? actuator->biasprm[0] : 0, + inherited ? actuator->biasprm[1] : 0, + inherited ? actuator->biasprm[2] : 0}; + double thermal[6] = {inherited ? actuator->dynprm[2] : 0, + inherited ? actuator->dynprm[3] : 0, + 0, + inherited ? actuator->gainprm[2] : 0, + inherited ? actuator->gainprm[3] : 0, + inherited ? actuator->dynprm[4] : 0}; + double lugre[6] = {inherited ? actuator->dynprm[5] : 0, + inherited ? actuator->dynprm[6] : 0, + inherited ? actuator->damping[0] : 0, + inherited ? actuator->biasprm[3] : 0, + inherited ? actuator->biasprm[4] : 0, + inherited ? actuator->biasprm[5] : 0}; + int input_mode = inherited ? (int)actuator->gainprm[9] : 0; + ReadAttr(elem, "motorconst", 2, motorconst, text, false, false); + ReadAttr(elem, "resistance", 1, &resistance, text); + ReadAttr(elem, "nominal", 3, nominal, text, false, false); + ReadAttr(elem, "saturation", 4, saturation, text, false, false); + ReadAttr(elem, "inductance", 2, inductance, text, false, false); + ReadAttr(elem, "cogging", 3, cogging, text, false, false); + ReadAttr(elem, "controller", 5, controller, text, false, false); + ReadAttr(elem, "thermal", 6, thermal, text, false, false); + ReadAttr(elem, "lugre", 6, lugre, text, false, false); + if (MapValue(elem, "input", &input_mode, dcmotorinput_map, dcmotorinput_sz)) { + // successfully parsed + } + err = mjs_setToDCMotor(actuator, motorconst, resistance, + nominal, saturation, inductance, + cogging, controller, thermal, lugre, input_mode); + } + else if (type == "plugin") { OnePlugin(elem, &actuator->plugin); int n; @@ -2962,7 +3032,8 @@ void mjXReader::Default(XMLElement* section, const mjsDefault* def, const mjVFS* name == "intvelocity" || name == "cylinder" || name == "muscle" || - name == "adhesion") { + name == "adhesion" || + name == "dcmotor") { OneActuator(elem, def->actuator); } diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index 4e0369ce..8c568b22 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -102,7 +102,7 @@ class mjXReader : public mjXBase { }; // MJCF schema -#define nMJCF 246 +#define nMJCF 248 extern std::vector MJCF[nMJCF]; #endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_ diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 144ffe65..e9aa9ea7 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -871,7 +871,7 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDe if (writingdefaults) { WriteAttrInt(elem, "actdim", actuator->actdim, def->Actuator().actdim); } else { - int default_actdim = actuator->dyntype == mjDYN_NONE ? 0 : 1; + int default_actdim = (actuator->dyntype != mjDYN_NONE && actuator->dyntype != mjDYN_DCMOTOR); WriteAttrInt(elem, "actdim", actuator->actdim, default_actdim); } WriteAttrKey(elem, "dyntype", dyn_map, dyn_sz, actuator->dyntype, def->Actuator().dyntype); diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index df8f863b..c32e6b1e 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -91,6 +91,8 @@ static const char* const kDampedPendulumPath = "engine/testdata/derivative/damped_pendulum.xml"; static const char* const kLinearPath = "engine/testdata/derivative/linear.xml"; +static const char* const kDCMotorPath = + "engine/testdata/derivative/dcmotor.xml"; static const char* const kModelPath = "testdata/model.xml"; // compare analytic and finite-difference d_smooth/d_qvel @@ -99,9 +101,12 @@ TEST_F(DerivativeTest, SmoothDvel) { for (const char* local_path : {kEnergyConservingPendulumPath, kTumblingThinObjectPath, kDampedActuatorsPath, - kDamperActuatorsPath}) { + kDamperActuatorsPath, + kDCMotorPath}) { const std::string xml_path = GetTestDataFilePath(local_path); - mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + char error[1024] = ""; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, testing::NotNull()) << "Failed to load model: " << error; int nD = model->nD; mjData* data = mj_makeData(model); @@ -758,9 +763,12 @@ TEST_F(DerivativeTest, DenseSparseRneEquivalent) { for (const char* local_path : {kEnergyConservingPendulumPath, kTumblingThinObjectPath, kDampedActuatorsPath, - kDamperActuatorsPath}) { + kDamperActuatorsPath, + kDCMotorPath}) { const std::string xml_path = GetTestDataFilePath(local_path); - mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0); + char error[1024] = ""; + mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(model, testing::NotNull()) << "Failed to load model: " << error; int nD = model->nD; mjtNum* qDeriv = (mjtNum*) mju_malloc(sizeof(mjtNum)*nD); mjData* data = mj_makeData(model); diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index cee67e29..f3acf0aa 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -1148,6 +1148,949 @@ TEST_F(ActuatorTest, DampRatioTendon) { mj_deleteModel(model); } +// ----------------------- DC motor actuators ---------------------------------- + +using DCMotorTest = MujocoTest; + +TEST_F(DCMotorTest, IntVelocityEquivalence) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // Apply a time-varying velocity command + while (data->time < 1.0) { + data->ctrl[0] = mju_sin(20 * data->time); + data->ctrl[1] = mju_sin(20 * data->time); + mj_step(model, data); + + // Both actuators should integrate identical states + EXPECT_MJTNUM_EQ(data->act[0], data->act[1]); + + // Both bodies should move identically + EXPECT_NEAR(data->qpos[0], data->qpos[1], MjTol(1e-14, 1e-7)); + EXPECT_NEAR(data->qvel[0], data->qvel[1], MjTol(1e-14, 1e-7)); + EXPECT_NEAR(data->qacc[0], data->qacc[1], MjTol(1e-14, 1e-6)); + + // Both actuators should produce identical force + EXPECT_NEAR(data->actuator_force[0], data->actuator_force[1], + MjTol(1e-14, 1e-6)); + } + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, StatelessSteadyState) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + double K = 0.05; + double R = 2.0; + double V = 12.0; + double omega = 3.0; + + data->ctrl[0] = V; + data->qvel[0] = omega; + mj_forward(model, data); + + double expected_force = K / R * (V - K * omega); + EXPECT_NEAR(data->actuator_force[0], expected_force, MjTol(1e-12, 1e-5)); + EXPECT_EQ(model->actuator_actnum[0], 0); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, CurrentFilterConverges) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + ASSERT_EQ(model->actuator_actnum[0], 1); + + double K = 0.05; + double R = 2.0; + double V = 12.0; + + data->ctrl[0] = V; + for (int i = 0; i < 10000; i++) { + mj_step(model, data); + } + + double omega = data->qvel[0]; + double i_ss = V / R - K / R * omega; + double expected_force = K * i_ss; + + EXPECT_NEAR(data->act[0], i_ss, MjTol(1e-6, 1e-4)); + EXPECT_NEAR(data->actuator_force[0], expected_force, MjTol(1e-6, 1e-4)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, CurrentFilterExactIntegration) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + double R = 2.0; + double te = 0.01 / R; + double V = 12.0; + + data->ctrl[0] = V; + mj_step(model, data); + + double h = model->opt.timestep; + double exact_current = V / R * (1 - mju_exp(-h / te)); + EXPECT_NEAR(data->act[0], exact_current, MjTol(1e-10, 1e-4)); + + double euler_current = V / R * h / te; + EXPECT_GT(std::abs(data->act[0] - euler_current), + std::abs(data->act[0] - exact_current)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, CoggingTorque) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + double A = 0.1, Np = 6, phi = 0; + double K = 0.05, R = 2.0; + double V = 5.0; + double pos = 1.0; + + data->ctrl[0] = V; + data->qpos[0] = pos; + mj_forward(model, data); + + double electrical_force = K / R * V; + double cogging = A * mju_sin(Np * pos + phi); + EXPECT_NEAR(data->actuator_force[0], electrical_force + cogging, + MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, CoggingBypassesSaturation) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + double A = 0.1, Np = 6, phi = 0; + double pos = 1.0; + + data->ctrl[0] = 100.0; + data->qpos[0] = pos; + mj_forward(model, data); + + double cogging = A * mju_sin(Np * pos + phi); + EXPECT_NEAR(model->actuator_forcerange[1], 0.001, MjTol(1e-12, 1e-5)); + EXPECT_GT(mju_abs(data->actuator_force[0]), 0.001); + EXPECT_NEAR(data->actuator_force[0], 0.001 + cogging, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, LuGreViscousFriction) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + ASSERT_EQ(model->actuator_actnum[0], 1); + + double sigma1 = 1, sigma2 = 0.01; + double K = 0.05, R = 2.0; + double omega = 2.0; + + data->ctrl[0] = 0; + data->qvel[0] = omega; + mj_forward(model, data); + + EXPECT_MJTNUM_EQ(model->actuator_damping[0], sigma2); + double electrical_force = K / R * (0 - K * omega); + double z = data->act[model->actuator_actadr[0]]; + double z_dot = data->act_dot[model->actuator_actadr[0]]; + double lugre_force = 100 * z + sigma1 * z_dot; + EXPECT_NEAR(data->actuator_force[0], electrical_force - lugre_force, + MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, ThermalRiseAndFall) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + int adr = model->actuator_actadr[0]; + ASSERT_EQ(model->actuator_actnum[0], 1); + EXPECT_EQ(data->act[adr], 0); + + double R = 2.0, V = 10.0; + double RT = 10.0, C = 5.0; + double h = model->opt.timestep; + double P = V * V / R; + + data->ctrl[0] = V; + + mj_step(model, data); + double dT1 = h * P / C; + EXPECT_NEAR(data->act[adr], dT1, MjTol(1e-11, 1e-4)); + + mj_step(model, data); + double dT2 = dT1 + h * (P - dT1 / RT) / C; + EXPECT_NEAR(data->act[adr], dT2, MjTol(1e-11, 1e-4)); + + data->ctrl[0] = 0; + mj_step(model, data); + double dT3 = dT2 + h * (0 - dT2 / RT) / C; + EXPECT_NEAR(data->act[adr], dT3, MjTol(1e-11, 1e-4)); + EXPECT_LT(data->act[adr], dT2); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, ThermalSteadyState) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + double R = 2.0, V = 10.0; + double RT = 0.1; + double dT_ss = RT * V * V / R; + + data->ctrl[0] = V; + for (int i = 0; i < 10000; i++) { + mj_step(model, data); + } + + int adr = model->actuator_actadr[0]; + EXPECT_NEAR(data->act[adr], dT_ss, 1e-4); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, ThermalAffectsForce) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + double K = 0.05, R = 2.0, V = 10.0; + double alpha = 0.004; + int adr = model->actuator_actadr[0]; + + data->ctrl[0] = V; + data->act[adr] = 0; + mj_forward(model, data); + double force_cold = data->actuator_force[0]; + EXPECT_NEAR(force_cold, K / R * V, MjTol(1e-12, 1e-5)); + + double dT = 50; + data->act[adr] = dT; + mj_forward(model, data); + double R_hot = R * (1 + alpha * dT); + double force_hot = data->actuator_force[0]; + EXPECT_NEAR(force_hot, K / R_hot * V, MjTol(1e-12, 1e-5)); + EXPECT_LT(force_hot, force_cold); + + mj_deleteData(data); + mj_deleteModel(model); +} + +// Temperature slot must be correctly offset past slew and integral states. +TEST_F(DCMotorTest, ThermalAffectsForceWithController) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // slot order: slew(0), integral(1), temperature(2) + ASSERT_EQ(model->actuator_actnum[0], 3); + int adr = model->actuator_actadr[0]; + int temp_adr = adr + 2; // temperature is slot 2 + + double K = 0.05, R = 2.0, alpha = 0.004; + double dT = 50; + data->act[adr] = 1.0; // slew state = ctrl: no rate-limiting applied + data->act[adr + 1] = 0.0; // integral state x_I = 0 + data->act[temp_adr] = dT; // temperature rise above ambient + data->ctrl[0] = 1.0; // position setpoint = 1.0, qpos = 0, error = 1.0 + mj_forward(model, data); + + // u_eff = ctrl = 1.0 (no slew applied since act[slew] == ctrl) + // V = kp*(u_eff - length) + ki*x_I - kd*omega = 1.0*1.0 + 1.0*0.0 - 0*0 = 1.0 + // R(T) = 2.0 * (1 + 0.004 * 50) = 2.4 + // stateless (no te): force = K/R(T) * V = 0.05/2.4 * 1.0 + double R_hot = R * (1 + alpha * dT); + EXPECT_NEAR(data->actuator_force[0], K / R_hot * 1.0, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, StatelessPositionMode) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // Position target 5.0, current pos 0.0, current vel 0.0 + data->ctrl[0] = 5.0; + mj_forward(model, data); + + // V = Kp * (u - theta) = 2.0 * 5.0 = 10.0 + // force = K / R * V + bias = (0.05 / 2.0) * 10.0 + 0 = 0.25 + EXPECT_NEAR(data->actuator_force[0], 0.25, MjTol(1e-12, 1e-5)); + + // Velocity penalty + data->qvel[0] = 2.0; + mj_forward(model, data); + // V = 10.0 - Kd * omega = 10.0 - (0.5 * 2.0) = 9.0 + // bias = - K^2 / R * omega = -0.0025 / 2.0 * 2.0 = -0.0025 + // force = K / R * V + bias = 0.225 - 0.0025 = 0.2225 + EXPECT_NEAR(data->actuator_force[0], 0.2225, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, StatelessVelocityMode) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // Velocity target 4.0, current vel 1.0 + data->ctrl[0] = 4.0; + data->qvel[0] = 1.0; + mj_forward(model, data); + + // V = Kp * (u - omega) = 3.0 * (4.0 - 1.0) = 9.0 + // bias = - K^2 / R * omega = -0.0025 / 2.0 * 1.0 = -0.00125 + // force = K / R * V + bias = (0.05 / 2.0) * 9.0 - 0.00125 = 0.22375 + EXPECT_NEAR(data->actuator_force[0], 0.22375, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, StatefulPositionMode) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // Controller states: 1 for slew, 1 for ki -> actnum = 2 + ASSERT_EQ(model->actuator_actnum[0], 2); + int adr = model->actuator_actadr[0]; + + // Current states + double u_prev = 1.0; + double x_I = 2.0; + data->act[adr] = u_prev; + data->act[adr+1] = x_I; + + // target 5.0 position, current 0.0 + data->ctrl[0] = 5.0; + data->qvel[0] = 0.5; + mj_forward(model, data); + + // slew bounding: s = 10.0, dt = 0.001. max_change = 0.01 + // Target = 5.0. It is upper bounded by u_prev + 0.01 = 1.01 + EXPECT_NEAR(data->act_dot[adr], 10.0, MjTol(1e-12, 1e-5)); + + // PI error: error = u_eff - length = 1.01 - 0.0 = 1.01 + EXPECT_NEAR(data->act_dot[adr+1], 1.01, MjTol(1e-12, 1e-5)); + + // V = Kp(u_eff - length) + Ki * x_I - Kd * omega + // V = 2.0 * 1.01 + 0.5 * 2.0 - 0.1 * 0.5 = 2.97 + // bias = - K^2/R * omega = -(0.05)^2 / 2.0 * 0.5 = -0.000625 + // force = K/R * V + bias = 0.025 * 2.97 - 0.000625 = 0.073625 + EXPECT_NEAR(data->actuator_force[0], 0.073625, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // Controller states: slew (0), ki (1), current (2). actnum = 3 + ASSERT_EQ(model->actuator_actnum[0], 3); + int adr = model->actuator_actadr[0]; + + double u_prev = 1.0; + double x_I = 2.0; + double current = 0.5; + data->act[adr] = u_prev; + data->act[adr+1] = x_I; + data->act[adr+2] = current; + + // Target 5.0 position, velocity 0.5 + data->ctrl[0] = 5.0; + data->qvel[0] = 0.5; + mj_forward(model, data); + + // Slew bounding: max_change = 0.01, u_eff = 1.01 + EXPECT_NEAR(data->act_dot[adr], 10.0, MjTol(1e-12, 1e-5)); + + // PI error: error = u_eff - length = 1.01 + EXPECT_NEAR(data->act_dot[adr+1], 1.01, MjTol(1e-12, 1e-5)); + + // Voltage computation: + // V = Kp(u_eff - length) + Ki * x_I - Kd * omega + // V = 2.0 * 1.01 + 0.5 * 2.0 - 0.1 * 0.5 = 2.97 + + // Current filter: + // t_e = L / R = 1.0 / 2.0 = 0.5 + // di/dt = (V/R - K/R * omega - i) / t_e + // di/dt = (2.97/2.0 - 0.05/2.0 * 0.5 - 0.5) / 0.5 + // di/dt = (1.485 - 0.0125 - 0.5) / 0.5 = 0.9725 / 0.5 = 1.945 + EXPECT_NEAR(data->act_dot[adr+2], 1.945, MjTol(1e-12, 1e-5)); + + // Force is just K * current since current is stateful + EXPECT_NEAR(data->actuator_force[0], 0.05 * 0.5, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, StatefulVelocityMode) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // Controller states: 1 for ki (no slew) + ASSERT_EQ(model->actuator_actnum[0], 1); + int adr = model->actuator_actadr[0]; + + double x_I = 2.0; // Exactly at Imax limit (Imax = 2.0) + data->act[adr] = x_I; + + // target vel 4.0, current vel 1.0 + data->ctrl[0] = 4.0; + data->qvel[0] = 1.0; + mj_forward(model, data); + + // integrate command directly: error = target = 4.0 + // since x_I == Imax (2.0) and error (4.0) > 0, act_dot should be clamped to 0 + EXPECT_NEAR(data->act_dot[adr], 0.0, MjTol(1e-12, 1e-5)); + + // V = Kp * (u_eff - omega) + Ki * (x_I - length) + // V = 3.0 * (4.0 - 1.0) + 1.0 * (2.0 - 0.0) = 9.0 + 2.0 = 11.0 + // bias = - K^2/R * omega = -(0.05)^2 / 2.0 * 1.0 = -0.00125 + // force = K/R * V + bias = 0.025 * 11.0 - 0.00125 = 0.275 - 0.00125 = 0.27375 + EXPECT_NEAR(data->actuator_force[0], 0.27375, MjTol(1e-12, 1e-5)); + + // repeat with non-zero joint position + data->qpos[0] = 1.5; + mj_forward(model, data); + + // V = 3.0 * (4.0 - 1.0) + 1.0 * (2.0 - 1.5) = 9.0 + 0.5 = 9.5 + // force = K/R * V + bias = 0.025 * 9.5 - 0.00125 = 0.2375 - 0.00125 = 0.23625 + EXPECT_NEAR(data->actuator_force[0], 0.23625, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, CurrentPlusThermal) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + ASSERT_EQ(model->actuator_actnum[0], 2); + int adr = model->actuator_actadr[0]; + + double K = 0.05, R = 2.0, V = 12.0; + double te = 0.01 / R; + double RT = 10.0, C = 5.0; + + double current = 3.0; + double dT = 10.0; + data->act[adr] = dT; + data->act[adr+1] = current; + data->ctrl[0] = V; + mj_forward(model, data); + + EXPECT_NEAR(data->actuator_force[0], K * current, MjTol(1e-12, 1e-5)); + + double R_hot = R * (1 + 0.004 * dT); + double T_dot = (R_hot * current * current - dT / RT) / C; + EXPECT_NEAR(data->act_dot[adr], T_dot, MjTol(1e-10, 1e-4)); + + double omega = data->qvel[0]; + double i_dot = (V/R_hot - K/R_hot*omega - current) / te; + EXPECT_NEAR(data->act_dot[adr+1], i_dot, MjTol(1e-10, 1e-3)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, CurrentRateLimit) { + // Verifies that saturation:current_rate clamps di/dt. + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + ASSERT_EQ(model->actuator_actnum[0], 1); + int adr = model->actuator_actadr[0]; + + double V = 12.0; + double dimax = 100.0; // A/s rate limit + + // unclamped: i_dot = (V/R - 0 - 0) / te = 6 / 0.005 = 1200 A/s >> dimax + data->act[adr] = 0; // current = 0 + data->ctrl[0] = V; + mj_forward(model, data); + + // i_dot should be clipped to +dimax + EXPECT_NEAR(data->act_dot[adr], dimax, MjTol(1e-12, 1e-5)); + + // reverse: large negative drive + data->ctrl[0] = -V; + mj_forward(model, data); + + // i_dot should be clipped to -dimax + EXPECT_NEAR(data->act_dot[adr], -dimax, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, LuGreExactIntegration) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + ASSERT_EQ(model->actuator_actnum[0], 1); + int adr = model->actuator_actadr[0]; + + double sigma0 = 100, F_C = 0.5, F_S = 0.7, v_S = 10; + double z0 = 0.002; + double v = 0.5; + double h = model->opt.timestep; + + data->act[adr] = z0; + data->qvel[0] = v; + + double ratio = v / v_S; + double g_v = F_C + (F_S - F_C) * mju_exp(-ratio*ratio); + double a = -sigma0 * std::abs(v) / g_v; + double exp_ah = mju_exp(a * h); + double int_h = (exp_ah - 1) / a; + double z_new = exp_ah * z0 + int_h * v; + + mj_step(model, data); + EXPECT_NEAR(data->act[adr], z_new, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, LuGreSteadyState) { + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + int adr = model->actuator_actadr[0]; + + double sigma0 = 100, sigma2 = 0.01; + double F_C = 0.5, F_S = 0.7, v_S = 10; + double K = 0.05, R = 2.0; + double v = 0.5; + + data->qvel[0] = v; + data->ctrl[0] = 0; + for (int i = 0; i < 10000; i++) { + mj_step(model, data); + } + + double ratio = v / v_S; + double g_v = F_C + (F_S - F_C) * mju_exp(-ratio*ratio); + double z_ss = g_v / sigma0; + EXPECT_NEAR(data->act[adr], z_ss, 1e-4); + + EXPECT_MJTNUM_EQ(model->actuator_damping[0], sigma2); + double back_emf = K * K / R * data->qvel[0]; + double lugre_ss = g_v; + EXPECT_NEAR(data->actuator_force[0], -back_emf - lugre_ss, 1e-3); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(DCMotorTest, LuGreBristleSpring) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + int adr = model->actuator_actadr[0]; + double sigma0 = 100; + double X = 0.01; + + data->act[adr] = X; + data->ctrl[0] = 0; + mj_forward(model, data); + + EXPECT_NEAR(data->actuator_force[0], -sigma0 * X, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + // ----------------------- filterexact actuators ------------------------------- using FilterExactTest = MujocoTest; diff --git a/test/engine/testdata/derivative/dcmotor.xml b/test/engine/testdata/derivative/dcmotor.xml new file mode 100644 index 00000000..d3c4b0ac --- /dev/null +++ b/test/engine/testdata/derivative/dcmotor.xml @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 45ae2393..bd1b517b 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -3003,6 +3003,325 @@ TEST_F(ActuatorParseTest, AdhesionInheritsFromGeneral) { mj_deleteModel(model); } +TEST_F(ActuatorParseTest, DCMotorBasicParsing) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->actuator_dyntype[0], mjDYN_DCMOTOR); + EXPECT_EQ(model->actuator_gaintype[0], mjGAIN_DCMOTOR); + EXPECT_EQ(model->actuator_biastype[0], mjBIAS_DCMOTOR); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[0], 2.0); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[1], 0.05); + EXPECT_MJTNUM_EQ(model->actuator_damping[0], 1.0); + EXPECT_MJTNUM_EQ(model->actuator_dampingpoly[0], 2.0); + EXPECT_MJTNUM_EQ(model->actuator_dampingpoly[1], 3.0); + EXPECT_MJTNUM_EQ(model->actuator_armature[0], 0.1); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorNominalDerivation) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + + // actuator 0: B = 0, Ke = vn/omega0 + { + double K = 12.0 / 600.0; + double R = K * 12.0 / 0.6; + EXPECT_MJTNUM_EQ(model->actuator_gainprm[0*mjNGAIN + 0], R); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[0*mjNGAIN + 1], K); + } + + // actuator 1: B > 0, R given, quadratic Ke^2*omega0 - Ke*vn + R*B*omega0 = 0 + { + double B = 0.0001, R = 0.4, vn = 12.0, omega0 = 600.0; + double disc = vn*vn - 4*R*B*omega0*omega0; + double Ke = (vn + sqrt(disc)) / (2*omega0); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[1*mjNGAIN + 0], R); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[1*mjNGAIN + 1], Ke); + } + + // actuator 2: B > 0, R from nominal, Ke = vn/omega0 - vn*B/tau0 + { + double B = 0.0001, vn = 12.0, tau0 = 0.6, omega0 = 600.0; + double Ke = vn / omega0 - vn*B / tau0; + double R = Ke * vn / tau0; + EXPECT_MJTNUM_EQ(model->actuator_gainprm[2*mjNGAIN + 0], R); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[2*mjNGAIN + 1], Ke); + } + + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorSaturation) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->actuator_forcelimited[0], 1); + EXPECT_MJTNUM_EQ(model->actuator_forcerange[0], -1.5); + EXPECT_MJTNUM_EQ(model->actuator_forcerange[1], 1.5); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorLuGreRemapping) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_MJTNUM_EQ(model->actuator_dynprm[5], 100); + EXPECT_MJTNUM_EQ(model->actuator_dynprm[6], 1); + EXPECT_MJTNUM_EQ(model->actuator_damping[0], 0.01); + EXPECT_MJTNUM_EQ(model->actuator_biasprm[3], 0.5); + EXPECT_MJTNUM_EQ(model->actuator_biasprm[4], 0.7); + EXPECT_MJTNUM_EQ(model->actuator_biasprm[5], 10); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorActdimStateless) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->actuator_actnum[0], 0); + EXPECT_EQ(model->actuator_actadr[0], -1); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorActdimCurrentOnly) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->actuator_actnum[0], 1); + EXPECT_MJTNUM_EQ(model->actuator_dynprm[0], 0.001 / 2.0); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorActdimThermalOnly) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->actuator_actnum[0], 1); + EXPECT_MJTNUM_EQ(model->actuator_dynprm[2], 10); + EXPECT_MJTNUM_EQ(model->actuator_dynprm[3], 5); + EXPECT_MJTNUM_EQ(model->actuator_dynprm[4], 25); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorActdimLuGreOnly) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->actuator_actnum[0], 1); + EXPECT_MJTNUM_EQ(model->actuator_dynprm[5], 100); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorActdimAllThree) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->actuator_actnum[0], 3); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorMissingKError) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("motor constant K must be positive")); +} + +TEST_F(ActuatorParseTest, DCMotorDefaultsPropagate) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[0], 1.5); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[1], 0.03); + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorMotorconstGeometricMean) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + double K = std::sqrt(0.03 * 0.05); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[0], 2.0); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[1], K); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[mjNGAIN + 1], 0.03); + mj_deleteModel(model); +} + TEST_F(ActuatorParseTest, ActdimDefaultsPropagate) { static constexpr char xml[] = R"( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index ea8ae869..1e6168a9 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -269,19 +269,22 @@ public enum mjtDyn : int{ mjDYN_FILTER = 2, mjDYN_FILTEREXACT = 3, mjDYN_MUSCLE = 4, - mjDYN_USER = 5, + mjDYN_DCMOTOR = 5, + mjDYN_USER = 6, } public enum mjtGain : int{ mjGAIN_FIXED = 0, mjGAIN_AFFINE = 1, mjGAIN_MUSCLE = 2, - mjGAIN_USER = 3, + mjGAIN_DCMOTOR = 3, + mjGAIN_USER = 4, } public enum mjtBias : int{ mjBIAS_NONE = 0, mjBIAS_AFFINE = 1, mjBIAS_MUSCLE = 2, - mjBIAS_USER = 3, + mjBIAS_DCMOTOR = 3, + mjBIAS_USER = 4, } public enum mjtObj : int{ mjOBJ_UNKNOWN = 0, diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 53013ccd..cfa83b40 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -9876,6 +9876,18 @@ std::string mjs_setToCylinder_wrapper(MjsActuator& actuator, double timeconst, d return std::string(mjs_setToCylinder(actuator.get(), timeconst, bias, area, diameter)); } +std::string mjs_setToDCMotor_wrapper(MjsActuator& actuator, const val& motorconst, double resistance, const val& nominal, const val& saturation, const val& inductance, const val& cogging, const val& controller, const val& thermal, const val& lugre, int input_mode) { + UNPACK_VALUE(double, motorconst); + UNPACK_VALUE(double, nominal); + UNPACK_VALUE(double, saturation); + UNPACK_VALUE(double, inductance); + UNPACK_VALUE(double, cogging); + UNPACK_VALUE(double, controller); + UNPACK_VALUE(double, thermal); + UNPACK_VALUE(double, lugre); + return std::string(mjs_setToDCMotor(actuator.get(), motorconst_.data(), resistance, nominal_.data(), saturation_.data(), inductance_.data(), cogging_.data(), controller_.data(), thermal_.data(), lugre_.data(), input_mode)); +} + std::string mjs_setToDamper_wrapper(MjsActuator& actuator, double kv) { return std::string(mjs_setToDamper(actuator.get(), kv)); } @@ -10812,6 +10824,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjBIAS_NONE", mjBIAS_NONE) .value("mjBIAS_AFFINE", mjBIAS_AFFINE) .value("mjBIAS_MUSCLE", mjBIAS_MUSCLE) + .value("mjBIAS_DCMOTOR", mjBIAS_DCMOTOR) .value("mjBIAS_USER", mjBIAS_USER); enum_("mjtBuiltin") .value("mjBUILTIN_NONE", mjBUILTIN_NONE) @@ -10912,6 +10925,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjDYN_FILTER", mjDYN_FILTER) .value("mjDYN_FILTEREXACT", mjDYN_FILTEREXACT) .value("mjDYN_MUSCLE", mjDYN_MUSCLE) + .value("mjDYN_DCMOTOR", mjDYN_DCMOTOR) .value("mjDYN_USER", mjDYN_USER); enum_("mjtEnableBit") .value("mjENBL_OVERRIDE", mjENBL_OVERRIDE) @@ -10974,6 +10988,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjGAIN_FIXED", mjGAIN_FIXED) .value("mjGAIN_AFFINE", mjGAIN_AFFINE) .value("mjGAIN_MUSCLE", mjGAIN_MUSCLE) + .value("mjGAIN_DCMOTOR", mjGAIN_DCMOTOR) .value("mjGAIN_USER", mjGAIN_USER); enum_("mjtGeom") .value("mjGEOM_PLANE", mjGEOM_PLANE) @@ -13295,6 +13310,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { function("mjs_setName", &mjs_setName_wrapper); function("mjs_setToAdhesion", &mjs_setToAdhesion_wrapper); function("mjs_setToCylinder", &mjs_setToCylinder_wrapper); + function("mjs_setToDCMotor", &mjs_setToDCMotor_wrapper); function("mjs_setToDamper", &mjs_setToDamper_wrapper); function("mjs_setToIntVelocity", &mjs_setToIntVelocity_wrapper); function("mjs_setToMotor", &mjs_setToMotor_wrapper); From e9de329e4ebc1692f1052505b8b3f9be3df0f4b4 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Wed, 1 Apr 2026 09:32:47 -0700 Subject: [PATCH 009/251] Import google-deepmind/mujoco_warp from GitHub. PiperOrigin-RevId: 892971818 Change-Id: Ibce0c41294a4fcf2825a246a9533e8927fdc7e82 --- .../mjx/third_party/mujoco_warp/__init__.py | 2 + .../mjx/third_party/mujoco_warp/_src/bvh.py | 602 +-- .../mujoco_warp/_src/collision_convex.py | 27 +- .../mujoco_warp/_src/collision_core.py | 213 +- .../mujoco_warp/_src/collision_driver.py | 31 +- .../mujoco_warp/_src/collision_flex.py | 834 +++++ .../mujoco_warp/_src/collision_gjk.py | 31 +- .../mujoco_warp/_src/collision_primitive.py | 38 +- .../_src/collision_primitive_core.py | 504 +++ .../mujoco_warp/_src/collision_sdf.py | 441 +-- .../mujoco_warp/_src/constraint.py | 3258 +++++++++-------- .../mujoco_warp/_src/derivative.py | 195 +- .../third_party/mujoco_warp/_src/forward.py | 421 +-- .../mjx/third_party/mujoco_warp/_src/io.py | 874 +++-- .../third_party/mujoco_warp/_src/island.py | 33 +- .../mjx/third_party/mujoco_warp/_src/math.py | 29 + .../third_party/mujoco_warp/_src/passive.py | 141 +- .../mjx/third_party/mujoco_warp/_src/ray.py | 22 +- .../third_party/mujoco_warp/_src/render.py | 384 +- .../mujoco_warp/_src/render_util.py | 38 + .../third_party/mujoco_warp/_src/sensor.py | 272 +- .../third_party/mujoco_warp/_src/smooth.py | 1352 ++++--- .../third_party/mujoco_warp/_src/solver.py | 1180 +++--- .../third_party/mujoco_warp/_src/support.py | 34 + .../mjx/third_party/mujoco_warp/_src/types.py | 281 +- .../third_party/mujoco_warp/_src/util_pkg.py | 20 +- .../third_party/mujoco_warp/_src/warp_util.py | 4 +- .../third_party/mujoco_warp/pyproject.toml | 4 +- .../mjx/third_party/mujoco_warp/viewer.py | 5 +- mjx/mujoco/mjx/warp/bvh.py | 31 +- mjx/mujoco/mjx/warp/collision_driver.py | 113 +- mjx/mujoco/mjx/warp/forward.py | 290 +- mjx/mujoco/mjx/warp/forward_test.py | 11 +- mjx/mujoco/mjx/warp/render.py | 16 +- mjx/mujoco/mjx/warp/smooth.py | 23 +- mjx/mujoco/mjx/warp/types.py | 82 +- 36 files changed, 7231 insertions(+), 4605 deletions(-) create mode 100644 mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_flex.py diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/__init__.py b/mjx/mujoco/mjx/third_party/mujoco_warp/__init__.py index 1ff05ff6..40653b62 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/__init__.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/__init__.py @@ -64,6 +64,7 @@ from mujoco.mjx.third_party.mujoco_warp._src.ray import rays as rays from mujoco.mjx.third_party.mujoco_warp._src.render import render as render from mujoco.mjx.third_party.mujoco_warp._src.render_util import get_depth as get_depth from mujoco.mjx.third_party.mujoco_warp._src.render_util import get_rgb as get_rgb +from mujoco.mjx.third_party.mujoco_warp._src.render_util import get_segmentation as get_segmentation from mujoco.mjx.third_party.mujoco_warp._src.sensor import energy_pos as energy_pos from mujoco.mjx.third_party.mujoco_warp._src.sensor import energy_vel as energy_vel from mujoco.mjx.third_party.mujoco_warp._src.sensor import sensor_acc as sensor_acc @@ -92,6 +93,7 @@ from mujoco.mjx.third_party.mujoco_warp._src.support import xfrc_accumulate as x from mujoco.mjx.third_party.mujoco_warp._src.types import BiasType as BiasType from mujoco.mjx.third_party.mujoco_warp._src.types import BroadphaseFilter as BroadphaseFilter from mujoco.mjx.third_party.mujoco_warp._src.types import BroadphaseType as BroadphaseType +from mujoco.mjx.third_party.mujoco_warp._src.types import Callback as Callback from mujoco.mjx.third_party.mujoco_warp._src.types import ConeType as ConeType from mujoco.mjx.third_party.mujoco_warp._src.types import Constraint as Constraint from mujoco.mjx.third_party.mujoco_warp._src.types import Contact as Contact diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/bvh.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/bvh.py index c40fcfc9..58f7b221 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/bvh.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/bvh.py @@ -189,12 +189,12 @@ def _compute_bvh_bounds( upper_out: wp.array(dtype=wp.vec3), group_out: wp.array(dtype=int), ): - world_id, geom_local_id = wp.tid() + worldid, geom_local_id = wp.tid() geom_id = enabled_geom_ids[geom_local_id] - pos = geom_xpos_in[world_id, geom_id] - rot = geom_xmat_in[world_id, geom_id] - size = geom_size[world_id % geom_size.shape[0], geom_id] + pos = geom_xpos_in[worldid, geom_id] + rot = geom_xmat_in[worldid, geom_id] + size = geom_size[worldid % geom_size.shape[0], geom_id] type = geom_type[geom_id] # TODO: Investigate branch elimination with static loop unrolling @@ -218,9 +218,9 @@ def _compute_bvh_bounds( hfield_center = pos + rot[:, 2] * size[2] lower_bound, upper_bound = _compute_box_bounds(hfield_center, rot, size) - lower_out[world_id * bvh_ngeom + geom_local_id] = lower_bound - upper_out[world_id * bvh_ngeom + geom_local_id] = upper_bound - group_out[world_id * bvh_ngeom + geom_local_id] = world_id + lower_out[worldid * bvh_ngeom + geom_local_id] = lower_bound + upper_out[worldid * bvh_ngeom + geom_local_id] = upper_bound + group_out[worldid * bvh_ngeom + geom_local_id] = worldid @wp.kernel @@ -235,14 +235,70 @@ def compute_bvh_group_roots( group_root_out[tid] = root +@wp.kernel +def _compute_flex_bvh_bounds( + # Model: + flex_vertadr: wp.array(dtype=int), + flex_vertnum: wp.array(dtype=int), + flex_edge: wp.array(dtype=wp.vec2i), + flex_radius: wp.array(dtype=float), + # Data in: + flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + # In: + flex_geom_flexid: wp.array(dtype=int), + flex_geom_edgeid: wp.array(dtype=int), + bvh_ngeom: int, + total_bvh_size: int, + # Out: + lower_out: wp.array(dtype=wp.vec3), + upper_out: wp.array(dtype=wp.vec3), + group_out: wp.array(dtype=int), +): + worldid, flexlocalid = wp.tid() + + flex_id = flex_geom_flexid[flexlocalid] + edge_id = flex_geom_edgeid[flexlocalid] + out_idx = worldid * total_bvh_size + bvh_ngeom + flexlocalid + radius = flex_radius[flex_id] + inflate = wp.vec3(radius, radius, radius) + + if edge_id >= 0: # capsule (1D edge) + edge = flex_edge[edge_id] + vert_adr = flex_vertadr[flex_id] + v0 = flexvert_xpos_in[worldid, vert_adr + edge[0]] + v1 = flexvert_xpos_in[worldid, vert_adr + edge[1]] + lower_out[out_idx] = wp.min(v0, v1) - inflate + upper_out[out_idx] = wp.max(v0, v1) + inflate + else: # mesh (2D/3D) + vert_adr = flex_vertadr[flex_id] + nvert = flex_vertnum[flex_id] + min_bound = wp.vec3(MJ_MAXVAL, MJ_MAXVAL, MJ_MAXVAL) + max_bound = wp.vec3(-MJ_MAXVAL, -MJ_MAXVAL, -MJ_MAXVAL) + for i in range(nvert): + v = flexvert_xpos_in[worldid, vert_adr + i] + min_bound = wp.min(min_bound, v) + max_bound = wp.max(max_bound, v) + lower_out[out_idx] = min_bound - inflate + upper_out[out_idx] = max_bound + inflate + + group_out[out_idx] = worldid + + def build_scene_bvh(mjm: mujoco.MjModel, mjd: mujoco.MjData, rc: RenderContext, nworld: int): """Build a global BVH for all geometries in all worlds.""" + total_bvh_size = rc.bvh_ngeom + rc.bvh_nflexgeom + geom_type = wp.array(mjm.geom_type, dtype=int) geom_dataid = wp.array(mjm.geom_dataid, dtype=int) geom_size = wp.array(np.tile(mjm.geom_size[np.newaxis, :, :], (nworld, 1, 1)), dtype=wp.vec3) geom_xpos = wp.array(np.tile(mjd.geom_xpos[np.newaxis, :, :], (nworld, 1, 1)), dtype=wp.vec3) geom_xmat = wp.array(np.tile(mjd.geom_xmat.reshape(mjm.ngeom, 3, 3)[np.newaxis, :, :, :], (nworld, 1, 1, 1)), dtype=wp.mat33) + flex_vertadr = wp.array(mjm.flex_vertadr, dtype=int) + flex_vertnum = wp.array(mjm.flex_vertnum, dtype=int) + flex_edge = wp.array(mjm.flex_edge, dtype=wp.vec2i) + flex_radius = wp.array(mjm.flex_radius, dtype=float) + wp.launch( kernel=_compute_bvh_bounds, dim=(nworld, rc.bvh_ngeom), @@ -252,7 +308,7 @@ def build_scene_bvh(mjm: mujoco.MjModel, mjd: mujoco.MjData, rc: RenderContext, geom_size, geom_xpos, geom_xmat, - rc.bvh_ngeom, + total_bvh_size, rc.enabled_geom_ids, rc.mesh_bounds_size, rc.hfield_bounds_size, @@ -262,6 +318,26 @@ def build_scene_bvh(mjm: mujoco.MjModel, mjd: mujoco.MjData, rc: RenderContext, ], ) + flexvert_xpos = wp.array(np.tile(mjd.flexvert_xpos[np.newaxis, :, :], (nworld, 1, 1)), dtype=wp.vec3) + wp.launch( + kernel=_compute_flex_bvh_bounds, + dim=(nworld, rc.bvh_nflexgeom), + inputs=[ + flex_vertadr, + flex_vertnum, + flex_edge, + flex_radius, + flexvert_xpos, + rc.flex_geom_flexid, + rc.flex_geom_edgeid, + rc.bvh_ngeom, + total_bvh_size, + rc.lower, + rc.upper, + rc.group, + ], + ) + bvh = wp.Bvh(rc.lower, rc.upper, groups=rc.group, constructor="sah") # BVH handle must be stored to avoid garbage collection @@ -277,6 +353,8 @@ def build_scene_bvh(mjm: mujoco.MjModel, mjd: mujoco.MjData, rc: RenderContext, def refit_scene_bvh(m: Model, d: Data, rc: RenderContext): + total_bvh_size = rc.bvh_ngeom + rc.bvh_nflexgeom + wp.launch( kernel=_compute_bvh_bounds, dim=(d.nworld, rc.bvh_ngeom), @@ -286,7 +364,7 @@ def refit_scene_bvh(m: Model, d: Data, rc: RenderContext): m.geom_size, d.geom_xpos, d.geom_xmat, - rc.bvh_ngeom, + total_bvh_size, rc.enabled_geom_ids, rc.mesh_bounds_size, rc.hfield_bounds_size, @@ -296,6 +374,26 @@ def refit_scene_bvh(m: Model, d: Data, rc: RenderContext): ], ) + if rc.bvh_nflexgeom > 0: + wp.launch( + kernel=_compute_flex_bvh_bounds, + dim=(d.nworld, rc.bvh_nflexgeom), + inputs=[ + m.flex_vertadr, + m.flex_vertnum, + m.flex_edge, + m.flex_radius, + d.flexvert_xpos, + rc.flex_geom_flexid, + rc.flex_geom_edgeid, + rc.bvh_ngeom, + total_bvh_size, + rc.lower, + rc.upper, + rc.group, + ], + ) + rc.bvh.refit() @@ -500,6 +598,12 @@ def build_hfield_bvh( @wp.kernel def accumulate_flex_vertex_normals( # Model: + nflex: int, + flex_dim: wp.array(dtype=int), + flex_vertadr: wp.array(dtype=int), + flex_elemadr: wp.array(dtype=int), + flex_elemnum: wp.array(dtype=int), + flex_elemdataadr: wp.array(dtype=int), flex_elem: wp.array(dtype=int), # Data in: flexvert_xpos_in: wp.array2d(dtype=wp.vec3), @@ -509,10 +613,22 @@ def accumulate_flex_vertex_normals( """Accumulate per-vertex normals by summing adjacent face normals.""" worldid, elemid = wp.tid() - elem_base = elemid * 3 - i0 = flex_elem[elem_base + 0] - i1 = flex_elem[elem_base + 1] - i2 = flex_elem[elem_base + 2] + for i in range(nflex): + locid = elemid - flex_elemadr[i] + if locid >= 0 and locid < flex_elemnum[i]: + f = i + break + + if flex_dim[f] == 1 or flex_dim[f] == 3: + return + + local_elemid = elemid - flex_elemadr[f] + elem_adr = flex_elemdataadr[f] + vert_adr = flex_vertadr[f] + elem_base = elem_adr + local_elemid * 3 + i0 = vert_adr + flex_elem[elem_base + 0] + i1 = vert_adr + flex_elem[elem_base + 1] + i2 = vert_adr + flex_elem[elem_base + 2] v0 = flexvert_xpos_in[worldid, i0] v1 = flexvert_xpos_in[worldid, i1] @@ -611,11 +727,12 @@ def _build_flex_2d_elements( @wp.kernel def _build_flex_2d_sides( + # Model: + flex_shell: wp.array(dtype=int), # Data in: flexvert_xpos_in: wp.array2d(dtype=wp.vec3), # In: flexvert_norm_in: wp.array2d(dtype=wp.vec3), - flex_shell_in: wp.array(dtype=int), shell_adr: int, vert_adr: int, face_offset: int, @@ -635,8 +752,8 @@ def _build_flex_2d_sides( worldid, shellid = wp.tid() base = shell_adr + 2 * shellid - i0 = vert_adr + flex_shell_in[base + 0] - i1 = vert_adr + flex_shell_in[base + 1] + i0 = vert_adr + flex_shell[base + 0] + i1 = vert_adr + flex_shell[base + 1] v0 = flexvert_xpos_in[worldid, i0] v1 = flexvert_xpos_in[worldid, i1] @@ -672,10 +789,11 @@ def _build_flex_2d_sides( @wp.kernel def _build_flex_3d_shells( + # Model: + flex_shell: wp.array(dtype=int), # Data in: flexvert_xpos_in: wp.array2d(dtype=wp.vec3), # In: - flex_shell_in: wp.array(dtype=int), shell_adr: int, vert_adr: int, face_offset: int, @@ -693,9 +811,9 @@ def _build_flex_3d_shells( worldid, shellid = wp.tid() base = shell_adr + shellid * 3 - i0 = vert_adr + flex_shell_in[base + 0] - i1 = vert_adr + flex_shell_in[base + 1] - i2 = vert_adr + flex_shell_in[base + 2] + i0 = vert_adr + flex_shell[base + 0] + i1 = vert_adr + flex_shell[base + 1] + i2 = vert_adr + flex_shell[base + 2] face_id = worldid * nface + face_offset + shellid base = face_id * 3 @@ -716,163 +834,163 @@ def _build_flex_3d_shells( @wp.kernel -def _update_flex_face_points( +def _update_flex_2d_face_points( # Model: - nflex: int, - flex_dim: wp.array(dtype=int), flex_vertadr: wp.array(dtype=int), flex_elemnum: wp.array(dtype=int), + flex_elemdataadr: wp.array(dtype=int), + flex_shelldataadr: wp.array(dtype=int), flex_elem: wp.array(dtype=int), + flex_shell: wp.array(dtype=int), + flex_radius: wp.array(dtype=float), # Data in: flexvert_xpos_in: wp.array2d(dtype=wp.vec3), # In: - flex_shell_in: wp.array(dtype=int), flexvert_norm_in: wp.array2d(dtype=wp.vec3), - flex_elemdataadr: wp.array(dtype=int), - flex_shelldataadr: wp.array(dtype=int), - flex_faceadr: wp.array(dtype=int), - flex_radius: wp.array(dtype=float), - flex_workadr: wp.array(dtype=int), - flex_worknum: wp.array(dtype=int), - nfaces: int, + flex_id: int, + nface: int, smooth: bool, # Out: face_point_out: wp.array(dtype=wp.vec3), ): worldid, workid = wp.tid() - # identify which flex this work item belongs to - f = int(0) - locid = int(0) - for i in range(nflex): - locid = workid - flex_workadr[i] - if locid >= 0 and locid < flex_worknum[i]: - f = i - break + elem_adr = flex_elemdataadr[flex_id] + vert_adr = flex_vertadr[flex_id] + radius = flex_radius[flex_id] + nelem = flex_elemnum[flex_id] + world_face_offset = worldid * nface - dim = flex_dim[f] - face_offset = flex_faceadr[f] - world_face_offset = worldid * nfaces - vert_adr = flex_vertadr[f] - - if dim == 2: - radius = flex_radius[f] - elem_count = flex_elemnum[f] - - if locid < elem_count: - # 2D element faces - elemid = locid - elem_adr = flex_elemdataadr[f] - ebase = elem_adr + elemid * 3 - i0 = vert_adr + flex_elem[ebase + 0] - i1 = vert_adr + flex_elem[ebase + 1] - i2 = vert_adr + flex_elem[ebase + 2] - - v0 = flexvert_xpos_in[worldid, i0] - v1 = flexvert_xpos_in[worldid, i1] - v2 = flexvert_xpos_in[worldid, i2] - - # TODO: Use static conditional - if smooth: - n0 = flexvert_norm_in[worldid, i0] - n1 = flexvert_norm_in[worldid, i1] - n2 = flexvert_norm_in[worldid, i2] - else: - face_nrm = wp.cross(v1 - v0, v2 - v0) - face_nrm = wp.normalize(face_nrm) - n0 = face_nrm - n1 = face_nrm - n2 = face_nrm - - p0_pos = v0 + radius * n0 - p1_pos = v1 + radius * n1 - p2_pos = v2 + radius * n2 - - p0_neg = v0 - radius * n0 - p1_neg = v1 - radius * n1 - p2_neg = v2 - radius * n2 - - face_id0 = world_face_offset + face_offset + (2 * elemid) - base0 = face_id0 * 3 - face_point_out[base0 + 0] = p0_pos - face_point_out[base0 + 1] = p1_pos - face_point_out[base0 + 2] = p2_pos - - face_id1 = world_face_offset + face_offset + (2 * elemid + 1) - base1 = face_id1 * 3 - face_point_out[base1 + 0] = p0_neg - face_point_out[base1 + 1] = p1_neg - face_point_out[base1 + 2] = p2_neg - else: - # 2D shell faces - shellid = locid - elem_count - shell_adr = flex_shelldataadr[f] - sbase = shell_adr + 2 * shellid - i0 = vert_adr + flex_shell_in[sbase + 0] - i1 = vert_adr + flex_shell_in[sbase + 1] - - v0 = flexvert_xpos_in[worldid, i0] - v1 = flexvert_xpos_in[worldid, i1] - - n0 = flexvert_norm_in[worldid, i0] - n1 = flexvert_norm_in[worldid, i1] - - shell_face_offset = face_offset + (2 * elem_count) - face_id0 = world_face_offset + shell_face_offset + (2 * shellid) - base0 = face_id0 * 3 - face_point_out[base0 + 0] = v0 + radius * n0 - face_point_out[base0 + 1] = v1 - radius * n1 - face_point_out[base0 + 2] = v1 + radius * n1 - - face_id1 = world_face_offset + shell_face_offset + (2 * shellid + 1) - base1 = face_id1 * 3 - face_point_out[base1 + 0] = v1 - radius * n1 - face_point_out[base1 + 1] = v0 + radius * n0 - face_point_out[base1 + 2] = v0 - radius * n0 - else: - # 3D shell faces - shellid = locid - shell_adr = flex_shelldataadr[f] - sbase = shell_adr + shellid * 3 - i0 = vert_adr + flex_shell_in[sbase + 0] - i1 = vert_adr + flex_shell_in[sbase + 1] - i2 = vert_adr + flex_shell_in[sbase + 2] + if workid < nelem: + # 2D element faces + elemid = workid + ebase = elem_adr + elemid * 3 + i0 = vert_adr + flex_elem[ebase + 0] + i1 = vert_adr + flex_elem[ebase + 1] + i2 = vert_adr + flex_elem[ebase + 2] v0 = flexvert_xpos_in[worldid, i0] v1 = flexvert_xpos_in[worldid, i1] v2 = flexvert_xpos_in[worldid, i2] - face_id = world_face_offset + face_offset + shellid - fbase = face_id * 3 + # TODO: Use static conditional + if smooth: + n0 = flexvert_norm_in[worldid, i0] + n1 = flexvert_norm_in[worldid, i1] + n2 = flexvert_norm_in[worldid, i2] + else: + face_nrm = wp.cross(v1 - v0, v2 - v0) + face_nrm = wp.normalize(face_nrm) + n0 = face_nrm + n1 = face_nrm + n2 = face_nrm - face_point_out[fbase + 0] = v0 - face_point_out[fbase + 1] = v1 - face_point_out[fbase + 2] = v2 + p0_pos = v0 + radius * n0 + p1_pos = v1 + radius * n1 + p2_pos = v2 + radius * n2 + + p0_neg = v0 - radius * n0 + p1_neg = v1 - radius * n1 + p2_neg = v2 - radius * n2 + + face_id0 = world_face_offset + (2 * elemid) + base0 = face_id0 * 3 + face_point_out[base0 + 0] = p0_pos + face_point_out[base0 + 1] = p1_pos + face_point_out[base0 + 2] = p2_pos + + face_id1 = world_face_offset + (2 * elemid + 1) + base1 = face_id1 * 3 + face_point_out[base1 + 0] = p0_neg + face_point_out[base1 + 1] = p1_neg + face_point_out[base1 + 2] = p2_neg + else: + # 2D shell faces + shell_adr = flex_shelldataadr[flex_id] + shellid = workid - nelem + sbase = shell_adr + 2 * shellid + i0 = vert_adr + flex_shell[sbase + 0] + i1 = vert_adr + flex_shell[sbase + 1] + + v0 = flexvert_xpos_in[worldid, i0] + v1 = flexvert_xpos_in[worldid, i1] + + n0 = flexvert_norm_in[worldid, i0] + n1 = flexvert_norm_in[worldid, i1] + + shell_face_offset = 2 * nelem + face_id0 = world_face_offset + shell_face_offset + (2 * shellid) + base0 = face_id0 * 3 + face_point_out[base0 + 0] = v0 + radius * n0 + face_point_out[base0 + 1] = v1 - radius * n1 + face_point_out[base0 + 2] = v1 + radius * n1 + + face_id1 = world_face_offset + shell_face_offset + (2 * shellid + 1) + base1 = face_id1 * 3 + face_point_out[base1 + 0] = v1 - radius * n1 + face_point_out[base1 + 1] = v0 + radius * n0 + face_point_out[base1 + 2] = v0 - radius * n0 + + +@wp.kernel +def _update_flex_3d_face_points( + # Model: + flex_vertadr: wp.array(dtype=int), + flex_shelldataadr: wp.array(dtype=int), + flex_shell: wp.array(dtype=int), + # Data in: + flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + # In: + flex_id: int, + nface: int, + # Out: + face_point_out: wp.array(dtype=wp.vec3), +): + worldid, shellid = wp.tid() + + shell_adr = flex_shelldataadr[flex_id] + vert_adr = flex_vertadr[flex_id] + + face_id = worldid * nface + shellid + fbase = face_id * 3 + + sbase = shell_adr + shellid * 3 + i0 = vert_adr + flex_shell[sbase + 0] + i1 = vert_adr + flex_shell[sbase + 1] + i2 = vert_adr + flex_shell[sbase + 2] + + face_point_out[fbase + 0] = flexvert_xpos_in[worldid, i0] + face_point_out[fbase + 1] = flexvert_xpos_in[worldid, i1] + face_point_out[fbase + 2] = flexvert_xpos_in[worldid, i2] def build_flex_bvh( - mjm: mujoco.MjModel, mjd: mujoco.MjData, nworld: int, constructor: str = "sah", leaf_size: int = 2 -) -> tuple[wp.Mesh, wp.array, wp.array, wp.array, wp.array, wp.array, int]: - """Create a Warp mesh BVH from flex data.""" - if (mjm.flex_dim == 1).any(): - raise ValueError("1D Flex objects are not currently supported.") - - nflex = mjm.nflex + mjm: mujoco.MjModel, + mjd: mujoco.MjData, + nworld: int, + flex_id: int, + constructor: str = "sah", + leaf_size: int = 2, +) -> tuple[wp.Mesh, wp.array, wp.array, wp.array, int]: + """Create a Warp mesh BVH for a single 2D or 3D flex.""" nflexvert = mjm.nflexvert - nflexelemdata = len(mjm.flex_elem) + flex_dim = wp.array(mjm.flex_dim, dtype=int) + flex_elemadr = wp.array(mjm.flex_elemadr, dtype=int) + flex_elemnum = wp.array(mjm.flex_elemnum, dtype=int) flex_elem = wp.array(mjm.flex_elem, dtype=int) + flex_elemdataadr = wp.array(mjm.flex_elemdataadr, dtype=int) + flex_vertadr = wp.array(mjm.flex_vertadr, dtype=int) flexvert_xpos = wp.array(np.tile(mjd.flexvert_xpos[np.newaxis, :, :], (nworld, 1, 1)), dtype=wp.vec3) - flex_faceadr = [0] - for f in range(nflex): - if mjm.flex_dim[f] == 2: - flex_faceadr.append(flex_faceadr[-1] + 2 * mjm.flex_elemnum[f] + 2 * mjm.flex_shellnum[f]) - elif mjm.flex_dim[f] == 3: - flex_faceadr.append(flex_faceadr[-1] + mjm.flex_shellnum[f]) + dim = int(mjm.flex_dim[flex_id]) + nelem = int(mjm.flex_elemnum[flex_id]) + nshell = int(mjm.flex_shellnum[flex_id]) - nface = int(flex_faceadr[-1]) - flex_faceadr = flex_faceadr[:-1] + if dim == 2: + nface = 2 * nelem + 2 * nshell + else: + nface = nshell face_point = wp.empty(nface * 3 * nworld, dtype=wp.vec3) face_index = wp.empty(nface * 3 * nworld, dtype=wp.int32) @@ -883,8 +1001,8 @@ def build_flex_bvh( wp.launch( kernel=accumulate_flex_vertex_normals, - dim=(nworld, nflexelemdata // 3), - inputs=[flex_elem, flexvert_xpos], + dim=(nworld, mjm.nflexelem), + inputs=[mjm.nflex, flex_dim, flex_vertadr, flex_elemadr, flex_elemnum, flex_elemdataadr, flex_elem, flexvert_xpos], outputs=[flexvert_norm], ) @@ -894,60 +1012,56 @@ def build_flex_bvh( inputs=[flexvert_norm], ) - for f in range(nflex): - dim = mjm.flex_dim[f] - elem_adr = mjm.flex_elemdataadr[f] - nelem = mjm.flex_elemnum[f] - shell_adr = mjm.flex_shelldataadr[f] - nshell = mjm.flex_shellnum[f] - vert_adr = mjm.flex_vertadr[f] + elem_adr = mjm.flex_elemdataadr[flex_id] + shell_adr = mjm.flex_shelldataadr[flex_id] + vert_adr = mjm.flex_vertadr[flex_id] - if dim == 2: - wp.launch( - kernel=_build_flex_2d_elements, - dim=(nworld, nelem), - inputs=[ - flex_elem, - flexvert_xpos, - flexvert_norm, - elem_adr, - vert_adr, - flex_faceadr[f], - mjm.flex_radius[f], - nface, - ], - outputs=[face_point, face_index, group], - ) + if dim == 2: + wp.launch( + kernel=_build_flex_2d_elements, + dim=(nworld, nelem), + inputs=[ + flex_elem, + flexvert_xpos, + flexvert_norm, + elem_adr, + vert_adr, + 0, # face_offset + mjm.flex_radius[flex_id], + nface, + ], + outputs=[face_point, face_index, group], + ) - wp.launch( - kernel=_build_flex_2d_sides, - dim=(nworld, nshell), - inputs=[ - flexvert_xpos, - flexvert_norm, - flex_shell, - shell_adr, - vert_adr, - flex_faceadr[f] + 2 * nelem, - mjm.flex_radius[f], - nface, - ], - outputs=[face_point, face_index, group], - ) - elif dim == 3: - wp.launch( - kernel=_build_flex_3d_shells, - dim=(nworld, nshell), - inputs=[ - flexvert_xpos, - flex_shell, - shell_adr, - vert_adr, - flex_faceadr[f], - nface, - ], - outputs=[face_point, face_index, group], - ) + wp.launch( + kernel=_build_flex_2d_sides, + dim=(nworld, nshell), + inputs=[ + flex_shell, + flexvert_xpos, + flexvert_norm, + shell_adr, + vert_adr, + 2 * nelem, # face_offset + mjm.flex_radius[flex_id], + nface, + ], + outputs=[face_point, face_index, group], + ) + elif dim == 3: + wp.launch( + kernel=_build_flex_3d_shells, + dim=(nworld, nshell), + inputs=[ + flex_shell, + flexvert_xpos, + shell_adr, + vert_adr, + 0, # face_offset + nface, + ], + outputs=[face_point, face_index, group], + ) flex_mesh = wp.Mesh( points=face_point, @@ -965,24 +1079,23 @@ def build_flex_bvh( outputs=[group_root], ) - return ( - flex_mesh, - face_point, - group_root, - flex_shell, - flex_faceadr, - nface, - ) + return flex_mesh, group_root def refit_flex_bvh(m: Model, d: Data, rc: RenderContext): - """Refit the flex BVH.""" + """Refit per-flex BVHs.""" flexvert_norm = wp.zeros(d.flexvert_xpos.shape, dtype=wp.vec3) wp.launch( kernel=accumulate_flex_vertex_normals, - dim=(d.nworld, m.nflexelemdata // 3), + dim=(d.nworld, m.nflexelem), inputs=[ + m.nflex, + m.flex_dim, + m.flex_vertadr, + m.flex_elemadr, + m.flex_elemnum, + m.flex_elemdataadr, m.flex_elem, d.flexvert_xpos, ], @@ -991,32 +1104,49 @@ def refit_flex_bvh(m: Model, d: Data, rc: RenderContext): wp.launch( kernel=normalize_vertex_normals, - dim=(d.nworld, m.nflexvert), + dim=(d.nworld, d.flexvert_xpos.shape[1]), inputs=[flexvert_norm], ) - wp.launch( - kernel=_update_flex_face_points, - dim=(d.nworld, rc.flex_nwork), - inputs=[ - m.nflex, - m.flex_dim, - m.flex_vertadr, - m.flex_elemnum, - m.flex_elem, - d.flexvert_xpos, - rc.flex_shell, - flexvert_norm, - rc.flex_elemdataadr, - rc.flex_shelldataadr, - rc.flex_faceadr, - rc.flex_radius, - rc.flex_workadr, - rc.flex_worknum, - rc.flex_nface, - rc.flex_render_smooth, - ], - outputs=[rc.flex_face_point], - ) + for i in range(m.nflex): + if rc.flex_dim_np[i] == 1: + continue + mesh = rc.flex_mesh_registry[i] + nface = mesh.points.shape[0] // (3 * d.nworld) - rc.flex_mesh.refit() + if rc.flex_dim_np[i] == 2: + wp.launch( + kernel=_update_flex_2d_face_points, + dim=(d.nworld, nface // 2), + inputs=[ + m.flex_vertadr, + m.flex_elemnum, + m.flex_elemdataadr, + m.flex_shelldataadr, + m.flex_elem, + m.flex_shell, + m.flex_radius, + d.flexvert_xpos, + flexvert_norm, + i, + nface, + rc.flex_render_smooth, + ], + outputs=[mesh.points], + ) + else: + wp.launch( + kernel=_update_flex_3d_face_points, + dim=(d.nworld, nface), + inputs=[ + m.flex_vertadr, + m.flex_shelldataadr, + m.flex_shell, + d.flexvert_xpos, + i, + nface, + ], + outputs=[mesh.points], + ) + + mesh.refit() diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py index 69ea9cbe..12d9824a 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py @@ -15,34 +15,35 @@ from typing import Tuple +import warp as wp + from mujoco.mjx.third_party.mujoco_warp._src.collision_core import CollisionContext -from mujoco.mjx.third_party.mujoco_warp._src.collision_core import contact_params from mujoco.mjx.third_party.mujoco_warp._src.collision_core import Geom +from mujoco.mjx.third_party.mujoco_warp._src.collision_core import contact_params from mujoco.mjx.third_party.mujoco_warp._src.collision_core import geom_collision_pair from mujoco.mjx.third_party.mujoco_warp._src.collision_core import write_contact from mujoco.mjx.third_party.mujoco_warp._src.collision_gjk import ccd from mujoco.mjx.third_party.mujoco_warp._src.collision_gjk import multicontact from mujoco.mjx.third_party.mujoco_warp._src.collision_gjk import support -from mujoco.mjx.third_party.mujoco_warp._src.collision_primitive import contact_params from mujoco.mjx.third_party.mujoco_warp._src.collision_primitive import Geom +from mujoco.mjx.third_party.mujoco_warp._src.collision_primitive import contact_params from mujoco.mjx.third_party.mujoco_warp._src.collision_primitive import geom_collision_pair from mujoco.mjx.third_party.mujoco_warp._src.collision_primitive import write_contact from mujoco.mjx.third_party.mujoco_warp._src.math import make_frame from mujoco.mjx.third_party.mujoco_warp._src.math import upper_trid_index -from mujoco.mjx.third_party.mujoco_warp._src.types import Data -from mujoco.mjx.third_party.mujoco_warp._src.types import EnableBit -from mujoco.mjx.third_party.mujoco_warp._src.types import GeomType -from mujoco.mjx.third_party.mujoco_warp._src.types import mat43 -from mujoco.mjx.third_party.mujoco_warp._src.types import mat63 from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAX_EPAFACES from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAX_EPAHORIZON from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXCONPAIR from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXVAL +from mujoco.mjx.third_party.mujoco_warp._src.types import Data +from mujoco.mjx.third_party.mujoco_warp._src.types import EnableBit +from mujoco.mjx.third_party.mujoco_warp._src.types import GeomType from mujoco.mjx.third_party.mujoco_warp._src.types import Model +from mujoco.mjx.third_party.mujoco_warp._src.types import mat43 +from mujoco.mjx.third_party.mujoco_warp._src.types import mat63 from mujoco.mjx.third_party.mujoco_warp._src.types import vec5 from mujoco.mjx.third_party.mujoco_warp._src.warp_util import cache_kernel from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope -import warp as wp # TODO(team): improve compile time to enable backward pass wp.set_module_options({"enable_backward": False}) @@ -233,6 +234,7 @@ def ccd_hfield_kernel_builder( contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -516,6 +518,7 @@ def ccd_hfield_kernel_builder( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -572,6 +575,7 @@ def ccd_hfield_kernel_builder( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -626,6 +630,7 @@ def ccd_hfield_kernel_builder( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -681,6 +686,7 @@ def ccd_hfield_kernel_builder( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -751,6 +757,7 @@ def ccd_kernel_builder( contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -871,6 +878,7 @@ def ccd_kernel_builder( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -956,6 +964,7 @@ def ccd_kernel_builder( contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -1070,6 +1079,7 @@ def ccd_kernel_builder( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -1156,6 +1166,7 @@ def convex_narrowphase(m: Model, d: Data, ctx: CollisionContext, collision_table d.contact.solimp, d.contact.dim, d.contact.geom, + d.contact.efc_address, d.contact.worldid, d.contact.type, d.contact.geomcollisionid, diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_core.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_core.py index f8294f65..b7affa7d 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_core.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_core.py @@ -18,14 +18,15 @@ import dataclasses from typing import Tuple +import warp as wp + from mujoco.mjx.third_party.mujoco_warp._src.math import safe_div +from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINMU +from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINVAL from mujoco.mjx.third_party.mujoco_warp._src.types import ContactType from mujoco.mjx.third_party.mujoco_warp._src.types import GeomType from mujoco.mjx.third_party.mujoco_warp._src.types import mat63 -from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINMU -from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINVAL from mujoco.mjx.third_party.mujoco_warp._src.types import vec5 -import warp as wp wp.set_module_options({"enable_backward": False}) @@ -63,30 +64,30 @@ class Geom: @wp.func def geom_collision_pair( - # Model: - geom_type: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - mesh_vertadr: wp.array(dtype=int), - mesh_vertnum: wp.array(dtype=int), - mesh_graphadr: wp.array(dtype=int), - mesh_vert: wp.array(dtype=wp.vec3), - mesh_graph: wp.array(dtype=int), - mesh_polynum: wp.array(dtype=int), - mesh_polyadr: wp.array(dtype=int), - mesh_polynormal: wp.array(dtype=wp.vec3), - mesh_polyvertadr: wp.array(dtype=int), - mesh_polyvertnum: wp.array(dtype=int), - mesh_polyvert: wp.array(dtype=int), - mesh_polymapadr: wp.array(dtype=int), - mesh_polymapnum: wp.array(dtype=int), - mesh_polymap: wp.array(dtype=int), - # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - # In: - geoms: wp.vec2i, - worldid: int, + # Model: + geom_type: wp.array(dtype=int), + geom_dataid: wp.array(dtype=int), + geom_size: wp.array2d(dtype=wp.vec3), + mesh_vertadr: wp.array(dtype=int), + mesh_vertnum: wp.array(dtype=int), + mesh_graphadr: wp.array(dtype=int), + mesh_vert: wp.array(dtype=wp.vec3), + mesh_graph: wp.array(dtype=int), + mesh_polynum: wp.array(dtype=int), + mesh_polyadr: wp.array(dtype=int), + mesh_polynormal: wp.array(dtype=wp.vec3), + mesh_polyvertadr: wp.array(dtype=int), + mesh_polyvertnum: wp.array(dtype=int), + mesh_polyvert: wp.array(dtype=int), + mesh_polymapadr: wp.array(dtype=int), + mesh_polymapnum: wp.array(dtype=int), + mesh_polymap: wp.array(dtype=int), + # Data in: + geom_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xmat_in: wp.array2d(dtype=wp.mat33), + # In: + geoms: wp.vec2i, + worldid: int, ) -> Tuple[Geom, Geom]: geom1 = Geom() geom2 = Geom() @@ -155,38 +156,39 @@ def geom_collision_pair( @wp.func def write_contact( - # Data in: - naconmax_in: int, - # In: - id_: int, - dist_in: float, - pos_in: wp.vec3, - frame_in: wp.mat33, - margin_in: float, - gap_in: float, - condim_in: int, - friction_in: vec5, - solref_in: wp.vec2, - solreffriction_in: wp.vec2, - solimp_in: vec5, - geoms_in: wp.vec2i, - pairid_in: wp.vec2i, - worldid_in: int, - # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + # Data in: + naconmax_in: int, + # In: + id_: int, + dist_in: float, + pos_in: wp.vec3, + frame_in: wp.mat33, + margin_in: float, + gap_in: float, + condim_in: int, + friction_in: vec5, + solref_in: wp.vec2, + solreffriction_in: wp.vec2, + solimp_in: vec5, + geoms_in: wp.vec2i, + pairid_in: wp.vec2i, + worldid_in: int, + # Data out: + contact_dist_out: wp.array(dtype=float), + contact_pos_out: wp.array(dtype=wp.vec3), + contact_frame_out: wp.array(dtype=wp.mat33), + contact_includemargin_out: wp.array(dtype=float), + contact_friction_out: wp.array(dtype=vec5), + contact_solref_out: wp.array(dtype=wp.vec2), + contact_solreffriction_out: wp.array(dtype=wp.vec2), + contact_solimp_out: wp.array(dtype=vec5), + contact_dim_out: wp.array(dtype=int), + contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), + contact_worldid_out: wp.array(dtype=int), + contact_type_out: wp.array(dtype=int), + contact_geomcollisionid_out: wp.array(dtype=int), + nacon_out: wp.array(dtype=int), ) -> int: """Atomically write a detected contact into the contact output arrays. @@ -222,33 +224,35 @@ def write_contact( contact_solimp_out[cid] = solimp_in contact_type_out[cid] = contact_type contact_geomcollisionid_out[cid] = id_ + for i in range(contact_efc_address_out.shape[1]): + contact_efc_address_out[cid, i] = -1 return int(active) return 0 @wp.func def contact_params( - # Model: - geom_condim: wp.array(dtype=int), - geom_priority: wp.array(dtype=int), - geom_solmix: wp.array2d(dtype=float), - geom_solref: wp.array2d(dtype=wp.vec2), - geom_solimp: wp.array2d(dtype=vec5), - geom_friction: wp.array2d(dtype=wp.vec3), - geom_margin: wp.array2d(dtype=float), - geom_gap: wp.array2d(dtype=float), - pair_dim: wp.array(dtype=int), - pair_solref: wp.array2d(dtype=wp.vec2), - pair_solreffriction: wp.array2d(dtype=wp.vec2), - pair_solimp: wp.array2d(dtype=vec5), - pair_margin: wp.array2d(dtype=float), - pair_gap: wp.array2d(dtype=float), - pair_friction: wp.array2d(dtype=vec5), - # In: - collision_pair_in: wp.array(dtype=wp.vec2i), - collision_pairid_in: wp.array(dtype=wp.vec2i), - cid: int, - worldid: int, + # Model: + geom_condim: wp.array(dtype=int), + geom_priority: wp.array(dtype=int), + geom_solmix: wp.array2d(dtype=float), + geom_solref: wp.array2d(dtype=wp.vec2), + geom_solimp: wp.array2d(dtype=vec5), + geom_friction: wp.array2d(dtype=wp.vec3), + geom_margin: wp.array2d(dtype=float), + geom_gap: wp.array2d(dtype=float), + pair_dim: wp.array(dtype=int), + pair_solref: wp.array2d(dtype=wp.vec2), + pair_solreffriction: wp.array2d(dtype=wp.vec2), + pair_solimp: wp.array2d(dtype=vec5), + pair_margin: wp.array2d(dtype=float), + pair_gap: wp.array2d(dtype=float), + pair_friction: wp.array2d(dtype=vec5), + # In: + collision_pair_in: wp.array(dtype=wp.vec2i), + collision_pairid_in: wp.array(dtype=wp.vec2i), + cid: int, + worldid: int, ): """Resolve contact parameters for a collision pair. @@ -267,9 +271,7 @@ def contact_params( condim = pair_dim[pairid] friction = pair_friction[worldid % pair_friction.shape[0], pairid] solref = pair_solref[worldid % pair_solref.shape[0], pairid] - solreffriction = pair_solreffriction[ - worldid % pair_solreffriction.shape[0], pairid - ] + solreffriction = pair_solreffriction[worldid % pair_solreffriction.shape[0], pairid] solimp = pair_solimp[worldid % pair_solimp.shape[0], pairid] else: g1 = geoms[0] @@ -305,44 +307,33 @@ def contact_params( mix = wp.where((solmix1 < MJ_MINVAL) and (solmix2 >= MJ_MINVAL), 0.0, mix) mix = wp.where((solmix1 >= MJ_MINVAL) and (solmix2 < MJ_MINVAL), 1.0, mix) condim = wp.max(condim1, condim2) - max_geom_friction = wp.max( - geom_friction[friction_id, g1], geom_friction[friction_id, g2] - ) + max_geom_friction = wp.max(geom_friction[friction_id, g1], geom_friction[friction_id, g2]) friction = vec5( - max_geom_friction[0], - max_geom_friction[0], - max_geom_friction[1], - max_geom_friction[2], - max_geom_friction[2], + max_geom_friction[0], + max_geom_friction[0], + max_geom_friction[1], + max_geom_friction[2], + max_geom_friction[2], ) - if ( - geom_solref[solref_id, g1][0] > 0.0 - and geom_solref[solref_id, g2][0] > 0.0 - ): - solref = ( - mix * geom_solref[solref_id, g1] - + (1.0 - mix) * geom_solref[solref_id, g2] - ) + if geom_solref[solref_id, g1][0] > 0.0 and geom_solref[solref_id, g2][0] > 0.0: + solref = mix * geom_solref[solref_id, g1] + (1.0 - mix) * geom_solref[solref_id, g2] else: solref = wp.min(geom_solref[solref_id, g1], geom_solref[solref_id, g2]) solreffriction = wp.vec2(0.0, 0.0) - solimp = ( - mix * geom_solimp[solimp_id, g1] - + (1.0 - mix) * geom_solimp[solimp_id, g2] - ) + solimp = mix * geom_solimp[solimp_id, g1] + (1.0 - mix) * geom_solimp[solimp_id, g2] # geom priority is ignored margin = geom_margin[margin_id, g1] + geom_margin[margin_id, g2] gap = geom_gap[gap_id, g1] + geom_gap[gap_id, g2] friction = vec5( - wp.max(MJ_MINMU, friction[0]), - wp.max(MJ_MINMU, friction[1]), - wp.max(MJ_MINMU, friction[2]), - wp.max(MJ_MINMU, friction[3]), - wp.max(MJ_MINMU, friction[4]), + wp.max(MJ_MINMU, friction[0]), + wp.max(MJ_MINMU, friction[1]), + wp.max(MJ_MINMU, friction[2]), + wp.max(MJ_MINMU, friction[3]), + wp.max(MJ_MINMU, friction[4]), ) return geoms, margin, gap, condim, friction, solref, solreffriction, solimp @@ -366,7 +357,7 @@ class CollisionContext: def create_collision_context(naconmax: int) -> CollisionContext: """Create a CollisionContext with allocated arrays.""" return CollisionContext( - collision_pair=wp.empty(naconmax, dtype=wp.vec2i), - collision_pairid=wp.empty(naconmax, dtype=wp.vec2i), - collision_worldid=wp.empty(naconmax, dtype=int), + collision_pair=wp.empty(naconmax, dtype=wp.vec2i), + collision_pairid=wp.empty(naconmax, dtype=wp.vec2i), + collision_worldid=wp.empty(naconmax, dtype=int), ) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py index 36cdaea1..786ee9b8 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py @@ -15,25 +15,27 @@ from typing import Any +import warp as wp + from mujoco.mjx.third_party.mujoco_warp._src.collision_convex import convex_narrowphase from mujoco.mjx.third_party.mujoco_warp._src.collision_core import CollisionContext from mujoco.mjx.third_party.mujoco_warp._src.collision_core import create_collision_context +from mujoco.mjx.third_party.mujoco_warp._src.collision_flex import flex_narrowphase from mujoco.mjx.third_party.mujoco_warp._src.collision_primitive import primitive_narrowphase from mujoco.mjx.third_party.mujoco_warp._src.collision_sdf import sdf_narrowphase from mujoco.mjx.third_party.mujoco_warp._src.math import upper_tri_index +from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXVAL from mujoco.mjx.third_party.mujoco_warp._src.types import BroadphaseFilter from mujoco.mjx.third_party.mujoco_warp._src.types import BroadphaseType from mujoco.mjx.third_party.mujoco_warp._src.types import CollisionType from mujoco.mjx.third_party.mujoco_warp._src.types import Data from mujoco.mjx.third_party.mujoco_warp._src.types import DisableBit from mujoco.mjx.third_party.mujoco_warp._src.types import GeomType +from mujoco.mjx.third_party.mujoco_warp._src.types import Model from mujoco.mjx.third_party.mujoco_warp._src.types import mat23 from mujoco.mjx.third_party.mujoco_warp._src.types import mat63 -from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXVAL -from mujoco.mjx.third_party.mujoco_warp._src.types import Model from mujoco.mjx.third_party.mujoco_warp._src.warp_util import cache_kernel from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope -import warp as wp wp.set_module_options({"enable_backward": False}) @@ -290,25 +292,13 @@ def _broadphase_filter(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound # 8: obb aabb_id = worldid % ngeom_aabb if wp.static(ngeom_aabb > 1) else 0 - center1, center2 = ( - geom_aabb[aabb_id, geom1, 0], - geom_aabb[aabb_id, geom2, 0], - ) # kernel_analyzer: ignore - size1, size2 = ( - geom_aabb[aabb_id, geom1, 1], - geom_aabb[aabb_id, geom2, 1], - ) # kernel_analyzer: ignore + center1, center2 = geom_aabb[aabb_id, geom1, 0], geom_aabb[aabb_id, geom2, 0] # kernel_analyzer: ignore + size1, size2 = geom_aabb[aabb_id, geom1, 1], geom_aabb[aabb_id, geom2, 1] # kernel_analyzer: ignore rbound_id = worldid % ngeom_rbound if wp.static(ngeom_rbound > 1) else 0 - rbound1, rbound2 = ( - geom_rbound[rbound_id, geom1], - geom_rbound[rbound_id, geom2], - ) # kernel_analyzer: ignore + rbound1, rbound2 = geom_rbound[rbound_id, geom1], geom_rbound[rbound_id, geom2] # kernel_analyzer: ignore margin_id = worldid % ngeom_margin if wp.static(ngeom_margin > 1) else 0 - margin1, margin2 = ( - geom_margin[margin_id, geom1], - geom_margin[margin_id, geom2], - ) # kernel_analyzer: ignore + margin1, margin2 = geom_margin[margin_id, geom1], geom_margin[margin_id, geom2] # kernel_analyzer: ignore xpos1, xpos2 = geom_xpos_in[worldid, geom1], geom_xpos_in[worldid, geom2] xmat1, xmat2 = geom_xmat_in[worldid, geom1], geom_xmat_in[worldid, geom2] @@ -757,6 +747,9 @@ def _narrowphase(m: Model, d: Data, ctx: CollisionContext): if m.has_sdf_geom: sdf_narrowphase(m, d, ctx) + if m.nflex > 0: + flex_narrowphase(m, d) + @event_scope def collision(m: Model, d: Data): diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_flex.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_flex.py new file mode 100644 index 00000000..215423e9 --- /dev/null +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_flex.py @@ -0,0 +1,834 @@ +# Copyright 2026 The Newton Developers +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Flex collision detection (geom vs flex triangles).""" + +import warp as wp + +from mujoco.mjx.third_party.mujoco_warp._src import collision_primitive_core +from mujoco.mjx.third_party.mujoco_warp._src.math import make_frame +from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXVAL +from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINMU +from mujoco.mjx.third_party.mujoco_warp._src.types import Data +from mujoco.mjx.third_party.mujoco_warp._src.types import GeomType +from mujoco.mjx.third_party.mujoco_warp._src.types import Model +from mujoco.mjx.third_party.mujoco_warp._src.types import vec5 +from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope + +wp.set_module_options({"enable_backward": False}) + + +@wp.func +def _write_flex_contact( + # Data in: + naconmax_in: int, + # In: + dist: float, + pos: wp.vec3, + frame: wp.mat33, + margin: float, + condim: int, + friction: vec5, + solref: wp.vec2, + solimp: vec5, + geom: int, + flexid: int, + vertid: int, + worldid: int, + # Data out: + contact_dist_out: wp.array(dtype=float), + contact_pos_out: wp.array(dtype=wp.vec3), + contact_frame_out: wp.array(dtype=wp.mat33), + contact_includemargin_out: wp.array(dtype=float), + contact_friction_out: wp.array(dtype=vec5), + contact_solref_out: wp.array(dtype=wp.vec2), + contact_solreffriction_out: wp.array(dtype=wp.vec2), + contact_solimp_out: wp.array(dtype=vec5), + contact_dim_out: wp.array(dtype=int), + contact_geom_out: wp.array(dtype=wp.vec2i), + contact_flex_out: wp.array(dtype=wp.vec2i), + contact_vert_out: wp.array(dtype=wp.vec2i), + contact_worldid_out: wp.array(dtype=int), + contact_type_out: wp.array(dtype=int), + contact_geomcollisionid_out: wp.array(dtype=int), + nacon_out: wp.array(dtype=int), +): + if dist >= margin or dist >= MJ_MAXVAL: + return + + id_ = wp.atomic_add(nacon_out, 0, 1) + if id_ >= naconmax_in: + return + + contact_dist_out[id_] = dist + contact_pos_out[id_] = pos + contact_frame_out[id_] = frame + contact_includemargin_out[id_] = margin + contact_friction_out[id_] = friction + contact_solref_out[id_] = solref + contact_solreffriction_out[id_] = wp.vec2(0.0, 0.0) + contact_solimp_out[id_] = solimp + contact_dim_out[id_] = condim + contact_geom_out[id_] = wp.vec2i(geom, -1) + contact_flex_out[id_] = wp.vec2i(-1, flexid) + contact_vert_out[id_] = wp.vec2i(-1, vertid) + contact_worldid_out[id_] = worldid + contact_type_out[id_] = 1 + contact_geomcollisionid_out[id_] = 0 + + +@wp.func +def _collide_geom_triangle( + # Data in: + naconmax_in: int, + # In: + gtype: int, + pos: wp.vec3, + rot: wp.mat33, + size_val: wp.vec3, + t1: wp.vec3, + t2: wp.vec3, + t3: wp.vec3, + tri_radius: float, + margin: float, + condim: int, + friction: vec5, + solref: wp.vec2, + solimp: vec5, + geomid: int, + flexid: int, + vertex_id: int, + worldid: int, + # Data out: + contact_dist_out: wp.array(dtype=float), + contact_pos_out: wp.array(dtype=wp.vec3), + contact_frame_out: wp.array(dtype=wp.mat33), + contact_includemargin_out: wp.array(dtype=float), + contact_friction_out: wp.array(dtype=vec5), + contact_solref_out: wp.array(dtype=wp.vec2), + contact_solreffriction_out: wp.array(dtype=wp.vec2), + contact_solimp_out: wp.array(dtype=vec5), + contact_dim_out: wp.array(dtype=int), + contact_geom_out: wp.array(dtype=wp.vec2i), + contact_flex_out: wp.array(dtype=wp.vec2i), + contact_vert_out: wp.array(dtype=wp.vec2i), + contact_worldid_out: wp.array(dtype=int), + contact_type_out: wp.array(dtype=int), + contact_geomcollisionid_out: wp.array(dtype=int), + nacon_out: wp.array(dtype=int), +): + if gtype == int(GeomType.SPHERE): + sphere_radius = size_val[0] + dist, contact_pos, nrm = collision_primitive_core.sphere_triangle(pos, sphere_radius, t1, t2, t3, tri_radius) + if dist < margin: + _write_flex_contact( + naconmax_in, + dist, + contact_pos, + make_frame(nrm), + margin, + condim, + friction, + solref, + solimp, + geomid, + flexid, + vertex_id, + worldid, + contact_dist_out, + contact_pos_out, + contact_frame_out, + contact_includemargin_out, + contact_friction_out, + contact_solref_out, + contact_solreffriction_out, + contact_solimp_out, + contact_dim_out, + contact_geom_out, + contact_flex_out, + contact_vert_out, + contact_worldid_out, + contact_type_out, + contact_geomcollisionid_out, + nacon_out, + ) + return + + # Capsule, box, cylinder all return up to 2 contacts - compute then share writing code + dists = wp.vec2(collision_primitive_core.MJ_MAXVAL, collision_primitive_core.MJ_MAXVAL) + poss = collision_primitive_core.mat23f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + nrms = collision_primitive_core.mat23f(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + if gtype == int(GeomType.CAPSULE): + cap_radius = size_val[0] + cap_half_len = size_val[1] + cap_axis = wp.vec3(rot[0, 2], rot[1, 2], rot[2, 2]) + dists, poss, nrms = collision_primitive_core.capsule_triangle( + pos, cap_axis, cap_radius, cap_half_len, t1, t2, t3, tri_radius + ) + elif gtype == int(GeomType.BOX): + dists, poss, nrms = collision_primitive_core.box_triangle(pos, rot, size_val, t1, t2, t3, tri_radius) + elif gtype == int(GeomType.CYLINDER): + cyl_radius = size_val[0] + cyl_half_height = size_val[1] + cyl_axis = wp.vec3(rot[0, 2], rot[1, 2], rot[2, 2]) + dists, poss, nrms = collision_primitive_core.cylinder_triangle( + pos, cyl_axis, cyl_radius, cyl_half_height, t1, t2, t3, tri_radius + ) + + # Write up to 2 contacts (shared code for capsule/box/cylinder) + if dists[0] < margin: + p1 = wp.vec3(poss[0, 0], poss[0, 1], poss[0, 2]) + n1 = wp.vec3(nrms[0, 0], nrms[0, 1], nrms[0, 2]) + _write_flex_contact( + naconmax_in, + dists[0], + p1, + make_frame(n1), + margin, + condim, + friction, + solref, + solimp, + geomid, + flexid, + vertex_id, + worldid, + contact_dist_out, + contact_pos_out, + contact_frame_out, + contact_includemargin_out, + contact_friction_out, + contact_solref_out, + contact_solreffriction_out, + contact_solimp_out, + contact_dim_out, + contact_geom_out, + contact_flex_out, + contact_vert_out, + contact_worldid_out, + contact_type_out, + contact_geomcollisionid_out, + nacon_out, + ) + if dists[1] < margin: + p2 = wp.vec3(poss[1, 0], poss[1, 1], poss[1, 2]) + n2 = wp.vec3(nrms[1, 0], nrms[1, 1], nrms[1, 2]) + _write_flex_contact( + naconmax_in, + dists[1], + p2, + make_frame(n2), + margin, + condim, + friction, + solref, + solimp, + geomid, + flexid, + vertex_id, + worldid, + contact_dist_out, + contact_pos_out, + contact_frame_out, + contact_includemargin_out, + contact_friction_out, + contact_solref_out, + contact_solreffriction_out, + contact_solimp_out, + contact_dim_out, + contact_geom_out, + contact_flex_out, + contact_vert_out, + contact_worldid_out, + contact_type_out, + contact_geomcollisionid_out, + nacon_out, + ) + + +@wp.kernel +def _flex_plane_narrowphase( + # Model: + ngeom: int, + nflexvert: int, + geom_type: wp.array(dtype=int), + geom_condim: wp.array(dtype=int), + geom_solref: wp.array2d(dtype=wp.vec2), + geom_solimp: wp.array2d(dtype=vec5), + geom_friction: wp.array2d(dtype=wp.vec3), + geom_margin: wp.array2d(dtype=float), + flex_condim: wp.array(dtype=int), + flex_friction: wp.array(dtype=wp.vec3), + flex_margin: wp.array(dtype=float), + flex_vertadr: wp.array(dtype=int), + flex_radius: wp.array(dtype=float), + flex_vertflexid: wp.array(dtype=int), + # Data in: + geom_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xmat_in: wp.array2d(dtype=wp.mat33), + flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + nworld_in: int, + naconmax_in: int, + # Data out: + contact_dist_out: wp.array(dtype=float), + contact_pos_out: wp.array(dtype=wp.vec3), + contact_frame_out: wp.array(dtype=wp.mat33), + contact_includemargin_out: wp.array(dtype=float), + contact_friction_out: wp.array(dtype=vec5), + contact_solref_out: wp.array(dtype=wp.vec2), + contact_solreffriction_out: wp.array(dtype=wp.vec2), + contact_solimp_out: wp.array(dtype=vec5), + contact_dim_out: wp.array(dtype=int), + contact_geom_out: wp.array(dtype=wp.vec2i), + contact_flex_out: wp.array(dtype=wp.vec2i), + contact_vert_out: wp.array(dtype=wp.vec2i), + contact_worldid_out: wp.array(dtype=int), + contact_type_out: wp.array(dtype=int), + contact_geomcollisionid_out: wp.array(dtype=int), + nacon_out: wp.array(dtype=int), +): + worldid, vertid = wp.tid() + + flexid = flex_vertflexid[vertid] + radius = flex_radius[flexid] + flex_margin_val = flex_margin[flexid] + flex_condim_val = flex_condim[flexid] + flex_fric = flex_friction[flexid] + # Convert global vertid to local vertex index within this flex + local_vertid = vertid - flex_vertadr[flexid] + + vert = flexvert_xpos_in[worldid, vertid] + + # TODO: Add a broadphase + for geomid in range(ngeom): + gtype = geom_type[geomid] + if gtype != int(GeomType.PLANE): + continue + + plane_pos = geom_xpos_in[worldid, geomid] + plane_rot = geom_xmat_in[worldid, geomid] + plane_normal = wp.vec3(plane_rot[0, 2], plane_rot[1, 2], plane_rot[2, 2]) + + margin = geom_margin[worldid % geom_margin.shape[0], geomid] + flex_margin_val + + diff = vert - plane_pos + signed_dist = wp.dot(diff, plane_normal) + dist = signed_dist - radius + + if dist < margin: + geom_condim_val = geom_condim[geomid] + condim = wp.max(geom_condim_val, flex_condim_val) + solref = geom_solref[worldid % geom_solref.shape[0], geomid] + solimp = geom_solimp[worldid % geom_solimp.shape[0], geomid] + geom_fric = geom_friction[worldid % geom_friction.shape[0], geomid] + fric0 = wp.max(geom_fric[0], flex_fric[0]) + fric1 = wp.max(geom_fric[1], flex_fric[1]) + fric2 = wp.max(geom_fric[2], flex_fric[2]) + friction = vec5( + wp.max(MJ_MINMU, fric0), + wp.max(MJ_MINMU, fric0), + wp.max(MJ_MINMU, fric1), + wp.max(MJ_MINMU, fric2), + wp.max(MJ_MINMU, fric2), + ) + + contact_pos = vert - plane_normal * (dist * 0.5 + radius) + _write_flex_contact( + naconmax_in, + dist, + contact_pos, + make_frame(plane_normal), + margin, + condim, + friction, + solref, + solimp, + geomid, + flexid, + local_vertid, + worldid, + contact_dist_out, + contact_pos_out, + contact_frame_out, + contact_includemargin_out, + contact_friction_out, + contact_solref_out, + contact_solreffriction_out, + contact_solimp_out, + contact_dim_out, + contact_geom_out, + contact_flex_out, + contact_vert_out, + contact_worldid_out, + contact_type_out, + contact_geomcollisionid_out, + nacon_out, + ) + + +@wp.kernel +def _flex_narrowphase_dim2( + # Model: + ngeom: int, + nflex: int, + geom_type: wp.array(dtype=int), + geom_contype: wp.array(dtype=int), + geom_conaffinity: wp.array(dtype=int), + geom_condim: wp.array(dtype=int), + geom_solref: wp.array2d(dtype=wp.vec2), + geom_solimp: wp.array2d(dtype=vec5), + geom_size: wp.array2d(dtype=wp.vec3), + geom_friction: wp.array2d(dtype=wp.vec3), + geom_margin: wp.array2d(dtype=float), + flex_contype: wp.array(dtype=int), + flex_conaffinity: wp.array(dtype=int), + flex_margin: wp.array(dtype=float), + flex_dim: wp.array(dtype=int), + flex_vertadr: wp.array(dtype=int), + flex_elemadr: wp.array(dtype=int), + flex_elemnum: wp.array(dtype=int), + flex_elemdataadr: wp.array(dtype=int), + flex_elem: wp.array(dtype=int), + flex_radius: wp.array(dtype=float), + # Data in: + geom_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xmat_in: wp.array2d(dtype=wp.mat33), + flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + nworld_in: int, + naconmax_in: int, + # Data out: + contact_dist_out: wp.array(dtype=float), + contact_pos_out: wp.array(dtype=wp.vec3), + contact_frame_out: wp.array(dtype=wp.mat33), + contact_includemargin_out: wp.array(dtype=float), + contact_friction_out: wp.array(dtype=vec5), + contact_solref_out: wp.array(dtype=wp.vec2), + contact_solreffriction_out: wp.array(dtype=wp.vec2), + contact_solimp_out: wp.array(dtype=vec5), + contact_dim_out: wp.array(dtype=int), + contact_geom_out: wp.array(dtype=wp.vec2i), + contact_flex_out: wp.array(dtype=wp.vec2i), + contact_vert_out: wp.array(dtype=wp.vec2i), + contact_worldid_out: wp.array(dtype=int), + contact_type_out: wp.array(dtype=int), + contact_geomcollisionid_out: wp.array(dtype=int), + nacon_out: wp.array(dtype=int), +): + worldid, elemid = wp.tid() + + flexid = int(-1) + for i in range(nflex): + if flex_dim[i] != 2: + continue + elem_adr = flex_elemadr[i] + elem_num = flex_elemnum[i] + if elemid >= elem_adr and elemid < elem_adr + elem_num: + flexid = i + break + + if flexid < 0: + return + + vert_adr = flex_vertadr[flexid] + tri_radius = flex_radius[flexid] + tri_margin = flex_margin[flexid] + + elem_data_idx = flex_elemdataadr[flexid] + (elemid - flex_elemadr[flexid]) * 3 + v0_local = flex_elem[elem_data_idx] + v1_local = flex_elem[elem_data_idx + 1] + v2_local = flex_elem[elem_data_idx + 2] + + t1 = flexvert_xpos_in[worldid, vert_adr + v0_local] + t2 = flexvert_xpos_in[worldid, vert_adr + v1_local] + t3 = flexvert_xpos_in[worldid, vert_adr + v2_local] + + # TODO: Add a broadphase + for geomid in range(ngeom): + gtype = geom_type[geomid] + if ( + gtype != int(GeomType.SPHERE) + and gtype != int(GeomType.CAPSULE) + and gtype != int(GeomType.BOX) + and gtype != int(GeomType.CYLINDER) + ): + continue + + g_contype = geom_contype[geomid] + g_conaffinity = geom_conaffinity[geomid] + f_contype = flex_contype[flexid] + f_conaffinity = flex_conaffinity[flexid] + if not ((g_contype & f_conaffinity) or (f_contype & g_conaffinity)): + continue + + geom_margin_val = geom_margin[worldid % geom_margin.shape[0], geomid] + margin = geom_margin_val + tri_margin + + geom_pos = geom_xpos_in[worldid, geomid] + geom_rot = geom_xmat_in[worldid, geomid] + geom_size_val = geom_size[worldid % geom_size.shape[0], geomid] + + condim = geom_condim[geomid] + gf = geom_friction[worldid % geom_friction.shape[0], geomid] + friction = vec5( + wp.max(MJ_MINMU, gf[0]), + wp.max(MJ_MINMU, gf[0]), + wp.max(MJ_MINMU, gf[1]), + wp.max(MJ_MINMU, gf[2]), + wp.max(MJ_MINMU, gf[2]), + ) + solref = geom_solref[worldid % geom_solref.shape[0], geomid] + solimp = geom_solimp[worldid % geom_solimp.shape[0], geomid] + + _collide_geom_triangle( + naconmax_in, + gtype, + geom_pos, + geom_rot, + geom_size_val, + t1, + t2, + t3, + tri_radius, + margin, + condim, + friction, + solref, + solimp, + geomid, + flexid, + v0_local, + worldid, + contact_dist_out, + contact_pos_out, + contact_frame_out, + contact_includemargin_out, + contact_friction_out, + contact_solref_out, + contact_solreffriction_out, + contact_solimp_out, + contact_dim_out, + contact_geom_out, + contact_flex_out, + contact_vert_out, + contact_worldid_out, + contact_type_out, + contact_geomcollisionid_out, + nacon_out, + ) + + +@wp.kernel +def _flex_narrowphase_dim3( + # Model: + ngeom: int, + nflex: int, + geom_type: wp.array(dtype=int), + geom_contype: wp.array(dtype=int), + geom_conaffinity: wp.array(dtype=int), + geom_condim: wp.array(dtype=int), + geom_solref: wp.array2d(dtype=wp.vec2), + geom_solimp: wp.array2d(dtype=vec5), + geom_size: wp.array2d(dtype=wp.vec3), + geom_friction: wp.array2d(dtype=wp.vec3), + geom_margin: wp.array2d(dtype=float), + flex_contype: wp.array(dtype=int), + flex_conaffinity: wp.array(dtype=int), + flex_margin: wp.array(dtype=float), + flex_dim: wp.array(dtype=int), + flex_vertadr: wp.array(dtype=int), + flex_shellnum: wp.array(dtype=int), + flex_shelldataadr: wp.array(dtype=int), + flex_shell: wp.array(dtype=int), + flex_radius: wp.array(dtype=float), + # Data in: + geom_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xmat_in: wp.array2d(dtype=wp.mat33), + flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + nworld_in: int, + naconmax_in: int, + # Data out: + contact_dist_out: wp.array(dtype=float), + contact_pos_out: wp.array(dtype=wp.vec3), + contact_frame_out: wp.array(dtype=wp.mat33), + contact_includemargin_out: wp.array(dtype=float), + contact_friction_out: wp.array(dtype=vec5), + contact_solref_out: wp.array(dtype=wp.vec2), + contact_solreffriction_out: wp.array(dtype=wp.vec2), + contact_solimp_out: wp.array(dtype=vec5), + contact_dim_out: wp.array(dtype=int), + contact_geom_out: wp.array(dtype=wp.vec2i), + contact_flex_out: wp.array(dtype=wp.vec2i), + contact_vert_out: wp.array(dtype=wp.vec2i), + contact_worldid_out: wp.array(dtype=int), + contact_type_out: wp.array(dtype=int), + contact_geomcollisionid_out: wp.array(dtype=int), + nacon_out: wp.array(dtype=int), +): + worldid, shellid = wp.tid() + + flexid = int(-1) + shell_offset = int(0) + for i in range(nflex): + if flex_dim[i] != 3: + continue + shell_num = flex_shellnum[i] + if shellid >= shell_offset and shellid < shell_offset + shell_num: + flexid = i + break + shell_offset += shell_num + + if flexid < 0: + return + + vert_adr = flex_vertadr[flexid] + tri_radius = flex_radius[flexid] + tri_margin = flex_margin[flexid] + + shell_adr = flex_shelldataadr[flexid] + local_shellid = shellid - shell_offset + shell_data_idx = shell_adr + local_shellid * 3 + + v0_local = flex_shell[shell_data_idx] + v1_local = flex_shell[shell_data_idx + 1] + v2_local = flex_shell[shell_data_idx + 2] + + t1 = flexvert_xpos_in[worldid, vert_adr + v0_local] + t2 = flexvert_xpos_in[worldid, vert_adr + v1_local] + t3 = flexvert_xpos_in[worldid, vert_adr + v2_local] + + # TODO: Add a broadphase + for geomid in range(ngeom): + gtype = geom_type[geomid] + if ( + gtype != int(GeomType.SPHERE) + and gtype != int(GeomType.CAPSULE) + and gtype != int(GeomType.BOX) + and gtype != int(GeomType.CYLINDER) + ): + continue + + g_contype = geom_contype[geomid] + g_conaffinity = geom_conaffinity[geomid] + f_contype = flex_contype[flexid] + f_conaffinity = flex_conaffinity[flexid] + if not ((g_contype & f_conaffinity) or (f_contype & g_conaffinity)): + continue + + geom_margin_val = geom_margin[worldid % geom_margin.shape[0], geomid] + margin = geom_margin_val + tri_margin + + geom_pos = geom_xpos_in[worldid, geomid] + geom_rot = geom_xmat_in[worldid, geomid] + geom_size_val = geom_size[worldid % geom_size.shape[0], geomid] + + condim = geom_condim[geomid] + gf = geom_friction[worldid % geom_friction.shape[0], geomid] + friction = vec5( + wp.max(MJ_MINMU, gf[0]), + wp.max(MJ_MINMU, gf[0]), + wp.max(MJ_MINMU, gf[1]), + wp.max(MJ_MINMU, gf[2]), + wp.max(MJ_MINMU, gf[2]), + ) + solref = geom_solref[worldid % geom_solref.shape[0], geomid] + solimp = geom_solimp[worldid % geom_solimp.shape[0], geomid] + + _collide_geom_triangle( + naconmax_in, + gtype, + geom_pos, + geom_rot, + geom_size_val, + t1, + t2, + t3, + tri_radius, + margin, + condim, + friction, + solref, + solimp, + geomid, + flexid, + v0_local, + worldid, + contact_dist_out, + contact_pos_out, + contact_frame_out, + contact_includemargin_out, + contact_friction_out, + contact_solref_out, + contact_solreffriction_out, + contact_solimp_out, + contact_dim_out, + contact_geom_out, + contact_flex_out, + contact_vert_out, + contact_worldid_out, + contact_type_out, + contact_geomcollisionid_out, + nacon_out, + ) + + +@event_scope +def flex_narrowphase(m: Model, d: Data): + """Runs collision detection between geoms and flex elements.""" + if m.nflex == 0: + return + + wp.launch( + _flex_narrowphase_dim2, + dim=(d.nworld, m.nflexelem), + inputs=[ + m.ngeom, + m.nflex, + m.geom_type, + m.geom_contype, + m.geom_conaffinity, + m.geom_condim, + m.geom_solref, + m.geom_solimp, + m.geom_size, + m.geom_friction, + m.geom_margin, + m.flex_contype, + m.flex_conaffinity, + m.flex_margin, + m.flex_dim, + m.flex_vertadr, + m.flex_elemadr, + m.flex_elemnum, + m.flex_elemdataadr, + m.flex_elem, + m.flex_radius, + d.geom_xpos, + d.geom_xmat, + d.flexvert_xpos, + d.nworld, + d.naconmax, + ], + outputs=[ + d.contact.dist, + d.contact.pos, + d.contact.frame, + d.contact.includemargin, + d.contact.friction, + d.contact.solref, + d.contact.solreffriction, + d.contact.solimp, + d.contact.dim, + d.contact.geom, + d.contact.flex, + d.contact.vert, + d.contact.worldid, + d.contact.type, + d.contact.geomcollisionid, + d.nacon, + ], + ) + + wp.launch( + _flex_narrowphase_dim3, + dim=(d.nworld, m.nflexshelldata // 3), + inputs=[ + m.ngeom, + m.nflex, + m.geom_type, + m.geom_contype, + m.geom_conaffinity, + m.geom_condim, + m.geom_solref, + m.geom_solimp, + m.geom_size, + m.geom_friction, + m.geom_margin, + m.flex_contype, + m.flex_conaffinity, + m.flex_margin, + m.flex_dim, + m.flex_vertadr, + m.flex_shellnum, + m.flex_shelldataadr, + m.flex_shell, + m.flex_radius, + d.geom_xpos, + d.geom_xmat, + d.flexvert_xpos, + d.nworld, + d.naconmax, + ], + outputs=[ + d.contact.dist, + d.contact.pos, + d.contact.frame, + d.contact.includemargin, + d.contact.friction, + d.contact.solref, + d.contact.solreffriction, + d.contact.solimp, + d.contact.dim, + d.contact.geom, + d.contact.flex, + d.contact.vert, + d.contact.worldid, + d.contact.type, + d.contact.geomcollisionid, + d.nacon, + ], + ) + + wp.launch( + _flex_plane_narrowphase, + dim=(d.nworld, m.nflexvert), + inputs=[ + m.ngeom, + m.nflexvert, + m.geom_type, + m.geom_condim, + m.geom_solref, + m.geom_solimp, + m.geom_friction, + m.geom_margin, + m.flex_condim, + m.flex_friction, + m.flex_margin, + m.flex_vertadr, + m.flex_radius, + m.flex_vertflexid, + d.geom_xpos, + d.geom_xmat, + d.flexvert_xpos, + d.nworld, + d.naconmax, + ], + outputs=[ + d.contact.dist, + d.contact.pos, + d.contact.frame, + d.contact.includemargin, + d.contact.friction, + d.contact.solref, + d.contact.solreffriction, + d.contact.solimp, + d.contact.dim, + d.contact.geom, + d.contact.flex, + d.contact.vert, + d.contact.worldid, + d.contact.type, + d.contact.geomcollisionid, + d.nacon, + ], + ) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_gjk.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_gjk.py index 7e4948ed..fe1c4445 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_gjk.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_gjk.py @@ -16,11 +16,12 @@ import math from typing import Tuple +import warp as wp + from mujoco.mjx.third_party.mujoco_warp._src.collision_core import Geom from mujoco.mjx.third_party.mujoco_warp._src.types import GeomType from mujoco.mjx.third_party.mujoco_warp._src.types import mat43 from mujoco.mjx.third_party.mujoco_warp._src.types import mat63 -import warp as wp # TODO(team): improve compile time to enable backward pass wp.set_module_options({"enable_backward": False}) @@ -581,16 +582,19 @@ def gjk( simplex_index2 = wp.vec4i() n = int(0) coordinates = wp.vec4() # barycentric coordinates - epsilon = wp.where(is_discrete, 0.0, 0.5 * tolerance * tolerance) + tol2 = tolerance * tolerance + epsilon = wp.where(is_discrete, 0.0, 0.5 * tol2) # set initial guess x_k = x1_0 - x2_0 + xnorm_old = FLOAT_MAX - for k in range(gjk_iterations): + for _ in range(gjk_iterations): xnorm = wp.dot(x_k, x_k) # TODO(kbayes): determine new constant here - if xnorm < 1e-12: + if xnorm < tol2 or wp.abs(xnorm_old - xnorm) < tol2: break + xnorm_old = xnorm dir_neg = x_k / wp.sqrt(xnorm) # compute kth support point in geom1 @@ -663,13 +667,6 @@ def gjk( if n == 4: break - if k == gjk_iterations - 1: - wp.printf( - "Warning: opt.ccd_iterations, currently set to %d, needs to be" - " increased.\n", - gjk_iterations, - ) - result = GJKResult() # compute the approximate witness points @@ -1205,7 +1202,6 @@ def _is_invalid_face(face: int) -> bool: def _epa( # In: tolerance: float, - gjk_iterations: int, epa_iterations: int, pt: Polytope, geom1: Geom, @@ -1226,7 +1222,7 @@ def _epa( # so iterations must be cap to limit the number of generated vertices # (one new vertex per iteration) epa_iterations = wp.min(epa_iterations, 1000) - for k in range(epa_iterations): + for _ in range(epa_iterations): pidx = idx idx = int(-1) lower2 = float(FLOAT_MAX) @@ -1325,13 +1321,6 @@ def _epa( # clear horizon pt.nhorizon = 0 - if k == epa_iterations - 1: - wp.printf( - "Warning: opt.ccd_iterations, currently set to %d, needs to be" - " increased.\n", - gjk_iterations, - ) - # return from valid face if idx > -1: x1, x2, dist = _epa_witness(pt, geom1, geom2, geomtype1, geomtype2, idx) @@ -2347,7 +2336,7 @@ def ccd( if pt.status: return result.dist, 1, result.x1, result.x2, -1 - dist, x1, x2, idx = _epa(tolerance, gjk_iterations, epa_iterations, pt, geom1, geom2, geomtype1, geomtype2, is_discrete) + dist, x1, x2, idx = _epa(tolerance, epa_iterations, pt, geom1, geom2, geomtype1, geomtype2, is_discrete) if idx == -1: return FLOAT_MAX, 0, wp.vec3(), wp.vec3(), -1 diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive.py index c7a40514..f1829de4 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive.py @@ -15,9 +15,11 @@ from typing import Tuple +import warp as wp + from mujoco.mjx.third_party.mujoco_warp._src.collision_core import CollisionContext -from mujoco.mjx.third_party.mujoco_warp._src.collision_core import contact_params from mujoco.mjx.third_party.mujoco_warp._src.collision_core import Geom +from mujoco.mjx.third_party.mujoco_warp._src.collision_core import contact_params from mujoco.mjx.third_party.mujoco_warp._src.collision_core import geom_collision_pair from mujoco.mjx.third_party.mujoco_warp._src.collision_core import write_contact from mujoco.mjx.third_party.mujoco_warp._src.collision_primitive_core import box_box @@ -34,15 +36,14 @@ from mujoco.mjx.third_party.mujoco_warp._src.collision_primitive_core import sph from mujoco.mjx.third_party.mujoco_warp._src.collision_primitive_core import sphere_sphere from mujoco.mjx.third_party.mujoco_warp._src.math import make_frame from mujoco.mjx.third_party.mujoco_warp._src.math import upper_trid_index +from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXVAL from mujoco.mjx.third_party.mujoco_warp._src.types import Data from mujoco.mjx.third_party.mujoco_warp._src.types import GeomType -from mujoco.mjx.third_party.mujoco_warp._src.types import mat43 -from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXVAL from mujoco.mjx.third_party.mujoco_warp._src.types import Model +from mujoco.mjx.third_party.mujoco_warp._src.types import mat43 from mujoco.mjx.third_party.mujoco_warp._src.types import vec5 from mujoco.mjx.third_party.mujoco_warp._src.warp_util import cache_kernel from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope -import warp as wp wp.set_module_options({"enable_backward": False}) @@ -304,6 +305,7 @@ def plane_sphere_wrapper( contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -339,6 +341,7 @@ def plane_sphere_wrapper( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -374,6 +377,7 @@ def sphere_sphere_wrapper( contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -408,6 +412,7 @@ def sphere_sphere_wrapper( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -443,6 +448,7 @@ def sphere_capsule_wrapper( contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -480,6 +486,7 @@ def sphere_capsule_wrapper( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -515,6 +522,7 @@ def capsule_capsule_wrapper( contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -564,6 +572,7 @@ def capsule_capsule_wrapper( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -599,6 +608,7 @@ def plane_capsule_wrapper( contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -644,6 +654,7 @@ def plane_capsule_wrapper( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -679,6 +690,7 @@ def plane_ellipsoid_wrapper( contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -713,6 +725,7 @@ def plane_ellipsoid_wrapper( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -748,6 +761,7 @@ def plane_box_wrapper( contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -784,6 +798,7 @@ def plane_box_wrapper( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -819,6 +834,7 @@ def plane_convex_wrapper( contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -855,6 +871,7 @@ def plane_convex_wrapper( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -890,6 +907,7 @@ def sphere_cylinder_wrapper( contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -934,6 +952,7 @@ def sphere_cylinder_wrapper( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -969,6 +988,7 @@ def plane_cylinder_wrapper( contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -1015,6 +1035,7 @@ def plane_cylinder_wrapper( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -1050,6 +1071,7 @@ def sphere_box_wrapper( contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -1083,6 +1105,7 @@ def sphere_box_wrapper( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -1118,6 +1141,7 @@ def capsule_box_wrapper( contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -1166,6 +1190,7 @@ def capsule_box_wrapper( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -1201,6 +1226,7 @@ def box_box_wrapper( contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -1245,6 +1271,7 @@ def box_box_wrapper( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -1327,6 +1354,7 @@ def _primitive_narrowphase(primitive_collisions_types, primitive_collisions_func contact_solimp_out: wp.array(dtype=vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), contact_geomcollisionid_out: wp.array(dtype=int), @@ -1416,6 +1444,7 @@ def _primitive_narrowphase(primitive_collisions_types, primitive_collisions_func contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -1511,6 +1540,7 @@ def primitive_narrowphase(m: Model, d: Data, ctx: CollisionContext, collision_ta d.contact.solimp, d.contact.dim, d.contact.geom, + d.contact.efc_address, d.contact.worldid, d.contact.type, d.contact.geomcollisionid, diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive_core.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive_core.py index 306a301e..8a85d241 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive_core.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive_core.py @@ -1489,3 +1489,507 @@ def capsule_box( mat23f(pos1[0], pos1[1], pos1[2], pos2[0], pos2[1], pos2[2]), mat23f(normal1[0], normal1[1], normal1[2], normal2[0], normal2[1], normal2[2]), ) + + +@wp.func +def _tri_area_sign(p1: wp.vec2, p2: wp.vec2, p3: wp.vec2) -> float: + """Sign of (signed) area of planar triangle.""" + return wp.sign((p1[0] - p3[0]) * (p2[1] - p3[1]) - (p2[0] - p3[0]) * (p1[1] - p3[1])) + + +@wp.func +def _tri_point_segment(p: wp.vec2, u: wp.vec2, v: wp.vec2) -> wp.vec2: + """Find nearest point to p within line segment (u, v).""" + uv = v - u + up = p - u + + denom = wp.max(MJ_MINVAL, wp.dot(uv, uv)) + a = wp.dot(uv, up) / denom + + if a <= 0.0: + return u + elif a >= 1.0: + return v + else: + return u + a * uv + + +@wp.func +def sphere_triangle( + sphere_pos: wp.vec3, + sphere_radius: float, + t1: wp.vec3, + t2: wp.vec3, + t3: wp.vec3, + tri_radius: float, +) -> Tuple[float, wp.vec3, wp.vec3]: + """Core contact geometry calculation for sphere-triangle collision. + + Port of mjraw_SphereTriangle from engine_collision_primitive.c + + Args: + sphere_pos: Center position of the sphere. + sphere_radius: Radius of the sphere. + t1: Triangle vertex positions. + t2: Triangle vertex positions. + t3: Triangle vertex positions. + tri_radius: Triangle (flex element) radius. + + Returns: + - Contact distance (MJ_MAXVAL if no collision). + - Contact position. + - Contact normal vector. + """ + S = sphere_pos - t1 + A = t2 - t1 + B = t3 - t1 + + N = wp.normalize(wp.cross(A, B)) + + dstS = wp.dot(N, S) + + P = S - dstS * N + + V1 = wp.normalize(A) + lenA = wp.length(A) + V2 = wp.normalize(wp.cross(N, A)) + + o = wp.vec2(0.0, 0.0) + a = wp.vec2(lenA, 0.0) + b = wp.vec2(wp.dot(V1, B), wp.dot(V2, B)) + p = wp.vec2(wp.dot(V1, P), wp.dot(V2, P)) + + sign1 = _tri_area_sign(p, o, a) + sign2 = _tri_area_sign(p, a, b) + sign3 = _tri_area_sign(p, b, o) + + X = wp.vec3(0.0) + if sign1 == sign2 and sign2 == sign3: + X = P + else: + x0 = _tri_point_segment(p, o, a) + x1 = _tri_point_segment(p, a, b) + x2 = _tri_point_segment(p, b, o) + + d0 = wp.length(p - x0) + d1 = wp.length(p - x1) + d2 = wp.length(p - x2) + + if d0 < d1 and d0 < d2: + X = x0[0] * V1 + x0[1] * V2 + elif d1 < d2: + X = x1[0] * V1 + x1[1] * V2 + else: + X = x2[0] * V1 + x2[1] * V2 + + nrm = X - S + dst = wp.length(nrm) + + if dst > MJ_MINVAL: + nrm = nrm / dst + else: + nrm = N + + dist = dst - sphere_radius - tri_radius + pos = sphere_pos + nrm * (sphere_radius + 0.5 * dist) + + return dist, pos, nrm + + +@wp.func +def box_triangle( + box_pos: wp.vec3, + box_rot: wp.mat33, + box_size: wp.vec3, + t1: wp.vec3, + t2: wp.vec3, + t3: wp.vec3, + tri_radius: float, +) -> Tuple[wp.vec2, mat23f, mat23f]: + """Core contact geometry calculation for box-triangle collision. + + Port of mjraw_BoxTriangle from engine_collision_primitive.c + + Args: + box_pos: Center position of the box. + box_rot: Orientation matrix of the box. + box_size: Half-sizes of the box. + t1: Triangle vertex positions. + t2: Triangle vertex positions. + t3: Triangle vertex positions. + tri_radius: Triangle (flex element) radius. + + Returns: + - wp.vec2 of distances for up to 2 contacts (MJ_MAXVAL if no collision). + - mat23f of contact positions (2 x vec3). + - mat23f of contact normals (2 x vec3). + """ + dist1 = MJ_MAXVAL + dist2 = MJ_MAXVAL + pos1 = wp.vec3(0.0) + pos2 = wp.vec3(0.0) + nrm1 = wp.vec3(0.0) + nrm2 = wp.vec3(0.0) + cnt = 0 + + box_rotT = wp.transpose(box_rot) + + for vi in range(3): + vert = wp.vec3(0.0) + if vi == 0: + vert = t1 + elif vi == 1: + vert = t2 + else: + vert = t3 + + diff = vert - box_pos + local = box_rotT @ diff + + maxaxis = 0 + maxval = wp.abs(local[0]) - box_size[0] + for j in range(1, 3): + val = wp.abs(local[j]) - box_size[j] + if val > maxval: + maxval = val + maxaxis = j + + inside = True + for j in range(3): + if wp.abs(local[j]) > box_size[j] + tri_radius: + inside = False + + if inside and cnt < 2: + nrm_local = wp.vec3(0.0) + if maxaxis == 0: + nrm_local = wp.vec3(wp.sign(local[0]), 0.0, 0.0) + elif maxaxis == 1: + nrm_local = wp.vec3(0.0, wp.sign(local[1]), 0.0) + else: + nrm_local = wp.vec3(0.0, 0.0, wp.sign(local[2])) + + nrm_global = box_rot @ nrm_local + d = maxval - tri_radius + offset = tri_radius + d * 0.5 + p = vert - nrm_global * offset + + if cnt == 0: + dist1 = d + pos1 = p + nrm1 = nrm_global + else: + dist2 = d + pos2 = p + nrm2 = nrm_global + cnt += 1 + + for i in range(8): + if cnt >= 2: + break + + vec = wp.vec3( + wp.where(i & 1, box_size[0], -box_size[0]), + wp.where(i & 2, box_size[1], -box_size[1]), + wp.where(i & 4, box_size[2], -box_size[2]), + ) + corner = box_rot @ vec + box_pos + + d, p, n = sphere_triangle(corner, 0.0, t1, t2, t3, tri_radius) + if d < MJ_MAXVAL: + if cnt == 0: + dist1 = d + pos1 = p + nrm1 = n + elif cnt == 1: + dist2 = d + pos2 = p + nrm2 = n + cnt += 1 + + return ( + wp.vec2(dist1, dist2), + mat23f(pos1[0], pos1[1], pos1[2], pos2[0], pos2[1], pos2[2]), + mat23f(nrm1[0], nrm1[1], nrm1[2], nrm2[0], nrm2[1], nrm2[2]), + ) + + +@wp.func +def capsule_triangle( + capsule_pos: wp.vec3, + capsule_axis: wp.vec3, + capsule_radius: float, + capsule_half_length: float, + t1: wp.vec3, + t2: wp.vec3, + t3: wp.vec3, + tri_radius: float, +) -> Tuple[wp.vec2, mat23f, mat23f]: + """Core contact geometry calculation for capsule-triangle collision. + + Port of mjraw_CapsuleTriangle from engine_collision_primitive.c + + Args: + capsule_pos: Center position of the capsule. + capsule_axis: Unit axis direction of the capsule. + capsule_radius: Radius of the capsule. + capsule_half_length: Half-length of the capsule cylinder. + t1: Triangle vertex positions. + t2: Triangle vertex positions. + t3: Triangle vertex positions. + tri_radius: Triangle (flex element) radius. + + Returns: + - wp.vec2 of distances for up to 2 contacts (MJ_MAXVAL if no collision). + - mat23f of contact positions (2 x vec3). + - mat23f of contact normals (2 x vec3). + """ + dist1 = MJ_MAXVAL + dist2 = MJ_MAXVAL + pos1 = wp.vec3(0.0) + pos2 = wp.vec3(0.0) + nrm1 = wp.vec3(0.0) + nrm2 = wp.vec3(0.0) + cnt = 0 + + p1 = capsule_pos - capsule_axis * capsule_half_length + p2 = capsule_pos + capsule_axis * capsule_half_length + + d, p, n = sphere_triangle(p1, capsule_radius, t1, t2, t3, tri_radius) + if d < MJ_MAXVAL: + dist1 = d + pos1 = p + nrm1 = n + cnt = 1 + + d, p, n = sphere_triangle(p2, capsule_radius, t1, t2, t3, tri_radius) + if d < MJ_MAXVAL and cnt < 2: + if cnt == 0: + dist1 = d + pos1 = p + nrm1 = n + else: + dist2 = d + pos2 = p + nrm2 = n + cnt += 1 + + ab = p2 - p1 + ab_len_sq = 4.0 * capsule_half_length * capsule_half_length + + for vi in range(3): + if cnt >= 2: + break + + vert = wp.vec3(0.0) + if vi == 0: + vert = t1 + elif vi == 1: + vert = t2 + else: + vert = t3 + + vec = vert - p1 + t_param = wp.dot(vec, ab) / wp.max(MJ_MINVAL, ab_len_sq) + + if t_param > MJ_MINVAL and t_param < 1.0 - MJ_MINVAL: + closest = p1 + ab * t_param + diff = vert - closest + dist_raw = wp.length(diff) + + if dist_raw > MJ_MINVAL: + nrm = diff / dist_raw + d = dist_raw - capsule_radius - tri_radius + p = (closest + vert + nrm * (capsule_radius - tri_radius)) * 0.5 + + if cnt == 0: + dist1 = d + pos1 = p + nrm1 = nrm + else: + dist2 = d + pos2 = p + nrm2 = nrm + cnt += 1 + + return ( + wp.vec2(dist1, dist2), + mat23f(pos1[0], pos1[1], pos1[2], pos2[0], pos2[1], pos2[2]), + mat23f(nrm1[0], nrm1[1], nrm1[2], nrm2[0], nrm2[1], nrm2[2]), + ) + + +@wp.func +def cylinder_triangle( + cylinder_pos: wp.vec3, + cylinder_axis: wp.vec3, + cylinder_radius: float, + cylinder_half_height: float, + t1: wp.vec3, + t2: wp.vec3, + t3: wp.vec3, + tri_radius: float, +) -> Tuple[wp.vec2, mat23f, mat23f]: + """Core contact geometry calculation for cylinder-triangle collision. + + Args: + cylinder_pos: Center position of the cylinder. + cylinder_axis: Unit axis direction of the cylinder. + cylinder_radius: Radius of the cylinder. + cylinder_half_height: Half-height of the cylinder. + t1: Triangle vertex positions. + t2: Triangle vertex positions. + t3: Triangle vertex positions. + tri_radius: Triangle (flex element) radius. + + Returns: + - wp.vec2 of distances for up to 2 contacts (MJ_MAXVAL if no collision). + - mat23f of contact positions (2 x vec3). + - mat23f of contact normals (2 x vec3). + """ + dist1 = MJ_MAXVAL + dist2 = MJ_MAXVAL + pos1 = wp.vec3(0.0) + pos2 = wp.vec3(0.0) + nrm1 = wp.vec3(0.0) + nrm2 = wp.vec3(0.0) + cnt = int(0) + + p1 = cylinder_pos - cylinder_axis * cylinder_half_height + p2 = cylinder_pos + cylinder_axis * cylinder_half_height + + ab = p2 - p1 + ab_len_sq = 4.0 * cylinder_half_height * cylinder_half_height + + for vi in range(3): + if cnt >= 2: + break + + vert = wp.vec3(0.0) + if vi == 0: + vert = t1 + elif vi == 1: + vert = t2 + else: + vert = t3 + + vec = vert - p1 + t_param = wp.dot(vec, ab) / wp.max(MJ_MINVAL, ab_len_sq) + + if t_param > MJ_MINVAL and t_param < 1.0 - MJ_MINVAL: + closest = p1 + ab * t_param + diff = vert - closest + dist_raw = wp.length(diff) + + if dist_raw < cylinder_radius + tri_radius: + if dist_raw > MJ_MINVAL: + nrm = diff / dist_raw + d = dist_raw - cylinder_radius - tri_radius + p = (closest + vert + nrm * (cylinder_radius - tri_radius)) * 0.5 + else: + dist_to_side = cylinder_radius + dist_to_p2 = (1.0 - t_param) * wp.sqrt(ab_len_sq) + dist_to_p1 = t_param * wp.sqrt(ab_len_sq) + + if dist_to_p2 < dist_to_side and dist_to_p2 < dist_to_p1: + nrm = cylinder_axis + d = -dist_to_p2 - tri_radius + p = vert + elif dist_to_p1 < dist_to_side: + nrm = -cylinder_axis + d = -dist_to_p1 - tri_radius + p = vert + else: + tri_normal = wp.normalize(wp.cross(t2 - t1, t3 - t1)) + nrm = tri_normal + d = -cylinder_radius - tri_radius + p = closest + + if cnt == 0: + dist1 = d + pos1 = p + nrm1 = nrm + else: + dist2 = d + pos2 = p + nrm2 = nrm + cnt += 1 + elif t_param <= MJ_MINVAL: + diff = vert - p1 + signed_dist = wp.dot(diff, cylinder_axis) + perp = diff - cylinder_axis * signed_dist + perp_len = wp.length(perp) + + if perp_len < cylinder_radius: + d = -signed_dist - tri_radius + nrm = -cylinder_axis + p = vert - nrm * (tri_radius + d * 0.5) + if cnt == 0: + dist1 = d + pos1 = p + nrm1 = nrm + else: + dist2 = d + pos2 = p + nrm2 = nrm + cnt += 1 + elif perp_len < cylinder_radius + tri_radius: + edge_dir = perp / perp_len + edge_point = p1 + edge_dir * cylinder_radius + diff_to_edge = vert - edge_point + dist_raw = wp.length(diff_to_edge) + if dist_raw > MJ_MINVAL: + nrm = diff_to_edge / dist_raw + d = dist_raw - tri_radius + p = vert - nrm * (tri_radius + d * 0.5) + if cnt == 0: + dist1 = d + pos1 = p + nrm1 = nrm + else: + dist2 = d + pos2 = p + nrm2 = nrm + cnt += 1 + else: + diff = vert - p2 + signed_dist = wp.dot(diff, cylinder_axis) + perp = diff - cylinder_axis * signed_dist + perp_len = wp.length(perp) + + if perp_len < cylinder_radius: + d = signed_dist - tri_radius + nrm = cylinder_axis + p = vert - nrm * (tri_radius + d * 0.5) + if cnt == 0: + dist1 = d + pos1 = p + nrm1 = nrm + else: + dist2 = d + pos2 = p + nrm2 = nrm + cnt += 1 + elif perp_len < cylinder_radius + tri_radius: + edge_dir = perp / perp_len + edge_point = p2 + edge_dir * cylinder_radius + diff_to_edge = vert - edge_point + dist_raw = wp.length(diff_to_edge) + if dist_raw > MJ_MINVAL: + nrm = diff_to_edge / dist_raw + d = dist_raw - tri_radius + p = vert - nrm * (tri_radius + d * 0.5) + if cnt == 0: + dist1 = d + pos1 = p + nrm1 = nrm + else: + dist2 = d + pos2 = p + nrm2 = nrm + cnt += 1 + + return ( + wp.vec2(dist1, dist2), + mat23f(pos1[0], pos1[1], pos1[2], pos2[0], pos2[1], pos2[2]), + mat23f(nrm1[0], nrm1[1], nrm1[2], nrm2[0], nrm2[1], nrm2[2]), + ) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_sdf.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_sdf.py index c84b67c8..4decf33c 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_sdf.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_sdf.py @@ -15,6 +15,8 @@ from typing import Tuple +import warp as wp + from mujoco.mjx.third_party.mujoco_warp._src.collision_core import CollisionContext from mujoco.mjx.third_party.mujoco_warp._src.collision_core import contact_params from mujoco.mjx.third_party.mujoco_warp._src.collision_core import geom_collision_pair @@ -27,9 +29,9 @@ from mujoco.mjx.third_party.mujoco_warp._src.types import Model from mujoco.mjx.third_party.mujoco_warp._src.types import vec5 from mujoco.mjx.third_party.mujoco_warp._src.types import vec8 from mujoco.mjx.third_party.mujoco_warp._src.types import vec8i +from mujoco.mjx.third_party.mujoco_warp._src.types import vec_pluginattr from mujoco.mjx.third_party.mujoco_warp._src.util_misc import halton from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope -import warp as wp wp.set_module_options({"enable_backward": False}) @@ -38,8 +40,8 @@ wp.set_module_options({"enable_backward": False}) class OptimizationParams: rel_mat: wp.mat33 rel_pos: wp.vec3 - attr1: wp.vec3 - attr2: wp.vec3 + attr1: vec_pluginattr + attr2: vec_pluginattr @wp.struct @@ -77,20 +79,24 @@ class MeshData: @wp.func def get_sdf_params( - # Model: - oct_child: wp.array(dtype=vec8i), - oct_aabb: wp.array2d(dtype=wp.vec3), - oct_coeff: wp.array(dtype=vec8), - mesh_octadr: wp.array(dtype=int), - plugin: wp.array(dtype=int), - plugin_attr: wp.array(dtype=wp.vec3f), - # In: - g_type: int, - g_size: wp.vec3, - plugin_id: int, - mesh_id: int, -) -> Tuple[wp.vec3, int, VolumeData, MeshData]: - attributes = g_size + # Model: + oct_child: wp.array(dtype=vec8i), + oct_aabb: wp.array2d(dtype=wp.vec3), + oct_coeff: wp.array(dtype=vec8), + mesh_octadr: wp.array(dtype=int), + plugin: wp.array(dtype=int), + plugin_attr: wp.array(dtype=vec_pluginattr), + # In: + g_type: int, + g_size: wp.vec3, + plugin_id: int, + mesh_id: int, +) -> Tuple[vec_pluginattr, int, VolumeData, MeshData]: + # default attributes from geom size, first 3 values copied + attributes = vec_pluginattr() + attributes[0] = g_size[0] + attributes[1] = g_size[1] + attributes[2] = g_size[2] plugin_index = -1 volume_data = VolumeData() @@ -108,6 +114,16 @@ def get_sdf_params( volume_data.oct_coeff = oct_coeff volume_data.valid = True + elif g_type == GeomType.MESH and mesh_id != -1 and mesh_octadr[mesh_id] != -1: + octadr = mesh_octadr[mesh_id] + volume_data.center = oct_aabb[octadr, 0] + volume_data.half_size = oct_aabb[octadr, 1] + volume_data.root = octadr + volume_data.oct_aabb = oct_aabb + volume_data.oct_child = oct_child + volume_data.oct_coeff = oct_coeff + volume_data.valid = True + return attributes, plugin_index, volume_data, MeshData() @@ -215,24 +231,28 @@ def grad_ellipsoid(p: wp.vec3, size: wp.vec3) -> wp.vec3: @wp.func -def user_sdf(p: wp.vec3, attr: wp.vec3, sdf_type: int) -> float: +def user_sdf(p: wp.vec3, attr: vec_pluginattr, sdf_type: int) -> float: + """User-defined SDF function. + + Access attributes via attr[i] where i is the attribute index (0 to _NPLUGINATTR-1). + """ wp.printf("ERROR: user_sdf function must be implemented by user code\n") return 0.0 @wp.func -def user_sdf_grad(p: wp.vec3, attr: wp.vec3, sdf_type: int) -> wp.vec3: +def user_sdf_grad(p: wp.vec3, attr: vec_pluginattr, sdf_type: int) -> wp.vec3: + """User-defined SDF gradient function. + + Access attributes via attr[i] where i is the attribute index (0 to _NPLUGINATTR-1). + """ wp.printf("ERROR: user_sdf_grad function must be implemented by user code\n") return wp.vec3(0.0) @wp.func def find_oct( - oct_child: wp.array(dtype=vec8i), - oct_aabb: wp.array2d(dtype=wp.vec3), - p: wp.vec3, - grad: bool, - root: int, + oct_child: wp.array(dtype=vec8i), oct_aabb: wp.array2d(dtype=wp.vec3), p: wp.vec3, grad: bool, root: int ) -> Tuple[int, Tuple[vec8, vec8, vec8]]: stack = root niter = int(100) @@ -268,14 +288,14 @@ def find_oct( # child indices are relative to root (mesh_octadr offset) child0 = oct_child[node][0] if ( - child0 == -1 - and oct_child[node][1] == -1 - and oct_child[node][2] == -1 - and oct_child[node][3] == -1 - and oct_child[node][4] == -1 - and oct_child[node][5] == -1 - and oct_child[node][6] == -1 - and oct_child[node][7] == -1 + child0 == -1 + and oct_child[node][1] == -1 + and oct_child[node][2] == -1 + and oct_child[node][3] == -1 + and oct_child[node][4] == -1 + and oct_child[node][5] == -1 + and oct_child[node][6] == -1 + and oct_child[node][7] == -1 ): for j in range(8): if not grad: @@ -342,13 +362,7 @@ def box_project(center: wp.vec3, half_size: wp.vec3, xyz: wp.vec3) -> Tuple[floa @wp.func def sample_volume_sdf(xyz: wp.vec3, volume_data: VolumeData) -> float: dist0, point = box_project(volume_data.center, volume_data.half_size, xyz) - node, weights = find_oct( - volume_data.oct_child, - volume_data.oct_aabb, - point, - grad=False, - root=volume_data.root, - ) + node, weights = find_oct(volume_data.oct_child, volume_data.oct_aabb, point, grad=False, root=volume_data.root) return dist0 + wp.dot(weights[0], volume_data.oct_coeff[node]) @@ -365,13 +379,7 @@ def sample_volume_grad(xyz: wp.vec3, volume_data: VolumeData) -> wp.vec3: grad_y = (sample_volume_sdf(xyz + dy, volume_data) - f) / h grad_z = (sample_volume_sdf(xyz + dz, volume_data) - f) / h return wp.vec3(grad_x, grad_y, grad_z) - node, weights = find_oct( - volume_data.oct_child, - volume_data.oct_aabb, - point, - grad=True, - root=volume_data.root, - ) + node, weights = find_oct(volume_data.oct_child, volume_data.oct_aabb, point, grad=True, root=volume_data.root) grad_x = wp.dot(weights[0], volume_data.oct_coeff[node]) grad_y = wp.dot(weights[1], volume_data.oct_coeff[node]) grad_z = wp.dot(weights[2], volume_data.oct_coeff[node]) @@ -379,15 +387,17 @@ def sample_volume_grad(xyz: wp.vec3, volume_data: VolumeData) -> wp.vec3: @wp.func -def sdf(type: int, p: wp.vec3, attr: wp.vec3, sdf_type: int, volume_data: VolumeData, mesh_data: MeshData) -> float: +def sdf(type: int, p: wp.vec3, attr: vec_pluginattr, sdf_type: int, volume_data: VolumeData, mesh_data: MeshData) -> float: + # extract first 3 elements as vec3 for primitive sdf functions + attr_vec3 = wp.vec3(attr[0], attr[1], attr[2]) if type == GeomType.PLANE: return p[2] elif type == GeomType.SPHERE: - return sphere(p, attr) + return sphere(p, attr_vec3) elif type == GeomType.BOX: - return box(p, attr) + return box(p, attr_vec3) elif type == GeomType.ELLIPSOID: - return ellipsoid(p, attr) + return ellipsoid(p, attr_vec3) elif type == GeomType.MESH and mesh_data.valid: mesh_data.pnt = p mesh_data.vec = -wp.normalize(p) @@ -425,21 +435,27 @@ def sdf(type: int, p: wp.vec3, attr: wp.vec3, sdf_type: int, volume_data: Volume return sample_volume_sdf(p, volume_data) else: return user_sdf(p, attr, sdf_type) + elif type == GeomType.MESH and volume_data.valid: + return sample_volume_sdf(p, volume_data) wp.printf("ERROR: SDF type not implemented\n") return 0.0 @wp.func -def sdf_grad(type: int, p: wp.vec3, attr: wp.vec3, sdf_type: int, volume_data: VolumeData, mesh_data: MeshData) -> wp.vec3: +def sdf_grad( + type: int, p: wp.vec3, attr: vec_pluginattr, sdf_type: int, volume_data: VolumeData, mesh_data: MeshData +) -> wp.vec3: + # extract first 3 elements as vec3 for primitive sdf functions + attr_vec3 = wp.vec3(attr[0], attr[1], attr[2]) if type == GeomType.PLANE: grad = wp.vec3(0.0, 0.0, 1.0) return grad elif type == GeomType.SPHERE: return grad_sphere(p) elif type == GeomType.BOX: - return grad_box(p, attr) + return grad_box(p, attr_vec3) elif type == GeomType.ELLIPSOID: - return grad_ellipsoid(p, attr) + return grad_ellipsoid(p, attr_vec3) elif type == GeomType.MESH and mesh_data.valid: mesh_data.pnt = p mesh_data.vec = -wp.normalize(p) @@ -466,6 +482,8 @@ def sdf_grad(type: int, p: wp.vec3, attr: wp.vec3, sdf_type: int, volume_data: V return sample_volume_grad(p, volume_data) else: return user_sdf_grad(p, attr, sdf_type) + elif type == GeomType.MESH and volume_data.valid: + return sample_volume_grad(p, volume_data) wp.printf("ERROR: SDF grad type not implemented\n") return wp.vec3(0.0) @@ -476,8 +494,8 @@ def clearance( type1: int, p1: wp.vec3, p2: wp.vec3, - s1: wp.vec3, - s2: wp.vec3, + s1: vec_pluginattr, + s2: vec_pluginattr, sdf_type1: int, sdf_type2: int, sfd_intersection: bool, @@ -606,8 +624,8 @@ def gradient_descent( # In: type1: int, x0_initial: wp.vec3, - attr1: wp.vec3, - attr2: wp.vec3, + attr1: vec_pluginattr, + attr2: vec_pluginattr, pos1: wp.vec3, rot1: wp.mat33, pos2: wp.vec3, @@ -645,76 +663,77 @@ def gradient_descent( @wp.kernel def _sdf_narrowphase( - # Model: - nmeshface: int, - oct_child: wp.array(dtype=vec8i), - oct_aabb: wp.array2d(dtype=wp.vec3), - oct_coeff: wp.array(dtype=vec8), - geom_type: wp.array(dtype=int), - geom_condim: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_priority: wp.array(dtype=int), - geom_solmix: wp.array2d(dtype=float), - geom_solref: wp.array2d(dtype=wp.vec2), - geom_solimp: wp.array2d(dtype=vec5), - geom_size: wp.array2d(dtype=wp.vec3), - geom_aabb: wp.array3d(dtype=wp.vec3), - geom_friction: wp.array2d(dtype=wp.vec3), - geom_margin: wp.array2d(dtype=float), - geom_gap: wp.array2d(dtype=float), - mesh_vertadr: wp.array(dtype=int), - mesh_vertnum: wp.array(dtype=int), - mesh_faceadr: wp.array(dtype=int), - mesh_octadr: wp.array(dtype=int), - mesh_graphadr: wp.array(dtype=int), - mesh_vert: wp.array(dtype=wp.vec3), - mesh_face: wp.array(dtype=wp.vec3i), - mesh_graph: wp.array(dtype=int), - mesh_polynum: wp.array(dtype=int), - mesh_polyadr: wp.array(dtype=int), - mesh_polynormal: wp.array(dtype=wp.vec3), - mesh_polyvertadr: wp.array(dtype=int), - mesh_polyvertnum: wp.array(dtype=int), - mesh_polyvert: wp.array(dtype=int), - mesh_polymapadr: wp.array(dtype=int), - mesh_polymapnum: wp.array(dtype=int), - mesh_polymap: wp.array(dtype=int), - pair_dim: wp.array(dtype=int), - pair_solref: wp.array2d(dtype=wp.vec2), - pair_solreffriction: wp.array2d(dtype=wp.vec2), - pair_solimp: wp.array2d(dtype=vec5), - pair_margin: wp.array2d(dtype=float), - pair_gap: wp.array2d(dtype=float), - pair_friction: wp.array2d(dtype=vec5), - plugin: wp.array(dtype=int), - plugin_attr: wp.array(dtype=wp.vec3f), - geom_plugin_index: wp.array(dtype=int), - # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - naconmax_in: int, - ncollision_in: wp.array(dtype=int), - # In: - collision_pair_in: wp.array(dtype=wp.vec2i), - collision_pairid_in: wp.array(dtype=wp.vec2i), - collision_worldid_in: wp.array(dtype=int), - sdf_initpoints: int, - sdf_iterations: int, - # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + # Model: + nmeshface: int, + oct_child: wp.array(dtype=vec8i), + oct_aabb: wp.array2d(dtype=wp.vec3), + oct_coeff: wp.array(dtype=vec8), + geom_type: wp.array(dtype=int), + geom_condim: wp.array(dtype=int), + geom_dataid: wp.array(dtype=int), + geom_priority: wp.array(dtype=int), + geom_solmix: wp.array2d(dtype=float), + geom_solref: wp.array2d(dtype=wp.vec2), + geom_solimp: wp.array2d(dtype=vec5), + geom_size: wp.array2d(dtype=wp.vec3), + geom_aabb: wp.array3d(dtype=wp.vec3), + geom_friction: wp.array2d(dtype=wp.vec3), + geom_margin: wp.array2d(dtype=float), + geom_gap: wp.array2d(dtype=float), + mesh_vertadr: wp.array(dtype=int), + mesh_vertnum: wp.array(dtype=int), + mesh_faceadr: wp.array(dtype=int), + mesh_octadr: wp.array(dtype=int), + mesh_graphadr: wp.array(dtype=int), + mesh_vert: wp.array(dtype=wp.vec3), + mesh_face: wp.array(dtype=wp.vec3i), + mesh_graph: wp.array(dtype=int), + mesh_polynum: wp.array(dtype=int), + mesh_polyadr: wp.array(dtype=int), + mesh_polynormal: wp.array(dtype=wp.vec3), + mesh_polyvertadr: wp.array(dtype=int), + mesh_polyvertnum: wp.array(dtype=int), + mesh_polyvert: wp.array(dtype=int), + mesh_polymapadr: wp.array(dtype=int), + mesh_polymapnum: wp.array(dtype=int), + mesh_polymap: wp.array(dtype=int), + pair_dim: wp.array(dtype=int), + pair_solref: wp.array2d(dtype=wp.vec2), + pair_solreffriction: wp.array2d(dtype=wp.vec2), + pair_solimp: wp.array2d(dtype=vec5), + pair_margin: wp.array2d(dtype=float), + pair_gap: wp.array2d(dtype=float), + pair_friction: wp.array2d(dtype=vec5), + plugin: wp.array(dtype=int), + plugin_attr: wp.array(dtype=vec_pluginattr), + geom_plugin_index: wp.array(dtype=int), + # Data in: + geom_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xmat_in: wp.array2d(dtype=wp.mat33), + naconmax_in: int, + ncollision_in: wp.array(dtype=int), + # In: + collision_pair_in: wp.array(dtype=wp.vec2i), + collision_pairid_in: wp.array(dtype=wp.vec2i), + collision_worldid_in: wp.array(dtype=int), + sdf_initpoints: int, + sdf_iterations: int, + # Data out: + contact_dist_out: wp.array(dtype=float), + contact_pos_out: wp.array(dtype=wp.vec3), + contact_frame_out: wp.array(dtype=wp.mat33), + contact_includemargin_out: wp.array(dtype=float), + contact_friction_out: wp.array(dtype=vec5), + contact_solref_out: wp.array(dtype=wp.vec2), + contact_solreffriction_out: wp.array(dtype=wp.vec2), + contact_solimp_out: wp.array(dtype=vec5), + contact_dim_out: wp.array(dtype=int), + contact_geom_out: wp.array(dtype=wp.vec2i), + contact_efc_address_out: wp.array2d(dtype=int), + contact_worldid_out: wp.array(dtype=int), + contact_type_out: wp.array(dtype=int), + contact_geomcollisionid_out: wp.array(dtype=int), + nacon_out: wp.array(dtype=int), ): i, contact_tid = wp.tid() if i >= sdf_initpoints: @@ -799,29 +818,11 @@ def _sdf_narrowphase( rot1 = geom1.rot attr1, g1_plugin_id, volume_data1, mesh_data1 = get_sdf_params( - oct_child, - oct_aabb, - oct_coeff, - mesh_octadr, - plugin, - plugin_attr, - type1, - geom1.size, - g1_plugin, - geom_dataid[g1], + oct_child, oct_aabb, oct_coeff, mesh_octadr, plugin, plugin_attr, type1, geom1.size, g1_plugin, geom_dataid[g1] ) attr2, g2_plugin_id, volume_data2, mesh_data2 = get_sdf_params( - oct_child, - oct_aabb, - oct_coeff, - mesh_octadr, - plugin, - plugin_attr, - type2, - geom2.size, - g2_plugin, - geom_dataid[g2], + oct_child, oct_aabb, oct_coeff, mesh_octadr, plugin, plugin_attr, type2, geom2.size, g2_plugin, geom_dataid[g2] ) mesh_data1.nmeshface = nmeshface @@ -900,6 +901,7 @@ def _sdf_narrowphase( contact_solimp_out, contact_dim_out, contact_geom_out, + contact_efc_address_out, contact_worldid_out, contact_type_out, contact_geomcollisionid_out, @@ -910,76 +912,77 @@ def _sdf_narrowphase( @event_scope def sdf_narrowphase(m: Model, d: Data, ctx: CollisionContext): wp.launch( - _sdf_narrowphase, - dim=(m.opt.sdf_initpoints, d.naconmax), - inputs=[ - m.nmeshface, - m.oct_child, - m.oct_aabb, - m.oct_coeff, - m.geom_type, - m.geom_condim, - m.geom_dataid, - m.geom_priority, - m.geom_solmix, - m.geom_solref, - m.geom_solimp, - m.geom_size, - m.geom_aabb, - m.geom_friction, - m.geom_margin, - m.geom_gap, - m.mesh_vertadr, - m.mesh_vertnum, - m.mesh_faceadr, - m.mesh_octadr, - m.mesh_graphadr, - m.mesh_vert, - m.mesh_face, - m.mesh_graph, - m.mesh_polynum, - m.mesh_polyadr, - m.mesh_polynormal, - m.mesh_polyvertadr, - m.mesh_polyvertnum, - m.mesh_polyvert, - m.mesh_polymapadr, - m.mesh_polymapnum, - m.mesh_polymap, - m.pair_dim, - m.pair_solref, - m.pair_solreffriction, - m.pair_solimp, - m.pair_margin, - m.pair_gap, - m.pair_friction, - m.plugin, - m.plugin_attr, - m.geom_plugin_index, - d.geom_xpos, - d.geom_xmat, - d.naconmax, - d.ncollision, - ctx.collision_pair, - ctx.collision_pairid, - ctx.collision_worldid, - m.opt.sdf_initpoints, - m.opt.sdf_iterations, - ], - outputs=[ - d.contact.dist, - d.contact.pos, - d.contact.frame, - d.contact.includemargin, - d.contact.friction, - d.contact.solref, - d.contact.solreffriction, - d.contact.solimp, - d.contact.dim, - d.contact.geom, - d.contact.worldid, - d.contact.type, - d.contact.geomcollisionid, - d.nacon, - ], + _sdf_narrowphase, + dim=(m.opt.sdf_initpoints, d.naconmax), + inputs=[ + m.nmeshface, + m.oct_child, + m.oct_aabb, + m.oct_coeff, + m.geom_type, + m.geom_condim, + m.geom_dataid, + m.geom_priority, + m.geom_solmix, + m.geom_solref, + m.geom_solimp, + m.geom_size, + m.geom_aabb, + m.geom_friction, + m.geom_margin, + m.geom_gap, + m.mesh_vertadr, + m.mesh_vertnum, + m.mesh_faceadr, + m.mesh_octadr, + m.mesh_graphadr, + m.mesh_vert, + m.mesh_face, + m.mesh_graph, + m.mesh_polynum, + m.mesh_polyadr, + m.mesh_polynormal, + m.mesh_polyvertadr, + m.mesh_polyvertnum, + m.mesh_polyvert, + m.mesh_polymapadr, + m.mesh_polymapnum, + m.mesh_polymap, + m.pair_dim, + m.pair_solref, + m.pair_solreffriction, + m.pair_solimp, + m.pair_margin, + m.pair_gap, + m.pair_friction, + m.plugin, + m.plugin_attr, + m.geom_plugin_index, + d.geom_xpos, + d.geom_xmat, + d.naconmax, + d.ncollision, + ctx.collision_pair, + ctx.collision_pairid, + ctx.collision_worldid, + m.opt.sdf_initpoints, + m.opt.sdf_iterations, + ], + outputs=[ + d.contact.dist, + d.contact.pos, + d.contact.frame, + d.contact.includemargin, + d.contact.friction, + d.contact.solref, + d.contact.solreffriction, + d.contact.solimp, + d.contact.dim, + d.contact.geom, + d.contact.efc_address, + d.contact.worldid, + d.contact.type, + d.contact.geomcollisionid, + d.nacon, + ], ) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/constraint.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/constraint.py index 228006e8..eec47583 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/constraint.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/constraint.py @@ -13,18 +13,18 @@ # limitations under the License. # ============================================================================== +import warp as wp + from mujoco.mjx.third_party.mujoco_warp._src import math from mujoco.mjx.third_party.mujoco_warp._src import support from mujoco.mjx.third_party.mujoco_warp._src import types from mujoco.mjx.third_party.mujoco_warp._src.types import ConstraintType from mujoco.mjx.third_party.mujoco_warp._src.types import ContactType from mujoco.mjx.third_party.mujoco_warp._src.types import DisableBit -from mujoco.mjx.third_party.mujoco_warp._src.types import SPARSE_CONSTRAINT_JACOBIAN -from mujoco.mjx.third_party.mujoco_warp._src.types import vec11 from mujoco.mjx.third_party.mujoco_warp._src.types import vec5 +from mujoco.mjx.third_party.mujoco_warp._src.types import vec11 from mujoco.mjx.third_party.mujoco_warp._src.warp_util import cache_kernel from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope -import warp as wp wp.set_module_options({"enable_backward": False}) @@ -36,6 +36,8 @@ def _zero_constraint_counts( nf_out: wp.array(dtype=int), nl_out: wp.array(dtype=int), nefc_out: wp.array(dtype=int), + # Out: + efc_nnz_out: wp.array(dtype=int), ): worldid = wp.tid() @@ -44,35 +46,36 @@ def _zero_constraint_counts( nf_out[worldid] = 0 nl_out[worldid] = 0 nefc_out[worldid] = 0 + efc_nnz_out[worldid] = 0 @wp.func def _efc_row( - # Model: - opt_disableflags: int, - # In: - worldid: int, - timestep: float, - efcid: int, - pos_aref: float, - pos_imp: float, - invweight: float, - solref: wp.vec2, - solimp: vec5, - margin: float, - vel: float, - frictionloss: float, - type: int, - id: int, - # Out: - type_out: wp.array2d(dtype=int), - id_out: wp.array2d(dtype=int), - pos_out: wp.array2d(dtype=float), - margin_out: wp.array2d(dtype=float), - D_out: wp.array2d(dtype=float), - vel_out: wp.array2d(dtype=float), - aref_out: wp.array2d(dtype=float), - frictionloss_out: wp.array2d(dtype=float), + # Model: + opt_disableflags: int, + # In: + worldid: int, + timestep: float, + efcid: int, + pos_aref: float, + pos_imp: float, + invweight: float, + solref: wp.vec2, + solimp: vec5, + margin: float, + vel: float, + frictionloss: float, + type: int, + id: int, + # Out: + type_out: wp.array2d(dtype=int), + id_out: wp.array2d(dtype=int), + pos_out: wp.array2d(dtype=float), + margin_out: wp.array2d(dtype=float), + D_out: wp.array2d(dtype=float), + vel_out: wp.array2d(dtype=float), + aref_out: wp.array2d(dtype=float), + frictionloss_out: wp.array2d(dtype=float), ): # calculate kbi timeconst = solref[0] @@ -108,9 +111,7 @@ def _efc_row( imp = wp.where(imp_x > 1.0, dmax, imp) # set outputs - D_out[worldid, efcid] = 1.0 / wp.max( - invweight * (1.0 - imp) / imp, types.MJ_MINVAL - ) + D_out[worldid, efcid] = 1.0 / wp.max(invweight * (1.0 - imp) / imp, types.MJ_MINVAL) vel_out[worldid, efcid] = vel aref_out[worldid, efcid] = -k * imp * pos_aref - b * vel pos_out[worldid, efcid] = pos_aref + margin @@ -122,52 +123,55 @@ def _efc_row( @wp.kernel def _equality_connect( - # Model: - nv: int, - nsite: int, - opt_timestep: wp.array(dtype=float), - opt_disableflags: int, - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - body_weldid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - body_invweight0: wp.array2d(dtype=wp.vec2), - dof_bodyid: wp.array(dtype=int), - dof_parentid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - eq_obj1id: wp.array(dtype=int), - eq_obj2id: wp.array(dtype=int), - eq_objtype: wp.array(dtype=int), - eq_solref: wp.array2d(dtype=wp.vec2), - eq_solimp: wp.array2d(dtype=vec5), - eq_data: wp.array2d(dtype=vec11), - is_sparse: bool, - eq_connect_adr: wp.array(dtype=int), - # Data in: - qvel_in: wp.array2d(dtype=float), - eq_active_in: wp.array2d(dtype=bool), - xpos_in: wp.array2d(dtype=wp.vec3), - xmat_in: wp.array2d(dtype=wp.mat33), - site_xpos_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), - njmax_in: int, - # Data out: - ne_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + # Model: + nv: int, + nsite: int, + opt_timestep: wp.array(dtype=float), + opt_disableflags: int, + body_parentid: wp.array(dtype=int), + body_rootid: wp.array(dtype=int), + body_weldid: wp.array(dtype=int), + body_dofnum: wp.array(dtype=int), + body_dofadr: wp.array(dtype=int), + body_invweight0: wp.array2d(dtype=wp.vec2), + dof_bodyid: wp.array(dtype=int), + dof_parentid: wp.array(dtype=int), + site_bodyid: wp.array(dtype=int), + eq_obj1id: wp.array(dtype=int), + eq_obj2id: wp.array(dtype=int), + eq_objtype: wp.array(dtype=int), + eq_solref: wp.array2d(dtype=wp.vec2), + eq_solimp: wp.array2d(dtype=vec5), + eq_data: wp.array2d(dtype=vec11), + is_sparse: bool, + eq_connect_adr: wp.array(dtype=int), + # Data in: + qvel_in: wp.array2d(dtype=float), + eq_active_in: wp.array2d(dtype=bool), + xpos_in: wp.array2d(dtype=wp.vec3), + xmat_in: wp.array2d(dtype=wp.mat33), + site_xpos_in: wp.array2d(dtype=wp.vec3), + subtree_com_in: wp.array2d(dtype=wp.vec3), + cdof_in: wp.array2d(dtype=wp.spatial_vector), + njmax_in: int, + njmax_nnz_in: int, + # Data out: + ne_out: wp.array(dtype=int), + nefc_out: wp.array(dtype=int), + efc_type_out: wp.array2d(dtype=int), + efc_id_out: wp.array2d(dtype=int), + efc_J_rownnz_out: wp.array2d(dtype=int), + efc_J_rowadr_out: wp.array2d(dtype=int), + efc_J_colind_out: wp.array3d(dtype=int), + efc_J_out: wp.array3d(dtype=float), + efc_pos_out: wp.array2d(dtype=float), + efc_margin_out: wp.array2d(dtype=float), + efc_D_out: wp.array2d(dtype=float), + efc_vel_out: wp.array2d(dtype=float), + efc_aref_out: wp.array2d(dtype=float), + efc_frictionloss_out: wp.array2d(dtype=float), + # Out: + efc_nnz_out: wp.array(dtype=int), ): """Calculates constraint rows for connect equality constraints.""" worldid, eqconnectid = wp.tid() @@ -182,6 +186,10 @@ def _equality_connect( if efcid >= njmax_in - 3: return + efcid0 = efcid + 0 + efcid1 = efcid + 1 + efcid2 = efcid + 2 + data = eq_data[worldid % eq_data.shape[0], eqid] anchor1 = wp.vec3f(data[0], data[1], data[2]) anchor2 = wp.vec3f(data[3], data[4], data[5]) @@ -207,26 +215,39 @@ def _equality_connect( Jqvel = wp.vec3f(0.0, 0.0, 0.0) if is_sparse: + # TODO(team): pre-compute number of non-zeros body1 = body_weldid[body1] body2 = body_weldid[body2] da1 = int(body_dofadr[body1] + body_dofnum[body1] - 1) da2 = int(body_dofadr[body2] + body_dofnum[body2] - 1) - efcid0 = efcid + 0 - efcid1 = efcid + 1 - efcid2 = efcid + 2 - - rowadr0 = efcid0 * nv - rowadr1 = efcid1 * nv - rowadr2 = efcid2 * nv - - efc_J_rowadr_out[worldid, efcid0] = rowadr0 - efc_J_rowadr_out[worldid, efcid1] = rowadr1 - efc_J_rowadr_out[worldid, efcid2] = rowadr2 - + # count non-zeros + pda1 = da1 + pda2 = da2 rownnz = int(0) + while pda1 >= 0 or pda2 >= 0: + da = wp.max(pda1, pda2) + if pda1 == da: + pda1 = dof_parentid[pda1] + if pda2 == da: + pda2 = dof_parentid[pda2] + rownnz += 1 + # get rowadr + rowadr = wp.atomic_add(efc_nnz_out, worldid, 3 * rownnz) + if rowadr + 3 * rownnz > njmax_nnz_in: + return + efc_J_rowadr_out[worldid, efcid0] = rowadr + efc_J_rowadr_out[worldid, efcid1] = rowadr + rownnz + efc_J_rowadr_out[worldid, efcid2] = rowadr + 2 * rownnz + + efc_J_rownnz_out[worldid, efcid0] = rownnz + efc_J_rownnz_out[worldid, efcid1] = rownnz + efc_J_rownnz_out[worldid, efcid2] = rownnz + + # compute J and colind + nnz = int(0) while da1 >= 0 or da2 >= 0: da = wp.max(da1, da2) if da1 == da: @@ -235,32 +256,32 @@ def _equality_connect( da2 = dof_parentid[da2] jacp1, _ = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - pos1, - body1, - da, - worldid, + body_parentid, + body_rootid, + dof_bodyid, + subtree_com_in, + cdof_in, + pos1, + body1, + da, + worldid, ) jacp2, _ = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - pos2, - body2, - da, - worldid, + body_parentid, + body_rootid, + dof_bodyid, + subtree_com_in, + cdof_in, + pos2, + body2, + da, + worldid, ) j1mj2 = jacp1 - jacp2 - sparseid0 = rowadr0 + rownnz - sparseid1 = rowadr1 + rownnz - sparseid2 = rowadr2 + rownnz + sparseid0 = rowadr + nnz + sparseid1 = rowadr + rownnz + nnz + sparseid2 = rowadr + 2 * rownnz + nnz efc_J_colind_out[worldid, 0, sparseid0] = da efc_J_colind_out[worldid, 0, sparseid1] = da @@ -272,49 +293,42 @@ def _equality_connect( Jqvel += j1mj2 * qvel_in[worldid, da] - rownnz += 1 - - efc_J_rownnz_out[worldid, efcid0] = rownnz - efc_J_rownnz_out[worldid, efcid1] = rownnz - efc_J_rownnz_out[worldid, efcid2] = rownnz + nnz += 1 else: # TODO(team): dof tree traversal for dofid in range(nv): jacp1, _ = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - pos1, - body1, - dofid, - worldid, + body_parentid, + body_rootid, + dof_bodyid, + subtree_com_in, + cdof_in, + pos1, + body1, + dofid, + worldid, ) jacp2, _ = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - pos2, - body2, - dofid, - worldid, + body_parentid, + body_rootid, + dof_bodyid, + subtree_com_in, + cdof_in, + pos2, + body2, + dofid, + worldid, ) j1mj2 = jacp1 - jacp2 - efc_J_out[worldid, efcid + 0, dofid] = j1mj2[0] - efc_J_out[worldid, efcid + 1, dofid] = j1mj2[1] - efc_J_out[worldid, efcid + 2, dofid] = j1mj2[2] + efc_J_out[worldid, efcid0, dofid] = j1mj2[0] + efc_J_out[worldid, efcid1, dofid] = j1mj2[1] + efc_J_out[worldid, efcid2, dofid] = j1mj2[2] Jqvel += j1mj2 * qvel_in[worldid, dofid] body_invweight0_id = worldid % body_invweight0.shape[0] - invweight = ( - body_invweight0[body_invweight0_id, body1][0] - + body_invweight0[body_invweight0_id, body2][0] - ) + invweight = body_invweight0[body_invweight0_id, body1][0] + body_invweight0[body_invweight0_id, body2][0] pos_imp = wp.length(pos) solref = eq_solref[worldid % eq_solref.shape[0], eqid] @@ -325,68 +339,71 @@ def _equality_connect( efcidi = efcid + i _efc_row( - opt_disableflags, - worldid, - timestep, - efcidi, - pos[i], - pos_imp, - invweight, - solref, - solimp, - 0.0, - Jqvel[i], - 0.0, - ConstraintType.EQUALITY, - eqid, - efc_type_out, - efc_id_out, - efc_pos_out, - efc_margin_out, - efc_D_out, - efc_vel_out, - efc_aref_out, - efc_frictionloss_out, + opt_disableflags, + worldid, + timestep, + efcidi, + pos[i], + pos_imp, + invweight, + solref, + solimp, + 0.0, + Jqvel[i], + 0.0, + ConstraintType.EQUALITY, + eqid, + efc_type_out, + efc_id_out, + efc_pos_out, + efc_margin_out, + efc_D_out, + efc_vel_out, + efc_aref_out, + efc_frictionloss_out, ) @wp.kernel def _equality_joint( - # Model: - nv: int, - opt_timestep: wp.array(dtype=float), - opt_disableflags: int, - qpos0: wp.array2d(dtype=float), - jnt_qposadr: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - dof_invweight0: wp.array2d(dtype=float), - eq_obj1id: wp.array(dtype=int), - eq_obj2id: wp.array(dtype=int), - eq_solref: wp.array2d(dtype=wp.vec2), - eq_solimp: wp.array2d(dtype=vec5), - eq_data: wp.array2d(dtype=vec11), - is_sparse: bool, - eq_jnt_adr: wp.array(dtype=int), - # Data in: - qpos_in: wp.array2d(dtype=float), - qvel_in: wp.array2d(dtype=float), - eq_active_in: wp.array2d(dtype=bool), - njmax_in: int, - # Data out: - ne_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + # Model: + nv: int, + opt_timestep: wp.array(dtype=float), + opt_disableflags: int, + qpos0: wp.array2d(dtype=float), + jnt_qposadr: wp.array(dtype=int), + jnt_dofadr: wp.array(dtype=int), + dof_invweight0: wp.array2d(dtype=float), + eq_obj1id: wp.array(dtype=int), + eq_obj2id: wp.array(dtype=int), + eq_solref: wp.array2d(dtype=wp.vec2), + eq_solimp: wp.array2d(dtype=vec5), + eq_data: wp.array2d(dtype=vec11), + is_sparse: bool, + eq_jnt_adr: wp.array(dtype=int), + # Data in: + qpos_in: wp.array2d(dtype=float), + qvel_in: wp.array2d(dtype=float), + eq_active_in: wp.array2d(dtype=bool), + njmax_in: int, + njmax_nnz_in: int, + # Data out: + ne_out: wp.array(dtype=int), + nefc_out: wp.array(dtype=int), + efc_type_out: wp.array2d(dtype=int), + efc_id_out: wp.array2d(dtype=int), + efc_J_rownnz_out: wp.array2d(dtype=int), + efc_J_rowadr_out: wp.array2d(dtype=int), + efc_J_colind_out: wp.array3d(dtype=int), + efc_J_out: wp.array3d(dtype=float), + efc_pos_out: wp.array2d(dtype=float), + efc_margin_out: wp.array2d(dtype=float), + efc_D_out: wp.array2d(dtype=float), + efc_vel_out: wp.array2d(dtype=float), + efc_aref_out: wp.array2d(dtype=float), + efc_frictionloss_out: wp.array2d(dtype=float), + # Out: + efc_nnz_out: wp.array(dtype=int), ): worldid, eqjntid = wp.tid() eqid = eq_jnt_adr[eqjntid] @@ -414,7 +431,9 @@ def _equality_joint( else: rownnz = 1 efc_J_rownnz_out[worldid, efcid] = rownnz - rowadr = efcid * nv + rowadr = wp.atomic_add(efc_nnz_out, worldid, rownnz) + if rowadr + rownnz > njmax_nnz_in: + return efc_J_rowadr_out[worldid, efcid] = rowadr efc_J_colind_out[worldid, 0, rowadr] = dofadr1 efc_J_out[worldid, 0, rowadr] = 1.0 @@ -430,19 +449,12 @@ def _equality_joint( dif = qpos_in[worldid, qposadr2] - qpos0[qpos0_id, qposadr2] # Horner's method for polynomials - rhs = data[0] + dif * ( - data[1] + dif * (data[2] + dif * (data[3] + dif * data[4])) - ) - deriv_2 = data[1] + dif * ( - 2.0 * data[2] + dif * (3.0 * data[3] + dif * 4.0 * data[4]) - ) + rhs = data[0] + dif * (data[1] + dif * (data[2] + dif * (data[3] + dif * data[4]))) + deriv_2 = data[1] + dif * (2.0 * data[2] + dif * (3.0 * data[3] + dif * 4.0 * data[4])) pos = qpos_in[worldid, qposadr1] - qpos0[qpos0_id, qposadr1] - rhs Jqvel = qvel_in[worldid, dofadr1] - qvel_in[worldid, dofadr2] * deriv_2 - invweight = ( - dof_invweight0[dof_invweight0_id, dofadr1] - + dof_invweight0[dof_invweight0_id, dofadr2] - ) + invweight = dof_invweight0[dof_invweight0_id, dofadr1] + dof_invweight0[dof_invweight0_id, dofadr2] if is_sparse: sparseid = rowadr + 1 @@ -458,67 +470,73 @@ def _equality_joint( # Update constraint parameters _efc_row( - opt_disableflags, - worldid, - opt_timestep[worldid % opt_timestep.shape[0]], - efcid, - pos, - pos, - invweight, - eq_solref[worldid % eq_solref.shape[0], eqid], - eq_solimp[worldid % eq_solimp.shape[0], eqid], - 0.0, - Jqvel, - 0.0, - ConstraintType.EQUALITY, - eqid, - efc_type_out, - efc_id_out, - efc_pos_out, - efc_margin_out, - efc_D_out, - efc_vel_out, - efc_aref_out, - efc_frictionloss_out, + opt_disableflags, + worldid, + opt_timestep[worldid % opt_timestep.shape[0]], + efcid, + pos, + pos, + invweight, + eq_solref[worldid % eq_solref.shape[0], eqid], + eq_solimp[worldid % eq_solimp.shape[0], eqid], + 0.0, + Jqvel, + 0.0, + ConstraintType.EQUALITY, + eqid, + efc_type_out, + efc_id_out, + efc_pos_out, + efc_margin_out, + efc_D_out, + efc_vel_out, + efc_aref_out, + efc_frictionloss_out, ) @wp.kernel def _equality_tendon( - # Model: - nv: int, - opt_timestep: wp.array(dtype=float), - opt_disableflags: int, - eq_obj1id: wp.array(dtype=int), - eq_obj2id: wp.array(dtype=int), - eq_solref: wp.array2d(dtype=wp.vec2), - eq_solimp: wp.array2d(dtype=vec5), - eq_data: wp.array2d(dtype=vec11), - tendon_length0: wp.array2d(dtype=float), - tendon_invweight0: wp.array2d(dtype=float), - is_sparse: bool, - eq_ten_adr: wp.array(dtype=int), - # Data in: - qvel_in: wp.array2d(dtype=float), - eq_active_in: wp.array2d(dtype=bool), - ten_J_in: wp.array3d(dtype=float), - ten_length_in: wp.array2d(dtype=float), - njmax_in: int, - # Data out: - ne_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + # Model: + nv: int, + opt_timestep: wp.array(dtype=float), + opt_disableflags: int, + eq_obj1id: wp.array(dtype=int), + eq_obj2id: wp.array(dtype=int), + eq_solref: wp.array2d(dtype=wp.vec2), + eq_solimp: wp.array2d(dtype=vec5), + eq_data: wp.array2d(dtype=vec11), + ten_J_rownnz: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), + tendon_length0: wp.array2d(dtype=float), + tendon_invweight0: wp.array2d(dtype=float), + is_sparse: bool, + eq_ten_adr: wp.array(dtype=int), + # Data in: + qvel_in: wp.array2d(dtype=float), + eq_active_in: wp.array2d(dtype=bool), + ten_J_in: wp.array2d(dtype=float), + ten_length_in: wp.array2d(dtype=float), + njmax_in: int, + njmax_nnz_in: int, + # Data out: + ne_out: wp.array(dtype=int), + nefc_out: wp.array(dtype=int), + efc_type_out: wp.array2d(dtype=int), + efc_id_out: wp.array2d(dtype=int), + efc_J_rownnz_out: wp.array2d(dtype=int), + efc_J_rowadr_out: wp.array2d(dtype=int), + efc_J_colind_out: wp.array3d(dtype=int), + efc_J_out: wp.array3d(dtype=float), + efc_pos_out: wp.array2d(dtype=float), + efc_margin_out: wp.array2d(dtype=float), + efc_D_out: wp.array2d(dtype=float), + efc_vel_out: wp.array2d(dtype=float), + efc_aref_out: wp.array2d(dtype=float), + efc_frictionloss_out: wp.array2d(dtype=float), + # Out: + efc_nnz_out: wp.array(dtype=int), ): worldid, eqtenid = wp.tid() eqid = eq_ten_adr[eqtenid] @@ -540,89 +558,118 @@ def _equality_tendon( solimp = eq_solimp[worldid % eq_solimp.shape[0], eqid] tendon_length0_id = worldid % tendon_length0.shape[0] tendon_invweight0_id = worldid % tendon_invweight0.shape[0] - pos1 = ( - ten_length_in[worldid, obj1id] - tendon_length0[tendon_length0_id, obj1id] - ) - jac1 = ten_J_in[worldid, obj1id] + pos1 = ten_length_in[worldid, obj1id] - tendon_length0[tendon_length0_id, obj1id] if obj2id > -1: - invweight = ( - tendon_invweight0[tendon_invweight0_id, obj1id] - + tendon_invweight0[tendon_invweight0_id, obj2id] - ) + invweight = tendon_invweight0[tendon_invweight0_id, obj1id] + tendon_invweight0[tendon_invweight0_id, obj2id] - pos2 = ( - ten_length_in[worldid, obj2id] - - tendon_length0[tendon_length0_id, obj2id] - ) - jac2 = ten_J_in[worldid, obj2id] + pos2 = ten_length_in[worldid, obj2id] - tendon_length0[tendon_length0_id, obj2id] dif = pos2 dif2 = dif * dif dif3 = dif2 * dif dif4 = dif3 * dif - pos = pos1 - ( - data[0] - + data[1] * dif - + data[2] * dif2 - + data[3] * dif3 - + data[4] * dif4 - ) - deriv = ( - data[1] - + 2.0 * data[2] * dif - + 3.0 * data[3] * dif2 - + 4.0 * data[4] * dif3 - ) + pos = pos1 - (data[0] + data[1] * dif + data[2] * dif2 + data[3] * dif3 + data[4] * dif4) + deriv = data[1] + 2.0 * data[2] * dif + 3.0 * data[3] * dif2 + 4.0 * data[4] * dif3 else: invweight = tendon_invweight0[tendon_invweight0_id, obj1id] pos = pos1 - data[0] deriv = 0.0 - Jqvel = float(0.0) + rownnz1 = ten_J_rownnz[obj1id] + rowadr1 = ten_J_rowadr[obj1id] + rownnz2 = 0 + rowadr2 = 0 + + if deriv != 0.0: + rownnz2 = ten_J_rownnz[obj2id] + rowadr2 = ten_J_rowadr[obj2id] - # TODO(team): sparse tendon jacobian if is_sparse: - rowadr = efcid * nv - efc_J_rownnz_out[worldid, efcid] = nv + # TODO(team): pre-compute rownnz + # count unique dofs + p1, p2 = int(0), int(0) + rownnz = int(0) + while p1 < rownnz1 or p2 < rownnz2: + col1 = nv + col2 = nv + if p1 < rownnz1: + col1 = ten_J_colind[rowadr1 + p1] + if p2 < rownnz2: + col2 = ten_J_colind[rowadr2 + p2] + if col1 <= col2: + p1 += 1 + if col2 <= col1: + p2 += 1 + rownnz += 1 + + rowadr = wp.atomic_add(efc_nnz_out, worldid, rownnz) + if rowadr + rownnz > njmax_nnz_in: + return efc_J_rowadr_out[worldid, efcid] = rowadr + ptr1 = int(0) + ptr2 = int(0) + + Jqvel = float(0.0) + + nnz = int(0) for i in range(nv): + J1 = float(0.0) + if ptr1 < rownnz1: + sparseid1 = rowadr1 + ptr1 + if ten_J_colind[sparseid1] == i: + J1 = ten_J_in[worldid, sparseid1] + ptr1 += 1 + + J = J1 if deriv != 0.0: - J = jac1[i] + jac2[i] * -deriv - else: - J = jac1[i] + J2 = float(0.0) + if ptr2 < rownnz2: + sparseid2 = rowadr2 + ptr2 + if ten_J_colind[sparseid2] == i: + J2 = ten_J_in[worldid, sparseid2] + ptr2 += 1 + J += J2 * -deriv + if is_sparse: - efc_J_colind_out[worldid, 0, rowadr + i] = i - efc_J_out[worldid, 0, rowadr + i] = J + if J != 0.0: + sparseid = rowadr + nnz + efc_J_colind_out[worldid, 0, sparseid] = i + efc_J_out[worldid, 0, sparseid] = J + nnz += 1 else: efc_J_out[worldid, efcid, i] = J + Jqvel += J * qvel_in[worldid, i] + if is_sparse: + efc_J_rownnz_out[worldid, efcid] = nnz + _efc_row( - opt_disableflags, - worldid, - opt_timestep[worldid % opt_timestep.shape[0]], - efcid, - pos, - pos, - invweight, - solref, - solimp, - 0.0, - Jqvel, - 0.0, - ConstraintType.EQUALITY, - eqid, - efc_type_out, - efc_id_out, - efc_pos_out, - efc_margin_out, - efc_D_out, - efc_vel_out, - efc_aref_out, - efc_frictionloss_out, + opt_disableflags, + worldid, + opt_timestep[worldid % opt_timestep.shape[0]], + efcid, + pos, + pos, + invweight, + solref, + solimp, + 0.0, + Jqvel, + 0.0, + ConstraintType.EQUALITY, + eqid, + efc_type_out, + efc_id_out, + efc_pos_out, + efc_margin_out, + efc_D_out, + efc_vel_out, + efc_aref_out, + efc_frictionloss_out, ) @@ -630,41 +677,50 @@ def _equality_tendon( def _equality_flex(is_sparse: bool): @wp.kernel(module="unique", enable_backward=False) def kernel( - # Model: - nv: int, - opt_timestep: wp.array(dtype=float), - opt_disableflags: int, - flexedge_length0: wp.array(dtype=float), - flexedge_invweight0: wp.array(dtype=float), - flexedge_J_rownnz: wp.array(dtype=int), - flexedge_J_rowadr: wp.array(dtype=int), - flexedge_J_colind: wp.array(dtype=int), - eq_solref: wp.array2d(dtype=wp.vec2), - eq_solimp: wp.array2d(dtype=vec5), - eq_flex_adr: wp.array(dtype=int), - # Data in: - qvel_in: wp.array2d(dtype=float), - flexedge_J_in: wp.array2d(dtype=float), - flexedge_length_in: wp.array2d(dtype=float), - njmax_in: int, - # Data out: - ne_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + # Model: + nv: int, + opt_timestep: wp.array(dtype=float), + opt_disableflags: int, + flex_edgeadr: wp.array(dtype=int), + flex_edgenum: wp.array(dtype=int), + flexedge_length0: wp.array(dtype=float), + flexedge_invweight0: wp.array(dtype=float), + flexedge_J_rownnz: wp.array(dtype=int), + flexedge_J_rowadr: wp.array(dtype=int), + flexedge_J_colind: wp.array(dtype=int), + eq_obj1id: wp.array(dtype=int), + eq_solref: wp.array2d(dtype=wp.vec2), + eq_solimp: wp.array2d(dtype=vec5), + eq_flex_adr: wp.array(dtype=int), + # Data in: + qvel_in: wp.array2d(dtype=float), + flexedge_J_in: wp.array2d(dtype=float), + flexedge_length_in: wp.array2d(dtype=float), + njmax_in: int, + njmax_nnz_in: int, + # Data out: + ne_out: wp.array(dtype=int), + nefc_out: wp.array(dtype=int), + efc_type_out: wp.array2d(dtype=int), + efc_id_out: wp.array2d(dtype=int), + efc_J_rownnz_out: wp.array2d(dtype=int), + efc_J_rowadr_out: wp.array2d(dtype=int), + efc_J_colind_out: wp.array3d(dtype=int), + efc_J_out: wp.array3d(dtype=float), + efc_pos_out: wp.array2d(dtype=float), + efc_margin_out: wp.array2d(dtype=float), + efc_D_out: wp.array2d(dtype=float), + efc_vel_out: wp.array2d(dtype=float), + efc_aref_out: wp.array2d(dtype=float), + efc_frictionloss_out: wp.array2d(dtype=float), + # Out: + efc_nnz_out: wp.array(dtype=int), ): worldid, eqflexid, edgeid = wp.tid() eqid = eq_flex_adr[eqflexid] + flexid = eq_obj1id[eqid] + if edgeid < flex_edgeadr[flexid] or edgeid >= flex_edgeadr[flexid] + flex_edgenum[flexid]: + return wp.atomic_add(ne_out, worldid, 1) efcid = wp.atomic_add(nefc_out, worldid, 1) @@ -683,7 +739,9 @@ def _equality_flex(is_sparse: bool): if wp.static(is_sparse): efc_J_rownnz_out[worldid, efcid] = rownnz - efc_rowadr = efcid * nv + efc_rowadr = wp.atomic_add(efc_nnz_out, worldid, rownnz) + if efc_rowadr + rownnz > njmax_nnz_in: + return efc_J_rowadr_out[worldid, efcid] = efc_rowadr for i in range(rownnz): flex_sparseid = flex_rowadr + i @@ -704,28 +762,28 @@ def _equality_flex(is_sparse: bool): Jqvel += J * qvel_in[worldid, colind] _efc_row( - opt_disableflags, - worldid, - opt_timestep[worldid % opt_timestep.shape[0]], - efcid, - pos, - pos, - flexedge_invweight0[edgeid], - solref, - solimp, - 0.0, - Jqvel, - 0.0, - ConstraintType.EQUALITY, - eqid, - efc_type_out, - efc_id_out, - efc_pos_out, - efc_margin_out, - efc_D_out, - efc_vel_out, - efc_aref_out, - efc_frictionloss_out, + opt_disableflags, + worldid, + opt_timestep[worldid % opt_timestep.shape[0]], + efcid, + pos, + pos, + flexedge_invweight0[edgeid], + solref, + solimp, + 0.0, + Jqvel, + 0.0, + ConstraintType.EQUALITY, + eqid, + efc_type_out, + efc_id_out, + efc_pos_out, + efc_margin_out, + efc_D_out, + efc_vel_out, + efc_aref_out, + efc_frictionloss_out, ) return kernel @@ -733,54 +791,57 @@ def _equality_flex(is_sparse: bool): @wp.kernel def _equality_weld( - # Model: - nv: int, - nsite: int, - opt_timestep: wp.array(dtype=float), - opt_disableflags: int, - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - body_weldid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - body_invweight0: wp.array2d(dtype=wp.vec2), - dof_bodyid: wp.array(dtype=int), - dof_parentid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - site_quat: wp.array2d(dtype=wp.quat), - eq_obj1id: wp.array(dtype=int), - eq_obj2id: wp.array(dtype=int), - eq_objtype: wp.array(dtype=int), - eq_solref: wp.array2d(dtype=wp.vec2), - eq_solimp: wp.array2d(dtype=vec5), - eq_data: wp.array2d(dtype=vec11), - is_sparse: bool, - eq_wld_adr: wp.array(dtype=int), - # Data in: - qvel_in: wp.array2d(dtype=float), - eq_active_in: wp.array2d(dtype=bool), - xpos_in: wp.array2d(dtype=wp.vec3), - xquat_in: wp.array2d(dtype=wp.quat), - xmat_in: wp.array2d(dtype=wp.mat33), - site_xpos_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), - njmax_in: int, - # Data out: - ne_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + # Model: + nv: int, + nsite: int, + opt_timestep: wp.array(dtype=float), + opt_disableflags: int, + body_parentid: wp.array(dtype=int), + body_rootid: wp.array(dtype=int), + body_weldid: wp.array(dtype=int), + body_dofnum: wp.array(dtype=int), + body_dofadr: wp.array(dtype=int), + body_invweight0: wp.array2d(dtype=wp.vec2), + dof_bodyid: wp.array(dtype=int), + dof_parentid: wp.array(dtype=int), + site_bodyid: wp.array(dtype=int), + site_quat: wp.array2d(dtype=wp.quat), + eq_obj1id: wp.array(dtype=int), + eq_obj2id: wp.array(dtype=int), + eq_objtype: wp.array(dtype=int), + eq_solref: wp.array2d(dtype=wp.vec2), + eq_solimp: wp.array2d(dtype=vec5), + eq_data: wp.array2d(dtype=vec11), + is_sparse: bool, + eq_wld_adr: wp.array(dtype=int), + # Data in: + qvel_in: wp.array2d(dtype=float), + eq_active_in: wp.array2d(dtype=bool), + xpos_in: wp.array2d(dtype=wp.vec3), + xquat_in: wp.array2d(dtype=wp.quat), + xmat_in: wp.array2d(dtype=wp.mat33), + site_xpos_in: wp.array2d(dtype=wp.vec3), + subtree_com_in: wp.array2d(dtype=wp.vec3), + cdof_in: wp.array2d(dtype=wp.spatial_vector), + njmax_in: int, + njmax_nnz_in: int, + # Data out: + ne_out: wp.array(dtype=int), + nefc_out: wp.array(dtype=int), + efc_type_out: wp.array2d(dtype=int), + efc_id_out: wp.array2d(dtype=int), + efc_J_rownnz_out: wp.array2d(dtype=int), + efc_J_rowadr_out: wp.array2d(dtype=int), + efc_J_colind_out: wp.array3d(dtype=int), + efc_J_out: wp.array3d(dtype=float), + efc_pos_out: wp.array2d(dtype=float), + efc_margin_out: wp.array2d(dtype=float), + efc_D_out: wp.array2d(dtype=float), + efc_vel_out: wp.array2d(dtype=float), + efc_aref_out: wp.array2d(dtype=float), + efc_frictionloss_out: wp.array2d(dtype=float), + # Out: + efc_nnz_out: wp.array(dtype=int), ): worldid, eqweldid = wp.tid() eqid = eq_wld_adr[eqweldid] @@ -794,6 +855,13 @@ def _equality_weld( if efcid >= njmax_in - 6: return + efcid0 = efcid + 0 + efcid1 = efcid + 1 + efcid2 = efcid + 2 + efcid3 = efcid + 3 + efcid4 = efcid + 4 + efcid5 = efcid + 5 + is_site = eq_objtype[eqid] == types.ObjType.SITE and nsite > 0 obj1id = eq_obj1id[eqid] @@ -812,12 +880,8 @@ def _equality_weld( pos2 = site_xpos_in[worldid, obj2id] site_quat_id = worldid % site_quat.shape[0] - quat = math.mul_quat( - xquat_in[worldid, body1], site_quat[site_quat_id, obj1id] - ) - quat1 = math.quat_inv( - math.mul_quat(xquat_in[worldid, body2], site_quat[site_quat_id, obj2id]) - ) + quat = math.mul_quat(xquat_in[worldid, body1], site_quat[site_quat_id, obj1id]) + quat1 = math.quat_inv(math.mul_quat(xquat_in[worldid, body2], site_quat[site_quat_id, obj2id])) else: body1 = obj1id @@ -833,35 +897,45 @@ def _equality_weld( Jqvelr = wp.vec3f(0.0, 0.0, 0.0) if is_sparse: + # TODO(team): pre-compute number of non-zeros body1 = body_weldid[body1] body2 = body_weldid[body2] da1 = int(body_dofadr[body1] + body_dofnum[body1] - 1) da2 = int(body_dofadr[body2] + body_dofnum[body2] - 1) - efcid0 = efcid + 0 - efcid1 = efcid + 1 - efcid2 = efcid + 2 - efcid3 = efcid + 3 - efcid4 = efcid + 4 - efcid5 = efcid + 5 - - rowadr0 = efcid0 * nv - rowadr1 = efcid1 * nv - rowadr2 = efcid2 * nv - rowadr3 = efcid3 * nv - rowadr4 = efcid4 * nv - rowadr5 = efcid5 * nv - - efc_J_rowadr_out[worldid, efcid0] = rowadr0 - efc_J_rowadr_out[worldid, efcid1] = rowadr1 - efc_J_rowadr_out[worldid, efcid2] = rowadr2 - efc_J_rowadr_out[worldid, efcid3] = rowadr3 - efc_J_rowadr_out[worldid, efcid4] = rowadr4 - efc_J_rowadr_out[worldid, efcid5] = rowadr5 - + # count non-zeros + pda1 = da1 + pda2 = da2 rownnz = int(0) + while pda1 >= 0 or pda2 >= 0: + da = wp.max(pda1, pda2) + if pda1 == da: + pda1 = dof_parentid[da] + if pda2 == da: + pda2 = dof_parentid[da] + rownnz += 1 + # get rowadr + rowadr = wp.atomic_add(efc_nnz_out, worldid, 6 * rownnz) + if rowadr + 6 * rownnz > njmax_nnz_in: + return + efc_J_rowadr_out[worldid, efcid0] = rowadr + efc_J_rowadr_out[worldid, efcid1] = rowadr + rownnz + efc_J_rowadr_out[worldid, efcid2] = rowadr + 2 * rownnz + efc_J_rowadr_out[worldid, efcid3] = rowadr + 3 * rownnz + efc_J_rowadr_out[worldid, efcid4] = rowadr + 4 * rownnz + efc_J_rowadr_out[worldid, efcid5] = rowadr + 5 * rownnz + + efc_J_rownnz_out[worldid, efcid0] = rownnz + efc_J_rownnz_out[worldid, efcid1] = rownnz + efc_J_rownnz_out[worldid, efcid2] = rownnz + efc_J_rownnz_out[worldid, efcid3] = rownnz + efc_J_rownnz_out[worldid, efcid4] = rownnz + efc_J_rownnz_out[worldid, efcid5] = rownnz + + # compute J and colind + nnz = int(0) while da1 >= 0 or da2 >= 0: da = wp.max(da1, da2) if da1 == da: @@ -870,26 +944,26 @@ def _equality_weld( da2 = dof_parentid[da] jacp1, jacr1 = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - pos1, - body1, - da, - worldid, + body_parentid, + body_rootid, + dof_bodyid, + subtree_com_in, + cdof_in, + pos1, + body1, + da, + worldid, ) jacp2, jacr2 = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - pos2, - body2, - da, - worldid, + body_parentid, + body_rootid, + dof_bodyid, + subtree_com_in, + cdof_in, + pos2, + body2, + da, + worldid, ) jacdifp = jacp1 - jacp2 @@ -898,12 +972,12 @@ def _equality_weld( jacdifrq = math.mul_quat(math.quat_mul_axis(quat1, jacdifr), quat) jacdifr = 0.5 * wp.vec3(jacdifrq[1], jacdifrq[2], jacdifrq[3]) - sparseid0 = rowadr0 + rownnz - sparseid1 = rowadr1 + rownnz - sparseid2 = rowadr2 + rownnz - sparseid3 = rowadr3 + rownnz - sparseid4 = rowadr4 + rownnz - sparseid5 = rowadr5 + rownnz + sparseid0 = rowadr + nnz + sparseid1 = rowadr + rownnz + nnz + sparseid2 = rowadr + 2 * rownnz + nnz + sparseid3 = rowadr + 3 * rownnz + nnz + sparseid4 = rowadr + 4 * rownnz + nnz + sparseid5 = rowadr + 5 * rownnz + nnz efc_J_colind_out[worldid, 0, sparseid0] = da efc_J_colind_out[worldid, 0, sparseid1] = da @@ -922,50 +996,45 @@ def _equality_weld( Jqvelp += jacdifp * qvel_in[worldid, da] Jqvelr += jacdifr * qvel_in[worldid, da] - rownnz += 1 - - efc_J_rownnz_out[worldid, efcid0] = rownnz - efc_J_rownnz_out[worldid, efcid1] = rownnz - efc_J_rownnz_out[worldid, efcid2] = rownnz - efc_J_rownnz_out[worldid, efcid3] = rownnz - efc_J_rownnz_out[worldid, efcid4] = rownnz - efc_J_rownnz_out[worldid, efcid5] = rownnz + nnz += 1 else: for dofid in range(nv): jacp1, jacr1 = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - pos1, - body1, - dofid, - worldid, + body_parentid, + body_rootid, + dof_bodyid, + subtree_com_in, + cdof_in, + pos1, + body1, + dofid, + worldid, ) jacp2, jacr2 = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - pos2, - body2, - dofid, - worldid, + body_parentid, + body_rootid, + dof_bodyid, + subtree_com_in, + cdof_in, + pos2, + body2, + dofid, + worldid, ) jacdifp = jacp1 - jacp2 - for i in range(3): - efc_J_out[worldid, efcid + i, dofid] = jacdifp[i] + efc_J_out[worldid, efcid0, dofid] = jacdifp[0] + efc_J_out[worldid, efcid1, dofid] = jacdifp[1] + efc_J_out[worldid, efcid2, dofid] = jacdifp[2] jacdifr = (jacr1 - jacr2) * torquescale jacdifrq = math.mul_quat(math.quat_mul_axis(quat1, jacdifr), quat) jacdifr = 0.5 * wp.vec3(jacdifrq[1], jacdifrq[2], jacdifrq[3]) - for i in range(3): - efc_J_out[worldid, efcid + 3 + i, dofid] = jacdifr[i] + efc_J_out[worldid, efcid3, dofid] = jacdifr[0] + efc_J_out[worldid, efcid4, dofid] = jacdifr[1] + efc_J_out[worldid, efcid5, dofid] = jacdifr[2] Jqvelp += jacdifp * qvel_in[worldid, dofid] Jqvelr += jacdifr * qvel_in[worldid, dofid] @@ -977,10 +1046,7 @@ def _equality_weld( crot = wp.vec3(crotq[1], crotq[2], crotq[3]) * torquescale body_invweight0_id = worldid % body_invweight0.shape[0] - invweight_t = ( - body_invweight0[body_invweight0_id, body1][0] - + body_invweight0[body_invweight0_id, body2][0] - ) + invweight_t = body_invweight0[body_invweight0_id, body1][0] + body_invweight0[body_invweight0_id, body2][0] pos_imp = wp.sqrt(wp.length_sq(cpos) + wp.length_sq(crot)) @@ -991,91 +1057,91 @@ def _equality_weld( for i in range(3): _efc_row( - opt_disableflags, - worldid, - timestep, - efcid + i, - cpos[i], - pos_imp, - invweight_t, - solref, - solimp, - 0.0, - Jqvelp[i], - 0.0, - ConstraintType.EQUALITY, - eqid, - efc_type_out, - efc_id_out, - efc_pos_out, - efc_margin_out, - efc_D_out, - efc_vel_out, - efc_aref_out, - efc_frictionloss_out, + opt_disableflags, + worldid, + timestep, + efcid + i, + cpos[i], + pos_imp, + invweight_t, + solref, + solimp, + 0.0, + Jqvelp[i], + 0.0, + ConstraintType.EQUALITY, + eqid, + efc_type_out, + efc_id_out, + efc_pos_out, + efc_margin_out, + efc_D_out, + efc_vel_out, + efc_aref_out, + efc_frictionloss_out, ) - invweight_r = ( - body_invweight0[body_invweight0_id, body1][1] - + body_invweight0[body_invweight0_id, body2][1] - ) + invweight_r = body_invweight0[body_invweight0_id, body1][1] + body_invweight0[body_invweight0_id, body2][1] for i in range(3): _efc_row( - opt_disableflags, - worldid, - timestep, - efcid + 3 + i, - crot[i], - pos_imp, - invweight_r, - solref, - solimp, - 0.0, - Jqvelr[i], - 0.0, - ConstraintType.EQUALITY, - eqid, - efc_type_out, - efc_id_out, - efc_pos_out, - efc_margin_out, - efc_D_out, - efc_vel_out, - efc_aref_out, - efc_frictionloss_out, + opt_disableflags, + worldid, + timestep, + efcid + 3 + i, + crot[i], + pos_imp, + invweight_r, + solref, + solimp, + 0.0, + Jqvelr[i], + 0.0, + ConstraintType.EQUALITY, + eqid, + efc_type_out, + efc_id_out, + efc_pos_out, + efc_margin_out, + efc_D_out, + efc_vel_out, + efc_aref_out, + efc_frictionloss_out, ) @wp.kernel def _friction_dof( - # Model: - nv: int, - opt_timestep: wp.array(dtype=float), - opt_disableflags: int, - dof_solref: wp.array2d(dtype=wp.vec2), - dof_solimp: wp.array2d(dtype=vec5), - dof_frictionloss: wp.array2d(dtype=float), - dof_invweight0: wp.array2d(dtype=float), - is_sparse: bool, - # Data in: - qvel_in: wp.array2d(dtype=float), - njmax_in: int, - # Data out: - nf_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + # Model: + nv: int, + opt_timestep: wp.array(dtype=float), + opt_disableflags: int, + dof_solref: wp.array2d(dtype=wp.vec2), + dof_solimp: wp.array2d(dtype=vec5), + dof_frictionloss: wp.array2d(dtype=float), + dof_invweight0: wp.array2d(dtype=float), + is_sparse: bool, + # Data in: + qvel_in: wp.array2d(dtype=float), + njmax_in: int, + njmax_nnz_in: int, + # Data out: + nf_out: wp.array(dtype=int), + nefc_out: wp.array(dtype=int), + efc_type_out: wp.array2d(dtype=int), + efc_id_out: wp.array2d(dtype=int), + efc_J_rownnz_out: wp.array2d(dtype=int), + efc_J_rowadr_out: wp.array2d(dtype=int), + efc_J_colind_out: wp.array3d(dtype=int), + efc_J_out: wp.array3d(dtype=float), + efc_pos_out: wp.array2d(dtype=float), + efc_margin_out: wp.array2d(dtype=float), + efc_D_out: wp.array2d(dtype=float), + efc_vel_out: wp.array2d(dtype=float), + efc_aref_out: wp.array2d(dtype=float), + efc_frictionloss_out: wp.array2d(dtype=float), + # Out: + efc_nnz_out: wp.array(dtype=int), ): worldid, dofid = wp.tid() @@ -1092,7 +1158,9 @@ def _friction_dof( if is_sparse: efc_J_rownnz_out[worldid, efcid] = 1 - rowadr = efcid * nv + rowadr = wp.atomic_add(efc_nnz_out, worldid, 1) + if rowadr + 1 > njmax_nnz_in: + return efc_J_rowadr_out[worldid, efcid] = rowadr efc_J_colind_out[worldid, 0, rowadr] = dofid efc_J_out[worldid, 0, rowadr] = 1.0 @@ -1107,61 +1175,67 @@ def _friction_dof( dof_solref_id = worldid % dof_solref.shape[0] dof_solimp_id = worldid % dof_solimp.shape[0] _efc_row( - opt_disableflags, - worldid, - opt_timestep[worldid % opt_timestep.shape[0]], - efcid, - 0.0, - 0.0, - dof_invweight0[dof_invweight0_id, dofid], - dof_solref[dof_solref_id, dofid], - dof_solimp[dof_solimp_id, dofid], - 0.0, - Jqvel, - dof_frictionloss[dof_frictionloss_id, dofid], - ConstraintType.FRICTION_DOF, - dofid, - efc_type_out, - efc_id_out, - efc_pos_out, - efc_margin_out, - efc_D_out, - efc_vel_out, - efc_aref_out, - efc_frictionloss_out, + opt_disableflags, + worldid, + opt_timestep[worldid % opt_timestep.shape[0]], + efcid, + 0.0, + 0.0, + dof_invweight0[dof_invweight0_id, dofid], + dof_solref[dof_solref_id, dofid], + dof_solimp[dof_solimp_id, dofid], + 0.0, + Jqvel, + dof_frictionloss[dof_frictionloss_id, dofid], + ConstraintType.FRICTION_DOF, + dofid, + efc_type_out, + efc_id_out, + efc_pos_out, + efc_margin_out, + efc_D_out, + efc_vel_out, + efc_aref_out, + efc_frictionloss_out, ) @wp.kernel def _friction_tendon( - # Model: - nv: int, - opt_timestep: wp.array(dtype=float), - opt_disableflags: int, - tendon_solref_fri: wp.array2d(dtype=wp.vec2), - tendon_solimp_fri: wp.array2d(dtype=vec5), - tendon_frictionloss: wp.array2d(dtype=float), - tendon_invweight0: wp.array2d(dtype=float), - is_sparse: bool, - # Data in: - qvel_in: wp.array2d(dtype=float), - ten_J_in: wp.array3d(dtype=float), - njmax_in: int, - # Data out: - nf_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + # Model: + nv: int, + opt_timestep: wp.array(dtype=float), + opt_disableflags: int, + ten_J_rownnz: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), + tendon_solref_fri: wp.array2d(dtype=wp.vec2), + tendon_solimp_fri: wp.array2d(dtype=vec5), + tendon_frictionloss: wp.array2d(dtype=float), + tendon_invweight0: wp.array2d(dtype=float), + is_sparse: bool, + # Data in: + qvel_in: wp.array2d(dtype=float), + ten_J_in: wp.array2d(dtype=float), + njmax_in: int, + njmax_nnz_in: int, + # Data out: + nf_out: wp.array(dtype=int), + nefc_out: wp.array(dtype=int), + efc_type_out: wp.array2d(dtype=int), + efc_id_out: wp.array2d(dtype=int), + efc_J_rownnz_out: wp.array2d(dtype=int), + efc_J_rowadr_out: wp.array2d(dtype=int), + efc_J_colind_out: wp.array3d(dtype=int), + efc_J_out: wp.array3d(dtype=float), + efc_pos_out: wp.array2d(dtype=float), + efc_margin_out: wp.array2d(dtype=float), + efc_D_out: wp.array2d(dtype=float), + efc_vel_out: wp.array2d(dtype=float), + efc_aref_out: wp.array2d(dtype=float), + efc_frictionloss_out: wp.array2d(dtype=float), + # Out: + efc_nnz_out: wp.array(dtype=int), ): worldid, tenid = wp.tid() @@ -1179,86 +1253,103 @@ def _friction_tendon( Jqvel = float(0.0) - # TODO(team): sparse tendon jacobian + rownnz_tenJ = ten_J_rownnz[tenid] + rowadr_tenJ = ten_J_rowadr[tenid] if is_sparse: - rowadr = efcid * nv - efc_J_rownnz_out[worldid, efcid] = nv - efc_J_rowadr_out[worldid, efcid] = rowadr + efc_J_rownnz_out[worldid, efcid] = rownnz_tenJ + rowadr_efc = wp.atomic_add(efc_nnz_out, worldid, rownnz_tenJ) + if rowadr_efc + rownnz_tenJ > njmax_nnz_in: + return + efc_J_rowadr_out[worldid, efcid] = rowadr_efc - for i in range(nv): - # TODO(team): sparse ten_J - J = ten_J_in[worldid, tenid, i] - if is_sparse: - efc_J_colind_out[worldid, 0, rowadr + i] = i - efc_J_out[worldid, 0, rowadr + i] = J - else: - efc_J_out[worldid, efcid, i] = J - - Jqvel += J * qvel_in[worldid, i] + for i in range(rownnz_tenJ): + sparseid_ten = rowadr_tenJ + i + sparseid_efc = rowadr_efc + i + colind = ten_J_colind[sparseid_ten] + J = ten_J_in[worldid, sparseid_ten] + efc_J_colind_out[worldid, 0, sparseid_efc] = colind + efc_J_out[worldid, 0, sparseid_efc] = J + Jqvel += J * qvel_in[worldid, colind] + else: + nnz = int(0) + colind = ten_J_colind[rowadr_tenJ] + for i in range(nv): + if nnz < rownnz_tenJ and i == colind: + J = ten_J_in[worldid, rowadr_tenJ + nnz] + efc_J_out[worldid, efcid, i] = J + Jqvel += J * qvel_in[worldid, i] + nnz += 1 + if nnz < rownnz_tenJ: + colind = ten_J_colind[rowadr_tenJ + nnz] + else: + efc_J_out[worldid, efcid, i] = 0.0 tendon_invweight0_id = worldid % tendon_invweight0.shape[0] tendon_solref_fri_id = worldid % tendon_solref_fri.shape[0] tendon_solimp_fri_id = worldid % tendon_solimp_fri.shape[0] _efc_row( - opt_disableflags, - worldid, - opt_timestep[worldid % opt_timestep.shape[0]], - efcid, - 0.0, - 0.0, - tendon_invweight0[tendon_invweight0_id, tenid], - tendon_solref_fri[tendon_solref_fri_id, tenid], - tendon_solimp_fri[tendon_solimp_fri_id, tenid], - 0.0, - Jqvel, - frictionloss, - ConstraintType.FRICTION_TENDON, - tenid, - efc_type_out, - efc_id_out, - efc_pos_out, - efc_margin_out, - efc_D_out, - efc_vel_out, - efc_aref_out, - efc_frictionloss_out, + opt_disableflags, + worldid, + opt_timestep[worldid % opt_timestep.shape[0]], + efcid, + 0.0, + 0.0, + tendon_invweight0[tendon_invweight0_id, tenid], + tendon_solref_fri[tendon_solref_fri_id, tenid], + tendon_solimp_fri[tendon_solimp_fri_id, tenid], + 0.0, + Jqvel, + frictionloss, + ConstraintType.FRICTION_TENDON, + tenid, + efc_type_out, + efc_id_out, + efc_pos_out, + efc_margin_out, + efc_D_out, + efc_vel_out, + efc_aref_out, + efc_frictionloss_out, ) @wp.kernel def _limit_slide_hinge( - # Model: - nv: int, - opt_timestep: wp.array(dtype=float), - opt_disableflags: int, - jnt_qposadr: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - jnt_solref: wp.array2d(dtype=wp.vec2), - jnt_solimp: wp.array2d(dtype=vec5), - jnt_range: wp.array2d(dtype=wp.vec2), - jnt_margin: wp.array2d(dtype=float), - dof_invweight0: wp.array2d(dtype=float), - is_sparse: bool, - jnt_limited_slide_hinge_adr: wp.array(dtype=int), - # Data in: - qpos_in: wp.array2d(dtype=float), - qvel_in: wp.array2d(dtype=float), - njmax_in: int, - # Data out: - nl_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + # Model: + nv: int, + opt_timestep: wp.array(dtype=float), + opt_disableflags: int, + jnt_qposadr: wp.array(dtype=int), + jnt_dofadr: wp.array(dtype=int), + jnt_solref: wp.array2d(dtype=wp.vec2), + jnt_solimp: wp.array2d(dtype=vec5), + jnt_range: wp.array2d(dtype=wp.vec2), + jnt_margin: wp.array2d(dtype=float), + dof_invweight0: wp.array2d(dtype=float), + is_sparse: bool, + jnt_limited_slide_hinge_adr: wp.array(dtype=int), + # Data in: + qpos_in: wp.array2d(dtype=float), + qvel_in: wp.array2d(dtype=float), + njmax_in: int, + njmax_nnz_in: int, + # Data out: + nl_out: wp.array(dtype=int), + nefc_out: wp.array(dtype=int), + efc_type_out: wp.array2d(dtype=int), + efc_id_out: wp.array2d(dtype=int), + efc_J_rownnz_out: wp.array2d(dtype=int), + efc_J_rowadr_out: wp.array2d(dtype=int), + efc_J_colind_out: wp.array3d(dtype=int), + efc_J_out: wp.array3d(dtype=float), + efc_pos_out: wp.array2d(dtype=float), + efc_margin_out: wp.array2d(dtype=float), + efc_D_out: wp.array2d(dtype=float), + efc_vel_out: wp.array2d(dtype=float), + efc_aref_out: wp.array2d(dtype=float), + efc_frictionloss_out: wp.array2d(dtype=float), + # Out: + efc_nnz_out: wp.array(dtype=int), ): worldid, jntlimitedid = wp.tid() jntid = jnt_limited_slide_hinge_adr[jntlimitedid] @@ -1285,7 +1376,9 @@ def _limit_slide_hinge( if is_sparse: efc_J_rownnz_out[worldid, efcid] = 1 - rowadr = efcid * nv + rowadr = wp.atomic_add(efc_nnz_out, worldid, 1) + if rowadr + 1 > njmax_nnz_in: + return efc_J_rowadr_out[worldid, efcid] = rowadr efc_J_colind_out[worldid, 0, rowadr] = dofadr efc_J_out[worldid, 0, rowadr] = J @@ -1300,65 +1393,68 @@ def _limit_slide_hinge( jnt_solref_id = worldid % jnt_solref.shape[0] jnt_solimp_id = worldid % jnt_solimp.shape[0] _efc_row( - opt_disableflags, - worldid, - opt_timestep[worldid % opt_timestep.shape[0]], - efcid, - pos, - pos, - dof_invweight0[dof_invweight0_id, dofadr], - jnt_solref[jnt_solref_id, jntid], - jnt_solimp[jnt_solimp_id, jntid], - jntmargin, - Jqvel, - 0.0, - ConstraintType.LIMIT_JOINT, - jntid, - efc_type_out, - efc_id_out, - efc_pos_out, - efc_margin_out, - efc_D_out, - efc_vel_out, - efc_aref_out, - efc_frictionloss_out, + opt_disableflags, + worldid, + opt_timestep[worldid % opt_timestep.shape[0]], + efcid, + pos, + pos, + dof_invweight0[dof_invweight0_id, dofadr], + jnt_solref[jnt_solref_id, jntid], + jnt_solimp[jnt_solimp_id, jntid], + jntmargin, + Jqvel, + 0.0, + ConstraintType.LIMIT_JOINT, + jntid, + efc_type_out, + efc_id_out, + efc_pos_out, + efc_margin_out, + efc_D_out, + efc_vel_out, + efc_aref_out, + efc_frictionloss_out, ) @wp.kernel def _limit_ball( - # Model: - nv: int, - opt_timestep: wp.array(dtype=float), - opt_disableflags: int, - jnt_qposadr: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - jnt_solref: wp.array2d(dtype=wp.vec2), - jnt_solimp: wp.array2d(dtype=vec5), - jnt_range: wp.array2d(dtype=wp.vec2), - jnt_margin: wp.array2d(dtype=float), - dof_invweight0: wp.array2d(dtype=float), - is_sparse: bool, - jnt_limited_ball_adr: wp.array(dtype=int), - # Data in: - qpos_in: wp.array2d(dtype=float), - qvel_in: wp.array2d(dtype=float), - njmax_in: int, - # Data out: - nl_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + # Model: + nv: int, + opt_timestep: wp.array(dtype=float), + opt_disableflags: int, + jnt_qposadr: wp.array(dtype=int), + jnt_dofadr: wp.array(dtype=int), + jnt_solref: wp.array2d(dtype=wp.vec2), + jnt_solimp: wp.array2d(dtype=vec5), + jnt_range: wp.array2d(dtype=wp.vec2), + jnt_margin: wp.array2d(dtype=float), + dof_invweight0: wp.array2d(dtype=float), + is_sparse: bool, + jnt_limited_ball_adr: wp.array(dtype=int), + # Data in: + qpos_in: wp.array2d(dtype=float), + qvel_in: wp.array2d(dtype=float), + njmax_in: int, + njmax_nnz_in: int, + # Data out: + nl_out: wp.array(dtype=int), + nefc_out: wp.array(dtype=int), + efc_type_out: wp.array2d(dtype=int), + efc_id_out: wp.array2d(dtype=int), + efc_J_rownnz_out: wp.array2d(dtype=int), + efc_J_rowadr_out: wp.array2d(dtype=int), + efc_J_colind_out: wp.array3d(dtype=int), + efc_J_out: wp.array3d(dtype=float), + efc_pos_out: wp.array2d(dtype=float), + efc_margin_out: wp.array2d(dtype=float), + efc_D_out: wp.array2d(dtype=float), + efc_vel_out: wp.array2d(dtype=float), + efc_aref_out: wp.array2d(dtype=float), + efc_frictionloss_out: wp.array2d(dtype=float), + # Out: + efc_nnz_out: wp.array(dtype=int), ): worldid, jntlimitedid = wp.tid() jntid = jnt_limited_ball_adr[jntlimitedid] @@ -1391,7 +1487,9 @@ def _limit_ball( if is_sparse: efc_J_rownnz_out[worldid, efcid] = 3 - rowadr = efcid * nv + rowadr = wp.atomic_add(efc_nnz_out, worldid, 3) + if rowadr + 3 > njmax_nnz_in: + return efc_J_rowadr_out[worldid, efcid] = rowadr sparseid0 = rowadr + 0 @@ -1420,69 +1518,70 @@ def _limit_ball( jnt_solref_id = worldid % jnt_solref.shape[0] jnt_solimp_id = worldid % jnt_solimp.shape[0] _efc_row( - opt_disableflags, - worldid, - opt_timestep[worldid % opt_timestep.shape[0]], - efcid, - pos, - pos, - dof_invweight0[dof_invweight0_id, dofadr], - jnt_solref[jnt_solref_id, jntid], - jnt_solimp[jnt_solimp_id, jntid], - jntmargin, - Jqvel, - 0.0, - ConstraintType.LIMIT_JOINT, - jntid, - efc_type_out, - efc_id_out, - efc_pos_out, - efc_margin_out, - efc_D_out, - efc_vel_out, - efc_aref_out, - efc_frictionloss_out, + opt_disableflags, + worldid, + opt_timestep[worldid % opt_timestep.shape[0]], + efcid, + pos, + pos, + dof_invweight0[dof_invweight0_id, dofadr], + jnt_solref[jnt_solref_id, jntid], + jnt_solimp[jnt_solimp_id, jntid], + jntmargin, + Jqvel, + 0.0, + ConstraintType.LIMIT_JOINT, + jntid, + efc_type_out, + efc_id_out, + efc_pos_out, + efc_margin_out, + efc_D_out, + efc_vel_out, + efc_aref_out, + efc_frictionloss_out, ) @wp.kernel def _limit_tendon( - # Model: - nv: int, - opt_timestep: wp.array(dtype=float), - opt_disableflags: int, - jnt_dofadr: wp.array(dtype=int), - tendon_adr: wp.array(dtype=int), - tendon_num: wp.array(dtype=int), - tendon_solref_lim: wp.array2d(dtype=wp.vec2), - tendon_solimp_lim: wp.array2d(dtype=vec5), - tendon_range: wp.array2d(dtype=wp.vec2), - tendon_margin: wp.array2d(dtype=float), - tendon_invweight0: wp.array2d(dtype=float), - wrap_type: wp.array(dtype=int), - wrap_objid: wp.array(dtype=int), - is_sparse: bool, - tendon_limited_adr: wp.array(dtype=int), - # Data in: - qvel_in: wp.array2d(dtype=float), - ten_J_in: wp.array3d(dtype=float), - ten_length_in: wp.array2d(dtype=float), - njmax_in: int, - # Data out: - nl_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + # Model: + nv: int, + opt_timestep: wp.array(dtype=float), + opt_disableflags: int, + ten_J_rownnz: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), + tendon_solref_lim: wp.array2d(dtype=wp.vec2), + tendon_solimp_lim: wp.array2d(dtype=vec5), + tendon_range: wp.array2d(dtype=wp.vec2), + tendon_margin: wp.array2d(dtype=float), + tendon_invweight0: wp.array2d(dtype=float), + is_sparse: bool, + tendon_limited_adr: wp.array(dtype=int), + # Data in: + qvel_in: wp.array2d(dtype=float), + ten_J_in: wp.array2d(dtype=float), + ten_length_in: wp.array2d(dtype=float), + njmax_in: int, + njmax_nnz_in: int, + # Data out: + nl_out: wp.array(dtype=int), + nefc_out: wp.array(dtype=int), + efc_type_out: wp.array2d(dtype=int), + efc_id_out: wp.array2d(dtype=int), + efc_J_rownnz_out: wp.array2d(dtype=int), + efc_J_rowadr_out: wp.array2d(dtype=int), + efc_J_colind_out: wp.array3d(dtype=int), + efc_J_out: wp.array3d(dtype=float), + efc_pos_out: wp.array2d(dtype=float), + efc_margin_out: wp.array2d(dtype=float), + efc_D_out: wp.array2d(dtype=float), + efc_vel_out: wp.array2d(dtype=float), + efc_aref_out: wp.array2d(dtype=float), + efc_frictionloss_out: wp.array2d(dtype=float), + # Out: + efc_nnz_out: wp.array(dtype=int), ): worldid, tenlimitedid = wp.tid() tenid = tendon_limited_adr[tenlimitedid] @@ -1506,122 +1605,123 @@ def _limit_tendon( Jqvel = float(0.0) scl = float(dist_min < dist_max) * 2.0 - 1.0 - # TODO(team): sparse tendon jacobian + rownnz_tenJ = ten_J_rownnz[tenid] + rowadr_tenJ = ten_J_rowadr[tenid] if is_sparse: - rowadr = efcid * nv - efc_J_rownnz_out[worldid, efcid] = nv - efc_J_rowadr_out[worldid, efcid] = rowadr - for i in range(nv): - efc_J_colind_out[worldid, 0, rowadr + i] = i - efc_J_out[worldid, 0, rowadr + i] = 0.0 + efc_J_rownnz_out[worldid, efcid] = rownnz_tenJ + rowadr_efc = wp.atomic_add(efc_nnz_out, worldid, rownnz_tenJ) + if rowadr_efc + rownnz_tenJ > njmax_nnz_in: + return + efc_J_rowadr_out[worldid, efcid] = rowadr_efc - adr = tendon_adr[tenid] - if wrap_type[adr] == types.WrapType.JOINT: - if not is_sparse: - for i in range(nv): - efc_J_out[worldid, efcid, i] = 0.0 - - ten_num = tendon_num[tenid] - for i in range(ten_num): - dofadr = jnt_dofadr[wrap_objid[adr + i]] - J = scl * ten_J_in[worldid, tenid, dofadr] - - if is_sparse: - efc_J_out[worldid, 0, rowadr + dofadr] = J - else: - efc_J_out[worldid, efcid, dofadr] = J - - Jqvel += J * qvel_in[worldid, dofadr] + for i in range(rownnz_tenJ): + sparseid_ten = rowadr_tenJ + i + sparseid_efc = rowadr_efc + i + colind = ten_J_colind[sparseid_ten] + J = scl * ten_J_in[worldid, sparseid_ten] + efc_J_colind_out[worldid, 0, sparseid_efc] = colind + efc_J_out[worldid, 0, sparseid_efc] = J + Jqvel += J * qvel_in[worldid, colind] else: + nnz = int(0) + colind = ten_J_colind[rowadr_tenJ] for i in range(nv): - J = scl * ten_J_in[worldid, tenid, i] - - if is_sparse: - efc_J_out[worldid, 0, rowadr + i] = J - else: + if nnz < rownnz_tenJ and i == colind: + J = scl * ten_J_in[worldid, rowadr_tenJ + nnz] efc_J_out[worldid, efcid, i] = J - - Jqvel += J * qvel_in[worldid, i] + Jqvel += J * qvel_in[worldid, i] + nnz += 1 + if nnz < rownnz_tenJ: + colind = ten_J_colind[rowadr_tenJ + nnz] + else: + efc_J_out[worldid, efcid, i] = 0.0 tendon_invweight0_id = worldid % tendon_invweight0.shape[0] tendon_solref_lim_id = worldid % tendon_solref_lim.shape[0] tendon_solimp_lim_id = worldid % tendon_solimp_lim.shape[0] _efc_row( - opt_disableflags, - worldid, - opt_timestep[worldid % opt_timestep.shape[0]], - efcid, - pos, - pos, - tendon_invweight0[tendon_invweight0_id, tenid], - tendon_solref_lim[tendon_solref_lim_id, tenid], - tendon_solimp_lim[tendon_solimp_lim_id, tenid], - tenmargin, - Jqvel, - 0.0, - ConstraintType.LIMIT_TENDON, - tenid, - efc_type_out, - efc_id_out, - efc_pos_out, - efc_margin_out, - efc_D_out, - efc_vel_out, - efc_aref_out, - efc_frictionloss_out, + opt_disableflags, + worldid, + opt_timestep[worldid % opt_timestep.shape[0]], + efcid, + pos, + pos, + tendon_invweight0[tendon_invweight0_id, tenid], + tendon_solref_lim[tendon_solref_lim_id, tenid], + tendon_solimp_lim[tendon_solimp_lim_id, tenid], + tenmargin, + Jqvel, + 0.0, + ConstraintType.LIMIT_TENDON, + tenid, + efc_type_out, + efc_id_out, + efc_pos_out, + efc_margin_out, + efc_D_out, + efc_vel_out, + efc_aref_out, + efc_frictionloss_out, ) @wp.kernel def _contact_pyramidal( - # Model: - nv: int, - opt_timestep: wp.array(dtype=float), - opt_disableflags: int, - opt_impratio_invsqrt: wp.array(dtype=float), - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - body_weldid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - body_invweight0: wp.array2d(dtype=wp.vec2), - dof_bodyid: wp.array(dtype=int), - dof_parentid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - is_sparse: bool, - # Data in: - qvel_in: wp.array2d(dtype=float), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), - njmax_in: int, - nacon_in: wp.array(dtype=int), - # In: - dist_in: wp.array(dtype=float), - condim_in: wp.array(dtype=int), - includemargin_in: wp.array(dtype=float), - worldid_in: wp.array(dtype=int), - geom_in: wp.array(dtype=wp.vec2i), - pos_in: wp.array(dtype=wp.vec3), - frame_in: wp.array(dtype=wp.mat33), - friction_in: wp.array(dtype=vec5), - solref_in: wp.array(dtype=wp.vec2), - solimp_in: wp.array(dtype=vec5), - type_in: wp.array(dtype=int), - # Data out: - nefc_out: wp.array(dtype=int), - contact_efc_address_out: wp.array2d(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + # Model: + nv: int, + opt_timestep: wp.array(dtype=float), + opt_disableflags: int, + opt_impratio_invsqrt: wp.array(dtype=float), + body_parentid: wp.array(dtype=int), + body_rootid: wp.array(dtype=int), + body_weldid: wp.array(dtype=int), + body_dofnum: wp.array(dtype=int), + body_dofadr: wp.array(dtype=int), + body_invweight0: wp.array2d(dtype=wp.vec2), + dof_bodyid: wp.array(dtype=int), + dof_parentid: wp.array(dtype=int), + geom_bodyid: wp.array(dtype=int), + flex_vertadr: wp.array(dtype=int), + flex_vertbodyid: wp.array(dtype=int), + is_sparse: bool, + # Data in: + qvel_in: wp.array2d(dtype=float), + subtree_com_in: wp.array2d(dtype=wp.vec3), + cdof_in: wp.array2d(dtype=wp.spatial_vector), + njmax_in: int, + njmax_nnz_in: int, + nacon_in: wp.array(dtype=int), + # In: + dist_in: wp.array(dtype=float), + condim_in: wp.array(dtype=int), + includemargin_in: wp.array(dtype=float), + worldid_in: wp.array(dtype=int), + geom_in: wp.array(dtype=wp.vec2i), + flex_in: wp.array(dtype=wp.vec2i), + vert_in: wp.array(dtype=wp.vec2i), + pos_in: wp.array(dtype=wp.vec3), + frame_in: wp.array(dtype=wp.mat33), + friction_in: wp.array(dtype=vec5), + solref_in: wp.array(dtype=wp.vec2), + solimp_in: wp.array(dtype=vec5), + type_in: wp.array(dtype=int), + # Data out: + nefc_out: wp.array(dtype=int), + contact_efc_address_out: wp.array2d(dtype=int), + efc_type_out: wp.array2d(dtype=int), + efc_id_out: wp.array2d(dtype=int), + efc_J_rownnz_out: wp.array2d(dtype=int), + efc_J_rowadr_out: wp.array2d(dtype=int), + efc_J_colind_out: wp.array3d(dtype=int), + efc_J_out: wp.array3d(dtype=float), + efc_pos_out: wp.array2d(dtype=float), + efc_margin_out: wp.array2d(dtype=float), + efc_D_out: wp.array2d(dtype=float), + efc_vel_out: wp.array2d(dtype=float), + efc_aref_out: wp.array2d(dtype=float), + efc_frictionloss_out: wp.array2d(dtype=float), + # Out: + efc_nnz_out: wp.array(dtype=int), ): conid, dimid = wp.tid() @@ -1655,8 +1755,20 @@ def _contact_pyramidal( contact_efc_address_out[conid, dimid] = efcid geom = geom_in[conid] - body1 = geom_bodyid[geom[0]] - body2 = geom_bodyid[geom[1]] + + if geom[0] >= 0: + body1 = geom_bodyid[geom[0]] + else: + flex = flex_in[conid] + vert = vert_in[conid] + body1 = flex_vertbodyid[flex_vertadr[flex[0]] + vert[0]] + + if geom[1] >= 0: + body2 = geom_bodyid[geom[1]] + else: + flex = flex_in[conid] + vert = vert_in[conid] + body2 = flex_vertbodyid[flex_vertadr[flex[1]] + vert[1]] con_pos = pos_in[conid] frame = frame_in[conid] @@ -1674,29 +1786,48 @@ def _contact_pyramidal( invweight = invweight + fri0 * fri0 * invweight invweight = invweight * 2.0 * fri0 * fri0 * impratio_invsqrt * impratio_invsqrt - if is_sparse: - rowadr = efcid * nv - efc_J_rowadr_out[worldid, efcid] = rowadr - Jqvel = float(0.0) # skip fixed bodies body1 = body_weldid[body1] body2 = body_weldid[body2] - da1 = body_dofadr[body1] + body_dofnum[body1] - 1 - da2 = body_dofadr[body2] + body_dofnum[body2] - 1 + da1 = int(body_dofadr[body1] + body_dofnum[body1] - 1) + da2 = int(body_dofadr[body2] + body_dofnum[body2] - 1) + + if is_sparse: + pda1 = da1 + pda2 = da2 + rownnz = int(0) + while pda1 >= 0 or pda2 >= 0: + da = wp.max(pda1, pda2) + # skip common dofs + if pda1 == da and pda2 == da: + break + if pda1 == da: + pda1 = dof_parentid[pda1] + if pda2 == da: + pda2 = dof_parentid[pda2] + rownnz += 1 + + # get rowadr + rowadr = wp.atomic_add(efc_nnz_out, worldid, rownnz) + if rowadr + rownnz > njmax_nnz_in: + return + efc_J_rowadr_out[worldid, efcid] = rowadr + efc_J_rownnz_out[worldid, efcid] = rownnz + da = wp.max(da1, da2) if is_sparse: - rownnz = int(0) + nnz = int(0) dofid = int(da) else: dofid = int(nv - 1) while True: if is_sparse: - if da1 < 0 and da2 < 0: + if nnz >= rownnz: break else: if dofid < 0: @@ -1749,13 +1880,15 @@ def _contact_pyramidal( J -= Ji * frii if is_sparse: - sparseid = rowadr + rownnz + sparseid = rowadr + nnz efc_J_colind_out[worldid, 0, sparseid] = dofid efc_J_out[worldid, 0, sparseid] = J - rownnz += 1 + nnz += 1 else: efc_J_out[worldid, efcid, dofid] = J Jqvel += J * qvel_in[worldid, dofid] + if is_sparse and nnz >= rownnz: + break # Advance tree pointers and recompute da for next iteration if da1 == da: @@ -1772,91 +1905,95 @@ def _contact_pyramidal( efc_J_out[worldid, efcid, dofid] = 0.0 dofid -= 1 - if is_sparse: - efc_J_rownnz_out[worldid, efcid] = rownnz - if condim == 1: efc_type = ConstraintType.CONTACT_FRICTIONLESS else: efc_type = ConstraintType.CONTACT_PYRAMIDAL _efc_row( - opt_disableflags, - worldid, - timestep, - efcid, - pos, - pos, - invweight, - solref_in[conid], - solimp_in[conid], - includemargin, - Jqvel, - 0.0, - efc_type, - conid, - efc_type_out, - efc_id_out, - efc_pos_out, - efc_margin_out, - efc_D_out, - efc_vel_out, - efc_aref_out, - efc_frictionloss_out, + opt_disableflags, + worldid, + timestep, + efcid, + pos, + pos, + invweight, + solref_in[conid], + solimp_in[conid], + includemargin, + Jqvel, + 0.0, + efc_type, + conid, + efc_type_out, + efc_id_out, + efc_pos_out, + efc_margin_out, + efc_D_out, + efc_vel_out, + efc_aref_out, + efc_frictionloss_out, ) @wp.kernel def _contact_elliptic( - # Model: - nv: int, - opt_timestep: wp.array(dtype=float), - opt_disableflags: int, - opt_impratio_invsqrt: wp.array(dtype=float), - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - body_weldid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - body_invweight0: wp.array2d(dtype=wp.vec2), - dof_bodyid: wp.array(dtype=int), - dof_parentid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - is_sparse: bool, - # Data in: - qvel_in: wp.array2d(dtype=float), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), - njmax_in: int, - nacon_in: wp.array(dtype=int), - # In: - dist_in: wp.array(dtype=float), - condim_in: wp.array(dtype=int), - includemargin_in: wp.array(dtype=float), - worldid_in: wp.array(dtype=int), - geom_in: wp.array(dtype=wp.vec2i), - pos_in: wp.array(dtype=wp.vec3), - frame_in: wp.array(dtype=wp.mat33), - friction_in: wp.array(dtype=vec5), - solref_in: wp.array(dtype=wp.vec2), - solreffriction_in: wp.array(dtype=wp.vec2), - solimp_in: wp.array(dtype=vec5), - type_in: wp.array(dtype=int), - # Data out: - nefc_out: wp.array(dtype=int), - contact_efc_address_out: wp.array2d(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + # Model: + nv: int, + opt_timestep: wp.array(dtype=float), + opt_disableflags: int, + opt_impratio_invsqrt: wp.array(dtype=float), + body_parentid: wp.array(dtype=int), + body_rootid: wp.array(dtype=int), + body_weldid: wp.array(dtype=int), + body_dofnum: wp.array(dtype=int), + body_dofadr: wp.array(dtype=int), + body_invweight0: wp.array2d(dtype=wp.vec2), + dof_bodyid: wp.array(dtype=int), + dof_parentid: wp.array(dtype=int), + geom_bodyid: wp.array(dtype=int), + flex_vertadr: wp.array(dtype=int), + flex_vertbodyid: wp.array(dtype=int), + is_sparse: bool, + # Data in: + qvel_in: wp.array2d(dtype=float), + subtree_com_in: wp.array2d(dtype=wp.vec3), + cdof_in: wp.array2d(dtype=wp.spatial_vector), + njmax_in: int, + njmax_nnz_in: int, + nacon_in: wp.array(dtype=int), + # In: + dist_in: wp.array(dtype=float), + condim_in: wp.array(dtype=int), + includemargin_in: wp.array(dtype=float), + worldid_in: wp.array(dtype=int), + geom_in: wp.array(dtype=wp.vec2i), + flex_in: wp.array(dtype=wp.vec2i), + vert_in: wp.array(dtype=wp.vec2i), + pos_in: wp.array(dtype=wp.vec3), + frame_in: wp.array(dtype=wp.mat33), + friction_in: wp.array(dtype=vec5), + solref_in: wp.array(dtype=wp.vec2), + solreffriction_in: wp.array(dtype=wp.vec2), + solimp_in: wp.array(dtype=vec5), + type_in: wp.array(dtype=int), + # Data out: + nefc_out: wp.array(dtype=int), + contact_efc_address_out: wp.array2d(dtype=int), + efc_type_out: wp.array2d(dtype=int), + efc_id_out: wp.array2d(dtype=int), + efc_J_rownnz_out: wp.array2d(dtype=int), + efc_J_rowadr_out: wp.array2d(dtype=int), + efc_J_colind_out: wp.array3d(dtype=int), + efc_J_out: wp.array3d(dtype=float), + efc_pos_out: wp.array2d(dtype=float), + efc_margin_out: wp.array2d(dtype=float), + efc_D_out: wp.array2d(dtype=float), + efc_vel_out: wp.array2d(dtype=float), + efc_aref_out: wp.array2d(dtype=float), + efc_frictionloss_out: wp.array2d(dtype=float), + # Out: + efc_nnz_out: wp.array(dtype=int), ): conid, dimid = wp.tid() @@ -1888,35 +2025,67 @@ def _contact_elliptic( contact_efc_address_out[conid, dimid] = efcid geom = geom_in[conid] - body1 = geom_bodyid[geom[0]] - body2 = geom_bodyid[geom[1]] + + if geom[0] >= 0: + body1 = geom_bodyid[geom[0]] + else: + flex = flex_in[conid] + vert = vert_in[conid] + body1 = flex_vertbodyid[flex_vertadr[flex[0]] + vert[0]] + + if geom[1] >= 0: + body2 = geom_bodyid[geom[1]] + else: + flex = flex_in[conid] + vert = vert_in[conid] + body2 = flex_vertbodyid[flex_vertadr[flex[1]] + vert[1]] con_pos = pos_in[conid] frame = frame_in[conid] - if is_sparse: - rowadr = efcid * nv - efc_J_rowadr_out[worldid, efcid] = rowadr - Jqvel = float(0.0) # skip fixed bodies body1 = body_weldid[body1] body2 = body_weldid[body2] - da1 = body_dofadr[body1] + body_dofnum[body1] - 1 - da2 = body_dofadr[body2] + body_dofnum[body2] - 1 + da1 = int(body_dofadr[body1] + body_dofnum[body1] - 1) + da2 = int(body_dofadr[body2] + body_dofnum[body2] - 1) + + if is_sparse: + # count non-zeros + pda1 = da1 + pda2 = da2 + rownnz = int(0) + while pda1 >= 0 or pda2 >= 0: + da = wp.max(pda1, pda2) + # skip common dofs + if pda1 == da and pda2 == da: + break + if pda1 == da: + pda1 = dof_parentid[pda1] + if pda2 == da: + pda2 = dof_parentid[pda2] + rownnz += 1 + + # get rowadr + rowadr = wp.atomic_add(efc_nnz_out, worldid, rownnz) + if rowadr + rownnz > njmax_nnz_in: + return + efc_J_rowadr_out[worldid, efcid] = rowadr + efc_J_rownnz_out[worldid, efcid] = rownnz + da = wp.max(da1, da2) if is_sparse: - rownnz = int(0) + nnz = int(0) dofid = int(da) else: dofid = int(nv - 1) while True: if is_sparse: - if da1 < 0 and da2 < 0: + if nnz >= rownnz: break else: if dofid < 0: @@ -1957,13 +2126,15 @@ def _contact_elliptic( J += frame[dimid - 3, xyz] * jac_dif if is_sparse: - sparseid = rowadr + rownnz + sparseid = rowadr + nnz efc_J_colind_out[worldid, 0, sparseid] = dofid efc_J_out[worldid, 0, sparseid] = J - rownnz += 1 + nnz += 1 else: efc_J_out[worldid, efcid, dofid] = J Jqvel += J * qvel_in[worldid, dofid] + if is_sparse and nnz >= rownnz: + break # Advance tree pointers and recompute da for next iteration if da1 == da: @@ -1980,9 +2151,6 @@ def _contact_elliptic( efc_J_out[worldid, efcid, dofid] = 0.0 dofid -= 1 - if is_sparse: - efc_J_rownnz_out[worldid, efcid] = rownnz - body_invweight0_id = worldid % body_invweight0.shape[0] invweight = body_invweight0[body_invweight0_id, body1][0] + body_invweight0[body_invweight0_id, body2][0] @@ -2013,561 +2181,599 @@ def _contact_elliptic( efc_type = ConstraintType.CONTACT_ELLIPTIC _efc_row( - opt_disableflags, - worldid, - timestep, - efcid, - pos_aref, - pos, - invweight, - ref, - solimp_in[conid], - includemargin, - Jqvel, - 0.0, - efc_type, - conid, - efc_type_out, - efc_id_out, - efc_pos_out, - efc_margin_out, - efc_D_out, - efc_vel_out, - efc_aref_out, - efc_frictionloss_out, + opt_disableflags, + worldid, + timestep, + efcid, + pos_aref, + pos, + invweight, + ref, + solimp_in[conid], + includemargin, + Jqvel, + 0.0, + efc_type, + conid, + efc_type_out, + efc_id_out, + efc_pos_out, + efc_margin_out, + efc_D_out, + efc_vel_out, + efc_aref_out, + efc_frictionloss_out, ) @event_scope def make_constraint(m: types.Model, d: types.Data): """Creates constraint jacobians and other supporting data.""" + efc_nnz = wp.empty((d.nworld,), dtype=int) + wp.launch( _zero_constraint_counts, dim=d.nworld, - inputs=[d.ne, d.nf, d.nl, d.nefc], + inputs=[d.ne, d.nf, d.nl, d.nefc, efc_nnz], ) - if types.SPARSE_CONSTRAINT_JACOBIAN: - d.contact.efc_address.fill_(-1) - if not (m.opt.disableflags & types.DisableBit.CONSTRAINT): if not (m.opt.disableflags & types.DisableBit.EQUALITY): wp.launch( - _equality_connect, - dim=(d.nworld, m.eq_connect_adr.size), - inputs=[ - m.nv, - m.nsite, - m.opt.timestep, - m.opt.disableflags, - m.body_parentid, - m.body_rootid, - m.body_weldid, - m.body_dofnum, - m.body_dofadr, - m.body_invweight0, - m.dof_bodyid, - m.dof_parentid, - m.site_bodyid, - m.eq_obj1id, - m.eq_obj2id, - m.eq_objtype, - m.eq_solref, - m.eq_solimp, - m.eq_data, - SPARSE_CONSTRAINT_JACOBIAN, - m.eq_connect_adr, - d.qvel, - d.eq_active, - d.xpos, - d.xmat, - d.site_xpos, - d.subtree_com, - d.cdof, - d.njmax, - ], - outputs=[ - d.ne, - d.nefc, - d.efc.type, - d.efc.id, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.pos, - d.efc.margin, - d.efc.D, - d.efc.vel, - d.efc.aref, - d.efc.frictionloss, - ], + _equality_connect, + dim=(d.nworld, m.eq_connect_adr.size), + inputs=[ + m.nv, + m.nsite, + m.opt.timestep, + m.opt.disableflags, + m.body_parentid, + m.body_rootid, + m.body_weldid, + m.body_dofnum, + m.body_dofadr, + m.body_invweight0, + m.dof_bodyid, + m.dof_parentid, + m.site_bodyid, + m.eq_obj1id, + m.eq_obj2id, + m.eq_objtype, + m.eq_solref, + m.eq_solimp, + m.eq_data, + m.is_sparse, + m.eq_connect_adr, + d.qvel, + d.eq_active, + d.xpos, + d.xmat, + d.site_xpos, + d.subtree_com, + d.cdof, + d.njmax, + d.njmax_nnz, + ], + outputs=[ + d.ne, + d.nefc, + d.efc.type, + d.efc.id, + d.efc.J_rownnz, + d.efc.J_rowadr, + d.efc.J_colind, + d.efc.J, + d.efc.pos, + d.efc.margin, + d.efc.D, + d.efc.vel, + d.efc.aref, + d.efc.frictionloss, + efc_nnz, + ], ) wp.launch( - _equality_weld, - dim=(d.nworld, m.eq_wld_adr.size), - inputs=[ - m.nv, - m.nsite, - m.opt.timestep, - m.opt.disableflags, - m.body_parentid, - m.body_rootid, - m.body_weldid, - m.body_dofnum, - m.body_dofadr, - m.body_invweight0, - m.dof_bodyid, - m.dof_parentid, - m.site_bodyid, - m.site_quat, - m.eq_obj1id, - m.eq_obj2id, - m.eq_objtype, - m.eq_solref, - m.eq_solimp, - m.eq_data, - SPARSE_CONSTRAINT_JACOBIAN, - m.eq_wld_adr, - d.qvel, - d.eq_active, - d.xpos, - d.xquat, - d.xmat, - d.site_xpos, - d.subtree_com, - d.cdof, - d.njmax, - ], - outputs=[ - d.ne, - d.nefc, - d.efc.type, - d.efc.id, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.pos, - d.efc.margin, - d.efc.D, - d.efc.vel, - d.efc.aref, - d.efc.frictionloss, - ], + _equality_weld, + dim=(d.nworld, m.eq_wld_adr.size), + inputs=[ + m.nv, + m.nsite, + m.opt.timestep, + m.opt.disableflags, + m.body_parentid, + m.body_rootid, + m.body_weldid, + m.body_dofnum, + m.body_dofadr, + m.body_invweight0, + m.dof_bodyid, + m.dof_parentid, + m.site_bodyid, + m.site_quat, + m.eq_obj1id, + m.eq_obj2id, + m.eq_objtype, + m.eq_solref, + m.eq_solimp, + m.eq_data, + m.is_sparse, + m.eq_wld_adr, + d.qvel, + d.eq_active, + d.xpos, + d.xquat, + d.xmat, + d.site_xpos, + d.subtree_com, + d.cdof, + d.njmax, + d.njmax_nnz, + ], + outputs=[ + d.ne, + d.nefc, + d.efc.type, + d.efc.id, + d.efc.J_rownnz, + d.efc.J_rowadr, + d.efc.J_colind, + d.efc.J, + d.efc.pos, + d.efc.margin, + d.efc.D, + d.efc.vel, + d.efc.aref, + d.efc.frictionloss, + efc_nnz, + ], ) wp.launch( - _equality_joint, - dim=(d.nworld, m.eq_jnt_adr.size), - inputs=[ - m.nv, - m.opt.timestep, - m.opt.disableflags, - m.qpos0, - m.jnt_qposadr, - m.jnt_dofadr, - m.dof_invweight0, - m.eq_obj1id, - m.eq_obj2id, - m.eq_solref, - m.eq_solimp, - m.eq_data, - SPARSE_CONSTRAINT_JACOBIAN, - m.eq_jnt_adr, - d.qpos, - d.qvel, - d.eq_active, - d.njmax, - ], - outputs=[ - d.ne, - d.nefc, - d.efc.type, - d.efc.id, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.pos, - d.efc.margin, - d.efc.D, - d.efc.vel, - d.efc.aref, - d.efc.frictionloss, - ], + _equality_joint, + dim=(d.nworld, m.eq_jnt_adr.size), + inputs=[ + m.nv, + m.opt.timestep, + m.opt.disableflags, + m.qpos0, + m.jnt_qposadr, + m.jnt_dofadr, + m.dof_invweight0, + m.eq_obj1id, + m.eq_obj2id, + m.eq_solref, + m.eq_solimp, + m.eq_data, + m.is_sparse, + m.eq_jnt_adr, + d.qpos, + d.qvel, + d.eq_active, + d.njmax, + d.njmax_nnz, + ], + outputs=[ + d.ne, + d.nefc, + d.efc.type, + d.efc.id, + d.efc.J_rownnz, + d.efc.J_rowadr, + d.efc.J_colind, + d.efc.J, + d.efc.pos, + d.efc.margin, + d.efc.D, + d.efc.vel, + d.efc.aref, + d.efc.frictionloss, + efc_nnz, + ], ) wp.launch( - _equality_tendon, - dim=(d.nworld, m.eq_ten_adr.size), - inputs=[ - m.nv, - m.opt.timestep, - m.opt.disableflags, - m.eq_obj1id, - m.eq_obj2id, - m.eq_solref, - m.eq_solimp, - m.eq_data, - m.tendon_length0, - m.tendon_invweight0, - SPARSE_CONSTRAINT_JACOBIAN, - m.eq_ten_adr, - d.qvel, - d.eq_active, - d.ten_J, - d.ten_length, - d.njmax, - ], - outputs=[ - d.ne, - d.nefc, - d.efc.type, - d.efc.id, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.pos, - d.efc.margin, - d.efc.D, - d.efc.vel, - d.efc.aref, - d.efc.frictionloss, - ], + _equality_tendon, + dim=(d.nworld, m.eq_ten_adr.size), + inputs=[ + m.nv, + m.opt.timestep, + m.opt.disableflags, + m.eq_obj1id, + m.eq_obj2id, + m.eq_solref, + m.eq_solimp, + m.eq_data, + m.ten_J_rownnz, + m.ten_J_rowadr, + m.ten_J_colind, + m.tendon_length0, + m.tendon_invweight0, + m.is_sparse, + m.eq_ten_adr, + d.qvel, + d.eq_active, + d.ten_J, + d.ten_length, + d.njmax, + d.njmax_nnz, + ], + outputs=[ + d.ne, + d.nefc, + d.efc.type, + d.efc.id, + d.efc.J_rownnz, + d.efc.J_rowadr, + d.efc.J_colind, + d.efc.J, + d.efc.pos, + d.efc.margin, + d.efc.D, + d.efc.vel, + d.efc.aref, + d.efc.frictionloss, + efc_nnz, + ], ) wp.launch( - _equality_flex(SPARSE_CONSTRAINT_JACOBIAN), - dim=(d.nworld, m.eq_flex_adr.size, m.nflexedge), - inputs=[ - m.nv, - m.opt.timestep, - m.opt.disableflags, - m.flexedge_length0, - m.flexedge_invweight0, - m.flexedge_J_rownnz, - m.flexedge_J_rowadr, - m.flexedge_J_colind, - m.eq_solref, - m.eq_solimp, - m.eq_flex_adr, - d.qvel, - d.flexedge_J, - d.flexedge_length, - d.njmax, - ], - outputs=[ - d.ne, - d.nefc, - d.efc.type, - d.efc.id, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.pos, - d.efc.margin, - d.efc.D, - d.efc.vel, - d.efc.aref, - d.efc.frictionloss, - ], + _equality_flex(m.is_sparse), + dim=(d.nworld, m.eq_flex_adr.size, m.nflexedge), + inputs=[ + m.nv, + m.opt.timestep, + m.opt.disableflags, + m.flex_edgeadr, + m.flex_edgenum, + m.flexedge_length0, + m.flexedge_invweight0, + m.flexedge_J_rownnz, + m.flexedge_J_rowadr, + m.flexedge_J_colind, + m.eq_obj1id, + m.eq_solref, + m.eq_solimp, + m.eq_flex_adr, + d.qvel, + d.flexedge_J, + d.flexedge_length, + d.njmax, + d.njmax_nnz, + ], + outputs=[ + d.ne, + d.nefc, + d.efc.type, + d.efc.id, + d.efc.J_rownnz, + d.efc.J_rowadr, + d.efc.J_colind, + d.efc.J, + d.efc.pos, + d.efc.margin, + d.efc.D, + d.efc.vel, + d.efc.aref, + d.efc.frictionloss, + efc_nnz, + ], ) if not (m.opt.disableflags & types.DisableBit.FRICTIONLOSS): wp.launch( - _friction_dof, - dim=(d.nworld, m.nv), - inputs=[ - m.nv, - m.opt.timestep, - m.opt.disableflags, - m.dof_solref, - m.dof_solimp, - m.dof_frictionloss, - m.dof_invweight0, - SPARSE_CONSTRAINT_JACOBIAN, - d.qvel, - d.njmax, - ], - outputs=[ - d.nf, - d.nefc, - d.efc.type, - d.efc.id, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.pos, - d.efc.margin, - d.efc.D, - d.efc.vel, - d.efc.aref, - d.efc.frictionloss, - ], + _friction_dof, + dim=(d.nworld, m.nv), + inputs=[ + m.nv, + m.opt.timestep, + m.opt.disableflags, + m.dof_solref, + m.dof_solimp, + m.dof_frictionloss, + m.dof_invweight0, + m.is_sparse, + d.qvel, + d.njmax, + d.njmax_nnz, + ], + outputs=[ + d.nf, + d.nefc, + d.efc.type, + d.efc.id, + d.efc.J_rownnz, + d.efc.J_rowadr, + d.efc.J_colind, + d.efc.J, + d.efc.pos, + d.efc.margin, + d.efc.D, + d.efc.vel, + d.efc.aref, + d.efc.frictionloss, + efc_nnz, + ], ) wp.launch( - _friction_tendon, - dim=(d.nworld, m.ntendon), - inputs=[ - m.nv, - m.opt.timestep, - m.opt.disableflags, - m.tendon_solref_fri, - m.tendon_solimp_fri, - m.tendon_frictionloss, - m.tendon_invweight0, - SPARSE_CONSTRAINT_JACOBIAN, - d.qvel, - d.ten_J, - d.njmax, - ], - outputs=[ - d.nf, - d.nefc, - d.efc.type, - d.efc.id, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.pos, - d.efc.margin, - d.efc.D, - d.efc.vel, - d.efc.aref, - d.efc.frictionloss, - ], + _friction_tendon, + dim=(d.nworld, m.ntendon), + inputs=[ + m.nv, + m.opt.timestep, + m.opt.disableflags, + m.ten_J_rownnz, + m.ten_J_rowadr, + m.ten_J_colind, + m.tendon_solref_fri, + m.tendon_solimp_fri, + m.tendon_frictionloss, + m.tendon_invweight0, + m.is_sparse, + d.qvel, + d.ten_J, + d.njmax, + d.njmax_nnz, + ], + outputs=[ + d.nf, + d.nefc, + d.efc.type, + d.efc.id, + d.efc.J_rownnz, + d.efc.J_rowadr, + d.efc.J_colind, + d.efc.J, + d.efc.pos, + d.efc.margin, + d.efc.D, + d.efc.vel, + d.efc.aref, + d.efc.frictionloss, + efc_nnz, + ], ) # limit if not (m.opt.disableflags & types.DisableBit.LIMIT): wp.launch( - _limit_ball, - dim=(d.nworld, m.jnt_limited_ball_adr.size), - inputs=[ - m.nv, - m.opt.timestep, - m.opt.disableflags, - m.jnt_qposadr, - m.jnt_dofadr, - m.jnt_solref, - m.jnt_solimp, - m.jnt_range, - m.jnt_margin, - m.dof_invweight0, - SPARSE_CONSTRAINT_JACOBIAN, - m.jnt_limited_ball_adr, - d.qpos, - d.qvel, - d.njmax, - ], - outputs=[ - d.nl, - d.nefc, - d.efc.type, - d.efc.id, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.pos, - d.efc.margin, - d.efc.D, - d.efc.vel, - d.efc.aref, - d.efc.frictionloss, - ], + _limit_ball, + dim=(d.nworld, m.jnt_limited_ball_adr.size), + inputs=[ + m.nv, + m.opt.timestep, + m.opt.disableflags, + m.jnt_qposadr, + m.jnt_dofadr, + m.jnt_solref, + m.jnt_solimp, + m.jnt_range, + m.jnt_margin, + m.dof_invweight0, + m.is_sparse, + m.jnt_limited_ball_adr, + d.qpos, + d.qvel, + d.njmax, + d.njmax_nnz, + ], + outputs=[ + d.nl, + d.nefc, + d.efc.type, + d.efc.id, + d.efc.J_rownnz, + d.efc.J_rowadr, + d.efc.J_colind, + d.efc.J, + d.efc.pos, + d.efc.margin, + d.efc.D, + d.efc.vel, + d.efc.aref, + d.efc.frictionloss, + efc_nnz, + ], ) wp.launch( - _limit_slide_hinge, - dim=(d.nworld, m.jnt_limited_slide_hinge_adr.size), - inputs=[ - m.nv, - m.opt.timestep, - m.opt.disableflags, - m.jnt_qposadr, - m.jnt_dofadr, - m.jnt_solref, - m.jnt_solimp, - m.jnt_range, - m.jnt_margin, - m.dof_invweight0, - SPARSE_CONSTRAINT_JACOBIAN, - m.jnt_limited_slide_hinge_adr, - d.qpos, - d.qvel, - d.njmax, - ], - outputs=[ - d.nl, - d.nefc, - d.efc.type, - d.efc.id, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.pos, - d.efc.margin, - d.efc.D, - d.efc.vel, - d.efc.aref, - d.efc.frictionloss, - ], + _limit_slide_hinge, + dim=(d.nworld, m.jnt_limited_slide_hinge_adr.size), + inputs=[ + m.nv, + m.opt.timestep, + m.opt.disableflags, + m.jnt_qposadr, + m.jnt_dofadr, + m.jnt_solref, + m.jnt_solimp, + m.jnt_range, + m.jnt_margin, + m.dof_invweight0, + m.is_sparse, + m.jnt_limited_slide_hinge_adr, + d.qpos, + d.qvel, + d.njmax, + d.njmax_nnz, + ], + outputs=[ + d.nl, + d.nefc, + d.efc.type, + d.efc.id, + d.efc.J_rownnz, + d.efc.J_rowadr, + d.efc.J_colind, + d.efc.J, + d.efc.pos, + d.efc.margin, + d.efc.D, + d.efc.vel, + d.efc.aref, + d.efc.frictionloss, + efc_nnz, + ], ) wp.launch( - _limit_tendon, - dim=(d.nworld, m.tendon_limited_adr.size), - inputs=[ - m.nv, - m.opt.timestep, - m.opt.disableflags, - m.jnt_dofadr, - m.tendon_adr, - m.tendon_num, - m.tendon_solref_lim, - m.tendon_solimp_lim, - m.tendon_range, - m.tendon_margin, - m.tendon_invweight0, - m.wrap_type, - m.wrap_objid, - SPARSE_CONSTRAINT_JACOBIAN, - m.tendon_limited_adr, - d.qvel, - d.ten_J, - d.ten_length, - d.njmax, - ], - outputs=[ - d.nl, - d.nefc, - d.efc.type, - d.efc.id, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.pos, - d.efc.margin, - d.efc.D, - d.efc.vel, - d.efc.aref, - d.efc.frictionloss, - ], + _limit_tendon, + dim=(d.nworld, m.tendon_limited_adr.size), + inputs=[ + m.nv, + m.opt.timestep, + m.opt.disableflags, + m.ten_J_rownnz, + m.ten_J_rowadr, + m.ten_J_colind, + m.tendon_solref_lim, + m.tendon_solimp_lim, + m.tendon_range, + m.tendon_margin, + m.tendon_invweight0, + m.is_sparse, + m.tendon_limited_adr, + d.qvel, + d.ten_J, + d.ten_length, + d.njmax, + d.njmax_nnz, + ], + outputs=[ + d.nl, + d.nefc, + d.efc.type, + d.efc.id, + d.efc.J_rownnz, + d.efc.J_rowadr, + d.efc.J_colind, + d.efc.J, + d.efc.pos, + d.efc.margin, + d.efc.D, + d.efc.vel, + d.efc.aref, + d.efc.frictionloss, + efc_nnz, + ], ) # contact if not (m.opt.disableflags & types.DisableBit.CONTACT): if m.opt.cone == types.ConeType.PYRAMIDAL: wp.launch( - _contact_pyramidal, - dim=(d.naconmax, m.nmaxpyramid), - inputs=[ - m.nv, - m.opt.timestep, - m.opt.disableflags, - m.opt.impratio_invsqrt, - m.body_parentid, - m.body_rootid, - m.body_weldid, - m.body_dofnum, - m.body_dofadr, - m.body_invweight0, - m.dof_bodyid, - m.dof_parentid, - m.geom_bodyid, - SPARSE_CONSTRAINT_JACOBIAN, - d.qvel, - d.subtree_com, - d.cdof, - d.njmax, - d.nacon, - d.contact.dist, - d.contact.dim, - d.contact.includemargin, - d.contact.worldid, - d.contact.geom, - d.contact.pos, - d.contact.frame, - d.contact.friction, - d.contact.solref, - d.contact.solimp, - d.contact.type, - ], - outputs=[ - d.nefc, - d.contact.efc_address, - d.efc.type, - d.efc.id, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.pos, - d.efc.margin, - d.efc.D, - d.efc.vel, - d.efc.aref, - d.efc.frictionloss, - ], + _contact_pyramidal, + dim=(d.naconmax, m.nmaxpyramid), + inputs=[ + m.nv, + m.opt.timestep, + m.opt.disableflags, + m.opt.impratio_invsqrt, + m.body_parentid, + m.body_rootid, + m.body_weldid, + m.body_dofnum, + m.body_dofadr, + m.body_invweight0, + m.dof_bodyid, + m.dof_parentid, + m.geom_bodyid, + m.flex_vertadr, + m.flex_vertbodyid, + m.is_sparse, + d.qvel, + d.subtree_com, + d.cdof, + d.njmax, + d.njmax_nnz, + d.nacon, + d.contact.dist, + d.contact.dim, + d.contact.includemargin, + d.contact.worldid, + d.contact.geom, + d.contact.flex, + d.contact.vert, + d.contact.pos, + d.contact.frame, + d.contact.friction, + d.contact.solref, + d.contact.solimp, + d.contact.type, + ], + outputs=[ + d.nefc, + d.contact.efc_address, + d.efc.type, + d.efc.id, + d.efc.J_rownnz, + d.efc.J_rowadr, + d.efc.J_colind, + d.efc.J, + d.efc.pos, + d.efc.margin, + d.efc.D, + d.efc.vel, + d.efc.aref, + d.efc.frictionloss, + efc_nnz, + ], ) elif m.opt.cone == types.ConeType.ELLIPTIC: wp.launch( - _contact_elliptic, - dim=(d.naconmax, m.nmaxcondim), - inputs=[ - m.nv, - m.opt.timestep, - m.opt.disableflags, - m.opt.impratio_invsqrt, - m.body_parentid, - m.body_rootid, - m.body_weldid, - m.body_dofnum, - m.body_dofadr, - m.body_invweight0, - m.dof_bodyid, - m.dof_parentid, - m.geom_bodyid, - SPARSE_CONSTRAINT_JACOBIAN, - d.qvel, - d.subtree_com, - d.cdof, - d.njmax, - d.nacon, - d.contact.dist, - d.contact.dim, - d.contact.includemargin, - d.contact.worldid, - d.contact.geom, - d.contact.pos, - d.contact.frame, - d.contact.friction, - d.contact.solref, - d.contact.solreffriction, - d.contact.solimp, - d.contact.type, - ], - outputs=[ - d.nefc, - d.contact.efc_address, - d.efc.type, - d.efc.id, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.pos, - d.efc.margin, - d.efc.D, - d.efc.vel, - d.efc.aref, - d.efc.frictionloss, - ], + _contact_elliptic, + dim=(d.naconmax, m.nmaxcondim), + inputs=[ + m.nv, + m.opt.timestep, + m.opt.disableflags, + m.opt.impratio_invsqrt, + m.body_parentid, + m.body_rootid, + m.body_weldid, + m.body_dofnum, + m.body_dofadr, + m.body_invweight0, + m.dof_bodyid, + m.dof_parentid, + m.geom_bodyid, + m.flex_vertadr, + m.flex_vertbodyid, + m.is_sparse, + d.qvel, + d.subtree_com, + d.cdof, + d.njmax, + d.njmax_nnz, + d.nacon, + d.contact.dist, + d.contact.dim, + d.contact.includemargin, + d.contact.worldid, + d.contact.geom, + d.contact.flex, + d.contact.vert, + d.contact.pos, + d.contact.frame, + d.contact.friction, + d.contact.solref, + d.contact.solreffriction, + d.contact.solimp, + d.contact.type, + ], + outputs=[ + d.nefc, + d.contact.efc_address, + d.efc.type, + d.efc.id, + d.efc.J_rownnz, + d.efc.J_rowadr, + d.efc.J_colind, + d.efc.J, + d.efc.pos, + d.efc.margin, + d.efc.D, + d.efc.vel, + d.efc.aref, + d.efc.frictionloss, + efc_nnz, + ], ) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/derivative.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/derivative.py index 406c36c4..20da751a 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/derivative.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/derivative.py @@ -15,6 +15,7 @@ import warp as wp +from mujoco.mjx.third_party.mujoco_warp._src.support import next_act from mujoco.mjx.third_party.mujoco_warp._src.types import BiasType from mujoco.mjx.third_party.mujoco_warp._src.types import Data from mujoco.mjx.third_party.mujoco_warp._src.types import DisableBit @@ -30,18 +31,24 @@ wp.set_module_options({"enable_backward": False}) @wp.kernel def _qderiv_actuator_passive_vel( # Model: + opt_timestep: wp.array(dtype=float), actuator_dyntype: wp.array(dtype=int), actuator_gaintype: wp.array(dtype=int), actuator_biastype: wp.array(dtype=int), actuator_actadr: wp.array(dtype=int), actuator_actnum: wp.array(dtype=int), actuator_forcelimited: wp.array(dtype=bool), + actuator_actlimited: wp.array(dtype=bool), + actuator_dynprm: wp.array2d(dtype=vec10f), actuator_gainprm: wp.array2d(dtype=vec10f), actuator_biasprm: wp.array2d(dtype=vec10f), + actuator_actearly: wp.array(dtype=bool), actuator_forcerange: wp.array2d(dtype=wp.vec2), + actuator_actrange: wp.array2d(dtype=wp.vec2), # Data in: act_in: wp.array2d(dtype=float), ctrl_in: wp.array2d(dtype=float), + act_dot_in: wp.array2d(dtype=float), actuator_force_in: wp.array2d(dtype=float), # Out: vel_out: wp.array2d(dtype=float), @@ -76,9 +83,24 @@ def _qderiv_actuator_passive_vel( vel = float(bias) if actuator_dyntype[actid] != DynType.NONE: if gain != 0.0: - act_first = actuator_actadr[actid] - act_last = act_first + actuator_actnum[actid] - 1 - vel += gain * act_in[worldid, act_last] + act_adr = actuator_actadr[actid] + actuator_actnum[actid] - 1 + + # use next activation if actearly is set (matching forward pass) + if actuator_actearly[actid]: + act = next_act( + opt_timestep[worldid % opt_timestep.shape[0]], + actuator_dyntype[actid], + actuator_dynprm[worldid % actuator_dynprm.shape[0], actid], + actuator_actrange[worldid % actuator_actrange.shape[0], actid], + act_in[worldid, act_adr], + act_dot_in[worldid, act_adr], + 1.0, + actuator_actlimited[actid], + ) + else: + act = act_in[worldid, act_adr] + + vel += gain * act else: if gain != 0.0: vel += gain * ctrl_in[worldid, actid] @@ -95,21 +117,20 @@ def _nonzero_mask(x: float) -> float: @wp.kernel -def _qderiv_actuator_passive_actuation_sparse( - # Model: - nu: int, - is_sparse: bool, - # Data in: - moment_rownnz_in: wp.array2d(dtype=int), - moment_rowadr_in: wp.array2d(dtype=int), - moment_colind_in: wp.array2d(dtype=int), - actuator_moment_in: wp.array2d(dtype=float), - # In: - vel_in: wp.array2d(dtype=float), - qMi: wp.array(dtype=int), - qMj: wp.array(dtype=int), - # Out: - qDeriv_out: wp.array3d(dtype=float), +def _qderiv_actuator_passive_actuation_dense( + # Model: + nu: int, + # Data in: + moment_rownnz_in: wp.array2d(dtype=int), + moment_rowadr_in: wp.array2d(dtype=int), + moment_colind_in: wp.array2d(dtype=int), + actuator_moment_in: wp.array2d(dtype=float), + # In: + vel_in: wp.array2d(dtype=float), + qMi: wp.array(dtype=int), + qMj: wp.array(dtype=int), + # Out: + qDeriv_out: wp.array3d(dtype=float), ): worldid, elemid = wp.tid() @@ -142,12 +163,63 @@ def _qderiv_actuator_passive_actuation_sparse( qderiv_contrib += moment_i * moment_j * vel - if is_sparse: - qDeriv_out[worldid, 0, elemid] = qderiv_contrib - else: - qDeriv_out[worldid, dofiid, dofjid] = qderiv_contrib - if dofiid != dofjid: - qDeriv_out[worldid, dofjid, dofiid] = qderiv_contrib + qDeriv_out[worldid, dofiid, dofjid] = qderiv_contrib + if dofiid != dofjid: + qDeriv_out[worldid, dofjid, dofiid] = qderiv_contrib + + +@wp.kernel +def _qderiv_actuator_passive_actuation_sparse( + # Model: + M_rownnz: wp.array(dtype=int), + M_rowadr: wp.array(dtype=int), + # Data in: + moment_rownnz_in: wp.array2d(dtype=int), + moment_rowadr_in: wp.array2d(dtype=int), + moment_colind_in: wp.array2d(dtype=int), + actuator_moment_in: wp.array2d(dtype=float), + # In: + vel_in: wp.array2d(dtype=float), + qMj: wp.array(dtype=int), + # Out: + qDeriv_out: wp.array3d(dtype=float), +): + worldid, actid = wp.tid() + + vel = vel_in[worldid, actid] + if vel == 0.0: + return + + rownnz = moment_rownnz_in[worldid, actid] + rowadr = moment_rowadr_in[worldid, actid] + + for i in range(rownnz): + rowadri = rowadr + i + moment_i = actuator_moment_in[worldid, rowadri] + if moment_i == 0.0: + continue + dofi = moment_colind_in[worldid, rowadri] + + for j in range(i + 1): + rowadrj = rowadr + j + moment_j = actuator_moment_in[worldid, rowadrj] + if moment_j == 0.0: + continue + dofj = moment_colind_in[worldid, rowadrj] + + contrib = moment_i * moment_j * vel + + # Search the corresponding elemid + # TODO: This could be precalculated for improved performance + row = dofi + col = dofj + row_startk = M_rowadr[row] - 1 + row_nnz = M_rownnz[row] + for k in range(row_nnz): + row_startk += 1 + if qMj[row_startk] == col: + wp.atomic_add(qDeriv_out[worldid, 0], row_startk, contrib) + break @wp.kernel @@ -176,7 +248,7 @@ def _qderiv_actuator_passive( else: qderiv = qDeriv_in[worldid, dofiid, dofjid] - if not opt_disableflags & DisableBit.DAMPER and dofiid == dofjid: + if not (opt_disableflags & DisableBit.DAMPER) and dofiid == dofjid: qderiv -= dof_damping[worldid % dof_damping.shape[0], dofiid] qderiv *= opt_timestep[worldid % opt_timestep.shape[0]] @@ -196,10 +268,13 @@ def _qderiv_tendon_damping( # Model: ntendon: int, opt_timestep: wp.array(dtype=float), + ten_J_rownnz: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), tendon_damping: wp.array2d(dtype=float), is_sparse: bool, # Data in: - ten_J_in: wp.array3d(dtype=float), + ten_J_in: wp.array2d(dtype=float), # In: qMi: wp.array(dtype=int), qMj: wp.array(dtype=int), @@ -213,7 +288,24 @@ def _qderiv_tendon_damping( qderiv = float(0.0) tendon_damping_id = worldid % tendon_damping.shape[0] for tenid in range(ntendon): - qderiv -= ten_J_in[worldid, tenid, dofiid] * ten_J_in[worldid, tenid, dofjid] * tendon_damping[tendon_damping_id, tenid] + damping = tendon_damping[tendon_damping_id, tenid] + if damping == 0.0: + continue + + rownnz = ten_J_rownnz[tenid] + rowadr = ten_J_rowadr[tenid] + Ji = float(0.0) + Jj = float(0.0) + for k in range(rownnz): + if Ji != 0.0 and Jj != 0.0: + break + sparseid = rowadr + k + colind = ten_J_colind[sparseid] + if colind == dofiid: + Ji = ten_J_in[worldid, sparseid] + if colind == dofjid: + Jj = ten_J_in[worldid, sparseid] + qderiv -= Ji * Jj * damping qderiv *= opt_timestep[worldid % opt_timestep.shape[0]] @@ -242,43 +334,47 @@ def deriv_smooth_vel(m: Model, d: Data, out: wp.array2d(dtype=float)): if ~(m.opt.disableflags & (DisableBit.ACTUATION | DisableBit.DAMPER)): # TODO(team): only clear elements not set by _qderiv_actuator_passive out.zero_() - if m.nu > 0 and not m.opt.disableflags & DisableBit.ACTUATION: + if m.nu > 0 and not (m.opt.disableflags & DisableBit.ACTUATION): vel = wp.empty((d.nworld, m.nu), dtype=float) wp.launch( _qderiv_actuator_passive_vel, dim=(d.nworld, m.nu), inputs=[ + m.opt.timestep, m.actuator_dyntype, m.actuator_gaintype, m.actuator_biastype, m.actuator_actadr, m.actuator_actnum, m.actuator_forcelimited, + m.actuator_actlimited, + m.actuator_dynprm, m.actuator_gainprm, m.actuator_biasprm, + m.actuator_actearly, m.actuator_forcerange, + m.actuator_actrange, d.act, d.ctrl, + d.act_dot, d.actuator_force, ], outputs=[vel], ) - wp.launch( + if m.is_sparse: + wp.launch( _qderiv_actuator_passive_actuation_sparse, - dim=(d.nworld, qMi.size), - inputs=[ - m.nu, - m.is_sparse, - d.moment_rownnz, - d.moment_rowadr, - d.moment_colind, - d.actuator_moment, - vel, - qMi, - qMj, - ], + dim=(d.nworld, m.nu), + inputs=[m.M_rownnz, m.M_rowadr, d.moment_rownnz, d.moment_rowadr, d.moment_colind, d.actuator_moment, vel, qMj], outputs=[out], - ) + ) + else: + wp.launch( + _qderiv_actuator_passive_actuation_dense, + dim=(d.nworld, qMi.size), + inputs=[m.nu, d.moment_rownnz, d.moment_rowadr, d.moment_colind, d.actuator_moment, vel, qMi, qMj], + outputs=[out], + ) wp.launch( _qderiv_actuator_passive, dim=(d.nworld, qMi.size), @@ -298,11 +394,22 @@ def deriv_smooth_vel(m: Model, d: Data, out: wp.array2d(dtype=float)): # TODO(team): directly utilize qM for these settings wp.copy(out, d.qM) - if not m.opt.disableflags & DisableBit.DAMPER: + if not (m.opt.disableflags & DisableBit.DAMPER): wp.launch( _qderiv_tendon_damping, dim=(d.nworld, qMi.size), - inputs=[m.ntendon, m.opt.timestep, m.tendon_damping, m.is_sparse, d.ten_J, qMi, qMj], + inputs=[ + m.ntendon, + m.opt.timestep, + m.ten_J_rownnz, + m.ten_J_rowadr, + m.ten_J_colind, + m.tendon_damping, + m.is_sparse, + d.ten_J, + qMi, + qMj, + ], outputs=[out], ) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/forward.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/forward.py index 0dc3de14..64bdd91f 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/forward.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/forward.py @@ -15,6 +15,8 @@ from typing import Optional +import warp as wp + from mujoco.mjx.third_party.mujoco_warp._src import collision_driver from mujoco.mjx.third_party.mujoco_warp._src import constraint from mujoco.mjx.third_party.mujoco_warp._src import derivative @@ -25,7 +27,9 @@ from mujoco.mjx.third_party.mujoco_warp._src import sensor from mujoco.mjx.third_party.mujoco_warp._src import smooth from mujoco.mjx.third_party.mujoco_warp._src import solver from mujoco.mjx.third_party.mujoco_warp._src import util_misc +from mujoco.mjx.third_party.mujoco_warp._src.support import next_act from mujoco.mjx.third_party.mujoco_warp._src.support import xfrc_accumulate +from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINVAL from mujoco.mjx.third_party.mujoco_warp._src.types import BiasType from mujoco.mjx.third_party.mujoco_warp._src.types import Data from mujoco.mjx.third_party.mujoco_warp._src.types import DisableBit @@ -34,14 +38,12 @@ from mujoco.mjx.third_party.mujoco_warp._src.types import EnableBit from mujoco.mjx.third_party.mujoco_warp._src.types import GainType from mujoco.mjx.third_party.mujoco_warp._src.types import IntegratorType from mujoco.mjx.third_party.mujoco_warp._src.types import JointType -from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINVAL from mujoco.mjx.third_party.mujoco_warp._src.types import Model from mujoco.mjx.third_party.mujoco_warp._src.types import TileSet from mujoco.mjx.third_party.mujoco_warp._src.types import TrnType from mujoco.mjx.third_party.mujoco_warp._src.types import vec10f from mujoco.mjx.third_party.mujoco_warp._src.warp_util import cache_kernel from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope -import warp as wp wp.set_module_options({"enable_backward": False}) @@ -127,55 +129,24 @@ def _next_velocity( qvel_out[worldid, dofid] = qvel_in[worldid, dofid] + qacc_scale_in * qacc_in[worldid, dofid] * timestep -# TODO(team): kernel analyzer array slice? -@wp.func -def _next_act( - # Model: - opt_timestep: float, # kernel_analyzer: ignore - actuator_dyntype: int, # kernel_analyzer: ignore - actuator_dynprm: vec10f, # kernel_analyzer: ignore - actuator_actrange: wp.vec2, # kernel_analyzer: ignore - # Data In: - act_in: float, # kernel_analyzer: ignore - act_dot_in: float, # kernel_analyzer: ignore - # In: - act_dot_scale: float, - clamp: bool, -) -> float: - # advance actuation - if actuator_dyntype == DynType.FILTEREXACT: - tau = wp.max(MJ_MINVAL, actuator_dynprm[0]) - act = act_in + act_dot_scale * act_dot_in * tau * (1.0 - wp.exp(-opt_timestep / tau)) - elif actuator_dyntype == DynType.USER: - return act_in - else: - act = act_in + act_dot_scale * act_dot_in * opt_timestep - - # clamp to actrange - if clamp: - act = wp.clamp(act, actuator_actrange[0], actuator_actrange[1]) - - return act - - @wp.kernel def _next_activation( - # Model: - opt_timestep: wp.array(dtype=float), - actuator_dyntype: wp.array(dtype=int), - actuator_actadr: wp.array(dtype=int), - actuator_actnum: wp.array(dtype=int), - actuator_actlimited: wp.array(dtype=bool), - actuator_dynprm: wp.array2d(dtype=vec10f), - actuator_actrange: wp.array2d(dtype=wp.vec2), - # Data in: - act_in: wp.array2d(dtype=float), - act_dot_in: wp.array2d(dtype=float), - # In: - act_dot_scale: float, - limit: bool, - # Data out: - act_out: wp.array2d(dtype=float), + # Model: + opt_timestep: wp.array(dtype=float), + actuator_dyntype: wp.array(dtype=int), + actuator_actadr: wp.array(dtype=int), + actuator_actnum: wp.array(dtype=int), + actuator_actlimited: wp.array(dtype=bool), + actuator_dynprm: wp.array2d(dtype=vec10f), + actuator_actrange: wp.array2d(dtype=wp.vec2), + # Data in: + act_in: wp.array2d(dtype=float), + act_dot_in: wp.array2d(dtype=float), + # In: + act_dot_scale: float, + limit: bool, + # Data out: + act_out: wp.array2d(dtype=float), ): worldid, uid = wp.tid() opt_timestep_id = worldid % opt_timestep.shape[0] @@ -184,15 +155,15 @@ def _next_activation( actadr = actuator_actadr[uid] actnum = actuator_actnum[uid] for j in range(actadr, actadr + actnum): - act = _next_act( - opt_timestep[opt_timestep_id], - actuator_dyntype[uid], - actuator_dynprm[actuator_dynprm_id, uid], - actuator_actrange[actuator_actrange_id, uid], - act_in[worldid, j], - act_dot_in[worldid, j], - act_dot_scale, - limit and actuator_actlimited[uid], + act = next_act( + opt_timestep[opt_timestep_id], + actuator_dyntype[uid], + actuator_dynprm[actuator_dynprm_id, uid], + actuator_actrange[actuator_actrange_id, uid], + act_in[worldid, j], + act_dot_in[worldid, j], + act_dot_scale, + limit and actuator_actlimited[uid], ) act_out[worldid, j] = act @@ -201,12 +172,16 @@ def _next_activation( def _next_time( # Model: opt_timestep: wp.array(dtype=float), + is_sparse: bool, # Data in: nefc_in: wp.array(dtype=int), time_in: wp.array(dtype=float), + efc_J_rownnz_in: wp.array2d(dtype=int), + efc_J_rowadr_in: wp.array2d(dtype=int), nworld_in: int, naconmax_in: int, njmax_in: int, + njmax_nnz_in: int, nacon_in: wp.array(dtype=int), ncollision_in: wp.array(dtype=int), # Data out: @@ -218,6 +193,11 @@ def _next_time( if nefc > njmax_in: wp.printf("nefc overflow - please increase njmax to %u\n", nefc) + elif nefc > 0 and is_sparse: + efcid = wp.min(nefc, njmax_in) - 1 + efc_nnz = efc_J_rowadr_in[worldid, efcid] + efc_J_rownnz_in[worldid, efcid] + if efc_nnz > njmax_nnz_in: + wp.printf("njmax_nnz overflow - please increase njmax_nnz to %u\n", efc_nnz) if worldid == 0: ncollision = ncollision_in[0] @@ -236,22 +216,22 @@ def _advance(m: Model, d: Data, qacc: wp.array, qvel: Optional[wp.array] = None) # advance activations wp.launch( - _next_activation, - dim=(d.nworld, m.nu), - inputs=[ - m.opt.timestep, - m.actuator_dyntype, - m.actuator_actadr, - m.actuator_actnum, - m.actuator_actlimited, - m.actuator_dynprm, - m.actuator_actrange, - d.act, - d.act_dot, - 1.0, - True, - ], - outputs=[d.act], + _next_activation, + dim=(d.nworld, m.nu), + inputs=[ + m.opt.timestep, + m.actuator_dyntype, + m.actuator_actadr, + m.actuator_actnum, + m.actuator_actlimited, + m.actuator_dynprm, + m.actuator_actrange, + d.act, + d.act_dot, + 1.0, + True, + ], + outputs=[d.act], ) wp.launch( @@ -274,7 +254,20 @@ def _advance(m: Model, d: Data, qacc: wp.array, qvel: Optional[wp.array] = None) wp.launch( _next_time, dim=d.nworld, - inputs=[m.opt.timestep, d.nefc, d.time, d.nworld, d.naconmax, d.njmax, d.nacon, d.ncollision], + inputs=[ + m.opt.timestep, + m.is_sparse, + d.nefc, + d.time, + d.efc.J_rownnz, + d.efc.J_rowadr, + d.nworld, + d.naconmax, + d.njmax, + d.njmax_nnz, + d.nacon, + d.ncollision, + ], outputs=[d.time], ) @@ -294,9 +287,7 @@ def _euler_damp_qfrc_sparse( timestep = opt_timestep[worldid % opt_timestep.shape[0]] adr = dof_Madr[tid] - qM_integration_out[worldid, 0, adr] += ( - timestep * dof_damping[worldid % dof_damping.shape[0], tid] - ) + qM_integration_out[worldid, 0, adr] += timestep * dof_damping[worldid % dof_damping.shape[0], tid] @cache_kernel @@ -336,7 +327,7 @@ def _tile_euler_dense(tile: TileSet): def euler(m: Model, d: Data): """Euler integrator, semi-implicit in velocity.""" # integrate damping implicitly - if not m.opt.disableflags & (DisableBit.EULERDAMP | DisableBit.DAMPER): + if not (m.opt.disableflags & (DisableBit.EULERDAMP | DisableBit.DAMPER)): qacc = wp.empty((d.nworld, m.nv), dtype=float) if m.is_sparse: qM = wp.clone(d.qM) @@ -390,22 +381,22 @@ def _rk_perturb_state( # activation if m.na and act_t0 is not None: wp.launch( - _next_activation, - dim=(d.nworld, m.nu), - inputs=[ - m.opt.timestep, - m.actuator_dyntype, - m.actuator_actadr, - m.actuator_actnum, - m.actuator_actlimited, - m.actuator_dynprm, - m.actuator_actrange, - act_t0, - d.act_dot, - scale, - False, - ], - outputs=[d.act], + _next_activation, + dim=(d.nworld, m.nu), + inputs=[ + m.opt.timestep, + m.actuator_dyntype, + m.actuator_actadr, + m.actuator_actnum, + m.actuator_actlimited, + m.actuator_dynprm, + m.actuator_actrange, + act_t0, + d.act_dot, + scale, + False, + ], + outputs=[d.act], ) @@ -548,14 +539,14 @@ def fwd_position(m: Model, d: Data, factorize: bool = True): @wp.kernel def _actuator_velocity( - # Data in: - qvel_in: wp.array2d(dtype=float), - moment_rownnz_in: wp.array2d(dtype=int), - moment_rowadr_in: wp.array2d(dtype=int), - moment_colind_in: wp.array2d(dtype=int), - actuator_moment_in: wp.array2d(dtype=float), - # Data out: - actuator_velocity_out: wp.array2d(dtype=float), + # Data in: + qvel_in: wp.array2d(dtype=float), + moment_rownnz_in: wp.array2d(dtype=int), + moment_rowadr_in: wp.array2d(dtype=int), + moment_colind_in: wp.array2d(dtype=int), + actuator_moment_in: wp.array2d(dtype=float), + # Data out: + actuator_velocity_out: wp.array2d(dtype=float), ): worldid, actid = wp.tid() @@ -571,50 +562,49 @@ def _actuator_velocity( actuator_velocity_out[worldid, actid] = vel -@cache_kernel -def _tendon_velocity(nv: int): - @wp.kernel(module="unique", enable_backward=False) - def tendon_velocity( - # Data in: - qvel_in: wp.array2d(dtype=float), - ten_J_in: wp.array3d(dtype=float), - # Data out: - ten_velocity_out: wp.array2d(dtype=float), - ): - worldid, tenid = wp.tid() - ten_J_tile = wp.tile_load(ten_J_in[worldid, tenid], shape=wp.static(nv)) - qvel_tile = wp.tile_load(qvel_in[worldid], shape=wp.static(nv)) - ten_J_qvel_tile = wp.tile_map(wp.mul, ten_J_tile, qvel_tile) - ten_velocity_tile = wp.tile_reduce(wp.add, ten_J_qvel_tile) - ten_velocity_out[worldid, tenid] = ten_velocity_tile[0] +@wp.kernel +def _tendon_velocity( + # Model: + ten_J_rownnz: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), + # Data in: + qvel_in: wp.array2d(dtype=float), + ten_J_in: wp.array2d(dtype=float), + # Data out: + ten_velocity_out: wp.array2d(dtype=float), +): + worldid, tenid = wp.tid() - return tendon_velocity + velocity = float(0.0) + rownnz = ten_J_rownnz[tenid] + rowadr = ten_J_rowadr[tenid] + for i in range(rownnz): + sparseid = rowadr + i + J = ten_J_in[worldid, sparseid] + if J != 0.0: + colind = ten_J_colind[sparseid] + velocity += J * qvel_in[worldid, colind] + + ten_velocity_out[worldid, tenid] = velocity @event_scope def fwd_velocity(m: Model, d: Data): """Velocity-dependent computations.""" - wp.launch_tiled( - _actuator_velocity, - dim=(d.nworld, m.nu), - inputs=[ - d.qvel, - d.moment_rownnz, - d.moment_rowadr, - d.moment_colind, - d.actuator_moment, - ], - outputs=[d.actuator_velocity], - block_dim=m.block_dim.actuator_velocity, + wp.launch( + _actuator_velocity, + dim=(d.nworld, m.nu), + inputs=[d.qvel, d.moment_rownnz, d.moment_rowadr, d.moment_colind, d.actuator_moment], + outputs=[d.actuator_velocity], + block_dim=m.block_dim.actuator_velocity, ) - # TODO(team): sparse version - wp.launch_tiled( - _tendon_velocity(m.nv), + wp.launch( + _tendon_velocity, dim=(d.nworld, m.ntendon), - inputs=[d.qvel, d.ten_J], + inputs=[m.ten_J_rownnz, m.ten_J_rowadr, m.ten_J_colind, d.qvel, d.ten_J], outputs=[d.ten_velocity], - block_dim=m.block_dim.tendon_velocity, ) smooth.com_vel(m, d) @@ -625,36 +615,36 @@ def fwd_velocity(m: Model, d: Data): @wp.kernel def _actuator_force( - # Model: - na: int, - opt_timestep: wp.array(dtype=float), - actuator_dyntype: wp.array(dtype=int), - actuator_gaintype: wp.array(dtype=int), - actuator_biastype: wp.array(dtype=int), - actuator_actadr: wp.array(dtype=int), - actuator_actnum: wp.array(dtype=int), - actuator_ctrllimited: wp.array(dtype=bool), - actuator_forcelimited: wp.array(dtype=bool), - actuator_actlimited: wp.array(dtype=bool), - actuator_dynprm: wp.array2d(dtype=vec10f), - actuator_gainprm: wp.array2d(dtype=vec10f), - actuator_biasprm: wp.array2d(dtype=vec10f), - actuator_actearly: wp.array(dtype=bool), - actuator_ctrlrange: wp.array2d(dtype=wp.vec2), - actuator_forcerange: wp.array2d(dtype=wp.vec2), - actuator_actrange: wp.array2d(dtype=wp.vec2), - actuator_acc0: wp.array2d(dtype=float), - actuator_lengthrange: wp.array2d(dtype=wp.vec2), - # Data in: - act_in: wp.array2d(dtype=float), - ctrl_in: wp.array2d(dtype=float), - actuator_length_in: wp.array2d(dtype=float), - actuator_velocity_in: wp.array2d(dtype=float), - # In: - dsbl_clampctrl: int, - # Data out: - act_dot_out: wp.array2d(dtype=float), - actuator_force_out: wp.array2d(dtype=float), + # Model: + na: int, + opt_timestep: wp.array(dtype=float), + actuator_dyntype: wp.array(dtype=int), + actuator_gaintype: wp.array(dtype=int), + actuator_biastype: wp.array(dtype=int), + actuator_actadr: wp.array(dtype=int), + actuator_actnum: wp.array(dtype=int), + actuator_ctrllimited: wp.array(dtype=bool), + actuator_forcelimited: wp.array(dtype=bool), + actuator_actlimited: wp.array(dtype=bool), + actuator_dynprm: wp.array2d(dtype=vec10f), + actuator_gainprm: wp.array2d(dtype=vec10f), + actuator_biasprm: wp.array2d(dtype=vec10f), + actuator_actearly: wp.array(dtype=bool), + actuator_ctrlrange: wp.array2d(dtype=wp.vec2), + actuator_forcerange: wp.array2d(dtype=wp.vec2), + actuator_actrange: wp.array2d(dtype=wp.vec2), + actuator_acc0: wp.array2d(dtype=float), + actuator_lengthrange: wp.array2d(dtype=wp.vec2), + # Data in: + act_in: wp.array2d(dtype=float), + ctrl_in: wp.array2d(dtype=float), + actuator_length_in: wp.array2d(dtype=float), + actuator_velocity_in: wp.array2d(dtype=float), + # In: + dsbl_clampctrl: int, + # Data out: + act_dot_out: wp.array2d(dtype=float), + actuator_force_out: wp.array2d(dtype=float), ): worldid, uid = wp.tid() @@ -693,7 +683,7 @@ def _actuator_force( if dyntype == DynType.INTEGRATOR or dyntype == DynType.NONE: act = act_in[worldid, act_last] - ctrl_act = _next_act( + ctrl_act = next_act( opt_timestep[worldid % opt_timestep.shape[0]], dyntype, dynprm, @@ -720,9 +710,7 @@ def _actuator_force( gain = gainprm[0] + gainprm[1] * length + gainprm[2] * velocity elif gaintype == GainType.MUSCLE: acc0 = actuator_acc0[worldid % actuator_acc0.shape[0], uid] - lengthrange = actuator_lengthrange[ - worldid % actuator_lengthrange.shape[0], uid - ] + lengthrange = actuator_lengthrange[worldid % actuator_lengthrange.shape[0], uid] gain = util_misc.muscle_gain(length, velocity, lengthrange, acc0, gainprm) # GainType.USER: gain stays 0, modified by act_gain_callback @@ -735,9 +723,7 @@ def _actuator_force( bias = biasprm[0] + biasprm[1] * length + biasprm[2] * velocity elif biastype == BiasType.MUSCLE: acc0 = actuator_acc0[worldid % actuator_acc0.shape[0], uid] - lengthrange = actuator_lengthrange[ - worldid % actuator_lengthrange.shape[0], uid - ] + lengthrange = actuator_lengthrange[worldid % actuator_lengthrange.shape[0], uid] bias = util_misc.muscle_bias(length, lengthrange, acc0, biasprm) force = gain * ctrl_act + bias @@ -795,14 +781,14 @@ def _tendon_actuator_force_clamp( @wp.kernel def _qfrc_actuator( - # Data in: - moment_rownnz_in: wp.array2d(dtype=int), - moment_rowadr_in: wp.array2d(dtype=int), - moment_colind_in: wp.array2d(dtype=int), - actuator_moment_in: wp.array2d(dtype=float), - actuator_force_in: wp.array2d(dtype=float), - # Data out: - qfrc_actuator_out: wp.array2d(dtype=float), + # Data in: + moment_rownnz_in: wp.array2d(dtype=int), + moment_rowadr_in: wp.array2d(dtype=int), + moment_colind_in: wp.array2d(dtype=int), + actuator_moment_in: wp.array2d(dtype=float), + actuator_force_in: wp.array2d(dtype=float), + # Data out: + qfrc_actuator_out: wp.array2d(dtype=float), ): worldid, actid = wp.tid() @@ -812,26 +798,23 @@ def _qfrc_actuator( for i in range(rownnz): sparseid = rowadr + i colind = moment_colind_in[worldid, sparseid] - qfrc = ( - actuator_moment_in[worldid, sparseid] - * actuator_force_in[worldid, actid] - ) + qfrc = actuator_moment_in[worldid, sparseid] * actuator_force_in[worldid, actid] wp.atomic_add(qfrc_actuator_out[worldid], colind, qfrc) @wp.kernel def _qfrc_actuator_gravcomp_limits( - # Model: - ngravcomp: int, - jnt_actfrclimited: wp.array(dtype=bool), - jnt_actgravcomp: wp.array(dtype=int), - jnt_actfrcrange: wp.array2d(dtype=wp.vec2), - dof_jntid: wp.array(dtype=int), - # Data in: - qfrc_gravcomp_in: wp.array2d(dtype=float), - qfrc_actuator_in: wp.array2d(dtype=float), - # Data out: - qfrc_actuator_out: wp.array2d(dtype=float), + # Model: + ngravcomp: int, + jnt_actfrclimited: wp.array(dtype=bool), + jnt_actgravcomp: wp.array(dtype=int), + jnt_actfrcrange: wp.array2d(dtype=wp.vec2), + dof_jntid: wp.array(dtype=int), + # Data in: + qfrc_gravcomp_in: wp.array2d(dtype=float), + qfrc_actuator_in: wp.array2d(dtype=float), + # Data out: + qfrc_actuator_out: wp.array2d(dtype=float), ): worldid, dofid = wp.tid() jntid = dof_jntid[dofid] @@ -917,30 +900,30 @@ def fwd_actuation(m: Model, d: Data): # TODO(team): optimize performance d.qfrc_actuator.zero_() wp.launch( - _qfrc_actuator, - dim=(d.nworld, m.nu), - inputs=[ - d.moment_rownnz, - d.moment_rowadr, - d.moment_colind, - d.actuator_moment, - d.actuator_force, - ], - outputs=[d.qfrc_actuator], + _qfrc_actuator, + dim=(d.nworld, m.nu), + inputs=[ + d.moment_rownnz, + d.moment_rowadr, + d.moment_colind, + d.actuator_moment, + d.actuator_force, + ], + outputs=[d.qfrc_actuator], ) wp.launch( - _qfrc_actuator_gravcomp_limits, - dim=(d.nworld, m.nv), - inputs=[ - m.ngravcomp, - m.jnt_actfrclimited, - m.jnt_actgravcomp, - m.jnt_actfrcrange, - m.dof_jntid, - d.qfrc_gravcomp, - d.qfrc_actuator, - ], - outputs=[d.qfrc_actuator], + _qfrc_actuator_gravcomp_limits, + dim=(d.nworld, m.nv), + inputs=[ + m.ngravcomp, + m.jnt_actfrclimited, + m.jnt_actgravcomp, + m.jnt_actfrcrange, + m.dof_jntid, + d.qfrc_gravcomp, + d.qfrc_actuator, + ], + outputs=[d.qfrc_actuator], ) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py index e516739c..0b53094b 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py @@ -14,24 +14,24 @@ # ============================================================================== import dataclasses -from typing import Any, Optional, Sequence import warnings +from typing import Any, Optional, Sequence import mujoco +import numpy as np +import warp as wp + from mujoco.mjx.third_party.mujoco_warp._src import bvh from mujoco.mjx.third_party.mujoco_warp._src import math as mjmath from mujoco.mjx.third_party.mujoco_warp._src import render_util from mujoco.mjx.third_party.mujoco_warp._src import smooth from mujoco.mjx.third_party.mujoco_warp._src import types from mujoco.mjx.third_party.mujoco_warp._src import warp_util -from mujoco.mjx.third_party.mujoco_warp._src.types import BiasType from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINVAL -from mujoco.mjx.third_party.mujoco_warp._src.types import SPARSE_CONSTRAINT_JACOBIAN +from mujoco.mjx.third_party.mujoco_warp._src.types import BiasType from mujoco.mjx.third_party.mujoco_warp._src.types import TrnType from mujoco.mjx.third_party.mujoco_warp._src.types import vec10 from mujoco.mjx.third_party.mujoco_warp._src.util_pkg import check_version -import numpy as np -import warp as wp def _create_array(data: Any, spec: wp.array, sizes: dict[str, int]) -> wp.array | None: @@ -114,9 +114,6 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: if unsupported: raise NotImplementedError(f"{mj_type(unsupported).name} is unsupported.") - if ((mjm.flex_contype != 0) | (mjm.flex_conaffinity != 0)).any(): - raise NotImplementedError("Flex collisions are not implemented.") - if mjm.opt.noslip_iterations > 0: raise NotImplementedError(f"noslip solver not implemented.") @@ -226,6 +223,8 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: m.is_sparse = is_sparse(mjm) m.has_fluid = mjm.opt.wind.any() or mjm.opt.density > 0 or mjm.opt.viscosity > 0 + m.max_ten_J_rownnz = int(mjm.ten_J_rownnz.max()) if mjm.ntendon else 0 + # body ids grouped by tree level (depth-based traversal) bodies, body_depth = {}, np.zeros(mjm.nbody, dtype=int) - 1 for i in range(mjm.nbody): @@ -364,6 +363,46 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: ) ) + # check for unsupported margin + multicontact / box-box CCD combinations + use_multiccd = mjm.opt.enableflags & types.EnableBit.MULTICCD + nativeccd_disabled = mjm.opt.disableflags & types.DisableBit.NATIVECCD + BOX = int(mujoco.mjtGeom.mjGEOM_BOX) + MESH = int(mujoco.mjtGeom.mjGEOM_MESH) + + has_boxbox = m.geom_pair_type_count[geom_trid_index(BOX, BOX)] > 0 + has_multiccd_pairs = has_boxbox or ( + use_multiccd + and (m.geom_pair_type_count[geom_trid_index(BOX, MESH)] > 0 or m.geom_pair_type_count[geom_trid_index(MESH, MESH)] > 0) + ) + + if has_multiccd_pairs: + + def _check_margin(name, t1, t2, margin): + if use_multiccd: + raise NotImplementedError( + f"{name} has non-zero margin ({margin}) with MULTICCD enabled. Set margin to 0 or disable MULTICCD." + ) + if t1 == BOX and t2 == BOX and not nativeccd_disabled: + raise NotImplementedError( + f"{name} has non-zero margin ({margin}) with NATIVECCD enabled. Set margin to 0 or disable NATIVECCD." + ) + + geom_name = lambda g: mujoco.mj_id2name(mjm, mujoco.mjtObj.mjOBJ_GEOM, g) or str(g) + + for idx in np.nonzero(nxn_include & (nxn_pairid_contact == -1))[0]: + g1, g2 = int(geom1[idx]), int(geom2[idx]) + t1, t2 = int(mjm.geom_type[g1]), int(mjm.geom_type[g2]) + m1, m2 = float(mjm.geom_margin[g1]), float(mjm.geom_margin[g2]) + if (m1 or m2) and t1 in (BOX, MESH) and t2 in (BOX, MESH): + _check_margin(f"geom pair ({geom_name(g1)}, {geom_name(g2)})", t1, t2, (m1, m2)) + + for pid in range(mjm.npair): + g1, g2 = int(mjm.pair_geom1[pid]), int(mjm.pair_geom2[pid]) + t1, t2 = int(mjm.geom_type[g1]), int(mjm.geom_type[g2]) + pm = float(mjm.pair_margin[pid]) + if pm and t1 in (BOX, MESH) and t2 in (BOX, MESH): + _check_margin(f"pair {pid} ({geom_name(g1)}, {geom_name(g2)})", t1, t2, pm) + m.nmaxpolygon = np.append(mjm.mesh_polyvertnum, 0).max() m.nmaxmeshdeg = np.append(mjm.mesh_polymapnum, 0).max() @@ -390,9 +429,11 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: current = [] else: current.append(v) - # Pad with zeros if less than 3 - attr_values += [0.0] * (3 - len(attr_values)) - m.plugin_attr.append(attr_values[:3]) + if len(attr_values) > types._NPLUGINATTR: + raise ValueError(f"Plugin has {len(attr_values)} attributes, which exceeds the maximum of {types._NPLUGINATTR}. ") + # pad with zeros to _NPLUGINATTR + attr_values += [0.0] * (types._NPLUGINATTR - len(attr_values)) + m.plugin_attr.append(attr_values[: types._NPLUGINATTR]) # equality constraint addresses m.eq_connect_adr = np.nonzero(mjm.eq_type == types.EqType.CONNECT)[0] @@ -542,6 +583,15 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: Madr_ki -= 1 m.qLD_updates = tuple(wp.array(qLD_updates[i], dtype=wp.vec3i) for i in sorted(qLD_updates)) + # Build concatenated updates for fused kernel + all_updates_flat = [] + level_offsets = [0] + for level in sorted(qLD_updates): + all_updates_flat.extend(qLD_updates[level]) + level_offsets.append(len(all_updates_flat)) + m.qLD_all_updates = all_updates_flat if all_updates_flat else [(0, 0, 0)] + m.qLD_level_offsets = level_offsets + # indices for sparse qM_fullm (used in solver) m.qM_fullm_i, m.qM_fullm_j = [], [] for i in range(mjm.nv): @@ -631,9 +681,168 @@ def _default_njmax(mjm: mujoco.MjModel, mjd: Optional[mujoco.MjData] = None) -> return int(valid_sizes[np.searchsorted(valid_sizes, njmax)]) -def _resolve_batch_size( - na: int | None, n: int | None, nworld: int, default: int -) -> int: +def _body_pair_nnz(mjm: mujoco.MjModel, body1: int, body2: int) -> int: + """Returns the number of unique DOFs in the kinematic tree union of two bodies.""" + body1 = mjm.body_weldid[body1] + body2 = mjm.body_weldid[body2] + da1 = mjm.body_dofadr[body1] + mjm.body_dofnum[body1] - 1 + da2 = mjm.body_dofadr[body2] + mjm.body_dofnum[body2] - 1 + nnz = 0 + while da1 >= 0 or da2 >= 0: + da = max(da1, da2) + if da1 == da: + da1 = mjm.dof_parentid[da1] + if da2 == da: + da2 = mjm.dof_parentid[da2] + nnz += 1 + return nnz + + +def _default_njmax_nnz(mjm: mujoco.MjModel, nconmax: int, njmax: int) -> int: + """Returns a heuristic estimate for the number of non-zeros in the sparse constraint Jacobian. + + Assumes all equality, friction, and limit constraints are active and computes + their non-zeros. For contacts, assumes njmax contact rows at the maximum + body-pair non-zeros from all enabled collision pairs. + + Args: + mjm: The model containing kinematic and dynamic information (host). + nconmax: Maximum number of contacts per world. + njmax: Maximum number of constraint rows per world. + + Returns: + Estimated number of non-zeros in the constraint Jacobian. + """ + total_nnz = 0 + + def _eq_bodies(i): + """Returns body pair for equality constraint i.""" + obj1id, obj2id = mjm.eq_obj1id[i], mjm.eq_obj2id[i] + if mjm.eq_objtype[i] == mujoco.mjtObj.mjOBJ_SITE: + return mjm.site_bodyid[obj1id], mjm.site_bodyid[obj2id] + return obj1id, obj2id + + # equality constraints (assume all active) + for i in range(mjm.neq): + eq_type = mjm.eq_type[i] + + if eq_type == mujoco.mjtEq.mjEQ_CONNECT: + total_nnz += 3 * _body_pair_nnz(mjm, *_eq_bodies(i)) + + elif eq_type == mujoco.mjtEq.mjEQ_WELD: + total_nnz += 6 * _body_pair_nnz(mjm, *_eq_bodies(i)) + + elif eq_type == mujoco.mjtEq.mjEQ_JOINT: + total_nnz += 2 if mjm.eq_obj2id[i] >= 0 else 1 + + elif eq_type == mujoco.mjtEq.mjEQ_TENDON: + obj1id = mjm.eq_obj1id[i] + obj2id = mjm.eq_obj2id[i] + rownnz1 = mjm.ten_J_rownnz[obj1id] if obj1id < mjm.ntendon else 0 + if obj2id >= 0 and obj2id < mjm.ntendon: + rowadr1 = mjm.ten_J_rowadr[obj1id] + rowadr2 = mjm.ten_J_rowadr[obj2id] + rownnz2 = mjm.ten_J_rownnz[obj2id] + cols = set() + for j in range(rownnz1): + cols.add(mjm.ten_J_colind[rowadr1 + j]) + for j in range(rownnz2): + cols.add(mjm.ten_J_colind[rowadr2 + j]) + total_nnz += len(cols) + else: + total_nnz += rownnz1 + + elif eq_type == mujoco.mjtEq.mjEQ_FLEX: + obj1id = mjm.eq_obj1id[i] + if obj1id < mjm.nflex: + edge_start = mjm.flex_edgeadr[obj1id] + edge_count = mjm.flex_edgenum[obj1id] + for e in range(edge_count): + total_nnz += mjm.flexedge_J_rownnz[edge_start + e] + + # friction constraints + total_nnz += (mjm.dof_frictionloss > 0).sum() + for i in range(mjm.ntendon): + if mjm.tendon_frictionloss[i] > 0: + total_nnz += mjm.ten_J_rownnz[i] + + # limit constraints (assume all active) + for i in range(mjm.njnt): + if mjm.jnt_limited[i]: + jnt_type = mjm.jnt_type[i] + if jnt_type == mujoco.mjtJoint.mjJNT_BALL: + total_nnz += 3 + elif jnt_type in (mujoco.mjtJoint.mjJNT_SLIDE, mujoco.mjtJoint.mjJNT_HINGE): + total_nnz += 1 + for i in range(mjm.ntendon): + if mjm.tendon_limited[i]: + total_nnz += mjm.ten_J_rownnz[i] + + # contact constraints: njmax rows at max body-pair non-zeros + max_contact_nnz = 0 + + # contact pairs + for i in range(mjm.npair): + g1, g2 = mjm.pair_geom1[i], mjm.pair_geom2[i] + b1, b2 = mjm.geom_bodyid[g1], mjm.geom_bodyid[g2] + max_contact_nnz = max(max_contact_nnz, _body_pair_nnz(mjm, b1, b2)) + + # filter geom-geom pairs (unique body pairs, filtered) + body_pair_seen = set() + for i in range(mjm.ngeom): + bi = mjm.geom_bodyid[i] + cti, cai = mjm.geom_contype[i], mjm.geom_conaffinity[i] + for j in range(i + 1, mjm.ngeom): + bj = mjm.geom_bodyid[j] + if bi == bj: + continue + if mjm.body_weldid[bi] == 0 and mjm.body_weldid[bj] == 0: + continue + bp = (min(bi, bj), max(bi, bj)) + if bp in body_pair_seen: + continue + ctj, caj = mjm.geom_contype[j], mjm.geom_conaffinity[j] + if not ((cti & caj) or (ctj & cai)): + continue + body_pair_seen.add(bp) + max_contact_nnz = max(max_contact_nnz, _body_pair_nnz(mjm, bi, bj)) + + # flex vertex contacts + for fi in range(mjm.nflex): + fct = mjm.flex_contype[fi] + fca = mjm.flex_conaffinity[fi] + + vert_start = mjm.flex_vertadr[fi] + vert_count = mjm.flex_vertnum[fi] + flex_bodies = {mjm.flex_vertbodyid[vert_start + v] for v in range(vert_count)} + + geom_bodies = set() + for g in range(mjm.ngeom): + ct, ca = mjm.geom_contype[g], mjm.geom_conaffinity[g] + if (fct & ca) or (ct & fca): + geom_bodies.add(mjm.geom_bodyid[g]) + + for fb in flex_bodies: + for gb in geom_bodies: + if fb != gb: + max_contact_nnz = max(max_contact_nnz, _body_pair_nnz(mjm, fb, gb)) + + # flex self-collision + if mjm.flex_selfcollide[fi]: + flex_body_list = sorted(flex_bodies) + for idx1 in range(len(flex_body_list)): + for idx2 in range(idx1 + 1, len(flex_body_list)): + max_contact_nnz = max( + max_contact_nnz, + _body_pair_nnz(mjm, flex_body_list[idx1], flex_body_list[idx2]), + ) + + total_nnz += njmax * max_contact_nnz + + return int(min(max(total_nnz, 1), njmax * mjm.nv)) + + +def _resolve_batch_size(na: int | None, n: int | None, nworld: int, default: int) -> int: if na is not None: return na if n is not None: @@ -647,6 +856,7 @@ def make_data( nconmax: Optional[int] = None, nccdmax: Optional[int] = None, njmax: Optional[int] = None, + njmax_nnz: Optional[int] = None, naconmax: Optional[int] = None, naccdmax: Optional[int] = None, ) -> types.Data: @@ -660,6 +870,7 @@ def make_data( nccdmax: Number of CCD contacts to allocate per world. Same semantics as nconmax. njmax: Number of constraints to allocate per world. Constraint arrays are batched by world: no world may have more than njmax constraints. + njmax_nnz: Number of non-zeros in constraint Jacobian (sparse). Defaults to njmax * nv. naconmax: Number of contacts to allocate for all worlds. Overrides nconmax. naccdmax: Maximum number of CCD contacts. Defaults to naconmax. @@ -709,20 +920,32 @@ def make_data( sizes["naconmax"] = naconmax sizes["njmax"] = njmax + if njmax_nnz is None: + if is_sparse(mjm): + njmax_nnz = _default_njmax_nnz(mjm, nconmax, njmax) + else: + njmax_nnz = njmax * mjm.nv + contact = types.Contact(**{f.name: _create_array(None, f.type, sizes) for f in dataclasses.fields(types.Contact)}) + contact.efc_address = wp.array(np.full((naconmax, sizes["nmaxpyramid"]), -1, dtype=int), dtype=int) efc = types.Constraint(**{f.name: _create_array(None, f.type, sizes) for f in dataclasses.fields(types.Constraint)}) - if SPARSE_CONSTRAINT_JACOBIAN: + if is_sparse(mjm): efc.J_rownnz = wp.zeros((nworld, njmax), dtype=int) efc.J_rowadr = wp.zeros((nworld, njmax), dtype=int) - efc.J_colind = wp.zeros((nworld, 1, njmax * mjm.nv), dtype=int) - efc.J = wp.zeros((nworld, 1, njmax * mjm.nv), dtype=float) + efc.J_colind = wp.zeros((nworld, 1, njmax_nnz), dtype=int) + efc.J = wp.zeros((nworld, 1, njmax_nnz), dtype=float) else: efc.J_rownnz = wp.zeros((nworld, 0), dtype=int) efc.J_rowadr = wp.zeros((nworld, 0), dtype=int) efc.J_colind = wp.zeros((nworld, 0, 0), dtype=int) efc.J = wp.zeros((nworld, sizes["njmax_pad"], sizes["nv_pad"]), dtype=float) + contact_kwargs = {} + for f in dataclasses.fields(types.Contact): + contact_kwargs[f.name] = _create_array(None, f.type, sizes) + contact = types.Contact(**contact_kwargs) + # world body and static geom (attached to the world) poses are precomputed # this speeds up scenes with many static geoms (e.g. terrains) # TODO(team): remove this when we introduce dof islands + sleeping @@ -734,65 +957,34 @@ def make_data( mocap_id = mjm.body_mocapid[mocap_body] d_kwargs = { - "qpos": wp.array( - np.tile(mjm.qpos0, nworld), shape=(nworld, mjm.nq), dtype=float - ), - "contact": contact, - "efc": efc, - "nworld": nworld, - "naconmax": naconmax, - "naccdmax": naccdmax, - "njmax": njmax, - "njmax_pad": sizes["njmax_pad"], - "qM": None, - "qLD": None, - # world body - "xquat": wp.array( - np.tile(mjd.xquat, (nworld, 1)), - shape=(nworld, mjm.nbody), - dtype=wp.quat, - ), - "xmat": wp.array( - np.tile(mjd.xmat, (nworld, 1)), - shape=(nworld, mjm.nbody), - dtype=wp.mat33, - ), - "ximat": wp.array( - np.tile(mjd.ximat, (nworld, 1)), - shape=(nworld, mjm.nbody), - dtype=wp.mat33, - ), - # static geoms - "geom_xpos": wp.array( - np.tile(mjd.geom_xpos, (nworld, 1)), - shape=(nworld, mjm.ngeom), - dtype=wp.vec3, - ), - "geom_xmat": wp.array( - np.tile(mjd.geom_xmat, (nworld, 1)), - shape=(nworld, mjm.ngeom), - dtype=wp.mat33, - ), - # mocap - "mocap_pos": wp.array( - np.tile(mjm.body_pos[mocap_body[mocap_id]], (nworld, 1)), - shape=(nworld, mjm.nmocap), - dtype=wp.vec3, - ), - "mocap_quat": wp.array( - np.tile(mjm.body_quat[mocap_body[mocap_id]], (nworld, 1)), - shape=(nworld, mjm.nmocap), - dtype=wp.quat, - ), - # equality constraints - "eq_active": wp.array( - np.tile(mjm.eq_active0.astype(bool), (nworld, 1)), - shape=(nworld, mjm.neq), - dtype=bool, - ), - # island arrays - "nisland": None, - "tree_island": None, + "qpos": wp.array(np.tile(mjm.qpos0, nworld), shape=(nworld, mjm.nq), dtype=float), + "contact": contact, + "efc": efc, + "nworld": nworld, + "naconmax": naconmax, + "naccdmax": naccdmax, + "njmax": njmax, + "njmax_pad": sizes["njmax_pad"], + "njmax_nnz": njmax_nnz, + "qM": None, + "qLD": None, + # world body + "xquat": wp.array(np.tile(mjd.xquat, (nworld, 1)), shape=(nworld, mjm.nbody), dtype=wp.quat), + "xmat": wp.array(np.tile(mjd.xmat, (nworld, 1)), shape=(nworld, mjm.nbody), dtype=wp.mat33), + "ximat": wp.array(np.tile(mjd.ximat, (nworld, 1)), shape=(nworld, mjm.nbody), dtype=wp.mat33), + # static geoms + "geom_xpos": wp.array(np.tile(mjd.geom_xpos, (nworld, 1)), shape=(nworld, mjm.ngeom), dtype=wp.vec3), + "geom_xmat": wp.array(np.tile(mjd.geom_xmat, (nworld, 1)), shape=(nworld, mjm.ngeom), dtype=wp.mat33), + # mocap + "mocap_pos": wp.array(np.tile(mjm.body_pos[mocap_body[mocap_id]], (nworld, 1)), shape=(nworld, mjm.nmocap), dtype=wp.vec3), + "mocap_quat": wp.array( + np.tile(mjm.body_quat[mocap_body[mocap_id]], (nworld, 1)), shape=(nworld, mjm.nmocap), dtype=wp.quat + ), + # equality constraints + "eq_active": wp.array(np.tile(mjm.eq_active0.astype(bool), (nworld, 1)), shape=(nworld, mjm.neq), dtype=bool), + # island arrays + "nisland": None, + "tree_island": None, } for f in dataclasses.fields(types.Data): if f.name in d_kwargs: @@ -822,6 +1014,7 @@ def put_data( nconmax: Optional[int] = None, nccdmax: Optional[int] = None, njmax: Optional[int] = None, + njmax_nnz: Optional[int] = None, naconmax: Optional[int] = None, naccdmax: Optional[int] = None, ) -> types.Data: @@ -836,6 +1029,7 @@ def put_data( nccdmax: Number of CCD contacts to allocate per world. Same semantics as nconmax. njmax: Number of constraints to allocate per world. Constraint arrays are batched by world: no world may have more than njmax constraints. + njmax_nnz: Number of non-zeros in constraint Jacobian (sparse). Defaults to njmax * nv. naconmax: Number of contacts to allocate for all worlds. Overrides nconmax. naccdmax: Maximum number of CCD contacts. Defaults to naconmax. @@ -898,6 +1092,12 @@ def put_data( sizes["naconmax"] = naconmax sizes["njmax"] = njmax + if njmax_nnz is None: + if is_sparse(mjm): + njmax_nnz = _default_njmax_nnz(mjm, nconmax, njmax) + else: + njmax_nnz = njmax * mjm.nv + # ensure static geom positions are computed # TODO: remove once MjData creation semantics are fixed mujoco.mj_kinematics(mjm, mjd) @@ -915,7 +1115,7 @@ def put_data( contact = types.Contact(**contact_kwargs) - contact.efc_address = np.zeros((naconmax, sizes["nmaxpyramid"]), dtype=int) + contact.efc_address = np.full((naconmax, sizes["nmaxpyramid"]), -1, dtype=int) for i in range(mjd.ncon): efc_address = mjd.contact.efc_address[i] if efc_address == -1: @@ -945,43 +1145,28 @@ def put_data( efc = types.Constraint(**efc_kwargs) - if SPARSE_CONSTRAINT_JACOBIAN: - # TODO(team): process efc_J sparsity structure for nv row shift - efc.J_rownnz = wp.array( - np.full((nworld, njmax), mjm.nv, dtype=int), dtype=int - ) - efc.J_rowadr = wp.array( - np.tile( - np.arange(0, njmax * mjm.nv, mjm.nv) - if mjm.nv - else np.zeros(njmax, dtype=int), - (nworld, 1), - ), - dtype=int, - ) - efc.J_colind = wp.array( - np.tile(np.arange(mjm.nv), (nworld, njmax)).reshape((nworld, 1, -1)), - dtype=int, - ) - - mj_efc_J = np.zeros((mjd.nefc, mjm.nv)) + if is_sparse(mjm): + J_rownnz = np.zeros(njmax, dtype=np.int32) + J_rowadr = np.zeros(njmax, dtype=np.int32) + J_colind = np.zeros(njmax_nnz, dtype=np.int32) + J = np.zeros(njmax_nnz, dtype=np.float64) if mjd.nefc: if mujoco.mj_isSparse(mjm): - mujoco.mju_sparse2dense( - mj_efc_J, - mjd.efc_J, - mjd.efc_J_rownnz, - mjd.efc_J_rowadr, - mjd.efc_J_colind, - ) + J_rownnz[: mjd.nefc] = mjd.efc_J_rownnz[: mjd.nefc] + J_rowadr[: mjd.nefc] = mjd.efc_J_rowadr[: mjd.nefc] + nnz = int(mjd.efc_J_rownnz[: mjd.nefc].sum()) + J_colind[:nnz] = mjd.efc_J_colind[:nnz] + J[:nnz] = mjd.efc_J[:nnz] else: - mj_efc_J = mjd.efc_J.reshape((mjd.nefc, mjm.nv)) - efc_J = np.zeros((njmax, mjm.nv), dtype=float) - efc_J[: mjd.nefc, : mjm.nv] = mj_efc_J - efc.J = wp.array( - np.tile(efc_J.reshape(-1), (nworld, 1, 1)).reshape((nworld, 1, -1)), - dtype=float, - ) + dense_J = mjd.efc_J.reshape((-1, mjm.nv))[: mjd.nefc] + mujoco.mju_dense2sparse( + J[: mjd.nefc * mjm.nv], dense_J, J_rownnz[: mjd.nefc], J_rowadr[: mjd.nefc], J_colind[: mjd.nefc * mjm.nv] + ) + + efc.J_rownnz = wp.array(np.tile(J_rownnz, (nworld, 1)), dtype=int) + efc.J_rowadr = wp.array(np.tile(J_rowadr, (nworld, 1)), dtype=int) + efc.J_colind = wp.array(np.tile(J_colind, (nworld, 1)).reshape((nworld, 1, -1)), dtype=int) + efc.J = wp.array(np.tile(J, (nworld, 1)).reshape((nworld, 1, -1)), dtype=float) else: efc.J_rownnz = wp.zeros((nworld, 0), dtype=int) efc.J_rowadr = wp.zeros((nworld, 0), dtype=int) @@ -990,13 +1175,7 @@ def put_data( mj_efc_J = np.zeros((mjd.nefc, mjm.nv)) if mjd.nefc: if mujoco.mj_isSparse(mjm): - mujoco.mju_sparse2dense( - mj_efc_J, - mjd.efc_J, - mjd.efc_J_rownnz, - mjd.efc_J_rowadr, - mjd.efc_J_colind, - ) + mujoco.mju_sparse2dense(mj_efc_J, mjd.efc_J, mjd.efc_J_rownnz, mjd.efc_J_rowadr, mjd.efc_J_colind) else: mj_efc_J = mjd.efc_J.reshape((mjd.nefc, mjm.nv)) efc_J = np.zeros((nworld, sizes["njmax_pad"], sizes["nv_pad"]), dtype=float) @@ -1005,22 +1184,22 @@ def put_data( # create data d_kwargs = { - "contact": contact, - "efc": efc, - "nworld": nworld, - "naconmax": naconmax, - "naccdmax": naccdmax, - "njmax": njmax, - "njmax_pad": sizes["njmax_pad"], - # fields set after initialization: - "solver_niter": None, - "qM": None, - "qLD": None, - "ten_J": None, - "nacon": None, - # island arrays - "nisland": None, - "tree_island": None, + "contact": contact, + "efc": efc, + "nworld": nworld, + "naconmax": naconmax, + "naccdmax": naccdmax, + "njmax": njmax, + "njmax_pad": sizes["njmax_pad"], + "njmax_nnz": njmax_nnz, + # fields set after initialization: + "solver_niter": None, + "qM": None, + "qLD": None, + "nacon": None, + # island arrays + "nisland": None, + "tree_island": None, } for f in dataclasses.fields(types.Data): if f.name in d_kwargs: @@ -1050,29 +1229,6 @@ def put_data( d.nisland = wp.array(np.full(nworld, mjd.nisland), dtype=int) d.tree_island = wp.array(np.tile(mjd.tree_island, (nworld, 1)), dtype=int) - ten_J = np.zeros((mjm.ntendon, mjm.nv)) - if mujoco.mj_isSparse(mjm) or check_version("mujoco>=3.5.1.dev872479828"): - if mjm.ntendon: - if check_version("mujoco>=3.5.1.dev875093374"): - mujoco.mju_sparse2dense( - ten_J, - mjd.ten_J.reshape(-1), - mjm.ten_J_rownnz, - mjm.ten_J_rowadr, - mjm.ten_J_colind.reshape(-1), - ) - else: - mujoco.mju_sparse2dense( - ten_J, - mjd.ten_J.reshape(-1), - mjd.ten_J_rownnz, - mjd.ten_J_rowadr, - mjd.ten_J_colind.reshape(-1), - ) - else: - ten_J = mjd.ten_J.reshape((mjm.ntendon, mjm.nv)) - d.ten_J = wp.array(np.full((nworld, mjm.ntendon, mjm.nv), ten_J), dtype=float) - d.nacon = wp.array([mjd.ncon * nworld], dtype=int) return d @@ -1233,14 +1389,14 @@ def get_data_into( mujoco.mj_factorM(mjm, result) if nefc > 0: - if SPARSE_CONSTRAINT_JACOBIAN: + if is_sparse(mjm): efc_J = np.zeros((nefc, mjm.nv)) mujoco.mju_sparse2dense( - efc_J, - d.efc.J.numpy()[world_id, 0], - d.efc.J_rownnz.numpy()[world_id, :nefc], - d.efc.J_rowadr.numpy()[world_id, :nefc], - d.efc.J_colind.numpy()[world_id, 0], + efc_J, + d.efc.J.numpy()[world_id, 0], + d.efc.J_rownnz.numpy()[world_id, :nefc], + d.efc.J_rowadr.numpy()[world_id, :nefc], + d.efc.J_colind.numpy()[world_id, 0], ) else: efc_J = d.efc.J.numpy()[world_id, :nefc, : mjm.nv] @@ -1248,11 +1404,11 @@ def get_data_into( # write to mujoco result (format depends on mj_isSparse) if mujoco.mj_isSparse(mjm): mujoco.mju_dense2sparse( - result.efc_J, - efc_J[efc_idx], - result.efc_J_rownnz, - result.efc_J_rowadr, - result.efc_J_colind, + result.efc_J, + efc_J[efc_idx], + result.efc_J_rownnz, + result.efc_J_rowadr, + result.efc_J_colind, ) else: result.efc_J[: nefc * mjm.nv] = efc_J[efc_idx].flatten() @@ -1276,24 +1432,7 @@ def get_data_into( # tendon result.ten_length[:] = d.ten_length.numpy()[world_id] - if check_version("mujoco>=3.5.1.dev869712136"): - ten_J = d.ten_J.numpy()[world_id] - if check_version("mujoco>=3.5.1.dev875093374"): - ten_J_rownnz = mjm.ten_J_rownnz - ten_J_rowadr = mjm.ten_J_rowadr - ten_J_colind = mjm.ten_J_colind.reshape(-1) - else: - ten_J_rownnz = result.ten_J_rownnz - ten_J_rowadr = result.ten_J_rowadr - ten_J_colind = result.ten_J_colind.reshape(-1) - mujoco.mju_dense2sparse( - result.ten_J, - ten_J, - ten_J_rownnz, - ten_J_rowadr, - ten_J_colind, - ) - else: + if mjm.ntendon > 0: result.ten_J[:] = d.ten_J.numpy()[world_id] result.ten_wrapadr[:] = d.ten_wrapadr.numpy()[world_id] result.ten_wrapnum[:] = d.ten_wrapnum.numpy()[world_id] @@ -1428,12 +1567,8 @@ def reset_data(m: types.Model, d: types.Data, reset: Optional[wp.array] = None): mocapid = body_mocapid[bodyid] if mocapid >= 0: - mocap_pos_out[worldid, mocapid] = body_pos[ - worldid % body_pos.shape[0], bodyid - ] - mocap_quat_out[worldid, mocapid] = body_quat[ - worldid % body_quat.shape[0], bodyid - ] + mocap_pos_out[worldid, mocapid] = body_pos[worldid % body_pos.shape[0], bodyid] + mocap_quat_out[worldid, mocapid] = body_quat[worldid % body_quat.shape[0], bodyid] @wp.kernel(module="unique", enable_backward=False) def reset_contact( @@ -1453,6 +1588,8 @@ def reset_data(m: types.Model, d: types.Data, reset: Optional[wp.array] = None): contact_solimp_out: wp.array(dtype=types.vec5), contact_dim_out: wp.array(dtype=int), contact_geom_out: wp.array(dtype=wp.vec2i), + contact_flex_out: wp.array(dtype=wp.vec2i), + contact_vert_out: wp.array(dtype=wp.vec2i), contact_efc_address_out: wp.array2d(dtype=int), contact_worldid_out: wp.array(dtype=int), contact_type_out: wp.array(dtype=int), @@ -1479,8 +1616,10 @@ def reset_data(m: types.Model, d: types.Data, reset: Optional[wp.array] = None): contact_solimp_out[conid] = types.vec5(0.0, 0.0, 0.0, 0.0, 0.0) contact_dim_out[conid] = 0 contact_geom_out[conid] = wp.vec2i(0, 0) + contact_flex_out[conid] = wp.vec2i(0, 0) + contact_vert_out[conid] = wp.vec2i(0, 0) for i in range(nefcaddress): - contact_efc_address_out[conid, i] = 0 + contact_efc_address_out[conid, i] = -1 contact_worldid_out[conid] = 0 contact_type_out[conid] = 0 contact_geomcollisionid_out[conid] = 0 @@ -1519,6 +1658,8 @@ def reset_data(m: types.Model, d: types.Data, reset: Optional[wp.array] = None): d.contact.solimp, d.contact.dim, d.contact.geom, + d.contact.flex, + d.contact.vert, d.contact.efc_address, d.contact.worldid, d.contact.type, @@ -1812,28 +1953,45 @@ def _finalize_body_invweight0( @wp.kernel def _copy_tendon_jacobian( tenid_target: int, - ten_J_in: wp.array3d(dtype=float), + ten_J_rownnz: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), + ten_J_in: wp.array2d(dtype=float), ten_J_vec_out: wp.array2d(dtype=float), ): worldid = wp.tid() nv = ten_J_in.shape[2] - for i in range(nv): - ten_J_vec_out[worldid, i] = ten_J_in[worldid, tenid_target, i] + rownnz = ten_J_rownnz[tenid_target] + rowadr = ten_J_rowadr[tenid_target] + for i in range(rownnz): + colind = ten_J_colind[rowadr + i] + ten_J_vec_out[worldid, colind] = ten_J_in[worldid, rowadr + i] @wp.kernel def _compute_tendon_dot_product( + # Model: + ten_J_rownnz: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), + # In: tenid_target: int, - nv: int, - ten_J_in: wp.array3d(dtype=float), + ten_J_in: wp.array2d(dtype=float), result_vec_in: wp.array2d(dtype=float), + # Out: tendon_invweight0_out: wp.array2d(dtype=float), ): worldid = wp.tid() tendon_invweight0_id = worldid % tendon_invweight0_out.shape[0] dot_prod = float(0.0) - for i in range(nv): - dot_prod += ten_J_in[worldid, tenid_target, i] * result_vec_in[worldid, i] + + rownnz = ten_J_rownnz[tenid_target] + rowadr = ten_J_rowadr[tenid_target] + for i in range(rownnz): + sparseid = rowadr + i + colind = ten_J_colind[sparseid] + dot_prod += ten_J_in[worldid, sparseid] * result_vec_in[worldid, colind] + tendon_invweight0_out[tendon_invweight0_id, tenid_target] = dot_prod @@ -1891,12 +2049,12 @@ def _compute_light_pos0( @wp.kernel def _copy_actuator_moment( - actid_target: int, - moment_rownnz_in: wp.array2d(dtype=int), - moment_rowadr_in: wp.array2d(dtype=int), - moment_colind_in: wp.array2d(dtype=int), - actuator_moment_in: wp.array2d(dtype=float), - act_moment_vec_out: wp.array2d(dtype=float), + actid_target: int, + moment_rownnz_in: wp.array2d(dtype=int), + moment_rowadr_in: wp.array2d(dtype=int), + moment_colind_in: wp.array2d(dtype=int), + actuator_moment_in: wp.array2d(dtype=float), + act_moment_vec_out: wp.array2d(dtype=float), ): worldid = wp.tid() nv = act_moment_vec_out.shape[1] @@ -1912,10 +2070,10 @@ def _copy_actuator_moment( @wp.kernel def _compute_actuator_acc0( - actid_target: int, - nv: int, - result_vec_in: wp.array2d(dtype=float), - actuator_acc0_out: wp.array2d(dtype=float), + actid_target: int, + nv: int, + result_vec_in: wp.array2d(dtype=float), + actuator_acc0_out: wp.array2d(dtype=float), ): worldid = wp.tid() norm_sq = float(0.0) @@ -1926,11 +2084,11 @@ def _compute_actuator_acc0( @wp.kernel def _compute_dof_M0( - dof_bodyid: wp.array(dtype=int), - dof_armature: wp.array2d(dtype=float), - cdof_in: wp.array2d(dtype=wp.spatial_vector), - crb_in: wp.array2d(dtype=vec10), - dof_M0_out: wp.array2d(dtype=float), + dof_bodyid: wp.array(dtype=int), + dof_armature: wp.array2d(dtype=float), + cdof_in: wp.array2d(dtype=wp.spatial_vector), + crb_in: wp.array2d(dtype=vec10), + dof_M0_out: wp.array2d(dtype=float), ): worldid, dofid = wp.tid() bodyid = dof_bodyid[dofid] @@ -1941,15 +2099,15 @@ def _compute_dof_M0( @wp.kernel def _resolve_dampratio( - actuator_biastype: wp.array(dtype=int), - actuator_gainprm: wp.array2d(dtype=types.vec10f), - moment_rownnz_in: wp.array2d(dtype=int), - moment_rowadr_in: wp.array2d(dtype=int), - moment_colind_in: wp.array2d(dtype=int), - actuator_moment_in: wp.array2d(dtype=float), - dof_M0_in: wp.array2d(dtype=float), - nv: int, - actuator_biasprm: wp.array2d(dtype=types.vec10f), + actuator_biastype: wp.array(dtype=int), + actuator_gainprm: wp.array2d(dtype=types.vec10f), + moment_rownnz_in: wp.array2d(dtype=int), + moment_rowadr_in: wp.array2d(dtype=int), + moment_colind_in: wp.array2d(dtype=int), + actuator_moment_in: wp.array2d(dtype=float), + dof_M0_in: wp.array2d(dtype=float), + nv: int, + actuator_biasprm: wp.array2d(dtype=types.vec10f), ): worldid, actid = wp.tid() biastype = actuator_biastype[actid] @@ -1992,15 +2150,15 @@ def _resolve_dampratio( @wp.kernel def _set_length_range( - actuator_trntype: wp.array(dtype=int), - actuator_trnid: wp.array(dtype=wp.vec2i), - actuator_gear: wp.array2d(dtype=wp.spatial_vector), - jnt_limited: wp.array(dtype=int), - jnt_range: wp.array2d(dtype=wp.vec2), - tendon_limited: wp.array(dtype=int), - tendon_range: wp.array2d(dtype=wp.vec2), - ntendon: int, - actuator_lengthrange_out: wp.array2d(dtype=wp.vec2), + actuator_trntype: wp.array(dtype=int), + actuator_trnid: wp.array(dtype=wp.vec2i), + actuator_gear: wp.array2d(dtype=wp.spatial_vector), + jnt_limited: wp.array(dtype=int), + jnt_range: wp.array2d(dtype=wp.vec2), + tendon_limited: wp.array(dtype=int), + tendon_range: wp.array2d(dtype=wp.vec2), + ntendon: int, + actuator_lengthrange_out: wp.array2d(dtype=wp.vec2), ): worldid, actid = wp.tid() trntype = actuator_trntype[actid] @@ -2167,16 +2325,22 @@ def set_const_0(m: types.Model, d: types.Data): # tendon_invweight0[t] = J_t * inv(M) * J_t' if m.ntendon > 0: - ten_J_vec = wp.zeros((d.nworld, m.nv), dtype=float) - ten_result_vec = wp.zeros((d.nworld, m.nv), dtype=float) + ten_J_vec = wp.empty((d.nworld, m.nv), dtype=float) + ten_result_vec = wp.empty((d.nworld, m.nv), dtype=float) for tenid in range(m.ntendon): - wp.launch(_copy_tendon_jacobian, dim=d.nworld, inputs=[tenid, d.ten_J], outputs=[ten_J_vec]) + ten_J_vec.zero_() + wp.launch( + _copy_tendon_jacobian, + dim=d.nworld, + inputs=[tenid, m.ten_J_rownnz, m.ten_J_rowadr, m.ten_J_colind, d.ten_J], + outputs=[ten_J_vec], + ) smooth.solve_m(m, d, ten_result_vec, ten_J_vec) wp.launch( _compute_tendon_dot_product, dim=d.nworld, - inputs=[tenid, m.nv, d.ten_J, ten_result_vec], + inputs=[m.ten_J_rownnz, m.ten_J_rowadr, m.ten_J_colind, tenid, d.ten_J, ten_result_vec], outputs=[m.tendon_invweight0], ) @@ -2201,16 +2365,10 @@ def set_const_0(m: types.Model, d: types.Data): for actid in range(m.nu): wp.launch( - _copy_actuator_moment, - dim=d.nworld, - inputs=[ - actid, - d.moment_rownnz, - d.moment_rowadr, - d.moment_colind, - d.actuator_moment, - ], - outputs=[act_moment_vec], + _copy_actuator_moment, + dim=d.nworld, + inputs=[actid, d.moment_rownnz, d.moment_rowadr, d.moment_colind, d.actuator_moment], + outputs=[act_moment_vec], ) smooth.solve_m(m, d, act_result_vec, act_moment_vec) wp.launch(_compute_actuator_acc0, dim=d.nworld, inputs=[actid, m.nv, act_result_vec], outputs=[m.actuator_acc0]) @@ -2219,25 +2377,25 @@ def set_const_0(m: types.Model, d: types.Data): if m.nu > 0 and m.nv > 0: dof_M0 = wp.zeros((d.nworld, m.nv), dtype=float) wp.launch( - _compute_dof_M0, - dim=(d.nworld, m.nv), - inputs=[m.dof_bodyid, m.dof_armature, d.cdof, d.crb], - outputs=[dof_M0], + _compute_dof_M0, + dim=(d.nworld, m.nv), + inputs=[m.dof_bodyid, m.dof_armature, d.cdof, d.crb], + outputs=[dof_M0], ) wp.launch( - _resolve_dampratio, - dim=(d.nworld, m.nu), - inputs=[ - m.actuator_biastype, - m.actuator_gainprm, - d.moment_rownnz, - d.moment_rowadr, - d.moment_colind, - d.actuator_moment, - dof_M0, - m.nv, - ], - outputs=[m.actuator_biasprm], + _resolve_dampratio, + dim=(d.nworld, m.nu), + inputs=[ + m.actuator_biastype, + m.actuator_gainprm, + d.moment_rownnz, + d.moment_rowadr, + d.moment_colind, + d.actuator_moment, + dof_M0, + m.nv, + ], + outputs=[m.actuator_biasprm], ) wp.copy(d.qpos, qpos_saved) @@ -2255,16 +2413,12 @@ def set_const(m: types.Model, d: types.Data): Field | Notes ---------------------------------|---------------------------------------------- qpos0, qpos_spring | - body_mass, body_inertia, | Mass and inertia are usually scaled - together + body_mass, body_inertia, | Mass and inertia are usually scaled together body_ipos, body_iquat | since inertia is sum(m * r^2). - body_pos, body_quat | Unsafe for static bodies (invalidates - BVH). - body_gravcomp | If changing from 0 to >0 bodies, - required. + body_pos, body_quat | Unsafe for static bodies (invalidates BVH). + body_gravcomp | If changing from 0 to >0 bodies, required. dof_armature | - eq_data | For connect/weld, offsets computed if not - set. + eq_data | For connect/weld, offsets computed if not set. hfield_size | tendon_stiffness, tendon_damping | Only if changing from/to zero. actuator_gainprm, actuator_biasprm | For position actuators with dampratio. @@ -2319,19 +2473,19 @@ def set_length_range(m: types.Model, d: types.Data, index: int = -1): return wp.launch( - _set_length_range, - dim=(d.nworld, m.nu), - inputs=[ - m.actuator_trntype, - m.actuator_trnid, - m.actuator_gear, - m.jnt_limited, - m.jnt_range, - m.tendon_limited, - m.tendon_range, - m.ntendon, - ], - outputs=[m.actuator_lengthrange], + _set_length_range, + dim=(d.nworld, m.nu), + inputs=[ + m.actuator_trntype, + m.actuator_trnid, + m.actuator_gear, + m.jnt_limited, + m.jnt_range, + m.tendon_limited, + m.tendon_range, + m.ntendon, + ], + outputs=[m.actuator_lengthrange], ) @@ -2486,6 +2640,7 @@ def create_render_context( cam_res: list[tuple[int, int]] | tuple[int, int] | None = None, render_rgb: list[bool] | bool | None = None, render_depth: list[bool] | bool | None = None, + render_seg: list[bool] | bool | None = None, use_textures: bool = True, use_shadows: bool = False, enabled_geom_groups: list[int] = [0, 1, 2], @@ -2502,6 +2657,8 @@ def create_render_context( MuJoCo model values. render_rgb: Whether to render RGB images. If None, uses the MuJoCo model values. render_depth: Whether to render depth images. If None, uses the MuJoCo model values. + render_seg: Whether to render segmentation (per-pixel geom IDs). If None, + uses the MuJoCo model values. use_textures: Whether to use textures. use_shadows: Whether to use shadows. enabled_geom_groups: The geom groups to render. @@ -2517,10 +2674,13 @@ def create_render_context( mjd = mujoco.MjData(mjm) mujoco.mj_forward(mjm, mjd) - # TODO(team): remove after mjwarp depends on warp-lang >= 1.12 in pyproject.toml - if use_textures and not hasattr(wp, "Texture2D"): - warnings.warn("Textures require warp >= 1.12. Disabling textures.") - use_textures = False + constructor = "sah" + if check_version("warp>=1.13.0.dev20260325"): + # TODO: The cubql constructor and is_cubql_available exist only in + # recent Warp 1.13+ builds, modify this after warp is updated to 1.13+. + _cubql_avail = getattr(wp, "is_cubql_available", None) + if callable(_cubql_avail) and _cubql_avail(): + constructor = "cubql" # Mesh BVHs nmesh = mjm.nmesh @@ -2534,7 +2694,7 @@ def create_render_context( mesh_bounds_size = [wp.vec3(0.0, 0.0, 0.0) for _ in range(nmesh)] for mid in used_mesh_id: - mesh, half = bvh.build_mesh_bvh(mjm, mid) + mesh, half = bvh.build_mesh_bvh(mjm, mid, constructor=constructor) mesh_registry[mesh.id] = mesh mesh_bvh_id[mid] = mesh.id mesh_bounds_size[mid] = half @@ -2551,7 +2711,7 @@ def create_render_context( hfield_bounds_size = [wp.vec3(0.0, 0.0, 0.0) for _ in range(nhfield)] for hid in used_hfield_id: - hmesh, hhalf = bvh.build_hfield_bvh(mjm, hid) + hmesh, hhalf = bvh.build_hfield_bvh(mjm, hid, constructor=constructor) hfield_registry[hmesh.id] = hmesh hfield_bvh_id[hid] = hmesh.id hfield_bounds_size[hid] = hhalf @@ -2560,65 +2720,33 @@ def create_render_context( hfield_bounds_size_arr = wp.array(hfield_bounds_size, dtype=wp.vec3) # Flex BVHs - flex_bvh_id = wp.uint64(0) - flex_group_root = wp.zeros(nworld, dtype=int) - flex_mesh = None - flex_face_point = None - flex_elemdataadr = None - flex_shell = None - flex_shelldataadr = None - flex_faceadr = None - flex_nface = 0 - flex_radius = None - flex_workadr = None - flex_worknum = None - flex_nwork = 0 + nflex = mjm.nflex + flex_registry = {} - if mjm.nflex > 0: - ( - fmesh, - face_point, - flex_group_roots, - flex_shell_data, - flex_faceadr_data, - flex_nface, - ) = bvh.build_flex_bvh(mjm, mjd, nworld) + # Scene BVH flex primitives: 1D → one capsule per edge, 2D/3D → one box per flex + flex_geom_flexid = [] + flex_geom_edgeid = [] + flex_bvh_id = np.full(nflex, 0, dtype=wp.uint64) + flex_group_root = np.zeros((nflex, nworld), dtype=int) - flex_mesh = fmesh - flex_bvh_id = fmesh.id - flex_face_point = face_point - flex_group_root = flex_group_roots - flex_elemdataadr = wp.array(mjm.flex_elemdataadr, dtype=int) - flex_shell = flex_shell_data - flex_shelldataadr = wp.array(mjm.flex_shelldataadr, dtype=int) - flex_faceadr = wp.array(flex_faceadr_data, dtype=int) - flex_radius = wp.array(mjm.flex_radius, dtype=float) - - # precompute work item layout for unified refit kernel - nflex = mjm.nflex - workadr = np.zeros(nflex, dtype=np.int32) - worknum = np.zeros(nflex, dtype=np.int32) - cumsum = 0 - for f in range(nflex): - workadr[f] = cumsum - if mjm.flex_dim[f] == 2: - worknum[f] = mjm.flex_elemnum[f] + mjm.flex_shellnum[f] - else: - worknum[f] = mjm.flex_shellnum[f] - cumsum += worknum[f] - flex_workadr = wp.array(workadr, dtype=int) - flex_worknum = wp.array(worknum, dtype=int) - flex_nwork = int(cumsum) + for f in range(nflex): + if mjm.flex_dim[f] == 1: + edge_adr = mjm.flex_edgeadr[f] + flex_geom_flexid.extend([f] * mjm.flex_edgenum[f]) + flex_geom_edgeid.extend([edge_adr + e for e in range(mjm.flex_edgenum[f])]) + flex_group_root[f] = np.zeros(nworld, dtype=int) + else: + flex_geom_flexid.append(f) + flex_geom_edgeid.append(-1) + fmesh, group_root = bvh.build_flex_bvh(mjm, mjd, nworld, f) + flex_registry[f] = fmesh + flex_bvh_id[f] = fmesh.id + flex_group_root[f] = group_root.numpy() textures_registry = [] - # TODO: remove after mjwarp depends on warp-lang >= 1.12 in pyproject.toml - if hasattr(wp, "Texture2D"): - for i in range(mjm.ntex): - textures_registry.append(render_util.create_warp_texture(mjm, i)) - textures = wp.array(textures_registry, dtype=wp.Texture2D) - else: - # Dummy array when texture support isn't available (warp < 1.12) - textures = wp.zeros(1, dtype=int) + for i in range(mjm.ntex): + textures_registry.append(render_util.create_warp_texture(mjm, i)) + textures = wp.array(textures_registry, dtype=wp.Texture2D) # Filter active cameras if cam_active is not None: @@ -2642,31 +2770,31 @@ def create_render_context( cam_res_arr = wp.array(active_cam_res, dtype=wp.vec2i) if render_rgb is None: - render_rgb = [ - mjm.cam_output[i] & mujoco.mjtCamOutBit.mjCAMOUT_RGB - for i in active_cam_indices - ] + render_rgb = [mjm.cam_output[i] & mujoco.mjtCamOutBit.mjCAMOUT_RGB for i in active_cam_indices] elif isinstance(render_rgb, bool): render_rgb = [render_rgb] * ncam if render_depth is None: - render_depth = [ - mjm.cam_output[i] & mujoco.mjtCamOutBit.mjCAMOUT_DEPTH - for i in active_cam_indices - ] + render_depth = [mjm.cam_output[i] & mujoco.mjtCamOutBit.mjCAMOUT_DEPTH for i in active_cam_indices] if isinstance(render_depth, bool): render_depth = [render_depth] * ncam - assert len(render_rgb) == ncam and len(render_depth) == ncam, ( - "render_rgb and render_depth must be a bool or a list of bools with" - f" length {ncam}" + if render_seg is None: + render_seg = [mjm.cam_output[i] & mujoco.mjtCamOutBit.mjCAMOUT_SEG for i in active_cam_indices] + elif isinstance(render_seg, bool): + render_seg = [render_seg] * ncam + + assert len(render_rgb) == ncam and len(render_depth) == ncam and len(render_seg) == ncam, ( + f"render_rgb, render_depth, and render_seg must be a bool or a list of bools with length {ncam}" ) rgb_adr = -1 * np.ones(ncam, dtype=int) depth_adr = -1 * np.ones(ncam, dtype=int) + seg_adr = -1 * np.ones(ncam, dtype=int) cam_res_np = cam_res_arr.numpy() ri = 0 di = 0 + si = 0 total = 0 for idx in range(ncam): @@ -2676,6 +2804,9 @@ def create_render_context( if render_depth[idx]: depth_adr[idx] = di di += cam_res_np[idx][0] * cam_res_np[idx][1] + if render_seg[idx]: + seg_adr[idx] = si + si += cam_res_np[idx][0] * cam_res_np[idx][1] total += cam_res_np[idx][0] * cam_res_np[idx][1] @@ -2729,26 +2860,20 @@ def create_render_context( hfield_registry=hfield_registry, hfield_bvh_id=hfield_bvh_id_arr, hfield_bounds_size=hfield_bounds_size_arr, - flex_mesh=flex_mesh, + flex_mesh_registry=flex_registry, flex_rgba=wp.array(mjm.flex_rgba, dtype=wp.vec4), - flex_bvh_id=flex_bvh_id, - flex_face_point=flex_face_point, - flex_faceadr=flex_faceadr, - flex_nface=flex_nface, - flex_nwork=flex_nwork, - flex_group_root=flex_group_root, - flex_elemdataadr=flex_elemdataadr, - flex_shell=flex_shell, - flex_shelldataadr=flex_shelldataadr, - flex_radius=flex_radius, - flex_workadr=flex_workadr, - flex_worknum=flex_worknum, + flex_bvh_id=wp.array(flex_bvh_id, dtype=wp.uint64), + flex_group_root=wp.array(flex_group_root, dtype=int), flex_render_smooth=flex_render_smooth, + bvh_nflexgeom=len(flex_geom_flexid), + flex_dim_np=mjm.flex_dim, + flex_geom_flexid=wp.array(flex_geom_flexid, dtype=int), + flex_geom_edgeid=wp.array(flex_geom_edgeid, dtype=int), bvh=None, bvh_id=None, - lower=wp.zeros(nworld * bvh_ngeom, dtype=wp.vec3), - upper=wp.zeros(nworld * bvh_ngeom, dtype=wp.vec3), - group=wp.zeros(nworld * bvh_ngeom, dtype=int), + lower=wp.zeros(nworld * (bvh_ngeom + len(flex_geom_flexid)), dtype=wp.vec3), + upper=wp.zeros(nworld * (bvh_ngeom + len(flex_geom_flexid)), dtype=wp.vec3), + group=wp.zeros(nworld * (bvh_ngeom + len(flex_geom_flexid)), dtype=int), group_root=wp.zeros(nworld, dtype=int), ray=ray, rgb_data=wp.zeros((nworld, ri), dtype=wp.uint32), @@ -2757,6 +2882,9 @@ def create_render_context( depth_adr=wp.array(depth_adr, dtype=int), render_rgb=wp.array(render_rgb, dtype=bool), render_depth=wp.array(render_depth, dtype=bool), + seg_data=wp.zeros((nworld, max(si, 1)), dtype=int), + seg_adr=wp.array(seg_adr, dtype=int), + render_seg=wp.array(render_seg, dtype=bool), znear=znear, total_rays=int(total), ) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/island.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/island.py index b5ae2846..c021db3f 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/island.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/island.py @@ -13,12 +13,13 @@ # limitations under the License. # ============================================================================== +import warp as wp + from mujoco.mjx.third_party.mujoco_warp._src import types from mujoco.mjx.third_party.mujoco_warp._src.types import ConstraintType from mujoco.mjx.third_party.mujoco_warp._src.types import EqType from mujoco.mjx.third_party.mujoco_warp._src.types import ObjType from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope -import warp as wp @wp.kernel @@ -180,17 +181,17 @@ def tree_edges(m: types.Model, d: types.Data, tree_tree: wp.array3d(dtype=int)): @wp.kernel def _flood_fill( - # Model: - ntree: int, - # In: - tree_tree_in: wp.array3d(dtype=int), - labels_in: wp.array2d(dtype=int), - stack_in: wp.array2d(dtype=int), - # Data out: - nisland_out: wp.array(dtype=int), - tree_island_out: wp.array2d(dtype=int), - # Out: - stack_out: wp.array2d(dtype=int), + # Model: + ntree: int, + # In: + tree_tree_in: wp.array3d(dtype=int), + labels_in: wp.array2d(dtype=int), + stack_in: wp.array2d(dtype=int), + # Data out: + nisland_out: wp.array(dtype=int), + tree_island_out: wp.array2d(dtype=int), + # Out: + stack_out: wp.array2d(dtype=int), ): """DFS flood fill to discover islands using tree_tree matrix.""" worldid = wp.tid() @@ -257,8 +258,8 @@ def island(m: types.Model, d: types.Data): stack_scratch = wp.empty((d.nworld, m.ntree * m.ntree), dtype=int) wp.launch( - _flood_fill, - dim=d.nworld, - inputs=[m.ntree, tree_tree, d.tree_island, stack_scratch], - outputs=[d.nisland, d.tree_island, stack_scratch], + _flood_fill, + dim=d.nworld, + inputs=[m.ntree, tree_tree, d.tree_island, stack_scratch], + outputs=[d.nisland, d.tree_island, stack_scratch], ) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/math.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/math.py index ec49041e..3ce2d1c5 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/math.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/math.py @@ -83,6 +83,35 @@ def quat_to_mat(quat: wp.quat) -> wp.mat33: ) +@wp.func +def quat_z2vec(vec: wp.vec3) -> wp.quat: + """Compute quaternion performing rotation from z-axis to given vector.""" + quat = wp.quat(0.0, 0.0, 0.0, 1.0) + + # normalize vector; if too small, no rotation + norm = wp.length(vec) + if norm < types.MJ_MINVAL: + return quat + vec = vec / norm + + axis = wp.vec3(-vec[1], vec[0], 0.0) + a = wp.length(axis) + + # almost parallel + if a < types.MJ_MINVAL: + # opposite: 180 deg rotation around x axis + if vec[2] < 0.0: + quat = wp.quat(1.0, 0.0, 0.0, 0.0) + return quat + + # make quaternion from angle and axis + axis = axis / a + angle = wp.atan2(a, vec[2]) + quat = axis_angle_to_quat(axis, angle) + + return quat + + @wp.func def quat_inv(quat: wp.quat) -> wp.quat: return wp.quat(quat[0], -quat[1], -quat[2], -quat[3]) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py index 0bcd13af..4abcff26 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py @@ -89,8 +89,8 @@ def _spring_damper_dof_passive( stiffness = jnt_stiffness[worldid % jnt_stiffness.shape[0], jntid] damping = dof_damping[worldid % dof_damping.shape[0], dofid] - has_stiffness = stiffness != 0.0 and not opt_disableflags & DisableBit.SPRING - has_damping = damping != 0.0 and not opt_disableflags & DisableBit.DAMPER + has_stiffness = stiffness != 0.0 and not (opt_disableflags & DisableBit.SPRING) + has_damping = damping != 0.0 and not (opt_disableflags & DisableBit.DAMPER) if not has_stiffness: qfrc_spring_out[worldid, dofid] = 0.0 @@ -182,11 +182,14 @@ def _spring_damper_dof_passive( @wp.kernel def _spring_damper_tendon_passive( # Model: + ten_J_rownnz: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), tendon_stiffness: wp.array2d(dtype=float), tendon_damping: wp.array2d(dtype=float), tendon_lengthspring: wp.array2d(dtype=wp.vec2), # Data in: - ten_J_in: wp.array3d(dtype=float), + ten_J_in: wp.array2d(dtype=float), ten_length_in: wp.array2d(dtype=float), ten_velocity_in: wp.array2d(dtype=float), # In: @@ -196,7 +199,7 @@ def _spring_damper_tendon_passive( qfrc_spring_out: wp.array2d(dtype=float), qfrc_damper_out: wp.array2d(dtype=float), ): - worldid, tenid, dofid = wp.tid() + worldid, tenid, dofid_sparse = wp.tid() stiffness = tendon_stiffness[worldid % tendon_stiffness.shape[0], tenid] damping = tendon_damping[worldid % tendon_damping.shape[0], tenid] @@ -207,7 +210,13 @@ def _spring_damper_tendon_passive( if not has_stiffness and not has_damping: return - J = ten_J_in[worldid, tenid, dofid] + rownnz = ten_J_rownnz[tenid] + if dofid_sparse >= rownnz: + return + rowadr = ten_J_rowadr[tenid] + sparseid = rowadr + dofid_sparse + J = ten_J_in[worldid, sparseid] + dofid = ten_J_colind[sparseid] if has_stiffness: # compute spring force along tendon @@ -265,28 +274,28 @@ def _gravity_force( @wp.kernel def _fluid_force( - # Model: - opt_wind: wp.array(dtype=wp.vec3), - opt_density: wp.array(dtype=float), - opt_viscosity: wp.array(dtype=float), - body_rootid: wp.array(dtype=int), - body_geomnum: wp.array(dtype=int), - body_geomadr: wp.array(dtype=int), - body_mass: wp.array2d(dtype=float), - body_inertia: wp.array2d(dtype=wp.vec3), - geom_type: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - geom_fluid: wp.array2d(dtype=float), - body_fluid_ellipsoid: wp.array(dtype=bool), - # Data in: - xipos_in: wp.array2d(dtype=wp.vec3), - ximat_in: wp.array2d(dtype=wp.mat33), - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cvel_in: wp.array2d(dtype=wp.spatial_vector), - # Out: - fluid_applied_out: wp.array2d(dtype=wp.spatial_vector), + # Model: + opt_wind: wp.array(dtype=wp.vec3), + opt_density: wp.array(dtype=float), + opt_viscosity: wp.array(dtype=float), + body_rootid: wp.array(dtype=int), + body_geomnum: wp.array(dtype=int), + body_geomadr: wp.array(dtype=int), + body_mass: wp.array2d(dtype=float), + body_inertia: wp.array2d(dtype=wp.vec3), + geom_type: wp.array(dtype=int), + geom_size: wp.array2d(dtype=wp.vec3), + geom_fluid: wp.array2d(dtype=float), + body_fluid_ellipsoid: wp.array(dtype=bool), + # Data in: + xipos_in: wp.array2d(dtype=wp.vec3), + ximat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xmat_in: wp.array2d(dtype=wp.mat33), + subtree_com_in: wp.array2d(dtype=wp.vec3), + cvel_in: wp.array2d(dtype=wp.spatial_vector), + # Out: + fluid_applied_out: wp.array2d(dtype=wp.spatial_vector), ): """Computes body-space fluid forces for both inertia-box and ellipsoid models.""" worldid, bodyid = wp.tid() @@ -495,29 +504,29 @@ def _fluid(m: Model, d: Data): fluid_applied = wp.empty((d.nworld, m.nbody), dtype=wp.spatial_vector) wp.launch( - _fluid_force, - dim=(d.nworld, m.nbody), - inputs=[ - m.opt.wind, - m.opt.density, - m.opt.viscosity, - m.body_rootid, - m.body_geomnum, - m.body_geomadr, - m.body_mass, - m.body_inertia, - m.geom_type, - m.geom_size, - m.geom_fluid, - m.body_fluid_ellipsoid, - d.xipos, - d.ximat, - d.geom_xpos, - d.geom_xmat, - d.subtree_com, - d.cvel, - ], - outputs=[fluid_applied], + _fluid_force, + dim=(d.nworld, m.nbody), + inputs=[ + m.opt.wind, + m.opt.density, + m.opt.viscosity, + m.body_rootid, + m.body_geomnum, + m.body_geomadr, + m.body_mass, + m.body_inertia, + m.geom_type, + m.geom_size, + m.geom_fluid, + m.body_fluid_ellipsoid, + d.xipos, + d.ximat, + d.geom_xpos, + d.geom_xmat, + d.subtree_com, + d.cvel, + ], + outputs=[fluid_applied], ) support.apply_ft(m, d, fluid_applied, d.qfrc_fluid, False) @@ -565,6 +574,7 @@ def _flex_elasticity( flex_edgeadr: wp.array(dtype=int), flex_elemadr: wp.array(dtype=int), flex_elemnum: wp.array(dtype=int), + flex_elemdataadr: wp.array(dtype=int), flex_elemedgeadr: wp.array(dtype=int), flex_vertbodyid: wp.array(dtype=int), flex_elem: wp.array(dtype=int), @@ -590,32 +600,39 @@ def _flex_elasticity( f = i break + local_elemid = elemid - flex_elemadr[f] dim = flex_dim[f] nvert = dim + 1 nedge = nvert * (nvert - 1) / 2 edges = wp.where( - dim == 3, - wp.matrix(0, 1, 1, 2, 2, 0, 2, 3, 0, 3, 1, 3, shape=(6, 2), dtype=int), - wp.matrix(1, 2, 2, 0, 0, 1, 0, 0, 0, 0, 0, 0, shape=(6, 2), dtype=int), + dim == 1, + wp.matrix(0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, shape=(6, 2), dtype=int), + wp.where( + dim == 3, + wp.matrix(0, 1, 1, 2, 2, 0, 2, 3, 0, 3, 1, 3, shape=(6, 2), dtype=int), + wp.matrix(1, 2, 2, 0, 0, 1, 0, 0, 0, 0, 0, 0, shape=(6, 2), dtype=int), + ), ) if timestep > 0.0 and not dsbl_damper: kD = flex_damping[f] / timestep else: kD = 0.0 + elem_data_adr = flex_elemdataadr[f] + local_elemid * (dim + 1) + vbase = flex_vertadr[f] gradient = wp.matrix(0.0, shape=(6, 6)) for e in range(nedge): - vert0 = flex_elem[(dim + 1) * elemid + edges[e, 0]] - vert1 = flex_elem[(dim + 1) * elemid + edges[e, 1]] - xpos0 = flexvert_xpos_in[worldid, vert0] - xpos1 = flexvert_xpos_in[worldid, vert1] + vert0 = flex_elem[elem_data_adr + edges[e, 0]] + vert1 = flex_elem[elem_data_adr + edges[e, 1]] + xpos0 = flexvert_xpos_in[worldid, vbase + vert0] + xpos1 = flexvert_xpos_in[worldid, vbase + vert1] for i in range(3): gradient[e, 0 + i] = xpos0[i] - xpos1[i] gradient[e, 3 + i] = xpos1[i] - xpos0[i] elongation = wp.spatial_vectorf(0.0) for e in range(nedge): - idx = flex_elemedge[elemid * nedge + e] + idx = flex_elemedge[flex_elemedgeadr[f] + local_elemid * nedge + e] vel = flexedge_velocity_in[worldid, flex_edgeadr[f] + idx] deformed = flexedge_length_in[worldid, flex_edgeadr[f] + idx] reference = flexedge_length0[flex_edgeadr[f] + idx] @@ -638,7 +655,7 @@ def _flex_elasticity( force[edges[ed2, i], x] -= elongation[ed1] * gradient[ed2, 3 * i + x] * metric[ed1, ed2] for v in range(nvert): - vert = flex_elem[(dim + 1) * elemid + v] + vert = flex_elem[elem_data_adr + v] bodyid = flex_vertbodyid[flex_vertadr[f] + vert] for x in range(3): wp.atomic_add(qfrc_spring_out, worldid, body_dofadr[bodyid] + x, force[v, x]) @@ -742,8 +759,11 @@ def passive(m: Model, d: Data): if m.ntendon: wp.launch( _spring_damper_tendon_passive, - dim=(d.nworld, m.ntendon, m.nv), + dim=(d.nworld, m.ntendon, m.max_ten_J_rownnz), inputs=[ + m.ten_J_rownnz, + m.ten_J_rowadr, + m.ten_J_colind, m.tendon_stiffness, m.tendon_damping, m.tendon_lengthspring, @@ -772,6 +792,7 @@ def passive(m: Model, d: Data): m.flex_edgeadr, m.flex_elemadr, m.flex_elemnum, + m.flex_elemdataadr, m.flex_elemedgeadr, m.flex_vertbodyid, m.flex_elem, diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/ray.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/ray.py index 5eaea821..57d6a5ba 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/ray.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/ray.py @@ -752,7 +752,8 @@ def ray_mesh_with_bvh_anyhit( @wp.func def ray_flex_with_bvh( # In: - bvh_id: wp.uint64, + flex_bvh_id: wp.array(dtype=wp.uint64), + flexid: int, group_root: int, pnt: wp.vec3, vec: wp.vec3, @@ -769,7 +770,7 @@ def ray_flex_with_bvh( n = wp.vec3(0.0, 0.0, 0.0) f = int(-1) - hit = wp.mesh_query_ray(bvh_id, pnt, vec, max_t, t, u, v, sign, n, f, group_root) + hit = wp.mesh_query_ray(flex_bvh_id[flexid], pnt, vec, max_t, t, u, v, sign, n, f, group_root) if hit: return t, n, u, v, f @@ -777,6 +778,23 @@ def ray_flex_with_bvh( return -1.0, wp.vec3(0.0, 0.0, 0.0), 0.0, 0.0, -1 +@wp.func +def ray_flex_with_bvh_anyhit( + # In: + flex_bvh_id: wp.array(dtype=wp.uint64), + flexid: int, + group_root: int, + pnt: wp.vec3, + vec: wp.vec3, + max_t: float, +) -> bool: + """Returns True if there is any hit for ray flex intersections. + + Requires wp.Mesh be constructed and their ids to be passed. Flex are already in world space. + """ + return wp.mesh_query_ray_anyhit(flex_bvh_id[flexid], pnt, vec, max_t, group_root) + + @wp.func def ray_geom(pos: wp.vec3, mat: wp.mat33, size: wp.vec3, pnt: wp.vec3, vec: wp.vec3, geomtype: int) -> Tuple[float, wp.vec3]: """Returns distance along ray to intersection with geom and normal at intersection point. diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render.py index 28a4284f..bc8d16c3 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render.py @@ -23,6 +23,7 @@ from mujoco.mjx.third_party.mujoco_warp._src.ray import ray_capsule from mujoco.mjx.third_party.mujoco_warp._src.ray import ray_cylinder from mujoco.mjx.third_party.mujoco_warp._src.ray import ray_ellipsoid from mujoco.mjx.third_party.mujoco_warp._src.ray import ray_flex_with_bvh +from mujoco.mjx.third_party.mujoco_warp._src.ray import ray_flex_with_bvh_anyhit from mujoco.mjx.third_party.mujoco_warp._src.ray import ray_mesh_with_bvh from mujoco.mjx.third_party.mujoco_warp._src.ray import ray_mesh_with_bvh_anyhit from mujoco.mjx.third_party.mujoco_warp._src.ray import ray_plane @@ -39,10 +40,6 @@ from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope wp.set_module_options({"enable_backward": False}) -# TODO(team): remove after mjwarp depends on warp-lang >= 1.12 in pyproject.toml -from mujoco.mjx.third_party.mujoco_warp._src.types import TEXTURE_DTYPE - - @wp.func def sample_texture( # Model: @@ -51,7 +48,7 @@ def sample_texture( # In: geom_id: int, tex_repeat: wp.vec2, - tex: TEXTURE_DTYPE, + tex: wp.Texture2D, pos: wp.vec3, rot: wp.mat33, mesh_facetexcoord: wp.array(dtype=wp.vec3i), @@ -94,17 +91,26 @@ def cast_ray( geom_type: wp.array(dtype=int), geom_dataid: wp.array(dtype=int), geom_size: wp.array2d(dtype=wp.vec3), + flex_vertadr: wp.array(dtype=int), + flex_edge: wp.array(dtype=wp.vec2i), + flex_radius: wp.array(dtype=float), # Data in: geom_xpos_in: wp.array2d(dtype=wp.vec3), geom_xmat_in: wp.array2d(dtype=wp.mat33), + flexvert_xpos_in: wp.array2d(dtype=wp.vec3), # In: bvh_id: wp.uint64, group_root: int, - world_id: int, + worldid: int, bvh_ngeom: int, + flex_bvh_ngeom: int, enabled_geom_ids: wp.array(dtype=int), mesh_bvh_id: wp.array(dtype=wp.uint64), hfield_bvh_id: wp.array(dtype=wp.uint64), + flex_geom_flexid: wp.array(dtype=int), + flex_geom_edgeid: wp.array(dtype=int), + flex_bvh_id: wp.array(dtype=wp.uint64), + flex_group_root: wp.array2d(dtype=int), ray_origin_world: wp.vec3, ray_dir_world: wp.vec3, ) -> Tuple[int, float, wp.vec3, float, float, int, int]: @@ -118,91 +124,127 @@ def cast_ray( query = wp.bvh_query_ray(bvh_id, ray_origin_world, ray_dir_world, group_root) bounds_nr = int(0) + ngeom = bvh_ngeom + flex_bvh_ngeom while wp.bvh_query_next(query, bounds_nr, dist): gi_global = bounds_nr - gi_bvh_local = gi_global - (world_id * bvh_ngeom) - gi = enabled_geom_ids[gi_bvh_local] + local_id = gi_global - (worldid * ngeom) + d = float(-1.0) hit_mesh_id = int(-1) u = float(0.0) v = float(0.0) f = int(-1) n = wp.vec3(0.0, 0.0, 0.0) + hit_geom_id = int(-1) + + if local_id < bvh_ngeom: + gi = enabled_geom_ids[local_id] + gtype = geom_type[gi] + else: + gi = local_id - bvh_ngeom + gtype = GeomType.FLEX + + hit_geom_id = gi # TODO: Investigate branch elimination with static loop unrolling - if geom_type[gi] == GeomType.PLANE: + if gtype == GeomType.PLANE: d, n = ray_plane( - geom_xpos_in[world_id, gi], - geom_xmat_in[world_id, gi], - geom_size[world_id % geom_size.shape[0], gi], + geom_xpos_in[worldid, gi], + geom_xmat_in[worldid, gi], + geom_size[worldid % geom_size.shape[0], gi], ray_origin_world, ray_dir_world, ) - if geom_type[gi] == GeomType.HFIELD: + if gtype == GeomType.HFIELD: d, n, u, v, f, geom_hfield_id = ray_mesh_with_bvh( hfield_bvh_id, geom_dataid[gi], - geom_xpos_in[world_id, gi], - geom_xmat_in[world_id, gi], + geom_xpos_in[worldid, gi], + geom_xmat_in[worldid, gi], ray_origin_world, ray_dir_world, dist, ) - if geom_type[gi] == GeomType.SPHERE: + if gtype == GeomType.SPHERE: d, n = ray_sphere( - geom_xpos_in[world_id, gi], - geom_size[world_id % geom_size.shape[0], gi][0] * geom_size[world_id % geom_size.shape[0], gi][0], + geom_xpos_in[worldid, gi], + geom_size[worldid % geom_size.shape[0], gi][0] * geom_size[worldid % geom_size.shape[0], gi][0], ray_origin_world, ray_dir_world, ) - if geom_type[gi] == GeomType.ELLIPSOID: + if gtype == GeomType.ELLIPSOID: d, n = ray_ellipsoid( - geom_xpos_in[world_id, gi], - geom_xmat_in[world_id, gi], - geom_size[world_id % geom_size.shape[0], gi], + geom_xpos_in[worldid, gi], + geom_xmat_in[worldid, gi], + geom_size[worldid % geom_size.shape[0], gi], ray_origin_world, ray_dir_world, ) - if geom_type[gi] == GeomType.CAPSULE: + if gtype == GeomType.CAPSULE: d, n = ray_capsule( - geom_xpos_in[world_id, gi], - geom_xmat_in[world_id, gi], - geom_size[world_id % geom_size.shape[0], gi], + geom_xpos_in[worldid, gi], + geom_xmat_in[worldid, gi], + geom_size[worldid % geom_size.shape[0], gi], ray_origin_world, ray_dir_world, ) - if geom_type[gi] == GeomType.CYLINDER: + if gtype == GeomType.CYLINDER: d, n = ray_cylinder( - geom_xpos_in[world_id, gi], - geom_xmat_in[world_id, gi], - geom_size[world_id % geom_size.shape[0], gi], + geom_xpos_in[worldid, gi], + geom_xmat_in[worldid, gi], + geom_size[worldid % geom_size.shape[0], gi], ray_origin_world, ray_dir_world, ) - if geom_type[gi] == GeomType.BOX: + if gtype == GeomType.BOX: d, all, n = ray_box( - geom_xpos_in[world_id, gi], - geom_xmat_in[world_id, gi], - geom_size[world_id % geom_size.shape[0], gi], + geom_xpos_in[worldid, gi], + geom_xmat_in[worldid, gi], + geom_size[worldid % geom_size.shape[0], gi], ray_origin_world, ray_dir_world, ) - if geom_type[gi] == GeomType.MESH: + if gtype == GeomType.MESH: d, n, u, v, f, hit_mesh_id = ray_mesh_with_bvh( mesh_bvh_id, geom_dataid[gi], - geom_xpos_in[world_id, gi], - geom_xmat_in[world_id, gi], + geom_xpos_in[worldid, gi], + geom_xmat_in[worldid, gi], ray_origin_world, ray_dir_world, dist, ) + if gtype == GeomType.FLEX: + hit_geom_id = -2 + flexid = flex_geom_flexid[gi] + edge_id = flex_geom_edgeid[gi] + + if edge_id >= 0: + edge = flex_edge[edge_id] + vert_adr = flex_vertadr[flexid] + v0 = flexvert_xpos_in[worldid, vert_adr + edge[0]] + v1 = flexvert_xpos_in[worldid, vert_adr + edge[1]] + pos = 0.5 * (v0 + v1) + vec = v1 - v0 + + length = wp.length(vec) + edgeq = math.quat_z2vec(vec) + mat = math.quat_to_mat(edgeq) + size = wp.vec3(flex_radius[flexid], 0.5 * length, 0.0) + + d, n = ray_capsule(pos, mat, size, ray_origin_world, ray_dir_world) + hit_mesh_id = flexid + else: + flex_gr = flex_group_root[worldid, flexid] + d, n, u, v, f = ray_flex_with_bvh(flex_bvh_id, flexid, flex_gr, ray_origin_world, ray_dir_world, dist) + if d >= 0.0: + hit_mesh_id = flexid if d >= 0.0 and d < dist: dist = d normal = n - geom_id = gi + geom_id = hit_geom_id bary_u = u bary_v = v face_idx = f @@ -217,17 +259,26 @@ def cast_ray_first_hit( geom_type: wp.array(dtype=int), geom_dataid: wp.array(dtype=int), geom_size: wp.array2d(dtype=wp.vec3), + flex_vertadr: wp.array(dtype=int), + flex_edge: wp.array(dtype=wp.vec2i), + flex_radius: wp.array(dtype=float), # Data in: geom_xpos_in: wp.array2d(dtype=wp.vec3), geom_xmat_in: wp.array2d(dtype=wp.mat33), + flexvert_xpos_in: wp.array2d(dtype=wp.vec3), # In: bvh_id: wp.uint64, group_root: int, - world_id: int, + worldid: int, bvh_ngeom: int, + bvh_nflexgeom: int, enabled_geom_ids: wp.array(dtype=int), mesh_bvh_id: wp.array(dtype=wp.uint64), hfield_bvh_id: wp.array(dtype=wp.uint64), + flex_geom_flexid: wp.array(dtype=int), + flex_geom_edgeid: wp.array(dtype=int), + flex_bvh_id: wp.array(dtype=wp.uint64), + flex_group_root: wp.array2d(dtype=int), ray_origin_world: wp.vec3, ray_dir_world: wp.vec3, max_dist: float, @@ -235,81 +286,119 @@ def cast_ray_first_hit( """A simpler version of casting rays that only checks for the first hit.""" query = wp.bvh_query_ray(bvh_id, ray_origin_world, ray_dir_world, group_root) bounds_nr = int(0) + ngeom = bvh_ngeom + bvh_nflexgeom while wp.bvh_query_next(query, bounds_nr, max_dist): gi_global = bounds_nr - gi_bvh_local = gi_global - (world_id * bvh_ngeom) - gi = enabled_geom_ids[gi_bvh_local] + local_id = gi_global - (worldid * ngeom) + + d = float(-1.0) + n = wp.vec3(0.0, 0.0, 0.0) + + if local_id < bvh_ngeom: + gi = enabled_geom_ids[local_id] + gtype = geom_type[gi] + else: + gi = local_id - bvh_ngeom + gtype = GeomType.FLEX # TODO: Investigate branch elimination with static loop unrolling - if geom_type[gi] == GeomType.PLANE: + if gtype == GeomType.PLANE: d, n = ray_plane( - geom_xpos_in[world_id, gi], - geom_xmat_in[world_id, gi], - geom_size[world_id % geom_size.shape[0], gi], + geom_xpos_in[worldid, gi], + geom_xmat_in[worldid, gi], + geom_size[worldid % geom_size.shape[0], gi], ray_origin_world, ray_dir_world, ) - if geom_type[gi] == GeomType.HFIELD: + if gtype == GeomType.HFIELD: d, n, u, v, f, geom_hfield_id = ray_mesh_with_bvh( hfield_bvh_id, geom_dataid[gi], - geom_xpos_in[world_id, gi], - geom_xmat_in[world_id, gi], + geom_xpos_in[worldid, gi], + geom_xmat_in[worldid, gi], ray_origin_world, ray_dir_world, max_dist, ) - if geom_type[gi] == GeomType.SPHERE: + if gtype == GeomType.SPHERE: d, n = ray_sphere( - geom_xpos_in[world_id, gi], - geom_size[world_id % geom_size.shape[0], gi][0] * geom_size[world_id % geom_size.shape[0], gi][0], + geom_xpos_in[worldid, gi], + geom_size[worldid % geom_size.shape[0], gi][0] * geom_size[worldid % geom_size.shape[0], gi][0], ray_origin_world, ray_dir_world, ) - if geom_type[gi] == GeomType.ELLIPSOID: + if gtype == GeomType.ELLIPSOID: d, n = ray_ellipsoid( - geom_xpos_in[world_id, gi], - geom_xmat_in[world_id, gi], - geom_size[world_id % geom_size.shape[0], gi], + geom_xpos_in[worldid, gi], + geom_xmat_in[worldid, gi], + geom_size[worldid % geom_size.shape[0], gi], ray_origin_world, ray_dir_world, ) - if geom_type[gi] == GeomType.CAPSULE: + if gtype == GeomType.CAPSULE: d, n = ray_capsule( - geom_xpos_in[world_id, gi], - geom_xmat_in[world_id, gi], - geom_size[world_id % geom_size.shape[0], gi], + geom_xpos_in[worldid, gi], + geom_xmat_in[worldid, gi], + geom_size[worldid % geom_size.shape[0], gi], ray_origin_world, ray_dir_world, ) - if geom_type[gi] == GeomType.CYLINDER: + if gtype == GeomType.CYLINDER: d, n = ray_cylinder( - geom_xpos_in[world_id, gi], - geom_xmat_in[world_id, gi], - geom_size[world_id % geom_size.shape[0], gi], + geom_xpos_in[worldid, gi], + geom_xmat_in[worldid, gi], + geom_size[worldid % geom_size.shape[0], gi], ray_origin_world, ray_dir_world, ) - if geom_type[gi] == GeomType.BOX: + if gtype == GeomType.BOX: d, all, n = ray_box( - geom_xpos_in[world_id, gi], - geom_xmat_in[world_id, gi], - geom_size[world_id % geom_size.shape[0], gi], + geom_xpos_in[worldid, gi], + geom_xmat_in[worldid, gi], + geom_size[worldid % geom_size.shape[0], gi], ray_origin_world, ray_dir_world, ) - if geom_type[gi] == GeomType.MESH: + if gtype == GeomType.MESH: hit = ray_mesh_with_bvh_anyhit( mesh_bvh_id, geom_dataid[gi], - geom_xpos_in[world_id, gi], - geom_xmat_in[world_id, gi], + geom_xpos_in[worldid, gi], + geom_xmat_in[worldid, gi], ray_origin_world, ray_dir_world, max_dist, ) d = 0.0 if hit else -1.0 + if gtype == GeomType.FLEX: + flexid = flex_geom_flexid[gi] + edge_id = flex_geom_edgeid[gi] + + if edge_id >= 0: + edge = flex_edge[edge_id] + vert_adr = flex_vertadr[flexid] + v0 = flexvert_xpos_in[worldid, vert_adr + edge[0]] + v1 = flexvert_xpos_in[worldid, vert_adr + edge[1]] + pos = 0.5 * (v0 + v1) + vec = v1 - v0 + + length = wp.length(vec) + edgeq = math.quat_z2vec(vec) + mat = math.quat_to_mat(edgeq) + size = wp.vec3(flex_radius[flexid], 0.5 * length, 0.0) + + d, n = ray_capsule(pos, mat, size, ray_origin_world, ray_dir_world) + else: + hit = ray_flex_with_bvh_anyhit( + flex_bvh_id, + flexid, + flex_group_root[worldid, flexid], + ray_origin_world, + ray_dir_world, + max_dist, + ) + d = 0.0 if hit else -1.0 if d >= 0.0 and d < max_dist: return True @@ -323,18 +412,27 @@ def compute_lighting( geom_type: wp.array(dtype=int), geom_dataid: wp.array(dtype=int), geom_size: wp.array2d(dtype=wp.vec3), + flex_vertadr: wp.array(dtype=int), + flex_edge: wp.array(dtype=wp.vec2i), + flex_radius: wp.array(dtype=float), # Data in: geom_xpos_in: wp.array2d(dtype=wp.vec3), geom_xmat_in: wp.array2d(dtype=wp.mat33), + flexvert_xpos_in: wp.array2d(dtype=wp.vec3), # In: use_shadows: bool, bvh_id: wp.uint64, group_root: int, bvh_ngeom: int, + bvh_nflexgeom: int, enabled_geom_ids: wp.array(dtype=int), - world_id: int, + worldid: int, mesh_bvh_id: wp.array(dtype=wp.uint64), hfield_bvh_id: wp.array(dtype=wp.uint64), + flex_geom_flexid: wp.array(dtype=int), + flex_geom_edgeid: wp.array(dtype=int), + flex_bvh_id: wp.array(dtype=wp.uint64), + flex_group_root: wp.array2d(dtype=int), lightactive: bool, lighttype: int, lightcastshadow: bool, @@ -385,15 +483,24 @@ def compute_lighting( geom_type, geom_dataid, geom_size, + flex_vertadr, + flex_edge, + flex_radius, geom_xpos_in, geom_xmat_in, + flexvert_xpos_in, bvh_id, group_root, - world_id, + worldid, bvh_ngeom, + bvh_nflexgeom, enabled_geom_ids, mesh_bvh_id, hfield_bvh_id, + flex_geom_flexid, + flex_geom_edgeid, + flex_bvh_id, + flex_group_root, shadow_origin, L, max_t, @@ -418,6 +525,7 @@ def render(m: Model, d: Data, rc: RenderContext): """ rc.rgb_data.fill_(rc.background_color) rc.depth_data.fill_(0.0) + rc.seg_data.fill_(-1) @wp.kernel(module="unique", enable_backward=False) def _render_megakernel( @@ -434,6 +542,9 @@ def render(m: Model, d: Data, rc: RenderContext): light_type: wp.array2d(dtype=int), light_castshadow: wp.array2d(dtype=bool), light_active: wp.array2d(dtype=bool), + flex_vertadr: wp.array(dtype=int), + flex_edge: wp.array(dtype=wp.vec2i), + flex_radius: wp.array(dtype=float), mesh_faceadr: wp.array(dtype=int), mat_texid: wp.array3d(dtype=int), mat_texrepeat: wp.array2d(dtype=wp.vec2), @@ -445,21 +556,25 @@ def render(m: Model, d: Data, rc: RenderContext): cam_xmat_in: wp.array2d(dtype=wp.mat33), light_xpos_in: wp.array2d(dtype=wp.vec3), light_xdir_in: wp.array2d(dtype=wp.vec3), + flexvert_xpos_in: wp.array2d(dtype=wp.vec3), # In: nrender: int, use_shadows: bool, bvh_ngeom: int, + bvh_nflexgeom: int, cam_res: wp.array(dtype=wp.vec2i), cam_id_map: wp.array(dtype=int), ray: wp.array(dtype=wp.vec3), rgb_adr: wp.array(dtype=int), depth_adr: wp.array(dtype=int), + seg_adr: wp.array(dtype=int), render_rgb: wp.array(dtype=bool), render_depth: wp.array(dtype=bool), + render_seg: wp.array(dtype=bool), bvh_id: wp.uint64, group_root: wp.array(dtype=int), - flex_bvh_id: wp.uint64, - flex_group_root: wp.array(dtype=int), + flex_bvh_id: wp.array(dtype=wp.uint64), + flex_group_root: wp.array2d(dtype=int), enabled_geom_ids: wp.array(dtype=int), mesh_bvh_id: wp.array(dtype=wp.uint64), mesh_facetexcoord: wp.array(dtype=wp.vec3i), @@ -467,46 +582,48 @@ def render(m: Model, d: Data, rc: RenderContext): mesh_texcoord_offsets: wp.array(dtype=int), hfield_bvh_id: wp.array(dtype=wp.uint64), flex_rgba: wp.array(dtype=wp.vec4), - # TODO: remove after mjwarp depends on warp-lang >= 1.12 in pyproject.toml - textures: wp.array(dtype=TEXTURE_DTYPE), + flex_geom_flexid: wp.array(dtype=int), + flex_geom_edgeid: wp.array(dtype=int), + textures: wp.array(dtype=wp.Texture2D), # Out: rgb_out: wp.array2d(dtype=wp.uint32), depth_out: wp.array2d(dtype=float), + seg_out: wp.array2d(dtype=int), ): - world_idx, ray_idx = wp.tid() + worldid, rayid = wp.tid() - # Map global ray_idx -> (cam_idx, ray_idx_local) using cumulative sizes + # Map global rayid -> (cam_idx, rayid_local) using cumulative sizes cam_idx = int(-1) - ray_idx_local = int(-1) + rayid_local = int(-1) accum = int(0) for i in range(nrender): num_i = cam_res[i][0] * cam_res[i][1] - if ray_idx < accum + num_i: + if rayid < accum + num_i: cam_idx = i - ray_idx_local = ray_idx - accum + rayid_local = rayid - accum break accum += num_i - if cam_idx == -1 or ray_idx_local < 0: + if cam_idx == -1 or rayid_local < 0: return - if not render_rgb[cam_idx] and not render_depth[cam_idx]: + if not render_rgb[cam_idx] and not render_depth[cam_idx] and not render_seg[cam_idx]: return # Map active camera index to MuJoCo camera ID mujoco_cam_id = cam_id_map[cam_idx] if wp.static(rc.use_precomputed_rays): - ray_dir_local_cam = ray[ray_idx] + ray_dir_local_cam = ray[rayid] else: img_w = cam_res[cam_idx][0] img_h = cam_res[cam_idx][1] - px = ray_idx_local % img_w - py = ray_idx_local // img_w + px = rayid_local % img_w + py = rayid_local // img_w ray_dir_local_cam = compute_ray( cam_projection[mujoco_cam_id], - cam_fovy[world_idx % cam_fovy.shape[0], mujoco_cam_id], + cam_fovy[worldid % cam_fovy.shape[0], mujoco_cam_id], cam_sensorsize[mujoco_cam_id], - cam_intrinsic[world_idx % cam_intrinsic.shape[0], mujoco_cam_id], + cam_intrinsic[worldid % cam_intrinsic.shape[0], mujoco_cam_id], img_w, img_h, px, @@ -514,38 +631,37 @@ def render(m: Model, d: Data, rc: RenderContext): wp.static(rc.znear), ) - ray_dir_world = cam_xmat_in[world_idx, mujoco_cam_id] @ ray_dir_local_cam - ray_origin_world = cam_xpos_in[world_idx, mujoco_cam_id] + ray_dir_world = cam_xmat_in[worldid, mujoco_cam_id] @ ray_dir_local_cam + ray_origin_world = cam_xpos_in[worldid, mujoco_cam_id] geom_id, dist, normal, u, v, f, mesh_id = cast_ray( geom_type, geom_dataid, geom_size, + flex_vertadr, + flex_edge, + flex_radius, geom_xpos_in, geom_xmat_in, + flexvert_xpos_in, bvh_id, - group_root[world_idx], - world_idx, + group_root[worldid], + worldid, bvh_ngeom, + bvh_nflexgeom, enabled_geom_ids, mesh_bvh_id, hfield_bvh_id, + flex_geom_flexid, + flex_geom_edgeid, + flex_bvh_id, + flex_group_root, ray_origin_world, ray_dir_world, ) - if wp.static(m.nflex > 0): - d, n, u, v, f = ray_flex_with_bvh( - flex_bvh_id, - flex_group_root[world_idx], - ray_origin_world, - ray_dir_world, - dist, - ) - if d >= 0.0 and d < dist: - dist = d - normal = n - geom_id = -2 + if render_seg[cam_idx] and geom_id != -1: + seg_out[worldid, seg_adr[cam_idx] + rayid_local] = geom_id # Early Out if geom_id == -1: @@ -556,9 +672,7 @@ def render(m: Model, d: Data, rc: RenderContext): # In camera-local coordinates, the optical axis is -Z. The Z-component of the # normalized ray direction is negative, so -ray_dir_local_cam[2] gives cos(θ) # between the ray and the optical axis. - depth_out[world_idx, depth_adr[cam_idx] + ray_idx_local] = dist * ( - -ray_dir_local_cam[2] - ) + depth_out[worldid, depth_adr[cam_idx] + rayid_local] = dist * (-ray_dir_local_cam[2]) if not render_rgb[cam_idx]: return @@ -567,31 +681,30 @@ def render(m: Model, d: Data, rc: RenderContext): hit_point = ray_origin_world + ray_dir_world * dist if geom_id == -2: - # TODO: Currently flex textures are not supported, and only the first rgba value - # is used until further flex support is added. - color = flex_rgba[0] - elif geom_matid[world_idx % geom_matid.shape[0], geom_id] == -1: - color = geom_rgba[world_idx % geom_rgba.shape[0], geom_id] + # We encode flex_id in mesh_id for flex ray hits during cast_ray + color = flex_rgba[mesh_id] + elif geom_matid[worldid % geom_matid.shape[0], geom_id] == -1: + color = geom_rgba[worldid % geom_rgba.shape[0], geom_id] else: - color = mat_rgba[world_idx % mat_rgba.shape[0], geom_matid[world_idx % geom_matid.shape[0], geom_id]] + color = mat_rgba[worldid % mat_rgba.shape[0], geom_matid[worldid % geom_matid.shape[0], geom_id]] base_color = wp.vec3(color[0], color[1], color[2]) hit_color = base_color if wp.static(rc.use_textures): if geom_id != -2: - mat_id = geom_matid[world_idx % geom_matid.shape[0], geom_id] + mat_id = geom_matid[worldid % geom_matid.shape[0], geom_id] if mat_id >= 0: - tex_id = mat_texid[world_idx % mat_texid.shape[0], mat_id, 1] + tex_id = mat_texid[worldid % mat_texid.shape[0], mat_id, 1] if tex_id >= 0: tex_color = sample_texture( geom_type, mesh_faceadr, geom_id, - mat_texrepeat[world_idx % mat_texrepeat.shape[0], mat_id], + mat_texrepeat[worldid % mat_texrepeat.shape[0], mat_id], textures[tex_id], - geom_xpos_in[world_idx, geom_id], - geom_xmat_in[world_idx, geom_id], + geom_xpos_in[worldid, geom_id], + geom_xmat_in[worldid, geom_id], mesh_facetexcoord, mesh_texcoord, mesh_texcoord_offsets, @@ -616,21 +729,30 @@ def render(m: Model, d: Data, rc: RenderContext): geom_type, geom_dataid, geom_size, + flex_vertadr, + flex_edge, + flex_radius, geom_xpos_in, geom_xmat_in, + flexvert_xpos_in, use_shadows, bvh_id, - group_root[world_idx], + group_root[worldid], bvh_ngeom, + bvh_nflexgeom, enabled_geom_ids, - world_idx, + worldid, mesh_bvh_id, hfield_bvh_id, - light_active[world_idx % light_active.shape[0], l], - light_type[world_idx % light_type.shape[0], l], - light_castshadow[world_idx % light_castshadow.shape[0], l], - light_xpos_in[world_idx, l], - light_xdir_in[world_idx, l], + flex_geom_flexid, + flex_geom_edgeid, + flex_bvh_id, + flex_group_root, + light_active[worldid % light_active.shape[0], l], + light_type[worldid % light_type.shape[0], l], + light_castshadow[worldid % light_castshadow.shape[0], l], + light_xpos_in[worldid, l], + light_xdir_in[worldid, l], normal, hit_point, ) @@ -639,7 +761,7 @@ def render(m: Model, d: Data, rc: RenderContext): hit_color = wp.min(result, wp.vec3(1.0, 1.0, 1.0)) hit_color = wp.max(hit_color, wp.vec3(0.0, 0.0, 0.0)) - rgb_out[world_idx, rgb_adr[cam_idx] + ray_idx_local] = pack_rgba_to_uint32( + rgb_out[worldid, rgb_adr[cam_idx] + rayid_local] = pack_rgba_to_uint32( hit_color[0] * 255.0, hit_color[1] * 255.0, hit_color[2] * 255.0, @@ -662,6 +784,9 @@ def render(m: Model, d: Data, rc: RenderContext): m.light_type, m.light_castshadow, m.light_active, + m.flex_vertadr, + m.flex_edge, + m.flex_radius, m.mesh_faceadr, m.mat_texid, m.mat_texrepeat, @@ -672,16 +797,20 @@ def render(m: Model, d: Data, rc: RenderContext): d.cam_xmat, d.light_xpos, d.light_xdir, + d.flexvert_xpos, rc.nrender, rc.use_shadows, rc.bvh_ngeom, + rc.bvh_nflexgeom, rc.cam_res, rc.cam_id_map, rc.ray, rc.rgb_adr, rc.depth_adr, + rc.seg_adr, rc.render_rgb, rc.render_depth, + rc.render_seg, rc.bvh_id, rc.group_root, rc.flex_bvh_id, @@ -693,10 +822,13 @@ def render(m: Model, d: Data, rc: RenderContext): rc.mesh_texcoord_offsets, rc.hfield_bvh_id, rc.flex_rgba, + rc.flex_geom_flexid, + rc.flex_geom_edgeid, rc.textures, ], outputs=[ rc.rgb_data, rc.depth_data, + rc.seg_data, ], ) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render_util.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render_util.py index ccb808ff..36958f8e 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render_util.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render_util.py @@ -206,3 +206,41 @@ def get_depth(rc: RenderContext, camera_index: int, depth_scale: float, depth_ou inputs=[rc.depth_data, rc.depth_adr, camera_index, depth_scale], outputs=[depth_out], ) + + +@wp.kernel +def _extract_seg_kernel( + # In: + seg_data: wp.array2d(dtype=int), + seg_adr: wp.array(dtype=int), + camera_index: int, + # Out: + seg_out: wp.array3d(dtype=int), +): + """Extract per-pixel geom IDs from the render context buffers for a given camera index.""" + worldid, pixelid = wp.tid() + xid = pixelid % seg_out.shape[2] + yid = pixelid // seg_out.shape[2] + + seg_adr_offset = seg_adr[camera_index] + seg_out[worldid, yid, xid] = seg_data[worldid, seg_adr_offset + pixelid] + + +def get_segmentation(rc: RenderContext, camera_index: int, seg_out: wp.array3d(dtype=int)): + """Get the segmentation data from the render context buffers for a given camera index. + + Each pixel contains the MuJoCo geom ID of the geometry hit by the ray, -1 for + background, or -2 for flex bodies. + + Args: + rc: The render context on device. + camera_index: The index of the camera to get the segmentation data for. + seg_out: The output array to store the geom IDs in, with shape + (nworld, height, width). + """ + wp.launch( + _extract_seg_kernel, + dim=(seg_out.shape[0], seg_out.shape[1] * seg_out.shape[2]), + inputs=[rc.seg_data, rc.seg_adr, camera_index], + outputs=[seg_out], + ) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py index 859ddf8a..2c8177b8 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py @@ -15,12 +15,17 @@ from typing import Any, Tuple +import warp as wp + from mujoco.mjx.third_party.mujoco_warp._src import math from mujoco.mjx.third_party.mujoco_warp._src import ray from mujoco.mjx.third_party.mujoco_warp._src import smooth from mujoco.mjx.third_party.mujoco_warp._src import support from mujoco.mjx.third_party.mujoco_warp._src.collision_sdf import get_sdf_params from mujoco.mjx.third_party.mujoco_warp._src.collision_sdf import sdf +from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXCONPAIR +from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXVAL +from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINVAL from mujoco.mjx.third_party.mujoco_warp._src.types import ConeType from mujoco.mjx.third_party.mujoco_warp._src.types import ConstraintType from mujoco.mjx.third_party.mujoco_warp._src.types import ContactType @@ -28,9 +33,6 @@ from mujoco.mjx.third_party.mujoco_warp._src.types import Data from mujoco.mjx.third_party.mujoco_warp._src.types import DataType from mujoco.mjx.third_party.mujoco_warp._src.types import DisableBit from mujoco.mjx.third_party.mujoco_warp._src.types import JointType -from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXCONPAIR -from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXVAL -from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINVAL from mujoco.mjx.third_party.mujoco_warp._src.types import Model from mujoco.mjx.third_party.mujoco_warp._src.types import ObjType from mujoco.mjx.third_party.mujoco_warp._src.types import SensorType @@ -40,10 +42,10 @@ from mujoco.mjx.third_party.mujoco_warp._src.types import vec5 from mujoco.mjx.third_party.mujoco_warp._src.types import vec6 from mujoco.mjx.third_party.mujoco_warp._src.types import vec8 from mujoco.mjx.third_party.mujoco_warp._src.types import vec8i +from mujoco.mjx.third_party.mujoco_warp._src.types import vec_pluginattr from mujoco.mjx.third_party.mujoco_warp._src.util_misc import inside_geom from mujoco.mjx.third_party.mujoco_warp._src.warp_util import cache_kernel from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope -import warp as wp wp.set_module_options({"enable_backward": False}) @@ -2081,16 +2083,16 @@ def _transform_spatial(vec: wp.spatial_vector, dif: wp.vec3) -> wp.vec3: @wp.kernel def _preprocess_tactile_contacts( - # Model: - body_weldid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - # Data in: - contact_geom_in: wp.array(dtype=wp.vec2i), - contact_worldid_in: wp.array(dtype=int), - nacon_in: wp.array(dtype=int), - # Out: - weld_geom_count_out: wp.array2d(dtype=int), - weld_geom_list_out: wp.array3d(dtype=int), + # Model: + body_weldid: wp.array(dtype=int), + geom_bodyid: wp.array(dtype=int), + # Data in: + contact_geom_in: wp.array(dtype=wp.vec2i), + contact_worldid_in: wp.array(dtype=int), + nacon_in: wp.array(dtype=int), + # Out: + weld_geom_count_out: wp.array2d(dtype=int), + weld_geom_list_out: wp.array3d(dtype=int), ): conid = wp.tid() ncon = nacon_in[0] @@ -2118,42 +2120,43 @@ def _preprocess_tactile_contacts( @wp.kernel def _sensor_tactile( - # Model: - body_rootid: wp.array(dtype=int), - body_weldid: wp.array(dtype=int), - oct_child: wp.array(dtype=vec8i), - oct_aabb: wp.array2d(dtype=wp.vec3), - oct_coeff: wp.array(dtype=vec8), - geom_type: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - mesh_vertadr: wp.array(dtype=int), - mesh_vertnum: wp.array(dtype=int), - mesh_octadr: wp.array(dtype=int), - mesh_normaladr: wp.array(dtype=int), - mesh_normalnum: wp.array(dtype=int), - mesh_vert: wp.array(dtype=wp.vec3), - mesh_normal: wp.array(dtype=wp.vec3), - mesh_quat: wp.array(dtype=wp.quat), - sensor_objid: wp.array(dtype=int), - sensor_refid: wp.array(dtype=int), - sensor_dim: wp.array(dtype=int), - sensor_adr: wp.array(dtype=int), - plugin: wp.array(dtype=int), - plugin_attr: wp.array(dtype=wp.vec3f), - geom_plugin_index: wp.array(dtype=int), - taxel_vertadr: wp.array(dtype=int), - taxel_sensorid: wp.array(dtype=int), - # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cvel_in: wp.array2d(dtype=wp.spatial_vector), - # In: - weld_geom_count_in: wp.array2d(dtype=int), - weld_geom_list_in: wp.array3d(dtype=int), - # Data out: - sensordata_out: wp.array2d(dtype=float), + # Model: + body_rootid: wp.array(dtype=int), + body_weldid: wp.array(dtype=int), + oct_child: wp.array(dtype=vec8i), + oct_aabb: wp.array2d(dtype=wp.vec3), + oct_coeff: wp.array(dtype=vec8), + geom_type: wp.array(dtype=int), + geom_bodyid: wp.array(dtype=int), + geom_dataid: wp.array(dtype=int), + geom_size: wp.array2d(dtype=wp.vec3), + mesh_vertadr: wp.array(dtype=int), + mesh_vertnum: wp.array(dtype=int), + mesh_octadr: wp.array(dtype=int), + mesh_normaladr: wp.array(dtype=int), + mesh_normalnum: wp.array(dtype=int), + mesh_vert: wp.array(dtype=wp.vec3), + mesh_normal: wp.array(dtype=wp.vec3), + mesh_quat: wp.array(dtype=wp.quat), + sensor_objid: wp.array(dtype=int), + sensor_refid: wp.array(dtype=int), + sensor_dim: wp.array(dtype=int), + sensor_adr: wp.array(dtype=int), + plugin: wp.array(dtype=int), + plugin_attr: wp.array(dtype=vec_pluginattr), + geom_plugin_index: wp.array(dtype=int), + taxel_vertadr: wp.array(dtype=int), + taxel_sensorid: wp.array(dtype=int), + # Data in: + geom_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xmat_in: wp.array2d(dtype=wp.mat33), + subtree_com_in: wp.array2d(dtype=wp.vec3), + cvel_in: wp.array2d(dtype=wp.spatial_vector), + # In: + weld_geom_count_in: wp.array2d(dtype=int), + weld_geom_list_in: wp.array3d(dtype=int), + # Data out: + sensordata_out: wp.array2d(dtype=float), ): worldid, taxelid = wp.tid() @@ -2211,40 +2214,25 @@ def _sensor_tactile( contact_type = geom_type[geom] plugin_attributes, plugin_index, volume_data, mesh_data = get_sdf_params( - oct_child, - oct_aabb, - oct_coeff, - mesh_octadr, - plugin, - plugin_attr, - contact_type, - geom_size[worldid % geom_size.shape[0], geom], - plugin_id, - mesh_id, + oct_child, + oct_aabb, + oct_coeff, + mesh_octadr, + plugin, + plugin_attr, + contact_type, + geom_size[worldid % geom_size.shape[0], geom], + plugin_id, + geom_dataid[geom], ) - depth = wp.min( - sdf( - contact_type, - lpos, - plugin_attributes, - plugin_index, - volume_data, - mesh_data, - ), - 0.0, - ) + depth = wp.min(sdf(contact_type, lpos, plugin_attributes, plugin_index, volume_data, mesh_data), 0.0) if depth >= 0.0: continue - vel_sensor = _transform_spatial( - cvel_in[worldid, parent_weld], - xpos - subtree_com_in[worldid, body_rootid[parent_weld]], - ) + vel_sensor = _transform_spatial(cvel_in[worldid, parent_weld], xpos - subtree_com_in[worldid, body_rootid[parent_weld]]) vel_other = _transform_spatial( - cvel_in[worldid, body], - geom_xpos_in[worldid, geom] - - subtree_com_in[worldid, body_rootid[body]], + cvel_in[worldid, body], geom_xpos_in[worldid, geom] - subtree_com_in[worldid, body_rootid[body]] ) vel_rel = vel_sensor - vel_other @@ -2259,24 +2247,9 @@ def _sensor_tactile( forceT[2] = wp.abs(wp.dot(vel_rel, tang2)) dim = sensor_dim[sensor_id] // 3 - wp.atomic_add( - sensordata_out, - worldid, - sensor_adr[sensor_id] + 0 * dim + vertid, - forceT[0], - ) - wp.atomic_add( - sensordata_out, - worldid, - sensor_adr[sensor_id] + 1 * dim + vertid, - forceT[1], - ) - wp.atomic_add( - sensordata_out, - worldid, - sensor_adr[sensor_id] + 2 * dim + vertid, - forceT[2], - ) + wp.atomic_add(sensordata_out, worldid, sensor_adr[sensor_id] + 0 * dim + vertid, forceT[0]) + wp.atomic_add(sensordata_out, worldid, sensor_adr[sensor_id] + 1 * dim + vertid, forceT[1]) + wp.atomic_add(sensordata_out, worldid, sensor_adr[sensor_id] + 2 * dim + vertid, forceT[2]) @wp.func @@ -2507,60 +2480,61 @@ def sensor_acc(m: Model, d: Data): weld_geom_count = wp.zeros((d.nworld, m.nbody), dtype=int) weld_geom_list = wp.full((d.nworld, m.nbody, MJ_MAXCONPAIR), -1, dtype=int) wp.launch( - _preprocess_tactile_contacts, - dim=d.naconmax, - inputs=[ - m.body_weldid, - m.geom_bodyid, - d.contact.geom, - d.contact.worldid, - d.nacon, - ], - outputs=[ - weld_geom_count, - weld_geom_list, - ], + _preprocess_tactile_contacts, + dim=d.naconmax, + inputs=[ + m.body_weldid, + m.geom_bodyid, + d.contact.geom, + d.contact.worldid, + d.nacon, + ], + outputs=[ + weld_geom_count, + weld_geom_list, + ], ) wp.launch( - _sensor_tactile, - dim=(d.nworld, m.nsensortaxel), - inputs=[ - m.body_rootid, - m.body_weldid, - m.oct_child, - m.oct_aabb, - m.oct_coeff, - m.geom_type, - m.geom_bodyid, - m.geom_size, - m.mesh_vertadr, - m.mesh_vertnum, - m.mesh_octadr, - m.mesh_normaladr, - m.mesh_normalnum, - m.mesh_vert, - m.mesh_normal, - m.mesh_quat, - m.sensor_objid, - m.sensor_refid, - m.sensor_dim, - m.sensor_adr, - m.plugin, - m.plugin_attr, - m.geom_plugin_index, - m.taxel_vertadr, - m.taxel_sensorid, - d.geom_xpos, - d.geom_xmat, - d.subtree_com, - d.cvel, - weld_geom_count, - weld_geom_list, - ], - outputs=[ - d.sensordata, - ], + _sensor_tactile, + dim=(d.nworld, m.nsensortaxel), + inputs=[ + m.body_rootid, + m.body_weldid, + m.oct_child, + m.oct_aabb, + m.oct_coeff, + m.geom_type, + m.geom_bodyid, + m.geom_dataid, + m.geom_size, + m.mesh_vertadr, + m.mesh_vertnum, + m.mesh_octadr, + m.mesh_normaladr, + m.mesh_normalnum, + m.mesh_vert, + m.mesh_normal, + m.mesh_quat, + m.sensor_objid, + m.sensor_refid, + m.sensor_dim, + m.sensor_adr, + m.plugin, + m.plugin_attr, + m.geom_plugin_index, + m.taxel_vertadr, + m.taxel_sensorid, + d.geom_xpos, + d.geom_xmat, + d.subtree_com, + d.cvel, + weld_geom_count, + weld_geom_list, + ], + outputs=[ + d.sensordata, + ], ) sensor_contact_nmatch = wp.empty((d.nworld, m.nsensorcontact), dtype=int) @@ -2882,12 +2856,12 @@ def energy_pos(m: Model, d: Data): wp.launch(_energy_pos_zero, dim=d.nworld, outputs=[d.energy]) # init potential energy: -sum_i(body_i.mass * dot(gravity, body_i.pos)) - if not m.opt.disableflags & DisableBit.GRAVITY: + if not (m.opt.disableflags & DisableBit.GRAVITY): wp.launch( _energy_pos_gravity, dim=(d.nworld, m.nbody - 1), inputs=[m.opt.gravity, m.body_mass, d.xipos], outputs=[d.energy] ) - if not m.opt.disableflags & DisableBit.SPRING: + if not (m.opt.disableflags & DisableBit.SPRING): # add joint-level springs wp.launch( _energy_pos_passive_joint, diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/smooth.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/smooth.py index 51b36403..51dfadb4 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/smooth.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/smooth.py @@ -14,29 +14,29 @@ # ============================================================================== +import warp as wp + from mujoco.mjx.third_party.mujoco_warp._src import math from mujoco.mjx.third_party.mujoco_warp._src import support from mujoco.mjx.third_party.mujoco_warp._src import util_misc +from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXVAL +from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINVAL from mujoco.mjx.third_party.mujoco_warp._src.types import CamLightType from mujoco.mjx.third_party.mujoco_warp._src.types import ConeType from mujoco.mjx.third_party.mujoco_warp._src.types import Data from mujoco.mjx.third_party.mujoco_warp._src.types import DisableBit from mujoco.mjx.third_party.mujoco_warp._src.types import EqType from mujoco.mjx.third_party.mujoco_warp._src.types import JointType -from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXVAL -from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINVAL from mujoco.mjx.third_party.mujoco_warp._src.types import Model from mujoco.mjx.third_party.mujoco_warp._src.types import ObjType -from mujoco.mjx.third_party.mujoco_warp._src.types import SPARSE_CONSTRAINT_JACOBIAN from mujoco.mjx.third_party.mujoco_warp._src.types import TileSet from mujoco.mjx.third_party.mujoco_warp._src.types import TrnType +from mujoco.mjx.third_party.mujoco_warp._src.types import WrapType +from mujoco.mjx.third_party.mujoco_warp._src.types import vec5 from mujoco.mjx.third_party.mujoco_warp._src.types import vec10 from mujoco.mjx.third_party.mujoco_warp._src.types import vec11 -from mujoco.mjx.third_party.mujoco_warp._src.types import vec5 -from mujoco.mjx.third_party.mujoco_warp._src.types import WrapType from mujoco.mjx.third_party.mujoco_warp._src.warp_util import cache_kernel from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope -import warp as wp wp.set_module_options({"enable_backward": False}) @@ -227,38 +227,59 @@ def _site_local_to_global( @wp.kernel def _flex_vertices( # Model: + nflex: int, + flex_vertadr: wp.array(dtype=int), + flex_vertnum: wp.array(dtype=int), flex_vertbodyid: wp.array(dtype=int), + flex_vert: wp.array(dtype=wp.vec3), + flex_centered: wp.array(dtype=bool), # Data in: xpos_in: wp.array2d(dtype=wp.vec3), + xmat_in: wp.array2d(dtype=wp.mat33), # Data out: flexvert_xpos_out: wp.array2d(dtype=wp.vec3), ): worldid, vertid = wp.tid() - flexvert_xpos_out[worldid, vertid] = xpos_in[worldid, flex_vertbodyid[vertid]] + + for f in range(nflex): + locid = vertid - flex_vertadr[f] + if locid >= 0 and locid < flex_vertnum[f]: + break + + bodyid = flex_vertbodyid[vertid] + xpos = xpos_in[worldid, bodyid] + + if flex_centered[f]: + flexvert_xpos_out[worldid, vertid] = xpos + else: + xmat = xmat_in[worldid, bodyid] + local_pos = flex_vert[vertid] + flexvert_xpos_out[worldid, vertid] = xmat @ local_pos + xpos @wp.kernel def _flex_edges( - # Model: - nflex: int, - body_rootid: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - flex_vertadr: wp.array(dtype=int), - flex_edgeadr: wp.array(dtype=int), - flex_edgenum: wp.array(dtype=int), - flex_vertbodyid: wp.array(dtype=int), - flex_edge: wp.array(dtype=wp.vec2i), - flexedge_J_rowadr: wp.array(dtype=int), - flexedge_J_colind: wp.array(dtype=int), - # Data in: - qvel_in: wp.array2d(dtype=float), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), - # Data out: - flexedge_J_out: wp.array2d(dtype=float), - flexedge_length_out: wp.array2d(dtype=float), - flexedge_velocity_out: wp.array2d(dtype=float), + # Model: + nflex: int, + body_rootid: wp.array(dtype=int), + body_dofnum: wp.array(dtype=int), + body_dofadr: wp.array(dtype=int), + flex_vertadr: wp.array(dtype=int), + flex_edgeadr: wp.array(dtype=int), + flex_edgenum: wp.array(dtype=int), + flex_vertbodyid: wp.array(dtype=int), + flex_edge: wp.array(dtype=wp.vec2i), + flexedge_J_rowadr: wp.array(dtype=int), + flexedge_J_colind: wp.array(dtype=int), + # Data in: + qvel_in: wp.array2d(dtype=float), + subtree_com_in: wp.array2d(dtype=wp.vec3), + cdof_in: wp.array2d(dtype=wp.spatial_vector), + flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + # Data out: + flexedge_J_out: wp.array2d(dtype=float), + flexedge_length_out: wp.array2d(dtype=float), + flexedge_velocity_out: wp.array2d(dtype=float), ): worldid, edgeid = wp.tid() for i in range(nflex): @@ -281,42 +302,56 @@ def _flex_edges( b1 = flex_vertbodyid[vbase0] b2 = flex_vertbodyid[vbase1] - dofi = body_dofadr[b1] - dofj = body_dofadr[b2] + dofnum1 = body_dofnum[b1] + dofnum2 = body_dofnum[b2] - vel1 = wp.vec3( - qvel_in[worldid, dofi], - qvel_in[worldid, dofi + 1], - qvel_in[worldid, dofi + 2], - ) - vel2 = wp.vec3( - qvel_in[worldid, dofj], - qvel_in[worldid, dofj + 1], - qvel_in[worldid, dofj + 2], - ) - flexedge_velocity_out[worldid, edgeid] = wp.dot(vel2 - vel1, edge) + # velocity via Jacobian: sum_k J_k * qvel_k for each body + vel = float(0.0) + if dofnum1 > 0: + dofi = body_dofadr[b1] + offset1 = pos1 - wp.vec3(subtree_com_in[worldid, body_rootid[b1]]) + for k in range(dofnum1): + cdof = cdof_in[worldid, dofi + k] + cdof_ang = wp.spatial_top(cdof) + cdof_lin = wp.spatial_bottom(cdof) + jacp1 = cdof_lin + wp.cross(cdof_ang, offset1) + vel -= wp.dot(jacp1, edge) * qvel_in[worldid, dofi + k] + if dofnum2 > 0: + dofj = body_dofadr[b2] + offset2 = pos2 - wp.vec3(subtree_com_in[worldid, body_rootid[b2]]) + for k in range(dofnum2): + cdof = cdof_in[worldid, dofj + k] + cdof_ang = wp.spatial_top(cdof) + cdof_lin = wp.spatial_bottom(cdof) + jacp2 = cdof_lin + wp.cross(cdof_ang, offset2) + vel += wp.dot(jacp2, edge) * qvel_in[worldid, dofj + k] + flexedge_velocity_out[worldid, edgeid] = vel rowadr = flexedge_J_rowadr[edgeid] - - # compute offsets once per body (avoids 12 redundant tree-ancestry walks in jac_dof) - offset1 = pos1 - wp.vec3(subtree_com_in[worldid, body_rootid[b1]]) - offset2 = pos2 - wp.vec3(subtree_com_in[worldid, body_rootid[b2]]) + nnz_offset = 0 # body1 DOFs: b1 is in subtree, b2 is not -> jacdif = 0 - jacp1 = -jacp1 - for k in range(3): - cdof = cdof_in[worldid, dofi + k] - cdof_ang = wp.spatial_top(cdof) - cdof_lin = wp.spatial_bottom(cdof) - jacp1 = cdof_lin + wp.cross(cdof_ang, offset1) - flexedge_J_out[worldid, rowadr + k] = wp.dot(-jacp1, edge) + if dofnum1 > 0: + dofi = body_dofadr[b1] + offset1 = pos1 - wp.vec3(subtree_com_in[worldid, body_rootid[b1]]) + for k in range(dofnum1): + cdof = cdof_in[worldid, dofi + k] + cdof_ang = wp.spatial_top(cdof) + cdof_lin = wp.spatial_bottom(cdof) + jacp1 = cdof_lin + wp.cross(cdof_ang, offset1) + flexedge_J_out[worldid, rowadr + nnz_offset + k] = wp.dot(-jacp1, edge) + nnz_offset += dofnum1 # body2 DOFs: b2 is in subtree, b1 is not -> jacdif = jacp2 - 0 = jacp2 - for k in range(3): - cdof = cdof_in[worldid, dofj + k] - cdof_ang = wp.spatial_top(cdof) - cdof_lin = wp.spatial_bottom(cdof) - jacp2 = cdof_lin + wp.cross(cdof_ang, offset2) - flexedge_J_out[worldid, rowadr + 3 + k] = wp.dot(jacp2, edge) + if dofnum2 > 0: + dofj = body_dofadr[b2] + offset2 = pos2 - wp.vec3(subtree_com_in[worldid, body_rootid[b2]]) + for k in range(dofnum2): + cdof = cdof_in[worldid, dofj + k] + cdof_ang = wp.spatial_top(cdof) + cdof_lin = wp.spatial_bottom(cdof) + jacp2 = cdof_lin + wp.cross(cdof_ang, offset2) + flexedge_J_out[worldid, rowadr + nnz_offset + k] = wp.dot(jacp2, edge) @event_scope @@ -382,13 +417,28 @@ def kinematics(m: Model, d: Data): @event_scope def flex(m: Model, d: Data): - wp.launch(_flex_vertices, dim=(d.nworld, m.nflexvert), inputs=[m.flex_vertbodyid, d.xpos], outputs=[d.flexvert_xpos]) + wp.launch( + _flex_vertices, + dim=(d.nworld, m.nflexvert), + inputs=[ + m.nflex, + m.flex_vertadr, + m.flex_vertnum, + m.flex_vertbodyid, + m.flex_vert, + m.flex_centered, + d.xpos, + d.xmat, + ], + outputs=[d.flexvert_xpos], + ) wp.launch( _flex_edges, dim=(d.nworld, m.nflexedge), inputs=[ m.nflex, m.body_rootid, + m.body_dofnum, m.body_dofadr, m.flex_vertadr, m.flex_edgeadr, @@ -790,9 +840,7 @@ def _qM_sparse( bodyid = dof_bodyid[dofid] # init M(i,i) with armature inertia - qM_out[worldid, 0, madr_ij] = dof_armature[ - worldid % dof_armature.shape[0], dofid - ] + qM_out[worldid, 0, madr_ij] = dof_armature[worldid % dof_armature.shape[0], dofid] # precompute buf = crb_body_i * cdof_i buf = math.inert_vec(crb_in[worldid, bodyid], cdof_in[worldid, dofid]) @@ -869,35 +917,55 @@ def _tendon_armature( # Model: dof_parentid: wp.array(dtype=int), dof_Madr: wp.array(dtype=int), + ten_J_rownnz: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), tendon_armature: wp.array2d(dtype=float), is_sparse: bool, # Data in: - ten_J_in: wp.array3d(dtype=float), + ten_J_in: wp.array2d(dtype=float), # Data out: qM_out: wp.array3d(dtype=float), ): worldid, tenid, dofid = wp.tid() - if is_sparse: # is_sparse is not batched - madr_ij = dof_Madr[dofid] - armature = tendon_armature[worldid % tendon_armature.shape[0], tenid] if armature == 0.0: return - ten_Ji = ten_J_in[worldid, tenid, dofid] + rownnz = ten_J_rownnz[tenid] + if dofid >= rownnz: + return + rowadr = ten_J_rowadr[tenid] + dofid_sparse = dofid + sparseid = rowadr + dofid_sparse + dofid = ten_J_colind[sparseid] + ten_Ji = ten_J_in[worldid, sparseid] if ten_Ji == 0.0: return + if is_sparse: + madr_ij = dof_Madr[dofid] + # sparse backward pass over ancestors dofidi = dofid + ptr = dofid_sparse while dofid >= 0: - if dofid != dofidi: - ten_Jj = ten_J_in[worldid, tenid, dofid] - else: + if dofid == dofidi: ten_Jj = ten_Ji + else: + # scan pointer backward to find matching colind entry + while ptr >= 0: + sparseid = rowadr + ptr + if ten_J_colind[sparseid] <= dofid: + break + ptr -= 1 + if ptr >= 0 and ten_J_colind[sparseid] == dofid: + ten_Jj = ten_J_in[worldid, sparseid] + else: + ten_Jj = float(0.0) qMij = armature * ten_Jj * ten_Ji @@ -917,8 +985,17 @@ def tendon_armature(m: Model, d: Data): """Add tendon armature to qM.""" wp.launch( _tendon_armature, - dim=(d.nworld, m.ntendon, m.nv), - inputs=[m.dof_parentid, m.dof_Madr, m.tendon_armature, m.is_sparse, d.ten_J], + dim=(d.nworld, m.ntendon, m.max_ten_J_rownnz), + inputs=[ + m.dof_parentid, + m.dof_Madr, + m.ten_J_rownnz, + m.ten_J_rowadr, + m.ten_J_colind, + m.tendon_armature, + m.is_sparse, + d.ten_J, + ], outputs=[d.qM], ) @@ -1504,19 +1581,93 @@ def rne_postconstraint(m: Model, d: Data): _rne_cfrc_backward(m, d) +@wp.func +def _accumulate_jac_dot_chain( + # Model: + body_parentid: wp.array(dtype=int), + body_dofnum: wp.array(dtype=int), + body_dofadr: wp.array(dtype=int), + jnt_type: wp.array(dtype=int), + jnt_dofadr: wp.array(dtype=int), + dof_jntid: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), + # Data in: + cdof_in: wp.array2d(dtype=wp.spatial_vector), + cvel_in: wp.array2d(dtype=wp.spatial_vector), + cdof_dot_in: wp.array2d(dtype=wp.spatial_vector), + # In: + offset: wp.vec3, + pvel_lin: wp.vec3, + dpnt: wp.vec3, + dvel: wp.vec3, + bodyid: int, + rowadr: int, + rownnz: int, + scale: float, + worldid: int, + # Out: + ten_Jdot_out: wp.array2d(dtype=float), +): + """Walk body chain from bodyid to root, accumulate Jdot contributions.""" + ptr = rownnz - 1 + bid = bodyid + while bid > 0: + bdofadr = body_dofadr[bid] + bdofnum = body_dofnum[bid] + # iterate DOFs in this body in descending order + for k_rev in range(bdofnum): + dof = bdofadr + bdofnum - 1 - k_rev + # scan pointer backward to find matching colind entry + while ptr >= 0: + sparseid = rowadr + ptr + if ten_J_colind[sparseid] <= dof: + break + ptr -= 1 + if ptr >= 0 and ten_J_colind[sparseid] == dof: + cdof = cdof_in[worldid, dof] + cdof_ang = wp.spatial_top(cdof) + cdof_lin = wp.spatial_bottom(cdof) + cdof_dot = cdof_dot_in[worldid, dof] + + # quaternion override: use cvel of DOF's body (which is bid) + dofjntid = dof_jntid[dof] + jnttype = jnt_type[dofjntid] + jntdofadr = jnt_dofadr[dofjntid] + if (jnttype == JointType.BALL) or ((jnttype == JointType.FREE) and dof >= jntdofadr + 3): + cdof_dot = math.motion_cross(cvel_in[worldid, bid], cdof) + + cdof_dot_ang = wp.spatial_top(cdof_dot) + cdof_dot_lin = wp.spatial_bottom(cdof_dot) + + # jacp_dot (from jac_dot_dof) + jacp_dot = cdof_dot_lin + wp.cross(cdof_dot_ang, offset) + wp.cross(cdof_ang, pvel_lin) + + # jacp (from jac_dof) + jacp = cdof_lin + wp.cross(cdof_ang, offset) + + # combined: dot(jacdot, dpnt) + dot(jac, dvel) + Jdot = (wp.dot(jacp_dot, dpnt) + wp.dot(jacp, dvel)) * scale + if Jdot != 0.0: + wp.atomic_add(ten_Jdot_out[worldid], sparseid, Jdot) + bid = body_parentid[bid] + + @wp.kernel def _tendon_dot( # Model: - nv: int, body_parentid: wp.array(dtype=int), body_rootid: wp.array(dtype=int), + body_dofnum: wp.array(dtype=int), + body_dofadr: wp.array(dtype=int), jnt_type: wp.array(dtype=int), jnt_dofadr: wp.array(dtype=int), - dof_bodyid: wp.array(dtype=int), dof_jntid: wp.array(dtype=int), site_bodyid: wp.array(dtype=int), tendon_adr: wp.array(dtype=int), tendon_num: wp.array(dtype=int), + ten_J_rownnz: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), tendon_armature: wp.array2d(dtype=float), wrap_type: wp.array(dtype=int), wrap_objid: wp.array(dtype=int), @@ -1528,7 +1679,7 @@ def _tendon_dot( cvel_in: wp.array2d(dtype=wp.spatial_vector), cdof_dot_in: wp.array2d(dtype=wp.spatial_vector), # Out: - ten_Jdot_out: wp.array3d(dtype=float), + ten_Jdot_out: wp.array2d(dtype=float), ): worldid, tenid = wp.tid() @@ -1565,13 +1716,11 @@ def _tendon_dot( # init sequence; assume it start with site wpnt0 = site_xpos_in[worldid, id0] - bodyid0 = site_bodyid[id0] - pos0 = site_xpos_in[worldid, id0] - cvel0 = cvel_in[worldid, bodyid0] - subtree_com0 = subtree_com_in[worldid, body_rootid[bodyid0]] - dif0 = pos0 - subtree_com0 - wvel0 = wp.spatial_bottom(cvel0) - wp.cross(dif0, wp.spatial_top(cvel0)) wbody0 = site_bodyid[id0] + cvel0 = cvel_in[worldid, wbody0] + subtree_com0 = subtree_com_in[worldid, body_rootid[wbody0]] + offset0 = wpnt0 - subtree_com0 + pvel_lin0 = wp.spatial_bottom(cvel0) - wp.cross(offset0, wp.spatial_top(cvel0)) # second object is geom: process site-geom-site if (type1 == WrapType.SPHERE) or (type1 == WrapType.CYLINDER): @@ -1582,12 +1731,10 @@ def _tendon_dot( wbody1 = site_bodyid[id1] wpnt1 = site_xpos_in[worldid, id1] - bodyid1 = site_bodyid[id1] - pos1 = site_xpos_in[worldid, id1] - cvel1 = cvel_in[worldid, bodyid1] - subtree_com1 = subtree_com_in[worldid, body_rootid[bodyid1]] - dif1 = pos1 - subtree_com1 - wvel1 = wp.spatial_bottom(cvel1) - wp.cross(dif1, wp.spatial_top(cvel1)) + cvel1 = cvel_in[worldid, wbody1] + subtree_com1 = subtree_com_in[worldid, body_rootid[wbody1]] + offset1 = wpnt1 - subtree_com1 + pvel_lin1 = wp.spatial_bottom(cvel1) - wp.cross(offset1, wp.spatial_top(cvel1)) # accumulate moments if consecutive points are in different bodies if wbody0 != wbody1: @@ -1595,6 +1742,8 @@ def _tendon_dot( dpnt, norm = math.normalize_with_norm(wpnt1 - wpnt0) # dvel = d / dt (dpnt) + wvel0 = wp.spatial_bottom(cvel0) - wp.cross(wpnt0 - subtree_com0, wp.spatial_top(cvel0)) + wvel1 = wp.spatial_bottom(cvel1) - wp.cross(wpnt1 - subtree_com1, wp.spatial_top(cvel1)) dvel = wvel1 - wvel0 dot = wp.dot(dpnt, dvel) dvel += dpnt * (-dot) @@ -1603,75 +1752,55 @@ def _tendon_dot( else: dvel = wp.vec3(0.0) - # get endpoint Jacobian time derivatives, subtract - # TODO(team): parallelize? - for i in range(nv): - jac1, _ = support.jac_dot_dof( - body_parentid, - body_rootid, - jnt_type, - jnt_dofadr, - dof_bodyid, - dof_jntid, - subtree_com_in, - cdof_in, - cvel_in, - cdof_dot_in, - wpnt0, - wbody0, - i, - worldid, - ) - jac2, _ = support.jac_dot_dof( - body_parentid, - body_rootid, - jnt_type, - jnt_dofadr, - dof_bodyid, - dof_jntid, - subtree_com_in, - cdof_in, - cvel_in, - cdof_dot_in, - wpnt1, - wbody1, - i, - worldid, - ) - jacdif = jac2 - jac1 + rownnz = ten_J_rownnz[tenid] + rowadr = ten_J_rowadr[tenid] + inv_divisor = math.safe_div(float(1.0), divisor) - # chain rule, first term: Jdot += d / dt (jac2 - jac1) * dpnt - Jdot = wp.dot(jacdif, dpnt) - - # get endpoint Jacobians, subtract - jac1, _ = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - wpnt0, - wbody0, - i, - worldid, - ) - jac2, _ = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - wpnt1, - wbody1, - i, - worldid, - ) - jacdif = jac2 - jac1 - - # chain rule, second term: Jdot += (jac2 - jac1) * d / dt (dpnt) - Jdot += wp.dot(jacdif, dvel) - - ten_Jdot_out[worldid, tenid, i] += math.safe_div(Jdot, divisor) + # body0 contributes with negative sign, body1 with positive + _accumulate_jac_dot_chain( + body_parentid, + body_dofnum, + body_dofadr, + jnt_type, + jnt_dofadr, + dof_jntid, + ten_J_colind, + cdof_in, + cvel_in, + cdof_dot_in, + offset0, + pvel_lin0, + dpnt, + dvel, + wbody0, + rowadr, + rownnz, + -inv_divisor, + worldid, + ten_Jdot_out, + ) + _accumulate_jac_dot_chain( + body_parentid, + body_dofnum, + body_dofadr, + jnt_type, + jnt_dofadr, + dof_jntid, + ten_J_colind, + cdof_in, + cvel_in, + cdof_dot_in, + offset1, + pvel_lin1, + dpnt, + dvel, + wbody1, + rowadr, + rownnz, + inv_divisor, + worldid, + ten_Jdot_out, + ) # TODO(team): j += 2 if geom wrapping j += 1 @@ -1680,33 +1809,45 @@ def _tendon_dot( @wp.kernel def _tendon_bias_coef( # Model: + ten_J_rownnz: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), tendon_armature: wp.array2d(dtype=float), # Data in: qvel_in: wp.array2d(dtype=float), # In: - ten_Jdot_in: wp.array3d(dtype=float), + ten_Jdot_in: wp.array2d(dtype=float), # Out: ten_bias_coef_out: wp.array2d(dtype=float), ): - worldid, tenid, dofid = wp.tid() + worldid, tenid, dofid_sparse = wp.tid() armature = tendon_armature[worldid % tendon_armature.shape[0], tenid] if armature == 0.0: return - ten_Jdot = ten_Jdot_in[worldid, tenid, dofid] + rownnz = ten_J_rownnz[tenid] + if dofid_sparse >= rownnz: + return + rowadr = ten_J_rowadr[tenid] + sparseid = rowadr + dofid_sparse + ten_Jdot = ten_Jdot_in[worldid, sparseid] if ten_Jdot == 0.0: return + dofid = ten_J_colind[sparseid] wp.atomic_add(ten_bias_coef_out[worldid], tenid, ten_Jdot * qvel_in[worldid, dofid]) @wp.kernel def _tendon_bias_qfrc( # Model: + ten_J_rownnz: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), tendon_armature: wp.array2d(dtype=float), # Data in: - ten_J_in: wp.array3d(dtype=float), + ten_J_in: wp.array2d(dtype=float), # In: ten_bias_coef_in: wp.array2d(dtype=float), # Out: @@ -1718,10 +1859,18 @@ def _tendon_bias_qfrc( if armature == 0.0: return - ten_J = ten_J_in[worldid, tenid, dofid] + rownnz = ten_J_rownnz[tenid] + if dofid >= rownnz: + return + rowadr = ten_J_rowadr[tenid] + sparseid = rowadr + dofid + ten_J = ten_J_in[worldid, sparseid] + if ten_J == 0.0: return + dofid = ten_J_colind[sparseid] + wp.atomic_add(qfrc_out[worldid], dofid, ten_J * armature * ten_bias_coef_in[worldid, tenid]) @@ -1735,21 +1884,24 @@ def tendon_bias(m: Model, d: Data, qfrc: wp.array2d(dtype=float)): qfrc: Force. """ # time derivative of tendon Jacobian - ten_Jdot = wp.zeros((d.nworld, m.ntendon, m.nv), dtype=float) + ten_Jdot = wp.zeros((d.nworld, m.nJten), dtype=float) wp.launch( _tendon_dot, dim=(d.nworld, m.ntendon), inputs=[ - m.nv, m.body_parentid, m.body_rootid, + m.body_dofnum, + m.body_dofadr, m.jnt_type, m.jnt_dofadr, - m.dof_bodyid, m.dof_jntid, m.site_bodyid, m.tendon_adr, m.tendon_num, + m.ten_J_rownnz, + m.ten_J_rowadr, + m.ten_J_colind, m.tendon_armature, m.wrap_type, m.wrap_objid, @@ -1767,15 +1919,15 @@ def tendon_bias(m: Model, d: Data, qfrc: wp.array2d(dtype=float)): ten_bias_coef = wp.zeros((d.nworld, m.ntendon), dtype=float) wp.launch( _tendon_bias_coef, - dim=(d.nworld, m.ntendon, m.nv), - inputs=[m.tendon_armature, d.qvel, ten_Jdot], + dim=(d.nworld, m.ntendon, m.max_ten_J_rownnz), + inputs=[m.ten_J_rownnz, m.ten_J_rowadr, m.ten_J_colind, m.tendon_armature, d.qvel, ten_Jdot], outputs=[ten_bias_coef], ) wp.launch( _tendon_bias_qfrc, - dim=(d.nworld, m.ntendon, m.nv), - inputs=[m.tendon_armature, d.ten_J, ten_bias_coef], + dim=(d.nworld, m.ntendon, m.max_ten_J_rownnz), + inputs=[m.ten_J_rownnz, m.ten_J_rowadr, m.ten_J_colind, m.tendon_armature, d.ten_J, ten_bias_coef], outputs=[qfrc], ) @@ -1888,45 +2040,44 @@ def com_vel(m: Model, d: Data): @wp.kernel def _transmission( - # Model: - nv: int, - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - body_weldid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - jnt_type: wp.array(dtype=int), - jnt_qposadr: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - dof_bodyid: wp.array(dtype=int), - dof_parentid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - site_quat: wp.array2d(dtype=wp.quat), - tendon_adr: wp.array(dtype=int), - tendon_num: wp.array(dtype=int), - wrap_type: wp.array(dtype=int), - wrap_objid: wp.array(dtype=int), - actuator_trntype: wp.array(dtype=int), - actuator_trnid: wp.array(dtype=wp.vec2i), - actuator_gear: wp.array2d(dtype=wp.spatial_vector), - actuator_cranklength: wp.array2d(dtype=float), - # Data in: - qpos_in: wp.array2d(dtype=float), - xquat_in: wp.array2d(dtype=wp.quat), - site_xpos_in: wp.array2d(dtype=wp.vec3), - site_xmat_in: wp.array2d(dtype=wp.mat33), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), - ten_J_in: wp.array3d(dtype=float), - ten_length_in: wp.array2d(dtype=float), - # In: - moment_nnz: wp.array(dtype=int), - # Data out: - actuator_length_out: wp.array2d(dtype=float), - moment_rownnz_out: wp.array2d(dtype=int), - moment_rowadr_out: wp.array2d(dtype=int), - moment_colind_out: wp.array2d(dtype=int), - actuator_moment_out: wp.array2d(dtype=float), + # Model: + nv: int, + body_parentid: wp.array(dtype=int), + body_rootid: wp.array(dtype=int), + body_weldid: wp.array(dtype=int), + body_dofnum: wp.array(dtype=int), + body_dofadr: wp.array(dtype=int), + jnt_type: wp.array(dtype=int), + jnt_qposadr: wp.array(dtype=int), + jnt_dofadr: wp.array(dtype=int), + dof_bodyid: wp.array(dtype=int), + dof_parentid: wp.array(dtype=int), + site_bodyid: wp.array(dtype=int), + site_quat: wp.array2d(dtype=wp.quat), + ten_J_rownnz: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), + actuator_trntype: wp.array(dtype=int), + actuator_trnid: wp.array(dtype=wp.vec2i), + actuator_gear: wp.array2d(dtype=wp.spatial_vector), + actuator_cranklength: wp.array2d(dtype=float), + # Data in: + qpos_in: wp.array2d(dtype=float), + xquat_in: wp.array2d(dtype=wp.quat), + site_xpos_in: wp.array2d(dtype=wp.vec3), + site_xmat_in: wp.array2d(dtype=wp.mat33), + subtree_com_in: wp.array2d(dtype=wp.vec3), + cdof_in: wp.array2d(dtype=wp.spatial_vector), + ten_J_in: wp.array2d(dtype=float), + ten_length_in: wp.array2d(dtype=float), + # In: + moment_nnz: wp.array(dtype=int), + # Data out: + actuator_length_out: wp.array2d(dtype=float), + moment_rownnz_out: wp.array2d(dtype=int), + moment_rowadr_out: wp.array2d(dtype=int), + moment_colind_out: wp.array2d(dtype=int), + actuator_moment_out: wp.array2d(dtype=float), ): worldid, actid = wp.tid() trntype = actuator_trntype[actid] @@ -2068,28 +2219,12 @@ def _transmission( # get Jacobians of axis(jacA) and vec(jac) jacp, jacr = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - site_xpos_idslider, - site_bodyid[idslider], - da, - worldid, + body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, site_xpos_idslider, site_bodyid[idslider], da, worldid ) jacS = jacp jacA = wp.cross(jacr, axis) jac, _ = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - site_xpos_id, - site_bodyid[id], - da, - worldid, + body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, site_xpos_id, site_bodyid[id], da, worldid ) jac -= jacS @@ -2110,38 +2245,18 @@ def _transmission( gear0 = gear[0] actuator_length_out[worldid, actid] = ten_length_in[worldid, tenid] * gear0 - # fixed - adr = tendon_adr[tenid] - if wrap_type[adr] == WrapType.JOINT: - ten_num = tendon_num[tenid] - rowadr = wp.atomic_add(moment_nnz, worldid, ten_num) - moment_rownnz_out[worldid, actid] = ten_num - moment_rowadr_out[worldid, actid] = rowadr + rownnz_ten = ten_J_rownnz[tenid] + rowadr_ten = ten_J_rowadr[tenid] - for i in range(ten_num): - dofadr = jnt_dofadr[wrap_objid[adr + i]] - sparseid = rowadr + i - moment_colind_out[worldid, sparseid] = dofadr - actuator_moment_out[worldid, sparseid] = ( - ten_J_in[worldid, tenid, dofadr] * gear0 - ) - else: # spatial - # TODO(team): sparse tendon jacobian - ten_nnz = int(0) - for dofadr in range(nv): - if ten_J_in[worldid, tenid, dofadr] != 0.0: - ten_nnz += 1 - rowadr = wp.atomic_add(moment_nnz, worldid, ten_nnz) - moment_rownnz_out[worldid, actid] = ten_nnz - moment_rowadr_out[worldid, actid] = rowadr - ptr = int(0) - for dofadr in range(nv): - J = ten_J_in[worldid, tenid, dofadr] - if J != 0.0: - sparseid = rowadr + ptr - moment_colind_out[worldid, sparseid] = dofadr - actuator_moment_out[worldid, sparseid] = J * gear0 - ptr += 1 + rowadr_mom = wp.atomic_add(moment_nnz, worldid, rownnz_ten) + moment_rownnz_out[worldid, actid] = rownnz_ten + moment_rowadr_out[worldid, actid] = rowadr_mom + + for k in range(rownnz_ten): + sparseid_ten = rowadr_ten + k + sparseid_mom = rowadr_mom + k + moment_colind_out[worldid, sparseid_mom] = ten_J_colind[sparseid_ten] + actuator_moment_out[worldid, sparseid_mom] = ten_J_in[worldid, sparseid_ten] * gear0 elif trntype == TrnType.BODY: # cannot compute meaningful length, set to zero actuator_length_out[worldid, actid] = 0.0 @@ -2195,19 +2310,17 @@ def _transmission( ptr = ndof - 1 while da >= 0: jacp, jacr = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - site_xpos_in[worldid, siteid], - site_bodyid[siteid], - da, - worldid, - ) - moment = wp.dot(jacp, wrench_translation) + wp.dot( - jacr, wrench_rotation + body_parentid, + body_rootid, + dof_bodyid, + subtree_com_in, + cdof_in, + site_xpos_in[worldid, siteid], + site_bodyid[siteid], + da, + worldid, ) + moment = wp.dot(jacp, wrench_translation) + wp.dot(jacr, wrench_rotation) sparseid = rowadr + ptr moment_colind_out[worldid, sparseid] = da actuator_moment_out[worldid, sparseid] = moment @@ -2306,26 +2419,10 @@ def _transmission( break jacp, jacr = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - site_xpos, - site_bodyid[siteid], - da, - worldid, + body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, site_xpos, site_bodyid[siteid], da, worldid ) jacpref, jacrref = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - ref_xpos, - site_bodyid[refid], - da, - worldid, + body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, ref_xpos, site_bodyid[refid], da, worldid ) moment = float(0.0) @@ -2349,37 +2446,37 @@ def _transmission( @wp.kernel def _transmission_body_moment( - # Model: - opt_cone: int, - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - dof_bodyid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - actuator_trnid: wp.array(dtype=wp.vec2i), - actuator_trntype_body_adr: wp.array(dtype=int), - # Data in: - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), - moment_rowadr_in: wp.array2d(dtype=int), - contact_dist_in: wp.array(dtype=float), - contact_pos_in: wp.array(dtype=wp.vec3), - contact_frame_in: wp.array(dtype=wp.mat33), - contact_includemargin_in: wp.array(dtype=float), - contact_dim_in: wp.array(dtype=int), - contact_geom_in: wp.array(dtype=wp.vec2i), - contact_efc_address_in: wp.array2d(dtype=int), - contact_worldid_in: wp.array(dtype=int), - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), - efc_J_colind_in: wp.array3d(dtype=int), - efc_J_in: wp.array3d(dtype=float), - nacon_in: wp.array(dtype=int), - # In: - efc_is_sparse: bool, - # Data out: - actuator_moment_out: wp.array2d(dtype=float), - # Out: - actuator_trntype_body_ncon_out: wp.array2d(dtype=int), + # Model: + opt_cone: int, + body_parentid: wp.array(dtype=int), + body_rootid: wp.array(dtype=int), + dof_bodyid: wp.array(dtype=int), + geom_bodyid: wp.array(dtype=int), + actuator_trnid: wp.array(dtype=wp.vec2i), + actuator_trntype_body_adr: wp.array(dtype=int), + # Data in: + subtree_com_in: wp.array2d(dtype=wp.vec3), + cdof_in: wp.array2d(dtype=wp.spatial_vector), + moment_rowadr_in: wp.array2d(dtype=int), + contact_dist_in: wp.array(dtype=float), + contact_pos_in: wp.array(dtype=wp.vec3), + contact_frame_in: wp.array(dtype=wp.mat33), + contact_includemargin_in: wp.array(dtype=float), + contact_dim_in: wp.array(dtype=int), + contact_geom_in: wp.array(dtype=wp.vec2i), + contact_efc_address_in: wp.array2d(dtype=int), + contact_worldid_in: wp.array(dtype=int), + efc_J_rownnz_in: wp.array2d(dtype=int), + efc_J_rowadr_in: wp.array2d(dtype=int), + efc_J_colind_in: wp.array3d(dtype=int), + efc_J_in: wp.array3d(dtype=float), + nacon_in: wp.array(dtype=int), + # In: + efc_is_sparse: bool, + # Data out: + actuator_moment_out: wp.array2d(dtype=float), + # Out: + actuator_trntype_body_ncon_out: wp.array2d(dtype=int), ): trnbodyid, conid, dofid = wp.tid() actid = actuator_trntype_body_adr[trnbodyid] @@ -2427,20 +2524,12 @@ def _transmission_body_moment( efc_rowadr = efc_J_rowadr_in[worldid, efcid0] efc_sparseid = efc_rowadr + dofid colind = efc_J_colind_in[worldid, 0, efc_sparseid] - wp.atomic_add( - actuator_moment_out[worldid], - rowadr + colind, - efc_J_in[worldid, 0, efc_sparseid], - ) + wp.atomic_add(actuator_moment_out[worldid], rowadr + colind, efc_J_in[worldid, 0, efc_sparseid]) else: return else: colind = dofid - wp.atomic_add( - actuator_moment_out[worldid], - rowadr + colind, - efc_J_in[worldid, efcid0, dofid], - ) + wp.atomic_add(actuator_moment_out[worldid], rowadr + colind, efc_J_in[worldid, efcid0, dofid]) else: npyramid = contact_dim - 1 # number of frictional directions efc_force = 0.5 / float(npyramid) @@ -2453,20 +2542,12 @@ def _transmission_body_moment( efc_rowadr = efc_J_rowadr_in[worldid, efcid] efc_sparseid = efc_rowadr + dofid colind = efc_J_colind_in[worldid, 0, efc_sparseid] - wp.atomic_add( - actuator_moment_out[worldid], - rowadr + colind, - efc_J_in[worldid, 0, efc_sparseid] * efc_force, - ) + wp.atomic_add(actuator_moment_out[worldid], rowadr + colind, efc_J_in[worldid, 0, efc_sparseid] * efc_force) else: return else: colind = dofid - wp.atomic_add( - actuator_moment_out[worldid], - rowadr + colind, - efc_J_in[worldid, efcid, dofid] * efc_force, - ) + wp.atomic_add(actuator_moment_out[worldid], rowadr + colind, efc_J_in[worldid, efcid, dofid] * efc_force) # excluded contact in gap: get Jacobian, accumulate elif contact_exclude == 1: @@ -2487,46 +2568,28 @@ def _transmission_body_moment( colind = dofid jacp1, _ = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - contact_pos, - b1, - colind, - worldid, + body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, contact_pos, b1, colind, worldid ) jacp2, _ = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - contact_pos, - b2, - colind, - worldid, + body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, contact_pos, b2, colind, worldid ) jacdif = jacp2 - jacp1 # project Jacobian along the normal of the contact frame - wp.atomic_add( - actuator_moment_out[worldid], rowadr + colind, wp.dot(normal, jacdif) - ) + wp.atomic_add(actuator_moment_out[worldid], rowadr + colind, wp.dot(normal, jacdif)) @wp.kernel def _transmission_body_moment_scale( - # Model: - actuator_trntype_body_adr: wp.array(dtype=int), - # Data in: - moment_rowadr_in: wp.array2d(dtype=int), - # In: - actuator_trntype_body_ncon_in: wp.array2d(dtype=int), - # Data out: - actuator_moment_out: wp.array2d(dtype=float), + # Model: + actuator_trntype_body_adr: wp.array(dtype=int), + # Data in: + moment_rowadr_in: wp.array2d(dtype=int), + # In: + actuator_trntype_body_ncon_in: wp.array2d(dtype=int), + # Data out: + actuator_moment_out: wp.array2d(dtype=float), ): worldid, trnbodyid, dofid = wp.tid() @@ -2549,47 +2612,40 @@ def transmission(m: Model, d: Data): moment_nnz = wp.zeros((d.nworld,), dtype=int) wp.launch( - _transmission, - dim=(d.nworld, m.nu), - inputs=[ - m.nv, - m.body_parentid, - m.body_rootid, - m.body_weldid, - m.body_dofnum, - m.body_dofadr, - m.jnt_type, - m.jnt_qposadr, - m.jnt_dofadr, - m.dof_bodyid, - m.dof_parentid, - m.site_bodyid, - m.site_quat, - m.tendon_adr, - m.tendon_num, - m.wrap_type, - m.wrap_objid, - m.actuator_trntype, - m.actuator_trnid, - m.actuator_gear, - m.actuator_cranklength, - d.qpos, - d.xquat, - d.site_xpos, - d.site_xmat, - d.subtree_com, - d.cdof, - d.ten_J, - d.ten_length, - moment_nnz, - ], - outputs=[ - d.actuator_length, - d.moment_rownnz, - d.moment_rowadr, - d.moment_colind, - d.actuator_moment, - ], + _transmission, + dim=(d.nworld, m.nu), + inputs=[ + m.nv, + m.body_parentid, + m.body_rootid, + m.body_weldid, + m.body_dofnum, + m.body_dofadr, + m.jnt_type, + m.jnt_qposadr, + m.jnt_dofadr, + m.dof_bodyid, + m.dof_parentid, + m.site_bodyid, + m.site_quat, + m.ten_J_rownnz, + m.ten_J_rowadr, + m.ten_J_colind, + m.actuator_trntype, + m.actuator_trnid, + m.actuator_gear, + m.actuator_cranklength, + d.qpos, + d.xquat, + d.site_xpos, + d.site_xmat, + d.subtree_com, + d.cdof, + d.ten_J, + d.ten_length, + moment_nnz, + ], + outputs=[d.actuator_length, d.moment_rownnz, d.moment_rowadr, d.moment_colind, d.actuator_moment], ) if m.nacttrnbody: @@ -2597,83 +2653,105 @@ def transmission(m: Model, d: Data): ncon = wp.zeros((d.nworld, m.nacttrnbody), dtype=int) wp.launch( - _transmission_body_moment, - dim=(m.nacttrnbody, d.naconmax, m.nv), - inputs=[ - m.opt.cone, - m.body_parentid, - m.body_rootid, - m.dof_bodyid, - m.geom_bodyid, - m.actuator_trnid, - m.actuator_trntype_body_adr, - d.subtree_com, - d.cdof, - d.moment_rowadr, - d.contact.dist, - d.contact.pos, - d.contact.frame, - d.contact.includemargin, - d.contact.dim, - d.contact.geom, - d.contact.efc_address, - d.contact.worldid, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.nacon, - SPARSE_CONSTRAINT_JACOBIAN, - ], - outputs=[d.actuator_moment, ncon], + _transmission_body_moment, + dim=(m.nacttrnbody, d.naconmax, m.nv), + inputs=[ + m.opt.cone, + m.body_parentid, + m.body_rootid, + m.dof_bodyid, + m.geom_bodyid, + m.actuator_trnid, + m.actuator_trntype_body_adr, + d.subtree_com, + d.cdof, + d.moment_rowadr, + d.contact.dist, + d.contact.pos, + d.contact.frame, + d.contact.includemargin, + d.contact.dim, + d.contact.geom, + d.contact.efc_address, + d.contact.worldid, + d.efc.J_rownnz, + d.efc.J_rowadr, + d.efc.J_colind, + d.efc.J, + d.nacon, + m.is_sparse, + ], + outputs=[d.actuator_moment, ncon], ) # scale moments wp.launch( - _transmission_body_moment_scale, - dim=(d.nworld, m.nacttrnbody, m.nv), - inputs=[m.actuator_trntype_body_adr, d.moment_rowadr, ncon], - outputs=[d.actuator_moment], + _transmission_body_moment_scale, + dim=(d.nworld, m.nacttrnbody, m.nv), + inputs=[m.actuator_trntype_body_adr, d.moment_rowadr, ncon], + outputs=[d.actuator_moment], ) -@wp.kernel -def _solve_LD_sparse_x_acc_up( - # In: - L: wp.array3d(dtype=float), - qLD_updates_: wp.array(dtype=wp.vec3i), - # Out: - x: wp.array2d(dtype=float), -): - worldid, nodeid = wp.tid() - update = qLD_updates_[nodeid] - i, k, Madr_ki = update[0], update[1], update[2] - wp.atomic_sub(x[worldid], i, L[worldid, 0, Madr_ki] * x[worldid, k]) +@cache_kernel +def _solve_LD_sparse_fused(nv: int, nlevels: int): + """Fused sparse backsubstitution: UP + diag + DOWN in one kernel.""" + @wp.func_native(snippet="WP_TILE_SYNC();") + def _syncthreads(): + pass -@wp.kernel -def _solve_LD_sparse_qLDiag_mul( - # In: - D: wp.array2d(dtype=float), - # Out: - out: wp.array2d(dtype=float), -): - worldid, dofid = wp.tid() - out[worldid, dofid] *= D[worldid, dofid] + @wp.kernel(module="unique", enable_backward=False) + def kernel( + # In: + L: wp.array3d(dtype=float), + D: wp.array2d(dtype=float), + all_updates: wp.array(dtype=wp.vec3i), + level_offsets: wp.array(dtype=int), + y: wp.array2d(dtype=float), + # Out: + x_out: wp.array2d(dtype=float), + ): + worldid, tid = wp.tid() + NV = wp.static(nv) + NLEVELS = wp.static(nlevels) + BLOCK_DIM = wp.block_dim() + # Copy y to x_out + for dofid in range(tid, NV, BLOCK_DIM): + x_out[worldid, dofid] = y[worldid, dofid] + _syncthreads() -@wp.kernel -def _solve_LD_sparse_x_acc_down( - # In: - L: wp.array3d(dtype=float), - qLD_updates_: wp.array(dtype=wp.vec3i), - # Out: - x: wp.array2d(dtype=float), -): - worldid, nodeid = wp.tid() - update = qLD_updates_[nodeid] - i, k, Madr_ki = update[0], update[1], update[2] - wp.atomic_sub(x[worldid], k, L[worldid, 0, Madr_ki] * x[worldid, i]) + # Forward substitution + for level in range(NLEVELS): + level_idx = NLEVELS - 1 - level + level_offset = level_offsets[level_idx] + level_size = level_offsets[level_idx + 1] - level_offset + + for u in range(tid, level_size, BLOCK_DIM): + update = all_updates[level_offset + u] + i, k, Madr_ki = update[0], update[1], update[2] + wp.atomic_sub(x_out[worldid], i, L[worldid, 0, Madr_ki] * x_out[worldid, k]) + _syncthreads() + + # Diagonal multiply + for dofid in range(tid, NV, BLOCK_DIM): + x_out[worldid, dofid] *= D[worldid, dofid] + _syncthreads() + + # Backward substitution + for level in range(NLEVELS): + level_idx = level + level_offset = level_offsets[level_idx] + level_size = level_offsets[level_idx + 1] - level_offset + + for u in range(tid, level_size, BLOCK_DIM): + update = all_updates[level_offset + u] + i, k, Madr_ki = update[0], update[1], update[2] + wp.atomic_sub(x_out[worldid], k, L[worldid, 0, Madr_ki] * x_out[worldid, i]) + _syncthreads() + + return kernel def _solve_LD_sparse( @@ -2685,14 +2763,20 @@ def _solve_LD_sparse( y: wp.array2d(dtype=float), ): """Computes sparse backsubstitution: x = inv(L'*D*L)*y.""" - wp.copy(x, y) - for qLD_updates in reversed(m.qLD_updates): - wp.launch(_solve_LD_sparse_x_acc_up, dim=(d.nworld, qLD_updates.size), inputs=[L, qLD_updates], outputs=[x]) + nlevels = len(m.qLD_updates) + if wp.get_device().is_cuda: + dim_block = m.block_dim.solve_LD_sparse_fused + else: + # Fallback for CPU + dim_block = 1 - wp.launch(_solve_LD_sparse_qLDiag_mul, dim=(d.nworld, m.nv), inputs=[D], outputs=[x]) - - for qLD_updates in m.qLD_updates: - wp.launch(_solve_LD_sparse_x_acc_down, dim=(d.nworld, qLD_updates.size), inputs=[L, qLD_updates], outputs=[x]) + wp.launch( + _solve_LD_sparse_fused(m.nv, nlevels), + dim=(d.nworld, dim_block), + inputs=[L, D, m.qLD_all_updates, m.qLD_level_offsets, y], + outputs=[x], + block_dim=dim_block, + ) @cache_kernel @@ -3005,6 +3089,9 @@ def _joint_tendon( # Model: jnt_qposadr: wp.array(dtype=int), jnt_dofadr: wp.array(dtype=int), + ten_J_rownnz: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), wrap_objid: wp.array(dtype=int), wrap_prm: wp.array(dtype=float), tendon_jnt_adr: wp.array(dtype=int), @@ -3012,34 +3099,87 @@ def _joint_tendon( # Data in: qpos_in: wp.array2d(dtype=float), # Data out: - ten_J_out: wp.array3d(dtype=float), + ten_J_out: wp.array2d(dtype=float), ten_length_out: wp.array2d(dtype=float), ): worldid, wrapid = wp.tid() - tendon_jnt_adr_ = tendon_jnt_adr[wrapid] - wrap_jnt_adr_ = wrap_jnt_adr[wrapid] - - wrap_objid_ = wrap_objid[wrap_jnt_adr_] - prm = wrap_prm[wrap_jnt_adr_] + tenid = tendon_jnt_adr[wrapid] + wrapjntid = wrap_jnt_adr[wrapid] + wrapobjid = wrap_objid[wrapjntid] + prm = wrap_prm[wrapjntid] # add to length - L = prm * qpos_in[worldid, jnt_qposadr[wrap_objid_]] - # TODO(team): compare atomic_add and for loop - wp.atomic_add(ten_length_out[worldid], tendon_jnt_adr_, L) + L = prm * qpos_in[worldid, jnt_qposadr[wrapobjid]] + wp.atomic_add(ten_length_out[worldid], tenid, L) # add to moment - ten_J_out[worldid, tendon_jnt_adr_, jnt_dofadr[wrap_objid_]] = prm + dofadr = jnt_dofadr[wrapobjid] + rowadr = ten_J_rowadr[tenid] + rownnz = ten_J_rownnz[tenid] + for k in range(rownnz): + if ten_J_colind[rowadr + k] == dofadr: + ten_J_out[worldid, rowadr + k] = prm + break + + +@wp.func +def _accumulate_jac_chain( + # Model: + body_parentid: wp.array(dtype=int), + body_dofnum: wp.array(dtype=int), + body_dofadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), + # Data in: + cdof_in: wp.array2d(dtype=wp.spatial_vector), + # In: + offset: wp.vec3, + vec: wp.vec3, + bodyid: int, + rowadr: int, + rownnz: int, + scale: float, + worldid: int, + # Data out: + ten_J_out: wp.array2d(dtype=float), +): + """Walk body chain from bodyid to root, accumulate Jacobian contributions.""" + ptr = rownnz - 1 + bid = bodyid + while bid > 0: + bdofadr = body_dofadr[bid] + bdofnum = body_dofnum[bid] + # iterate DOFs in this body in descending order + for k_rev in range(bdofnum): + dof = bdofadr + bdofnum - 1 - k_rev + # scan pointer backward to find matching colind entry + while ptr >= 0: + sparseid = rowadr + ptr + if ten_J_colind[sparseid] <= dof: + break + ptr -= 1 + if ptr >= 0 and ten_J_colind[sparseid] == dof: + cdof = cdof_in[worldid, dof] + cdof_ang = wp.spatial_top(cdof) + cdof_lin = wp.spatial_bottom(cdof) + jacp = cdof_lin + wp.cross(cdof_ang, offset) + J = wp.dot(jacp, vec) * scale + if J != 0.0: + wp.atomic_add(ten_J_out[worldid], sparseid, J) + bid = body_parentid[bid] @wp.kernel def _spatial_site_tendon( # Model: - nv: int, body_parentid: wp.array(dtype=int), body_rootid: wp.array(dtype=int), - dof_bodyid: wp.array(dtype=int), + body_dofnum: wp.array(dtype=int), + body_dofadr: wp.array(dtype=int), site_bodyid: wp.array(dtype=int), + ten_J_rownnz: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), wrap_objid: wp.array(dtype=int), tendon_site_pair_adr: wp.array(dtype=int), wrap_site_pair_adr: wp.array(dtype=int), @@ -3049,14 +3189,14 @@ def _spatial_site_tendon( subtree_com_in: wp.array2d(dtype=wp.vec3), cdof_in: wp.array2d(dtype=wp.spatial_vector), # Data out: - ten_J_out: wp.array3d(dtype=float), + ten_J_out: wp.array2d(dtype=float), ten_length_out: wp.array2d(dtype=float), ): worldid, elementid = wp.tid() # site pairs site_pair_adr = wrap_site_pair_adr[elementid] - ten_adr = tendon_site_pair_adr[elementid] + tenid = tendon_site_pair_adr[elementid] # pulley scaling pulley_scale = wrap_pulley_scale[site_pair_adr] @@ -3068,7 +3208,7 @@ def _spatial_site_tendon( pnt1 = site_xpos_in[worldid, id1] dif = pnt1 - pnt0 vec, length = math.normalize_with_norm(dif) - wp.atomic_add(ten_length_out[worldid], ten_adr, length * pulley_scale) + wp.atomic_add(ten_length_out[worldid], tenid, length * pulley_scale) if length < MJ_MINVAL: vec = wp.vec3(1.0, 0.0, 0.0) @@ -3076,26 +3216,55 @@ def _spatial_site_tendon( body0 = site_bodyid[id0] body1 = site_bodyid[id1] if body0 != body1: - # TODO(team): parallelize - for i in range(nv): - jacp1, _ = support.jac_dof(body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, pnt0, body0, i, worldid) - jacp2, _ = support.jac_dof(body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, pnt1, body1, i, worldid) - - J = wp.dot(jacp2 - jacp1, vec) - if J: - wp.atomic_add(ten_J_out[worldid, ten_adr], i, J * pulley_scale) + rownnz = ten_J_rownnz[tenid] + rowadr = ten_J_rowadr[tenid] + offset0 = pnt0 - subtree_com_in[worldid, body_rootid[body0]] + offset1 = pnt1 - subtree_com_in[worldid, body_rootid[body1]] + _accumulate_jac_chain( + body_parentid, + body_dofnum, + body_dofadr, + ten_J_colind, + cdof_in, + offset0, + vec, + body0, + rowadr, + rownnz, + -pulley_scale, + worldid, + ten_J_out, + ) + _accumulate_jac_chain( + body_parentid, + body_dofnum, + body_dofadr, + ten_J_colind, + cdof_in, + offset1, + vec, + body1, + rowadr, + rownnz, + pulley_scale, + worldid, + ten_J_out, + ) @wp.kernel def _spatial_geom_tendon( # Model: - nv: int, body_parentid: wp.array(dtype=int), body_rootid: wp.array(dtype=int), - dof_bodyid: wp.array(dtype=int), + body_dofnum: wp.array(dtype=int), + body_dofadr: wp.array(dtype=int), geom_bodyid: wp.array(dtype=int), geom_size: wp.array2d(dtype=wp.vec3), site_bodyid: wp.array(dtype=int), + ten_J_rownnz: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), wrap_type: wp.array(dtype=int), wrap_objid: wp.array(dtype=int), wrap_prm: wp.array(dtype=float), @@ -3109,14 +3278,14 @@ def _spatial_geom_tendon( subtree_com_in: wp.array2d(dtype=wp.vec3), cdof_in: wp.array2d(dtype=wp.spatial_vector), # Data out: - ten_J_out: wp.array3d(dtype=float), + ten_J_out: wp.array2d(dtype=float), ten_length_out: wp.array2d(dtype=float), # Out: wrap_geom_xpos_out: wp.array2d(dtype=wp.spatial_vector), ): worldid, elementid = wp.tid() wrap_adr = wrap_geom_adr[elementid] - ten_adr = tendon_geom_adr[elementid] + tenid = tendon_geom_adr[elementid] # pulley scaling pulley_scale = wrap_pulley_scale[wrap_adr] @@ -3154,6 +3323,9 @@ def _spatial_geom_tendon( # store geom points wrap_geom_xpos_out[worldid, elementid] = wp.spatial_vector(geom_pnt0, geom_pnt1) + rownnz = ten_J_rownnz[tenid] + rowadr = ten_J_rowadr[tenid] + if length_geomgeom >= 0.0: dif_sitegeom = geom_pnt0 - site_pnt0 dif_geomsite = site_pnt1 - geom_pnt1 @@ -3164,7 +3336,7 @@ def _spatial_geom_tendon( length_sitegeomsite = length_sitegeom + length_geomgeom + length_geomsite if length_sitegeomsite: - wp.atomic_add(ten_length_out[worldid], ten_adr, length_sitegeomsite * pulley_scale) + wp.atomic_add(ten_length_out[worldid], tenid, length_sitegeomsite * pulley_scale) # moment if length_sitegeom < MJ_MINVAL: @@ -3176,61 +3348,120 @@ def _spatial_geom_tendon( dif_body_sitegeom = bodyid_site0 != bodyid_geom dif_body_geomsite = bodyid_geom != bodyid_site1 - # TODO(team): parallelize - for i in range(nv): - J = float(0.0) - # site-geom - if dif_body_sitegeom: - jacp_site0, _ = support.jac_dof( - body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, site_pnt0, bodyid_site0, i, worldid - ) + # site-geom segment + if dif_body_sitegeom: + offset_site0 = site_pnt0 - subtree_com_in[worldid, body_rootid[bodyid_site0]] + offset_geom0 = geom_pnt0 - subtree_com_in[worldid, body_rootid[bodyid_geom]] + _accumulate_jac_chain( + body_parentid, + body_dofnum, + body_dofadr, + ten_J_colind, + cdof_in, + offset_site0, + vec_sitegeom, + bodyid_site0, + rowadr, + rownnz, + -pulley_scale, + worldid, + ten_J_out, + ) + _accumulate_jac_chain( + body_parentid, + body_dofnum, + body_dofadr, + ten_J_colind, + cdof_in, + offset_geom0, + vec_sitegeom, + bodyid_geom, + rowadr, + rownnz, + pulley_scale, + worldid, + ten_J_out, + ) - jacp_geom0, _ = support.jac_dof( - body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, geom_pnt0, bodyid_geom, i, worldid - ) - - J += wp.dot(jacp_geom0 - jacp_site0, vec_sitegeom) - - # geom-site - if dif_body_geomsite: - jacp_geom1, _ = support.jac_dof( - body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, geom_pnt1, bodyid_geom, i, worldid - ) - - jacp_site1, _ = support.jac_dof( - body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, site_pnt1, bodyid_site1, i, worldid - ) - - J += wp.dot(jacp_site1 - jacp_geom1, vec_geomsite) - - if J: - wp.atomic_add(ten_J_out[worldid, ten_adr], i, J * pulley_scale) + # geom-site segment + if dif_body_geomsite: + offset_geom1 = geom_pnt1 - subtree_com_in[worldid, body_rootid[bodyid_geom]] + offset_site1 = site_pnt1 - subtree_com_in[worldid, body_rootid[bodyid_site1]] + _accumulate_jac_chain( + body_parentid, + body_dofnum, + body_dofadr, + ten_J_colind, + cdof_in, + offset_geom1, + vec_geomsite, + bodyid_geom, + rowadr, + rownnz, + -pulley_scale, + worldid, + ten_J_out, + ) + _accumulate_jac_chain( + body_parentid, + body_dofnum, + body_dofadr, + ten_J_colind, + cdof_in, + offset_site1, + vec_geomsite, + bodyid_site1, + rowadr, + rownnz, + pulley_scale, + worldid, + ten_J_out, + ) else: dif_sitesite = site_pnt1 - site_pnt0 vec_sitesite, length_sitesite = math.normalize_with_norm(dif_sitesite) # length if length_sitesite: - wp.atomic_add(ten_length_out[worldid], ten_adr, length_sitesite * pulley_scale) + wp.atomic_add(ten_length_out[worldid], tenid, length_sitesite * pulley_scale) # moment if length_sitesite < MJ_MINVAL: vec_sitesite = wp.vec3(1.0, 0.0, 0.0) if bodyid_site0 != bodyid_site1: - # TODO(team): parallelize - for i in range(nv): - jacp1, _ = support.jac_dof( - body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, site_pnt0, bodyid_site0, i, worldid - ) - jacp2, _ = support.jac_dof( - body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, site_pnt1, bodyid_site1, i, worldid - ) - - J = wp.dot(jacp2 - jacp1, vec_sitesite) - - if J: - wp.atomic_add(ten_J_out[worldid, ten_adr], i, J * pulley_scale) + offset_site0 = site_pnt0 - subtree_com_in[worldid, body_rootid[bodyid_site0]] + offset_site1 = site_pnt1 - subtree_com_in[worldid, body_rootid[bodyid_site1]] + _accumulate_jac_chain( + body_parentid, + body_dofnum, + body_dofadr, + ten_J_colind, + cdof_in, + offset_site0, + vec_sitesite, + bodyid_site0, + rowadr, + rownnz, + -pulley_scale, + worldid, + ten_J_out, + ) + _accumulate_jac_chain( + body_parentid, + body_dofnum, + body_dofadr, + ten_J_colind, + cdof_in, + offset_site1, + vec_sitesite, + bodyid_site1, + rowadr, + rownnz, + pulley_scale, + worldid, + ten_J_out, + ) @wp.kernel @@ -3412,7 +3643,18 @@ def tendon(m: Model, d: Data): wp.launch( _joint_tendon, dim=(d.nworld, m.wrap_jnt_adr.size), - inputs=[m.jnt_qposadr, m.jnt_dofadr, m.wrap_objid, m.wrap_prm, m.tendon_jnt_adr, m.wrap_jnt_adr, d.qpos], + inputs=[ + m.jnt_qposadr, + m.jnt_dofadr, + m.ten_J_rownnz, + m.ten_J_rowadr, + m.ten_J_colind, + m.wrap_objid, + m.wrap_prm, + m.tendon_jnt_adr, + m.wrap_jnt_adr, + d.qpos, + ], outputs=[d.ten_J, d.ten_length], ) @@ -3428,11 +3670,14 @@ def tendon(m: Model, d: Data): _spatial_site_tendon, dim=(d.nworld, m.wrap_site_pair_adr.size), inputs=[ - m.nv, m.body_parentid, m.body_rootid, - m.dof_bodyid, + m.body_dofnum, + m.body_dofadr, m.site_bodyid, + m.ten_J_rownnz, + m.ten_J_rowadr, + m.ten_J_colind, m.wrap_objid, m.tendon_site_pair_adr, m.wrap_site_pair_adr, @@ -3449,13 +3694,16 @@ def tendon(m: Model, d: Data): _spatial_geom_tendon, dim=(d.nworld, m.wrap_geom_adr.size), inputs=[ - m.nv, m.body_parentid, m.body_rootid, - m.dof_bodyid, + m.body_dofnum, + m.body_dofadr, m.geom_bodyid, m.geom_size, m.site_bodyid, + m.ten_J_rownnz, + m.ten_J_rowadr, + m.ten_J_colind, m.wrap_type, m.wrap_objid, m.wrap_prm, diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/solver.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/solver.py index 82ac23c7..2fabebe0 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/solver.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/solver.py @@ -17,17 +17,17 @@ import dataclasses from math import ceil from math import sqrt +import warp as wp + from mujoco.mjx.third_party.mujoco_warp._src import math from mujoco.mjx.third_party.mujoco_warp._src import smooth from mujoco.mjx.third_party.mujoco_warp._src import support from mujoco.mjx.third_party.mujoco_warp._src import types from mujoco.mjx.third_party.mujoco_warp._src.block_cholesky import create_blocked_cholesky_func from mujoco.mjx.third_party.mujoco_warp._src.block_cholesky import create_blocked_cholesky_solve_func -from mujoco.mjx.third_party.mujoco_warp._src.types import SPARSE_CONSTRAINT_JACOBIAN from mujoco.mjx.third_party.mujoco_warp._src.warp_util import cache_kernel from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope from mujoco.mjx.third_party.mujoco_warp._src.warp_util import scoped_mathdx_gemm_disabled -import warp as wp wp.set_module_options({"enable_backward": False}) @@ -91,14 +91,14 @@ def create_inverse_context(m: types.Model, d: types.Data) -> InverseContext: njmax = d.njmax return InverseContext( - Jaref=wp.empty((nworld, njmax), dtype=float), - search_dot=wp.empty((nworld,), dtype=float), - gauss=wp.empty((nworld,), dtype=float), - cost=wp.empty((nworld,), dtype=float), - prev_cost=wp.empty((nworld,), dtype=float), - done=wp.empty((nworld,), dtype=bool), - changed_efc_ids=wp.empty((nworld, 0), dtype=int), - changed_efc_count=wp.empty((0,), dtype=int), + Jaref=wp.empty((nworld, njmax), dtype=float), + search_dot=wp.empty((nworld,), dtype=float), + gauss=wp.empty((nworld,), dtype=float), + cost=wp.empty((nworld,), dtype=float), + prev_cost=wp.empty((nworld,), dtype=float), + done=wp.empty((nworld,), dtype=bool), + changed_efc_ids=wp.empty((nworld, 0), dtype=int), + changed_efc_count=wp.empty((0,), dtype=int), ) @@ -121,36 +121,28 @@ def create_solver_context(m: types.Model, d: types.Data) -> SolverContext: alloc_hfactor = alloc_h and nv > _BLOCK_CHOLESKY_DIM return SolverContext( - Jaref=wp.empty((nworld, njmax), dtype=float), - search_dot=wp.empty((nworld,), dtype=float), - gauss=wp.empty((nworld,), dtype=float), - cost=wp.empty((nworld,), dtype=float), - prev_cost=wp.empty((nworld,), dtype=float), - done=wp.empty((nworld,), dtype=bool), - grad=wp.zeros((nworld, nv_pad), dtype=float), - grad_dot=wp.empty((nworld,), dtype=float), - Mgrad=wp.zeros((nworld, nv_pad), dtype=float), - search=wp.empty((nworld, nv), dtype=float), - mv=wp.empty((nworld, nv), dtype=float), - jv=wp.empty((nworld, njmax), dtype=float), - quad=wp.empty((nworld, njmax), dtype=wp.vec3), - quad_gauss=wp.empty((nworld,), dtype=wp.vec3), - alpha=wp.empty((nworld,), dtype=float), - prev_grad=wp.empty((nworld, nv), dtype=float), - prev_Mgrad=wp.empty((nworld, nv), dtype=float), - beta=wp.empty((nworld,), dtype=float), - h=wp.zeros((nworld, nv_pad, nv_pad), dtype=float) - if alloc_h - else wp.empty((nworld, 0, 0), dtype=float), - hfactor=wp.zeros((nworld, nv_pad, nv_pad), dtype=float) - if alloc_hfactor - else wp.empty((nworld, 0, 0), dtype=float), - changed_efc_ids=wp.empty((nworld, njmax), dtype=int) - if alloc_h - else wp.empty((nworld, 0), dtype=int), - changed_efc_count=wp.empty((nworld,), dtype=int) - if alloc_h - else wp.empty((0,), dtype=int), + Jaref=wp.empty((nworld, njmax), dtype=float), + search_dot=wp.empty((nworld,), dtype=float), + gauss=wp.empty((nworld,), dtype=float), + cost=wp.empty((nworld,), dtype=float), + prev_cost=wp.empty((nworld,), dtype=float), + done=wp.empty((nworld,), dtype=bool), + grad=wp.zeros((nworld, nv_pad), dtype=float), + grad_dot=wp.empty((nworld,), dtype=float), + Mgrad=wp.zeros((nworld, nv_pad), dtype=float), + search=wp.empty((nworld, nv), dtype=float), + mv=wp.empty((nworld, nv), dtype=float), + jv=wp.empty((nworld, njmax), dtype=float), + quad=wp.empty((nworld, njmax), dtype=wp.vec3), + quad_gauss=wp.empty((nworld,), dtype=wp.vec3), + alpha=wp.empty((nworld,), dtype=float), + prev_grad=wp.empty((nworld, nv), dtype=float), + prev_Mgrad=wp.empty((nworld, nv), dtype=float), + beta=wp.empty((nworld,), dtype=float), + h=wp.zeros((nworld, nv_pad, nv_pad), dtype=float) if alloc_h else wp.empty((nworld, 0, 0), dtype=float), + hfactor=wp.zeros((nworld, nv_pad, nv_pad), dtype=float) if alloc_hfactor else wp.empty((nworld, 0, 0), dtype=float), + changed_efc_ids=wp.empty((nworld, njmax), dtype=int) if alloc_h else wp.empty((nworld, 0), dtype=int), + changed_efc_count=wp.empty((nworld,), dtype=int) if alloc_h else wp.empty((0,), dtype=int), ) @@ -892,18 +884,19 @@ def _compute_efc_eval_pt_3alphas_elliptic( @cache_kernel -def linesearch_iterative(ls_iterations: int, cone_type: types.ConeType, fuse_jv: bool): +def linesearch_iterative(ls_iterations: int, cone_type: types.ConeType, fuse_jv: bool, is_sparse: bool): """Factory for iterative linesearch kernel. Args: - block_dim: Number of threads per block for tile reductions. ls_iterations: Max linesearch iterations (compile-time constant for loop optimization). cone_type: Friction cone type (PYRAMIDAL or ELLIPTIC) for compile-time optimization. fuse_jv: Whether to compute jv = J @ search in-kernel (efficient for small nv). + is_sparse: Use sparse matrix representation for constraint Jacobian. """ LS_ITERATIONS = ls_iterations IS_ELLIPTIC = cone_type == types.ConeType.ELLIPTIC FUSE_JV = fuse_jv + IS_SPARSE = is_sparse # Native snippet for CUDA __syncthreads() @wp.func_native(snippet="WP_TILE_SYNC();") @@ -922,46 +915,46 @@ def linesearch_iterative(ls_iterations: int, cone_type: types.ConeType, fuse_jv: @wp.kernel(module="unique", enable_backward=False) def kernel( - # Model: - nv: int, - opt_tolerance: wp.array(dtype=float), - opt_ls_tolerance: wp.array(dtype=float), - opt_impratio_invsqrt: wp.array(dtype=float), - stat_meaninertia: wp.array(dtype=float), - # Data in: - ne_in: wp.array(dtype=int), - nf_in: wp.array(dtype=int), - nefc_in: wp.array(dtype=int), - qfrc_smooth_in: wp.array2d(dtype=float), - contact_friction_in: wp.array(dtype=types.vec5), - contact_dim_in: wp.array(dtype=int), - contact_efc_address_in: wp.array2d(dtype=int), - efc_type_in: wp.array2d(dtype=int), - efc_id_in: wp.array2d(dtype=int), - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), - efc_J_colind_in: wp.array3d(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_D_in: wp.array2d(dtype=float), - efc_frictionloss_in: wp.array2d(dtype=float), - njmax_in: int, - nacon_in: wp.array(dtype=int), - # In: - ctx_Jaref_in: wp.array2d(dtype=float), - ctx_search_in: wp.array2d(dtype=float), - ctx_search_dot_in: wp.array(dtype=float), - ctx_gauss_in: wp.array(dtype=float), - ctx_mv_in: wp.array2d(dtype=float), - ctx_jv_in: wp.array2d(dtype=float), - ctx_quad_in: wp.array2d(dtype=wp.vec3), - ctx_done_in: wp.array(dtype=bool), - # Data out: - qacc_out: wp.array2d(dtype=float), - efc_Ma_out: wp.array2d(dtype=float), - # Out: - ctx_Jaref_out: wp.array2d(dtype=float), - ctx_jv_out: wp.array2d(dtype=float), - ctx_quad_out: wp.array2d(dtype=wp.vec3), + # Model: + nv: int, + opt_tolerance: wp.array(dtype=float), + opt_ls_tolerance: wp.array(dtype=float), + opt_impratio_invsqrt: wp.array(dtype=float), + stat_meaninertia: wp.array(dtype=float), + # Data in: + ne_in: wp.array(dtype=int), + nf_in: wp.array(dtype=int), + nefc_in: wp.array(dtype=int), + qfrc_smooth_in: wp.array2d(dtype=float), + contact_friction_in: wp.array(dtype=types.vec5), + contact_dim_in: wp.array(dtype=int), + contact_efc_address_in: wp.array2d(dtype=int), + efc_type_in: wp.array2d(dtype=int), + efc_id_in: wp.array2d(dtype=int), + efc_J_rownnz_in: wp.array2d(dtype=int), + efc_J_rowadr_in: wp.array2d(dtype=int), + efc_J_colind_in: wp.array3d(dtype=int), + efc_J_in: wp.array3d(dtype=float), + efc_D_in: wp.array2d(dtype=float), + efc_frictionloss_in: wp.array2d(dtype=float), + njmax_in: int, + nacon_in: wp.array(dtype=int), + # In: + ctx_Jaref_in: wp.array2d(dtype=float), + ctx_search_in: wp.array2d(dtype=float), + ctx_search_dot_in: wp.array(dtype=float), + ctx_gauss_in: wp.array(dtype=float), + ctx_mv_in: wp.array2d(dtype=float), + ctx_jv_in: wp.array2d(dtype=float), + ctx_quad_in: wp.array2d(dtype=wp.vec3), + ctx_done_in: wp.array(dtype=bool), + # Data out: + qacc_out: wp.array2d(dtype=float), + efc_Ma_out: wp.array2d(dtype=float), + # Out: + ctx_Jaref_out: wp.array2d(dtype=float), + ctx_jv_out: wp.array2d(dtype=float), + ctx_quad_out: wp.array2d(dtype=wp.vec3), ): worldid, tid = wp.tid() @@ -976,15 +969,13 @@ def linesearch_iterative(ls_iterations: int, cone_type: types.ConeType, fuse_jv: if wp.static(FUSE_JV): for efcid in range(tid, nefc, wp.block_dim()): jv = float(0.0) - if wp.static(SPARSE_CONSTRAINT_JACOBIAN): + if wp.static(IS_SPARSE): rownnz = efc_J_rownnz_in[worldid, efcid] rowadr = efc_J_rowadr_in[worldid, efcid] for k in range(rownnz): sparseid = rowadr + k colind = efc_J_colind_in[worldid, 0, sparseid] - jv += ( - efc_J_in[worldid, 0, sparseid] * ctx_search_in[worldid, colind] - ) + jv += efc_J_in[worldid, 0, sparseid] * ctx_search_in[worldid, colind] else: for i in range(nv): jv += efc_J_in[worldid, efcid, i] * ctx_search_in[worldid, i] @@ -1360,42 +1351,42 @@ def _linesearch_iterative(m: types.Model, d: types.Data, ctx: SolverContext, fus fuse_jv: Whether jv is computed in-kernel (True) or pre-computed (False). """ wp.launch_tiled( - linesearch_iterative(m.opt.ls_iterations, m.opt.cone, fuse_jv), - dim=d.nworld, - inputs=[ - m.nv, - m.opt.tolerance, - m.opt.ls_tolerance, - m.opt.impratio_invsqrt, - m.stat.meaninertia, - d.ne, - d.nf, - d.nefc, - d.qfrc_smooth, - d.contact.friction, - d.contact.dim, - d.contact.efc_address, - d.efc.type, - d.efc.id, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.D, - d.efc.frictionloss, - d.njmax, - d.nacon, - ctx.Jaref, - ctx.search, - ctx.search_dot, - ctx.gauss, - ctx.mv, - ctx.jv, - ctx.quad, - ctx.done, - ], - outputs=[d.qacc, d.efc.Ma, ctx.Jaref, ctx.jv, ctx.quad], - block_dim=m.block_dim.linesearch_iterative, + linesearch_iterative(m.opt.ls_iterations, m.opt.cone, fuse_jv, m.is_sparse), + dim=d.nworld, + inputs=[ + m.nv, + m.opt.tolerance, + m.opt.ls_tolerance, + m.opt.impratio_invsqrt, + m.stat.meaninertia, + d.ne, + d.nf, + d.nefc, + d.qfrc_smooth, + d.contact.friction, + d.contact.dim, + d.contact.efc_address, + d.efc.type, + d.efc.id, + d.efc.J_rownnz, + d.efc.J_rowadr, + d.efc.J_colind, + d.efc.J, + d.efc.D, + d.efc.frictionloss, + d.njmax, + d.nacon, + ctx.Jaref, + ctx.search, + ctx.search_dot, + ctx.gauss, + ctx.mv, + ctx.jv, + ctx.quad, + ctx.done, + ], + outputs=[d.qacc, d.efc.Ma, ctx.Jaref, ctx.jv, ctx.quad], + block_dim=m.block_dim.linesearch_iterative, ) @@ -1420,20 +1411,20 @@ def linesearch_zero_jv( @cache_kernel -def linesearch_jv_fused(opt_is_sparse: bool, nv: int, dofs_per_thread: int): +def linesearch_jv_fused(is_sparse: bool, nv: int, dofs_per_thread: int): @wp.kernel(module="unique", enable_backward=False) def kernel( - # Data in: - nefc_in: wp.array(dtype=int), - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), - efc_J_colind_in: wp.array3d(dtype=int), - efc_J_in: wp.array3d(dtype=float), - # In: - ctx_search_in: wp.array2d(dtype=float), - ctx_done_in: wp.array(dtype=bool), - # Out: - ctx_jv_out: wp.array2d(dtype=float), + # Data in: + nefc_in: wp.array(dtype=int), + efc_J_rownnz_in: wp.array2d(dtype=int), + efc_J_rowadr_in: wp.array2d(dtype=int), + efc_J_colind_in: wp.array3d(dtype=int), + efc_J_in: wp.array3d(dtype=float), + # In: + ctx_search_in: wp.array2d(dtype=float), + ctx_done_in: wp.array(dtype=bool), + # Out: + ctx_jv_out: wp.array2d(dtype=float), ): worldid, efcid, dofstart = wp.tid() @@ -1446,23 +1437,21 @@ def linesearch_jv_fused(opt_is_sparse: bool, nv: int, dofs_per_thread: int): jv_out = float(0.0) if wp.static(dofs_per_thread >= nv): - if wp.static(SPARSE_CONSTRAINT_JACOBIAN): + if wp.static(is_sparse): # Sparse: iterate over non-zero entries in the row rownnz = efc_J_rownnz_in[worldid, efcid] rowadr = efc_J_rowadr_in[worldid, efcid] for k in range(rownnz): sparseid = rowadr + k colind = efc_J_colind_in[worldid, 0, sparseid] - jv_out += ( - efc_J_in[worldid, 0, sparseid] * ctx_search_in[worldid, colind] - ) + jv_out += efc_J_in[worldid, 0, sparseid] * ctx_search_in[worldid, colind] else: for i in range(wp.static(min(dofs_per_thread, nv))): jv_out += efc_J_in[worldid, efcid, i] * ctx_search_in[worldid, i] ctx_jv_out[worldid, efcid] = jv_out else: - if wp.static(SPARSE_CONSTRAINT_JACOBIAN): + if wp.static(is_sparse): # Sparse: thread 0 handles entire row (sparse entries << nv typically) if dofstart == 0: rownnz = efc_J_rownnz_in[worldid, efcid] @@ -1470,9 +1459,7 @@ def linesearch_jv_fused(opt_is_sparse: bool, nv: int, dofs_per_thread: int): for k in range(rownnz): sparseid = rowadr + k colind = efc_J_colind_in[worldid, 0, sparseid] - jv_out += ( - efc_J_in[worldid, 0, sparseid] * ctx_search_in[worldid, colind] - ) + jv_out += efc_J_in[worldid, 0, sparseid] * ctx_search_in[worldid, colind] ctx_jv_out[worldid, efcid] = jv_out else: for i in range(wp.static(dofs_per_thread)): @@ -1583,10 +1570,7 @@ def linesearch_prepare_quad( dim = contact_dim_in[conid] friction = contact_friction_in[conid] - mu = ( - friction[0] - * opt_impratio_invsqrt[worldid % opt_impratio_invsqrt.shape[0]] - ) + mu = friction[0] * opt_impratio_invsqrt[worldid % opt_impratio_invsqrt.shape[0]] u0 = Jaref * mu v0 = jv * mu @@ -1707,18 +1691,10 @@ def _linesearch(m: types.Model, d: types.Data, ctx: SolverContext, cost: wp.arra ) wp.launch( - linesearch_jv_fused(m.is_sparse, m.nv, dofs_per_thread), - dim=(d.nworld, d.njmax, threads_per_efc), - inputs=[ - d.nefc, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - ctx.search, - ctx.done, - ], - outputs=[ctx.jv], + linesearch_jv_fused(m.is_sparse, m.nv, dofs_per_thread), + dim=(d.nworld, d.njmax, threads_per_efc), + inputs=[d.nefc, d.efc.J_rownnz, d.efc.J_rowadr, d.efc.J_colind, d.efc.J, ctx.search, ctx.done], + outputs=[ctx.jv], ) if m.opt.ls_parallel: @@ -1744,19 +1720,19 @@ def solve_init_efc( @cache_kernel -def solve_init_jaref(opt_is_sparse: bool, nv: int, dofs_per_thread: int): +def solve_init_jaref(is_sparse: bool, nv: int, dofs_per_thread: int): @wp.kernel(module="unique", enable_backward=False) def kernel( - # Data in: - nefc_in: wp.array(dtype=int), - qacc_in: wp.array2d(dtype=float), - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), - efc_J_colind_in: wp.array3d(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_aref_in: wp.array2d(dtype=float), - # Out: - ctx_Jaref_out: wp.array2d(dtype=float), + # Data in: + nefc_in: wp.array(dtype=int), + qacc_in: wp.array2d(dtype=float), + efc_J_rownnz_in: wp.array2d(dtype=int), + efc_J_rowadr_in: wp.array2d(dtype=int), + efc_J_colind_in: wp.array3d(dtype=int), + efc_J_in: wp.array3d(dtype=float), + efc_aref_in: wp.array2d(dtype=float), + # Out: + ctx_Jaref_out: wp.array2d(dtype=float), ): worldid, efcid, dofstart = wp.tid() @@ -1764,7 +1740,7 @@ def solve_init_jaref(opt_is_sparse: bool, nv: int, dofs_per_thread: int): return jaref = float(0.0) - if wp.static(SPARSE_CONSTRAINT_JACOBIAN): + if wp.static(is_sparse): rownnz = efc_J_rownnz_in[worldid, efcid] rowadr = efc_J_rowadr_in[worldid, efcid] for i in range(rownnz): @@ -1785,9 +1761,7 @@ def solve_init_jaref(opt_is_sparse: bool, nv: int, dofs_per_thread: int): jaref += efc_J_in[worldid, efcid, ii] * qacc_in[worldid, ii] if dofstart == 0: - wp.atomic_add( - ctx_Jaref_out, worldid, efcid, jaref - efc_aref_in[worldid, efcid] - ) + wp.atomic_add(ctx_Jaref_out, worldid, efcid, jaref - efc_aref_in[worldid, efcid]) else: wp.atomic_add(ctx_Jaref_out, worldid, efcid, jaref) @@ -1834,30 +1808,30 @@ def update_constraint_efc(track_changes: bool): @wp.kernel(module="unique", enable_backward=False) def kernel( - # Model: - opt_impratio_invsqrt: wp.array(dtype=float), - # Data in: - ne_in: wp.array(dtype=int), - nf_in: wp.array(dtype=int), - nefc_in: wp.array(dtype=int), - contact_friction_in: wp.array(dtype=types.vec5), - contact_dim_in: wp.array(dtype=int), - contact_efc_address_in: wp.array2d(dtype=int), - efc_type_in: wp.array2d(dtype=int), - efc_id_in: wp.array2d(dtype=int), - efc_D_in: wp.array2d(dtype=float), - efc_frictionloss_in: wp.array2d(dtype=float), - nacon_in: wp.array(dtype=int), - # In: - ctx_Jaref_in: wp.array2d(dtype=float), - ctx_done_in: wp.array(dtype=bool), - # Data out: - efc_force_out: wp.array2d(dtype=float), - efc_state_out: wp.array2d(dtype=int), - # Out: - ctx_cost_out: wp.array(dtype=float), - changed_ids_out: wp.array2d(dtype=int), - changed_count_out: wp.array(dtype=int), + # Model: + opt_impratio_invsqrt: wp.array(dtype=float), + # Data in: + ne_in: wp.array(dtype=int), + nf_in: wp.array(dtype=int), + nefc_in: wp.array(dtype=int), + contact_friction_in: wp.array(dtype=types.vec5), + contact_dim_in: wp.array(dtype=int), + contact_efc_address_in: wp.array2d(dtype=int), + efc_type_in: wp.array2d(dtype=int), + efc_id_in: wp.array2d(dtype=int), + efc_D_in: wp.array2d(dtype=float), + efc_frictionloss_in: wp.array2d(dtype=float), + nacon_in: wp.array(dtype=int), + # In: + ctx_Jaref_in: wp.array2d(dtype=float), + ctx_done_in: wp.array(dtype=bool), + # Data out: + efc_force_out: wp.array2d(dtype=float), + efc_state_out: wp.array2d(dtype=int), + # Out: + ctx_cost_out: wp.array(dtype=float), + changed_ids_out: wp.array2d(dtype=int), + changed_count_out: wp.array(dtype=int), ): worldid, efcid = wp.tid() @@ -1869,9 +1843,7 @@ def update_constraint_efc(track_changes: bool): # Read old QUADRATIC status before overwriting if wp.static(TRACK_CHANGES): - old_quad = ( - efc_state_out[worldid, efcid] == types.ConstraintState.QUADRATIC.value - ) + old_quad = efc_state_out[worldid, efcid] == types.ConstraintState.QUADRATIC.value efc_D = efc_D_in[worldid, efcid] Jaref = ctx_Jaref_in[worldid, efcid] @@ -1919,10 +1891,7 @@ def update_constraint_efc(track_changes: bool): dim = contact_dim_in[conid] friction = contact_friction_in[conid] - mu = ( - friction[0] - * opt_impratio_invsqrt[worldid % opt_impratio_invsqrt.shape[0]] - ) + mu = friction[0] * opt_impratio_invsqrt[worldid % opt_impratio_invsqrt.shape[0]] efcid0 = contact_efc_address_in[conid, 0] if efcid0 < 0: @@ -1984,17 +1953,17 @@ def update_constraint_efc(track_changes: bool): @wp.kernel def update_constraint_init_qfrc_constraint_sparse( - # Data in: - nefc_in: wp.array(dtype=int), - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), - efc_J_colind_in: wp.array3d(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_force_in: wp.array2d(dtype=float), - # In: - ctx_done_in: wp.array(dtype=bool), - # Data out: - qfrc_constraint_out: wp.array2d(dtype=float), + # Data in: + nefc_in: wp.array(dtype=int), + efc_J_rownnz_in: wp.array2d(dtype=int), + efc_J_rowadr_in: wp.array2d(dtype=int), + efc_J_colind_in: wp.array3d(dtype=int), + efc_J_in: wp.array3d(dtype=float), + efc_force_in: wp.array2d(dtype=float), + # In: + ctx_done_in: wp.array(dtype=bool), + # Data out: + qfrc_constraint_out: wp.array2d(dtype=float), ): worldid, efcid = wp.tid() @@ -2017,15 +1986,15 @@ def update_constraint_init_qfrc_constraint_sparse( @wp.kernel def update_constraint_init_qfrc_constraint_dense( - # Data in: - nefc_in: wp.array(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_force_in: wp.array2d(dtype=float), - njmax_in: int, - # In: - ctx_done_in: wp.array(dtype=bool), - # Data out: - qfrc_constraint_out: wp.array2d(dtype=float), + # Data in: + nefc_in: wp.array(dtype=int), + efc_J_in: wp.array3d(dtype=float), + efc_force_in: wp.array2d(dtype=float), + njmax_in: int, + # In: + ctx_done_in: wp.array(dtype=bool), + # Data out: + qfrc_constraint_out: wp.array2d(dtype=float), ): worldid, dofid = wp.tid() @@ -2076,23 +2045,23 @@ def update_constraint_gauss_cost(nv: int, dofs_per_thread: int): gauss_cost += (efc_Ma_in[worldid, ii] - qfrc_smooth_in[worldid, ii]) * ( qacc_in[worldid, ii] - qacc_smooth_in[worldid, ii] ) - wp.atomic_add(ctx_gauss_out, worldid, gauss_cost) - wp.atomic_add(ctx_cost_out, worldid, gauss_cost) + wp.atomic_add(ctx_gauss_out, worldid, 0.5 * gauss_cost) + wp.atomic_add(ctx_cost_out, worldid, 0.5 * gauss_cost) return kernel @wp.kernel def update_gradient_h_incremental( - # Data in: - efc_J_in: wp.array3d(dtype=float), - efc_D_in: wp.array2d(dtype=float), - efc_state_in: wp.array2d(dtype=int), - # In: - changed_ids_in: wp.array2d(dtype=int), - changed_count_in: wp.array(dtype=int), - # Out: - ctx_h_out: wp.array3d(dtype=float), + # Data in: + efc_J_in: wp.array3d(dtype=float), + efc_D_in: wp.array2d(dtype=float), + efc_state_in: wp.array2d(dtype=int), + # In: + changed_ids_in: wp.array2d(dtype=int), + changed_count_in: wp.array(dtype=int), + # Out: + ctx_h_out: wp.array3d(dtype=float), ): """Incrementally update lower triangle of H for changed constraints. @@ -2131,18 +2100,18 @@ def update_gradient_h_incremental( @wp.kernel def update_gradient_h_incremental_sparse( - # Data in: - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), - efc_J_colind_in: wp.array3d(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_D_in: wp.array2d(dtype=float), - efc_state_in: wp.array2d(dtype=int), - # In: - changed_ids_in: wp.array2d(dtype=int), - changed_count_in: wp.array(dtype=int), - # Out: - ctx_h_out: wp.array3d(dtype=float), + # Data in: + efc_J_rownnz_in: wp.array2d(dtype=int), + efc_J_rowadr_in: wp.array2d(dtype=int), + efc_J_colind_in: wp.array3d(dtype=int), + efc_J_in: wp.array3d(dtype=float), + efc_D_in: wp.array2d(dtype=float), + efc_state_in: wp.array2d(dtype=int), + # In: + changed_ids_in: wp.array2d(dtype=int), + changed_count_in: wp.array(dtype=int), + # Out: + ctx_h_out: wp.array3d(dtype=float), ): """Incrementally update lower triangle of H for changed constraints (sparse J).""" worldid, change_idx = wp.tid() @@ -2182,12 +2151,7 @@ def update_gradient_h_incremental_sparse( wp.atomic_add(ctx_h_out[worldid, colindj], colindi, h) -def _update_constraint( - m: types.Model, - d: types.Data, - ctx: SolverContext | InverseContext, - track_changes: bool = False, -): +def _update_constraint(m: types.Model, d: types.Data, ctx: SolverContext | InverseContext, track_changes: bool = False): """Update constraint arrays after each solve iteration.""" wp.launch( update_constraint_init_cost, @@ -2197,58 +2161,44 @@ def _update_constraint( ) efc_inputs = [ - m.opt.impratio_invsqrt, - d.ne, - d.nf, - d.nefc, - d.contact.friction, - d.contact.dim, - d.contact.efc_address, - d.efc.type, - d.efc.id, - d.efc.D, - d.efc.frictionloss, - d.nacon, - ctx.Jaref, - ctx.done, + m.opt.impratio_invsqrt, + d.ne, + d.nf, + d.nefc, + d.contact.friction, + d.contact.dim, + d.contact.efc_address, + d.efc.type, + d.efc.id, + d.efc.D, + d.efc.frictionloss, + d.nacon, + ctx.Jaref, + ctx.done, ] wp.launch( - update_constraint_efc(track_changes), - dim=(d.nworld, d.njmax), - inputs=efc_inputs, - outputs=[ - d.efc.force, - d.efc.state, - ctx.cost, - ctx.changed_efc_ids, - ctx.changed_efc_count, - ], + update_constraint_efc(track_changes), + dim=(d.nworld, d.njmax), + inputs=efc_inputs, + outputs=[d.efc.force, d.efc.state, ctx.cost, ctx.changed_efc_ids, ctx.changed_efc_count], ) # qfrc_constraint = efc_J.T @ efc_force - if SPARSE_CONSTRAINT_JACOBIAN: + if m.is_sparse: d.qfrc_constraint.zero_() wp.launch( - update_constraint_init_qfrc_constraint_sparse, - dim=(d.nworld, d.njmax), - inputs=[ - d.nefc, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.force, - ctx.done, - ], - outputs=[d.qfrc_constraint], + update_constraint_init_qfrc_constraint_sparse, + dim=(d.nworld, d.njmax), + inputs=[d.nefc, d.efc.J_rownnz, d.efc.J_rowadr, d.efc.J_colind, d.efc.J, d.efc.force, ctx.done], + outputs=[d.qfrc_constraint], ) else: wp.launch( - update_constraint_init_qfrc_constraint_dense, - dim=(d.nworld, m.nv), - inputs=[d.nefc, d.efc.J, d.efc.force, d.njmax, ctx.done], - outputs=[d.qfrc_constraint], + update_constraint_init_qfrc_constraint_dense, + dim=(d.nworld, m.nv), + inputs=[d.nefc, d.efc.J, d.efc.force, d.njmax, ctx.done], + outputs=[d.qfrc_constraint], ) # if we are only using 1 thread, it makes sense to do more dofs and skip the atomics. @@ -2441,9 +2391,7 @@ def update_gradient_JTDAJ_dense_tiled(nv_pad: int, tile_size: int, njmax: int): nefc = nefc_in[worldid] - sum_val = wp.tile_load( - qM_in[worldid], shape=(nv_pad, nv_pad), bounds_check=True - ) + sum_val = wp.tile_load(qM_in[worldid], shape=(nv_pad, nv_pad), bounds_check=True) # Each tile processes one output tile by looping over all constraints for k in range(0, njmax, TILE_SIZE_K): @@ -2453,12 +2401,7 @@ def update_gradient_JTDAJ_dense_tiled(nv_pad: int, tile_size: int, njmax: int): # AD: leaving bounds-check disabled here because I'm not entirely sure that # everything always hits the fast path. The padding takes care of any # potential OOB accesses. - J_kj = wp.tile_load( - efc_J_in[worldid], - shape=(TILE_SIZE_K, nv_pad), - offset=(k, 0), - bounds_check=False, - ) + J_kj = wp.tile_load(efc_J_in[worldid], shape=(TILE_SIZE_K, nv_pad), offset=(k, 0), bounds_check=False) # state check D_k = wp.tile_load(efc_D_in[worldid], shape=TILE_SIZE_K, offset=k, bounds_check=False) @@ -2473,11 +2416,7 @@ def update_gradient_JTDAJ_dense_tiled(nv_pad: int, tile_size: int, njmax: int): active_tile = wp.tile_map(active_check, tid_tile, threshold_tile) D_k = wp.tile_map(wp.mul, active_tile, D_k) - J_ki = wp.tile_map( - wp.mul, - wp.tile_transpose(J_kj), - wp.tile_broadcast(D_k, shape=(nv_pad, TILE_SIZE_K)), - ) + J_ki = wp.tile_map(wp.mul, wp.tile_transpose(J_kj), wp.tile_broadcast(D_k, shape=(nv_pad, TILE_SIZE_K))) sum_val += wp.tile_matmul(J_ki, J_kj) @@ -2489,32 +2428,32 @@ def update_gradient_JTDAJ_dense_tiled(nv_pad: int, tile_size: int, njmax: int): # TODO(thowell): combine with JTDAJ ? @wp.kernel def update_gradient_JTCJ_sparse( - # Model: - opt_impratio_invsqrt: wp.array(dtype=float), - dof_tri_row: wp.array(dtype=int), - dof_tri_col: wp.array(dtype=int), - # Data in: - contact_dist_in: wp.array(dtype=float), - contact_includemargin_in: wp.array(dtype=float), - contact_friction_in: wp.array(dtype=types.vec5), - contact_dim_in: wp.array(dtype=int), - contact_efc_address_in: wp.array2d(dtype=int), - contact_worldid_in: wp.array(dtype=int), - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), - efc_J_colind_in: wp.array3d(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_D_in: wp.array2d(dtype=float), - efc_state_in: wp.array2d(dtype=int), - naconmax_in: int, - nacon_in: wp.array(dtype=int), - # In: - ctx_Jaref_in: wp.array2d(dtype=float), - ctx_done_in: wp.array(dtype=bool), - nblocks_perblock: int, - dim_block: int, - # Out: - h_out: wp.array3d(dtype=float), + # Model: + opt_impratio_invsqrt: wp.array(dtype=float), + dof_tri_row: wp.array(dtype=int), + dof_tri_col: wp.array(dtype=int), + # Data in: + contact_dist_in: wp.array(dtype=float), + contact_includemargin_in: wp.array(dtype=float), + contact_friction_in: wp.array(dtype=types.vec5), + contact_dim_in: wp.array(dtype=int), + contact_efc_address_in: wp.array2d(dtype=int), + contact_worldid_in: wp.array(dtype=int), + efc_J_rownnz_in: wp.array2d(dtype=int), + efc_J_rowadr_in: wp.array2d(dtype=int), + efc_J_colind_in: wp.array3d(dtype=int), + efc_J_in: wp.array3d(dtype=float), + efc_D_in: wp.array2d(dtype=float), + efc_state_in: wp.array2d(dtype=int), + naconmax_in: int, + nacon_in: wp.array(dtype=int), + # In: + ctx_Jaref_in: wp.array2d(dtype=float), + ctx_done_in: wp.array(dtype=bool), + nblocks_perblock: int, + dim_block: int, + # Out: + ctx_h_out: wp.array3d(dtype=float), ): conid_start, elementid = wp.tid() @@ -2529,20 +2468,37 @@ def update_gradient_JTCJ_sparse( worldid = contact_worldid_in[conid] if ctx_done_in[worldid]: - return + continue condim = contact_dim_in[conid] if condim == 1: - return + continue # check contact status if contact_dist_in[conid] - contact_includemargin_in[conid] >= 0.0: - return + continue efcid0 = contact_efc_address_in[conid, 0] if efc_state_in[worldid, efcid0] != types.ConstraintState.CONE: - return + continue + + # All dims share the same sparsity pattern. Scan colind once to find + # the sparse positions of dof1id and dof2id. Skip if either is absent. + rownnz = efc_J_rownnz_in[worldid, efcid0] + rowadr0 = efc_J_rowadr_in[worldid, efcid0] + pos1 = int(-1) + pos2 = int(-1) + for k in range(rownnz): + col = efc_J_colind_in[worldid, 0, rowadr0 + k] + if col == dof1id: + pos1 = k + if col == dof2id: + pos2 = k + if pos1 >= 0 and pos2 >= 0: + break + if pos1 < 0 or pos2 < 0: + continue fri = contact_friction_in[conid] mu = fri[0] * opt_impratio_invsqrt[worldid % opt_impratio_invsqrt.shape[0]] @@ -2551,7 +2507,7 @@ def update_gradient_JTCJ_sparse( dm = math.safe_div(efc_D_in[worldid, efcid0], mu2 * (1.0 + mu2)) if dm == 0.0: - return + continue n = ctx_Jaref_in[worldid, efcid0] * mu u = types.vec6(n, 0.0, 0.0, 0.0, 0.0, 0.0) @@ -2570,52 +2526,40 @@ def update_gradient_JTCJ_sparse( t = wp.max(t, types.MJ_MINVAL) ttt = wp.max(t * t * t, types.MJ_MINVAL) + # Precompute common subexpressions. + mu_over_t = math.safe_div(mu, t) + mu_n_over_ttt = mu * math.safe_div(n, ttt) + mu2_minus_mu_n_over_t = mu2 - mu * math.safe_div(n, t) + h = float(0.0) for dim1id in range(condim): if dim1id == 0: - efcid1 = efcid0 + rowadr1 = rowadr0 + dm_fri1 = dm * mu else: efcid1 = contact_efc_address_in[conid, dim1id] + rowadr1 = efc_J_rowadr_in[worldid, efcid1] + dm_fri1 = dm * fri[dim1id - 1] - # TODO(team): improve performance for sparse code path - rownnz1 = efc_J_rownnz_in[worldid, efcid1] - rowadr1 = efc_J_rowadr_in[worldid, efcid1] - - efc_J11 = float(0.0) - efc_J12 = float(0.0) - for i1 in range(rownnz1): - sparseid1 = rowadr1 + i1 - colind1 = efc_J_colind_in[worldid, 0, sparseid1] - if dof1id == colind1: - efc_J11 = efc_J_in[worldid, 0, sparseid1] - if dof2id == colind1: - efc_J12 = efc_J_in[worldid, 0, sparseid1] - if efc_J11 != 0.0 and efc_J12 != 0.0: - break + # Direct J reads using cached sparse positions. + efc_J11 = efc_J_in[worldid, 0, rowadr1 + pos1] + efc_J12 = efc_J_in[worldid, 0, rowadr1 + pos2] ui = u[dim1id] for dim2id in range(0, dim1id + 1): if dim2id == 0: - efcid2 = efcid0 + rowadr2 = rowadr0 + dm_fri12 = dm_fri1 * mu else: efcid2 = contact_efc_address_in[conid, dim2id] + rowadr2 = efc_J_rowadr_in[worldid, efcid2] + dm_fri12 = dm_fri1 * fri[dim2id - 1] - rownnz2 = efc_J_rownnz_in[worldid, efcid2] - rowadr2 = efc_J_rowadr_in[worldid, efcid2] - - efc_J21 = float(0.0) - efc_J22 = float(0.0) - for i2 in range(rownnz2): - sparseid2 = rowadr2 + i2 - colind2 = efc_J_colind_in[worldid, 0, sparseid2] - if dof1id == colind2: - efc_J21 = efc_J_in[worldid, 0, sparseid2] - if dof2id == colind2: - efc_J22 = efc_J_in[worldid, 0, sparseid2] - if efc_J21 != 0.0 and efc_J22 != 0.0: - break + # Direct J reads using cached sparse positions. + efc_J21 = efc_J_in[worldid, 0, rowadr2 + pos1] + efc_J22 = efc_J_in[worldid, 0, rowadr2 + pos2] uj = u[dim2id] @@ -2623,28 +2567,17 @@ def update_gradient_JTCJ_sparse( if dim1id == 0 and dim2id == 0: hcone = 1.0 elif dim1id == 0: - hcone = -math.safe_div(mu, t) * uj + hcone = -mu_over_t * uj elif dim2id == 0: - hcone = -math.safe_div(mu, t) * ui + hcone = -mu_over_t * ui else: - hcone = mu * math.safe_div(n, ttt) * ui * uj + hcone = mu_n_over_ttt * ui * uj # add to diagonal: mu^2 - mu * n / t if dim1id == dim2id: - hcone += mu2 - mu * math.safe_div(n, t) + hcone += mu2_minus_mu_n_over_t - # pre and post multiply by diag(mu, friction) scale by dm - if dim1id == 0: - fri1 = mu - else: - fri1 = fri[dim1id - 1] - - if dim2id == 0: - fri2 = mu - else: - fri2 = fri[dim2id - 1] - - hcone *= dm * fri1 * fri2 + hcone *= dm_fri12 if hcone != 0.0: h += hcone * efc_J11 * efc_J22 @@ -2652,34 +2585,34 @@ def update_gradient_JTCJ_sparse( if dim1id != dim2id: h += hcone * efc_J12 * efc_J21 - h_out[worldid, dof1id, dof2id] += h + ctx_h_out[worldid, dof1id, dof2id] += h @wp.kernel def update_gradient_JTCJ_dense( - # Model: - opt_impratio_invsqrt: wp.array(dtype=float), - dof_tri_row: wp.array(dtype=int), - dof_tri_col: wp.array(dtype=int), - # Data in: - contact_dist_in: wp.array(dtype=float), - contact_includemargin_in: wp.array(dtype=float), - contact_friction_in: wp.array(dtype=types.vec5), - contact_dim_in: wp.array(dtype=int), - contact_efc_address_in: wp.array2d(dtype=int), - contact_worldid_in: wp.array(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_D_in: wp.array2d(dtype=float), - efc_state_in: wp.array2d(dtype=int), - naconmax_in: int, - nacon_in: wp.array(dtype=int), - # In: - ctx_Jaref_in: wp.array2d(dtype=float), - ctx_done_in: wp.array(dtype=bool), - nblocks_perblock: int, - dim_block: int, - # Out: - ctx_h_out: wp.array3d(dtype=float), + # Model: + opt_impratio_invsqrt: wp.array(dtype=float), + dof_tri_row: wp.array(dtype=int), + dof_tri_col: wp.array(dtype=int), + # Data in: + contact_dist_in: wp.array(dtype=float), + contact_includemargin_in: wp.array(dtype=float), + contact_friction_in: wp.array(dtype=types.vec5), + contact_dim_in: wp.array(dtype=int), + contact_efc_address_in: wp.array2d(dtype=int), + contact_worldid_in: wp.array(dtype=int), + efc_J_in: wp.array3d(dtype=float), + efc_D_in: wp.array2d(dtype=float), + efc_state_in: wp.array2d(dtype=int), + naconmax_in: int, + nacon_in: wp.array(dtype=int), + # In: + ctx_Jaref_in: wp.array2d(dtype=float), + ctx_done_in: wp.array(dtype=bool), + nblocks_perblock: int, + dim_block: int, + # Out: + ctx_h_out: wp.array3d(dtype=float), ): conid_start, elementid = wp.tid() @@ -2863,40 +2796,86 @@ def padding_h(nv: int, ctx_done_in: wp.array(dtype=bool), ctx_h_out: wp.array3d( ctx_h_out[worldid, dofid, dofid] = 1.0 -def _cholesky_factorize_solve( - m: types.Model, d: types.Data, ctx: SolverContext -): +def _cholesky_factorize_solve(m: types.Model, d: types.Data, ctx: SolverContext): """Cholesky factorize ctx.h and solve for Mgrad.""" if m.nv <= _BLOCK_CHOLESKY_DIM: wp.launch_tiled( - update_gradient_cholesky(m.nv), - dim=d.nworld, - inputs=[ctx.grad, ctx.h, ctx.done], - outputs=[ctx.Mgrad], - block_dim=m.block_dim.update_gradient_cholesky, + update_gradient_cholesky(m.nv), + dim=d.nworld, + inputs=[ctx.grad, ctx.h, ctx.done], + outputs=[ctx.Mgrad], + block_dim=m.block_dim.update_gradient_cholesky, ) else: wp.launch( - padding_h, - dim=(d.nworld, m.nv_pad - m.nv), - inputs=[m.nv, ctx.done], - outputs=[ctx.h], + padding_h, + dim=(d.nworld, m.nv_pad - m.nv), + inputs=[m.nv, ctx.done], + outputs=[ctx.h], ) wp.launch_tiled( - update_gradient_cholesky_blocked(types.TILE_SIZE_JTDAJ_DENSE, m.nv_pad), - dim=d.nworld, - inputs=[ - ctx.done, - ctx.grad.reshape(shape=(d.nworld, ctx.grad.shape[1], 1)), - ctx.h, - ctx.hfactor, - ], - outputs=[ctx.Mgrad.reshape(shape=(d.nworld, ctx.Mgrad.shape[1], 1))], - block_dim=m.block_dim.update_gradient_cholesky_blocked, + update_gradient_cholesky_blocked(types.TILE_SIZE_JTDAJ_DENSE, m.nv_pad), + dim=d.nworld, + inputs=[ctx.done, ctx.grad.reshape(shape=(d.nworld, ctx.grad.shape[1], 1)), ctx.h, ctx.hfactor], + outputs=[ctx.Mgrad.reshape(shape=(d.nworld, ctx.Mgrad.shape[1], 1))], + block_dim=m.block_dim.update_gradient_cholesky_blocked, ) +@wp.kernel +def _JTDAJ_sparse( + # Data in: + nefc_in: wp.array(dtype=int), + efc_J_rownnz_in: wp.array2d(dtype=int), + efc_J_rowadr_in: wp.array2d(dtype=int), + efc_J_colind_in: wp.array3d(dtype=int), + efc_J_in: wp.array3d(dtype=float), + efc_D_in: wp.array2d(dtype=float), + efc_state_in: wp.array2d(dtype=int), + # In: + ctx_done_in: wp.array(dtype=bool), + # Out: + h_out: wp.array3d(dtype=float), +): + worldid, efcid = wp.tid() + + if ctx_done_in[worldid]: + return + + if efcid >= nefc_in[worldid]: + return + + efc_D = efc_D_in[worldid, efcid] + efc_state = efc_state_in[worldid, efcid] + + if state_check(efc_D, efc_state) == 0.0: + return + + rownnz = efc_J_rownnz_in[worldid, efcid] + rowadr = efc_J_rowadr_in[worldid, efcid] + + for i in range(rownnz): + sparseidi = rowadr + i + Ji = efc_J_in[worldid, 0, sparseidi] + colindi = efc_J_colind_in[worldid, 0, sparseidi] + for j in range(i, rownnz): + if j == i: + sparseidj = sparseidi + Jj = Ji + colindj = colindi + else: + sparseidj = rowadr + j + Jj = efc_J_in[worldid, 0, sparseidj] + colindj = efc_J_colind_in[worldid, 0, sparseidj] + + h = Ji * Jj * efc_D + wp.atomic_add(h_out[worldid, colindi], colindj, h) + + if i != j: + wp.atomic_add(h_out[worldid, colindj], colindi, h) + + def _update_gradient(m: types.Model, d: types.Data, ctx: SolverContext): # grad = Ma - qfrc_smooth - qfrc_constraint wp.launch(update_gradient_zero_grad_dot, dim=(d.nworld), inputs=[ctx.done], outputs=[ctx.grad_dot]) @@ -2912,126 +2891,15 @@ def _update_gradient(m: types.Model, d: types.Data, ctx: SolverContext): smooth.solve_m(m, d, ctx.Mgrad, ctx.grad) elif m.opt.solver == types.SolverType.NEWTON: # h = qM + (efc_J.T * efc_D * active) @ efc_J - if SPARSE_CONSTRAINT_JACOBIAN: - # TODO(team): improve performance for sparse code path - @wp.kernel(module="unique", enable_backward=False) - def _JTDAJ_sparse( - # Data in: - nefc_in: wp.array(dtype=int), - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), - efc_J_colind_in: wp.array3d(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_D_in: wp.array2d(dtype=float), - efc_state_in: wp.array2d(dtype=int), - # In: - ctx_done_in: wp.array(dtype=bool), - # Out: - h_out: wp.array3d(dtype=float), - ): - worldid, efcid = wp.tid() - - if ctx_done_in[worldid]: - return - - if efcid >= nefc_in[worldid]: - return - - efc_D = efc_D_in[worldid, efcid] - efc_state = efc_state_in[worldid, efcid] - - if state_check(efc_D, efc_state) == 0.0: - return - - rownnz = efc_J_rownnz_in[worldid, efcid] - rowadr = efc_J_rowadr_in[worldid, efcid] - - for i in range(rownnz): - sparseidi = rowadr + i - Ji = efc_J_in[worldid, 0, sparseidi] - colindi = efc_J_colind_in[worldid, 0, sparseidi] - for j in range(i, rownnz): - if j == i: - sparseidj = sparseidi - Jj = Ji - colindj = colindi - else: - sparseidj = rowadr + j - Jj = efc_J_in[worldid, 0, sparseidj] - colindj = efc_J_colind_in[worldid, 0, sparseidj] - - h = Ji * Jj * efc_D - wp.atomic_add(h_out[worldid, colindi], colindj, h) - - if i != j: - wp.atomic_add(h_out[worldid, colindj], colindi, h) - + if m.is_sparse: + ctx.h.zero_() wp.launch( - _JTDAJ_sparse, - dim=(d.nworld, d.njmax), - inputs=[ - d.nefc, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.D, - d.efc.state, - ctx.done, - ], - outputs=[ctx.h], + _JTDAJ_sparse, + dim=(d.nworld, d.njmax), + inputs=[d.nefc, d.efc.J_rownnz, d.efc.J_rowadr, d.efc.J_colind, d.efc.J, d.efc.D, d.efc.state, ctx.done], + outputs=[ctx.h], ) - if m.is_sparse: - wp.launch( - update_gradient_set_h_qM_lower_sparse, - dim=(d.nworld, m.qM_fullm_i.size), - inputs=[m.qM_fullm_i, m.qM_fullm_j, d.qM, ctx.done], - outputs=[ctx.h], - ) - else: - # dense M: copy qM directly into h - @wp.kernel(module="unique", enable_backward=False) - def _set_h_qM_dense( - nv: int, - qM_in: wp.array3d(dtype=float), - ctx_done_in: wp.array(dtype=bool), - ctx_h_out: wp.array3d(dtype=float), - ): - worldid, i, j = wp.tid() - if ctx_done_in[worldid]: - return - if i >= nv or j >= nv: - return - if i >= j: - ctx_h_out[worldid, i, j] += qM_in[worldid, i, j] - - wp.launch( - _set_h_qM_dense, - dim=(d.nworld, m.nv, m.nv), - inputs=[m.nv, d.qM, ctx.done], - outputs=[ctx.h], - ) - elif m.is_sparse: - num_blocks_ceil = ceil(m.nv / types.TILE_SIZE_JTDAJ_SPARSE) - lower_triangle_dim = int(num_blocks_ceil * (num_blocks_ceil + 1) / 2) - with scoped_mathdx_gemm_disabled(): - wp.launch_tiled( - update_gradient_JTDAJ_sparse_tiled( - types.TILE_SIZE_JTDAJ_SPARSE, d.njmax - ), - dim=(d.nworld, lower_triangle_dim), - inputs=[ - d.nefc, - d.efc.J, - d.efc.D, - d.efc.state, - ctx.done, - ], - outputs=[ctx.h], - block_dim=m.block_dim.update_gradient_JTDAJ_sparse, - ) - wp.launch( update_gradient_set_h_qM_lower_sparse, dim=(d.nworld, m.qM_fullm_i.size), @@ -3041,20 +2909,18 @@ def _update_gradient(m: types.Model, d: types.Data, ctx: SolverContext): else: with scoped_mathdx_gemm_disabled(): wp.launch_tiled( - update_gradient_JTDAJ_dense_tiled( - m.nv_pad, types.TILE_SIZE_JTDAJ_DENSE, d.njmax - ), - dim=d.nworld, - inputs=[ - d.nefc, - d.qM, - d.efc.J, - d.efc.D, - d.efc.state, - ctx.done, - ], - outputs=[ctx.h], - block_dim=m.block_dim.update_gradient_JTDAJ_dense, + update_gradient_JTDAJ_dense_tiled(m.nv_pad, types.TILE_SIZE_JTDAJ_DENSE, d.njmax), + dim=d.nworld, + inputs=[ + d.nefc, + d.qM, + d.efc.J, + d.efc.D, + d.efc.state, + ctx.done, + ], + outputs=[ctx.h], + block_dim=m.block_dim.update_gradient_JTDAJ_dense, ) if m.opt.cone == types.ConeType.ELLIPTIC: @@ -3081,60 +2947,60 @@ def _update_gradient(m: types.Model, d: types.Data, ctx: SolverContext): nblocks_perblock = int((d.naconmax + dim_block - 1) / dim_block) - if SPARSE_CONSTRAINT_JACOBIAN: + if m.is_sparse: wp.launch( - update_gradient_JTCJ_sparse, - dim=(d.naconmax, m.dof_tri_row.size), - inputs=[ - m.opt.impratio_invsqrt, - m.dof_tri_row, - m.dof_tri_col, - d.contact.dist, - d.contact.includemargin, - d.contact.friction, - d.contact.dim, - d.contact.efc_address, - d.contact.worldid, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.D, - d.efc.state, - d.naconmax, - d.nacon, - ctx.Jaref, - ctx.done, - nblocks_perblock, - dim_block, - ], - outputs=[ctx.h], + update_gradient_JTCJ_sparse, + dim=(dim_block, m.dof_tri_row.size), + inputs=[ + m.opt.impratio_invsqrt, + m.dof_tri_row, + m.dof_tri_col, + d.contact.dist, + d.contact.includemargin, + d.contact.friction, + d.contact.dim, + d.contact.efc_address, + d.contact.worldid, + d.efc.J_rownnz, + d.efc.J_rowadr, + d.efc.J_colind, + d.efc.J, + d.efc.D, + d.efc.state, + d.naconmax, + d.nacon, + ctx.Jaref, + ctx.done, + nblocks_perblock, + dim_block, + ], + outputs=[ctx.h], ) else: wp.launch( - update_gradient_JTCJ_dense, - dim=(dim_block, m.dof_tri_row.size), - inputs=[ - m.opt.impratio_invsqrt, - m.dof_tri_row, - m.dof_tri_col, - d.contact.dist, - d.contact.includemargin, - d.contact.friction, - d.contact.dim, - d.contact.efc_address, - d.contact.worldid, - d.efc.J, - d.efc.D, - d.efc.state, - d.naconmax, - d.nacon, - ctx.Jaref, - ctx.done, - nblocks_perblock, - dim_block, - ], - outputs=[ctx.h], + update_gradient_JTCJ_dense, + dim=(dim_block, m.dof_tri_row.size), + inputs=[ + m.opt.impratio_invsqrt, + m.dof_tri_row, + m.dof_tri_col, + d.contact.dist, + d.contact.includemargin, + d.contact.friction, + d.contact.dim, + d.contact.efc_address, + d.contact.worldid, + d.efc.J, + d.efc.D, + d.efc.state, + d.naconmax, + d.nacon, + ctx.Jaref, + ctx.done, + nblocks_perblock, + dim_block, + ], + outputs=[ctx.h], ) _cholesky_factorize_solve(m, d, ctx) @@ -3142,58 +3008,51 @@ def _update_gradient(m: types.Model, d: types.Data, ctx: SolverContext): raise ValueError(f"Unknown solver type: {m.opt.solver}") -def _update_gradient_incremental( - m: types.Model, d: types.Data, ctx: SolverContext -): +def _update_gradient_incremental(m: types.Model, d: types.Data, ctx: SolverContext): """Incremental gradient update: update H for changed constraints + re-factorize. Skips the full J^T*D*J rebuild by applying only the delta from constraints that changed QUADRATIC state, then re-factorizes and solves. """ - wp.launch( - update_gradient_zero_grad_dot, - dim=(d.nworld), - inputs=[ctx.done], - outputs=[ctx.grad_dot], - ) + wp.launch(update_gradient_zero_grad_dot, dim=(d.nworld), inputs=[ctx.done], outputs=[ctx.grad_dot]) wp.launch( - update_gradient_grad, - dim=(d.nworld, m.nv), - inputs=[d.qfrc_smooth, d.qfrc_constraint, d.efc.Ma, ctx.done], - outputs=[ctx.grad, ctx.grad_dot], + update_gradient_grad, + dim=(d.nworld, m.nv), + inputs=[d.qfrc_smooth, d.qfrc_constraint, d.efc.Ma, ctx.done], + outputs=[ctx.grad, ctx.grad_dot], ) # Update lower triangle of H with delta from changed constraints - if SPARSE_CONSTRAINT_JACOBIAN: + if m.is_sparse: wp.launch( - update_gradient_h_incremental_sparse, - dim=(d.nworld, ctx.changed_efc_ids.shape[1]), - inputs=[ - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.D, - d.efc.state, - ctx.changed_efc_ids, - ctx.changed_efc_count, - ], - outputs=[ctx.h], + update_gradient_h_incremental_sparse, + dim=(d.nworld, ctx.changed_efc_ids.shape[1]), + inputs=[ + d.efc.J_rownnz, + d.efc.J_rowadr, + d.efc.J_colind, + d.efc.J, + d.efc.D, + d.efc.state, + ctx.changed_efc_ids, + ctx.changed_efc_count, + ], + outputs=[ctx.h], ) else: lower_tri_dim = m.nv * (m.nv + 1) // 2 wp.launch( - update_gradient_h_incremental, - dim=(d.nworld, lower_tri_dim), - inputs=[ - d.efc.J, - d.efc.D, - d.efc.state, - ctx.changed_efc_ids, - ctx.changed_efc_count, - ], - outputs=[ctx.h], + update_gradient_h_incremental, + dim=(d.nworld, lower_tri_dim), + inputs=[ + d.efc.J, + d.efc.D, + d.efc.state, + ctx.changed_efc_ids, + ctx.changed_efc_count, + ], + outputs=[ctx.h], ) _cholesky_factorize_solve(m, d, ctx) @@ -3347,10 +3206,7 @@ def _solver_iteration( # path in update_constraint_efc has early returns that skip state change # tracking, and the additional JTCJ Hessian term depends on Jaref which # changes every iteration. - incremental = ( - m.opt.solver == types.SolverType.NEWTON - and m.opt.cone != types.ConeType.ELLIPTIC - ) + incremental = m.opt.solver == types.SolverType.NEWTON and m.opt.cone != types.ConeType.ELLIPTIC if incremental: # Must complete before update_constraint_efc which atomically increments. @@ -3422,18 +3278,10 @@ def init_context(m: types.Model, d: types.Data, ctx: SolverContext | InverseCont ctx.Jaref.zero_() wp.launch( - solve_init_jaref(m.is_sparse, m.nv, dofs_per_thread), - dim=(d.nworld, d.njmax, threads_per_efc), - inputs=[ - d.nefc, - d.qacc, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, - d.efc.J, - d.efc.aref, - ], - outputs=[ctx.Jaref], + solve_init_jaref(m.is_sparse, m.nv, dofs_per_thread), + dim=(d.nworld, d.njmax, threads_per_efc), + inputs=[d.nefc, d.qacc, d.efc.J_rownnz, d.efc.J_rowadr, d.efc.J_colind, d.efc.J, d.efc.aref], + outputs=[ctx.Jaref], ) # Ma = qM @ qacc diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/support.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/support.py index b45472c4..995b2a7c 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/support.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/support.py @@ -18,18 +18,52 @@ from typing import Optional, Tuple import warp as wp from mujoco.mjx.third_party.mujoco_warp._src.math import motion_cross +from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINVAL from mujoco.mjx.third_party.mujoco_warp._src.types import ConeType from mujoco.mjx.third_party.mujoco_warp._src.types import Data +from mujoco.mjx.third_party.mujoco_warp._src.types import DynType from mujoco.mjx.third_party.mujoco_warp._src.types import JointType from mujoco.mjx.third_party.mujoco_warp._src.types import Model from mujoco.mjx.third_party.mujoco_warp._src.types import State from mujoco.mjx.third_party.mujoco_warp._src.types import vec5 +from mujoco.mjx.third_party.mujoco_warp._src.types import vec10f from mujoco.mjx.third_party.mujoco_warp._src.warp_util import cache_kernel from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope wp.set_module_options({"enable_backward": False}) +# TODO(team): kernel analyzer array slice? +@wp.func +def next_act( + # Model: + opt_timestep: float, # kernel_analyzer: ignore + actuator_dyntype: int, # kernel_analyzer: ignore + actuator_dynprm: vec10f, # kernel_analyzer: ignore + actuator_actrange: wp.vec2, # kernel_analyzer: ignore + # Data In: + act_in: float, # kernel_analyzer: ignore + act_dot_in: float, # kernel_analyzer: ignore + # In: + act_dot_scale: float, + clamp: bool, +) -> float: + # advance actuation + if actuator_dyntype == DynType.FILTEREXACT: + tau = wp.max(MJ_MINVAL, actuator_dynprm[0]) + act = act_in + act_dot_scale * act_dot_in * tau * (1.0 - wp.exp(-opt_timestep / tau)) + elif actuator_dyntype == DynType.USER: + return act_in + else: + act = act_in + act_dot_scale * act_dot_in * opt_timestep + + # clamp to actrange + if clamp: + act = wp.clamp(act, actuator_actrange[0], actuator_actrange[1]) + + return act + + @cache_kernel def mul_m_sparse(check_skip: bool): @wp.kernel(module="unique") diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py index 94434ff9..11b7a7c0 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py @@ -33,11 +33,8 @@ MJ_MAX_EPAFACES = 5 TILE_SIZE_JTDAJ_SPARSE = 16 TILE_SIZE_JTDAJ_DENSE = 16 -# TODO(team): remove after improving performance for sparse constraint jacobian -SPARSE_CONSTRAINT_JACOBIAN = False - -# TODO(team): remove after mjwarp depends on warp-lang >= 1.12 in pyproject.toml -TEXTURE_DTYPE = wp.Texture2D if hasattr(wp, "Texture2D") else int +# maximum number of plugin attributes +_NPLUGINATTR = 128 # TODO(team): add check that all wp.launch_tiled 'block_dim' settings are configurable @@ -53,7 +50,6 @@ class BlockDim: # forward euler_dense: int = 32 actuator_velocity: int = 32 - tendon_velocity: int = 32 # ray ray: int = 64 # sensor @@ -63,6 +59,7 @@ class BlockDim: cholesky_factorize: int = 32 cholesky_solve: int = 32 cholesky_factorize_solve: int = 32 + solve_LD_sparse_fused: int = 64 # solver update_gradient_cholesky: int = 64 update_gradient_cholesky_blocked: int = 32 @@ -351,6 +348,7 @@ class GeomType(enum.IntEnum): BOX: box MESH: mesh SDF: sdf + FLEX: flex """ PLANE = mujoco.mjtGeom.mjGEOM_PLANE @@ -362,6 +360,7 @@ class GeomType(enum.IntEnum): BOX = mujoco.mjtGeom.mjGEOM_BOX MESH = mujoco.mjtGeom.mjGEOM_MESH SDF = mujoco.mjtGeom.mjGEOM_SDF + FLEX = mujoco.mjtGeom.mjGEOM_FLEX # unsupported: NGEOMTYPES, ARROW*, LINE, SKIN, LABEL, NONE @@ -662,6 +661,10 @@ class vec11f(wp.types.vector(length=11, dtype=float)): pass +class vec_pluginattr(wp.types.vector(length=_NPLUGINATTR, dtype=float)): + pass + + class mat23f(wp.types.matrix(shape=(2, 3), dtype=float)): pass @@ -679,6 +682,7 @@ vec6 = vec6f vec8 = vec8f vec10 = vec10f vec11 = vec11f +vec128 = vec_pluginattr mat23 = mat23f mat43 = mat43f mat63 = mat63f @@ -841,6 +845,7 @@ class Model: nflexelem: number of elements in all flexes nflexelemdata: number of element vertex ids in all flexes nflexelemedge: number of element edge ids in all flexes + nflexshelldata: number of shell fragment vertex ids in all flexes nJfe: number of non-zeros in sparse flexedge Jacobian nmesh: number of meshes nmeshvert: number of vertices for all meshes @@ -857,6 +862,7 @@ class Model: nexclude: number of excluded geom pairs neq: number of equality constraints ntendon: number of tendons + nJten: number of non-zeros in sparse tendon Jacobian nwrap: number of wrap objects in all tendon paths nsensor: number of sensors nmocap: number of mocap bodies @@ -973,6 +979,11 @@ class Model: light_poscom0: global position rel. to sub-com in qpos0 (*, nlight, 3) light_pos0: global position rel. to body in qpos0 (*, nlight, 3) light_dir0: global direction in qpos0 (*, nlight, 3) + flex_contype: flex contact type (nflex,) + flex_conaffinity: flex contact affinity (nflex,) + flex_condim: contact dimensionality (1, 3, 4, 6) (nflex,) + flex_friction: friction for (slide, spin, roll) (nflex, 3) + flex_margin: detect contact if dist= 1.12 in pyproject.toml - textures: array("*", TEXTURE_DTYPE) - textures_registry: list[TEXTURE_DTYPE] + textures: array("*", wp.Texture2D) + textures_registry: list[wp.Texture2D] hfield_registry: dict hfield_bvh_id: array("nhfield", wp.uint64) hfield_bounds_size: array("nhfield", wp.vec3) - flex_mesh: wp.Mesh + flex_mesh_registry: dict flex_rgba: array("nflex", wp.vec4) - flex_bvh_id: wp.uint64 - flex_face_point: array("*", wp.vec3) - flex_faceadr: array("nflex", int) - flex_nface: int - flex_nwork: int - flex_group_root: array("nworld", int) - flex_elemdataadr: array("nflex", int) - flex_shell: array("*", int) - flex_shelldataadr: array("nflex", int) - flex_radius: array("nflex", float) - flex_workadr: array("nflex", int) - flex_worknum: array("nflex", int) + flex_bvh_id: array("*", wp.uint64) + flex_group_root: array("nworld", "*", int) flex_render_smooth: bool + bvh_nflexgeom: int + flex_dim_np: array("nflex", int) + flex_geom_flexid: array("*", int) + flex_geom_edgeid: array("*", int) bvh: wp.Bvh bvh_id: wp.uint64 lower: array("*", wp.vec3) @@ -1988,5 +1978,8 @@ class RenderContext: depth_adr: array("ncam", int) render_rgb: array("ncam", bool) render_depth: array("ncam", bool) + seg_data: array("*", int) + seg_adr: array("ncam", int) + render_seg: array("ncam", bool) znear: float total_rays: int diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/util_pkg.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/util_pkg.py index b7debea4..e2f8acf9 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/util_pkg.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/util_pkg.py @@ -37,9 +37,7 @@ def _parse_version(version_str: str) -> tuple[tuple[int, int | str], ...]: """ # Split on both '.' and '-' parts = re.split(r"[.\-]", version_str) - return tuple( - [(0, int(p)) if p.isdigit() else (-1, p) for p in parts] + [(0, 0)] - ) + return tuple([(0, int(p)) if p.isdigit() else (-1, p) for p in parts] + [(0, 0)]) def check_version(spec: str) -> bool: @@ -65,9 +63,7 @@ def check_version(spec: str) -> bool: """ match = re.match(r"^([a-zA-Z0-9_\-]+)(>=|<=|>|<|==|!=)(.+)$", spec) if not match: - raise ValueError( - f"Invalid version spec '{spec}'. Expected format: 'package>=version'" - ) + raise ValueError(f"Invalid version spec '{spec}'. Expected format: 'package>=version'") package_name, op, version_str = match.groups() required_version = _parse_version(version_str) @@ -87,11 +83,11 @@ def check_version(spec: str) -> bool: installed_version = _parse_version(installed_str) ops = { - ">=": operator.ge, - "<=": operator.le, - ">": operator.gt, - "<": operator.lt, - "==": operator.eq, - "!=": operator.ne, + ">=": operator.ge, + "<=": operator.le, + ">": operator.gt, + "<": operator.lt, + "==": operator.eq, + "!=": operator.ne, } return ops[op](installed_version, required_version) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/warp_util.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/warp_util.py index 4ac1cb0d..960e4266 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/warp_util.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/warp_util.py @@ -146,13 +146,13 @@ def check_toolkit_driver(): if wp.get_device().is_cuda: if not wp.is_conditional_graph_supported(): warnings.warn( - """ + """ CUDA version < 12.4 detected - graph capture may be unreliable for < 12.3 - conditional graph nodes are not available for < 12.4 Model.opt.graph_conditional should be set to False """, - stacklevel=2, + stacklevel=2, ) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/pyproject.toml b/mjx/mujoco/mjx/third_party/mujoco_warp/pyproject.toml index 37305248..e8d98eb4 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/pyproject.toml +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/pyproject.toml @@ -28,7 +28,7 @@ requires-python = ">=3.10" dependencies = [ "absl-py", "etils[epath]", - "mujoco>=3.5.0", + "mujoco>=3.6.0", "numpy", "warp-lang>=1.12", ] @@ -55,7 +55,7 @@ dev = [ "ruff", "pygls>=1.0.0,<2.0.0", "lsprotocol>=2023.0.1,<2024.0.0", - "mujoco>=3.5.0.dev0", + "mujoco>=3.6.0.dev0", "warp-lang>=1.11.0.dev0", ] # TODO(team): cpu and cuda JAX optional dependencies are temporary, remove after we land MJX:Warp diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/viewer.py b/mjx/mujoco/mjx/third_party/mujoco_warp/viewer.py index 5f1de2d4..cf65f02a 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/viewer.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/viewer.py @@ -18,7 +18,7 @@ Usage: mjwarp-viewer [flags] Example: - mjwarp-viewer benchmark/humanoid/humanoid.xml -o "opt.solver=cg" + mjwarp-viewer benchmarks/humanoid/humanoid.xml -o "opt.solver=cg" """ import copy @@ -56,6 +56,7 @@ _CLEAR_WARP_CACHE = flags.DEFINE_bool("clear_warp_cache", False, "Clear warp cac _ENGINE = flags.DEFINE_enum_class("engine", EngineOptions.WARP, EngineOptions, "Simulation engine") _NCONMAX = flags.DEFINE_integer("nconmax", None, "Maximum number of contacts.") _NJMAX = flags.DEFINE_integer("njmax", None, "Maximum number of constraints per world.") +_NJMAX_NNZ = flags.DEFINE_integer("njmax_nnz", None, "Maximum number of non-zeros in constraint Jacobian.") _NCCDMAX = flags.DEFINE_integer("nccdmax", None, "Maximum number of CCD contacts per world.") _OVERRIDE = flags.DEFINE_multi_string("override", [], "Model overrides (notation: foo.bar = baz)", short_name="o") _KEYFRAME = flags.DEFINE_integer("keyframe", 0, "keyframe to initialize simulation.") @@ -149,7 +150,7 @@ def _main(argv: Sequence[str]) -> None: override_model(mjm, _OVERRIDE.value) m = mjw.put_model(mjm) override_model(m, _OVERRIDE.value) - d = mjw.put_data(mjm, mjd, nconmax=_NCONMAX.value, njmax=_NJMAX.value, nccdmax=_NCCDMAX.value) + d = mjw.put_data(mjm, mjd, nconmax=_NCONMAX.value, njmax=_NJMAX.value, njmax_nnz=_NJMAX_NNZ.value, nccdmax=_NCCDMAX.value) graph = _compile_step(m, d) if wp.get_device().is_cuda else None if graph is None: mjw.step(m, d) # warmup step diff --git a/mjx/mujoco/mjx/warp/bvh.py b/mjx/mujoco/mjx/warp/bvh.py index 2bb522f2..2c1fee49 100644 --- a/mjx/mujoco/mjx/warp/bvh.py +++ b/mjx/mujoco/mjx/warp/bvh.py @@ -48,20 +48,27 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) + @ffi.format_args_for_warp def _refit_bvh_shim( # Model nworld: int, flex_dim: wp.array(dtype=int), + flex_edge: wp.array(dtype=wp.vec2i), flex_elem: wp.array(dtype=int), + flex_elemadr: wp.array(dtype=int), + flex_elemdataadr: wp.array(dtype=int), flex_elemnum: wp.array(dtype=int), + flex_radius: wp.array(dtype=float), + flex_shell: wp.array(dtype=int), + flex_shelldataadr: wp.array(dtype=int), flex_vertadr: wp.array(dtype=int), + flex_vertnum: wp.array(dtype=int), geom_dataid: wp.array(dtype=int), geom_size: wp.array2d(dtype=wp.vec3), geom_type: wp.array(dtype=int), nflex: int, - nflexelemdata: int, - nflexvert: int, + nflexelem: int, # Data flexvert_xpos: wp.array2d(dtype=wp.vec3), geom_xmat: wp.array2d(dtype=wp.mat33), @@ -77,15 +84,21 @@ def _refit_bvh_shim( _d.efc = _e _d.contact = _c _m.flex_dim = flex_dim + _m.flex_edge = flex_edge _m.flex_elem = flex_elem + _m.flex_elemadr = flex_elemadr + _m.flex_elemdataadr = flex_elemdataadr _m.flex_elemnum = flex_elemnum + _m.flex_radius = flex_radius + _m.flex_shell = flex_shell + _m.flex_shelldataadr = flex_shelldataadr _m.flex_vertadr = flex_vertadr + _m.flex_vertnum = flex_vertnum _m.geom_dataid = geom_dataid _m.geom_size = geom_size _m.geom_type = geom_type _m.nflex = nflex - _m.nflexelemdata = nflexelemdata - _m.nflexvert = nflexvert + _m.nflexelem = nflexelem _d.flexvert_xpos = flexvert_xpos _d.geom_xmat = geom_xmat _d.geom_xpos = geom_xpos @@ -113,15 +126,21 @@ def _refit_bvh_jax_impl( out = jf( d.qpos.shape[0], m._impl.flex_dim, + m._impl.flex_edge, m._impl.flex_elem, + m._impl.flex_elemadr, + m._impl.flex_elemdataadr, m._impl.flex_elemnum, + m._impl.flex_radius, + m._impl.flex_shell, + m._impl.flex_shelldataadr, m._impl.flex_vertadr, + m._impl.flex_vertnum, m.geom_dataid, m.geom_size, m.geom_type, m._impl.nflex, - m._impl.nflexelemdata, - m._impl.nflexvert, + m._impl.nflexelem, d._impl.flexvert_xpos, d.geom_xmat, d.geom_xpos, diff --git a/mjx/mujoco/mjx/warp/collision_driver.py b/mjx/mujoco/mjx/warp/collision_driver.py index 265bfba3..9d5412d9 100644 --- a/mjx/mujoco/mjx/warp/collision_driver.py +++ b/mjx/mujoco/mjx/warp/collision_driver.py @@ -52,8 +52,26 @@ def _collision_shim( # Model nworld: int, block_dim: mjwp_types.BlockDim, + flex_conaffinity: wp.array(dtype=int), + flex_condim: wp.array(dtype=int), + flex_contype: wp.array(dtype=int), + flex_dim: wp.array(dtype=int), + flex_elem: wp.array(dtype=int), + flex_elemadr: wp.array(dtype=int), + flex_elemdataadr: wp.array(dtype=int), + flex_elemnum: wp.array(dtype=int), + flex_friction: wp.array(dtype=wp.vec3), + flex_margin: wp.array(dtype=float), + flex_radius: wp.array(dtype=float), + flex_shell: wp.array(dtype=int), + flex_shelldataadr: wp.array(dtype=int), + flex_shellnum: wp.array(dtype=int), + flex_vertadr: wp.array(dtype=int), + flex_vertflexid: wp.array(dtype=int), geom_aabb: wp.array3d(dtype=wp.vec3), + geom_conaffinity: wp.array(dtype=int), geom_condim: wp.array(dtype=int), + geom_contype: wp.array(dtype=int), geom_dataid: wp.array(dtype=int), geom_friction: wp.array2d(dtype=wp.vec3), geom_gap: wp.array2d(dtype=float), @@ -90,6 +108,10 @@ def _collision_shim( mesh_vert: wp.array(dtype=wp.vec3), mesh_vertadr: wp.array(dtype=int), mesh_vertnum: wp.array(dtype=int), + nflex: int, + nflexelem: int, + nflexshelldata: int, + nflexvert: int, ngeom: int, nmaxmeshdeg: int, nmaxpolygon: int, @@ -108,7 +130,7 @@ def _collision_shim( pair_solref: wp.array2d(dtype=wp.vec2), pair_solreffriction: wp.array2d(dtype=wp.vec2), plugin: wp.array(dtype=int), - plugin_attr: wp.array(dtype=wp.vec3f), + plugin_attr: wp.array(dtype=mjwp_types.vec_pluginattr), opt__broadphase: int, opt__broadphase_filter: int, opt__ccd_iterations: int, @@ -120,12 +142,15 @@ def _collision_shim( # Data naccdmax: int, naconmax: int, + flexvert_xpos: wp.array2d(dtype=wp.vec3), geom_xmat: wp.array2d(dtype=wp.mat33), geom_xpos: wp.array2d(dtype=wp.vec3), nacon: wp.array(dtype=int), ncollision: wp.array(dtype=int), contact__dim: wp.array(dtype=int), contact__dist: wp.array(dtype=float), + contact__efc_address: wp.array2d(dtype=int), + contact__flex: wp.array(dtype=wp.vec2i), contact__frame: wp.array(dtype=wp.mat33), contact__friction: wp.array(dtype=mjwp_types.vec5), contact__geom: wp.array(dtype=wp.vec2i), @@ -136,6 +161,7 @@ def _collision_shim( contact__solref: wp.array(dtype=wp.vec2), contact__solreffriction: wp.array(dtype=wp.vec2), contact__type: wp.array(dtype=int), + contact__vert: wp.array(dtype=wp.vec2i), contact__worldid: wp.array(dtype=int), ): _m.stat = _s @@ -144,8 +170,26 @@ def _collision_shim( _d.efc = _e _d.contact = _c _m.block_dim = block_dim + _m.flex_conaffinity = flex_conaffinity + _m.flex_condim = flex_condim + _m.flex_contype = flex_contype + _m.flex_dim = flex_dim + _m.flex_elem = flex_elem + _m.flex_elemadr = flex_elemadr + _m.flex_elemdataadr = flex_elemdataadr + _m.flex_elemnum = flex_elemnum + _m.flex_friction = flex_friction + _m.flex_margin = flex_margin + _m.flex_radius = flex_radius + _m.flex_shell = flex_shell + _m.flex_shelldataadr = flex_shelldataadr + _m.flex_shellnum = flex_shellnum + _m.flex_vertadr = flex_vertadr + _m.flex_vertflexid = flex_vertflexid _m.geom_aabb = geom_aabb + _m.geom_conaffinity = geom_conaffinity _m.geom_condim = geom_condim + _m.geom_contype = geom_contype _m.geom_dataid = geom_dataid _m.geom_friction = geom_friction _m.geom_gap = geom_gap @@ -182,6 +226,10 @@ def _collision_shim( _m.mesh_vert = mesh_vert _m.mesh_vertadr = mesh_vertadr _m.mesh_vertnum = mesh_vertnum + _m.nflex = nflex + _m.nflexelem = nflexelem + _m.nflexshelldata = nflexshelldata + _m.nflexvert = nflexvert _m.ngeom = ngeom _m.nmaxmeshdeg = nmaxmeshdeg _m.nmaxpolygon = nmaxpolygon @@ -211,6 +259,8 @@ def _collision_shim( _m.plugin_attr = plugin_attr _d.contact.dim = contact__dim _d.contact.dist = contact__dist + _d.contact.efc_address = contact__efc_address + _d.contact.flex = contact__flex _d.contact.frame = contact__frame _d.contact.friction = contact__friction _d.contact.geom = contact__geom @@ -221,7 +271,9 @@ def _collision_shim( _d.contact.solref = contact__solref _d.contact.solreffriction = contact__solreffriction _d.contact.type = contact__type + _d.contact.vert = contact__vert _d.contact.worldid = contact__worldid + _d.flexvert_xpos = flexvert_xpos _d.geom_xmat = geom_xmat _d.geom_xpos = geom_xpos _d.naccdmax = naccdmax @@ -238,6 +290,8 @@ def _collision_jax_impl(m: types.Model, d: types.Data): 'ncollision': d._impl.ncollision.shape, 'contact__dim': d._impl.contact__dim.shape, 'contact__dist': d._impl.contact__dist.shape, + 'contact__efc_address': d._impl.contact__efc_address.shape, + 'contact__flex': d._impl.contact__flex.shape, 'contact__frame': d._impl.contact__frame.shape, 'contact__friction': d._impl.contact__friction.shape, 'contact__geom': d._impl.contact__geom.shape, @@ -248,11 +302,12 @@ def _collision_jax_impl(m: types.Model, d: types.Data): 'contact__solref': d._impl.contact__solref.shape, 'contact__solreffriction': d._impl.contact__solreffriction.shape, 'contact__type': d._impl.contact__type.shape, + 'contact__vert': d._impl.contact__vert.shape, 'contact__worldid': d._impl.contact__worldid.shape, } jf = ffi.jax_callable_variadic_tuple( _collision_shim, - num_outputs=15, + num_outputs=18, output_dims=output_dims, vmap_method=None, in_out_argnames=set([ @@ -260,6 +315,8 @@ def _collision_jax_impl(m: types.Model, d: types.Data): 'ncollision', 'contact__dim', 'contact__dist', + 'contact__efc_address', + 'contact__flex', 'contact__frame', 'contact__friction', 'contact__geom', @@ -270,6 +327,7 @@ def _collision_jax_impl(m: types.Model, d: types.Data): 'contact__solref', 'contact__solreffriction', 'contact__type', + 'contact__vert', 'contact__worldid', ]), stage_in_argnames=set([ @@ -299,8 +357,26 @@ def _collision_jax_impl(m: types.Model, d: types.Data): out = jf( d.qpos.shape[0], m._impl.block_dim, + m._impl.flex_conaffinity, + m._impl.flex_condim, + m._impl.flex_contype, + m._impl.flex_dim, + m._impl.flex_elem, + m._impl.flex_elemadr, + m._impl.flex_elemdataadr, + m._impl.flex_elemnum, + m._impl.flex_friction, + m._impl.flex_margin, + m._impl.flex_radius, + m._impl.flex_shell, + m._impl.flex_shelldataadr, + m._impl.flex_shellnum, + m._impl.flex_vertadr, + m._impl.flex_vertflexid, m.geom_aabb, + m.geom_conaffinity, m.geom_condim, + m.geom_contype, m.geom_dataid, m.geom_friction, m.geom_gap, @@ -337,6 +413,10 @@ def _collision_jax_impl(m: types.Model, d: types.Data): m.mesh_vert, m.mesh_vertadr, m.mesh_vertnum, + m._impl.nflex, + m._impl.nflexelem, + m._impl.nflexshelldata, + m._impl.nflexvert, m.ngeom, m._impl.nmaxmeshdeg, m._impl.nmaxpolygon, @@ -366,12 +446,15 @@ def _collision_jax_impl(m: types.Model, d: types.Data): m.opt._impl.sdf_iterations, d._impl.naccdmax, d._impl.naconmax, + d._impl.flexvert_xpos, d.geom_xmat, d.geom_xpos, d._impl.nacon, d._impl.ncollision, d._impl.contact__dim, d._impl.contact__dist, + d._impl.contact__efc_address, + d._impl.contact__flex, d._impl.contact__frame, d._impl.contact__friction, d._impl.contact__geom, @@ -382,6 +465,7 @@ def _collision_jax_impl(m: types.Model, d: types.Data): d._impl.contact__solref, d._impl.contact__solreffriction, d._impl.contact__type, + d._impl.contact__vert, d._impl.contact__worldid, ) d = d.tree_replace({ @@ -389,17 +473,20 @@ def _collision_jax_impl(m: types.Model, d: types.Data): '_impl.ncollision': out[1], '_impl.contact__dim': out[2], '_impl.contact__dist': out[3], - '_impl.contact__frame': out[4], - '_impl.contact__friction': out[5], - '_impl.contact__geom': out[6], - '_impl.contact__geomcollisionid': out[7], - '_impl.contact__includemargin': out[8], - '_impl.contact__pos': out[9], - '_impl.contact__solimp': out[10], - '_impl.contact__solref': out[11], - '_impl.contact__solreffriction': out[12], - '_impl.contact__type': out[13], - '_impl.contact__worldid': out[14], + '_impl.contact__efc_address': out[4], + '_impl.contact__flex': out[5], + '_impl.contact__frame': out[6], + '_impl.contact__friction': out[7], + '_impl.contact__geom': out[8], + '_impl.contact__geomcollisionid': out[9], + '_impl.contact__includemargin': out[10], + '_impl.contact__pos': out[11], + '_impl.contact__solimp': out[12], + '_impl.contact__solref': out[13], + '_impl.contact__solreffriction': out[14], + '_impl.contact__type': out[15], + '_impl.contact__vert': out[16], + '_impl.contact__worldid': out[17], }) return d diff --git a/mjx/mujoco/mjx/warp/forward.py b/mjx/mujoco/mjx/warp/forward.py index e3833601..e0f5c053 100644 --- a/mjx/mujoco/mjx/warp/forward.py +++ b/mjx/mujoco/mjx/warp/forward.py @@ -138,6 +138,10 @@ def _forward_shim( eq_type: wp.array(dtype=int), eq_wld_adr: wp.array(dtype=int), flex_bending: wp.array2d(dtype=float), + flex_centered: wp.array(dtype=bool), + flex_conaffinity: wp.array(dtype=int), + flex_condim: wp.array(dtype=int), + flex_contype: wp.array(dtype=int), flex_damping: wp.array(dtype=float), flex_dim: wp.array(dtype=int), flex_edge: wp.array(dtype=wp.vec2i), @@ -146,12 +150,22 @@ def _forward_shim( flex_edgenum: wp.array(dtype=int), flex_elem: wp.array(dtype=int), flex_elemadr: wp.array(dtype=int), + flex_elemdataadr: wp.array(dtype=int), flex_elemedge: wp.array(dtype=int), flex_elemedgeadr: wp.array(dtype=int), flex_elemnum: wp.array(dtype=int), + flex_friction: wp.array(dtype=wp.vec3), + flex_margin: wp.array(dtype=float), + flex_radius: wp.array(dtype=float), + flex_shell: wp.array(dtype=int), + flex_shelldataadr: wp.array(dtype=int), + flex_shellnum: wp.array(dtype=int), flex_stiffness: wp.array2d(dtype=float), + flex_vert: wp.array(dtype=wp.vec3), flex_vertadr: wp.array(dtype=int), flex_vertbodyid: wp.array(dtype=int), + flex_vertflexid: wp.array(dtype=int), + flex_vertnum: wp.array(dtype=int), flexedge_J_colind: wp.array(dtype=int), flexedge_J_rowadr: wp.array(dtype=int), flexedge_J_rownnz: wp.array(dtype=int), @@ -159,7 +173,9 @@ def _forward_shim( flexedge_length0: wp.array(dtype=float), geom_aabb: wp.array3d(dtype=wp.vec3), geom_bodyid: wp.array(dtype=int), + geom_conaffinity: wp.array(dtype=int), geom_condim: wp.array(dtype=int), + geom_contype: wp.array(dtype=int), geom_dataid: wp.array(dtype=int), geom_fluid: wp.array2d(dtype=float), geom_friction: wp.array2d(dtype=wp.vec3), @@ -213,6 +229,7 @@ def _forward_shim( light_targetbodyid: wp.array(dtype=int), mapM2M: wp.array(dtype=int), mat_rgba: wp.array2d(dtype=wp.vec4), + max_ten_J_rownnz: int, mesh_face: wp.array(dtype=wp.vec3i), mesh_faceadr: wp.array(dtype=int), mesh_graph: wp.array(dtype=int), @@ -235,6 +252,7 @@ def _forward_shim( mesh_vertadr: wp.array(dtype=int), mesh_vertnum: wp.array(dtype=int), nC: int, + nJten: int, na: int, nacttrnbody: int, nbody: int, @@ -244,6 +262,7 @@ def _forward_shim( nflex: int, nflexedge: int, nflexelem: int, + nflexshelldata: int, nflexvert: int, ngeom: int, ngravcomp: int, @@ -279,7 +298,9 @@ def _forward_shim( pair_solref: wp.array2d(dtype=wp.vec2), pair_solreffriction: wp.array2d(dtype=wp.vec2), plugin: wp.array(dtype=int), - plugin_attr: wp.array(dtype=wp.vec3f), + plugin_attr: wp.array(dtype=mjwp_types.vec_pluginattr), + qLD_all_updates: wp.array(dtype=wp.vec3i), + qLD_level_offsets: wp.array(dtype=int), qLD_updates: tuple[wp.array(dtype=wp.vec3i), ...], qM_fullm_i: wp.array(dtype=int), qM_fullm_j: wp.array(dtype=int), @@ -323,6 +344,9 @@ def _forward_shim( site_type: wp.array(dtype=int), taxel_sensorid: wp.array(dtype=int), taxel_vertadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_rownnz: wp.array(dtype=int), tendon_actfrclimited: wp.array(dtype=bool), tendon_actfrcrange: wp.array2d(dtype=wp.vec2), tendon_adr: wp.array(dtype=int), @@ -382,6 +406,7 @@ def _forward_shim( naccdmax: int, naconmax: int, njmax: int, + njmax_nnz: int, act: wp.array2d(dtype=float), act_dot: wp.array2d(dtype=float), actuator_force: wp.array2d(dtype=float), @@ -446,7 +471,7 @@ def _forward_shim( subtree_angmom: wp.array2d(dtype=wp.vec3), subtree_com: wp.array2d(dtype=wp.vec3), subtree_linvel: wp.array2d(dtype=wp.vec3), - ten_J: wp.array3d(dtype=float), + ten_J: wp.array2d(dtype=float), ten_length: wp.array2d(dtype=float), ten_velocity: wp.array2d(dtype=float), ten_wrapadr: wp.array2d(dtype=int), @@ -466,6 +491,7 @@ def _forward_shim( contact__dim: wp.array(dtype=int), contact__dist: wp.array(dtype=float), contact__efc_address: wp.array2d(dtype=int), + contact__flex: wp.array(dtype=wp.vec2i), contact__frame: wp.array(dtype=wp.mat33), contact__friction: wp.array(dtype=mjwp_types.vec5), contact__geom: wp.array(dtype=wp.vec2i), @@ -476,6 +502,7 @@ def _forward_shim( contact__solref: wp.array(dtype=wp.vec2), contact__solreffriction: wp.array(dtype=wp.vec2), contact__type: wp.array(dtype=int), + contact__vert: wp.array(dtype=wp.vec2i), contact__worldid: wp.array(dtype=int), efc__D: wp.array2d(dtype=float), efc__J: wp.array3d(dtype=float), @@ -585,6 +612,10 @@ def _forward_shim( _m.eq_type = eq_type _m.eq_wld_adr = eq_wld_adr _m.flex_bending = flex_bending + _m.flex_centered = flex_centered + _m.flex_conaffinity = flex_conaffinity + _m.flex_condim = flex_condim + _m.flex_contype = flex_contype _m.flex_damping = flex_damping _m.flex_dim = flex_dim _m.flex_edge = flex_edge @@ -593,12 +624,22 @@ def _forward_shim( _m.flex_edgenum = flex_edgenum _m.flex_elem = flex_elem _m.flex_elemadr = flex_elemadr + _m.flex_elemdataadr = flex_elemdataadr _m.flex_elemedge = flex_elemedge _m.flex_elemedgeadr = flex_elemedgeadr _m.flex_elemnum = flex_elemnum + _m.flex_friction = flex_friction + _m.flex_margin = flex_margin + _m.flex_radius = flex_radius + _m.flex_shell = flex_shell + _m.flex_shelldataadr = flex_shelldataadr + _m.flex_shellnum = flex_shellnum _m.flex_stiffness = flex_stiffness + _m.flex_vert = flex_vert _m.flex_vertadr = flex_vertadr _m.flex_vertbodyid = flex_vertbodyid + _m.flex_vertflexid = flex_vertflexid + _m.flex_vertnum = flex_vertnum _m.flexedge_J_colind = flexedge_J_colind _m.flexedge_J_rowadr = flexedge_J_rowadr _m.flexedge_J_rownnz = flexedge_J_rownnz @@ -606,7 +647,9 @@ def _forward_shim( _m.flexedge_length0 = flexedge_length0 _m.geom_aabb = geom_aabb _m.geom_bodyid = geom_bodyid + _m.geom_conaffinity = geom_conaffinity _m.geom_condim = geom_condim + _m.geom_contype = geom_contype _m.geom_dataid = geom_dataid _m.geom_fluid = geom_fluid _m.geom_friction = geom_friction @@ -660,6 +703,7 @@ def _forward_shim( _m.light_targetbodyid = light_targetbodyid _m.mapM2M = mapM2M _m.mat_rgba = mat_rgba + _m.max_ten_J_rownnz = max_ten_J_rownnz _m.mesh_face = mesh_face _m.mesh_faceadr = mesh_faceadr _m.mesh_graph = mesh_graph @@ -682,6 +726,7 @@ def _forward_shim( _m.mesh_vertadr = mesh_vertadr _m.mesh_vertnum = mesh_vertnum _m.nC = nC + _m.nJten = nJten _m.na = na _m.nacttrnbody = nacttrnbody _m.nbody = nbody @@ -691,6 +736,7 @@ def _forward_shim( _m.nflex = nflex _m.nflexedge = nflexedge _m.nflexelem = nflexelem + _m.nflexshelldata = nflexshelldata _m.nflexvert = nflexvert _m.ngeom = ngeom _m.ngravcomp = ngravcomp @@ -753,6 +799,8 @@ def _forward_shim( _m.pair_solreffriction = pair_solreffriction _m.plugin = plugin _m.plugin_attr = plugin_attr + _m.qLD_all_updates = qLD_all_updates + _m.qLD_level_offsets = qLD_level_offsets _m.qLD_updates = qLD_updates _m.qM_fullm_i = qM_fullm_i _m.qM_fullm_j = qM_fullm_j @@ -797,6 +845,9 @@ def _forward_shim( _m.stat.meaninertia = stat__meaninertia _m.taxel_sensorid = taxel_sensorid _m.taxel_vertadr = taxel_vertadr + _m.ten_J_colind = ten_J_colind + _m.ten_J_rowadr = ten_J_rowadr + _m.ten_J_rownnz = ten_J_rownnz _m.tendon_actfrclimited = tendon_actfrclimited _m.tendon_actfrcrange = tendon_actfrcrange _m.tendon_adr = tendon_adr @@ -842,6 +893,7 @@ def _forward_shim( _d.contact.dim = contact__dim _d.contact.dist = contact__dist _d.contact.efc_address = contact__efc_address + _d.contact.flex = contact__flex _d.contact.frame = contact__frame _d.contact.friction = contact__friction _d.contact.geom = contact__geom @@ -852,6 +904,7 @@ def _forward_shim( _d.contact.solref = contact__solref _d.contact.solreffriction = contact__solreffriction _d.contact.type = contact__type + _d.contact.vert = contact__vert _d.contact.worldid = contact__worldid _d.crb = crb _d.ctrl = ctrl @@ -895,6 +948,7 @@ def _forward_shim( _d.nf = nf _d.nisland = nisland _d.njmax = njmax + _d.njmax_nnz = njmax_nnz _d.nl = nl _d.qLD = qLD _d.qLDiagInv = qLDiagInv @@ -1018,6 +1072,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): 'contact__dim': d._impl.contact__dim.shape, 'contact__dist': d._impl.contact__dist.shape, 'contact__efc_address': d._impl.contact__efc_address.shape, + 'contact__flex': d._impl.contact__flex.shape, 'contact__frame': d._impl.contact__frame.shape, 'contact__friction': d._impl.contact__friction.shape, 'contact__geom': d._impl.contact__geom.shape, @@ -1028,6 +1083,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): 'contact__solref': d._impl.contact__solref.shape, 'contact__solreffriction': d._impl.contact__solreffriction.shape, 'contact__type': d._impl.contact__type.shape, + 'contact__vert': d._impl.contact__vert.shape, 'contact__worldid': d._impl.contact__worldid.shape, 'efc__D': d._impl.efc__D.shape, 'efc__J': d._impl.efc__J.shape, @@ -1047,7 +1103,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): } jf = ffi.jax_callable_variadic_tuple( _forward_shim, - num_outputs=100, + num_outputs=102, output_dims=output_dims, vmap_method=None, in_out_argnames=set([ @@ -1125,6 +1181,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): 'contact__dim', 'contact__dist', 'contact__efc_address', + 'contact__flex', 'contact__frame', 'contact__friction', 'contact__geom', @@ -1135,6 +1192,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): 'contact__solref', 'contact__solreffriction', 'contact__type', + 'contact__vert', 'contact__worldid', 'efc__D', 'efc__J', @@ -1417,6 +1475,10 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m.eq_type, m._impl.eq_wld_adr, m._impl.flex_bending, + m._impl.flex_centered, + m._impl.flex_conaffinity, + m._impl.flex_condim, + m._impl.flex_contype, m._impl.flex_damping, m._impl.flex_dim, m._impl.flex_edge, @@ -1425,12 +1487,22 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m._impl.flex_edgenum, m._impl.flex_elem, m._impl.flex_elemadr, + m._impl.flex_elemdataadr, m._impl.flex_elemedge, m._impl.flex_elemedgeadr, m._impl.flex_elemnum, + m._impl.flex_friction, + m._impl.flex_margin, + m._impl.flex_radius, + m._impl.flex_shell, + m._impl.flex_shelldataadr, + m._impl.flex_shellnum, m._impl.flex_stiffness, + m._impl.flex_vert, m._impl.flex_vertadr, m._impl.flex_vertbodyid, + m._impl.flex_vertflexid, + m._impl.flex_vertnum, m._impl.flexedge_J_colind, m._impl.flexedge_J_rowadr, m._impl.flexedge_J_rownnz, @@ -1438,7 +1510,9 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m._impl.flexedge_length0, m.geom_aabb, m.geom_bodyid, + m.geom_conaffinity, m.geom_condim, + m.geom_contype, m.geom_dataid, m.geom_fluid, m.geom_friction, @@ -1492,6 +1566,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m._impl.light_targetbodyid, m._impl.mapM2M, m.mat_rgba, + m._impl.max_ten_J_rownnz, m.mesh_face, m.mesh_faceadr, m.mesh_graph, @@ -1514,6 +1589,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m.mesh_vertadr, m.mesh_vertnum, m.nC, + m.nJten, m.na, m._impl.nacttrnbody, m.nbody, @@ -1523,6 +1599,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m._impl.nflex, m._impl.nflexedge, m._impl.nflexelem, + m._impl.nflexshelldata, m._impl.nflexvert, m.ngeom, m.ngravcomp, @@ -1559,6 +1636,8 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m.pair_solreffriction, m._impl.plugin, m._impl.plugin_attr, + m._impl.qLD_all_updates, + m._impl.qLD_level_offsets, m._impl.qLD_updates, m._impl.qM_fullm_i, m._impl.qM_fullm_j, @@ -1602,6 +1681,9 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m.site_type, m._impl.taxel_sensorid, m._impl.taxel_vertadr, + m._impl.ten_J_colind, + m._impl.ten_J_rowadr, + m._impl.ten_J_rownnz, m.tendon_actfrclimited, m.tendon_actfrcrange, m.tendon_adr, @@ -1660,6 +1742,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): d._impl.naccdmax, d._impl.naconmax, d._impl.njmax, + d._impl.njmax_nnz, d.act, d.act_dot, d.actuator_force, @@ -1744,6 +1827,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): d._impl.contact__dim, d._impl.contact__dist, d._impl.contact__efc_address, + d._impl.contact__flex, d._impl.contact__frame, d._impl.contact__friction, d._impl.contact__geom, @@ -1754,6 +1838,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): d._impl.contact__solref, d._impl.contact__solreffriction, d._impl.contact__type, + d._impl.contact__vert, d._impl.contact__worldid, d._impl.efc__D, d._impl.efc__J, @@ -1846,32 +1931,34 @@ def _forward_jax_impl(m: types.Model, d: types.Data): '_impl.contact__dim': out[71], '_impl.contact__dist': out[72], '_impl.contact__efc_address': out[73], - '_impl.contact__frame': out[74], - '_impl.contact__friction': out[75], - '_impl.contact__geom': out[76], - '_impl.contact__geomcollisionid': out[77], - '_impl.contact__includemargin': out[78], - '_impl.contact__pos': out[79], - '_impl.contact__solimp': out[80], - '_impl.contact__solref': out[81], - '_impl.contact__solreffriction': out[82], - '_impl.contact__type': out[83], - '_impl.contact__worldid': out[84], - '_impl.efc__D': out[85], - '_impl.efc__J': out[86], - '_impl.efc__J_colind': out[87], - '_impl.efc__J_rowadr': out[88], - '_impl.efc__J_rownnz': out[89], - '_impl.efc__Ma': out[90], - '_impl.efc__aref': out[91], - '_impl.efc__force': out[92], - '_impl.efc__frictionloss': out[93], - '_impl.efc__id': out[94], - '_impl.efc__margin': out[95], - '_impl.efc__pos': out[96], - '_impl.efc__state': out[97], - '_impl.efc__type': out[98], - '_impl.efc__vel': out[99], + '_impl.contact__flex': out[74], + '_impl.contact__frame': out[75], + '_impl.contact__friction': out[76], + '_impl.contact__geom': out[77], + '_impl.contact__geomcollisionid': out[78], + '_impl.contact__includemargin': out[79], + '_impl.contact__pos': out[80], + '_impl.contact__solimp': out[81], + '_impl.contact__solref': out[82], + '_impl.contact__solreffriction': out[83], + '_impl.contact__type': out[84], + '_impl.contact__vert': out[85], + '_impl.contact__worldid': out[86], + '_impl.efc__D': out[87], + '_impl.efc__J': out[88], + '_impl.efc__J_colind': out[89], + '_impl.efc__J_rowadr': out[90], + '_impl.efc__J_rownnz': out[91], + '_impl.efc__Ma': out[92], + '_impl.efc__aref': out[93], + '_impl.efc__force': out[94], + '_impl.efc__frictionloss': out[95], + '_impl.efc__id': out[96], + '_impl.efc__margin': out[97], + '_impl.efc__pos': out[98], + '_impl.efc__state': out[99], + '_impl.efc__type': out[100], + '_impl.efc__vel': out[101], }) return d @@ -1980,6 +2067,10 @@ def _step_shim( eq_type: wp.array(dtype=int), eq_wld_adr: wp.array(dtype=int), flex_bending: wp.array2d(dtype=float), + flex_centered: wp.array(dtype=bool), + flex_conaffinity: wp.array(dtype=int), + flex_condim: wp.array(dtype=int), + flex_contype: wp.array(dtype=int), flex_damping: wp.array(dtype=float), flex_dim: wp.array(dtype=int), flex_edge: wp.array(dtype=wp.vec2i), @@ -1988,12 +2079,22 @@ def _step_shim( flex_edgenum: wp.array(dtype=int), flex_elem: wp.array(dtype=int), flex_elemadr: wp.array(dtype=int), + flex_elemdataadr: wp.array(dtype=int), flex_elemedge: wp.array(dtype=int), flex_elemedgeadr: wp.array(dtype=int), flex_elemnum: wp.array(dtype=int), + flex_friction: wp.array(dtype=wp.vec3), + flex_margin: wp.array(dtype=float), + flex_radius: wp.array(dtype=float), + flex_shell: wp.array(dtype=int), + flex_shelldataadr: wp.array(dtype=int), + flex_shellnum: wp.array(dtype=int), flex_stiffness: wp.array2d(dtype=float), + flex_vert: wp.array(dtype=wp.vec3), flex_vertadr: wp.array(dtype=int), flex_vertbodyid: wp.array(dtype=int), + flex_vertflexid: wp.array(dtype=int), + flex_vertnum: wp.array(dtype=int), flexedge_J_colind: wp.array(dtype=int), flexedge_J_rowadr: wp.array(dtype=int), flexedge_J_rownnz: wp.array(dtype=int), @@ -2001,7 +2102,9 @@ def _step_shim( flexedge_length0: wp.array(dtype=float), geom_aabb: wp.array3d(dtype=wp.vec3), geom_bodyid: wp.array(dtype=int), + geom_conaffinity: wp.array(dtype=int), geom_condim: wp.array(dtype=int), + geom_contype: wp.array(dtype=int), geom_dataid: wp.array(dtype=int), geom_fluid: wp.array2d(dtype=float), geom_friction: wp.array2d(dtype=wp.vec3), @@ -2055,6 +2158,7 @@ def _step_shim( light_targetbodyid: wp.array(dtype=int), mapM2M: wp.array(dtype=int), mat_rgba: wp.array2d(dtype=wp.vec4), + max_ten_J_rownnz: int, mesh_face: wp.array(dtype=wp.vec3i), mesh_faceadr: wp.array(dtype=int), mesh_graph: wp.array(dtype=int), @@ -2077,6 +2181,7 @@ def _step_shim( mesh_vertadr: wp.array(dtype=int), mesh_vertnum: wp.array(dtype=int), nC: int, + nJten: int, nM: int, na: int, nacttrnbody: int, @@ -2087,6 +2192,7 @@ def _step_shim( nflex: int, nflexedge: int, nflexelem: int, + nflexshelldata: int, nflexvert: int, ngeom: int, ngravcomp: int, @@ -2122,7 +2228,9 @@ def _step_shim( pair_solref: wp.array2d(dtype=wp.vec2), pair_solreffriction: wp.array2d(dtype=wp.vec2), plugin: wp.array(dtype=int), - plugin_attr: wp.array(dtype=wp.vec3f), + plugin_attr: wp.array(dtype=mjwp_types.vec_pluginattr), + qLD_all_updates: wp.array(dtype=wp.vec3i), + qLD_level_offsets: wp.array(dtype=int), qLD_updates: tuple[wp.array(dtype=wp.vec3i), ...], qM_fullm_i: wp.array(dtype=int), qM_fullm_j: wp.array(dtype=int), @@ -2166,6 +2274,9 @@ def _step_shim( site_type: wp.array(dtype=int), taxel_sensorid: wp.array(dtype=int), taxel_vertadr: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_rownnz: wp.array(dtype=int), tendon_actfrclimited: wp.array(dtype=bool), tendon_actfrcrange: wp.array2d(dtype=wp.vec2), tendon_adr: wp.array(dtype=int), @@ -2226,6 +2337,7 @@ def _step_shim( naccdmax: int, naconmax: int, njmax: int, + njmax_nnz: int, act: wp.array2d(dtype=float), act_dot: wp.array2d(dtype=float), actuator_force: wp.array2d(dtype=float), @@ -2290,7 +2402,7 @@ def _step_shim( subtree_angmom: wp.array2d(dtype=wp.vec3), subtree_com: wp.array2d(dtype=wp.vec3), subtree_linvel: wp.array2d(dtype=wp.vec3), - ten_J: wp.array3d(dtype=float), + ten_J: wp.array2d(dtype=float), ten_length: wp.array2d(dtype=float), ten_velocity: wp.array2d(dtype=float), ten_wrapadr: wp.array2d(dtype=int), @@ -2310,6 +2422,7 @@ def _step_shim( contact__dim: wp.array(dtype=int), contact__dist: wp.array(dtype=float), contact__efc_address: wp.array2d(dtype=int), + contact__flex: wp.array(dtype=wp.vec2i), contact__frame: wp.array(dtype=wp.mat33), contact__friction: wp.array(dtype=mjwp_types.vec5), contact__geom: wp.array(dtype=wp.vec2i), @@ -2320,6 +2433,7 @@ def _step_shim( contact__solref: wp.array(dtype=wp.vec2), contact__solreffriction: wp.array(dtype=wp.vec2), contact__type: wp.array(dtype=int), + contact__vert: wp.array(dtype=wp.vec2i), contact__worldid: wp.array(dtype=int), efc__D: wp.array2d(dtype=float), efc__J: wp.array3d(dtype=float), @@ -2429,6 +2543,10 @@ def _step_shim( _m.eq_type = eq_type _m.eq_wld_adr = eq_wld_adr _m.flex_bending = flex_bending + _m.flex_centered = flex_centered + _m.flex_conaffinity = flex_conaffinity + _m.flex_condim = flex_condim + _m.flex_contype = flex_contype _m.flex_damping = flex_damping _m.flex_dim = flex_dim _m.flex_edge = flex_edge @@ -2437,12 +2555,22 @@ def _step_shim( _m.flex_edgenum = flex_edgenum _m.flex_elem = flex_elem _m.flex_elemadr = flex_elemadr + _m.flex_elemdataadr = flex_elemdataadr _m.flex_elemedge = flex_elemedge _m.flex_elemedgeadr = flex_elemedgeadr _m.flex_elemnum = flex_elemnum + _m.flex_friction = flex_friction + _m.flex_margin = flex_margin + _m.flex_radius = flex_radius + _m.flex_shell = flex_shell + _m.flex_shelldataadr = flex_shelldataadr + _m.flex_shellnum = flex_shellnum _m.flex_stiffness = flex_stiffness + _m.flex_vert = flex_vert _m.flex_vertadr = flex_vertadr _m.flex_vertbodyid = flex_vertbodyid + _m.flex_vertflexid = flex_vertflexid + _m.flex_vertnum = flex_vertnum _m.flexedge_J_colind = flexedge_J_colind _m.flexedge_J_rowadr = flexedge_J_rowadr _m.flexedge_J_rownnz = flexedge_J_rownnz @@ -2450,7 +2578,9 @@ def _step_shim( _m.flexedge_length0 = flexedge_length0 _m.geom_aabb = geom_aabb _m.geom_bodyid = geom_bodyid + _m.geom_conaffinity = geom_conaffinity _m.geom_condim = geom_condim + _m.geom_contype = geom_contype _m.geom_dataid = geom_dataid _m.geom_fluid = geom_fluid _m.geom_friction = geom_friction @@ -2504,6 +2634,7 @@ def _step_shim( _m.light_targetbodyid = light_targetbodyid _m.mapM2M = mapM2M _m.mat_rgba = mat_rgba + _m.max_ten_J_rownnz = max_ten_J_rownnz _m.mesh_face = mesh_face _m.mesh_faceadr = mesh_faceadr _m.mesh_graph = mesh_graph @@ -2526,6 +2657,7 @@ def _step_shim( _m.mesh_vertadr = mesh_vertadr _m.mesh_vertnum = mesh_vertnum _m.nC = nC + _m.nJten = nJten _m.nM = nM _m.na = na _m.nacttrnbody = nacttrnbody @@ -2536,6 +2668,7 @@ def _step_shim( _m.nflex = nflex _m.nflexedge = nflexedge _m.nflexelem = nflexelem + _m.nflexshelldata = nflexshelldata _m.nflexvert = nflexvert _m.ngeom = ngeom _m.ngravcomp = ngravcomp @@ -2599,6 +2732,8 @@ def _step_shim( _m.pair_solreffriction = pair_solreffriction _m.plugin = plugin _m.plugin_attr = plugin_attr + _m.qLD_all_updates = qLD_all_updates + _m.qLD_level_offsets = qLD_level_offsets _m.qLD_updates = qLD_updates _m.qM_fullm_i = qM_fullm_i _m.qM_fullm_j = qM_fullm_j @@ -2643,6 +2778,9 @@ def _step_shim( _m.stat.meaninertia = stat__meaninertia _m.taxel_sensorid = taxel_sensorid _m.taxel_vertadr = taxel_vertadr + _m.ten_J_colind = ten_J_colind + _m.ten_J_rowadr = ten_J_rowadr + _m.ten_J_rownnz = ten_J_rownnz _m.tendon_actfrclimited = tendon_actfrclimited _m.tendon_actfrcrange = tendon_actfrcrange _m.tendon_adr = tendon_adr @@ -2688,6 +2826,7 @@ def _step_shim( _d.contact.dim = contact__dim _d.contact.dist = contact__dist _d.contact.efc_address = contact__efc_address + _d.contact.flex = contact__flex _d.contact.frame = contact__frame _d.contact.friction = contact__friction _d.contact.geom = contact__geom @@ -2698,6 +2837,7 @@ def _step_shim( _d.contact.solref = contact__solref _d.contact.solreffriction = contact__solreffriction _d.contact.type = contact__type + _d.contact.vert = contact__vert _d.contact.worldid = contact__worldid _d.crb = crb _d.ctrl = ctrl @@ -2741,6 +2881,7 @@ def _step_shim( _d.nf = nf _d.nisland = nisland _d.njmax = njmax + _d.njmax_nnz = njmax_nnz _d.nl = nl _d.qLD = qLD _d.qLDiagInv = qLDiagInv @@ -2868,6 +3009,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): 'contact__dim': d._impl.contact__dim.shape, 'contact__dist': d._impl.contact__dist.shape, 'contact__efc_address': d._impl.contact__efc_address.shape, + 'contact__flex': d._impl.contact__flex.shape, 'contact__frame': d._impl.contact__frame.shape, 'contact__friction': d._impl.contact__friction.shape, 'contact__geom': d._impl.contact__geom.shape, @@ -2878,6 +3020,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): 'contact__solref': d._impl.contact__solref.shape, 'contact__solreffriction': d._impl.contact__solreffriction.shape, 'contact__type': d._impl.contact__type.shape, + 'contact__vert': d._impl.contact__vert.shape, 'contact__worldid': d._impl.contact__worldid.shape, 'efc__D': d._impl.efc__D.shape, 'efc__J': d._impl.efc__J.shape, @@ -2897,7 +3040,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): } jf = ffi.jax_callable_variadic_tuple( _step_shim, - num_outputs=104, + num_outputs=106, output_dims=output_dims, vmap_method=None, in_out_argnames=set([ @@ -2979,6 +3122,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): 'contact__dim', 'contact__dist', 'contact__efc_address', + 'contact__flex', 'contact__frame', 'contact__friction', 'contact__geom', @@ -2989,6 +3133,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): 'contact__solref', 'contact__solreffriction', 'contact__type', + 'contact__vert', 'contact__worldid', 'efc__D', 'efc__J', @@ -3275,6 +3420,10 @@ def _step_jax_impl(m: types.Model, d: types.Data): m.eq_type, m._impl.eq_wld_adr, m._impl.flex_bending, + m._impl.flex_centered, + m._impl.flex_conaffinity, + m._impl.flex_condim, + m._impl.flex_contype, m._impl.flex_damping, m._impl.flex_dim, m._impl.flex_edge, @@ -3283,12 +3432,22 @@ def _step_jax_impl(m: types.Model, d: types.Data): m._impl.flex_edgenum, m._impl.flex_elem, m._impl.flex_elemadr, + m._impl.flex_elemdataadr, m._impl.flex_elemedge, m._impl.flex_elemedgeadr, m._impl.flex_elemnum, + m._impl.flex_friction, + m._impl.flex_margin, + m._impl.flex_radius, + m._impl.flex_shell, + m._impl.flex_shelldataadr, + m._impl.flex_shellnum, m._impl.flex_stiffness, + m._impl.flex_vert, m._impl.flex_vertadr, m._impl.flex_vertbodyid, + m._impl.flex_vertflexid, + m._impl.flex_vertnum, m._impl.flexedge_J_colind, m._impl.flexedge_J_rowadr, m._impl.flexedge_J_rownnz, @@ -3296,7 +3455,9 @@ def _step_jax_impl(m: types.Model, d: types.Data): m._impl.flexedge_length0, m.geom_aabb, m.geom_bodyid, + m.geom_conaffinity, m.geom_condim, + m.geom_contype, m.geom_dataid, m.geom_fluid, m.geom_friction, @@ -3350,6 +3511,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): m._impl.light_targetbodyid, m._impl.mapM2M, m.mat_rgba, + m._impl.max_ten_J_rownnz, m.mesh_face, m.mesh_faceadr, m.mesh_graph, @@ -3372,6 +3534,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): m.mesh_vertadr, m.mesh_vertnum, m.nC, + m.nJten, m.nM, m.na, m._impl.nacttrnbody, @@ -3382,6 +3545,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): m._impl.nflex, m._impl.nflexedge, m._impl.nflexelem, + m._impl.nflexshelldata, m._impl.nflexvert, m.ngeom, m.ngravcomp, @@ -3418,6 +3582,8 @@ def _step_jax_impl(m: types.Model, d: types.Data): m.pair_solreffriction, m._impl.plugin, m._impl.plugin_attr, + m._impl.qLD_all_updates, + m._impl.qLD_level_offsets, m._impl.qLD_updates, m._impl.qM_fullm_i, m._impl.qM_fullm_j, @@ -3461,6 +3627,9 @@ def _step_jax_impl(m: types.Model, d: types.Data): m.site_type, m._impl.taxel_sensorid, m._impl.taxel_vertadr, + m._impl.ten_J_colind, + m._impl.ten_J_rowadr, + m._impl.ten_J_rownnz, m.tendon_actfrclimited, m.tendon_actfrcrange, m.tendon_adr, @@ -3520,6 +3689,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): d._impl.naccdmax, d._impl.naconmax, d._impl.njmax, + d._impl.njmax_nnz, d.act, d.act_dot, d.actuator_force, @@ -3604,6 +3774,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): d._impl.contact__dim, d._impl.contact__dist, d._impl.contact__efc_address, + d._impl.contact__flex, d._impl.contact__frame, d._impl.contact__friction, d._impl.contact__geom, @@ -3614,6 +3785,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): d._impl.contact__solref, d._impl.contact__solreffriction, d._impl.contact__type, + d._impl.contact__vert, d._impl.contact__worldid, d._impl.efc__D, d._impl.efc__J, @@ -3710,32 +3882,34 @@ def _step_jax_impl(m: types.Model, d: types.Data): '_impl.contact__dim': out[75], '_impl.contact__dist': out[76], '_impl.contact__efc_address': out[77], - '_impl.contact__frame': out[78], - '_impl.contact__friction': out[79], - '_impl.contact__geom': out[80], - '_impl.contact__geomcollisionid': out[81], - '_impl.contact__includemargin': out[82], - '_impl.contact__pos': out[83], - '_impl.contact__solimp': out[84], - '_impl.contact__solref': out[85], - '_impl.contact__solreffriction': out[86], - '_impl.contact__type': out[87], - '_impl.contact__worldid': out[88], - '_impl.efc__D': out[89], - '_impl.efc__J': out[90], - '_impl.efc__J_colind': out[91], - '_impl.efc__J_rowadr': out[92], - '_impl.efc__J_rownnz': out[93], - '_impl.efc__Ma': out[94], - '_impl.efc__aref': out[95], - '_impl.efc__force': out[96], - '_impl.efc__frictionloss': out[97], - '_impl.efc__id': out[98], - '_impl.efc__margin': out[99], - '_impl.efc__pos': out[100], - '_impl.efc__state': out[101], - '_impl.efc__type': out[102], - '_impl.efc__vel': out[103], + '_impl.contact__flex': out[78], + '_impl.contact__frame': out[79], + '_impl.contact__friction': out[80], + '_impl.contact__geom': out[81], + '_impl.contact__geomcollisionid': out[82], + '_impl.contact__includemargin': out[83], + '_impl.contact__pos': out[84], + '_impl.contact__solimp': out[85], + '_impl.contact__solref': out[86], + '_impl.contact__solreffriction': out[87], + '_impl.contact__type': out[88], + '_impl.contact__vert': out[89], + '_impl.contact__worldid': out[90], + '_impl.efc__D': out[91], + '_impl.efc__J': out[92], + '_impl.efc__J_colind': out[93], + '_impl.efc__J_rowadr': out[94], + '_impl.efc__J_rownnz': out[95], + '_impl.efc__Ma': out[96], + '_impl.efc__aref': out[97], + '_impl.efc__force': out[98], + '_impl.efc__frictionloss': out[99], + '_impl.efc__id': out[100], + '_impl.efc__margin': out[101], + '_impl.efc__pos': out[102], + '_impl.efc__state': out[103], + '_impl.efc__type': out[104], + '_impl.efc__vel': out[105], }) return d diff --git a/mjx/mujoco/mjx/warp/forward_test.py b/mjx/mujoco/mjx/warp/forward_test.py index 15e874e3..b80c921d 100644 --- a/mjx/mujoco/mjx/warp/forward_test.py +++ b/mjx/mujoco/mjx/warp/forward_test.py @@ -157,7 +157,16 @@ class ForwardTest(parameterized.TestCase): m.ten_J_rowadr, m.ten_J_colind, ) - tu.assert_eq(dx._impl.ten_J, ten_J, 'ten_J') + # convert sparse warp ten_J to dense representation + warp_ten_J = np.zeros((m.ntendon, m.nv)) + mujoco.mju_sparse2dense( + warp_ten_J, + np.asarray(dx._impl.ten_J), + mx._impl.ten_J_rownnz, + mx._impl.ten_J_rowadr, + mx._impl.ten_J_colind, + ) + tu.assert_eq(warp_ten_J, ten_J, 'ten_J') tu.assert_attr_eq(dx._impl, d, 'ten_wrapadr') tu.assert_attr_eq(dx._impl, d, 'ten_wrapnum') tu.assert_attr_eq(dx._impl, d, 'wrap_xpos') diff --git a/mjx/mujoco/mjx/warp/render.py b/mjx/mujoco/mjx/warp/render.py index 0bab51a3..e723e6b0 100644 --- a/mjx/mujoco/mjx/warp/render.py +++ b/mjx/mujoco/mjx/warp/render.py @@ -48,6 +48,7 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) + @ffi.format_args_for_warp def _render_shim( # Model @@ -56,6 +57,9 @@ def _render_shim( cam_intrinsic: wp.array2d(dtype=wp.vec4), cam_projection: wp.array(dtype=int), cam_sensorsize: wp.array(dtype=wp.vec2), + flex_edge: wp.array(dtype=wp.vec2i), + flex_radius: wp.array(dtype=float), + flex_vertadr: wp.array(dtype=int), geom_dataid: wp.array(dtype=int), geom_matid: wp.array2d(dtype=int), geom_rgba: wp.array2d(dtype=wp.vec4), @@ -68,11 +72,11 @@ def _render_shim( mat_texid: wp.array3d(dtype=int), mat_texrepeat: wp.array2d(dtype=wp.vec2), mesh_faceadr: wp.array(dtype=int), - nflex: int, nlight: int, # Data cam_xmat: wp.array2d(dtype=wp.mat33), cam_xpos: wp.array2d(dtype=wp.vec3), + flexvert_xpos: wp.array2d(dtype=wp.vec3), geom_xmat: wp.array2d(dtype=wp.mat33), geom_xpos: wp.array2d(dtype=wp.vec3), light_xdir: wp.array2d(dtype=wp.vec3), @@ -91,6 +95,9 @@ def _render_shim( _m.cam_intrinsic = cam_intrinsic _m.cam_projection = cam_projection _m.cam_sensorsize = cam_sensorsize + _m.flex_edge = flex_edge + _m.flex_radius = flex_radius + _m.flex_vertadr = flex_vertadr _m.geom_dataid = geom_dataid _m.geom_matid = geom_matid _m.geom_rgba = geom_rgba @@ -103,10 +110,10 @@ def _render_shim( _m.mat_texid = mat_texid _m.mat_texrepeat = mat_texrepeat _m.mesh_faceadr = mesh_faceadr - _m.nflex = nflex _m.nlight = nlight _d.cam_xmat = cam_xmat _d.cam_xpos = cam_xpos + _d.flexvert_xpos = flexvert_xpos _d.geom_xmat = geom_xmat _d.geom_xpos = geom_xpos _d.light_xdir = light_xdir @@ -155,6 +162,9 @@ def _render_jax_impl(m: types.Model, d: types.Data, ctx: RenderContextPytree): m.cam_intrinsic, m._impl.cam_projection, m.cam_sensorsize, + m._impl.flex_edge, + m._impl.flex_radius, + m._impl.flex_vertadr, m.geom_dataid, m.geom_matid, m.geom_rgba, @@ -167,10 +177,10 @@ def _render_jax_impl(m: types.Model, d: types.Data, ctx: RenderContextPytree): m.mat_texid, m._impl.mat_texrepeat, m.mesh_faceadr, - m._impl.nflex, m.nlight, d.cam_xmat, d.cam_xpos, + d._impl.flexvert_xpos, d.geom_xmat, d.geom_xpos, d._impl.light_xdir, diff --git a/mjx/mujoco/mjx/warp/smooth.py b/mjx/mujoco/mjx/warp/smooth.py index eeef2d92..3fe3a712 100644 --- a/mjx/mujoco/mjx/warp/smooth.py +++ b/mjx/mujoco/mjx/warp/smooth.py @@ -297,17 +297,20 @@ def kinematics_vmap( def _tendon_shim( # Model nworld: int, + body_dofadr: wp.array(dtype=int), + body_dofnum: wp.array(dtype=int), body_parentid: wp.array(dtype=int), body_rootid: wp.array(dtype=int), - dof_bodyid: wp.array(dtype=int), geom_bodyid: wp.array(dtype=int), geom_size: wp.array2d(dtype=wp.vec3), jnt_dofadr: wp.array(dtype=int), jnt_qposadr: wp.array(dtype=int), ntendon: int, - nv: int, nwrap: int, site_bodyid: wp.array(dtype=int), + ten_J_colind: wp.array(dtype=int), + ten_J_rowadr: wp.array(dtype=int), + ten_J_rownnz: wp.array(dtype=int), tendon_adr: wp.array(dtype=int), tendon_geom_adr: wp.array(dtype=int), tendon_jnt_adr: wp.array(dtype=int), @@ -327,7 +330,7 @@ def _tendon_shim( qpos: wp.array2d(dtype=float), site_xpos: wp.array2d(dtype=wp.vec3), subtree_com: wp.array2d(dtype=wp.vec3), - ten_J: wp.array3d(dtype=float), + ten_J: wp.array2d(dtype=float), ten_length: wp.array2d(dtype=float), ten_wrapadr: wp.array2d(dtype=int), ten_wrapnum: wp.array2d(dtype=int), @@ -339,17 +342,20 @@ def _tendon_shim( _m.callback = _cb _d.efc = _e _d.contact = _c + _m.body_dofadr = body_dofadr + _m.body_dofnum = body_dofnum _m.body_parentid = body_parentid _m.body_rootid = body_rootid - _m.dof_bodyid = dof_bodyid _m.geom_bodyid = geom_bodyid _m.geom_size = geom_size _m.jnt_dofadr = jnt_dofadr _m.jnt_qposadr = jnt_qposadr _m.ntendon = ntendon - _m.nv = nv _m.nwrap = nwrap _m.site_bodyid = site_bodyid + _m.ten_J_colind = ten_J_colind + _m.ten_J_rowadr = ten_J_rowadr + _m.ten_J_rownnz = ten_J_rownnz _m.tendon_adr = tendon_adr _m.tendon_geom_adr = tendon_geom_adr _m.tendon_jnt_adr = tendon_jnt_adr @@ -416,17 +422,20 @@ def _tendon_jax_impl(m: types.Model, d: types.Data): ) out = jf( d.qpos.shape[0], + m.body_dofadr, + m.body_dofnum, m.body_parentid, m.body_rootid, - m.dof_bodyid, m.geom_bodyid, m.geom_size, m.jnt_dofadr, m.jnt_qposadr, m.ntendon, - m.nv, m.nwrap, m.site_bodyid, + m._impl.ten_J_colind, + m._impl.ten_J_rowadr, + m._impl.ten_J_rownnz, m.tendon_adr, m._impl.tendon_geom_adr, m._impl.tendon_jnt_adr, diff --git a/mjx/mujoco/mjx/warp/types.py b/mjx/mujoco/mjx/warp/types.py index ff85189f..ef75edc4 100644 --- a/mjx/mujoco/mjx/warp/types.py +++ b/mjx/mujoco/mjx/warp/types.py @@ -83,7 +83,7 @@ class BlockDim: qderiv_actuator_dense: int ray: int segmented_sort: int - tendon_velocity: int + solve_LD_sparse_fused: int update_gradient_JTDAJ_dense: int update_gradient_JTDAJ_sparse: int update_gradient_cholesky: int @@ -141,6 +141,10 @@ class ModelWarp(PyTreeNode): eq_ten_adr: np.ndarray eq_wld_adr: np.ndarray flex_bending: np.ndarray + flex_centered: np.ndarray + flex_conaffinity: np.ndarray + flex_condim: np.ndarray + flex_contype: np.ndarray flex_damping: np.ndarray flex_dim: np.ndarray flex_edge: np.ndarray @@ -149,12 +153,21 @@ class ModelWarp(PyTreeNode): flex_edgenum: np.ndarray flex_elem: np.ndarray flex_elemadr: np.ndarray + flex_elemdataadr: np.ndarray flex_elemedge: np.ndarray flex_elemedgeadr: np.ndarray flex_elemnum: np.ndarray + flex_friction: np.ndarray + flex_margin: np.ndarray + flex_radius: np.ndarray + flex_shell: np.ndarray + flex_shelldataadr: np.ndarray + flex_shellnum: np.ndarray flex_stiffness: np.ndarray + flex_vert: np.ndarray flex_vertadr: np.ndarray flex_vertbodyid: np.ndarray + flex_vertflexid: np.ndarray flex_vertnum: np.ndarray flexedge_J_colind: np.ndarray flexedge_J_rowadr: np.ndarray @@ -173,6 +186,7 @@ class ModelWarp(PyTreeNode): light_targetbodyid: np.ndarray mapM2M: np.ndarray mat_texrepeat: jax.Array + max_ten_J_rownnz: int mesh_polyadr: np.ndarray mesh_polymap: np.ndarray mesh_polymapadr: np.ndarray @@ -191,6 +205,7 @@ class ModelWarp(PyTreeNode): nflexelem: int nflexelemdata: int nflexelemedge: int + nflexshelldata: int nflexvert: int nmaxcondim: int nmaxmeshdeg: int @@ -213,6 +228,8 @@ class ModelWarp(PyTreeNode): oct_coeff: np.ndarray plugin: np.ndarray plugin_attr: np.ndarray + qLD_all_updates: np.ndarray + qLD_level_offsets: np.ndarray qLD_updates: Tuple[np.ndarray, ...] qM_fullm_i: np.ndarray qM_fullm_j: np.ndarray @@ -240,6 +257,9 @@ class ModelWarp(PyTreeNode): sensor_vel_adr: np.ndarray taxel_sensorid: np.ndarray taxel_vertadr: np.ndarray + ten_J_colind: np.ndarray + ten_J_rowadr: np.ndarray + ten_J_rownnz: np.ndarray ten_wrapadr_site: np.ndarray ten_wrapnum_site: np.ndarray tendon_geom_adr: np.ndarray @@ -266,6 +286,7 @@ class DataWarp(PyTreeNode): contact__dim: jax.Array contact__dist: jax.Array contact__efc_address: jax.Array + contact__flex: jax.Array contact__frame: jax.Array contact__friction: jax.Array contact__geom: jax.Array @@ -276,6 +297,7 @@ class DataWarp(PyTreeNode): contact__solref: jax.Array contact__solreffriction: jax.Array contact__type: jax.Array + contact__vert: jax.Array contact__worldid: jax.Array crb: jax.Array efc__D: jax.Array @@ -312,6 +334,7 @@ class DataWarp(PyTreeNode): nf: jax.Array nisland: jax.Array njmax: int + njmax_nnz: int njmax_pad: int nl: jax.Array nworld: int @@ -335,6 +358,7 @@ DATA_NON_VMAP = { 'contact__dim', 'contact__dist', 'contact__efc_address', + 'contact__flex', 'contact__frame', 'contact__friction', 'contact__geom', @@ -345,12 +369,14 @@ DATA_NON_VMAP = { 'contact__solref', 'contact__solreffriction', 'contact__type', + 'contact__vert', 'contact__worldid', 'naccdmax', 'nacon', 'naconmax', 'ncollision', 'njmax', + 'njmax_nnz', 'njmax_pad', 'nworld', } @@ -398,6 +424,7 @@ _NDIM = { 'contact__dim': 1, 'contact__dist': 1, 'contact__efc_address': 2, + 'contact__flex': 2, 'contact__frame': 3, 'contact__friction': 2, 'contact__geom': 2, @@ -408,6 +435,7 @@ _NDIM = { 'contact__solref': 2, 'contact__solreffriction': 2, 'contact__type': 1, + 'contact__vert': 2, 'contact__worldid': 1, 'crb': 3, 'ctrl': 2, @@ -451,6 +479,7 @@ _NDIM = { 'nf': 1, 'nisland': 1, 'njmax': 0, + 'njmax_nnz': 0, 'njmax_pad': 0, 'nl': 1, 'nworld': 0, @@ -480,7 +509,7 @@ _NDIM = { 'subtree_angmom': 3, 'subtree_com': 3, 'subtree_linvel': 3, - 'ten_J': 3, + 'ten_J': 2, 'ten_length': 2, 'ten_velocity': 2, 'ten_wrapadr': 2, @@ -535,7 +564,7 @@ _NDIM = { 'block_dim__qderiv_actuator_dense': 0, 'block_dim__ray': 0, 'block_dim__segmented_sort': 0, - 'block_dim__tendon_velocity': 0, + 'block_dim__solve_LD_sparse_fused': 0, 'block_dim__update_gradient_JTDAJ_dense': 0, 'block_dim__update_gradient_JTDAJ_sparse': 0, 'block_dim__update_gradient_cholesky': 0, @@ -608,6 +637,10 @@ _NDIM = { 'eq_wld_adr': 1, 'exclude_signature': 1, 'flex_bending': 2, + 'flex_centered': 1, + 'flex_conaffinity': 1, + 'flex_condim': 1, + 'flex_contype': 1, 'flex_damping': 1, 'flex_dim': 1, 'flex_edge': 2, @@ -616,12 +649,21 @@ _NDIM = { 'flex_edgenum': 1, 'flex_elem': 1, 'flex_elemadr': 1, + 'flex_elemdataadr': 1, 'flex_elemedge': 1, 'flex_elemedgeadr': 1, 'flex_elemnum': 1, + 'flex_friction': 2, + 'flex_margin': 1, + 'flex_radius': 1, + 'flex_shell': 1, + 'flex_shelldataadr': 1, + 'flex_shellnum': 1, 'flex_stiffness': 2, + 'flex_vert': 2, 'flex_vertadr': 1, 'flex_vertbodyid': 1, + 'flex_vertflexid': 1, 'flex_vertnum': 1, 'flexedge_J_colind': 1, 'flexedge_J_rowadr': 1, @@ -692,6 +734,7 @@ _NDIM = { 'mat_rgba': 3, 'mat_texid': 3, 'mat_texrepeat': 3, + 'max_ten_J_rownnz': 0, 'mesh_face': 2, 'mesh_faceadr': 1, 'mesh_graph': 1, @@ -717,6 +760,7 @@ _NDIM = { 'nC': 0, 'nJfe': 0, 'nJmom': 0, + 'nJten': 0, 'nM': 0, 'na': 0, 'nacttrnbody': 0, @@ -730,6 +774,7 @@ _NDIM = { 'nflexelem': 0, 'nflexelemdata': 0, 'nflexelemedge': 0, + 'nflexshelldata': 0, 'nflexvert': 0, 'ngeom': 0, 'ngravcomp': 0, @@ -811,6 +856,8 @@ _NDIM = { 'pair_solreffriction': 3, 'plugin': 1, 'plugin_attr': 2, + 'qLD_all_updates': 2, + 'qLD_level_offsets': 1, 'qLD_updates': -1, 'qM_fullm_i': 1, 'qM_fullm_j': 1, @@ -856,6 +903,9 @@ _NDIM = { 'stat__meaninertia': 1, 'taxel_sensorid': 1, 'taxel_vertadr': 1, + 'ten_J_colind': 1, + 'ten_J_rowadr': 1, + 'ten_J_rownnz': 1, 'ten_wrapadr_site': 1, 'ten_wrapnum_site': 1, 'tendon_actfrclimited': 1, @@ -940,6 +990,7 @@ _BATCH_DIM = { 'contact__dim': False, 'contact__dist': False, 'contact__efc_address': False, + 'contact__flex': False, 'contact__frame': False, 'contact__friction': False, 'contact__geom': False, @@ -950,6 +1001,7 @@ _BATCH_DIM = { 'contact__solref': False, 'contact__solreffriction': False, 'contact__type': False, + 'contact__vert': False, 'contact__worldid': False, 'crb': True, 'ctrl': True, @@ -993,6 +1045,7 @@ _BATCH_DIM = { 'nf': True, 'nisland': True, 'njmax': False, + 'njmax_nnz': False, 'njmax_pad': False, 'nl': True, 'nworld': False, @@ -1077,7 +1130,7 @@ _BATCH_DIM = { 'block_dim__qderiv_actuator_dense': False, 'block_dim__ray': False, 'block_dim__segmented_sort': False, - 'block_dim__tendon_velocity': False, + 'block_dim__solve_LD_sparse_fused': False, 'block_dim__update_gradient_JTDAJ_dense': False, 'block_dim__update_gradient_JTDAJ_sparse': False, 'block_dim__update_gradient_cholesky': False, @@ -1150,6 +1203,10 @@ _BATCH_DIM = { 'eq_wld_adr': False, 'exclude_signature': False, 'flex_bending': False, + 'flex_centered': False, + 'flex_conaffinity': False, + 'flex_condim': False, + 'flex_contype': False, 'flex_damping': False, 'flex_dim': False, 'flex_edge': False, @@ -1158,12 +1215,21 @@ _BATCH_DIM = { 'flex_edgenum': False, 'flex_elem': False, 'flex_elemadr': False, + 'flex_elemdataadr': False, 'flex_elemedge': False, 'flex_elemedgeadr': False, 'flex_elemnum': False, + 'flex_friction': False, + 'flex_margin': False, + 'flex_radius': False, + 'flex_shell': False, + 'flex_shelldataadr': False, + 'flex_shellnum': False, 'flex_stiffness': False, + 'flex_vert': False, 'flex_vertadr': False, 'flex_vertbodyid': False, + 'flex_vertflexid': False, 'flex_vertnum': False, 'flexedge_J_colind': False, 'flexedge_J_rowadr': False, @@ -1234,6 +1300,7 @@ _BATCH_DIM = { 'mat_rgba': True, 'mat_texid': True, 'mat_texrepeat': True, + 'max_ten_J_rownnz': False, 'mesh_face': False, 'mesh_faceadr': False, 'mesh_graph': False, @@ -1259,6 +1326,7 @@ _BATCH_DIM = { 'nC': False, 'nJfe': False, 'nJmom': False, + 'nJten': False, 'nM': False, 'na': False, 'nacttrnbody': False, @@ -1272,6 +1340,7 @@ _BATCH_DIM = { 'nflexelem': False, 'nflexelemdata': False, 'nflexelemedge': False, + 'nflexshelldata': False, 'nflexvert': False, 'ngeom': False, 'ngravcomp': False, @@ -1353,6 +1422,8 @@ _BATCH_DIM = { 'pair_solreffriction': True, 'plugin': False, 'plugin_attr': False, + 'qLD_all_updates': False, + 'qLD_level_offsets': False, 'qLD_updates': False, 'qM_fullm_i': False, 'qM_fullm_j': False, @@ -1398,6 +1469,9 @@ _BATCH_DIM = { 'stat__meaninertia': True, 'taxel_sensorid': False, 'taxel_vertadr': False, + 'ten_J_colind': False, + 'ten_J_rowadr': False, + 'ten_J_rownnz': False, 'ten_wrapadr_site': False, 'ten_wrapnum_site': False, 'tendon_actfrclimited': False, From db011b5100cae88c12444a05ce0b277a9696144a Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Thu, 2 Apr 2026 04:42:11 -0700 Subject: [PATCH 010/251] Add condition to team notification of github actions that it's from 'main' PiperOrigin-RevId: 893437916 Change-Id: I0c0b22747e4e4cac23ad0f44cf8b667863cc79ae --- .github/workflows/build.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index f2c06805..1a207fd9 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -347,7 +347,7 @@ jobs: CHATMSG_AUTHOR_EMAIL: ${{ github.event.head_commit.author.email }} CHATMSG_COMMIT_MESSAGE: ${{ github.event.head_commit.message }} CHATMSG_JOB_ID: ${{ matrix.label }} - if: ${{ failure() && github.event_name == 'push' && env.GCHAT_API_URL != '' }} + if: failure() && github.ref_name == 'main' && github.event_name == 'push' && env.GCHAT_API_URL != '' run: bash ./.github/workflows/build_steps.sh notify_team_chat # This job quickly determines if MuJoCo Studio is broken. @@ -406,7 +406,7 @@ jobs: CHATMSG_AUTHOR_EMAIL: ${{ github.event.head_commit.author.email }} CHATMSG_COMMIT_MESSAGE: ${{ github.event.head_commit.message }} CHATMSG_JOB_ID: ${{ matrix.label }} - if: ${{ failure() && github.event_name == 'push' && env.GCHAT_API_URL != '' }} + if: failure() && github.ref_name == 'main' && github.event_name == 'push' && env.GCHAT_API_URL != '' run: bash ./.github/workflows/build_steps.sh notify_team_chat @@ -451,5 +451,5 @@ jobs: CHATMSG_AUTHOR_EMAIL: ${{ github.event.head_commit.author.email }} CHATMSG_COMMIT_MESSAGE: ${{ github.event.head_commit.message }} CHATMSG_JOB_ID: ${{ env.label }} - if: ${{ failure() && github.event_name == 'push' && env.GCHAT_API_URL != '' }} + if: failure() && github.ref_name == 'main' && github.event_name == 'push' && env.GCHAT_API_URL != '' run: bash ./.github/workflows/build_steps.sh notify_team_chat From 0e3306b62dbd9aa8f287e70422ea1634e1121c1c Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 3 Apr 2026 00:12:46 -0700 Subject: [PATCH 011/251] Add useful flex arrays to MJX API. This enables using name2id for flexes. PiperOrigin-RevId: 893923892 Change-Id: I7c2a71791e26fa2280cfa75fd66db328f4599b0b --- mjx/mujoco/mjx/_src/support.py | 2 ++ mjx/mujoco/mjx/_src/types.py | 5 +++++ 2 files changed, 7 insertions(+) diff --git a/mjx/mujoco/mjx/_src/support.py b/mjx/mujoco/mjx/_src/support.py index 24031821..ee2e0e40 100644 --- a/mjx/mujoco/mjx/_src/support.py +++ b/mjx/mujoco/mjx/_src/support.py @@ -248,6 +248,7 @@ def _getnum(m: Union[Model, mujoco.MjModel], obj: mujoco._enums.mjtObj) -> int: mujoco.mjtObj.mjOBJ_NUMERIC: m.nnumeric, mujoco.mjtObj.mjOBJ_TUPLE: m.ntuple, mujoco.mjtObj.mjOBJ_KEY: m.nkey, + mujoco.mjtObj.mjOBJ_FLEX: m.nflex, }.get(obj, 0) @@ -271,6 +272,7 @@ def _getadr( mujoco.mjtObj.mjOBJ_NUMERIC: m.name_numericadr, mujoco.mjtObj.mjOBJ_TUPLE: m.name_tupleadr, mujoco.mjtObj.mjOBJ_KEY: m.name_keyadr, + mujoco.mjtObj.mjOBJ_FLEX: m.name_flexadr, }[obj] diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index f4cda34c..8bc60bd2 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -611,6 +611,7 @@ class Model(PyTreeNode): nsite: int ncam: int nlight: int + nflex: int nmesh: int nmeshvert: int nmeshnormal: int @@ -774,6 +775,9 @@ class Model(PyTreeNode): mesh_texcoordadr: np.ndarray mesh_texcoordnum: np.ndarray mesh_texcoord: np.ndarray + flex_vertadr: np.ndarray + flex_vertnum: np.ndarray + flex_vert0: np.ndarray hfield_size: np.ndarray hfield_nrow: np.ndarray hfield_ncol: np.ndarray @@ -881,6 +885,7 @@ class Model(PyTreeNode): name_geomadr: np.ndarray name_siteadr: np.ndarray name_camadr: np.ndarray + name_flexadr: np.ndarray name_meshadr: np.ndarray name_hfieldadr: np.ndarray name_pairadr: np.ndarray From 28d8ddcbebeee137c5fdd282c1959571f502a1a9 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 7 Apr 2026 03:24:09 -0700 Subject: [PATCH 012/251] Rename FilamentBuffers class to Mesh. Update the Mesh class so that it manages the lifetime of the vertex and index buffers. PiperOrigin-RevId: 895794022 Change-Id: I5eda578b8b7ed7f918e792f6181ec724729b5215 --- .../filament/filament/buffer_util.h | 57 +++++++++-- .../filament/filament/builtins.cc | 87 ++++++++--------- src/experimental/filament/filament/builtins.h | 20 ++-- .../filament/filament/drawable.cc | 19 ++-- .../filament/filament/geom_util.cc | 18 ++-- .../filament/filament/geom_util.h | 5 +- .../filament/filament/gui_view.cc | 35 +++---- src/experimental/filament/filament/gui_view.h | 2 +- .../filament/filament/model_objects.cc | 76 ++++++--------- .../filament/filament/model_objects.h | 14 +-- .../filament/filament/renderables.cc | 95 +++++++++---------- .../filament/filament/renderables.h | 31 +++--- 12 files changed, 238 insertions(+), 221 deletions(-) diff --git a/src/experimental/filament/filament/buffer_util.h b/src/experimental/filament/filament/buffer_util.h index b6167ff8..5b5c576b 100644 --- a/src/experimental/filament/filament/buffer_util.h +++ b/src/experimental/filament/filament/buffer_util.h @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -30,15 +31,59 @@ // Functions for creating filament vertex and index buffers. namespace mujoco { -// Simple tuple-type of a IndexBuffer+VertexBuffer. -struct FilamentBuffers { - filament::IndexBuffer* index_buffer = nullptr; - filament::VertexBuffer* vertex_buffer = nullptr; - std::optional bounds = std::nullopt; - filament::RenderableManager::PrimitiveType type = +// Owns a Vertex and Index buffer representing a geometry mesh. +class Mesh { + public: + Mesh(filament::Engine* engine, filament::IndexBuffer* index_buffer, + filament::VertexBuffer* vertex_buffer, + std::optional bounds = std::nullopt, + filament::RenderableManager::PrimitiveType type = + filament::RenderableManager::PrimitiveType::TRIANGLES) + : engine_(engine), + index_buffer_(index_buffer), + vertex_buffer_(vertex_buffer), + type_(type), + bounds_(bounds) {} + + ~Mesh() { + if (index_buffer_) { + engine_->destroy(index_buffer_); + } + if (vertex_buffer_) { + engine_->destroy(vertex_buffer_); + } + } + + filament::IndexBuffer* GetFilamentIndexBuffer() const { + return index_buffer_; + } + filament::VertexBuffer* GetFilamentVertexBuffer() const { + return vertex_buffer_; + } + filament::RenderableManager::PrimitiveType GetPrimitiveType() const { + return type_; + } + bool HasBounds() const { + return bounds_.has_value(); + } + filament::Box GetBounds() const { + return bounds_.value(); + } + + Mesh(const Mesh&) = delete; + Mesh& operator=(const Mesh&) = delete; + + private: + filament::Engine* engine_ = nullptr; + filament::IndexBuffer* index_buffer_ = nullptr; + filament::VertexBuffer* vertex_buffer_ = nullptr; + filament::RenderableManager::PrimitiveType type_ = filament::RenderableManager::PrimitiveType::TRIANGLES; + std::optional bounds_; }; +using MeshPtr = std::unique_ptr; + // Function that fills in the given buffer with actual data. using FillBufferFn = std::function; diff --git a/src/experimental/filament/filament/builtins.cc b/src/experimental/filament/filament/builtins.cc index 825aa953..3e4732e4 100644 --- a/src/experimental/filament/filament/builtins.cc +++ b/src/experimental/filament/filament/builtins.cc @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -422,8 +423,8 @@ class ConeBuilder { int idx = 0; for (int j = 0; j < num_slices_; ++j) { - const float angle1 = (j+0) * delta_angle; - const float angle2 = (j+1) * delta_angle; + const float angle1 = (j + 0) * delta_angle; + const float angle2 = (j + 1) * delta_angle; ptr[idx++] = MakeVert(angle1, delta_radius); ptr[idx++] = MakeVert(angle2, delta_radius); @@ -436,12 +437,12 @@ class ConeBuilder { // the rest: use quads for (int i = 1; i < num_stacks_; ++i) { - const float radius1 = delta_radius * (i+0); - const float radius2 = delta_radius * (i+1); + const float radius1 = delta_radius * (i + 0); + const float radius2 = delta_radius * (i + 1); for (int j = 0; j < num_slices_; ++j) { - const float angle1 = (j+0) * delta_angle; - const float angle2 = (j+1) * delta_angle; + const float angle1 = (j + 0) * delta_angle; + const float angle2 = (j + 1) * delta_angle; ptr[idx++] = MakeVert(angle1, radius2); ptr[idx++] = MakeVert(angle2, radius2); @@ -676,34 +677,34 @@ class DomeBuilder { (num_quads_body * kNumIndicesPerQuad); } - void GenerateVertices(VertexType* ptr, size_t num) const { - const float lat_angle_delta = - 0.5 * std::numbers::pi / static_cast(num_stacks_); - const float lon_angle_delta = - 2.0 * std::numbers::pi / static_cast(num_slices_); + void GenerateVertices(VertexType* ptr, size_t num) const { + const float lat_angle_delta = + 0.5 * std::numbers::pi / static_cast(num_stacks_); + const float lon_angle_delta = + 2.0 * std::numbers::pi / static_cast(num_slices_); - // Add the pole. - int idx = 0; - ptr[idx++] = MakeVert(0, 0, 1); + // Add the pole. + int idx = 0; + ptr[idx++] = MakeVert(0, 0, 1); - // Vertices by latitude. - for (int lat = 0; lat < num_stacks_; ++lat) { - // +1 because we handle the north pole (which would be at a lat angle of - // 0-degrees) explicitly. - const float lat_angle = static_cast(lat + 1) * lat_angle_delta; - const float cos_lat_angle = std::cos(lat_angle); - const float sin_lat_angle = std::sin(lat_angle); - const float z = cos_lat_angle; + // Vertices by latitude. + for (int lat = 0; lat < num_stacks_; ++lat) { + // +1 because we handle the north pole (which would be at a lat angle of + // 0-degrees) explicitly. + const float lat_angle = static_cast(lat + 1) * lat_angle_delta; + const float cos_lat_angle = std::cos(lat_angle); + const float sin_lat_angle = std::sin(lat_angle); + const float z = cos_lat_angle; - for (int lon = 0; lon < num_slices_; ++lon) { - const float lon_angle = static_cast(lon) * lon_angle_delta; - const float cos_lon_angle = std::cos(lon_angle); - const float sin_lon_angle = std::sin(lon_angle); + for (int lon = 0; lon < num_slices_; ++lon) { + const float lon_angle = static_cast(lon) * lon_angle_delta; + const float cos_lon_angle = std::cos(lon_angle); + const float sin_lon_angle = std::sin(lon_angle); - const float x = sin_lat_angle * cos_lon_angle; - const float y = sin_lat_angle * sin_lon_angle; - ptr[idx++] = MakeVert(x, y, z); - } + const float x = sin_lat_angle * cos_lon_angle; + const float y = sin_lat_angle * sin_lon_angle; + ptr[idx++] = MakeVert(x, y, z); + } } } @@ -756,9 +757,8 @@ class DomeBuilder { int num_slices_; }; - template -FilamentBuffers CreateFromBuilder(filament::Engine* engine, const T& builder) { +MeshPtr CreateFromBuilder(filament::Engine* engine, const T& builder) { using VertexType = typename T::VertexType; using IndexType = typename T::IndexType; @@ -786,46 +786,47 @@ FilamentBuffers CreateFromBuilder(filament::Engine* engine, const T& builder) { auto vb = CreateVertexBuffer(engine, num_vertices, vertices); auto ib = CreateIndexBuffer(engine, num_indices, indices); - return {ib, vb, builder.GetBounds(), T::kPrimitiveType}; + return std::make_unique(engine, ib, vb, builder.GetBounds(), + T::kPrimitiveType); } -FilamentBuffers CreateLine(filament::Engine* engine) { +MeshPtr CreateLine(filament::Engine* engine) { return CreateFromBuilder(engine, LineBuilder()); } -FilamentBuffers CreatePlane(filament::Engine* engine, int nquad) { +MeshPtr CreatePlane(filament::Engine* engine, int nquad) { return CreateFromBuilder(engine, PlaneBuilder(nquad)); } -FilamentBuffers CreateTriangle(filament::Engine* engine) { +MeshPtr CreateTriangle(filament::Engine* engine) { return CreateFromBuilder(engine, TriangleBuilder()); } -FilamentBuffers CreateBox(filament::Engine* engine, int nquad) { +MeshPtr CreateBox(filament::Engine* engine, int nquad) { return CreateFromBuilder(engine, BoxBuilder(nquad)); } -FilamentBuffers CreateLineBox(filament::Engine* engine) { +MeshPtr CreateLineBox(filament::Engine* engine) { return CreateFromBuilder(engine, LineBoxBuilder()); } -FilamentBuffers CreateSphere(filament::Engine* engine, int nstack, int nslice) { +MeshPtr CreateSphere(filament::Engine* engine, int nstack, int nslice) { return CreateFromBuilder(engine, SphereBuilder(nstack, nslice)); } -FilamentBuffers CreateTube(filament::Engine* engine, int nstack, int nslice) { +MeshPtr CreateTube(filament::Engine* engine, int nstack, int nslice) { return CreateFromBuilder(engine, TubeBuilder(nstack, nslice)); } -FilamentBuffers CreateDisk(filament::Engine* engine, int nslice) { +MeshPtr CreateDisk(filament::Engine* engine, int nslice) { return CreateFromBuilder(engine, DiskBuilder(nslice)); } -FilamentBuffers CreateDome(filament::Engine* engine, int nstack, int nslice) { +MeshPtr CreateDome(filament::Engine* engine, int nstack, int nslice) { return CreateFromBuilder(engine, DomeBuilder(nstack, nslice)); } -FilamentBuffers CreateCone(filament::Engine* engine, int nstack, int nslice) { +MeshPtr CreateCone(filament::Engine* engine, int nstack, int nslice) { return CreateFromBuilder(engine, ConeBuilder(nstack, nslice)); } diff --git a/src/experimental/filament/filament/builtins.h b/src/experimental/filament/filament/builtins.h index 713fa6d1..b79083a3 100644 --- a/src/experimental/filament/filament/builtins.h +++ b/src/experimental/filament/filament/builtins.h @@ -21,16 +21,16 @@ // Generates buffers for built-in shapes. namespace mujoco { -FilamentBuffers CreateLine(filament::Engine* engine); -FilamentBuffers CreatePlane(filament::Engine* engine, int nquad); -FilamentBuffers CreateTriangle(filament::Engine* engine); -FilamentBuffers CreateBox(filament::Engine* engine, int nquad); -FilamentBuffers CreateLineBox(filament::Engine* engine); -FilamentBuffers CreateSphere(filament::Engine* engine, int nstack, int nslice); -FilamentBuffers CreateTube(filament::Engine* engine, int nstack, int nslice); -FilamentBuffers CreateDisk(filament::Engine* engine, int nslice); -FilamentBuffers CreateDome(filament::Engine* engine, int nstack, int nslice); -FilamentBuffers CreateCone(filament::Engine* engine, int nstack, int nslice); +MeshPtr CreateLine(filament::Engine* engine); +MeshPtr CreatePlane(filament::Engine* engine, int nquad); +MeshPtr CreateTriangle(filament::Engine* engine); +MeshPtr CreateBox(filament::Engine* engine, int nquad); +MeshPtr CreateLineBox(filament::Engine* engine); +MeshPtr CreateSphere(filament::Engine* engine, int nstack, int nslice); +MeshPtr CreateTube(filament::Engine* engine, int nstack, int nslice); +MeshPtr CreateDisk(filament::Engine* engine, int nslice); +MeshPtr CreateDome(filament::Engine* engine, int nstack, int nslice); +MeshPtr CreateCone(filament::Engine* engine, int nstack, int nslice); } // namespace mujoco diff --git a/src/experimental/filament/filament/drawable.cc b/src/experimental/filament/filament/drawable.cc index f32afe1a..a377d1f6 100644 --- a/src/experimental/filament/filament/drawable.cc +++ b/src/experimental/filament/filament/drawable.cc @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -173,12 +174,12 @@ void Drawable::Update(const mjModel* model, const mjvScene* scene, if (geom.type == mjGEOM_FLEX || geom.type == mjGEOM_SKIN) { // Flex geometry is updated every frame with new vertex data. filament::Engine* engine = renderables_.GetEngine(); - FilamentBuffers buffers = CreateGeomBuffers(engine, model, scene, geom); + std::unique_ptr mesh = CreateGeomBuffers(engine, model, scene, geom); if (renderables_.GetNumEntities() == 0) { - renderables_.Append(std::move(buffers)); + renderables_.Append(std::move(mesh)); } else { - renderables_.Update(0, std::move(buffers)); + renderables_.Update(0, std::move(mesh)); } } @@ -192,27 +193,27 @@ void Drawable::Update(const mjModel* model, const mjvScene* scene, } void Drawable::AddMesh(int data_id) { - const FilamentBuffers* buffers = model_objs_->GetMeshBuffer(data_id); + const Mesh* buffers = model_objs_->GetMeshBuffer(data_id); if (buffers == nullptr) { mju_error("Unknown mesh %d", data_id); } - renderables_.Append(*buffers); + renderables_.Append(buffers); } void Drawable::AddHeightField(int hfield_id) { - const FilamentBuffers* buffers = model_objs_->GetHeightFieldBuffer(hfield_id); + const Mesh* buffers = model_objs_->GetHeightFieldBuffer(hfield_id); if (buffers == nullptr) { mju_error("Unknown height field %d", hfield_id); } - renderables_.Append(*buffers); + renderables_.Append(buffers); } void Drawable::AddShape(ModelObjects::ShapeType shape_type) { - const FilamentBuffers* buffers = model_objs_->GetShapeBuffer(shape_type); + const Mesh* buffers = model_objs_->GetShapeBuffer(shape_type); if (buffers == nullptr) { mju_error("Unknown shape %d", shape_type); } - renderables_.Append(*buffers); + renderables_.Append(buffers); } void Drawable::AddToScene(filament::Scene* scene) { diff --git a/src/experimental/filament/filament/geom_util.cc b/src/experimental/filament/filament/geom_util.cc index a26fd073..38d7509b 100644 --- a/src/experimental/filament/filament/geom_util.cc +++ b/src/experimental/filament/filament/geom_util.cc @@ -18,8 +18,10 @@ #include #include #include +#include #include +#include #include #include #include @@ -162,9 +164,8 @@ static filament::IndexBuffer* BuildIndexBuffer(filament::Engine* engine, } } -FilamentBuffers CreateGeomBuffers(filament::Engine* engine, - const mjModel* model, const mjvScene* scene, - const mjvGeom& geom) { +MeshPtr CreateGeomBuffers(filament::Engine* engine, const mjModel* model, + const mjvScene* scene, const mjvGeom& geom) { auto positions = GetPositions(model, scene, geom); auto normals = GetNormals(model, scene, geom); auto uvs = GetUvs(model, scene, geom); @@ -175,14 +176,13 @@ FilamentBuffers CreateGeomBuffers(filament::Engine* engine, num_indices = 3 * scene->flexfaceused[geom.objid]; } - FilamentBuffers buffers; float3 vmin = {FLT_MAX, FLT_MAX, FLT_MAX}; float3 vmax = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; - buffers.vertex_buffer = - BuildVertexBuffer(engine, positions, normals, uvs, &vmin, &vmax); - buffers.index_buffer = BuildIndexBuffer(engine, indices, num_indices); - buffers.bounds.emplace().set(vmin, vmax); - return buffers; + auto vertex_buffer = BuildVertexBuffer(engine, positions, normals, uvs, &vmin, &vmax); + auto index_buffer = BuildIndexBuffer(engine, indices, num_indices); + filament::Box bounds; + bounds.set(vmin, vmax); + return std::make_unique(engine, index_buffer, vertex_buffer, bounds); } } // namespace mujoco diff --git a/src/experimental/filament/filament/geom_util.h b/src/experimental/filament/filament/geom_util.h index b6b1e6a1..55c14db0 100644 --- a/src/experimental/filament/filament/geom_util.h +++ b/src/experimental/filament/filament/geom_util.h @@ -22,9 +22,8 @@ namespace mujoco { // Populates the FilamentBuffers for a flex geometry. -FilamentBuffers CreateGeomBuffers(filament::Engine* engine, - const mjModel* model, const mjvScene* scene, - const mjvGeom& geom); +MeshPtr CreateGeomBuffers(filament::Engine* engine, const mjModel* model, + const mjvScene* scene, const mjvGeom& geom); } // namespace mujoco diff --git a/src/experimental/filament/filament/gui_view.cc b/src/experimental/filament/filament/gui_view.cc index 1876936e..2217c95b 100644 --- a/src/experimental/filament/filament/gui_view.cc +++ b/src/experimental/filament/filament/gui_view.cc @@ -62,10 +62,7 @@ GuiView::~GuiView() { } auto& em = utils::EntityManager::get(); em.destroy(renderable_); - for (auto& buffer : buffers_) { - engine_->destroy(buffer.vertex_buffer); - engine_->destroy(buffer.index_buffer); - } + meshes_.clear(); for (auto& instance : instances_) { engine_->destroy(instance); } @@ -84,12 +81,7 @@ void GuiView::ResetRenderable() { em.destroy(renderable_); renderable_ = utils::Entity(); } - - for (auto& buffer : buffers_) { - engine_->destroy(buffer.vertex_buffer); - engine_->destroy(buffer.index_buffer); - } - buffers_.clear(); + meshes_.clear(); } uintptr_t GuiView::UploadImage(uintptr_t tex_id, const uint8_t* pixels, @@ -275,12 +267,7 @@ void GuiView::UpdateRenderable() { builder.build(*engine_, renderable_); scene_->addEntity(renderable_); } - - for (auto& buffer : buffers_) { - engine_->destroy(buffer.vertex_buffer); - engine_->destroy(buffer.index_buffer); - } - buffers_.clear(); + meshes_.clear(); auto ri = rm.getInstance(renderable_); @@ -299,10 +286,13 @@ void GuiView::UpdateRenderable() { } std::memcpy(dst, cmds->IdxBuffer.Data, size); }; - buffers_.push_back( - {CreateIndexBuffer(engine_, cmds->IdxBuffer.Size, ifill), - CreateVertexBuffer(engine_, cmds->VtxBuffer.Size, vfill)}); - const mujoco::FilamentBuffers& buffer = buffers_.back(); + filament::IndexBuffer* index_buffer = + CreateIndexBuffer(engine_, cmds->IdxBuffer.Size, ifill); + filament::VertexBuffer* vertex_buffer = + CreateVertexBuffer(engine_, cmds->VtxBuffer.Size, vfill); + + meshes_.push_back(std::make_unique(engine_, index_buffer, vertex_buffer)); + const auto& mesh = meshes_.back(); int index_offset = 0; for (const ImDrawCmd& command : cmds->CmdBuffer) { @@ -327,8 +317,9 @@ void GuiView::UpdateRenderable() { rm.setMaterialInstanceAt( ri, drawable_index, GetMaterialInstance(drawable_index, clip_rect, command.GetTexID())); - rm.setGeometryAt(ri, drawable_index, kTriangles, buffer.vertex_buffer, - buffer.index_buffer, index_offset, command.ElemCount); + rm.setGeometryAt( + ri, drawable_index, kTriangles, mesh->GetFilamentVertexBuffer(), + mesh->GetFilamentIndexBuffer(), index_offset, command.ElemCount); rm.setBlendOrderAt(ri, drawable_index, drawable_index); index_offset += command.ElemCount; diff --git a/src/experimental/filament/filament/gui_view.h b/src/experimental/filament/filament/gui_view.h index 2b189324..d1f80cd8 100644 --- a/src/experimental/filament/filament/gui_view.h +++ b/src/experimental/filament/filament/gui_view.h @@ -71,7 +71,7 @@ class GuiView { filament::View* view_ = nullptr; filament::Material* material_ = nullptr; utils::Entity renderable_; - std::vector buffers_; + std::vector meshes_; std::vector instances_; std::unordered_map> textures_; int num_elements_ = 0; diff --git a/src/experimental/filament/filament/model_objects.cc b/src/experimental/filament/filament/model_objects.cc index 3df4f2e2..7f8a438c 100644 --- a/src/experimental/filament/filament/model_objects.cc +++ b/src/experimental/filament/filament/model_objects.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -29,7 +30,6 @@ #include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/texture.h" - namespace mujoco { ModelObjects::ModelObjects(const mjModel* model, filament::Engine* engine) @@ -73,14 +73,7 @@ ModelObjects::~ModelObjects() { for (auto& iter : indirect_lights_) { engine_->destroy(iter); } - for (auto& iter : meshes_) { - engine_->destroy(iter.second.vertex_buffer); - engine_->destroy(iter.second.index_buffer); - } - for (auto& iter : shapes_) { - engine_->destroy(iter.vertex_buffer); - engine_->destroy(iter.index_buffer); - } + meshes_.clear(); textures_.clear(); } @@ -88,32 +81,25 @@ void ModelObjects::UploadMesh(const mjModel* model, int id) { if (model != model_) { mju_error("Model mismatch."); } - if (id < 0 || id >= model->nmesh) { + if (id < 0 || id >= model->nmesh) { mju_error("Invalid mesh index %d", id); } + meshes_.erase(id); + convex_hulls_.erase(id); - if (auto iter = meshes_.find(id); iter != meshes_.end()) { - engine_->destroy(iter->second.vertex_buffer); - engine_->destroy(iter->second.index_buffer); - } - if (auto iter = convex_hulls_.find(id); iter != convex_hulls_.end()) { - engine_->destroy(iter->second.vertex_buffer); - engine_->destroy(iter->second.index_buffer); - } - - FilamentBuffers& buffers = meshes_[id]; - buffers.vertex_buffer = CreateVertexBuffer( - engine_, model, id, MeshType::kNormal, &buffers.bounds.emplace()); - buffers.index_buffer = - CreateIndexBuffer(engine_, model, id, MeshType::kNormal); + filament::Box bounds; + auto vertex_buffer = + CreateVertexBuffer(engine_, model, id, MeshType::kNormal, &bounds); + auto index_buffer = CreateIndexBuffer(engine_, model, id, MeshType::kNormal); + meshes_[id] = + std::make_unique(engine_, index_buffer, vertex_buffer, bounds); if (model->mesh_graphadr[id] >= 0) { - FilamentBuffers& hull_buffers = convex_hulls_[id]; - hull_buffers.vertex_buffer = - CreateVertexBuffer(engine_, model, id, MeshType::kConvexHull, - &hull_buffers.bounds.emplace()); - hull_buffers.index_buffer = - CreateIndexBuffer(engine_, model, id, MeshType::kConvexHull); + vertex_buffer = + CreateVertexBuffer(engine_, model, id, MeshType::kConvexHull, &bounds); + index_buffer = CreateIndexBuffer(engine_, model, id, MeshType::kConvexHull); + convex_hulls_[id] = + std::make_unique(engine_, index_buffer, vertex_buffer, bounds); } } @@ -172,43 +158,41 @@ void ModelObjects::UploadHeightField(const mjModel* model, int id) { mju_error("Invalid height field index %d", id); } - if (auto iter = height_fields_.find(id); iter != height_fields_.end()) { - engine_->destroy(iter->second.vertex_buffer); - engine_->destroy(iter->second.index_buffer); - } + height_fields_.erase(id); - FilamentBuffers& buffers = height_fields_[id]; - buffers.vertex_buffer = CreateVertexBuffer( - engine_, model, id, MeshType::kHeightField, &buffers.bounds.emplace()); - buffers.index_buffer = + filament::Box bounds; + auto vertex_buffer = + CreateVertexBuffer(engine_, model, id, MeshType::kHeightField, &bounds); + auto index_buffer = CreateIndexBuffer(engine_, model, id, MeshType::kHeightField); + height_fields_[id] = + std::make_unique(engine_, index_buffer, vertex_buffer, bounds); } -const FilamentBuffers* ModelObjects::GetMeshBuffer(int data_id) const { +const Mesh* ModelObjects::GetMeshBuffer(int data_id) const { // As defined by mjv_updateScene: // original mesh: mesh_id * 2 // convex hull: (mesh_id * 2) + 1 const int mesh_id = data_id / 2; if (data_id % 2 == 0) { auto it = meshes_.find(mesh_id); - return it != meshes_.end() ? &it->second : nullptr; + return it != meshes_.end() ? it->second.get() : nullptr; } else { auto it = convex_hulls_.find(mesh_id); - return it != convex_hulls_.end() ? &it->second : nullptr; + return it != convex_hulls_.end() ? it->second.get() : nullptr; } } -const FilamentBuffers* ModelObjects::GetHeightFieldBuffer( - int hfield_id) const { +const Mesh* ModelObjects::GetHeightFieldBuffer(int hfield_id) const { auto it = height_fields_.find(hfield_id); - return it != height_fields_.end() ? &it->second : nullptr; + return it != height_fields_.end() ? it->second.get() : nullptr; } -const FilamentBuffers* ModelObjects::GetShapeBuffer(ShapeType shape) const { +const Mesh* ModelObjects::GetShapeBuffer(ShapeType shape) const { if (shape < 0 || shape >= kNumShapes) { mju_error("Invalid shape type: %d", shape); } - return &shapes_[shape]; + return shapes_[shape].get(); } const Texture* ModelObjects::GetTexture(int tex_id) const { diff --git a/src/experimental/filament/filament/model_objects.h b/src/experimental/filament/filament/model_objects.h index 7007e81a..f694b4de 100644 --- a/src/experimental/filament/filament/model_objects.h +++ b/src/experimental/filament/filament/model_objects.h @@ -61,9 +61,9 @@ class ModelObjects { filament::Engine* GetEngine() const { return engine_; } // Returns the cached instance of a filament object created from the mjModel. - const FilamentBuffers* GetShapeBuffer(ShapeType shape) const; - const FilamentBuffers* GetMeshBuffer(int data_id) const; - const FilamentBuffers* GetHeightFieldBuffer(int hfield_id) const; + const Mesh* GetShapeBuffer(ShapeType shape) const; + const Mesh* GetMeshBuffer(int data_id) const; + const Mesh* GetHeightFieldBuffer(int hfield_id) const; const Texture* GetTexture(int tex_id) const; const Texture* GetTexture(int mat_id, int role) const; @@ -84,10 +84,10 @@ class ModelObjects { filament::Engine* engine_ = nullptr; std::vector skyboxes_; std::vector indirect_lights_; - std::array shapes_; - std::unordered_map meshes_; - std::unordered_map convex_hulls_; - std::unordered_map height_fields_; + std::array shapes_; + std::unordered_map meshes_; + std::unordered_map convex_hulls_; + std::unordered_map height_fields_; std::unordered_map> textures_; float specular_multiplier_ = 0.2f; float shininess_multiplier_ = 0.1f; diff --git a/src/experimental/filament/filament/renderables.cc b/src/experimental/filament/filament/renderables.cc index fcce336f..5be61556 100644 --- a/src/experimental/filament/filament/renderables.cc +++ b/src/experimental/filament/filament/renderables.cc @@ -15,7 +15,7 @@ #include "experimental/filament/filament/renderables.h" #include -#include +#include #include #include @@ -49,67 +49,65 @@ void Renderables::RemoveLast() { engine_->destroy(entity); em.destroy(entity); entities_.pop_back(); - - if (owned_buffers_.back().owned) { - engine_->destroy(owned_buffers_.back().buffers.vertex_buffer); - engine_->destroy(owned_buffers_.back().buffers.index_buffer); - } - owned_buffers_.pop_back(); + meshes_.pop_back(); } -void Renderables::Update(int index, const FilamentBuffers& buffers) { +void Renderables::Update(int index, const Mesh* mesh) { if (index < 0 || index >= entities_.size()) { mju_error("Invalid index %d for renderable.", index); } utils::Entity& entity = entities_[index]; - UpdateEntity(entity, buffers); - UpdateBuffers(index, buffers, false); + UpdateEntity(entity, mesh); + UpdateMeshes(index, mesh); } -void Renderables::Update(int index, FilamentBuffers&& buffers) { +void Renderables::Update(int index, MeshPtr mesh) { if (index < 0 || index >= entities_.size()) { mju_error("Invalid index %d for renderable.", index); } utils::Entity& entity = entities_[index]; - UpdateEntity(entity, buffers); - UpdateBuffers(index, buffers, true); + UpdateEntity(entity, mesh.get()); + UpdateMeshes(index, mesh.get(), std::move(mesh)); } -void Renderables::Append(const FilamentBuffers& buffers) { - utils::Entity entity = CreateEntity(buffers); +void Renderables::Append(const Mesh* mesh) { + utils::Entity entity = CreateEntity(mesh); entities_.push_back(entity); - owned_buffers_.push_back({.owned = false, .buffers = buffers}); + meshes_.emplace_back(nullptr, mesh); } -void Renderables::Append(FilamentBuffers&& buffers) { - utils::Entity entity = CreateEntity(buffers); +void Renderables::Append(MeshPtr mesh) { + utils::Entity entity = CreateEntity(mesh.get()); entities_.push_back(entity); - owned_buffers_.push_back({.owned = true, .buffers = buffers}); + meshes_.emplace_back(std::move(mesh), mesh.get()); } -utils::Entity Renderables::CreateEntity(const FilamentBuffers& buffers) { - if (buffers.vertex_buffer == nullptr) { +utils::Entity Renderables::CreateEntity(const Mesh* mesh) { + filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer(); + if (vertex_buffer == nullptr) { mju_error("Invalid (null) vertex buffer."); } - if (buffers.index_buffer == nullptr) { + + filament::IndexBuffer* index_buffer = mesh->GetFilamentIndexBuffer(); + if (index_buffer == nullptr) { mju_error("Invalid (null) index buffer."); } + utils::Entity entity = utils::EntityManager::get().create(); if (entity.isNull()) { mju_error("Failed to create entity."); } filament::RenderableManager::Builder builder(1); - builder.geometry(0, buffers.type, buffers.vertex_buffer, - buffers.index_buffer); - if (material_instance_) { - builder.material(0, material_instance_); - } - if (buffers.bounds.has_value()) { - builder.boundingBox(buffers.bounds.value()); + builder.geometry(0, mesh->GetPrimitiveType(), vertex_buffer, index_buffer); + if (mesh->HasBounds()) { + builder.boundingBox(mesh->GetBounds()); } else { builder.culling(false); } + if (material_instance_) { + builder.material(0, material_instance_); + } builder.castShadows(cast_shadows_); builder.receiveShadows(receive_shadows_); builder.layerMask(0xff, layer_mask_); @@ -123,30 +121,29 @@ utils::Entity Renderables::CreateEntity(const FilamentBuffers& buffers) { return entity; } -void Renderables::UpdateEntity(utils::Entity entity, - const FilamentBuffers& buffers) { - if (buffers.vertex_buffer == nullptr) { +void Renderables::UpdateEntity(utils::Entity entity, const Mesh* mesh) { + filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer(); + if (vertex_buffer == nullptr) { mju_error("Invalid (null) vertex buffer."); } - if (buffers.index_buffer == nullptr) { + + filament::IndexBuffer* index_buffer = mesh->GetFilamentIndexBuffer(); + if (index_buffer == nullptr) { mju_error("Invalid (null) index buffer."); } + filament::RenderableManager& rm = engine_->getRenderableManager(); - rm.setGeometryAt(rm.getInstance(entity), 0, buffers.type, - buffers.vertex_buffer, buffers.index_buffer, 0, - buffers.index_buffer->getIndexCount()); + rm.setGeometryAt(rm.getInstance(entity), 0, mesh->GetPrimitiveType(), + vertex_buffer, index_buffer, 0, + index_buffer->getIndexCount()); } -void Renderables::UpdateBuffers(int index, FilamentBuffers buffers, bool owned) { - if (index < 0 || index >= owned_buffers_.size()) { +void Renderables::UpdateMeshes(int index, const Mesh* mesh, MeshPtr owned_mesh) { + if (index < 0 || index >= meshes_.size()) { mju_error("Invalid index %d for renderable.", index); } - if (owned_buffers_[index].owned) { - engine_->destroy(owned_buffers_[index].buffers.vertex_buffer); - engine_->destroy(owned_buffers_[index].buffers.index_buffer); - } - owned_buffers_[index].buffers = buffers; - owned_buffers_[index].owned = owned; + meshes_[index].owned_mesh = std::move(owned_mesh); + meshes_[index].mesh = mesh; } void Renderables::AddToScene(filament::Scene* scene) { @@ -239,11 +236,13 @@ void Renderables::SetWireframe(bool wireframe) { filament::RenderableManager& rm = engine_->getRenderableManager(); for (int i = 0; i < entities_.size(); ++i) { utils::Entity& entity = entities_[i]; - FilamentBuffers& buffers = owned_buffers_[i].buffers; + const Mesh* mesh = meshes_[i].mesh; + filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer(); + filament::IndexBuffer* index_buffer = mesh->GetFilamentIndexBuffer(); rm.setGeometryAt(rm.getInstance(entity), 0, - wireframe_ ? kWireframeType : buffers.type, - buffers.vertex_buffer, buffers.index_buffer, 0, - buffers.index_buffer->getIndexCount()); + wireframe_ ? kWireframeType : mesh->GetPrimitiveType(), + vertex_buffer, index_buffer, 0, + index_buffer->getIndexCount()); } } } diff --git a/src/experimental/filament/filament/renderables.h b/src/experimental/filament/filament/renderables.h index bd6b3602..2108442a 100644 --- a/src/experimental/filament/filament/renderables.h +++ b/src/experimental/filament/filament/renderables.h @@ -16,7 +16,6 @@ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDERABLES_H_ #include -#include #include #include @@ -39,13 +38,13 @@ class Renderables { Renderables(const Renderables&) = delete; Renderables& operator=(const Renderables&) = delete; - // Appends a new renderable entity built from the given buffers. - void Append(const FilamentBuffers& buffers); - void Append(FilamentBuffers&& buffers); + // Appends a new renderable entity built from the given mesh. + void Append(const Mesh* mesh); + void Append(MeshPtr mesh); - // Updates the entity at the index with new buffers. - void Update(int index, const FilamentBuffers& buffers); - void Update(int index, FilamentBuffers&& buffers); + // Updates the entity at the index with new mesh. + void Update(int index, const Mesh* mesh); + void Update(int index, MeshPtr mesh); // Removes the last entity. void RemoveLast(); @@ -53,7 +52,7 @@ class Renderables { // Returns the entity at the given index. utils::Entity operator[](int index) { return entities_[index]; } - // Returns the owned buffers at the given index. + // Returns the number of Entities that make up this renderable. int GetNumEntities() const { return entities_.size(); } // Hides all managed entities. @@ -84,22 +83,20 @@ class Renderables { filament::Engine* GetEngine() { return engine_; } private: - utils::Entity CreateEntity(const FilamentBuffers& buffers); - void UpdateEntity(utils::Entity entity, const FilamentBuffers& buffers); - void UpdateBuffers(int index, FilamentBuffers buffers, bool owned); + utils::Entity CreateEntity(const Mesh* mesh); + void UpdateEntity(utils::Entity entity, const Mesh* mesh); + void UpdateMeshes(int index, const Mesh* mesh, MeshPtr owned_mesh = nullptr); - // Tracks whether of not the filament buffers should be destroyed by this - // class. - struct OwnedBuffers { - bool owned = false; - FilamentBuffers buffers; + struct MeshWrapper { + MeshPtr owned_mesh; + const Mesh* mesh = nullptr; }; filament::Engine* engine_ = nullptr; filament::Scene* assigned_scene_ = nullptr; filament::MaterialInstance* material_instance_ = nullptr; std::vector entities_; - std::vector owned_buffers_; + std::vector meshes_; std::uint8_t priority_ = kDefaultPriority; std::uint8_t layer_mask_ = kDefaultLayerMask; bool wireframe_ = false; From 6273331d82ae4c3d68dc37e86f9c825b42029230 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 7 Apr 2026 03:57:38 -0700 Subject: [PATCH 013/251] Define a struct that can be used to create a Mesh. A future change will update all callers to use this method for creating Meshes after which we will remove the constructor that takes explicit filament objects. PiperOrigin-RevId: 895807063 Change-Id: I50833817961ae57fbb9ed6b875c1ef286b9fdd26 --- .../filament/filament/buffer_util.cc | 300 +++++++++++++++++- .../filament/filament/buffer_util.h | 154 +++++++-- 2 files changed, 430 insertions(+), 24 deletions(-) diff --git a/src/experimental/filament/filament/buffer_util.cc b/src/experimental/filament/filament/buffer_util.cc index 82363b2f..17f62091 100644 --- a/src/experimental/filament/filament/buffer_util.cc +++ b/src/experimental/filament/filament/buffer_util.cc @@ -14,12 +14,311 @@ #include "experimental/filament/filament/buffer_util.h" +#include #include +#include +#include +#include +#include +#include +#include +#include #include +#include "third_party/filament/libs/filabridge/include/filament/MaterialEnums.h" +#include +#include +#include +#include +#include "experimental/filament/filament/math_util.h" +#include "experimental/filament/filament/vertex_util.h" namespace mujoco { +using filament::math::float3; +using filament::math::float4; + +static filament::VertexAttribute GetUsage(const VertexAttribute& attrib) { + switch (attrib.usage) { + case mjVERTEX_ATTRIBUTE_POSITION: + return filament::VertexAttribute::POSITION; + case mjVERTEX_ATTRIBUTE_NORMAL: + return filament::VertexAttribute::TANGENTS; + case mjVERTEX_ATTRIBUTE_TANGENTS: + return filament::VertexAttribute::TANGENTS; + case mjVERTEX_ATTRIBUTE_UV: + return filament::VertexAttribute::UV0; + case mjVERTEX_ATTRIBUTE_COLOR: + return filament::VertexAttribute::COLOR; + default: + mju_error("Unsupported vertex attribute usage: %d", attrib.usage); + return filament::VertexAttribute::POSITION; + } +} + +static filament::VertexBuffer::AttributeType GetType( + const VertexAttribute& attrib) { + switch (attrib.type) { + case mjVERTEX_ATTRIBUTE_TYPE_FLOAT2: + return filament::VertexBuffer::AttributeType::FLOAT2; + case mjVERTEX_ATTRIBUTE_TYPE_FLOAT3: + return filament::VertexBuffer::AttributeType::FLOAT3; + case mjVERTEX_ATTRIBUTE_TYPE_FLOAT4: + return filament::VertexBuffer::AttributeType::FLOAT4; + case mjVERTEX_ATTRIBUTE_TYPE_UBYTE4: + return filament::VertexBuffer::AttributeType::UBYTE4; + default: + mju_error("Unsupported vertex attribute type: %d", attrib.type); + return filament::VertexBuffer::AttributeType::FLOAT3; + } +} + +int VertexAttributeTypeSize(const VertexAttribute& attrib) { + switch (attrib.type) { + case mjVERTEX_ATTRIBUTE_TYPE_FLOAT2: + return sizeof(float) * 2; + case mjVERTEX_ATTRIBUTE_TYPE_FLOAT3: + return sizeof(float) * 3; + case mjVERTEX_ATTRIBUTE_TYPE_FLOAT4: + return sizeof(float) * 4; + case mjVERTEX_ATTRIBUTE_TYPE_UBYTE4: + return sizeof(uint8_t) * 4; + default: + mju_error("Unsupported vertex attribute type: %d", attrib.type); + return 0; + } +} + +// Initializes the MeshData to default values. +void DefaultMeshData(MeshData* data) { + std::memset(data, 0, sizeof(MeshData)); +} + +Mesh::Mesh(filament::Engine* engine, const MeshData& data) + : engine_(engine) { + type_ = data.primitive_type == mjPRIM_TYPE_TRIANGLES + ? filament::RenderableManager::PrimitiveType::TRIANGLES + : filament::RenderableManager::PrimitiveType::LINES; + + // If the user has provided a release callback, then we need to ensure we + // call is when filament is done with the mesh data. + if (data.release_callback) { + release_callbacks_.push_back([=]() { + data.release_callback(data.user_data); + }); + } + + BuildVertexBuffer(data); + BuildIndexBuffer(data); + UpdateBounds(data); +} + +Mesh::~Mesh() { + ReleaseResources(); + if (index_buffer_) { + engine_->destroy(index_buffer_); + } + if (vertex_buffer_) { + engine_->destroy(vertex_buffer_); + } +} + +void Mesh::BuildVertexBuffer(const MeshData& data) { + if (data.nvertices == 0) { + mju_error("MeshData has no vertices."); + } + + // The filament BufferDescriptor callback for releasing the memory. We assume + // that ReleaseResources() can be called multiple times, so we assign this + // callback to each buffer descriptor. + auto callback = +[](void* buffer, size_t size, void* user) { + static_cast(user)->ReleaseResources(); + }; + + // Pointers to specific attributes in the mesh data, used for additional + // validation and processing. + const VertexAttribute* positions = nullptr; + const VertexAttribute* normals = nullptr; + const VertexAttribute* tangents = nullptr; + + // Calculate the stride of the vertex buffer. + int stride = 0; + for (int i = 0; i < data.nattributes; ++i) { + stride += VertexAttributeTypeSize(data.attributes[i]); + if (data.attributes[i].usage == mjVERTEX_ATTRIBUTE_POSITION) { + positions = &data.attributes[i]; + } else if (data.attributes[i].usage == mjVERTEX_ATTRIBUTE_NORMAL) { + normals = &data.attributes[i]; + } else if (data.attributes[i].usage == mjVERTEX_ATTRIBUTE_TANGENTS) { + tangents = &data.attributes[i]; + } + } + + if (!positions) { + mju_error("MeshData has no positions."); + } + if (data.attributes[0].usage != mjVERTEX_ATTRIBUTE_POSITION) { + mju_error("Positions must be the first attribute."); + } + if (normals && tangents) { + mju_error("MeshData has both normals and tangents."); + } + if (normals && data.interleaved) { + // We need to build orientations from normals and so we require each + // attribute to be in a separate buffer. + mju_error("Cannot support normals with interleaved vertex attributes."); + } + + // Build the vertex buffer. + filament::VertexBuffer::Builder vb_builder; + vb_builder.bufferCount(data.interleaved ? 1 : data.nattributes); + vb_builder.vertexCount(data.nvertices); + + int offset = 0; + for (int i = 0; i < data.nattributes; ++i) { + const VertexAttribute& attrib = data.attributes[i]; + const filament::VertexAttribute usage = GetUsage(attrib); + const filament::VertexBuffer::AttributeType type = GetType(attrib); + if (data.interleaved) { + vb_builder.attribute(usage, 0, type, offset, stride); + } else { + vb_builder.attribute(usage, i, type); + } + if (usage == filament::VertexAttribute::COLOR) { + vb_builder.normalized(usage); + } + offset += VertexAttributeTypeSize(attrib); + } + vertex_buffer_ = vb_builder.build(*engine_); + + if (data.interleaved) { + const VertexAttribute& attrib = data.attributes[0]; + const size_t nbytes = data.nvertices * stride; + filament::backend::BufferDescriptor desc(attrib.bytes, nbytes, callback, + this); + vertex_buffer_->setBufferAt(*engine_, 0, std::move(desc)); + } else { + for (int i = 0; i < data.nattributes; ++i) { + const VertexAttribute& attrib = data.attributes[i]; + const size_t nbytes = data.nvertices * VertexAttributeTypeSize(attrib); + if (attrib.usage == mjVERTEX_ATTRIBUTE_NORMAL) { + const float4* orientations = + BuildOrientationsFromNormals(data.nvertices, attrib); + filament::backend::BufferDescriptor desc(orientations, nbytes, callback, + this); + vertex_buffer_->setBufferAt(*engine_, i, std::move(desc)); + } else { + filament::backend::BufferDescriptor desc(attrib.bytes, nbytes, callback, + this); + vertex_buffer_->setBufferAt(*engine_, i, std::move(desc)); + } + } + } +} + +void Mesh::BuildIndexBuffer(const MeshData& data) { + if (data.nindices == 0) { + return; + } + + const int element_size = data.index_type == mjINDEX_TYPE_USHORT + ? sizeof(uint16_t) + : sizeof(uint32_t); + const int num_bytes = data.nindices * element_size; + + // If indices == 0 and nindices > 0, then the user is specifying that the + // vertices are provided "in order", i.e. the indices are 0, 1, 2, 3, ... + // In this case, we need to create the sequence of indices explicitly. + const void* indices = data.indices; + if (indices == nullptr) { + std::byte* sequence = new std::byte[num_bytes]; + release_callbacks_.push_back([=]() { + delete[] sequence; + }); + + if (data.index_type == mjINDEX_TYPE_USHORT) { + FillSequence(sequence, num_bytes); + } else { + FillSequence(sequence, num_bytes); + } + indices = sequence; + } + + filament::IndexBuffer::Builder ib_builder; + ib_builder.indexCount(data.nindices); + ib_builder.bufferType(data.index_type == mjINDEX_TYPE_USHORT + ? filament::IndexBuffer::IndexType::USHORT + : filament::IndexBuffer::IndexType::UINT); + index_buffer_ = ib_builder.build(*engine_); + // We don't worry about setting a release callback here because the release + // callback for the vertex buffer will call release_callbacks_. + filament::backend::BufferDescriptor desc(indices, num_bytes); + index_buffer_->setBuffer(*engine_, std::move(desc)); +} + +float4* Mesh::BuildOrientationsFromNormals(int nvertices, const VertexAttribute& normals) { + float4* orientations = new float4[nvertices]; + release_callbacks_.push_back([=]() { + delete[] orientations; + }); + const float* normals_ptr = reinterpret_cast(normals.bytes); + for (int i = 0; i < nvertices; ++i) { + orientations[i] = CalculateOrientation(ReadFloat3(normals_ptr, i)); + } + return orientations; +} + +void Mesh::UpdateBounds(const MeshData& data) { + float3 bounds_min = ReadFloat3(data.bounds_min); + float3 bounds_max = ReadFloat3(data.bounds_max); + if (bounds_min != bounds_max) { + bounds_.emplace().set(bounds_min, bounds_max); + } else if (data.compute_bounds) { + bounds_min = float3(FLT_MAX, FLT_MAX, FLT_MAX); + bounds_max = float3(-FLT_MAX, -FLT_MAX, -FLT_MAX); + + if (data.attributes[0].usage != mjVERTEX_ATTRIBUTE_POSITION) { + mju_error("MeshData has no positions."); + } + const float* positions = + reinterpret_cast(data.attributes[0].bytes); + + for (int i = 0; i < data.nvertices; ++i) { + const float3 position = ReadFloat3(positions, i); + bounds_min = min(bounds_min, position); + bounds_max = max(bounds_max, position); + } + bounds_.emplace().set(bounds_min, bounds_max); + } +} + +void Mesh::ReleaseResources() { + for (const auto& callback : release_callbacks_) { + callback(); + } + release_callbacks_.clear(); +} + +filament::IndexBuffer* Mesh::GetFilamentIndexBuffer() const { + return index_buffer_; +} + +filament::VertexBuffer* Mesh::GetFilamentVertexBuffer() const { + return vertex_buffer_; +} + +filament::RenderableManager::PrimitiveType Mesh::GetPrimitiveType() const { + return type_; +} + +bool Mesh::HasBounds() const { + return bounds_.has_value(); +} + +filament::Box Mesh::GetBounds() const { + return bounds_.value(); +} + filament::backend::BufferDescriptor CreateBufferDescriptor( std::size_t num_bytes, const FillBufferFn& fill) { std::byte* bytes = new std::byte[num_bytes]; @@ -30,5 +329,4 @@ filament::backend::BufferDescriptor CreateBufferDescriptor( }; return filament::backend::BufferDescriptor(bytes, num_bytes, callback, bytes); } - } // namespace mujoco diff --git a/src/experimental/filament/filament/buffer_util.h b/src/experimental/filament/filament/buffer_util.h index 5b5c576b..d72afedb 100644 --- a/src/experimental/filament/filament/buffer_util.h +++ b/src/experimental/filament/filament/buffer_util.h @@ -21,19 +21,125 @@ #include #include #include +#include + #include #include #include #include #include #include +#include // Functions for creating filament vertex and index buffers. namespace mujoco { +// The type of data stored in an index buffer. +typedef enum mjtIndexType_ { + mjINDEX_TYPE_USHORT = 0, + mjINDEX_TYPE_UINT = 1, +} mjtIndexType; + +// The type of primitive to be drawn by vertex data. +typedef enum mjtMeshPrimitiveType_ { + mjPRIM_TYPE_TRIANGLES = 0, + mjPRIM_TYPE_LINES = 1, +} mjtMeshPrimitiveType; + +// The usage/purpose of an attribute of a vertex. +typedef enum mjtVertexAttributeUsage_ { + mjVERTEX_ATTRIBUTE_POSITION = 0, + mjVERTEX_ATTRIBUTE_NORMAL = 1, + mjVERTEX_ATTRIBUTE_TANGENTS = 2, + mjVERTEX_ATTRIBUTE_UV = 3, + mjVERTEX_ATTRIBUTE_COLOR = 4, +} mjtVertexAttributeUsage; + +// The data format of an attribute of a vertex. +typedef enum mjtVertexAttributeType_ { + mjVERTEX_ATTRIBUTE_TYPE_FLOAT2 = 0, + mjVERTEX_ATTRIBUTE_TYPE_FLOAT3 = 1, + mjVERTEX_ATTRIBUTE_TYPE_FLOAT4 = 2, + mjVERTEX_ATTRIBUTE_TYPE_UBYTE4 = 3, +} mjtVertexAttributeType; + +// Information about a single attribute of a vertex. +struct VertexAttribute { + // The data for the attribute. + const void* bytes; + + // The usage/purpose of the attribute. + mjtVertexAttributeUsage usage; + + // The data format of the attribute. + mjtVertexAttributeType type; +}; + +// The binary contents of a mesh. +struct MeshData { + // The number of vertices in the mesh. Each of the vertex arrays below is + // assumed to have this number of elements. + size_t nvertices; + + // The number of attributes for each vertex in the mesh. + int nattributes; + + // Information about each attribute of a vertex in the mesh. See `interleaved` + // for more details. + VertexAttribute attributes[16]; + + // Whether the vertex attributes are interleaved or not. + // + // If true, assumes that the attributes are packed in the order specified in + // the attributes array, with no padding in-between. Additionally, the + // `data` pointer for each attribute is assumed to point to the first element + // of that type. + // + // If false, assume each attribute is stored in a separate array as defined + // by the `data` field of the attribute. + bool interleaved; + + // The number of indices in the mesh. The indices array is assumed to have + // this number of elements. + size_t nindices; + + // The indices of the mesh, stored as either ushort or uint depending on the + // index type. + const void* indices; + + // The type of data stored in the indices array. + mjtIndexType index_type; + + // The type of primitive to be drawn by vertex data. + mjtMeshPrimitiveType primitive_type; + + // Whether to compute the bounds of the mesh using the vertex positions. + bool compute_bounds; + + // The bounds of the mesh. If bounds_min == bounds_max, then we assume that + // that the bounds are not set (i.e. the bounds is empty). + float bounds_min[3]; + float bounds_max[3]; + + // Because rendering may be multithreaded, we cannot make assumptions about + // when the mesh data will finish uploading to the GPU. As such, we will use + // this callback to notify callers when it is safe to free the mesh data. + void (*release_callback)(void* user_data); + + // User data to pass to the release callback. + void* user_data; +}; + +// Initializes the MeshData to default values. +void DefaultMeshData(MeshData* data); + // Owns a Vertex and Index buffer representing a geometry mesh. class Mesh { public: + // Creates a Mesh from the given MeshData. + Mesh(filament::Engine* engine, const MeshData& data); + + // Create a Mesh directly from filament objects. Internal use only. Mesh(filament::Engine* engine, filament::IndexBuffer* index_buffer, filament::VertexBuffer* vertex_buffer, std::optional bounds = std::nullopt, @@ -45,41 +151,43 @@ class Mesh { type_(type), bounds_(bounds) {} - ~Mesh() { - if (index_buffer_) { - engine_->destroy(index_buffer_); - } - if (vertex_buffer_) { - engine_->destroy(vertex_buffer_); - } - } + ~Mesh(); - filament::IndexBuffer* GetFilamentIndexBuffer() const { - return index_buffer_; - } - filament::VertexBuffer* GetFilamentVertexBuffer() const { - return vertex_buffer_; - } - filament::RenderableManager::PrimitiveType GetPrimitiveType() const { - return type_; - } - bool HasBounds() const { - return bounds_.has_value(); - } - filament::Box GetBounds() const { - return bounds_.value(); - } + // Returns the filament IndexBuffer for the mesh. + filament::IndexBuffer* GetFilamentIndexBuffer() const; + + // Returns the filament VertexBuffer for the mesh. + filament::VertexBuffer* GetFilamentVertexBuffer() const; + + // Returns the primitive type of the mesh. + filament::RenderableManager::PrimitiveType GetPrimitiveType() const; + + // Returns whether the mesh has bounds. + bool HasBounds() const; + + // Returns the bounds of the mesh. + filament::Box GetBounds() const; Mesh(const Mesh&) = delete; Mesh& operator=(const Mesh&) = delete; private: + void BuildVertexBuffer(const MeshData& data); + void BuildIndexBuffer(const MeshData& data); + void UpdateBounds(const MeshData& data); + + filament::math::float4* BuildOrientationsFromNormals( + int nvertices, const VertexAttribute& normals); + + void ReleaseResources(); + filament::Engine* engine_ = nullptr; filament::IndexBuffer* index_buffer_ = nullptr; filament::VertexBuffer* vertex_buffer_ = nullptr; filament::RenderableManager::PrimitiveType type_ = filament::RenderableManager::PrimitiveType::TRIANGLES; std::optional bounds_; + std::vector> release_callbacks_; }; using MeshPtr = std::unique_ptr; From 9f3babd60c34d518388649aad6d961e497eab825 Mon Sep 17 00:00:00 2001 From: Matija Kecman Date: Tue, 7 Apr 2026 04:57:45 -0700 Subject: [PATCH 014/251] Group platform components into descriptive subfolders PiperOrigin-RevId: 895832492 Change-Id: I6cc56b0e4b002b2998579cde95f4b88dfbd88184 --- src/experimental/platform/CMakeLists.txt | 78 +++++++++---------- .../platform/{ => hal}/egl_utils.cc | 2 +- .../platform/{ => hal}/egl_utils.h | 6 +- .../platform/{ => hal}/graphics_mode.cc | 3 +- .../platform/{ => hal}/graphics_mode.h | 6 +- .../platform/{ => hal}/renderer.cc | 26 +++---- .../platform/{ => hal}/renderer.h | 8 +- src/experimental/platform/{ => hal}/window.cc | 38 ++++----- src/experimental/platform/{ => hal}/window.h | 10 +-- .../platform/{ => hal}/window_osx.mm | 0 .../platform/{ => sim}/model_holder.cc | 2 +- .../platform/{ => sim}/model_holder.h | 6 +- .../platform/{ => sim}/sim_history.cc | 3 +- .../platform/{ => sim}/sim_history.h | 6 +- .../platform/{ => sim}/sim_profiler.cc | 4 +- .../platform/{ => sim}/sim_profiler.h | 6 +- .../platform/{ => sim}/step_control.cc | 2 +- .../platform/{ => sim}/step_control.h | 6 +- .../platform/{ => ux}/enum_utils.h | 6 +- .../platform/{ => ux}/file_dialog.cc | 3 +- .../platform/{ => ux}/file_dialog.h | 6 +- .../platform/{ => ux}/file_dialog_cocoa.mm | 2 +- .../platform/{ => ux}/file_dialog_win.cc | 2 +- .../platform/{ => ux}/file_dialog_zenity.cc | 8 +- src/experimental/platform/{ => ux}/gui.cc | 6 +- src/experimental/platform/{ => ux}/gui.h | 6 +- .../platform/{ => ux}/gui_spec.cc | 6 +- src/experimental/platform/{ => ux}/gui_spec.h | 8 +- .../platform/{ => ux}/imgui_widgets.cc | 37 ++++----- .../platform/{ => ux}/imgui_widgets.h | 20 +++-- .../platform/{ => ux}/interaction.cc | 18 +++-- .../platform/{ => ux}/interaction.h | 11 ++- .../{ => ux}/object_launcher_plugin.cc | 11 ++- .../platform/{ => ux}/picture_gui.cc | 10 +-- .../platform/{ => ux}/picture_gui.h | 10 +-- src/experimental/platform/{ => ux}/plugin.cc | 2 +- src/experimental/platform/{ => ux}/plugin.h | 8 +- .../platform/{ => ux}/spec_editor.cc | 36 ++++----- .../platform/{ => ux}/spec_editor.h | 16 ++-- src/experimental/studio/app.cc | 44 +++++------ src/experimental/studio/app.h | 24 +++--- src/experimental/studio/main.cc | 2 +- src/experimental/studio/wasm.cc | 2 +- 43 files changed, 252 insertions(+), 264 deletions(-) rename src/experimental/platform/{ => hal}/egl_utils.cc (99%) rename src/experimental/platform/{ => hal}/egl_utils.h (83%) rename src/experimental/platform/{ => hal}/graphics_mode.cc (98%) rename src/experimental/platform/{ => hal}/graphics_mode.h (90%) rename src/experimental/platform/{ => hal}/renderer.cc (93%) rename src/experimental/platform/{ => hal}/renderer.h (94%) rename src/experimental/platform/{ => hal}/window.cc (90%) rename src/experimental/platform/{ => hal}/window.h (93%) rename src/experimental/platform/{ => hal}/window_osx.mm (100%) rename src/experimental/platform/{ => sim}/model_holder.cc (98%) rename src/experimental/platform/{ => sim}/model_holder.h (93%) rename src/experimental/platform/{ => sim}/sim_history.cc (97%) rename src/experimental/platform/{ => sim}/sim_history.h (94%) rename src/experimental/platform/{ => sim}/sim_profiler.cc (99%) rename src/experimental/platform/{ => sim}/sim_profiler.h (88%) rename src/experimental/platform/{ => sim}/step_control.cc (99%) rename src/experimental/platform/{ => sim}/step_control.h (95%) rename src/experimental/platform/{ => ux}/enum_utils.h (96%) rename src/experimental/platform/{ => ux}/file_dialog.cc (97%) rename src/experimental/platform/{ => ux}/file_dialog.h (90%) rename src/experimental/platform/{ => ux}/file_dialog_cocoa.mm (97%) rename src/experimental/platform/{ => ux}/file_dialog_win.cc (99%) rename src/experimental/platform/{ => ux}/file_dialog_zenity.cc (98%) rename src/experimental/platform/{ => ux}/gui.cc (99%) rename src/experimental/platform/{ => ux}/gui.h (97%) rename src/experimental/platform/{ => ux}/gui_spec.cc (99%) rename src/experimental/platform/{ => ux}/gui_spec.h (88%) rename src/experimental/platform/{ => ux}/imgui_widgets.cc (94%) rename src/experimental/platform/{ => ux}/imgui_widgets.h (97%) rename src/experimental/platform/{ => ux}/interaction.cc (97%) rename src/experimental/platform/{ => ux}/interaction.h (92%) rename src/experimental/platform/{ => ux}/object_launcher_plugin.cc (96%) rename src/experimental/platform/{ => ux}/picture_gui.cc (93%) rename src/experimental/platform/{ => ux}/picture_gui.h (80%) rename src/experimental/platform/{ => ux}/plugin.cc (98%) rename src/experimental/platform/{ => ux}/plugin.h (96%) rename src/experimental/platform/{ => ux}/spec_editor.cc (93%) rename src/experimental/platform/{ => ux}/spec_editor.h (92%) diff --git a/src/experimental/platform/CMakeLists.txt b/src/experimental/platform/CMakeLists.txt index 733b9ad4..4bf46f26 100644 --- a/src/experimental/platform/CMakeLists.txt +++ b/src/experimental/platform/CMakeLists.txt @@ -20,67 +20,67 @@ add_library(${MUJOCO_PLATFORM_TARGET_NAME} STATIC) target_sources(${MUJOCO_PLATFORM_TARGET_NAME} PUBLIC - egl_utils.cc - egl_utils.h - enum_utils.h - file_dialog.h - graphics_mode.cc - graphics_mode.h - gui.cc - gui.h - gui_spec.cc - gui_spec.h helpers.cc helpers.h - imgui_widgets.cc - imgui_widgets.h - interaction.cc - interaction.h - model_holder.cc - model_holder.h - picture_gui.h - picture_gui.cc - plugin.cc - plugin.h - renderer.cc - renderer.h - sim_history.cc - sim_history.h - sim_profiler.cc - sim_profiler.h - spec_editor.cc - spec_editor.h - step_control.cc - step_control.h - window.cc - window.h + hal/egl_utils.cc + hal/egl_utils.h + hal/graphics_mode.cc + hal/graphics_mode.h + hal/renderer.cc + hal/renderer.h + hal/window.cc + hal/window.h + sim/model_holder.cc + sim/model_holder.h + sim/sim_history.cc + sim/sim_history.h + sim/sim_profiler.cc + sim/sim_profiler.h + sim/step_control.cc + sim/step_control.h + ux/enum_utils.h + ux/file_dialog.h + ux/gui.cc + ux/gui.h + ux/gui_spec.cc + ux/gui_spec.h + ux/imgui_widgets.cc + ux/imgui_widgets.h + ux/interaction.cc + ux/interaction.h + ux/picture_gui.h + ux/picture_gui.cc + ux/plugin.cc + ux/plugin.h + ux/spec_editor.cc + ux/spec_editor.h ) if(NOT WIN32) target_sources(${MUJOCO_PLATFORM_TARGET_NAME} PUBLIC - object_launcher_plugin.cc + ux/object_launcher_plugin.cc ) endif() if(APPLE) - set_source_files_properties(window_osx.mm PROPERTIES + set_source_files_properties(hal/window_osx.mm PROPERTIES COMPILE_FLAGS "-x objective-c++") target_sources(${MUJOCO_PLATFORM_TARGET_NAME} PUBLIC - window_osx.mm - file_dialog_cocoa.mm + hal/window_osx.mm + ux/file_dialog_cocoa.mm ) elseif(UNIX AND NOT APPLE) target_sources(${MUJOCO_PLATFORM_TARGET_NAME} PUBLIC - file_dialog_zenity.cc + ux/file_dialog_zenity.cc ) elseif(WIN32) target_sources(${MUJOCO_PLATFORM_TARGET_NAME} PUBLIC - file_dialog_win.cc + ux/file_dialog_win.cc ) else() target_sources(${MUJOCO_PLATFORM_TARGET_NAME} PUBLIC - file_dialog.cc + ux/file_dialog.cc ) endif() diff --git a/src/experimental/platform/egl_utils.cc b/src/experimental/platform/hal/egl_utils.cc similarity index 99% rename from src/experimental/platform/egl_utils.cc rename to src/experimental/platform/hal/egl_utils.cc index a91109ad..7ec0fc48 100644 --- a/src/experimental/platform/egl_utils.cc +++ b/src/experimental/platform/hal/egl_utils.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/egl_utils.h" +#include "experimental/platform/hal/egl_utils.h" #include diff --git a/src/experimental/platform/egl_utils.h b/src/experimental/platform/hal/egl_utils.h similarity index 83% rename from src/experimental/platform/egl_utils.h rename to src/experimental/platform/hal/egl_utils.h index dc01dfda..342b15b9 100644 --- a/src/experimental/platform/egl_utils.h +++ b/src/experimental/platform/hal/egl_utils.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_EGL_UTILS_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_EGL_UTILS_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_HAL_EGL_UTILS_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_HAL_EGL_UTILS_H_ #include @@ -25,4 +25,4 @@ std::shared_ptr CreateEglContext(); } // namespace mujoco::platform -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_EGL_UTILS_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_HAL_EGL_UTILS_H_ diff --git a/src/experimental/platform/graphics_mode.cc b/src/experimental/platform/hal/graphics_mode.cc similarity index 98% rename from src/experimental/platform/graphics_mode.cc rename to src/experimental/platform/hal/graphics_mode.cc index e291ed78..d992f1f4 100644 --- a/src/experimental/platform/graphics_mode.cc +++ b/src/experimental/platform/hal/graphics_mode.cc @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/graphics_mode.h" +#include "experimental/platform/hal/graphics_mode.h" #include + #include namespace mujoco::platform { diff --git a/src/experimental/platform/graphics_mode.h b/src/experimental/platform/hal/graphics_mode.h similarity index 90% rename from src/experimental/platform/graphics_mode.h rename to src/experimental/platform/hal/graphics_mode.h index e5e79764..d81dfa96 100644 --- a/src/experimental/platform/graphics_mode.h +++ b/src/experimental/platform/hal/graphics_mode.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_GRAPHICS_MODE_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_GRAPHICS_MODE_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_HAL_GRAPHICS_MODE_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_HAL_GRAPHICS_MODE_H_ #include @@ -56,4 +56,4 @@ GraphicsMode GraphicsModeFromString(std::string_view str, } // namespace mujoco::platform -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_GRAPHICS_MODE_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_HAL_GRAPHICS_MODE_H_ diff --git a/src/experimental/platform/renderer.cc b/src/experimental/platform/hal/renderer.cc similarity index 93% rename from src/experimental/platform/renderer.cc rename to src/experimental/platform/hal/renderer.cc index 42c77d5f..4a9be6c1 100644 --- a/src/experimental/platform/renderer.cc +++ b/src/experimental/platform/hal/renderer.cc @@ -12,23 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/renderer.h" +#include "experimental/platform/hal/renderer.h" #include #include #include #include -#include - -#include #include +#include +#include #if !defined(__EMSCRIPTEN__) && !defined(__APPLE__) -#include "experimental/platform/egl_utils.h" +#include "experimental/platform/hal/egl_utils.h" #endif #include "experimental/filament/render_context_filament.h" -#include "experimental/platform/graphics_mode.h" -#include "experimental/platform/plugin.h" +#include "experimental/platform/hal/graphics_mode.h" +#include "experimental/platform/ux/plugin.h" namespace mujoco::platform { @@ -47,9 +46,9 @@ Renderer::Renderer(void* native_window, GraphicsMode gfx) : native_window_(native_window), gfx_(gfx) { if (IsClassic(gfx_)) { if (native_window == nullptr) { - #if !defined(__EMSCRIPTEN__) && !defined(__APPLE__) - graphics_api_context_ = CreateEglContext(); - #endif +#if !defined(__EMSCRIPTEN__) && !defined(__APPLE__) + graphics_api_context_ = CreateEglContext(); +#endif } if (ImGui::GetCurrentContext()) { ImGui_ImplOpenGL3_Init(); @@ -147,8 +146,7 @@ void Renderer::Render(const mjModel* model, mjData* data, vis_option = &default_opt; } - mjv_updateScene(model, data, vis_option, perturb, camera, mjCAT_ALL, - &scene_); + mjv_updateScene(model, data, vis_option, perturb, camera, mjCAT_ALL, &scene_); const bool render_to_texture = !pixels.empty(); if (render_to_texture) { @@ -215,8 +213,8 @@ int Renderer::UploadImage(int texture_id, const std::byte* pixels, int width, return 0; } else { return mjrf_uploadGuiImage(texture_id, - reinterpret_cast(pixels), - width, height, bpp, &render_context_); + reinterpret_cast(pixels), + width, height, bpp, &render_context_); } } diff --git a/src/experimental/platform/renderer.h b/src/experimental/platform/hal/renderer.h similarity index 94% rename from src/experimental/platform/renderer.h rename to src/experimental/platform/hal/renderer.h index 2ac900b5..5efee347 100644 --- a/src/experimental/platform/renderer.h +++ b/src/experimental/platform/hal/renderer.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_RENDERER_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_RENDERER_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_HAL_RENDERER_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_HAL_RENDERER_H_ #include #include @@ -23,7 +23,7 @@ #include #include -#include "experimental/platform/graphics_mode.h" +#include "experimental/platform/hal/graphics_mode.h" namespace mujoco::platform { @@ -113,4 +113,4 @@ class Renderer { } // namespace mujoco::platform -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_RENDERER_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_HAL_RENDERER_H_ diff --git a/src/experimental/platform/window.cc b/src/experimental/platform/hal/window.cc similarity index 90% rename from src/experimental/platform/window.cc rename to src/experimental/platform/hal/window.cc index 466db1a9..faf9a6d1 100644 --- a/src/experimental/platform/window.cc +++ b/src/experimental/platform/hal/window.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/window.h" +#include "experimental/platform/hal/window.h" #include #include @@ -32,10 +32,9 @@ #include #include #include -#include "experimental/platform/graphics_mode.h" +#include "experimental/platform/hal/graphics_mode.h" #include "user/user_resource.h" - // Because X11/Xlib.h defines Status. #ifdef Status #undef Status @@ -126,9 +125,9 @@ Window::Window(std::string_view title, int width, int height, Config config) } const float content_scale = ImGui_ImplSDL2_GetContentScaleForDisplay(0); - sdl_window_ = SDL_CreateWindow(title.data(), SDL_WINDOWPOS_UNDEFINED, - SDL_WINDOWPOS_UNDEFINED, width, - height, window_flags); + sdl_window_ = + SDL_CreateWindow(title.data(), SDL_WINDOWPOS_UNDEFINED, + SDL_WINDOWPOS_UNDEFINED, width, height, window_flags); if (!sdl_window_) { mju_error("Error creating window: %s", SDL_GetError()); } @@ -156,14 +155,14 @@ Window::Window(std::string_view title, int width, int height, Config config) SDL_VERSION(&wmi.version); SDL_GetWindowWMInfo(sdl_window_, &wmi); - #if defined(__linux__) - native_window_ = reinterpret_cast(wmi.info.x11.window); - #elif defined(__WIN32__) - native_window_ = reinterpret_cast(wmi.info.win.window); - #elif defined(__APPLE__) - native_window_ = - GetNativeWindowOsx(reinterpret_cast(wmi.info.cocoa.window)); - #endif +#if defined(__linux__) + native_window_ = reinterpret_cast(wmi.info.x11.window); +#elif defined(__WIN32__) + native_window_ = reinterpret_cast(wmi.info.win.window); +#elif defined(__APPLE__) + native_window_ = + GetNativeWindowOsx(reinterpret_cast(wmi.info.cocoa.window)); +#endif } int drawable_width = width; @@ -244,8 +243,7 @@ void Window::Present(std::span pixels) { SDL_Surface* surface = SDL_GetWindowSurface(sdl_window_); const unsigned char* src = reinterpret_cast(pixels.data()); - unsigned char* dst = - static_cast(surface->pixels); + unsigned char* dst = static_cast(surface->pixels); for (int i = 0; i < height_; ++i) { for (int j = 0; j < width_; ++j) { @@ -260,14 +258,12 @@ void Window::Present(std::span pixels) { } SDL_RenderPresent(sdl_renderer_); - } else if (config_.gfx_mode != GraphicsMode::FilamentVulkan - && config_.gfx_mode != GraphicsMode::FilamentOpenGl) { + } else if (config_.gfx_mode != GraphicsMode::FilamentVulkan && + config_.gfx_mode != GraphicsMode::FilamentOpenGl) { SDL_GL_SwapWindow(sdl_window_); } } -GraphicsMode Window::GetGraphicsMode() const { - return config_.gfx_mode; -} +GraphicsMode Window::GetGraphicsMode() const { return config_.gfx_mode; } } // namespace mujoco::platform diff --git a/src/experimental/platform/window.h b/src/experimental/platform/hal/window.h similarity index 93% rename from src/experimental/platform/window.h rename to src/experimental/platform/hal/window.h index 63927ccb..4e9f504a 100644 --- a/src/experimental/platform/window.h +++ b/src/experimental/platform/hal/window.h @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_WINDOW_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_WINDOW_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_HAL_WINDOW_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_HAL_WINDOW_H_ #include #include #include #include -#include #include -#include "experimental/platform/graphics_mode.h" +#include +#include "experimental/platform/hal/graphics_mode.h" namespace mujoco::platform { @@ -104,4 +104,4 @@ class Window { } // namespace mujoco::platform -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_WINDOW_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_HAL_WINDOW_H_ diff --git a/src/experimental/platform/window_osx.mm b/src/experimental/platform/hal/window_osx.mm similarity index 100% rename from src/experimental/platform/window_osx.mm rename to src/experimental/platform/hal/window_osx.mm diff --git a/src/experimental/platform/model_holder.cc b/src/experimental/platform/sim/model_holder.cc similarity index 98% rename from src/experimental/platform/model_holder.cc rename to src/experimental/platform/sim/model_holder.cc index 52652eaa..402dde8e 100644 --- a/src/experimental/platform/model_holder.cc +++ b/src/experimental/platform/sim/model_holder.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/model_holder.h" +#include "experimental/platform/sim/model_holder.h" #include #include diff --git a/src/experimental/platform/model_holder.h b/src/experimental/platform/sim/model_holder.h similarity index 93% rename from src/experimental/platform/model_holder.h rename to src/experimental/platform/sim/model_holder.h index e14d2c6a..abb13b90 100644 --- a/src/experimental/platform/model_holder.h +++ b/src/experimental/platform/sim/model_holder.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_MODEL_HOLDER_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_MODEL_HOLDER_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_MODEL_HOLDER_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_MODEL_HOLDER_H_ #include #include @@ -81,4 +81,4 @@ class ModelHolder { }; } // namespace mujoco::platform -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_MODEL_HOLDER_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_MODEL_HOLDER_H_ diff --git a/src/experimental/platform/sim_history.cc b/src/experimental/platform/sim/sim_history.cc similarity index 97% rename from src/experimental/platform/sim_history.cc rename to src/experimental/platform/sim/sim_history.cc index db22c6ca..b32d7232 100644 --- a/src/experimental/platform/sim_history.cc +++ b/src/experimental/platform/sim/sim_history.cc @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/sim_history.h" +#include "experimental/platform/sim/sim_history.h" #include #include #include + #include namespace mujoco::platform { diff --git a/src/experimental/platform/sim_history.h b/src/experimental/platform/sim/sim_history.h similarity index 94% rename from src/experimental/platform/sim_history.h rename to src/experimental/platform/sim/sim_history.h index 46601f58..cb1bf065 100644 --- a/src/experimental/platform/sim_history.h +++ b/src/experimental/platform/sim/sim_history.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_HISTORY_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_HISTORY_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_SIM_HISTORY_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_SIM_HISTORY_H_ #include #include @@ -91,4 +91,4 @@ class SimHistory { } // namespace mujoco::platform -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_HISTORY_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_SIM_HISTORY_H_ diff --git a/src/experimental/platform/sim_profiler.cc b/src/experimental/platform/sim/sim_profiler.cc similarity index 99% rename from src/experimental/platform/sim_profiler.cc rename to src/experimental/platform/sim/sim_profiler.cc index a2032db9..7aca0c4c 100644 --- a/src/experimental/platform/sim_profiler.cc +++ b/src/experimental/platform/sim/sim_profiler.cc @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/sim_profiler.h" +#include "experimental/platform/sim/sim_profiler.h" -#include #include #include +#include namespace mujoco::platform { diff --git a/src/experimental/platform/sim_profiler.h b/src/experimental/platform/sim/sim_profiler.h similarity index 88% rename from src/experimental/platform/sim_profiler.h rename to src/experimental/platform/sim/sim_profiler.h index 11887563..4e9cce46 100644 --- a/src/experimental/platform/sim_profiler.h +++ b/src/experimental/platform/sim/sim_profiler.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_PROFILER_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_PROFILER_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_SIM_PROFILER_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_SIM_PROFILER_H_ #include @@ -52,4 +52,4 @@ class SimProfiler { } // namespace mujoco::platform -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_PROFILER_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_SIM_PROFILER_H_ diff --git a/src/experimental/platform/step_control.cc b/src/experimental/platform/sim/step_control.cc similarity index 99% rename from src/experimental/platform/step_control.cc rename to src/experimental/platform/sim/step_control.cc index 38c97fe2..7dbb9340 100644 --- a/src/experimental/platform/step_control.cc +++ b/src/experimental/platform/sim/step_control.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/step_control.h" +#include "experimental/platform/sim/step_control.h" #include #include diff --git a/src/experimental/platform/step_control.h b/src/experimental/platform/sim/step_control.h similarity index 95% rename from src/experimental/platform/step_control.h rename to src/experimental/platform/sim/step_control.h index 7cebbd1b..0fec3025 100644 --- a/src/experimental/platform/step_control.h +++ b/src/experimental/platform/sim/step_control.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_STEP_CONTROL_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_STEP_CONTROL_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_STEP_CONTROL_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_STEP_CONTROL_H_ #include #include @@ -123,4 +123,4 @@ class StepControl { } // namespace mujoco::platform -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_STEP_CONTROL_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SIM_STEP_CONTROL_H_ diff --git a/src/experimental/platform/enum_utils.h b/src/experimental/platform/ux/enum_utils.h similarity index 96% rename from src/experimental/platform/enum_utils.h rename to src/experimental/platform/ux/enum_utils.h index a6e8ec0b..16ca6607 100644 --- a/src/experimental/platform/enum_utils.h +++ b/src/experimental/platform/ux/enum_utils.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_ENUM_UTILS_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_ENUM_UTILS_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_ENUM_UTILS_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_ENUM_UTILS_H_ #include #include @@ -167,4 +167,4 @@ constexpr std::string_view enum_to_string(E value) { } // namespace mujoco::platform::enum_utils -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_ENUM_UTILS_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_ENUM_UTILS_H_ diff --git a/src/experimental/platform/file_dialog.cc b/src/experimental/platform/ux/file_dialog.cc similarity index 97% rename from src/experimental/platform/file_dialog.cc rename to src/experimental/platform/ux/file_dialog.cc index 661b86dd..df1e0675 100644 --- a/src/experimental/platform/file_dialog.cc +++ b/src/experimental/platform/ux/file_dialog.cc @@ -12,10 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/file_dialog.h" +#include "experimental/platform/ux/file_dialog.h" #include - #include #include #include diff --git a/src/experimental/platform/file_dialog.h b/src/experimental/platform/ux/file_dialog.h similarity index 90% rename from src/experimental/platform/file_dialog.h rename to src/experimental/platform/ux/file_dialog.h index 303cf850..2ae80b09 100644 --- a/src/experimental/platform/file_dialog.h +++ b/src/experimental/platform/ux/file_dialog.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_FILE_DIALOG_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_FILE_DIALOG_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_FILE_DIALOG_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_FILE_DIALOG_H_ #include #include @@ -56,4 +56,4 @@ DialogResult SelectPathDialog(std::string_view path); } // namespace mujoco::platform -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_FILE_DIALOG_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_FILE_DIALOG_H_ diff --git a/src/experimental/platform/file_dialog_cocoa.mm b/src/experimental/platform/ux/file_dialog_cocoa.mm similarity index 97% rename from src/experimental/platform/file_dialog_cocoa.mm rename to src/experimental/platform/ux/file_dialog_cocoa.mm index b9caa2bd..a17e20ce 100644 --- a/src/experimental/platform/file_dialog_cocoa.mm +++ b/src/experimental/platform/ux/file_dialog_cocoa.mm @@ -16,7 +16,7 @@ #include #include -#include "experimental/platform/file_dialog.h" +#include "experimental/platform/ux/file_dialog.h" namespace mujoco::platform { diff --git a/src/experimental/platform/file_dialog_win.cc b/src/experimental/platform/ux/file_dialog_win.cc similarity index 99% rename from src/experimental/platform/file_dialog_win.cc rename to src/experimental/platform/ux/file_dialog_win.cc index 8b970bff..37a774df 100644 --- a/src/experimental/platform/file_dialog_win.cc +++ b/src/experimental/platform/ux/file_dialog_win.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/file_dialog.h" +#include "experimental/platform/ux/file_dialog.h" #ifndef UNICODE #define UNICODE diff --git a/src/experimental/platform/file_dialog_zenity.cc b/src/experimental/platform/ux/file_dialog_zenity.cc similarity index 98% rename from src/experimental/platform/file_dialog_zenity.cc rename to src/experimental/platform/ux/file_dialog_zenity.cc index 21d51dc1..20d7a421 100644 --- a/src/experimental/platform/file_dialog_zenity.cc +++ b/src/experimental/platform/ux/file_dialog_zenity.cc @@ -12,10 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/file_dialog.h" - #include -#include #include #include #include @@ -23,11 +20,14 @@ #include #include +#include #include -#include #include +#include #include +#include "experimental/platform/ux/file_dialog.h" + namespace mujoco::platform { static DialogResult RunZenity(std::vector& args) { diff --git a/src/experimental/platform/gui.cc b/src/experimental/platform/ux/gui.cc similarity index 99% rename from src/experimental/platform/gui.cc rename to src/experimental/platform/ux/gui.cc index f9a4c5b8..a5405cb1 100644 --- a/src/experimental/platform/gui.cc +++ b/src/experimental/platform/ux/gui.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/gui.h" +#include "experimental/platform/ux/gui.h" #include #include @@ -26,8 +26,8 @@ #include #include #include "experimental/platform/helpers.h" -#include "experimental/platform/interaction.h" -#include "experimental/platform/imgui_widgets.h" +#include "experimental/platform/ux/imgui_widgets.h" +#include "experimental/platform/ux/interaction.h" namespace mujoco::platform { diff --git a/src/experimental/platform/gui.h b/src/experimental/platform/ux/gui.h similarity index 97% rename from src/experimental/platform/gui.h rename to src/experimental/platform/ux/gui.h index d0b1ff79..cfa8fd80 100644 --- a/src/experimental/platform/gui.h +++ b/src/experimental/platform/ux/gui.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_GUI_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_GUI_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_GUI_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_GUI_H_ // A collection of functions for building ImGui panels for common MuJoCo // visualization and manipulation UX. These functions are primarily used by @@ -131,4 +131,4 @@ void StatsGui(const mjModel* model, const mjData* data, bool paused, float fps); } // namespace mujoco::platform -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_GUI_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_GUI_H_ diff --git a/src/experimental/platform/gui_spec.cc b/src/experimental/platform/ux/gui_spec.cc similarity index 99% rename from src/experimental/platform/gui_spec.cc rename to src/experimental/platform/ux/gui_spec.cc index 7991fc03..d845ab09 100644 --- a/src/experimental/platform/gui_spec.cc +++ b/src/experimental/platform/ux/gui_spec.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/gui_spec.h" +#include "experimental/platform/ux/gui_spec.h" #include #include @@ -20,8 +20,8 @@ #include #include #include -#include "experimental/platform/imgui_widgets.h" -#include "experimental/platform/spec_editor.h" +#include "experimental/platform/ux/imgui_widgets.h" +#include "experimental/platform/ux/spec_editor.h" // Define the mujoco X macros to add fields to the ImGui_DataTable. // We limit the fields to the ones with a matching element by comparing the diff --git a/src/experimental/platform/gui_spec.h b/src/experimental/platform/ux/gui_spec.h similarity index 88% rename from src/experimental/platform/gui_spec.h rename to src/experimental/platform/ux/gui_spec.h index 6ff5b0cc..21b437ab 100644 --- a/src/experimental/platform/gui_spec.h +++ b/src/experimental/platform/ux/gui_spec.h @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_GUI_SPEC_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_GUI_SPEC_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_GUI_SPEC_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_GUI_SPEC_H_ #include -#include "experimental/platform/spec_editor.h" +#include "experimental/platform/ux/spec_editor.h" namespace mujoco::platform { @@ -44,4 +44,4 @@ void ElementModelGui(const mjModel* model, mjsElement* element); } // namespace mujoco::platform -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_GUI_SPEC_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_GUI_SPEC_H_ diff --git a/src/experimental/platform/imgui_widgets.cc b/src/experimental/platform/ux/imgui_widgets.cc similarity index 94% rename from src/experimental/platform/imgui_widgets.cc rename to src/experimental/platform/ux/imgui_widgets.cc index 5e828dae..9ffbbe3e 100644 --- a/src/experimental/platform/imgui_widgets.cc +++ b/src/experimental/platform/ux/imgui_widgets.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/imgui_widgets.h" +#include "experimental/platform/ux/imgui_widgets.h" #include #include @@ -62,8 +62,7 @@ KeyValues ReadIniSection(const std::string& contents, } ImGui_DataPtrTable::ImGui_DataPtrTable(float w1, float w2) { - ImGui::BeginTable("##PropertiesTable", 2, - ImGuiTableFlags_RowBg); + ImGui::BeginTable("##PropertiesTable", 2, ImGuiTableFlags_RowBg); const float width = ImGui::GetContentRegionAvail().x; ImGui::TableSetupColumn("", ImGuiTableColumnFlags_WidthFixed, width * w1); ImGui::TableSetupColumn("", ImGuiTableColumnFlags_WidthFixed, width * w2); @@ -87,43 +86,44 @@ void ImGui_DataPtrTable::DataPtr(const char* label, const uintptr_t* ptr, } void ImGui_DataPtrTable::DataPtr(const char* label, const char* ptr, int index, - int n) { + int n) { MakeLabel(label); ScopedStyle style; style.Color(ImGuiCol_Text, ImColor(255, 0, 0, 255)); ImGui::Text("%s", "(char not implemented, please report bug)"); } -void ImGui_DataPtrTable::DataPtr(const char* label, const mjtByte* ptr, int index, - int n) { +void ImGui_DataPtrTable::DataPtr(const char* label, const mjtByte* ptr, + int index, int n) { for (int i = 0; i < n; ++i) { MakeLabel(label, i, n); ImGui::Text("%s", ptr[index + i] ? "true" : "false"); } } -void ImGui_DataPtrTable::DataPtr(const char* label, const mjtSize* ptr, int index, - int n) { +void ImGui_DataPtrTable::DataPtr(const char* label, const mjtSize* ptr, + int index, int n) { Numeric(label, ptr, index, n); } void ImGui_DataPtrTable::DataPtr(const char* label, const int* ptr, int index, - int n) { + int n) { Numeric(label, ptr, index, n); } void ImGui_DataPtrTable::DataPtr(const char* label, const float* ptr, int index, - int n) { + int n) { Numeric(label, ptr, index, n); } -void ImGui_DataPtrTable::DataPtr(const char* label, const double* ptr, int index, - int n) { +void ImGui_DataPtrTable::DataPtr(const char* label, const double* ptr, + int index, int n) { Numeric(label, ptr, index, n); } template -void ImGui_DataPtrTable::Numeric(const char* label, const T* ptr, int index, int n) { +void ImGui_DataPtrTable::Numeric(const char* label, const T* ptr, int index, + int n) { const T* addr = ptr + index * n; using U = std::conditional_t, float, int>; @@ -137,8 +137,7 @@ void ImGui_DataPtrTable::Numeric(const char* label, const T* ptr, int index, int } } - constexpr const char* fmt1 = - std::is_floating_point_v ? "%f" : "%d"; + constexpr const char* fmt1 = std::is_floating_point_v ? "%f" : "%d"; constexpr const char* fmt2 = std::is_floating_point_v ? "%f %f" : "%d %d"; constexpr const char* fmt3 = @@ -146,9 +145,7 @@ void ImGui_DataPtrTable::Numeric(const char* label, const T* ptr, int index, int constexpr const char* fmt4 = std::is_floating_point_v ? "%f %f %f %f" : "%d %d %d %d"; - auto text1 = [&](int offset) { - ImGui::Text(fmt1, (U)(addr[offset])); - }; + auto text1 = [&](int offset) { ImGui::Text(fmt1, (U)(addr[offset])); }; auto text2 = [&](int offset) { ImGui::Text(fmt2, (U)(addr[offset + 0]), (U)(addr[offset + 1])); }; @@ -299,7 +296,8 @@ void ImGui_SpecElementTable::operator()(const char* name, const char* alt_name, (*this)(name, quat, ref_quat, tooltip); break; case mjORIENTATION_AXISANGLE: - (*this)(aname("axisangle").c_str(), alt.axisangle, ref_alt.axisangle, tooltip); + (*this)(aname("axisangle").c_str(), alt.axisangle, ref_alt.axisangle, + tooltip); break; case mjORIENTATION_XYAXES: (*this)(aname("xyaxes").c_str(), alt.xyaxes, ref_alt.xyaxes, tooltip); @@ -328,7 +326,6 @@ bool ImGui_Slider(const char* name, mjtNum* value, mjtNum min, mjtNum max) { return res; } - bool ImGui_BeginHSplit(const char* id, float* height, bool* open) { const ImVec2 region = ImGui::GetContentRegionAvail(); if (*height < 0) { diff --git a/src/experimental/platform/imgui_widgets.h b/src/experimental/platform/ux/imgui_widgets.h similarity index 97% rename from src/experimental/platform/imgui_widgets.h rename to src/experimental/platform/ux/imgui_widgets.h index 9bf2a4f3..619e1055 100644 --- a/src/experimental/platform/imgui_widgets.h +++ b/src/experimental/platform/ux/imgui_widgets.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_IMGUI_WIDGETS_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_IMGUI_WIDGETS_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_IMGUI_WIDGETS_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_IMGUI_WIDGETS_H_ #include #include @@ -28,7 +28,7 @@ #include #include #include -#include "experimental/platform/enum_utils.h" +#include "experimental/platform/ux/enum_utils.h" namespace mujoco::platform { @@ -140,9 +140,7 @@ struct ScopedStyle { return *this; } - ImVec4 CurrentColor(ImGuiCol col) { - return ImGui::GetStyle().Colors[col]; - } + ImVec4 CurrentColor(ImGuiCol col) { return ImGui::GetStyle().Colors[col]; } void Reset() { ImGui::PopStyleVar(num_vars); @@ -200,8 +198,7 @@ class ImGui_DataPtrTable { class ImGui_SpecElementTable : public ImGui_DataPtrTable { public: explicit ImGui_SpecElementTable(bool read_only = true) - : read_only_(read_only) { - } + : read_only_(read_only) {} // Scalar values used by mjSpec elements. void operator()(const char* label, mjtByte& val, const mjtByte& ref, @@ -273,8 +270,9 @@ class ImGui_SpecElementTable : public ImGui_DataPtrTable { } // Special handling for treating enum values as integers. - template , T>> - void operator()(const char* label, T& val, const T& ref, const char* tooltip) { + template , T>> + void operator()(const char* label, T& val, const T& ref, + const char* tooltip) { Label(label, tooltip); Input(val, ref); } @@ -570,4 +568,4 @@ void MaybeSaveToClipboard(const std::string& contents); } // namespace mujoco::platform -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_IMGUI_WIDGETS_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_IMGUI_WIDGETS_H_ diff --git a/src/experimental/platform/interaction.cc b/src/experimental/platform/ux/interaction.cc similarity index 97% rename from src/experimental/platform/interaction.cc rename to src/experimental/platform/ux/interaction.cc index 149e0c1a..646684b2 100644 --- a/src/experimental/platform/interaction.cc +++ b/src/experimental/platform/ux/interaction.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/interaction.h" +#include "experimental/platform/ux/interaction.h" #include #include @@ -245,7 +245,8 @@ void MoveCamera(const mjModel* m, const mjData* d, mjvCamera* cam, // y movement: either dolly (forward/back) or pedestal (up/down) mju_addToScl3(cam->lookat, - (motion == CameraMotion::TRUCK_PEDESTAL) ? up : forward, dy); + (motion == CameraMotion::TRUCK_PEDESTAL) ? up : forward, + dy); // x movement: camera truck (left/right) mju_addToScl3(cam->lookat, right, dx); @@ -341,8 +342,9 @@ static PickResult PickGeom(const mjModel* m, const mjData* d, const mjtNum ray_pos[3], const mjtNum ray_dir[3], const mjvOption* vis_options) { PickResult result; - result.dist = mj_ray(m, d, ray_pos, ray_dir, vis_options->geomgroup, - vis_options->flags[mjVIS_STATIC], -1, &result.geom, nullptr); + result.dist = + mj_ray(m, d, ray_pos, ray_dir, vis_options->geomgroup, + vis_options->flags[mjVIS_STATIC], -1, &result.geom, nullptr); mju_addScl3(result.point, ray_pos, ray_dir, result.dist); result.body = m->geom_bodyid[result.geom]; return result; @@ -541,8 +543,8 @@ static PickResult PickSkin(const mjModel* m, const mjData* d, int vertid; mjtNum test_dist = mju_raySkin(m->skin_facenum[i], m->skin_vertnum[i], - m->skin_face + 3 * m->skin_faceadr[i], - skinvert, ray_pos, ray_dir, &vertid); + m->skin_face + 3 * m->skin_faceadr[i], + skinvert, ray_pos, ray_dir, &vertid); if (test_dist < 0) { continue; } else if (result.dist >= 0 && test_dist >= result.dist) { @@ -554,9 +556,9 @@ static PickResult PickSkin(const mjModel* m, const mjData* d, // find body with largest weight for this vertex float best_weight = -1; for (int j = m->skin_boneadr[i]; - j < m->skin_boneadr[i] + m->skin_bonenum[i]; j++) { + j < m->skin_boneadr[i] + m->skin_bonenum[i]; j++) { for (int k = m->skin_bonevertadr[j]; - k < m->skin_bonevertadr[j] + m->skin_bonevertnum[j]; k++) { + k < m->skin_bonevertadr[j] + m->skin_bonevertnum[j]; k++) { // get vertex id and weight const int vertex_id = m->skin_bonevertid[k]; const float vertex_weight = m->skin_bonevertweight[k]; diff --git a/src/experimental/platform/interaction.h b/src/experimental/platform/ux/interaction.h similarity index 92% rename from src/experimental/platform/interaction.h rename to src/experimental/platform/ux/interaction.h index ef9e8c2c..9e6e9972 100644 --- a/src/experimental/platform/interaction.h +++ b/src/experimental/platform/ux/interaction.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_INTERACTION_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_INTERACTION_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_INTERACTION_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_INTERACTION_H_ #include @@ -22,7 +22,7 @@ namespace mujoco::platform { // The result of a pick operation. struct PickResult { mjtNum point[3] = {0, 0, 0}; // World coordinates - mjtNum dist = -1; // Distance from the camera. + mjtNum dist = -1; // Distance from the camera. int body = -1; int geom = -1; int flex = -1; @@ -77,9 +77,8 @@ void InitPerturb(const mjModel* m, const mjData* d, const mjvCamera* cam, mjvPerturb* pert, mjtPertBit active); void MovePerturb(const mjModel* m, const mjData* d, const mjvCamera* cam, - mjvPerturb* pert, mjtMouse action, mjtNum reldx, - mjtNum reldy); + mjvPerturb* pert, mjtMouse action, mjtNum reldx, mjtNum reldy); } // namespace mujoco::platform -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_INTERACTION_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_INTERACTION_H_ diff --git a/src/experimental/platform/object_launcher_plugin.cc b/src/experimental/platform/ux/object_launcher_plugin.cc similarity index 96% rename from src/experimental/platform/object_launcher_plugin.cc rename to src/experimental/platform/ux/object_launcher_plugin.cc index ad065bae..d4de9414 100644 --- a/src/experimental/platform/object_launcher_plugin.cc +++ b/src/experimental/platform/ux/object_launcher_plugin.cc @@ -19,8 +19,8 @@ #include #include -#include "experimental/platform/imgui_widgets.h" -#include "experimental/platform/plugin.h" +#include "experimental/platform/ux/imgui_widgets.h" +#include "experimental/platform/ux/plugin.h" namespace mujoco::studio { @@ -53,7 +53,9 @@ class ObjectLauncher { } } - void HandleKeyboardEvent() { if (enabled_) active_ = true; } + void HandleKeyboardEvent() { + if (enabled_) active_ = true; + } bool UpdateSpecPreCompile(mjSpec* spec, const mjModel* model, const mjData* data, const mjvCamera* camera) { @@ -88,7 +90,8 @@ class ObjectLauncher { if (!geom) return false; ObjectInfo& object = objects_.emplace_back(); - object.name = "projectile" + std::to_string(counter_++);; + object.name = "projectile" + std::to_string(counter_++); + ; object.expiration = data->time + lifetime_; mjtNum pos[3]; diff --git a/src/experimental/platform/picture_gui.cc b/src/experimental/platform/ux/picture_gui.cc similarity index 93% rename from src/experimental/platform/picture_gui.cc rename to src/experimental/platform/ux/picture_gui.cc index 33f169af..7281c3d7 100644 --- a/src/experimental/platform/picture_gui.cc +++ b/src/experimental/platform/ux/picture_gui.cc @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/picture_gui.h" +#include "experimental/platform/ux/picture_gui.h" #include #include #include #include -#include "experimental/platform/imgui_widgets.h" -#include "experimental/platform/renderer.h" -#include "experimental/platform/window.h" +#include "experimental/platform/hal/renderer.h" +#include "experimental/platform/hal/window.h" +#include "experimental/platform/ux/imgui_widgets.h" namespace mujoco::platform { @@ -44,7 +44,7 @@ static bool PipGuiImpl(const mjModel* model, mjData* data, const int height = width / window->GetAspectRatio(); std::vector output(width * height * 3); - const int combo_width = (width-30) / 2; + const int combo_width = (width - 30) / 2; ImGui::PushID(pip); ImGui::SetNextItemWidth(combo_width); diff --git a/src/experimental/platform/picture_gui.h b/src/experimental/platform/ux/picture_gui.h similarity index 80% rename from src/experimental/platform/picture_gui.h rename to src/experimental/platform/ux/picture_gui.h index 73abdb2b..ef80d1f4 100644 --- a/src/experimental/platform/picture_gui.h +++ b/src/experimental/platform/ux/picture_gui.h @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_PICTURE_GUI_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_PICTURE_GUI_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_PICTURE_GUI_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_PICTURE_GUI_H_ #include #include -#include "experimental/platform/renderer.h" -#include "experimental/platform/window.h" +#include "experimental/platform/hal/renderer.h" +#include "experimental/platform/hal/window.h" namespace mujoco::platform { @@ -37,4 +37,4 @@ void PipGui(const mjModel* model, mjData* data, platform::Window* window, } // namespace mujoco::platform -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_PICTURE_GUI_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_PICTURE_GUI_H_ diff --git a/src/experimental/platform/plugin.cc b/src/experimental/platform/ux/plugin.cc similarity index 98% rename from src/experimental/platform/plugin.cc rename to src/experimental/platform/ux/plugin.cc index 3ddb6e1b..058e9bd5 100644 --- a/src/experimental/platform/plugin.cc +++ b/src/experimental/platform/ux/plugin.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/plugin.h" +#include "experimental/platform/ux/plugin.h" #include #include diff --git a/src/experimental/platform/plugin.h b/src/experimental/platform/ux/plugin.h similarity index 96% rename from src/experimental/platform/plugin.h rename to src/experimental/platform/ux/plugin.h index 5e47fae9..a8eaf23e 100644 --- a/src/experimental/platform/plugin.h +++ b/src/experimental/platform/ux/plugin.h @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_PLUGIN_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_PLUGIN_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_PLUGIN_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_PLUGIN_H_ #include + #include namespace mujoco::platform { @@ -30,7 +31,6 @@ void RegisterPlugin(T plugin); template void ForEachPlugin(const std::function& fn); - // Plugin for processing custom UI windows. The plugin will be listed in the // "Plugins" main menu and, when selected, an ImGui window will be opened with // the name of the plugin as the title. The `update` function can then be used @@ -123,4 +123,4 @@ struct SpecEditorPlugin final { } // namespace mujoco::platform -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_PLUGIN_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_PLUGIN_H_ diff --git a/src/experimental/platform/spec_editor.cc b/src/experimental/platform/ux/spec_editor.cc similarity index 93% rename from src/experimental/platform/spec_editor.cc rename to src/experimental/platform/ux/spec_editor.cc index dab71b49..38eab60d 100644 --- a/src/experimental/platform/spec_editor.cc +++ b/src/experimental/platform/ux/spec_editor.cc @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/platform/spec_editor.h" +#include "experimental/platform/ux/spec_editor.h" #include #include + #include -#include "experimental/platform/model_holder.h" +#include "experimental/platform/sim/model_holder.h" namespace mujoco::platform { @@ -62,9 +63,7 @@ std::unique_ptr SpecEditor::Compile() { return holder; } -mjSpec* SpecEditor::GetActiveSpec() const { - return active_spec_.get(); -} +mjSpec* SpecEditor::GetActiveSpec() const { return active_spec_.get(); } mjsElement* SpecEditor::AddElement(mjtObj type) { // TODO: check that type is only for spec elements. @@ -135,7 +134,7 @@ void SpecEditor::SetActiveElement(mjsElement* element) { int index = 0; for (mjsElement* iter = mjs_firstElement(spec, type); iter != nullptr; - iter = mjs_nextElement(spec, iter), ++index) { + iter = mjs_nextElement(spec, iter), ++index) { if (iter == element) { active_element_key_ = active_map_.LookupElementKey({type, index}); if (active_element_key_ == kInvalidElementKey) { @@ -171,13 +170,9 @@ void SpecEditor::UpdateReferenceElement() { } } -mjsElement* SpecEditor::GetActiveElement() const { - return active_element_; -} +mjsElement* SpecEditor::GetActiveElement() const { return active_element_; } -mjsElement* SpecEditor::GetRefElement() const { - return ref_element_; -} +mjsElement* SpecEditor::GetRefElement() const { return ref_element_; } void SpecEditor::CommitChanges(mjsElement* element) { if (element == nullptr) { @@ -209,14 +204,13 @@ void SpecEditor::Undo() { active_map_.Insert(entry.key, entry.type_index); } - active_element_ = active_map_.Resolve(active_spec_.get(), active_element_key_); + active_element_ = + active_map_.Resolve(active_spec_.get(), active_element_key_); UpdateReferenceElement(); } } -bool SpecEditor::CanUndo() const { - return cursor_ > 0; -} +bool SpecEditor::CanUndo() const { return cursor_ > 0; } void SpecEditor::Redo() { if (CanRedo()) { @@ -230,14 +224,13 @@ void SpecEditor::Redo() { active_map_.Remove(entry.key); } - active_element_ = active_map_.Resolve(active_spec_.get(), active_element_key_); + active_element_ = + active_map_.Resolve(active_spec_.get(), active_element_key_); UpdateReferenceElement(); } } -bool SpecEditor::CanRedo() const { - return cursor_ < history_.size() - 1; -} +bool SpecEditor::CanRedo() const { return cursor_ < history_.size() - 1; } void SpecEditor::AppendHistory(HistoryEntry entry) { ++cursor_; @@ -312,7 +305,8 @@ SpecEditor::SpecPtr SpecEditor::Copy(const mjSpec* spec) { return SpecPtr(mj_copySpec(spec), mj_deleteSpec); } -mjsElement* SpecEditor::AddElementToSpec(mjSpec* spec, mjtObj type, mjsBody* body) { +mjsElement* SpecEditor::AddElementToSpec(mjSpec* spec, mjtObj type, + mjsBody* body) { if (spec == nullptr || type == mjOBJ_UNKNOWN) { return nullptr; } diff --git a/src/experimental/platform/spec_editor.h b/src/experimental/platform/ux/spec_editor.h similarity index 92% rename from src/experimental/platform/spec_editor.h rename to src/experimental/platform/ux/spec_editor.h index b9338c94..1576396d 100644 --- a/src/experimental/platform/spec_editor.h +++ b/src/experimental/platform/ux/spec_editor.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SPEC_EDITOR_H_ -#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SPEC_EDITOR_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_SPEC_EDITOR_H_ +#define MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_SPEC_EDITOR_H_ #include #include @@ -22,7 +22,7 @@ #include #include -#include "experimental/platform/model_holder.h" +#include "experimental/platform/sim/model_holder.h" namespace mujoco::platform { @@ -139,10 +139,10 @@ class SpecEditor { // A single entry in the history buffer. struct HistoryEntry { - SpecPtr spec; // A full copy of a spec. - Operation op; // The operation performed on the spec. - ElementKey key; // The key of the element being modified. - TypeIndex type_index; // The type/index of the element after the change. + SpecPtr spec; // A full copy of a spec. + Operation op; // The operation performed on the spec. + ElementKey key; // The key of the element being modified. + TypeIndex type_index; // The type/index of the element after the change. }; // Appends a new HistoryEntry to the history buffer. @@ -174,4 +174,4 @@ class SpecEditor { }; } // namespace mujoco::platform -#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_SPEC_EDITOR_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_SPEC_EDITOR_H_ diff --git a/src/experimental/studio/app.cc b/src/experimental/studio/app.cc index 7dadad45..b9cf91fb 100644 --- a/src/experimental/studio/app.cc +++ b/src/experimental/studio/app.cc @@ -35,19 +35,19 @@ #include #include #include -#include "experimental/platform/file_dialog.h" -#include "experimental/platform/graphics_mode.h" -#include "experimental/platform/gui.h" -#include "experimental/platform/gui_spec.h" +#include "experimental/platform/hal/graphics_mode.h" +#include "experimental/platform/hal/renderer.h" +#include "experimental/platform/hal/window.h" #include "experimental/platform/helpers.h" -#include "experimental/platform/imgui_widgets.h" -#include "experimental/platform/interaction.h" -#include "experimental/platform/model_holder.h" -#include "experimental/platform/picture_gui.h" -#include "experimental/platform/plugin.h" -#include "experimental/platform/renderer.h" -#include "experimental/platform/step_control.h" -#include "experimental/platform/window.h" +#include "experimental/platform/sim/model_holder.h" +#include "experimental/platform/sim/step_control.h" +#include "experimental/platform/ux/file_dialog.h" +#include "experimental/platform/ux/gui.h" +#include "experimental/platform/ux/gui_spec.h" +#include "experimental/platform/ux/imgui_widgets.h" +#include "experimental/platform/ux/interaction.h" +#include "experimental/platform/ux/picture_gui.h" +#include "experimental/platform/ux/plugin.h" namespace mujoco::studio { @@ -727,7 +727,7 @@ void App::HandleKeyboardEvents() { ui_.camera_idx = platform::SetCamera(model(), &camera_, ui_.camera_idx - 1); } else if (has_model() && ImGui_IsChordJustPressed(ImGuiKey_RightBracket)) { ui_.camera_idx = platform::SetCamera(model(), &camera_, ui_.camera_idx + 1); - // WASD camera controls for free camera. + // WASD camera controls for free camera. } else if (is_freecam_wasd && (ImGui::IsKeyDown(ImGuiKey_W) || ImGui::IsKeyDown(ImGuiKey_S) || ImGui::IsKeyDown(ImGuiKey_A) || ImGui::IsKeyDown(ImGuiKey_D) || @@ -1129,8 +1129,8 @@ void App::SpecExplorerGui() { mjsElement* element = tmp_.curr_element; bool open = element != nullptr; - if (platform::ImGui_BeginHSplit("SpecExplorerTree", - &tmp_.explorer_split, &open)) { + if (platform::ImGui_BeginHSplit("SpecExplorerTree", &tmp_.explorer_split, + &open)) { platform::SpecTreeGui(&element, spec()); if (element != tmp_.curr_element) { @@ -1147,8 +1147,8 @@ void App::SpecExplorerGui() { } } - if (platform::ImGui_HSplit("SpecExplorerProperties", - &tmp_.explorer_split, &open)) { + if (platform::ImGui_HSplit("SpecExplorerProperties", &tmp_.explorer_split, + &open)) { ImGui::Text("%s", mju_type2Str(tmp_.curr_element->elemtype)); ImGui::SameLine(); ImGui::Text("(%d)", mjs_getId(tmp_.curr_element)); @@ -1434,8 +1434,9 @@ void App::ToolBarGui() { const float label_width = GetExpectedLabelWidth(); const float copy_btn_width = ImGui::CalcTextSize(ICON_COPY_CAMERA).x + ImGui::GetStyle().FramePadding.x * 2; - const float theme_width = ImGui::CalcTextSize(platform::ICON_FA_CIRCLE_O).x + - ImGui::GetStyle().FramePadding.x * 2; + const float theme_width = + ImGui::CalcTextSize(platform::ICON_FA_CIRCLE_O).x + + ImGui::GetStyle().FramePadding.x * 2; const float sp = ImGui::GetStyle().ItemSpacing.x; const float right_width = label_width + sp + label_width + sp + label_width + sp + copy_btn_width + sp + @@ -1766,8 +1767,7 @@ void App::MainMenuGui() { } ImGui::Separator(); - - #ifdef __linux__ +#ifdef __linux__ if (ImGui::BeginMenu("Graphics Mode (Experimental)")) { std::optional mode; if (ImGui::MenuItem( @@ -1814,7 +1814,7 @@ void App::MainMenuGui() { } ImGui::EndMenu(); } - #endif // __linux__ +#endif // __linux__ ImGui::EndMenu(); } diff --git a/src/experimental/studio/app.h b/src/experimental/studio/app.h index 7b438d68..222cdf73 100644 --- a/src/experimental/studio/app.h +++ b/src/experimental/studio/app.h @@ -26,18 +26,18 @@ #include #include -#include "experimental/platform/graphics_mode.h" -#include "experimental/platform/gui.h" -#include "experimental/platform/gui_spec.h" -#include "experimental/platform/interaction.h" -#include "experimental/platform/model_holder.h" -#include "experimental/platform/picture_gui.h" -#include "experimental/platform/renderer.h" -#include "experimental/platform/sim_history.h" -#include "experimental/platform/sim_profiler.h" -#include "experimental/platform/spec_editor.h" -#include "experimental/platform/step_control.h" -#include "experimental/platform/window.h" +#include "experimental/platform/hal/graphics_mode.h" +#include "experimental/platform/hal/renderer.h" +#include "experimental/platform/hal/window.h" +#include "experimental/platform/sim/model_holder.h" +#include "experimental/platform/sim/sim_history.h" +#include "experimental/platform/sim/sim_profiler.h" +#include "experimental/platform/sim/step_control.h" +#include "experimental/platform/ux/gui.h" +#include "experimental/platform/ux/gui_spec.h" +#include "experimental/platform/ux/interaction.h" +#include "experimental/platform/ux/picture_gui.h" +#include "experimental/platform/ux/spec_editor.h" namespace mujoco::studio { diff --git a/src/experimental/studio/main.cc b/src/experimental/studio/main.cc index 87f67351..9d48834f 100644 --- a/src/experimental/studio/main.cc +++ b/src/experimental/studio/main.cc @@ -26,7 +26,7 @@ #include #include #include -#include "experimental/platform/graphics_mode.h" +#include "experimental/platform/hal/graphics_mode.h" #include "experimental/studio/app.h" ABSL_FLAG(int, window_width, 1400, "Window width"); diff --git a/src/experimental/studio/wasm.cc b/src/experimental/studio/wasm.cc index 1a4738a7..8e5e8768 100644 --- a/src/experimental/studio/wasm.cc +++ b/src/experimental/studio/wasm.cc @@ -25,7 +25,7 @@ #include #include -#include "experimental/platform/graphics_mode.h" +#include "experimental/platform/hal/graphics_mode.h" #include "experimental/studio/app.h" // Global app instance. Lifetime is controlled by Init/Deinit calls which are From a3c9115d1e01059b5535e973f1d603e7715ee1ff Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 7 Apr 2026 06:40:51 -0700 Subject: [PATCH 015/251] Use MeshData struct for creating Meshes. Consolidate all Vertex and Index buffer creation inside the Mesh class. Users now set data pointers in the MeshData from which the Mesh class will create the filament buffers. PiperOrigin-RevId: 895871213 Change-Id: Ie086fcb5c31adf90e54cba168b03501e21720726 --- .../filament/filament/buffer_util.cc | 117 +-- .../filament/filament/buffer_util.h | 123 --- .../filament/filament/builtins.cc | 819 +++++++----------- .../filament/filament/geom_util.cc | 95 +- .../filament/filament/gui_view.cc | 43 +- .../filament/filament/model_objects.cc | 30 +- .../filament/filament/model_util.cc | 358 +++----- .../filament/filament/model_util.h | 19 +- .../filament/filament/renderables.cc | 4 +- .../filament/filament/vertex_util.h | 54 -- 10 files changed, 579 insertions(+), 1083 deletions(-) diff --git a/src/experimental/filament/filament/buffer_util.cc b/src/experimental/filament/filament/buffer_util.cc index 17f62091..a1ac791a 100644 --- a/src/experimental/filament/filament/buffer_util.cc +++ b/src/experimental/filament/filament/buffer_util.cc @@ -25,7 +25,6 @@ #include #include #include -#include "third_party/filament/libs/filabridge/include/filament/MaterialEnums.h" #include #include #include @@ -89,6 +88,17 @@ int VertexAttributeTypeSize(const VertexAttribute& attrib) { } } +// Fills an index buffer with a basic incrementing sequence. +template +int FillSequence(std::byte* buffer, std::size_t num_bytes) { + const T num = num_bytes / sizeof(T); + T* ptr = reinterpret_cast(buffer); + for (T i = 0; i < num; ++i) { + ptr[i] = i; + } + return num; +} + // Initializes the MeshData to default values. void DefaultMeshData(MeshData* data) { std::memset(data, 0, sizeof(MeshData)); @@ -140,11 +150,7 @@ void Mesh::BuildVertexBuffer(const MeshData& data) { const VertexAttribute* positions = nullptr; const VertexAttribute* normals = nullptr; const VertexAttribute* tangents = nullptr; - - // Calculate the stride of the vertex buffer. - int stride = 0; for (int i = 0; i < data.nattributes; ++i) { - stride += VertexAttributeTypeSize(data.attributes[i]); if (data.attributes[i].usage == mjVERTEX_ATTRIBUTE_POSITION) { positions = &data.attributes[i]; } else if (data.attributes[i].usage == mjVERTEX_ATTRIBUTE_NORMAL) { @@ -153,7 +159,6 @@ void Mesh::BuildVertexBuffer(const MeshData& data) { tangents = &data.attributes[i]; } } - if (!positions) { mju_error("MeshData has no positions."); } @@ -171,47 +176,66 @@ void Mesh::BuildVertexBuffer(const MeshData& data) { // Build the vertex buffer. filament::VertexBuffer::Builder vb_builder; - vb_builder.bufferCount(data.interleaved ? 1 : data.nattributes); vb_builder.vertexCount(data.nvertices); - int offset = 0; - for (int i = 0; i < data.nattributes; ++i) { - const VertexAttribute& attrib = data.attributes[i]; - const filament::VertexAttribute usage = GetUsage(attrib); - const filament::VertexBuffer::AttributeType type = GetType(attrib); - if (data.interleaved) { - vb_builder.attribute(usage, 0, type, offset, stride); - } else { - vb_builder.attribute(usage, i, type); - } - if (usage == filament::VertexAttribute::COLOR) { - vb_builder.normalized(usage); - } - offset += VertexAttributeTypeSize(attrib); - } - vertex_buffer_ = vb_builder.build(*engine_); - if (data.interleaved) { - const VertexAttribute& attrib = data.attributes[0]; - const size_t nbytes = data.nvertices * stride; - filament::backend::BufferDescriptor desc(attrib.bytes, nbytes, callback, - this); - vertex_buffer_->setBufferAt(*engine_, 0, std::move(desc)); - } else { + // For an interleaved vertex buffer, we will create a single buffer which + // contains the data in the order specified by the attributes array, + // starting from the first attribute's payload. + vb_builder.bufferCount(1); + int total_vertex_size = 0; + for (int i = 0; i < data.nattributes; ++i) { + total_vertex_size += VertexAttributeTypeSize(data.attributes[i]); + } + const void* bytes = data.attributes[0].bytes; + const size_t nbytes = data.nvertices * total_vertex_size; + + // We assume the buffer is tightly packed with no padding between + // attributes. As such, the stride is equal to the total vertex size and + // each offset is the sum of the sizes of the preceding attributes. + int offset = 0; for (int i = 0; i < data.nattributes; ++i) { const VertexAttribute& attrib = data.attributes[i]; - const size_t nbytes = data.nvertices * VertexAttributeTypeSize(attrib); - if (attrib.usage == mjVERTEX_ATTRIBUTE_NORMAL) { - const float4* orientations = - BuildOrientationsFromNormals(data.nvertices, attrib); - filament::backend::BufferDescriptor desc(orientations, nbytes, callback, - this); - vertex_buffer_->setBufferAt(*engine_, i, std::move(desc)); - } else { - filament::backend::BufferDescriptor desc(attrib.bytes, nbytes, callback, - this); - vertex_buffer_->setBufferAt(*engine_, i, std::move(desc)); + const filament::VertexAttribute usage = GetUsage(attrib); + filament::VertexBuffer::AttributeType type = GetType(attrib); + vb_builder.attribute(usage, 0, type, offset, total_vertex_size); + if (usage == filament::VertexAttribute::COLOR) { + vb_builder.normalized(usage); } + offset += VertexAttributeTypeSize(attrib); + } + vertex_buffer_ = vb_builder.build(*engine_); + vertex_buffer_->setBufferAt(*engine_, 0, {bytes, nbytes, callback, this}); + } else { + // For a non-interleaved vertex buffer, we assign a separate buffer to each + // attribute. + vb_builder.bufferCount(data.nattributes); + for (int i = 0; i < data.nattributes; ++i) { + const VertexAttribute& attrib = data.attributes[i]; + const filament::VertexAttribute usage = GetUsage(attrib); + filament::VertexBuffer::AttributeType type = GetType(attrib); + if (attrib.usage == mjVERTEX_ATTRIBUTE_NORMAL) { + // We will replace normals with orientations. + type = filament::VertexBuffer::AttributeType::FLOAT4; + } + vb_builder.attribute(usage, i, type); + if (usage == filament::VertexAttribute::COLOR) { + vb_builder.normalized(usage); + } + } + vertex_buffer_ = vb_builder.build(*engine_); + + // Assign the individual data buffers. + for (int i = 0; i < data.nattributes; ++i) { + const VertexAttribute& attrib = data.attributes[i]; + const void* bytes = attrib.bytes; + size_t nbytes = data.nvertices * VertexAttributeTypeSize(attrib); + if (attrib.usage == mjVERTEX_ATTRIBUTE_NORMAL) { + // Replace normals with orientations. + nbytes = data.nvertices * sizeof(float4); + bytes = BuildOrientationsFromNormals(data.nvertices, attrib); + } + vertex_buffer_->setBufferAt(*engine_, i, {bytes, nbytes, callback, this}); } } } @@ -318,15 +342,4 @@ bool Mesh::HasBounds() const { filament::Box Mesh::GetBounds() const { return bounds_.value(); } - -filament::backend::BufferDescriptor CreateBufferDescriptor( - std::size_t num_bytes, const FillBufferFn& fill) { - std::byte* bytes = new std::byte[num_bytes]; - fill(bytes, num_bytes); - const auto callback = [](void* buffer, size_t size, void* user) { - auto* ptr = reinterpret_cast(user); - delete[] ptr; - }; - return filament::backend::BufferDescriptor(bytes, num_bytes, callback, bytes); -} } // namespace mujoco diff --git a/src/experimental/filament/filament/buffer_util.h b/src/experimental/filament/filament/buffer_util.h index d72afedb..4bc6dd70 100644 --- a/src/experimental/filament/filament/buffer_util.h +++ b/src/experimental/filament/filament/buffer_util.h @@ -16,14 +16,11 @@ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUFFER_UTIL_H_ #include -#include #include #include #include -#include #include -#include #include #include #include @@ -139,18 +136,6 @@ class Mesh { // Creates a Mesh from the given MeshData. Mesh(filament::Engine* engine, const MeshData& data); - // Create a Mesh directly from filament objects. Internal use only. - Mesh(filament::Engine* engine, filament::IndexBuffer* index_buffer, - filament::VertexBuffer* vertex_buffer, - std::optional bounds = std::nullopt, - filament::RenderableManager::PrimitiveType type = - filament::RenderableManager::PrimitiveType::TRIANGLES) - : engine_(engine), - index_buffer_(index_buffer), - vertex_buffer_(vertex_buffer), - type_(type), - bounds_(bounds) {} - ~Mesh(); // Returns the filament IndexBuffer for the mesh. @@ -192,114 +177,6 @@ class Mesh { using MeshPtr = std::unique_ptr; -// Function that fills in the given buffer with actual data. -using FillBufferFn = std::function; - -// Creates and populates a BufferDescriptor (for vertex and index buffers). -filament::backend::BufferDescriptor CreateBufferDescriptor( - std::size_t num_bytes, const FillBufferFn& fill); - -// Creates a filament::VertexBuffer based on the VertexType. The fill function -// will be used to populate the buffer. -template -filament::VertexBuffer* CreateVertexBuffer(filament::Engine* engine, - std::size_t num_vertices, - const FillBufferFn& fill) { - int vertex_size = 0; - if constexpr (VertexType::kHasPosition) { - vertex_size += sizeof(VertexType::position); - } - if constexpr (VertexType::kHasPosition2d) { - vertex_size += sizeof(VertexType::position); - } - if constexpr (VertexType::kHasOrientation) { - vertex_size += sizeof(VertexType::orientation); - } - if constexpr (VertexType::kHasUv) { - vertex_size += sizeof(VertexType::uv); - } - if constexpr (VertexType::kHasColor) { - vertex_size += sizeof(VertexType::color); - } - - auto builder = filament::VertexBuffer::Builder(); - builder.bufferCount(1); - builder.vertexCount(num_vertices); - - int offset = 0; - if constexpr (VertexType::kHasPosition) { - builder.attribute(filament::VertexAttribute::POSITION, 0, - filament::VertexBuffer::AttributeType::FLOAT3, offset, - vertex_size); - offset += sizeof(VertexType::position); - } - if constexpr (VertexType::kHasPosition2d) { - builder.attribute(filament::VertexAttribute::POSITION, 0, - filament::VertexBuffer::AttributeType::FLOAT2, offset, - vertex_size); - offset += sizeof(VertexType::position); - } - if constexpr (VertexType::kHasOrientation) { - builder.attribute(filament::VertexAttribute::TANGENTS, 0, - filament::VertexBuffer::AttributeType::FLOAT4, offset, - vertex_size); - offset += sizeof(VertexType::orientation); - } - if constexpr (VertexType::kHasUv) { - builder.attribute(filament::VertexAttribute::UV0, 0, - filament::VertexBuffer::AttributeType::FLOAT2, offset, - vertex_size); - offset += sizeof(VertexType::uv); - } - if constexpr (VertexType::kHasColor) { - builder.attribute(filament::VertexAttribute::COLOR, 0, - filament::VertexBuffer::AttributeType::UBYTE4, offset, - vertex_size); - builder.normalized(filament::VertexAttribute::COLOR); - offset += sizeof(VertexType::color); - } - - auto vb = builder.build(*engine); - const std::size_t buffer_size = num_vertices * vertex_size; - vb->setBufferAt(*engine, 0, CreateBufferDescriptor(buffer_size, fill)); - return vb; -} - -// Creates a filament::IndexBuffer. The IndexType should be either uin16_t or -// uint32_t. The fill function will be used to populate the buffer. -template -filament::IndexBuffer* CreateIndexBuffer(filament::Engine* engine, - std::size_t num_indices, - const FillBufferFn& fill) { - static_assert(std::is_same::value || - std::is_same::value); - - constexpr auto type = std::is_same::value - ? filament::IndexBuffer::IndexType::USHORT - : filament::IndexBuffer::IndexType::UINT; - - auto builder = filament::IndexBuffer::Builder(); - builder.bufferType(type); - builder.indexCount(num_indices); - - auto ib = builder.build(*engine); - - const std::size_t buffer_size = num_indices * sizeof(IndexType); - ib->setBuffer(*engine, CreateBufferDescriptor(buffer_size, fill)); - return ib; -} - -// Fills an index buffer with a basic incrementing sequence. -template -int FillSequence(std::byte* buffer, std::size_t num_bytes) { - const T num = num_bytes / sizeof(T); - T* ptr = reinterpret_cast(buffer); - for (T i = 0; i < num; ++i) { - ptr[i] = i; - } - return num; -} - } // namespace mujoco #endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUFFER_UTIL_H_ diff --git a/src/experimental/filament/filament/builtins.cc b/src/experimental/filament/filament/builtins.cc index 3e4732e4..21f6c7cc 100644 --- a/src/experimental/filament/filament/builtins.cc +++ b/src/experimental/filament/filament/builtins.cc @@ -19,13 +19,14 @@ #include #include #include +#include +#include #include #include #include #include #include -#include #include "experimental/filament/filament/buffer_util.h" #include "experimental/filament/filament/vertex_util.h" @@ -40,263 +41,227 @@ static constexpr size_t kNumVerticesPerQuad = 4; static constexpr size_t kNumIndicesPerTriangle = 3; static constexpr size_t kNumIndicesPerQuad = 6; -static int AppendQuadIndices(uint16_t* ptr, int idx, uint16_t a, uint16_t b, - uint16_t c, uint16_t d) { - ptr[idx++] = a; - ptr[idx++] = b; - ptr[idx++] = c; - ptr[idx++] = a; - ptr[idx++] = c; - ptr[idx++] = d; - return idx; +static void AppendQuadIndices(std::vector& vec, uint16_t a, + uint16_t b, uint16_t c, uint16_t d) { + vec.push_back(a); + vec.push_back(b); + vec.push_back(c); + vec.push_back(a); + vec.push_back(c); + vec.push_back(d); } -std::size_t NumVerticesPerSide(int num_quads_per_axis) { +static std::size_t NumVerticesPerSide(int num_quads_per_axis) { return (num_quads_per_axis + 1) * (num_quads_per_axis + 1); } -std::size_t NumIndicesPerSide(int num_quads_per_axis) { +static std::size_t NumIndicesPerSide(int num_quads_per_axis) { return kNumIndicesPerQuad * num_quads_per_axis * num_quads_per_axis; } -class LineBuilder { +class BuiltinBuilder : MeshData { public: - using VertexType = VertexNoUv; - using IndexType = uint16_t; - static constexpr filament::RenderableManager::PrimitiveType kPrimitiveType = - filament::RenderableManager::PrimitiveType::LINES; + BuiltinBuilder() { DefaultMeshData(this); } + virtual ~BuiltinBuilder() = default; - explicit LineBuilder() {} - - std::size_t NumVertices() const { - return 2; + template + static MeshPtr Create(filament::Engine* engine, Args&&... args) { + auto builder = new T(std::forward(args)...); + MeshData* mesh_data = builder->PrepareMeshData(); + mesh_data->release_callback = +[](void* user_data) { + delete static_cast(user_data); + }; + mesh_data->user_data = builder; + return std::make_unique(engine, *mesh_data); } - std::size_t NumIndices() const { - return 2; + MeshData* PrepareMeshData() { + // Update the `MeshData` fields. + nattributes = 2; + attributes[0].usage = mjVERTEX_ATTRIBUTE_POSITION; + attributes[0].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT3; + attributes[0].bytes = reinterpret_cast(positions_.data()); + attributes[1].usage = mjVERTEX_ATTRIBUTE_TANGENTS; + attributes[1].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT4; + attributes[1].bytes = reinterpret_cast(orientations_.data()); + nvertices = positions_.size(); + + indices = indices_.data(); + nindices = indices_.size(); + primitive_type = + primitive_type_ == filament::backend::PrimitiveType::TRIANGLES + ? mjPRIM_TYPE_TRIANGLES + : mjPRIM_TYPE_LINES; + index_type = mjINDEX_TYPE_USHORT; + bounds_min[0] = bounds_.getMin().x; + bounds_min[1] = bounds_.getMin().y; + bounds_min[2] = bounds_.getMin().z; + bounds_max[0] = bounds_.getMax().x; + bounds_max[1] = bounds_.getMax().y; + bounds_max[2] = bounds_.getMax().z; + return this; } - void GenerateVertices(VertexType* ptr, size_t num) const { - constexpr float4 kOrientation = {0, 0, 0, 1}; // Unused for lines. - ptr[0] = VertexType({0, 0, 0}, kOrientation); - ptr[1] = VertexType({0, 0, 1}, kOrientation); - } + protected: + std::vector positions_; + std::vector orientations_; + std::vector indices_; + filament::Box bounds_; + filament::RenderableManager::PrimitiveType primitive_type_ = + filament::RenderableManager::PrimitiveType::TRIANGLES; +}; - void GenerateIndices(IndexType* ptr, size_t num) const { - ptr[0] = 0; - ptr[1] = 1; - } +class LineBuilder : public BuiltinBuilder { + public: + LineBuilder() { + primitive_type_ = filament::RenderableManager::PrimitiveType::LINES; - filament::Box GetBounds() const { - return filament::Box().set({-0.001, -0.001, 0}, {0.001, 0.001, 1}); + positions_.reserve(2); + positions_.emplace_back(0, 0, 0); + positions_.emplace_back(0, 0, 1); + + orientations_.resize(positions_.size(), {0, 0, 0, 1}); + + indices_.reserve(2); + indices_.push_back(0); + indices_.push_back(1); + + bounds_.set({0, 0, 0}, {0, 0, 1}); } }; -class PlaneBuilder { +class PlaneBuilder : public BuiltinBuilder { public: - using VertexType = VertexNoUv; - using IndexType = uint16_t; - static constexpr filament::RenderableManager::PrimitiveType kPrimitiveType = - filament::RenderableManager::PrimitiveType::TRIANGLES; + explicit PlaneBuilder(int num_quads_per_axis) { + const int num_vertices = NumVerticesPerSide(num_quads_per_axis); + positions_.reserve(num_vertices); - explicit PlaneBuilder(int num_quads_per_axis) - : num_quads_per_axis_(num_quads_per_axis), - orientation_(CalculateOrientation({0, 0, 1})) {} - - std::size_t NumVertices() const { - return NumVerticesPerSide(num_quads_per_axis_); - } - - std::size_t NumIndices() const { - return NumIndicesPerSide(num_quads_per_axis_); - } - - void GenerateVertices(VertexType* ptr, size_t num) const { - const float delta = 2.0f / num_quads_per_axis_; - - int idx = 0; - for (int x = 0; x <= num_quads_per_axis_; ++x) { - for (int y = 0; y <= num_quads_per_axis_; ++y) { + const float delta = 2.0f / num_quads_per_axis; + for (int x = 0; x <= num_quads_per_axis; ++x) { + for (int y = 0; y <= num_quads_per_axis; ++y) { const float dx = delta * static_cast(x); const float dy = delta * static_cast(y); - ptr[idx++] = VertexType({dx - 1.0f, dy - 1.0f, 0}, orientation_); + positions_.emplace_back(dx - 1.0f, dy - 1.0f, 0); } } - } - void GenerateIndices(IndexType* ptr, size_t num) const { - int idx = 0; - for (int x = 0; x < num_quads_per_axis_; ++x) { - for (int y = 0; y < num_quads_per_axis_; ++y) { - const int base_idx = x * (num_quads_per_axis_ + 1) + y; + orientations_.resize(positions_.size(), CalculateOrientation({0, 0, 1})); + + const int num_indices = NumIndicesPerSide(num_quads_per_axis); + indices_.reserve(num_indices); + for (int x = 0; x < num_quads_per_axis; ++x) { + for (int y = 0; y < num_quads_per_axis; ++y) { + const int base_idx = x * (num_quads_per_axis + 1) + y; const int i0 = base_idx + 0; const int i1 = base_idx + 1; - const int i2 = base_idx + num_quads_per_axis_ + 2; - const int i3 = base_idx + num_quads_per_axis_ + 1; - idx = AppendQuadIndices(ptr, idx, i0, i1, i2, i3); + const int i2 = base_idx + num_quads_per_axis + 2; + const int i3 = base_idx + num_quads_per_axis + 1; + AppendQuadIndices(indices_, i0, i1, i2, i3); } } - } - filament::Box GetBounds() const { - return filament::Box().set({-1, -1, -0.001}, {1, 1, 0.001}); + bounds_.set({-1, -1, -0.001}, {1, 1, 0.001}); } - - private: - int num_quads_per_axis_; - float4 orientation_; }; -class TriangleBuilder { +class TriangleBuilder : public BuiltinBuilder { public: - using VertexType = VertexNoUv; - using IndexType = uint16_t; - static constexpr filament::RenderableManager::PrimitiveType kPrimitiveType = - filament::RenderableManager::PrimitiveType::TRIANGLES; + TriangleBuilder() { + positions_.reserve(3); + positions_.emplace_back(0, 0, 0); + positions_.emplace_back(1, 0, 0); + positions_.emplace_back(0, 1, 0); - TriangleBuilder() - : orientation_(CalculateOrientation({0, 0, 1})) {} + orientations_.resize(positions_.size(), CalculateOrientation({0, 0, 1})); - std::size_t NumVertices() const { - return 3; + indices_.reserve(3); + indices_.emplace_back(0); + indices_.emplace_back(1); + indices_.emplace_back(2); + + bounds_.set({-1, -1, -0.001}, {1, 1, 0.001}); } - - std::size_t NumIndices() const { - return 3; - } - - void GenerateVertices(VertexType* ptr, size_t num) const { - ptr[0] = VertexType({0, 0, 0}, orientation_); - ptr[1] = VertexType({1, 0, 0}, orientation_); - ptr[2] = VertexType({0, 1, 0}, orientation_); - } - - void GenerateIndices(IndexType* ptr, size_t num) const { - ptr[0] = 0; - ptr[1] = 1; - ptr[2] = 2; - } - - filament::Box GetBounds() const { - return filament::Box().set({-1, -1, -0.001}, {1, 1, 0.001}); - } - - private: - float4 orientation_; }; -class LineBoxBuilder { +class LineBoxBuilder : public BuiltinBuilder { public: - using VertexType = VertexNoUv; - using IndexType = uint16_t; - static constexpr filament::RenderableManager::PrimitiveType kPrimitiveType = - filament::RenderableManager::PrimitiveType::LINES; + explicit LineBoxBuilder() { + primitive_type_ = filament::RenderableManager::PrimitiveType::LINES; - explicit LineBoxBuilder() {} + positions_.reserve(8); + positions_.emplace_back(-1.0f, -1.0f, -1.0f); + positions_.emplace_back( 1.0f, -1.0f, -1.0f); + positions_.emplace_back(-1.0f, 1.0f, -1.0f); + positions_.emplace_back( 1.0f, 1.0f, -1.0f); + positions_.emplace_back(-1.0f, -1.0f, 1.0f); + positions_.emplace_back( 1.0f, -1.0f, 1.0f); + positions_.emplace_back(-1.0f, 1.0f, 1.0f); + positions_.emplace_back( 1.0f, 1.0f, 1.0f); - std::size_t NumVertices() const { - return 8; - } + orientations_.resize(positions_.size(), {0, 0, 0, 1}); - std::size_t NumIndices() const { - return 24; - } - - void GenerateVertices(VertexType* ptr, size_t num) const { - constexpr float4 kOrientation = {0, 0, 0, 1}; // Unused for lines. - ptr[0] = VertexType({-1.0f, -1.0f, -1.0f}, kOrientation); - ptr[1] = VertexType({ 1.0f, -1.0f, -1.0f}, kOrientation); - ptr[2] = VertexType({-1.0f, 1.0f, -1.0f}, kOrientation); - ptr[3] = VertexType({ 1.0f, 1.0f, -1.0f}, kOrientation); - ptr[4] = VertexType({-1.0f, -1.0f, 1.0f}, kOrientation); - ptr[5] = VertexType({ 1.0f, -1.0f, 1.0f}, kOrientation); - ptr[6] = VertexType({-1.0f, 1.0f, 1.0f}, kOrientation); - ptr[7] = VertexType({ 1.0f, 1.0f, 1.0f}, kOrientation); - } - - void GenerateIndices(IndexType* ptr, size_t num) const { - // Bottom square (where z == -1). - ptr[0] = 0; - ptr[1] = 1; - ptr[2] = 1; - ptr[3] = 3; - ptr[4] = 3; - ptr[5] = 2; - ptr[6] = 2; - ptr[7] = 0; + indices_.reserve(24); + indices_.push_back(0); + indices_.push_back(1); + indices_.push_back(1); + indices_.push_back(3); + indices_.push_back(3); + indices_.push_back(2); + indices_.push_back(2); + indices_.push_back(0); // Top square (where z == 1). - ptr[8] = 4; - ptr[9] = 5; - ptr[10] = 5; - ptr[11] = 7; - ptr[12] = 7; - ptr[13] = 6; - ptr[14] = 6; - ptr[15] = 4; + indices_.push_back(4); + indices_.push_back(5); + indices_.push_back(5); + indices_.push_back(7); + indices_.push_back(7); + indices_.push_back(6); + indices_.push_back(6); + indices_.push_back(4); // Connect edges from bottom to top. - ptr[16] = 2; - ptr[17] = 6; - ptr[18] = 3; - ptr[19] = 7; - ptr[20] = 0; - ptr[21] = 4; - ptr[22] = 1; - ptr[23] = 5; - } + indices_.push_back(2); + indices_.push_back(6); + indices_.push_back(3); + indices_.push_back(7); + indices_.push_back(0); + indices_.push_back(4); + indices_.push_back(1); + indices_.push_back(5); - filament::Box GetBounds() const { - return filament::Box().set({-1, -1, -1}, {1, 1, 1}); + bounds_.set({-1, -1, -1}, {1, 1, 1}); } }; -class BoxBuilder { +class BoxBuilder : public BuiltinBuilder { public: - using VertexType = VertexNoUv; - using IndexType = uint16_t; - static constexpr filament::RenderableManager::PrimitiveType kPrimitiveType = - filament::RenderableManager::PrimitiveType::TRIANGLES; - static constexpr int kNumSides = 6; explicit BoxBuilder(int num_quads_per_axis) : num_quads_per_axis_(num_quads_per_axis) { quad_size_ = 2.0f / static_cast(num_quads_per_axis_); - } - std::size_t NumVertices() const { - return NumVerticesPerSide(num_quads_per_axis_) * kNumSides; - } - - std::size_t NumIndices() const { - return NumIndicesPerSide(num_quads_per_axis_) * kNumSides; - } - - void GenerateVertices(VertexType* ptr, size_t num) const { - int idx = 0; - idx = GenerateVerticesForSide(ptr, idx, {0, 1, 0}, [](float2 pt) { - return float3{pt.x, 1.0f, pt.y}; - }); - idx = GenerateVerticesForSide(ptr, idx, {0, -1, 0}, [](float2 pt) { - return float3{pt.x, -1.0f, pt.y}; - }); - idx = GenerateVerticesForSide(ptr, idx, {1, 0, 0}, [](float2 pt) { - return float3{1.0f, pt.x, pt.y}; - }); - idx = GenerateVerticesForSide(ptr, idx, {-1, 0, 0}, [](float2 pt) { - return float3{-1.0f, pt.x, pt.y}; - }); - idx = GenerateVerticesForSide(ptr, idx, {0, 0, 1}, [](float2 pt) { - return float3{pt.x, pt.y, 1.0f}; - }); - idx = GenerateVerticesForSide(ptr, idx, {0, 0, -1}, [](float2 pt) { - return float3{pt.x, pt.y, -1.0f}; - }); - } - - void GenerateIndices(IndexType* ptr, size_t num) const { const int vertices_per_side = NumVerticesPerSide(num_quads_per_axis_); + const int indices_per_side = NumIndicesPerSide(num_quads_per_axis_); + const int num_vertices = vertices_per_side * kNumSides; + const int num_indices = indices_per_side * kNumSides; + + positions_.reserve(num_vertices); + orientations_.reserve(num_vertices); + indices_.reserve(num_indices); + + GenerateVerticesForSide({0, 1, 0}, + [](float2 pt) { return float3{pt.x, 1.0f, pt.y}; }); + GenerateVerticesForSide( + {0, -1, 0}, [](float2 pt) { return float3{pt.x, -1.0f, pt.y}; }); + GenerateVerticesForSide({1, 0, 0}, + [](float2 pt) { return float3{1.0f, pt.x, pt.y}; }); + GenerateVerticesForSide( + {-1, 0, 0}, [](float2 pt) { return float3{-1.0f, pt.x, pt.y}; }); + GenerateVerticesForSide({0, 0, 1}, + [](float2 pt) { return float3{pt.x, pt.y, 1.0f}; }); + GenerateVerticesForSide( + {0, 0, -1}, [](float2 pt) { return float3{pt.x, pt.y, -1.0f}; }); - int idx = 0; for (int i = 0; i < kNumSides; ++i) { for (int x = 0; x < num_quads_per_axis_; ++x) { for (int y = 0; y < num_quads_per_axis_; ++y) { @@ -306,279 +271,199 @@ class BoxBuilder { const int i1 = base_idx + 1; const int i2 = base_idx + num_quads_per_axis_ + 2; const int i3 = base_idx + num_quads_per_axis_ + 1; - idx = AppendQuadIndices(ptr, idx, i0, i1, i2, i3); + AppendQuadIndices(indices_, i0, i1, i2, i3); } } } - } - filament::Box GetBounds() const { - return filament::Box().set({-1, -1, -1}, {1, 1, 1}); + bounds_.set({-1, -1, -1}, {1, 1, 1}); } private: template - int GenerateVerticesForSide(VertexType* ptr, int idx, float3 normal, - const F& pt_gen) const { + void GenerateVerticesForSide(float3 normal, const F& pt_gen) { float4 orientation = CalculateOrientation(normal); for (int x = 0; x <= num_quads_per_axis_; ++x) { for (int y = 0; y <= num_quads_per_axis_; ++y) { const float dx = -1.0f + (quad_size_ * static_cast(x)); const float dy = -1.0f + (quad_size_ * static_cast(y)); const float3 position = pt_gen({dx, dy}); - ptr[idx++] = VertexType(position, orientation); + positions_.push_back(position); + orientations_.push_back(orientation); } } - return idx; } int num_quads_per_axis_; float quad_size_; }; -class TubeBuilder { +class TubeBuilder : public BuiltinBuilder { public: - using VertexType = VertexNoUv; - using IndexType = uint16_t; - static constexpr filament::RenderableManager::PrimitiveType kPrimitiveType = - filament::RenderableManager::PrimitiveType::TRIANGLES; + TubeBuilder(int num_stacks, int num_slices) { + const int num_vertices = num_slices * (num_stacks + 1); + positions_.reserve(num_vertices); + orientations_.reserve(num_vertices); - TubeBuilder(int num_stacks, int num_slices) - : num_stacks_(num_stacks), num_slices_(num_slices) {} - - std::size_t NumVertices() const { - return num_slices_ * (num_stacks_ + 1); - } - - std::size_t NumIndices() const { - return kNumIndicesPerQuad * num_slices_ * num_stacks_; - } - - void GenerateVertices(VertexType* ptr, size_t num) const { - const float delta_angle = 2.f * std::numbers::pi / (float)num_slices_; - const float delta_stack = 2.f / static_cast(num_stacks_); - - int idx = 0; - for (int i = 0; i < num_slices_; ++i) { + const float delta_angle = 2.f * std::numbers::pi / (float)num_slices; + const float delta_stack = 2.f / static_cast(num_stacks); + for (int i = 0; i < num_slices; ++i) { const float angle = static_cast(i) * delta_angle; const float2 pt{std::cos(angle), std::sin(angle)}; const float4 orientation = CalculateOrientation({pt.x, pt.y, 0}); - for (int j = 0; j <= num_stacks_; ++j) { + for (int j = 0; j <= num_stacks; ++j) { const float z = -1.0f + (static_cast(j) * delta_stack); - ptr[idx++] = VertexType({pt.x, pt.y, z}, orientation); + positions_.emplace_back(pt.x, pt.y, z); + orientations_.push_back(orientation); } } - } - void GenerateIndices(IndexType* ptr, size_t num) const { - const int num_vertices = NumVertices(); - const int num_vertices_in_spine = num_stacks_ + 1; + const int num_indices = kNumIndicesPerQuad * num_slices * num_stacks; + indices_.reserve(num_indices); - int idx = 0; - for (int i = 0; i < num_slices_; ++i) { - for (int j = 0; j < num_stacks_; ++j) { + const int num_vertices_in_spine = num_stacks + 1; + for (int i = 0; i < num_slices; ++i) { + for (int j = 0; j < num_stacks; ++j) { const int base_idx = (i * num_vertices_in_spine) + j; const int i0 = base_idx + 0; const int i1 = base_idx + 1; - const int i2 = (base_idx + num_stacks_ + 2) % num_vertices; - const int i3 = (base_idx + num_stacks_ + 1) % num_vertices; - idx = AppendQuadIndices(ptr, idx, i0, i1, i2, i3); + const int i2 = (base_idx + num_stacks + 2) % num_vertices; + const int i3 = (base_idx + num_stacks + 1) % num_vertices; + AppendQuadIndices(indices_, i0, i1, i2, i3); } } - } - filament::Box GetBounds() const { - return filament::Box().set({-1, -1, -1}, {1, 1, 1}); + bounds_.set({-1, -1, -1}, {1, 1, 1}); } - - private: - int num_stacks_; - int num_slices_; }; -class ConeBuilder { +class ConeBuilder : public BuiltinBuilder { public: - using VertexType = VertexNoUv; - using IndexType = uint16_t; - static constexpr filament::RenderableManager::PrimitiveType kPrimitiveType = - filament::RenderableManager::PrimitiveType::TRIANGLES; + ConeBuilder(int num_stacks, int num_slices) { + const int num_vertices = + (num_slices * kNumVerticesPerTriangle) + + ((num_stacks - 1) * num_slices * kNumVerticesPerQuad); + positions_.reserve(num_vertices); + orientations_.reserve(num_vertices); - ConeBuilder(int num_stacks, int num_slices) - : num_stacks_(num_stacks), num_slices_(num_slices) {} - - std::size_t NumVertices() const { - return (num_slices_ * kNumVerticesPerTriangle) + - ((num_stacks_ - 1) * num_slices_ * kNumVerticesPerQuad); - } - - std::size_t NumIndices() const { - return (num_slices_ * kNumIndicesPerTriangle) + - ((num_stacks_ - 1) * num_slices_ * kNumIndicesPerQuad); - } - void GenerateVertices(VertexType* ptr, std::size_t num) const { // pole: use triangles const float delta_angle = - 2.0 * std::numbers::pi / static_cast(num_slices_); - const float delta_radius = 1.0f / static_cast(num_stacks_); + 2.0 * std::numbers::pi / static_cast(num_slices); + const float delta_radius = 1.0f / static_cast(num_stacks); - int idx = 0; - for (int j = 0; j < num_slices_; ++j) { + for (int j = 0; j < num_slices; ++j) { const float angle1 = (j + 0) * delta_angle; const float angle2 = (j + 1) * delta_angle; - ptr[idx++] = MakeVert(angle1, delta_radius); - ptr[idx++] = MakeVert(angle2, delta_radius); + AppendVert(angle1, delta_radius); + AppendVert(angle2, delta_radius); - VertexType v3; - v3.position = {0, 0, 1}; - v3.orientation = CalculateOrientation(v3.position); - ptr[idx++] = v3; + positions_.emplace_back(0, 0, 1); + orientations_.emplace_back(CalculateOrientation({0, 0, 1})); } // the rest: use quads - for (int i = 1; i < num_stacks_; ++i) { + for (int i = 1; i < num_stacks; ++i) { const float radius1 = delta_radius * (i + 0); const float radius2 = delta_radius * (i + 1); - for (int j = 0; j < num_slices_; ++j) { + for (int j = 0; j < num_slices; ++j) { const float angle1 = (j + 0) * delta_angle; const float angle2 = (j + 1) * delta_angle; - - ptr[idx++] = MakeVert(angle1, radius2); - ptr[idx++] = MakeVert(angle2, radius2); - ptr[idx++] = MakeVert(angle2, radius1); - ptr[idx++] = MakeVert(angle1, radius1); + AppendVert(angle1, radius2); + AppendVert(angle2, radius2); + AppendVert(angle2, radius1); + AppendVert(angle1, radius1); } } - } - void GenerateIndices(IndexType* ptr, std::size_t num) const { - int idx = 0; - for (int j = 0; j < num_slices_ * 3; ++j) { - ptr[idx] = idx; - ++idx; + const int num_indices = + (num_slices * kNumIndicesPerTriangle) + + ((num_stacks - 1) * num_slices * kNumIndicesPerQuad); + indices_.reserve(num_indices); + for (int j = 0; j < num_slices * 3; ++j) { + indices_.push_back(j); } - int quad_idx = idx; - for (int i = 1; i < num_stacks_; ++i) { - for (int j = 0; j < num_slices_; ++j) { + int quad_idx = num_slices * 3; + for (int i = 1; i < num_stacks; ++i) { + for (int j = 0; j < num_slices; ++j) { const int i0 = quad_idx + 0; const int i1 = quad_idx + 1; const int i2 = quad_idx + 2; const int i3 = quad_idx + 3; quad_idx += 4; - idx = AppendQuadIndices(ptr, idx, i0, i1, i2, i3); + AppendQuadIndices(indices_, i0, i1, i2, i3); } } - } - filament::Box GetBounds() const { - return filament::Box().set({-1, -1, 0}, {1, 1, 1}); + bounds_.set({-1, -1, 0}, {1, 1, 1}); } private: - static VertexType MakeVert(float theta, float radius) { + void AppendVert(float theta, float radius) { static constexpr float kNormalScale = 0.70710678118f; const float cz = std::cos(theta); const float sz = std::sin(theta); const float3 pt{cz * radius, sz * radius, 1.f - radius}; const float3 n{cz * kNormalScale, sz * kNormalScale, kNormalScale}; - return VertexType(pt, CalculateOrientation(n)); + positions_.push_back(pt); + orientations_.push_back(CalculateOrientation(n)); } - - int num_stacks_; - int num_slices_; }; -class DiskBuilder { +class DiskBuilder : public BuiltinBuilder { public: - using VertexType = VertexNoUv; - using IndexType = uint16_t; - static constexpr filament::RenderableManager::PrimitiveType kPrimitiveType = - filament::RenderableManager::PrimitiveType::TRIANGLES; - - explicit DiskBuilder(int num_slices) : num_slices_(num_slices) { - orientation_ = CalculateOrientation({0, 0, 1}); - } - - std::size_t NumVertices() const { - return num_slices_ + 1; - } - - std::size_t NumIndices() const { - return num_slices_ * kNumVerticesPerTriangle; - } - - void GenerateVertices(VertexType* ptr, std::size_t num) const { + explicit DiskBuilder(int num_slices) { + const int num_vertices = num_slices + 1; + positions_.reserve(num_vertices); const float delta_angle = - 2.0 * std::numbers::pi / static_cast(num_slices_); + 2.0 * std::numbers::pi / static_cast(num_slices); - int idx = 0; - ptr[idx++] = VertexType(float3{0, 0, 0}, orientation_); - for (int i = 0; i < num_slices_; ++i) { + positions_.push_back({0, 0, 0}); + for (int i = 0; i < num_slices; ++i) { const float angle = static_cast(i) * delta_angle; const float x = std::cos(angle); const float y = std::sin(angle); - ptr[idx++] = VertexType(float3{x, y, 0}, orientation_); + positions_.push_back({x, y, 0}); } - } - void GenerateIndices(IndexType* ptr, std::size_t num) const { - int idx = 0; - for (int i = 0; i < num_slices_; ++i) { - const int next = i < (num_slices_ - 1) ? i + 1 : 0; - ptr[idx++] = 0; - ptr[idx++] = 1 + i; - ptr[idx++] = 1 + next; + orientations_.resize(positions_.size(), CalculateOrientation({0, 0, 1})); + + const int num_indices = num_slices * kNumVerticesPerTriangle; + indices_.reserve(num_indices); + for (int i = 0; i < num_slices; ++i) { + const int next = i < (num_slices - 1) ? i + 1 : 0; + indices_.push_back(0); + indices_.push_back(1 + i); + indices_.push_back(1 + next); } - } - filament::Box GetBounds() const { - return filament::Box().set({-1, -1, -0.001}, {1, 1, 0.001}); + bounds_.set({-1, -1, -0.001}, {1, 1, 0.001}); } - - private: - int num_slices_; - float4 orientation_; }; -class SphereBuilder { +class SphereBuilder : public BuiltinBuilder { public: - using VertexType = VertexNoUv; - using IndexType = uint16_t; - static constexpr filament::RenderableManager::PrimitiveType kPrimitiveType = - filament::RenderableManager::PrimitiveType::TRIANGLES; + SphereBuilder(int num_stacks, int num_slices) { + static constexpr uint16_t kNorthPoleIndex = 0; + static constexpr uint16_t kSouthPoleIndex = 1; - static constexpr IndexType kNorthPoleIndex = 0; - static constexpr IndexType kSouthPoleIndex = 1; + const int num_vertices = (num_stacks * num_slices) + 2; // +2 for poles + positions_.reserve(num_vertices); + orientations_.reserve(num_vertices); - SphereBuilder(int num_stacks, int num_slices) - : num_stacks_(num_stacks), num_slices_(num_slices) {} - - std::size_t NumVertices() const { - return (num_stacks_ * num_slices_) + 2; // +2 for poles - } - - std::size_t NumIndices() const { - const size_t num_tris_polar_cap = num_slices_; - const size_t num_quads_body = num_slices_ * (num_stacks_ - 1); - return (2 * num_tris_polar_cap * kNumIndicesPerTriangle) + - (num_quads_body * kNumIndicesPerQuad); - } - - void GenerateVertices(VertexType* ptr, size_t num) const { const float lat_angle_delta = - std::numbers::pi / static_cast(num_stacks_ + 1); + std::numbers::pi / static_cast(num_stacks + 1); const float lon_angle_delta = - 2.0 * std::numbers::pi / static_cast(num_slices_); + 2.0 * std::numbers::pi / static_cast(num_slices); // Add the north and south poles. - int idx = 0; - ptr[idx++] = MakeVert(0, 0, 1); - ptr[idx++] = MakeVert(0, 0, -1); + AppendVert(0, 0, 1); + AppendVert(0, 0, -1); // Vertices by latitude. - for (int lat = 0; lat < num_stacks_; ++lat) { + for (int lat = 0; lat < num_stacks; ++lat) { // +1 because we handle the north pole (which would be at a lat angle of // 0-degrees) explicitly. const float lat_angle = static_cast(lat + 1) * lat_angle_delta; @@ -586,7 +471,7 @@ class SphereBuilder { const float sin_lat_angle = std::sin(lat_angle); const float z = cos_lat_angle; - for (int lon = 0; lon < num_slices_; ++lon) { + for (int lon = 0; lon < num_slices; ++lon) { const float lon_angle = static_cast(lon) * lon_angle_delta; const float cos_lon_angle = std::cos(lon_angle); @@ -594,101 +479,83 @@ class SphereBuilder { const float x = sin_lat_angle * cos_lon_angle; const float y = sin_lat_angle * sin_lon_angle; - ptr[idx++] = MakeVert(x, y, z); + AppendVert(x, y, z); } } - } - void GenerateIndices(IndexType* ptr, size_t num) const { - int idx = 0; + const size_t num_tris_polar_cap = num_slices; + const size_t num_quads_body = num_slices * (num_stacks - 1); + const int num_indices = (2 * num_tris_polar_cap * kNumIndicesPerTriangle) + + (num_quads_body * kNumIndicesPerQuad); + indices_.reserve(num_indices); // The first two vertices are the poles, so the first vertex in the first // row starts at index 2. - IndexType row_start = kSouthPoleIndex + 1; + uint16_t row_start = kSouthPoleIndex + 1; // North polar cap. - for (int lon = 0; lon < num_slices_; ++lon) { - const int next = lon < (num_slices_ - 1) ? lon + 1 : 0; - ptr[idx++] = kNorthPoleIndex; - ptr[idx++] = row_start + next; - ptr[idx++] = row_start + lon; + for (int lon = 0; lon < num_slices; ++lon) { + const int next = lon < (num_slices - 1) ? lon + 1 : 0; + indices_.push_back(kNorthPoleIndex); + indices_.push_back(row_start + next); + indices_.push_back(row_start + lon); } // Latitudinal triangle strips. - for (int lat = 0; lat < num_stacks_ - 1; lat++) { - const IndexType north_start = row_start; - const IndexType south_start = row_start + num_slices_; - for (int lon = 0; lon < num_slices_; ++lon) { + for (int lat = 0; lat < num_stacks - 1; lat++) { + const uint16_t north_start = row_start; + const uint16_t south_start = row_start + num_slices; + for (int lon = 0; lon < num_slices; ++lon) { // The offset to the index that is adjacent to the current index. - const int adjacent = lon < (num_slices_ - 1) ? lon + 1 : 0; + const int adjacent = lon < (num_slices - 1) ? lon + 1 : 0; const int i0 = (north_start + lon); const int i1 = (south_start + lon); const int i2 = (south_start + adjacent); const int i3 = (north_start + adjacent); - idx = AppendQuadIndices(ptr, idx, i0, i1, i2, i3); + AppendQuadIndices(indices_, i0, i1, i2, i3); } - row_start += num_slices_; + row_start += num_slices; } // South polar cap. - for (int lon = 0; lon < num_slices_; ++lon) { - const int adjacent = lon < (num_slices_ - 1) ? lon + 1 : 0; - ptr[idx++] = kSouthPoleIndex; - ptr[idx++] = row_start + lon; - ptr[idx++] = row_start + adjacent; + for (int lon = 0; lon < num_slices; ++lon) { + const int adjacent = lon < (num_slices - 1) ? lon + 1 : 0; + indices_.push_back(kSouthPoleIndex); + indices_.push_back(row_start + lon); + indices_.push_back(row_start + adjacent); } - } - filament::Box GetBounds() const { - return filament::Box().set({-1, -1, -1}, {1, 1, 1}); + bounds_.set({-1, -1, -1}, {1, 1, 1}); } private: - static VertexType MakeVert(float x, float y, float z) { + void AppendVert(float x, float y, float z) { const float3 pt{x, y, z}; - return VertexType(pt, CalculateOrientation(pt)); + positions_.push_back(pt); + orientations_.push_back(CalculateOrientation(pt)); } - - int num_stacks_; - int num_slices_; }; -class DomeBuilder { +class DomeBuilder : public BuiltinBuilder { public: - using VertexType = VertexNoUv; - using IndexType = uint16_t; - static constexpr filament::RenderableManager::PrimitiveType kPrimitiveType = - filament::RenderableManager::PrimitiveType::TRIANGLES; + DomeBuilder(int num_stacks, int num_slices) { + static constexpr uint16_t kPoleIndex = 0; - static constexpr IndexType kPoleIndex = 0; + const int num_vertices = (num_stacks * num_slices) + 1; // +1 for poles + positions_.reserve(num_vertices); + orientations_.reserve(num_vertices); - DomeBuilder(int num_stacks, int num_slices) - : num_stacks_(num_stacks), num_slices_(num_slices) {} - - std::size_t NumVertices() const { - return (num_stacks_ * num_slices_) + 1; // +1 for poles - } - - std::size_t NumIndices() const { - const size_t num_tris_polar_cap = num_slices_; - const size_t num_quads_body = num_slices_ * (num_stacks_ - 1); - return (num_tris_polar_cap * kNumIndicesPerTriangle) + - (num_quads_body * kNumIndicesPerQuad); - } - - void GenerateVertices(VertexType* ptr, size_t num) const { const float lat_angle_delta = - 0.5 * std::numbers::pi / static_cast(num_stacks_); + 0.5 * std::numbers::pi / static_cast(num_stacks); const float lon_angle_delta = - 2.0 * std::numbers::pi / static_cast(num_slices_); + 2.0 * std::numbers::pi / static_cast(num_slices); // Add the pole. - int idx = 0; - ptr[idx++] = MakeVert(0, 0, 1); + AppendVert(0, 0, 1); // Vertices by latitude. - for (int lat = 0; lat < num_stacks_; ++lat) { + for (int lat = 0; lat < num_stacks; ++lat) { // +1 because we handle the north pole (which would be at a lat angle of // 0-degrees) explicitly. const float lat_angle = static_cast(lat + 1) * lat_angle_delta; @@ -696,138 +563,102 @@ class DomeBuilder { const float sin_lat_angle = std::sin(lat_angle); const float z = cos_lat_angle; - for (int lon = 0; lon < num_slices_; ++lon) { + for (int lon = 0; lon < num_slices; ++lon) { const float lon_angle = static_cast(lon) * lon_angle_delta; const float cos_lon_angle = std::cos(lon_angle); const float sin_lon_angle = std::sin(lon_angle); const float x = sin_lat_angle * cos_lon_angle; const float y = sin_lat_angle * sin_lon_angle; - ptr[idx++] = MakeVert(x, y, z); + AppendVert(x, y, z); } } - } - void GenerateIndices(IndexType* ptr, size_t num) const { - int idx = 0; + const size_t num_tris_polar_cap = num_slices; + const size_t num_quads_body = num_slices * (num_stacks - 1); + const int num_indices = (num_tris_polar_cap * kNumIndicesPerTriangle) + + (num_quads_body * kNumIndicesPerQuad); + indices_.reserve(num_indices); // The first vertex is the poles, so the first vertex in the first row // starts at index 1. - IndexType row_start = kPoleIndex + 1; + uint16_t row_start = kPoleIndex + 1; // North polar cap. - for (int lon = 0; lon < num_slices_; ++lon) { - const int next = lon < (num_slices_ - 1) ? lon + 1 : 0; - - ptr[idx++] = kPoleIndex; - ptr[idx++] = row_start + next; - ptr[idx++] = row_start + lon; + for (int lon = 0; lon < num_slices; ++lon) { + const int next = lon < (num_slices - 1) ? lon + 1 : 0; + indices_.push_back(kPoleIndex); + indices_.push_back(row_start + next); + indices_.push_back(row_start + lon); } // Latitudinal quad strips. The first "stack" was handled above, so we // only need to iterate over N-1 stacks. - for (int lat = 0; lat < num_stacks_ - 1; lat++) { + for (int lat = 0; lat < num_stacks - 1; lat++) { const int north_start = row_start; - const int south_start = row_start + num_slices_; - for (int lon = 0; lon < num_slices_; ++lon) { + const int south_start = row_start + num_slices; + for (int lon = 0; lon < num_slices; ++lon) { // The offset to the index that is adjacent to the current index. - const int adjacent = lon < (num_slices_ - 1) ? lon + 1 : 0; + const int adjacent = lon < (num_slices - 1) ? lon + 1 : 0; const int i0 = (north_start + lon); const int i1 = (south_start + lon); const int i2 = (south_start + adjacent); const int i3 = (north_start + adjacent); - idx = AppendQuadIndices(ptr, idx, i0, i1, i2, i3); + AppendQuadIndices(indices_, i0, i1, i2, i3); } - row_start += num_slices_; + row_start += num_slices; } - } - filament::Box GetBounds() const { - return filament::Box().set({-1, -1, 0}, {1, 1, 1}); + bounds_.set({-1, -1, 0}, {1, 1, 1}); } private: - static VertexType MakeVert(float x, float y, float z) { + void AppendVert(float x, float y, float z) { const float3 pt{x, y, z}; - return VertexType(pt, CalculateOrientation(pt)); + positions_.push_back(pt); + orientations_.push_back(CalculateOrientation(pt)); } - - int num_stacks_; - int num_slices_; }; -template -MeshPtr CreateFromBuilder(filament::Engine* engine, const T& builder) { - using VertexType = typename T::VertexType; - using IndexType = typename T::IndexType; - - const int num_vertices = builder.NumVertices(); - const int num_indices = builder.NumIndices(); - if (num_vertices == 0 || num_indices == 0) { - return {}; - } - - auto vertices = [&](std::byte* buffer, std::size_t len) { - auto* ptr = reinterpret_cast(buffer); - if (sizeof(*ptr) * num_vertices != len) { - mju_error("Buffer size mismatch."); - } - builder.GenerateVertices(ptr, num_vertices); - }; - - auto indices = [&](std::byte* buffer, std::size_t len) { - auto* ptr = reinterpret_cast(buffer); - if (sizeof(*ptr) * num_indices != len) { - mju_error("Buffer size mismatch."); - } - builder.GenerateIndices(ptr, num_indices); - }; - - auto vb = CreateVertexBuffer(engine, num_vertices, vertices); - auto ib = CreateIndexBuffer(engine, num_indices, indices); - return std::make_unique(engine, ib, vb, builder.GetBounds(), - T::kPrimitiveType); -} - MeshPtr CreateLine(filament::Engine* engine) { - return CreateFromBuilder(engine, LineBuilder()); + return BuiltinBuilder::Create(engine); } MeshPtr CreatePlane(filament::Engine* engine, int nquad) { - return CreateFromBuilder(engine, PlaneBuilder(nquad)); + return BuiltinBuilder::Create(engine, nquad); } MeshPtr CreateTriangle(filament::Engine* engine) { - return CreateFromBuilder(engine, TriangleBuilder()); + return BuiltinBuilder::Create(engine); } MeshPtr CreateBox(filament::Engine* engine, int nquad) { - return CreateFromBuilder(engine, BoxBuilder(nquad)); + return BuiltinBuilder::Create(engine, nquad); } MeshPtr CreateLineBox(filament::Engine* engine) { - return CreateFromBuilder(engine, LineBoxBuilder()); + return BuiltinBuilder::Create(engine); } MeshPtr CreateSphere(filament::Engine* engine, int nstack, int nslice) { - return CreateFromBuilder(engine, SphereBuilder(nstack, nslice)); + return BuiltinBuilder::Create(engine, nstack, nslice); } MeshPtr CreateTube(filament::Engine* engine, int nstack, int nslice) { - return CreateFromBuilder(engine, TubeBuilder(nstack, nslice)); + return BuiltinBuilder::Create(engine, nstack, nslice); } MeshPtr CreateDisk(filament::Engine* engine, int nslice) { - return CreateFromBuilder(engine, DiskBuilder(nslice)); + return BuiltinBuilder::Create(engine, nslice); } MeshPtr CreateDome(filament::Engine* engine, int nstack, int nslice) { - return CreateFromBuilder(engine, DomeBuilder(nstack, nslice)); + return BuiltinBuilder::Create(engine, nstack, nslice); } MeshPtr CreateCone(filament::Engine* engine, int nstack, int nslice) { - return CreateFromBuilder(engine, ConeBuilder(nstack, nslice)); + return BuiltinBuilder::Create(engine, nstack, nslice); } } // namespace mujoco diff --git a/src/experimental/filament/filament/geom_util.cc b/src/experimental/filament/filament/geom_util.cc index 38d7509b..fb64e8d0 100644 --- a/src/experimental/filament/filament/geom_util.cc +++ b/src/experimental/filament/filament/geom_util.cc @@ -14,27 +14,19 @@ #include "experimental/filament/filament/geom_util.h" -#include #include -#include #include #include #include -#include #include #include #include -#include #include #include "experimental/filament/filament/buffer_util.h" -#include "experimental/filament/filament/math_util.h" -#include "experimental/filament/filament/vertex_util.h" namespace mujoco { -using filament::math::float3; - static std::span GetPositions(const mjModel* model, const mjvScene* scene, const mjvGeom& geom) { @@ -106,64 +98,6 @@ static std::span GetIndices(const mjModel* model, } } -template -static void FillVertices(std::byte* buffer, std::size_t len, - std::span positions, - std::span normals, - std::span uvs, - float3* vmin, - float3* vmax) { - const int num_vertices = len / sizeof(T); - T* ptr = reinterpret_cast(buffer); - for (int i = 0; i < num_vertices; ++i) { - ptr->position = ReadFloat3(positions.data(), i); - *vmin = min(*vmin, ptr->position); - *vmax = max(*vmax, ptr->position); - ptr->orientation = CalculateOrientation(ReadFloat3(normals.data(), i)); - if constexpr (T::kHasUv) { - ptr->uv.x = uvs[i * 2]; - ptr->uv.y = uvs[i * 2 + 1]; - } - ++ptr; - } -} - -static filament::VertexBuffer* BuildVertexBuffer( - filament::Engine* engine, std::span positions, - std::span normals, std::span uvs, float3* vmin, - float3* vmax) { - const int num_vertices = positions.size() / 3; - if (uvs.data() != nullptr) { - using VertexType = VertexWithUv; - auto fill = [&](std::byte* buffer, std::size_t len) { - FillVertices(buffer, len, positions, normals, uvs, vmin, - vmax); - }; - return CreateVertexBuffer(engine, num_vertices, fill); - } else { - using VertexType = VertexNoUv; - auto fill = [&](std::byte* buffer, std::size_t len) { - FillVertices(buffer, len, positions, normals, uvs, vmin, - vmax); - }; - return CreateVertexBuffer(engine, num_vertices, fill); - } -} - -static filament::IndexBuffer* BuildIndexBuffer(filament::Engine* engine, - std::span indices, - int num_indices) { - if (indices.data() == nullptr) { - auto fill_indices = FillSequence; - return CreateIndexBuffer(engine, num_indices, fill_indices); - } else { - auto fill_indices = [&](std::byte* buffer, std::size_t len) { - std::memcpy(buffer, indices.data(), len); - }; - return CreateIndexBuffer(engine, indices.size(), fill_indices); - } -} - MeshPtr CreateGeomBuffers(filament::Engine* engine, const mjModel* model, const mjvScene* scene, const mjvGeom& geom) { auto positions = GetPositions(model, scene, geom); @@ -176,13 +110,28 @@ MeshPtr CreateGeomBuffers(filament::Engine* engine, const mjModel* model, num_indices = 3 * scene->flexfaceused[geom.objid]; } - float3 vmin = {FLT_MAX, FLT_MAX, FLT_MAX}; - float3 vmax = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; - auto vertex_buffer = BuildVertexBuffer(engine, positions, normals, uvs, &vmin, &vmax); - auto index_buffer = BuildIndexBuffer(engine, indices, num_indices); - filament::Box bounds; - bounds.set(vmin, vmax); - return std::make_unique(engine, index_buffer, vertex_buffer, bounds); + MeshData data; + DefaultMeshData(&data); + + data.nattributes = uvs.data() ? 3 : 2; + data.attributes[0].usage = mjVERTEX_ATTRIBUTE_POSITION; + data.attributes[0].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT3; + data.attributes[0].bytes = positions.data(); + data.attributes[1].usage = mjVERTEX_ATTRIBUTE_NORMAL; + data.attributes[1].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT3; + data.attributes[1].bytes = normals.data(); + data.attributes[2].usage = mjVERTEX_ATTRIBUTE_UV; + data.attributes[2].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT2; + data.attributes[2].bytes = uvs.data(); + data.nvertices = positions.size() / 3; + data.nindices = num_indices; + data.indices = indices.data(); + data.index_type = mjINDEX_TYPE_UINT; + data.primitive_type = mjPRIM_TYPE_TRIANGLES; + data.compute_bounds = true; + data.release_callback = nullptr; + data.user_data = nullptr; + return std::make_unique(engine, data); } } // namespace mujoco diff --git a/src/experimental/filament/filament/gui_view.cc b/src/experimental/filament/filament/gui_view.cc index 2217c95b..6a3bc82d 100644 --- a/src/experimental/filament/filament/gui_view.cc +++ b/src/experimental/filament/filament/gui_view.cc @@ -33,7 +33,6 @@ #include #include "experimental/filament/filament/buffer_util.h" #include "experimental/filament/filament/texture.h" -#include "experimental/filament/filament/vertex_util.h" namespace mujoco { @@ -202,10 +201,14 @@ void GuiView::UpdateRenderable() { } commands->ScaleClipRects(scale); + // 2 floats for position, 2 floats for uv, 4 bytes for color. + constexpr size_t kExpectedVertexSize = + sizeof(float) * 4 + sizeof(uint8_t) * 4; + int num_elements = 0; for (int n = 0; n < commands->CmdListsCount; ++n) { const ImDrawList* cmds = commands->CmdLists[n]; - if (sizeof(GuiVertex) != sizeof(cmds->VtxBuffer.Data[0])) { + if (kExpectedVertexSize != sizeof(cmds->VtxBuffer.Data[0])) { mju_error("Invalid vertex buffer size."); } if (sizeof(uint16_t) != sizeof(cmds->IdxBuffer.Data[0])) { @@ -274,24 +277,26 @@ void GuiView::UpdateRenderable() { int drawable_index = 0; for (int n = 0; n < commands->CmdListsCount; ++n) { const ImDrawList* cmds = commands->CmdLists[n]; - auto vfill = [&](std::byte* dst, std::size_t size) { - if (size != cmds->VtxBuffer.size_in_bytes()) { - mju_error("Invalid vertex buffer size."); - } - std::memcpy(dst, cmds->VtxBuffer.Data, size); - }; - auto ifill = [&](std::byte* dst, std::size_t size) { - if (size != cmds->IdxBuffer.size_in_bytes()) { - mju_error("Invalid index buffer size."); - } - std::memcpy(dst, cmds->IdxBuffer.Data, size); - }; - filament::IndexBuffer* index_buffer = - CreateIndexBuffer(engine_, cmds->IdxBuffer.Size, ifill); - filament::VertexBuffer* vertex_buffer = - CreateVertexBuffer(engine_, cmds->VtxBuffer.Size, vfill); - meshes_.push_back(std::make_unique(engine_, index_buffer, vertex_buffer)); + MeshData data; + DefaultMeshData(&data); + data.nattributes = 3; + data.attributes[0].usage = mjVERTEX_ATTRIBUTE_POSITION; + data.attributes[0].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT2; + data.attributes[0].bytes = cmds->VtxBuffer.Data; + data.attributes[1].usage = mjVERTEX_ATTRIBUTE_UV; + data.attributes[1].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT2; + data.attributes[1].bytes = cmds->VtxBuffer.Data + sizeof(float) * 2; + data.attributes[2].usage = mjVERTEX_ATTRIBUTE_COLOR; + data.attributes[2].type = mjVERTEX_ATTRIBUTE_TYPE_UBYTE4; + data.attributes[2].bytes = cmds->VtxBuffer.Data + sizeof(float) * 4; + data.interleaved = true; + data.nvertices = cmds->VtxBuffer.Size; + data.nindices = cmds->IdxBuffer.Size; + data.indices = cmds->IdxBuffer.Data; + data.index_type = mjINDEX_TYPE_USHORT; + data.primitive_type = mjPRIM_TYPE_TRIANGLES; + meshes_.push_back(std::make_unique(engine_, data)); const auto& mesh = meshes_.back(); int index_offset = 0; diff --git a/src/experimental/filament/filament/model_objects.cc b/src/experimental/filament/filament/model_objects.cc index 7f8a438c..3f638695 100644 --- a/src/experimental/filament/filament/model_objects.cc +++ b/src/experimental/filament/filament/model_objects.cc @@ -87,19 +87,16 @@ void ModelObjects::UploadMesh(const mjModel* model, int id) { meshes_.erase(id); convex_hulls_.erase(id); - filament::Box bounds; - auto vertex_buffer = - CreateVertexBuffer(engine_, model, id, MeshType::kNormal, &bounds); - auto index_buffer = CreateIndexBuffer(engine_, model, id, MeshType::kNormal); - meshes_[id] = - std::make_unique(engine_, index_buffer, vertex_buffer, bounds); + MeshData data; + DefaultMeshData(&data); + UpdateMeshData(&data, model, id, MeshType::kNormal); + meshes_[id] = std::make_unique(engine_, data); if (model->mesh_graphadr[id] >= 0) { - vertex_buffer = - CreateVertexBuffer(engine_, model, id, MeshType::kConvexHull, &bounds); - index_buffer = CreateIndexBuffer(engine_, model, id, MeshType::kConvexHull); - convex_hulls_[id] = - std::make_unique(engine_, index_buffer, vertex_buffer, bounds); + MeshData convex_hull_data; + DefaultMeshData(&convex_hull_data); + UpdateMeshData(&convex_hull_data, model, id, MeshType::kConvexHull); + convex_hulls_[id] = std::make_unique(engine_, convex_hull_data); } } @@ -160,13 +157,10 @@ void ModelObjects::UploadHeightField(const mjModel* model, int id) { height_fields_.erase(id); - filament::Box bounds; - auto vertex_buffer = - CreateVertexBuffer(engine_, model, id, MeshType::kHeightField, &bounds); - auto index_buffer = - CreateIndexBuffer(engine_, model, id, MeshType::kHeightField); - height_fields_[id] = - std::make_unique(engine_, index_buffer, vertex_buffer, bounds); + MeshData data; + DefaultMeshData(&data); + UpdateMeshData(&data, model, id, MeshType::kHeightField); + height_fields_[id] = std::make_unique(engine_, data); } const Mesh* ModelObjects::GetMeshBuffer(int data_id) const { diff --git a/src/experimental/filament/filament/model_util.cc b/src/experimental/filament/filament/model_util.cc index 2bd581dd..26ecfaac 100644 --- a/src/experimental/filament/filament/model_util.cc +++ b/src/experimental/filament/filament/model_util.cc @@ -16,15 +16,11 @@ #include #include -#include #include #include +#include -#include -#include -#include -#include -#include +#include #include #include #include @@ -40,6 +36,30 @@ using filament::math::float2; using filament::math::float3; using filament::math::float4; +struct MeshBuilder { + MeshBuilder(int nvertices) : nvertices(nvertices) { + positions.reserve(nvertices); + orientations.reserve(nvertices); + uvs.reserve(nvertices); + } + + void Append(const float3& position, const float4& orientation, + const float2& uv) { + positions.push_back(position); + orientations.push_back(orientation); + uvs.push_back(uv); + bounds_min = min(bounds_min, position); + bounds_max = max(bounds_max, position); + } + + int nvertices = 0; + float3 bounds_min = {FLT_MAX, FLT_MAX, FLT_MAX}; + float3 bounds_max = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + std::vector positions; + std::vector orientations; + std::vector uvs; +}; + static bool UseFaceNormal(const float3& face_normal, const float3& mesh_normal) { // clang-format off @@ -49,74 +69,42 @@ static bool UseFaceNormal(const float3& face_normal, // clang-format on } -static void UpdateBounds(const float3& v, float3* vmin, float3* vmax) { - vmin->x = std::min(vmin->x, v.x); - vmin->y = std::min(vmin->y, v.y); - vmin->z = std::min(vmin->z, v.z); - vmax->x = std::max(vmax->x, v.x); - vmax->y = std::max(vmax->y, v.y); - vmax->z = std::max(vmax->z, v.z); -} -template -static void FillConvexHullBuffer(T* ptr, std::size_t num, const mjModel* model, - int meshid, float3* vmin, float3* vmax) { +static void FillConvexHullBuffer(MeshBuilder& builder, const mjModel* model, + int meshid) { const int numvert = model->mesh_graph[model->mesh_graphadr[meshid]]; const int numface = model->mesh_graph[model->mesh_graphadr[meshid] + 1]; - - const int vertadr = model->mesh_vertadr[meshid]; - const float* vertices = model->mesh_vert + (3 * vertadr); - const int texcoordadr = model->mesh_texcoordadr[meshid]; - const float* texcoords = model->mesh_texcoord + (2 * texcoordadr); - - if (num != numface * 3) { - mju_error("Invalid vertex count."); + if (builder.nvertices != numface * 3) { + mju_error("Invalid vertex count (%d vs %d).", builder.nvertices, numface * 3); return; } - for (int face = 0; face < numface; ++face) { - int j = - model->mesh_graphadr[meshid] + 2 + 3 * numvert + 3 * numface + 3 * face; + const int dataadr = model->mesh_graphadr[meshid] + 2; + const int vertadr = model->mesh_vertadr[meshid]; + const float* vertices = model->mesh_vert + (3 * vertadr); + const int texcoordadr = model->mesh_texcoordadr[meshid]; + const float* texcoords = texcoordadr >= 0 ? model->mesh_texcoord + (2 * texcoordadr) : nullptr; + for (int face = 0; face < numface; ++face) { + const int j = dataadr + (3 * numvert) + (3 * numface) + (3 * face); const float3 p1 = ReadFloat3(vertices, model->mesh_graph[j + 0]); const float3 p2 = ReadFloat3(vertices, model->mesh_graph[j + 1]); const float3 p3 = ReadFloat3(vertices, model->mesh_graph[j + 2]); const float4 orientation = CalculateOrientation(p1, p2, p3); - - UpdateBounds(p1, vmin, vmax); - UpdateBounds(p2, vmin, vmax); - UpdateBounds(p3, vmin, vmax); - - ptr->position = p1; - ptr->orientation = orientation; - if constexpr (T::kHasUv) { - ptr->uv = ReadFloat2(texcoords, model->mesh_graph[j + 0]); - } - ++ptr; - - ptr->position = p2; - ptr->orientation = orientation; - if constexpr (T::kHasUv) { - ptr->uv = ReadFloat2(texcoords, model->mesh_graph[j + 1]); - } - ++ptr; - - ptr->position = p3; - ptr->orientation = orientation; - if constexpr (T::kHasUv) { - ptr->uv = ReadFloat2(texcoords, model->mesh_graph[j + 2]); - } - ++ptr; + const float2 uv1 = texcoords ? ReadFloat2(texcoords, model->mesh_graph[j + 0]) : float2(0, 0); + const float2 uv2 = texcoords ? ReadFloat2(texcoords, model->mesh_graph[j + 1]) : float2(0, 0); + const float2 uv3 = texcoords ? ReadFloat2(texcoords, model->mesh_graph[j + 2]) : float2(0, 0); + builder.Append(p1, orientation, uv1); + builder.Append(p2, orientation, uv2); + builder.Append(p3, orientation, uv3); } } -template -static void FillMeshBuffer(T* ptr, std::size_t num, const mjModel* model, - int meshid, float3* vmin, float3* vmax) { +static void FillMeshBuffer(MeshBuilder& builder, const mjModel* model, int meshid) { const int faceadr = model->mesh_faceadr[meshid]; const int facenum = model->mesh_facenum[meshid]; - if (num != facenum * 3) { - mju_error("Invalid vertex count."); + if (builder.nvertices != facenum * 3) { + mju_error("Invalid vertex count (%d vs %d).", builder.nvertices, facenum * 3); return; } @@ -125,7 +113,7 @@ static void FillMeshBuffer(T* ptr, std::size_t num, const mjModel* model, const int normaladr = model->mesh_normaladr[meshid]; const float* normals = model->mesh_normal + 3 * normaladr; const int texcoordadr = model->mesh_texcoordadr[meshid]; - const float* texcoords = model->mesh_texcoord + (2 * texcoordadr); + const float* texcoords = texcoordadr >= 0 ? model->mesh_texcoord + (2 * texcoordadr) : nullptr; for (int i = 0; i < facenum; ++i) { const int face = 3 * (faceadr + i); @@ -133,70 +121,41 @@ static void FillMeshBuffer(T* ptr, std::size_t num, const mjModel* model, const float3 p1 = ReadFloat3(vertices, model->mesh_face[face + 0]); const float3 p2 = ReadFloat3(vertices, model->mesh_face[face + 1]); const float3 p3 = ReadFloat3(vertices, model->mesh_face[face + 2]); - - UpdateBounds(p1, vmin, vmax); - UpdateBounds(p2, vmin, vmax); - UpdateBounds(p3, vmin, vmax); - const float3 face_normal = CalculateNormal(p1, p2, p3); const float3 n1 = ReadFloat3(normals, model->mesh_facenormal[face + 0]); const float3 n2 = ReadFloat3(normals, model->mesh_facenormal[face + 1]); const float3 n3 = ReadFloat3(normals, model->mesh_facenormal[face + 2]); + const float2 uv1 = texcoords ? ReadFloat2(texcoords, model->mesh_facetexcoord[face + 0]) : float2(0, 0); + const float2 uv2 = texcoords ? ReadFloat2(texcoords, model->mesh_facetexcoord[face + 1]) : float2(0, 0); + const float2 uv3 = texcoords ? ReadFloat2(texcoords, model->mesh_facetexcoord[face + 2]) : float2(0, 0); - ptr->position = p1; - if constexpr (T::kHasUv) { - ptr->orientation = CalculateOrientation(n1); - ptr->uv = ReadFloat2(texcoords, model->mesh_facetexcoord[face + 0]); - } else if (UseFaceNormal(face_normal, n1)) { - ptr->orientation = CalculateOrientation(face_normal); + if (UseFaceNormal(face_normal, n1)) { + builder.Append(p1, CalculateOrientation(face_normal), uv1); } else { - ptr->orientation = CalculateOrientation(n1); + builder.Append(p1, CalculateOrientation(n1), uv1); } - ++ptr; - ptr->position = p2; - if constexpr (T::kHasUv) { - ptr->orientation = CalculateOrientation(n2); - ptr->uv = ReadFloat2(texcoords, model->mesh_facetexcoord[face + 1]); - } else if (UseFaceNormal(face_normal, n2)) { - ptr->orientation = CalculateOrientation(face_normal); + if (UseFaceNormal(face_normal, n2)) { + builder.Append(p2, CalculateOrientation(face_normal), uv2); } else { - ptr->orientation = CalculateOrientation(n2); + builder.Append(p2, CalculateOrientation(n2), uv2); } - ++ptr; - ptr->position = p3; - if constexpr (T::kHasUv) { - ptr->orientation = CalculateOrientation(n3); - ptr->uv = ReadFloat2(texcoords, model->mesh_facetexcoord[face + 2]); - } else if (UseFaceNormal(face_normal, n3)) { - ptr->orientation = CalculateOrientation(face_normal); + if (UseFaceNormal(face_normal, n3)) { + builder.Append(p3, CalculateOrientation(face_normal), uv3); } else { - ptr->orientation = CalculateOrientation(n3); + builder.Append(p3, CalculateOrientation(n3), uv3); } - ++ptr; } } -static void FillHeightFieldBuffer(VertexNoUv* ptr, std::size_t num, - const mjModel* model, int hfieldid, - float3* vmin, float3* vmax) { - int count = 0; +static void FillHeightFieldBuffer(MeshBuilder& builder, const mjModel* model, + int hfieldid) { auto append_tri = [&](float3 a, float3 b, float3 c) { float4 orientation = CalculateOrientation(a, b, c); - ptr[count].position = a; - ptr[count].orientation = orientation; - ++count; - ptr[count].position = b; - ptr[count].orientation = orientation; - ++count; - ptr[count].position = c; - ptr[count].orientation = orientation; - ++count; - - UpdateBounds(a, vmin, vmax); - UpdateBounds(b, vmin, vmax); - UpdateBounds(c, vmin, vmax); + builder.Append(a, orientation, float2(0, 0)); + builder.Append(b, orientation, float2(0, 0)); + builder.Append(c, orientation, float2(0, 0)); }; auto append_quad = [&](float3 a, float3 b, float3 c, float3 d) { append_tri(a, b, d); @@ -314,9 +273,6 @@ static void FillHeightFieldBuffer(VertexNoUv* ptr, std::size_t num, {x1, y0, -sz[3]}); } } - if (count != num) { - mju_error("Vertex count mismatch."); - } } static int CalculateHeightFieldVertexCount(const mjModel* model, int hfieldid) { @@ -340,149 +296,85 @@ static int CalculateHeightFieldVertexCount(const mjModel* model, int hfieldid) { return total_count; } -template -static filament::VertexBuffer* CreateVertexBuffer(filament::Engine* engine, - const mjModel* model, int id, - int vertex_count, - FillFn fill_fn, - filament::Box* bounds) { - float3 vmin = {FLT_MAX, FLT_MAX, FLT_MAX}; - float3 vmax = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; - filament::VertexBuffer* buffer = CreateVertexBuffer( - engine, vertex_count, [&](std::byte* buffer, std::size_t num_bytes) { - auto* ptr = reinterpret_cast(buffer); - fill_fn(ptr, num_bytes / sizeof(T), model, id, &vmin, &vmax); - }); - bounds->set(vmin, vmax); - return buffer; +static bool HasUvs(const mjModel* model, int id, MeshType mesh_type) { + return mesh_type != MeshType::kHeightField && + model->mesh_texcoordadr[id] >= 0; } -filament::VertexBuffer* CreateVertexBuffer(filament::Engine* engine, - const mjModel* model, int id, - MeshType mesh_type, - filament::Box* bounds) { - if (id < 0) { - mju_error("Invalid mesh index %d", id); - return nullptr; - } - - int vertex_count = 0; +static bool IsValidIndex(const mjModel* model, int id, MeshType mesh_type) { switch (mesh_type) { case MeshType::kNormal: - if (id >= model->nmesh) { - mju_error("Invalid mesh index %d", id); - return nullptr; - } - vertex_count = 3 * model->mesh_facenum[id]; - break; + return id >= 0 && id < model->nmesh; case MeshType::kConvexHull: - if (id >= model->nmesh) { - mju_error("Invalid mesh index %d", id); - return nullptr; - } - vertex_count = 3 * model->mesh_graph[model->mesh_graphadr[id] + 1]; - break; + return id >= 0 && id < model->nmesh; case MeshType::kHeightField: - if (id >= model->nhfield) { - mju_error("Invalid height field index %d", id); - return nullptr; - } - vertex_count = CalculateHeightFieldVertexCount(model, id); - break; + return id >= 0 && id < model->nhfield; } - - if (vertex_count == 0) { - mju_error("Vertex count is zero."); - return nullptr; - } - - const bool has_texcoords = mesh_type == MeshType::kHeightField - ? false - : model->mesh_texcoordadr[id] >= 0; - if (has_texcoords) { - using VertexType = VertexWithUv; - switch (mesh_type) { - case MeshType::kNormal: - return CreateVertexBuffer(engine, model, id, vertex_count, - FillMeshBuffer, - bounds); - break; - case MeshType::kConvexHull: - return CreateVertexBuffer(engine, model, id, vertex_count, - FillConvexHullBuffer, - bounds); - break; - case MeshType::kHeightField: - mju_error("Height fields do not support UV coordinates."); - return nullptr; - } - } else { - using VertexType = VertexNoUv; - switch (mesh_type) { - case MeshType::kNormal: - return CreateVertexBuffer(engine, model, id, vertex_count, - FillMeshBuffer, - bounds); - break; - case MeshType::kConvexHull: - return CreateVertexBuffer(engine, model, id, vertex_count, - FillConvexHullBuffer, - bounds); - break; - case MeshType::kHeightField: - return CreateVertexBuffer(engine, model, id, vertex_count, - FillHeightFieldBuffer, bounds); - break; - } - } - - return nullptr; } -filament::IndexBuffer* CreateIndexBuffer(filament::Engine* engine, - const mjModel* model, int id, - MeshType mesh_type) { - if (id < 0) { - mju_error("Invalid index %d", id); - return nullptr; - } - - int index_count = 0; +static int GetNumVertices(const mjModel* model, int id, MeshType mesh_type) { switch (mesh_type) { case MeshType::kNormal: - if (id >= model->nmesh) { - mju_error("Invalid mesh index %d", id); - return nullptr; - } - index_count = 3 * model->mesh_facenum[id]; + return 3 * model->mesh_facenum[id]; + case MeshType::kConvexHull: + return 3 * model->mesh_graph[model->mesh_graphadr[id] + 1]; + case MeshType::kHeightField: + return CalculateHeightFieldVertexCount(model, id); + } +} + +void UpdateMeshData(MeshData* data, const mjModel* model, int id, + MeshType mesh_type) { + if (!IsValidIndex(model, id, mesh_type)) { + mju_error("Invalid index %d for type %d", id, mesh_type); + return; + } + + const int num_vertices = GetNumVertices(model, id, mesh_type); + const bool has_uvs = HasUvs(model, id, mesh_type); + + MeshBuilder* builder = new MeshBuilder(num_vertices); + data->user_data = builder; + data->release_callback = [](void* user_data) { + delete static_cast(user_data); + }; + + switch (mesh_type) { + case MeshType::kNormal: + FillMeshBuffer(*builder, model, id); break; case MeshType::kConvexHull: - if (id >= model->nmesh) { - mju_error("Invalid mesh index %d", id); - return nullptr; - } - index_count = 3 * model->mesh_graph[model->mesh_graphadr[id] + 1]; + FillConvexHullBuffer(*builder, model, id); break; case MeshType::kHeightField: - if (id >= model->nhfield) { - mju_error("Invalid height field index %d", id); - return nullptr; - } - index_count = CalculateHeightFieldVertexCount(model, id); + FillHeightFieldBuffer(*builder, model, id); break; } - if (index_count == 0) { - mju_error("Index count is zero."); - return nullptr; - } - - if (index_count >= std::numeric_limits::max()) { - return CreateIndexBuffer(engine, index_count, - FillSequence); - } else { - return CreateIndexBuffer(engine, index_count, - FillSequence); + data->primitive_type = mjPRIM_TYPE_TRIANGLES; + data->nvertices = num_vertices; + data->nindices = data->nvertices; + data->indices = nullptr; + data->index_type = data->nvertices >= std::numeric_limits::max() + ? mjINDEX_TYPE_UINT + : mjINDEX_TYPE_USHORT; + data->nattributes = has_uvs ? 3 : 2; + data->attributes[0].usage = mjVERTEX_ATTRIBUTE_POSITION; + data->attributes[0].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT3; + data->attributes[0].bytes = builder->positions.data(); + data->attributes[1].usage = mjVERTEX_ATTRIBUTE_TANGENTS; + data->attributes[1].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT4; + data->attributes[1].bytes = builder->orientations.data(); + if (has_uvs) { + data->attributes[2].usage = mjVERTEX_ATTRIBUTE_UV; + data->attributes[2].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT2; + data->attributes[2].bytes = builder->uvs.data(); } + data->bounds_min[0] = builder->bounds_min.x; + data->bounds_min[1] = builder->bounds_min.y; + data->bounds_min[2] = builder->bounds_min.z; + data->bounds_max[0] = builder->bounds_max.x; + data->bounds_max[1] = builder->bounds_max.y; + data->bounds_max[2] = builder->bounds_max.z; } } // namespace mujoco diff --git a/src/experimental/filament/filament/model_util.h b/src/experimental/filament/filament/model_util.h index edf14fed..4ca36c86 100644 --- a/src/experimental/filament/filament/model_util.h +++ b/src/experimental/filament/filament/model_util.h @@ -17,16 +17,12 @@ #include -#include -#include -#include -#include -#include #include #include #include #include #include +#include "experimental/filament/filament/buffer_util.h" namespace mujoco { @@ -37,16 +33,9 @@ enum class MeshType { kHeightField, }; -// Generates a filament VertexBuffer for a given mesh in the mjModel. -filament::VertexBuffer* CreateVertexBuffer(filament::Engine* engine, - const mjModel* model, int id, - MeshType mesh_type, - filament::Box* bounds); - -// Generates a filament IndexBuffer for a given mesh in the mjModel. -filament::IndexBuffer* CreateIndexBuffer(filament::Engine* engine, - const mjModel* model, int id, - MeshType mesh_type); +// Populates the given MeshData with data for the element in the model. +void UpdateMeshData(MeshData* data, const mjModel* model, int id, + MeshType mesh_type); // Reads a value with the given name from the mjModel's data sections. The // default_value is returned if the named element is not found. diff --git a/src/experimental/filament/filament/renderables.cc b/src/experimental/filament/filament/renderables.cc index 5be61556..f8cbab1c 100644 --- a/src/experimental/filament/filament/renderables.cc +++ b/src/experimental/filament/filament/renderables.cc @@ -73,13 +73,13 @@ void Renderables::Update(int index, MeshPtr mesh) { void Renderables::Append(const Mesh* mesh) { utils::Entity entity = CreateEntity(mesh); entities_.push_back(entity); - meshes_.emplace_back(nullptr, mesh); + meshes_.push_back({nullptr, mesh}); } void Renderables::Append(MeshPtr mesh) { utils::Entity entity = CreateEntity(mesh.get()); entities_.push_back(entity); - meshes_.emplace_back(std::move(mesh), mesh.get()); + meshes_.push_back({std::move(mesh), mesh.get()}); } utils::Entity Renderables::CreateEntity(const Mesh* mesh) { diff --git a/src/experimental/filament/filament/vertex_util.h b/src/experimental/filament/filament/vertex_util.h index f31c5e62..e8bd7919 100644 --- a/src/experimental/filament/filament/vertex_util.h +++ b/src/experimental/filament/filament/vertex_util.h @@ -15,7 +15,6 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_VERTEX_UTIL_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_VERTEX_UTIL_H_ -#include #include #include @@ -37,59 +36,6 @@ filament::math::float4 CalculateOrientation( const filament::math::float3& p2, const filament::math::float3& p3); -// A standard vertex with no UV coordinates. -struct VertexNoUv { - VertexNoUv() = default; - VertexNoUv(filament::math::float3 position, - filament::math::float4 orientation) - : position(position), orientation(orientation) {} - - filament::math::float3 position; - filament::math::float4 orientation; - - static constexpr bool kHasPosition = true; - static constexpr bool kHasPosition2d = false; - static constexpr bool kHasOrientation = true; - static constexpr bool kHasUv = false; - static constexpr bool kHasColor = false; -}; - -// A standard vertex with UV coordinates. -struct VertexWithUv { - VertexWithUv() = default; - VertexWithUv(filament::math::float3 position, - filament::math::float4 orientation, filament::math::float2 uv) - : position(position), orientation(orientation), uv(uv) {} - - filament::math::float3 position; - filament::math::float4 orientation; - filament::math::float2 uv; - - static constexpr bool kHasPosition = true; - static constexpr bool kHasPosition2d = false; - static constexpr bool kHasOrientation = true; - static constexpr bool kHasUv = true; - static constexpr bool kHasColor = false; -}; - -// A vertex for rendering GUI elements. -struct GuiVertex { - GuiVertex() = default; - GuiVertex(filament::math::float2 position, filament::math::float2 uv, - filament::math::ubyte4 color) - : position(position), uv(uv), color(color) {} - - filament::math::float2 position; - filament::math::float2 uv; - filament::math::ubyte4 color; - - static constexpr bool kHasPosition = false; - static constexpr bool kHasPosition2d = true; - static constexpr bool kHasOrientation = false; - static constexpr bool kHasUv = true; - static constexpr bool kHasColor = true; -}; - } // namespace mujoco #endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_VERTEX_UTIL_H_ From b61d3041260559a86407bdbdba937629b8b6ce0b Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 7 Apr 2026 07:48:57 -0700 Subject: [PATCH 016/251] Rename buffer_util to mesh. PiperOrigin-RevId: 895897940 Change-Id: Ic6d4315fd6429efda81d24168548271a1a1f03c5 --- src/experimental/filament/CMakeLists.txt | 4 ++-- src/experimental/filament/filament/builtins.cc | 2 +- src/experimental/filament/filament/builtins.h | 2 +- src/experimental/filament/filament/drawable.cc | 2 +- src/experimental/filament/filament/geom_util.cc | 2 +- src/experimental/filament/filament/geom_util.h | 2 +- src/experimental/filament/filament/gui_view.cc | 2 +- src/experimental/filament/filament/gui_view.h | 2 +- .../filament/filament/{buffer_util.cc => mesh.cc} | 2 +- .../filament/filament/{buffer_util.h => mesh.h} | 6 +++--- src/experimental/filament/filament/model_objects.cc | 3 +-- src/experimental/filament/filament/model_objects.h | 2 +- src/experimental/filament/filament/model_util.cc | 2 +- src/experimental/filament/filament/model_util.h | 2 +- src/experimental/filament/filament/renderables.cc | 2 +- src/experimental/filament/filament/renderables.h | 2 +- 16 files changed, 19 insertions(+), 20 deletions(-) rename src/experimental/filament/filament/{buffer_util.cc => mesh.cc} (99%) rename src/experimental/filament/filament/{buffer_util.h => mesh.h} (96%) diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index 457a5db9..139daf8d 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -23,8 +23,6 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} PUBLIC render_context_filament.h render_context_filament.cc - filament/buffer_util.cc - filament/buffer_util.h filament/builtins.cc filament/builtins.h filament/color_grading_options.cc @@ -47,6 +45,8 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/material.h filament/math_util.cc filament/math_util.h + filament/mesh.cc + filament/mesh.h filament/model_objects.cc filament/model_objects.h filament/model_util.cc diff --git a/src/experimental/filament/filament/builtins.cc b/src/experimental/filament/filament/builtins.cc index 21f6c7cc..40b727af 100644 --- a/src/experimental/filament/filament/builtins.cc +++ b/src/experimental/filament/filament/builtins.cc @@ -27,7 +27,7 @@ #include #include #include -#include "experimental/filament/filament/buffer_util.h" +#include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/vertex_util.h" namespace mujoco { diff --git a/src/experimental/filament/filament/builtins.h b/src/experimental/filament/filament/builtins.h index b79083a3..c698bca8 100644 --- a/src/experimental/filament/filament/builtins.h +++ b/src/experimental/filament/filament/builtins.h @@ -16,7 +16,7 @@ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUILTINS_H_ #include -#include "experimental/filament/filament/buffer_util.h" +#include "experimental/filament/filament/mesh.h" // Generates buffers for built-in shapes. namespace mujoco { diff --git a/src/experimental/filament/filament/drawable.cc b/src/experimental/filament/filament/drawable.cc index a377d1f6..b3649350 100644 --- a/src/experimental/filament/filament/drawable.cc +++ b/src/experimental/filament/filament/drawable.cc @@ -32,10 +32,10 @@ #include #include #include -#include "experimental/filament/filament/buffer_util.h" #include "experimental/filament/filament/geom_util.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/math_util.h" +#include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/model_objects.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/texture.h" diff --git a/src/experimental/filament/filament/geom_util.cc b/src/experimental/filament/filament/geom_util.cc index fb64e8d0..7b02a02a 100644 --- a/src/experimental/filament/filament/geom_util.cc +++ b/src/experimental/filament/filament/geom_util.cc @@ -23,7 +23,7 @@ #include #include #include -#include "experimental/filament/filament/buffer_util.h" +#include "experimental/filament/filament/mesh.h" namespace mujoco { diff --git a/src/experimental/filament/filament/geom_util.h b/src/experimental/filament/filament/geom_util.h index 55c14db0..939c8d91 100644 --- a/src/experimental/filament/filament/geom_util.h +++ b/src/experimental/filament/filament/geom_util.h @@ -17,7 +17,7 @@ #include #include -#include "experimental/filament/filament/buffer_util.h" +#include "experimental/filament/filament/mesh.h" namespace mujoco { diff --git a/src/experimental/filament/filament/gui_view.cc b/src/experimental/filament/filament/gui_view.cc index 6a3bc82d..9c522325 100644 --- a/src/experimental/filament/filament/gui_view.cc +++ b/src/experimental/filament/filament/gui_view.cc @@ -31,7 +31,7 @@ #include #include #include -#include "experimental/filament/filament/buffer_util.h" +#include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/texture.h" namespace mujoco { diff --git a/src/experimental/filament/filament/gui_view.h b/src/experimental/filament/filament/gui_view.h index d1f80cd8..5ba9fdd9 100644 --- a/src/experimental/filament/filament/gui_view.h +++ b/src/experimental/filament/filament/gui_view.h @@ -29,7 +29,7 @@ #include #include #include -#include "experimental/filament/filament/buffer_util.h" +#include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/texture.h" namespace mujoco { diff --git a/src/experimental/filament/filament/buffer_util.cc b/src/experimental/filament/filament/mesh.cc similarity index 99% rename from src/experimental/filament/filament/buffer_util.cc rename to src/experimental/filament/filament/mesh.cc index a1ac791a..bfdad8be 100644 --- a/src/experimental/filament/filament/buffer_util.cc +++ b/src/experimental/filament/filament/mesh.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/filament/filament/buffer_util.h" +#include "experimental/filament/filament/mesh.h" #include #include diff --git a/src/experimental/filament/filament/buffer_util.h b/src/experimental/filament/filament/mesh.h similarity index 96% rename from src/experimental/filament/filament/buffer_util.h rename to src/experimental/filament/filament/mesh.h index 4bc6dd70..5953d0da 100644 --- a/src/experimental/filament/filament/buffer_util.h +++ b/src/experimental/filament/filament/mesh.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUFFER_UTIL_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUFFER_UTIL_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MESH_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MESH_H_ #include #include @@ -179,4 +179,4 @@ using MeshPtr = std::unique_ptr; } // namespace mujoco -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUFFER_UTIL_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MESH_H_ diff --git a/src/experimental/filament/filament/model_objects.cc b/src/experimental/filament/filament/model_objects.cc index 3f638695..474b982d 100644 --- a/src/experimental/filament/filament/model_objects.cc +++ b/src/experimental/filament/filament/model_objects.cc @@ -19,14 +19,13 @@ #include #include -#include #include #include #include #include #include -#include "experimental/filament/filament/buffer_util.h" #include "experimental/filament/filament/builtins.h" +#include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/texture.h" diff --git a/src/experimental/filament/filament/model_objects.h b/src/experimental/filament/filament/model_objects.h index f694b4de..0921169c 100644 --- a/src/experimental/filament/filament/model_objects.h +++ b/src/experimental/filament/filament/model_objects.h @@ -25,7 +25,7 @@ #include #include #include -#include "experimental/filament/filament/buffer_util.h" +#include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/texture.h" namespace mujoco { diff --git a/src/experimental/filament/filament/model_util.cc b/src/experimental/filament/filament/model_util.cc index 26ecfaac..e1309bc1 100644 --- a/src/experimental/filament/filament/model_util.cc +++ b/src/experimental/filament/filament/model_util.cc @@ -26,8 +26,8 @@ #include #include #include -#include "experimental/filament/filament/buffer_util.h" #include "experimental/filament/filament/math_util.h" +#include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/vertex_util.h" namespace mujoco { diff --git a/src/experimental/filament/filament/model_util.h b/src/experimental/filament/filament/model_util.h index 4ca36c86..6a23ee8f 100644 --- a/src/experimental/filament/filament/model_util.h +++ b/src/experimental/filament/filament/model_util.h @@ -22,7 +22,7 @@ #include #include #include -#include "experimental/filament/filament/buffer_util.h" +#include "experimental/filament/filament/mesh.h" namespace mujoco { diff --git a/src/experimental/filament/filament/renderables.cc b/src/experimental/filament/filament/renderables.cc index f8cbab1c..19f530de 100644 --- a/src/experimental/filament/filament/renderables.cc +++ b/src/experimental/filament/filament/renderables.cc @@ -22,7 +22,7 @@ #include #include #include -#include "experimental/filament/filament/buffer_util.h" +#include "experimental/filament/filament/mesh.h" namespace mujoco { diff --git a/src/experimental/filament/filament/renderables.h b/src/experimental/filament/filament/renderables.h index 2108442a..28bca459 100644 --- a/src/experimental/filament/filament/renderables.h +++ b/src/experimental/filament/filament/renderables.h @@ -21,7 +21,7 @@ #include #include #include -#include "experimental/filament/filament/buffer_util.h" +#include "experimental/filament/filament/mesh.h" namespace mujoco { From 382474bb9dd1bd5fa248cba28bf965c5854e74c5 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 7 Apr 2026 08:10:42 -0700 Subject: [PATCH 017/251] Minor improvements to `dcmotor` PiperOrigin-RevId: 895907301 Change-Id: Ia50a6d06c1ede9894cc71f375db837d104f0211e --- doc/APIreference/functions.rst | 2 ++ doc/XMLreference.rst | 46 ++++++++++++------------ include/mujoco/mujoco.h | 1 + python/mujoco/introspect/functions.py | 8 +++++ src/user/user_api.cc | 50 +++++++++++++-------------- src/user/user_api.h | 7 ++++ test/user/user_api_test.cc | 41 ++++++++++++++++++++++ wasm/codegen/generated/bindings.cc | 16 ++++----- 8 files changed, 114 insertions(+), 57 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index fa61555b..ef49bf39 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -4667,6 +4667,8 @@ Set actuator to active adhesion; return error if any. Set actuator to DC motor; return error if any. +*Nullable:* ``motorconst``, ``nominal``, ``saturation``, ``inductance``, ``cogging``, ``controller``, ``thermal``, ``lugre`` + .. _AddAssets: Assets diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 2552f932..72bfd3d8 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -6327,17 +6327,16 @@ This element has a subset of the common attributes and two custom attributes. :el-prefix:`actuator/` |-| **dcmotor** |*| ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -This element creates a DC motor actuator. Note that :el:`dcmotor` is quite different from the :ref:`general actuation -model`. Unlike the general model where the components of force generation are independent affine functions -mapping from control to force, :el:`dcmotor` relies on highly coupled physical dynamics. See the `DC motor technical -note <_static/dcmotor.pdf>`__ for complete mathematical formulations and parameter semantics, but we include a few -important notes here: +This element creates a DC motor actuator. See the `DC motor technical note <_static/dcmotor.pdf>`__ for complete +mathematical formulations and parameter semantics, but we include a few important notes below. Note that :el:`dcmotor` +does not conform to the affine gain / bias structure of the :ref:`general actuation model`, except for +the stateless case. -- Note that while :ref:`resistance`, :ref:`motorconst` and - :ref:`nominal` are each optional, some combination of them is required. +- :ref:`resistance`, :ref:`motorconst` and + :ref:`nominal` are each optional, but some combination of them is required. See Section 2.1 of the `technical note <_static/dcmotor.pdf>`__. -- The control :ref:`input` semantic is either the voltage applied to the motor terminals, or a - position or velocity target for a PID :ref:`controller`. +- The control :ref:`input` semantic is either the voltage applied to the motor terminals (the + default), or a position or velocity target for a :ref:`PID controller`. - Optional features include electrical dynamics (:ref:`inductance`), :ref:`cogging torque`, :ref:`thermal resistance variation`, and :ref:`LuGre` friction. @@ -6408,7 +6407,7 @@ This element has the following custom attributes in addition to the common attri .. _actuator-dcmotor-resistance: :at:`resistance`: :at-val:`real, optional` - Terminal resistance :math:`R` in Ohm. (see `tech note <_static/dcmotor.pdf>`__ for details) + Terminal resistance :math:`R` in Ohm. (see `tech note <_static/dcmotor.pdf>`__, Sections 1.1 and 2.1) .. _actuator-dcmotor-motorconst: @@ -6416,16 +6415,15 @@ This element has the following custom attributes in addition to the common attri Motor constants, defined as :at:`motorconst` = ":at-val:`Kt` :at-val:`Ke`" (N·m/A, equivalently V·s/rad). :at-val:`Kt` is the torque constant and :at-val:`Ke` the back-EMF constant; they can differ when magnetic saturation is present. If both are positive, the effective constant is :math:`K = \sqrt{K_t K_e}` (geometric mean). If only one - is positive, :math:`K` equals that value; a single value is interpreted as :math:`K_t = K_e`. If your datasheet gives - the speed constant :math:`K_v` in rad/(V·s), use :math:`K_e = 1/K_v`. (see `tech note <_static/dcmotor.pdf>`__ for - details) + is positive, :math:`K` equals that value. If a datasheet specifies the speed constant :math:`K_v` in rad/(V·s), use + :math:`K_e = 1/K_v`. (see `tech note <_static/dcmotor.pdf>`__, Sections 1.1 and 2.1) .. _actuator-dcmotor-nominal: :at:`nominal`: :at-val:`real(3), optional` Nominal operating point, defined as :at:`nominal` = ":at-val:`voltage` :at-val:`stall_torque` :at-val:`no_load_speed`". The compiler derives :math:`K =` :at-val:`voltage` / :at-val:`no_load_speed` and :math:`R = - K` · :at-val:`voltage` / :at-val:`stall_torque`. (see `tech note <_static/dcmotor.pdf>`__ for details) + K` · :at-val:`voltage` / :at-val:`stall_torque`. (see `tech note <_static/dcmotor.pdf>`__, Sections 1.1 and 2.1) .. _actuator-dcmotor-inductance: @@ -6434,7 +6432,7 @@ This element has the following custom attributes in addition to the common attri alternative specifications: :at-val:`L` is the winding inductance and :at-val:`timeconst` :math:`= L/R` is the electrical time constant. Specify one; if both are given, :at-val:`L` takes precedence. If both are 0 (the default), no electrical dynamics are modeled and the current is computed algebraically. Adds one activation variable for - armature current. (see `tech note <_static/dcmotor.pdf>`__ for details) + armature current. (see `tech note <_static/dcmotor.pdf>`__, Sections 1.1.1 and 2.2) .. _actuator-dcmotor-thermal: @@ -6444,7 +6442,7 @@ This element has the following custom attributes in addition to the common attri specify the thermal time constant: :at-val:`timeconst` = :at-val:`resistance` :math:`\times` :at-val:`capacitance`. Specify either :at-val:`timeconst` directly, or :at-val:`resistance` and :at-val:`capacitance`; if all three are given, :at-val:`timeconst` takes precedence. If all are 0 (the default), thermal modeling is disabled. Adds one - activation variable for winding temperature. (see `tech note <_static/dcmotor.pdf>`__ for details) + activation variable for winding temperature. (see `tech note <_static/dcmotor.pdf>`__, Sections 1.3 and 2.3) .. _actuator-dcmotor-saturation: @@ -6455,7 +6453,7 @@ This element has the following custom attributes in addition to the common attri given, :at-val:`torque` takes precedence. Sets :at:`forcerange` to [:math:`-\tau_{\max},\, \tau_{\max}`]. :at-val:`voltage` sets the maximum voltage :math:`V_{\max}`. :at-val:`current_rate` sets the maximum rate of change of current :math:`(di/dt)_{\max}` (requires :ref:`inductance`). A value of 0 (the - default) for any sub-value disables the respective limit. (see `tech note <_static/dcmotor.pdf>`__ for details) + default) for any sub-value disables the respective limit. (see `tech note <_static/dcmotor.pdf>`__, Section 2) .. _actuator-dcmotor-cogging: @@ -6463,7 +6461,7 @@ This element has the following custom attributes in addition to the common attri Cogging torque, defined as :at:`cogging` = ":at-val:`amplitude` :at-val:`poles` :at-val:`phase`" (N·m, integer, rad). Adds a position-dependent torque :math:`= \textsf{amplitude} \cdot \sin(\textsf{poles} \cdot \theta + \textsf{phase})`. Disabled when :at-val:`amplitude` = 0 (the default). - (see `tech note <_static/dcmotor.pdf>`__ for details) + (see `tech note <_static/dcmotor.pdf>`__, Sections 1.2 and 2.1) .. _actuator-dcmotor-lugre: @@ -6473,28 +6471,28 @@ This element has the following custom attributes in addition to the common attri :at-val:`stiffness` = 0 (the default). Adds one activation variable for bristle deflection. Note that the :at-val:`viscous` coefficient is mapped directly to the actuator :ref:`damping` array (specifically the linear term, :at-val:`damping[0]`). If both are specified, their values are summed. - (see `tech note <_static/dcmotor.pdf>`__ for details) + (see `tech note <_static/dcmotor.pdf>`__, Sections 1.4 and 2.4) .. _actuator-dcmotor-input: :at:`input`: :at-val:`[voltage, position, velocity], "voltage"` Specifies the input signal semantics. In "voltage" mode, the control directly sets applied motor voltage. In - "position" or "velocity" modes, the PID :ref:`controller` uses the control as a - reference setpoint relative to the joint trajectory. (see `tech note <_static/dcmotor.pdf>`__ for details) + "position" or "velocity" modes, the :ref:`PID controller` uses the control as a + reference setpoint relative to the joint trajectory. (see `tech note <_static/dcmotor.pdf>`__, Section 2.5) .. _actuator-dcmotor-controller: :at:`controller`: :at-val:`real(5), "0 0 0 0 0"` PID controller parameters, defined as :at:`controller` = ":at-val:`kp` :at-val:`ki` :at-val:`kd` :at-val:`slewmax` :at-val:`Imax`". Depending on the :at:`input` mode, the controller stabilizes either position or - velocity. If the :at:`input` mode is voltage, the controller is ignored. A value of 0 (the default) disables the + velocity. If the :at:`input` mode is voltage, this attribute is ignored. A value of 0 (the default) disables the respective feature: :at-val:`slewmax` = 0 means no slew-rate limiting, :at-val:`Imax` = 0 means no anti-windup - clamping. (see `tech note <_static/dcmotor.pdf>`__ for details) + clamping. (see `tech note <_static/dcmotor.pdf>`__, Section 2.5) .. _actuator-plugin: :el-prefix:`actuator/` |-| **plugin** |?| -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Associate this actuator with an :ref:`engine plugin`. Either :at:`plugin` or :at:`instance` are required. diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index ba496360..e8bbe692 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1727,6 +1727,7 @@ MJAPI const char* mjs_setToMuscle(mjsActuator* actuator, double timeconst[2], do MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain); // Set actuator to DC motor; return error if any. +// Nullable: motorconst, nominal, saturation, inductance, cogging, controller, thermal, lugre MJAPI const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance, double nominal[3], double saturation[4], double inductance[2], double cogging[3], double controller[5], double thermal[6], diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 0ab6df20..262d3430 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -10805,6 +10805,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ inner_type=ValueType(name='double'), extents=(2,), ), + nullable=True, ), FunctionParameterDecl( name='resistance', @@ -10816,6 +10817,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ inner_type=ValueType(name='double'), extents=(3,), ), + nullable=True, ), FunctionParameterDecl( name='saturation', @@ -10823,6 +10825,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ inner_type=ValueType(name='double'), extents=(4,), ), + nullable=True, ), FunctionParameterDecl( name='inductance', @@ -10830,6 +10833,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ inner_type=ValueType(name='double'), extents=(2,), ), + nullable=True, ), FunctionParameterDecl( name='cogging', @@ -10837,6 +10841,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ inner_type=ValueType(name='double'), extents=(3,), ), + nullable=True, ), FunctionParameterDecl( name='controller', @@ -10844,6 +10849,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ inner_type=ValueType(name='double'), extents=(5,), ), + nullable=True, ), FunctionParameterDecl( name='thermal', @@ -10851,6 +10857,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ inner_type=ValueType(name='double'), extents=(6,), ), + nullable=True, ), FunctionParameterDecl( name='lugre', @@ -10858,6 +10865,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ inner_type=ValueType(name='double'), extents=(6,), ), + nullable=True, ), FunctionParameterDecl( name='input_mode', diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 2317cc3a..8d320463 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -1125,18 +1125,18 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double double nominal[3], double saturation[4], double inductance[2], double cogging[3], double controller[5], double thermal[6], double lugre[6], int input_mode) { - double Kt = motorconst[0]; // torque constant - double Ke = motorconst[1]; // back-EMF constant - double R = resistance; // electrical resistance - double vn = nominal[0]; // nominal voltage - double tau0 = nominal[1]; // stall torque - double omega0 = nominal[2]; // no-load speed + double R = resistance; // electrical resistance + double Kt = motorconst ? motorconst[0] : 0; // torque constant + double Ke = motorconst ? motorconst[1] : 0; // back-EMF constant + double vn = nominal ? nominal[0] : 0; // nominal voltage + double tau0 = nominal ? nominal[1] : 0; // stall torque + double omega0 = nominal ? nominal[2] : 0; // no-load speed // derive Ke from nominal: omega0 = vn*Ke / (Ke^2 + R*B) if (vn > 0 && Ke <= 0 && omega0 > 0) { // viscous damping (linear), add lugre sigma2 contribution if any double B = actuator->damping[0]; - if (lugre[0] > 0) B += lugre[2]; + if (lugre && lugre[0] > 0) B += lugre[2]; if (B > 0 && R > 0) { // R known: solve quadratic Ke^2*omega0 - Ke*vn + R*B*omega0 = 0 @@ -1176,24 +1176,24 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double actuator->gainprm[1] = K; // controller parameters: gainprm[4:6] for kp, ki, kd - actuator->gainprm[4] = controller[0]; // kp - actuator->gainprm[5] = controller[1]; // ki - actuator->gainprm[6] = controller[2]; // kd + actuator->gainprm[4] = controller ? controller[0] : 0; // kp + actuator->gainprm[5] = controller ? controller[1] : 0; // ki + actuator->gainprm[6] = controller ? controller[2] : 0; // kd // controller parameters: dynprm[7,8] for slewmax, Imax - actuator->dynprm[7] = controller[3]; // slewmax - actuator->dynprm[8] = controller[4]; // Imax + actuator->dynprm[7] = controller ? controller[3] : 0; // slewmax + actuator->dynprm[8] = controller ? controller[4] : 0; // Imax // saturation: [tau_max, i_max, (di/dt)_max, v_max] - if (saturation[2] > 0) { + if (saturation && saturation[2] > 0) { actuator->dynprm[1] = saturation[2]; // (di/dt)_max } - if (saturation[3] > 0) { + if (saturation && saturation[3] > 0) { actuator->gainprm[7] = saturation[3]; // v_max } // saturation -> forcerange - if (saturation[0] > 0 || saturation[1] > 0) { + if (saturation && (saturation[0] > 0 || saturation[1] > 0)) { double tau_max = saturation[0]; if (tau_max == 0 && saturation[1] > 0) { tau_max = K * saturation[1]; // tau_max = K * i_max @@ -1204,34 +1204,34 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double } // cogging: [amplitude, periodicity, phase] -> biasprm[0:3] - actuator->biasprm[0] = cogging[0]; // amplitude - actuator->biasprm[1] = cogging[1]; // periodicity - actuator->biasprm[2] = cogging[2]; // phase + actuator->biasprm[0] = cogging ? cogging[0] : 0; // amplitude + actuator->biasprm[1] = cogging ? cogging[1] : 0; // periodicity + actuator->biasprm[2] = cogging ? cogging[2] : 0; // phase // count activation variables: slot order is slew, integral, temperature, bristle, current int actdim = 0; // inductance: [L, te] - if (inductance[0] < 0) return "DC motor: inductance must be non-negative"; - if (inductance[1] < 0) return "DC motor: electrical time constant must be non-negative"; - double te = inductance[0] > 0 ? inductance[0] / R : inductance[1]; + if (inductance && inductance[0] < 0) return "DC motor: inductance must be non-negative"; + if (inductance && inductance[1] < 0) return "DC motor: electrical time constant must be non-negative"; + double te = (inductance && inductance[0] > 0) ? inductance[0] / R : (inductance ? inductance[1] : 0); actuator->dynprm[0] = te; if (te > 0) { actdim++; } // controller states: slew rate limiting - if (controller[3] > 0) { // slewmax + if (controller && controller[3] > 0) { // slewmax actdim++; } // controller states: integral - if (controller[1] > 0) { // ki + if (controller && controller[1] > 0) { // ki actdim++; } // thermal -> temperature activation - if (thermal[0] > 0 || thermal[1] > 0 || thermal[2] > 0) { + if (thermal && (thermal[0] > 0 || thermal[1] > 0 || thermal[2] > 0)) { double RT = thermal[0]; // thermal resistance double C = thermal[1]; // thermal capacitance double tth = thermal[2]; // thermal time constant @@ -1259,7 +1259,7 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double } // lugre: {stiffness, damping, viscous, coulomb, static, stribeck} - if (lugre[0] > 0) { + if (lugre && lugre[0] > 0) { actuator->dynprm[5] = lugre[0]; // stiffness -> sigma0 actuator->dynprm[6] = lugre[1]; // damping -> sigma1 actuator->damping[0] += lugre[2]; // viscous -> sigma2 diff --git a/src/user/user_api.h b/src/user/user_api.h index 9bae6bdd..307f1a53 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -192,6 +192,13 @@ MJAPI const char* mjs_setToMuscle(mjsActuator* actuator, double timeconst[2], do // Set actuator to adhesion, return error on failure. MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain); +// Set actuator to DC motor, return error on failure. +MJAPI const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance, + double nominal[3], double saturation[4], double inductance[2], + double cogging[3], double controller[5], double thermal[6], + double lugre[6], int input_mode); + + //---------------------------------- Add assets ---------------------------------------------------- // Add mesh. diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index eb8a28b2..b36557aa 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -239,6 +239,47 @@ TEST_F(MujocoTest, DeletePlugin) { mj_deleteModel(newmodel); } +TEST_F(MujocoTest, SetToDCMotorNullable) { + mjSpec* spec = mj_makeSpec(); + mjsActuator* actuator = mjs_addActuator(spec, 0); + + double motorconst[2] = {0.05, 0.05}; + double resistance = 2.0; + + const char* err = mjs_setToDCMotor(actuator, motorconst, resistance, + nullptr, nullptr, nullptr, + nullptr, nullptr, nullptr, + nullptr, 0); + EXPECT_STREQ(err, ""); + EXPECT_EQ(actuator->gainprm[0], 2.0); + EXPECT_EQ(actuator->gainprm[1], 0.05); + EXPECT_EQ(actuator->gainprm[4], 0); + EXPECT_EQ(actuator->gainprm[5], 0); + EXPECT_EQ(actuator->gainprm[6], 0); + EXPECT_EQ(actuator->dynprm[7], 0); + EXPECT_EQ(actuator->dynprm[8], 0); + + mj_deleteSpec(spec); +} + +TEST_F(MujocoTest, SetToDCMotorDeriveKe) { + mjSpec* spec = mj_makeSpec(); + mjsActuator* actuator = mjs_addActuator(spec, 0); + + double resistance = 2.0; + double nominal[3] = {12.0, 0, 100.0}; // vn=12, omega0=100 + + const char* err = mjs_setToDCMotor(actuator, nullptr, resistance, + nominal, nullptr, nullptr, + nullptr, nullptr, nullptr, + nullptr, 0); + EXPECT_STREQ(err, ""); + EXPECT_EQ(actuator->gainprm[0], 2.0); + EXPECT_NEAR(actuator->gainprm[1], 0.12, 1e-5); + + mj_deleteSpec(spec); +} + static constexpr char xml_plugin_1[] = R"( diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index cfa83b40..dc2f2fc9 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -9877,14 +9877,14 @@ std::string mjs_setToCylinder_wrapper(MjsActuator& actuator, double timeconst, d } std::string mjs_setToDCMotor_wrapper(MjsActuator& actuator, const val& motorconst, double resistance, const val& nominal, const val& saturation, const val& inductance, const val& cogging, const val& controller, const val& thermal, const val& lugre, int input_mode) { - UNPACK_VALUE(double, motorconst); - UNPACK_VALUE(double, nominal); - UNPACK_VALUE(double, saturation); - UNPACK_VALUE(double, inductance); - UNPACK_VALUE(double, cogging); - UNPACK_VALUE(double, controller); - UNPACK_VALUE(double, thermal); - UNPACK_VALUE(double, lugre); + UNPACK_NULLABLE_VALUE(double, motorconst); + UNPACK_NULLABLE_VALUE(double, nominal); + UNPACK_NULLABLE_VALUE(double, saturation); + UNPACK_NULLABLE_VALUE(double, inductance); + UNPACK_NULLABLE_VALUE(double, cogging); + UNPACK_NULLABLE_VALUE(double, controller); + UNPACK_NULLABLE_VALUE(double, thermal); + UNPACK_NULLABLE_VALUE(double, lugre); return std::string(mjs_setToDCMotor(actuator.get(), motorconst_.data(), resistance, nominal_.data(), saturation_.data(), inductance_.data(), cogging_.data(), controller_.data(), thermal_.data(), lugre_.data(), input_mode)); } From f7fd5fc58bd214638f91592338f18c2c4dd04ecb Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 7 Apr 2026 12:29:53 -0700 Subject: [PATCH 018/251] Rename RenderTargetAndTextures to RenderTarget. Also rename render_target_util to render_target. PiperOrigin-RevId: 896033907 Change-Id: I123cf98dc308f7cc4916f7b79dcc59a05564022d --- src/experimental/filament/CMakeLists.txt | 4 +-- .../filament/filament/filament_context.cc | 34 +++++++++---------- .../filament/filament/filament_context.h | 6 ++-- .../filament/filament/gui_view.cc | 6 ++-- src/experimental/filament/filament/gui_view.h | 4 +-- ...render_target_util.cc => render_target.cc} | 16 ++++----- .../{render_target_util.h => render_target.h} | 20 +++++------ .../filament/filament/scene_view.cc | 11 +++--- .../filament/filament/scene_view.h | 6 ++-- 9 files changed, 54 insertions(+), 53 deletions(-) rename src/experimental/filament/filament/{render_target_util.cc => render_target.cc} (79%) rename src/experimental/filament/filament/{render_target_util.h => render_target.h} (83%) diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index 139daf8d..ca8f3f07 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -53,8 +53,8 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/model_util.h filament/object_manager.cc filament/object_manager.h - filament/render_target_util.cc - filament/render_target_util.h + filament/render_target.cc + filament/render_target.h filament/renderables.cc filament/renderables.h filament/scene_view.cc diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index cbd06da2..b960eb49 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -43,7 +43,7 @@ #include "experimental/filament/filament/imgui_editor.h" #include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/object_manager.h" -#include "experimental/filament/filament/render_target_util.h" +#include "experimental/filament/filament/render_target.h" #include "experimental/filament/filament/scene_view.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" @@ -185,12 +185,12 @@ void FilamentContext::SetFrameBuffer(int framebuffer) { } void FilamentContext::PrepareRenderTargets(int width, int height) { - color_target_ = std::make_unique( + color_target_ = std::make_unique( engine_, RenderTargetTextureType::kColor, RenderTargetTextureType::kDepth); color_target_->Prepare(width, height); - depth_target_ = std::make_unique( + depth_target_ = std::make_unique( engine_, RenderTargetTextureType::kDepthColor, RenderTargetTextureType::kDepth); depth_target_->Prepare(width, height); @@ -202,23 +202,25 @@ void FilamentContext::DestroyRenderTargets() { } static void ReadColorPixels(filament::Renderer* renderer, - filament::RenderTarget* target, mjrRect viewport, + RenderTarget* target, mjrRect viewport, unsigned char* buffer, size_t num_bytes) { filament::backend::PixelBufferDescriptor descriptor( buffer, num_bytes, filament::backend::PixelDataFormat::RGB, filament::backend::PixelDataType::UBYTE); - renderer->readPixels(target, viewport.left, viewport.bottom, viewport.width, - viewport.height, std::move(descriptor)); + renderer->readPixels(target->GetFilamentRenderTarget(), viewport.left, + viewport.bottom, viewport.width, viewport.height, + std::move(descriptor)); } static void ReadDepthPixels(filament::Renderer* renderer, - filament::RenderTarget* target, mjrRect viewport, + RenderTarget* target, mjrRect viewport, float* buffer, size_t num_bytes) { filament::backend::PixelBufferDescriptor descriptor( buffer, num_bytes, filament::backend::PixelDataFormat::R, filament::backend::PixelDataType::FLOAT); - renderer->readPixels(target, viewport.left, viewport.bottom, viewport.width, - viewport.height, std::move(descriptor)); + renderer->readPixels(target->GetFilamentRenderTarget(), viewport.left, + viewport.bottom, viewport.width, viewport.height, + std::move(descriptor)); } void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb, @@ -238,17 +240,15 @@ void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb, if (rgb) { if (renderer_->beginFrame(offscreen_swap_chain_)) { - scene_view_->Render(renderer_, last_render_mode_, - color_target_->GetRenderTarget()); + scene_view_->Render(renderer_, last_render_mode_, color_target_.get()); // Render the GUI to the texture as well if requested. if (gui_view_ && gui_swap_chain_target_ == kOffscreenSwapChain) { - gui_view_->Render(renderer_, color_target_->GetRenderTarget()); + gui_view_->Render(renderer_, color_target_.get()); } const size_t num_bytes = viewport.width * viewport.height * 3; - ReadColorPixels(renderer_, color_target_->GetRenderTarget(), viewport, - rgb, num_bytes); + ReadColorPixels(renderer_, color_target_.get(), viewport, rgb, num_bytes); renderer_->endFrame(); } @@ -257,11 +257,11 @@ void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb, if (depth) { if (renderer_->beginFrame(offscreen_swap_chain_)) { scene_view_->Render(renderer_, SceneView::DrawMode::kDepth, - depth_target_->GetRenderTarget()); + depth_target_.get()); const size_t num_bytes = viewport.width * viewport.height * sizeof(float); - ReadDepthPixels(renderer_, depth_target_->GetRenderTarget(), viewport, - depth, num_bytes); + ReadDepthPixels(renderer_, depth_target_.get(), viewport, depth, + num_bytes); renderer_->endFrame(); } diff --git a/src/experimental/filament/filament/filament_context.h b/src/experimental/filament/filament/filament_context.h index 7e2e2a9a..8de07824 100644 --- a/src/experimental/filament/filament/filament_context.h +++ b/src/experimental/filament/filament/filament_context.h @@ -26,7 +26,7 @@ #include #include "experimental/filament/filament/gui_view.h" #include "experimental/filament/filament/object_manager.h" -#include "experimental/filament/filament/render_target_util.h" +#include "experimental/filament/filament/render_target.h" #include "experimental/filament/filament/scene_view.h" #include "experimental/filament/render_context_filament.h" @@ -82,8 +82,8 @@ class FilamentContext { SceneView::DrawMode last_render_mode_ = SceneView::DrawMode::kNormal; SwapChainType scene_swap_chain_target_ = kWindowSwapChain; SwapChainType gui_swap_chain_target_ = kWindowSwapChain; - std::unique_ptr color_target_; - std::unique_ptr depth_target_; + std::unique_ptr color_target_; + std::unique_ptr depth_target_; std::unique_ptr object_manager_; std::unique_ptr scene_view_; std::unique_ptr gui_view_; diff --git a/src/experimental/filament/filament/gui_view.cc b/src/experimental/filament/filament/gui_view.cc index 9c522325..a6572366 100644 --- a/src/experimental/filament/filament/gui_view.cc +++ b/src/experimental/filament/filament/gui_view.cc @@ -32,6 +32,7 @@ #include #include #include "experimental/filament/filament/mesh.h" +#include "experimental/filament/filament/render_target.h" #include "experimental/filament/filament/texture.h" namespace mujoco { @@ -352,13 +353,12 @@ filament::MaterialInstance* GuiView::GetMaterialInstance(int index, return instance; } -void GuiView::Render(filament::Renderer* renderer, - filament::RenderTarget* target) { +void GuiView::Render(filament::Renderer* renderer, RenderTarget* target) { if (num_elements_ == 0) { return; } - view_->setRenderTarget(target); + view_->setRenderTarget(target ? target->GetFilamentRenderTarget() : nullptr); renderer->render(view_); view_->setRenderTarget(nullptr); } diff --git a/src/experimental/filament/filament/gui_view.h b/src/experimental/filament/filament/gui_view.h index 5ba9fdd9..0346ca57 100644 --- a/src/experimental/filament/filament/gui_view.h +++ b/src/experimental/filament/filament/gui_view.h @@ -30,6 +30,7 @@ #include #include #include "experimental/filament/filament/mesh.h" +#include "experimental/filament/filament/render_target.h" #include "experimental/filament/filament/texture.h" namespace mujoco { @@ -45,8 +46,7 @@ class GuiView { // correctly synced. void UpdateRenderable(); - void Render(filament::Renderer* renderer, - filament::RenderTarget* target = nullptr); + void Render(filament::Renderer* renderer, RenderTarget* target = nullptr); // Uploads texture to be used with ImGui's Image and ImageButton functions. uintptr_t UploadImage(uintptr_t tex_id, const uint8_t* pixels, int width, diff --git a/src/experimental/filament/filament/render_target_util.cc b/src/experimental/filament/filament/render_target.cc similarity index 79% rename from src/experimental/filament/filament/render_target_util.cc rename to src/experimental/filament/filament/render_target.cc index 2e4ce391..dec68a82 100644 --- a/src/experimental/filament/filament/render_target_util.cc +++ b/src/experimental/filament/filament/render_target.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/filament/filament/render_target_util.h" +#include "experimental/filament/filament/render_target.h" #include @@ -23,16 +23,16 @@ namespace mujoco { -RenderTargetAndTextures::RenderTargetAndTextures(filament::Engine* engine, +RenderTarget::RenderTarget(filament::Engine* engine, RenderTargetTextureType color, RenderTargetTextureType depth) : engine_(engine), color_type_(color), depth_type_(depth) {} -RenderTargetAndTextures::~RenderTargetAndTextures() noexcept { +RenderTarget::~RenderTarget() noexcept { Destroy(); } -void RenderTargetAndTextures::Prepare(int width, int height) { +void RenderTarget::Prepare(int width, int height) { if (width == width_ && height == height_) { return; } @@ -53,7 +53,7 @@ void RenderTargetAndTextures::Prepare(int width, int height) { render_target_ = builder.build(*engine_); } -void RenderTargetAndTextures::Destroy() { +void RenderTarget::Destroy() { if (render_target_) { engine_->destroy(render_target_); render_target_ = nullptr; @@ -62,15 +62,15 @@ void RenderTargetAndTextures::Destroy() { depth_texture_.reset(); } -Texture* RenderTargetAndTextures::GetColorTexture() const { +Texture* RenderTarget::GetColorTexture() const { return color_texture_.get(); } -Texture* RenderTargetAndTextures::GetDepthTexture() const { +Texture* RenderTarget::GetDepthTexture() const { return depth_texture_.get(); } -filament::RenderTarget* RenderTargetAndTextures::GetRenderTarget() const { +filament::RenderTarget* RenderTarget::GetFilamentRenderTarget() const { return render_target_; } diff --git a/src/experimental/filament/filament/render_target_util.h b/src/experimental/filament/filament/render_target.h similarity index 83% rename from src/experimental/filament/filament/render_target_util.h rename to src/experimental/filament/filament/render_target.h index ce6b96dc..3e5530cc 100644 --- a/src/experimental/filament/filament/render_target_util.h +++ b/src/experimental/filament/filament/render_target.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDER_TARGET_UTIL_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDER_TARGET_UTIL_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDER_TARGET_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDER_TARGET_H_ #include @@ -24,17 +24,17 @@ namespace mujoco { // Manages a filament RenderTarget and the textures which are bound to it. -class RenderTargetAndTextures { +class RenderTarget { public: // Defines the types of textures to create for the color and depth // attachments. - RenderTargetAndTextures(filament::Engine* engine, + RenderTarget(filament::Engine* engine, RenderTargetTextureType color, RenderTargetTextureType depth); - ~RenderTargetAndTextures() noexcept; + ~RenderTarget() noexcept; - RenderTargetAndTextures(const RenderTargetAndTextures&) = delete; - RenderTargetAndTextures& operator=(const RenderTargetAndTextures&) = delete; + RenderTarget(const RenderTarget&) = delete; + RenderTarget& operator=(const RenderTarget&) = delete; // Creates the textures and render target if the width of height differ from // the last time the render target was prepared. @@ -46,8 +46,8 @@ class RenderTargetAndTextures { // Returns the depth texture. Texture* GetDepthTexture() const; - // Returns the render target. - filament::RenderTarget* GetRenderTarget() const; + // Returns the underlying filament render target. + filament::RenderTarget* GetFilamentRenderTarget() const; private: void Destroy(); @@ -64,4 +64,4 @@ class RenderTargetAndTextures { } // namespace mujoco -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDER_TARGET_UTIL_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDER_TARGET_H_ diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 7009dd17..4f6f1ceb 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -49,7 +49,7 @@ #include "experimental/filament/filament/model_objects.h" #include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/object_manager.h" -#include "experimental/filament/filament/render_target_util.h" +#include "experimental/filament/filament/render_target.h" #include "experimental/filament/filament/texture.h" namespace mujoco { @@ -253,7 +253,7 @@ SceneView::~SceneView() { } void SceneView::Render(filament::Renderer* renderer, DrawMode draw_mode, - filament::RenderTarget* target) { + RenderTarget* target) { filament::View* view = PrepareRenderView(draw_mode); filament::MultiSampleAntiAliasingOptions options = view->getMultiSampleAntiAliasingOptions(); @@ -274,7 +274,8 @@ void SceneView::Render(filament::Renderer* renderer, DrawMode draw_mode, drawable->SetLayerMask(0x00); // Render the reflection to its render target. - reflect_view_->setRenderTarget(reflect_targets_[i]->GetRenderTarget()); + reflect_view_->setRenderTarget( + reflect_targets_[i]->GetFilamentRenderTarget()); renderer->render(reflect_view_); // Unhide the reflective surface. @@ -282,7 +283,7 @@ void SceneView::Render(filament::Renderer* renderer, DrawMode draw_mode, } } - view->setRenderTarget(target); + view->setRenderTarget(target ? target->GetFilamentRenderTarget() : nullptr); renderer->render(view); view->setRenderTarget(nullptr); @@ -516,7 +517,7 @@ void SceneView::AddReflectiveDrawable(Drawable* drawable) { // drawables. filament::Engine* engine = object_mgr_->GetEngine(); while (reflect_targets_.size() < reflectives_.size()) { - reflect_targets_.push_back(std::make_unique( + reflect_targets_.push_back(std::make_unique( engine, RenderTargetTextureType::kReflectionColor, RenderTargetTextureType::kDepth)); } diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index 8ced8be1..8fb686e1 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -38,7 +38,7 @@ #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/model_objects.h" #include "experimental/filament/filament/object_manager.h" -#include "experimental/filament/filament/render_target_util.h" +#include "experimental/filament/filament/render_target.h" namespace mujoco { @@ -71,7 +71,7 @@ class SceneView { using DrawMode = Material::DrawMode; void Render(filament::Renderer* renderer, DrawMode draw_mode, - filament::RenderTarget* target = nullptr); + RenderTarget* target = nullptr); void UploadMesh(const mjModel* model, int id); void UploadTexture(const mjModel* model, int id); @@ -128,7 +128,7 @@ class SceneView { // Each reflective drawable has its own render target which is used to render // the reflected image. - std::vector> reflect_targets_; + std::vector> reflect_targets_; }; } // namespace mujoco From 49b45bd993c3d1ff7d921fcace2ddd89e93d2209 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 7 Apr 2026 13:52:39 -0700 Subject: [PATCH 019/251] Merge vertex_util into math_util. PiperOrigin-RevId: 896071837 Change-Id: Ieec0e4b4b11988805219739a84f413871ae34f1b --- src/experimental/filament/CMakeLists.txt | 2 - .../filament/filament/builtins.cc | 2 +- .../filament/filament/math_util.cc | 39 +++++++++++ .../filament/filament/math_util.h | 16 +++++ src/experimental/filament/filament/mesh.cc | 1 - .../filament/filament/model_util.cc | 1 - .../filament/filament/vertex_util.cc | 65 ------------------- .../filament/filament/vertex_util.h | 41 ------------ 8 files changed, 56 insertions(+), 111 deletions(-) delete mode 100644 src/experimental/filament/filament/vertex_util.cc delete mode 100644 src/experimental/filament/filament/vertex_util.h diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index ca8f3f07..7e972270 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -61,8 +61,6 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/scene_view.h filament/texture.cc filament/texture.h - filament/vertex_util.cc - filament/vertex_util.h ) if(MUJOCO_USE_FILAMENT_MJR_COMPAT) target_sources(${MUJOCO_FILAMENT_TARGET_NAME} diff --git a/src/experimental/filament/filament/builtins.cc b/src/experimental/filament/filament/builtins.cc index 40b727af..5ee489b3 100644 --- a/src/experimental/filament/filament/builtins.cc +++ b/src/experimental/filament/filament/builtins.cc @@ -27,8 +27,8 @@ #include #include #include +#include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" -#include "experimental/filament/filament/vertex_util.h" namespace mujoco { diff --git a/src/experimental/filament/filament/math_util.cc b/src/experimental/filament/filament/math_util.cc index 4449b16c..34e0cb24 100644 --- a/src/experimental/filament/filament/math_util.cc +++ b/src/experimental/filament/filament/math_util.cc @@ -13,8 +13,11 @@ // limitations under the License. #include "experimental/filament/filament/math_util.h" +#include +#include #include +#include #include #include #include @@ -23,7 +26,9 @@ namespace mujoco { using filament::math::float3; using filament::math::float4; +using filament::math::mat3f; using filament::math::mat4; +using filament::math::quatf; mat4 ToReflectionMatrix(const mat4& xform) { const float3 normal = xform[2].xyz; @@ -81,4 +86,38 @@ mat4 CalculateObliqueProjection(const mat4& projection, const float4& plane) { return res; } + +float4 CalculateOrientation(const float3& normal) { + float3 tangent; + float3 bitangent; + if (normal.y < -1.0f + std::numeric_limits::epsilon()) { + // Handle the singularity. + tangent = float3{-1.0f, 0.0f, 0.0f}; + bitangent = float3{0.0f, 0.0f, -1.0f}; + } else { + const float a = 1.0f / (1.0f + normal.y); + const float b = -normal.z * normal.x * a; + tangent = float3(b, -normal.z, 1.0f - normal.z * normal.z * a); + bitangent = float3(1.0f - normal.x * normal.x * a, -normal.x, b); + } + quatf orientation = mat3f::packTangentFrame({tangent, bitangent, normal}); + return float4(orientation.xyz, orientation.w); +} + +float3 CalculateNormal( + const filament::math::float3& p1, + const filament::math::float3& p2, + const filament::math::float3& p3) { + const float3 v12 = p2 - p1; + const float3 v13 = p3 - p1; + return normalize(cross(v12, v13)); +} + +float4 CalculateOrientation( + const filament::math::float3& p1, + const filament::math::float3& p2, + const filament::math::float3& p3) { + return CalculateOrientation(CalculateNormal(p1, p2, p3)); +} + } // namespace mujoco diff --git a/src/experimental/filament/filament/math_util.h b/src/experimental/filament/filament/math_util.h index 61473cec..ee3b2741 100644 --- a/src/experimental/filament/filament/math_util.h +++ b/src/experimental/filament/filament/math_util.h @@ -64,6 +64,22 @@ filament::math::mat4 CalculateObliqueProjection( const filament::math::mat4& projection, const filament::math::float4& plane); +// Calculates the normal of a triangle given its three vertices. +filament::math::float3 CalculateNormal( + const filament::math::float3& p1, + const filament::math::float3& p2, + const filament::math::float3& p3); + +// Calculates the orientation of a vertex given just its normal. +filament::math::float4 CalculateOrientation( + const filament::math::float3& normal); + +// Calculates the orientation of a triangle given its three vertices. +filament::math::float4 CalculateOrientation( + const filament::math::float3& p1, + const filament::math::float3& p2, + const filament::math::float3& p3); + } // namespace mujoco #endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MATH_UTIL_H_ diff --git a/src/experimental/filament/filament/mesh.cc b/src/experimental/filament/filament/mesh.cc index bfdad8be..8acc2d59 100644 --- a/src/experimental/filament/filament/mesh.cc +++ b/src/experimental/filament/filament/mesh.cc @@ -30,7 +30,6 @@ #include #include #include "experimental/filament/filament/math_util.h" -#include "experimental/filament/filament/vertex_util.h" namespace mujoco { diff --git a/src/experimental/filament/filament/model_util.cc b/src/experimental/filament/filament/model_util.cc index e1309bc1..9a4d9e21 100644 --- a/src/experimental/filament/filament/model_util.cc +++ b/src/experimental/filament/filament/model_util.cc @@ -28,7 +28,6 @@ #include #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" -#include "experimental/filament/filament/vertex_util.h" namespace mujoco { diff --git a/src/experimental/filament/filament/vertex_util.cc b/src/experimental/filament/filament/vertex_util.cc deleted file mode 100644 index 3304921d..00000000 --- a/src/experimental/filament/filament/vertex_util.cc +++ /dev/null @@ -1,65 +0,0 @@ -// Copyright 2025 DeepMind Technologies Limited -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "experimental/filament/filament/vertex_util.h" - -#include - -#include -#include -#include -#include -#include - -namespace mujoco { - -using filament::math::float3; -using filament::math::float4; -using filament::math::mat3f; -using filament::math::quatf; - -float4 CalculateOrientation(const float3& normal) { - float3 tangent; - float3 bitangent; - if (normal.y < -1.0f + std::numeric_limits::epsilon()) { - // Handle the singularity. - tangent = float3{-1.0f, 0.0f, 0.0f}; - bitangent = float3{0.0f, 0.0f, -1.0f}; - } else { - const float a = 1.0f / (1.0f + normal.y); - const float b = -normal.z * normal.x * a; - tangent = float3(b, -normal.z, 1.0f - normal.z * normal.z * a); - bitangent = float3(1.0f - normal.x * normal.x * a, -normal.x, b); - } - quatf orientation = mat3f::packTangentFrame({tangent, bitangent, normal}); - return float4(orientation.xyz, orientation.w); -} - -float3 CalculateNormal( - const filament::math::float3& p1, - const filament::math::float3& p2, - const filament::math::float3& p3) { - const float3 v12 = p2 - p1; - const float3 v13 = p3 - p1; - return normalize(cross(v12, v13)); -} - -float4 CalculateOrientation( - const filament::math::float3& p1, - const filament::math::float3& p2, - const filament::math::float3& p3) { - return CalculateOrientation(CalculateNormal(p1, p2, p3)); -} - -} // namespace mujoco diff --git a/src/experimental/filament/filament/vertex_util.h b/src/experimental/filament/filament/vertex_util.h deleted file mode 100644 index e8bd7919..00000000 --- a/src/experimental/filament/filament/vertex_util.h +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2025 DeepMind Technologies Limited -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_VERTEX_UTIL_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_VERTEX_UTIL_H_ - -#include -#include - -namespace mujoco { - -// Calculates the normal of a triangle given its three vertices. -filament::math::float3 CalculateNormal( - const filament::math::float3& p1, - const filament::math::float3& p2, - const filament::math::float3& p3); - -// Calculates the orientation of a vertex given just its normal. -filament::math::float4 CalculateOrientation( - const filament::math::float3& normal); - -// Calculates the orientation of a triangle given its three vertices. -filament::math::float4 CalculateOrientation( - const filament::math::float3& p1, - const filament::math::float3& p2, - const filament::math::float3& p3); - -} // namespace mujoco - -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_VERTEX_UTIL_H_ From 87b0ea785e18f4230a3a3379ed71cc3ee0a911cf Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 8 Apr 2026 01:47:57 -0700 Subject: [PATCH 020/251] Move UpdateMeshData into model_objects.cc. PiperOrigin-RevId: 896346482 Change-Id: Id57586d652faca75d07ccfe0cee72e97afc823fc --- src/experimental/filament/CMakeLists.txt | 1 - .../filament/filament/model_objects.cc | 366 ++++++++++++++++- .../filament/filament/model_objects.h | 1 - .../filament/filament/model_util.cc | 379 ------------------ .../filament/filament/model_util.h | 12 - 5 files changed, 364 insertions(+), 395 deletions(-) delete mode 100644 src/experimental/filament/filament/model_util.cc diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index 7e972270..86d0b009 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -49,7 +49,6 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/mesh.h filament/model_objects.cc filament/model_objects.h - filament/model_util.cc filament/model_util.h filament/object_manager.cc filament/object_manager.h diff --git a/src/experimental/filament/filament/model_objects.cc b/src/experimental/filament/filament/model_objects.cc index 474b982d..5c955032 100644 --- a/src/experimental/filament/filament/model_objects.cc +++ b/src/experimental/filament/filament/model_objects.cc @@ -15,6 +15,10 @@ #include "experimental/filament/filament/model_objects.h" #include +#include +#include +#include +#include #include #include #include @@ -23,14 +27,373 @@ #include #include #include +#include +#include +#include +#include +#include +#include #include #include "experimental/filament/filament/builtins.h" +#include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/texture.h" namespace mujoco { +using filament::math::float2; +using filament::math::float3; +using filament::math::float4; +using filament::math::mat3f; + +enum class MeshType { + kNormal, + kConvexHull, + kHeightField, +}; + +struct MeshBuilder { + MeshBuilder(int nvertices) : nvertices(nvertices) { + positions.reserve(nvertices); + orientations.reserve(nvertices); + uvs.reserve(nvertices); + } + + void Append(const float3& position, const float4& orientation, + const float2& uv) { + positions.push_back(position); + orientations.push_back(orientation); + uvs.push_back(uv); + bounds_min = min(bounds_min, position); + bounds_max = max(bounds_max, position); + } + + int nvertices = 0; + float3 bounds_min = {FLT_MAX, FLT_MAX, FLT_MAX}; + float3 bounds_max = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + std::vector positions; + std::vector orientations; + std::vector uvs; +}; + +static bool UseFaceNormal(const float3& face_normal, + const float3& mesh_normal) { + // clang-format off + return (face_normal[0] * mesh_normal[0] + + face_normal[1] * mesh_normal[1] + + face_normal[2] * mesh_normal[2]) < 0.8f; + // clang-format on +} + +static void FillConvexHullBuffer(MeshBuilder& builder, const mjModel* model, + int meshid) { + const int numvert = model->mesh_graph[model->mesh_graphadr[meshid]]; + const int numface = model->mesh_graph[model->mesh_graphadr[meshid] + 1]; + if (builder.nvertices != numface * 3) { + mju_error("Invalid vertex count (%d vs %d).", builder.nvertices, numface * 3); + return; + } + + const int dataadr = model->mesh_graphadr[meshid] + 2; + const int vertadr = model->mesh_vertadr[meshid]; + const float* vertices = model->mesh_vert + (3 * vertadr); + const int texcoordadr = model->mesh_texcoordadr[meshid]; + const float* texcoords = texcoordadr >= 0 ? model->mesh_texcoord + (2 * texcoordadr) : nullptr; + + for (int face = 0; face < numface; ++face) { + const int j = dataadr + (3 * numvert) + (3 * numface) + (3 * face); + const float3 p1 = ReadFloat3(vertices, model->mesh_graph[j + 0]); + const float3 p2 = ReadFloat3(vertices, model->mesh_graph[j + 1]); + const float3 p3 = ReadFloat3(vertices, model->mesh_graph[j + 2]); + const float4 orientation = CalculateOrientation(p1, p2, p3); + const float2 uv1 = texcoords ? ReadFloat2(texcoords, model->mesh_graph[j + 0]) : float2(0, 0); + const float2 uv2 = texcoords ? ReadFloat2(texcoords, model->mesh_graph[j + 1]) : float2(0, 0); + const float2 uv3 = texcoords ? ReadFloat2(texcoords, model->mesh_graph[j + 2]) : float2(0, 0); + builder.Append(p1, orientation, uv1); + builder.Append(p2, orientation, uv2); + builder.Append(p3, orientation, uv3); + } +} + +static void FillMeshBuffer(MeshBuilder& builder, const mjModel* model, int meshid) { + const int faceadr = model->mesh_faceadr[meshid]; + const int facenum = model->mesh_facenum[meshid]; + if (builder.nvertices != facenum * 3) { + mju_error("Invalid vertex count (%d vs %d).", builder.nvertices, facenum * 3); + return; + } + + const int vertadr = model->mesh_vertadr[meshid]; + const float* vertices = model->mesh_vert + (3 * vertadr); + const int normaladr = model->mesh_normaladr[meshid]; + const float* normals = model->mesh_normal + 3 * normaladr; + const int texcoordadr = model->mesh_texcoordadr[meshid]; + const float* texcoords = texcoordadr >= 0 ? model->mesh_texcoord + (2 * texcoordadr) : nullptr; + + for (int i = 0; i < facenum; ++i) { + const int face = 3 * (faceadr + i); + + const float3 p1 = ReadFloat3(vertices, model->mesh_face[face + 0]); + const float3 p2 = ReadFloat3(vertices, model->mesh_face[face + 1]); + const float3 p3 = ReadFloat3(vertices, model->mesh_face[face + 2]); + const float3 face_normal = CalculateNormal(p1, p2, p3); + const float3 n1 = ReadFloat3(normals, model->mesh_facenormal[face + 0]); + const float3 n2 = ReadFloat3(normals, model->mesh_facenormal[face + 1]); + const float3 n3 = ReadFloat3(normals, model->mesh_facenormal[face + 2]); + const float2 uv1 = texcoords ? ReadFloat2(texcoords, model->mesh_facetexcoord[face + 0]) : float2(0, 0); + const float2 uv2 = texcoords ? ReadFloat2(texcoords, model->mesh_facetexcoord[face + 1]) : float2(0, 0); + const float2 uv3 = texcoords ? ReadFloat2(texcoords, model->mesh_facetexcoord[face + 2]) : float2(0, 0); + + if (UseFaceNormal(face_normal, n1)) { + builder.Append(p1, CalculateOrientation(face_normal), uv1); + } else { + builder.Append(p1, CalculateOrientation(n1), uv1); + } + + if (UseFaceNormal(face_normal, n2)) { + builder.Append(p2, CalculateOrientation(face_normal), uv2); + } else { + builder.Append(p2, CalculateOrientation(n2), uv2); + } + + if (UseFaceNormal(face_normal, n3)) { + builder.Append(p3, CalculateOrientation(face_normal), uv3); + } else { + builder.Append(p3, CalculateOrientation(n3), uv3); + } + } +} + +static void FillHeightFieldBuffer(MeshBuilder& builder, const mjModel* model, + int hfieldid) { + auto append_tri = [&](float3 a, float3 b, float3 c) { + float4 orientation = CalculateOrientation(a, b, c); + builder.Append(a, orientation, float2(0, 0)); + builder.Append(b, orientation, float2(0, 0)); + builder.Append(c, orientation, float2(0, 0)); + }; + auto append_quad = [&](float3 a, float3 b, float3 c, float3 d) { + append_tri(a, b, d); + append_tri(d, b, c); + }; + + const float* data = model->hfield_data + model->hfield_adr[hfieldid]; + const int nrow = model->hfield_nrow[hfieldid]; + const int ncol = model->hfield_ncol[hfieldid]; + const float height = 0.5f * (nrow - 1); + const float width = 0.5f * (ncol - 1); + float sz[4]; + for (int i = 0; i < 4; ++i) { + sz[i] = static_cast(model->hfield_size[4 * hfieldid + i]); + } + + auto get_pos = [=](int r, int c) { + const float x = sz[0] * (c / width - 1.0f); + const float y = sz[1] * (r / height - 1.0f); + const float z = sz[2] * data[(r * ncol) + c]; + return float3{x, y, z}; + }; + + // For each quad defined by 4 points in the height field, we will create 4 + // triangles by introducing a vertex in the middle of the quad. + // a---b + // |\ /| + // | m | + // |/ \| + // d---c + for (int row = 0; row < nrow - 1; ++row) { + for (int col = 0; col < ncol - 1; ++col) { + const float3 a = get_pos(row, col); + const float3 b = get_pos(row, col + 1); + const float3 c = get_pos(row + 1, col + 1); + const float3 d = get_pos(row + 1, col); + + const float mid_x = (a.x + b.x) * 0.5f; + const float mid_y = (a.y + d.y) * 0.5f; + + // To determine the height of the middle vertex, we look at the heights + // of the opposing corners (i.e. {a, c} and {b, d}). Our goal is to avoid + // creating any odd bumps or valleys in the height field if possible. + // + // If one of the two opposing corners are of the same height, then we + // set the middle vertex such that we're effectively rendering two + // triangles, preventing an odd bump. Otherwise, we use the higher + // midpoint between two opposing corners to prevent valleys. + // 0---0 0---0 6---4 + // |\ | | /| |\ /| + // | 0 | | 0 | | 7 | + // | \| |/ | |/ \| + // 2---0 0---2 0---8 + float mid_z = 0; + if (a.z == c.z && b.z != d.z) { + mid_z = a.z; + } else if (a.z != c.z && b.z == d.z) { + mid_z = b.z; + } else { + const float mid_z_ac = (a.z + c.z) * 0.5f; + const float mid_z_bd = (b.z + d.z) * 0.5f; + mid_z = std::max(mid_z_ac, mid_z_bd); + } + + const float3 mid = {mid_x, mid_y, mid_z}; + append_tri(a, b, mid); + append_tri(b, c, mid); + append_tri(c, d, mid); + append_tri(d, a, mid); + } + } + // Build the left edge. + for (int row = 0; row < nrow - 1; ++row) { + const float3 a = get_pos(row, 0); + const float3 b = get_pos(row + 1, 0); + const float3 c = {b.x, b.y, -sz[3]}; + const float3 d = {a.x, a.y, -sz[3]}; + append_quad(a, b, c, d); + } + // Build the right edge. + for (int row = 0; row < nrow - 1; ++row) { + const float3 a = get_pos(row + 1, ncol - 1); + const float3 b = get_pos(row, ncol - 1); + const float3 c = {b.x, b.y, -sz[3]}; + const float3 d = {a.x, a.y, -sz[3]}; + append_quad(a, b, c, d); + } + // Build the front edge. + for (int col = 0; col < ncol - 1; ++col) { + const float3 a = get_pos(0, col); + const float3 b = get_pos(0, col + 1); + const float3 c = {b.x, b.y, -sz[3]}; + const float3 d = {a.x, a.y, -sz[3]}; + append_quad(a, b, c, d); + } + // Build the back edge. + for (int col = 0; col < ncol - 1; ++col) { + const float3 a = get_pos(nrow - 1, col + 1); + const float3 b = get_pos(nrow - 1, col); + const float3 c = {b.x, b.y, -sz[3]}; + const float3 d = {a.x, a.y, -sz[3]}; + append_quad(a, b, c, d); + } + // Build the base. We use the visualization quality as the size rather than + // the height field dimensions. + const float base_width = (0.5f * model->vis.quality.numquads); + const float base_height = (0.5f * model->vis.quality.numquads); + for (int row = 0; row < model->vis.quality.numquads; ++row) { + for (int col = 0; col < model->vis.quality.numquads; ++col) { + const float x0 = sz[0] * ((col + 0) / base_width - 1.0f); + const float x1 = sz[0] * ((col + 1) / base_width - 1.0f); + const float y0 = sz[1] * ((row + 0) / base_height - 1.0f); + const float y1 = sz[1] * ((row + 1) / base_height - 1.0f); + append_quad({x0, y0, -sz[3]}, {x0, y1, -sz[3]}, {x1, y1, -sz[3]}, + {x1, y0, -sz[3]}); + } + } +} + +static int CalculateHeightFieldVertexCount(const mjModel* model, int hfieldid) { + const int nrow = model->hfield_nrow[hfieldid]; + const int ncol = model->hfield_ncol[hfieldid]; + + // For details, see the logic in FillHeightFieldBuffer for how many vertices + // we need. But, in general... + + // We use 4 triangles (i.e. 12 vertices) per quad. + const int surface_count = 12 * (nrow - 1) * (ncol - 1); + // We use 1 quad (i.e. 6 vertices) per edge element. We double this because + // we have two edges per dimension (e.g. left/right and front/back). + const int edge_count = (12 * (nrow - 1)) + (12 * (ncol - 1)); + // We use 1 quad (i.e. 6 vertices) per base element. We use the visualization + // quality as the size rather than the height field dimensions. + const int base_count = + 6 * model->vis.quality.numquads * model->vis.quality.numquads; + + const int total_count = surface_count + edge_count + base_count; + return total_count; +} + +static bool HasUvs(const mjModel* model, int id, MeshType mesh_type) { + return mesh_type != MeshType::kHeightField && + model->mesh_texcoordadr[id] >= 0; +} + +static bool IsValidIndex(const mjModel* model, int id, MeshType mesh_type) { + switch (mesh_type) { + case MeshType::kNormal: + return id >= 0 && id < model->nmesh; + case MeshType::kConvexHull: + return id >= 0 && id < model->nmesh; + case MeshType::kHeightField: + return id >= 0 && id < model->nhfield; + } +} + +static int GetNumVertices(const mjModel* model, int id, MeshType mesh_type) { + switch (mesh_type) { + case MeshType::kNormal: + return 3 * model->mesh_facenum[id]; + case MeshType::kConvexHull: + return 3 * model->mesh_graph[model->mesh_graphadr[id] + 1]; + case MeshType::kHeightField: + return CalculateHeightFieldVertexCount(model, id); + } +} + +static void UpdateMeshData(MeshData* data, const mjModel* model, int id, + MeshType mesh_type) { + if (!IsValidIndex(model, id, mesh_type)) { + mju_error("Invalid index %d for type %d", id, mesh_type); + return; + } + + const int num_vertices = GetNumVertices(model, id, mesh_type); + const bool has_uvs = HasUvs(model, id, mesh_type); + + MeshBuilder* builder = new MeshBuilder(num_vertices); + data->user_data = builder; + data->release_callback = [](void* user_data) { + delete static_cast(user_data); + }; + + switch (mesh_type) { + case MeshType::kNormal: + FillMeshBuffer(*builder, model, id); + break; + case MeshType::kConvexHull: + FillConvexHullBuffer(*builder, model, id); + break; + case MeshType::kHeightField: + FillHeightFieldBuffer(*builder, model, id); + break; + } + + data->primitive_type = mjPRIM_TYPE_TRIANGLES; + data->nvertices = num_vertices; + data->nindices = data->nvertices; + data->indices = nullptr; + data->index_type = data->nvertices >= std::numeric_limits::max() + ? mjINDEX_TYPE_UINT + : mjINDEX_TYPE_USHORT; + data->nattributes = has_uvs ? 3 : 2; + data->attributes[0].usage = mjVERTEX_ATTRIBUTE_POSITION; + data->attributes[0].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT3; + data->attributes[0].bytes = builder->positions.data(); + data->attributes[1].usage = mjVERTEX_ATTRIBUTE_TANGENTS; + data->attributes[1].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT4; + data->attributes[1].bytes = builder->orientations.data(); + if (has_uvs) { + data->attributes[2].usage = mjVERTEX_ATTRIBUTE_UV; + data->attributes[2].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT2; + data->attributes[2].bytes = builder->uvs.data(); + } + data->bounds_min[0] = builder->bounds_min.x; + data->bounds_min[1] = builder->bounds_min.y; + data->bounds_min[2] = builder->bounds_min.z; + data->bounds_max[0] = builder->bounds_max.x; + data->bounds_max[1] = builder->bounds_max.y; + data->bounds_max[2] = builder->bounds_max.z; +} + ModelObjects::ModelObjects(const mjModel* model, filament::Engine* engine) : model_(model), engine_(engine) { const int nstack = model->vis.quality.numstacks; @@ -218,8 +581,7 @@ filament::IndirectLight* ModelObjects::CreateIndirectLight(int tex_id, } builder.intensity(intensity); // Rotate the light to match mujoco's Z-up convention. - builder.rotation(filament::math::mat3f::rotation( - filament::math::f::PI / 2, filament::math::float3{1, 0, 0})); + builder.rotation(mat3f::rotation(filament::math::f::PI / 2, float3{1, 0, 0})); filament::IndirectLight* indirect_light = builder.build(*engine_); indirect_lights_.push_back(indirect_light); return indirect_light; diff --git a/src/experimental/filament/filament/model_objects.h b/src/experimental/filament/filament/model_objects.h index 0921169c..f132d9ba 100644 --- a/src/experimental/filament/filament/model_objects.h +++ b/src/experimental/filament/filament/model_objects.h @@ -23,7 +23,6 @@ #include #include #include -#include #include #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/texture.h" diff --git a/src/experimental/filament/filament/model_util.cc b/src/experimental/filament/filament/model_util.cc deleted file mode 100644 index 9a4d9e21..00000000 --- a/src/experimental/filament/filament/model_util.cc +++ /dev/null @@ -1,379 +0,0 @@ -// Copyright 2025 DeepMind Technologies Limited -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "experimental/filament/filament/model_util.h" - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include "experimental/filament/filament/math_util.h" -#include "experimental/filament/filament/mesh.h" - -namespace mujoco { - -using filament::math::float2; -using filament::math::float3; -using filament::math::float4; - -struct MeshBuilder { - MeshBuilder(int nvertices) : nvertices(nvertices) { - positions.reserve(nvertices); - orientations.reserve(nvertices); - uvs.reserve(nvertices); - } - - void Append(const float3& position, const float4& orientation, - const float2& uv) { - positions.push_back(position); - orientations.push_back(orientation); - uvs.push_back(uv); - bounds_min = min(bounds_min, position); - bounds_max = max(bounds_max, position); - } - - int nvertices = 0; - float3 bounds_min = {FLT_MAX, FLT_MAX, FLT_MAX}; - float3 bounds_max = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; - std::vector positions; - std::vector orientations; - std::vector uvs; -}; - -static bool UseFaceNormal(const float3& face_normal, - const float3& mesh_normal) { - // clang-format off - return face_normal[0] * mesh_normal[0] + - face_normal[1] * mesh_normal[1] + - face_normal[2] * mesh_normal[2] < 0.8f; - // clang-format on -} - - -static void FillConvexHullBuffer(MeshBuilder& builder, const mjModel* model, - int meshid) { - const int numvert = model->mesh_graph[model->mesh_graphadr[meshid]]; - const int numface = model->mesh_graph[model->mesh_graphadr[meshid] + 1]; - if (builder.nvertices != numface * 3) { - mju_error("Invalid vertex count (%d vs %d).", builder.nvertices, numface * 3); - return; - } - - const int dataadr = model->mesh_graphadr[meshid] + 2; - const int vertadr = model->mesh_vertadr[meshid]; - const float* vertices = model->mesh_vert + (3 * vertadr); - const int texcoordadr = model->mesh_texcoordadr[meshid]; - const float* texcoords = texcoordadr >= 0 ? model->mesh_texcoord + (2 * texcoordadr) : nullptr; - - for (int face = 0; face < numface; ++face) { - const int j = dataadr + (3 * numvert) + (3 * numface) + (3 * face); - const float3 p1 = ReadFloat3(vertices, model->mesh_graph[j + 0]); - const float3 p2 = ReadFloat3(vertices, model->mesh_graph[j + 1]); - const float3 p3 = ReadFloat3(vertices, model->mesh_graph[j + 2]); - const float4 orientation = CalculateOrientation(p1, p2, p3); - const float2 uv1 = texcoords ? ReadFloat2(texcoords, model->mesh_graph[j + 0]) : float2(0, 0); - const float2 uv2 = texcoords ? ReadFloat2(texcoords, model->mesh_graph[j + 1]) : float2(0, 0); - const float2 uv3 = texcoords ? ReadFloat2(texcoords, model->mesh_graph[j + 2]) : float2(0, 0); - builder.Append(p1, orientation, uv1); - builder.Append(p2, orientation, uv2); - builder.Append(p3, orientation, uv3); - } -} - -static void FillMeshBuffer(MeshBuilder& builder, const mjModel* model, int meshid) { - const int faceadr = model->mesh_faceadr[meshid]; - const int facenum = model->mesh_facenum[meshid]; - if (builder.nvertices != facenum * 3) { - mju_error("Invalid vertex count (%d vs %d).", builder.nvertices, facenum * 3); - return; - } - - const int vertadr = model->mesh_vertadr[meshid]; - const float* vertices = model->mesh_vert + (3 * vertadr); - const int normaladr = model->mesh_normaladr[meshid]; - const float* normals = model->mesh_normal + 3 * normaladr; - const int texcoordadr = model->mesh_texcoordadr[meshid]; - const float* texcoords = texcoordadr >= 0 ? model->mesh_texcoord + (2 * texcoordadr) : nullptr; - - for (int i = 0; i < facenum; ++i) { - const int face = 3 * (faceadr + i); - - const float3 p1 = ReadFloat3(vertices, model->mesh_face[face + 0]); - const float3 p2 = ReadFloat3(vertices, model->mesh_face[face + 1]); - const float3 p3 = ReadFloat3(vertices, model->mesh_face[face + 2]); - const float3 face_normal = CalculateNormal(p1, p2, p3); - const float3 n1 = ReadFloat3(normals, model->mesh_facenormal[face + 0]); - const float3 n2 = ReadFloat3(normals, model->mesh_facenormal[face + 1]); - const float3 n3 = ReadFloat3(normals, model->mesh_facenormal[face + 2]); - const float2 uv1 = texcoords ? ReadFloat2(texcoords, model->mesh_facetexcoord[face + 0]) : float2(0, 0); - const float2 uv2 = texcoords ? ReadFloat2(texcoords, model->mesh_facetexcoord[face + 1]) : float2(0, 0); - const float2 uv3 = texcoords ? ReadFloat2(texcoords, model->mesh_facetexcoord[face + 2]) : float2(0, 0); - - if (UseFaceNormal(face_normal, n1)) { - builder.Append(p1, CalculateOrientation(face_normal), uv1); - } else { - builder.Append(p1, CalculateOrientation(n1), uv1); - } - - if (UseFaceNormal(face_normal, n2)) { - builder.Append(p2, CalculateOrientation(face_normal), uv2); - } else { - builder.Append(p2, CalculateOrientation(n2), uv2); - } - - if (UseFaceNormal(face_normal, n3)) { - builder.Append(p3, CalculateOrientation(face_normal), uv3); - } else { - builder.Append(p3, CalculateOrientation(n3), uv3); - } - } -} - -static void FillHeightFieldBuffer(MeshBuilder& builder, const mjModel* model, - int hfieldid) { - auto append_tri = [&](float3 a, float3 b, float3 c) { - float4 orientation = CalculateOrientation(a, b, c); - builder.Append(a, orientation, float2(0, 0)); - builder.Append(b, orientation, float2(0, 0)); - builder.Append(c, orientation, float2(0, 0)); - }; - auto append_quad = [&](float3 a, float3 b, float3 c, float3 d) { - append_tri(a, b, d); - append_tri(d, b, c); - }; - - const float* data = model->hfield_data + model->hfield_adr[hfieldid]; - const int nrow = model->hfield_nrow[hfieldid]; - const int ncol = model->hfield_ncol[hfieldid]; - const float height = 0.5f * (nrow - 1); - const float width = 0.5f * (ncol - 1); - float sz[4]; - for (int i = 0; i < 4; ++i) { - sz[i] = static_cast(model->hfield_size[4 * hfieldid + i]); - } - - auto get_pos = [=](int r, int c) { - const float x = sz[0] * (c / width - 1.0f); - const float y = sz[1] * (r / height - 1.0f); - const float z = sz[2] * data[(r * ncol) + c]; - return float3{x, y, z}; - }; - - // For each quad defined by 4 points in the height field, we will create 4 - // triangles by introducing a vertex in the middle of the quad. - // a---b - // |\ /| - // | m | - // |/ \| - // d---c - for (int row = 0; row < nrow - 1; ++row) { - for (int col = 0; col < ncol - 1; ++col) { - const float3 a = get_pos(row, col); - const float3 b = get_pos(row, col + 1); - const float3 c = get_pos(row + 1, col + 1); - const float3 d = get_pos(row + 1, col); - - const float mid_x = (a.x + b.x) * 0.5f; - const float mid_y = (a.y + d.y) * 0.5f; - - // To determine the height of the middle vertex, we look at the heights - // of the opposing corners (i.e. {a, c} and {b, d}). Our goal is to avoid - // creating any odd bumps or valleys in the height field if possible. - // - // If one of the two opposing corners are of the same height, then we - // set the middle vertex such that we're effectively rendering two - // triangles, preventing an odd bump. Otherwise, we use the higher - // midpoint between two opposing corners to prevent valleys. - // 0---0 0---0 6---4 - // |\ | | /| |\ /| - // | 0 | | 0 | | 7 | - // | \| |/ | |/ \| - // 2---0 0---2 0---8 - float mid_z = 0; - if (a.z == c.z && b.z != d.z) { - mid_z = a.z; - } else if (a.z != c.z && b.z == d.z) { - mid_z = b.z; - } else { - const float mid_z_ac = (a.z + c.z) * 0.5f; - const float mid_z_bd = (b.z + d.z) * 0.5f; - mid_z = std::max(mid_z_ac, mid_z_bd); - } - - const float3 mid = {mid_x, mid_y, mid_z}; - append_tri(a, b, mid); - append_tri(b, c, mid); - append_tri(c, d, mid); - append_tri(d, a, mid); - } - } - // Build the left edge. - for (int row = 0; row < nrow - 1; ++row) { - const float3 a = get_pos(row, 0); - const float3 b = get_pos(row + 1, 0); - const float3 c = {b.x, b.y, -sz[3]}; - const float3 d = {a.x, a.y, -sz[3]}; - append_quad(a, b, c, d); - } - // Build the right edge. - for (int row = 0; row < nrow - 1; ++row) { - const float3 a = get_pos(row + 1, ncol - 1); - const float3 b = get_pos(row, ncol - 1); - const float3 c = {b.x, b.y, -sz[3]}; - const float3 d = {a.x, a.y, -sz[3]}; - append_quad(a, b, c, d); - } - // Build the front edge. - for (int col = 0; col < ncol - 1; ++col) { - const float3 a = get_pos(0, col); - const float3 b = get_pos(0, col + 1); - const float3 c = {b.x, b.y, -sz[3]}; - const float3 d = {a.x, a.y, -sz[3]}; - append_quad(a, b, c, d); - } - // Build the back edge. - for (int col = 0; col < ncol - 1; ++col) { - const float3 a = get_pos(nrow - 1, col + 1); - const float3 b = get_pos(nrow - 1, col); - const float3 c = {b.x, b.y, -sz[3]}; - const float3 d = {a.x, a.y, -sz[3]}; - append_quad(a, b, c, d); - } - // Build the base. We use the visualization quality as the size rather than - // the height field dimensions. - const float base_width = (0.5f * model->vis.quality.numquads); - const float base_height = (0.5f * model->vis.quality.numquads); - for (int row = 0; row < model->vis.quality.numquads; ++row) { - for (int col = 0; col < model->vis.quality.numquads; ++col) { - const float x0 = sz[0] * ((col + 0) / base_width - 1.0f); - const float x1 = sz[0] * ((col + 1) / base_width - 1.0f); - const float y0 = sz[1] * ((row + 0) / base_height - 1.0f); - const float y1 = sz[1] * ((row + 1) / base_height - 1.0f); - append_quad({x0, y0, -sz[3]}, {x0, y1, -sz[3]}, {x1, y1, -sz[3]}, - {x1, y0, -sz[3]}); - } - } -} - -static int CalculateHeightFieldVertexCount(const mjModel* model, int hfieldid) { - const int nrow = model->hfield_nrow[hfieldid]; - const int ncol = model->hfield_ncol[hfieldid]; - - // For details, see the logic in FillHeightFieldBuffer for how many vertices - // we need. But, in general... - - // We use 4 triangles (i.e. 12 vertices) per quad. - const int surface_count = 12 * (nrow - 1) * (ncol - 1); - // We use 1 quad (i.e. 6 vertices) per edge element. We double this because - // we have two edges per dimension (e.g. left/right and front/back). - const int edge_count = (12 * (nrow - 1)) + (12 * (ncol - 1)); - // We use 1 quad (i.e. 6 vertices) per base element. We use the visualization - // quality as the size rather than the height field dimensions. - const int base_count = - 6 * model->vis.quality.numquads * model->vis.quality.numquads; - - const int total_count = surface_count + edge_count + base_count; - return total_count; -} - -static bool HasUvs(const mjModel* model, int id, MeshType mesh_type) { - return mesh_type != MeshType::kHeightField && - model->mesh_texcoordadr[id] >= 0; -} - -static bool IsValidIndex(const mjModel* model, int id, MeshType mesh_type) { - switch (mesh_type) { - case MeshType::kNormal: - return id >= 0 && id < model->nmesh; - case MeshType::kConvexHull: - return id >= 0 && id < model->nmesh; - case MeshType::kHeightField: - return id >= 0 && id < model->nhfield; - } -} - -static int GetNumVertices(const mjModel* model, int id, MeshType mesh_type) { - switch (mesh_type) { - case MeshType::kNormal: - return 3 * model->mesh_facenum[id]; - case MeshType::kConvexHull: - return 3 * model->mesh_graph[model->mesh_graphadr[id] + 1]; - case MeshType::kHeightField: - return CalculateHeightFieldVertexCount(model, id); - } -} - -void UpdateMeshData(MeshData* data, const mjModel* model, int id, - MeshType mesh_type) { - if (!IsValidIndex(model, id, mesh_type)) { - mju_error("Invalid index %d for type %d", id, mesh_type); - return; - } - - const int num_vertices = GetNumVertices(model, id, mesh_type); - const bool has_uvs = HasUvs(model, id, mesh_type); - - MeshBuilder* builder = new MeshBuilder(num_vertices); - data->user_data = builder; - data->release_callback = [](void* user_data) { - delete static_cast(user_data); - }; - - switch (mesh_type) { - case MeshType::kNormal: - FillMeshBuffer(*builder, model, id); - break; - case MeshType::kConvexHull: - FillConvexHullBuffer(*builder, model, id); - break; - case MeshType::kHeightField: - FillHeightFieldBuffer(*builder, model, id); - break; - } - - data->primitive_type = mjPRIM_TYPE_TRIANGLES; - data->nvertices = num_vertices; - data->nindices = data->nvertices; - data->indices = nullptr; - data->index_type = data->nvertices >= std::numeric_limits::max() - ? mjINDEX_TYPE_UINT - : mjINDEX_TYPE_USHORT; - data->nattributes = has_uvs ? 3 : 2; - data->attributes[0].usage = mjVERTEX_ATTRIBUTE_POSITION; - data->attributes[0].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT3; - data->attributes[0].bytes = builder->positions.data(); - data->attributes[1].usage = mjVERTEX_ATTRIBUTE_TANGENTS; - data->attributes[1].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT4; - data->attributes[1].bytes = builder->orientations.data(); - if (has_uvs) { - data->attributes[2].usage = mjVERTEX_ATTRIBUTE_UV; - data->attributes[2].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT2; - data->attributes[2].bytes = builder->uvs.data(); - } - data->bounds_min[0] = builder->bounds_min.x; - data->bounds_min[1] = builder->bounds_min.y; - data->bounds_min[2] = builder->bounds_min.z; - data->bounds_max[0] = builder->bounds_max.x; - data->bounds_max[1] = builder->bounds_max.y; - data->bounds_max[2] = builder->bounds_max.z; -} -} // namespace mujoco diff --git a/src/experimental/filament/filament/model_util.h b/src/experimental/filament/filament/model_util.h index 6a23ee8f..06501ae7 100644 --- a/src/experimental/filament/filament/model_util.h +++ b/src/experimental/filament/filament/model_util.h @@ -22,21 +22,9 @@ #include #include #include -#include "experimental/filament/filament/mesh.h" namespace mujoco { -// The types of meshes stored in the mjModel. -enum class MeshType { - kNormal, - kConvexHull, - kHeightField, -}; - -// Populates the given MeshData with data for the element in the model. -void UpdateMeshData(MeshData* data, const mjModel* model, int id, - MeshType mesh_type); - // Reads a value with the given name from the mjModel's data sections. The // default_value is returned if the named element is not found. template From c6a434a408d7cb8a7ff679c9bffbc3e7d9a8cf4f Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 8 Apr 2026 04:37:30 -0700 Subject: [PATCH 021/251] Move flex/skin Mesh creation into ModelObjects. PiperOrigin-RevId: 896419032 Change-Id: Ib0f0cd0c80c9f630127f2f72dbaed7c644046663 --- src/experimental/filament/CMakeLists.txt | 2 - .../filament/filament/drawable.cc | 20 +-- .../filament/filament/geom_util.cc | 137 ------------------ .../filament/filament/geom_util.h | 30 ---- .../filament/filament/model_objects.cc | 121 ++++++++++++++++ .../filament/filament/model_objects.h | 5 +- 6 files changed, 135 insertions(+), 180 deletions(-) delete mode 100644 src/experimental/filament/filament/geom_util.cc delete mode 100644 src/experimental/filament/filament/geom_util.h diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index 86d0b009..367f2725 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -33,8 +33,6 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/filament_context.h filament/filament_platform_factory.cc filament/filament_platform_factory.h - filament/geom_util.cc - filament/geom_util.h filament/gui_view.cc filament/gui_view.h filament/imgui_editor.cc diff --git a/src/experimental/filament/filament/drawable.cc b/src/experimental/filament/filament/drawable.cc index b3649350..53c91144 100644 --- a/src/experimental/filament/filament/drawable.cc +++ b/src/experimental/filament/filament/drawable.cc @@ -16,9 +16,7 @@ #include #include -#include #include -#include #include #include @@ -32,7 +30,6 @@ #include #include #include -#include "experimental/filament/filament/geom_util.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" @@ -171,15 +168,18 @@ Drawable::Drawable(ObjectManager* object_mgr, ModelObjects* model_objects, void Drawable::Update(const mjModel* model, const mjvScene* scene, const mjvGeom& geom) { - if (geom.type == mjGEOM_FLEX || geom.type == mjGEOM_SKIN) { - // Flex geometry is updated every frame with new vertex data. - filament::Engine* engine = renderables_.GetEngine(); - std::unique_ptr mesh = CreateGeomBuffers(engine, model, scene, geom); - + // Flex and skin geometries are recreated every frame from the scene data. + if (geom.type == mjGEOM_FLEX) { if (renderables_.GetNumEntities() == 0) { - renderables_.Append(std::move(mesh)); + renderables_.Append(model_objs_->CreateFlexMesh(scene, geom)); } else { - renderables_.Update(0, std::move(mesh)); + renderables_.Update(0, model_objs_->CreateFlexMesh(scene, geom)); + } + } else if (geom.type == mjGEOM_SKIN) { + if (renderables_.GetNumEntities() == 0) { + renderables_.Append(model_objs_->CreateSkinMesh(scene, geom)); + } else { + renderables_.Update(0, model_objs_->CreateSkinMesh(scene, geom)); } } diff --git a/src/experimental/filament/filament/geom_util.cc b/src/experimental/filament/filament/geom_util.cc deleted file mode 100644 index 7b02a02a..00000000 --- a/src/experimental/filament/filament/geom_util.cc +++ /dev/null @@ -1,137 +0,0 @@ -// Copyright 2025 DeepMind Technologies Limited -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "experimental/filament/filament/geom_util.h" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include "experimental/filament/filament/mesh.h" - -namespace mujoco { - -static std::span GetPositions(const mjModel* model, - const mjvScene* scene, - const mjvGeom& geom) { - if (geom.type == mjGEOM_FLEX) { - const int num = 9 * scene->flexfaceused[geom.objid]; - const int addr = scene->flexfaceadr[geom.objid]; - const float* ptr = scene->flexface + (9 * addr); - return {ptr, static_cast(num)}; - } else { - const int num = 3 * scene->skinvertnum[geom.objid]; - const int addr = scene->skinvertadr[geom.objid]; - const float* ptr = scene->skinvert + (3 * addr); - return {ptr, static_cast(num)}; - } -} - -static std::span GetNormals(const mjModel* model, - const mjvScene* scene, - const mjvGeom& geom) { - if (geom.type == mjGEOM_FLEX) { - const int num = 9 * scene->flexfaceused[geom.objid]; - const int addr = scene->flexfaceadr[geom.objid]; - const float* ptr = scene->flexnormal + (9 * addr); - return {ptr, static_cast(num)}; - } else { - const int num = 3 * scene->skinvertnum[geom.objid]; - const int addr = scene->skinvertadr[geom.objid]; - const float* ptr = scene->skinnormal + (3 * addr); - return {ptr, static_cast(num)}; - } -} - -static std::span GetUvs(const mjModel* model, - const mjvScene* scene, - const mjvGeom& geom) { - if (geom.type == mjGEOM_FLEX) { - if (geom.texcoord && geom.matid >= 0) { - const int num = 6 * scene->flexfaceused[geom.objid]; - const int addr = scene->flexfaceadr[geom.objid]; - const float* ptr = scene->flextexcoord + (6 * addr); - return {ptr, static_cast(num)}; - } else { - const float* ptr = nullptr; - return {ptr, 0}; - } - } else { - if (model->skin_texcoordadr[geom.objid] >= 0) { - const int num = 3 * scene->skinvertnum[geom.objid]; - const int addr = model->skin_texcoordadr[geom.objid]; - const float* ptr = model->skin_texcoord + (2 * addr); - return {ptr, static_cast(num)}; - } else { - const float* ptr = nullptr; - return {ptr, 0}; - } - } -} - -static std::span GetIndices(const mjModel* model, - const mjvScene* scene, - const mjvGeom& geom) { - if (geom.type == mjGEOM_FLEX) { - const int* ptr = nullptr; - return {ptr, 0}; - } else { - const int num = 3 * model->skin_facenum[geom.objid]; - const int* ptr = model->skin_face + 3 * model->skin_faceadr[geom.objid]; - return {ptr, static_cast(num)}; - } -} - -MeshPtr CreateGeomBuffers(filament::Engine* engine, const mjModel* model, - const mjvScene* scene, const mjvGeom& geom) { - auto positions = GetPositions(model, scene, geom); - auto normals = GetNormals(model, scene, geom); - auto uvs = GetUvs(model, scene, geom); - auto indices = GetIndices(model, scene, geom); - - int num_indices = indices.size(); - if (num_indices == 0 && geom.type == mjGEOM_FLEX) { - num_indices = 3 * scene->flexfaceused[geom.objid]; - } - - MeshData data; - DefaultMeshData(&data); - - data.nattributes = uvs.data() ? 3 : 2; - data.attributes[0].usage = mjVERTEX_ATTRIBUTE_POSITION; - data.attributes[0].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT3; - data.attributes[0].bytes = positions.data(); - data.attributes[1].usage = mjVERTEX_ATTRIBUTE_NORMAL; - data.attributes[1].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT3; - data.attributes[1].bytes = normals.data(); - data.attributes[2].usage = mjVERTEX_ATTRIBUTE_UV; - data.attributes[2].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT2; - data.attributes[2].bytes = uvs.data(); - data.nvertices = positions.size() / 3; - data.nindices = num_indices; - data.indices = indices.data(); - data.index_type = mjINDEX_TYPE_UINT; - data.primitive_type = mjPRIM_TYPE_TRIANGLES; - data.compute_bounds = true; - data.release_callback = nullptr; - data.user_data = nullptr; - return std::make_unique(engine, data); -} - -} // namespace mujoco diff --git a/src/experimental/filament/filament/geom_util.h b/src/experimental/filament/filament/geom_util.h deleted file mode 100644 index 939c8d91..00000000 --- a/src/experimental/filament/filament/geom_util.h +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2025 DeepMind Technologies Limited -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_GEOM_UTIL_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_GEOM_UTIL_H_ - -#include -#include -#include "experimental/filament/filament/mesh.h" - -namespace mujoco { - -// Populates the FilamentBuffers for a flex geometry. -MeshPtr CreateGeomBuffers(filament::Engine* engine, const mjModel* model, - const mjvScene* scene, const mjvGeom& geom); - -} // namespace mujoco - -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_GEOM_UTIL_H_ diff --git a/src/experimental/filament/filament/model_objects.cc b/src/experimental/filament/filament/model_objects.cc index 5c955032..1f88dce0 100644 --- a/src/experimental/filament/filament/model_objects.cc +++ b/src/experimental/filament/filament/model_objects.cc @@ -17,9 +17,11 @@ #include #include #include +#include #include #include #include +#include #include #include @@ -339,6 +341,77 @@ static int GetNumVertices(const mjModel* model, int id, MeshType mesh_type) { } } +static std::span GetPositions(const mjModel* model, + const mjvScene* scene, + const mjvGeom& geom) { + if (geom.type == mjGEOM_FLEX) { + const int num = 9 * scene->flexfaceused[geom.objid]; + const int addr = scene->flexfaceadr[geom.objid]; + const float* ptr = scene->flexface + (9 * addr); + return {ptr, static_cast(num)}; + } else { + const int num = 3 * scene->skinvertnum[geom.objid]; + const int addr = scene->skinvertadr[geom.objid]; + const float* ptr = scene->skinvert + (3 * addr); + return {ptr, static_cast(num)}; + } +} + +static std::span GetNormals(const mjModel* model, + const mjvScene* scene, + const mjvGeom& geom) { + if (geom.type == mjGEOM_FLEX) { + const int num = 9 * scene->flexfaceused[geom.objid]; + const int addr = scene->flexfaceadr[geom.objid]; + const float* ptr = scene->flexnormal + (9 * addr); + return {ptr, static_cast(num)}; + } else { + const int num = 3 * scene->skinvertnum[geom.objid]; + const int addr = scene->skinvertadr[geom.objid]; + const float* ptr = scene->skinnormal + (3 * addr); + return {ptr, static_cast(num)}; + } +} + +static std::span GetUvs(const mjModel* model, + const mjvScene* scene, + const mjvGeom& geom) { + if (geom.type == mjGEOM_FLEX) { + if (geom.texcoord && geom.matid >= 0) { + const int num = 6 * scene->flexfaceused[geom.objid]; + const int addr = scene->flexfaceadr[geom.objid]; + const float* ptr = scene->flextexcoord + (6 * addr); + return {ptr, static_cast(num)}; + } else { + const float* ptr = nullptr; + return {ptr, 0}; + } + } else { + if (model->skin_texcoordadr[geom.objid] >= 0) { + const int num = 3 * scene->skinvertnum[geom.objid]; + const int addr = model->skin_texcoordadr[geom.objid]; + const float* ptr = model->skin_texcoord + (2 * addr); + return {ptr, static_cast(num)}; + } else { + const float* ptr = nullptr; + return {ptr, 0}; + } + } +} + +static std::span GetIndices(const mjModel* model, + const mjvScene* scene, + const mjvGeom& geom) { + if (geom.type == mjGEOM_FLEX) { + const int* ptr = nullptr; + return {ptr, 0}; + } else { + const int num = 3 * model->skin_facenum[geom.objid]; + const int* ptr = model->skin_face + 3 * model->skin_faceadr[geom.objid]; + return {ptr, static_cast(num)}; + } +} + static void UpdateMeshData(MeshData* data, const mjModel* model, int id, MeshType mesh_type) { if (!IsValidIndex(model, id, mesh_type)) { @@ -394,6 +467,38 @@ static void UpdateMeshData(MeshData* data, const mjModel* model, int id, data->bounds_max[2] = builder->bounds_max.z; } +void UpdateSkinFlexMeshData(MeshData* data, const mjModel* model, + const mjvScene* scene, const mjvGeom& geom) { + auto positions = GetPositions(model, scene, geom); + auto normals = GetNormals(model, scene, geom); + auto uvs = GetUvs(model, scene, geom); + auto indices = GetIndices(model, scene, geom); + + int num_indices = indices.size(); + if (num_indices == 0 && geom.type == mjGEOM_FLEX) { + num_indices = 3 * scene->flexfaceused[geom.objid]; + } + + data->nattributes = uvs.data() ? 3 : 2; + data->attributes[0].usage = mjVERTEX_ATTRIBUTE_POSITION; + data->attributes[0].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT3; + data->attributes[0].bytes = positions.data(); + data->attributes[1].usage = mjVERTEX_ATTRIBUTE_NORMAL; + data->attributes[1].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT3; + data->attributes[1].bytes = normals.data(); + data->attributes[2].usage = mjVERTEX_ATTRIBUTE_UV; + data->attributes[2].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT2; + data->attributes[2].bytes = uvs.data(); + data->nvertices = positions.size() / 3; + data->nindices = num_indices; + data->indices = indices.data(); + data->index_type = mjINDEX_TYPE_UINT; + data->primitive_type = mjPRIM_TYPE_TRIANGLES; + data->compute_bounds = true; + data->release_callback = nullptr; + data->user_data = nullptr; +} + ModelObjects::ModelObjects(const mjModel* model, filament::Engine* engine) : model_(model), engine_(engine) { const int nstack = model->vis.quality.numstacks; @@ -525,6 +630,22 @@ void ModelObjects::UploadHeightField(const mjModel* model, int id) { height_fields_[id] = std::make_unique(engine_, data); } +MeshPtr ModelObjects::CreateFlexMesh(const mjvScene* scene, + const mjvGeom& geom) { + MeshData data; + DefaultMeshData(&data); + UpdateSkinFlexMeshData(&data, model_, scene, geom); + return std::make_unique(engine_, data); +} + +MeshPtr ModelObjects::CreateSkinMesh(const mjvScene* scene, + const mjvGeom& geom) { + MeshData data; + DefaultMeshData(&data); + UpdateSkinFlexMeshData(&data, model_, scene, geom); + return std::make_unique(engine_, data); +} + const Mesh* ModelObjects::GetMeshBuffer(int data_id) const { // As defined by mjv_updateScene: // original mesh: mesh_id * 2 diff --git a/src/experimental/filament/filament/model_objects.h b/src/experimental/filament/filament/model_objects.h index f132d9ba..bc6a005b 100644 --- a/src/experimental/filament/filament/model_objects.h +++ b/src/experimental/filament/filament/model_objects.h @@ -15,7 +15,6 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_OBJECTS_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_OBJECTS_H_ -#include #include #include #include @@ -24,6 +23,7 @@ #include #include #include +#include #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/texture.h" @@ -66,6 +66,9 @@ class ModelObjects { const Texture* GetTexture(int tex_id) const; const Texture* GetTexture(int mat_id, int role) const; + MeshPtr CreateFlexMesh(const mjvScene* scene, const mjvGeom& geom); + MeshPtr CreateSkinMesh(const mjvScene* scene, const mjvGeom& geom); + filament::Skybox* CreateSkybox(); filament::IndirectLight* CreateIndirectLight(int tex_id, float intensity); From 5a2cc6cee373dc921e9ddc7378fffcb497237d2b Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 8 Apr 2026 06:04:07 -0700 Subject: [PATCH 022/251] Improve torque-speed Figure (3b) in dcmotor.pdf PiperOrigin-RevId: 896453507 Change-Id: I3973b173db9035b71d69bc2c9dea990689d06a1c --- doc/_static/dcmotor.pdf | Bin 599056 -> 599037 bytes doc/dcmotor/dcmotor.tex | 10 ++++------ 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/doc/_static/dcmotor.pdf b/doc/_static/dcmotor.pdf index caf8a36c1ce25a04fb7a8c06d2b45dc5f7cbc829..a811acdcc1b69efa07875518434fa27ef6fbea67 100644 GIT binary patch delta 13821 zcmai)Q*fpYu&ra;wr$(CZB1;yu{E*nWMWO6Of<1GvF3|8vCn@_o!ede;;GeDtE;;D zc6FyrP<@P3HC5w4@baaZyU+uh?v&&0pa1~+-PK|cuO$B|UMQC@(<*>!GdHCIPjn(! zEX@vs2VM<~PXF$qb#-#0^$YZ4p8vJbPq92NZFZDBJc#KWMR0aW&&$ti1zk-JsYi`OMd@_ZB- zxbezO$Vx=g?RWp5^lcwY!3&0UzuXgXa1h&fw(392>o@CB83A+X`F>NbC#gv>Ar#V(?vKGsn=lOnVeBHHnU?O!UlrjP|TlaQVu~83gssJm=3|YTvlZV2ie1IPR{d7*8*^UvU@> zaExrZv$3k~`hkJXE0lOhoq}XH>lx9#<9j8V+t#l%4wE{8YEJ>Wn@*0P^`XdNtasNw z=DQZ0OmsiYS07!G>8vN!I#mCQ(7MNHVqYsSNgkfN&fedspu3qfbH5&a;?*x7KRp-T zNV@#!fwj%(VXWvh(s-LLgOiuRs>JRKUDJV=$HXyNZeXdk8m8bMli7xaYwLM5@9w+) zjPng)H}43M?XKcfbdI`th~xg<$3-2lDNY97bb>@R7j4Nl;}Lnb*4X_DDypuIIThl2 zPvo?8<55w=<66&yNTvoc`NVHD$~@mVX1iTCwM=h@TQ( zZ?5M@*=N~&?5@Q+R~I3U)#n0fEftJNWwpuqIlvsedYVkCbkjIzMSzs~w|oV6bM(4n zDvu+}R2U&z-1brMH@@I3EY`^F3p+ABzPfBJzV^=3OM+dKstxH_6e~;E>TfSu$Ac6L zp9zy9<`7|Gc9>$;2JwQA-H!FnaCbj}w=ci;8+_r^DA1If3QcR`tfl(Vz&f+b>hf)l zH!x>Nd}O-qa<1j$pHN73Ttn5v$?^4TrPS*-pPuq)bt4Bqm8YV!(j#8_Tj#vG-(dAt3EXBk?r{+W5cR{*#AOBM~O7FZmu^?I^dJ z0q?HR%UN;L+i&6HpIA%Ww`a7i1k1%-uc`Yy8)r@vTrBnpJ3Eb&(+&a&Y|W-jq&nK* zv9~5TJSFYXgubmCKL#jE+Wk5=S1afXAw;yEzLV>t*~MQDKh06=3rM}|KqW#O z9l>Gm24KTH=fSY9dDeX|E$>4c3P7N39%iQ@>kta5_Z3l%Uo!;_68Z7s=Cy<1-L&od ziG(ZG)Zh6l_%zh!mcyCXpHz0&u!(Hrj2*!=h>s(%Vb{o`e77MqbIzS%yIfSJeO-By zRbOa=1*dci!eFy@o4?BJh@y6L`Mvi&S*($go( z$BEA!J{LA>o;13b$s`NG6 zmY%Z?cN>6XpfqOdahme07sx~pY1!=93;nzJ>+Ofoy}xm3Ib%d(42h+hmyFxG`9~8& z78kNFd<~lPck%|Ge52sk-J;LhK)vfXjLFppTptW^8BRQ61u%V={xeq{g7lfzG?^g% z*7Ww|&v{Ve$Sus_QBmKUyzo7W0GW3{w#nK40#s35T=t31sR^qADlprwhr=g>tumAq zB#m-4C4g)0##Y$n8mDQd{1m-HX&qf%bX}k|{1ht!P(U88N)el(pJ_ZB3g{R6_09wM!twQ2vOmuCGjBYg?!98P=fx zK&~)*7(wF}{=5?80*t&ijNDH$EsjR8R@TaY1hp@rb>$gn>~8m?bzGy=DPxZzv^zd& z3uJ-jw%v8H#ct83yz&s=V{Ge1yMH#-rhho;_pZ*RRs6aqGRtd>sH>CDbgrxnrx+n% z_THR%^`BdjMl5t^tsmV!W$-mzK%Hr|@j7XO&~+_qf#~PC8+Bjo9wC zz&V3I-&P(nxVgl+Vw+^UyE02Ji2o#1lL(E!^#hfZj?O_Nx^j6qKk}5b_D$7)|G3Vg=Go&S0%#179J0;>Is>D3h(#_aD=;{d z_eP+Yv~ftbv6!rP1+KCQ>s6{81AezF;gZ5c)2l#7beB?grl1wka=0hF2#Rchj`fA5 zU#~mgOq?NV_4Ix8Y4(wMP5Jnk)C+T}qrvPW7~d}mpF`i9Kf!CbO_o>V(~d@IMTjrg zna_Q_0LP?fkSwX?jBXL1xz7mN`tvEK=(@-@WiZs{z{icm=cp}3jVvn?>QZ9(Ek4b0 z^E$2i{kz}&n*fu_c0g;HLMY{{b7#Meq{CWC&vFbd3$s^*Wco~GMd3g8{L*&;Lds?S zhfI|MR)*hf)`#b_5ijItzr!Cp^AwNDT|$Fb0iTuOSnymH^gel&#j5kZa=1nNxIJ9v zz}s*HTlzYXu*Fs$se#;vw$S+*(#K~k!m&qn-X-m39nN$nH9RKK*eP4X8T8N_dKgUK z#s280p12^-0Fzz&OkJ3F@0wzm_v1)Iae>I~m#bIdV-We&&aCJ*Gr17SSn4O`7t2%; z@bRkyDn4OR&iAS9CyO(~06Rpg4L9l9;2FH3Fy>H$khsU|P1y)N%0lEC=(jY%BOv@X ze(~+*gqt#TJRpMLz48c^dZYY~azFv;>XgGpXKEXkdVPGa^vHXrcqJ!)YhqH*hd_ zh{HsBzUOSw%`=PGd;J_B#kp7ijCg@QhDVNKP=Oa^8@IYoF-SdKN;4p-+UNNp^PmA2 zt@wcT?9^&ZCScAFt<`~28e5LpQO!JDUH9_DiGU5DXYiu{Xo(Q1OjD9dE2qqkuk zP9_hW{4^QgNr;$f21)CTRW-_%%b5}W4HL`~pi{}1sswu{W&3S^#$>$-q@)`7xfvVq z0J+FJEY~?o>shAR>(`YGmCbzZBwp(dyNIx9_!EDcoDrtTd8^TUv&hX7hL_RRi0xy; zn{TVeqBN^s{TzL_u@x~dPs2?XDXMT>x^L7@ZD~3>uizL0h2kap{*7ye3?=?sw+c4i zv;DJ(KpouKudhAn-O4};jA`V&L#wD*T~$GS-t9Ie#`dijxFeTERDNW7b|0>k&{1IkH6k%@2t%IBZsr01 zqQQhlzi@r#Th;biN!a3>^FL1;qhZIt`dp8Ct@6g8z8qb>_oiSgCeZ%=wc=t`w-i(3 zvcc(#ZJztkb&*KcpSYsYpJzS1;-m2g=|ib!4`kf&%=u)Q$#1WFPPEmAcond*NGg>t z?RYK`*DNn~q^1+V(nc_?-93dbX#YFSfrT3Vmf6s3GdNKTn##cC6t->ICeU zV;^|fzf1DL38r)D&W-LmS~sw^{IKAkNyS#kKYDTrVA3+8lsI9Y9NLb)(2=ekk7}wK zuh)`6vK|}szx=^=m(Wex@o~{b98VB4-f?WGjs!3{e{BUxZB6r_d1bR z)U4T8)zQ^KqQvI!t)3BC4&5E49ZT3J2*3GNRzD;C_&5he-EdgxcxT>qX{>bX>IKjkSG^oMkX1Zr2S-RaT4F_+-K;r$cwH}SOrV7Nx z=iyRGptIPm`2vkCU9)23xB?@%QTKb=yYwgdvEK?XDlprsavlKRWiZ-zXKsQN#Hd#N zeU#NzmY6`s*^0z(g%2?P{lI#EOVNdth*!n_n;J#6O8oZ#iE-xb#r5iS9Og*&ED}w$ z{k-NB_*P(Nf8Vz&W{Dg@_xFDa8sGg*zHytJ5aF%Od>uzxL~cE-jZX_+1-611f(Gb! z6|UHT9?R=1-D`vK%%^LF%l4i3_rsU#B~{=$UoYm9-)>o+#z}F7Mjrpw$SlN@LQ;6qM~Kiy4tS^0LxP+YYIdtL zJC-M6?Q}&;YN!G1V;tBsJV?7BcX2Nho}Yba^z)QXmw#Zsg9Lv!vlHu zdG%|);}#{q4NPYA>Q?j2fQBJV-|ljZ}hrDVaHWB6&1+1)k-e@N>|lr zE+-G3;?;%G=BmD1Hg{k&ynRv4lYDBHS)4&X88UkTSS z_kA=1e{r~pel?Jb21?dtZiE}+ny0+2ZxIQ_SN!B(M}iJ_$Q_l1q0P^77VJ1 z180;faorCQ7-LM5m(%^MS;=`F%3)4O;$u2z45^fa6!v#H{Q+D9o~=#@sm21FHRH$1 zCjQjwUMMUWz<;oLHN#uy#gUO66q6So<4Zk-hExCxo1^F#(`D(GK$x(~HbO=qw5elJ zdO_3WnaNt0Hr6#kIS95;V~5Co^C8-#SGsqc&ww5@LJ@WnL^qcNS&HdfS8Puhgs&`d z{LF(^%86%bYA?@7NM*6a{g=bfH8fc&JuSx(Y-Vu+5Hy0y)s6YXz-~d!8QZ0pLp~;g z^R8MM&!C88Xa3bXo=YVkI;s|XquPW5kJ9`@ooKg0CtC>vJy-Cw!t2JG&k=9IF29Zo z2_hFSmp@DzVF6P|v7BGi76XL?2Ct$7p`*SYw3)|KTybP5Rw4R*fw9Wy4*8dYQnadyp$jw(sW9Us5L$j=e|NDekc2+wK*{(0|h<#$}RsK z?v-1SoD_$haV)&0GS#rl5{DgbcF2LY~i88K*M0T@3(G}vVR++@DE1Q9A5esom6c}ZRr>=hKTWHi?1 z`+7na390K3dr`%Fpe;!%+}Y?S2=R!t!40oj&63fWLF0J{6ACFjt(Yj^3Pd2DJC5!!2Tyb(mia{U~_(yD(jo4Svh`(uka4qkz+t z3UT^2C1Vu~E`4#@TQSHU`2%0(N@^NLWNxLXni0po?<}=MaXR6nQK{@9l@KHi=RI#-{+AABpl!z#Mr|ScBxk@w@!zw zLqqonGDFki#=~g4pj8|48?e8HXoSu+J=Pv9=d;hnR*9BsIBLZJR)1G%*zg6$qwFQM zEf%ZGgLL}UJr43Ui)0HJGbSLjWVu8gB};})%tU68qQX(WJ8Ai%Mk4&kKaa3p*yqF} zDN-t6RUXKWL$hI&#+7x5!j3B+7H7)T_|`p^a+*VnK@$IOC3mNp9Xf3+|3yYQr%~ai z??r$+_lnpn|M)Kkz=6h~o14crWEF5iw^>bZm0(;ixT}Q4ApBRYS}(tJV^BcJMZ9j% zvPmX#z8b!}@XbUl@?0xGkrltp(=+V%cL^-+L@zMxLLG!e1W-&5zm8Uo+UBBq<+=PW z+_`$xl(7!d=%9Lp*_!#fnkBO=K?pD>_&IDkgmO|yXm9*%48hb1`^tmRp0>#UNH zeRp?1if)IHxKU_GA%3QoPBa{@k)UdA*qeUMeI7lURG*ijyyQ4O(s)~Pq^NF>*e-rP zZ@B7pIK6)kpf$p$2NwZ_wl45*x#WWP9ApH}8B|CUYC5YKgpqeqGf$Il2AB5x48PDB+m`R{^0s#v5Drs+Fd?W=uz292M@O(GuMD?5R za3D6OL9tS3ndA)Xmh6o7K*4g%^HTFL^0eXl&O%jGr5SZuxKviG+kGuNaXo=@Z3B4Z zgj+#7fF5q@qBx#A3?>Y9X!UJ;Fch4?2c`hq>N^9s!|H*X>W@k=;(~9m1tY{B1k7on zw%>0(tu1At7z1|N(!lhZ17URpO5Tx>`0i+83%#0`{_zffIY$|--FbstB&3A33prA zQgK^Cfk4+VP%Gl-8TFFd+2}j8;8vLvrLdw&o7a=rW#H}ZIT4$Zn?INDBdImxC+L;- zhf5eD$0`k%TNFa(dzv1%2Ri2ff*@Btv=pSGIZbr}D9Su-i(3|A6S|d}M+O`*24u9P z$$b6yC%IahH*Yb7O;;KoA2)eG+)HVB@^8;flc>_{vHG4vnA-)Vr8^>PH@YOhz^g^W zG-p11K>Bsu0g|brX7)>7AHu47%pvVHa^uc+bi-5y(e`s;jXsGRw=w9oD#Nc=$N95A z=`e_R{7oR~$&2XvoNs5%t+(;e*L3{$e>7C<4xi6x`=Gyw3jIzF`GN+k5Rtva%UQOT zp)392bO6-TGYJdhn|j1nFYuo%o@>xA{;p=Yd>4`l#%wK!hTh<_ELa=AlzR3YP$Msg z+l|dS5y79D4oE7upo;|Qyl`i)4t9(~dZWvI$e&pFcEN9Z%m7~Id1W$BP2|3ODPF$? zWsiFdioR)i-d~H#qn>Fz+xIHG=*!CZ5AKS6M1U5p_R5r5(Df+~Z_Lu|WqaP=8_MI}n4NuCF08m)ssv9hQ`=mgyyn}= zPCjXSeU5&-KX#M{eKa4o%Y%78cIz-tu(3Q9QD3DIw%wzg_a!@}N)443>bg%dZ9NhB z{Q+#!?Zp%mWckiN<%XGyb*87--S-R@uN9oz6nW14DxI!Xh6hV^&}TTvUQ_Y@UUCKQ z6(-u6o$ocev@49&XKN-8b#2`sqmRkP;0(Qi(t6XN)kdvz-S!8@z1J1@kIC*}Y?A?t zM%}HoM#x?@&OeQ;7i$!c=QwZNNgw_b!N8@}z-)_Y+Ir*eMW+8_ox~e<`L@GSu(iWL zOuOOLMq|RI?!qIZbI<6{4}l%$-!20JU8eF|%mP=Lv`>sVZ`yO)2&ci^50R7S!5QA2 z#XY8P+sv5Py6I1Kb8i`EAGMDr9|4_e{id3`jRV)KXHVy^Z;fD{rZ^`9y7K^R3Gme~ z@w5`PBZ#)0Mcv(~?P)rA(qFp}R8w~MPxhGj`_2@VrjzL}UdQvyI36-Q5V-jUN zpVf8Yh@3Ew9y$S|$Hlarr2evcldh5$hUz!8_$o#$s`4rq9h$t&L^&;G){V?9-UXUb zW7*OR^m0bz>7?DC+6lGj6cKYC%D{f?5&D*mjqx5-)F5|-vTz9;3H!az3UV1qTWU2G zZp0rl+$M^Bs8xpLHl!6(3LUxV^|N8SsC~3h$oKXV5m}zMa8hK7ces^#?9W-_WmucH zz)0P_s1W2>A}VbSk2b|@eEFN3{k|n2ui9#pgm@%DA2~TRa%b7pa%f%TgIgV$;TGU**bteckICryc6jv*A7Hgq{=OWn^qTDE%b^6uo$3}*=&)0{`vXAIiY zo0B5J1}dS7HCH8Y#!(Z-@&E!nxw5kt35tyDqLu|Df&)aEfxB?%SQQ2)vG@EQNrrao zR&=3G-fiU%@ZvH_dxgSr#c4e>@&SOKwwWz0~uP}Zq)ybQJqu+q9cBy&u5 zs$z4FGh(Ohc}5_TwF4E^ z6-TO%h(QTmjNvIJe)^>llR3623ao!-MQUKK2kbfds0P*c;t;$rB*gY92rDta&*_l& zV-mawM25RCvBGSZ6#(xa&${o;Q5{ecNH06BNYcq-aFo&qjO?p#=&@8CV^ulnhSE_b z*s#c@6$CtCCot0p6kG<(5^AOJtz@bvWi`dHKH^?dg@Z-dR%CGsVriRIdRb^QieL@0)8Rs>1n+ScgVZJG3LS}(HyL=)vPa9`iU7h3@Y!->^Clxoudt*X zqwAmJIm`))z8Y!g!k29f&z8m1P-~cA@m{=4tB!IS*iv%d>S9`kIehs?>{uQ9gi%V@ z)I6;INzYM~YiY@Q41eRS(^CZ`ocj$hYR1%|J*LWaEV#g5$2C(NbTZC%5O*z=6PQJ) z>mw_}nS|;QZ~>{BILh6nm-@IkwpQ(Fx0a&{9P*~hNzLs5|E*da7qqDq!QschkKT~2 zT`j*k_^KfU^)0LyEgBLOLbi^nklF1k$?@4vkgR8NYM6o z%VAXQuA~$xEIL|1?i7dQER^VsL>oIjW^k0X( z3wdPqZMAQtoPFcZd%$&KIjl~bZ-zpXa!qofcWW$F_- zd`mKI&Q5fciMKe5i4jy2V;cH8W!mZ>G+Q%krH-30A3_}AW*zZ-@XPXIC1`>DM; zO)iWBX-8a}SzDaJcSMncq|WdV<_EMQh8z)k6n~~DolUKMKeR=|veKwjt%GwZ%l>y} z+A;tyk02$)s|BLA%#?f9!gow;L4D3fFjJI@NuHvi8<5>@gK)213OJ{CJT(59kkxYiwyt^?R>sOtxi+9cTn&u!BKzxz@gp|Zd&t; z19iURj)vP7mfC^dt+s>^DmDFrOFp?J3V&L?Tg<^Mb_kuFEaF(sTJM~Yn|+K7`>bQd zUyK#q&O!oATsBb*PH=JDLS(4?&~P1rMot5k#c(sm)m$MLEWAM-dEpx-;)+zGEEWJp zK!Lbbh=-K?N31;AiaM)M#uY3$sAN);5n(U8p7NhspQ;5|>LU5UW!>vqizU}x>P6v- zeV}3CP<+UM%1W4GDVVF5oVMpwqW!H1g$r#H(^LQfI*S82N52O$)XlOTMEPqRG&`pK zDKq|UqXB;yT!>lTLLOfj7`dj9m^7eQWYBial|~e0uIT7veK3N!i`D&NJ(MLl$5l#{w z%w~k8#QN(q!A;cTZ`kP&VojP@6*b4l^t0w^9LU!=Kwe+58z;rrhW11ly#~1Bo~6Y8 zWRrqg#@Il_eq~md!o#uEepAH732jZ!z{_G}oM-N65pA?;HKrUkkra1;$!&1Zwjsf! zj%XHWm~)y6{`Ds(x8~J_)k!J2hAD&#GGf5P)?bd>^BUqidg5Y5!Kq^1>ItaeboW#7 zzg-^xI=1d}kjEb;^mgD2?E|{Bza=^NFVDk^!FFCWZs27Y6Xtnhs6|-vc40Y2OM~o0 zm%nY_ZUA3SBdiy>ms1Vv`=smKn;x2O3TpoLyN;Dpie*{evmza5R|S{nJL~(SeXW+; zf^Vc-0-MdBW4T$Kv(AYo+X8QYo##4clbp^k?u=)@Zd(I_-%&QJOaap^r+^N=_15mz zb)FAF8*2Tdd{v&!;I{*tO;bnSC)TUZ?ezc?gZ|5WRz7|M$8O`G>*?(>27^Jm1X!pd z;i-t*dl#>k1!3cwKPt;HOOnf~*J5+jGb#iR0T%9fs3?WrgHS7%A?CU3DupgALZVNF zq;s|htgI?q6-P_l1puJ7>ekAv_-7ZT??c$KBr-Egk?t`eh7on8wd-#+@04U;SrHJF z@y`f_%;;vzpzd$6lYPsE(f+xQE=_0azZ-MY*#$<~sut{IG^y5GIPp(|B(%Ts8-Wu1 zoB7A(>_T=`;df&9dF~fam(ITpmn8>(kARjwou{IK0anu~av=Cw%{w{k*RLWT4*G#T ze%9MR7C>jOz0d9s=PADdyMdh|@6iGhhpNLseVG=MARo|O;LBtG;`ox0SBfPdZ@4lB z+U%d5J&QUf3Y+QwL$WdI14<=3CY_?csq!)QJUa3uulEs}coY;-Qpp{Ag0uxi%g*wu zs?eU^1_xA&0+V*Vn8w|kx@VoLiqjiWxH<@Jqo_u2BBI1E3_nld$Bc5rYb+nKVi<~PynHzvDVfDuKetWdK_of`&aeBle^25h3;ed% zRy>Gn7pTX9EsWcT<6f60Ur;YxU%fsCl>0)KG|`urfI#%Ny!q;j_yKTP=qL{~1-u`; zmjJ0hZTOVLoJuoiQJZ8v`}pj&QU50C&wcrJZlvSubyyOo<)d=tOc^#4&}{ngcM_2* zW4mF1rvHBGH#|SYf4ZeAXl|5`{&UsK^pMfzU3Pe`$tQ^0)iKlT2}8>Qv5usz2$((+ zv)*(8Vk3s@?!C3O(QJ2;g3;+kPeMumoL{cuXdA;1YGHh5A?@D%WRW@T7->C-P!H2t zo>0te*!=t*j%7b2v0Kku35Rokdw3U2HUHVj7deba*=3_FGqoh7p?Z&S>i_AhaJW@K zI{$2YqZ@*QF06~ksA%)}-mAtSszkP$I2Kw2IO|?E%^M3hW-@;9GVm65xA*S4DE{)| zZ*-@HotbqhY|xFIeL-FO#?et|`lS~0ux#+6^ukzWDALd)JI*^=b176SN`Kh;n(SZ7 z!hQGTbN;RHyPt0Eg$`)_-N-4jtl04p^*sEh%b9$< zc_V#Ijkd2o&MD0IGr{uYzxC`{C(X28VJW12-7HMM&F^Ouo_#G%x2Fjg9M3PRtIVE+ za^nf!EU2tLKF+kaxWoyYy+Jb4$&OqGRB{LUn3qzpwrG9xTK4P(FWNy8B+1;PQsgOyq6z-`t_vlle;g zs7Dpf;oH4h!#vtL2%p%5Aa#x~R`|7YR9T>%I5ne=zm<;KjtiQS)K;qtSb!E%@!3WBAg)SpnCa$;@TT|CXMG~| z&epA+u94TbV@FPygNlaE(`*KQ?(959(o}iif`)(o8uL_r=dOYA#j01Nz&#$gFsX+O z{QGcppgC~zN782e(#kQp@23U*l&4h|%-#wfuD5DAYF5&pD@J{n{7gTjL-l#YZbbIg zidOE6*H7nm!-ovt8>ivf?JBZ{xc48h^o~2+XS&B`hNdw1XZrJzX@Mkn?4w-yEtt$ z*7Y0j=p;^1V?e4Iq1@ui^z6}m>U)gNOD$pEeciraxJazBoM8kY^Sm$RlOjHSf$K>i2Re?b2S%zwakKFGtt&r|WSvvYH?aU0MhuqwM)Ntt_ElhH{D zaIkUkuyL{df3uT<0A2-3L!6gSLYiMvhL@j5nvQ{sA4N+TL6=zk5Y zzV5hOqYc;WOEh(1F+oltlBb79+VVgxkNvfdR);|_!`va)LQfwA?+vf!s0NR|K`90p zigWk4ec$N64Z-y??1)SXs3r44G$yxza~-T4URF3lP~H;UKC1DAtNSYZ_NA8%;q}7% z_Rt%OG44q98mC)|6Yf~`l1#c46T8sts7&6PLF`EM5~pJhQT2}cZl|XWVf6<2ZqiqU z+55uYcGFdbfh4* zO%>`cI?!^L&Cm67R;SudV3sk8bq}dI`>t%#lYAMn!#knd;grDBLT1ev6@a&F7*A3G zW8yb&p(IX2x%I@{=#9`t=#%aMI)2^6I|MS6@r3ie!n{yL_id1KxGuR8q2^>Jy1Y+x#?*8k@pKew2%MA6%>ATZ-Ypi zWsi+AjSWW~((m&iynS`%ed&YqriRG^<{(Tk`%qaB(&Op8acXg#Ur_KCC`S3>h zIS?uXd>+5>>1NAs!JBr5SB@Wom8hx|^I6=Y@;KoO&`$|6{kffY-@tvY#(lYoN6?BZ z-l0suU=WJox=yPJiP~;1se{Uq7{tPLh^KrLy+TR|2*E5llb~kIut-guB4LdUB11;m zaY%MF)*;5ic-R0EyFwW(@r3{WE8i)GV!8a6jh4tL58P_?t@~Hc_kgUj?2hj{6K(GZ z>4ha3x)7r-+9W%XYKTS?~;-(Mv!PyWaO-lGZ|;%gMg} z?>Jo*`HUaFcd|7EjGvEx>hx-EziX{1c;Lvb7}UJQEyFPyvna0U|J-M+@n^iwqhB!t zU69tuFzy1uWGhzO#J1%!1~cOy8Z$cjo~91z+A2@-i5-))$-JCn45Gs64$Th9itPpu zoBWT9Q(71@IJUSTRlM5%~VbV>^A(sXV%RdC}N;c#dkTA zEs)Y!>=qP}+xgJVWiEqh^NO=JErJl^Jk~V}Gla&6GT~B)V<$Qe!B0L5@1_#tA(5Z9 zk#7pdzc3T)JevimczTrPVQ%N29KAzK{B}_ZzO$(#elju$PL2Em&!V^7N3eFb^6+%G VHqW}DgvW(v<3^ySmR6NP_&?iV>4*RT delta 13857 zcmai)Q*59Ou&}q?+S=N-ZFg(iw#~Q3)?3@QZEbDawtc=o`EO6o#gj}jb2G`z&CK&T zLK^pvw6+u-l#4Y%(Vhy>PM{fTI^yXO_Tywa<1Ed5hQ%k30a^dqoYsr94|xH3X?JP2 zx~W7`);Cd;bFH1GsZ^%C=|34vVI(?3&kz=aZN7UV!`DQ3nm`@w#xU-`b@bjuN6e@Oj4KVxIRH*Tpd1r7uv4 z_`YkkE0YSiwispTf3d$NRQq)?GELXibY|Ogc&yDoS?Z_Dia$K|L$G~E-dS~Yc%)1| z8Fp!Ixic(}-2qO%s@WWu&kTTd&bF;CFx$X*V1WwEav`vbM>#pHCJFJ=e$RU54?jo< z@L0BNrI-_p@lK$NzxR2uWHEoVd-r0Z55F@51O{)3DdWN6$5jL?k0oKkoOn@4F|sX? zG8p{rua8d37VjmW`7LT;huCzq=vG~AnxA5tEt&*sKtD{bZwvp1Z4ScW_!+@+#Ueas zK{Ez=f0*R-m}*AG>40z+tll{%Rz;Y^1I?s*$KkGJ3uq0g!xovIqvRw(O=nuoKgRJM z_ms5~&MC>x+TN?y-qB2YhuY}h?Z~9gr1RFy2vg@UNWJ*U% za!3APU*nU?aGj%e{k-y1{SuMq=HFSUjm?q0N5b{^amM1nom&HU<@fYm?Uo&kA}1nH z(}Ea6g+L*IKIzaja~~j&K9W;C+Gj|`Sey`*3uC!ruc)~70x3@k3 z6n#lb*p8ApR3CzDowCgNpRhEEaY`DyuoF~Ijf%Be<_Qxi8T@>*>zJx;UjJD}oEZ## z-J)Q!kE1e}y+Vqe($a+wj7JKAPw4(TA~mk}r-cSSm+@2glb>~ zK;ypkrp>WW{wDtwMh8Va%Z-bz}9@Auk`u=7%zWH>#47hnJAKi^h z>@uEA_)%~fkG%Q^a`Fol6Ooae{bnl|d*{zVhJPBp7+AR(5}V;}y9$@ZRk}b}J#PT7 zEg>2H@i4?z+VK6({_dtgFK_>*cUr8210$^6eqw{U~_|4w;*G=A_n!RJD4)!cgM!z{N&@Ova68tIWzx^V^hUB%0lbwlkKat zgo{n5-Slo_Rr?iD#%JEO)GkIS(YJHcsxJeG*odZ=1|7#mK-2wzT@!Dj#QKtaoIb4J zirMD%54(m+4*g^S4Q(Le+Qxa$?VN07H)tx0gpEOMFqr5-$LrnJ);#+)ia-x4#wZPU zjnDAE;d1NL6i5NP{LDo@@)%m0HtEZL5xnWj!=%8UIas|^yU7SWsK9V{K6xN8Dq+EU zUm!gccsFK(xQB~GI|k0=f&#*Ix)cBO7thAXsl&8u!`8IM%LAon5a+n)z3@%nQLcow zAVQ2QyLN4DRohsxZ64^`qloW!&yEt#BNjdqtA)iBueEf`cx%x;hfVA6|LytW{?cJ6 zHDURynJcVb($d11a|D-Q&2%5%&P_~g{Z07`WwVE}AOA*DO~~Y}q3#31K=h%Rs>r)} z-jIn8dkO;A1YnH?LfK&pi~pORuXB4$q-L2+s$F;`{DcWY)f_DaZwax&NpjvlB@RXr z4QtP9Wc0A)`*GBMEbOtHA*gh_;rmnVnu=ki!nm7$k+bI42A#Vq z2pU11PD@*FtjF-%Kn84u)arKfko708Cb!HqNb{>yYkRuMshJfwrEuk}QOjm!&9ja! zSe|MNFB@!(xN46<+6dI6PoJA~O!9n7#qjJBQ0;0{mGRvTS=x)ySQ6!HhBe`6o8$O7 ziYv>I!J+LCLrWc;{ucHl(!%`@hb$g*Ym}kd(MDn_m>#NFxS#dSBsj}79^?Koprfm= zDS|4R0Oeg6X-ze@6)+io*5}-L{cg_Pf3zgw(^hUnDGpun^3sUP7@tF+Ho1VjTHdn_ zRI#szEt+X|(^n9vcmLw)rD^fKaO!xsSJi-Nsk}<|>2I-)haFRh-$q5O^!!uMwO@m& zosgr7>S0)SubJK2_8_}k{Em&2EmHu;(6889a=LM?{(g_c7gUzC!tlpJP+%sy#QIgI zEqieCOo3{8wP)1@>+4t2#+#p}(NDPnz%YL(*R8GFeZ1vlb2o@ziJIviN=~L%$#i#C zwomD)YCoo}@62VaIYnpO<<`(4bx%dGl!n_07W&qcm}DfiXMaG~FMCnB@Y}EB`j9;L zPm*hjDaw|^eMry%%3)fB7gU^hYyWm9q(Gd&k^o`TaO9eo<6P&QTm;IgePqsj; zpMKa-nYX>03YCNC_SMp|&F@QYmlW*V{-UB7hw%Eg+686)P~}8@bK_v{lC8P2doePs zVjG{$G)4hYKF^siH|NpJ2f7&s;H_GVlSy!%ET3ak^39|x1x<)s7Wtz*guzc?m)5kY zQzFXy-WOJ~f04LtCY8Ro0fZ$AS3SZ|60ky@* zQ3`lfv#07z%(Iu|%vAQuuf}+4T2jO&eYMe|_QTt?ZWJw6aD=qbs9aOF$l((#=}i=>&RO z!StmQq{YWu`$#{7je<+3yFQJ;4;1$mpL(qlFh$L4hxCcquM?d#;G~U4 zgKq96sHMEW3keY%%BSi(WX~9s?j_#*be0iEEF(+f=PEmN#I~EaQeg;jXjPF9*k)e` z&qqyGMsj5-;tbse)H4RaugM=Gjfu&T#tvN4Rn2+r91KBe4KIXLF~j({X|{H9f9c4+ z%VbG$p$G}39B*${9iY;!cr{S{B7b>uYoQLHSY!Mkk`1LDa6M(S9sjR=gGMZ*zh7~) z!}EP}6M{uwZzdk%`^|rA1wa^|!uC|!Y+!o7`4cjWr<9rjc&f4_jO8NJ`;yh2$-C^) zMBmqtv`@QoBkV}!bH$XZr!EJ^w16o1^Ws`ury!UBcP!9hN*O$m#rR}SOdP<*hSRk? zXC!o4w~lo-oBxE2&dKVw7#1A+@Q>nJyHkv3#F@4*lRh@ zP%X0IsJH_RX@@KYOFlHdnqj&~e|Zt|+J1Gfq7%cFniOTWU$E<@8YGunUhBha@9lgP zo4Q9tAOv$X#8NtP;`q$-5=aDBk4 z4Opwfr>5+>CYLuKCYun?7Bh}z%UDp|%J2*sLOhokR>H^s>L&Fqmma*6s!GZR+Nm%; zZckYvs$MiRE4sZ=RXQoXuG znhgPXt1jIG#Tf$C@$oH*=^>K(;n6Q}ae9ehZ_RXRBN6AZMTjmm`!(VLzZ3^S{;Y$M zmq(e?Cu(ys@E#!98ENMlN@{7Qy`s#JQPc7ttFc3kxb;^~-_4X+w%6O6-=~HjYe^P! zCFD+YqI8vf@v%a2ACBmVV=Xls%ROyf6d~YzbE3E)Lpzd!GVVDu+ZCtLj4uvV=dJ>M zubSk@K$IDln^~B2saWRDcG~h86_~5QkQ+ZAbtk=hzIyMs=&L7WuX>_)7kC=u6~6X7 zk31|OE#XmCY)xauqZ?2im$`3psh=9g^7Dhl{5H_Y*^HBB>+899D;M+L0Li?M%mJV? zp%fIQ?0?Ys(A26J8S$q6YSPGylWG@eB$fQV$W&RD6c@v9_V$Ae``{g8^Z(#z4!10-IN9+Why&D(o#E*}Y`Y3at#ZNnvaM;C+kxM2hTMJ?5sN?Zc z*V!IK=ZxEyv>wI`z{I*;h5EFkltW+r7tY zox8ac(6*>~JnGS>&GN|Oz$5bXs?_UyEUdR#?crNXBNz~xh2uIV*f}=FmRCKPWVQSu z-;s3K*tuUs-J;*A;7i^e+lR*HHLdx#XLQx8#fcaXZXH)l2yVSS zF>~Y2V)>cfijLn6KKgPU2_yiI{@x4U@&ft-jpF*e9cE6x(nv_GlOH70z+v-cxszho z(GY*icVQQDJvOg7=r=K=VE{EQTU<|OGz;s~XNm5=tM&;WrevheB_b!TE*_=p!}=*& zx3%PW5&9W;5_p0_Gdw5Z)_Z)IhKw19uEBFwSI`0eQ`TcUy~~>w6hx&duu)zuTH#8 zV}>Rss&U{u24A$j0a!6Ts7*!#)neN17|%G&KZ~TZ`~C=i;8ElcZ5pOj=+iGcpM~ZE z^cHrw(E7`&B+$I%cefZPt#7TS$dr7&eE3d zY+o+y_(+&TlWgYp%U;kK=q+UgreyJb>7PXC-cGDRfDvd0CU3{bwhO%%`Nt~bcaO#h zDjWDRiMh1u@qqlia*XgG1|A=|P)0#^y1#Ihsx|8ZXD95zM)smnCR-ow zLd%Q>0Hi5~K8EiyI&791zcMNG&Pl*A$#6X=FddP)4^tmg(RvYEwswD1DvOO5N8|E# zuW)={%hoKRc;4VR5*B1qvypY{NYhajb_bmTdE=QbBuBpHm~R2j;w1TAs+R@mJICn0x03ru?RY|1RD zON>1s5qcogQ&Mtcz0e+s?oX)%NQ{@?fGAfQG{OvRbmxFru4hPU1qz88b3%AWB9_sG zao@K|duhF>4*HXTcctXtMs2txsBf_IxE-W*?1#Hg+mKPxS?|Nbl%>9u-C9G@0>|n( zMkos-iz6s>Dp7e#hWI0yEJ@CPmka~_(@-)!+$cJCR?y+048`T zvDmMfi3$`T5YysK^WT2s1ZR+(s9ykUf(T<0RI@_^<%+l_wuC$!(~k(DWqxii8F35p zS(sm1Zo)FH-01ACtV(}%B`|2a=us{W7#T%~zYfep{5-ZN+x{{GcvBg96$d2dg0tR!D1t9OD=xuSBkOjEcSO(7`7K3QD3Z!QT<8-JE z86caxUB;J)ENT1}C_!%I>cqmB*OHCNIL7`bi~cM67D(Kd$9Zu(w%kj_gNc7g15*=2 zLX7+VFiDv$2jK+nB|2~a)!`)Af(1jC13f?Fk`F;A0d)t#@rXb&R3O6M0~C3T-V5_^ zXfr@YTi;9uzhYi~;o1L6q759W%tb_|#jY=%c=joz?qehLIzMM2Mom-zA=Fwqzl#&? z$T@KO)j(rB8gAQ$p*;9I4UeiZj`7ha1SB?L6 zJC{n=6qbqbAh@qO&})WW9WV;TXCXDJ)A^~JTc0N&=pv5Mj7YowsMZY6XrQDWfeCtq zFM5)gKLe_7M^qiahIl|Xhe_VBfHWBAswe=)->AC559uRXMu-f(2Zac)Q}AJ`Lz}Palr^nsC6yLb3)c~O?Yo#njDt2m7W+$Fssc=~| zNj&W)GT2Ft(_ktA#bFk(u(gy_;`Y`Vt(9wtzsW5r&rWUp6J10{{8dD_T`bZBHrZ z9}De+A1Mb8r|3hCb}0MvUI+`tCmOT+pp9o#If3aJ&EW(*$g8t|8X~Z*U$2y1M#dH0zaw)q;Z48>-Y5BEhiLSrn>$pOq?alW0)+TkTbadHZmo#(Ts4ITgCO z*8tp9MqkNuA6l0TwGl2Ym@sI#O_t6TG58{$d9uu}vvTdXY1$9t++7jJa>cBVVq0be{Tnbr|L6mD6s(j2(B_YY5XaScj%f?_La*$)&g(NBU^rauTAneYrFelPM zSZxs`_^KFT*H0SVbiz+T94czdjt%@nURN?g4Bgb?@L|~JhaxG^LF?37cwbFRk+H0yKYpNNeUtG{?~iDnpedO8-$~eP=ijZd>dO8xQ%yl~d-QzK z$)mju|2*33>k0zJ3MYw24}>Sj=UUt(+2@$qq*76`yxiJklLF3^OXHOm=BE_eT9>L_ zaR9^ZOPAmXM|G0s(H|Lm+2;4jSU|Z7{I(gUjNgT^HY~JK7ne5s-Y0oR16inaEbj3}Agm=T(??Nl2w$6XocfW%bMBNX)-RZ5UVE@iqXTf`4G51daGuR@vm zQ)MQSZOuJ=)PTag+2Jf`hHdnJV{K)*^B=$SP4m%S@bhg!7Z<{8%vMCL768`V(A-HC zDxrKd8@co|2E3t{u(Im?_hRvLmM+S_Yn@q&VOB!%6KUPxY~1Q}Z>1S|XlQ|#;g^4y1UcjL+sJUZ>_eqPQ-=e$1KO2NUF|F z?S%&j+8N; zK38x$OkYgv0fq@EFJ@D*0TM!5Dh&`&IZAGFWtVjP_J z1?cY7A~L!4{iC;81?TKgus~$F46NWoYldBR3)rLw+Zg!co$7?)bCtWG%h?g>?GAw4 z;s4zV+348ufgHRJ{M>95!be%22Q<>+70evgxvR->A*Ow>-J;it4wyV89WvBFo{1nvex%#xT7q8}6oD zvPo*qfwwNZ@ubw*rQ;xAxiL=fJ`c=n2D@@JZOR5d*B@>+dU1ek%PzVnylp-JHFCM? z3Nv(kCm2*N<5=w>h?^)8c1&V5XZhOMvvdb1sy;5#B3^8yn??zCno>1>Md~MW72&5T zQ!bG+x&QK3lDUxm1sHtB`$9D*Gf}vTL*MVQ$tJTd!%oL_dBy_vj$&`8?b4 zBXevyY1Pi&s4=-x=X0kiddq8J=HFLl9gud~DzusXs)p5M81zq%$PrB7+eL)J|%f+9IVy4Ck zvfM?z7!qXSBTl<91^gtq&LBME;TEenV`%r{y1Y?H9#;&+EGj4JSbzd}Q$>kBJtjp- z-YOy5>}rL$xa@AtG#fh(Vnwzt3vH)u(F`ri(5Q=mO zhm17W08~uSsH##lT zxH-6d?V_rGHA_HFnn^SplVD_4pV?o9#d5tVFmt%SreYR4l~9|g=42$;+P1K>Nxh96 z#wiLxYTTyvWw6RgGZxkPG^rXBJoA4P;JQJnmK@IsAtY7!egVG&YS-&EQx=EJjluq= z<(UM^p6_`pXBm!?jfS+Am9ki|OsA%lRj4r8)4ab}a(U7hwTBVAU=h2MQIQ6-wk>*c zV&X|EN5E5-LPuGRK7wKANXfXY2nBhos7*Rudu!PaXEa0j#~>6@dEzrL83+8XRg=MC z5mq2O`@y6n4+aQ>chlAwtMn&=hF1r&1j`P_`G<~{x;MIhs|QclO);pb`ae+2A+eP~ z#P>>wso?#C7%~&4F@%G{4sYAxP>+cCL1=+6R***?g;eibIg?Neqn{G;j)G&TdhFl<1o^M&sa1;2}O`k1oGy^ z76;;QI4=Omo}hqQTmU6Ch^$>sXeqCnQQ)#`*G{*nngt9r+)$ zOPjSC4MdWvNJOh*Ad0r8dk*Om`YWOKn9N-%E1kdz*B(xcjy!*0MI24CqSP5w+pCpi zA-2S?3AzNWh*_``0(qfwP1SO52A^0qrskK#TJ&^;`kPv69i$@a7$5oL25Rd52{opX z(exCp;NEm4mKt6wTop7Wt{zjRmQlN|3_GejbK$hwg7U=gd;3yp#+f z^c1vi8MakC$!bnGC7QViB_nJI)O42`R4srKLB=NPDpKV;@qpieBDfd=6AL9Ov=Fp5 z3Awn12_yB3Sf>|4Tmuyb9^a8BNns;2&A_G#0t6cWH!)g9Eqfj@axg7#gyI|#c!VG# zEK_N{k>-VQUK>n_nVBurpyX0pk_Y3?j$C@oY6~^eATg8%@|`K|j1?IQqLrhQk~C0( zLMVeGC2beW*DY?wCWOshqd5}FBj}W>ZRS{7#pO3-#9og@_os}6_(~9@4lJPoM;Z^4 zzM)hB_b{Db#ihBmAa6f_s%%qGN;{W7iy&S1*Mg9d{fJj$`+pa-Ug%W);yKwMI4L%ikZU`>`sFd+TXz1=5oFqG4vRt2c&9~3V7KLGQ+4xrCl^mBRZ`l zp`$3~k;YeUYFY6sdU%@>%P-Wj^0tWfV2S3;VIvzWLC~d9n9>-(Mb5}wte;{Md>NM- zUvagh5w%|YbifVnjZCvEoYx?-3~{J96u#AOj(I*fB;O^^VB}nPg9bY5JX!GCa7hy{k(=&% zYh}Sokuc#ZIksES>sFR}Q`5+bx_Tl;`Ok4)f15;P$wWRQ zd-4RBYHrPkJ1D}&-)318`j?2K5-zIKhNu0OwjT`#7RQi9e}Jt=Z4DxWL8a}1Q1`zr z_B)8!a|A_JQ zz&AHB3X!mYFu-AE>aqcFW+W;SPuZ}9@Eajt4Bh$(O(f0k&NFv8G1ir{|qoG@NQSC%PK`Kfo#dy3dojD z&tg4$9z0;gSL40(hb~_S8DMS5;@#Hq@Vnv@$eteD*2`>_o-DEE26`&@&9swid+@y8 zy@!Qi=@?QJF0hc*@ym~a(Oaiv$Os96?=$MdHe9dP9nChi(cP^%lcu)*W&0W#yW{)a zf$q#rbs)ytlm=fr+-$!t)WYp_!`_q3U@$BZ25G0_FbVd?Gq=HHf5$k>F4 z@siKYWW;Yf{qE$TpSO7z_zaNA0=6bfenZZBdv!x!cAoQB-P)VUETOZ0T>_VXc=&$H zkG$4LnRLB9jUB(lYE{`5>@ddK77U8GWz%qQh`5Ggy$H;`HbdnQ} z<*(aMCMHjXHLg88u3CIjGwq_UA*aTkV>9ium*nR5Kc5tU#90rDc9)i#1^eve)Fya_ zN;Hee>$WFfKkhqKr+H;O!;Ul?nv!liMBZhr_<@i4Nh@~^hF7v=*-yz&yoGpNPS3ah zUU|p!mYkCJ+lfJh?wATHW?!Qthu1;8`OeMTqc-T(s{~W8ChF}YvFB@DUuBP$Uz*p! zzxzFhWe{H0T@h9i`YZ1T#=p1tWD~v&p8t)!ihMt~Ogg0cuZhyhR=d(R21;@@mxuX} zR2~D}+O}hGwCjq0C8r?MCl0GL;1e!8Gl8U-*y2B&5l(W>-ya$uZ=T;!(nVCZb4H$= zwBxJA6*yLGA1ZRxL-sVaWx7J)<98T)qgGiA?=cu2I)r;wHe7S6i$OWBd0D<=)86<2 zo-ID!L-Sigj;cR-k3m?!Dhoof8j}*ZVj%scQoO_XSyFlVs2&Esgkn}7SK$pO4*;%? zsY4B$qm9^T{_7if7Unm$W<%kA`IV#dND7gu&DXp*02~0fK%R9DsjD|Jo1UgDx*Gs9 z+O)uZIdxk^pxj(!acaB!&UM-^(;k2SY-ZI-VPST6YQ6k8I`vXL$^3TcA~!yNeS8kl zkRMB$PV8~YKiY5i*6~%&T$?I*10tFEE51uhUX)k>_m=LXwoLb;Q9IS_$>~KkPM9&X z?^=E+f-#1I_}ZJ&1J3EKx-QnZYCM%`5L`xNYd??9Fkyi)kc=)}J@w1yOhd7OjIY6 z|MR0mcXo2t>BE-sy&PFqfT+&Zte*nk{t$sBdt+}i2@&^~etd>gbo}9+bT_-=D@Ck2 zwsvc;E3F&2LqA&9st5>%!azT@g1;;g(Jx$Dm$w@{d8(O71BVhH+t?~9CsMlIK`jPpxd zeG4aUcSZV4R~wn>*dI8qDDXdj1ztJTVQMq z3RqWTrdxO!eFcd{xDcC| z5W5gF8?z{j7?&`k7%P(~D=U|{7>6hm6Q>XtKOyh`S)vQWAY*E0?qWg6#LU5!;z$cd znX-lt4G*}<#Y{zUXHUnJdGgc7vSi~S?i=z($FN}Qf)j%UxFBzWef`oU6%|#0mPEl4 zBO=3s(gV{M%PS;8ew+shs?3Eo)|x(_9C+_)F#r74I`{7B5`i)ggYDaMss*j24OT!v z$KS?Kz)48K6T_nPm-UcP5K{mRyFn-z%SFKL;{k%p{jCC*ckv1<1CTvbJNd`FX8buo zuYZ`5SoB->uFJ2&iSM!PUKZGZ6x^iWJ}JdORC%D@cu}nj@@$EA@+BJgYIKfzbWn{1 zAatgAbWn~2L3>2rx+%$lCEO(MJu1jSDBrx@xGBhi&)*>I+m>V^nC(37+ZJR(n>qgC zt^leM8~SiNvA592rfo` z*NaY>cxca1QICnByQw@tqMVVb<>_ZSa9g_U9Un~M&;cJ# z$^>d&|DtGENQaWp?skpmo9*B!z@Cl{29gll=e21wx5`yH8Py3Bf4priKD%^OtT%11 zUWmxHLX|#Ra4u&|fo~K!&&y%(9A8{)V1Qze9&31WW!X7tEm$fZ4`!je z{=c9^%ok5%dLSkLUmz+91)p3cklD#4t1nO176xsTkh+ewtgwuKzbbUiRd<`##Gg16eROCY*i5WTb! zGI9T&)ea)5nK_7qh?&v$*lY3r6W>;22l=l+N%>KpR$!k3eGLRSfBdsoaY-wQXXySX z8oOb@YqfNHWA-+g?JIYd3anTdWiK-0KP{l#wV-;FH_t#r6@FH2B+ zxzH|CR;wfk8Vg*s(g&xR;;WjgTv1M-*0_BjP!gl*t_(9Lqw({QE4LZael9$#d|4K$ zF)BKOC@W7oa+O@HTyWO43QpRxPbhD3+P5u(pn0ZCuxh-!e^ioH1G?{XR2mjt^%Aiy zU2}Me7LAg~5DUdM4?m2R3aai-2PPY}4od=6GF08SldM|KUZMNSYjk8i9)%| z?CN?iuTieL<8F-If&S@l(ho1DT|J;bM;00YvTv>vpb?Kq;kG3)6nvQSuK2p>Wm9K^ z)4GS)8e)IuXE0Sa2r+`mf!<}5fOFF~2+D;!3umJ4Wg;CHvL9mz%DFh_XWSp=pl)WK z9EoyJv~%_RaJtJ#*zO#%+Mg;uq-YBM1{u{hIE68_GjVoFStEhQhGu4hAtMu$7l-*D D^7 Date: Wed, 8 Apr 2026 07:15:59 -0700 Subject: [PATCH 023/251] Remove ``mjWARN_VGEOMFULL``, handle visual geom buffer full warnings in `mjvScene`. PiperOrigin-RevId: 896483023 Change-Id: I6dde2c20d8e8e229cf95e1f002dd10525d3376e6 --- doc/changelog.rst | 2 ++ doc/includes/references.h | 5 +++-- include/mujoco/mjdata.h | 1 - include/mujoco/mjvisualize.h | 4 +++- plugin/sdf/sdf.cc | 6 +++++- plugin/sensor/touch_grid.cc | 6 +++++- python/mujoco/introspect/enums.py | 11 +++++------ python/mujoco/introspect/structs.py | 4 ++-- python/mujoco/introspect/structs_test.py | 4 +--- simulate/simulate.cc | 16 ++++++++-------- simulate/simulate.h | 2 -- src/engine/engine_util_misc.c | 4 ---- src/engine/engine_vis_visualize.c | 11 +++++------ test/engine/engine_vis_visualize_test.cc | 1 - unity/Runtime/Bindings/MjBindings.cs | 12 +++++------- unity/Runtime/Components/MjScene.cs | 10 +++------- wasm/codegen/generated/bindings.cc | 1 - 17 files changed, 47 insertions(+), 53 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 1bd56f67..c14b133b 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -44,6 +44,8 @@ General - The ``vertcollide`` field in :ref:`mjsFlex` has been removed. It is no longer required since :doc:`MuJoCo Warp ` supports native flex collisions. + - The :ref:`mjtWarning` enum value ``mjWARN_VGEOMFULL`` is removed. Exhaustion of visual geoms is now handled + internally by the :ref:`mjvScene`. Bug fixes ^^^^^^^^^ diff --git a/doc/includes/references.h b/doc/includes/references.h index fd855a1c..89b2e81f 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -63,7 +63,6 @@ typedef enum mjtWarning_ { // warning types mjWARN_INERTIA = 0, // (near) singular inertia matrix mjWARN_CONTACTFULL, // too many contacts in contact list mjWARN_CNSTRFULL, // too many constraints - mjWARN_VGEOMFULL, // too many visual geoms mjWARN_BADQPOS, // bad number in qpos mjWARN_BADQVEL, // bad number in qvel mjWARN_BADQACC, // bad number in qacc @@ -3096,7 +3095,9 @@ struct mjvScene_ { // abstract scene passed to OpenGL renderer // framing int framewidth; // frame pixel width; 0: disable framing float framergb[3]; // frame color - int status; // status; 0: ok, 1: geoms exhausted + + // geom buffer status + int status; // 0: ok, 1: geoms exhausted, warning issued }; typedef struct mjvScene_ mjvScene; struct mjvFigure_ { // abstract 2D figure passed to OpenGL renderer diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 03585e6c..f98de218 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -77,7 +77,6 @@ typedef enum mjtWarning_ { // warning types mjWARN_INERTIA = 0, // (near) singular inertia matrix mjWARN_CONTACTFULL, // too many contacts in contact list mjWARN_CNSTRFULL, // too many constraints - mjWARN_VGEOMFULL, // too many visual geoms mjWARN_BADQPOS, // bad number in qpos mjWARN_BADQVEL, // bad number in qvel mjWARN_BADQACC, // bad number in qacc diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h index 10127c7c..68e22d21 100644 --- a/include/mujoco/mjvisualize.h +++ b/include/mujoco/mjvisualize.h @@ -355,7 +355,9 @@ struct mjvScene_ { // abstract scene passed to OpenGL renderer // framing int framewidth; // frame pixel width; 0: disable framing float framergb[3]; // frame color - int status; // status; 0: ok, 1: geoms exhausted + + // geom buffer status + int status; // 0: ok, 1: geoms exhausted, warning issued }; typedef struct mjvScene_ mjvScene; diff --git a/plugin/sdf/sdf.cc b/plugin/sdf/sdf.cc index c881c2c6..d3e77d0a 100644 --- a/plugin/sdf/sdf.cc +++ b/plugin/sdf/sdf.cc @@ -91,7 +91,11 @@ void SdfVisualizer::Visualize(const mjModel* m, const mjData* d, for (int k = 0; k < 2; k++) { for (int j = 0; j < (k == 0 ? 2 : n-1); j++) { if (scn->ngeom >= scn->maxgeom) { - mj_warning((mjData*)d, mjWARN_VGEOMFULL, scn->maxgeom); + if (!scn->status) { + mju_warning("Pre-allocated visual geom buffer is full. " + "Increase maxgeom above %d.", scn->maxgeom); + scn->status = 1; + } return; } mjvGeom* thisgeom = scn->geoms + scn->ngeom; diff --git a/plugin/sensor/touch_grid.cc b/plugin/sensor/touch_grid.cc index 1dfdfd02..98ba90f5 100644 --- a/plugin/sensor/touch_grid.cc +++ b/plugin/sensor/touch_grid.cc @@ -427,7 +427,11 @@ void TouchGrid::Visualize(const mjModel* m, mjData* d, const mjvOption* opt, continue; } if (scn->ngeom >= scn->maxgeom) { - mj_warning(d, mjWARN_VGEOMFULL, scn->maxgeom); + if (!scn->status) { + mju_warning("Pre-allocated visual geom buffer is full. " + "Increase maxgeom above %d.", scn->maxgeom); + scn->status = 1; + } mj_freeStack(d); return; } else { diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index bc28f78d..d686e861 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -578,12 +578,11 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjWARN_INERTIA', 0), ('mjWARN_CONTACTFULL', 1), ('mjWARN_CNSTRFULL', 2), - ('mjWARN_VGEOMFULL', 3), - ('mjWARN_BADQPOS', 4), - ('mjWARN_BADQVEL', 5), - ('mjWARN_BADQACC', 6), - ('mjWARN_BADCTRL', 7), - ('mjNWARNING', 8), + ('mjWARN_BADQPOS', 3), + ('mjWARN_BADQVEL', 4), + ('mjWARN_BADQACC', 5), + ('mjWARN_BADCTRL', 6), + ('mjNWARNING', 7), ]), )), ('mjtTimer', diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index c1838efa..57a0476e 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -5416,7 +5416,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ name='warning', type=ArrayType( inner_type=ValueType(name='mjWarningStat'), - extents=(8,), + extents=(7,), ), doc='warning statistics (mutable)', ), @@ -10524,7 +10524,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ StructFieldDecl( name='status', type=ValueType(name='int'), - doc='status; 0: ok, 1: geoms exhausted', + doc='0: ok, 1: geoms exhausted, warning issued', ), ), )), diff --git a/python/mujoco/introspect/structs_test.py b/python/mujoco/introspect/structs_test.py index 0bec9a8b..e43e3a4c 100644 --- a/python/mujoco/introspect/structs_test.py +++ b/python/mujoco/introspect/structs_test.py @@ -14,8 +14,6 @@ # ============================================================================== """Tests for structs.py.""" -import re - from absl.testing import absltest from . import ast_nodes @@ -36,7 +34,7 @@ class StructsTest(absltest.TestCase): field_names.add(field.name) if field.name == 'warning': self.assertEqual(field.type, - type_parsing.parse_type('mjWarningStat[8]')) + type_parsing.parse_type('mjWarningStat[7]')) self.assertEqual(field.doc, 'warning statistics (mutable)') elif field.name == 'qpos': self.assertEqual(field.type, type_parsing.parse_type('mjtNum*')) diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 8144ad8f..522404b9 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -2142,11 +2142,7 @@ void Simulate::Sync(bool state_only) { m_->stat = m_passive_->stat; } - // synchronize number of mjWARN_VGEOMFULL warnings - if (d_passive_->warning[mjWARN_VGEOMFULL].number > warn_vgeomfull_prev_) { - d_->warning[mjWARN_VGEOMFULL].number += - d_passive_->warning[mjWARN_VGEOMFULL].number - warn_vgeomfull_prev_; - } + } if (pending_.save_xml) { @@ -2334,7 +2330,7 @@ void Simulate::Sync(bool state_only) { mjopt_prev_ = m_passive_->opt; mjvis_prev_ = m_passive_->vis; mjstat_prev_ = m_passive_->stat; - warn_vgeomfull_prev_ = d_passive_->warning[mjWARN_VGEOMFULL].number; + } // update settings @@ -2550,7 +2546,7 @@ void Simulate::LoadOnRenderThread() { mjopt_prev_ = m_->opt; opt_prev_ = opt; cam_prev_ = cam; - warn_vgeomfull_prev_ = d_->warning[mjWARN_VGEOMFULL].number; + // full copy on init m_passive_ = mj_copyModel(nullptr, m_); @@ -3022,7 +3018,11 @@ void Simulate::RenderLoop() { int nusergeom = user_scn_geoms_.size(); int ngeom = std::min(nusergeom, this->scn.maxgeom - this->scn.ngeom); if (ngeom < nusergeom) { - mj_warning(d_passive_, mjWARN_VGEOMFULL, this->scn.maxgeom); + if (!this->scn.status) { + mju_warning("Pre-allocated visual geom buffer is full. " + "Increase maxgeom above %d.", this->scn.maxgeom); + this->scn.status = 1; + } } std::memcpy(this->scn.geoms + this->scn.ngeom, user_scn_geoms_.data(), ngeom * sizeof(mjvGeom)); diff --git a/simulate/simulate.h b/simulate/simulate.h index 4fad3961..27727ee7 100644 --- a/simulate/simulate.h +++ b/simulate/simulate.h @@ -147,8 +147,6 @@ class Simulate { mjvOption opt_prev_; mjvCamera cam_prev_; - int warn_vgeomfull_prev_; - // pending GUI-driven actions, to be applied at the next call to Sync struct { std::optional save_xml; diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 65057a51..8a867b1d 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -1595,10 +1595,6 @@ const char* mju_warningText(int warning, size_t info) { "Increase arena memory allocation above %s bytes.", mju_writeNumBytes(info)); break; - case mjWARN_VGEOMFULL: - mjSNPRINTF(str, "Pre-allocated visual geom buffer is full. Increase maxgeom above %zu.", info); - break; - case mjWARN_BADQPOS: mjSNPRINTF(str, "Nan, Inf or huge value in QPOS at DOF %zu. The simulation is unstable.", info); break; diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 3e8f7cf1..b1b74d21 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -169,7 +169,11 @@ static int bodycategory(const mjModel* m, int bodyid) { mjvGeom* acquireGeom(mjvScene* scn, int objid, int category, int objtype) { // check for overflow, SHOULD NOT OCCUR if (scn->ngeom >= scn->maxgeom) { - scn->status = 1; + if (!scn->status) { + mju_warning("Pre-allocated visual geom buffer is full. " + "Increase maxgeom above %d.", scn->maxgeom); + scn->status = 1; + } return NULL; } @@ -3379,7 +3383,6 @@ void mjv_updateScene(const mjModel* m, mjData* d, const mjvOption* opt, const mjvPerturb* pert, mjvCamera* cam, int catmask, mjvScene* scn) { // clear geoms scn->ngeom = 0; - scn->status = 0; // trigger plugin visualization hooks if (m->nplugin) { @@ -3416,10 +3419,6 @@ void mjv_updateScene(const mjModel* m, mjData* d, const mjvOption* opt, if (opt->flags[mjVIS_SKIN]) { mjv_updateActiveSkin(m, d, scn, opt); } - - if (scn->status) { - mj_warning(d, mjWARN_VGEOMFULL, scn->maxgeom); - } } diff --git a/test/engine/engine_vis_visualize_test.cc b/test/engine/engine_vis_visualize_test.cc index 90ec776e..31462697 100644 --- a/test/engine/engine_vis_visualize_test.cc +++ b/test/engine/engine_vis_visualize_test.cc @@ -101,7 +101,6 @@ TEST_F(MjvSceneTest, UpdateSceneGeomsExhausted) { mjv_updateScene(model, data, &opt_, &pert_, &cam_, mjCAT_ALL, &scn_); EXPECT_EQ(scn_.status, 1); EXPECT_EQ(scn_.ngeom, maxgeoms); - EXPECT_EQ(data->warning[mjWARN_VGEOMFULL].number, 1); mj_deleteData(data); FreeSceneObjects(); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 1e6168a9..ca287bd6 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -489,12 +489,11 @@ public enum mjtWarning : int{ mjWARN_INERTIA = 0, mjWARN_CONTACTFULL = 1, mjWARN_CNSTRFULL = 2, - mjWARN_VGEOMFULL = 3, - mjWARN_BADQPOS = 4, - mjWARN_BADQVEL = 5, - mjWARN_BADQACC = 6, - mjWARN_BADCTRL = 7, - mjNWARNING = 8, + mjWARN_BADQPOS = 3, + mjWARN_BADQVEL = 4, + mjWARN_BADQACC = 5, + mjWARN_BADCTRL = 6, + mjNWARNING = 7, } public enum mjtTimer : int{ mjTIMER_STEP = 0, @@ -5699,7 +5698,6 @@ public unsafe struct mjData_ { public mjWarningStat_ warning4; public mjWarningStat_ warning5; public mjWarningStat_ warning6; - public mjWarningStat_ warning7; public mjTimerStat_ timer0; public mjTimerStat_ timer1; public mjTimerStat_ timer2; diff --git a/unity/Runtime/Components/MjScene.cs b/unity/Runtime/Components/MjScene.cs index c9219d8e..13111a6b 100644 --- a/unity/Runtime/Components/MjScene.cs +++ b/unity/Runtime/Components/MjScene.cs @@ -359,22 +359,18 @@ public class MjScene : MonoBehaviour { } if (Data->warning3.number > 0) { Data->warning3.number = 0; - throw new PhysicsRuntimeException("VGEOMFULL: who constructed a mjvScene?!"); + throw new PhysicsRuntimeException("BADQPOS: NaN/inf in qpos."); } if (Data->warning4.number > 0) { Data->warning4.number = 0; - throw new PhysicsRuntimeException("BADQPOS: NaN/inf in qpos."); + throw new PhysicsRuntimeException("BADQVEL: NaN/inf in qvel."); } if (Data->warning5.number > 0) { Data->warning5.number = 0; - throw new PhysicsRuntimeException("BADQVEL: NaN/inf in qvel."); + throw new PhysicsRuntimeException("BADQACC: NaN/inf in qacc."); } if (Data->warning6.number > 0) { Data->warning6.number = 0; - throw new PhysicsRuntimeException("BADQACC: NaN/inf in qacc."); - } - if (Data->warning7.number > 0) { - Data->warning7.number = 0; throw new PhysicsRuntimeException("BADCTRL: NaN/inf in ctrl."); } } diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index dc2f2fc9..410cef5a 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -11380,7 +11380,6 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjWARN_INERTIA", mjWARN_INERTIA) .value("mjWARN_CONTACTFULL", mjWARN_CONTACTFULL) .value("mjWARN_CNSTRFULL", mjWARN_CNSTRFULL) - .value("mjWARN_VGEOMFULL", mjWARN_VGEOMFULL) .value("mjWARN_BADQPOS", mjWARN_BADQPOS) .value("mjWARN_BADQVEL", mjWARN_BADQVEL) .value("mjWARN_BADQACC", mjWARN_BADQACC) From 8191bf1e41c7d914a65ea352bfdf1a2eb4e59f9d Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Wed, 8 Apr 2026 08:20:53 -0700 Subject: [PATCH 024/251] Update MuJoCo README with a link to mjswan. Add a note about mjswan, which extends the JavaScript/WebAssembly bindings with real-time policy control and interactive features. Requested by tatsuki. PiperOrigin-RevId: 896513129 Change-Id: I984a41d281ffa1ad307f0b3db909140a201dd717 --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 98bee9e2..22a23e0e 100644 --- a/README.md +++ b/README.md @@ -137,7 +137,8 @@ These packages give users of various languages access to MuJoCo functionality: DeepMind's related environment stack, includes [PyMJCF](https://github.com/google-deepmind/dm_control/blob/main/dm_control/mjcf/README.md), a module for procedural manipulation of MuJoCo models. -- [JavaScript bindings and WebAssembly support](/wasm/README.md) (inspired [stillonearth](https://github.com/stillonearth) and [zalo](https://github.com/zalo)'s community projects). +- [JavaScript bindings and WebAssembly support](/wasm/README.md) (inspired [stillonearth](https://github.com/stillonearth) and [zalo](https://github.com/zalo)'s community projects; [mjswan](https://github.com/ttktjmt/mjswan) extends these with real-time policy control, interactive force +application, and more). - [C# bindings and Unity plug-in](https://mujoco.readthedocs.io/en/stable/unity.html) #### Third-party bindings: From 33fd2fe40e33087e4e1a28ec18a8eb83a49d9e43 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 8 Apr 2026 08:48:37 -0700 Subject: [PATCH 025/251] Improve `mju_addToSclSparseInc` with two-pointer loop. Profiling `leaves.xml` with contacts disabled, this function takes up most of the time (in the implicit integrator). Total `testspeed` run time: BEFORE: 18.5s AFTER: 16.5 PiperOrigin-RevId: 896525902 Change-Id: I57f4a984d178956a2902212deb857bf53f53f01b --- src/engine/engine_util_sparse.c | 53 +++++++++------------------------ 1 file changed, 14 insertions(+), 39 deletions(-) diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 9d9c2146..9d78e409 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -339,51 +339,26 @@ void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNu // dst += scl*src, only at common non-zero indices -void mju_addToSclSparseInc(mjtNum* dst, const mjtNum* src, - int nnzdst, const int* inddst, - int nnzsrc, const int* indsrc, mjtNum scl) { +void mju_addToSclSparseInc(mjtNum* restrict dst, const mjtNum* restrict src, + int nnzdst, const int* restrict inddst, + int nnzsrc, const int* restrict indsrc, mjtNum scl) { if (!nnzdst || !nnzsrc) { return; } - int adrs = 0, adrd = 0, inds = indsrc[0], indd = inddst[0]; - while (1) { - // common non-zero index + int adrs = 0, adrd = 0; + while (adrs < nnzsrc && adrd < nnzdst) { + int inds = indsrc[adrs]; + int indd = inddst[adrd]; + if (inds == indd) { - // add dst[adrd] += scl * src[adrs]; - - // advance src - if (++adrs < nnzsrc) { - inds = indsrc[adrs]; - } else { - return; - } - - // advance dst - if (++adrd < nnzdst) { - indd = inddst[adrd]; - } else { - return; - } - } - - // src non-zero index smaller: advance src - else if (inds < indd) { - if (++adrs < nnzsrc) { - inds = indsrc[adrs]; - } else { - return; - } - } - - // dst non-zero index smaller: advance dst - else { - if (++adrd < nnzdst) { - indd = inddst[adrd]; - } else { - return; - } + adrs++; + adrd++; + } else if (inds < indd) { + adrs++; + } else { + adrd++; } } } From de3912811408d61b661e82cf5d4bb64549b4f72f Mon Sep 17 00:00:00 2001 From: Tarik Kelestemur Date: Wed, 8 Apr 2026 14:09:45 -0400 Subject: [PATCH 026/251] fix render batching --- mjx/mujoco/mjx/_src/render_util.py | 19 +++--- mjx/mujoco/mjx/_src/render_util_test.py | 81 ++++++++++++++++--------- 2 files changed, 62 insertions(+), 38 deletions(-) diff --git a/mjx/mujoco/mjx/_src/render_util.py b/mjx/mujoco/mjx/_src/render_util.py index d0bd1110..098291c3 100644 --- a/mjx/mujoco/mjx/_src/render_util.py +++ b/mjx/mujoco/mjx/_src/render_util.py @@ -18,6 +18,7 @@ from typing import TYPE_CHECKING import jax import jax.numpy as jnp + import mujoco.mjx.warp as mjxw if TYPE_CHECKING: @@ -34,10 +35,11 @@ def get_rgb( Args: rc: RenderContextPytree. cam_id: Camera index to extract. - rgb_data: Packed render output, shape (total_pixels,) as uint32. + rgb_data: Packed render output, shape (..., total_pixels) as uint32. Returns: - Float32 RGB array with shape (H, W, 3), values in [0, 1]. + Float32 RGB array with shape (..., H, W, 3), values in [0, 1]. + Any leading batch axes in `rgb_data` are preserved. Raises: RuntimeError: If Warp is not installed. @@ -59,14 +61,14 @@ def get_rgb( height = int(warp_rc.cam_res.numpy()[cam_id][1]) packed = jax.lax.dynamic_slice_in_dim( - rgb_data, rgb_adr, width * height, axis=0 + rgb_data, rgb_adr, width * height, axis=rgb_data.ndim - 1 ) b = (packed & 0xFF).astype(jnp.float32) / 255.0 g = ((packed >> 8) & 0xFF).astype(jnp.float32) / 255.0 r = ((packed >> 16) & 0xFF).astype(jnp.float32) / 255.0 rgb = jnp.stack([r, g, b], axis=-1) - return rgb.reshape(height, width, 3) + return rgb.reshape(packed.shape[:-1] + (height, width, 3)) def get_depth( @@ -80,11 +82,12 @@ def get_depth( Args: rc: RenderContextPytree. cam_id: Camera index to extract. - depth_data: Raw depth output, shape (total_pixels,) as float32. + depth_data: Raw depth output, shape (..., total_pixels) as float32. depth_scale: Scale factor for normalizing depth values. Returns: - Float32 depth array with shape (H, W), clamped to [0, 1]. + Float32 depth array with shape (..., H, W, 1), clamped to [0, 1]. + Any leading batch axes in `depth_data` are preserved. Raises: RuntimeError: If Warp is not installed. @@ -106,8 +109,8 @@ def get_depth( height = int(warp_rc.cam_res.numpy()[cam_id][1]) raw = jax.lax.dynamic_slice_in_dim( - depth_data, depth_adr, width * height, axis=0 + depth_data, depth_adr, width * height, axis=depth_data.ndim - 1 ) depth = jnp.clip(raw / depth_scale, 0.0, 1.0) - return depth.reshape(height, width, 1) + return depth.reshape(raw.shape[:-1] + (height, width, 1)) diff --git a/mjx/mujoco/mjx/_src/render_util_test.py b/mjx/mujoco/mjx/_src/render_util_test.py index 7549b0af..b09ed7a3 100644 --- a/mjx/mujoco/mjx/_src/render_util_test.py +++ b/mjx/mujoco/mjx/_src/render_util_test.py @@ -12,19 +12,16 @@ # See the License for the specific language governing permissions and # limitations under the License. # ============================================================================== -import os +import contextlib from unittest import mock from absl.testing import absltest import jax import jax.numpy as jnp -from mujoco.mjx._src import io -from mujoco.mjx._src import render_util -import mujoco.mjx.warp as mjxw -from mujoco.mjx.warp.render_context import RenderContextPytree import numpy as np -_FORCE_TEST = os.environ.get('MJX_WARP_FORCE_TEST', '0') == '1' +from mujoco.mjx._src import render_util +from mujoco.mjx.warp.render_context import RenderContextPytree def _fake_render_context(ncam, width, height): @@ -39,15 +36,17 @@ def _fake_render_context(ncam, width, height): return rc -class RenderUtilTest(absltest.TestCase): +@contextlib.contextmanager +def _mock_render_runtime(warp_rc): + with mock.patch.object(render_util.mjxw, 'WARP_INSTALLED', True): + with mock.patch.dict( + 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', + {(0, None): warp_rc}, + ): + yield - def setUp(self): - super().setUp() - if not _FORCE_TEST: - if not mjxw.WARP_INSTALLED: - self.skipTest('Warp not installed.') - if not io.has_cuda_gpu_device(): - self.skipTest('No CUDA GPU device available.') + +class RenderUtilTest(absltest.TestCase): def test_get_rgb(self): width, height = 4, 4 @@ -55,24 +54,35 @@ class RenderUtilTest(absltest.TestCase): rc = mock.MagicMock(spec=RenderContextPytree, key=0) rgb_data = jnp.zeros((width * height,), dtype=jnp.uint32) - with mock.patch.dict( - 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', - {(0, None): warp_rc}, - ): + with _mock_render_runtime(warp_rc): rgb = jax.jit(render_util.get_rgb, static_argnums=(0, 1))(rc, 0, rgb_data) self.assertEqual(rgb.shape, (height, width, 3)) + def test_get_rgb_preserves_leading_dims(self): + width, height = 4, 4 + warp_rc = _fake_render_context(1, width, height) + rc = mock.MagicMock(spec=RenderContextPytree, key=0) + + with _mock_render_runtime(warp_rc): + for leading_shape in ((1,), (3,), (2, 3)): + with self.subTest(leading_shape=leading_shape): + rgb_data = jnp.zeros( + leading_shape + (width * height,), dtype=jnp.uint32 + ) + rgb = jax.jit(render_util.get_rgb, static_argnums=(0, 1))( + rc, 0, rgb_data + ) + + self.assertEqual(rgb.shape, leading_shape + (height, width, 3)) + def test_get_rgb_vmap(self): nworld, width, height = 3, 4, 4 warp_rc = _fake_render_context(1, width, height) rc = mock.MagicMock(spec=RenderContextPytree, key=0) rgb_data = jnp.zeros((nworld, width * height), dtype=jnp.uint32) - with mock.patch.dict( - 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', - {(0, None): warp_rc}, - ): + with _mock_render_runtime(warp_rc): rgb = jax.jit( jax.vmap(render_util.get_rgb, in_axes=(None, None, 0)), static_argnums=(0, 1), @@ -86,26 +96,37 @@ class RenderUtilTest(absltest.TestCase): rc = mock.MagicMock(spec=RenderContextPytree, key=0) depth_data = jnp.zeros((width * height,), dtype=jnp.float32) - with mock.patch.dict( - 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', - {(0, None): warp_rc}, - ): + with _mock_render_runtime(warp_rc): depth = jax.jit(render_util.get_depth, static_argnums=(0, 1, 3))( rc, 0, depth_data, 5.0 ) self.assertEqual(depth.shape, (height, width, 1)) + def test_get_depth_preserves_leading_dims(self): + width, height = 4, 4 + warp_rc = _fake_render_context(1, width, height) + rc = mock.MagicMock(spec=RenderContextPytree, key=0) + + with _mock_render_runtime(warp_rc): + for leading_shape in ((1,), (3,), (2, 3)): + with self.subTest(leading_shape=leading_shape): + depth_data = jnp.zeros( + leading_shape + (width * height,), dtype=jnp.float32 + ) + depth = jax.jit(render_util.get_depth, static_argnums=(0, 1, 3))( + rc, 0, depth_data, 5.0 + ) + + self.assertEqual(depth.shape, leading_shape + (height, width, 1)) + def test_get_depth_vmap(self): nworld, width, height = 3, 4, 4 warp_rc = _fake_render_context(1, width, height) rc = mock.MagicMock(spec=RenderContextPytree, key=0) depth_data = jnp.zeros((nworld, width * height), dtype=jnp.float32) - with mock.patch.dict( - 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', - {(0, None): warp_rc}, - ): + with _mock_render_runtime(warp_rc): depth = jax.jit( jax.vmap(render_util.get_depth, in_axes=(None, None, 0, None)), static_argnums=(0, 1, 3), From f2461f9ce65f80a725457caabad9491a4a1a3e26 Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Wed, 8 Apr 2026 11:34:33 -0700 Subject: [PATCH 027/251] Modify mj_PLUGIN_LIB_INIT to support multiple plugins in the same compilation unit. Previously on MSVC we used DllMain to register plugins, you cannot have multiple definitions of DllMain in a single unit so you would get errors if you tried to register two plugins. This modifies the implementation to insert a function pointer into the C runtime initialization instead. PiperOrigin-RevId: 896612678 Change-Id: I07732147b955d741c836acff6da986db0e9b9eff --- doc/APIreference/APIglobals.rst | 16 ++--- doc/changelog.rst | 4 ++ doc/programming/extension.rst | 11 ++-- include/mujoco/mjplugin.h | 61 +++++++++++-------- plugin/actuator/register.cc | 2 +- plugin/elasticity/register.cc | 2 +- plugin/obj_decoder/obj_decoder.cc | 2 +- plugin/sdf/register.cc | 3 +- plugin/sensor/register.cc | 2 +- plugin/stl_decoder/stl_decoder.cc | 2 +- plugin/usd_decoder/usd_decoder.cc | 2 +- src/experimental/mjz/mjz_decoder.cc | 2 +- src/experimental/platform/hal/renderer.cc | 2 +- .../platform/ux/object_launcher_plugin.cc | 2 +- 14 files changed, 60 insertions(+), 53 deletions(-) diff --git a/doc/APIreference/APIglobals.rst b/doc/APIreference/APIglobals.rst index 951d0b34..c2e614d8 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -691,17 +691,13 @@ mjPLUGIN_LIB_INIT .. code-block:: C - #define mjPLUGIN_LIB_INIT \ - static void _mjplugin_dllmain(void); \ - mjEXTERNC int __stdcall mjDLLMAIN(void* hinst, unsigned long reason, void* reserved) { \ - if (reason == 1) { \ - _mjplugin_dllmain(); \ - } \ - return 1; \ - } \ - static void _mjplugin_dllmain(void) + #define mjPLUGIN_LIB_INIT(n) \ + static void _mj_init_##n(void) __attribute__((constructor)); \ + static void _mj_init_##n(void) -Register a plugin as a dynamic library. See :ref:`plugin registration` for more details. +Register a plugin before `main()` is called. This macro takes a unique identifier `n` as an argument that is used to avoid +name collisions between different plugin initialization functions. See :ref:`plugin registration` for +more details. .. _tyXMacro: diff --git a/doc/changelog.rst b/doc/changelog.rst index c14b133b..b97dce5e 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -44,6 +44,10 @@ General - The ``vertcollide`` field in :ref:`mjsFlex` has been removed. It is no longer required since :doc:`MuJoCo Warp ` supports native flex collisions. + - :ref:`mjPLUGIN_LIB_INIT` macro now requires a name argument to avoid initialization function name collisions. + When building with MSVC, we now use the C runtime initialization section to initialize plugins instead of + ``DllMain``. See :ref:`plugin registration` for more details. + - The :ref:`mjtWarning` enum value ``mjWARN_VGEOMFULL`` is removed. Exhaustion of visual geoms is now handled internally by the :ref:`mjvScene`. diff --git a/doc/programming/extension.rst b/doc/programming/extension.rst index 2b25d493..f4a8ee0f 100644 --- a/doc/programming/extension.rst +++ b/doc/programming/extension.rst @@ -230,12 +230,11 @@ troubleshoot issues with a model) can be statically linked into the application. :ref:`mjpPlugin` struct in the ``main`` function, then passing it to :ref:`mjp_registerPlugin` to be registered with MuJoCo. -Generally, reusable plugins are expected to be packaged as dynamic libraries. A dynamic library containing one or more -MuJoCo plugins should make sure that all plugins are registered when the library is loaded. In GCC-compatible compilers, -this can be achieved by calling :ref:`mjp_registerPlugin` in a function that is declared with -``__attribute__((constructor))``, while in MSVC this can be done in a DLL entry point (canonically known as -``DllMain``). MuJoCo provides a convenience macro :ref:`mjPLUGIN_LIB_INIT` that expands to either of these -constructs depending on the compiler used. +Generally, reusable plugins are expected to be packaged as libraries and should be registered when the library is +loaded. In GCC-compatible compilers, this can be achieved by calling :ref:`mjp_registerPlugin` in a function that is +declared with ``__attribute__((constructor))``, while in MSVC this can be done by injecting code into the C runtime +initialization. MuJoCo provides a convenience macro :ref:`mjPLUGIN_LIB_INIT` that expands to either of these constructs +depending on the compiler used. Users of plugins that are delivered as dynamic libraries as described above can load the library using the function :ref:`mj_loadPluginLibrary`. This is the preferred way to load dynamic libraries containing MuJoCo plugins (rather than, diff --git a/include/mujoco/mjplugin.h b/include/mujoco/mjplugin.h index 7c6300b9..174a1f63 100644 --- a/include/mujoco/mjplugin.h +++ b/include/mujoco/mjplugin.h @@ -182,39 +182,46 @@ struct mjSDF_ { }; typedef struct mjSDF_ mjSDF; +//------------------------------------ Initialization ---------------------------------------------- + #if defined(__has_attribute) - #if __has_attribute(constructor) - #define mjPLUGIN_LIB_INIT __attribute__((constructor)) static void _mjplugin_init(void) - #endif // __has_attribute(constructor) - -#elif defined(_MSC_VER) - - #ifndef mjDLLMAIN - #define mjDLLMAIN DllMain + #define mjPLUGIN_LIB_INIT(n) \ + static void _mj_init_##n(void) __attribute__((constructor)); \ + static void _mj_init_##n(void) #endif - - #if !defined(mjEXTERNC) - #if defined(__cplusplus) - #define mjEXTERNC extern "C" +#elif defined(_MSC_VER) + // on x86, symbols are decorated with a leading underscore + #ifdef _M_IX86 + #define LINKER_NAME "__mj_ptr_" #else - #define mjEXTERNC - #endif // defined(__cplusplus) - #endif // !defined(mjEXTERNC) + #define LINKER_NAME "_mj_ptr_" + #endif - // NOLINTBEGIN(runtime/int) - #define mjPLUGIN_LIB_INIT \ - static void _mjplugin_dllmain(void); \ - mjEXTERNC int __stdcall mjDLLMAIN(void* hinst, unsigned long reason, void* reserved) { \ - if (reason == 1) { \ - _mjplugin_dllmain(); \ - } \ - return 1; \ - } \ - static void _mjplugin_dllmain(void) - // NOLINTEND(runtime/int) + #pragma section(".CRT$XCU", read) -#endif // defined(_MSC_VER) + #if !defined(mjEXTERNC) + #if defined(__cplusplus) + #define mjEXTERNC extern "C" + #else + #define mjEXTERNC + #endif // defined(__cplusplus) + #endif // !defined(mjEXTERNC) + + #define mjPLUGIN_LIB_INIT(n) \ + static void __cdecl _mj_init_##n(void); \ + /* use mjEXTERNC to prevent C++ name mangling */ \ + /* allocate the function pointer to the .CRT$XCU section of the executable */ \ + /* functions in this section are executed on startup before calling main() */ \ + mjEXTERNC __declspec(allocate(".CRT$XCU")) \ + void (__cdecl * _mj_ptr_##n)(void) = _mj_init_##n; \ + /* Force the linker to include the pointer symbol */ \ + __pragma(comment(linker, "/include:" LINKER_NAME #n)) \ + static void __cdecl _mj_init_##n(void) + +#else + #error "Unknown compiler: Plugin registration not supported." +#endif // function pointer type for mj_loadAllPluginLibraries callback typedef void (*mjfPluginLibraryLoadCallback)(const char* filename, int first, int count); diff --git a/plugin/actuator/register.cc b/plugin/actuator/register.cc index 66f24b93..e3c3e2f5 100644 --- a/plugin/actuator/register.cc +++ b/plugin/actuator/register.cc @@ -17,6 +17,6 @@ namespace mujoco::plugin::actuator { -mjPLUGIN_LIB_INIT { Pid::RegisterPlugin(); } +mjPLUGIN_LIB_INIT(actuator) { Pid::RegisterPlugin(); } } // namespace mujoco::plugin::actuator diff --git a/plugin/elasticity/register.cc b/plugin/elasticity/register.cc index ab8d283a..b4a68754 100644 --- a/plugin/elasticity/register.cc +++ b/plugin/elasticity/register.cc @@ -17,7 +17,7 @@ namespace mujoco::plugin::elasticity { -mjPLUGIN_LIB_INIT { +mjPLUGIN_LIB_INIT(elasticity) { Cable::RegisterPlugin(); } diff --git a/plugin/obj_decoder/obj_decoder.cc b/plugin/obj_decoder/obj_decoder.cc index 18cfdf5f..929e959a 100644 --- a/plugin/obj_decoder/obj_decoder.cc +++ b/plugin/obj_decoder/obj_decoder.cc @@ -114,7 +114,7 @@ int CanDecode(const mjResource* resource) { } // namespace -mjPLUGIN_LIB_INIT { +mjPLUGIN_LIB_INIT(obj_decoder) { mjpDecoder decoder; mjp_defaultDecoder(&decoder); decoder.content_type = "model/obj"; diff --git a/plugin/sdf/register.cc b/plugin/sdf/register.cc index 4e727e38..97f140da 100644 --- a/plugin/sdf/register.cc +++ b/plugin/sdf/register.cc @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include "bolt.h" #include "bowl.h" #include "gear.h" @@ -20,7 +21,7 @@ namespace mujoco::plugin::sdf { -mjPLUGIN_LIB_INIT { +mjPLUGIN_LIB_INIT(sdf) { Bolt::RegisterPlugin(); Bowl::RegisterPlugin(); Gear::RegisterPlugin(); diff --git a/plugin/sensor/register.cc b/plugin/sensor/register.cc index dd8a70d8..b3a587af 100644 --- a/plugin/sensor/register.cc +++ b/plugin/sensor/register.cc @@ -17,7 +17,7 @@ namespace mujoco::plugin::sensor { -mjPLUGIN_LIB_INIT { +mjPLUGIN_LIB_INIT(sensor) { TouchGrid::RegisterPlugin(); } diff --git a/plugin/stl_decoder/stl_decoder.cc b/plugin/stl_decoder/stl_decoder.cc index c6932d9a..7e1d1a5a 100644 --- a/plugin/stl_decoder/stl_decoder.cc +++ b/plugin/stl_decoder/stl_decoder.cc @@ -132,7 +132,7 @@ int CanDecode(const mjResource* resource) { } // namespace -mjPLUGIN_LIB_INIT { +mjPLUGIN_LIB_INIT(stl_decoder) { mjpDecoder decoder; mjp_defaultDecoder(&decoder); decoder.content_type = "model/stl"; diff --git a/plugin/usd_decoder/usd_decoder.cc b/plugin/usd_decoder/usd_decoder.cc index 58a24590..f1ab1c05 100644 --- a/plugin/usd_decoder/usd_decoder.cc +++ b/plugin/usd_decoder/usd_decoder.cc @@ -2459,7 +2459,7 @@ int CanDecode(const mjResource* resource) { } // namespace // clang-format off -mjPLUGIN_LIB_INIT { +mjPLUGIN_LIB_INIT(usd_decoder) { mjpDecoder decoder; mjp_defaultDecoder(&decoder); decoder.content_type = "model/usd"; diff --git a/src/experimental/mjz/mjz_decoder.cc b/src/experimental/mjz/mjz_decoder.cc index 8defa31d..ee29a80f 100644 --- a/src/experimental/mjz/mjz_decoder.cc +++ b/src/experimental/mjz/mjz_decoder.cc @@ -192,7 +192,7 @@ static mjSpec* ParseZipBuffer(const void* buffer, int nbuffer, const char* name, return mj_parseXML(root.c_str(), vfs, error, error_sz); } -mjPLUGIN_LIB_INIT { +mjPLUGIN_LIB_INIT(mjz_decoder) { mjpDecoder decoder; decoder.content_type = "application/zip"; decoder.extension = ".mjz|.zip"; diff --git a/src/experimental/platform/hal/renderer.cc b/src/experimental/platform/hal/renderer.cc index 4a9be6c1..2dc60e18 100644 --- a/src/experimental/platform/hal/renderer.cc +++ b/src/experimental/platform/hal/renderer.cc @@ -238,7 +238,7 @@ void Renderer::UpdateFps() { } // namespace mujoco::platform -mjPLUGIN_LIB_INIT { +mjPLUGIN_LIB_INIT(renderer) { mujoco::platform::GuiPlugin plugin; plugin.name = "Filament"; plugin.update = [](mujoco::platform::GuiPlugin* self) { diff --git a/src/experimental/platform/ux/object_launcher_plugin.cc b/src/experimental/platform/ux/object_launcher_plugin.cc index d4de9414..b012e957 100644 --- a/src/experimental/platform/ux/object_launcher_plugin.cc +++ b/src/experimental/platform/ux/object_launcher_plugin.cc @@ -175,7 +175,7 @@ class ObjectLauncher { } // namespace mujoco::studio -mjPLUGIN_LIB_INIT { +mjPLUGIN_LIB_INIT(object_launcher) { using mujoco::studio::ObjectLauncher; static ObjectLauncher plugin; From 05e26e961c3c9e61dceb4f0c0fe7ca94d8e4ffe0 Mon Sep 17 00:00:00 2001 From: Tom Power Date: Thu, 9 Apr 2026 01:42:29 -0700 Subject: [PATCH 028/251] Refactor: Expose flex-related fields directly on mjx.Model. PiperOrigin-RevId: 896958292 Change-Id: I94b32048e69d2b6844db0911f037318d5fbf4a6a --- mjx/mujoco/mjx/warp/bvh.py | 7 +++---- mjx/mujoco/mjx/warp/collision_driver.py | 5 ++--- mjx/mujoco/mjx/warp/forward.py | 13 ++++++------- mjx/mujoco/mjx/warp/render.py | 3 +-- mjx/mujoco/mjx/warp/types.py | 3 --- 5 files changed, 12 insertions(+), 19 deletions(-) diff --git a/mjx/mujoco/mjx/warp/bvh.py b/mjx/mujoco/mjx/warp/bvh.py index 2c1fee49..157315db 100644 --- a/mjx/mujoco/mjx/warp/bvh.py +++ b/mjx/mujoco/mjx/warp/bvh.py @@ -48,7 +48,6 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) - @ffi.format_args_for_warp def _refit_bvh_shim( # Model @@ -134,12 +133,12 @@ def _refit_bvh_jax_impl( m._impl.flex_radius, m._impl.flex_shell, m._impl.flex_shelldataadr, - m._impl.flex_vertadr, - m._impl.flex_vertnum, + m.flex_vertadr, + m.flex_vertnum, m.geom_dataid, m.geom_size, m.geom_type, - m._impl.nflex, + m.nflex, m._impl.nflexelem, d._impl.flexvert_xpos, d.geom_xmat, diff --git a/mjx/mujoco/mjx/warp/collision_driver.py b/mjx/mujoco/mjx/warp/collision_driver.py index 9d5412d9..b0330c4b 100644 --- a/mjx/mujoco/mjx/warp/collision_driver.py +++ b/mjx/mujoco/mjx/warp/collision_driver.py @@ -46,7 +46,6 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) - @ffi.format_args_for_warp def _collision_shim( # Model @@ -371,7 +370,7 @@ def _collision_jax_impl(m: types.Model, d: types.Data): m._impl.flex_shell, m._impl.flex_shelldataadr, m._impl.flex_shellnum, - m._impl.flex_vertadr, + m.flex_vertadr, m._impl.flex_vertflexid, m.geom_aabb, m.geom_conaffinity, @@ -413,7 +412,7 @@ def _collision_jax_impl(m: types.Model, d: types.Data): m.mesh_vert, m.mesh_vertadr, m.mesh_vertnum, - m._impl.nflex, + m.nflex, m._impl.nflexelem, m._impl.nflexshelldata, m._impl.nflexvert, diff --git a/mjx/mujoco/mjx/warp/forward.py b/mjx/mujoco/mjx/warp/forward.py index e0f5c053..3d12681a 100644 --- a/mjx/mujoco/mjx/warp/forward.py +++ b/mjx/mujoco/mjx/warp/forward.py @@ -46,7 +46,6 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) - @ffi.format_args_for_warp def _forward_shim( # Model @@ -1499,10 +1498,10 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m._impl.flex_shellnum, m._impl.flex_stiffness, m._impl.flex_vert, - m._impl.flex_vertadr, + m.flex_vertadr, m._impl.flex_vertbodyid, m._impl.flex_vertflexid, - m._impl.flex_vertnum, + m.flex_vertnum, m._impl.flexedge_J_colind, m._impl.flexedge_J_rowadr, m._impl.flexedge_J_rownnz, @@ -1596,7 +1595,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m._impl.nbranch, m.ncam, m.neq, - m._impl.nflex, + m.nflex, m._impl.nflexedge, m._impl.nflexelem, m._impl.nflexshelldata, @@ -3444,10 +3443,10 @@ def _step_jax_impl(m: types.Model, d: types.Data): m._impl.flex_shellnum, m._impl.flex_stiffness, m._impl.flex_vert, - m._impl.flex_vertadr, + m.flex_vertadr, m._impl.flex_vertbodyid, m._impl.flex_vertflexid, - m._impl.flex_vertnum, + m.flex_vertnum, m._impl.flexedge_J_colind, m._impl.flexedge_J_rowadr, m._impl.flexedge_J_rownnz, @@ -3542,7 +3541,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): m._impl.nbranch, m.ncam, m.neq, - m._impl.nflex, + m.nflex, m._impl.nflexedge, m._impl.nflexelem, m._impl.nflexshelldata, diff --git a/mjx/mujoco/mjx/warp/render.py b/mjx/mujoco/mjx/warp/render.py index e723e6b0..6a3f66ab 100644 --- a/mjx/mujoco/mjx/warp/render.py +++ b/mjx/mujoco/mjx/warp/render.py @@ -48,7 +48,6 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) - @ffi.format_args_for_warp def _render_shim( # Model @@ -164,7 +163,7 @@ def _render_jax_impl(m: types.Model, d: types.Data, ctx: RenderContextPytree): m.cam_sensorsize, m._impl.flex_edge, m._impl.flex_radius, - m._impl.flex_vertadr, + m.flex_vertadr, m.geom_dataid, m.geom_matid, m.geom_rgba, diff --git a/mjx/mujoco/mjx/warp/types.py b/mjx/mujoco/mjx/warp/types.py index ef75edc4..564fe57e 100644 --- a/mjx/mujoco/mjx/warp/types.py +++ b/mjx/mujoco/mjx/warp/types.py @@ -165,10 +165,8 @@ class ModelWarp(PyTreeNode): flex_shellnum: np.ndarray flex_stiffness: np.ndarray flex_vert: np.ndarray - flex_vertadr: np.ndarray flex_vertbodyid: np.ndarray flex_vertflexid: np.ndarray - flex_vertnum: np.ndarray flexedge_J_colind: np.ndarray flexedge_J_rowadr: np.ndarray flexedge_J_rownnz: np.ndarray @@ -200,7 +198,6 @@ class ModelWarp(PyTreeNode): nJfe: int nacttrnbody: int nbranch: int - nflex: int nflexedge: int nflexelem: int nflexelemdata: int From cc0933af8b4c73b6a06fc32d0595fbf13e520bd9 Mon Sep 17 00:00:00 2001 From: Tom Power Date: Thu, 9 Apr 2026 02:10:07 -0700 Subject: [PATCH 029/251] Add `com_pos` to Mujoco Warp shims. PiperOrigin-RevId: 896969879 Change-Id: Idc3eb067a8473251a98404525df0c8c134ac2032 --- mjx/mujoco/mjx/warp/smooth.py | 122 +++++++++++++++++++++++++++++ mjx/mujoco/mjx/warp/smooth_test.py | 82 +++++++++++++++++++ 2 files changed, 204 insertions(+) diff --git a/mjx/mujoco/mjx/warp/smooth.py b/mjx/mujoco/mjx/warp/smooth.py index 3fe3a712..209dc6b0 100644 --- a/mjx/mujoco/mjx/warp/smooth.py +++ b/mjx/mujoco/mjx/warp/smooth.py @@ -483,3 +483,125 @@ def tendon(m: types.Model, d: types.Data): def tendon_vmap(unused_axis_size, is_batched, m: types.Model, d: types.Data): d = tendon(m, d) return d, is_batched[1] + + +@ffi.format_args_for_warp +def _com_pos_shim( + # Model + nworld: int, + body_inertia: wp.array2d(dtype=wp.vec3), + body_mass: wp.array2d(dtype=float), + body_parentid: wp.array(dtype=int), + body_rootid: wp.array(dtype=int), + body_subtreemass: wp.array2d(dtype=float), + body_tree: tuple[wp.array(dtype=int), ...], + jnt_bodyid: wp.array(dtype=int), + jnt_dofadr: wp.array(dtype=int), + jnt_type: wp.array(dtype=int), + nbody: int, + njnt: int, + # Data + cdof: wp.array2d(dtype=wp.spatial_vector), + cinert: wp.array2d(dtype=mjwp_types.vec10), + subtree_com: wp.array2d(dtype=wp.vec3), + xanchor: wp.array2d(dtype=wp.vec3), + xaxis: wp.array2d(dtype=wp.vec3), + ximat: wp.array2d(dtype=wp.mat33), + xipos: wp.array2d(dtype=wp.vec3), + xmat: wp.array2d(dtype=wp.mat33), +): + _m.stat = _s + _m.opt = _o + _m.callback = _cb + _d.efc = _e + _d.contact = _c + _m.body_inertia = body_inertia + _m.body_mass = body_mass + _m.body_parentid = body_parentid + _m.body_rootid = body_rootid + _m.body_subtreemass = body_subtreemass + _m.body_tree = body_tree + _m.jnt_bodyid = jnt_bodyid + _m.jnt_dofadr = jnt_dofadr + _m.jnt_type = jnt_type + _m.nbody = nbody + _m.njnt = njnt + _d.cdof = cdof + _d.cinert = cinert + _d.subtree_com = subtree_com + _d.xanchor = xanchor + _d.xaxis = xaxis + _d.ximat = ximat + _d.xipos = xipos + _d.xmat = xmat + _d.nworld = nworld + mjwarp.com_pos(_m, _d) + + +def _com_pos_jax_impl(m: types.Model, d: types.Data): + output_dims = { + 'cdof': d.cdof.shape, + 'cinert': d._impl.cinert.shape, + 'subtree_com': d.subtree_com.shape, + } + jf = ffi.jax_callable_variadic_tuple( + _com_pos_shim, + num_outputs=3, + output_dims=output_dims, + vmap_method=None, + in_out_argnames=set(['cdof', 'cinert', 'subtree_com']), + stage_in_argnames=set([ + 'body_inertia', + 'body_mass', + 'body_subtreemass', + 'cdof', + 'subtree_com', + 'xanchor', + 'xaxis', + 'ximat', + 'xipos', + 'xmat', + ]), + stage_out_argnames=set(['cdof', 'subtree_com']), + graph_mode=m.opt._impl.graph_mode, + has_side_effect=False, + ) + out = jf( + d.qpos.shape[0], + m.body_inertia, + m.body_mass, + m.body_parentid, + m.body_rootid, + m.body_subtreemass, + m._impl.body_tree, + m.jnt_bodyid, + m.jnt_dofadr, + m.jnt_type, + m.nbody, + m.njnt, + d.cdof, + d._impl.cinert, + d.subtree_com, + d.xanchor, + d.xaxis, + d.ximat, + d.xipos, + d.xmat, + ) + d = d.tree_replace( + {'cdof': out[0], '_impl.cinert': out[1], 'subtree_com': out[2]} + ) + return d + + +@jax.custom_batching.custom_vmap +@ffi.marshal_jax_warp_callable +def com_pos(m: types.Model, d: types.Data): + return _com_pos_jax_impl(m, d) + + +@com_pos.def_vmap +@ffi.marshal_custom_vmap +def com_pos_vmap(unused_axis_size, is_batched, m: types.Model, d: types.Data): + d = com_pos(m, d) + return d, is_batched[1] diff --git a/mjx/mujoco/mjx/warp/smooth_test.py b/mjx/mujoco/mjx/warp/smooth_test.py index 92c02875..e7ffb92b 100644 --- a/mjx/mujoco/mjx/warp/smooth_test.py +++ b/mjx/mujoco/mjx/warp/smooth_test.py @@ -100,6 +100,37 @@ class SmoothTest(parameterized.TestCase): tu.assert_attr_eq(d, dx, 'site_xpos') tu.assert_eq(d.site_xmat.reshape((-1, 3, 3)), dx.site_xmat, 'site_xmat') + def test_com_pos(self): + """Tests com_pos with unbatched data.""" + if not _FORCE_TEST: + if not mjxw.WARP_INSTALLED: + self.skipTest('Warp not installed.') + if not io.has_cuda_gpu_device(): + self.skipTest('No CUDA GPU device available.') + + m = tu.load_test_file('pendula.xml') + + d = mujoco.MjData(m) + mx = mjx.put_model(m, impl='warp') + mx = mx.replace(_impl=mx._impl.replace(qM_tiles=())) + + rng = jax.random.PRNGKey(0) + dx = mjx.make_data(m, impl='warp') + _, key = jax.random.split(rng) + qpos = jax.random.uniform(key, (m.nq,)) + dx = dx.replace(qpos=qpos) + + dx = jax.jit(smooth.kinematics)(mx, dx) + dx = jax.jit(smooth.com_pos)(mx, dx) + + d.qpos[:] = qpos + mujoco.mj_kinematics(m, d) + mujoco.mj_comPos(m, d) + + tu.assert_attr_eq(d, dx, 'subtree_com') + tu.assert_attr_eq(d, dx, 'cdof') + tu.assert_eq(d.cinert, dx._impl.cinert, 'cinert') + def test_kinematics_vmap(self): """Tests kinematics with batched data.""" if not mjxw.WARP_INSTALLED: @@ -144,6 +175,57 @@ class SmoothTest(parameterized.TestCase): tu.assert_attr_eq(d, dx, 'site_xpos') tu.assert_eq(d.site_xmat.reshape((-1, 3, 3)), dx.site_xmat, 'site_xmat') + def test_com_pos_vmap(self): + """Tests com_pos with batched data.""" + if not mjxw.WARP_INSTALLED: + self.skipTest('Warp not installed.') + if not io.has_cuda_gpu_device(): + self.skipTest('No CUDA GPU device available.') + + m = tu.load_test_file('pendula.xml') + + batch_size = 7 + d = mujoco.MjData(m) + mx = mjx.put_model(m, impl='warp') + mx = mx.replace(_impl=mx._impl.replace(qM_tiles=())) + + worldids = jp.arange(batch_size) + dx_batch = jax.vmap(functools.partial(tu.make_data, m))(worldids) + for f in ( + 'xanchor', + 'xaxis', + 'xpos', + 'xipos', + 'site_xpos', + 'site_xmat', + 'subtree_com', + 'cdof', + ): + dx_batch = dx_batch.replace(**{f: jp.zeros_like(getattr(dx_batch, f))}) + dx_batch = dx_batch.tree_replace( + {'_impl.cinert': jp.zeros_like(dx_batch._impl.cinert)} + ) + + dx_batch = jax.jit(jax.vmap(smooth.kinematics, in_axes=(None, 0)))( + mx, dx_batch + ) + dx_batch = jax.jit(jax.vmap(smooth.com_pos, in_axes=(None, 0)))( + mx, dx_batch + ) + + for i in range(batch_size): + dx = dx_batch[i] + + d.qpos[:] = dx.qpos + d.mocap_pos[:] = dx.mocap_pos + d.mocap_quat[:] = dx.mocap_quat + mujoco.mj_kinematics(m, d) + mujoco.mj_comPos(m, d) + + tu.assert_attr_eq(d, dx, 'subtree_com') + tu.assert_attr_eq(d, dx, 'cdof') + tu.assert_eq(d.cinert, dx._impl.cinert, 'cinert') + def test_kinematics_nested_vmap(self): """Tests kinematics with nested batch data.""" if not _FORCE_TEST: From 229d821915f29c6b815faeecc9ba30c87358113b Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Thu, 9 Apr 2026 03:06:18 -0700 Subject: [PATCH 030/251] Enable multiccd for golden data. PiperOrigin-RevId: 896996806 Change-Id: I7f842d25b4497e135831879e4ac1b268d0307aa6 --- test/testdata/catenary.xml | 4 ++++ test/testdata/delay.xml | 4 +++- test/testdata/flex.xml | 2 +- test/testdata/frustum.xml | 4 ++++ test/testdata/hammock.xml | 4 +++- test/testdata/model.xml | 2 +- test/testdata/tendon_wrap.xml | 2 +- 7 files changed, 17 insertions(+), 5 deletions(-) diff --git a/test/testdata/catenary.xml b/test/testdata/catenary.xml index 13d0b34d..a3ba6f3e 100644 --- a/test/testdata/catenary.xml +++ b/test/testdata/catenary.xml @@ -1,4 +1,8 @@ + + diff --git a/test/testdata/delay.xml b/test/testdata/delay.xml index b4166ca4..ed2f127e 100644 --- a/test/testdata/delay.xml +++ b/test/testdata/delay.xml @@ -1,5 +1,7 @@ - diff --git a/test/testdata/flex.xml b/test/testdata/flex.xml index 0bc6403a..07c08b6d 100644 --- a/test/testdata/flex.xml +++ b/test/testdata/flex.xml @@ -1,6 +1,6 @@ diff --git a/test/testdata/frustum.xml b/test/testdata/frustum.xml index 46fe5830..f95a93b8 100644 --- a/test/testdata/frustum.xml +++ b/test/testdata/frustum.xml @@ -2,6 +2,10 @@ The frustum should match exactly the fron face of the box. --> + + diff --git a/test/testdata/hammock.xml b/test/testdata/hammock.xml index 17f0bcd4..5a4499a3 100644 --- a/test/testdata/hammock.xml +++ b/test/testdata/hammock.xml @@ -22,7 +22,9 @@ Simple hammock, implemented as a 2D grid composite, pinned at the corners. --> - diff --git a/test/testdata/model.xml b/test/testdata/model.xml index 20328265..9bd11249 100644 --- a/test/testdata/model.xml +++ b/test/testdata/model.xml @@ -1,6 +1,6 @@ diff --git a/test/testdata/tendon_wrap.xml b/test/testdata/tendon_wrap.xml index 7608fd8f..d21eb5f8 100644 --- a/test/testdata/tendon_wrap.xml +++ b/test/testdata/tendon_wrap.xml @@ -1,6 +1,6 @@ From edc807895f065d454b6c99214edff3dceeba7945 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 9 Apr 2026 03:32:17 -0700 Subject: [PATCH 031/251] Split SceneView into two parts: SceneView and SceneBridge. SceneView owns the filament Scene, View, and Camera objects. is responsible for rendering the Scene (to which Entities have been added). SceneBridge takes the mjvScene as an input and updates the corresponding light and drawable Entities in the SceneView. PiperOrigin-RevId: 897007401 Change-Id: I3ec245f5bd48c3654672b11a55fb011f113454cc --- src/experimental/filament/CMakeLists.txt | 2 + .../filament/filament/filament_context.cc | 51 +- .../filament/filament/filament_context.h | 3 + .../filament/filament/imgui_editor.cc | 10 +- .../filament/filament/imgui_editor.h | 4 +- .../filament/filament/scene_bridge.cc | 370 +++++++++++++ .../filament/filament/scene_bridge.h | 85 +++ .../filament/filament/scene_view.cc | 492 +++++------------- .../filament/filament/scene_view.h | 112 ++-- 9 files changed, 682 insertions(+), 447 deletions(-) create mode 100644 src/experimental/filament/filament/scene_bridge.cc create mode 100644 src/experimental/filament/filament/scene_bridge.h diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index 367f2725..a495863b 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -54,6 +54,8 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/render_target.h filament/renderables.cc filament/renderables.h + filament/scene_bridge.cc + filament/scene_bridge.h filament/scene_view.cc filament/scene_view.h filament/texture.cc diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index b960eb49..cbc79c24 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -44,6 +44,7 @@ #include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/render_target.h" +#include "experimental/filament/filament/scene_bridge.h" #include "experimental/filament/filament/scene_view.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" @@ -82,6 +83,7 @@ FilamentContext::FilamentContext(const mjrFilamentConfig* config) FilamentContext::~FilamentContext() { DestroyRenderTargets(); gui_view_.reset(); + scene_bridge_.reset(); scene_view_.reset(); object_manager_.reset(); engine_->destroy(renderer_); @@ -91,7 +93,9 @@ FilamentContext::~FilamentContext() { } void FilamentContext::Init(const mjModel* model) { - scene_view_ = std::make_unique(object_manager_.get(), model); + scene_view_ = std::make_unique(engine_); + scene_bridge_ = std::make_unique(object_manager_.get(), model, + scene_view_.get()); gui_view_ = std::make_unique( engine_, object_manager_->GetMaterial(ObjectManager::kUnlitUi)); @@ -120,8 +124,7 @@ void FilamentContext::Render(const mjrRect& viewport, const mjvScene* scene) { window_height_ = viewport.height; } - scene_view_->SetViewport(viewport); - scene_view_->UpdateScene(scene); + scene_bridge_->Update(viewport, scene); // Update the UX renderable entity after processing the scene in case there // are any elements in the scene which generate UX draw calls (e.g. labels). if (gui_view_ && gui_swap_chain_target_ == scene_swap_chain_target_) { @@ -137,6 +140,7 @@ void FilamentContext::Render(const mjrRect& viewport, const mjvScene* scene) { } else if (scene->flags[mjRND_DEPTH]) { last_render_mode_ = SceneView::DrawMode::kDepth; } + last_camera_ = mjv_averageCamera(scene->camera, scene->camera + 1); // Render the frame if we're not rendering to a texture. if (scene_swap_chain_target_ == kWindowSwapChain) { @@ -146,7 +150,11 @@ void FilamentContext::Render(const mjrRect& viewport, const mjvScene* scene) { } if (renderer_->beginFrame(window_swap_chain_)) { - scene_view_->Render(renderer_, last_render_mode_); + SceneView::RenderRequest request; + request.draw_mode = last_render_mode_; + request.viewport = viewport; + request.camera = last_camera_; + scene_view_->Render(renderer_, request); if (gui_view_ && gui_swap_chain_target_ == kWindowSwapChain) { gui_view_->Render(renderer_); @@ -240,7 +248,12 @@ void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb, if (rgb) { if (renderer_->beginFrame(offscreen_swap_chain_)) { - scene_view_->Render(renderer_, last_render_mode_, color_target_.get()); + SceneView::RenderRequest request; + request.draw_mode = last_render_mode_; + request.viewport = viewport; + request.target = color_target_.get(); + request.camera = last_camera_; + scene_view_->Render(renderer_, request); // Render the GUI to the texture as well if requested. if (gui_view_ && gui_swap_chain_target_ == kOffscreenSwapChain) { @@ -256,8 +269,12 @@ void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb, if (depth) { if (renderer_->beginFrame(offscreen_swap_chain_)) { - scene_view_->Render(renderer_, SceneView::DrawMode::kDepth, - depth_target_.get()); + SceneView::RenderRequest request; + request.draw_mode = SceneView::DrawMode::kDepth; + request.viewport = viewport; + request.target = depth_target_.get(); + request.camera = last_camera_; + scene_view_->Render(renderer_, request); const size_t num_bytes = viewport.width * viewport.height * sizeof(float); ReadDepthPixels(renderer_, depth_target_.get(), viewport, depth, @@ -276,24 +293,24 @@ void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb, } void FilamentContext::UploadMesh(const mjModel* model, int id) { - if (!scene_view_) { - mju_error("SceneView is not initialized."); + if (!scene_bridge_) { + mju_error("SceneBridge is not initialized."); } - scene_view_->UploadMesh(model, id); + scene_bridge_->UploadMesh(model, id); } void FilamentContext::UploadTexture(const mjModel* model, int id) { - if (!scene_view_) { - mju_error("SceneView is not initialized."); + if (!scene_bridge_) { + mju_error("SceneBridge is not initialized."); } - scene_view_->UploadTexture(model, id); + scene_bridge_->UploadTexture(model, id); } void FilamentContext::UploadHeightField(const mjModel* model, int id) { - if (!scene_view_) { - mju_error("SceneView is not initialized."); + if (!scene_bridge_) { + mju_error("SceneBridge is not initialized."); } - scene_view_->UploadHeightField(model, id); + scene_bridge_->UploadHeightField(model, id); } uintptr_t FilamentContext::UploadGuiImage(uintptr_t tex_id, @@ -315,6 +332,6 @@ double FilamentContext::GetFrameRate() const { return 1.0e9 / static_cast(ns); } -void FilamentContext::UpdateGui() { DrawGui(scene_view_.get()); } +void FilamentContext::UpdateGui() { DrawGui(scene_bridge_.get()); } } // namespace mujoco diff --git a/src/experimental/filament/filament/filament_context.h b/src/experimental/filament/filament/filament_context.h index 8de07824..77c22a88 100644 --- a/src/experimental/filament/filament/filament_context.h +++ b/src/experimental/filament/filament/filament_context.h @@ -27,6 +27,7 @@ #include "experimental/filament/filament/gui_view.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/render_target.h" +#include "experimental/filament/filament/scene_bridge.h" #include "experimental/filament/filament/scene_view.h" #include "experimental/filament/render_context_filament.h" @@ -80,12 +81,14 @@ class FilamentContext { std::unique_ptr platform_; SceneView::DrawMode last_render_mode_ = SceneView::DrawMode::kNormal; + mjvGLCamera last_camera_; SwapChainType scene_swap_chain_target_ = kWindowSwapChain; SwapChainType gui_swap_chain_target_ = kWindowSwapChain; std::unique_ptr color_target_; std::unique_ptr depth_target_; std::unique_ptr object_manager_; std::unique_ptr scene_view_; + std::unique_ptr scene_bridge_; std::unique_ptr gui_view_; int window_width_ = 0; int window_height_ = 0; diff --git a/src/experimental/filament/filament/imgui_editor.cc b/src/experimental/filament/filament/imgui_editor.cc index 6b67072e..bfc773f4 100644 --- a/src/experimental/filament/filament/imgui_editor.cc +++ b/src/experimental/filament/filament/imgui_editor.cc @@ -35,6 +35,7 @@ #include #include #include "experimental/filament/filament/color_grading_options.h" +#include "experimental/filament/filament/scene_bridge.h" #include "experimental/filament/filament/scene_view.h" namespace mujoco { @@ -560,7 +561,7 @@ void DrawCameraGui(SceneView* scene_view) { Ui("Direction", &direction); } -void DrawIndirectLightGui(SceneView* scene_view) { +void DrawIndirectLightGui(SceneBridge* scene_bridge, SceneView* scene_view) { filament::View* view = scene_view->GetDefaultRenderView(); auto ibl = view->getScene()->getIndirectLight(); @@ -575,7 +576,7 @@ void DrawIndirectLightGui(SceneView* scene_view) { static char filename[256]; ImGui::InputText("Filename", filename, sizeof(filename)); if (ImGui::Button("Load")) { - scene_view->SetEnvironmentLight(filename, intensity); + scene_bridge->SetEnvironmentLight(filename, intensity); } } @@ -649,7 +650,8 @@ void DrawLightGui(filament::LightManager& lm, } } -void DrawGui(SceneView* scene_view) { +void DrawGui(SceneBridge* scene_bridge) { + SceneView* scene_view = scene_bridge->GetSceneView(); filament::View* view = scene_view->GetDefaultRenderView(); filament::Engine* engine = scene_view->GetEngine(); filament::LightManager& lm = engine->getLightManager(); @@ -716,7 +718,7 @@ void DrawGui(SceneView* scene_view) { } if (ImGui::TreeNodeEx("Lights")) { if (ImGui::TreeNodeEx("Indirect (Image-based) Light")) { - DrawIndirectLightGui(scene_view); + DrawIndirectLightGui(scene_bridge, scene_view); ImGui::TreePop(); } view->getScene()->forEach([&](utils::Entity entity) { diff --git a/src/experimental/filament/filament/imgui_editor.h b/src/experimental/filament/filament/imgui_editor.h index b8f45357..f073fe45 100644 --- a/src/experimental/filament/filament/imgui_editor.h +++ b/src/experimental/filament/filament/imgui_editor.h @@ -15,12 +15,12 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_IMGUI_EDITOR_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_IMGUI_EDITOR_H_ -#include "experimental/filament/filament/scene_view.h" +#include "experimental/filament/filament/scene_bridge.h" namespace mujoco { // Generates a ImGui Window for the given scene views. -void DrawGui(SceneView* scene_views); +void DrawGui(SceneBridge* scene_bridge); } // namespace mujoco diff --git a/src/experimental/filament/filament/scene_bridge.cc b/src/experimental/filament/filament/scene_bridge.cc new file mode 100644 index 00000000..6b445035 --- /dev/null +++ b/src/experimental/filament/filament/scene_bridge.cc @@ -0,0 +1,370 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "experimental/filament/filament/scene_bridge.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "experimental/filament/filament/color_grading_options.h" +#include "experimental/filament/filament/drawable.h" +#include "experimental/filament/filament/gui_view.h" +#include "experimental/filament/filament/light.h" +#include "experimental/filament/filament/math_util.h" +#include "experimental/filament/filament/model_objects.h" +#include "experimental/filament/filament/model_util.h" +#include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/filament/scene_view.h" + +namespace mujoco { + +using filament::math::float3; +using filament::math::float4; +using filament::math::mat3; +using filament::math::mat4; + +SceneBridge::SceneBridge(ObjectManager* object_mgr, const mjModel* model, + SceneView* scene_view) + : scene_view_(scene_view), object_mgr_(object_mgr) { + model_objects_ = + std::make_unique(model, object_mgr_->GetEngine()); + + // Configure options for the normal view. + auto cg = scene_view_->GetColorGradingOptions(); + cg.exposure = ReadElement(model, "filament.out.exposure", cg.exposure); + cg.contrast = ReadElement(model, "filament.out.contrast", cg.contrast); + cg.vibrance = ReadElement(model, "filament.out.vibrance", cg.vibrance); + cg.saturation = ReadElement(model, "filament.out.saturation", cg.saturation); + cg.temperature = ReadElement(model, "filament.out.temperature", cg.temperature); + cg.tint = ReadElement(model, "filament.out.tint", cg.tint); + + auto tone_mapping = + ReadElement(model, "filament.out.tone_mapping"); + if (tone_mapping == "aces") { + cg.tone_mapper = ToneMapperType::kACES; + } else if (tone_mapping == "aces_legacy") { + cg.tone_mapper = ToneMapperType::kACESLegacy; + } else if (tone_mapping == "filmic") { + cg.tone_mapper = ToneMapperType::kFilmic; + } else if (tone_mapping == "linear") { + cg.tone_mapper = ToneMapperType::kLinear; + } else if (tone_mapping == "pbr_neutral") { + cg.tone_mapper = ToneMapperType::kPBRNeutral; + } + scene_view_->SetColorGradingOptions(cg); + + filament::View* fview = scene_view_->GetDefaultRenderView(); + auto ao = fview->getAmbientOcclusionOptions(); + ao.enabled = ReadElement(model, "filament.ao.enabled", true); + ao.bentNormals = ReadElement(model, "filament.ao.bent_normals", false); + ao.ssct.enabled = ReadElement(model, "filament.ao.ssct", ao.ssct.enabled); + ao.quality = filament::QualityLevel::ULTRA; + ao.lowPassFilter = filament::QualityLevel::ULTRA; + ao.upsampling = filament::QualityLevel::ULTRA; + ao.bilateralThreshold = 0.5f; + fview->setAmbientOcclusionOptions(ao); + + auto msaa = fview->getMultiSampleAntiAliasingOptions(); + msaa.enabled = ReadElement(model, "filament.msaa.enabled", true); + fview->setMultiSampleAntiAliasingOptions(msaa); + + default_shadow_map_size_ = ReadElement( + model, "filament.shadows.map_size", default_shadow_map_size_); + default_vsm_blur_width_ = ReadElement( + model, "filament.shadows.vsm_blur_width", default_vsm_blur_width_); + + auto shadow_type = fview->getShadowType(); + shadow_type = ReadElement(model, "filament.shadows.type", shadow_type); + fview->setShadowType(shadow_type); + + auto fog_opts = fview->getFogOptions(); + fog_opts.enabled = + ReadElement(model, "filament.fog.enabled", fog_opts.enabled); + fog_opts.color = ReadElement(model, "filament.fog.color", fog_opts.color); + fog_opts.distance = ReadElement( + model, "filament.fog.distance", fog_opts.distance); + fog_opts.density = ReadElement( + model, "filament.fog.density", fog_opts.density); + fog_opts.cutOffDistance = ReadElement( + model, "filament.fog.cutOffDistance", fog_opts.cutOffDistance); + fog_opts.maximumOpacity = ReadElement( + model, "filament.fog.maximumOpacity", fog_opts.maximumOpacity); + fog_opts.height = ReadElement(model, "filament.fog.height", fog_opts.height); + fog_opts.heightFalloff = ReadElement( + model, "filament.fog.heightFalloff", fog_opts.heightFalloff); + fog_opts.inScatteringStart = ReadElement( + model, "filament.fog.inScatteringStart", fog_opts.inScatteringStart); + fog_opts.inScatteringSize = ReadElement( + model, "filament.fog.inScatteringSize", fog_opts.inScatteringSize); + fview->setFogOptions(fog_opts); + + fallback_head_light_intensity_ = + ReadElement(model, "filament.fallback.head_light_intensity", + fallback_head_light_intensity_); + fallback_scene_light_intensity_ = + ReadElement(model, "filament.fallback.scene_light_intensity", + fallback_scene_light_intensity_); + fallback_environment_light_intensity_ = + ReadElement(model, "filament.fallback.environment_light_intensity", + fallback_environment_light_intensity_); + + // Create an empty/black indirect light to ensure that the skybox is oriented + // to respect mujoco's Z-up convention. + filament::IndirectLight* empty_ibl = + model_objects_->CreateIndirectLight(-1, 100000); + scene_view_->AddToScene(empty_ibl); + + PrepareLights(); +} + +SceneBridge::~SceneBridge() { + for (auto& iter : lights_) { + scene_view_->RemoveFromScene(iter.get()); + } + lights_.clear(); + + for (auto& iter : drawables_) { + scene_view_->RemoveFromScene(iter.get()); + } + drawables_.clear(); +} + +void SceneBridge::SetEnvironmentLight(std::string_view filename, + float intensity) { + filament::IndirectLight* ibl = nullptr; + scene_view_->AddToScene(ibl); + object_mgr_->LoadFallbackIndirectLight(filename, intensity); + ibl = object_mgr_->GetFallbackIndirectLight(); + scene_view_->AddToScene(ibl); +} + +std::optional SceneBridge::ClipFromWorld(const float3& pos) const{ + const float4 clip_pos = clip_from_world_ * float4(pos, 1.0f); + if (clip_pos.w == 0.0f) { + return std::nullopt; + } + return clip_pos.xyz / clip_pos.w; +} + +void SceneBridge::PrepareLights() { + filament::Engine* engine = object_mgr_->GetEngine(); + const mjModel* model = model_objects_->GetModel(); + filament::Skybox* skybox = model_objects_->CreateSkybox(); + if (skybox) { + scene_view_->AddToScene(skybox); + } + + float total_light_intensity = 0.0f; + + for (int i = 0; i < model->nlight; ++i) { + total_light_intensity += model->light_intensity[i]; + + if (model->light_type[i] == mjLIGHT_IMAGE) { + auto* indirect_light = model_objects_->CreateIndirectLight( + model->light_texid[i], model->light_intensity[i]); + if (indirect_light) { + scene_view_->AddToScene(indirect_light); + } + // Add an nullptr as a placeholder so that our indices still match. + lights_.emplace_back(nullptr); + } else { + Light::Params params; + params.color = ReadFloat3(model->light_diffuse); + params.type = (mjtLightType)model->light_type[i]; + params.castshadow = model->light_castshadow[i]; + params.bulbradius = model->light_bulbradius[i]; + params.range = model->light_range[i]; + params.intensity = model->light_intensity[i]; + params.shadow_map_size = default_shadow_map_size_; + params.vsm_blur_width = default_vsm_blur_width_; + if (params.type == mjLIGHT_SPOT) { + params.spot_cone_angle = model->light_cutoff[i]; + } + + auto light_obj = std::make_unique(engine, params); +#ifndef __EMSCRIPTEN__ + // TODO(b/458045799): Re-enable when lights work on glinux and chromebook. + scene_view_->AddToScene(light_obj.get()); +#endif + lights_.emplace_back(std::move(light_obj)); + } + } + + // Add a placeholder (black) headlight as our last light. Going forward, we'll + // assume lights_.back() is always the headlight. + { + Light::Params params; + params.color = float3(0, 0, 0); + params.headlight = true; + params.type = mjLIGHT_DIRECTIONAL; + params.castshadow = 0; + params.intensity = 0; + auto light_obj = std::make_unique(engine, params); +#ifndef __EMSCRIPTEN__ + // TODO(b/458045799): Re-enable when lights work on glinux and chromebook. + scene_view_->AddToScene(light_obj.get()); +#endif + lights_.emplace_back(std::move(light_obj)); + } + + // There are no "physical" lights in the scene which means we're likely + // dealing with a "classic renderer" scene. In this case, let's add a + // default environment light and set the light intensity ourselves. + if (total_light_intensity == 0.0f) { + auto* ibl = object_mgr_->GetFallbackIndirectLight(); + if (ibl) { + ibl->setIntensity(fallback_environment_light_intensity_); + scene_view_->AddToScene(ibl); + } + const float intensity = fallback_scene_light_intensity_ / lights_.size(); + for (auto& light : lights_) { + if (light) { + light->SetIntensity( + light->IsHeadlight() ? fallback_head_light_intensity_ : intensity); + } + } + } +} + +filament::math::mat4 CalculateClipFromWorld(const mjrRect& viewport, + const mjvGLCamera& cam) { + const float3 cam_pos(cam.pos[0], cam.pos[1], cam.pos[2]); + const float3 cam_fwd(cam.forward[0], cam.forward[1], cam.forward[2]); + const float3 cam_up(cam.up[0], cam.up[1], cam.up[2]); + const float3 cam_at = cam_pos + cam_fwd; + const float aspect_ratio = (float)viewport.width / (float)viewport.height; + const float halfwidth = + cam.frustum_width + ? cam.frustum_width + : 0.5f * aspect_ratio * (cam.frustum_top - cam.frustum_bottom); + const float left = cam.frustum_center - halfwidth; + const float right = cam.frustum_center + halfwidth; + + mat4 projection; + if (cam.orthographic) { + projection = mat4::ortho(left, right, cam.frustum_bottom, cam.frustum_top, + cam.frustum_near, cam.frustum_far); + } else { + projection = mat4::frustum(left, right, cam.frustum_bottom, cam.frustum_top, + cam.frustum_near, cam.frustum_far); + projection[2][2] = -1.0f; + projection[3][2] = -2.0f * cam.frustum_near; + } + mat4 look_at = mat4::lookAt(cam_pos, cam_at, cam_up); + return projection * inverse(look_at); +} + +void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { + filament::View* view = scene_view_->GetDefaultRenderView(); + view->setShadowingEnabled(scene->flags[mjRND_SHADOW] ? true : false); + + mjtNum hpos[3], hfwd[3]; + float headpos[3], gazedir[3]; + mjv_cameraInModel(hpos, hfwd, nullptr, scene); + mju_n2f(headpos, hpos, 3); + mju_n2f(gazedir, hfwd, 3); + + const mjvGLCamera gl_camera = + mjv_averageCamera(scene->camera, scene->camera + 1); + clip_from_world_ = CalculateClipFromWorld(viewport, gl_camera); + + // Remove all drawables from previous render and prepare new ones. + for (auto& iter : drawables_) { + scene_view_->RemoveFromScene(iter.get()); + } + drawables_.clear(); + for (int i = 0; i < scene->ngeom; ++i) { + const mjvGeom* geom = scene->geoms + i; + + if (geom->label[0] != 0) { + if (auto pos = ClipFromWorld(ReadFloat3(geom->pos))) { + DrawTextAt(geom->label, pos->x, pos->y, pos->z); + } + } + + auto drawable = + std::make_unique(object_mgr_, model_objects_.get(), *geom); + drawable->Update(model_objects_->GetModel(), scene, *geom); + scene_view_->AddToScene(drawable.get()); + drawables_.push_back(std::move(drawable)); + } + + bool headlight_enabled = false; + for (int i = 0; i < scene->nlight; ++i) { + const mjvLight& scene_light = scene->lights[i]; + if (scene_light.id < 0 && scene_light.headlight) { + // We position the headlight slightly behind the camera to avoid some + // odd clipping issues. + headlight_enabled = true; + headpos[0] -= gazedir[0] * 0.05f; + headpos[1] -= gazedir[1] * 0.05f; + headpos[2] -= gazedir[2] * 0.05f; + + // The headlight is always the "back" light. + std::unique_ptr& light = lights_.back(); + light->SetColor(ReadFloat3(scene_light.diffuse)); + light->SetTransform(ReadFloat3(headpos), ReadFloat3(gazedir)); + continue; + } else if (scene_light.id < lights_.size() - 1) { + std::unique_ptr& light = lights_[scene_light.id]; + if (light) { + light->SetColor(ReadFloat3(scene_light.diffuse)); + light->SetTransform(ReadFloat3(scene_light.pos), + ReadFloat3(scene_light.dir)); + } + } else { + mju_error("Unexpected light id: %d", scene_light.id); + } + } + + // Enable/disable the headlight based on whether or not it's in the scene. + if (headlight_enabled) { + lights_.back()->Enable(); + } else { + lights_.back()->Disable(); + } +} + +void SceneBridge::UploadMesh(const mjModel* model, int id) { + model_objects_->UploadMesh(model, id); +} + +void SceneBridge::UploadTexture(const mjModel* model, int id) { + model_objects_->UploadTexture(model, id); +} + +void SceneBridge::UploadHeightField(const mjModel* model, int id) { + model_objects_->UploadHeightField(model, id); +} +} // namespace mujoco diff --git a/src/experimental/filament/filament/scene_bridge.h b/src/experimental/filament/filament/scene_bridge.h new file mode 100644 index 00000000..a1e56796 --- /dev/null +++ b/src/experimental/filament/filament/scene_bridge.h @@ -0,0 +1,85 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_SCENE_BRIDGE_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_SCENE_BRIDGE_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include "experimental/filament/filament/drawable.h" +#include "experimental/filament/filament/light.h" +#include "experimental/filament/filament/model_objects.h" +#include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/filament/scene_view.h" + +namespace mujoco { + +// Manages all mjModel data and updates a SceneView using an mjvScene. +class SceneBridge { + public: + SceneBridge(ObjectManager* object_mgr, const mjModel* model, + SceneView* scene_view); + ~SceneBridge(); + + // Updates the environment light using the KTX image at the given path. + void SetEnvironmentLight(std::string_view filename, float intensity); + + // Updates the environment light to the fallback light + void SetFallbackEnvironmentLight(float intensity); + + // Updates the Entities in the filament Scene to match the current mjvScene + // state. + void Update(const mjrRect& viewport, const mjvScene* scene); + + // Creates the filament objects from the mjModel. + void UploadMesh(const mjModel* model, int id); + void UploadTexture(const mjModel* model, int id); + void UploadHeightField(const mjModel* model, int id); + + SceneView* GetSceneView() const { return scene_view_; } + + SceneBridge(const SceneBridge&) = delete; + SceneBridge& operator=(const SceneBridge&) = delete; + + private: + void PrepareLights(); + + // Converts a point in world space to clip space, eg. in the range [-1,-1, 0] + // to [1, 1, 1]. Returns std::nullopt if the point is behind the camera. + std::optional ClipFromWorld( + const filament::math::float3& pos) const; + + SceneView* scene_view_ = nullptr; + ObjectManager* object_mgr_ = nullptr; + std::unique_ptr model_objects_; + std::vector> lights_; + std::vector> drawables_; + filament::math::mat4 clip_from_world_; + int default_shadow_map_size_ = 2048; + float default_vsm_blur_width_ = 0.0f; + float fallback_head_light_intensity_ = 0.f; + float fallback_scene_light_intensity_ = 80'000.f; + float fallback_environment_light_intensity_ = 5'000.f; +}; + +} // namespace mujoco + +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_SCENE_BRIDGE_H_ diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 4f6f1ceb..3cd7f929 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -14,12 +14,10 @@ #include "experimental/filament/filament/scene_view.h" +#include #include #include #include -#include -#include -#include #include #include @@ -43,12 +41,9 @@ #include #include "experimental/filament/filament/color_grading_options.h" #include "experimental/filament/filament/drawable.h" -#include "experimental/filament/filament/gui_view.h" #include "experimental/filament/filament/light.h" +#include "experimental/filament/filament/material.h" #include "experimental/filament/filament/math_util.h" -#include "experimental/filament/filament/model_objects.h" -#include "experimental/filament/filament/model_util.h" -#include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/render_target.h" #include "experimental/filament/filament/texture.h" @@ -56,21 +51,17 @@ namespace mujoco { using filament::math::float3; using filament::math::float4; -using filament::math::mat3; using filament::math::mat4; static constexpr int kNormalIndex = - static_cast(SceneView::DrawMode::kNormal); + static_cast(Material::DrawMode::kNormal); static constexpr int kDepthIndex = - static_cast(SceneView::DrawMode::kDepth); + static_cast(Material::DrawMode::kDepth); static constexpr int kSegmentIndex = - static_cast(SceneView::DrawMode::kSegmentation); + static_cast(Material::DrawMode::kSegmentation); -static filament::Viewport ReadViewport(mjrRect rect) { - return filament::Viewport(rect.left, rect.bottom, rect.width, rect.height); -} - -filament::ColorGrading::Builder ToBuilder(const ColorGradingOptions& opts) { +static filament::ColorGrading::Builder ToBuilder( + const ColorGradingOptions& opts) { return filament::ColorGrading::Builder() .format(opts.format) .dimensions(opts.dimension) @@ -89,6 +80,27 @@ filament::ColorGrading::Builder ToBuilder(const ColorGradingOptions& opts) { .curves(opts.shadow_gamma, opts.mid_point, opts.highlight_scale); } +static void SetupCamera(const mjvGLCamera& cam, + const filament::Viewport& viewport, + filament::Camera* camera) { + const filament::Camera::Projection type = + cam.orthographic ? filament::Camera::Projection::ORTHO + : filament::Camera::Projection::PERSPECTIVE; + const float3 cam_pos(cam.pos[0], cam.pos[1], cam.pos[2]); + const float3 cam_fwd(cam.forward[0], cam.forward[1], cam.forward[2]); + const float3 cam_up(cam.up[0], cam.up[1], cam.up[2]); + const float3 cam_at = cam_pos + cam_fwd; + const float aspect_ratio = (float)viewport.width / (float)viewport.height; + const float halfwidth = + cam.frustum_width + ? cam.frustum_width + : 0.5f * aspect_ratio * (cam.frustum_top - cam.frustum_bottom); + camera->lookAt(cam_pos, cam_at, cam_up); + camera->setProjection(type, cam.frustum_center - halfwidth, + cam.frustum_center + halfwidth, cam.frustum_bottom, + cam.frustum_top, cam.frustum_near, cam.frustum_far); +} + // Sets up the `reflection_camera`'s projection matrix so that it is a // reflection of the `src_camera` across the plane defined by the // `surface_xform`. The generated projection is an oblique projection so that @@ -115,11 +127,7 @@ static void SetupReflectionCamera(const mat4& surface_xform, reflection_camera->setCustomProjection(oblique, near, far); } -SceneView::SceneView(ObjectManager* object_mgr, const mjModel* model) - : object_mgr_(object_mgr) { - filament::Engine* engine = object_mgr_->GetEngine(); - model_objects_ = std::make_unique(model, engine); - +SceneView::SceneView(filament::Engine* engine) : engine_(engine) { scene_ = engine->createScene(); camera_ = engine->createCamera(utils::EntityManager::get().create()); reflect_camera_ = engine->createCamera(utils::EntityManager::get().create()); @@ -136,53 +144,6 @@ SceneView::SceneView(ObjectManager* object_mgr, const mjModel* model) reflect_view_->setShadowingEnabled(false); reflect_view_->setPostProcessingEnabled(false); - // Configure options for the normal view. - auto& cg = color_grading_options_; - cg.exposure = ReadElement(model, "filament.out.exposure", cg.exposure); - cg.contrast = ReadElement(model, "filament.out.contrast", cg.contrast); - cg.vibrance = ReadElement(model, "filament.out.vibrance", cg.vibrance); - cg.saturation = ReadElement(model, "filament.out.saturation", cg.saturation); - cg.temperature = ReadElement(model, "filament.out.temperature", cg.temperature); - cg.tint = ReadElement(model, "filament.out.tint", cg.tint); - - auto tone_mapping = - ReadElement(model, "filament.out.tone_mapping"); - if (tone_mapping == "aces") { - cg.tone_mapper = ToneMapperType::kACES; - } else if (tone_mapping == "aces_legacy") { - cg.tone_mapper = ToneMapperType::kACESLegacy; - } else if (tone_mapping == "filmic") { - cg.tone_mapper = ToneMapperType::kFilmic; - } else if (tone_mapping == "linear") { - cg.tone_mapper = ToneMapperType::kLinear; - } else if (tone_mapping == "pbr_neutral") { - cg.tone_mapper = ToneMapperType::kPBRNeutral; - } - SetColorGradingOptions(cg); - - auto ao = views_[kNormalIndex]->getAmbientOcclusionOptions(); - ao.enabled = ReadElement(model, "filament.ao.enabled", true); - ao.bentNormals = ReadElement(model, "filament.ao.bent_normals", false); - ao.ssct.enabled = ReadElement(model, "filament.ao.ssct", ao.ssct.enabled); - ao.quality = filament::QualityLevel::ULTRA; - ao.lowPassFilter = filament::QualityLevel::ULTRA; - ao.upsampling = filament::QualityLevel::ULTRA; - ao.bilateralThreshold = 0.5f; - views_[kNormalIndex]->setAmbientOcclusionOptions(ao); - - auto msaa = views_[kNormalIndex]->getMultiSampleAntiAliasingOptions(); - msaa.enabled = ReadElement(model, "filament.msaa.enabled", true); - views_[kNormalIndex]->setMultiSampleAntiAliasingOptions(msaa); - - default_shadow_map_size_ = ReadElement( - model, "filament.shadows.map_size", default_shadow_map_size_); - default_vsm_blur_width_ = ReadElement( - model, "filament.shadows.vsm_blur_width", default_vsm_blur_width_); - - auto shadow_type = views_[kNormalIndex]->getShadowType(); - shadow_type = ReadElement(model, "filament.shadows.type", shadow_type); - views_[kNormalIndex]->setShadowType(shadow_type); - // Disable post processing for the depth and segmentation views to preserve // the values. views_[kDepthIndex]->setPostProcessingEnabled(false); @@ -192,79 +153,115 @@ SceneView::SceneView(ObjectManager* object_mgr, const mjModel* model) auto fog = views_[kNormalIndex]->getFogEntity(); auto& tm = engine->getTransformManager(); tm.create(fog); - auto rotation_axis = ReadElement( - model, "filament.fog.rotation_axis", float3{-1, 0, 0}); tm.setTransform(tm.getInstance(fog), - mat4::rotation(filament::math::f::PI / 2, rotation_axis)); - - auto fog_opts = views_[kNormalIndex]->getFogOptions(); - fog_opts.enabled = - ReadElement(model, "filament.fog.enabled", fog_opts.enabled); - fog_opts.color = ReadElement(model, "filament.fog.color", fog_opts.color); - fog_opts.distance = ReadElement( - model, "filament.fog.distance", fog_opts.distance); - fog_opts.density = ReadElement( - model, "filament.fog.density", fog_opts.density); - fog_opts.cutOffDistance = ReadElement( - model, "filament.fog.cutOffDistance", fog_opts.cutOffDistance); - fog_opts.maximumOpacity = ReadElement( - model, "filament.fog.maximumOpacity", fog_opts.maximumOpacity); - fog_opts.height = ReadElement(model, "filament.fog.height", fog_opts.height); - fog_opts.heightFalloff = ReadElement( - model, "filament.fog.heightFalloff", fog_opts.heightFalloff); - fog_opts.inScatteringStart = ReadElement( - model, "filament.fog.inScatteringStart", fog_opts.inScatteringStart); - fog_opts.inScatteringSize = ReadElement( - model, "filament.fog.inScatteringSize", fog_opts.inScatteringSize); - views_[kNormalIndex]->setFogOptions(fog_opts); - - fallback_head_light_intensity_ = - ReadElement(model, "filament.fallback.head_light_intensity", - fallback_head_light_intensity_); - fallback_scene_light_intensity_ = - ReadElement(model, "filament.fallback.scene_light_intensity", - fallback_scene_light_intensity_); - fallback_environment_light_intensity_ = - ReadElement(model, "filament.fallback.environment_light_intensity", - fallback_environment_light_intensity_); - - // Create an empty/black indirect light to ensure that the skybox is oriented - // to respect mujoco's Z-up convention. - scene_->setIndirectLight(model_objects_->CreateIndirectLight(-1, 100000)); - - PrepareLights(); + mat4::rotation(filament::math::f::PI / 2, float3{-1, 0, 0})); } SceneView::~SceneView() { + for (auto& light : lights_) { + light->RemoveFromScene(scene_); + } + for (auto& drawable : drawables_) { + drawable->RemoveFromScene(scene_); + } lights_.clear(); drawables_.clear(); reflect_targets_.clear(); - - filament::Engine* engine = object_mgr_->GetEngine(); - engine->destroyCameraComponent(reflect_camera_->getEntity()); - engine->destroy(reflect_view_); - - engine->destroyCameraComponent(camera_->getEntity()); - engine->destroy(views_[kNormalIndex]->getColorGrading()); - for (auto& view : views_) { - engine->destroy(view); + engine_->destroyCameraComponent(reflect_camera_->getEntity()); + engine_->destroy(reflect_view_); + engine_->destroyCameraComponent(camera_->getEntity()); + if (color_grading_) { + engine_->destroy(color_grading_); + } + engine_->destroy(scene_); + for (auto& view : views_) { + engine_->destroy(view); } - engine->destroy(scene_); } -void SceneView::Render(filament::Renderer* renderer, DrawMode draw_mode, - RenderTarget* target) { - filament::View* view = PrepareRenderView(draw_mode); +void SceneView::AddToScene(Light* light) { + if (lights_.insert(light).second) { + light->AddToScene(scene_); + } +} + +void SceneView::RemoveFromScene(Light* light) { + if (lights_.erase(light)) { + light->RemoveFromScene(scene_); + } +} + +void SceneView::AddToScene(Drawable* drawable) { + if (drawables_.insert(drawable).second) { + drawable->AddToScene(scene_); + if (drawable->IsReflective()) { + AddReflectiveDrawable(drawable); + } + } +} + +void SceneView::RemoveFromScene(Drawable* drawable) { + if (drawables_.erase(drawable)) { + auto it = std::find(reflectives_.begin(), reflectives_.end(), drawable); + if (it != reflectives_.end()) { + reflectives_.erase(it); + } + drawable->RemoveFromScene(scene_); + } +} + +void SceneView::AddToScene(filament::Skybox* skybox) { + skybox_ = skybox; + scene_->setSkybox(skybox); +} + +void SceneView::RemoveFromScene(filament::Skybox* skybox) { + if (skybox_ == skybox) { + skybox_ = nullptr; + scene_->setSkybox(nullptr); + } +} + +void SceneView::AddToScene(filament::IndirectLight* indirect_light) { + indirect_light_ = indirect_light; + scene_->setIndirectLight(indirect_light); +} + +void SceneView::RemoveFromScene(filament::IndirectLight* indirect_light) { + if (indirect_light_ == indirect_light) { + indirect_light_ = nullptr; + scene_->setIndirectLight(nullptr); + } +} + +void SceneView::Render(filament::Renderer* renderer, + const RenderRequest& request) { + filament::Viewport viewport(request.viewport.left, request.viewport.bottom, + request.viewport.width, request.viewport.height); + for (auto& view : views_) { + view->setViewport(viewport); + } + reflect_view_->setViewport(viewport); + + SetupCamera(request.camera, viewport, camera_); + + for (auto& iter : drawables_) { + iter->SetDrawMode(request.draw_mode); + } + + filament::View* view = views_[static_cast(request.draw_mode)]; filament::MultiSampleAntiAliasingOptions options = view->getMultiSampleAntiAliasingOptions(); - if (target) { + filament::RenderTarget* render_target = + request.target ? request.target->GetFilamentRenderTarget() : nullptr; + if (render_target) { // We need to disable msaa in order to render to texture. view->setMultiSampleAntiAliasingOptions({.enabled = false}); } // Render reflection passes. - if (draw_mode == DrawMode::kNormal) { + if (request.draw_mode == DrawMode::kNormal) { for (size_t i = 0; i < reflectives_.size(); ++i) { Drawable* drawable = reflectives_[i]; @@ -283,242 +280,24 @@ void SceneView::Render(filament::Renderer* renderer, DrawMode draw_mode, } } - view->setRenderTarget(target ? target->GetFilamentRenderTarget() : nullptr); + view->setRenderTarget(render_target); renderer->render(view); view->setRenderTarget(nullptr); - if (target) { + if (request.target) { view->setMultiSampleAntiAliasingOptions(options); } } -filament::View* SceneView::PrepareRenderView(DrawMode mode) { - for (auto& iter : drawables_) { - iter->SetDrawMode(mode); - } - return views_[static_cast(mode)]; -} - -void SceneView::SetViewport(mjrRect viewport) { - auto filament_viewport = ReadViewport(viewport); - aspect_ratio_ = (float)viewport.width / (float)viewport.height; - for (auto& view : views_) { - view->setViewport(filament_viewport); - } - reflect_view_->setViewport(filament_viewport); -} - -void SceneView::SetColorGradingOptions(const ColorGradingOptions& opts) { - filament::Engine* engine = object_mgr_->GetEngine(); - - auto tone_mapper = CreateToneMapper(opts.tone_mapper); - auto color_grading = ToBuilder(color_grading_options_) - .toneMapper(tone_mapper.get()) - .build(*engine); - views_[kNormalIndex]->setColorGrading(color_grading); - engine->destroy(color_grading_); - color_grading_ = color_grading; - color_grading_options_ = opts; -} - -void SceneView::SetEnvironmentLight(std::string_view filename, - float intensity) { - scene_->setIndirectLight(nullptr); - object_mgr_->LoadFallbackIndirectLight(filename, intensity); - scene_->setIndirectLight(object_mgr_->GetFallbackIndirectLight()); -} - -void SceneView::SetFallbackEnvironmentLight(float intensity) { - auto* ibl = object_mgr_->GetFallbackIndirectLight(); - if (ibl) { - ibl->setIntensity(intensity); - scene_->setIndirectLight(ibl); - } -} - -void SceneView::UpdateCamera(const mjvGLCamera* cameras) { - const mjvGLCamera cam = mjv_averageCamera(cameras, cameras + 1); - const filament::Camera::Projection type = - cam.orthographic ? filament::Camera::Projection::ORTHO - : filament::Camera::Projection::PERSPECTIVE; - float3 cam_pos(cam.pos[0], cam.pos[1], cam.pos[2]); - float3 cam_fwd(cam.forward[0], cam.forward[1], cam.forward[2]); - float3 cam_up(cam.up[0], cam.up[1], cam.up[2]); - float3 cam_at = cam_pos + cam_fwd; - camera_->lookAt(cam_pos, cam_at, cam_up); - float halfwidth = cam.frustum_width ? cam.frustum_width - : 0.5f * aspect_ratio_ * (cam.frustum_top - cam.frustum_bottom); - camera_->setProjection(type, cam.frustum_center - halfwidth, - cam.frustum_center + halfwidth, cam.frustum_bottom, - cam.frustum_top, cam.frustum_near, cam.frustum_far); - clip_from_world_ = camera_->getProjectionMatrix() * camera_->getViewMatrix(); -} - -std::optional SceneView::ClipFromWorld(const float3& pos) const{ - const float4 clip_pos = clip_from_world_ * float4(pos, 1.0f); - if (clip_pos.w == 0.0f) { - return std::nullopt; - } - return clip_pos.xyz / clip_pos.w; -} - -void SceneView::PrepareLights() { - filament::Engine* engine = object_mgr_->GetEngine(); - const mjModel* model = model_objects_->GetModel(); - filament::Skybox* skybox = model_objects_->CreateSkybox(); - if (skybox) { - scene_->setSkybox(skybox); - } - - float total_light_intensity = 0.0f; - - for (int i = 0; i < model->nlight; ++i) { - total_light_intensity += model->light_intensity[i]; - - if (model->light_type[i] == mjLIGHT_IMAGE) { - auto* indirect_light = model_objects_->CreateIndirectLight( - model->light_texid[i], model->light_intensity[i]); - if (indirect_light) { - scene_->setIndirectLight(indirect_light); - } - // Add an nullptr as a placeholder so that our indices still match. - lights_.emplace_back(nullptr); - } else { - Light::Params params; - params.color = ReadFloat3(model->light_diffuse); - params.type = (mjtLightType)model->light_type[i]; - params.castshadow = model->light_castshadow[i]; - params.bulbradius = model->light_bulbradius[i]; - params.range = model->light_range[i]; - params.intensity = model->light_intensity[i]; - params.shadow_map_size = default_shadow_map_size_; - params.vsm_blur_width = default_vsm_blur_width_; - if (params.type == mjLIGHT_SPOT) { - params.spot_cone_angle = model->light_cutoff[i]; - } - - auto light_obj = std::make_unique(engine, params); -#ifndef __EMSCRIPTEN__ - // TODO(b/458045799): Re-enable when lights work on glinux and chromebook. - light_obj->AddToScene(scene_); -#endif - lights_.emplace_back(std::move(light_obj)); - } - } - - // Add a placeholder (black) headlight as our last light. Going forward, we'll - // assume lights_.back() is always the headlight. - { - Light::Params params; - params.color = float3(0, 0, 0); - params.headlight = true; - params.type = mjLIGHT_DIRECTIONAL; - params.castshadow = 0; - params.intensity = 0; - auto light_obj = std::make_unique(engine, params); -#ifndef __EMSCRIPTEN__ - // TODO(b/458045799): Re-enable when lights work on glinux and chromebook. - light_obj->AddToScene(scene_); -#endif - lights_.emplace_back(std::move(light_obj)); - } - - // There are no "physical" lights in the scene which means we're likely - // dealing with a "classic renderer" scene. In this case, let's add a - // default environment light and set the light intensity ourselves. - if (total_light_intensity == 0.0f) { - SetFallbackEnvironmentLight(fallback_environment_light_intensity_); - const float intensity = fallback_scene_light_intensity_ / lights_.size(); - for (auto& light : lights_) { - if (light) { - light->SetIntensity( - light->IsHeadlight() ? fallback_head_light_intensity_ : intensity); - } - } - } -} - -void SceneView::UpdateScene(const mjvScene* scene) { - views_[kNormalIndex]->setShadowingEnabled(scene->flags[mjRND_SHADOW]); - - mjtNum hpos[3], hfwd[3]; - float headpos[3], gazedir[3]; - mjv_cameraInModel(hpos, hfwd, nullptr, scene); - mju_n2f(headpos, hpos, 3); - mju_n2f(gazedir, hfwd, 3); - UpdateCamera(scene->camera); - - // Remove all drawables from previous render and prepare new ones. - for (auto& iter : drawables_) { - iter->RemoveFromScene(scene_); - } - drawables_.clear(); - reflectives_.clear(); - for (int i = 0; i < scene->ngeom; ++i) { - const mjvGeom* geom = scene->geoms + i; - - if (geom->label[0] != 0) { - if (auto pos = ClipFromWorld(ReadFloat3(geom->pos))) { - DrawTextAt(geom->label, pos->x, pos->y, pos->z); - } - } - - auto drawable = - std::make_unique(object_mgr_, model_objects_.get(), *geom); - drawable->AddToScene(scene_); - drawable->Update(model_objects_->GetModel(), scene, *geom); - if (drawable->IsReflective()) { - AddReflectiveDrawable(drawable.get()); - } - drawables_.push_back(std::move(drawable)); - } - - bool headlight_enabled = false; - for (int i = 0; i < scene->nlight; ++i) { - const mjvLight& scene_light = scene->lights[i]; - if (scene_light.id < 0 && scene_light.headlight) { - // We position the headlight slightly behind the camera to avoid some - // odd clipping issues. - headlight_enabled = true; - headpos[0] -= gazedir[0] * 0.05f; - headpos[1] -= gazedir[1] * 0.05f; - headpos[2] -= gazedir[2] * 0.05f; - - // The headlight is always the "back" light. - std::unique_ptr& light = lights_.back(); - light->SetColor(ReadFloat3(scene_light.diffuse)); - light->SetTransform(ReadFloat3(headpos), ReadFloat3(gazedir)); - continue; - } else if (scene_light.id < lights_.size() - 1) { - std::unique_ptr& light = lights_[scene_light.id]; - if (light) { - light->SetColor(ReadFloat3(scene_light.diffuse)); - light->SetTransform(ReadFloat3(scene_light.pos), - ReadFloat3(scene_light.dir)); - } - } else { - mju_error("Unexpected light id: %d", scene_light.id); - } - } - - // Enable/disable the headlight based on whether or not it's in the scene. - if (headlight_enabled) { - lights_.back()->Enable(); - } else { - lights_.back()->Disable(); - } -} - void SceneView::AddReflectiveDrawable(Drawable* drawable) { const int index = reflectives_.size(); reflectives_.push_back(drawable); // Ensure we have the same number of render targets as we do reflective // drawables. - filament::Engine* engine = object_mgr_->GetEngine(); while (reflect_targets_.size() < reflectives_.size()) { reflect_targets_.push_back(std::make_unique( - engine, RenderTargetTextureType::kReflectionColor, + engine_, RenderTargetTextureType::kReflectionColor, RenderTargetTextureType::kDepth)); } @@ -529,20 +308,17 @@ void SceneView::AddReflectiveDrawable(Drawable* drawable) { drawable->UpdateReflectionTexture(target->GetColorTexture()); } -void SceneView::UploadMesh(const mjModel* model, int id) { - model_objects_->UploadMesh(model, id); -} - -void SceneView::UploadTexture(const mjModel* model, int id) { - model_objects_->UploadTexture(model, id); -} - -void SceneView::UploadHeightField(const mjModel* model, int id) { - model_objects_->UploadHeightField(model, id); -} - -filament::Engine* SceneView::GetEngine() const { - return object_mgr_->GetEngine(); +void SceneView::SetColorGradingOptions(const ColorGradingOptions& opts) { + auto tone_mapper = CreateToneMapper(opts.tone_mapper); + auto color_grading = ToBuilder(color_grading_options_) + .toneMapper(tone_mapper.get()) + .build(*engine_); + views_[kNormalIndex]->setColorGrading(color_grading); + if (color_grading_) { + engine_->destroy(color_grading_); + } + color_grading_ = color_grading; + color_grading_options_ = opts; } filament::View* SceneView::GetDefaultRenderView() { diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index 8fb686e1..a7d7c3aa 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -17,120 +17,100 @@ #include #include -#include -#include +#include #include #include #include #include -#include #include #include -#include -#include -#include -#include #include #include "experimental/filament/filament/color_grading_options.h" #include "experimental/filament/filament/drawable.h" #include "experimental/filament/filament/light.h" #include "experimental/filament/filament/material.h" -#include "experimental/filament/filament/model_objects.h" -#include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/render_target.h" namespace mujoco { -// Creates and owns filament Scene and View classes given a mjvScene. +// Creates and owns the filament Scene and View (and Camera) classes. // -// The filament Scene is populated with the objects (e.g. lights, geoms, -// cameras, etc.) defined by the mjvScene. Multiple Views are created to allow -// different rendering modes (e.g. normal, depth, segmentation, etc.) +// The filament Scene is populated with the objects (e.g. lights, renderables, +// skybox, etc.). It manages multiple views to support a variety of draw modes +// (e.g. normal, depth, segmentation, etc.) as well as reflective surfaces. class SceneView { public: - SceneView(ObjectManager* object_mgr, const mjModel* model); + SceneView(filament::Engine* engine); ~SceneView(); - // Updates all views to render into the given viewport. - void SetViewport(mjrRect viewport); - - // Updates the color grading options for the main render view. - void SetColorGradingOptions(const ColorGradingOptions& opts); - - // Updates the environment light using the KTX image at the given path. - void SetEnvironmentLight(std::string_view filename, float intensity); - - // Updates the environment light to the fallback light - void SetFallbackEnvironmentLight(float intensity); - - // Updates the Entities in the filament Scene to match the current mjvScene - // state. - void UpdateScene(const mjvScene* scene); + // Adds/removes entities from the scene. + void AddToScene(Light* light); + void RemoveFromScene(Light* light); + void AddToScene(Drawable* drawable); + void RemoveFromScene(Drawable* drawable); + void AddToScene(filament::Skybox* skybox); + void RemoveFromScene(filament::Skybox* skybox); + void AddToScene(filament::IndirectLight* indirect_light); + void RemoveFromScene(filament::IndirectLight* indirect_light); + // Parameters for rendering the scene. using DrawMode = Material::DrawMode; + struct RenderRequest { + // The draw mode (e.g. normal, depth, segmentation) to render. + DrawMode draw_mode = DrawMode::kNormal; + // The target viewport for the rendered image. + mjrRect viewport; + // The camera from which to render the scene. + mjvGLCamera camera; + // An optional render target into which the scene will be rendered. + RenderTarget* target = nullptr; + }; - void Render(filament::Renderer* renderer, DrawMode draw_mode, - RenderTarget* target = nullptr); + // Renders the scene. + void Render(filament::Renderer* renderer, const RenderRequest& request); - void UploadMesh(const mjModel* model, int id); - void UploadTexture(const mjModel* model, int id); - void UploadHeightField(const mjModel* model, int id); + // Returns the filament Engine managing the scene. + filament::Engine* GetEngine() const { return engine_; } - // Accessors. - filament::Engine* GetEngine() const; + // Returns the underlying filament View that is used for normal rendering. + // Callers can update rendering settings (e.g. post processing) directly. filament::View* GetDefaultRenderView(); + + // Helpers for managing the color grading options for the default render view. ColorGradingOptions GetColorGradingOptions() const; + void SetColorGradingOptions(const ColorGradingOptions& opts); SceneView(const SceneView&) = delete; SceneView& operator=(const SceneView&) = delete; private: - // Prepares and returns the filament View for the given draw mode. - filament::View* PrepareRenderView(DrawMode mode); - - void UpdateCamera(const mjvGLCamera* cameras); - - void PrepareLights(); - - // Registers the given drawable as a reflective surface. + // Marks a drawable as reflective. Reflective drawables have to be rendered + // in their own passes to create the reflective texture. void AddReflectiveDrawable(Drawable* drawable); - // Converts a point in world space to clip space, eg. in the range [-1,-1, 0] - // to [1, 1, 1]. Returns std::nullopt if the point is behind the camera. - std::optional ClipFromWorld( - const filament::math::float3& pos) const; - - ObjectManager* object_mgr_ = nullptr; + filament::Engine* engine_ = nullptr; filament::Scene* scene_ = nullptr; filament::Camera* camera_ = nullptr; filament::ColorGrading* color_grading_ = nullptr; - std::vector> lights_; - std::vector> drawables_; - std::unique_ptr model_objects_; - std::array views_; - filament::math::mat4 clip_from_world_; ColorGradingOptions color_grading_options_; + std::array views_; DrawMode active_mode_ = DrawMode::kNumDrawModes; - float aspect_ratio_ = 1.0f; - int default_shadow_map_size_ = 2048; - float default_vsm_blur_width_ = 0.0f; - float fallback_head_light_intensity_ = 0.f; - float fallback_scene_light_intensity_ = 80'000.f; - float fallback_environment_light_intensity_ = 5'000.f; + + // Scene objects. + std::unordered_set lights_; + std::unordered_set drawables_; + filament::Skybox* skybox_ = nullptr; + filament::IndirectLight* indirect_light_ = nullptr; // Custom view and camera for reflective surfaces. filament::View* reflect_view_ = nullptr; filament::Camera* reflect_camera_ = nullptr; - // The list of drawables that are reflective. + // The list of reflective drawables and their corresponding render targets. std::vector reflectives_; - - // Each reflective drawable has its own render target which is used to render - // the reflected image. std::vector> reflect_targets_; }; - } // namespace mujoco #endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_SCENE_VIEW_H_ From 1c310ca7be37098debdb7a7ca5503f19d4db4344 Mon Sep 17 00:00:00 2001 From: Tarik Kelestemur Date: Thu, 9 Apr 2026 08:14:15 -0400 Subject: [PATCH 032/251] keep render util test setup Restore the existing Warp/CUDA-gated render_util test setup so the review change stays minimal while preserving the new batched shape coverage. Made-with: Cursor --- mjx/mujoco/mjx/_src/render_util_test.py | 54 +++++++++++++++++-------- 1 file changed, 37 insertions(+), 17 deletions(-) diff --git a/mjx/mujoco/mjx/_src/render_util_test.py b/mjx/mujoco/mjx/_src/render_util_test.py index b09ed7a3..f97c1e0a 100644 --- a/mjx/mujoco/mjx/_src/render_util_test.py +++ b/mjx/mujoco/mjx/_src/render_util_test.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # ============================================================================== -import contextlib +import os from unittest import mock from absl.testing import absltest @@ -20,9 +20,13 @@ import jax import jax.numpy as jnp import numpy as np +from mujoco.mjx._src import io from mujoco.mjx._src import render_util +import mujoco.mjx.warp as mjxw from mujoco.mjx.warp.render_context import RenderContextPytree +_FORCE_TEST = os.environ.get('MJX_WARP_FORCE_TEST', '0') == '1' + def _fake_render_context(ncam, width, height): """Fake RenderContext for testing.""" @@ -36,25 +40,26 @@ def _fake_render_context(ncam, width, height): return rc -@contextlib.contextmanager -def _mock_render_runtime(warp_rc): - with mock.patch.object(render_util.mjxw, 'WARP_INSTALLED', True): - with mock.patch.dict( - 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', - {(0, None): warp_rc}, - ): - yield - - class RenderUtilTest(absltest.TestCase): + def setUp(self): + super().setUp() + if not _FORCE_TEST: + if not mjxw.WARP_INSTALLED: + self.skipTest('Warp not installed.') + if not io.has_cuda_gpu_device(): + self.skipTest('No CUDA GPU device available.') + def test_get_rgb(self): width, height = 4, 4 warp_rc = _fake_render_context(1, width, height) rc = mock.MagicMock(spec=RenderContextPytree, key=0) rgb_data = jnp.zeros((width * height,), dtype=jnp.uint32) - with _mock_render_runtime(warp_rc): + with mock.patch.dict( + 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', + {(0, None): warp_rc}, + ): rgb = jax.jit(render_util.get_rgb, static_argnums=(0, 1))(rc, 0, rgb_data) self.assertEqual(rgb.shape, (height, width, 3)) @@ -64,7 +69,10 @@ class RenderUtilTest(absltest.TestCase): warp_rc = _fake_render_context(1, width, height) rc = mock.MagicMock(spec=RenderContextPytree, key=0) - with _mock_render_runtime(warp_rc): + with mock.patch.dict( + 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', + {(0, None): warp_rc}, + ): for leading_shape in ((1,), (3,), (2, 3)): with self.subTest(leading_shape=leading_shape): rgb_data = jnp.zeros( @@ -82,7 +90,10 @@ class RenderUtilTest(absltest.TestCase): rc = mock.MagicMock(spec=RenderContextPytree, key=0) rgb_data = jnp.zeros((nworld, width * height), dtype=jnp.uint32) - with _mock_render_runtime(warp_rc): + with mock.patch.dict( + 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', + {(0, None): warp_rc}, + ): rgb = jax.jit( jax.vmap(render_util.get_rgb, in_axes=(None, None, 0)), static_argnums=(0, 1), @@ -96,7 +107,10 @@ class RenderUtilTest(absltest.TestCase): rc = mock.MagicMock(spec=RenderContextPytree, key=0) depth_data = jnp.zeros((width * height,), dtype=jnp.float32) - with _mock_render_runtime(warp_rc): + with mock.patch.dict( + 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', + {(0, None): warp_rc}, + ): depth = jax.jit(render_util.get_depth, static_argnums=(0, 1, 3))( rc, 0, depth_data, 5.0 ) @@ -108,7 +122,10 @@ class RenderUtilTest(absltest.TestCase): warp_rc = _fake_render_context(1, width, height) rc = mock.MagicMock(spec=RenderContextPytree, key=0) - with _mock_render_runtime(warp_rc): + with mock.patch.dict( + 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', + {(0, None): warp_rc}, + ): for leading_shape in ((1,), (3,), (2, 3)): with self.subTest(leading_shape=leading_shape): depth_data = jnp.zeros( @@ -126,7 +143,10 @@ class RenderUtilTest(absltest.TestCase): rc = mock.MagicMock(spec=RenderContextPytree, key=0) depth_data = jnp.zeros((nworld, width * height), dtype=jnp.float32) - with _mock_render_runtime(warp_rc): + with mock.patch.dict( + 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', + {(0, None): warp_rc}, + ): depth = jax.jit( jax.vmap(render_util.get_depth, in_axes=(None, None, 0, None)), static_argnums=(0, 1, 3), From f6baacfa86a73a395c7689297778e2ed4a5f7250 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 9 Apr 2026 05:33:36 -0700 Subject: [PATCH 033/251] Remove ObjectManager dependency from Material. PiperOrigin-RevId: 897056556 Change-Id: Ic02239d7a411b52333b48a069292b21afb547b19 --- .../filament/filament/drawable.cc | 59 +++++++++++------- src/experimental/filament/filament/drawable.h | 6 +- .../filament/filament/material.cc | 62 +++++++++---------- src/experimental/filament/filament/material.h | 23 ++++--- .../filament/filament/scene_bridge.cc | 13 +++- .../filament/filament/scene_bridge.h | 2 + 6 files changed, 99 insertions(+), 66 deletions(-) diff --git a/src/experimental/filament/filament/drawable.cc b/src/experimental/filament/filament/drawable.cc index 53c91144..2db25b95 100644 --- a/src/experimental/filament/filament/drawable.cc +++ b/src/experimental/filament/filament/drawable.cc @@ -86,10 +86,20 @@ static bool IsBehind(const mjtNum* headpos, const float* pos, const float* mat) } Drawable::Drawable(ObjectManager* object_mgr, ModelObjects* model_objects, - const mjvGeom& geom) - : material_(object_mgr), + const mjvGeom& geom, + const Material::Textures* fallback_textures) + : material_(object_mgr->GetEngine()), model_objs_(model_objects), + object_mgr_(object_mgr), renderables_(object_mgr->GetEngine()) { + material_.SetMaterial( + Material::DrawMode::kDepth, + object_mgr_->GetMaterial(ObjectManager::kUnlitDepth)); + material_.SetMaterial( + Material::DrawMode::kSegmentation, + object_mgr_->GetMaterial(ObjectManager::kUnlitSegmentation)); + material_.SetFallbackTextures(fallback_textures); + if (geom.category == mjCAT_DECOR) { renderables_.SetCastShadows(false); renderables_.SetReceiveShadows(false); @@ -229,7 +239,9 @@ void Drawable::SetDrawMode(Material::DrawMode mode) { } void Drawable::UpdateReflectionTexture(const Texture* tex) { - material_.UpdateReflectionTexture(tex); + Material::Textures textures = material_.GetTextures(); + textures.reflection = tex; + material_.UpdateTextures(textures); } void Drawable::SetLayerMask(std::uint8_t mask) { @@ -353,6 +365,11 @@ void Drawable::SetTransform(const mjvGeom& geom) { } } +void Drawable::SetNormalMaterial(ObjectManager::MaterialType material_type) { + filament::Material* material = object_mgr_->GetMaterial(material_type); + material_.SetMaterial(Material::DrawMode::kNormal, material); +} + void Drawable::UpdateMaterial(const mjvGeom& geom, bool use_segid_color, bool enable_reflection, const mjtNum* headpos) { const mjModel* model = model_objs_->GetModel(); @@ -385,21 +402,21 @@ void Drawable::UpdateMaterial(const mjvGeom& geom, bool use_segid_color, } if (geom.type == mjGEOM_LINE || geom.type == mjGEOM_LINEBOX) { - material_.SetNormalMaterialType(ObjectManager::kUnlitLine); + SetNormalMaterial(ObjectManager::kUnlitLine); } else { bool material_assigned = false; if (geom.matid >= 0) { material_assigned = true; if (textures.orm) { - material_.SetNormalMaterialType(ObjectManager::kPbrPacked); + SetNormalMaterial(ObjectManager::kPbrPacked); } else if (textures.metallic) { - material_.SetNormalMaterialType(ObjectManager::kPbr); + SetNormalMaterial(ObjectManager::kPbr); } else if (textures.roughness) { - material_.SetNormalMaterialType(ObjectManager::kPbr); + SetNormalMaterial(ObjectManager::kPbr); } else if (model->mat_metallic[geom.matid] >= 0) { - material_.SetNormalMaterialType(ObjectManager::kPbr); + SetNormalMaterial(ObjectManager::kPbr); } else if (model->mat_roughness[geom.matid] >= 0) { - material_.SetNormalMaterialType(ObjectManager::kPbr); + SetNormalMaterial(ObjectManager::kPbr); } else { material_assigned = false; } @@ -418,36 +435,36 @@ void Drawable::UpdateMaterial(const mjvGeom& geom, bool use_segid_color, if (textures.color == nullptr) { if (color.a < 1.0f) { - material_.SetNormalMaterialType(ObjectManager::kPhongColorFade); + SetNormalMaterial(ObjectManager::kPhongColorFade); } else if (reflective_) { - material_.SetNormalMaterialType(ObjectManager::kPhongColorReflect); + SetNormalMaterial(ObjectManager::kPhongColorReflect); } else { - material_.SetNormalMaterialType(ObjectManager::kPhongColor); + SetNormalMaterial(ObjectManager::kPhongColor); } } else if (textures.color->GetFilamentTexture()->getTarget() == filament::Texture::Sampler::SAMPLER_CUBEMAP) { if (color.a < 1.0f) { - material_.SetNormalMaterialType(ObjectManager::kPhongCubeFade); + SetNormalMaterial(ObjectManager::kPhongCubeFade); } else if (reflective_) { - material_.SetNormalMaterialType(ObjectManager::kPhongCubeReflect); + SetNormalMaterial(ObjectManager::kPhongCubeReflect); } else { - material_.SetNormalMaterialType(ObjectManager::kPhongCube); + SetNormalMaterial(ObjectManager::kPhongCube); } } else if (has_texcoords) { if (color.a < 1.0f) { - material_.SetNormalMaterialType(ObjectManager::kPhong2dUvFade); + SetNormalMaterial(ObjectManager::kPhong2dUvFade); } else if (reflective_) { - material_.SetNormalMaterialType(ObjectManager::kPhong2dUvReflect); + SetNormalMaterial(ObjectManager::kPhong2dUvReflect); } else { - material_.SetNormalMaterialType(ObjectManager::kPhong2dUv); + SetNormalMaterial(ObjectManager::kPhong2dUv); } } else { if (color.a < 1.0f) { - material_.SetNormalMaterialType(ObjectManager::kPhong2dFade); + SetNormalMaterial(ObjectManager::kPhong2dFade); } else if (reflective_) { - material_.SetNormalMaterialType(ObjectManager::kPhong2dReflect); + SetNormalMaterial(ObjectManager::kPhong2dReflect); } else { - material_.SetNormalMaterialType(ObjectManager::kPhong2d); + SetNormalMaterial(ObjectManager::kPhong2d); } } } diff --git a/src/experimental/filament/filament/drawable.h b/src/experimental/filament/filament/drawable.h index 2f2dc81e..08956903 100644 --- a/src/experimental/filament/filament/drawable.h +++ b/src/experimental/filament/filament/drawable.h @@ -34,7 +34,7 @@ namespace mujoco { class Drawable { public: Drawable(ObjectManager* object_mgr, ModelObjects* model_objects, - const mjvGeom& geom); + const mjvGeom& geom, const Material::Textures* fallback_textures); ~Drawable() noexcept = default; Drawable(const Drawable&) = delete; @@ -79,12 +79,16 @@ class Drawable { // Updates the transform of the drawable for rendering. void SetTransform(const mjvGeom& geom); + // Sets the material for the drawable. + void SetNormalMaterial(ObjectManager::MaterialType material_type); + // Updates the material parameters of the drawable for rendering. void UpdateMaterial(const mjvGeom& geom, bool use_segid_color, bool enable_reflection, const mjtNum* headpos); Material material_; ModelObjects* model_objs_ = nullptr; + ObjectManager* object_mgr_ = nullptr; Renderables renderables_; bool reflective_ = false; filament::math::mat4 transform_; diff --git a/src/experimental/filament/filament/material.cc b/src/experimental/filament/filament/material.cc index aaf9c6ba..10538d28 100644 --- a/src/experimental/filament/filament/material.cc +++ b/src/experimental/filament/filament/material.cc @@ -19,45 +19,35 @@ #include #include #include -#include -#include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/texture.h" namespace mujoco { -Material::Material(ObjectManager* object_mgr) : object_mgr_(object_mgr) { - instances_[kDepth] = - object_mgr_->GetMaterial(ObjectManager::kUnlitDepth)->createInstance(); - instances_[kSegmentation] = - object_mgr_->GetMaterial(ObjectManager::kUnlitSegmentation) - ->createInstance(); +Material::Material(filament::Engine* engine) + : engine_(engine) { } Material::~Material() noexcept { - filament::Engine* engine = object_mgr_->GetEngine(); for (int i = 0; i < kNumDrawModes; ++i) { if (instances_[i]) { - engine->destroy(instances_[i]); + engine_->destroy(instances_[i]); } } } -void Material::SetNormalMaterialType( - ObjectManager::MaterialType material_type) { - filament::Material* material = object_mgr_->GetMaterial(material_type); - - if (instances_[kNormal]) { +void Material::SetMaterial(DrawMode mode, filament::Material* material) { + if (instances_[mode]) { const filament::Material* current_material = - instances_[kNormal]->getMaterial(); + instances_[mode]->getMaterial(); if (current_material == material) { return; } - object_mgr_->GetEngine()->destroy(instances_[kNormal]); - instances_[kNormal] = nullptr; + engine_->destroy(instances_[mode]); + instances_[mode] = nullptr; } if (material) { - instances_[kNormal] = material->createInstance(); + instances_[mode] = material->createInstance(); UpdateMaterialInstances(); } } @@ -72,9 +62,8 @@ void Material::UpdateTextures(const Textures& textures) { UpdateMaterialInstances(); } -void Material::UpdateReflectionTexture(const Texture* tex) { - textures_.reflection = tex; - UpdateMaterialInstances(); +void Material::SetFallbackTextures(const Textures* fallback_textures) { + fallback_textures_ = fallback_textures; } void Material::UpdateMaterialInstances() { @@ -130,25 +119,32 @@ void Material::UpdateMaterialInstances() { filament::TextureSampler::MinFilter::LINEAR_MIPMAP_LINEAR); auto TrySetTexture = [&](const char* name, const Texture* texture, - mjtTextureRole role) { + const Texture* fallback) { if (material->hasParameter(name)) { if (texture) { instance->setParameter(name, texture->GetFilamentTexture(), sampler); - } else { - auto* fallback = object_mgr_->GetFallbackTexture(role); + } else if (fallback) { instance->setParameter(name, fallback->GetFilamentTexture(), sampler); } } }; - TrySetTexture("BaseColor", textures_.color, mjTEXROLE_RGB); - TrySetTexture("Normal", textures_.normal, mjTEXROLE_NORMAL); - TrySetTexture("Metallic", textures_.metallic, mjTEXROLE_METALLIC); - TrySetTexture("Roughness", textures_.roughness, mjTEXROLE_ROUGHNESS); - TrySetTexture("Occlusion", textures_.occlusion, mjTEXROLE_OCCLUSION); - TrySetTexture("ORM", textures_.orm, mjTEXROLE_ORM); - TrySetTexture("Emissive", textures_.emissive, mjTEXROLE_EMISSIVE); - TrySetTexture("Reflection", textures_.reflection, mjTEXROLE_USER); + TrySetTexture("BaseColor", textures_.color, + fallback_textures_ ? fallback_textures_->color : nullptr); + TrySetTexture("Normal", textures_.normal, + fallback_textures_ ? fallback_textures_->normal : nullptr); + TrySetTexture("Metallic", textures_.metallic, + fallback_textures_ ? fallback_textures_->metallic : nullptr); + TrySetTexture("Roughness", textures_.roughness, + fallback_textures_ ? fallback_textures_->roughness : nullptr); + TrySetTexture("Occlusion", textures_.occlusion, + fallback_textures_ ? fallback_textures_->occlusion : nullptr); + TrySetTexture("ORM", textures_.orm, + fallback_textures_ ? fallback_textures_->orm : nullptr); + TrySetTexture("Emissive", textures_.emissive, + fallback_textures_ ? fallback_textures_->emissive : nullptr); + TrySetTexture("Reflection", textures_.reflection, + fallback_textures_ ? fallback_textures_->reflection : nullptr); } } // namespace mujoco diff --git a/src/experimental/filament/filament/material.h b/src/experimental/filament/filament/material.h index b717b9c4..03d0ffb8 100644 --- a/src/experimental/filament/filament/material.h +++ b/src/experimental/filament/filament/material.h @@ -20,7 +20,6 @@ #include #include #include -#include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/texture.h" namespace mujoco { @@ -65,24 +64,29 @@ class Material { bool tex_uniform = false; }; - Material(ObjectManager* object_mgr); + explicit Material(filament::Engine* engine); ~Material() noexcept; Material(const Material&) = delete; Material& operator=(const Material&) = delete; // Assigns a material to the draw mode. - void SetNormalMaterialType(ObjectManager::MaterialType material_type); + void SetMaterial(DrawMode mode, filament::Material* material); - // Updates the material parameters of the drawable for rendering. + // Sets the fallback textures for the material. + void SetFallbackTextures(const Textures* fallback_textures); + + // Updates the parameters for the material. void UpdateParams(const Params& params); - // Updates the material textures of the drawable for rendering. + // Updates the textures for the material. void UpdateTextures(const Textures& textures); - // Update the reflection texture. We do this separately since the reflection - // texture needs to be rendered before it can be applied to the material. - void UpdateReflectionTexture(const Texture* tex); + // Returns the current material parameters. + const Params& GetParams() const { return params_; } + + // Returns the current material textures. + const Textures& GetTextures() const { return textures_; } // Returns the material instance assigned to the draw mode. filament::MaterialInstance* GetMaterialInstance(DrawMode mode) { @@ -94,8 +98,9 @@ class Material { // textures. void UpdateMaterialInstances(); - ObjectManager* object_mgr_ = nullptr; + filament::Engine* engine_ = nullptr; filament::MaterialInstance* instances_[kNumDrawModes] = {nullptr}; + const Textures* fallback_textures_ = nullptr; Params params_; Textures textures_; }; diff --git a/src/experimental/filament/filament/scene_bridge.cc b/src/experimental/filament/filament/scene_bridge.cc index 6b445035..89a99bc0 100644 --- a/src/experimental/filament/filament/scene_bridge.cc +++ b/src/experimental/filament/filament/scene_bridge.cc @@ -139,6 +139,15 @@ SceneBridge::SceneBridge(ObjectManager* object_mgr, const mjModel* model, ReadElement(model, "filament.fallback.environment_light_intensity", fallback_environment_light_intensity_); + fallback_textures_.color = object_mgr_->GetFallbackTexture(mjTEXROLE_RGB); + fallback_textures_.normal = object_mgr_->GetFallbackTexture(mjTEXROLE_NORMAL); + fallback_textures_.metallic = object_mgr_->GetFallbackTexture(mjTEXROLE_METALLIC); + fallback_textures_.roughness = object_mgr_->GetFallbackTexture(mjTEXROLE_ROUGHNESS); + fallback_textures_.occlusion = object_mgr_->GetFallbackTexture(mjTEXROLE_OCCLUSION); + fallback_textures_.orm = object_mgr_->GetFallbackTexture(mjTEXROLE_ORM); + fallback_textures_.emissive = object_mgr_->GetFallbackTexture(mjTEXROLE_EMISSIVE); + fallback_textures_.reflection = object_mgr_->GetFallbackTexture(mjTEXROLE_USER); + // Create an empty/black indirect light to ensure that the skybox is oriented // to respect mujoco's Z-up convention. filament::IndirectLight* empty_ibl = @@ -313,8 +322,8 @@ void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { } } - auto drawable = - std::make_unique(object_mgr_, model_objects_.get(), *geom); + auto drawable = std::make_unique( + object_mgr_, model_objects_.get(), *geom, &fallback_textures_); drawable->Update(model_objects_->GetModel(), scene, *geom); scene_view_->AddToScene(drawable.get()); drawables_.push_back(std::move(drawable)); diff --git a/src/experimental/filament/filament/scene_bridge.h b/src/experimental/filament/filament/scene_bridge.h index a1e56796..e3d4917d 100644 --- a/src/experimental/filament/filament/scene_bridge.h +++ b/src/experimental/filament/filament/scene_bridge.h @@ -26,6 +26,7 @@ #include #include "experimental/filament/filament/drawable.h" #include "experimental/filament/filament/light.h" +#include "experimental/filament/filament/material.h" #include "experimental/filament/filament/model_objects.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/scene_view.h" @@ -78,6 +79,7 @@ class SceneBridge { float fallback_head_light_intensity_ = 0.f; float fallback_scene_light_intensity_ = 80'000.f; float fallback_environment_light_intensity_ = 5'000.f; + Material::Textures fallback_textures_; }; } // namespace mujoco From 26fb65c7a7a22dc2856ae3135e9d2adaf4fe17d1 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 9 Apr 2026 06:34:05 -0700 Subject: [PATCH 034/251] Expose Drawable Material allowing users to manipulate it directly. PiperOrigin-RevId: 897079632 Change-Id: Iec51ab30dc087562b8e6a464e122525f144e73a9 --- .../filament/filament/drawable.cc | 35 +++++++++---------- src/experimental/filament/filament/drawable.h | 11 ++---- src/experimental/filament/filament/material.h | 1 + .../filament/filament/scene_view.cc | 8 +++-- 4 files changed, 25 insertions(+), 30 deletions(-) diff --git a/src/experimental/filament/filament/drawable.cc b/src/experimental/filament/filament/drawable.cc index 2db25b95..9861d032 100644 --- a/src/experimental/filament/filament/drawable.cc +++ b/src/experimental/filament/filament/drawable.cc @@ -238,10 +238,8 @@ void Drawable::SetDrawMode(Material::DrawMode mode) { renderables_.SetMaterialInstance(material_.GetMaterialInstance(mode)); } -void Drawable::UpdateReflectionTexture(const Texture* tex) { - Material::Textures textures = material_.GetTextures(); - textures.reflection = tex; - material_.UpdateTextures(textures); +Material& Drawable::GetMaterial() { + return material_; } void Drawable::SetLayerMask(std::uint8_t mask) { @@ -374,16 +372,17 @@ void Drawable::UpdateMaterial(const mjvGeom& geom, bool use_segid_color, bool enable_reflection, const mjtNum* headpos) { const mjModel* model = model_objs_->GetModel(); - float4 color = ReadFloat4(geom.rgba); + Material::Params params; + params.color = ReadFloat4(geom.rgba); if (geom.type == mjGEOM_PLANE) { if (IsBehind(headpos, geom.pos, geom.mat)) { - color[3] *= 0.3; + params.color[3] *= 0.3; renderables_.SetReceiveShadows(false); - reflective_ = false; + params.reflective = false; } else { renderables_.SetReceiveShadows(true); - reflective_ = - enable_reflection && geom.reflectance > 0 && color.a == 1.0f; + params.reflective = + enable_reflection && geom.reflectance > 0 && params.color.a == 1.0f; } } @@ -434,34 +433,34 @@ void Drawable::UpdateMaterial(const mjvGeom& geom, bool use_segid_color, } if (textures.color == nullptr) { - if (color.a < 1.0f) { + if (params.color.a < 1.0f) { SetNormalMaterial(ObjectManager::kPhongColorFade); - } else if (reflective_) { + } else if (params.reflective) { SetNormalMaterial(ObjectManager::kPhongColorReflect); } else { SetNormalMaterial(ObjectManager::kPhongColor); } } else if (textures.color->GetFilamentTexture()->getTarget() == filament::Texture::Sampler::SAMPLER_CUBEMAP) { - if (color.a < 1.0f) { + if (params.color.a < 1.0f) { SetNormalMaterial(ObjectManager::kPhongCubeFade); - } else if (reflective_) { + } else if (params.reflective) { SetNormalMaterial(ObjectManager::kPhongCubeReflect); } else { SetNormalMaterial(ObjectManager::kPhongCube); } } else if (has_texcoords) { - if (color.a < 1.0f) { + if (params.color.a < 1.0f) { SetNormalMaterial(ObjectManager::kPhong2dUvFade); - } else if (reflective_) { + } else if (params.reflective) { SetNormalMaterial(ObjectManager::kPhong2dUvReflect); } else { SetNormalMaterial(ObjectManager::kPhong2dUv); } } else { - if (color.a < 1.0f) { + if (params.color.a < 1.0f) { SetNormalMaterial(ObjectManager::kPhong2dFade); - } else if (reflective_) { + } else if (params.reflective) { SetNormalMaterial(ObjectManager::kPhong2dReflect); } else { SetNormalMaterial(ObjectManager::kPhong2d); @@ -470,8 +469,6 @@ void Drawable::UpdateMaterial(const mjvGeom& geom, bool use_segid_color, } } - Material::Params params; - params.color = color; params.reflectance = geom.reflectance; params.emissive = geom.emission; params.specular = geom.specular; diff --git a/src/experimental/filament/filament/drawable.h b/src/experimental/filament/filament/drawable.h index 08956903..ab0f6390 100644 --- a/src/experimental/filament/filament/drawable.h +++ b/src/experimental/filament/filament/drawable.h @@ -26,7 +26,6 @@ #include "experimental/filament/filament/model_objects.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/renderables.h" -#include "experimental/filament/filament/texture.h" namespace mujoco { @@ -63,13 +62,8 @@ class Drawable { // or hide the drawable from specific passes. The default layer mask is 0x01. void SetLayerMask(std::uint8_t mask); - // Returns true if the drawable is reflective. - bool IsReflective() const { return reflective_; } - - // Sets the reflection texture for the drawable. We have a separate setter - // because we need to render the reflection texture before it can be applied - // to the material. - void UpdateReflectionTexture(const Texture* tex); + // Returns the material for the drawable. + Material& GetMaterial(); private: void AddMesh(int data_id); @@ -90,7 +84,6 @@ class Drawable { ModelObjects* model_objs_ = nullptr; ObjectManager* object_mgr_ = nullptr; Renderables renderables_; - bool reflective_ = false; filament::math::mat4 transform_; }; diff --git a/src/experimental/filament/filament/material.h b/src/experimental/filament/filament/material.h index 03d0ffb8..1f1c98b6 100644 --- a/src/experimental/filament/filament/material.h +++ b/src/experimental/filament/filament/material.h @@ -62,6 +62,7 @@ class Material { float emissive = -1.0f; float reflectance = 0.0f; bool tex_uniform = false; + bool reflective = false; }; explicit Material(filament::Engine* engine); diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 3cd7f929..a978554b 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -194,7 +194,7 @@ void SceneView::RemoveFromScene(Light* light) { void SceneView::AddToScene(Drawable* drawable) { if (drawables_.insert(drawable).second) { drawable->AddToScene(scene_); - if (drawable->IsReflective()) { + if (drawable->GetMaterial().GetParams().reflective) { AddReflectiveDrawable(drawable); } } @@ -305,7 +305,11 @@ void SceneView::AddReflectiveDrawable(Drawable* drawable) { auto viewport = reflect_view_->getViewport(); auto& target = reflect_targets_[index]; target->Prepare(viewport.width, viewport.height); - drawable->UpdateReflectionTexture(target->GetColorTexture()); + + Material& material = drawable->GetMaterial(); + Material::Textures textures = material.GetTextures(); + textures.reflection = target->GetColorTexture(); + material.UpdateTextures(textures); } void SceneView::SetColorGradingOptions(const ColorGradingOptions& opts) { From 81720071b816d57598bbb438383e3ff259faabb9 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 9 Apr 2026 06:51:57 -0700 Subject: [PATCH 035/251] Changes to `dcmotor`: - Remove `lugre:viscous`, should now be added directly to actuator `damping`. Trying to do this for the user was incompatible with default inheritance (compounding instead of overriding). - Move voltage limiting from the `saturation` to the `controller` attribute. - Fix indexing issues in default inheritance. PiperOrigin-RevId: 897087642 Change-Id: I5388c2633e15c7e223992e7eb5d6a28db75a6438 --- doc/XMLreference.rst | 33 +++--- doc/_static/dcmotor.pdf | Bin 599037 -> 599178 bytes doc/dcmotor/dcmotor.tex | 26 +++-- doc/includes/references.h | 6 +- include/mujoco/mujoco.h | 6 +- python/mujoco/introspect/functions.py | 6 +- python/mujoco/specs.cc | 12 +-- src/user/user_api.cc | 32 +++--- src/user/user_api.h | 6 +- src/xml/xml_native_reader.cc | 23 ++-- test/engine/engine_forward_test.cc | 110 +++++++++++++++++++- test/engine/testdata/derivative/dcmotor.xml | 2 +- test/user/user_api_test.cc | 49 +++++++++ test/xml/xml_native_reader_test.cc | 109 ++++++++++++++++++- 14 files changed, 335 insertions(+), 85 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 72bfd3d8..a1443753 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -6446,14 +6446,14 @@ This element has the following custom attributes in addition to the common attri .. _actuator-dcmotor-saturation: -:at:`saturation`: :at-val:`real(4), "0 0 0 0"` - Limits on the actuator, defined as :at:`saturation` = ":at-val:`torque` :at-val:`current` :at-val:`voltage` +:at:`saturation`: :at-val:`real(3), "0 0 0"` + Limits on the actuator, defined as :at:`saturation` = ":at-val:`torque` :at-val:`current` :at-val:`current_rate`". :at-val:`torque` and :at-val:`current` are alternative specifications of the maximum continuous torque: if :at-val:`current` is given, :at-val:`torque` :math:`= K \cdot` :at-val:`current`; if both are given, :at-val:`torque` takes precedence. Sets :at:`forcerange` to [:math:`-\tau_{\max},\, \tau_{\max}`]. - :at-val:`voltage` sets the maximum voltage :math:`V_{\max}`. :at-val:`current_rate` sets the maximum rate of change - of current :math:`(di/dt)_{\max}` (requires :ref:`inductance`). A value of 0 (the - default) for any sub-value disables the respective limit. (see `tech note <_static/dcmotor.pdf>`__, Section 2) + :at-val:`current_rate` sets the maximum rate of change of current :math:`(di/dt)_{\max}` (requires + :ref:`inductance`). A value of 0 (the default) for any sub-value disables the respective + limit. (see `tech note <_static/dcmotor.pdf>`__, Section 2) .. _actuator-dcmotor-cogging: @@ -6465,12 +6465,12 @@ This element has the following custom attributes in addition to the common attri .. _actuator-dcmotor-lugre: -:at:`lugre`: :at-val:`real(6), "0 0 0 0 0 0"` - LuGre friction, defined as :at:`lugre` = ":at-val:`stiffness` :at-val:`damping` :at-val:`viscous` :at-val:`coulomb` - :at-val:`static` :at-val:`stribeck`" (N·m/rad, N·m·s/rad, N·m·s/rad, N·m, N·m, rad/s). Disabled when +:at:`lugre`: :at-val:`real(5), "0 0 0 0 0"` + LuGre friction, defined as :at:`lugre` = ":at-val:`stiffness` :at-val:`damping` :at-val:`coulomb` + :at-val:`static` :at-val:`stribeck`" (N·m/rad, N·m·s/rad, N·m, N·m, rad/s). Disabled when :at-val:`stiffness` = 0 (the default). Adds one activation variable for bristle deflection. Note that the - :at-val:`viscous` coefficient is mapped directly to the actuator :ref:`damping` array - (specifically the linear term, :at-val:`damping[0]`). If both are specified, their values are summed. + viscous damping coefficient :math:`\sigma_2` is not part of the :at:`lugre` attribute and should be + added to the standard actuator :ref:`damping` attribute. (see `tech note <_static/dcmotor.pdf>`__, Sections 1.4 and 2.4) .. _actuator-dcmotor-input: @@ -6482,12 +6482,15 @@ This element has the following custom attributes in addition to the common attri .. _actuator-dcmotor-controller: -:at:`controller`: :at-val:`real(5), "0 0 0 0 0"` +:at:`controller`: :at-val:`real(6), "0 0 0 0 0 0"` PID controller parameters, defined as :at:`controller` = ":at-val:`kp` :at-val:`ki` :at-val:`kd` - :at-val:`slewmax` :at-val:`Imax`". Depending on the :at:`input` mode, the controller stabilizes either position or - velocity. If the :at:`input` mode is voltage, this attribute is ignored. A value of 0 (the default) disables the - respective feature: :at-val:`slewmax` = 0 means no slew-rate limiting, :at-val:`Imax` = 0 means no anti-windup - clamping. (see `tech note <_static/dcmotor.pdf>`__, Section 2.5) + :at-val:`slewmax` :at-val:`Imax` :at-val:`Vmax`". Depending on the :at:`input` mode, the controller stabilizes + either position or velocity. If the :at:`input` mode is voltage, :at-val:`kp`, :at-val:`ki`, :at-val:`kd` are + ignored. :at-val:`Vmax` sets the maximum drive voltage :math:`v_{\max}` (Volt); in position/velocity modes it clamps + the controller output, in voltage mode it clamps the control signal (if :at:`ctrlrange` is also set, the tighter + limit wins). A value of 0 (the default) disables the respective feature. When positive, :at-val:`slewmax` limits the + setpoint rate-of-change, :at-val:`Imax` clamps the integrator state (anti-windup), and :at-val:`Vmax` clamps the + drive voltage. (see `tech note <_static/dcmotor.pdf>`__, Section 2.5) .. _actuator-plugin: diff --git a/doc/_static/dcmotor.pdf b/doc/_static/dcmotor.pdf index a811acdcc1b69efa07875518434fa27ef6fbea67..cfee21c4ac9a76c928a047de74136c889e8645c5 100644 GIT binary patch delta 64848 zcmV)7K*zuR$0Ul$B(SFz12Hf*lTmLfe{&8`)?3CNn?WEf4%;=Qqp4e{z4f zS{?TFmn)OzKRm9lm-qKS)F$iwVZFJ1TJP^p+I)Xy;(S{O*H63MYD4Pb>QCSN8r@lF zJ!J38bMMy=>%)GyS~wHt-`uU(=BK|*{-mqlE=npG4+YO%fw0+u_W|I+pyRbpc z|ERp$?k?VCvwc`^miOJuwX(cie_xg7P5b@RU)tw)Ti0LoEDSl?#9wrdgN+fR99(u~ zkPpkhb)Kj({n5`(+O7BH*@rR@vxU)qu-;6k48QIkFouQkzRcgpm_Fs@G9lu5>j!rI z>7fu#2P1@I)9nG%g^zB2z1$Sq{%ODHt<_<fsUnO7qojd3f5bz$*!ycZpYA zQuD_vZ{{m9%U~a}qBsusberGN%h1(h!xMEHFA~^?cQRn$d$#?tGbf_vMxJ z_HKQP6)g*{vl>i_(|O{^8-u zDhy{-e-|-(cM|xEmH^{>ksFBnCdFs9x*qe=UB}>5m^w@UPEg8Vqdh zNvkini{EaSH)SrreiRmi&g^%RGj^O0D2U_pV;WL2qK;HEs=V*>8PVXM%sz|_Ut9#O zlk1My#39A*ki9-z0DC%O3_xF8gF5@aX|rz!#)|nDaM+jl^SvmXFkMapjHh*K_wj9{jONpTr4bD>$76#Fyuo~Prs#NrA1Ym zR>8WsJ5N+(-iJKkj=j^#Cd(d*N>d{~oC z`}Cs9FfR4i4mYsfIp}MCZD5DPk1<{ow|+9MK@AF;-)D3um1oN|E@_S4-ml8cYC4QC zv0pBelp7;|e|-|`y`P%v^S{tu?!@z01nDp&a?qywM^G7qp4>#V5m@(&&CSy_x_Q0o zI5Bib@GV!}_QmG<{;9g4sdX72=(yU|bqQ+ReEY8ar6W2vUbp9P+E!inkj;MEcct(P z!;t^A+U$OifTH8xKCG4-_Q2ZtVYgZy%A;-TC&;gV1t4s*?sovUS=O5`%O9+*#CNti zal7>r?C^2t&XJlID`F6Okg3?)?`k{q%xC=MLU0tzA5*fg5a=|VwuYhe{PvI#ZaV4cJKL+Pq&OBn1xVgsUJ@Wb75b6dYNAan67 zz>^>L1V*Px4_~t~<0Ff5&UL_lM=}s^{A;H`D=9%{QxCFsrntTAoYL z0eXE7?g5u~e3XS-#YeHl+w$|_u6!+gjt}d9qIei@EH|qf+dqpfCgZcYbjV?5%LqW( zZCms}a_QmyRvvo2E@9FY;zopzF{nDDxa_cgSd~kf3Va4tRFM@;ks+^loLqkT-=F`Y zd|2=MBFfhF7k3guMl%>4CK2SS!&_-kHlGHLePOc{woiWF<*>x@2IJ;my!%l-^bSgY zAa;A0Uh3h|2A}JSnCMsOUN2e?&ed2JJVq5x!Iyo74(`i37jbb0;-b%h3ash=uzI9a zF-{KZXt5@oL^B7wwZ31!-9bU_OA{g)f^^{5Wg+dMpPTjMoiED>75ZIOwXlf$^@6&0 zeO=Y@BLsyHhp$t)m1GXH8a(R5-ECEWjK8PpE1u+uEvzUNpRjIf{zlC2N^GypB7ICd zc>FEiRW;NQxSL&--`&0tw(kzBO%=w9wQToQI+x|o-EvnIKyebwb(;(Ph-a)nxw){a zl>Kd4yw=Z?NyPLVV4SantE)|xZl>snQL8;e_bRO>!!tk4#Ka?nzPL=#BvAw4&zF94QcOR83B7>HiG&X})XL52Q zD`VYhpnilR_e0kDJxp@S^H0$q9WMYVm{Udo(AIAy z^X$&DsZI&VGntC*?qhh!o zr-L5jp=CQ^gCAF{>kF>#YtXhDZikO9h8q{A+2$b`UuN0AHp@wsS@y3vKA-l&4H%qg zCr|2y?_tpP^dWujH_+~vQL^6S4YO5d@_6$j9-qz`<c!^fS!aLTFl-uZny#1q z5tD2p7=IUBQ~rec1?LG3hAK`8KQMWPK+)R@4S#-8B;+*Qrq!rk3gK|6Py@ehwhKfj ztkYneWlGfzzpplj3*JrWSKUNRAe71JG^@^d+~7h!oo9?as0t6UM`u+b9JCt3IZ6R_ zDs@f-8(p4%f47oPv|#Jaro@H#_HMT-0WI1v{C^=$hAhK%={0T3>w1h}v_Yhv-ex-h}4fz#f<8n#3@s*`Fu z;yKTs!zmuZ8DRWrze-Fxiwnvz5tlcm;78atBq^1oC&JW<;2WSl?dfN* z^#cX&X&6D0I;Umv{f1a)9_JxFA5GR?2WcqOXb++6h0 zKE03tw*5}f=tc0#gFuHDgUS>9-nDXz&$hA{Z`Z}TkdIDHl@RulK??5Gu+9G?VBXmhP+?!+uOG{g z=Co6W6$iYLa$_WIdY)w(yg`^82dh%EH_uYnh&Bh09!1SWcYG;zod+G=3#n`C3FNw2 zR0;D{!L44-IS)ZO$z0o`cJHdx@A@eVfuml4C8tU2m*Uv7=l!hvyaMB@(0{$;1sfjq z5Vh~;JB)vp@9$HJ~ zy%snxPjy)iCq|Dy)`*@zEPr?Pf@f|fek`i3o4vr$))MO{FT*ma+H?}&06ab&%IAg{ z9~NzLh7!b7oUF7Gq@SFp^q0)W@yBidLHpJ4o{~(>c1XlmhtTG*6)fp#Nf^hCVz)%^i%5%+9u7aI@l-|LOtS}Y zxm(>&OF&+#qx;DV^?z;ROQ?(PA*cM8>sxr*7YjI~on7{-I4&j;tcCLsS;q!x3P!RB zGc^3wOj^GY*TTcMAQ}ICb@HzsaF6=`>F4W**66Ee*G#T&nF6AxnSbQ@uiUm>RW7R6 zyx8aRpR_kZR)gnE;9o>2tbm212J+XlafZge|9uy-yDl?vjDICWw{?hIPVbJ?d%3JL zi=DP*T@K+_7h^ zM~l|Q^OpbIezj&YUztikg?ilyA*aEHfXCgJD^9=P&EEWB_GVK$|4cxB`gr^9KR^39 zE=sf8{j7YyUVo0?$I(R#&&j1MJu!$Fl=8C_h_4#<_)V}F2Ap|<>(;UF3o z8J`jnoA|8ZZF4YizyoecO_y{bLHuHH$UR%+zM=i~k_ zGOp*{yg!$$=}5nwL4PN{ztH~vN!aCPDyIo6Zo4TGVad21m! zW=y_L-a~1vTBEZ_#^TPc0$>f&RS&Yjw8jB0&LG#TiG{s$0iq4TG znD35I?i2y-GJJ&-wX0*wG;lfD`gV?dXg}0zkCzrcP+)J&>i4s6N7mG{U*rEji@Nm; zw#=X+?K%X*9kf5Mt5oN0TJxsENU|%l}R1qGwK@4fDUmI-aPhT&gSsUB|yq zPdN$^Uu*Q31Z|FbY68%-R;|$_Nqg$iPh5Ry{9=d1hYt|_XKLnD=+7T=CiHczi*c}D z&3`6d;`ok9q;#5@i-WT1@0%5`EbJmy$9veUo@FN}V-48(8h};-t&tT5V>NrqjUV zs|&%!S>!U_=2^v43#Jw)(bV&I=d9C$SAQd(9GuEWXmxqS6DFYbw^IE3TdBff9lD=? z*N`8+16FHpQmriJhgPbn<(2YuL|elN6axHATbUOO$WyX<~pb0krD2`CKo zfMd}s?X2?IR$|H#HwO{%N|qrQIu8yqm+}yC$MP6A7_DK5K{R-_t;9sxO>pTfUxz1+ zaS>mD0nw>w3o(GR3|K5t@N-U;h<`p}5kd4mlxN2Q(Fwd&(6o)dj3%LTeBB7q8H};7 zw4{{{60zwTa+y51z(*p=$xJqYR$g(%MWA_wh^-PMRx*=_yObGX>|??IWV)!b#DLcD z)kv&Kk`!1Bl?eX?>yY3O+Zx63ZEc0gBoU#xlU`Z}Uxvem5sk*VO2Z(aiGQJ)BUq#m zJz=#(nlp7K;UHsxQ5vdX%gTZnvJGgd(aEwlHZpqUlxx1kScz#+Dth#XW?q#562w5$ z&*~bdWyTrYBJvBQnH++tM6iXMMZp=$(l~lzhyh3py7tl0V=_WBiB_o4vJ+y$)v>Ut zqmh!7H@aZyb&+TXgMk?m@qec*gJViusW9V4Nx3YnV`)VsC8Rn&=lM!M?h}bf2CXYG zVXTQn_-$GS?E(`}91Ykfvhsw)I2h%4&j7n0LL;Vtg(6WWUv8P0X$(cu&O;%=+jJ}#i76FH1TiTK3rNQ*5@VFit_L>+ z#VLpM`CK?3-U|c~P6CVAIZuTaBLa6p3wVhTVNf9&;!!)|?4+R^tYrboxcX$o{*G7@ z9d8wtEP@a68f=q0Ehv9?UWyngGZlpv5~d!cfXB(~1Sn2tIp6fkLz0F;m!X1YbP+<3 zf{&0r(3GySg%}WW!eCcHJ!qRNF+-0T60umdC44}Wpg%x#Mi;gZtfgg*9f$@@YAX># zz}$lfol+E9;2S5zHL&J^yhR$rWY+(I7_u(vhroxniksztk0^f?Btqdu#cVQ!&llDW z+^Qr%e*{`#=$}ettgJ~?hM5}@nNU?J%=l~6R*2lz03u6T#0OnzP%oHr5PdX7fkc`K z*fJ7bHCk9F9at>DPNZD#Xjt;<<(Ar4Z9!H_)Oa8&V~fCp5i!*cU5EVSP`Xa$h9);8 zM(E_yBR@E(5Kn(%@UAFH3Y+HYb6Q|DbtT3CVI>jrRd^%vv%@TaXrV((KhYDeHHi=$ z--vJ*m9#|oQbvx+hAx*LDZ>}giL+r*u`WRnDw_(&fCxF@w8KQfn#m}sTm$!%;#)BI zxm*qoH-_^rdf7B$fGw3!5GpUqC}9GurJiBP1nG&Dh%kRj9`35XfuRXrd1>d2Es8R6 zSAgiHLzV`m&@y#7Nzh~kox}qT>?VohwV0NetTfax8Kx4m^+vj=awu9278^>&2wJCf zE)q_A&uvHoYu92UWfeHNP>Jw4WGL_;VD$?T>bSrZnexgY6GV)?wuAzJR&gZy!15;& zsZ?xvjthUGcnDb#STF>LHq@ofjN)RH2N?lvNhGG|IJ+?Dw%F9{`Qi;?SkPc8gRjJ- zS?2+<2B*jJaM}Pm`XIscbs1zF&A{n`P3QUI1 zv0>U!m6$XtX`hvE)SNiy)aOy>N+t5ZkVTw5B*<4eCtghqUe2PWZnSx$?Rg4-AHP#=zo{NPdDJgmBfWpwa<>*gqc*1-q6I!55fulps zFWjk zzRfV0{Mg5~780a-bGB^6`%N}yOAMD-(MO-M1H=qYtvh2KiAsO$l*K5f+MIKv;p>{k z89s6@tw&JNPPrUB0!7gzs!0T>!Q1Gk(jz64PM?Yy`OXsemdY%S0*gy&p>l@DSxIZv&FVJx7k@QsGVC@HD% zQ=?-Jg!95Y^E_8e;T8r>QmNtEDkk=4gi3ka>MGTFplD zPOh5%(VQDeD_7exnto8Nsxj`-O(6Ie?U7ns3S-3$@e_aV0|O)iXQqw{?LaVf8AqoE zRA{@ZAVvpB@WW`%oF!TpU0v>@W5t>GzIu$K6GpT#+jigPuJMH(4A@UyJFcjYl|@H4 zkS{zU7Bo|r(Qq40ug=^{Q}D;BCn|=sLOVyN6`Tg8W$e_!75-E8VWTr_h<{eR;OIEh zOhcvWsgr*VYIe2kt=3}< zHXw!yT;L~hi4jPw#U(X>p}@A4977$Vw7Er%Eh+*LlW`V5al6VYIb)zj&0s6c-2yV` z5a`+BPqerVv=+FG;T=6ETSLnh2ZDxpv&MlfVg!HD!?;dEODm{=o5Y}?C1aJFM5~rt zQC3iSjBBC&l+8sIjuEh>SurL|VrA+{R{`WPss_>8LL0+xDb{f#QNnafc&F;HF`g$e zn*!sFk-w$8m4lgJ1eTFqF-{|v02+-;It3j_Ybt33C#d$pA$bC0keEaEr*MZjyQ$zr zBQAdeBGhB?bw>on%6@?rc4$}&hz6FzOi&ri9m%TqIN~>!;j1sYdqjFb^t!=j4HFVO z1{*g4k4VhLbc`qy!T($rcf_N_r}q_1YseL=dC=iJ!MH3v5tF897)>i;Nk75OEO#U< z%QFIOOfrOA0k#Ikfe7cYgsdZI$Jhf(D@=djJrLnolmKkR0hyyv72RuqA!tw>6)>#G zB4}`@isLszkRUqe%3O|MC5WNja)dRR_5d|b@J|rwbLI(B3Qt7|+n!>obYFt2m}&)I zEhG3Ukt0eAVnv)sG?zIOql#4=abG4)ROOcuA+}68sNyt^STZBYWQtcmqRvd4#8`i^ z=YM08rY6(YB-ch~R z{6fL*U4i(OdYcjAjYigEnB{VXa&5K zJjrY@&`?_0Ne)CXkh0b-Rl*CJG>m^a72+y6BtOZ%pc70A+ex0rlXqho&zR8hWVAfZ z{BTlqRSsj0$cy3k(`*u75zD!zr&`|0b7!Df3|`Go5x)W_%&X)2O}kMM1pv-g z7Wo{L&om!!InT1xOr-h}oE(4i*_vEe{HtT)+fay#A~>eVxw)7xO8S^gXX#}qifPQc zJ8nTA%``pFQ5J{dn8)Wnz!l>kGyX|(sl~+^YXjgh1QUt{A8QB_p~qUJf2=mZQkv@g zpOhs)hH9)fsb8o)e^#vh)wFQtAAOzTSab2(r+-?R$7|*0AL=+h)Ny}&sN?uh$MK<# z<3kzO4|N>>v^tKjf7Dy>&*7PeK917| ze7ew37ewm9O_VYE z%(M%x_E6X^T-yb9yAXeH4=e6r%3biegHm_l>@M!z1;~3ydKYr)sS?HO}b?%z5112p-ZnKre8YKEwbsB<8%vmx+OlnTA*&x zP`A9OSBKOoSL%ONHFe9MdUaC0f~tP)RliEBTh7(5{OVVU^()Hy)o1-mwSHAwzXGmb zE!VHC>sR6REB1QDef>JX3pIoN`oexKV!v*&U*p)XhwRr*_UkPBHJSbT&2Fh@zbdp} zLE5c0?N_Gui&(qmuKmi`eid!M;|vWP?W?n#W)V}li;u`Vh`<5<+EAY(oBhF$1`&RQ zYUm1o=Pv34vwgn8D{w==hC~jMDycczCCrQ@cws@uf+c48tl$Z@x(YlMGw?H$tg$8r zXt6WcOcJVE)ytXSj6t%j2#t#sp?l)2WJF`DZ6Uf6nD%Du5^B&&r%X0=VX6`u%~f;- z6N`U&XIc@8!B-%~I&K}rAc?A{0x1x7TQ=*my7+92Kr%CwOt5KPd~d2VEV(4P5H}Td zMs4O9^D=dnD&}ovA3rU}!KGC9P=qc+lFd6lZw(iA>@Hy7`g{lgW{zMw0Ce|iH5S@>@6^BNu6wrvIGL?Vf; z@o6UIR*_+7#X*B;sfLXpx&gC-2#tSMix8A(!Ys%W`e-WX!Cg=tktgtH1sRB<642!K zX^6^WB_{X|mPF@{HP$6W^AdpT5%VC=g9g8%7;s7Gx{__<5PT&@!I3%j+UEv)fHP@V z7%bOFouFeT*L&`9W((sIp48-ePdlv&{upQp!Sw{dV%-gkH5-yt1rd^66cB&aRwfRo zc?^YZ_=X|Vdx_19o+G~D$o1qSxC*&q8KtC-kejcTM5=LA!Q3*bl^9Kc{_zx~npLH` z_rmvB6Jv`{32UM0APWzx!Ua*P;>h=OKC7Z4%xo2NoUD1NMF^~^@>KVp9=EE+8^ki? zj<7wsz1_epu57Xl4ywRTmRNt8w3sWS5+L_ENkdGUX-Ec4dJ>v)1?gs4ntrBnmf^wh?uQxxtz#&h|5UC3Wv7$E*Qc~5*2=DMH4(vq6hmEE`?v? zDnJ4Ez7Sz=YB%Nd+5&XK_cpD70^TBthy!iS`J}`IlHi=sm8zi@Ig)?Ar{YlQ$SkyuhgR5#Sx$4u`rD*;qpsPKqD6hu9!-F#5(jiI1aA;JZ0V1zZH(BKib zQVGS)f(gdfY7mg%fh0n)32PH75<$qqlNco(QjiGrun>bPE0)DR7MiPYYG&4kK%8Wq zTZsn)4(_Dsn0>1PK`eiQfE6KM#saNYBJt1^C#R;gDgnYIp|s>y)#GbqRuiP5^O|*R zxC11Ixn&lI+M456EM_B{(W00-BH|7d5KAyQyRp=P;WCQ~#Zj*5Lq%tcI~qD3UXt1k zq_hR<)A^yPCNf0c^n?FnKIoj;_HA zhR%Yd-F+?UB$A*EgLhC>OI45x9`TZl$LiL&L2JR11QJJtVUFbwikWJfZ-JOAHB&K> zs+yaqOL%0<$tcFA1|(petbpq1SB)1>Qoai@QynpTUNHX z8Y>$D^VGl01GMeDNzK_oiCRjWYUl=mBqcjd(xa?l26KOuE|3~nVsJzZ2;n(W8P@tn zO^h)}g}QCE9?(k!nwStk&`~20!jAN@m`^b2_*S-p36j)-%hom`T$qw3z@TeXL-g9j zLN(Fr5e0yT$O5+~2E!vZ5a}aX$)%?e9DrY6sPfc`0yrk?h&)v9lNf-cbclHSs{Xa? z7|pySyJvrO8sC;@@FL)^Q?=ldC;(g?QWf2DV_LkA$lD1l%krK^go|2;jPG0n0X#LD z0I9xWLCi#2dP_$9t5%L60wCCC^d^oVK*$siP!ZOr7VSIDB!^+x65CywII)Ea@vl0< z38seyWgZ)ieGB-#;HO4XXO+LHA5DP@Ghy2?;&*?{!}Dk%6DvGIGXUF=iRoqAfIeEK zOs~lZ@}S3{9}E`-)61|wW8BXixnO$mLh2;YX^BifGyYV%VmV&dUQ*VQt_38X*$H}2 zMCyEv^GJn13=sjfg!V#(eoGV)hQ(Rc{?dg-;4Eb&#^uTibc?O*&nww%*NCv!P>`B| zrmKHim6^vx31O^`>vP+(-!OKjW~%<%OB>T;DhnXZ0O9Z4nI2LN-Mc_!M5AoD!5?+Bci&^3v#~owJXvx_f8) zx_5SXaqsL{_s;$Yx6XE~zp>`!t+QLBSGUeSx!bd?vt!#j`+S%4fBfv8_upar^y`be zZO;}bdscTg+opfVtR%~v^T}r0Z(rPO`^)X-{>A;aJA2Pd%eS5L1t&*~O}DG>9ln2i zR7DFIh4G}|vuBww9s{mUHoB)P72Am#aeA8i=^f~~q}fTMeosse;|apk4w#_29WYEY z+sX6Ih3Dtq^W*tU{Q1R+yVQKTrt^==I&QXQ3H#fjx4&mBw9xW$3B{ffapj?U7MC3L zW_h}I^z(17Ql85JgbN;EYn7b1Elqz`n=5?%?#kqLRvtd=%Q<@G{8YB$5rSwo9wEqI z%MpS+QkzdGhn(}AG=>N5m~ftqepAl>zTGW(i+UP3nYet~-);Hu@Au_vIm8bi%I@)U ziult;Cgo>5=OrFn?C+L0?e4qPZ8`WC?brL$uDH`f?R17J>G8*^+y?vccM#o`%3-m(-O0`;f0j~ZSe!wzRyobzDjO-!Xj)p zlWpY$*6o3ZEhBQ6f3aEZ4(op9AJ)*riwU;a&U70ELZ&cW&b!yr73OFVtF_$%Q>T~8)`NDR89@WZ!55= zT|tP;@)zRCYMn3pju&`bAmXiTIbK!OLzwxYokPmcdmcx5w|sZSBiKQ4RRcWd0W|(~ zM-4M*D<^Xt595?B_YQwxW-xhrYUd~YLlv{@vm3&i$#Zs{%dyyca(exj9ll{kM!&{3 zOi1_%+kAWRl*YL`KcMkX;I9BJB6_YcR+h}{`FWvKewaY48;t`$-LBeUp}TrsEm!_; zRBk+VYk9xlmWJzcUgb^>nw{S~U9SqA^B=C)WNbaZ{-F@-skMLdnetaow|rc8&ya75 zQ`>n|e^kb2^&j;_U25j<@7C8mBlE5_*{#a6`MKH(M?u-_AJzxqL-}Pi5*8kn9Ie?q zLYD^8wbSD{fCDCC%@2?FQ0PZFV6%72>#F^pGpuNM(?81<)5ybtdA!Qw$<~GL>NM;rBoEB@fe3&oeza^SmhNa^mTBvp+1$s$D!!^fx^z<2ij{dYtt| zDbq{_caAwfS4}aM3F2)0raMhh&(AH>#M&(co#7d{tS}3y9XEe_Tc#l?xKHplW#!+; z#(heYa7Q-*kCUiJc>!^gB1k3ycaup-Ee${KUaSLrO6LV36XN-`}qrAVpHp?ChQ$*Lp-Yn?R${=r1%V7IzD=`1YBY z{r>Vy;#Vx>!ka=$yYRsmrj(2I{@IT|i^T@dzFUaGmwxe)qn-&wpM% z`}57S&%beM;fvA>xwyF<+VZZ@)-G-~iyxPNf4_gO%H`|lEBtMC&sW}x<=1t)K5ien zZF6|OveJd+H_rne9~t+wtDdj4HOtTA&-YDNKmYmWyJvs7$q?>EQ3f+7+?3&Q!cE8! zZp)&?h?(%8gf`SGP|Jk{s&|B{3x8lOR_L^J5pf-{CY(@Z;k8+eu)bA7gTUTq&o-H3~3OO85_wU<+l{n1c4M@5_$+LN`|Ah*w=&*GY(e5`Q93_n#qa z%181s>5-Zcsp-e$Mhpd~oC8`RMPil6O=&35>SBcTeY4s%)g}_%KHyg@>w1%&P+dx! z?L36SOOxw;!gp6#VaA<*nU|Z=F5h7IQZHXT$9xQEPn_}>845MzP1%Jig4D5Zl_gMU zy;zN~exIih^79I$1>H-=On<(3X0OPXmqqEyseFBxSt6!zSnuN%a?Hz}Jw`3>4x*&U|1wWxl78d+~C>oKpcMTn+Gk?=0MPZ>ZN0`^; zF*iX{qbn0<$WLZy(f|gHg*!s}KEq32STCQzNndE+b5iy?UdcQ+or~Oj&8xb3jz8}r zXNe_ly{!-H_t_!K(%F6;K^Kns3$X;{BT92C-C`m6$wIo$TBbj(c2ag&@nzwHp01ET zUDLpR`PFlY!L8)-?tgB3Nb0xRKkT;MX;bHi=e!?TrkMo6W0)GI<}N2ax-haFFTCqR z{8Ls*ND$F(4_Ecr_c_b$p*`({yoapJX+5D0OjcPUU^O05awI0#lpQ$ziM8tO1U8TBy8e zS-Wg^P1k-tw2~XJh8UE3YM;+syJ*wP*$D2dw#KE`e%X+oYU}Q*o9LV_95m=h__w{U zeCg*}v%o?woK9-whMd>xHLar^78DU>{8EvP%(00{<_}sM-bgXw( z7oYoO*F@mHUVoFuxd$rR+@j5O6+-#r}bwq-w`?r)o8-aW#zoOdKkTtP`tdOD=o+Ef-KyI5v>#-b=e3E~_uISs zRdd|bsgtfg?psn4#f0tiU+7-5K)gbp4tm0@YXQ@tlGj~9g5ay{s`h@`4rrI7_n=GR zNB6c)1%FV5KktZf%4lL7Zo6Z{&o-xZ9pSIhK74F$A9Aish*Nyh?yKF7E@4T|5{Wfl z_95XN0z8aWS-ITQ)#i0|*wCF=zP+!Hb$r0=r;B}b$>{N#mIFUL9n;m@ZFk?Cy7+wG z5J_(!&)IGx^JQvVOPKXj_dqWbu)VKtJ6cJQmVd9S4#TQxj%r?~z&+Lvkv+#Q0=oNc z8(-x`h2IK2AJ_C2COS*PspHjZA&Y-Keb;0xM`+H@-NH$?tM$eyOagJZmC z<6!Xa8}d=N>v76E#%| z41d36_ZgU*x~sOkb}o8|vm!rKl392Rx=el!%)q=&iI8%6gWe!ir0VVRuf#JFoLyRo za=Hkl!(D3dpWDNR8HLyh%W5B)T*Eg$k(^m!mlYX~dQEI6q$F*0*&|~}`|7Z+6ZE*v z&g{NAZg=m=rfNvWu{qt*vHa!>O>(zsoL}#f8Z$H?do{FJEaNlal4+mjM+hR(;`rYxIV?m$kbrZ zLbPX?_-ZB?6JzX5Vd0$`7~@hsnru=s=A*7BoBFma8Tc!?CRG|aO*WMRkN!c?+<&F3 z(6?yHLLfM{|;q!k!50Is<%Q;_Gump`=g|D&(3@)jgcDJq`DtL>rKUh$`{H{6e>d#}Z zna;$-)OOjdnt%tGVZ+3C4uuTmn12h`!-gEMr7g6~GJDd1g)ia9z2x~K$swKarQ7b4 z&(N<6@;OK?ds3YMZnAE$LKH10Gx3v$$v?%e>rX6JhvFfZjfDi#&HpT!C(xHdCd3G-N4196Y`z^ zK%zzb$i+l$vpaP$bi5cr8^MW0b)H`U)BJuGiN9@jJ5CI}jqCuzCOVFF7vBY6R~e4H z;m9P84@N2P=3ja?%5!QbrJjlYpvgK{9bJJf98>J;c(*6B?3bt8YJW|`I<^TZc^~WT z?fcXKS?{;c+4D~=*2gP8&W@a*k5t>%a=!7AkPE+myXE zxR)76j=VTes(!#D@pv2kxmDM!;PsPR!?_fusp^&Mj1+p7BZabYM(P3#a5%~Ihx`?qp0q!92G$Zkj zW0Ux}oB3Rc$Pp7}onqmZ{_N1v&B!yzF^5nRbF>*VCkzJJlv7oU0CO}=vha@6z&+NN z`AM*Od4TsHgqn0S$iuWF5*$75==Yh9Rth0-DZ-RtxsH8doqwJ6o11%TCycu#?P!v7 z%n=d$)~iDb&e(~MA=23`kN4FmyzXb5;LtQ}mTA;{c5)nSUGTZdnb2Mq7Pf7KwAmgJ z=^#o8dN~iwCb@(arCp>7taIcjJS*g7fQG>Pzd5LY>fk5`t)Hz8ZvOW5U#^5|bY}*C zmV$WQi#Oe<%YT?H$0vF#H-6r9Th`2L*eqHioYg?Pl5Kzmb4*M75bs~p&HH#Aa}c;l;WtsoO1N)*vu)e$Lv`LEo=XQh{NV~; zM-dvSsejUhBMkVgkNhb#XQH)E89}rBNojvI_~*G-^M5QLDY=~f_ofKEx&rT&Gadg0 zE$!koOh zvD|!Y3x8N(5XYO0B2vUZGtom;hEs5Qy?YX;D1oWWW2`E{Orxi)>N2Eh=)Ukl4e3K+ z5Kx9ft8iW}zTcoB%WpZDfUjUpGW>7*+}EWwMdY0&MkDStA2z0fw_iVKOpTU#yHFNR z*laiB23(>1#4LQ3)t%%TP=b>Obzct`$+w^z4u5y)YMcgwf%Hm1pKsg9Lsw$@)l`c< zjPl8x@GNPkAlk=69UI!ljGtH@w{89Tre0UQ`UG~@B(K#ktK%lZs`%4(b+viF;#5{n zgFnfpkY>z@yi*XYDUaJWKbJnZ56`7S>~fQ}@rBmma%s)F@mzHA0k0@7;H<4_YxW5baQr9-EM5R>}PBlEKCD|@$cp6uPo3;`hR@A-uFl^FW#BFpJ+g!1=a6()4&Ga z9R5JlLqaFMcz*9u+T%Zrvn&;ZA<4uM`~`0)|05)UHCalJxOb)Ceml(2egy1cyMM*A zzb^)s4ugGC58N6@ZO zPD5drv3`+SqQoNalvHkM)|y%XZ!tI(#`ID30ko zq?eg-ube4_a8x5Jp$b1~;98`*0Iy3}N^>6gSS$5dJ<{Of$($2Bc7P|kZ7H*Q@XI=j zd@k>q>1~L4)(RU_=-zL$PqD&wWH%-%bvE4T+$65A&TP+44y>y}IA);WN`Lc-lVel? zhkxZcu*u70!bvC-Lu6)o8uM$gX42i1F=@oB^NGJfg_jP#yDF4~A|vgh z3!}+}qjIuPvcjco}gZZFmq2UKCnTneClcv7-{9AgOEGjk88DrjZD6g_jh;DQz|L1CmrqA`iea zBt(Ojj3hZLT(@uYrMt;L#qiy$QWHY9ds6az%uR(gmkfKV_I1{u=^^K2-~KoK$lF@07-Rspma zKNIOfL2}NRFr{(aynAI3mw}}!IKiZQI2l4hR%yj*6~(~oB1jKmCFXxSD#ItBEv%+Z z6$7?_zY$36l8{yaEx5`sh>0U<1}wO^P6DJl7)%7FP8u}BK;DUk=m2PhiINe>R}PXf zF*{MJ18owUz`#&i#e)n`E>=QV4P{TY;z5QI+9g0I00=DBt(k}Daa!e5eS*U3@HMtvP^B3P@s@40vhL3R3;E> zpz`a`RyIL3?1K!9>hYh6P^?-REJI6-DFV@$wh&mTOyQIa5gH%_k_JTqTA~VKL_CLJ zk$P2lE=?+R4}k(PUXPS^VJH--AQ}w~MJL^>sB9nv##I_}7@-t< zL(35qkE7R2$xLr)k_lScs23?MXA|Om-Y88JL%+i(-k)lMB5NI~&T+2YK zqr!=~L`TwGl0Fhq!r(3ifh7ze{M;l<&~1=#$3UZ!ggO$7I95z?jRC2XFU?^LSWu ztl6-7atCU&;TD@WMfoJ_-6w`y>;YU)g%2oH-m;KY(~SBV~G{3GcN`~ zI4mZty-eG^d?YnD%l{3{^-AT;$dr;?jdGHgbyr$Y_)(%CcmRFnY07zXg|hjA&NS1) z4Pk#dua5$T;~kBNdpgAJWbfuRgMg&NGgK|~md|h`%!{qz`C$Y!{6oHG(S*43KP!m( zQ#|-^?PNojKmJtOM+z`=Ri1P>E$%mHbG_JgsJ~1&pQQttU0>`^dDv%KdWy|&}_kA2Z)%UdmY+_z2Fu}ZK%mIHqQ zCuYRzKOk1LZ$rar()mP7mqPsbNd3cnzv|~Jg%4C8wvwut zZ`l8en>*$yn>*&W+1z3Ee|?kjMW)n>qI0b$hT*o(Ch?*lF<|{J>(N*WB`>%z6?gLf zy`n?3h;RB6Q8#fDJM_W}(T=-&1Y3W{{pfSubT7M^P5zJ_CPkkV#pK30Eh*zjY&_6y zBI3|=k*ryIsA;9Mk!* zQjxg6kN9b-)?C_4sO|Rdz%zf9J?FKq&d0nk{b?hc4jIO)vx;x06=1vScvttJ4J73l zOE9>Wky!WW{yY^ZBM_GMyfLq{8}m2MOBdZDDj1Az(dz3u-5*F4xhk25`f66cdQlTC z3g!A^8sno?ijUTw&q>gIe-x{Yx9PyDU%rFofj~m+!L4M3Xx$M~JpF&!-_dcyr__|> zjDv$uNsARaNqYNRsb}vWsWf?&(Wn%}n5o~M+XSPG<5*`!59x zo=KDn|o19fZkKdYrcN%CaYPkjF8PhVA01)i+$QxEj45 z53d+8J`-B-fx1{d$ItrT==b7cs8Nb4-pJ8;4Fs;}#b!=z7zBTQw@~^0vEnc2kb$9t zqFEou_$w9F2&p-Gg&EDh=!Y%t`NDxgAM`H;8}tE>CJ{!Apgya3e>ap(XH;z}=k6-D zRU&bQPGZfXj!U5i@-{C#(2ZY4<#|;vy~)a?EFaNJ5`o$aDu*0gSv6B?SBnLfU*=7N z|NoR_UDn5gBb$F?^T1A#AcMI%t*I(LXX2XUFDcyQHb_%xc-d}zAW0!A_mJZUPu1Y-F~~<#H)W)+e;rA7|y<^sY;5FQ#vuX zvh;POirtgERCo4LP1Z~OYjt;^@=^9f&GMVw>0R_lzb3nx z*2v@j=nwHY2O^*|sm4GbqvdoBb7r|=W7Fvm`fLeFV4HyCa5C!LZhscP7z5uqw_4BZ zI9uBMBD;TZpr~?=tF;B~>Q3C77kTZZK3=ZE;JJoJJl7YwC1DHNA^NR7*GZDw^9Rqy zUGB4G+17=5pTD)YtDqr#_%+>K{vwr^y=QCs^M^48%49~|Rlb!)Us!0TY_rqDcEL>! zef}}8JrN&@yqBRY^P1z;l)(%ss3i3etIQ#%D!G4HM01@!0TRzL^w8$EOAZLMbrDQC zU10rJZh{z2^u>~Jf*hLkNey~>UbWHg0CubncWjVw9rH7x|GG^Oqtpy;5b8HaN(y|O zQnUHcdweYK>5A-C&4ba1Dc0SI(ynd1-EH@jm8Z+G^A%AO&iOo_9)b@aW9%b+FsD0> zlD~ia;+4e8v5AJ_zV5#?l5!K(yOd4Z=d_omYAu*VDtAkzuQ~gwIZ-%S*wwo$XU0Bi zWnYif2Ysg|2e!D#gi7d!Qr*z=gX);l-t&Lp+nGiOV}g)j7zQ09NCJCs#+uFz6W2+8@;E2>%v)^kdps+c}v zU~P3<=LH~`FggD(9KohpXkx z?X?Q%Px1k>z0k+WauQ%fQJN1+0QjsY_U@0*)1fN}o;%-V^D#k9Pg$$1h`BP!&eMNI z%w9~wwWVX9uypJvFCF{HCW;cafXpbu^B+uViSSzp!$rIrVnT#Ikwrg{z;x)kVLzFk zMJ9=SqN{Y-=~19<1vt$E!u2d-RdW(wdyy%`#widNS|Y_bG_xoBW1fIomeN zP1D?+E7HML4K+1$SyEDoQ-b}$OX)xQui@FTAx#;?_&Gi6meqDwz25a1M9u^xl#521 z)s^hQ`Dk3S-=~MESF>2byY`Y#D1_do>G{XR{txO*^cRybW~%}#4wKzxD1Uoqr5Gv8 zY=|IZPN9j+Ysa)=-lNeh(6^2Si^el4G(Hw+Jz7YUAuwYgQiT!8(@HTD(1PAXCJne) zK(at$HQFGU1w02VAdwED9r$?&cHkGhW)A$K&?5(aF&O3okW0J=Jkrf8v>*nlQr0qv zP$~)_W=exOL?{hYkg5j1gMWpx%8pp zM!;@DQ%5pQBPk3zMvSDeuoW3Vzy-=fV+hb1A|yPajiiod+QN)d;wx+Mxx`mwW(lk4 z;Hd+1k&ZAZ?CR`DVqml&Zwgpg;}!Xh>?f6m0~K{Fj7{c*7A8U;2@UsHLAFHW2o3p| ze#Z}!5NIrAFK~plL668Y)&YnFFDC#N&V^4wD1ijQ1%_V0yQDaDcGeRyXdGDyA31QB zL7@|=!<;wZ8xRp54_SwTqK-w!d5RVU1w}mU?IOh!H1mS2hT!l5&Xn+`H_B6pF>etQ z5~1O}Qj?%)a0Gf=|CW;!X;gph4@r}}``Myv`bA>U*x-E&IYrO%8-um0mDv)pTW4bf zqG9BX=;t#hgN{HtzoA1thAp8eLfO_5AHOZNTqu!+^0t)0oc!(@MhSZ!HRS~l}; zl6VAQ+!rA`qf+&`{i26q-j&|wypGZ@`<+&$UO9k9eFK`$DQY@_`LdfN%SUz^0C1ed zJRrmzuH`LVTA(?(B(4i*Bs7xoX}GjdEySLULc!z3VQhvv19I`-oNFpAHTY ztGD>OT?p?I*I9o~x#)iy`RkK~{6X041F0tjPrH(5DG7e<;%ZCVyL}|tEjDjsr|Q-S z`4heS=Ggb-I=8Eg`D(&qaB=nva)9gcDF3IN9O}kxOj`@hoSz%gOjI|9v{SEvlkC1d*%< z36T>(0t7E6$HxPr99$D^0-}Q8rhh*2+6Tzh5rIIk zpwlA5O?`stxsY`3^{UJ{JXE7G#{*-8;lX=moaJ;>XC&rUMC%=Q&iA6} zsYH*11wk8VUMs;}RLmI5h0_RTjIG}7A$2?)R%ssTKo>Kc&I$#u&*U(aYBvb|5X8f@ z{jY!jhYpB?vfNw67JsYx{BkRuVJ>Q}j2@Ul%`zQzLjFLz*19DZw z1oOy+(s7{%lu?)s(a;G!a=B5%#JWDmmI&6cn^-@ETJQ8 z8(>$pAF+xCstp)do51PBU0GU`^?sZw%Zu36jiDr%7v;s>ctDb~^)ieFzN+$a!PI*A z(|z^oq8e8@TQAq6>>O1@9o^@Kj^^X0=0GG$Et-L(dyvf_&|S=i`#Ifc=%!9_^v07W zZG?ZNhv{hYBiIyH3J=2>%r8Xh-8< zsFrLTWU5irSUn6;P1o^pRTZIc6?Qe0@gA2HQ{&b$4HWe8k^0mvHF zrKS9Z*`TC7YAWs|T)OV|y}9pU>gr^;PVU>0W9i~DrF^gsP3{Y{k&u1ax0^h-X5Y?Y z&{;D&IkuDKbzKtwMcFq#-6ThQVD|NEd~^Z3ZkzFD6CBMkv#}JCD@;tM4R>@MEj)iW zlopLfvD}{qtMS#m&4PYw7TlKhWGmf3s;1j=$D^#ngVUwBwa7t&whbGFYJIZPlapFP zc3N*6vWx83_pj;y*-cg5E&rKix2r#jNx{o}Jgsi>X)#&yS&`+7Y*~ZOZi{I?pDnKW zO?5l}$JKc91^eck=W7N256Fg8e+siEZ&EY@IWm`VECeW%qN5Uj&~yCY$;In9&h9$B z^WxB~`q}gE zjh=DEq%vZ5eLK@?X02vY+S&DT_8}8jzh3`vcwzQo&Uv0^Z`Kd1YF%xvB+uHit+yK- z_WXOrX9X*aFbFDtpUru}cpf4BXVp|!b7fWb=}NfFU(0M+?XKoLTQv2})m+#t4tcl# zVf)SYKXw7EGc(Hyn{x#FVXh02Ef?!;yKRVRKX|z1qTQD;fL+afDT}}40q}c;@ob-e~*5D0SiPNZt<+~Yxw)s+$fH# z^UJ`2`(3;!!_Q*70#f-)c#*(oS#7p0(g2pY+b?jQd;E__OuHvH^RjJ$X8X3F<8aS6q4~m&lc-vAAZeeOLzu| zGh5i%CqHsM6VkC9#4n$%Wx< zxRg4#*|*gW~ez}S7cSQt~cchaaSjAE0?kl40(TGtRo`85Cu~vQpjtD#dfo6$59Xwg?C<2D3g(=i&2v^1QR%hB+pw4Le{T>d4sIv#D986Gg&Yfdi+d#}HT$^p!9*UAOw>p!N zl0R*G`?y8*iF*si0Yu&3d+<1H>p)?=r^g9@{74a_u*`82s9_>{#M_$XMIf+A2>eSf zRUB@5U@g-^z~Yalib6>NW%#T&C<6V8fLO3xQUC{gHUK!bD?(UijZ0X(-YoZvGg8*E z_;;&n0k&e%?alJ@rd-#Hb1eR2TwqJ>gtJT*^67jz%6SE6ii?Evz=kyIDb6#U8i@mc z{d(5kT!A57omt;|QiL_rxx>AavgoV}#bFK{q~4%Nt8d^(WK)DQ%yRX`@PEm8H*Zk(6FBeV%C@5*-H^n6H)l{+2tW4*4=>gEwK z=K|Gp%Cqb84*~?2aH$CmT@zppGs=E{AoHZsJdrSVG8CXqWK$ z11UKuk)&lY6wc%N&gHQ9R1jACyM|WhpcyrIuTci}uba5mThQuWaLx*Zqy)l$tA&v{dD!FYTCLS)g{^jL8SVBOLU1g@M~q~BR%Zfae4X;*Ij-7gx=^D16prs-fg{? zy6kC_M?YfKmUq?j=)>-66uamC2|Cqyb5XbW@8^--Oo<7xM%!#xtEw3{dYyaox~w-C zQnp=W7j!EUs*sA<7IpfnBrxH&+Ndy&I$uMOe8!tsyKdsBc;aXBC#mb ziZT`N-$)mOTdyz#kJPD2Ipjd3-k?h$syvF=aumnjpa^jnf8S6#Q!~!>pYb6c$q2+b z``24(tw6yS{3$7jG&7i{B4%1hAeK5LU5c!{>39xvR*q--1#qTN*}EUU`99n^WOMdD zU54pC;Oguaqi($DXNr!0((+-hRgot7hB6HAVv-M`VxkYTV4P>WhZV@7pE(Aahdf)4 zH~ z3dT=!te~HMZp#+%+@T5wz5eYt;TIDYnEM$?;sFUw1bpoicA(^cw)++}AzZtItK{$B zhTo9cgNM80lz2Z7-(TLsvbUJ$`xKvj_d5&kbjpKFpk<&5!S$ASRj@5;F_um z`P9X>i8uRm#b$PUvJhh&pFCYyZf)NiHKNhTBry(M#`R%FPv z%ARD)2oUc{5H3K77DOuT6%L5Z8k5)ib-*$5$|>lnPzx7&s4DyD zfKEllei#LtqF{E|lh0Q^1MBIHx!N6Igc*6ft~X?YS#g_xt^f6Ii}63Ta-Z_u1rU*` zEim9}&~LX(Mb^M$7ucrB*}qQk?1vkri#TrYA>1+gZJ=*2a?r%`f+&fX7r zcDODBs_zbeJf4SU{UQG~!s*Y4%m)@w$IQsvk*RW#eK(LuPm_ZT%A*VxoeU0;@ord$ z48H)Gk<^1?xlYrTNBiE#TY@8?lF;B?`A6;9BgmvoWO7&*;|_VcSyWBAnc8)oGXoeu zfs@DG0^lGR2OP^x=Xala2AJtJFR-#F60-(2)SESbpSJ7e;k(e`sCa1$JCJT55F#_j zd}*$GMo`(Xjii=TmS?Zy6Ola8;C#O%-@zwuVS(`c{YVMY1>K6!Z%SXt`)&T+yYB-s zH+2~UF3oSHRz4O7DNugDn@rLrN-&SJX;`|nGc-XX)^JZttF)O~HeuFwoZ zpb2|_vuzH+<#5UHD!o?iz;o~c2ksYa?&mN;d2|^Fq}pm7ORS9#7U#<<{tt${D=A8_ z&ECj)-?0R4+W2bmh`oc+@5!BXy%=Xe)NAquE{lFjEm^R)ozeyu2mtp9qy3r&g1#pT8m&0wFUNhTQmisz1M;>Z0}h^J(kZJ#Wh8 z#Vd+c))$W<&qS_CX^3 zxhT!F6e9`6WKo(qK&UtF)xj&PMol5e0Mpt$#avGY5QPPp+%j#a0tkC?0C7|9>P5`o z9h{JhBM8LiG=$JX9D&hoc3PZ&bRG>oWx=z1%swEr*89fzXB%z|yZWd}Ea%zV_Iv|h z>}orW5kX77tw^sHJ0(HV-+)J_bmx+8(Fs)Ah8zsu5A5M9AR(;&nvSa6p`mFQRl#1q zB!VAq1aK{2Yn_B1Of+x9u1gUyiEXS1L zA&aFF_a^}+ecMQH&}E>gop8*-MQ;#>7Z2bjR?Xwwd+MkvS?*lA<)OZp>u!e*?B&zd zaQ~)Uji(CX`CB4~=L(-shUlJ~UPJI;Q!V&P7KGJqMkr3dW5u zI{*HYi?AMlrt$owi7uB)G3?n-CM5zrcS=Yw4$r%)+R#2|ei~g(5>gr_VDm2~*L)ggiomLQ18!Fe&kr?#|CKu zoz^AOo**=N!6cWL@AYx~#9qfi=lA*uZE~*%lhS)i-Jdkgz)TUqVDp5zoY{Nh#|h94 zI!k~av?&4%CnX7xOgbLa|GRiSI((Ak^;iROp#~yDT@tu=KhFCj70E*Ta#!CuI8&00 zqf(P_u^@jT_aq&(VVd*H>2MrBL5Dc#JROeECh0Jkl%_*}QaVPT%@t=($4XP+^V~6)CYf_lX!j$`=Lfdqn`+4XSqL_a2mP~=x<$- zgzljxDZwoQE2<%A8?~}=-<{|;{a9x5)WlZ3M)AbR3YI(7H?!T;kp}9}x#lC$`|`G( zlwYjcG7i^?7{9~TK9Sv*v8_)7$9~(Cmx--U{pBLN-LF>RDpf~lK#g6!tVR+D?nt&&X=8l7D%TG$h>Zk76}bPx zG1#PG6HB_YC+%FDswKn^50i7(7wMO@WPm6xR(0$i7712{fq(&^Tpjvcg&@#(Ok=}` zWwJX)@b)xkC>_{hF;h;lLdvO}A)bFtQWGgd>A(YU7ML6y$-GEEw#;?@M+w zIMbSGbqI2VB+w{e1ZI$h}Tp8Y7x-)r%6bZ!Vwea z&r~bPdmEI}tE1JS>u<1A_%45K)RoG!e-IqXbBr-v!lIl^1}odwS9Iw`12sDo`Co;i z#SdhpT2;^(nu0)aIE&WV3v5fX;m2ezXkW({M18{BN}Ih3j>w!B;A0hn=mQ$-w4HK& zx}dqdP+9=oq_m(*6iuf0o}%H!|G&UH5@7Kx=;$-B7@31+y#s5DRZW8}O|gj|pENZ%6Y&}Zm=M6wr)I^OJ~czdOW{RN zciWF#eDDT$d@HpLF$w$|oua0W1AQ45?)!enFmSo--u2!d{(83sfh|Y{64)m+h5Po_ zuR|gD(|y%cBhsdN9>af0Yi3vw4jTJR3Dzh(zWunWHg}y2oU*tF$10WpJXu0hZg%T> zx2w0Cx&H?ivhE__f8jR~j?HuB=5AF@d|*U^J>o{sq=evU@^lY(U{j5nXqr2~0+-9^ z@wPbwTs_jMKGHUKgp4PwCO(I$%8a=h6FTgOV*2gV%4msU6gui@P~e3#1~Vv9Z(rHy<~JP@jP-b=++8oa&kj+n*&dZ(ovcziyW<*7aT-6VW( zh@+okT|VprSpOXdoNyPM$5u*;v#8=3YTKjHFpX0|AATZ?)Bj@S1DiB_EavCR*j}Djx714Dt zohoi$SMlFigFS_i78%f=q>O!2$b_c?-N7~mv(qY_L^M6mdjOWRTumOIk<4mGeWoBe zG79?5a?yYPqySmh0w0a5UP}IBbqltb`c4zr2k-s$j%`y7mk`|=eI7f-1Z)R^U0Pzs zjzEKHjj?W!O2B*ZWG_D4mFw;n zoxj9EK`VAGIy?(GOpD!3b8Em{RPBiO(?f~uWFPiHdOR={>vIty1A#YB`l`b1$181o z4^&t97vmWG+Tloxq>veXZYKmZ2!Sp#3=>0~r$H+Z9v1?5@&?LGWHjfqvN|t~9W>ytI z08Ef<9~=A9jiXRURvup-MK-&evDueT*y#U%UO##Mg;O)1dn3&3`gW$(%vzg!VQ1IN z+4tF(Wl`T;&85h;SMcZOtGO03d-axoNwcWR&En&gH(7r5&+D(cPM?3Fc(2Fr=bSOt zb|f5h1b?6-rFB;JI)Xl)e3l**5W_z@{*dIp?|8I<8_@ zJ`{D)l&}!PvZ86~>So`RIj+WJ*H=!$tm3=HV!Pkq3>$pFv)gSQ-!yfx*{!R8-LBeh z;*-^Ow<}?Mo;{-mt;^+p0YfUzv*tcrux!0;utR*d*lu=B5yyH#u3meey-si3aOu`% zPicxV6DbB$Y(E~+%?jADxv{YF`#?XK*J4}O<)T@AoL&jElu{x=K3oZ#74;&k0dPXVg++>JX@By zC3O`aFXO{^d!)3@nyRT_CB!0|`vSMz0}DMo1V#mp#Rnz}8xAj)MN{nV%d*+U`$e&d z4I*Qg>1nqZd{u0g@$r5KkEn5x^E9wS4eW$7pD%FY_htC^6TTFYf3Q`5N1N)Gq6tI> zG4QaFWoj5UW4=o(pohGxizUch_CTf`M&DGpxFIx`hGn0D@-@`l3d~U3p$=vWXX+iL^$(`r3d9T%7+IFBME+WuP zD3$G2TO^6i(*5=dL_1!8rZz1j&jZDPUMAPZC&HPeM-F72E9bIRwJGD(Tc%miBgM_C zOkB%md{!gt;>DXUpP+O?Q$Ap7=oP-@u#cXleZ)rVz{HUgicPgZ=EmnuwN5QM^A z$gZblGFja5|Tv<0&fj2yGX6)9P8CZ79c+#Gr!q-K!Z|5@{wdG2j zR@5%g*Jit}HbKwADA+MDVbCH~__yEgg9gJ-X;qgjg)(VD$6TaLU(NK{Rq5Pw75O!F zxFPrzcxTpYLNlwFw5*ch${43VlYlq~u9hq}-VCO;E`BC|$jWst`m;jyB89;6J8by1i4)!dFh~O+|awH=MBc~M0ZtLwj+U{5H z^7yD_J5mp3n$m%>hgO4cVn07^0$%*EX-BFSO+a7=q%a@wT2dGhKEO8$5JEsF3W1(F zqoA=F_v6`rT>@o$NYq(`dI2{t%T>92aT>^YV8?H^8#E_~IjpvwtXMF@eWXZo z0)FtC5E&tOE)kz+%i98q3>EyPOS)_ zog|l}P0qO(@ODE98ZJrpnHz!FqehYnx)|ykN>g%wl?yu<>HQV#S+Uv&-++O&DeCen zjsxyR`4Ge=ym)_w`UEvGp`n3TE|+Kr`&0|IEilB3*9~nhm_96ar1OnrgC5Gs9>6~W z%m;H^Y*(w|0l{l{js_JU7nIQLs?FUhxdQ0gNpLApoNY$$GPZk7BTD5-MCn{^q=hvZ z=i-=u4?(nET|asEgeOtvn&rUBTKe3<)-2XfzW*niE#V!^7)a`7A41FZEbyg+%hl|= zCx46n@L-NzSFlQ#wt!+5usVj8Gkr?VF6)ZgCFHt6S7RMz%q;Sah|C14z$h>Zg5^57 zT_P7FO$WOvboN~XVtKnqJ8yONSu{hS$|!z+Lk_SxV@dmc3MvVMYjI&^Z)+lhtEIz>$lh6z9CJ2+_N}F(zrB2XEwVlDg;-|WC0?7sG8*F@8bJF zc9MM<#&PBxyzn>l_VOqRVZp0%w~JT`I|XGu-6h$V^@qMd14Qx@nm!^{jQB)Bkm>?K zz~+>~gG+f{P@o5}`k0j|g8Uc|YdQDd42RE2_&~ zwd?7_vFyO-C_C?~;!*6+wa%@7CHa4#P{GP7b3P7(5x1`=RE6zq;FL21HbLZ8iJ?!^ zhX8Yj@rIyV@oH7AD{%ksE(z5J4L-M^eS%uVi{1Wa-o@hy%8vmi#w$YL^F>4z%?}3` zVr*oRpW>a;W%*=Yl9JU z*_k0lqcf!SVpToFmtw%NE(c#8eO{(IMJ@zE3fO&g(?N+acY+WaTr?+s2ST(R9TNgu zfveP8`6qNvO(~50nLk)PC=C-Ipb#=w`jT+#{LqwvWFBH$ z-%xAl&3~k0*3T2D)?6ZVzu$)F2**4ESD9UD)bq#ZBn)PDu#&a zYEJnK=+5k(Mdy2eS0UU055u;ejUA{b+TrZt!bhHL0)S96#vC`3R8A<>Be@())LjE9GH86WPd^~368d>FK7AX$i~BX8p4$b9$+^>Ap| zDeXL_%mY(X6zdQ~P}BFtE@J=9b{LC_cOQ}?Ngy;eVaq^&pBsA-LeoQpHf^{%yx93| zt?>!zYp}lUCU;I)AFjXaqh@VANQDOJ7h@%Ay*)&`z$Z}PLk-j@d~$$WZC~8vFlZan z5nx&|ztqy$3oy3deOzBjh7bY%Zh^&x_cr_c2KVzw&v*UuTqrl5qv}WGs5v^*+l{s_ z#sOxTDQpjaqxA8+6(CIM)nLV-)8Jk9=WM12Xu z*2Cd7V?Dh-oLpb855McD3rwFjQsyXSxNI|0>x0FA0jFcYpx5qGv|zx8WnDfGYhF(B zAh$NROg%p$P_=zbUEQaqt9=KrDK-D?=dZwgT6HGyYw6@7ei$$j{7<3Y z@aQ%@>P$D>Y}3Ptm*Pde4u;(JcZ!UCO!r^wWgnA`UE3_Hv23D z2E&b^O_$GY$lV_GKPsdd_21@WT{Pi%F%Hy!=9rvpOIGlGGnLH4x7T#0$tE8iS~P7( zP1$lmor-ak%&m|k;e+ zI303P%Cz5r3(_OLa=BDP+|bI`(M^gW@8VD{&G0 zlJ#;w%$0>&(NM5OP9Agjz$nYe>Zs3lZQQ`*k`uudq1Zt`;!Q7F816AF7%oEkN=Sc3 zi#bY*!7d4qxexbMTg`+$-j?NZ7cXyrineSJ?^42JOWKneAh8fB0?|4Y{Kmx?-xg5` z1EhJkkB9(KxXA9SyZf?^t&aB|KLDo%oQqu|TzC}7LApDEM$aPnU4csyEe!HtLhrHs zbU6{5i3PeCx{d#O^&{9xuNdnDhK80PIe=flmKvu|fdg(;h$jp|w4FI%H^hm5w};L! z|18`9MT6Zyq6FEo$OH(ZQ8mTVXoS(|F-*YdL(hn1%qbjAPIC}i?K*?)cgAF?>i)%IvG>uP0)E&ct9N#6KDe#EsF2pP8I1F@Uu#MO?271Di0wv@hu; zaYYke=N#Hlqx3rfA5q%=C^U7rVmjNA`vF|TLRkz&BG3klZFx&m$Ong)OL{-Nb{T)n ze$?e{&_2{yQkl-o41Emda*igWM}lNyJ?rx)NRLx}LcJA+Q8RBT_z1RtE!K!cn)5UF zXJ%gf4l}1Uj4?74#y&z3PJ2lbh~dFmG0K2;HGMA$m0|LfRK|Jol&)K}jZcFu_E<-E znV&R{0cxI0guucbyxc!HeNBN~pKEXb(Db#$qBo3mGm&e3Hg(%JpZD4y=WfTHP9|@O z(TVKs-B;ch|!$C=ZJ$#A|+JiA?@?tvl+E3G=ZFH6nz1~jIVLT{FhryszCi;=t z>of!5ZrH*-F)5Zltt*f&9F}1EC6a1Gtl<+d&9iqZRNC=4E3TdZ7F^KX}&06Qa zfRb1jcbkyvWeBgO-v1nXUX~>E#6VZ5Qe*zY!PVs8E=Is{v@TeGfc*}u-xw;gZrF?0 ztm!i&RE%U3!tCB<3)HRKhc-YH0<;~u@%n6$Z$Ie*9H=dE9%t|=*ba0Rd#&g?1ef9q zIL&2Slolry{f~GNiHMDY4RFX`pU}D)C^=Qp#M;9XbFXtI$C+_(RGQYgX!8citLfre zb8zg+)oQXqJj+yMH1~m{&DBLVD z)uGE&BOl7m+62EjMB;20iDQS-O4-(B;KkSbFY9!9p#4;|>m2_oT4wvX^)4hA9;G~d z(^_dG`ff2VHJ3c`{|Pu>o17p>rvQW}A_J^$9|n-%_8DO5nK#Rqhct zhamR>g=mXh5d6XrT}$pe#u;z-A$H$mhjBU<8r@9MlH>4`7+RE?vNsqhx(dpdH# z*@TkBK018U$|MXkI$ILe!78#Hn5Mv=^N|1sb*hc-PK03pBAPsSjuEWHD0p)?HWEJB z){8P;j`E3rW_XGaZIlqb;$zVkAB!_I@HZ2u{48jy7&66z+%Lw6nMLCYnpqO$DY^4%uVWIBXa3ArHjWEz_P#hRjQqi z8sY(etMgaEkag*%k4gEO1YQT!F6z5-d`5Cqfx;Pe+7CA0_o0v`W6onJ^p**I`5+ve zYbE=m7@8u+4Mur%WlLQw+Y&l%GK||saEf$EM@ZAw#+z?Hf0f6pPunuW_+tk1?nJG( zqn}^s{OY*|W+U}GhT>rk+9eI9v1lX}%%C!VOCjj^;*9PsCK7CqC1Vf=`SB!3abu7|H-GuN!5}>egHol`a+1n7C#W3$O&yX1hg@ZU zul6hO(xSc+;PN>;w(tiq5YavGN>G9De~-bd7)Wz?CVh2K!y^hp;~!}?6mv@x+@~GQ zM3aZ5vxd|}KTc53W&bv@gFc%CrHb)h1s;Wp)(gv zPNmrG_UngqOr43dTRK+f(hZ~0p_6_UO?;Am??QAu+{U7~KfFt4$|tI6$T|mqrXFqT zz%Q9TjWT8GKx0nj`^A*0Lv@e>=PJ8tf6)bRcH7nA7k@BVj_%LuBUz>|bB=~exSa9i zi~mCX;zVxR+L^@DKN0eQraAi)Rrm}B(drA2?3Compq#^QDVXiBx?wneUpJ}23o4NT zz!LP{Y~Jhjacdt|ymCHRAhn@?z;ql_O1VvqZXTzXNYaShI^X{tnO{ko!5N8kaHrR6 z4+M!L9_8I>szDA-NtSEaH=%XT)XDSb3Uhk;#JL8g!IKp;p>%G!@2zx(HOX#ppkqQ} zv`-`3eWjfKY`q)Nb6Y4xv;v7mTr_k`@XqTO=+i|((ZiZEAcslr0 zy4rxKXn(S9Cw9i24@V$>M|C^8Z_1oJ+42G(QIl#O@WrTBq7y5-eRMmhqAx_t8l_Cz74QdMx zdps&S2oVReWI_OhbEC?VxT-;PjVipkU`O%|d45XCfKAuRJVqU!s@}n)vU&z84`7WSoN)-}gQH3eM;$~e;Azu`x}SQv ztaxWwb%0PEE(gVbJE7{pq$=G()xmM1+1c#B@K?XirISKAsfG!5F>SmV=$%tPuQ=g4 z>)iMuAfjN;QBLFc%iPVS5*MauDjnj$&=ESN@odM*U!@b%5kg>LZYhF{qT0^! zqd-6O8%*#n96d;7V(~eVKa~ny!1l*H1q#z}%i{KLH;2J`L+?Ht-&4~mw*QWx5=>3TcfDay~RyNcQ<;;p+QlO8G{uf2unS7I;(I^8sH8YoS zECeWj)m&|p+cuK^?q9*%FUzS|0A)a$zYN|%_$pVmo43ktWh+j+xew0PmMqaTI%}kf z#Ie_VcmMsK20)6SKuI&1Wb3LD4+VlCy3yTu`UPagb-{|SpRv*3KfZeQ=~u=SoL5e3 zQM`IvXjNEiD<^F6YFWH4MWwI)_UcdlBgN|(=R%j?e|_`C)l6Dle!ksYNl~`D?Rwqp zsPCs=DPFkB8DVHdR?N7qgs8)S-_AEzqAow;Y0k>|qP?HD+uc6=Y;LcFE#F+t(4$$# zE}QUoyP1U>+bdqqcm6If7h6AE_j=!RvAw;!Z)Y?Y|8`GZc>8K|jXt$1m%G(_dRG&E zzNbO9f9rOB?HhFYcDKC^x9-AA_p5fb-Grv?E_CuwHS4YKu~@YoVd#Q6+|D=4eU+QV ztE*aq3{SvqczW5a!|(n5f`+-FU*f=@=Yg}eDX=(}apLUjnRK9Jal1v24Ci0R0q-IS zyLKPm5m+k9?H$oHUng@kVUA(729%fKkXNwQM=WQW`sf3ld ze_j_qJbM|H#}+mCZ4~jl8>_A>W#hE}a3x$BrtbL^*d&xMM*h3yO<$LHKRPSRie=`g+<9ZoR?9zxTPQns&c;N+8NG8j#=c^3~ zl9J_5OtW4;XUm4x3M|+xo1h!G&!?A3n~FnvA0SexT^7UTG8L1uFw#}r)V&p{%lVr% zSuCweE?+>ktOypF4NM|#9<-+loxW(^&hOW4&sKw1$+}x@oydYnnU3M0X;TlTe~X_y zT2_f3c7I1!YsQ7A6$q6o`z?*Saj2#n?RAj5nb!5d*r3yTO`05JYHT&HgYamru|?4owJGoS z&2F}jdcIiw#H6CVjr!2_&hL3%f2-mny4*Kyi>+!O2KcaQZ$kV1X4UR<*0H^rw^uXE z1mxMj`(ya^CT>Y|eLMF=+`lCgz3|rPe%I_E5D-&detZ9??dRLj6;s~Rl`NO59h4)k zwjOQkg9*fCCpt0Kd_n&<**!`1bofisv8KANqJ~SIc7|bi7w@QntZ`mfe@rRN0d`M? zDcogV2X&FQ7<3|`px6HR>2AIx{iWT9xws|oe!eFX<_kY{5=C3_nx~}FjQMmY}2kKaS46GYSQ&BT|B4s#P1b<`u zwrw`ya?>oD{hmZIqbSS!CT`n=gCu~&=G|sX)BmsuH&(&MHah|+v5S`)y19*=cFjC) z|2G}@*eyR0@1RQodl7B0CR|pQi)cA-yv-EH=uz{C9zD!Jk9lZpf9JdFrVUL)teBHo z>i<3u$6&A{u%t0>n#`^?>*hPeV(o4{F(B)ZCgfOSJNyG#Cu)v{kuC{q*?_e|!%s|@ ztN8HsHKYd%E}ba%b6A9V7C6&NlUOhd3Ra83&!%1A-A0IRSPV43gH(pgmESC3J4txD z#${aTeTxOW4Q)V+e`n=(f~}^cT}>&!usNWhxE=0jqG| z#N`N8b^w4|SxNXi(zr@6j{try(DCcS_kPAI>0nDg6x1C4>mCIxufWB^!PGfJPvSRF zL->Dqy|^vXQTrjd5|w14fD!^;fCU)k7_4@^x=x3Nxw5_`lLo0WcR$7J;!%MJV8MM%d zaWZ|&;xOyJe+|POKYBd=@jMp(!O@i0!^V!ZWBQNSgu!kSy1JQwsekMirCT>vXj(b- zAfbIZe13KqpkMMtH+01;5NBQ0NI@NJFrh3{6v%#h?=*#oUWJ3IR=ruQ!B-S@wx= zPl!B0f4mIXQW#c#l6{=+pw<# ztX-7BF8J^p%b>kP2JI8b;BaJaDuoS(SapV>e}SK4c0QfZ3SM!-Hv=kHnRgtd_8cp6 z;%qA;`B`yRmC~1q^Cb#Ag(std9@yu>W)gRa*340& zrouA@KKLO%z&y#x?$UP)+YIo7BY;M?ZvBF5#eqPur3pe--DB&+y;*)V(|f50VtTL7@&+a6)CbchiK3#1$#5WW@D1m4MSW)jwE*A&m*9xWw& z2;9Y#g?{8ZYE=n`4N+PK4(SOuGxq8_`E~s9sy0akCF5+qKU&VwNa=D$a%LSpe=mzX zrp9V=gMM084|G)qglmjJgmcoTNy0;~u2UDXHzCS6NjHUrk4aC*8j&7I>M5EMWYX;ewUTe|mMr%Z(4`Q~ZpHB!ZNrHn=&J6EP}@AemtV2|NGI zmNGIQV)Us#diH&0R7Rmd)k%)eZEA8ged5}M^f(%PJTae)QG>wYC=>JV;o&Yws6#^5 z^+Cdr;|YI#01q4_UM=RttaY4Apejc3^2wb;k_U(|iR(#h@?Ag{5>@fze>wxr%I~PS zV32&5F#h-}qR)L?Yam!9qIdeAH_av=4sX3Zm91I6b<6U>G~Qmu}#5`wjq>tv`^{8 zE@I&Zef}f6rzqegmRTxTW$xi?vh<-g5ez0J0w!%Ep}s<78Qc6)(uL1?UpMW9{K7}h z%df7asP1NE6^BZlRok6PhHX;wj_ha(;UkKnW9G7`At_uJQu> zH>1jn-cv%FvUCB~0^ z+sv0AXZv>U{Ra_kV2Y_n7<$H;#zcIf6tl9+;QDIZe`sfEv+!e%2O7=CIX6JdNA(P5 zQiQ|>yi)o+s|ZK3DF{x%^6kc#Y{2>~m#bK{>Su<%kjxS;ddY!G=@c_}TZ;_bn6UIJ z55!nC!mATIpyS6oAnfP56HmN&8I9`oc2M54X_j5xDv9rl41***K?-IL0eKFPyjD8D zj|0u|EL2@z5z)@%ye*S{2XpDP07L@{8g1rY3O(WfQU<&B(FRb7iL9#6^kDX9U~ zzOs}}yM(ITkQa*Aj&KcHIvu8)`|uYs>(`hrH}RtN1xv9bcC0A9gyqKj!6A0xE1o*D zLC)VcG=BHRNlQU)F$A=ZHsZ^ln3Rpz8v4pk^C>1%QhS&%cwhDx z0&~Z<%kiwDaDPcIT9m6lAJ+=Q(pS3Xg=MS~d|WFm2PNIE;ut3>=uX}yp(#??!Z?PN zf9~u2LIs>*67#6#$d^x**U2f}B}0Eq$taB#CP>8r@#P{zu7V%Q#RcR7K^i4b(W`;L z0ZoqS9lLVCqJ|ojIp?~nDOYZ^;LKzfoYs=5giSL$;XbHJra0p+!>w9Z8iIcTw|o@& z6t^64%VxPH5ksU64oxnY#<+9nhDm8EaZkv&GGufk}Bt@ z;!4>^g{HKf-Xnoy&IhJ2KrSvNv3(mp5<213Wj@WmAdztTMx> zR!*Kb7@B0s*+;%aGHhV=1r|f|Tc9O=(BL#cG^$s*@PE&XFqZ2!$EX1vyTN)>&72G2xn1eP62V+(UFlv z$wnTaa#?AG^DZJPHL1X+e+e0792EP|YeE2Pf6&0Oc!C5kji*H>!c3~3JP8uQ`ZUD; z{tcb(D*uZPTKN|5*7;|XlO_ce0GpOfMLrzbNWGt*gtR9-ph_Q|>Mp}ssL6A(R76t) zGiYoG1gB4&YV?H~LY+7$+Ye^J6Lx%3w)J@q1y}QGampB5@nM~!IW?n&jNo(6&i=! znlJhAr7z2PYIu=3EPTl5Tw2aSXasw=FY2&ehM3HZ$QE_{c;!q8+e_iY_W1Mh(&`QZ zX&0AwbOqY~56U}+g+4r#cW`kyc*y0coG$L$>1+p|2tqLQf3o7qQl;EZrAqlIA)8B; zno3D=oVM%^dlL~cZCUqe%gCxgH>DjZK3t?N$>(Qfc{)k*KlDl?4oj+3HWQ^lJRT?No31q&aopGIe>?4Qfzy=jz(Wl zM{|}LS#Ltve^~G$kBa?|GJ6`+rt^ctu>EhH+GG5Zy6m2Q((InTB)g}7VRkR( zZMqZ11LM@j@6?Q?AKOSRQhPL!`jeBXKbcj4eCkhMnED%iw1dMvqyp^WJVWV+)YDN0 z1!iO`U3jI>8iqr0&97$|Y^8L4nPG73k%l1%#3r(8e-~pkE=FuZ7k-gc~|zu#Q}UB0&lRH z%<8Eewc*16&px)qx_ln1vb;FhAP&}BMcjJZG|MhRN#*#s^NL{FO?>o3tYuUqJ@uUk z*Bfq+|I)zA;A14|=isROuN7o@V5Xj2q{n9Jf2FeLGW9Z_sqc0w+ZP->QHout3KV&o zt+kKLvwKWypDynuA>YQj0!}$Vko-l7FJKxN23rU^HQYI_6M0wkIh%LY(kFTa%(C$( zGGj`Vnfh$r6;q*|xqEgx?`mL=U@RZ?5~F>$>-V#z`#Ua5}4g$^)FB;RYQDEB_@@EnRn^}g`&hHvu6{Ng1*IZ zB?1>7#&qm>9eZK+l zPw^atF)`M@flShPvf4#&hN58{uLIcxMEs3yKI^ROyyHSSN~h13;VQorV~gb9fB4~M zwQeR}nrE33Kw{=>cr-XmT8y2g<(@tlIw*1{4{^TUZm##MWi0-rCNCk>u}{_0k1ieR zyCix0jK6a-H43;kks9Sf-6IoP`RpA37vvmS zJ|~)AyIT7ipa)3U>4clbW^6DY&hua0)iKldBPHTOPK$+{7V~~>KIP_5q?4H+X5C11 z`vd7`6L6^w@^MT;I|{VM#j#_B{6F6H(U%HkZe(+Ga%Ev{3T19&Z(?c+lg8*b0yQ|7 zaV!KVf4y2!ZyUK0e%G(yWAB9`Ih-L01P9z*nrnd;2rhZJHV6vYQ4`|~E4Ae|{rCO8 z)yma6l6DjC#V|nb?kxFn$eHIkm8HK*h?%WkM~$l%va)>%l5=)4H&PmawwU%6Xr@3NW4&E#U zFjI-HRBjC(*N|+Hia02TkBY;#0tWV}3U|3QbSk#K1|B(1h}L)`x~hR445QF1^jx(Q z#c-B@7#Eeh$I(jU6GMkb3%aYddznX=2ZnD~+6wHW@e1e3wX8f?(KYQJ;h>k)e-_W4 zEtaqU`*H1-Kiu4Gwu|Mf`-^RN{gFuYygmvK z?ys-ko|L1v!+zjUkI7ji!13`k3l@+^<#Gu=wefl35^!GXdAXR1H`rmUf6-Ui+Ss5m zsFyf0xrF2mo_>;s_QLRVzO)fFi6scZVdmx)j%m`!64mD_(}Z!;*GIeMmOpK-H@B}o zt}fS3cJI7gZMW;&8<%#MfBdxl`PFu{UGu2o#$x$mbF=ME;tP1mn0L3AFW|XjPWubY zmt}tei>jf&fESNA3;h#ge`M@0;GVEqdojk}{W}%g1JB;Sg8^he4}5Tx{S#Vlw*5Q6 zrh5bkWD)rK3!*Oj3!v8hX!J??@C1IL?ni^ar?1{!z#&K5VS^f%FK;)Ouhy_%=RzGe&xz1H+ zQ=h;TYQ`~cZ~`w^w-^Ed@=m;SHg}ku#DkE?J9h0(v1A>)Hui1mTj^Wt+ui$b=W%AS zz}eIITk?TasKWUmNLmP9S~~5{-mN}dTb<9z$CNyr0W8aW0e_)$hn;El)+uz{#oK%? zK1%Tsz96F|Cx9tne|*rRtnm4IS}S+k+pCN9RX|a6dxJ`VfLc+ z%qFe>bG283))BIl3ThM)GwZ zs!if>c}_sc77#Gg{{cZcEtf9@B#)>Q)43_sX($KkR14hnsF>kYwrHox0pGJ)y zMHZL~^1KpYV`z~Lr^Z@>(T0yYtnKRJ`XIKn8QBrdf3Q7Z0U78r;^|9_H5nx?goLWJVN<0F_ye@d`JRSfya0tRdQ_d>+P(yfqhOf>47o z!Um|J0ahai!Erx96%V3Xno}i$col*KG_C#Tz(vh=_ z`MW|ce@x%nX_9z#w_06ZuP#!U^SI_>KCuo7ewGnYk&1g(i4(Mgc8c+o$Gl_jg2#Br zK<+f}cqC@6b!U1^jv=&6k%RY$6XZz7s>yk|hhSfjut}aMEXiw4(}WGVXpuNgSX+e) zoH#OBr^Jbvb70-GOcR#O;;2m%PPro2oroE!eqC>0g^j0ctcL>{C)zv-dzyT`8WOG7=a zOxE)y>-m!Pe93ygWIbQ9e!S9;SNicvKVFeJ`4hI@bCV4`fnP^(d^|r&8Hj?EI>GPG zf0&%JWMVkB7Nlz<*n`0Y4SA`BbUrldoMo=qDMLXMA`c`(BtkTC1(~80jdL=&f9*fMNe(+eFdzj-;1`e%PzSI~1eE8jBuQ6` z-2+m%8AG7>GC(t$w1n0rWf`oykZptO2v{CjFKEoSueEg_=&)tkf{z{jhU2Xj0Fw_R zM>&QW6=SxWU(mLKw4{R3KvASWdLBo+m{A`{iy~=oV#b(ucph+#0EsiYTNtnpe@e0l z&f2BS$k$?mO$A@ZoJ?EIxHE!01Svcsv*~XB?xzooh2?8&u|=olL<70gIniUWWC)Zp zDyNkJc6>Yqyl~{}^kX?#GFXd^$|1w-ekCy`2M+8-^}O6e+N1>*8>a~)AR#>V!BM zd4Q_dkrXA)$sgc#SW+p19El}OlgRQZ0W+6@?E)!(#awN3 z+%}T_?q4zYjk@*Fcn9g!UX|nQW|hR*TU+PUm6J+QGnB++W;h`^aFI6JmDu^k-xUug zVT+|;saA&OxVS0LmP(uC(3TB8*<@93x5aKveWeciexNUXix{`Slz*j`%q)Nl%Q&?d zCX-dyY$C+afU9EXXFd~Ia#QTCJx%MXUDZ2$*#VQnCWj>sf;@1Z=2{bf5A~tl+yT=n zOD@lr@O9Z?pO5%YS?;K%tF~p^mHRe@v9mn+;Rae_i zsR&4*u1JFP8pwseDrrD}JC>VjTXp67WrRQj%oq#0B2vQ?G1V`8c8}H{nTnF@8dsZY zALDXfHis`@jg;yq%*4XL0>Qj!Bzek#E}p8~%X;6Gw-L%XEUI1v-~Gs#i7K@aB5C8i zK?`)_@kJ6cY#g)D&2h{la$0hkr!1R@`{Pa7lu?4Ev{Bhem1=l@iVJ5nBZv9&P&lL) zwpS|SI#ys=^g2|rj8x3VRm>(w-ytScn+N4JH82>?8`x6I)QHTRnfG3fTEzre_t_HM zV7UhaH=2WIL!J~kpLSalncWTg3H-h(w)@t1r7>jElvg0|-Krn5DWcxy7Z5|;+<9}_ zfln#cWK;DOy(~6=o7y>`YIlA7tZ*I-{*+#-GWq4OMSm1}_(@1WZ=I(CBE3L@I{*2q~4*_V3=a% zRsq8jKHQ#x8!x)5Mh~B{r0s^NG@@Z5St>Uu{4&L2=Mk zJW4er_4}V))wK^O0%wd-#MdlKS_c*IsfjPj2l_0^61LnFE%kTyWs`NeT$jXSxm%Z{ z@ASn1se2iJGpD9lzrbOXPOj>P9(p-dq&WD>oAA1%pD4|Z(=do&&dl&_#3A8p@>%`b zR}LK9Q6QZq=)V35nMxrS;DxyVQbZ(^r`9Z%!AHcDrc`xoAS4>=m~!O>W6y5icSh=HA<$SK0(~5Wiu*kYab(u- zNkEYlF0D=cnv~aIOHP{bZqC^e^}k1`=Kxwvo{z{skIbK?Qu2`eed8j|ZMDWQh{LrbA7%l)7dPl^uq=Ril^&`3FxjX(U{#drgT|^%emZGV*5g}$ zM>J`_I;L>2emS`K$@)=7Ssx&vt$n&Rzyfrl2n%8fl$GFO^cc~{v{Qq@tC+!$QW4Ni znTI0y%o$gVGHFwrizsPfXUIA1$QOMPO{KsE&1PV6{V=K4W=20kwzy8kSTr+9mSOjE zTFMdxB%7fDf$jOXef@##f!_Jx1<%rdxkQV7+%IQV?wfTpq)Pa+t*&>)CR!>enm%JE z-&w9dB_qICtD~{@{aJ2~u^dZ7N8dajcYW#Ue8xK{mh5|67Ih0AU(9vB1poxt=WeF6 zI0BCLIn{*7V4-DIZ}$hn<^fWPrfA4=6rnTBo8nVjUxtEDd=UwM0KRH2D@X-@o2pM8 zeel9Ihv}Qz2e1aEg8tfNk2QTVWT<5mB^BbXcjy_Y$=lNP2UFSB4n` zAAJ3WKD+dQ5*uLFs-egBu-|X)Vp9iT52puWf0&O7I*bbMXQKinrYgdp0X4GE|qMk@v4Wf-XSFRUaU{{s5{j zN0mC&ud0h8za5+{w$b2`rN-oAPjwr0tCFizvA|+1aIoRi;=wHtsh{G>V{2+e3O;az zzaH@9`TXdJ}IXGeW_gO z6Q%NaK_jAoh}O-l&s3IScD5?#X~gbh8u3ki=)hajGw9rJhtj(l4Z!_;M-4-sy^LW* zLh;fVpD`Lem&4*Wq2rEn0$Oug*f;@rG}P7#!!Q0O%YotV=Lr#y_VK3Llryu7M>#MF z@^)U;Wl^lp?^qbm@h8Q9a}=jmBu)UI4ho!#NnlepO!o2TgWhqPkq+A|)0~pq6KaO} zs_8>p-QCdSy(jDAEItJ8?hE3px}ug#DnIm{3#v{>wmzOu++J5*-%vv1ou3miJ$`iS zy_=M-{&w-~w`Uw4*@7=DO9h7n0PtG|9%Z$C_TeYCSVJ334j9#cEV5zsq*YJkt2QW$C4^#!UMfPdw=!L93sS#F+?|v^)(TdLt>FzOKz13 z5F8}HfPZrj@cJQtLfHV|2X2nv44S%JqhQK2?mR}`SNF2MYCj_0ZD!;+9fz@WexLm6 zkCR{hKa#(GBKbUJ##hMJbL0o#yr*w03#SP-P)41>>|Qj*`YHU3HaI6tIfZXy^I>U7 zIfuS|2{$jX zq9T&lfD$A-X2=pEV&S|gc+!bmf9NleVHOlj1Z4v@HIS290fv!feadU@C%i%@-_=-L z{06vBsEXbf(bq6Z{ zLI^;Z2q*&IyFiE!q-}>S{i4mW&A~Skn)3T6bLJET+U+l2zdHZ^=dWJB`RBzy-_BI2 zW2{&sq4QV&`}DqFp1*qcK5jAj7;K-tu6h+JymR`9T1-f z7n_+S%7L~w^}5{5ED?yiX$mK2KP|$qVzW6RZEt`5+W!2X51q>>{1J<*y59$;94`)T zB=`iwbFkt(<>I9ID#)X9J^?qO+4f_ZJWnO_cKZrB2K92~3xL$;;Pie6VQPq1h;X0@ z5Dxr*iwH;Ld6s-twyUPv_eGvKy(U-S%gjlj6C)U89F!RQ!7%GrK9RdFs@=ZvDcguY z0J#h-x=(hqag#PGc^mDejb_Q)vaN6zepQl>Is$K{!>^%%i`XzjI=avj7A9LP^Hdp6 zG@qyG+c-^_IlS@p0C;r!<~5xYaL!;L@_Xukula?&p+cEdL5JY0x5vXG$bMJ4Jc^-n>JH6470cy+ zc=3c`(ZzdxEJlbn_HpJAGY4N`NXL{2LONh2AJuiTZ{Sx= z)g**Z$BHL1i)MSDa=$G4X_Rw7HhPTsBa{bU%}`#!4{GpRYV!npQJbd{3_f;@@uLI} zU(FD_E*qp75JHZ}XKwME@)2^j^JK?=0I4*o1;iD{qa(s?Dl$32qd`>^=nA)fd*y$B zU!4I=i_9BQcGi0zVem8qjCj7IvXxd2wu42j17l(@XZRb~o5l;{cIu&>&v%9C#Z0v6 zdyAKmP{A{ZZ(J?}xCNiI{T!JDl)?xsRvte)HDfQbnXt=bDpdA)!miF`sUc zi_=Vt2LaO9dF2pYQB7rEN{=mnl!lLRyJh(3rfjzTA~g|k_3euv;#58ys$b8*xLl5> zT^0LcRZZso=Eb*T0Q8vnc@cN&+KN%#sh9%?Hmp3o+M-HQLE4`u z7(rxHtUq;A741`Vz*loQpoZ1CzNRfQEK+Uvo2om6_~KJjttV!L-^ZP>G%8x7A1$d$ zO*UH65l_8`scMJN*Ol9Uq|1}LSv5bI6iO49WybT#yf;M~$gwHbGcH)3oKD3A1MZcL z>%ObFt{WIN_$n~`*Y#mjlT)S_R{$_ls$c%0b&p9@eGV4ePh}C$vsJ4OClTsCY+g6n9#GN<+SA^Uve7lmc+i z?Qqn4v{65?8dq^+v9;)xgT?V^pfzC_$RT+HSMZ4B4R@7Bn+b2KUF9l&Sc*odNk;@e zgCjhJ!A~26Ic*TL5pH4Kh?UWEGN34TA!>Zd=m*IIXba%sj7*82NIi&0*O1_F>K5^;1|#~ zBNvO+i_at$lRLwsy$#gim`Id4Et2nNKrOS$*MnWS^4kfn4Bk)BpR&g^zeOxLOSyqT z!3Ag8%pS3QIc%&Q1p{Qu$wuAOy}3ogjk+PYx3r7PXcw1IPQRXAuw@%g1KGrQ%QiNR z*NoHq)RQxR*h`y&(SR3DRexam?ZIDd!eSL?Tqj%nedvnI3bG7*zQY6AkGY}urf$3F z1}EoCp4{QgGMVg)rr4Gp9)FA+>f1mD?9QnQTqxRy^_NlT>NvdL7X7(C0?hu{8~nIx z%5viy+(A8m@edPzMaN%RQgrk|bMRO9w|#Fq`^u7k_O6Ydk?YDW&Xy-te|yjkyy`C$ zWw_@+e_a(EFd!(zVFzIn;jW;|O4i@}4=yX^nL`@GiG1kh#1y51WVe(Vr_=wIs`Ku} zQPn1g@1SjW*j}D#4pYX@k|On2G)&=cQiFv!gT=iGcrEBo3Lbn1zl?_&hkGWOnE8JO z)=2DsmhN9kBT4p#4$D{04B@3;0n=aoGwwRouI}h%_`fdT@AaYS>mt*meU&>9huija zKg>d42Afw27PsAAG z;nmUMye76^(b+@By=eN43V7kpaJw<+ue%I?cWTETJRhvBK=T*sq&F2ZWe~pRk0bVD z@!XZI9KZjKZm*Y!J9MZZ8>$HH+CLj9$xIjsN z+^Tk4e{_W>LI$K|SkunN$Gl>De2Pju!Bka*a|K9koRRqR3?KB26f|f3{j1S@R=fK+ z;e_Wg36QCT<&kL(Qb~6|btwEjRtmAr0^<&bg+yIM#>0#4qb=nxKwT~G3;PpY0hh>n zT@laajGRE-hq=aQ5ru$si0bQfKFHU9*MaY*>Tioq_=RkUFN(&M%@)&rj_H#o+mw3ll zjj$?GhFkCMN|j1@_VR3LGAwVRI&pV&U{fwBhAIe;{l!sy{8)7>w5uDst_HAz{*MX3 zI4l4ENEl~(Q51}Be$~yS1FoC<>oG@PNbUdAqh{^yBU-@5H#HLR(cRcqCi$wS_b$Ra z)4=dt(o0`c>kvzSbj37xWf592D4OE;E}G&iDkuABirsvBN6il5P%WSX-5=Pwr1Arn@Q@0)m0WuQ-IhP=%LV< zrT_>N@HgA;Z`R}ar>Kd9@cwj&`F}*lfF3$fmK$1ww7^{E`}x}B>d(Vpj@xKQGN-fb zDj82n`gM;2 zkC`3pK1j#jE2)l(@WSV~euWKIK|?)XKi<#)IV$FyE&cB=N$7}BR`jq_NK zf5fUIYSf`*T%A|x^NPImf|VPcoG&!frN3#?6b9L}ueFf9 zzh$;vOu9{D=sI6_f7=*^<5{NP!zl?m35{KVcT{TN#uA>^I6#og8P3?;j6hFSU?d|d z<&RRiZt%ti1f^(U{b-0IlEbGjUx(?HU~E9-?!F6$a>-2F0Gg(X{@oa2gAkQ1fY(9}0f27QiY8f8DdcZ$V_fKlh zSti>XRPE+UnLKbGU&S!IkS|R`6V5Gy@AB#=b0edrk!mnF)#5H)pZkh(hk==v1wq)O z{NbsZ*yy`@)ZnU+#|D=djL)&R8mUjn%2_aUQBZn zo2Xgz1gPJ~f8bT*CGBd|lQg8Jc-NKz($(X{n^Df;HMqD%P#s63eIYVi?8A@@wY$UNUN;eb3jXAA<_=i@kCVl^b}ibjmi2fdLD z(UjuXg`!Jc_$stdqpDJKRt@L(+^x}YXPK!t#5oUrw2lB6z*;Biokt%m>~Is}+`aC| zl4d*$e*oztTQJE*7!5|1dVNY*-ft+JkrLH$9u=8H^0?~c6k&6!2vuz2UvUISeDmbj zskBER`FDtFT{e_jQH~Zke1vT%44L~(1TT6PiXhY$pMo;$oS$Wfd^Haz$p1P-JTHxs z>WjZWKo6#OQ~7MR7lKyf5^Y2fE^kan!JU>fe|U~K0Fg`Q5bh{*s+W2C zMv`yBIv+ng=R^Y+?LkXX@6KdX+JdEnBj3k-og`)FZ8fam-k%7(BA!3>wKh5LST!&F zFE|U*2)-Q`DLRTM^SS52E{m@GP5Pfhga3jh1=KlW3awW3fod>Aq#k zf1tXRi=TDd68k$^TI3T1`v}yZ8J!)W0jTp@KkKK2a%7Uf%(67g6enrOvj^H~B-fv= z5?Y*X4pv0Un!P|F9Qu=zVXi{7Kix%JH;^*?QuXD@*IU@l$HZ8Dy9HpnFLu-zUAnJ; zq6vr452krYrc?xc{x4(BzWP28KnGyje;i40&Fvq7Frpm%inA=r-^b+Emw1w}liGs> z?+|RRVwhJd$a;a!4e+mt$@)wq%e$;EfC`gztB z1|};_!R%CgPnsHQ#qk@K^;aTL?|;`eAv{6qh8WLkXHWsu+rw>YEApEu4 zbo^EFk%-r2b%H}xkMhFAZutu_qLR*JJSgI>Mo2Nz{j5V)v0 zCO5gAZS{?@WSS$%)0*cJ^!jgtR-~qMe-+V*hQ|E>GL1yOD17#6+ z-W-`iw9);* z+B^%~pWUihItyoLSJ#?JTO+{kdMT0QN^W01q0EooF|c)zWI^J1Y?|A_%e!n#S5vB&OHiV3*+(9XP4*KvlvFBo8!2f`e@Y!hg%_fM_SK>% ze$p%6=wY9z#j%(; zCH{6Qief;}q>0<+D&2D3vAR(50!=$UI1`D));ds;wddWxMNehu-I;EX(IDTbjat-9 zfzA>ogr5JHT^5Ks?%!#)#|%JY?QWlnQ$0EjmZU@-J?f!DI_=Xd$-B^#t)zZ7=fFCAT?x`R)HxiGvqVCpsB4I+c8 z3`Go7eo7G^;pFlsh8AuDLP5i`Ui^*7Y+JNYo%h>KfAwsrbwC1~OaMz%pIiFsme;;M z?p2sWC1hGM4)a##jWrUhZ17!H`R`IIaf!IGxWL^_I%4EuAAl6-6mvqK9EXYcx~_bd zQ(V^IzALZ*XWhNjdA}J<%4xUdc0WZHj}z_PdJrnm(I0q5ZH`1NjW@7o;E8K4Qf)46 zf^`Jqf9p2~rcI8=k%%fEbCjXlC3lHTMZ8u2)ykb{+oXuWWB}Bj7!Q=Z?c&}P%=Z9} zBMkK<(c>b|IUVJYEYAN}w7faaOQc>*>PKmiX?T+BzrOyWZa;gfExZC-xc9@hlAke5 z&jtOgB8480a0mi8C{AqScC67_KP7b*R~=p@e;BAomaAyyAAuc3&3uG+!5(~bDir2U zH(3U!_=i>W zx#D{|cbI5JEd-Db5u5TO)OF(P1YOP(oUw(SQy^UxxalT4aI`;` zSu_U~Q0quSh~fhavKnr0m+P2(afmzs5&&N1shE^Bo^2*QMtw@@mo?s&wK_6}aaxzO z@>?b^zQ~Jb;_QlyM4rtWw1s(mKIJPD(=FsOaH!GBCD$)gCrk7lB=lQ2qrSw5;K!79 zUmo9Io5cAgr%I!j6~v<2ihH^F&ayB|j#x5%AAi!#e0Md;pZ_MS$6!ZSiaq@B-nvqE ze4-VKSi$VXm3l*T3>gYv6Dr;4Q%`cmB!gP5HbSUmpz!B+6r*fVH4BS%!~K)MaYGT$zG1$RGW#m)L)py4e^>2Pts;J#MN z+($r*CY0hXN-D6u7+zFsKL33Sa4n+r@>?%Q?J=(w2L3C9Rv3saS!0|yqKB}lH=+~^ zs9);O1hPDGgpXP0N1!20H?A3-`eVqV1seMa{qKos#|*!gl>Qaza4Oi8 z7wiD6X}bTtYWH$QqXR;6EeafjJ`XPS^*knXOwS!F9k>k}19mhq_G@_nP?uXDehqz( zn|8O={Jtf6@YLuf+T{d~3KEJKvpPaxA2^QK134Y&TUxbQ?`V(iKP~`lNZ8u(X)r>I z7b_XS&(TBs(CPE=`H?21aU%sP~dJQUbnqoo;}ntFBM7uHxhh+%(%uB6~?sf zP1Dp%Z;@+9@UVd@m3r$7U`v6aUC{8g$aPN&Ga%m{KzP7*+Kr_a_WCe>n4P!H0;L)- z(vDFVMMs?Gi?6P0)wZTXkbs;s0=}#Zs_tJ@7I2Utl%D3P7qxx4{D4&1F{-INw4!JR zD&bB9(*T&3AP_Ix<=B8pehnDpFW{`Sp}MfywOufgrh%YOSVDjWu;E=oe^Z5W%~uHA z;!yhdZG-sNcTh89Q`csgD7$RHO?Sm@D)(pb@}OCiZwLDIA!SM4s#Osn;yrD?>O}2sM@Uk< z%EE<)bb^&Kb{3rvsD?rjNEy?`=53p+Yq?>GdcU0!%nX9L_TO7tzS$$P7*YT-5d78M zLmRtxZRj@JIf)7saoph=^&N9AC{S9|i#dI4=I#7irIALKRZRFp^Hb(B`W7-|=n{dj z)*92zg3ZzM7P2(|16TagFH#0oksUq>Mc%wPo9su0wdryIa6i5?R$GSR`jWASV>2xA zW|5^>EoysZoc-$+)OEr)z81qVQ3J)6OOp}dxlew28nQfffwj+2McK;z;#`PaUlQ50 z^m2-25<(B%m~}~AHAOiBC)Y)_4Jk@_T^? z+DIHtxQFKy05_52dN1(mA3K#O$c~VoA zN1M)1p7YXA$Mx}<)GD(G3CHz-Im-{wM05UKig=af+vg(zD?X}aFTd>=yBR^DUuE(g zco}Qz%VI`J8TgYWO&A5+F%}UR;Y^Zusb3U8`{=m_L@GM=3+C;Bw;DsZcClEEI~{4{&Yl|1GQaOJemvP)b#&A2~T8M1Ib|qqDJoO45{g1U0bUk#N zAQ=&&O}DPtt)!SRK&O;})>VN@}mQ_AYON^7KrKrPefhQSQqsfyE3k_bB~P-$KunW^`*^Bw}!I#aJH!1;57}TO|@W3>h6riL0QhpVVPA+xPye z@4yeH+M!lash-B~D7a1%yO`Jxc<~b8zJAXh{=Jxxa&Atg*+M)6WP2lLCHMR*OzNKh zH3@<_hkSCzkfeF@=7BeqWx*c{sT)WZ?x3QfP!YH1pp zhb^!C>p$^OJ3rMDx!nMxul1fwoK@^L+T|;rhKaM1{e_tZ(GIp>N zev72R|3cbTJ`{9j(Q#HO4p`|kU#{oa1IQdW#gkq~i0NwH>47mav$o8#EpI#7QFe-M zIUS=Hth%YdV3cG~lM{|it(W4PS!m_Fyk7U@X4)AMoAaFu@XB(&`=7efHQpUtp*4!Z z;E*{P4J$djX|J`4%dBk1)QQ(jV_1pot(k_ja{kk+=n5kYG4bvqb(1H}YpM@}0eGL& zUP83n+8STzzfx!q^cVfg)0`J8nJ*miwhAlW8uC|6@tl|pp%*ZuPO6TW@)Vh5Q1LIP zthUd(zEx^ptfmLy3fn*;8I}vg>ex~4;3DsZ0jLsuYoF?Bf2cfiSp8I|S8p3Q@$t^_>;^ZB>-Hh< z^6l}Jwqe@nM=}S`fcn#p$}2-ci~)3MRFLQiikMsQ6|cl=W-NqBpO?nStBjiRFE&Gbs&sq71;bZs2@{^%E!sm-!|Cd2JI%E)r)X|k>JB~9X z)I60)(QSWQ1*sHK-{Fc6Yyi+^+saIoW?e*xMzA;Ig}z4gHx&`|b}J(ZaCsz#7N#yz z?^Dye=SFfy=AF$Be15T20sO1v8&k7@%tMUmG9Q9>%#=vX&^Y0*?3{j}(-ZO47X@fl z7yM3I6a%n5wnjl+j9=mwBM}*y>0`ih>;!`Xzo3esrbo16CUUyggW^M&G;&72ES&9* zx)!mK>nBjt;vOBn6Wb?OdieKWMB$MY$Mioq#b}5(cZCsXcf0nO0dS7_1z#jThd*{k zfQ+#;S|RD|eyzLKms8Mi^oS1o)iQtSwB;7Sd5>!KT-TTH2n`iS2vXNmE^RBJnE`VY z+GtR|16%VWw})%wyAG6@JVNr{-9~8Ohlg0fnd=5nqzC3^_e-P%bvOjE%0rt~sD+zS z#7$ATLON?3@1Yx&0e<7~Dy`Y&RBS!+M{BY#Dn*cmH&9g0ZwDB9HelttTqdJ<@%7fDV9SvI!&`_xE)MrqdIhuEpuYBEm zMM=RFasH{A;x~<+!{l2*foYjG6zQQx^vsJD>-xWcUy7bXk03|f*((Eu^ z#k!*GtIcGNk-@m)MR6e845TA(o4XD%@ds)#@dy561@vE=V!)-gmeZe>I^W}()R9)j zZOlzSCEAs^qS0g{j`~C5?nFG*!|+3fA(PRfqQjMvo*IS@3`aHXH14E(d+R#%2n*)F zq6I&i-1YtTyoMpSVUAarEEx%2qvNWinZ+P^PPSB9DA{HmN8RpmGsnBrcI!hrSB_Q? zwm@2R<&o-EVn9wA6~(yO)ug_R8rW-ODS1}hH>X+Qa3KEg1xzj&#ahK1#z?{_Yg+K> z!RbNr0D8rQ_}I8U(`GbA4sjwX<9d0qjQ!aNc?w85X)64`9SrJl9<2ghk!YZeEUJr1*Aj`jsSI1K)Fr?g&<@^_K-7u71W)~ zjB;v)!~Uv>#>hg)-~s-%o)F~36r;cxX`yVg!~62$D1QaPis-6y6<)-uq0a^^FZ}N>X2j9$LI{U zX3FVE{|2zPNugO$yO)XQ$iC#@sN+bdD2eusBXM}``X``!1pH~9h%Pf z$%l6v=ZT(9kM8X|Tgh<`?t>mRT?|?ZAAG%9h~8Vxo7v|<4s6e#2RihSiYqg*-l)?0VXn4xAV+hh|8Z^{N@Xk(^S zl`muvD5lX$h)L^Mzn4@~d3#kb%KG=cAV$Cy5V+z#_2@7LIFj*DfA&Et#AL#inO69$ z@7g2Z$BPn~7w-nWR6gi*&CIm#TG(+%;)1m6PL_E3U7kaw-6lSC(lquL{Bf2BG1sG8i9wXlt&ZvSZamAqsQ>e{aL@i(h zDXYoR>lw&H%3#Ak9xt&2TO4F&<^%IL|XU7L<82{9ahjqqcGFO)(z@cRXeDHY zxJtSQ;bo{>-FWpckxUBC?*tJzSw|t8rO18R){CdbRwBOjL1bL^gx?EM>$dHu&30~= z)jULUyCTUp<%Z=}>pE1NQ-=TtJ23X@0yu~p`Vq35!m5p8_d!;vSf?7DipcZ}O*7w_ zY8AQzNlr2TKmr2Xc{vlje1cEiv*)+XQ)z$)Mh!{E8veFBlr5C6FH#8g}Qzr9(H>1)m`r!M3R5oi5N<$EP%=H@vhVd-rDtmo~= zoYk$N<^Fl%{$S$hap=PS^(kS8hh4!6NlF7AKG*rC-b8E_UoYe?2PaKDpWS(XZ=D>f z-XJ3(cId;lxk`4Mh7JJe>R)|J7+MnnxHv57(QZG8h1jK3935R4KR#V-encnX{fw0%S?=;cVo+z%vOWDU(-T%KAsv$ua4Bl)$? zId2|GOW(M1A+ya9LySJza%T)`>;z<^MjbhIV#_@tMweX;A_EHW3ObNy5)D*?d2WaW zv7Rt;F#0tn-5S;~9ELBp+=r&^8}DQL_kf)aV^9ut9tVL{iz|k)8M)aG~H3 z2XxW8&*`FfKAsQ63^f_ao?aA2?v8WL3kF`_=Jve32aDIU)zMQ%YC3XOF3xkPeYtqF zJ3sCs^yq)xz5$5w4_%wGk^>$a-09RUkcwfz>x06TtOmVJaP>TJH6f)94La2KC!G~4 zsp0&CH;X!~za`UX!k^Q4PN4c>=uZqvt{6yfqW~I5D5}+$v~xrpBHeqTFXDs1 zZhhlKP}xMB?`QJkOeRGlvmStBbpJt^CWa=H)aJw%{Xs-=W*7gRV6RQJRF;nt;4+HT zxS>hQzL5YF>VoPeX9x#Ejt2h9NHszD)$1GQpRnC7eS!&?pUI8a{*2JBdK>tB5~1 z(g<%?ToUAP(}?1HLF-5!pIe}k^D8l4@QoUKxq~-XK|5$n0dWVrnRyDo!;P5dHD13pM3b(#%Nz|3YXSzOINelSlE8}i zF}0jTN+gGnqYMdI-zYMOjf+?V@uD;kmP$`PrOi43v~5*jc4swo$Vu)Lt&y-B%}0It z^=yPh95+7$k zJI{Y#Osu>`1~Q+oq)!@e?z(m+4yJ4lrsMq?D&7&EJVTwNNLu9IiJ9XWN25oOW0x&X zjf{=;{$&9jl*{@hfI=jdmdp!oK9UKhNlu<%P40`yM1@PFC=G8zJ5l`>pTT$%X#!Y? zIuuU@RnM;rt$%BCRp25);e|h^gF+_UGi}zKUx$@r7ej)rU$`v1sK0tcEVvV}5{dm7 zMb?4dx2{Ds(vE7>P5Vconw3F-B@bbeo0JbW=9i2xi?-?!CaRC{$Ji8IdQ^Cxq0R{Q z)J&>kB3&>?kX5B{KKvXGg)-iCGZ~;IO&30^s_u|p51`Kzf+-pct7 z$!k94uraD!M}1NqpKr~<3vt0@5nN87DYXF95drRuV5lrnA}_hD-6gIuWt}b$qBc#L z-=P(f$u@UtujYkBrv$_Vy;L@!}11oB;vs8Bwz4YnDOMfPZdB_=>j5} zg|7$I1ol3sz=dL~BX=3w+P0L&wg^{d=-JLbXIMB37q(7N=86$VX%Sy)vW{Gp>)$Yk63yka$ZGf<96p~<1+@sGUtRQ+HXULj ziUtXSmpdImp6T=57}Sq)>IPs%z(N|A`IYw*DC>DxA;8Sn6$LVh>);do6r4r`igkP1?|!YSV7TiwF* zkt;HrodvRl;)QnR*$ZUf+_(n&@?q9ETBbZq>6E5dU|xVoH@$h&w2qv4?%fdj;Iq6B!eJEj9b2jQG+1{I+5JZkV2P5 zPZg`xI8XosR{j-<#)Wnskt!eqmC=lNG1q(#U9CD+heM(SW=;M9mU==^tfpKsJ!-q` zQ*QO*`<}3R;1VSQn{^v;&Z!(wv1|iGK4nYP=BoABwjhtc`a8j3C1)NV@^7N1^Ls#8- zRc2fe9-$kTNj@!da`e<1kc^eHDRB+Et{Z$z%|^NUh3Ca3x9yCC6~Fq5m6D|zR@3F% zhyf8|JF9ie@Y1nM(uuC6$9<*bpK*E{?hruKVM%|#5PA@Sg9y(Sck+fO&GCMk6-8pi zAX8Z5utiS$#j=TyC*IF8YDq^UZ0dc%Gt%$jB!Od;Qb`(EU4Q488+r55#}!z_eHfp+ zIzM_REaF3it03z6z#Tp3SUi$TEge2E;C7iQ$`MUOAT6!mxH((D+~N&wz)T+o=d3}a z5!I?~Q`MA2RT&dN>IQ|3tX{ER->3G6SZCK0+3|6`WSofxSScR#7TMtU+|3Q`I zsrlC{s*zz{y$SkM0%(y9lsOSKP%?`Swn(zC;knn0#?v9stl8yFVF^Dq5PJLKaThhg zVAoND80LwN{nl}OZ3Y9VL3VnlMF0;oq!#ssa=Q)5%>mKf+-9pn1Q{~(a!F(!-{07`AT3Jl>*kt6LsXO&=s zY%VYWG&dDg{4`<^P)%}`MZk`d z!#s+$_JSZAz<7L(#${qU<4d@vBIWb2MD9p$>T-m4ph`0DC52b?Yo$?zGzm6C;SmRC zUm?oI3Hp`v00lr8%!@>9M~kSkDsmyC%CteyA;99o3>q^ek8mL>agYtYSw&*2+(o?x zr~?`8M9>L3=^HQu&NJJTE4lS+_cpzS`xnj=EaB!=zYtw2*(9WJcBG=pt@?cm{MkCh zI@S_3D_whnL_=;hm^y7hHXhCFq6EvD5w%R<)l1CSQr^CR(E!U*xh|f?xr>k9`~jRr zJ<;En?eC9}!>0LcQn1kCmDQq3wyH5r2`o+6SS}R(C|V4E;PL;ERM3jwH%clZZ`v9g z8ulE*S7W=Vg%a!UMkw2G^=2#?VRmZ_S@qeIwy`K`M_3g`oJG{E0@>zBxLD=F!L=6e z`d5P~HA$~)v|~ZVU1fVNCKCincvq0EPyufk-f2;PXT`TnrIdXGv-9!hP;RS5L#Ul= z5Do;I(ot^O4=GLG+JbMpsk(@SHy&i4UpVdIR4T8`opOXRpwjKDm;|qSz!0=`+=ymr zge2jE-~#MYp8ZTiPa0*cuXbM03KL?@$D?pl7_&shmQ&5$Eyx&xuG(JmRnqV≦$y zQ4ge9a)QfJOQX3~M(O*F0km;>%x|!n+ZmA310qpQ9QFK8IBCZtUft@rEXMT9ej92n zj5f-`Bfhh1C|IrHdP6t)f8z~i@sI}ID#=VEr2*~@vaVD!ofbOKZe4QHIrDqRT>^N> zc9x;sC@_5r<+C9b6ffcfn3bA$%0BRqqxA0PUITTD2asnj(aLhoM0BAJGs6Eu9Gk76 z+QAVXDoG5_N0J+F0{+dpjXCW~TmI7u`im*qStY+1m$@Xi- zEEEv1U1k4(LSDI9t#GZ4QTzs@qa%nY@Dy*(1G{kQn!3p)&wiBQ#-5w6lIuD!N}bTC zj_jG(X@_Dy?7WqSjC2GdSRuyK?m`Z~y`ts5+L_Io^MHI*h5_~-9Y229(s=abj3s-E zR1CMtjQK9^uwVsR+@Hx3hI3XFuKH`Tup3~w-r8-N_kgOJ`L9fLgyv@|%J4TPqqm2* z;$VQ0>P$;_Z{fgBr`l$YPOnx1k4|lkMRX?^xEBoG3xb(}#w^=c9$!|~G565Dq=Iis zS+|=bvfsuuudpXE8i%8?@|kGmw27~F3(csfHI7)IwGXw-={LP~uEX{ox*a|c90s&{ zPa$VTCsS2N?R)+s!P^cPSv-Rnvj-?g5=5&>jF8ZHq$ObpfajA5h+1EtNlW^3=tN%EmHbM6MI zVYvs6IXLYGhW%bqv#Th6m!MN5tg$4$IlSjT4CvZ#lSunr(=B2jO8i)_l({=6I#2s? z+Wv*?l_70S@f4IA(X2U+HC5;ujvx#h?rCN*!bkVuAx(IXtAd%xre)7^YzNT$&0HJ` zw)Ru(bUr@T^U5-Xc!H8J|B1>EE)NQUav8W2)sP=3kqisB+6{Z@m4=?QF-R3g$3)lO z73Jz?6)@v0BQ&}hbRgsox>)u605+qDaeMAS>ra{>IwVlxalmC4=-Rn@6zrx|UdY{k zCRrQ~uR3bhod^r4K#|bN+XqNzJKF!eb^f9Pc`u8+l@+wc;E8N!EwAFlUw= zr_k)kk){T({{nqyrfxK?hXk~9`2zQbi>$KGoEx=oT~Jztg+^@M&jIaORwko1!^o5e zNnAg_4N~Xy;5fs(`^5i+F^7}A?#Fg z*t9+6H=lA3{rKSD*c}RXV{=sUP?~zL`GChE26)>vgjo=XW}6#S z@ML7PG?NrtZju%E>;a>wn84^}O#`@c2{r@?Lbm1By!IOe7dhKgtsL%t@?h&F?>bUE zd-M>p&Q7-I09XttzI@|~mP?k^!!SRIM;6pgr$Gp%q818k5I5Dw%n;Quc(B^T1j0OJ4a?@nRh-*{iL+*7A{+1L=e%+vJ={xy{eyA+S!)Xmfbh-z3L?Nkg&N zZ5Q!-*w%29UMp{_-`*sn+gWR*U9r^g8S*;o$X5~GD6M+ z)^9pylavN95F|NXxjA#fmgM*)wzfyd8LSD5*`baL8x`_ciHwua+xpJrCS8eUxujaN zkJV(Ff^{@8Eejw0J29*JIku}+*MOM~P3cRfHS{e~pd#=u3;h%na1cg-4^;bh;d(%_ zwd|DAkf{#j`Epndd;L@}Tn^WEp&Qmm9rcW_g)bo(U&pPngNwU|XPu_**^Iq67l2Bx z+vnq9O%Cw=lS?~4vV8US=>B*(+JAYohAV7%y;V6zeTV)cUMlH6yi$tLdxPzyoZ2bq z9j#pDCXWUo)R=TebQp1z=X!pie(nIrh z@Kwn7Q&u^)-^q9RUGm-ZgIIk!{;b!_rhAL{5_Frx9V1-iPg-!O?;L2Xnv5s zsF~eYB=|1X$kL)m_&(aM>7Co{YvcTQjQo6&9bZ1*GhccU6Gvp@!zCq*ZXd)B_X%EZ zG;=F_U#A^4-GGmSgJZy3AIwI|kf(ve-u3Oim_z&)bp+nJ3T8S>*zS+y>DHowcstEi zqIYd*zsonfQkE_{`FeVVTW^K@ctvFeT`BQA0t_fdm+h#fL@a}jN(1QQ5Hn6TCgu@a z>;C-cA|a5;rk+byyF@JNRV<_0{qTjre{|aASr}<`nlNnyTg$w|BtNji*QZubx0SRwNQ# zP3`mUa^D>;yGr)hOsvFiJMzxk!4ZmqmnT`(iXmGsf;`}-YVzv+OHU~|ImY0$z)sia zd1_~D9#ET9K&ujb_gsz_m}jy&Hfv_QZ8do*-&(xMnRMyHl2-6#Ios{z{Wy@5jVV81F0U+X)LmKkP2I_P z;;v_rTN(%9aDZ@a6d>>xLD>fX6)c`5l*n~^ydt-$xY}1lYO(}{>?iLeXxWou*($Ka z8j$;`9OKpSw46?TD0tq}@$vdRvP6DP=p>KA6U_vBQA{^A#h8$dWK&NEV$vBmiyCYw>@&<+VbQo6RlP?voKu#&`?@kzS|Kn z{L2Y>gaY{q(4YAI6JS39-tq_q0rLG*HR3=GN)G7KvT)iEMf#l9W57F*q3GRAEw8GI zr&d_Dz)7aDm`L&`5A9D^ey&IuQPl+0C{vu=!iXMCmhngu&5GgGpOjzg- zB}z#9^T=r9P?B_EsIxME%9CKw*wJ4!$^c74!PKmNz5xtI1Bk^FxTHu7NNXU8L2`;* zuGn%v2TIUds$cNx0t58GOoFBGm_o@w_M@HC)%k_DkI z&Yv_qtl0OPRdu<{9DOpkwk{Y$d6nItQVKR!dR7*q24WwKuJIINA|i^^nSvd!4eeKE z?ybL&^lE6YxzKtL45k2F;NWWCFciRSaCrSL93Amoe?z?BRlkrPilWcel@V5fu&s*a zM6z<|CRTG7HIe{!m_uCvb%?}2nF=nBI@Xm$diC{~kE^ewhZGXB{b25bKJBUAnGxFz z=lzorS8!@=?AGY{Zsi%B4JjPl0U#Mz!N-pAjsLw}K(JRpTMOX3Bq_jD z0MqP_A;XJr{D#sisQtmGtG9^N-5I07&u@Q~@`*zzZ%@bXnL)hB+UGn2&FhD*uy@Fq z9;V>K1^2lpf;RAt^u@TriLwPvV+<3toqn9_X<@nmJUrj_FrpM6MeBkTP{4HUPta )7cv(MQ?gvrU=0|R-ncXSxY5YO zfXM@an|NP2#|1KnOd$&xze48nb_3QY>Zh=G3n5= zFSNl@QA1w11mC1&W{j#m;xTKbk*E>bH1tgntsBR;7yFkZkUK*xcK`#l?%zDNJrQdN zjFDmbE0Y5SdP%>yCBibz2~Cl{_lWleqS~h?zhGEs&qsP~|IrItZ zfO@E1RHZ*~Fz4c3KsB*^%(rKg>Bgt@6j)1=GIZ@D&pFps3b2k)8PC~$iNEHRYQwuk z8kwOf!2+5p&6TJKxL@6U04_#0mWurdHu0%r66!$zXsi{rssMkR_B3nLnvvRKTExhX z&6Z6YOqYx#v&nAk448Jt|Mg35*XlOh8fQ+;-rqOb}3sSZ3E=` zbZ#)?i^+Bi*l>H%LMuBvdv_L1Wa$n(&>s9sS9(Zm#voqanvEy_!Fz0i0uCRl=1V

%aHP_gm0_8?)Mgh;4^QqAdfimVo7@2RK6~51OSXUNovW1?C>Qiq|=s|6KG{B zX$^#WRiMvL2S>lH36tpv(qH@;oVst6Z@yeODu440f!SyjY4DHq_X4v)%krC&>-Is?8LAH?8Mv| zM6er+KRL5i4o{Gn6_pzMqfRn6sGxcNxC88eeu1dJoEk^zhwM3?N~*VWz0dYYG}zu& zGd@T3D|XGGTP>thn>|KpNKu$ssKZ{`*5Pet>1?T!W{Av={*HUF!*@&?emaK>N)Ty| zjS%LjI%3J_oht?BS}cX%olikuEfeiBmi&@gO0No~Z;zEAOAoZ5N-0wrOG&iviUn}> zN7dN5j6?KlS08ldU?+1eulZMN^P)Shn;G?shAfFx#L}8qqR<60rO4;MDB6X^(=7CP zi4)^~;{4XuSyc|53t|e6So&UkG5rV4$~&Mn%9BSQJY^4oY?;LBy)~_;d9*g+dfBGl zjuIb$pPe62yt50sHV!T9cn(5XD-GcRvvB|fvmu7~S4NazMml*uBL97VG zivxqZYuuS_i#aJaBf)wZ)nH6^*Q09k3vKtq!nDVb;1U%4mbniLNxSkP0?+i9%F zQA?6D>_!B!Wv1SoZ-A^9^8B!Ovd;U>-QEJvKaNqrD2jzjKaMIoRuO9MC9JlBj#qv4 zYi>vaC!f3$KmWI|b&&Xy{op#$I$uv0@3$f17#4peaOmsbQn5IB75+}1+>Q5!t6MT? z@ugo#Cj9B>zeI3_Dxt(1!5;uG?miA@zfQ6EErk$*bP+^U`0IIc??VzgMwp<<%Yjg; zV)B56SQM((K{zjPvv;^P`HP`O8zIlqG3GCUe7sM#v)9q_&Q#ONz+8Z0p14iR-SeN^ z!|hTM_ml;(@WNSyV(etPJF6K_wTvWG*_RD2KN1a-XQ3L-};-2f?aF{S-$0!0xu zawvPd8!9I31_i{b=$)Ga1raqgiMKZ{59D?^s4N#3__IeVP~+~#m~wHrc1=<`zZ(Pi zQ?iFsZckj?151M2(9m};@MnsSPfXZNX?XJPPV!W^xk{Z*G2J2Ii_ULeD1QE@8X{QH z@#IqWZko353N@D8tpMm2)!cZrMCF4G^D=4H*?&FKsB0Ad;(&I%X}3hyBOU$l6>%5O z70PjpJKt&>e|kOy(FXh6zoME2+5+0u@D!5e8QXn@ihWViZRD?VUzfl`O%lDkMn1Xe zX7X5bHx1}Yc^|0?60zkyac72XK*n)cL(h;~8OP7gC=M~^0Wnf_u;VySQD;iUjN_Js zh2B+Y+uM1~cQ0@q5|3r#kR&A=PGv1~QcVM-N?l=kq8b`Jl$Mf8%sXI5a9(ncqTZNC z!CfP2gk37ySGCW|cEu?Qs_-!Rr+-I&Zt9vq%y%qkk91tvJ~%b;4PN^j|3xDy+|)CA z*-})gW}6uXIFlim`og8q1WB+1X)t|m3x6MLMDE0!xm6GZ?>2L1(xDdor1lx^=)OYb z5On!E^%dq%onYlhZ+l<-@dejm0=G*KXcHpeySiiu1#h;XZWugvg?Z1&vCN6NU&F^S zV4ENr`;3uZ-vL^)gLaO{KDCa`6>VGhVP`%3QqP|O1RYRHbtlXZUvVB^&#h0_&w4m0 zjXp(LzK5&mX`W!e`b3qTW7EFTUNJ4YP7&yk@|iyUY~lFb>4opIdrz@1Vwb?hXgFe| z{WBoEgeR&P5SC&k(%_0bm$34rZHqK-z;2|t*^3xN%7jiEy8MOC=vB|W5${vAGWoAT z4;{!8AkqELD)@G6h2`*^oOyNOtDjsN#~& zv}Xk;LrF=}WLpFgHhMld_b3rV35pdf&$nA@faQJ~-RqI}6g_W`i}_2uo@siB0q&8loN(D^+(KcXw1P6z?W)JRofb75wj95>{q=q)kT-?sIcM zPA?d6QcdYve>6odEO$@8*Ni0wEp`sLHFinNTl{$jyG4e^oIbp2J#q=~;FIzN?Gg8yapH!Q`Kue-!zhWNxJM71I1$t zX4&e1JB#(Yn*Opx^pNPs|J9J0rr>QEQA3&)P#$$OfWS|sOvz^=(xLs`%Uv1STerBF zuE4K+qFAS|f=MK+lznuH(;cfxX`(JJ74!#s;xi8M8DW(wQJs8z6k7aacq$cu3a6Z{ z2ogd-hicfUM$UW^;3$P`R7Re{YfxrAShTr(<2Qzc=FdT7*p6UU z{|pu1Nzk@8;+)M+2gqU~&Pc7Q%YY^RcqYJF5f3tQtW6!o*nuyCtmeB`DowOqGkUmv za2vcmwQI2v?5G?I)jP;WW4dy&!Lm)oTbD+R)?^7d*jQYJW<;gHikcjs>ajNX1D+4w zdNV~G6Ec~IgPxwAfssX%0*X$?&RE#M*#w_ln3Iv7k(HjA{=X+%5V0WTKovw;g_s1G zn3!4E*%?LHM3`BHMFd3H*@T7Y1(=xxSXg=Sx&PlwbfD;@O>E7a&G8u+*;4GOK`8(v z$r_=_Di|X(_cZrDOb6Dq2i;M!oD1ex6AEG@TMllfn<;4w zFolGag89s`C;1WX=6NuQ{-Nm9Foi+1z+62&Y`pBRw3NR^ z`Wgg*jc6{4t}&-fBU!rZ7je)F`!xmbiKl=lxR{R{3rC_8ZOHWu<8#r=RVcssis{lLHxlTaq1l z^|chtmW0|6YO6-kO{~b#14#5KK2Qvbw1?gd>fj9$X$S(_EPk5c%IiGa7qnWh$+fZ51 zn_!EeXI-vy7qhN~r+#sUCjAMU_JEuHE{9yo`n0IvGp`uXN4RL7eanBUh@SPk6qy3m zb}NRK4@_(k+NB4`hu{E-^)YlSiaBOZ8z_)v%CcuECx%czM9<@im!<&JgM1zOPYdV5 zauV3=YV9j@@KilVBdM5(hmuh4_73Kot9N7|E~dspsPLRiTGZJYDplC&)$kL4y=^Z& zJ9kuUH0@|y2+6SHl|GuYuOzqvU(L5!ltg4Xxw>B6|B5$-yPb*8$#eJ+n_xk9F^yvXHk5`v&F?@O zg$Kfe#@kkTY(&VRv+o?nw3#V!5-Gk?yIA1x?u%i6?3*aIAOHiTIUaNHTR-<#_}`4^ z_C5jVozBXbM1kMtv*kvZHFdc3%0?ji*2AW=z@m41DyprYRRC22eTT*%zT>h#AjAF+ z`h>IGQRMy@m_>0fBL2u052bCz0-}Ql&JE+3RV#hpv_cFvL|t(fuSeuQA*#ASC_O5< z*e7?(zf2{J{e`!+;?Px$ZH2)^jL8S5^r7{kgzNFbd^X|4y@@Xz&{tuf{_@VkkJ>3a zrlx>Z*#hWsnKN!8U7bfSoMSnyT^))S5gRh>q;O==-E_`UQ8{VFVKw)9yB2Wj z1)p*kYf-ChQaEjg-csZ>YS-Jh^@palD5t`;D%9kZK+KbLAANDlLJgL~8qySO1cfIe5u&TWGjmiZ{cDw{j z#w=Mjic_jiBq^44{=t$cgi8p;wfiYLa-kaGsr+tAly5g-rBsX;LmkBZC?lXy( zc9y-49RPB5LvK`fU=9^zkG9OH9P!6;viqlrcf*#Y^=;I MP$VQGa-vZG0}|SP_W%F@ delta 64617 zcmV(}K+wO6$t3;9B(SFz128u@mqCL8Cx7jI+j1kvwdH$%1^jSgz!;&N_lrE5IF@94 zB-@&hr5HcRen69G5;h4?04Q1W^|RK_%mNBbkdiMGJ`o)uHGz$+s=Vy$+PgB<>~^MR zzj>qT-;ck3^YdRjJQ8w;Md|Y5TM<%_bxI zW?_Sx|4DhZ-CexPX8W+-EbqIQYky^VxxOmToA&$XzqZfswywYESr~G(iNEL^2OA?s zIk@b~ARm^0>pW3o`lFwnv|I1Xvkzq+W(%YJV7-}68GhY8U zUmxzaPlxhl^|$NQ>IR%q+Y1Zf!=z{Bs?E*xVWG|iuXUV^)x#tDmFBD6^6<1LWKI`2q#-IhSzvU~>iMesG@}Qf-1#!| z?#nCb?cMqoD_Ry@XEm4g~Am4aJ?<_vKE#Rc@z3~S3b;__fM-2dhCI#_JH+lRswhm#+RX1H4D`M{@*y6jh9zxno!#(zgO)3c}so$@ST z+G23C>xVbr{8`Oz@Ef`}#2jbeOUs9uQDJ~=;q(3Mt2bX3l^D#7LZ~z+q1`LyA(&Lg z^wkd!S5{#p3;er<%tf!}pRV@TyY*w~s?)~KKiO_>*5x}J{Q4?o>OVc-UFkW7mgi6R z%b#zkV>y)W5TYB5k51=9a5=p^1eZEP2zF?Sonl&7Jj^^=MpAF3n+3Ck*6EKQOz^MIWEu>Xsd=#?CJZ6x z(%bK9JM+wE{OCe(6wBn$?g^6*v9jaM`ne=%?+1-A$1ApeCJXk&2ZHYn#W?YRCeLoS zg5NYGU%j}SL#O``JDus+rzR=?y+0OXFwd|^asG$yG#H4;f8|T5C~O+-=yau^yS4C& z8~KE)Zm`Z_0o+jg&sYZn8xRD89qyK!+xngHn2Treo&2ybe;;==cZ|k&luc@Sk`LQ* z-J#w1J6^lJKP+!oJ#JlYr~;yzZ&tV9RcTGFJeQaQ^!gm!0}k)_C=0KOk7A3r<>$j) z`C8ZIy#%FWski)!|5rDGWw%~!}(!=|$JoI{9qNFQ?jffs& zP<2Le*fzA@pR09sh+yg9%j8LE7RZRf4%qzTE5~)o|wW4Qt=7vrsZ$M`mV(E$}G~wgfFJ5N&S7b(-63uU6tS6z7V$W z4y#QS#tOA;_fK46ulHr+CxUA%n{pW@XMziAmr! zQH1EX2H#vT^V|V5e*XK_^_4X=K{Sc5N4mI{qU;^mjHn(w$NRp2rXy@_eZ*I*%7$W; zf3S*=Pdfn<`T1ehOdXM}`up8#vwc`?mgVcR`f=r3u>-tv+s7`qyZ=^&rEtsYZpkNn zw-&je3>Eo3@xYp=INCe@im^5MSv7$d7CWEiiJME`6mGh&vEj)y%J3o_g?pOL{eHDS zztJ<8T8aIlhz9fn-FUi|&z0**+^i0jfAXCDn@yMDUs1`e@0TSu;4~a2S`2EJ>wC9+ zhu$4N>?5TTGmqfJ1T0k+9^DiyW#2mPm>aB4r)+!4wM-Wu28St51%$eyBe#$cpC>YM zjQKl0+~wz4Jw^KOxBFgh0;*V9cUdOk4L4snI4n_~nW%>VX@o2M?Hl-k{IUK4f2lQI z)YglW=EWyI9qrjI>v(l+j`-zAA7jl|pEzN6#1Pr|=@UQMKHVdJ1lzy;@IwB@XCkRb zFANXUjf3$SCg-u)CsFkLdGz}_EKc2fv!hCZBl>Q)zGg#e*P*-Ah!-v`b%aBd&4s0& z@-dvE{5l&W)H(H*PMt3*$uMQ(f9uYK`624bgZ8(ZOXj3+fqvF^)xwDZnl2H&NlwzI z>%F81(rJp|7cG48g9>LUtK~gn+?yX-NZM6A>*@^s{)r0bbEuo`C)=wQ%$6ky6CB3r zFs|wa!TS+{2EFXz#r{}9J2t;8Xtm9&f_A*uOM-Ux#IB&7KJou>b)NQpe<91>&OhBP zAv_+&lKvzAF3X?&!?pyAjM7MW_q4e#ZYsYq0+MDmAJ@R@30plA!LB?(bNcY3#3>HZ zh1ccDMH=>hJ^RP(Yw+kbBa+8^gc|&&<_{&>r1K-cN(}DlGH+JP-3KMv$KayocFmyG znVj6n%2;ddhO681fAfn=@QpONWVDPMOwvDU?){Lp{t+fQ<@v|xkB%2$4$LV% zb!h9ik|1_xNl&K)ikXJnyPFl$gTc?4`DA81Fn?Skd7)QFu6)_71dzCqV?^1(Dw8peeO5V?w7Hw-s26EN@ntS^AjGQ&Kcz|FUso0=H^*vf7&o?8f==b zm;DKoj3O9+7W7g6g!u*M2@QrSP66jOZ61^ZL?h< zI$=2lQ8!bHWB7fwIb85=LccgBVgjK|PN!LQ#^VMTCcqNL9#n;K*rT)J4Gvnt)wIvl zsgxrT6m)t1{oP7F(Sofvn^IwdZ|`=i5_qBw!ynRrWXLjHmtNDhyspOxMw5mz0 zJRK~}3{>hbPljhSZYv4<698j>C{C5jrU3jOFv)4^EM?n}l3ICjU0ODOnhoAA@0m>5 zmtUCK@1`oIu{gCJb=hj)3kQ=K$k?!M3dXfQ zY4RR&EaV$fBgGOB5l(HOQ+mNK9k)8-sAcAjz_Oz#; z!PXBHxTk%+cC}`qU1_uI7sTzKBzYiMX#H>_1%`qp3mrFrDw}WLmf|_mTF6qqJ*<}d zr`_tIg3|kL@^ofg!ffPOH1=4tNnPc=nO9i+uzqAL<>BG)OVX#hL@ck=TIu;`n{vT_ z@1H)G*ovRUx$3(&LB)&Wl);kf^&mBiEu0{hkNf)IZ6~N(7^Me^3{Ix`wHR-N?=Jah zA79AR+I}Z!^dflW;gKfdVo)jEV8PC@0UWXrZ`Z}L_%N+86vCb*j1~FkcL@jN(@B1e zU(b|ouX0vY@CRo(2Lb+$pkxiLyP6t*29uSBOfv@eYP9D65e)Ba=BcNz@7IszM|0XK z!+HSTNV(MdWO@!|8oWVt90#jXGc(T;%LptTctdqw5{fTE)HdVO|VG29U4Y% zWFC3dIDcKq^)K}CBDf5V7^gAGB@R!gLCL=!U6%3e@7C{%6U3O#3?_$vY0y*40o;7I zzP)SiEkc<)a{Tg2QjvOIYc*cz92nQl|L28bh1~Vm$ILtKe6!^$eZMK+O71YbWAE-) z?^?%}_wNerwch-Xi=`7U^rj;uPmk3rxSmv&E&I1eJM&XGD4HSX{3HB)OyjAFyqP8q z;AywAoR)XIROR-w7pl*H#FtPPy+Tg;uh+M5urJnENISdi6nF5~ABWL@uXyN9w&? z)|tgl+cI*<1-fCR;e*tyLrw^{`s~n;`|&RK#~&BRU*FBXoJr@BV}b8@qvQ7+zzXiz zv(}?U>*9IKe{R28GnubUC7?pR?u3xjU_-#;?#mUY-|uE`{y2NHDV={NAU}V+efOWA z{1O+X+3kK-zF#kY$M56lqJ`(=QkI?=L<~y#Sqg+z4SW11SPTQsJh5@BVCP93FSl?I zjW%vVL~P=-hPTbZzyS}qB{f~rg#_`7!6iHMnMp@)CszkEBbW(NJ8_;x}2`)6U7o2i^8xS5XSS)L#4$PR{Ds%Ijy~MfC{E{vY3-BAEvvH_muEB<7tDj^drfGHyQUPK zBTE=NxCrG=5zsEfS2$6-I;Knmmy@mU&d7)ML%sHRY2gC}_QtG!Kl^56O+EV@|NlkQ zt!J=h1{ERSqNbg3ft5Tadu=LDX?!7^kWy9X*YfXwqW&^owP=@(cV5(6U(dc875YUT zbuv#G-F9FB=pOGWhZswvv4B6sqT7!qNz73bgJ+ljo6begoIV@oe~EQGQB!aOa<)`; zKKy-p%29~;(xJyBXmiw46M&|*YK1d7IqP<<2`KFaZXMHiq@tyIi_$=hcs<-HlWtg z#j&To@i215gSFC7UheN)Wq2DwLCpu1V2m$}sI_X1COGb7rOq0j4XpNfanj{+tv0hs z(`n%G<%QtlEOHrd^Q_RR1yhTYXzKZ^ZPsaj!K)EZ4o>AGw7NXv2@}xz8`%@#cT#-A zGI2lut|32sL#vkBfLwxid}tMlTHg0gNA%^DIB;eOs`ytzs?+)S{~uc&m8rx>8gA7X zVKGHBQeTO%CT;%(5mr`y5@GSX#7$~y?0YfRr0qYqZ)c15Ea*yFPlGNJB*@}!4yQqX zSN%Q)T@xPp>EV~oL(o3DWn}djWCCd;Dod*Y*OL>U6V|k1ljMgy3AR0W|R$`*;Cb)E# zFRPQrxX2^G0MV&v3o-EhKd@M$;OCrwDiM9eB7*3BD9?@qq7!(lplKU@8BIdx_%;xt zGZJwQunq|hv8_=Y-=$WVOcD{AJL#o$@MSn`7}02)t27LM0-6|_ zIf6wB(Gylnq&ZV(5)Lv37^R^Kwydn>+nY9^rRG~er9Y9;E2muZ4Z})IgHq9>KQ!~I z1dt#Gntm45I4v{I;1-czAkE|uOeKOX+$;*tP?pBg6GIF@V$ijZjvkW{nn|=mg_fNV z6RwVhO&yJtWV+D>ORtMWI~WXq%#esbWf>eZ;Yx)Ww=v3PVI341uPVaI{9+T#7tvAcX;nu z`@^iF51PcFoynDm5XO5Ft>q>aAbK?*co!OkPTF}WBzT*Sbs{mPB8ebOCS_saSUh5k zlG*j(hM+j*+(RT6&WHB`L4=dQ+I7xTp~Z;6UC;tvB19Neh=zF7jyOAM=mtwzKr*gA z8L_`3mPE%}MJ0>iF)QdplUFS$fA>X-7%4Lqg%%Q~p05+&aWXpriql!nH@)(Zq+!ry zsGu2Lgb<|QBV-RWrK@Zq285h2*i}#u+U82k&|`)~ELLr4xfu-f2Z+w-!uEkBwBE`q z{02;FD-lD$+=B?6QWRR?izdW1u;zihMH<6omi>VkvM%a}z=yVqTiJk*5GWNSLg7Wl zY%+w;7uF5jNRtgN9Dh;XkWZvVe9)B!^@1q}(MMAhNTiv7EhEuYqlI&PJIb(~Wgqgxzd+Cs+K`FFMT}~1-SwSc9 zKm)r;;&?5lC4VL>4K+-Lsl;r(kuItnidKWghLSOY)+wEfgwx)0laRoYwb)2m1x_wh zB76=R3Ooo{{X&E~E-*!=yfVlH5o50{p#Y#&9Em=#>WM@u69IVVHh_*kh_D6a zF?2v6^&kaXH~^%W*l#|pK38Hc2n=WzMo3m6XF3&JRys|w2qTCwC?i*^X1TTEE1Cot zppUXnWr7&0sR(}u#2Cy-i_qqz;WkBoLTU(VKrw;}-zChG|1pV$!IjeOA6vbK;y+ zpGTc5mB<5^M>IcdL!rW6U_Q+ZhJy}~ZB%#x5;|;WHQ>pG=)EO~06m5N0by z7e{J_6^f+7K`fS1Yr4je>6s@I9*gqTn%=Q71x%C1ocTsgnXgLlz4*cRSe&BTz&cECL94)HuttT@s0q5B`(8kW z>r|8RFe!h_T~-Xmz)^%l@j*z9m}xzO6ULycaGs&Fj3+w|+|`k>o)sQjW^%#V>amgv zJB@Xq59ngie44N=_}9;z_ye|ffev$oj&htEd!)j9CRK4$t}+P-G~lgcAuI=D>Y}sG zOgw42y1ouqwmd;xfq<4eCtghq6d5X2wlVLIw7}tQp(7NU5 zPiuI>d=(Q~piF_IL(MPTN0W>)9|EizldUo+3{S1503Drc2c5(nbCcmR7k`&zQ!7)W zgelRfXP4Vl8K^#m8Yx9R?F^NRYwcRM@fY^pPDhv+cQs<046rHgc{XpEp-R>8ZHB?* z$3C{TkRa8Yvt=XRZ?ZXCVz|VLKKhg$AZBoC-5KjhRAQ$rMlsdqoEvc?pNcblh zprV~}Id}w$qDfSf2vCE!(SJ{+M@lB0J{2?aoh9xu5l9*uZQPmgvT{Ew=ca;A^CO9t ziBjM@fwl=$Qy;RiU=yiP(|{U+HdCbpiBhmRX{x_;6yqPJx{mLD8m-UtqDX4P}krh-P z<63AxWph!5V}Ar}X;zF0lUSL0(p3O?jH*Gjw$R4#TZ(nuNR%+$65gpgY>ekg%%;G2 zW8`n?ZslMm7=dMESB%q$C0H%SC7pr}q&1Z^f)iBx;E+6lF-XiI`%}0>oZVD#q7fGX z5$dt{x+8*OWi!7DJ2b2XL<37(i#y^`;?w&IrZwb>)ja5Mo?u*-o`^|PG>oPdv812iW|rFpmgN}% zHYOQDt^iwu;y{FRSVGnjv}5c6r4=Ud9*A%(N&q(EfXq>-itaVQ5Hu)`3K&*o5j40{ z#qk>d z)lev0Fv8f5rdgFRvfr^Er2+^6z)cU6lmXKt=#`%&7YHo43Z{-(2oPh|^_DGPL9_y1N}gmk z7-%T1>?8*w7)V*`mMY-|O&Z3W3UQSjlAmN>&68jw&pRkWSg+7E#b=iZRg>v0eJ9vuwX|cNuK=tC8EN@I!H_H9GbRoR zox7sA#|$HD1*8_?G!;p~NBA~PS_Tf=O@FhSk=D%B{ssTmR^RUn8vKT<9`P6 z(M;3x9A$ARj(L3U(pxe9G2@>kms(t$u{Ho6LolIO@Uey<5qhje`p0SmETyT=|4CT_ zWT?h!llq0)^Jm4{Urq~W{?XSdjx`srefp=BdAwF`{=SameI3X9I*#{s9PjHm-q&%w zuj6=M$ML?7<9!{+`#O&IbsX>OIDg*PalEhNcwfiyPpjkj>LFPpaUGS_6vvmQvE_Bxg`Fe0*7dY%gj9oCY3tM&p&Mp+%!%Vy2 zY7d3&!nIvsw+jLHu;L!3+y$RID0LUk?&98EfV_vKcOmyKINt&ByI_AeEq~BWAaqj= zy_`fhdC^UC^b#Vy%t<%H(oMy5lQ!M-PB#(MO&RraOT8RaCsozUXmwLx-6UBzo%THK z)=kfK^LX8aUpF<_O(J&Fjorj#CxO|^b9NJ=-PCC}$=XfFb`!U~6mKsV+)oK9R;Js{lH?7`Hz<+mB_1&a?H~rtM6X+EbbW0EVMGD<=hHl|Qw?v{_Owp^l z=$2=63psifAKlWBUJXgN*rZ#Q(yL$T6}t2)V)~^s-6ETAIZn55r(5FFs|D&74Rywj0Z^()}|)pGsH zx_%X2zhbXf+}E!IyihaPuP^M^BKGSR`!$aJddPn5WWUa`Uz6Fd-|Uup_Nzks6{Ov2 z(|%=YzlgP4?%J=6?N`zED{i~Rx&1ocZpm-II=Ej++$}opmM8Zsnfq1G-3sY`HT8wc z>wcAXx1hUU``s-Q?|)a6_p8tQmFoSf_Fnyaw@|)cQ{S(@@7C(~>-zhz0CZm#=)YIc ze+QxezC!oahTc06-Ipc0uT^wk!05iJ(S50-^YTaM9g*J4BvWK!x-Z60-%*ab0bV3S zbt{e|i#TNOp2SE!HdqlFD|773XhT`lQLtA`R1!~X5R0cP6n`Y7r*Bp!o7;^@R8241 zmfSbdCdFMxqU;f*D-Fz!-?>$csu3cD&vQ-TiRKa9W@cUB1ctyhrA znIu%Ts+Tjt8G~e55gHdOLifa3$%w{Q+d_0DFzwCQCDfpmPMK`#!c-+RnycsvCKmI~ zv?3IPuRw}*+&YLs5>-zHQXuTMY}RFU@!1%GWM(LtV1LuP_})}!SaL~nA#N(_jM~gI z=4I+CRm|JUK7LwGflI0Gp$J`u$X!Va2_a2xbsi2zN77Vc0}5&aqgrx3fySp26CHNY z;2xD#v)qTCR-5srtpT8;LupE$$4ksuAQG$<)5eStREau3jMNi>P=PPY>ll?Vr2wL< z;0$3CF@GeWVKV9+ZQY9%;d89>3sn^${fRV0O{zg8@KF&}2eLu2nb1lY1A+!(F8Iw= zVj=(tnuGFas0rf(X{ZnYq>7MO9wCpYKeCvqVj)V(WI%L2>Q)>YsZu~AlG5OjOqkR& zZBXJ)Pt|NB{e-|mR}#X@EIBFDfL_lq%OVu=V1J)b0C3_lnVb(=sA)k0HwGRZX^;xl zh{@|H4dIB4bs9dxxDynDXVgk|t@u~`v4*w;LJaxdvkXO#KnNp6;wv%M`w@vGw#KKK zlv_oHp%n)WqNN%(g6Ia!3L-RGEkaPD39}$i=%cBi2X{erM4rH(6=WcaN)N7v`>;cZCU16|X zBXxp~nOyI=$C)jROL$U~>pktXD)?idB?Q+K0E=}uEY@sDQWZofQ_A zV@-@LJ|(P$rh_aztO^%Isfr`t)A_85iZHWP%yF{jr4}Kurpi;@dwSfe7H<&CkUPTm zfXrXd+H=}BnH6{MSGZJMl4R8iNY2Y;*L zETbL};oqbNPiXE$OCn;nvgL9j;~_315i1+JSZOQPq-9*jjI3! z-1|a=y{X*{gl-DZ3E$ha0t$GGBq9#9HRqEO6G(z{LRYGWTI5LfCn5^P*$@iMLLtI) zuN4BUL}Ez|QQc5$A2YqjtOQVXp?|_70#Ok4pmy^?wKs->QiTW?uz?ZQghGQy+)5=B zHwz{hTdP4pf(Mca#U`vxs7M4M3r}K{bVxxW(8EFus;pQR`&ekM!l{{A8v=2Xb#5gd z3^=%xrepT43IwqT0#<~484I*piNr%woSd4{sssp=gwm2*RgbTcSxu0J&VOsxvEdGo z9Ojl;9BONhU$K~tY(|S>>WGLtP(Uof$2ZqZmDilY#rVkaJE$(ROcz8)_H;~d6 zs88pIW>*Dukl5R?9;L1j@da=K=9j5-(;$PAla^FEcd8rV1+WEoz~pIgIl2Zf7&;4* zcK5ZYlSqOx4BkOiEmc7(cz?u8G9Ig2;|8q-OA<&N5r#RIKPYCZX}$$wuGCD$M5=0T zqAuZ)EhnQGn;MXSb+Q7gqhB@7VtB?t(NWK4JC;8rEJ-4)o=?jkJO@0JZ5@LiaRify zp%H?I8qy6i0HZ4@9niYgl9C#wEYY#{uhqf`kO0Hzn8j)CJaL2+gnwwXA8CjkiQ5T! zXc~$Djw?i;bsk{^=pEQ>%UWnt0W4w?ncfh*uk6|s5bMyPP^(i-yudr|jBHui;%cmH z2+ULeG7r$U^CmTC2PJALajKyk1d^2OG)a%Lh8fIJx|*g&L@WF?oLMsNUreWA)zD+=J4tRwPJy-#8QlF}jK?W_9NvST#! zlI)(xrhYU9D$Im!$B5rC56`28Oswz-%>ZmeCZ?Bd1Nvx{GQB1v z$b%k(elT1VOn)!K{)}-ybL4{Q!3(LAK&K@#{ml4N>5AoeU3*DcOS%@2cxET)JrSw% zHO?ay{xC!Y)DqeY75XhvL>LxlRr^a97J;*rl^B;RE6^>rvOlk6vt1*?UPD1@2AZyF zRc0O&C4{j$uFq}De#6+AnyLD4FKtYZsVsmr1BAbGXMcJ~HFWO+kr9p3rJdh7KG>)zSn z#l5p*-G4j#AKW_IvHr%Im$%Mtjb7b4`{ZuVw$6@i>+JJg&j0bVdw%>5+oxY&+--Zd zIN7thv)MNNJ7y(W=A2J9+kX4vX4_wEH}@~@x82!$URu8GoG&;zT5P&qeecZOqbgd! zD2xXLpFPWj@$_$Xve7+Vsn|}`h|@#VPwzm_C4bFM8ufc(au^Q|o_4?l)$M>`n%PdC zZ!SDP_nsfmXW}m}PTZyD(>0xcP}Xs?Elb$n4!!+7W1)qXmrE%2jEE}_)w8(ds5i?) zy`!IheUU&|qW_)vC_m&3!K zHZm!{;5jex*kXUTylHpet!~SCzi7YSpLWHa9%`rKd|5`aUVV4v^L+XCzS76jTy>CR ze@R=_#^u|-&d%zF=Ny8~zSthx#dqbBAb;l{%PH`ucU^Sm=5!&fcRa}UJ3dTKn#T77 zXXS@S&Noj^C+YEid#FFkXuK0$y4zRkw@*vV+J+Za3be%!T>Cyh9r-G`=?ROl-AuNX z6Iizgp0SL`VgA`>wL7er{1VHi{-pi}>PQZMJblKqv*I+Ir1`JPp^Q5WC@M4m^na7` z_dy=fIcM)=HVfD659fTcE8XMAb?sjN1o<@oymwk7;XGp|hwbe;huukG*7-r=f9;-Z zNq?|={Nntff0DUoI7~ooqL$7Nut#*gIB8W6Xy}Td^7@ z+j%*+93M>;aT4XsENP%1x6Al8k>fuC+y?Xb{YJ+GE4|2HZ(p1QTX-)~F9bvdtcCkM^WZ=SAK zh0gg8*K0Dio?rh^i1pN3`AqpMr&~UMuDeIaH^r&#JgPq_>Z&?1L@kK@f^Sb6S3xp$9pLBqa3i= zyXAG&e$N?JG`#7b<%((K;lMmz=6C(DEHZ-&YyaeP5kQ&B_VD{3`jUrPX{hI! zo}77J6m&W9bi3IfmSxp0o+tX-o|N&NzA!z``l6I+CWAZ2oS&2FKgPy=Op}a9Hvtcm#7B7nc9T9xCINPn zWk@XzzwBPD1AOH5=HgSo=#!sFPXY9k??_q!@RLeOasjWCy-7d>IT{oelL1O80yZ+2 z(F+48f8AVJljAm$e(ztQeb@+f*rJIW^o%#w$Ji0>9nW-6&CY}E2e!nj(vFU@DE0C0 z_sdL>A}Of4+-uLnI@%>NnFJDvL|#a+oZZdJ*>}&%$?q@E6n@K@ngv@Z^9BCyH_sQrmGf_!c6nGo9M}8Z^MzAB&cA&g@%X^Fr%nBQ zVVs?RCO<#ykInO+Z@z!_r<)Anp)9Isr-a)oK2Ere8NyvvR2VT6{!?iz^$OH#=71VJ zf1#QJShEE>t-K&E1#83!T@}ID*#PV7{b{rBSV5z@J*=0^0O?~$&W{W2b$N{f%#<#} z2T5>+c6vxbd)M{lhJB$MSLTSA|62}yVLZK|GH)!D29@Y^p%erl@$st+ZD_@oYX0Br zcDX;b!jILVzLP5=XtUlm_2EiKKe7;ge+7}!L8!#sN~cje?MU|nzJVd$1FcYH;z*gB zxmcjp*#PT@{bIAPS3+C+fZuZ7G^;FKrcy53d5lF+HrM-zy}odz9d`ajUOXx{e}myG zGk@_MZ12&YII}M@6dH(?b~9Z>Wu$MNC9pJRwisZ2lBW=2@(QFy(+R*tj9htLe~f}E zDqoGo==;n{u!YC69xjAKenz7Y!z8|Gbm7 zoSN{oR5^*BZ!FK^DkM7r&PUZT zErt3MA4Q>NjFe6CNoW{e=HpI;sEoyJmw}y8FXc3hQeruMh#%lShxeEA2PfQ zg$wEloD78t9Vb<%%C*XK(|h6OYhKm+=lJtZI7=+?%XPC`zRwO>Ro->$2r6&LUx+0r zA5fZG=?)7ij26;$)-wHRwUe^LimwVE&3J`;c}>xV`Pa`C2Dec2ySw!+e`(oz`>rhKnxH-|+rnY-LTd=a!Bt(}LQb^{vZe1P@!elW|)Tpge# z$N^iem+K=rA@3m#1{&fafB9uio6~m59G5yTTF%Ye&HmVa*0+)yuz?s$bsVFk!wYJ=HeHtKOjZrtr|9zru~}T;fZ2*4Y^ra^X!vHgdn-$!{WGCh3{5?Qoa) z_v*W6;69W(yN5&5w(Qi?{q6pcT{iSA z=Obxj;r=*rlEQTte{Int#wl75LykeMqe0`27)^Ke5`LyY1aQ13J~whBz9t7E z;)Q1IDG>9ntn-dNt-5VbTNYbtXt$T&`}N)ZVt-gQhuow!hiyxWq8KrK`V0MQX9!Km z+d-FT1Nz@3z4cWMA%eTIsoVQ;GZ=#!yashzmStz|bc98~f1h{6F>MWV45E41^Rv}y z*$DhK+J~FX?L*W}1tF3r?Y7>Ks9G44-D2Q|S6%RThfofT)lSV2+x78&e>%$Z?Vd<_g9S70HnBu>he~Z51^U3-aJ*l)@+vJQ`mNFPVM%Xc!m}ek9eKez zERjE-zTbZht{RWNPY-?L)TYP@!q|JkRx z1RfNVw6olAAGq0lyQZ-757)JTF^F@TM&9O$8fyfGf8VkP4a`+@tk;`%DtZXDgeILt zRzVVB&zM43f@zxy!Q=b}y+No*+q?OniDx7@o3tR+cnv6D#Lg}K$9lJtrB_<4s;w}& zfqQ%+IfJs8*JL}IC9$25lC;rfhm0X@>)oB_2cUa2ba=?&ydo$Mv>J+!=Uzq>%$@O#oL}Cs-qfR-;+?3DT0E zhyWAwYB1H@FO*u*bu3PO7ojE!l!J9wPo12bj00v1_R}BtzOV{Q?Pde4SIsSq#E8-q zbtLtRK%<1#x;r!p(1R~%h=CgmUgS* z@LZuit5Zy%+;8|bD(}${1Qx#t?rI1E2Y7BcWmOFYHL$TTC7Vx18U-b!`JDJr60Fc?E)9`n6;8n_sBLSFM9NVJF_i;QpG?l-5S1dbOYXa$@|)aUsHFwO6$ zLj3K1v*AGJt*`?KEAbhdqr3~gE;1Z>!_kP059TQE;9oj56wX(+OFR?((dZQ1MR-Fk z&Deh1$laDKa+sfP>m?2A&?cnhe|Rj{x9?K}WVqixXQw~4*c`IHGs}+~x+_ilxvVYY zM$trws?)zw!@5<$MAqI8CZAK?w(5gZmGR*ey+eAw$0PB0E3Vw)xL?5GCy$0RDNIw{ zDc1=p^ejgT<>IU|6IyhJmW2lAeW3jT*nh9i^dusPo_b3hd`&7BTt5rqbJU&j`tt34ICQLfT!mYyDm!q43SCC^4p(N(O z*bSHydt=-)C#hPahts5r;N5VVzQ|94%gci<;iv1oJd8Uc!7;;*KFM^1=pPEbjAMr7 zIu3}D-4o`X+KIz%In$E-e_{@X*sWgdQe2i&2NVUJJ#x9P2a$C*;{=DMX|qhD=ChOI zVe4YZP0on+s&KGv1EkgZfFK6}N;LCnOg73QARu#v(ouAdR()hvsLR+4G53FQJOS0g z0S;O}TN~W`<(ofUiP9>}e&~H!ir-Bq-gKic!?hfr=&juNdCP6ye=x6MvuKHMLIdqm z(&x~1V#KF!t{>|2Ht#j8CCEpab(5**X?UxLgyRIWcEG**5SQuTM7fk3jXvp?E4kt7`?Jk0U+a1vn*e}ah&x`akwx57h@sK@KEUV0cLlR4X2(H?rR1s&fkmQ8s_z3C9qn4+GA6%-Y+Bx)c_a3EP{$ZR| zr5OxSCXV1Qcta_mQW4mYsSF5x7aF$G!{lrSf4~m5nLYd4tY>LI*eCVC4U-Bps_-TT z?%@Kq86Y~0-j%SX1H>MdBc-oCZK;Q zU%KfV*qk3O&QEV=FK1I~=*O2-Eu__%n2w0Wkg6W-w^c8TZ25qAWet!Yb&( zj~cis$uq-3fny6(Y0m>6YNZaVL+X7zmGgp!9`H!F%~e(pe$ix+&-FbMy$vzXnqg}T zQ@W2d!y}bC8SZ#)64w`Jwr3|the9JFf3r|Tb-KG~&g1$US2Qn)A|^87ex&2OiG5F0_IoBkf`eYsiPAda_ut!f^#{#RT5) zE2}^t1b28ZQVgPu_X^clok3o`T5|4B{3M z5g8SLKo{Bv%|J{3Xs|O4MdE^G`+yKCxYAm3PXdjLV4<*{A}TzLFlm;7kjrS`^l8c* z1j|iwO0R@v&}1Tvtr)1OmJ6|@e~xe=Y$ZTqG|eB{iBVZ@(|9seo|XnQ67g!&8B{pV zZ10Sgjw(20q^@a0&e@VNtwMxb21Oy9HqJ0VAW4-X@&K%2LbPbfDw4CpS3w43>9j)` z0d%y(>zHvi7Nv*&G+Z$4nRJT~DOycFQQ_epG9vt82usD~*WjPPK!&1Ye+B?Xtz>c? zZF%9jfed-&n8A)3D!~{-GYYSPfrJpyC8>uH83sdFBBG82ix?o&;}x7^g}yC`T_s}R zF{dg(Q~`v75g265Sj@AfJcF~+B&N?P$SQyi<7Xlf{F9tBCQPY}JD~Y!5txCcI{K1H z4{$O{1z7+rK-0ga6>GGJK`=#B0e`|u%=c7>Pe4~VL;ETgYyp1-NbFLORsbEi$}otD zCus&OdRZqCQXMTO0#m0fnqeSs#zJ%ew51ivh~z6r#h92Kk?Kg>#5OW8R!;LEBb19% z5LQbGR7xrku!44p&U^R!+)5SB`1V{ zzEBV?LmQd!$Vvf1CNM*qfV!$un^X@5*&?9zUW+n;SOb+`hqiJFYG5B!WYmEFtU$49 zRdfukFeU+_G2+f>~>nC?BXU zQ^J8IsB=l|5X?c#MbA*^tA8MUks>q1=|J8{8W#msP&wF5Fj`TRe$dY7@)^wyjaPl z)0R;~RYqxmR;{lT<3U(=R)1l^9PJtrDY{G&0?h`U>lo;aC>*Rf=s-`>T#-I1MZ(}N#S%*x zK=`>ymY~}p;huq3Ckgc=7BW^$ZbF1};sjJ#F}UMEY3yA52+yH{h?)NPXLc=C(?G5q z#beS94BR6T*++HM$DGN(>FZk zIy5xE`6Szk_GRyTPdcF;914s}Js$C}0a&n-HtTz&3+H>vyh+R`bGSH9;>%e!G*R#Z zEcD?5gZCd9ntwP;XHNoVyM&Q~fzJjf9ae{Q4w#3-nqp0c)sfp%n+>)7Z?01+ zXGW%!KT}-#^|DSB`u=>Bg%lINwZAH7wPzFDujO?N)oOJ!^;d2)N$Z{=!u?wJ8tYhwFEhPeNdnK5H2|zr_|Bz;uL-F6n?2;Hz|UY~Mj2vL#}JqdjrZHq~IU z)&o!!Q#_yO;hSA^w`rD`V>%sHX%g4>f}f^p$<@7t+OF?*JX6_1-DQ0~;lV0x{peH--FFAETDg_8s+fUyusje*h&{NK zj1a9mLXtC}-5nh^d`eMC&Nz7Zlz+5Xp_5c_zn6Lr;gM34R~d~;L5v;y?YT`b$~aCd zvOj1J&L>%>_V2?Z{K$}GI5Y=qp+7h>wwdFRslgfjX_1SUDwlY!5>tUP;(k1{*UQ9= za@mP6p7uX-wd-W#YJDFBr4vj!I2FFuIR;0 z&!tkq9s<8xsQm6w@#l2Tz<<&)(X5YS!WNajS}ZXCB5xYP|D`PJvP2#nSsnHd>=cz`FjuD~6@SL(Ox*tPOA0r+ z4boHwUbY{eNz$&8GfDYU{p$B9HoF*==Fpggi!z%-qwlJ&HcxJD<+}p%;R4{E1&^T+ zsI;!S0S_I_reA~l5&D0t_>VVID4FOx59><&N8iU9VEqdiK2PVJh!4bT;V<27*PE4G zrQ%-t#6W-iMUPdIKz~l@(A>h&C!!{%L2_mAN4uro7iuo|-B&6$Px4az*-N!qFZIv$ z-Hz%<*$=hzZ#SoR;*owsb~CM!$Nka2%83p{Kxa~|g+4~h=^EzDal_W710eLZ5|Y3+ z0mo{B6{35$>pr~?=t8)eI>W$07X(>{N1Z&yP@g!o&!yZlM2pgPaic1I8;2Fhec z{8heH#pFE3Hq!(IIHt@8q|6+0s*{UFG&ku3 zAaa#5d9l#Ft?v4!-t|dPc zyKmdXa*&$A4MP3>fsz8hPN}(k=si4{_jE<}s^P&H`d-rUL}}MnZa3>KW##E|=zIxk z!Z{!5Gkx$8B*s3_=W>pxLGqVhypmWs?8Q*rH{EweQh#ouewVUI+nn|?RILS*Nab#+ z^fhN+4JQgm3%h=I<=EJ-TG`hl^zEn4M{Gd9fwDTO5;09^j!&_O&U3e2!gQUD+EPgP96Vqk53+vEix+BiOpoJ__QNyv4l0xz#?j2v=^j!D~zHF?A| zWP3AIjP#_@87k+Q+J~#<%plC)^jwTkb4Ve_=A0`5da|v@{`g?iTsdJ~=D9~-iL9K>$=xIa z=8XAp2293uli%>GCfjDYX_~roMLM}^pr$4+OG+wnO0YY5seJb>JU29?DT62<({p}a zuP`_Kfk??9awZ_5Tr{#@T*)4s&&DPDeR>#sHM0f0>!A3ALhNjsnSM&_e*xva@ZXbL zW~%~A4U-6GD1YzFN-pfMkKhYP3Nx3wRD#Kq4JPJMi-m?7%O0%^dhep+^qAJ7_Fq8#uz+phx5x>i|T8mlFUB=fbBTlt6;u0z)t0 zT~Zu6JL`!UG>)u4QM{6 zsObde%Wje^AK7UDz;P1ufDm)Imiy#}0Tew6@m&hJvnM7ky0O;gs(Cjw%At7)$$e4x zuDhVlzBSwJBUXKXIygkE-s10eA-qdmXZ<1(9 zt1WHs_K{?_*u0IMs#_!EPxS7aW8ahO+^#b2Ytr{kLh-+DaCAWB-D+Xcv&C3$PxX4& zYTFuQ*{x8XR!(hwb+^}A^N}{!g1u+i`K-Dm3@0a}?8BdTIm_OSuk%s%vRG94qFQ3J z>>G)4l)cZF#j2d-%lgEme)Tq=&c-i_d-hS_g_A66XMZjB$=Z0X706E6V+T%VRnBi# z*Eji+4qIwB9tVpT92@oo#d}Vh_#r5_TJlTp2w~2aB0K_*n<9 zS~pCm0{2l4(itU@W(MS{hzaJA3#H>i4Je~98=|2TdgOAWhKY53jx7E|lTdAhe-~tS1a#i*M=V3&A`I9|qqww`zOdLRX^)zTJNcGwyM1r!dziU839gg+cBELkL`)eU ztV5If0_`JYU-Iqd&aKI}vleugj82N}Bzav|#D7upjZgQ;(H@w5{Td(LzpmS6yx9Xs zbIfE+f#zT%rww;>8?7U%f7M9$�CAvtTv8nzu>NZ%u;RvYu?E8%RZ5S?w`J4TT4% zJ8_HZ#Y=mc6sq;fPESs13E64AYsfCLU*Er`|7SN~2nU=}=TV-ml*(0F(%-NXZ`WytLRPSSS?g zUIpf}`&mAF`79s&{_W=3^B;_!amA!EVs>*k(`sg|W>VVO&2siG6IZ|8{CId__HNF3 zo@cMtkE?23ZLTEG+On;;8yxoh2gPRvD~vD*Du18NdBKe40n*>Arn;IdtFjMQ!e#zi zX3J`KHRsu)sc*05!e(*EoBfa5Z@2%k3t*j@SytGbBiMIyU5ISCSZ~{HLrnY5!z~x> zzJvknYVM55e%Y=o9P5$dtk60jwOf>{vH=vdEV2?2#Ebi8yMF|90Lx{#Q*RLGb{qbC z@_!3hAmVU~XN_OOU$5pyaa^5W1`gcs;zb#L7TXn&%3s2Z1U}1Zvu%+Eu*BVdf%81z ze>`H^J-MBiZ3{Hpx7F@z1jw_B-7}J0hAWu({=VMaUq3#SyJ}>w5DgR1XsWvk7bObg z7cB~%wGUOi@lZF*a9IT|K+=R8+q>gn3V#=A^9~ns>ktO642X&h!Lxl^1&F`qI^R{~ zCf-~**zns`wTsM}&-koCj>U_&o^7R+nI7^o-YhrEskc<-25%uIoFzBEzj^k@Gald6 zh84M-nSwJ}@Y!Ph?A>qqYzfZ*ab^oU``|~eXF>|*G+eG`zdZZzD2<{=uMU*sOnP_&B!L3I)c*^YQs*}NuG)b_J$8agBWMk?)(jC;g_}eWHWc2s()AHE zmT|5+-r3c=w%jZt2P7*3{h>jBaNuK?8{D}ftD1GaDOZTPI(b{Ulz(8z+XH7E5dn@U zm@*LsuNoHH&8{6sK|~bZcs1ey`+qk|P-=PB?!qnPItr2Ob-Y{5G~BhGeYq)^1A5JK zRuoZ^THj!UW5zR$E-%2V`M_igWH9 zz0fBK47T z7XvjZ&@f7;MD)BfLa$~0&9bFl>+NEK9S~9|8oOlkPM6#Pq}>q+LJH@;h&z%)03#wV zCf??h`T8~v^y~#b!5{nT46D6}d-yXY=nzB|x=3W_k(5RWxRa*JP&kk4PnR=Mq{6k@ z-#4^*08M+;oL_gmpEBNao>)M>F>De>f;CWnztBvd!zU!P-odwQcr%wsx9xU=h4^R z)hNx+{S$Ql@#dm#@!yXl+nJIQVxqR$u2xkuZl84|=F76)#ErzLIs_oA>cbgj@;m7# z3AlC%kB+HNDu0yvopffn^%5_5=R=cn1ar@NgD!!n@-BZfs^WWtBE((%eMMW(W#{iAXlM-pFFiB3RnUZ&?K zNMFYwqFUhKhZ4bUj-k_mu|Y6AG!Tw(!zT?vdF|5N(C`=VHnWhu5>ecMir^VXBgFV= z#ub#nFKyWZk~@?y(4OCY8-6i+feFZwlpdH49@DQkK79w8WV>%s-N3awxHRUx~& z*f#NIe{OtIHiyyiqC%cQw*z>p4!L4_;*fb#$A8R|Po(;-lsd_a;-J@L#?*=oSytJT z)ENQdEeXN}2qrSO(q7kqysR;myuf8+5%Hw zkAG)(jbZ|nhRPw56;~LHVtxsW~YT=06 zAfJ1(PREr?7n)3jn=d@Qz%@9v4F^0V%kyH=y&>0B5NmRR$y|gry`@odh0fj%cy_oh z1FG*H+=|C${VxA4!s*Y4%$NH}%TM)sWPhq$WZw@Y($nN1gYqbYMJIy;WW2A_A;T{~ zW+WA%zygn%_CDUyLY$B|;BU%5YtJ4*CS@X%!y*}X$kWZDYRb*ju4|0D#iwxcxKH4K zlZ%-I%nvx9x&@f_HP=|@6Ny;^8|uxPPvP~F@||~nRJ^o>9Y{A22$7j%zCxOw5r0%R zY$K^9mF3yX_(UX6G4I)W=(nXW>-|3e{>={oncKRIF%787 zAu_+_c}~rmy@V;@`B&tOh*H^*W@oWogZ=k~P4A82Z=N;#4eGwOURP)axihdg+vX6$ z4VMhB(reWYJO|wE3ik^(_hXo#Jb$_j1X69ajs@4o$7Azl75@iA-W3=n*k*6!yyI8` zH*I|Fc*Nep=(prfx?U_YAnFbI0+&TU8K~uBp561NTwc7QSaiL9 z3h5kjO-h4ok!4bDAcRkg#v5u)ifJR$HG$rTDL)xc;Mmpbecv4!Ab$%6Wwic^_$ryY zL|whq_3;I9O19t|dMb{w652+JI0BQo1xcGCnX4-ycXQD~$EC_YnYBssDy(f6GRJ31_#0BS#3yQc2z9+E*0!qkw%UbN4rrOXtTND@wlZ(>6LVLlY zG;=PgD84xOW!0!H85@lfH|j zH|R1@)J{0&;D4ew2qTO~@Dr=%Y3`kM)RkTWlUN}X(sJGH(1E?GCfvU*SL4}2c>bD5 z<2f8;VR?p|q->gOss&-OAUwxWbmS}`Fs2B@Q3=?8Cy^BcTn0qun0u7kO90YiyTM>q z<_fN=a0_G69*L)g0F1O?N({D%PU|WFTK%yvS1~);K7W)kntHkdc_~->idJC2=MKnT z%&d^C=(-@h%1X1GfjuyyJ7_Wg?;x_<4YRKM$GF(0t(Iw~6!x9_4kR!mJnHU2tE|sW z92{|kGAprq=_zbVs@l*#Xnq=A zO%f8E0l~%PgglO)AY>eLo{&dqlY|^hN)xg_>4Yav6O^W=GE`_c<$V}%<>pxBRb873 zDNt|E6o2Gc@y7;f0iD+6(>_6H@`B0G5OLXFA6Lgu>~$P;ey@+vCii+UDZSVINz)X} z6ah?Om^7C&dvE+W0lGnF3DAQ!MS$U?Bmt61$D{gx7q3T$Pm;VI%OEb4L1ZXR0{8C6 zd4HrRS!i%B&1Y04QWB&g1`n~X z-j_7y(iB<1qARn&jTN?9W0e8S|9^QGt^9ptXgs&~P@-4)RC^E3xPMq{h|kBX8I2E1 zK%UFCtL1;dwH)dVZRfeRhaP|U=h(Dy73Ev!ze3D0UU(5qoh~3%55ya7Q-9VQR8ic+n2czg88_vM-oo~3SA-1LTv<1U`cH?BXjt$P5F2&9fEv4cS&bwR+>val(#H6D zRjwbeiH!t86}bPxG1#PGA4|Hk$BwSJj7`-N;)kcpx$BGcH?(AcC@xlYZ1WWfR)&Fq z0iawRju;6+pzoN*o)61p*RcQkG-oIs*kUnLPO(DDsehay9#>KmDMRVN18^3Y9308K zNZOqe8WjGtfniSea(HA1!V}pEBD5Rr1VL4Z1ur38R{^3D6*fbIX)70t>Jd-Vp`t)v&Hpw{84QabgZ&(MJI)HWDy9f9SFrYOn(xSk zAzn}Ut3^QHAKM^NTGUY$`Q}~T+n|(Q9jy+He}99W!gpz-u2i1=jo?t8V~pt%7Ug6z zSlPb5qDwCtsM(>&zZ8lV%U-Eg6*PvXAW$66qIGtSooP1wnCzPNb$mtCC%mn+*^A(a z%y|JmRw0O1ps{A#Dc7e9n#&8N1;9;83%W$nWP0x@8eaVWGrS{g=&_)q&$vgPHTvGs zw0{F4qzq?7`VPq9q#+)RxVgg5W}pjkvHxLY4x05ItSweG4K_8!K7M@C)Zk3SYY<>U z07I*q6=z!23>7bh7d_o=KXUQG8{F~T)HcK<@NaaAnmP{jWmvfHTOPx}<+6L%dwcln z-4+D4AQebppU@ZXJ6yjGh2RelRa1>fn}6zg3@6>0VL>=(?7JmcqwM(h{i@pBcQSCw z;vO8USOV~52~D}#t?S*c-frgpA6Uq`i-7-$-$*$2&y}0|RW2oMh6fPhr&Yei5l5HUJ!jF-&%i z0=YS2;(e|wWK6wl$$u0WYl(rY?t1j&$t9iZ@WH6uWUqE*xd^v@DsL}Q z^zjWP3w-0s`wQ>Jb#*b+)xU*z>+qp2e~2ymT$DEQZSg>;-jOdATWRq2x;teS|LYx? za^Ug#T$ZQy=!cZ>y&;Z%igo$83t;`H9hBb;TI+ZH!|~)pIKUBXg-R>NaevGbeNeUW zT2=WoLh+Tl8xU5q*dIS)g|}nI=_C=;OGb=3EEAt4OKR=ADSeJ#34y?rJB*yo^wA8^ zoF@lz68Y$WSx^z(3Di@??dvN3qie9I5Yi$8`jeEgZwi_4RG>T9reJnjrIU!J2X7C+ za)|dPPtZtawWGc=kQ^BW{eNb;=pWuE>ssKWan(!7zpw7V7E|AC0{h^-zuxg^s^b!( zTcgiorr2NR0>w5#%So+YM6G|&$o{)jcSDWeS%dH!^?z<;`3rjlu}?sG zA7AECSwuK4=Rtqtg;zJ=4e!Fgn{68|sz1B1-dA&Se;V1Jaq$PG{UyfP_-Jqliu@EF zOk=DYq!RF6Jlu;9cjdafMdvSZP|%89iw@614%1?H)7%;`7gamr{q#^GTiJ(wkRA_A z#rj-C$UxxDlfJ5O`(5cu8{Y#J{>3;3KYuvVA}M4>AKeK74ML!c48z1l+pG?U=}7e+ z&X%8K)9}$Tn?}UmM0eL!d~0fDXmS2Bhb7?Y2L6}PH4duia|^11dwzAD$E1hm=>GtZ z4>Wv}qq!&pF)}xo(F+48f6Y8=liN0u-~B7(4_C*jwP?H_&iU$ciId$po76h9w{;h9 zr8p92hKU|ylCtCc`s)V32Pja|Y?7`0B9S2202=-72Fc=XK^9*=A(Q|AzJBuj3nLab zw~DF7_3c86h1NQ^OfRn2i|?~9%c8!yT5^`{ui)qBtEFT-d-axZf3>R0-Rk3&Rat)Z z_v^2EpwGV$bOiDHB_)J(fS3V5@B@H^lvz0f1Tda|!DaL+_IXPf^$#a-g{4{5ctE>- z>n{rI3iDV}np-LTDw^`4sEf9Qg(#90ZCh72hqladH7dKlG8|^*pRHE=!wzTIVgt=? z_qBi0*2S*bR!vjwe|NrRvu~Ob#;4h{)Sz{_KCEC!L21_By9<(S_bmd%w$*;uw1pq* zMRN7h+U#|BV&bK1lRZsij3|#Wm}2+x1aF$bjxCjjmESx3!Ms-cx-M7k=HvWIpeN@X z5%S@R>8z;Nz5y2W?ofv75t61<5z!fpz;w>JAK*NT&1>KI?tqxqSzEOgtR%6>_P)R^w?IM<4~|fQVzEJGZo}ch zx@e2$zAW3u->-_D@8Ai$46XfQ@KLc_`{qLfO{sB_@+6Q$4djF~U#@WC_oe&$2_G^~ zKiI3sP4!FBe>x-s8(7%LGIVsCv24N$(nH?W#Tt0dQ+bMrnBdue0dpuIGNhxIc!QkC zMd;6>{v0^*Ws*=C5b9glrqnRmo2#YCUUn2t7b>?J-iK$D*j#C|;JKn<(kwyCDmRuc zmZM&GVcP+dxCn#i-zEm!i4Tw`gjtppB3S+WO zwJZJATOx^*BgM_83{*=NbWtPe;>DXUpCETaR~ivDK!tZH?4u=NAF{M7)88uoZ+@GV z06OC_uJ*SmhjdmUp9>)W-F~^*7i-@d(E!{ZcI&eC{R-dWC~&`Ni@P$s(v}a;;8L;d z!+lwoX~7L0ze>t$34*nnky*(0lw~!jKbkpJe+73ym33Pcctag!day<vGR3y|` z%#6uZ$lOxl=`{e{CFm7sXEExMCRR49SvkR#5t@FDgT;}cYEE)xRW!A2@pA&LROW0v ze>+65VFAHnL}_@lI)F|-A*CG}lfn~lPE0f%l9&t-1BlI0C$G=MpwAqOFiLX5CpN&f4U2e+ti_?aT1#-qdc`DFztA9NmAvMvWT|Z6rhO!wd3GYAUo?Uz026| zb=sm7Hfd2BlPj)aP0AQQ=7VFce^=K}-aVm#m8m2-P_pDUH?TFU?UV2SP8Mr;1~Uef zn#Bj#bGvYKY2b3R`0mMHygwYxu?GdKG+_%;!~$AJrTI*r;WJ16ghPq%X#hM`7+`;Wed*D*|<(J>;a zV-jWw+Ljz_vtqq=UK@D*)va&x1{O4c5MQ;mx5M27XMW<7v%=YRPwX?*Aj-H%?Q{e| z=aP!?PXA|`V*nu}$<8=Se?%47sl8L|eH_C*O^mS-bh#p6MuOW9OIytttkPy*qy1uM zW`tf!dPWG{S?B`8YZ&8-Iu%6_LVy%p7k@NR@&{D=!lFf z9yVBH-b!uDJ5)Ps-xdffB`k$z+4jfJ|M#08KL6~`-(G+FCaL0{fB7+j#7$f1L}zzJ zg~k<8nKO|;RBdqc8~@y)JxM+U<2XwSUih2z_EOIYZo!+fX}m3kKtWi~gG|EGc7O#E zz@tAQ>Al5@5g*S8TwK5i*gYli;F6!0c)$nNj`_fws{nKezrf8Jqa)y^xDDq0278@U z!G6@SF$S8W(;MIne_Cn5M@4ha>5X?!a^oI`KWOSYFGKI;sJqu6V|_IN8H_8cJEb*0 z^k4cJo*glwTg`LJWIW=)$uDrf+)kZC0+Bga=3*oa-RF@I`kY0=0P7SICL<=1FdlJ) zgs%g?AJgZY;UvK;s>`NoM)GjVJMcR4&ikr(6!}vrbFGu~e;)}{(6U0EkHcWZ?dus) zp?e)T=gfdkV7V47_G!i@z|vs6!O2#<+Em*L-2b~vLbXnUFEvOXCl>LdIovG!czgo$ zQv(y@6)y1k!dn&X4}%LaRWjfd`SgIvn$o2Q^w_aEa$(XQ&$@cKZqIldwhp-Vzhu+nVf zBq`zZC`wW_Geb<|q;*4tJdXmT>Uk8P4kmV3W!UXDopV*nM=A>VstWf3>q;ZF7p4hOZ zcO)dLf5hA<*%@d^v{ETAq20Ci9Dn#W;rtgc!+y1+K9E$D}%Y!!J;aMLA#I+1J#Q8r4GlIfwKMXVjPi6&C#gQZgj8+1v5*{+4eBXf7pK4445fAidGCV4W4CxEO+$~1sDbcLbD(_ z#A!e3XgF}bWgjlH3ki>OP}qP035t>AfUeltl_L<~ z6jqg@lj1Px);IJ*Y*O|z{XYurO5)}K3?uiRehT>wU08ut%}Q zo13cl>acNkqpp)itOPGS9dZ#ub?<-+GD5yEITx|r(DBf*{mF^*(4s7Cx^N0Oo%EB8 zc3TaGaez@Ib4P|vvc2?~e~-I2dygC344=9s;D{+ZwNRV=CYW1#ykDIZ;Ci;e1M5lB zPJ-guAJWTpf-A9Hj*Fm|te4t2SLSL(UBOmz@|e2^LRouKM|swCaRZfenh35iK?eDV zH=}5wyT`DgyKw0%#_bs?W>QiNc8LMceYmf>Y9@quTbAp_U)~g5f7u}3g@ngGsZSPQ ziMdD-u+~uU>ldSJS471Wpyu7dTLfr@v+TaQyDw|s>v-?+1#nuxxo867T$4jb(%o@5 zdghJa4Y(v;!ypX?>)n^1u4iI1zCag4xBhQbKY~C;#aJh7Xy^%?1NMt#j!I4#2UH6e zPl%0Zy)Zy-XeZtdf4~HF$?CBc4SEBS;&?|R5uh23qRE#=qZy4J!;Bf-Lq;kz@{}E& zoMseS?SX+grS!1*s$< z7C}g!Qx`9RzS6|ATon(+syZ=EB#6%v!@J}p&%S#8dM4&fe_WCw8f;?mPlf_^BWa7D zsGN$};_onGMj#Y;Z!}@@PnSeR8u2ox&?hy@7y$T)K#`+PY3xuzWY#421E_|$vX~T# zKpL#}H81!qMGGd4rf zOG057JSCA)8a$=z&f3OSr;9Dt(Ou>zO=EzP<{V9621c&Z|CID~%GkBJwEA~VUvn&a z!$>zs9MrC7Qny|A<*5I0?sf`vHhG&EJ(0bg9W+)=e~jf)h+$|78v7Mt{9Ej3kMgogBu^{ z#?ktie*nUbi`)pxtA5yv*DTXVLsFrTor|VhldVvM?jO21jElqe=*H`_wY=lpiX}j% ziQ^g3tDqv#BOLWgmm8Q6M!;#VyV^59Deq?Z3y(zXFDfB+_=9)sfz3r4SF44sfuV}Wxi_qS*=!f$i=1Ar=Gj>Q8W5}E{nmP+H zeF5O+@g6563oz%wIMZIIL)k^5?>(HR1CAzWt-WXhLP_HuO2Oi*x4@6{EpU z1{m+FX5~(I;SKKW%o#v7$B39yAuO(nn%6C(PEXUtU9iGV-s343VfEykM1P7Ge{m^o zUr`)RbMJj?NUFOX*{$(RbFvaxSWJx{igsWBunyVlq#6Q{MVk(Db=gxU{V56tzRbpj z$hrf}ccpLr^Wm%gXZwF|d|%9*SI8dzZbT-Y8yFa7AMPuEs%bjvwg@l4zYk3++XZ)z zv=!obYxZB8%Z@VP`bZY=@r-C{G^6 zS{j~f#UtbOPrh!kL)ayCiq~Dy_DJXI^IC|QSuLbk&aJ%QM4pUJ5>=r%+>6EsJbvH^ znDqIyk3sFIV6~6s>!zsh%IP7{Nr??5spE{Wp>}Kp%4AS|Y6QL|OkO?+e-*hEd_0Ou zV?>!~lt-8D)Wy215u_@^z_d5Z5ijA4Y}i_V^X=!aa)0${S10Me*0Ai43{Qp45I9&$ zAhx7p!(0Q*L7H$zL1I-*s0f00x){5Ea7O7AGZ{{R<7!_JL>Po2MZ}p_=&^QSn^b)CkazYf11EsjWeGnm}6wD z4Csy@irAbqr?l9QCoMEHwb0VtU;YzpK#$6qvy%Ai<^-DE-`q*ir4%B2b=ZK9>SY$= zu7D>#ixsOkEDJDR48s2bBXmAmXz<|uW)Q^_3_|B0)1t^H&}Jx5f0{HE&7KI)+N3o4 zaRz%P`;VFZu-U9dE(jf!8K%y!Z69hZ-0QRhI1A2=0=IhOZ!u(sA0cSG|9PSaes-De zT=>lF;R#E=_PfyQlqnupuRCX+5}H$@l78ZZvGrI(|ATye;qhtISw>{uE5uy9%%xTd zS3oHI`h))vg<}2-e-_ZRt?;Wdt{$rCPtw`!ui=vw$|*qn`=zY+%N_vDoa2JuNZG8D zRi90B*gk}F_=KI^&Z#WtCJwhdAmcJ2-x5CP!Fu}bqHxp>&%&YknR20IAqNqUR&|V> zIBqki>Ofk~6*REy=IM<1wOwXFWH;R>Yw)JoZ-!4AVKx9=f4|MgAH&8Wae@In!gErLi!ZS%_^5|1eVYdX#HZI?Y#~;fj5j{v{YQV4r znYUPupgwNxaY-+w(E?K&3PeXSXO`>G>E>}VDTx}EYh%YBK>7z!6CDBxCzD1{M-~t} zqEXr(7mJdFe}a=-!oD%7at4z;ac)Ck(o&u{xFICV30p7$b5~;`SYXk$@h4VJh62Fn z4pADK(SF7liZi*zQUpeFYV7D!W}(7l_LKuYN|B7zu-sqCFNUxS_{Ff~1dv~>xP)lA zI7hU&!=jf$DTRC5&qj@Y&$&66k0+9ACB~pbi4tBPcBm-Bk&VsAF7_NO&Y{$3s1=>Fgdc~y>||k*$t5*lp8z9)>OYl_ zSHLRLo-9oAz$l=HQ#&n8I&`sjcSwb3Uz=o|7P783$<@=$)+TkpfuUj8Q?*H{nNDhx z0x&~!f8Lc(xLDE)@YQc`hagO<;%Bh&qS_B*95EzHKVmGYTroQ?pa>xcO`__FVUs6i z6)xnEEDB*@znMwxj9*n0R7{l?UA`vzyR7MP-J8o$V8$ruLd?Gg1v&~%&Y?j63!xyQ z0$BfMzr8t$0+FZ2_48>+1MydnSMtR!QV1uC)kAvdOnk}P&+#ldM`RPG6A0h=3 zraFa?No3!c=-US_$_H?r8Pd4(C7~A3wgTQ&uV_z{A5eKJ9WaU>FuTC`!xQrYKL*0^Ji8esM#lEx{{w}~ z7G#rw(I^7|dO(H0I5Ihx(F+48f89K5lN+~@-}Niz@+Grn5ytzWl&>oBs~ne8maRA+ z(NU01aF`UxF<{^`Y& zPruT-VvIPYc=h6SrDSD{As#kzV~bE2;P{^HNwE7i*dV_enWfAhuFe?k~lf4<*c z30}8{{dU_NaO|gFNme=HG}kyItrkoZ=i-FlEq7Pk*6-nMM(gFOy#qq z*u2HHn(*~4PO{&&%WL1Ef9ltV{cU)37ZyEk+Rc6!y7q@K$iLNW_kP4`(&7d0TO= z2{!`T+v>+B&m;4gf64;BwM2Z!xwLgeSQoba$1CpYuyv28fF>>_1%MF);cT$a2vvX4 z99M^pKNi2|(q6Tdgb6d=t917CUNhQBa~VM-cZoTGZ2}!-aJ^heN@@KDL22HHBLe;_ z5hPPE@NHzI!c6rH9RoAnB^GaUlZ9TJih_0Lq%jz1Uc2G0smuv<5QH{dy&KPF`e)$SoAmCI#5JT6VB5S5mcP%Anq zV(aCrElMmUYbKrnYZ)FSG98FSc-V6f34^|9UN7&rZHHEn&_>vF+A5<3o+BNm15TTJ zF+cq5)tY2{f0#Z;QftAuhZPutSR(_hf8A|2Z>|EPC?VQgI8%di~(!9k*))}TPS4*!UhIyx5&wXraCF5!#=MA@kpTO z<)I0U`(txh9FcZF-K_B4&CgUw)Nc|r1-1`f?|IJZe_lH_Z42tki$~`YS%#Ut6e+5= zcrGea1JdbqF1Qf|QhJ4do1)Ud(Mb6Xxh9eCL_VJq2BblFA_o+}D3GaiuqTj4@#O9b zYdE4{)L;-E6)yYJr-$X*a}|6w^>MRXHQ`CnA<)?0w~;atbVU@1y&MOKB?^cPA4APv zf+dV8e=1K61iR4Mb^02|M!gt}heI1WgSCLIUax^#^*unJzuY6k35&Gfp&`>Awlfpb zWTwG6E6^o>MApXcC{?hD2Wo<$5;}gSTwlePudjha@emxZkIVM{uq;}gsMek$H5hzuyd0rppWsv?cE26PrEZe<033E88}lb)=i&XS6iz?|Hxfh=E0q0(kg5 z01K2*`9EI$mAZ_AtubC%I0C062NUJVV$!+APPA&QW=|O4807sgA!rJun4n6Ll4dvH`Dd3`dxQ@Fx~Xk>HO1uto)0ke=To^ zoe8ul{1Y_6WH%XHX(xcxKcz!S=SC}qCnq07w9kj{FZv0(BhPq45o*9(v&2gANZt^Z zaD}{~B|!`(-f(O{x@lAz5F0Ef{^6mBGXM#6oNP*kvzWx~5Ik{Aw{0y*P! zW)o&p;}%6V7=(6FX`uumc)-KuwL*KT_(pifc^M#92ZNJqT7Ob}9iNmSe-mKs>I|?= z;eA0Ecn5<)|0qlagwD{8B@~&+Ym)+$idd$r!Jt`)=e7*K6-VZQIaDOWbpb7k7%D4H zEEv~*(7zQ{fk_k|ECW95_LH!j(;pMC1tvTNf-@88KYH%JF%6naXwZBB8XV5dG?AcS z5LIUo8t^$r=Z7O&&Im*Me`WyXrm#j&wdYV#B4t|S8s1XLPF%PPIL?Vl~Whp(&WRrM%?4*dXN01zHSe~Yf~%}lz!yEk4i zD5IGG`(oH9h+)F8+^Rz>TuTP0w9UIYG2lwLfn5iF0T7C?rZ?>Ru7PFOcMeUvzPGSQ zVLLrGG=kTJ#D*uv{KR5J09#u(WB3ogMBW7Z7T$09VNqb+@dFbhLpQ; zA6Nsxzn%Y^lwObH7(!VLVkpy;f&%8|oG>2E(yunx_ZWK?TGj?r2`;1(Fn`sB_%L&~ zxq}9+o9*U`f7Lr53&;2wfu{5 z`Tus)?8@;#C&Fod_PgXp{;n9N#uz7~F#_m9A|$k8*tHx1cAXOozW+F2TMm{n7x8{gW);9|P>~H_dh(Y6nuvFn8XEASlpzKrssrA|zS1Z_5BN z**P*fe-c0~6*DXxEDUhm3JI2MYE8IS!vxE~de0W>Fm3bYeL$Mh^4>zUnioL=%tX%w zMtw5#z9_;b;Y6j%VA~WX058LPQr)0`6y^fC=VUh3Hw8d7F?NKe6EGJuUch>zgx9)& z7fU#I=w5P>yMHQ6d=iH-POw2!M!rY$I7i2rfAfdRcyfMF?kmt@XwHBOX(GYDyJ-${ znJ;%@0byb$&BfdsG*UH6;OlL(yKZlu7QkgvK)V_M@?dyJ1Sy55IW_}`tpBA9g;Xb? z2uMSlPncXvlnZ-6emxd>7f)@ zI-9q#kOBLB!2pS*h(Q-#$hJzZ>f`3xf9LyX{Z?=7c<1fI=uNB?75Id z4Bx-I|MULyeV9E2^t;c086Q9`9hFZ0mEfUFUy7)CCOe@CpDmB)nHI(rf(B@!ouq) zRIejH_%hU8vmTbaJS!kbfGdd6OmC2%Fu-0)Ro3HR&8V_9$ul5?$fVhve`#-jz12Oc zn4^w>qRc#wd4hChnIV1);sh%VcC%x13igl@!>Ii0*U$~ z#pW(Hq6_IzIYmN6vu!N;`#1&ALoXrF&)bL2@4tAEr_H4$nz_n=-_@1GfB7>NqVbG_ z))p2SrYf&9f`VZg?T`1be~#^@y>Go7##-n{>Tz)JH=(3vAHD?(7aAU`#Y&`Yb4+zP zA(G~?)BFMkamVDRw^Q8BvvT#3pcoHNrhN#_O@)_ZMNVP9i9TASv~Q=Swiv)tmQ_Z8 z1QFv>TQR7JMh202J#@C3fD;zdz)4%8Xf(clh&6B;q=tqCWbVx8f86WD9P^^250Io7 z?ze?ZR@fo<_bjf?3-AIxf^BsHykJ9WDNX^bzy=57WD4-unG;}2V29M_oL4#L%#BW* z>f(vxVN#jnX<=qO2uXyV+Lt4ASw$3({srjrk>pe8a)>TdL>JEx0feG#D7lDC_n)Pm zVf6|eBnLP+!>#!Tf5c!R6JekS4^8ztZH4Dsp%@tM-t}wz$3F{;IOj9;k6A=gfENp$ zhKu}H48HL!Y3)MT1_(q&-2?IsW#kTp&6EJZ;F8W^)s-~tQQ;|N=Jlk~7X+tZf>LY> z$ONpVF#)m*32-w#0Vy?M7_vT3fQD#eP7t63e#L}jK|leAe_EjOfrOe1iHVXWmLXU9 zt0jX$Gl&MtVkL=|Bi!)$qbAvg*0P>({Qh07ZfLq3Am+sRl+g zb0CXHvCr_pSdt6+#sSRH>15uA@g@lg#;HHj0TO~q%@~)2KsspE>B!G?Ir_( zO{bSlTc7NZ;BLP6w}axA#4 zeD>z9SjOQmWkwDIWv5^|6IcgE@lqLajuu#JSZwQWl$CA@F;Hn(2U)%^`s~f-*Y0j} z(e9Aax*ZytU3%^Be~oU#9+U0zP@vY20*YbfVQHEz;f7R)9sXCc1Wo9m2%2HBR&8JH>k%A9b38~9; zT^fOisY~0XE+eV}+?aaggfTm+nHgoDMY`r3Z;6g`7T_m|Q0K}O`*`&)>XP~w^&#qC zvgifu09>3)Y8x&Lhh0=MGh_W~aL8DRFh;&_Dw#Qlj#A(N{A+E`1`hgK8g)S}f6ZBB z6rBlfV$qA3RQyk}d&>E{V_3J@Rm%O=={?FWN$^Sc|0TmGFUj!9Uzy>H37vGUcp#kG z_)9Ni>IXG~i7hXW_{sF2m`(qQkC6TwCGi+IhggL@+-E5N5NbZiAORW0Y8OV-*<{|N zz~-@n;Y5+0AyV1PitHSHq+kdHf3b_GI>ZnSI>qp08M~t$fL(djR2i+R2s6{1aLLco zdx$sqxa1(-uv`aL+^u2}hV{Rf)P`oDX(0S)J9)bA_+V_clq)k#k+@!lJN{Uq?fDLd z-7eb|GTBXt^o)(DP!aU|!w+!043Js!;=?irrpF>PefWS{ILkv32txyuf6AXE>(N7cY_eWRb1qpg%E|hqsZ3Xi@PV@I7-_c{0>;XH z`N&kew`t7>%zFtSaBUn+Oo>(cS0%o{s397>eJu9teotT~^U9e#n|W2jr+PUoEJ6@2 zBt@yVXEU#`6~Jxx?tJD|OMglGQHyBhhh>Y*WnRTc%DevW@&hiff0IJ@wOnfPF%y$w zJ~64!-qo0tQqLqNf&2}e{{<{1EfCPD%%qgeON;X?g_%ie&SoY#{wBwS+R+PVXA2Er zF9P;(mZ$KyGq|&lTt#c5l)HebFf)m_d@s#R8ZYJ^otqp}nW9B1W0nZ6KVWH$ba?so zG6{XIHby!P5}Fmre+l=3GG|blQ1pE`wctdu5ClylV@_7Gn2Ci3m`J`wgY^-Ly6?g> zv~89Lw7&c+U{>!#b96-lySg8BESI%g5eYqa=YJMA2(}%^E7mM0>Ob@svf_jY^WcSSFxo~C}3d z3Ok)~GoXqMC}?iWKU}L~rtK$8#D$y|4LL399ours&EHE$DL>4*`CnH}*V4}>;Bpz{ z1DR;+0Bng<&6pwo8yy^(pbBMfWOHysEKigmCAB^{qOsIyDP)%SlZ2b8$bX_ z?T|Yh&W~?~8(VV0#U?K1j5fPe3)<>(GPKQQGnYbi8qg-EnM1S{tvO2^iZip71Xp6j zFuS7B37W>I>T1TYx{5hDf4L?RE30b-Ph)dYFf#?bRwL=Fi>9(l0gnnfiXDdq9j;y! zodjMIMp0Z6ZRlWTZtQ^0u$S&}soVhelo~dG*6IoqU21rXf~e&RN-WYUJK0GPuC3@4 ztc2}J0i%|k|}f~q*cn|#0)H8=TdVaXHHsW2OE!=f03{Y2$oDk!gz)b zI8=;M4q&x_CkQLoZa?aB)4H;RmbBQ41}tH0$sS&!kvqmVG?JT)jmCx!wyO!!PB;qX zp(e$t33sQ0PARJd325f-RC%zZ!OF_*VJ+2eLCi87c!J!uRX%DscJgPznUKb!vkVMc-Nm6B|M}|Xm(}`*-TuRN|1Q7`!~69g*MD07_%3#X)%xe{<-xs+QhmYx zV^v=xts{Es4~`lCgY0|v;)PqkaqFLUZ+C9}(*5rC?t(u|zuMjS=*6esyYIeR;rD!g zV6(pAfMZmzf3%%szCqPvDn1u`lA(WIeZ1Rm{pIF!&yhZd9k_!B>`2ZKhqKVUL48Fb z9eSi#eaKHTb9r^x`cHQsKW%T}+1f)^UP9b(TEYG7)_?um-(e@NCiMljb8~ln{r)5! zot=1S&?Fs<$%h&1sbmj-GD(*$d4qLL(IvkopCzFqF&v#QZ9+_fy%(5V&dux8-i|8^^ftrpXA$F1563eicIzK^*Sr0j&zsAw zlYY+Y&Ec@!-?-Ed|M2VKr#FYqVM|rTAFK7N-OZuX#8+^VG53G3U%{QkoQ?y$FUxTN zi>l!`e=x?vVE{kFXx$GOuIq6%s8tHbITgnP$3858IdXgtv?|K+2`x9?YJ6ZYP6m=h;jXTzq@?1h5bTyFJHO!+wHFh z_kQdWUT;2bR}l8iVS979g`?@F)9`P1cl*oje{C0iyTAUj{cyGUVfU-+T!l9E3Gf2I z!`B8U@Orby5{`kRBk!EeEnZICL6!(R`nH!cK}X-lp-n?8Lu*6Z&wsy+Gm8bjp2pwc zB#4CI^9y36h2W*7)9&oU=F_#+<(xb~kB2jWWtlI4FEKR_JJaf|6X>*yx8+=Xl;R_N ze?dk|NC54}`anln;q&#hRBjLZtBdXBFR_KRAUiqu2*pcqB(2a$iVp-S2_9SPnM2zC z=jzZFjD0XdPjG(EiMGxiPjYg7_i?}d$A0_ot6K~|ET=cMdV(?yUSoDN!wR(Mfx>1f zO<9hU446_0Zu#(ZY@p;hbQDlL+AkQlS zE#%+epc_jG=0%4Nz zh6I5u%HWAq%phh){ezUdsv>s}wBHHHJDn&&GnlH5a6UI1^)t4G=>ETa6$D z`hEf{J_u`RNs$NwRtV;#F#HSaga}|25JcgaCuFi9e$L1$lyy^)4xMGZe_y+39}y@| zqr|J*&F1oYbCJT5`kIUR#5yDZT1H4kF78<+PLK}TIo4Ai1COH*o&p{ty3@ep5t*^l z_EaE!M5n<3VdErSW(A_$Oxc3#3ZNd zJ|nzz8m*iz0-eG!kI+|DmKL9aZ==f70=1%IUjU(!p9zF?05}6{y!Ytap*A$&%47pz zvH>vJ0GMn5Of~=}8`dksdSzIz4C@tH5 z#Gtkoq-!J4gTVw1uF*m|9~yPZI#=w3p@0dI3ds<%n^#lC81pMl>r()hIglx zr3===6Y;}&q2vXNIN28{20XdIdV#b{fPaVx&vgP%E~z2aI#4~7k|Ike@L6YyN&go( zDag+8S~cYVu*VEof2+I>FF@JJzWwc+hLy*Pwo6swd0mcS zLCL`L3xZAtI4S|U1J7TP7i7he*#sl)VnK=Uh~RCrz_%dkf9KSfwR*-TtUE5~aCvze zlyDIl5kJl-$k<|Xq6G+jLLi$3Pezc4gxq^K8g=L{I>1d%@>{F1Mdu|&Bf8T`(POe? zh?Fu(rlcN-V~&Y*n{&g z6~SvNNG*vU5R6z-DGUevI?#BtNk>hek>@hk6->(vS5&G zw%}Q+q^2>>ugm#DDxDmfvc@ZuEO(ntv0c%(LIvM`q&IyFAGW}df2NjP&wvY)F>2A7 zNS1BAju1l~E{m-ndCv3Xy4YTMnpRb_+->n;3rup892VFK^1xVXl_Gy0c86wt3rx#A zxtK5D?XtyhKH;CT+)_(hZOW!C_e~0Y=QjEA8d_rHN+Cf6+qT}V*Jb^x0fBCc&r#yB z->Qj`)RZ_MLL@(BDvL=nX@F9bC;t-_Cbnd;A+bO91$Eux)Zs<>Qzk^YY^xiTk$>1Y z6#)tCDv}^Q26Ew7CN+O($6{S=sGV|8UQ7bb}t9!lx zGg$6nfosLUvLR0rj8C)K5t;2Z+6jEWE;jqde@cCbq%JQ(;M--_WnDzA&95MaU4849 z(-v$>v7$A#yQF`I#d^JS2B_Lz9bYS)1%sc`OI0Ml9yVx?LJL1~323ct%0ZHD?Ds7i z^@UPAxtVL36m?Zxtjlp1BPm)7<_?9^o4m1AHx})Y0&S1mBW#J=Bd|q^nPj&`sX;fz z(#-<8CA_$qgBdT{YKIm+&yuF?z0!b&k&L4YKQ)_)2Oa#zzuC#Q-O2Os$*yejDyrMY$*1|G~UGkh8`Ncfm+ zRyX&R0|&R{NGA!ptAAXif{PhgA?DxY5y{xp=-DE8iI@^BP6aiE%n9b1>luS5P3czE z-idiv^^%3AV!12x3lp0dZN zBH~sV#jURas-m%8fN5ME_V>fnAE!)~0ATT(Z;?Y6sEv1l-j7x>c$4NVp9L?)?VbQX zGHLe&ph$9;#w31D$}3n)PMYv+%GeS0pCi;W04>_u0r_W<@$*y&7LvcOoyWPUR_F$? zd*^?bZoO78ZBhlF4lyb@6Rtol>l%sX4*l0PI{Muffiknn-*z5M0}3D=uD?J%M9c(A zd4O)JEt*XHx+&U2?d?5w482B{*t)1u!FbMNOBNq5aX1$VH2wj`2y_9FN*u;P1QHdh zG$1H8a2+wBniRsO&~LOm8uF_4B5*^LpO=4S(|8~Rcmf`M;iar(nj1Od6{f+hV-EW7 zdXzSy$s%Dk*Bv^ccX0K4S)ewp=?k}DP~LlT3@|l1Imf|$m(CeF%j=B;K5PL44UL@c zv1L5^Q928wpR=jZM^fnHeqFGG7n6YAiyL${m=?giQVmqSA8hbBU{#dry^B?9dpdt; zlE&j(M>J`_Jce*Ee-=#qX#Oao+#SH7t$euE!vZv-2n%8Xlo4QJ)DY3fv{S)?m$3yu z2t`0Qg$+gU%n?_NGGS64Zlv9Vx;@YQ=@n7WQ8vsCn zeQqZ{2AqdBu7i^eI-1-sTI_nc+w2bxpW|JKR_+ZcZsKN4cj+gQb;%>V0bGAoUsMnR z)|Cqxq18JrymVN;-g(DWqcqS?V-s3<_j~R8ajvxo^JA*smG~jrkIko{~c6#oDaAGBGpr@cWh2IPr>dDFxNfS zTis*5l};JN^%K^6Rk;jR?QbvZ-6n2?GVPMlwaYR`1t_JHQ5T3)hm?OFCyy|l9_PoT zWkm0y)_=lSO3g7WTb9#=3PYA?&Jve&>FE^!gm#A(Ealw21p4>;q4c;>DS_7Ay)=xo zBQVj0iagg-!7DcPAtE8oyAlTq?vXh3;w)Q3J(9VHi)e75!OK?bGBVaU2me6(1qidiIg%m;EhH^N}Hc1a-g$aUjkep_n0n-zkc&J92~=uF|ZDp$n)tBX(5KN z#*j75I0z16U4VbO)?>M@i%>Rz?SY%)JH3`JRw$Tq&YW?uT_P{<67&P&-M)pz;TZI# z>|OG!KTdx2|49DoiR3efXIyuyDe{ANKF~WR3x^5zN(LW;VS8B@tEccY*vlL--rE&Z0=-Bqg2R)k_ky{wCOT_4r7&Gm)K#zaG!&s2HnT4-CqasZCkC@;(Ok1&vfx&zGv_8F6K&1ZnHa^aajpXWBgy(P5y zDq;E+=1+L?27m&FH(fj|6aZmoP{EjvEc<=GK!%=?2jPV7O{yURH5}GPo_7JRz8l_h zm3+U$RPukP#rF__W0-|u6vU73O8{rC9->+{@i>>k>dV-%{2>NfECc_)Bo;%ze*X7& z6FKP^D@H>M^6S_Cdiu9toxOg4&&LbRSchfDlzV>&w?C|lmSm(l8R^8QRvWs*9aLvnx9 zx{Ko*0X9K13#-_sjGtV-5+W$eM))RNwq0M^+EiqI-M&VSLA~5Qw^jgDSLJ6ArUI9R zAcud_9Pk%G4!1T>zAl?(UG2LNAWpBg5^R}1iFINGgN%cOV%J$N(_H!x?y9J^``QO< z1O6~rgB9I{x%se3X_>r>ex6OY++NW zJ<)8Mrtji3DWKu4&ji4w3)ps472u3PPvn0O^u1yuVtOnvq;m7}br@RO)UbTGI~k!7 zcouOojn40gZn<*`<4Qo%BcYyAFx`R6Q((ctlL-K`y05y)BF?dw;eY~IW0}G2)8KQ@ zoZrK;G!Ndih-n_rBZGpURmSC+Smu#q7NJ7wRKf?rTknpgA;^APx-cq3;{+Jtj*5R- zF`PWEv*_f#IdTU}B9nHYWOACh82AL{GMJ1IZS3RBA#4ubLYIzF5x8^=WXY?mWM9u+ zCbmOR$2=!8i_7*d<$hY!(d! zZ%q)qDr=+}5Q4>GnHzkke1M$IG}(VKKq_=<0CC0f=zwsO@?4DYXiyjcbcGweo$`OU ztImL@dF~fcKJTm#*I2s%J)UhTNu{Ji?y#a(!D7l@P4L&VH})6W?aM;|tMKu*&5$2W-lT=5*)psv{j8l2Pt9~;9JMPjG*wOPhMIYH2L+8xi;q6Kdc4!lNHM_WAnoUI zM&Q|G*45ZlMf21c@by#-s9<)ku4qFHlT@4ix@r$0zIbe?^~8wqhqw_IT1M0KgDI7% z&IeOE;Hk4P)y^UGRpozn=4^61spcnxLZRcb%$XgHdtEev9P45=VS>fU;bcrOU|zFf z>USB}bq&1+Zv_kg&F-+?kx`}xmjEzQ%3uDWd5=j{J_n1yKfNII=>;+A;YLg1ZSlEsE1g)A&&Jw>HgV$unCH1d);EDO?Y1GkcuOFftM`PVR(8%!aJ|<4~+>XOEu{qCJcd@gPFp94?56xb_Mw z+^)>CXs-Y^xfNXv>xQD7=!7bDGN15m#F1DuV#8Eb9d~~uCLcHwSK!fa%sJo}&^93# zgV~E`5{tHO;nCg(>TpaXQlA#d4-=p^Gs!pox^U^Y6I^LLji4U0$27e~ELfH@4V{7s z&hv>qV)LrsSUU;^$QGlGy0Lk4lY|>}y>oA97dN9_TtYeBe73=sZ8!`>6T>ar*f3r* zPVZBXDz1MQCWS=QJl@+~(?&Nq z8Oy}U9nL%#$-bzIP1$1MW2Am>4H2-bP~|vLv=8f#p3p&Yc)lsRI${FMzTypTF6**f z`-ZSC8v}kxg|AsswDdxK@CWrb-Pct0nkCI`6Fq-O+m;&~%_dcMR?v04?2ZrRxaUB> zE(;DA5ESCD1viOsm($TCu)GK?;adgZB%;) zZQH}{hJ6>3h)L&9Jg*!J5E5t2W+?$|+LOg+jCGKFCv8J&|#aC__YSGPr?hg8CcX(>MYw2P5KNj$Qb*Q_%$hdy8ay40aP5LY{9g%luGtmI3&*mgdUU!tZj1LHOsMSOyv?2e;E=K8-(|g{D`G5TVF7Dj zs)_BFRC<^(FPd(n0v@>PZPy3=y88T6JNAEI`Cx82E`KghT2n644BXdrMPk<%vo3Ar z`2Alrd!0mF-JyhNXjl7V8_?(veY-j%DsOS?YzdkaDNgLikf~s&eSMT(8=*H7sqQdF z6aOC{oEg8T@X`DnFZTkc*B|UQA%f)6Z4wQ3F8ib10|5g3#=@}Ci~ipfNP%0`ZtH)K z4(vq8fUpd6+WD~3E4IfcDa8{ERYf?LfYgQ&iLYn)s3xSKSk|4k8jNR@JAD%lcovfY zEtN1mGR{Ft;ZCFWiNA+RA+}kdU13;A)On;oJjOoQQvMRu)vzwHKQ0K*K?Gh`_!nYA zPN44nSi^Eej^~6ZzfSc*KEDop*HwR0M)`$kD6>O$C*m2Eay{J8$zt=Kr^*ai_6$u- zyTEeoe^0QJ>mXMRT&%FvLeU}I7&l6!xzwH;4>e<(a(H0f404RIVmFngirFy7=vm38 zSg|Tvl#8Jox?^-exA) z1PMLv{{f=LP=}L&(G$0g2LT@w0yH?6(cA$P1Tr=?IG1r*0w|a68Ud3AD#}`l!Ya3i z8v*h^13)Lgw|rXxLlS>niyB)@zH`+~;FC2@|1$xC-XVdI9q&F{l$eT8Sa*7j$zDe- z*L+^fS+L}rdJDN>xKW+DO{(HSl?f%2?+VP;P z=Ou;edf&_!Zg=Xb-T*6Nq9xcJ9KrTzbMrArQ*?WM2K_g8K+Au#P!e*$4Y?YGi(Gw4 zKthIGl;F>c4!u3v8$T5`$myX!Q8yC;)LQ+r*m5((o9OBBXZdKrav9>W?y~7$&4#F|B`Od>L1eA{NpM?v8XVF8{=N z9$47&sl!KkGMXopQRlA8(L=zS{a`T6Rg}CHYqtx4yM8;IhK@Peo#Jn|h1fcKc)di= z*!(*8pcDnw^QXw@TybACK8H1Ub^d1jkdzUKe8g*Vk4}p&JYBSJTi6r+Uu~2`dW})1 z)soTJ5oUjaBEkMhF6diGuq{i)=mtbe6WDnlsI0A9xC2DMC?RWoW+A0WV|K<+vCSwK z7i$G^0R%!qCm{x0E?B9syn46+pw%f*_Hxub@J|@?pP5`Eav8D9mkPyuCBHLAa2hLZ*k2^G=^9VQqRU_C(9Lhhd zLe}A7htV9`5kd73@UNQ&hD%u+e<;L%yE8~*~_?e<`~>2 z5|}WPfc>kq;~udynj?kU_H!^V$J>5hFiL5+amY>Wq|)IJH@ulTroqOiZ>wO-5tCeT zLM-_yr86BFD<-(;9#JR?<%06S(!hT;GP8KW|}yj*BmjFaavFLdyuB@4Nn zRq_hSqbAiHhnqg*_+&hywg?N+6VUzbv3N)wnlf`9bKj?f#;Xyq+^ADo|{~z<@Pyc0$2#H zqtxI?>;&I+$W$2@Xyf~KY4uucL0fG~&snqANCCUV)R;OF)cq7!HZ(0gM)J0#Q>mB( z7vAisGP<}GMnRXsW#J@Vvmt+3rrnv}(F&{%!-d;CMAVyTDiU>-?m-S=*1%KtkNW+E zF^D2W9l2^Z3DZ2k8K@#iCJXxkDF1~`@7A|~w9tz;42*v`h^6#V?Gzm_-_eK_Fa<+f zMWH(+`r%YFQ3a*i+H{QkQvh0Mgtn3yn=(c911*ZAPcxO6|1EVSbA^A#hL}UWx9T*~ zmobIgpeHA`4^YD(DdI`p=z8Y+_I{J+8)EX`cl@!B?9fah*u_j+i5~P9y`sb}IVf>; zftnMUy7RE9!#qs2Xs}Ove%FOnBRof1SDx1%Jn9T->;YXsRo~np zo6QWVe273emq?BanPh+XjndFoiF5tS+L|6!jPReP7E%|}^|tI{LiSxMuX#|@=G{^* z#AJq#NnxH8dPu**2h}9`JDK=y-A+!Zh|9?C9>MWoW&*lw0C==#sO=Ef|J1InSCo%; zO2(qmRGeO$0g&4Tcs?NSkOS5q`ZFp0l@R?mh>7|mLW zIHtLRBbzAmS44_Y@0VDM5VL$II_l*sm({q4l3ZH%{Pi$4z38(c={avRQ{jULO~lNs z#6pOl3;zgauql7RVA6|s#&%;ni76BvWZDg{SRZQA4lT5K;PsvtJklpcBewTSTjc#S zb0A2yTkRh?S20EdZ{bVemA)I(5}xDHC@U`gJ_{?SB{-Sb72O|+IAQ=4Ci=w`Hn>o( z-PAsWBcZ;sR)tiEBByFLBZh~7gvRTLg}gTWJ)~M6!Ym3M-U%qj5vSt#CRJr}S-F0$hXjT7><(WH(W~5o%xqWprwe_#fOTVw zU6|Tl(9(Z><+H(DBu|P#Y47i)`>^*$ z&z~Y752~`FxuyylyX#U!<{QDA%8YIXIRDma?{t6vQu>bdO;V$!fxCMpKO3k)_PVdh zo)N#lr`k}HO@RnV(fjGkJ4PU`On<0cKPmQQLAE?oFAEY`d8r_i*-to|sxXebCqP&t z-)BuQ41Bm#eY|4jjsknsP>I`uUhtLa>5z4Po!)rl%krgC{=j0$FhLj4-xhET_zbe~ z-WGon$UP#4Fil>l$nB?wp`~VX>XFvau&_U~b$fbt4=6~&f12#32YBtKsCtjc26D7Y z)in@(>sdWdpO{8HKpI4;89L!hHs;srb!P2g8XHU3SamHi=;tqwBxiJ=z-Bn_R!)hY zz6suEQgDZi_;t8@Q=))R4?nVqxnM$sMALt{8JTO9y-_X*U%UQ30~I?u3Qj_#Lp8;k zx7TftGBADmnvSZ_F)WNkXn8o&*Jw){ctpW9YmTCa_WBT+?d0)oLoegFhT=^FaY;iY zQtoD?IvGKapE&?6F8x+jh!q}cEi_OBz-$(Gy5w6iu&J+?iUMF{)h5OSgwV6`w?uzm zJAwb{uJydc4wP;I~h#^8<^Kq{D&Wj0*v zjz6)T@>6^fa1+mD2`OUaX}{s&&e4Ajs>;TiAl|azcV(HtboK*^#o=or#xS=0mZ#EY zyoZPr1Az7|B4!`|Joz%QU1l*Gv#FEgu%X@*5B>aP=;4K(Q*A5wuWx5! zSl9U?53d8_d}K#57*4L_!MEgbP+K68m- z=}>y`VmMgq7AfiAxbMW0Q2ohWnq18sa##SWPt6Yxt}i? zK2EUrM+_W3HNRm_2b3)ck@s|!)Oa-<ZTg9j3I@Dk5(*-iDpY1%SZh_lZ>)X-j28o}5#JD=---aSxIZ zY-?Y3xA&A5x8}uSCbV+g$7^E4E2_81SiqQNFliIgE}JBRjR{I!$DFjo(v9!#^$%l% zzb77jPJV%mqQ(N9{DcJdv7Xiv8y-2?hqOdn?FHnbTxwuXmIHVVng?K6%07*RT;VM7 zlVN?N(3s!EX{TkmhFj|r-HU1rH*Az_s83^DhUk9@qK0V@oOeLNp{6y|`is>UX;fN< zcNCjPh+ISY-7hpgd%{>U2E{EMymWy3kg7}bX=>dkjxcA@Hbsw{amNXmidFZU0PC6X z3dKo4TjnF;+5tnv&`l0(Ma`&A>FfUbAAY34qR$n2g-01e;L7hxa&ItQEv?=woHmNH zd5u!i;38L#B}@R0lz+(p_GR9vg@Q53YC7!9k&NWaL6#_ zYN}vzdxO@UJRy!^r|Ou{FVsE6DMi9%H&Xps&tW5;UI*-tNaolZ&L{q)t-Ki%+F?o~ z21Y@2uavz`1}(d1_;2i?3Attdm^3vFNCv@*QL;&V^uJsBJhAH&O|{aS7Gdmp?|}5~ z>$~{-*ZrZ;vxrzp40ZT=Y+d^ND|*&c^eXaxjE2YK7fV^-TnB_zT|RfjlKN_vPk)Sp z^w@sPu?oOkVkH1B<@k2eP4x{`AvetW;yDp}HTQFWA*Uk|`Z_$Wmik(N{jE!x4ht?} z4LabgV@odd$`54GdW>qF1p@O@a$#Qc!F)K!DeWHl$SYoG9)Y_t29YA{`qSZ;;J2!P z0&J42P_-J!kU_CWO64?A9dTe5~4Kk#eMkL_kB zWeC70999P}@8w2`oYpyMUAggNQF8N!VW!P_ie|5om~Z+*_phBuOCYA=ZcJg*cn-k} zcP@z>e8}N9sigd`Ys&m);qLPC=|3wLRpl!_z2!`8B4^=Zsd~2Hvm_Xk0R^EJB#Rv- zo!0K~;U&w1aUKF1^Qn|^X^znmHl=cyr8qz?Q^N(7TE(chI|K`3mYKaQ1u$l`+F}4>U6e*6&lC0CBf+Jr#(nSUeq5~ zJ+QIkpxW~}VPesMV_wznx!TvR92igF_doi`B-HM#OmhF^f>o7<}dbwFM2hPUB^JDQOHH1Xl>>dcPFQSv3SLfTBo z?l1rPAC*w`Dk%%cQi%_NXQ-5O679~9u4DclecQf^-*dlSTv2?Bcs~4HBRi&tiY=22 zA{ZU=_qAGWA*4>wQos`qcmC$RoRY$+6rbEp3s$ALF2z6+8QUPqA07f`XMk3+?hd51 z1|=lUN1w@qbatFBSIaK(qDl&8bvW$t=c=xbBJ|+ixmAK%BZt``E3+aB+p6y@M^;Am zDry;WH@?u;Y`*mgpS0_xFI-?d{Vdmm>>CtI*R`(;{%Eo_MzEm`{JJoQ51sx1BS^n^JM*W7&P>;Cd5_}YLtw?Z<%n3CV z))+SJnSA=R#*OCwcW zgL)>!y&gIbD$e@3%b^7?W+J|XJH$&~;&llm63U7FwdLpU^9!71SY%uhIBcC0)K^JT zn-7loI?f61zbFp!9X^U5kBPa-ZG)bv7b*2pUq7|J%GhELWF{#S!H;6~2e4}kD>_E@ zcLl5P*GdNJ3d{khebpW4cUvU4KT`V_Zdg_Tc!hD(2umYa2}fHyf2>#3J#x9we#CNRyC&?NL8VK478VCk!4a)pK%&AMA4JSO# z1m9!z#eFUc9o05)T*Q&_`O%b-yU_&mmDE|0NGnAhWgEn`Cbs*B%ww_&Trs!Ar1I#k zP=xR`3nE%jG89jtf_03Q=xq8PR)*2i_hORV`upWE^D|*QUd{{#DeV?r6gJLr^Sis# z4jZ3T9%5E_b~vo8Qlioc)2fXLmiVaTNOq$z6#stu9-mhtn7D8~-lnbr_sPu89&;G&*$lOH6&uM{?dE|w4EC7*I7#_?5ZU>@x zzLQay@i6Z3R1$U$cmfY2qm%bASW&|VECop=Dy5$so}}+Ug|2=lF476ZasOmw4_t-) zBBD{Xv}61<1iaKB(Ck<}v#l30eqNtZBFJxZo+b!>-NYX)B~CQ*e0SY$y>^3x%)c;wtU)ucm&Le$-`h9*$q{USnC zxFFRW6f?7Yk}h^jQTqX%c>MXmD6Be?fECb6T4lCk6HD1RM8KUg*)-1S3rHfWQ2$9{ zSYW?6NMSXbbg!n`lGo|j*i{_|xO^K?B&GF%iS=kfy=uzQkJP{={zP?%2(ZI*<_>cz?Lt z@wSvaYrax!0r1WxR%)5)koCT9$m2ihc`m@!a{;bY5~^?Al)Ww75vE2y+@3YH=DO

3Iwmu3Uo+I)h)?zl$gLj`;~ZauJLldG+quO#L~ z1d;`!XKjiZZvn=9`JuDxJKkXpvE`v5sK}>{vYfSv0LNShB61@!4)FNP#ZcZDV+zLD z4*mSF;VVzh_kZ_{**32>BDtdQS$a8HU7L}|ok_}!aJOnr%#U;1atttfDF|-3IU22g zXxQDz@{exUq)(JsPPB^q!`sQ)Jea^W3B_0U^yeU4Aq|_k>^QOBe}!1Znek#0Xqps+ zQi2^00M7A(0B_uDF0)LNl{em)wYEixxp8_YPClLNs%iZ!dc@T11@j#G<@3Xrat#i@ zkBQKS6~^VrhX&RvE-ZptxfQhak=ZvE<~@FlNOK+E6Gw4r@@Fh@p(wWx5{{uvSo%Jl zMMmMVF!CyWzNp!MVm80iB!O9N<>Uhbi(>*I08pHde^EauZ6qUK#&PFavbdsUU;j8O zg@SKzOpO@kp@PC|bIMsTMdE_7;0gNvHWJN7l)&QS(a4AR;8@?mC#a)0t8cWlaCp8yq+W@Hk1qF9+GI${>>`UK z18}gmry~i&Be4($j`mmg-!Pti?RHxPhA&FCz11bZ=5 z!eY$cJ+m%twqZLRR4>JAcM*)<`cOh=N|XAc56Z1F*8UvZ#g`ImF~62zb6y9EulwkA zyN;I@d-uy8ep31yvIz!N<`wqK8g$O}gKy>o6jyB`BFqg-DMkpUUFsh4J)Qq_w_7!f zRf~1@3u&hsel+|ko)B_KP@1G*vTs<}KkxFt)}0IKJ@0HW(irfRpHgsgc-rBtL)+wz z-oA{V&vLD%-5#zD7f6`q|8xUhTm;a3DR*OIi$g`%EW8-K$AG8wpJtR+y~^#C#(?;t zX`5_L(&@}lOzKQ^;o_ZYI)cFEZL+pzSB9+do1I9;Rp8AZfy*1C=eK*x=eLE8hu4d} zzahIHZ%-ecL!nEjb1!FiRf0O*Zx={Na*G**%FD67XwJFhbn*DapgM#jcfGtU@q+~M z8O@4lCP0qcUjZ*b8|_}-UY@T@Fn}@cHiP_hNMvK=uFs2^hwrmpvtIU-{mQl6)s(#E z9rF5gc+okUu;iCSsLjZ{`N^YEP(RF|VRzoYJe|+JGDi90k)d6!$LTxQuPcu&Mtyxh z4tU{B6COKf3Nw$l?C?qp6eJUG-W=I-PWL=>F=JwdUi2B@$5ZQm_+0GV79hfWd^PnJ z(L)jJh&M|9hautpr45I87PB|6UOj(|yMQUv&pJy17Od5C^hvz8;a~cW$Y#^P$<3K8 z=&TgA<6RE;zX;|M_MacS;LP;tsNT(FSh65<2P7i|`f3*jxPf{$RB0Mdg^V`}U3 zW+$qjIYess0IA>k=s0Mt3|ONt0%hILefUM}vW!~FHU%<~PsK}8I{5__rMB%uE=qFf z%W~^#NU&DrThJ}W%A-9&bL4w}dKBdFXObt9=X(T==|$y{?In8sML$+`H|BF&*#l`^y$Kx^@vsbk(ZubL0q!WfA<%uG+oR$Wj`B)&Tt_*OTcz=G7fEAq>!&nHd!&b^ zH&=h-joJ`ZE8qxgZ#E5%${QRIxSz0^OZk2CuDRMU#CG^q*;Fh-VwnC6i#1+@;|!^$ z;i2Dl8%i%kHF#6bH!{M8cvn3btxF4yTLulY?1zc3gTFE;M2zu0*IW*6XFdS<|Xe-$W*o{!RHH< z4JZ;U+}6%Up;auz0lGdPVNh$l;N^)*)TV0lSwvdhqbapFBY@)Fq)Dsix|_W3t#T!wshtPGmA*baNV_njboy zW!Wfjxg%4U!LacTtlIRJmnoE4q~TC&r*3L)n(iM#iZ6N101`>Rz3>`H$ClLy=4$cv zDw%}{)nl_TC^TVg%i=O2r#y0qCP+8E0%R-*==3#c)P{H$nF|$>*R}bxhLYLRI9c@a zro9sI;D{pp)Z(NSd=7N0XgNJ2`PYQ37R6@xNNo56 zN?togqmHOozz%g8hI7&{!~r4xBCcc>>O~gSR)&N}r8jc6QLk2X1w6aqsPjYYO3Ahw z1Ld4*fpc>u|Bo!nnfzoav0>Au&A3v3fv?s@F|Lbp{5O9 zk2D=b2ej)qV-AtgkuQ9#FN>aKURE_1HA^{egy311e~+=-XlkSHsA|xr7?il^@0dr|)zP9)kbX$NE0Y6`hZKrnFppPF&1+>f%^F z!rFU`YI3PAu*Ng6=3~p|wT>cX?GXK#M5mE6O_h5EV+O>B7zWvEo-`)goNQ^92<$%5 z((&vnywS*{_9gIWm0kL{_k{%Z^AN1+1WwI+!8pV}CmqN>}@f9U( z(U($lAHPo%-VZ=NB3!w4Qh8eP|B5(;7x6x%Nx_s3gED5>@-vbhbPTS=Af53Y(C{w* zplPuV6$Yds%(|jnHr2>0TL02OziT;&&$d`USFV!9dl@8)#<?B2xoy zWEvigZ$qlROL;Fy?7NtV8>7spdu^IKx+mNLX#lAI_Dk$Z38@GY%E*$=1gPpck@l0| z@2~8hsd#e&UTP0&R!%g|inOpT3?|flA8E==He!=%1kQ+OHU#mW+GIFM=lX+j=_AGy zVR6QaU8;i?V!4#3#Eb=n&Dn(mIAAt=-trz%hatQR*)wzgb(X=EjvyF#^oQFC*BL9bB$8j#Y+{X|NK<)b~_@Y1^w) zYQxn+I#c(Fc2yHW#ryjge@fVSSmV|4YCA(mR%})2+*lvo2v&}Z*omsno9UZrqu1R& z_S=$Sba4e*`ETP0Q1PyWdi`H%C$Cc~xM&U9Ivsf)~RV^+c^G_;Ho<1jOXT_PIeA`Gw* z^wD(kDvO+Z$fiXSGy7Qd=3YO>2a34(d{(9{llIwA1_mL-ClKhZeJ5IfQh+C({L)-W zSPGk1Ds66Y3Vv{0JdiUq0<%`lwYfou{*BqT9i-FUqUu>c~0Tc6ZIhb$v~kXpEMs`3XI7eH00ce1_GwA!A# zv|0+si+pBWrhLkQ#$E&I(!eUZM#HzIUp=eCOUB!>-F7NbS-i6xN@qey=3;4Hm&n$+ z4B!s9Koo^_Tao+FbwsWyXuevKQ^^z_+T35fCBWrJ2#)A4w^|o7WF0-T#?%6vD$xwl zgB_RB%{FN}hkvN=0LTO>o{E|%I;7ZLj0TeP-kHQwTT_%Z?*2+=5Bp~>1OhGXrC^QC zW}%KYEE$^xZPohRj4;8+e|7G#*FzeKwjO;Fgs`tz@yXsLi_9B^!brf(7v5fy?hr86 z<XMI<9&LJprE7~7SEB=54JNlWv*eYw!i+EGFk(u`kNB`IDB9Xm;z z;warN7?WZW=ixb;nv?%dD(8xgRXErRZ7j3TOfa$seVY3?IjpANBAq6rNw^J}kR&Ys z9$6__B&f7U901N_Q6^zGUQU};k%Jgtt^~TDZIgS=$szqtu*}JvHnL3?0BBo#*ORlDGKa^DB_2 zxHtUkvgB zxl#WPdL_DpRuqNlMy#R*e|PGv{)c|eUb6vL;wBEY-5~qC*u#+e1xV{mQ3tyMG_;Pg zgOGY)xfaDGoi0wWq`N%7)kGXvt^8RuITTN)(VaEjS71WxL^20Bmmqg8&9-_BjQY7I z2{6cvk!I6rNO}6!4tm>D%~dR-X+P)u!g&|JN@Zp4lslXmopI0AG-TBij<~(^Mk#wO zD7hM(08pEH4K{@}XPLR$^#)ua$_^4$w)@wXbjrrlFf0m`{;uJ>%nV^xk3;;fmuZ2h zz%s9RVUrmWb=_*Z8_YRLMG2=yzI8Wl6p^`!RJH@DsQkDY&e}Ui1?r+Mhjx52GD1=-PCDRc!i^tTd0_Gl)e(v zlKTNfu`i+6ww(?+ir|bcEID8FC#7I8fzTVh>54P-AO_wGi&tTBpmWdjX~I*mc_$2Vkbx zUnp3Za~fJw)ImkU=hZW`HrHBnjDAS=^=gc!von+iUBNz%vRm64XsKPp!pUi^V~Q%5 z?m47k)U&hiY{9ht&mUB2&eu8QeH?l%>UG!s)Y9a8EmMBwMK#~6()LB1))iN&`Hk%t z6NBs+6yG9*i>;WP^rnv=+b`e*M6uydn~#jrqV6A&7#pAvoA7M-ha{`hBkS(GCsCSN z9$*X1%z5H+#7x%S*vL|IbxWpC_LMKio?{jZUP2!G4xl}M3U>?T{3}XIiI9-^eT-MO zFP;XnX%gUFPTAyA_27kQiLqqeMU`Oje%@#XcR*4*V&WutM-6N!g_kP@WHHDXWM=Ea zsB;}>w)W^Pk60dQ#OB>pLa;^PRA3O$EFw(cnhGK0(vZ{C#8d8(veGlP1{$A z<6b_mLu8*8MMU+Zj)dQ!H5i}Y!e-Xlp4#WQ;YV!bE=x~keDsN&9W=+%6Qy)6s5|y z(p`I1X>`~YQ!}_GTk$(v+H7pmwmmS()Xd;9u`fQ`B%ZJ5th!BW^RiQZorR>{qB8Lk zzM-Ad>d@l9ZjL+M%48GOUQ0sw5S(i5+=~dW8gN&k8;?O_UL&GA;>LRsXttQWa5c!U zfVqv;z3dE)o@WfmeU7iGiW*RpZL+dxL9#o8#Az`zvMk*9&7W6H^Mu3o8&moQ!p0G_8=~W&`?KrF*p8kDs=q@#B&+=lGx1X!G z&T*63PeW|<(6FnSmN<9)rW@V&ks-iyxE5|F6sqT;HT3}Q#Lgp)2{u`X^PqozYX;Qu zG~_f)Qgl1!rD|^ZLPu~2VexI72k=x9?T8aZ?J69NU3ZDkayI8%1w4Zkf2>u!7|9Ck zFd)i%c-rGZ<1(T736H7SZ&=rjp@Ju#S<|!~huxPcbRLqAs`OGt;Rrh(Tf6hdaZdwycKK+&+ zH|^l)xdx$$%?X6gCfRVW9^B0o*B7m$bu}4rX}IR= zb0h!yOsv*PH4AqpowX81{KRJZEPh?K<4#kDi6acVSIgXO2d~!&zQ?0GyO%K3pSk~V zAQ_F>OxHz#d2Fn;rs-nC`5EQ?9t`Czr50@Q+R|fZnVVki=7EQ7Cwsb&Ei`DOMY8`s zJv8+uv}(yV)W|Qe1zSuf%36lzN}B|jUJBXQPH^3Q+#+W-v}7(>*D$v@K#QS2ElpB$ zp}|=Iwvf8^N|!>?9Hk|y^_uDfN2i18crTMFh#UC!^SlV38YwNQQ}Tm5=9~odjEFO_ zGs~6EAJ{S1nAb&zFGzwPm;LvGg8JQB)#~!U;O(m3*(H zuLM$qPo;KLEkD(chlu?Wq=Yj9cX#(LG7oP7a~te;BGBNkq8rD{ZGpREUXD&a&pZE4 zOxB{Ifsqe)TYw!(F2s=BGZBq~lIS{xzck^&#@_c@?b>s5v_q=iA zF0C1abxA-+Qap3+b*ElqbYyr9EXP(3S1gHQ`zZskKBEz!@B`! z%Tod~cc*TzCw477c)LE1*ESAM9r)f?ZZ;fPx^U$}m+E+Q5zT?=v-;fxpUm(3PR)8f zey%K8NSMH(JNN!xE=`>-{AJ+l<%<({)!*6>=?}52NZ`_o)3dLKpbyq3_q(-c&#O%O z^RtH8m68R(;kpI#_3^m({bp(f;0JjA$0(TY_*9_5_=DkHOz)jcu3`d$Ve&b5zQz>qay;ahEX2nD!_xPb^;GYt8*_W-`^UKuc zDiFV$D9I5CiPvcP`}3E*huhsf(Q#+Q(%9XbM#3Dw`-|&=-BS5S$5qYUx*mW;zc!jS z)X1JT;>ZOK0WMn$-{1>f|Vlhd^Lph_k&Us#|_}LSUj%qY4kv8Xo3C2@((us3C@`pt%kZa#?)JX@~EsA z+hv8j=lQfYw)ZpMIg0m~3Pb1EqWeYEd2%;|+iymbK#6X9^k3e{bXn~(@cKTyBR2hn z@%!uk)BX;8yZKAP_aw#y=R~Kr!~MvWvmb$#Ji3y6OD%BUWQQ#$et`L)METyepWoe+ z{->1q0WKRex8bmSG36b-*Iq`-Y9VP~AwJx$xU7Sijeu0OfY{RSQ)t%h?xEEiiVPa}_suvpBZN7>Z^y9= z5(vvwcb~DAXA;x%B7yn%KIknP>~5l`)_&;?j4293l7y(_uCTYL<*uVJAI>l4(9a~e z`CU;D6$LPIrV11$vN-|(Jey0X(r_RMg8>UQw+>7iddM+aD9o-P96niq!60f7G073^ zm}MyIZn#1SRN)U0o=~f@T>|5LN>i+z8<1>;Sn94ge~G-#Jurry$OILyAZdAOF@BVR zU<$)fhofMB69sfHVg%u~JiSqfq`pz&6^()}1z{1Ad?`>BqCR*4dmovwCy&A}6rx$Q zv_gYmvjn%Wyc`Tri3C1+?pde>i(*5!X@6Y<^k}j^;)cQ92&FPKCapfY3Zr1}q`zReP|Qe)x#D+6*+3G*0|=5( zh*0>zYoK_k9!moNm5A{k#dRqLwlL~ChMHylsTQ0|c;+NrNLW%#avDU)eN`qH#$VPo zwzrE9;WN@Z^VWwFOsrS<@4J4W|2|6iy03U){&(av)*WNm5G*_0ERSVtnh;=PviLDH zHWo_CCbf(!b}WvJ<CkPGXJ0Kndh|!> z3ihp7vwXWHitHOdcEosaz!UuBiG=!;zN4R(%@wue!x>~u@0N1J0B|Qr`6?P{gjrL`E&M(F*qu2#1QJ(D+M@-Z$zLuTouHU9j`<@-YUg;eG!rAOsTZ z(Q0!8w!d13?heVlVgbyi%jS~HOidswVctNiq3ckW}W zo4GHk=26hc(=5N~g&Ms~)0bX{FbM#8RUoJ5(p7;Uba zri_G>gHqRxC+43=LbcAXjohC%0!E&e4VEk%`kVsE5&8`%1LHgh+}v|GnVx?n4KpBB zG6D@%*Z|#Wiwv;qQi-*(axrf9^sKvHW;amDX1fXL0|5>xBC|OO1z3PTu8w=TJPAIR zVRxkgbTeb#F>h^rJF&l4_jjGyd>{7XUpzEvo_KCsD6_dw5zK%63*rXT0=UcU6O@48 zaLdqDSiU2Oiy>qp{`^(_`u3%Qq|~%T;o$7<^Hy2g%tYpjuJQncKJ3XxV{?5jPaHMa8 ziwnKs)xD+W=IZLwY2US-1(1&a=}aBL$r?=O^>K-AyLIQ9{kb}QYJYy2EYRZameLbb zYNm=S*eU1NI5e$wxj7<%1f6>^y_I#lYdAN~egrA#NxA`To{LNPAWLJ82rmY&ZM(G9 z*|{Z8SQQ+>krf%fmsYq-{{t};q6ToK){T^JV>=i$SVa1tjqT z(FB11s431yEianf(V8#=%3)K=k_{MiA;E=0p}-$$Jw1a`oQJ*gC(3x*7LwS6b{Va? ze`PFA=~&4=zxhf4M^4AgmuK49EMQET=Ni|?A%`)|EqSz1j-JhAwkdMF=>g zL(aRCu<^b7sol4Jy1_>elft15ieaOOX7E5E95Wsl%E1b~?n-4X^r5_dh4 zCQR3FJXrzM>E?`?&E`J4N|K2077>UUe*UN?w2U4B*n}PcVGT?Vl7G$v;obe$b(hq8 zgq{&EBRMG29o=zSE!_%lNDfqbQ%yJKg~wdkkHeN-=-0WGa3I$9k4fpZ8j?JpdTi14 zP>)gPZxdTrwOh9fT)4Th68qV9o4E)DI&>*}po;d8!-6vNmLU_%#&uOQ1@G|rOgE(m z+}_#3+D03IX?&Ay4kmKKkyg^Mb#smSF2gUq;HQJ#oprufzBbMofpIJ#i#Q4fQv|tq ze?^$2x2XEcF=34z=-i7VegRV%5ecNIOSt5+gXjk7I)7)oz?;d>aGYOFU|}79!bFf$ zRex3S=ViVv01IQG;;KLgC;p((g23U2SfKy3M>@F$+`hgKf+?~8ImjS_%OZlS{3Gi9 z%N%K>HMSH=K?9sh2VVp-#Hv861-c2)23YoG`{RNhXNS7T&Rnz(-r>X5#m#)pw^+@h zi*N=;aOfeUej9Xoi>6OU4J;306F{;K%gcyc{`UNZrQTOW=Zi+3s(cTl59)YEl)#SF zyJP^Q66ggt21{Z7&7vCUZ+#he2oV2W#o$pNEROkCgKFQf@+5v*V8VKJO*n7D9xdrM zj3tMJf7%MO0?q>7pMgJ&abL0W#z6wXnVz98wbFM~lMo{Qflx zfw0=HqcV{uxH+yZgFre1(2%zKP_nV=c9{b(uj9$4Bd!u|UY^0S#s})mqN!F0q7CEr z5O0TVNhZvB`R#dbrwR*nd&jj4aDsQO709N`FEV)uk=dkL%*@*q zzOS4?oS<5>itY5&%Mqy-Y9G>5@ZC`tr(Vu;;me8Kz)8}!`MDHoZ<@5Uq&&(TgbPUZ zK~2)Wz+Hlt`SyuQc4$pycZ=8^+|s)xo`J`sNQrYCE7=yLIz`HsczyLmH8gmctR|IE zwZM+kKjxhAKeLRpd&gFaS{1f!8eNxfOE4Bz5ny>NfX045WR*!OdPv}keb~`GGB@%5 zqY;YyMLj8u$~`{Q9$vF{lWTtokSQGT$*a@^MzH~5Hg)BU{;+R30uapERSrV*k-rt- z$qjwAyo=)Wx!iChP<$!<_y8G=X~}i-dw)Rl^V4I7w$2D#7bzRLh&29*~f#;n57!AmH+wt)a7-Tl+ciII z$9r5mn=xH5>*=RF`VYbS?aowR=>)gKx4GsVck*rensw1_idc70kmbIzbo|##tf&KC zixJ*llyay9Bi9etd!~G+P~;`U0m#h!GW_93GIoAUosgn61S!mC2Qeei>5$2Hw?7E! z_=*{}{XI&wX3lH1ks!@P0R4J&VB0|@&fQXgPkCl&Ji|*xh)+9_{<1{FrKU)aP8{P{ z1mpo2Q84&Dn2uZzO&p_iBmF351zRR-dRe^!-7|)bn0%_eBCu8CX3F)Fsl^-tq$e*8 zliKZ=F-sD|;s@*fTNyVqIr%<3=S=}_87_3yFiB5dQaL*Rmd6o0;C2$@ou$PHEA=lA zn@vCC(iBi3)kaN(LpM%V>_2al~0@|!m0`y}VJ)Utrq3ssrK!d?D{~SU7fWKqtCW{1k$LK+`1Sw&( z72iIQEr3Pd9}Egghx}H{=w5wx#x5=Q&3@3|r$z|#3#~QhO(a{OIzuR+#8Jo^+jg8d zhidf0`GWM5FS&WZ_Gc>f!ur69{q({v1+Ecg?Dfr;XHx)xmW&yL0RE>C<5A{%z4iyq z48EamMp044I{0{vqC|UPKF>vWpt?ptRkum zkbQJzQGInPD;bKvR8ExY4K%Td<#clnPw{&qwP=bpB&C9Xz)yU{BftGzrAE^jofw6e z+#H@tBS!?NlrV1FATMW|uMaNZV>LvTo#QB0Ewz3^O zLAui%cSSiacyP7AS8aBs8w@jOc0GY6wyum5)zm~}_o&2Q(F~suoYc(i+a0cg#NmO& z`YrW+5nH$IpEKA)BXZWM>W};;Ug?5bj>j7pXQ>*ZCmc@)+Mr;OE8Enfkl|WXwrXk@T};l zh-e>}Lf(;nD_JL}9$>?!KyfiMv$HX!{=tF8{NHtJHV&*jgpv>^m#`SOh&U%VhZrj} zCp#+_3x|*x7drQ>FDjUsMzW2Jn@w>jz{OVnP-L}8}IZ(bUT-Sz12R;E$oD>*r&H=GF zd|(=?2nA<=x=pNt3`pt&?FuYoDFY3?LCmAc%h>tGa z8^1LG+ZOJ^PeSb{>l*UdN=ocU>+<#3pe_xt^nkwYpehZ-26#x^v?xwP$UC6(R8}W7 z_Stl4bq1C^2ib_=seDL^{J!&1 zX-l+2&_Pc8c{PO$Ga%Q1R10J$4#Esov`+NWm#VyI`I)h3c&?egJkfLlwTP0ZzE95D zePxyu<3W=K2yBIHhLHhH{4;GpEA^HJis9$SYhGaDqvhC5My!%GGwLF zSqzRLnXItc$fr0dUhH;%-$323Mm*T@2Y==jzeDJOK*8m~w4IjW;x*k|lKU3@p!pT3 zhCksE?fNGl2N%ScH3nkJ0F6-3HW*r0D;V%+h#7-mTWJk!IDmtRz#>N+O(-gAUhe^a zv0`*H>_^0?LfFPHOanO42EuUPO;|}EvI|GflW&!o-YoQ}y^2eEX*^@ksA%k_0Uy9- zdAl4RpV|8mvfQuHeOd~i{mn39vEIdzUkP08#3C7|X`ugJNkq-#p=HsL*2hivEC~Ia zri@O*_OjVvXc{!xZ0Hgojb7z(7o##4VSlo#IToQV_dB`T<6fd1hxYSvwNj(}HeO{( z${9m)Nvr%VViAT`mqB_-vwfGg+zUW^ok_i9aCkvjE>8Q`k7&t+9pAi2Tx)9NLupD) z(?#DZNmcIYcXZ1*Wh^J_FpVI8l2yG`taP*1gNtw7S$ZU_{kFWvd2EpL7e!a9UzW-Y zCz+1L?=~*xT_yVg=otzh502e4PT1TOlp1>Zw&~E`woHfp!k}7x>1R8tT?zpF<}1tY zv^1di!Y5cC#o?`P`G+hI^|j-1D03X>11?yuX!pp4ds4mLw^RI^u%l}Q1( zFuQM=0#yG`F2vFAIfNF1BVgRb)3AIC63^d2zV^0y<;*PXGV_ diff --git a/doc/dcmotor/dcmotor.tex b/doc/dcmotor/dcmotor.tex index 185e6e41..0af0fa2c 100644 --- a/doc/dcmotor/dcmotor.tex +++ b/doc/dcmotor/dcmotor.tex @@ -81,7 +81,7 @@ \newcommand{\atNLS}{\texttt{nominal:no\_load\_speed}} \newcommand{\atTMAX}{\texttt{saturation:torque}} \newcommand{\atIMAX}{\texttt{saturation:current}} -\newcommand{\atVMAX}{\texttt{saturation:voltage}} +\newcommand{\atVMAX}{\texttt{controller:Vmax}} \newcommand{\atCRATE}{\texttt{saturation:current\_rate}} \newcommand{\atKP}{\texttt{controller:kp}} \newcommand{\atKI}{\texttt{controller:ki}} @@ -104,7 +104,6 @@ \newcommand{\atTAUC}{\texttt{lugre:coulomb}} \newcommand{\atTAUS}{\texttt{lugre:static}} \newcommand{\atWS}{\texttt{lugre:stribeck}} -\newcommand{\atSIGV}{\texttt{lugre:viscous}} \title{MuJoCo DC Motor Model} \author{Google DeepMind} @@ -338,7 +337,7 @@ Motor drivers often impose a hard limit on $di/dt$ to protect windings and elect \subsection{Mechanical Model} \label{sec:mechanical} -Several purely mechanical phenomena affect the motor's behavior and the effective delivered torque. +Several purely mechanical phenomena affect the motor's behavior and delivered torque, warping the electromagnetic performance envelope. \paragraph{Mechanical losses.} These reduce the net torque available at the shaft: $\tau_{\text{net}} = \tau_{\text{elec}} - \tau_{\text{loss}}$. @@ -355,7 +354,7 @@ These reduce the net torque available at the shaft: $\tau_{\text{net}} = \tau_{\ \end{equation} This provides one constraint on two unknowns ($\tau_c$ and $B$). Without additional data, the user must either assume one dominates or obtain friction measurements at multiple speeds. In MuJoCo terms, $\tau_c$ maps to \texttt{frictionloss} and $B$ to \texttt{damping}. -\noindent Combining current saturation with both mechanical losses, the net torque is: +Combining current saturation with both mechanical losses, the net torque is: \begin{equation*} \tau_{\text{net}} = \text{clip}\!\left( \frac{K}{R}(v - K \, \omega),\; \pm K\, i_{\max} \right) - B \, \omega - \tau_c \, \text{sgn}(\omega) @@ -485,7 +484,7 @@ where $A$ is the amplitude, $N_p$ is the number of pole pairs times the number o Symbol & Description & Formula / Note \\ \midrule $\tau_c$ & Coulomb friction & $\tau_c\,\text{sgn}(\omega)$ \\ -$B$ & Viscous drag (linear) & $B\,\omega$ \\ +$B$ & Viscous drag (linear) & $-B\,\omega$ \\ $\omega_0$ & No-load speed & $\omega_0 = v\,K / (K^2 + R\,B)$ \\ $J_r$ & Rotor inertia & units: kg$\cdot$m$^2$ \\ @@ -496,7 +495,7 @@ $N_p$ & Cogging periodicity & poles $\times$ slots/pole \\ $\phi$ & Cogging phase & offset \\ \bottomrule \end{tabular} -\caption{Named constants related to mechanical properties. Note that unlike in Table~\ref{tab:electromech_constants}, the non-approximate expression for $\omega_0$ takes into account the linear drag $B$ (assuming no high-order terms).} +\caption{Named constants related to mechanical properties. Unlike in Table~\ref{tab:electromech_constants}, the non-approximate expression for $\omega_0$ takes into account the linear drag $B$ (assuming no high-order terms).} \label{tab:key_constants} \end{table} @@ -795,7 +794,7 @@ Here we describe MuJoCo's \texttt{dcmotor} actuator. Some scalars are grouped in \begin{table}[H] \centering \footnotesize -\begin{tabular}{@{}lll@{}} +\begin{tabular}{@{}llp{5cm}@{}} \toprule Attribute & Size & Description \\ \midrule @@ -804,18 +803,18 @@ Attribute & Size & Description \\ \texttt{nominal} & 3 & Nominal operating point ($v_n, \tau_0, \omega_0$) \\ \texttt{inductance} & 2 & Electrical dynamics ($L, t_e$) \\ \texttt{thermal} & 6 & Thermal model ($R_T, C, t_T, \alpha, T_0, T_a$) \\ -\texttt{saturation} & 4 & Limits ($\tau_{\max}, i_{\max}, v_{\max}, (di{/}dt)_{\max}$) \\ +\texttt{saturation} & 3 & Limits ($\tau_{\max}, i_{\max}, (di{/}dt)_{\max}$) \\ \midrule \texttt{cogging} & 3 & Cogging torque ($A, N_p, \phi$) \\ -\texttt{lugre} & 6 & LuGre friction ($\sigma_0, \sigma_1, \sigma_2, \tau_c, \tau_s, \omega_s$) \\ +\texttt{lugre} & 5 & LuGre friction ($\sigma_0, \sigma_1, \tau_c, \tau_s, \omega_s$) \\ \texttt{damping} & 3 & Viscous damping coefficients \\ \texttt{armature} & 1 & Armature inertia \\ \midrule \texttt{input} & keyword & Mode (voltage/position/velocity) \\ -\texttt{controller} & 5 & Gains and slew ($k_p, k_i, k_d, s, I_{\max}$) \\ +\texttt{controller} & 6 & Gains, slew, and voltage saturation ($k_p, k_i, k_d, s, I_{\max}, v_{\max}$) \\ \bottomrule \end{tabular} -\caption{MJCF attributes for the \texttt{dcmotor} actuator, split into electrical, mechanical and control groupings.} +\caption{MJCF attributes for the \texttt{dcmotor} actuator, split into electrical, mechanical and controller groupings.} \label{tab:mjcf_attributes} \end{table} @@ -1025,7 +1024,7 @@ Iron losses (\S\ref{sec:thermal_losses}), magnet flux derating (\S\ref{sec:magne A bristle deflection state governed by the LuGre model (\S\ref{sec:lugre}) is added if the bristle stiffness $\sigma_0 > 0$. -The Stribeck function $g(\omega)$, Eq.~\eqref{eq:stribeck}, determines velocity-dependent friction, and the friction force is given by Eq.~\eqref{eq:lugre_force}. The bristle state is integrated using the exact ZOH scheme~\eqref{eq:zoh}. The viscous term $\sigma_2 \omega$ is mapped directly to the standard \texttt{actuator\_damping} attribute to leverage MuJoCo's implicit integration, while maintaining the $\sigma_2$ \texttt{lugre} sub-attribute for convenience. +The Stribeck function $g(\omega)$, Eq.~\eqref{eq:stribeck}, determines velocity-dependent friction, and the friction force is given by Eq.~\eqref{eq:lugre_force}. The bristle state is integrated using the exact ZOH scheme~\eqref{eq:zoh}. The viscous term $\sigma_2 \omega$ is specified by via the standard \texttt{damping} attribute to leverage MuJoCo's implicit integration. \paragraph{Integration.} The bristle stiffness $\sigma_0$ is typically very large ($10^5$--$10^6$ N$\cdot$m/rad), creating a stiff ODE. At constant velocity, the state equation~\eqref{eq:lugre_state} has the form $\dot{z} = a z + b \omega$ where $a = -\sigma_0 |\omega| / g(\omega)$ and $b = 1$. Euler integration is unstable unless $|1 + a \Delta t| < 1$, requiring impractically small timesteps ($\Delta t < 2g(\omega)/(\sigma_0 |\omega|)$, on the order of microseconds). Under a zero-order hold assumption ($\omega$ constant over the timestep), the linear ODE $\dot{z} = az + b\omega$ can be solved exactly: @@ -1045,7 +1044,6 @@ Attribute & Symbol & Units \\ \midrule \atSIG{} & $\sigma_0$ & N$\cdot$m/rad \\ \atSIGD{} & $\sigma_1$ & N$\cdot$m$\cdot$s/rad \\ -\atSIGV{} & $\sigma_2$ & N$\cdot$m$\cdot$s/rad \\ \atTAUC{} & $\tau_c$ & N$\cdot$m \\ \atTAUS{} & $\tau_s$ & N$\cdot$m \\ \atWS{} & $\omega_s$ & rad/s \\ @@ -1081,7 +1079,7 @@ Attribute & Type & Description \\ \label{tab:controller_attributes} \end{table} -\noindent Unlike the motor parameters in Table~\ref{tab:datasheet}, controller gains are user-specified firmware settings. Gains are in {\em voltage-space} (e.g., $k_p$ in V/rad) since the output is a voltage $v$. To convert from physical torque-space (N$\cdot$m/rad), multiply by $R/K$. +\noindent Unlike the motor parameters in Table~\ref{tab:datasheet}, controller gains are user-specified firmware settings with units that vary by manufacturer. MuJoCo uses direct {\em voltage-space} units (e.g., $k_p$ in V/rad). Torque-space (N$\cdot$m/rad) gains can be converted by multiplying by $R/K$, though empirical calibration is often necessary due to unknown internal units on real hardware. The controller computes a target voltage $v$ from the \texttt{ctrl} command. All motor physics --- cogging, saturation, friction, etc. --- apply identically downstream of $v$. The \texttt{input} attribute selects the controller: diff --git a/doc/includes/references.h b/doc/includes/references.h index 89b2e81f..bea7f4d9 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3664,9 +3664,9 @@ const char* mjs_setToMuscle(mjsActuator* actuator, double timeconst[2], double t double lmax, double vmax, double fpmax, double fvmax); const char* mjs_setToAdhesion(mjsActuator* actuator, double gain); const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance, - double nominal[3], double saturation[4], double inductance[2], - double cogging[3], double controller[5], double thermal[6], - double lugre[6], int input_mode); + double nominal[3], double saturation[3], double inductance[2], + double cogging[3], double controller[6], double thermal[6], + double lugre[5], int input_mode); mjsMesh* mjs_addMesh(mjSpec* s, const mjsDefault* def); mjsHField* mjs_addHField(mjSpec* s); mjsSkin* mjs_addSkin(mjSpec* s); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index e8bbe692..90a6569b 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1729,9 +1729,9 @@ MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain); // Set actuator to DC motor; return error if any. // Nullable: motorconst, nominal, saturation, inductance, cogging, controller, thermal, lugre MJAPI const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance, - double nominal[3], double saturation[4], double inductance[2], - double cogging[3], double controller[5], double thermal[6], - double lugre[6], int input_mode); + double nominal[3], double saturation[3], double inductance[2], + double cogging[3], double controller[6], double thermal[6], + double lugre[5], int input_mode); //---------------------------------- Assets -------------------------------------------------------- diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 262d3430..f05bd8a5 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -10823,7 +10823,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ name='saturation', type=ArrayType( inner_type=ValueType(name='double'), - extents=(4,), + extents=(3,), ), nullable=True, ), @@ -10847,7 +10847,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ name='controller', type=ArrayType( inner_type=ValueType(name='double'), - extents=(5,), + extents=(6,), ), nullable=True, ), @@ -10863,7 +10863,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ name='lugre', type=ArrayType( inner_type=ValueType(name='double'), - extents=(6,), + extents=(5,), ), nullable=True, ), diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 28af8af1..82d43057 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -1307,10 +1307,10 @@ PYBIND11_MODULE(_specs, m) { "set_to_dcmotor", [](raw::MjsActuator* self, std::array motorconst, double resistance, - std::array nominal, std::array saturation, + std::array nominal, std::array saturation, std::array inductance, std::array cogging, - std::array controller, std::array thermal, - std::array lugre, int input_mode) { + std::array controller, std::array thermal, + std::array lugre, int input_mode) { std::string err = mjs_setToDCMotor( self, motorconst.data(), resistance, nominal.data(), saturation.data(), inductance.data(), cogging.data(), @@ -1321,12 +1321,12 @@ PYBIND11_MODULE(_specs, m) { }, py::arg("motorconst"), py::arg("resistance"), py::arg("nominal") = std::array{0, 0, 0}, - py::arg("saturation") = std::array{0, 0, 0, 0}, + py::arg("saturation") = std::array{0, 0, 0}, py::arg("inductance") = std::array{0, 0}, py::arg("cogging") = std::array{0, 0, 0}, - py::arg("controller") = std::array{0, 0, 0, 0, 0}, + py::arg("controller") = std::array{0, 0, 0, 0, 0, 0}, py::arg("thermal") = std::array{0, 0, 0, 0, 0, 0}, - py::arg("lugre") = std::array{0, 0, 0, 0, 0, 0}, + py::arg("lugre") = std::array{0, 0, 0, 0, 0}, py::arg("input_mode") = 0); // ============================= MJSTENDONPATH =============================== diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 8d320463..e2277bfd 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -1122,9 +1122,9 @@ const char* mjs_setToAdhesion(mjsActuator* actuator, double gain) { const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance, - double nominal[3], double saturation[4], double inductance[2], - double cogging[3], double controller[5], double thermal[6], - double lugre[6], int input_mode) { + double nominal[3], double saturation[3], double inductance[2], + double cogging[3], double controller[6], double thermal[6], + double lugre[5], int input_mode) { double R = resistance; // electrical resistance double Kt = motorconst ? motorconst[0] : 0; // torque constant double Ke = motorconst ? motorconst[1] : 0; // back-EMF constant @@ -1134,9 +1134,8 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double // derive Ke from nominal: omega0 = vn*Ke / (Ke^2 + R*B) if (vn > 0 && Ke <= 0 && omega0 > 0) { - // viscous damping (linear), add lugre sigma2 contribution if any + // viscous damping (linear) double B = actuator->damping[0]; - if (lugre && lugre[0] > 0) B += lugre[2]; if (B > 0 && R > 0) { // R known: solve quadratic Ke^2*omega0 - Ke*vn + R*B*omega0 = 0 @@ -1184,12 +1183,9 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double actuator->dynprm[7] = controller ? controller[3] : 0; // slewmax actuator->dynprm[8] = controller ? controller[4] : 0; // Imax - // saturation: [tau_max, i_max, (di/dt)_max, v_max] - if (saturation && saturation[2] > 0) { - actuator->dynprm[1] = saturation[2]; // (di/dt)_max - } - if (saturation && saturation[3] > 0) { - actuator->gainprm[7] = saturation[3]; // v_max + // controller parameters: gainprm[7] for v_max + if (controller && controller[5] > 0) { + actuator->gainprm[7] = controller[5]; // v_max } // saturation -> forcerange @@ -1203,6 +1199,11 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double actuator->forcelimited = 1; } + // saturation: [tau_max, i_max, (di/dt)_max] + if (saturation && saturation[2] > 0) { + actuator->dynprm[1] = saturation[2]; // (di/dt)_max + } + // cogging: [amplitude, periodicity, phase] -> biasprm[0:3] actuator->biasprm[0] = cogging ? cogging[0] : 0; // amplitude actuator->biasprm[1] = cogging ? cogging[1] : 0; // periodicity @@ -1258,14 +1259,13 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double actdim++; } - // lugre: {stiffness, damping, viscous, coulomb, static, stribeck} + // lugre: {stiffness, damping, coulomb, static, stribeck} if (lugre && lugre[0] > 0) { actuator->dynprm[5] = lugre[0]; // stiffness -> sigma0 actuator->dynprm[6] = lugre[1]; // damping -> sigma1 - actuator->damping[0] += lugre[2]; // viscous -> sigma2 - actuator->biasprm[3] = lugre[3]; // coulomb -> tau_c - actuator->biasprm[4] = lugre[4]; // static -> tau_s - actuator->biasprm[5] = lugre[5]; // stribeck -> omega_s + actuator->biasprm[3] = lugre[2]; // coulomb -> tau_c + actuator->biasprm[4] = lugre[3]; // static -> tau_s + actuator->biasprm[5] = lugre[4]; // stribeck -> omega_s actdim++; } diff --git a/src/user/user_api.h b/src/user/user_api.h index 307f1a53..2b73c12e 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -194,9 +194,9 @@ MJAPI const char* mjs_setToAdhesion(mjsActuator* actuator, double gain); // Set actuator to DC motor, return error on failure. MJAPI const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double resistance, - double nominal[3], double saturation[4], double inductance[2], - double cogging[3], double controller[5], double thermal[6], - double lugre[6], int input_mode); + double nominal[3], double saturation[3], double inductance[2], + double cogging[3], double controller[6], double thermal[6], + double lugre[5], int input_mode); //---------------------------------- Add assets ---------------------------------------------------- diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 352790b5..df10509f 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -2526,14 +2526,14 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { double motorconst[2] = {inherited ? actuator->gainprm[1] : 0, 0}; double resistance = inherited ? actuator->gainprm[0] : 0; double nominal[3] = {0, 0, 0}; - double saturation[4] = {0, 0, - inherited ? actuator->dynprm[1] : 0, - inherited ? actuator->gainprm[8] : 0}; - double controller[5] = {inherited ? actuator->gainprm[5] : 0, + double saturation[3] = {0, 0, + inherited ? actuator->dynprm[1] : 0}; + double controller[6] = {inherited ? actuator->gainprm[4] : 0, + inherited ? actuator->gainprm[5] : 0, inherited ? actuator->gainprm[6] : 0, - inherited ? actuator->gainprm[7] : 0, inherited ? actuator->dynprm[7] : 0, - inherited ? actuator->dynprm[8] : 0}; + inherited ? actuator->dynprm[8] : 0, + inherited ? actuator->gainprm[7] : 0}; double inductance[2] = {0, inherited ? actuator->dynprm[0] : 0}; double cogging[3] = {inherited ? actuator->biasprm[0] : 0, inherited ? actuator->biasprm[1] : 0, @@ -2544,22 +2544,21 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { inherited ? actuator->gainprm[2] : 0, inherited ? actuator->gainprm[3] : 0, inherited ? actuator->dynprm[4] : 0}; - double lugre[6] = {inherited ? actuator->dynprm[5] : 0, + double lugre[5] = {inherited ? actuator->dynprm[5] : 0, inherited ? actuator->dynprm[6] : 0, - inherited ? actuator->damping[0] : 0, inherited ? actuator->biasprm[3] : 0, inherited ? actuator->biasprm[4] : 0, inherited ? actuator->biasprm[5] : 0}; - int input_mode = inherited ? (int)actuator->gainprm[9] : 0; + int input_mode = inherited ? (int)actuator->gainprm[8] : 0; ReadAttr(elem, "motorconst", 2, motorconst, text, false, false); ReadAttr(elem, "resistance", 1, &resistance, text); ReadAttr(elem, "nominal", 3, nominal, text, false, false); - ReadAttr(elem, "saturation", 4, saturation, text, false, false); + ReadAttr(elem, "saturation", 3, saturation, text, false, false); ReadAttr(elem, "inductance", 2, inductance, text, false, false); ReadAttr(elem, "cogging", 3, cogging, text, false, false); - ReadAttr(elem, "controller", 5, controller, text, false, false); + ReadAttr(elem, "controller", 6, controller, text, false, false); ReadAttr(elem, "thermal", 6, thermal, text, false, false); - ReadAttr(elem, "lugre", 6, lugre, text, false, false); + ReadAttr(elem, "lugre", 5, lugre, text, false, false); if (MapValue(elem, "input", &input_mode, dcmotorinput_map, dcmotorinput_sz)) { // successfully parsed } diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index f3acf0aa..3c9a05d4 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -1416,7 +1416,7 @@ TEST_F(DCMotorTest, LuGreViscousFriction) { + damping="0.01" lugre="100 1 0.5 0.7 10"/> )"; @@ -1929,7 +1929,7 @@ TEST_F(DCMotorTest, CurrentRateLimit) { + inductance="0.01 0" saturation="0 0 100"/> )"; @@ -1963,6 +1963,106 @@ TEST_F(DCMotorTest, CurrentRateLimit) { mj_deleteModel(model); } + +TEST_F(DCMotorTest, VoltageLimit) { + // verifies that saturation:voltage clamps voltage + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // Vmax = 10.0, ctrl = 20.0 + // force = K/R * Vmax = 0.05 / 2.0 * 10.0 = 0.25 + data->ctrl[0] = 20.0; + mj_forward(model, data); + + EXPECT_NEAR(data->actuator_force[0], 0.25, MjTol(1e-12, 1e-5)); + + // negative drive + data->ctrl[0] = -20.0; + mj_forward(model, data); + + EXPECT_NEAR(data->actuator_force[0], -0.25, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + + +TEST_F(DCMotorTest, IntegralClamp) { + // verifies that controller Imax clamps integral state + static constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + // Imax = 5.0 + ASSERT_EQ(model->actuator_actnum[0], 1); // only ki is stateful + int adr = model->actuator_actadr[0]; + + // set integral state to Imax + data->act[adr] = 5.0; + + // set target to generate positive error (ctrl - length) + data->ctrl[0] = 1.0; // target + data->qpos[0] = 0.0; // length = 0 + + mj_forward(model, data); + + // act_dot should be clamped to 0 because act >= Imax and error > 0 + EXPECT_NEAR(data->act_dot[adr], 0.0, MjTol(1e-12, 1e-5)); + + // set target to generate negative error + data->ctrl[0] = -1.0; + mj_forward(model, data); + + // act_dot should be negative (not clamped) + EXPECT_NEAR(data->act_dot[adr], -1.0, MjTol(1e-12, 1e-5)); + + // set integral state to -Imax + data->act[adr] = -5.0; + + // set target to generate negative error + data->ctrl[0] = -1.0; + data->qpos[0] = 0.0; + mj_forward(model, data); + + // act_dot should be clamped to 0 because act <= -Imax and error < 0 + EXPECT_NEAR(data->act_dot[adr], 0.0, MjTol(1e-12, 1e-5)); + + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(DCMotorTest, LuGreExactIntegration) { static constexpr char xml[] = R"( @@ -1975,7 +2075,7 @@ TEST_F(DCMotorTest, LuGreExactIntegration) { + damping="0.01" lugre="100 1 0.5 0.7 10"/> )"; @@ -2021,7 +2121,7 @@ TEST_F(DCMotorTest, LuGreSteadyState) { + damping="0.01" lugre="100 1 0.5 0.7 10"/> )"; @@ -2068,7 +2168,7 @@ TEST_F(DCMotorTest, LuGreBristleSpring) { + damping="0.01" lugre="100 1 0.5 0.7 10"/> )"; diff --git a/test/engine/testdata/derivative/dcmotor.xml b/test/engine/testdata/derivative/dcmotor.xml index d3c4b0ac..a1053bb6 100644 --- a/test/engine/testdata/derivative/dcmotor.xml +++ b/test/engine/testdata/derivative/dcmotor.xml @@ -30,6 +30,6 @@ + damping="0.001" lugre="1e4 100 0.005 0.008 0.1"/> diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index b36557aa..2a7d8a53 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -280,6 +280,55 @@ TEST_F(MujocoTest, SetToDCMotorDeriveKe) { mj_deleteSpec(spec); } +TEST_F(MujocoTest, SetToDCMotorFull) { + mjSpec* spec = mj_makeSpec(); + mjsActuator* actuator = mjs_addActuator(spec, 0); + + double motorconst[2] = {0.05, 0.05}; + double resistance = 2.0; + double saturation[3] = {1.0, 2.0, 3.0}; + double controller[6] = {10.0, 20.0, 30.0, 40.0, 50.0, 60.0}; + + const char* err = mjs_setToDCMotor(actuator, motorconst, resistance, + nullptr, saturation, nullptr, + nullptr, controller, nullptr, + nullptr, 0); + EXPECT_STREQ(err, ""); + EXPECT_EQ(actuator->gainprm[0], 2.0); // resistance + EXPECT_EQ(actuator->gainprm[1], 0.05); // K + EXPECT_EQ(actuator->gainprm[4], 10.0); // kp + EXPECT_EQ(actuator->gainprm[5], 20.0); // ki + EXPECT_EQ(actuator->gainprm[6], 30.0); // kd + EXPECT_EQ(actuator->dynprm[7], 40.0); // slewmax + EXPECT_EQ(actuator->dynprm[8], 50.0); // Imax + EXPECT_EQ(actuator->gainprm[7], 60.0); // Vmax + EXPECT_EQ(actuator->dynprm[1], 3.0); // (di/dt)_max + + mj_deleteSpec(spec); +} + +TEST_F(MujocoTest, SetToDCMotorLuGre) { + mjSpec* spec = mj_makeSpec(); + mjsActuator* actuator = mjs_addActuator(spec, 0); + + double motorconst[2] = {0.05, 0.05}; + double resistance = 2.0; + double lugre[5] = {100.0, 1.0, 0.5, 0.7, 10.0}; + + const char* err = mjs_setToDCMotor(actuator, motorconst, resistance, + nullptr, nullptr, nullptr, + nullptr, nullptr, nullptr, + lugre, 0); + EXPECT_STREQ(err, ""); + EXPECT_EQ(actuator->dynprm[5], 100.0); // stiffness + EXPECT_EQ(actuator->dynprm[6], 1.0); // damping + EXPECT_EQ(actuator->biasprm[3], 0.5); // coulomb + EXPECT_EQ(actuator->biasprm[4], 0.7); // static + EXPECT_EQ(actuator->biasprm[5], 10.0); // stribeck + + mj_deleteSpec(spec); +} + static constexpr char xml_plugin_1[] = R"( diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index bd1b517b..5f6ecef7 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -3108,7 +3108,55 @@ TEST_F(ActuatorParseTest, DCMotorSaturation) { mj_deleteModel(model); } -TEST_F(ActuatorParseTest, DCMotorLuGreRemapping) { + +TEST_F(ActuatorParseTest, DCMotorInheritedDefaults) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + + // check motorconst and resistance are overridden by instance + EXPECT_MJTNUM_EQ(model->actuator_gainprm[1], 0.05); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[0], 2.0); + + // check controller gains (kp, ki, kd) in gainprm[4:6] + EXPECT_MJTNUM_EQ(model->actuator_gainprm[4], 2.0); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[5], 0.5); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[6], 0.1); + + // check controller limits (slewmax, Imax) in dynprm[7,8] + EXPECT_MJTNUM_EQ(model->actuator_dynprm[7], 10.0); + EXPECT_MJTNUM_EQ(model->actuator_dynprm[8], 5.0); + + // check Vmax in gainprm[7] + EXPECT_MJTNUM_EQ(model->actuator_gainprm[7], 12.0); + + // check input mode in gainprm[8] + EXPECT_MJTNUM_EQ(model->actuator_gainprm[8], 2.0); + + // check inductance (te) in dynprm[0] + EXPECT_MJTNUM_EQ(model->actuator_dynprm[0], 0.01); + + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorControllerFull) { static constexpr char xml[] = R"( @@ -3119,7 +3167,36 @@ TEST_F(ActuatorParseTest, DCMotorLuGreRemapping) { + controller="1.0 2.0 3.0 4.0 5.0 6.0"/> + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + + EXPECT_MJTNUM_EQ(model->actuator_gainprm[4], 1.0); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[5], 2.0); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[6], 3.0); + EXPECT_MJTNUM_EQ(model->actuator_dynprm[7], 4.0); + EXPECT_MJTNUM_EQ(model->actuator_dynprm[8], 5.0); + EXPECT_MJTNUM_EQ(model->actuator_gainprm[7], 6.0); + + mj_deleteModel(model); +} + +TEST_F(ActuatorParseTest, DCMotorLuGreRemapping) { + static constexpr char xml[] = R"( + + + + + + + + + )"; @@ -3135,6 +3212,30 @@ TEST_F(ActuatorParseTest, DCMotorLuGreRemapping) { mj_deleteModel(model); } +TEST_F(ActuatorParseTest, DCMotorLuGreInheritedDefaults) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_MJTNUM_EQ(model->actuator_damping[0], 0.01); + mj_deleteModel(model); +} + TEST_F(ActuatorParseTest, DCMotorActdimStateless) { static constexpr char xml[] = R"( @@ -3216,7 +3317,7 @@ TEST_F(ActuatorParseTest, DCMotorActdimLuGreOnly) { + lugre="100 1 0.5 0.7 10"/> )"; @@ -3241,7 +3342,7 @@ TEST_F(ActuatorParseTest, DCMotorActdimAllThree) { + lugre="100 1 0.5 0.7 10"/> )"; From a744b366fb5af0b89821aa12b90532d7ed442bb7 Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Thu, 9 Apr 2026 07:57:37 -0700 Subject: [PATCH 036/251] Include obj and stl decoder plugins as sources in CMake builds. This removes the need to load these decoders via mj_loadAllPluginLibraries when using MuJoCo built with CMake. Other plugins are unchanged. PiperOrigin-RevId: 897114719 Change-Id: Iee914cc5e05798186f384a67901f1cf86bc2f887 --- .github/workflows/build_steps.sh | 4 --- CMakeLists.txt | 1 + cmake/ShellTests.cmake | 1 - doc/changelog.rst | 2 ++ plugin/obj_decoder/CMakeLists.txt | 47 +++---------------------------- plugin/stl_decoder/CMakeLists.txt | 41 ++------------------------- sample/testspeed.cc | 6 ---- test/user/CMakeLists.txt | 9 ------ test/xml/CMakeLists.txt | 3 -- wasm/CMakeLists.txt | 7 ++++- wasm/tests/CMakeLists.txt | 7 ++++- 11 files changed, 21 insertions(+), 107 deletions(-) diff --git a/.github/workflows/build_steps.sh b/.github/workflows/build_steps.sh index 6c8877d4..4c55e5d4 100755 --- a/.github/workflows/build_steps.sh +++ b/.github/workflows/build_steps.sh @@ -107,8 +107,6 @@ copy_plugins_posix() { mkdir -p ${TMPDIR}/mujoco_install/mujoco_plugin && cp lib/libactuator.* ${TMPDIR}/mujoco_install/mujoco_plugin && cp lib/libelasticity.* ${TMPDIR}/mujoco_install/mujoco_plugin && - cp lib/libobj_decoder.* ${TMPDIR}/mujoco_install/mujoco_plugin && - cp lib/libstl_decoder.* ${TMPDIR}/mujoco_install/mujoco_plugin && cp lib/libsensor.* ${TMPDIR}/mujoco_install/mujoco_plugin && cp lib/libsdf_plugin.* ${TMPDIR}/mujoco_install/mujoco_plugin } @@ -119,8 +117,6 @@ copy_plugins_window() { mkdir -p ${TMPDIR}/mujoco_install/mujoco_plugin && cp bin/Release/actuator.dll ${TMPDIR}/mujoco_install/mujoco_plugin && cp bin/Release/elasticity.dll ${TMPDIR}/mujoco_install/mujoco_plugin && - cp bin/Release/obj_decoder.dll ${TMPDIR}/mujoco_install/mujoco_plugin && - cp bin/Release/stl_decoder.dll ${TMPDIR}/mujoco_install/mujoco_plugin && cp bin/Release/sensor.dll ${TMPDIR}/mujoco_install/mujoco_plugin } diff --git a/CMakeLists.txt b/CMakeLists.txt index f79153c5..2ade5c70 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -112,6 +112,7 @@ if(NOT EMSCRIPTEN) endif() add_subdirectory(plugin/obj_decoder) add_subdirectory(plugin/stl_decoder) + add_subdirectory(src/engine) add_subdirectory(src/user) add_subdirectory(src/xml) diff --git a/cmake/ShellTests.cmake b/cmake/ShellTests.cmake index d2480a25..b26b1658 100644 --- a/cmake/ShellTests.cmake +++ b/cmake/ShellTests.cmake @@ -38,7 +38,6 @@ function(add_mujoco_shell_test TEST_NAME TARGET_BINARY) "CMAKE_SOURCE_DIR=${CMAKE_SOURCE_DIR}" "TARGET_BINARY=$" "TEST_TMPDIR=${TEST_TMPDIR}" - "MUJOCO_PLUGIN_DIR=$" ) if(WIN32) # Define the directory containing the mujoco DLL library so that it can be added to the PATH. diff --git a/doc/changelog.rst b/doc/changelog.rst index b97dce5e..82fae188 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -40,6 +40,8 @@ General (previously the scalar), and subsequent elements are the higher-order :ref:`polynomial` coefficients. **Migration:** Replace assignments like ``joint.stiffness = val`` with ``joint.stiffness[0] = val``. + - ``.obj`` and ``.stl`` decoders are now included as source when building MuJoCo with CMake. This fixes the + behaviour from the previous release where it required downstream code to load these plugins explicitly. - The ``vertcollide`` field in :ref:`mjsFlex` has been removed. It is no longer required since :doc:`MuJoCo Warp ` supports native flex collisions. diff --git a/plugin/obj_decoder/CMakeLists.txt b/plugin/obj_decoder/CMakeLists.txt index 17b67199..689dfc51 100644 --- a/plugin/obj_decoder/CMakeLists.txt +++ b/plugin/obj_decoder/CMakeLists.txt @@ -12,48 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. -if(EMSCRIPTEN) - add_library(obj_decoder OBJECT obj_decoder.cc) -else() - set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) - set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_LIBDIR}") - - add_library(obj_decoder SHARED obj_decoder.cc) -endif() - -target_include_directories(obj_decoder PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/../.. - ${CMAKE_CURRENT_SOURCE_DIR}/../../include +target_compile_definitions(mujoco PRIVATE TINYOBJLOADER_IMPLEMENTATION) +target_sources(mujoco PRIVATE + obj_decoder.cc ) -if(EMSCRIPTEN) - target_link_libraries(obj_decoder PRIVATE - tinyobjloader - ) -else() - target_link_libraries(obj_decoder PRIVATE - mujoco - tinyobjloader - ) -endif() - -target_compile_definitions(obj_decoder PRIVATE TINYOBJLOADER_IMPLEMENTATION) - -target_compile_options(obj_decoder PRIVATE - ${AVX_COMPILE_OPTIONS} - ${MUJOCO_MACOS_COMPILE_OPTIONS} - ${EXTRA_COMPILE_OPTIONS} - ${MUJOCO_CXX_FLAGS} -) - -if(NOT EMSCRIPTEN) - target_link_options(obj_decoder PRIVATE - ${MUJOCO_MACOS_LINK_OPTIONS} - ${EXTRA_LINK_OPTIONS} - ) - - install( - TARGETS obj_decoder - LIBRARY DESTINATION "${CMAKE_INSTALL_BINDIR}/mujoco_plugin" - ) -endif() +target_link_libraries(mujoco PRIVATE tinyobjloader) diff --git a/plugin/stl_decoder/CMakeLists.txt b/plugin/stl_decoder/CMakeLists.txt index 1d383632..50eef18d 100644 --- a/plugin/stl_decoder/CMakeLists.txt +++ b/plugin/stl_decoder/CMakeLists.txt @@ -12,43 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -if(EMSCRIPTEN) - add_library(stl_decoder OBJECT stl_decoder.cc) -else() - set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) - set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_LIBDIR}") - - add_library(stl_decoder SHARED stl_decoder.cc) -endif() - -target_include_directories(stl_decoder PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/../.. - ${CMAKE_CURRENT_SOURCE_DIR}/../../include +target_sources(mujoco PRIVATE + stl_decoder.cc ) - -if(EMSCRIPTEN) - target_link_libraries(stl_decoder PRIVATE) -else() - target_link_libraries(stl_decoder PRIVATE - mujoco - ) -endif() - -target_compile_options(stl_decoder PRIVATE - ${AVX_COMPILE_OPTIONS} - ${MUJOCO_MACOS_COMPILE_OPTIONS} - ${EXTRA_COMPILE_OPTIONS} - ${MUJOCO_CXX_FLAGS} -) - -if(NOT EMSCRIPTEN) - target_link_options(stl_decoder PRIVATE - ${MUJOCO_MACOS_LINK_OPTIONS} - ${EXTRA_LINK_OPTIONS} - ) - - install( - TARGETS stl_decoder - LIBRARY DESTINATION "${CMAKE_INSTALL_BINDIR}/mujoco_plugin" - ) -endif() diff --git a/sample/testspeed.cc b/sample/testspeed.cc index 544edfc6..f14e6572 100644 --- a/sample/testspeed.cc +++ b/sample/testspeed.cc @@ -187,12 +187,6 @@ int main(int argc, char** argv) { nthread = mjMAX(1, mjMIN(maxthread, nthread)); npoolthread = mjMAX(1, mjMIN(maxthread, npoolthread)); - // load plugins from MUJOCO_PLUGIN_DIR if set - const char* plugin_dir = std::getenv("MUJOCO_PLUGIN_DIR"); - if (plugin_dir) { - mj_loadAllPluginLibraries(plugin_dir, nullptr); - } - // get filename, determine file type std::string filename(argv[1]); bool binary = (filename.find(".mjb") != std::string::npos); // NOLINT diff --git a/test/user/CMakeLists.txt b/test/user/CMakeLists.txt index 7cbc667f..912a26c0 100644 --- a/test/user/CMakeLists.txt +++ b/test/user/CMakeLists.txt @@ -14,9 +14,6 @@ mujoco_test( user_model_test - PROPERTIES - ENVIRONMENT - "MUJOCO_PLUGIN_DIR=$" ADDITIONAL_LINK_LIBRARIES absl::str_format ) @@ -31,16 +28,10 @@ mujoco_test( mujoco_test( user_flex_test - PROPERTIES - ENVIRONMENT - "MUJOCO_PLUGIN_DIR=$" ) mujoco_test( user_mesh_test - PROPERTIES - ENVIRONMENT - "MUJOCO_PLUGIN_DIR=$" ADDITIONAL_LINK_LIBRARIES absl::str_format ) diff --git a/test/xml/CMakeLists.txt b/test/xml/CMakeLists.txt index 63348c62..d4cefdb1 100644 --- a/test/xml/CMakeLists.txt +++ b/test/xml/CMakeLists.txt @@ -16,9 +16,6 @@ mujoco_test(xml_api_test) mujoco_test( xml_native_reader_test - PROPERTIES - ENVIRONMENT - "MUJOCO_PLUGIN_DIR=$" ) mujoco_test(xml_utils_test) diff --git a/wasm/CMakeLists.txt b/wasm/CMakeLists.txt index 909401d1..08004839 100644 --- a/wasm/CMakeLists.txt +++ b/wasm/CMakeLists.txt @@ -67,6 +67,11 @@ set_target_properties(mujoco_wasm PROPERTIES OUTPUT_NAME "mujoco" ) -target_link_libraries(mujoco_wasm ccd lodepng mujoco tinyxml2 qhullstatic_r obj_decoder stl_decoder) +# Link the mujoco library as a whole archive to avoid losing plugin +# registration such as obj_decoder and stl_decoder. +target_link_libraries(mujoco_wasm PRIVATE + -Wl,--whole-archive mujoco -Wl,--no-whole-archive + ccd lodepng tinyxml2 qhullstatic_r +) install(TARGETS mujoco_wasm DESTINATION ${DIVISIBLE_INSTALL_BIN_DIR}) diff --git a/wasm/tests/CMakeLists.txt b/wasm/tests/CMakeLists.txt index 0a21ec31..f6d74289 100644 --- a/wasm/tests/CMakeLists.txt +++ b/wasm/tests/CMakeLists.txt @@ -53,6 +53,11 @@ add_executable(mujoco_wasm_benchmark ${MUJOCO_WASM_FILES}) set_target_properties(mujoco_wasm_benchmark PROPERTIES LINK_FLAGS "${EMCC_LINKER_FLAGS_STR}") -target_link_libraries(mujoco_wasm_benchmark ccd lodepng mujoco tinyxml2 qhullstatic_r obj_decoder stl_decoder) +# Link the mujoco library as a whole archive to avoid losing plugin +# registration such as obj_decoder and stl_decoder. +target_link_libraries(mujoco_wasm_benchmark PRIVATE + -Wl,--whole-archive mujoco -Wl,--no-whole-archive + ccd lodepng tinyxml2 qhullstatic_r +) install(TARGETS mujoco_wasm_benchmark DESTINATION ${DIVISIBLE_INSTALL_BIN_DIR}) From 6b724616c0d129aca57bf36778957d31e463a2fc Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 9 Apr 2026 13:12:40 -0700 Subject: [PATCH 037/251] Prevent deletion from an attached mjSpec. This change disallows calling `mjs_delete` on an mjSpec that has been attached to another mjSpec via `mjs_attach`. Attempting to delete an element from an attached spec will now result in an error. The Python bindings for `mjSpec.delete` have been updated to raise a ValueError when this occurs. PiperOrigin-RevId: 897266695 Change-Id: Ic0670125a3028191ec50eca890f02b5910ec8b03 --- python/mujoco/specs.cc | 72 +++++++++++++++++++++++++++---------- python/mujoco/specs_test.py | 16 ++++++++- src/user/user_api.cc | 6 +++- src/user/user_model.h | 3 ++ test/user/user_api_test.cc | 30 ++++++++++++++++ 5 files changed, 107 insertions(+), 20 deletions(-) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 82d43057..e58d66cc 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -450,7 +450,9 @@ PYBIND11_MODULE(_specs, m) { }, py::return_value_policy::reference_internal); mjSpec.def("delete", [](MjSpec& self, raw::MjsBody& body) { - mjs_delete(self.ptr, body.element); + if (mjs_delete(self.ptr, body.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); mjSpec.def( "attach", @@ -867,7 +869,9 @@ PYBIND11_MODULE(_specs, m) { // ============================= MJSFRAME ==================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsFrame& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); mjsFrame.def("set_frame", [](raw::MjsFrame& self, raw::MjsFrame& frame) { if (mjs_setFrame(self.element, &frame) != 0) { @@ -906,7 +910,9 @@ PYBIND11_MODULE(_specs, m) { // ============================= MJSGEOM ===================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsGeom& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); mjsGeom.def("set_frame", [](raw::MjsGeom& self, raw::MjsFrame& frame) { if (mjs_setFrame(self.element, &frame) != 0) { @@ -937,7 +943,9 @@ PYBIND11_MODULE(_specs, m) { // ============================= MJSJOINT ==================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsJoint& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); mjsJoint.def("set_frame", [](raw::MjsJoint& self, raw::MjsFrame& frame) { if (mjs_setFrame(self.element, &frame) != 0) { @@ -968,7 +976,9 @@ PYBIND11_MODULE(_specs, m) { // ============================= MJSSITE ===================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsSite& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); mjsSite.def("set_frame", [](raw::MjsSite& self, raw::MjsFrame& frame) { if (mjs_setFrame(self.element, &frame) != 0) { @@ -1016,7 +1026,9 @@ PYBIND11_MODULE(_specs, m) { // ============================= MJSCAMERA =================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsCamera& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); mjsCamera.def("set_frame", [](raw::MjsCamera& self, raw::MjsFrame& frame) { if (mjs_setFrame(self.element, &frame) != 0) { @@ -1047,7 +1059,9 @@ PYBIND11_MODULE(_specs, m) { // ============================= MJSLIGHT ==================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsLight& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); mjsLight.def("set_frame", [](raw::MjsLight& self, raw::MjsFrame& frame) { if (mjs_setFrame(self.element, &frame) != 0) { @@ -1078,7 +1092,9 @@ PYBIND11_MODULE(_specs, m) { // ============================= MJSMATERIAL ================================= mjSpec.def("delete", [](MjSpec& self, raw::MjsMaterial& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); mjsMaterial.def_property( "classname", @@ -1092,7 +1108,9 @@ PYBIND11_MODULE(_specs, m) { // ============================= MJSMESH ===================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsMesh& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); mjsMesh.def_property( "classname", @@ -1452,47 +1470,65 @@ PYBIND11_MODULE(_specs, m) { // ============================= MJSFLEX ===================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsFlex& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); // ============================= MJSHFIELD =================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsHField& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); // ============================= MJSSKIN ===================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsSkin& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); // ============================= MJSTEXTURE ================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsTexture& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); // ============================= MJSKEY ====================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsKey& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); // ============================= MJSTEXT ===================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsText& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); // ============================= MJSNUMERIC ================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsNumeric& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); // ============================= MJSEXCLUDE ================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsExclude& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); // ============================= MJSTUPLE ==================================== mjSpec.def("delete", [](MjSpec& self, raw::MjsTuple& obj) { - mjs_delete(self.ptr, obj.element); + if (mjs_delete(self.ptr, obj.element) != 0) { + throw pybind11::value_error(mjs_getError(self.ptr)); + } }); // ============================= MJSPLUGIN =================================== diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index a82646dd..8bb9360e 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -979,7 +979,6 @@ class SpecsTest(absltest.TestCase): self.assertEqual(mesh.plugin.name, 'inst') self.assertEqual(mesh.plugin.plugin_name, 'mujoco.sdf.torus') - def test_duplicate_name_error(self): main_xml = """ @@ -1391,6 +1390,21 @@ class SpecsTest(absltest.TestCase): with self.assertRaisesRegex(ValueError, 'Frame not found.'): parent.attach(child4, frame='invalid_frame', prefix='child3-') + def test_delete_from_attached_spec_error(self): + parent = mujoco.MjSpec() + child = mujoco.MjSpec() + body = child.worldbody.add_body(name='child_body') + geom = body.add_geom(name='child_geom') + + frame = parent.worldbody.add_frame() + parent.attach(child, frame=frame, prefix='child_') + + # Now child spec is attached. Deleting from it should raise ValueError. + with self.assertRaisesRegex( + ValueError, 'Cannot delete element from an attached mjSpec.' + ): + child.delete(geom) + def test_attach_valid_child_lists(self): xml1 = """ diff --git a/src/user/user_api.cc b/src/user/user_api.cc index e2277bfd..13b9168c 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -474,8 +474,12 @@ int mj_copyBack(mjSpec* s, const mjModel* m) { // remove body from mjSpec, return 0 on success int mjs_delete(mjSpec* s, mjsElement* element) { mjCModel* model = static_cast(s->element); + if (model->IsAttached()) { + model->SetError(mjCError(nullptr, "Cannot delete element from an attached mjSpec.")); + return -1; + } if (!element) { - model->SetError(mjCError(0, "Element is null.")); + model->SetError(mjCError(nullptr, "Element is null.")); return -1; } try { diff --git a/src/user/user_model.h b/src/user/user_model.h index 025f8097..90feb1ed 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -327,6 +327,9 @@ class mjCModel : public mjCModel_, private mjSpec { // set attached flag void SetAttached(bool deepcopy) { attached_ |= !deepcopy; } + // check if model is attached + bool IsAttached() const { return attached_; } + // check for repeated names in list void CheckRepeat(mjtObj type); diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 2a7d8a53..3c7af426 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -173,6 +173,36 @@ TEST_F(MujocoTest, TreeTraversal) { mj_deleteSpec(spec); } +TEST_F(MujocoTest, AttachAndChildDeletion) { + mjSpec* child_spec = mj_makeSpec(); + mjsBody* child_world = mjs_findBody(child_spec, "world"); + mjsBody* child_body = mjs_addBody(child_world, 0); + mjsJoint* freejoint = mjs_addJoint(child_body, 0); + freejoint->type = mjJNT_FREE; + mjs_setName(freejoint->element, "child_freejoint"); + + mjSpec* parent_spec = mj_makeSpec(); + mjsBody* parent_world = mjs_findBody(parent_spec, "world"); + mjsBody* parent_body = mjs_addBody(parent_world, 0); + + // Attach child spec to parent_body + mjsElement* attached = + mjs_attach(parent_body->element, child_spec->element, "pre_", ""); + ASSERT_THAT(attached, NotNull()); + + // Delete freejoint from child_spec, should fail because it is attached + int result = mjs_delete(child_spec, freejoint->element); + EXPECT_EQ(result, -1); + + // The freejoint should still be in parent_spec because deletion failed + mjsElement* found_joint = + mjs_findElement(parent_spec, mjOBJ_JOINT, "pre_child_freejoint"); + EXPECT_THAT(found_joint, NotNull()); + + mj_deleteSpec(child_spec); + mj_deleteSpec(parent_spec); +} + TEST_F(MujocoTest, ActivatePlugin) { mjSpec* spec = mj_makeSpec(); mjs_activatePlugin(spec, "mujoco.elasticity.cable"); From 025ba59fab44294e2bd90a05d7307b0a500192d3 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 10 Apr 2026 04:06:20 -0700 Subject: [PATCH 038/251] Implement sparse Jacobian time derivative PiperOrigin-RevId: 897609541 Change-Id: Ic86b6026cfc369c584d3edfe12a9a2fd14cc4357 --- src/engine/engine_core_smooth.c | 72 ++++++++++++++++++------- src/engine/engine_core_util.c | 86 ++++++++++++++++++++++++++++++ src/engine/engine_core_util.h | 6 +++ test/engine/engine_support_test.cc | 74 +++++++++++++++++++++++++ 4 files changed, 220 insertions(+), 18 deletions(-) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index ef4b3f7f..a0b4e3aa 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1113,6 +1113,8 @@ void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) { // allocate stack arrays mj_markStack(d); + int issparse = mj_isSparse(m); + int* chain = issparse ? mjSTACKALLOC(d, nv, int) : NULL; mjtNum* jac1 = mjSTACKALLOC(d, 3*nv, mjtNum); mjtNum* jac2 = mjSTACKALLOC(d, 3*nv, mjtNum); mjtNum* jacdif = mjSTACKALLOC(d, 3*nv, mjtNum); @@ -1178,30 +1180,64 @@ void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) { mju_addToScl3(dvel, dpnt, -dot); mju_scl3(dvel, dvel, norm > mjMINVAL ? 1/norm : 0); - // TODO(tassa ) write sparse branch, requires mj_jacDotSparse - // if (mj_isSparse(m)) { ... } + // sparse + if (issparse) { + // construct merged chain + int NV = mj_mergeChain(m, chain, wbody[0], wbody[1], /*flg_skipcommon=*/0); - // get endpoint JacobianDots, subtract - mj_jacDot(m, d, jac1, 0, wpnt, wbody[0]); - mj_jacDot(m, d, jac2, 0, wpnt+3, wbody[1]); - mju_sub(jacdif, jac2, jac1, 3*nv); + if (NV) { + // get endpoint JacobianDots, subtract + mj_jacDotSparse(m, d, jac1, 0, wpnt, wbody[0], NV, chain); + mj_jacDotSparse(m, d, jac2, 0, wpnt+3, wbody[1], NV, chain); + mju_sub(jacdif, jac2, jac1, 3*NV); - // chain rule, first term: Jdot += d/dt(jac2 - jac1) * dpnt - mju_mulMatTVec(tmp, jacdif, dpnt, 3, nv); + // chain rule, first term: Jdot += d/dt(jac2 - jac1) * dpnt + mju_mulMatTVec(tmp, jacdif, dpnt, 3, NV); - // add to existing - mju_addToScl(Jdot, tmp, 1/divisor, nv); + // scatter into dense output + for (int k=0; k < NV; k++) { + Jdot[chain[k]] += tmp[k] / divisor; + } - // get endpoint Jacobians, subtract - mj_jac(m, d, jac1, 0, wpnt, wbody[0]); - mj_jac(m, d, jac2, 0, wpnt+3, wbody[1]); - mju_sub(jacdif, jac2, jac1, 3*nv); + // get endpoint Jacobians, subtract + mj_jacSparse(m, d, jac1, 0, wpnt, wbody[0], NV, chain, /*flg_skipcommon=*/0); + mj_jacSparse(m, d, jac2, 0, wpnt+3, wbody[1], NV, chain, /*flg_skipcommon=*/0); + mju_sub(jacdif, jac2, jac1, 3*NV); - // chain rule, second term: Jdot += (jac2 - jac1) * d/dt(dpnt) - mju_mulMatTVec(tmp, jacdif, dvel, 3, nv); + // chain rule, second term: Jdot += (jac2 - jac1) * d/dt(dpnt) + mju_mulMatTVec(tmp, jacdif, dvel, 3, NV); - // add to existing - mju_addToScl(Jdot, tmp, 1/divisor, nv); + // scatter into dense output + for (int k=0; k < NV; k++) { + Jdot[chain[k]] += tmp[k] / divisor; + } + } + } + + // dense + else { + // get endpoint JacobianDots, subtract + mj_jacDot(m, d, jac1, 0, wpnt, wbody[0]); + mj_jacDot(m, d, jac2, 0, wpnt+3, wbody[1]); + mju_sub(jacdif, jac2, jac1, 3*nv); + + // chain rule, first term: Jdot += d/dt(jac2 - jac1) * dpnt + mju_mulMatTVec(tmp, jacdif, dpnt, 3, nv); + + // add to existing + mju_addToScl(Jdot, tmp, 1/divisor, nv); + + // get endpoint Jacobians, subtract + mj_jac(m, d, jac1, 0, wpnt, wbody[0]); + mj_jac(m, d, jac2, 0, wpnt+3, wbody[1]); + mju_sub(jacdif, jac2, jac1, 3*nv); + + // chain rule, second term: Jdot += (jac2 - jac1) * d/dt(dpnt) + mju_mulMatTVec(tmp, jacdif, dvel, 3, nv); + + // add to existing + mju_addToScl(Jdot, tmp, 1/divisor, nv); + } } // advance diff --git a/src/engine/engine_core_util.c b/src/engine/engine_core_util.c index 642dc896..c49f3ee5 100644 --- a/src/engine/engine_core_util.c +++ b/src/engine/engine_core_util.c @@ -660,6 +660,92 @@ void mj_jacDot(const mjModel* m, const mjData* d, } +// compute 3/6-by-NV sparse Jacobian time derivative of global point attached to given body +void mj_jacDotSparse(const mjModel* m, const mjData* d, + mjtNum* jacp, mjtNum* jacr, const mjtNum* point, int body, + int NV, const int* chain) { + mjtNum offset[3]; + mjtNum pvel[6]; + + // clear jacobians, compute offset and pvel if required + if (jacp) { + mju_zero(jacp, 3*NV); + const mjtNum* com = d->subtree_com+3*m->body_rootid[body]; + mju_sub3(offset, point, com); + mju_transformSpatial(pvel, d->cvel+6*body, 0, point, com, 0); + } + if (jacr) { + mju_zero(jacr, 3*NV); + } + + // skip fixed bodies + body = m->body_weldid[body]; + + // no movable body found: nothing to do + if (!body) { + return; + } + + // get last dof that affects this body + int da = m->body_dofadr[body] + m->body_dofnum[body] - 1; + + // start at end of chain (chain is in increasing order) + int ci = NV-1; + + // backward pass over dof ancestor chain + while (da >= 0) { + // find chain index for this dof + while (ci >= 0 && chain[ci] > da) { + ci--; + } + + // dof not in chain: SHOULD NOT OCCUR + if (ci < 0 || chain[ci] != da) { + mjERROR("dof index %d not found in chain", da); + } + + mjtNum cdof_dot[6]; + mji_copy6(cdof_dot, d->cdof_dot+6*da); + mjtNum* cdof = d->cdof+6*da; + + // check for quaternion + mjtJoint type = m->jnt_type[m->dof_jntid[da]]; + int dofadr = m->jnt_dofadr[m->dof_jntid[da]]; + int is_quat = type == mjJNT_BALL || (type == mjJNT_FREE && da >= dofadr + 3); + + // compute cdof_dot for quaternion (use current body cvel) + if (is_quat) { + mji_crossMotion(cdof_dot, d->cvel+6*m->dof_bodyid[da], cdof); + } + + // construct rotation jacobian + if (jacr) { + jacr[ci+0*NV] += cdof_dot[0]; + jacr[ci+1*NV] += cdof_dot[1]; + jacr[ci+2*NV] += cdof_dot[2]; + } + + // construct translation jacobian (correct for rotation) + if (jacp) { + // first correction term, account for varying cdof + mjtNum tmp1[3]; + mji_cross(tmp1, cdof_dot, offset); + + // second correction term, account for point translational velocity + mjtNum tmp2[3]; + mji_cross(tmp2, cdof, pvel + 3); + + jacp[ci+0*NV] += cdof_dot[3] + tmp1[0] + tmp2[0]; + jacp[ci+1*NV] += cdof_dot[4] + tmp1[1] + tmp2[1]; + jacp[ci+2*NV] += cdof_dot[5] + tmp1[2] + tmp2[2]; + } + + // advance to parent dof + da = m->dof_parentid[da]; + } +} + + // compute subtree angular momentum matrix void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body) { int nv = m->nv; diff --git a/src/engine/engine_core_util.h b/src/engine/engine_core_util.h index 98093d6f..39633ff0 100644 --- a/src/engine/engine_core_util.h +++ b/src/engine/engine_core_util.h @@ -18,6 +18,7 @@ #include #include #include +#include #ifdef __cplusplus extern "C" { @@ -85,6 +86,11 @@ void mj_jacSparseSimple(const mjModel* m, const mjData* d, mjtNum* jacdifp, mjtNum* jacdifr, const mjtNum* point, int body, int flg_second, int NV, int start); +// compute 3/6-by-NV sparse Jacobian time derivative of global point attached to given body +MJAPI void mj_jacDotSparse(const mjModel* m, const mjData* d, + mjtNum* jacp, mjtNum* jacr, const mjtNum* point, int body, + int NV, const int* chain); + // dense or sparse Jacobian difference for two body points: pos2 - pos1, global MJAPI int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain, int b1, int b2, const mjtNum pos1[3], const mjtNum pos2[3], diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index 8fb8502e..629517ae 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -17,6 +17,7 @@ #include "src/engine/engine_core_util.h" #include "src/engine/engine_support.h" +#include #include #include #include @@ -483,6 +484,79 @@ TEST_F(JacobianTest, JacDot) { } } +// compare mj_jacDotSparse with dense mj_jacDot +TEST_F(JacobianTest, JacDotSparse) { + for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) { + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + int nv = model->nv; + mjData* data = mj_makeData(model); + + // load keyframe if present, step for a bit + if (model->nkey) mj_resetDataKeyframe(model, data, 0); + while (data->time < 0.1) { + mj_step(model, data); + } + + // minimal call required for mj_jacDot outputs to be valid + mj_kinematics(model, data); + mj_comPos(model, data); + mj_comVel(model, data); + + // get bodyid and site position + int bodyid = mj_name2id(model, mjOBJ_BODY, "query"); + EXPECT_GT(bodyid, 0); + int siteid = mj_name2id(model, mjOBJ_SITE, "query"); + EXPECT_GT(siteid, -1); + mjtNum point[3]; + mju_copy3(point, data->site_xpos+3*siteid); + + // dense jacDot + vector jacp_dense(3*nv); + vector jacr_dense(3*nv); + mj_jacDot(model, data, jacp_dense.data(), jacr_dense.data(), point, bodyid); + + // compute body chain using public mjModel fields + vector chain(nv); + int NV = 0; + int weldbody = model->body_weldid[bodyid]; + if (weldbody) { + int da = model->body_dofadr[weldbody] + model->body_dofnum[weldbody] - 1; + while (da >= 0) { + chain[NV++] = da; + da = model->dof_parentid[da]; + } + std::reverse(chain.begin(), chain.begin() + NV); + } + EXPECT_GT(NV, 0); + + // sparse jacDot + vector jacp_sparse(3*NV); + vector jacr_sparse(3*NV); + mj_jacDotSparse(model, data, jacp_sparse.data(), jacr_sparse.data(), + point, bodyid, NV, chain.data()); + + // expand sparse to dense and compare + vector jacp_expanded(3*nv, 0); + vector jacr_expanded(3*nv, 0); + for (int ci = 0; ci < NV; ci++) { + int di = chain[ci]; + for (int r = 0; r < 3; r++) { + jacp_expanded[di+r*nv] = jacp_sparse[ci+r*NV]; + jacr_expanded[di+r*nv] = jacr_sparse[ci+r*NV]; + } + } + + // expect bitwise equality + EXPECT_EQ(jacp_expanded, jacp_dense); + EXPECT_EQ(jacr_expanded, jacr_dense); + + mj_deleteData(data); + mj_deleteModel(model); + } +} + using Name2idTest = MujocoTest; static constexpr char name2idTestingModel[] = R"( From f114ea803819cc3181a3b9d304c1936c330819f1 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 10 Apr 2026 04:57:18 -0700 Subject: [PATCH 039/251] Optimize `mj_tendonBias` by computing Jdot * qvel directly PiperOrigin-RevId: 897628283 Change-Id: Iee26a95d6aaa379730b89be14746308742111e2d --- src/engine/engine_core_smooth.c | 42 +++++++++----------------- src/engine/engine_core_smooth.h | 5 +-- test/engine/engine_core_smooth_test.cc | 9 +++--- 3 files changed, 22 insertions(+), 34 deletions(-) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index a0b4e3aa..2ae4d5b4 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -1093,22 +1093,20 @@ void mj_tendon(const mjModel* m, mjData* d) { } -// compute time derivative of dense tendon Jacobian for one tendon -void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) { +// return dot product of tendon Jacobian time derivative with vector +mjtNum mj_tendonDot(const mjModel* m, mjData* d, int id, const mjtNum* vec) { int nv = m->nv; + mjtNum res = 0; // tendon id is invalid: return if (id < 0 || id >= m->ntendon) { - return; + return 0; } - // clear output - mju_zero(Jdot, nv); - // fixed tendon has zero Jdot: return int adr = m->tendon_adr[id]; if (m->wrap_type[adr] == mjWRAP_JOINT) { - return; + return 0; } // allocate stack arrays @@ -1194,9 +1192,8 @@ void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) { // chain rule, first term: Jdot += d/dt(jac2 - jac1) * dpnt mju_mulMatTVec(tmp, jacdif, dpnt, 3, NV); - // scatter into dense output for (int k=0; k < NV; k++) { - Jdot[chain[k]] += tmp[k] / divisor; + res += (tmp[k] / divisor) * vec[chain[k]]; } // get endpoint Jacobians, subtract @@ -1207,9 +1204,8 @@ void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) { // chain rule, second term: Jdot += (jac2 - jac1) * d/dt(dpnt) mju_mulMatTVec(tmp, jacdif, dvel, 3, NV); - // scatter into dense output for (int k=0; k < NV; k++) { - Jdot[chain[k]] += tmp[k] / divisor; + res += (tmp[k] / divisor) * vec[chain[k]]; } } } @@ -1224,8 +1220,7 @@ void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) { // chain rule, first term: Jdot += d/dt(jac2 - jac1) * dpnt mju_mulMatTVec(tmp, jacdif, dpnt, 3, nv); - // add to existing - mju_addToScl(Jdot, tmp, 1/divisor, nv); + res += mju_dot(tmp, vec, nv) / divisor; // get endpoint Jacobians, subtract mj_jac(m, d, jac1, 0, wpnt, wbody[0]); @@ -1235,8 +1230,7 @@ void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) { // chain rule, second term: Jdot += (jac2 - jac1) * d/dt(dpnt) mju_mulMatTVec(tmp, jacdif, dvel, 3, nv); - // add to existing - mju_addToScl(Jdot, tmp, 1/divisor, nv); + res += mju_dot(tmp, vec, nv) / divisor; } } @@ -1245,6 +1239,7 @@ void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot) { } mj_freeStack(d); + return res; } @@ -2668,9 +2663,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { // add bias force due to tendon armature void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc) { int sleep_filter = mjENABLED(mjENBL_SLEEP) && d->ntree_awake < m->ntree; - int ntendon = m->ntendon, nv = m->nv; - mjtNum* ten_Jdot = NULL; - mj_markStack(d); + int ntendon = m->ntendon; // add bias term due to tendon armature for (int i=0; i < ntendon; i++) { @@ -2686,16 +2679,11 @@ void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc) { continue; } - // allocate if required - if (!ten_Jdot) { - ten_Jdot = mjSTACKALLOC(d, nv, mjtNum); - } - - // get dense d/dt(tendon Jacobian) for tendon i - mj_tendonDot(m, d, i, ten_Jdot); + // get d/dt(tendon Jacobian) dotted with qvel for tendon i + mjtNum dot = mj_tendonDot(m, d, i, d->qvel); // add bias term: qfrc += ten_J * armature * dot(ten_Jdot, qvel) - mjtNum coef = armature * mju_dot(ten_Jdot, d->qvel, nv); + mjtNum coef = armature * dot; if (coef) { // sparse @@ -2708,6 +2696,4 @@ void mj_tendonBias(const mjModel* m, mjData* d, mjtNum* qfrc) { } } } - - mj_freeStack(d); } diff --git a/src/engine/engine_core_smooth.h b/src/engine/engine_core_smooth.h index 6e18fe1a..258b625f 100644 --- a/src/engine/engine_core_smooth.h +++ b/src/engine/engine_core_smooth.h @@ -18,6 +18,7 @@ #include #include #include +#include #ifdef __cplusplus extern "C" { @@ -45,8 +46,8 @@ MJAPI void mj_flex(const mjModel* m, mjData* d); // compute tendon lengths, velocities and moment arms MJAPI void mj_tendon(const mjModel* m, mjData* d); -// compute time derivative of dense tendon Jacobian for one tendon -MJAPI void mj_tendonDot(const mjModel* m, mjData* d, int id, mjtNum* Jdot); +// return dot product of tendon Jacobian time derivative with vector +MJAPI mjtNum mj_tendonDot(const mjModel* m, mjData* d, int id, const mjtNum* vec); // compute actuator transmission lengths and moments MJAPI void mj_transmission(const mjModel* m, mjData* d); diff --git a/test/engine/engine_core_smooth_test.cc b/test/engine/engine_core_smooth_test.cc index a185f382..3120b328 100644 --- a/test/engine/engine_core_smooth_test.cc +++ b/test/engine/engine_core_smooth_test.cc @@ -191,10 +191,8 @@ TEST_F(CoreSmoothTest, TendonJdot) { mj_forward(m, d); - // get current J and Jdot for the tendon + // get current J for the tendon vector ten_J(d->ten_J, d->ten_J + nv); - vector ten_Jdot(nv, 0); - mj_tendonDot(m, d, 0, ten_Jdot.data()); // compute finite-differenced Jdot mjtNum h = MjTol(1e-7, 5e-4); @@ -206,7 +204,10 @@ TEST_F(CoreSmoothTest, TendonJdot) { mju_subFrom(ten_Jh.data(), ten_J.data(), nv); mju_scl(ten_Jh.data(), ten_Jh.data(), 1.0 / h, nv); - EXPECT_THAT(ten_Jdot, Pointwise(MjNear(1e-6, 2e-3), ten_Jh)); + // test dot product against finite differences + mjtNum dot = mj_tendonDot(m, d, 0, d->qvel); + mjtNum expected_dot = mju_dot(ten_Jh.data(), d->qvel, nv); + EXPECT_NEAR(dot, expected_dot, MjTol(1e-5, 2e-3)); } mj_deleteData(d); From bd4ee537b81dbbe40cbdf9cf37bf001e65615530 Mon Sep 17 00:00:00 2001 From: Tom Erez Date: Fri, 10 Apr 2026 20:25:39 +0000 Subject: [PATCH 040/251] Add protocols for authoring simulation environments. This is for preview only, we discourage users from using this in production code. PiperOrigin-RevId: 897857927 Change-Id: I82ce0a23465429e7025334e83ba00dfbe5085649 --- doc/SKILL.md | 876 ++++++++++++++++++ .../reaf/core/action_space_adapter.py | 43 + .../reaf/core/commands_processor.py | 91 ++ .../data_acquisition_and_control_layer.py | 170 ++++ .../reaf/core/default_discount_provider.py | 79 ++ .../core/default_observation_space_adapter.py | 231 +++++ src/experimental/reaf/core/device.py | 54 ++ .../reaf/core/device_coordinator.py | 87 ++ .../reaf/core/discount_provider.py | 61 ++ src/experimental/reaf/core/entity.py | 65 ++ src/experimental/reaf/core/environment.py | 490 ++++++++++ .../reaf/core/features_observer.py | 34 + .../reaf/core/features_producer.py | 56 ++ src/experimental/reaf/core/logger.py | 80 ++ .../reaf/core/numpy_mock_assertions.py | 98 ++ .../reaf/core/observation_space_adapter.py | 42 + .../core/pass_through_action_space_adapter.py | 55 ++ src/experimental/reaf/core/reward_provider.py | 292 ++++++ .../reaf/core/substep_commands_processor.py | 104 +++ .../core/substep_measurements_processor.py | 103 ++ .../reaf/core/task_logic_layer.py | 342 +++++++ .../reaf/core/termination_checker.py | 94 ++ src/experimental/reaf/core/trigger.py | 29 + .../reaf/core/zero_reward_provider.py | 48 + 24 files changed, 3624 insertions(+) create mode 100644 doc/SKILL.md create mode 100644 src/experimental/reaf/core/action_space_adapter.py create mode 100644 src/experimental/reaf/core/commands_processor.py create mode 100644 src/experimental/reaf/core/data_acquisition_and_control_layer.py create mode 100644 src/experimental/reaf/core/default_discount_provider.py create mode 100644 src/experimental/reaf/core/default_observation_space_adapter.py create mode 100644 src/experimental/reaf/core/device.py create mode 100644 src/experimental/reaf/core/device_coordinator.py create mode 100644 src/experimental/reaf/core/discount_provider.py create mode 100644 src/experimental/reaf/core/entity.py create mode 100644 src/experimental/reaf/core/environment.py create mode 100644 src/experimental/reaf/core/features_observer.py create mode 100644 src/experimental/reaf/core/features_producer.py create mode 100644 src/experimental/reaf/core/logger.py create mode 100644 src/experimental/reaf/core/numpy_mock_assertions.py create mode 100644 src/experimental/reaf/core/observation_space_adapter.py create mode 100644 src/experimental/reaf/core/pass_through_action_space_adapter.py create mode 100644 src/experimental/reaf/core/reward_provider.py create mode 100644 src/experimental/reaf/core/substep_commands_processor.py create mode 100644 src/experimental/reaf/core/substep_measurements_processor.py create mode 100644 src/experimental/reaf/core/task_logic_layer.py create mode 100644 src/experimental/reaf/core/termination_checker.py create mode 100644 src/experimental/reaf/core/trigger.py create mode 100644 src/experimental/reaf/core/zero_reward_provider.py diff --git a/doc/SKILL.md b/doc/SKILL.md new file mode 100644 index 00000000..ecd4f464 --- /dev/null +++ b/doc/SKILL.md @@ -0,0 +1,876 @@ +--- +name: mujoco-python +description: > + Build, manipulate, and simulate MuJoCo physics models using the Python + bindings (MjSpec, MjModel, MjData). Covers choosing between compute + backends (C++ for full features and noslip, MJWarp for GPU batch RL). Use when constructing scenes + programmatically via the spec API, compiling and stepping simulations, + reading sensor/body/geom data, attaching sub-models, composing specs with + prefixed names, using contact sensors for fixed-size observation spaces, + configuring collision filtering, offscreen rendering (context management, + cameras, depth/segmentation), or performing spatial math (quaternion, pose, + rotation conversions via mju_). Covers gotchas around compilation + lifecycle, named indexing vs bind, geom size semantics, camera conventions, + and orientation representations. +--- + +# MuJoCo Python Bindings + +## Compilation Lifecycle + +``` +MjSpec ──spec.compile()──▶ MjModel ──MjData(model)──▶ MjData + │ │ │ + │ (mutable blueprint) │ (compiled, mostly frozen) │ (simulation state) + │ │ │ + └── spec.recompile(m, d) ─────┴────────────────────────────┘ +``` + +1. **MjSpec** — mutable data structure you edit to define the simulation. +2. **`spec.compile()`** — produces `MjModel` + you create `MjData(model)`. + After this, changing the spec has **no effect** until you recompile. +3. **Most `MjModel` fields are unsafe to mutate.** Changing them requires + `spec.recompile(model, data)`, which returns **new** model and data objects + (preserving physics state for existing elements). + +```python +import mujoco + +spec = mujoco.MjSpec() +body = spec.worldbody.add_body(pos=[0, 0, 1]) +geom = body.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE, size=[0.1]) +body.add_freejoint() + +model = spec.compile() +data = mujoco.MjData(model) +mujoco.mj_forward(model, data) + +# Later: add another body, recompile keeping state +body2 = spec.worldbody.add_body(pos=[1, 0, 1]) +body2.add_geom(size=[0.1]) +body2.add_freejoint() +model, data = spec.recompile(model, data) # state preserved +``` + +> [!CAUTION] +> `recompile` returns **new** objects. Always reassign: `model, data = spec.recompile(model, data)`. + +### Loading and Serializing + +```python +spec = mujoco.MjSpec() # empty +spec = mujoco.MjSpec.from_string(xml_string) # from XML string +spec = mujoco.MjSpec.from_file('/path/to.xml') # from file +model = mujoco.MjModel.from_xml_string(xml) # direct to model (no spec) + +xml_out = spec.to_xml() # serialize back +``` + +### Compile Error Debugging + +Use the `.info` field on spec elements for traceability: + +```python +geom = spec.worldbody.add_geom() +geom.info = 'created at my_file.py:42' +spec.compile() # Error: "size 0 must be positive in geom\nElement name '', id 0, created at my_file.py:42" +``` + +--- + +## Compute Backends + +MuJoCo has two compute backends. **Choose early** — the backend determines +which features, solvers, and APIs are available. + +| | C++ (default) | MJWarp (NVIDIA GPU) | +|---|---|---| +| **Import** | `import mujoco` | `import mujoco_warp as mjw` | +| **Optimized for** | Latency (single scene) | Throughput (big batches) | +| **Hardware** | CPU | NVIDIA GPU | +| **Solvers** | All (Newton, CG, PGS, **noslip**) | All except PGS, **noslip**, islands | +| **Plugins** | ✅ All | SDF only | +| **Precision** | float64 | float32 | +| **Named access / bind** | ✅ | Via wrapper libraries or MJX `bind()` | +| **Contact sensors** | ✅ | ✅ | +| **Sparse Jacobians** | ✅ | ❌ (dense only) | +| **Batch rendering** | ❌ | ✅ (BVH ray tracing) | + +### When to use which + +- **C++ (default)**: Real-time control, model predictive control, interactive + visualization, any workflow needing full feature support (noslip solver, + PGS, islands, plugins, ellipsoidal fluid model, sparse Jacobians). Also the + only backend with native `bind()`. Use this unless you need massive + parallelism. (For MJWarp, named access is available via wrapper + libraries or MJX `bind()`.) + +- **MJWarp**: Reinforcement learning with large batch sizes on NVIDIA GPUs. + Scales better for contact-rich scenes and large meshes than the legacy + MJX-JAX backend. Not differentiable. May degrade for scenes beyond ~60 DoFs. + +> [!IMPORTANT] +> The `noslip` solver (post-constraint velocity correction for exact zero +> slip at contacts) is **only available in the C++ backend**. If your task +> requires accurate friction modeling without any tangential sliding at +> contacts, you must use C++. + +> [!WARNING] +> MJWarp uses **float32**, which can cause numerical differences vs C++ +> (float64). Solver convergence, small friction values, and long rollouts +> may be sensitive to this. If you see NaNs or instability on GPU, try +> increasing solver iterations or simplifying the model. + +--- + +## Building Models with MjSpec + +### Adding elements + +Most `add_*` methods accept keyword arguments matching MJCF XML attributes: + +```python +spec = mujoco.MjSpec() + +body = spec.worldbody.add_body(name='arm', pos=[0, 0, 1], quat=[1, 0, 0, 0]) +geom = body.add_geom( + name='arm_geom', + type=mujoco.mjtGeom.mjGEOM_CAPSULE, + size=[0.05, 0.3], + rgba=[1, 0, 0, 1], +) +joint = body.add_joint( + name='hinge1', + type=mujoco.mjtJoint.mjJNT_HINGE, + axis=[0, 1, 0], + range=[-1.57, 1.57], +) +site = body.add_site(name='sensor_site', pos=[0, 0, 0.3]) +cam = body.add_camera(name='arm_cam', pos=[0, -2, 0], xyaxes=[1,0,0, 0,0,1]) +``` + +### Orientation alternatives + +In addition to `quat`, you can specify orientation with `euler`, `axisangle`, +`xyaxes`, or `zaxis`. Only one can be set at a time: + +```python +body.add_geom(euler=[0, 90, 0]) # Euler angles (degrees by default) +body.add_geom(axisangle=[0, 1, 0, 1.57]) # axis + angle +body.add_geom(zaxis=[0, 1, 0]) # minimal rotation to align Z +body.add_geom(xyaxes=[1,0,0, 0,0,1]) # explicit X and Y axes +``` + +### Top-level elements + +Sensors, actuators, tendons, materials, textures, and meshes are added directly +to the spec (not to bodies): + +```python +spec.add_material(name='red', rgba=[1, 0, 0, 1]) +spec.add_actuator(name='motor', joint=joint.name, gear=[1, 0, 0, 0, 0, 0]) +spec.add_sensor( + name='joint_pos', + type=mujoco.mjtSensor.mjSENS_JOINTPOS, + objtype=mujoco.mjtObj.mjOBJ_JOINT, + objname=joint.name, +) +``` + +> [!IMPORTANT] +> Always use `element.name` (e.g., `joint.name`, `geom.name`, `site.name`) +> instead of hardcoded strings when referencing spec elements. This keeps +> references correct if the element is renamed or attached with a prefix. + +### Geom size semantics + +| Type | Size params | +| --------- | ----------------------------------------------- | +| sphere | `[radius]` | +| capsule | `[radius, half_length]` or `[radius]` + fromto | +| cylinder | `[radius, half_length]` or `[radius]` + fromto | +| box | `[half_x, half_y, half_z]` | +| ellipsoid | `[radius_x, radius_y, radius_z]` | +| plane | `[half_x, half_y, grid_spacing]` | + +> [!WARNING] +> Capsule/cylinder `size` changes meaning with `fromto`. Without `fromto`, +> `size=[radius, half_length]`. With `fromto`, `size=[radius]` only — the +> length is computed from the two endpoints. + +--- + +## Accessing Compiled Data: Named Access vs Bind + +There are **two** recommended ways to read/write compiled model and data fields. +**Prefer `bind`** when working with spec elements; use **named access** otherwise. + +### 1. Named Access (on MjModel / MjData) + +```python +model.geom('my_geom').size # → numpy view of geom_size for 'my_geom' +data.body('torso').xpos # → numpy view of body_xpos +data.joint('knee').qpos # → shape depends on joint type +data.actuator('motor').ctrl = 1.0 # writable view +``` + +Aliases: `joint` / `jnt`, `camera` / `cam`, `tendon` / `ten`, `material` / `mat`, +`texture` / `tex`, `equality` / `eq`, `keyframe` / `key`. + +> [!WARNING] +> Named access returns **views, not copies.** After `mj_step`, old references +> reflect new values. Use `.copy()` when logging: +> `positions.append(data.body('torso').xpos.copy())` + +### 2. Bind (bridges MjSpec elements → MjModel / MjData) + +`bind()` connects spec elements (or lists of them) to their compiled +counterparts. **Use `.set()` to write through bind:** + +```python +geom = spec.worldbody.add_geom(name='ball', size=[0.1], type=mujoco.mjtGeom.mjGEOM_SPHERE) +joint = body.add_joint(name='j1', type=mujoco.mjtJoint.mjJNT_HINGE) +model = spec.compile() +data = mujoco.MjData(model) +mujoco.mj_forward(model, data) + +# Reading via bind +model.bind(geom).size # → array([0.1, 0., 0.]) +data.bind(geom).xpos # → array([0., 0., 0.]) + +# Writing via bind — always use .set() +data.bind(joint).set('qpos', 1.5) # sets the joint's qpos + +# Bind a list of spec elements +joints = [spec.joint('j1'), spec.joint('j2')] +data.bind(joints).qpos # → concatenated array +data.bind(joints).set('qpos', np.array([0.5, 1.0])) # write to both +``` + +> [!CAUTION] +> The spec must match the compiled model. If you modify the spec after +> `compile()`, you must recompile before calling `bind()`, or you get: +> `ValueError: 'The mjSpec does not match mjModel. Please recompile the mjSpec.'` + +--- + +## Attachments: Composing Specs + +Attach child specs/bodies to parent specs via frames or sites: + +```python +parent = mujoco.MjSpec() +child = mujoco.MjSpec() +child_body = child.worldbody.add_body(name='arm') +child_body.add_geom(name='arm_geom', size=[0.05, 0.3], type=mujoco.mjtGeom.mjGEOM_CAPSULE) +child_body.add_joint(name='arm_joint', type=mujoco.mjtJoint.mjJNT_HINGE) + +frame = parent.worldbody.add_frame(pos=[0, 0, 1]) +frame.attach_body(child_body, prefix='left_') +# 'arm' → 'left_arm', 'arm_geom' → 'left_arm_geom', 'arm_joint' → 'left_arm_joint' + +# Or attach entire spec to a site +site = parent.worldbody.add_site(name='attach_point', pos=[0, 0, 2]) +parent.attach(child, site=site, prefix='right_', suffix='_v2') +``` + +> [!IMPORTANT] +> **Cross-spec references require a shared parent.** +> If you need to create an element (e.g., an equality constraint) that +> references elements from *two different child specs*, you must first +> attach both children to the same parent, then add the cross-referencing +> element to the **parent** spec using the final prefixed/suffixed names: + +```python +# Two robot arms, each defined as a separate spec +arm_spec = mujoco.MjSpec() +arm_body = arm_spec.worldbody.add_body(name='hand') +arm_body.add_geom(name='hand_geom', size=[0.05]) +wrist_joint = arm_body.add_joint(name='wrist', type=mujoco.mjtJoint.mjJNT_HINGE) + +# Attach both to the parent with different prefixes +parent = mujoco.MjSpec() +left_prefix, right_prefix = 'left_', 'right_' + +frame_l = parent.worldbody.add_frame(pos=[-0.5, 0, 1]) +frame_l.attach_body(arm_body, prefix=left_prefix) # left_wrist, left_hand, ... + +frame_r = parent.worldbody.add_frame(pos=[0.5, 0, 1]) +frame_r.attach_body(arm_body, prefix=right_prefix) # right_wrist, right_hand, ... + +# NOW add a constraint linking both arms — look up the prefixed joints +# from the parent spec, don't hardcode the names +left_wrist = parent.joint(f'{left_prefix}{wrist_joint.name}') +right_wrist = parent.joint(f'{right_prefix}{wrist_joint.name}') +parent.add_equality(type=mujoco.mjtEq.mjEQ_JOINT, + name1=left_wrist.name, name2=right_wrist.name) +model = parent.compile() +``` + +### Attachment Transforms + +When attaching to a site or frame, the child body's position is transformed +relative to the parent's attachment point. Attachment also handles unit +conversion (degrees vs radians) between parent and child specs automatically. + +### Assets Get Renamed Too + +Prefix/suffix changes apply to asset filenames: +```python +child.assets = {'mesh.obj': data} +parent.attach(child, prefix='robot_') +# Asset key becomes 'robot_mesh.obj' in parent +``` + +--- + +## Cameras + +### Orientation + +MuJoCo cameras look down the **negative Z axis**. The camera frame is: +- **-Z** → forward (viewing direction) +- **+X** → right +- **+Y** → up + +To point a camera downward (looking at the ground), set its Z axis to `[0, 0, 1]`: + +```python +body.add_camera( + name='overhead', + xyaxes=[1, 0, 0, 0, 1, 0], # x=[1,0,0], y=[0,1,0] → z=[0,0,1] → looks DOWN (-z) + pos=[0, 0, 5], +) +``` + +### Geom Group Visibility + +Each camera/viewer has 6 geom groups (0–5). Default visibility: + +| Group | Default Visible | Typical Use | +|-------|----------------|-------------| +| 0 | ✅ Yes | Standard geoms (default group for new geoms) | +| 1 | ✅ Yes | Secondary visual geoms | +| 2 | ✅ Yes | Tertiary visual geoms | +| 3 | ❌ No | Collision-only or debug geoms | +| 4 | ❌ No | Hidden geoms | +| 5 | ❌ No | Hidden geoms | + +A newly created geom is in **group 0** by default. Toggle visibility at runtime +via `mjvOption.geomgroup[i]`. The same 3-on/3-off default applies to sites, +joints, tendons, actuators, flexes, and skins. + +--- + +## Contacts: Use Sensors, Not the Contact Array + +### The problem with `data.contact` + +`data.contact` is a **variable-length** array that changes size every timestep +depending on what's colliding. Iterating over it directly is fragile and +**incompatible with learning-based agents** and fixed-size observation spaces. + +```python +# ❌ WRONG — don't iterate data.contact for reward/observation logic +for c in data.contact: + if c.geom1 == target_geom_id: + force = ... # fragile, variable-length, non-deterministic order +``` + +> [!CAUTION] +> Never iterate `data.contact` to build observations or compute rewards. +> The array's length and ordering can change between timesteps and even +> between MuJoCo versions. Use **contact sensors** instead. + +### Contact sensors: fixed-size, declarative contact queries + +A `` sensor selects contacts via declarative matching criteria, reduces +them to a fixed number of slots, and extracts requested data fields into +`data.sensordata` — always the same size, every timestep. + +The pipeline has three stages: +1. **Matching** — filter contacts by geom, body, subtree, or site volume +2. **Reduction** — keep the top `num` contacts (by order, min distance, max force, or net force) +3. **Extraction** — copy requested fields (`found`, `force`, `torque`, `dist`, `pos`, `normal`, `tangent`) + +### Example: detect contact force between a gripper and an object + +```python +import mujoco +import numpy as np + +spec = mujoco.MjSpec() + +# Build a simple scene: floor + falling object +floor = spec.worldbody.add_geom( + name='floor', type=mujoco.mjtGeom.mjGEOM_PLANE, size=[1, 1, 0.01] +) +obj_body = spec.worldbody.add_body(name='obj', pos=[0, 0, 0.5]) +obj_body.add_freejoint() +obj_geom = obj_body.add_geom( + name='obj_geom', type=mujoco.mjtGeom.mjGEOM_SPHERE, + size=[0.05], mass=0.1, +) + +# Add a contact sensor: report force for contacts involving obj_geom +contact_sensor = spec.add_sensor( + name='obj_contact', + type=mujoco.mjtSensor.mjSENS_CONTACT, + # Match any contact involving this geom — use .name, not a literal string: + objname=obj_geom.name, objtype=mujoco.mjtObj.mjOBJ_GEOM, +) + +model = spec.compile() +data = mujoco.MjData(model) + +# Step the simulation until the object lands +mujoco.mj_step(model, data, nstep=500) +mujoco.mj_forward(model, data) + +# Read the contact sensor via bind — always fixed-size in data.sensordata +contact_data = data.bind(contact_sensor).sensordata +print(f'Contact sensor output: {contact_data}') +``` + +### XML-based contact sensor (common pattern) + +When loading from XML, contact sensors are even cleaner: + +```xml + + + + + + + +``` + +The output size is deterministic: `num × size(data fields)`. For `"found force +normal"` with `num=3`, you get 3 × (1+3+3) = 21 numbers every timestep, padded +with zeros if fewer contacts match. + +### Touch sensor: simpler alternative for scalar normal force + +If you only need a scalar "how hard is something pressing on this site", use a +`touch` sensor instead: + +```python +site = body.add_site(name='fingertip', pos=[0, 0, 0.05], size=[0.02]) +spec.add_sensor( + name='fingertip_touch', + type=mujoco.mjtSensor.mjSENS_TOUCH, + objname=site.name, objtype=mujoco.mjtObj.mjOBJ_SITE, +) +``` + +The touch sensor sums normal contact forces within the site volume — one scalar +output, always present in `sensordata`. + +--- + +## Spatial Math Utilities (mju_) + +MuJoCo ships a library of spatial computation functions under the `mju_` +namespace — quaternion algebra, rotation conversions, pose composition, and +coordinate transforms. **Always check for an existing `mju_` function before +implementing spatial math from scratch.** For basic vector arithmetic (add, +subtract, dot product, norm), just use NumPy/JAX/Torch directly. + +### Quaternion Operations + +```python +res = np.zeros(3) +mujoco.mju_rotVecQuat(res, vec, quat) # rotate vector by quaternion + +quat = np.zeros(4) +mujoco.mju_mat2Quat(quat, mat3x3) # 3x3 rotation matrix → quaternion +mujoco.mju_quat2Mat(mat, quat) # quaternion → 3x3 matrix +mujoco.mju_axisAngle2Quat(quat, axis, angle) # axis-angle → quaternion +mujoco.mju_euler2Quat(quat, euler, 'xyz') # Euler angles → quaternion +mujoco.mju_mulQuat(res, q1, q2) # multiply quaternions +mujoco.mju_negQuat(res, quat) # conjugate +mujoco.mju_quatZ2Vec(quat, vec) # quat that rotates z-axis to vec +mujoco.mju_quatIntegrate(quat, vel, scale) # integrate quat with angular velocity +``` + +> [!TIP] +> `mju_quatZ2Vec` is particularly useful: given a target direction vector, it +> returns the quaternion that rotates the Z-axis to point in that direction. + +### Pose Operations + +```python +mujoco.mju_mulPose(pos_res, quat_res, pos1, quat1, pos2, quat2) # compose poses +mujoco.mju_negPose(pos_res, quat_res, pos, quat) # invert pose +mujoco.mju_trnVecPose(res, pos, quat, vec) # transform vector by pose +``` + +--- + +## Common Gotchas + +### 1. Computed fields are read-only + +`data.xpos`, `data.xmat`, `data.xquat`, `data.geom_xpos` are **output** fields +computed by `mj_forward()`. You cannot assign to them directly. Instead, modify +input fields (`data.qpos`, `data.qvel`, `data.ctrl`) and call `mj_forward()` or +`mj_step()`. + +### 2. Duplicate names are forbidden + +```python +spec.add_material(name='yellow') +spec.add_material(name='yellow') # ValueError: "repeated name 'yellow' in material" +``` + +Names must be unique within each element type. + +### 3. Orientation keywords are mutually exclusive + +```python +body.add_geom(axisangle=[1, 0, 0, 1.57], euler=[0, 0, 0]) +# ValueError: 'Only one of: axisangle, xyaxes, zaxis, or euler can be set.' +``` + +Pick one orientation representation. Quaternion (`quat`) is the native format. + +### 4. `size` must be positive for geoms + +A geom with `size[0] == 0` will fail compilation. Always set at least +`size=[radius]` for spheres/capsules, or `size=[hx, hy, hz]` for boxes. + +### 5. `mj_step` with `nstep` repeats the same control + +```python +mujoco.mj_step(model, data, nstep=100) # 100 steps, same ctrl each step +``` + +This is much faster than a Python loop and is fine for passive simulation or +constant-control scenarios. But if you need to update `data.ctrl` between steps, +you must step one at a time. + +### 6. Euler sequence matters + +`mju_euler2Quat` takes a 3-character sequence string. Lowercase = intrinsic +rotations, uppercase = extrinsic: + +```python +mujoco.mju_euler2Quat(quat, [roll, pitch, yaw], 'xyz') # intrinsic x-y-z +mujoco.mju_euler2Quat(quat, [roll, pitch, yaw], 'XYZ') # extrinsic X-Y-Z +``` + +The sequence must be exactly 3 characters from `xyzXYZ`. + +### 7. `copy()` vs view semantics + +NumPy arrays from MjModel/MjData are **views** into C memory. `mj_step` changes +them in-place. Always `.copy()` when storing values for later comparison. + +### 8. Default class handling + +```python +main = spec.default # global default class (always named 'main') +child_class = spec.add_default('high_friction', main) +child_class.geom.friction = [1.5, 0.005, 0.0001] + +geom = body.add_geom(child_class) # use specific default class +geom = body.add_geom() # uses 'main' class implicitly +``` + +### 9. Gravity is -Z by default + +MuJoCo convention: **+Z is up**, gravity is `[0, 0, -9.81]`. The viewer and +all built-in models assume this. Don't fight it — orient your scene accordingly. + +### 10. Capsule/cylinder size with and without fromto + +```python +# With explicit pos/quat: size = [radius, half_length] +body.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE, size=[0.05, 0.3]) + +# With fromto: size = [radius] only — length is inferred from endpoints +body.add_geom( + type=mujoco.mjtGeom.mjGEOM_CAPSULE, + size=[0.05], + fromto=[0, 0, 0, 0, 0, 0.6], +) +``` + +### 11. Collision filtering with contype/conaffinity + +Two geoms collide only if `(g1.contype & g2.conaffinity) || (g2.contype & g1.conaffinity)`. +By default both are `1`, so everything collides with everything. + +```python +# Visual-only geom: set contype=0, conaffinity=0 to disable collisions +body.add_geom(size=[0.1], contype=0, conaffinity=0, group=1) + +# Separate collision groups using bitmasks: +robot_geom = body.add_geom(size=[0.05], contype=1, conaffinity=2) +tool_geom = body.add_geom(size=[0.03], contype=2, conaffinity=1) +# Robot and tool collide (1&1=0, but 2&2=0… wait): +# contype=1 & conaffinity=1 → collide; contype=2 & conaffinity=2 → collide +``` + +> [!TIP] +> **`condim` and `friction` interact.** Each geom has `friction=[tangential, torsional, rolling]` +> (default `[1, 0.005, 0.0001]`). The `condim` value controls which friction coefficients are +> *active* in a contact: +> +> | condim | Active friction | Geom `friction` indices used | +> |--------|----------------|------------------------------| +> | 1 | None (frictionless, normal force only) | — | +> | 3 | Tangential (opposes sliding) | `friction[0]` | +> | 4 | Tangential + torsional (opposes sliding and twisting around contact normal) | `friction[0:2]` | +> | 6 | Tangential + torsional + rolling (also opposes rolling around tangent axes) | `friction[0:3]` | +> +> Torsional friction models a surface contact patch resisting twist — useful for soft fingers. +> Rolling friction dissipates energy from local deformations — useful for stopping balls from rolling +> forever. Both torsional and rolling coefficients have **units of length** (roughly the contact +> patch diameter or deformation depth). +> +> ```python +> # A soft finger pad: enable torsional friction for stable grasping +> finger_geom = body.add_geom( +> type=mujoco.mjtGeom.mjGEOM_CAPSULE, +> size=[0.01, 0.02], +> condim=4, +> friction=[1.0, 0.01, 0.0001], # tangential=1.0, torsional=0.01 +> ) +> +> # A ball that should stop rolling on a surface +> ball_geom = body.add_geom( +> type=mujoco.mjtGeom.mjGEOM_SPHERE, +> size=[0.05], +> condim=6, +> friction=[0.8, 0.005, 0.002], # tangential=0.8, torsional=0.005, rolling=0.002 +> ) +> ``` + +--- + +## Offscreen Rendering + +Offscreen rendering produces images (RGB, depth, segmentation) without a +display. It requires an OpenGL context — MuJoCo auto-detects the best +available backend (EGL on headless Linux, GLFW on desktop, OSMesa as +fallback). + +### The `Renderer` class + +`mujoco.Renderer` wraps GL context creation, scene management, and buffer +readback. **Always use it as a context manager** to ensure GPU resources are +freed: + +```python +import mujoco +import numpy as np + +# Define a camera in the spec and keep a reference +overhead_cam = spec.worldbody.add_camera( + name='overhead', + pos=[0, 0, 3], + quat=[0.707, 0.707, 0, 0], # looking down + fovy=60, +) + +model = spec.compile() +data = mujoco.MjData(model) + +# Create renderer — width/height must not exceed offscreen buffer (see below) +with mujoco.Renderer(model, height=480, width=640) as renderer: + mujoco.mj_forward(model, data) + + # Use the spec element's .name — never a literal string + renderer.update_scene(data, camera=overhead_cam.name) + rgb = renderer.render() # → np.ndarray (H, W, 3), dtype=uint8 + + # Depth rendering + renderer.enable_depth_rendering() + renderer.update_scene(data, camera=overhead_cam.name) + depth = renderer.render() # → np.ndarray (H, W), dtype=float32 (meters) + renderer.disable_depth_rendering() + + # Segmentation rendering + renderer.enable_segmentation_rendering() + renderer.update_scene(data, camera=overhead_cam.name) + seg = renderer.render() # → np.ndarray (H, W, 2), dtype=int32 + # seg[:,:,0] = object ID, seg[:,:,1] = object type; background = (-1, -1) + renderer.disable_segmentation_rendering() +``` + +> [!WARNING] +> Forgetting to close the renderer (or not using `with`) leaks GPU memory and +> GL contexts. In loops, create the renderer **once** outside the loop. + +### What the `Renderer` holds internally + +When you create `mujoco.Renderer(model, height, width)`, it allocates three +internal objects that must be freed together: + +1. **`GLContext`** — an offscreen OpenGL context (EGL, GLFW, or OSMesa, + auto-detected). Created with the requested `width × height`. +2. **`MjrContext`** — MuJoCo's GPU rendering resources (shaders, textures, + framebuffers), bound to the GLContext. Set to the offscreen framebuffer. +3. **`MjvScene`** — geometry buffer holding the scene snapshot passed to the + GPU each frame. + +The context manager (`with Renderer(...) as r:`) calls `r.close()` on exit, +which frees the MjrContext first and then the GLContext — **order matters**. +If you use the renderer without `with`, call `renderer.close()` manually. + +> [!CAUTION] +> Internally, `MjrContext.free()` must be called **before** `GLContext.free()`. +> Reversing the order leaks GPU resources or segfaults. The `Renderer` class +> handles this automatically — prefer it over manual context management. + +### Offscreen framebuffer size + +The renderer cannot exceed the offscreen buffer dimensions. The defaults are +640×480. Set larger buffers **before compilation** via `spec.visual`: + +```python +spec.visual.global_.offwidth = 1920 +spec.visual.global_.offheight = 1080 +model = spec.compile() + +# Now you can render up to 1920×1080 +with mujoco.Renderer(model, height=1080, width=1920) as renderer: + ... +``` + +> [!IMPORTANT] +> Increasing offscreen buffer size consumes GPU memory. For batch rendering +> of many cameras, keep the per-frame resolution modest. + +### Cameras + +MuJoCo has two camera systems: **fixed cameras** defined in the model, and +the **free camera** for interactive viewing. + +#### Defining cameras in MjSpec + +Always store the return value of `add_camera` and use its `.name` or `.id` +to reference the camera later — never hardcode literal strings: + +```python +# Fixed camera on worldbody — good for evaluation/recording +overhead_cam = spec.worldbody.add_camera( + name='overhead', + pos=[0, 0, 3], + quat=[0.707, 0.707, 0, 0], + fovy=60, +) + +# Camera attached to a body — moves with the body +wrist_cam = wrist_body.add_camera( + name='wrist_cam', + pos=[0.05, 0, 0], + xyaxes=[0, -1, 0, 0, 0, -1], + fovy=90, +) +``` + +#### Selecting a camera for rendering + +`update_scene` accepts a camera **name** (str), **id** (int), or an +`MjvCamera` object. Always derive from the spec element: + +```python +# By name via spec element (recommended — survives recompilation) +renderer.update_scene(data, camera=overhead_cam.name) + +# By id via spec element (after compile; matches model.cam_* arrays) +renderer.update_scene(data, camera=overhead_cam.id) + +# Free camera (default) — no camera argument needed +renderer.update_scene(data) + +# Custom free camera with explicit lookat/distance/angles +cam = mujoco.MjvCamera() +cam.type = mujoco.mjtCamera.mjCAMERA_FREE +cam.lookat[:] = [0, 0, 0.5] +cam.distance = 3.0 +cam.azimuth = 135 +cam.elevation = -25 +renderer.update_scene(data, camera=cam) +``` + +#### Camera properties reference + +| Property | Type | Description | +|----------|------|-------------| +| `pos` | `real(3)` | Position in parent body frame | +| `quat` | `real(4)` | Orientation quaternion (w, x, y, z) | +| `xyaxes` | `real(6)` | Alternative orientation: `[x_axis(3), y_axis(3)]` | +| `fovy` | `real` | Vertical field of view (degrees, default 45) | +| `resolution` | `int(2)` | Sensor resolution — only for camera-based sensors | +| `targetbody` | `str` | Track this body (camera always looks at it) | +| `mode` | `str` | `"fixed"`, `"track"`, `"trackcom"`, `"targetbody"`, `"targetbodycom"` | + +### Scene options + +Control what is visualized via `MjvOption`: + +```python +scene_option = mujoco.MjvOption() +# geomgroup is a bool array indexed by group number (0–5). +# Each geom's `group` attribute (default 0) assigns it to a group. +# Toggle visibility of each group: +scene_option.geomgroup[:] = False # hide all groups +scene_option.geomgroup[0] = True # show group 0 (e.g. ground plane) +scene_option.geomgroup[3] = True # show group 3 (e.g. visualization geoms) + +# Toggle rendering flags +scene_option.flags[mujoco.mjtVisFlag.mjVIS_CONTACTFORCE] = True +scene_option.flags[mujoco.mjtVisFlag.mjVIS_JOINT] = True + +renderer.update_scene(data, camera=overhead_cam.name, scene_option=scene_option) +``` + +### Filament backend (experimental) + +MuJoCo's default renderer uses OpenGL. An alternative **Filament** backend +(Vulkan-based) is available experimentally and provides higher-quality +rendering. Filament does **not** require vertical flip (`np.flipud` is a +no-op). It is selected via build flags — see the MuJoCo Filament +[source](../src/experimental/filament) for details. + +--- + +## Key References + +### Documentation + +| Document | Description | +|----------|-------------| +| [XMLreference.rst](XMLreference.rst) | Complete MJCF XML element and attribute reference | +| [python.rst](python.rst) | Python bindings API: named access, bind, enums, callbacks | +| [modeling.rst](modeling.rst) | MJCF modeling guide: coordinate frames, defaults, attachments | +| [simulation.rst](programming/simulation.rst) | Simulation loop, state, forward/inverse dynamics | +| [modeledit.rst](programming/modeledit.rst) | Procedural model editing with MjSpec | +| [visualization.rst](programming/visualization.rst) | Rendering, cameras, scene management | +| [APIfunctions.rst](APIreference/APIfunctions.rst) | C API function reference (mj_, mju_, mjv_, mjr_) | +| [APItypes.rst](APIreference/APItypes.rst) | All MuJoCo structs and enums | + +### Test Files (Executable Examples) + +| Test file | Key patterns demonstrated | +|-----------|--------------------------| +| [specs_test.py](../../py/mujoco/specs_test.py) | MjSpec API: compile, recompile, attach, bind, defaults, delete, actuator shortcuts | +| [bindings_test.py](../../py/mujoco/bindings_test.py) | Named indexing, mju_ functions, copy/pickle, contacts, mj_step | +| [support_test.py](../../py/mujoco/mjx/_src/support_test.py) | MJX bind `.set()` pattern, JAX functional updates | + +### Source Code + +| File | Description | +|------|-------------| +| [mujoco.h](../include/mujoco.h) | Main C API header with all mju_ function signatures | +| [XMLschema.rst](XMLschema.rst) | Schema-level XML structure documentation | diff --git a/src/experimental/reaf/core/action_space_adapter.py b/src/experimental/reaf/core/action_space_adapter.py new file mode 100644 index 00000000..014e02eb --- /dev/null +++ b/src/experimental/reaf/core/action_space_adapter.py @@ -0,0 +1,43 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Adapts environment action into suitable commands format accepted by REAF.""" + +import abc +from collections.abc import Mapping + +from gdm_robotics.interfaces import types as gdmr_types + + +class ActionSpaceAdapter(abc.ABC): + """Adapts environment action into suitable commands format accepted by REAF. + + Implementations of this interface are responsible for converting the more + generic action accepted by the environment (e.g. a flat numpy array) into the + more constraining format accepted as commands by REAF, i.e. a dictionary of + string to tensors. + """ + + @abc.abstractmethod + def commands_from_environment_action( + self, environment_action: gdmr_types.ActionType + ) -> Mapping[str, gdmr_types.ArrayType]: + """Converts the environment action into commands accepted by REAF.""" + + @abc.abstractmethod + def action_spec(self) -> gdmr_types.ActionSpec: + """Returns the action spec exposed by the environment.""" + + @abc.abstractmethod + def task_commands_keys(self) -> set[str]: + """Returns the keys for the commands exposed to the task layer.""" diff --git a/src/experimental/reaf/core/commands_processor.py b/src/experimental/reaf/core/commands_processor.py new file mode 100644 index 00000000..4cc12b4a --- /dev/null +++ b/src/experimental/reaf/core/commands_processor.py @@ -0,0 +1,91 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Abstract class for commands manipulation in the task logic layer.""" + +import abc +from collections.abc import Mapping + +from gdm_robotics.interfaces import types as gdmr_types + + +class CommandsProcessor(abc.ABC): + """Perform commands manipulation. + + The following describes the processing pipeline starting from the top (closer + to the policy) to the bottom (interfacing with the DACL commands spec). + + Assume that we have two processing units: + Processor 1) has a consumed_commands_spec for two keys: "p1/c1" and "p1/c2". + Its produced_commands_keys are "p2/c1". + Processor 2) has a consumed_commands_spec for "p2/c1". Its + produced_commands_keys are "p3/c1" and "p3/c2". + + Specs are propagated starting from the bottom: + 1) In this example assume that the DACL exposes "p3/c1", "p3/c2" and "p3/c3". + 2) Processor 2) returns ("p3/c1", "p3/c2") from input "p2/c1". This means that + the global commands spec exposed at this level is "p2/c1" and the + unprocessed "p3/c3". + 3) Processor 1) returns "p2/c1" from input ("p1/c1", "p1/c2"). By applying the + same transformation rule, we can obtain the final commands spec exposed by + the full processing pipeline: "p1/c1", "p1/c2" and "p3/c3". + + "p1/c1" "p1/c2" "p3/c3" + | | | + ----------------- | + | P1 | | + ----------------- | + | "p2/c1" | + ----------------- | + | P2 | | + ----------------- | + | "p3/c1" | "p3/c2" | + | | | + ------------------------------------ + | DACL | + ------------------------------------ + """ + + @property + @abc.abstractmethod + def name(self) -> str: + """Returns a unique string identifier for this object.""" + + @abc.abstractmethod + def process_commands( + self, consumed_commands: Mapping[str, gdmr_types.ArrayType] + ) -> Mapping[str, gdmr_types.ArrayType]: + """Processes the commands and returns a new modified version of it. + + Args: + consumed_commands: the commands up in the processing chain (or provided by + the Environment) that are required by this processor, i.e. with keys + specified by `consumed_commands_spec`. + + Returns the new commands. Note that the data in consumed_commands is removed + from the global commands dictionary. If users want to keep some of the + elements it is their responsibility to retain them in the output + dictionary. + """ + + @abc.abstractmethod + def consumed_commands_spec(self) -> Mapping[str, gdmr_types.AnyArraySpec]: + """Spec of the commands consumed by this processor.""" + + @abc.abstractmethod + def produced_commands_keys(self) -> set[str]: + """Keys of the commands produced by this processor.""" + + def reset(self) -> None: + """Resets the internal state of the command processor.""" + ... diff --git a/src/experimental/reaf/core/data_acquisition_and_control_layer.py b/src/experimental/reaf/core/data_acquisition_and_control_layer.py new file mode 100644 index 00000000..e8fcc8ca --- /dev/null +++ b/src/experimental/reaf/core/data_acquisition_and_control_layer.py @@ -0,0 +1,170 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""REAF data acquisition and control layer to interface with the robotic setup.""" + +from collections.abc import Iterable, Mapping + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types +from reaf.core import device as reaf_device +from reaf.core import device_coordinator as reaf_coordinator +from reaf.core import trigger + + +class DataAcquisitionAndControlLayer: + """REAF data acquisition and control layer. + + The DACL is responsible to provide an interface for the robotic setup. + """ + + def __init__( + self, + *, + device_coordinator: reaf_coordinator.DeviceCoordinator, + commands_trigger: trigger.Trigger | None, + measurements_trigger: trigger.Trigger | None, + ): + """Initializes the DataAcquisitionAndControlLayer. + + Args: + device_coordinator: The coordinator representing a specific robotic setup. + Note that callers need to explicitly initialize and finalise the + coordinator. + commands_trigger: A trigger to unblock processing commands during a call + to `step`. + measurements_trigger: A trigger to unblock processing measurements during + a call to `step`. + """ + self._coordinator = device_coordinator + self._devices = self._coordinator.get_devices() + # The following checks that names of the devices are unique and their keys + # are "mergeable". + self._check_device_names_and_keys(self._devices) + + self._commands_trigger = commands_trigger + self._measurements_trigger = measurements_trigger + + # Create a map of supported commands keys for each Device. + self._commands_for_device = { + device.name: device.commands_spec().keys() for device in self._devices + } + + def begin_stepping(self) -> Mapping[str, gdmr_types.ArrayType]: + """Begins stepping the DACL and returns the current measurements.""" + self._coordinator.on_begin_stepping() + + # Wait for the first trigger to happen before collecting the measurements. + if self._measurements_trigger is not None: + self._measurements_trigger.wait_for_event() + return self._get_measurements() + + def end_stepping(self) -> None: + """Ends stepping the data acquisition and control layer.""" + self._coordinator.on_end_stepping() + + def _set_commands(self, commands: Mapping[str, gdmr_types.ArrayType]) -> None: + """Sets the commands of the data acquisition and control layer.""" + self._coordinator.before_set_commands() + for device in self._devices: + device_commands = { + k: v + for k, v in commands.items() + if k in self._commands_for_device[device.name] + } + device.set_commands(device_commands) + self._coordinator.after_set_commands() + + def _get_measurements(self) -> Mapping[str, gdmr_types.ArrayType]: + """Gets the measurements of the data acquisition and control layer.""" + measurements = {} + self._coordinator.before_get_measurements() + for device in self._devices: + measurements.update(device.get_measurements()) + + return measurements + + def step( + self, commands: Mapping[str, gdmr_types.ArrayType] + ) -> Mapping[str, gdmr_types.ArrayType]: + """Steps the data acquisition and control layer.""" + if self._commands_trigger is not None: + self._commands_trigger.wait_for_event() + self._set_commands(commands) + + if self._measurements_trigger is not None: + self._measurements_trigger.wait_for_event() + return self._get_measurements() + + def commands_spec(self) -> Mapping[str, gdmr_types.AnyArraySpec]: + """Returns the specs for the commands.""" + spec = {} + for device in self._devices: + spec.update(device.commands_spec()) + return spec + + def measurements_spec(self) -> Mapping[str, specs.Array]: + """Returns the specs for the measurements.""" + spec = {} + for device in self._devices: + spec.update(device.measurements_spec()) + return spec + + @property + def device_coordinator(self) -> reaf_coordinator.DeviceCoordinator: + return self._coordinator + + def _check_keys_have_been_formatted_correctly( + self, current_key_set: Iterable[str] + ) -> None: + """Check that keys haven't been left unformatted.""" + for key in current_key_set: + if key.find("{}") != -1: + raise ValueError( + "Keys should not contain '{}'. Did you mean to use format()?" + ) + + def _check_device_names_and_keys( + self, devices: Iterable[reaf_device.Device] + ) -> None: + """Raises error if device names are not unique or keys are not exclusive.""" + # Check names first. + all_names = [device.name for device in devices] + unique_names = set(all_names) + if len(unique_names) != len(all_names): + raise RuntimeError(f"Duplicate names when checking devices: {all_names}") + + # Check commands. + devices = tuple(devices) + current_specs = set() + for device in devices: + device_keys = device.commands_spec().keys() + self._check_keys_have_been_formatted_correctly(device_keys) + if not current_specs.isdisjoint(device_keys): + raise RuntimeError( + f"Duplicate keys when checking device {device.name}:" + f" {current_specs.intersection(device_keys)}" + ) + current_specs.update(device_keys) + + # Check measurements. + current_specs = set() + for device in devices: + device_keys = device.measurements_spec().keys() + self._check_keys_have_been_formatted_correctly(device_keys) + if not current_specs.isdisjoint(device_keys): + raise RuntimeError( + f"Duplicate keys when checking device {device.name}:" + f" {current_specs.intersection(device_keys)}" + ) + current_specs.update(device_keys) diff --git a/src/experimental/reaf/core/default_discount_provider.py b/src/experimental/reaf/core/default_discount_provider.py new file mode 100644 index 00000000..ff216b7c --- /dev/null +++ b/src/experimental/reaf/core/default_discount_provider.py @@ -0,0 +1,79 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Computes a constant discount given the termination state. + +This provider returns a discount of 0.0 in case of termination and 1.0 +otherwise (i.e. for truncation and not termination). + +It is usually safe to use this discount provider for environments that return +strictly positive rewards. +""" + +from collections.abc import Mapping + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types +import numpy as np +from reaf.core import discount_provider +from reaf.core import termination_checker +import tree + + +class DefaultDiscountProvider(discount_provider.DiscountProvider): + """Computes a constant discount given the termination state. + + This provider returns a discount of 0.0 in case of termination and 1.0 + otherwise (i.e. for truncation and not termination). + + It is usually safe to use this discount provider for environments that return + strictly positive rewards. + """ + + def __init__(self, name: str = "default_discount_provider"): + self._name = name + self._spec = specs.BoundedArray( + shape=(), dtype=np.float64, minimum=0.0, maximum=1.0, name="discount" + ) + + def name(self) -> str: + """Returns a unique string identifier for this object.""" + return self._name + + def compute_discount( + self, + unused_required_features: Mapping[str, gdmr_types.ArrayType], + termination_state: termination_checker.TerminationResult, + ) -> tree.Structure[gdmr_types.ArrayType]: + """Computes the discount. + + Args: + unused_required_features: Unused + termination_state: The termination state as computed by the termination + checkers. Returns the discount. + + Returns: + The discount. + """ + if termination_state == termination_state.TERMINATE: + return np.asarray(0).astype(self._spec.dtype) + else: # TRUNCATION or DO_NOT_TERMINATE + return np.asarray(1.0).astype(self._spec.dtype) + + def discount_spec(self) -> tree.Structure[specs.Array]: + """Returns the spec of the discount.""" + return self._spec + + def required_features_keys(self) -> set[str]: + """Returns the feature keys that are required to compute the discount.""" + return set() diff --git a/src/experimental/reaf/core/default_observation_space_adapter.py b/src/experimental/reaf/core/default_observation_space_adapter.py new file mode 100644 index 00000000..1261b3ca --- /dev/null +++ b/src/experimental/reaf/core/default_observation_space_adapter.py @@ -0,0 +1,231 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""ObservationSpaceAdapter supporting filtering, renaming and type conversion.""" + +import abc +from collections.abc import Iterable, Mapping +import dataclasses + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types +import numpy as np +import numpy.typing as npt +from reaf.core import observation_space_adapter +import tree + + +@dataclasses.dataclass(frozen=True, kw_only=True) +class RenameInfo: + original_key: str + renamed_key: str + + +class ObservationTypeMapper(abc.ABC): + """Maps from REAF features and specs into corresponding environment types.""" + + @abc.abstractmethod + def to_observation_spec( + self, features_spec: Mapping[str, specs.Array] + ) -> gdmr_types.ObservationSpec: + """Convert the features spec into the environment observation spec.""" + + @abc.abstractmethod + def to_observations( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> tree.Structure[gdmr_types.ArrayType]: + """Convert the features into the environment observations.""" + + +class _DefaultObservationTypeMapper(ObservationTypeMapper): + """An ObservationTypeMapper that returns the input features specs and dict. + + This `ObservationTypeMapper` maps observations from the more constrained + `Mapping[str, ArrayType]` used in the task layer to the more generic + `tree.Structure[ArrayType]` exposed by the GDM Environment. + """ + + def to_observation_spec( + self, features_spec: Mapping[str, specs.Array] + ) -> gdmr_types.ObservationSpec: + """Returns the features spec, unmodified, as a `gdmr_types.ObservationSpec`.""" + return features_spec + + def to_observations( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> tree.Structure[gdmr_types.ArrayType]: + """Returns the features, unmodified, as a `tree.Structure`.""" + return features + + +class DefaultObservationSpaceAdapter( + observation_space_adapter.ObservationSpaceAdapter +): + """Observation adapter supporting filtering, renaming and type conversion. + + This adapter supports filtering, renaming, and converting REAF features into + environment observations. + + The order of operations is the following: + 1) Filtering, i.e. feature selection. + 2) Downcasting floats to max_float_dtype. + 3) Renaming. + 4) Type conversion. + + Please refer to the constructor documentation for more information. + """ + + def __init__( + self, + *, + task_features_spec: Mapping[str, specs.Array], + selected_features: Iterable[str] | None, + renamed_features: Iterable[RenameInfo] | None, + observation_type_mapper: ObservationTypeMapper | None, + max_float_dtype: npt.DTypeLike = np.float64, + ): + """Initializes the observation space adapter. + + Args: + task_features_spec: The spec of all the features exposed by the task + layer. + selected_features: The features that will be exposed as observations. If + None, all features will be exposed, i.e. no filtering. + renamed_features: `RenameInfo` objects specifying which features should be + renamed and the corresponding new name. If empty or None, no renaming + will occur. + observation_type_mapper: An `ObservationTypeMapper` specifying how to + convert the task layer features data type (i.e. a Mapping[str, + ArrayType]) into the more generic type exposed by the GDM Environment + (i.e. a tree.Structure[ArrayType]). If None, an instance of + `_DefaultObservationTypeMapper` is used which converts the task logic + layer features dictionary to the more generic type (i.e. + `tree.Structure[ArrayType])` exposed by the environment. + max_float_dtype: The maximum float dtype to use for downcasting floats. + """ + if not np.issubdtype(max_float_dtype, np.floating): + raise ValueError( + 'max_float_dtype must be a floating point dtype. Got' + f' {max_float_dtype}' + ) + self._max_float_dtype = max_float_dtype + self._max_bits = np.finfo(self._max_float_dtype).bits + self._task_features_spec = task_features_spec + self._selected_filter = selected_features + self._renamed_features = renamed_features or () + self._observation_type_mapper = ( + observation_type_mapper or _DefaultObservationTypeMapper() + ) + self._check_specs_consistency() + # Compute the observation spec only once. + self._observation_spec = self._compute_observation_spec() + + def _check_specs_consistency(self) -> None: + # Check that filter keys are present in the spec. + if self._selected_filter is not None: + all_features = self._task_features_spec.keys() + features = set() + for feature in self._selected_filter: + if feature not in all_features: + raise ValueError(f'Feature {feature} is not present in the spec.') + features.add(feature) + else: + # No filter applied. Select all features. + features = set(self._task_features_spec.keys()) + + # Check renaming. + for rename_info in self._renamed_features: + if rename_info.original_key not in features: + raise ValueError( + f'Feature {rename_info.original_key} is not present in the spec.' + ) + + def observations_from_features( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> tree.Structure[gdmr_types.ArrayType]: + """Converts the features into the final environment observations.""" + # 1. Filter the observations. + if (selected_features := self._selected_filter) is None: + # No filter. Expose all observations. + filtered_features = dict(features) + else: + filtered_features = { + k: v for k, v in features.items() if k in selected_features # pytype: disable=unsupported-operands + } + + # 2. Downcast floats to max_float_dtype. + filtered_features = { + k: self._downcast_if_necessary(v) for k, v in filtered_features.items() + } + + # 3. Rename. + for rename_info in self._renamed_features: + # Rename the feature. + value = filtered_features[rename_info.original_key] + del filtered_features[rename_info.original_key] + filtered_features[rename_info.renamed_key] = value + + # 4. Convert type. + return self._observation_type_mapper.to_observations(filtered_features) + + def _compute_observation_spec(self) -> gdmr_types.ObservationSpec: + """Computes the observation spec.""" + # 1. Filter the specs + if (features_to_filter := self._selected_filter) is None: + # The observation spec corresponds to the task features spec. + filtered_specs = dict(self._task_features_spec) + else: + filtered_specs = { + k: v + for k, v in self._task_features_spec.items() + if k in features_to_filter # pytype: disable=unsupported-operands + } + + # 2. Downcast floats to max_float_dtype. + for k, v in filtered_specs.items(): + if self._dtype_needs_downcast(v.dtype): + filtered_specs[k] = v.replace(dtype=self._max_float_dtype) + + # 3. Rename. + for rename_info in self._renamed_features: + # Rename the feature. + value = filtered_specs[rename_info.original_key] + del filtered_specs[rename_info.original_key] + filtered_specs[rename_info.renamed_key] = value + + # 4. Convert the type. + return self._observation_type_mapper.to_observation_spec(filtered_specs) + + def observation_spec(self) -> gdmr_types.ObservationSpec: + """Returns the observation spec.""" + return self._observation_spec + + def task_features_keys(self) -> set[str]: + """Returns the task features keys that will be converted by this adapter.""" + return set(self._task_features_spec.keys()) + + def _downcast_if_necessary( + self, value: gdmr_types.ArrayType + ) -> gdmr_types.ArrayType: + if ( + hasattr(value, 'dtype') and self._dtype_needs_downcast(value.dtype) + ) or self._dtype_needs_downcast(type(value)): + return np.asarray(value).astype(self._max_float_dtype) + else: + return value + + def _dtype_needs_downcast(self, dtype: npt.DTypeLike) -> bool: + return ( + np.issubdtype(dtype, np.floating) + and np.finfo(dtype).bits > self._max_bits + ) diff --git a/src/experimental/reaf/core/device.py b/src/experimental/reaf/core/device.py new file mode 100644 index 00000000..cc53a0ca --- /dev/null +++ b/src/experimental/reaf/core/device.py @@ -0,0 +1,54 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""REAF basic device to interface with the robotic setup.""" + +import abc +from collections.abc import Mapping +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types + + +class Device(abc.ABC): + """REAF basic device to interface with the robotic setup. + + A device defines a single piece in the robotic setup. It should be + hermetic, that is, not depending on other Devices. The coordination of the + devices is responsibility of the DeviceCoordinator. + + Important: a Device should return the commands and measurements specs + immediately after initialisation without the need for any explicit + initialisation, nor for resource acquisition (e.g. connecting to the + hardware). + """ + + @property + @abc.abstractmethod + def name(self) -> str: + """Returns the name of this device.""" + + @abc.abstractmethod + def commands_spec(self) -> Mapping[str, gdmr_types.AnyArraySpec]: + """Returns the commands specs for this device.""" + + @abc.abstractmethod + def measurements_spec(self) -> Mapping[str, specs.Array]: + """Returns the measurements specs for this device.""" + + @abc.abstractmethod + def set_commands(self, commands: Mapping[str, gdmr_types.ArrayType]) -> None: + """Sets the commands for this device.""" + + @abc.abstractmethod + def get_measurements(self) -> Mapping[str, gdmr_types.ArrayType]: + """Returns the measurements provided by this device.""" diff --git a/src/experimental/reaf/core/device_coordinator.py b/src/experimental/reaf/core/device_coordinator.py new file mode 100644 index 00000000..233705bb --- /dev/null +++ b/src/experimental/reaf/core/device_coordinator.py @@ -0,0 +1,87 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Coordinates the devices composing a robotic setup.""" + +import abc +from collections.abc import Iterable +from reaf.core import device + + +class DeviceCoordinator(abc.ABC): + """Coordinates the devices composing a robotic setup. + + The `DeviceCoordinator` object is responsible for coordinating all the + devices constituting the robotic setup. Whilst the Device is hermetic, + the coordinator is responsible for passing information from one device to + the other if required. For example in a bimanual setup the coordinator is + charged with passing the position of each robot to the other so we can ensure + proper and safe interaction such as for example collision avoidance. + + The `DeviceCoordinator` can be configurable to enable different + properties on the robotic setup, e.g. adding or not adding a `Device` or + forwarding configuration to each `Device`. + + At the very least, the coordinator must implement `get_devices` + to return all the devices. We also provide `on_begin_stepping` and + `on_end_stepping` methods that will be called before the start of an episode + and after the end of the episode respectively. Note that resource acquisition + and subsequent release is completely up to the implementation. + + Finally, `before_set_commands`/`before_get_measurements` can be implemented to + coordinate devices behaviour before their corresponding functions are + called. + """ + + @property + @abc.abstractmethod + def name(self) -> str: + """Returns the name of the coordinator.""" + + @abc.abstractmethod + def get_devices(self) -> Iterable[device.Device]: + """Returns the devices composing the embodiment.""" + + # Lifecycle methods. + + def on_begin_stepping(self) -> None: + """Prepares the coordinator for having its devices called repeatedly. + + After `on_begin_stepping` the devices returned by `get_devices` will have + their `set_commands` and `get_measurements` called repeatedly until + `on_end_stepping` is called on this coordinator. + """ + + def on_end_stepping(self) -> None: + """Notifies the coordinator that the devices are no longer called. + + After `on_end_stepping` the devices returned by `get_devices` will not have + their `set_commands` and `get_measurements` called anymore until this + coordinator `on_begin_stepping` method is notified again. + """ + + # Step hooks methods. + + def before_set_commands(self) -> None: + """Prepares the coordinator to have its devices set_commands called.""" + + def after_set_commands(self) -> None: + """Notifies the coordinator that its devices got `set_commands` called.""" + + def before_get_measurements(self) -> None: + """Prepares the coordinator to have its devices get_measurements called. + + This method gets called immediately before the devices `get_measurements` + method is called and can be used to customise the devices state given the + whole setup state. + """ diff --git a/src/experimental/reaf/core/discount_provider.py b/src/experimental/reaf/core/discount_provider.py new file mode 100644 index 00000000..df1b22fa --- /dev/null +++ b/src/experimental/reaf/core/discount_provider.py @@ -0,0 +1,61 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Computes the discount.""" + +import abc +from collections.abc import Mapping + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types +from reaf.core import termination_checker +import tree + + +class DiscountProvider(abc.ABC): + """Computes the discount.""" + + @abc.abstractmethod + def name(self) -> str: + """Returns a unique string identifier for this object.""" + + @abc.abstractmethod + def compute_discount( + self, + required_features: Mapping[str, gdmr_types.ArrayType], + termination_state: termination_checker.TerminationResult, + ) -> tree.Structure[gdmr_types.ArrayType]: + """Computes the discount. + + Args: + required_features: Measurements and features computed by the task logic + that are required by this provider, i.e. that have keys specified by + `required_features_keys`. + termination_state: The termination state as computed by the termination + checkers. Returns the discount. + + Returns: + The discount. + """ + + @abc.abstractmethod + def discount_spec(self) -> tree.Structure[specs.Array]: + """Returns the spec of the discount.""" + + @abc.abstractmethod + def required_features_keys(self) -> set[str]: + """Returns the feature keys that are required to compute the discount.""" + + def reset(self) -> None: + """Resets the internal state of the discount provider.""" + ... diff --git a/src/experimental/reaf/core/entity.py b/src/experimental/reaf/core/entity.py new file mode 100644 index 00000000..68c2e38d --- /dev/null +++ b/src/experimental/reaf/core/entity.py @@ -0,0 +1,65 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Basic REAF-sim protocol to interface with the simulation.""" + +from collections.abc import Mapping +import typing + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types + + +class Entity(typing.Protocol): + """Basic REAF component to interface with the simulation. + + An entity defines a single component in the simulation that consumes substep + commands and outputs substep measurements at every simulation substep. It + should be hermetic, that is, not depending on other Entities. + + Important: an Entity should return the substep commands and substep + measurements specs immediately after initialisation without the need for any + explicit initialisation. + """ + + @property + def name(self) -> str: + """Instance name.""" + + def reset(self): + """Resets the entity.""" + + def substep_commands_spec( + self, + ) -> Mapping[str, specs.Array]: + """Spec for the substep commands.""" + + def substep_measurements_spec( + self, + ) -> Mapping[str, specs.Array]: + """Spec for the substep measurements.""" + + def set_substep_commands( + self, + model: typing.Any, + data: typing.Any, + consumed_substep_commands: Mapping[str, gdmr_types.ArrayType], + ) -> None: + """Sets the substep commands.""" + + def get_substep_measurements( + self, + model: typing.Any, + data: typing.Any, + ) -> Mapping[str, gdmr_types.ArrayType]: + """Returns the substep measurements.""" diff --git a/src/experimental/reaf/core/environment.py b/src/experimental/reaf/core/environment.py new file mode 100644 index 00000000..dca306f0 --- /dev/null +++ b/src/experimental/reaf/core/environment.py @@ -0,0 +1,490 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""The Robotics Environment Authoring Framework (REAF) Environment class.""" + +import abc +from collections.abc import Mapping +import enum +from typing import Generic + +from absl import logging +import dm_env +from dm_env import specs +from gdm_robotics.interfaces import environment as gdmr_env +from gdm_robotics.interfaces import types as gdmr_types +import numpy as np +from reaf.core import action_space_adapter as reaf_action_space_adapter +from reaf.core import data_acquisition_and_control_layer as reaf_dacl +from reaf.core import default_observation_space_adapter +from reaf.core import logger as reaf_logger +from reaf.core import observation_space_adapter as reaf_observation_space_adapter +from reaf.core import pass_through_action_space_adapter +from reaf.core import task_logic_layer as reaf_tll +import tree + + +class ActionSpecEnforcementOption(enum.StrEnum): + """Options for action spec enforcement.""" + + CLIP_TO_SPEC = "clip_to_spec" + IGNORE = "ignore" + WARNING = "warning" + RAISE_ERROR = "raise_error" + + +class EnvironmentReset(abc.ABC, Generic[gdmr_env.ResetOptions]): + """Support for general resets adhering to the GDM environment API.""" + + @abc.abstractmethod + def do_reset( + self, + config: gdmr_env.ResetOptions, + ) -> None: + """Resets the environment.""" + + def default_reset_configuration(self) -> gdmr_env.ResetOptions: + """Returns the default reset configuration.""" + return gdmr_env.Options() + + +class EndOfEpisodeHandler: + """Handler called after the last episode step.""" + + def on_end_of_episode_stepping(self, final_timestep: dm_env.TimeStep) -> None: + """Called when the episode has ended stepping. + + This will be called at the end of every episode, after all other triggers + have been resolved. Episodes can end either due to truncation or + termination, i.e. `timestep.step_type` is `StepType.LAST`, or due to an + early call to `Environment.reset()`. To verify whether it has indeed + ended due to truncation or termination, the implementer should test + `timestep.last()`. + + Note that the first reset after environment construction will not trigger + this handler, but it will be triggered before resolving any subsequent + environment resets, either implicit or explicit. + + Args: + final_timestep: The final timestep of the episode that ended stepping. + """ + + +class EnvironmentCloser(abc.ABC): + """Handler called when the environment is closed.""" + + @abc.abstractmethod + def close(self) -> None: + """Releases resources when the environment is closed. + + This method is called automatically when exiting the environment's + context manager (`with` statement). + """ + + +class Environment(gdmr_env.Environment): + """The Robotics Environment Authoring Framework (REAF) Environment class.""" + + def __init__( + self, + *, + data_acquisition_and_control_layer: reaf_dacl.DataAcquisitionAndControlLayer, + task_logic_layer: reaf_tll.TaskLogicLayer, + environment_reset: EnvironmentReset, + action_space_adapter: ( + reaf_action_space_adapter.ActionSpaceAdapter | None + ) = None, + observation_space_adapter: ( + reaf_observation_space_adapter.ObservationSpaceAdapter | None + ) = None, + end_of_episode_handler: EndOfEpisodeHandler | None = None, + environment_closer: EnvironmentCloser | None = None, + action_spec_enforcement_option: ActionSpecEnforcementOption = ActionSpecEnforcementOption.RAISE_ERROR, + ): + """Creates an environment. + + Args: + data_acquisition_and_control_layer: The layer for communicating with the + specific robotic setup. + task_logic_layer: The layer in charge of defining the task. + environment_reset: The `EnvironmentReset` specifying the function to be + called at environment reset and the default environment reset + configuration. + action_space_adapter: Adapter from the agent action space to the flattened + commands accepted by the task layer. If None the + PassThroughActionSpaceAdapter is used, meaning the entirety of the + commands dictionary is exposed to the agent. + observation_space_adapter: Adapter from the computed features to the + observations that are exposed to the agent. If None the + DefaultObservationSpaceAdapter is used, meaning all the features are + exposed to the agent as observations. + end_of_episode_handler: Called at the end of an episode, after the last + step. + environment_closer: Specifies the handler to be called when the + environment is closed. This is called automatically on exit if the + environment is used as a context manager. If None, no action is + performed at close. + action_spec_enforcement_option: How to enforce the action spec. If + `CLIP_TO_SPEC`, the action will be clipped to the spec. If `WARNING`, an + warning logged if the action is outside the spec. If `RAISE_ERROR`, an + error will be raised if the action is outside the spec. If `IGNORE`, + the action will be passed through. Default is `RAISE_ERROR`. + """ + + self._data_acquisition_and_control_layer = ( + data_acquisition_and_control_layer + ) + self._task_logic_layer = task_logic_layer + self._end_of_episode_handler = ( + end_of_episode_handler or EndOfEpisodeHandler() + ) + self._environment_reset = environment_reset + self._environment_closer = environment_closer + self._action_spec_enforcement_option = action_spec_enforcement_option + + # Before assigning the adapters, validate the specs on the task logic layer + # and the DACL. + self._validate_dacl_and_ttl_specs() + + ttl_commands_spec = self._task_logic_layer.commands_spec( + self._data_acquisition_and_control_layer.commands_spec() + ) + ttl_features_spec = self._task_logic_layer.features_spec( + self._data_acquisition_and_control_layer.measurements_spec() + ) + + if action_space_adapter is None: + action_space_adapter = ( + pass_through_action_space_adapter.PassThroughActionSpaceAdapter( + commands_spec=ttl_commands_spec + ) + ) + self._action_space_adapter = action_space_adapter + + if observation_space_adapter is None: + observation_space_adapter = ( + default_observation_space_adapter.DefaultObservationSpaceAdapter( + task_features_spec=ttl_features_spec, + selected_features=None, + renamed_features=None, + observation_type_mapper=None, + ) + ) + self._observation_space_adapter = observation_space_adapter + + # Now we can validate the adapters. + self._validate_adapters_specs() + + self._last_timestep: dm_env.TimeStep | None = None + self._should_finalize_episode = False + self._timestep_spec = gdmr_types.TimeStepSpec( + step_type=gdmr_types.STEP_TYPE_SPEC, + reward=self._task_logic_layer.reward_spec(), + discount=self._task_logic_layer.discount_spec(), + # The observation spec corresponds to the one exposed by the adapter. + observation=self._observation_space_adapter.observation_spec(), + ) + + self._zero_reward, self._zero_discount = tree.map_structure( + _read_only_zeros_like_spec, + (self._timestep_spec.reward, self._timestep_spec.discount), + ) + + def close(self) -> None: + """Frees any resources used by the environment.""" + if self._environment_closer is not None: + self._environment_closer.close() + + def default_reset_options(self) -> gdmr_env.ResetOptions: + return self._environment_reset.default_reset_configuration() + + def reset_with_options( + self, + *, + options: gdmr_env.ResetOptions, + ) -> dm_env.TimeStep: + """Starts a new sequence and returns the first `TimeStep`.""" + if self._should_finalize_episode: + self._finalize_episode() + self._environment_reset.do_reset(options) + self._task_logic_layer.perform_reset() + measurements = self._data_acquisition_and_control_layer.begin_stepping() + features = self._task_logic_layer.compute_all_features(measurements) + observations = self._compute_observations_from_features(features) + + self._last_timestep = self._restart(observation=observations) + # Make sure any early reset after this one triggers `_finalize_episode`. + self._should_finalize_episode = True + return self._last_timestep + + def action_spec(self) -> gdmr_types.ActionSpec: + """Defines the actions that should be provided to `step`.""" + # The action spec corresponds to the one exposed by the adapter. + return self._action_space_adapter.action_spec() + + def timestep_spec(self) -> gdmr_types.TimeStepSpec: + """Returns the spec associated to the returned TimeStep.""" + return self._timestep_spec + + def step(self, action: gdmr_types.ActionType) -> dm_env.TimeStep: + """Updates the environment according to action and returns a `TimeStep`.""" + + action = self._enforce_action_spec(action) + if self._last_timestep is None or self._last_timestep.last(): + return self.reset() + + # Process the action to obtain a command. + commands = self._compute_commands_from_agent_action(action) + commands = self._task_logic_layer.compute_final_commands(commands) + measurements = self._data_acquisition_and_control_layer.step(commands) + + # Compute all the features. + features = self._task_logic_layer.compute_all_features(measurements) + + # Compute the elements of the timestep. + reward = self._task_logic_layer.compute_reward(features) + termination_state = self._task_logic_layer.check_for_termination(features) + discount = self._task_logic_layer.compute_discount( + features, termination_state + ) + + observations = self._compute_observations_from_features(features) + + if termination_state.is_terminated(): + self._last_timestep = self._termination( + reward=reward, observation=observations + ) + elif termination_state.is_truncated(): + self._last_timestep = self._truncation( + reward=reward, observation=observations, discount=discount + ) + else: + self._last_timestep = self._transition( + reward=reward, observation=observations, discount=discount + ) + + if self._last_timestep.last(): + self._finalize_episode() + return self._last_timestep + + def _finalize_episode(self) -> None: + self._data_acquisition_and_control_layer.end_stepping() + # It's crucial to call `end_stepping` on the dacl before invoking the end + # of episode handler. This ensures no further `set_command` or + # `get_measurements` calls are made. In contrast, the end of episode + # handler might interact with devices, requiring them to be informed + # beforehand. + self._end_of_episode_handler.on_end_of_episode_stepping(self._last_timestep) + self._should_finalize_episode = False + + @property + def data_acquisition_and_control_layer( + self, + ) -> reaf_dacl.DataAcquisitionAndControlLayer: + return self._data_acquisition_and_control_layer + + @property + def task_logic_layer(self) -> reaf_tll.TaskLogicLayer: + return self._task_logic_layer + + @property + def environment_reset(self) -> EnvironmentReset: + return self._environment_reset + + @environment_reset.setter + def environment_reset(self, environment_reset: EnvironmentReset) -> None: + self._environment_reset = environment_reset + + def add_logger(self, logger: reaf_logger.Logger) -> None: + self._task_logic_layer.add_logger(logger) + + def remove_logger(self, logger: reaf_logger.Logger) -> None: + self._task_logic_layer.remove_logger(logger) + + def _validate_dacl_and_ttl_specs(self) -> None: + """Validates the specs on the task logic layer.""" + # Validate the spec on the task logic layer. + self._task_logic_layer.validate_spec( + dacl_commands_spec=( + self._data_acquisition_and_control_layer.commands_spec() + ), + dacl_measurements_spec=( + self._data_acquisition_and_control_layer.measurements_spec() + ), + ) + + def _validate_adapters_specs(self) -> None: + # Collect the full commands and features spec and validate them against + # the adapters. + commands_spec = set( + self._task_logic_layer.commands_spec( + self._data_acquisition_and_control_layer.commands_spec() + ).keys() + ) + features_spec = set( + self._task_logic_layer.features_spec( + self._data_acquisition_and_control_layer.measurements_spec() + ) + ) + + # Check the action space adapter. + adapter_keys = self._action_space_adapter.task_commands_keys() + + if adapter_keys != commands_spec: + raise ValueError( + "Mismatch between commands exposed by the action space adapter:" + f" {adapter_keys} and commands spec expected by the task layer:" + f" {commands_spec}." + ) + + # Check the observation spec adapter. + adapter_keys = self._observation_space_adapter.task_features_keys() + if not adapter_keys.issubset(features_spec): + raise ValueError( + "Failed to validate observation space adapter specs. Missing keys:" + f" {adapter_keys - features_spec}" + ) + + def _compute_observations_from_features( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> tree.Structure[gdmr_types.ArrayType]: + return self._observation_space_adapter.observations_from_features(features) + + def _compute_commands_from_agent_action( + self, agent_action: gdmr_types.ActionType + ) -> Mapping[str, gdmr_types.ArrayType]: + return self._action_space_adapter.commands_from_environment_action( + agent_action + ) + + def _restart( + self, + observation: tree.Structure[gdmr_types.ArrayType], + ) -> dm_env.TimeStep: + """Returns a `TimeStep` with `step_type` set to `StepType.FIRST`.""" + return dm_env.TimeStep( + step_type=np.asarray(dm_env.StepType.FIRST, dtype=np.uint8), + observation=observation, + reward=self._zero_reward, + discount=self._zero_discount, + ) + + def _transition( + self, + reward: tree.Structure[gdmr_types.ArrayType], + observation: tree.Structure[gdmr_types.ArrayType], + discount: tree.Structure[gdmr_types.ArrayType], + ) -> dm_env.TimeStep: + """Returns a `TimeStep` with `step_type` set to `StepType.MID`.""" + return dm_env.TimeStep( + step_type=np.asarray(dm_env.StepType.MID, dtype=np.uint8), + observation=observation, + reward=reward, + discount=discount, + ) + + def _termination( + self, + reward: tree.Structure[gdmr_types.ArrayType], + observation: tree.Structure[gdmr_types.ArrayType], + ) -> dm_env.TimeStep: + """Returns a `TimeStep` with `step_type` set to `StepType.LAST`.""" + return dm_env.TimeStep( + step_type=np.asarray(dm_env.StepType.LAST, dtype=np.uint8), + observation=observation, + reward=reward, + discount=self._zero_discount, + ) + + def _truncation( + self, + reward: tree.Structure[gdmr_types.ArrayType], + observation: tree.Structure[gdmr_types.ArrayType], + discount: tree.Structure[gdmr_types.ArrayType], + ) -> dm_env.TimeStep: + """Returns a `TimeStep` with `step_type` set to `StepType.LAST`.""" + return dm_env.TimeStep( + step_type=np.asarray(dm_env.StepType.LAST, dtype=np.uint8), + observation=observation, + reward=reward, + discount=discount, + ) + + def _enforce_action_spec( + self, action: gdmr_types.ActionType + ) -> gdmr_types.ActionType: + """Enforces the action spec.""" + match self._action_spec_enforcement_option: + case ActionSpecEnforcementOption.IGNORE: + pass + case ActionSpecEnforcementOption.CLIP_TO_SPEC: + try: + + def clip_to_spec(a, s): + if isinstance(s, specs.BoundedArray): + return np.clip(a, s.minimum, s.maximum) + return a + + action = tree.map_structure( + clip_to_spec, + action, + self._action_space_adapter.action_spec(), + ) + except ValueError as e: + raise ValueError( + "Failed to clip action to spec. Action:" + f" {action} and spec: {self._action_space_adapter.action_spec()}" + ) from e + case ActionSpecEnforcementOption.WARNING: + + def _validate_without_raising(a, s): + dtype_ok = s.dtype == a.dtype + shape_ok = s.shape == a.shape + minimum_ok = True + maximum_ok = True + if isinstance(s, specs.BoundedArray): + minimum_ok = (s.minimum <= a).all() + maximum_ok = (a <= s.maximum).all() + return dtype_ok and shape_ok and minimum_ok and maximum_ok + + if not all( + tree.flatten( + tree.map_structure( + _validate_without_raising, + action, + self._action_space_adapter.action_spec(), + ) + ) + ): + logging.warning( + "Failed to validate action against spec. Action: %r and spec: %r", + action, + self._action_space_adapter.action_spec(), + ) + case ActionSpecEnforcementOption.RAISE_ERROR: + action = tree.map_structure( + lambda a, spec: spec.validate(a), action, self.action_spec() + ) + case _: + raise ValueError( + "Unknown action spec enforcement option:" + f" {self._action_spec_enforcement_option}" + ) + return action + + +def _read_only_zeros_like_spec(spec: specs.Array) -> np.ndarray: + """Returns a zero array matching the specified spec.""" + arr = np.zeros(shape=spec.shape, dtype=spec.dtype) + arr.flags.writeable = False + return arr diff --git a/src/experimental/reaf/core/features_observer.py b/src/experimental/reaf/core/features_observer.py new file mode 100644 index 00000000..cc87f0d3 --- /dev/null +++ b/src/experimental/reaf/core/features_observer.py @@ -0,0 +1,34 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Observe all the produced features and measurements.""" + +import abc +from collections.abc import Mapping + +from gdm_robotics.interfaces import types as gdmr_types + + +class FeaturesObserver(abc.ABC): + """Observe all the produced features and measurements.""" + + @property + @abc.abstractmethod + def name(self) -> str: + """Returns a unique string identifier for this object.""" + + @abc.abstractmethod + def observe_features( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> None: + """Observes all the features and measurements.""" diff --git a/src/experimental/reaf/core/features_producer.py b/src/experimental/reaf/core/features_producer.py new file mode 100644 index 00000000..8ce44e94 --- /dev/null +++ b/src/experimental/reaf/core/features_producer.py @@ -0,0 +1,56 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Produces additional features to be exposed by the task logic layer.""" + +import abc +from collections.abc import Mapping + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types + + +class FeaturesProducer(abc.ABC): + """Produces additional features to be exposed by the task logic layer.""" + + @property + @abc.abstractmethod + def name(self) -> str: + """Returns a unique string identifier for this object.""" + + @abc.abstractmethod + def produce_features( + self, required_features: Mapping[str, gdmr_types.ArrayType] + ) -> Mapping[str, gdmr_types.ArrayType]: + """Produces additional features for the environment. + + Args: + required_features: Measurements and features generated by previous + producers in the processing chain that are required by this processor, + i.e. with keys specified by `required_features_keys`. + + Returns additional features that will be added to the global measurements + and features dictionary. + """ + + @abc.abstractmethod + def produced_features_spec(self) -> Mapping[str, specs.Array]: + """Returns the spec of the features produced by this producer.""" + + @abc.abstractmethod + def required_features_keys(self) -> set[str]: + """Returns the keys that are required to produce the new features.""" + + def reset(self) -> None: + """Resets the internal state of the feature producer.""" + ... diff --git a/src/experimental/reaf/core/logger.py b/src/experimental/reaf/core/logger.py new file mode 100644 index 00000000..63bb5f33 --- /dev/null +++ b/src/experimental/reaf/core/logger.py @@ -0,0 +1,80 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Support logging inside the task logic layer.""" + +import abc +from collections.abc import Mapping + +from gdm_robotics.interfaces import types as gdmr_types + + +class Logger(abc.ABC): + """Support logging inside the task logic layer. + + Lifecycle + For each environment step, these member functions are called in this order: + 1. `record_measurements` is called with raw measurements from the sensors. + 2. `record_features` is called with features derived from the measurements. + 3. `record_commands_processing` is called for each + `CommandsProcessor.process_commands` invocation, tracking the + transformation of commands. + 4. `record_final_commands` is called once with the final commands sent to + the DACL. + + Notes: + An environment is first reset(). This triggers the first two steps above. + See reset_with_options in ./environment.py. + + After reset, step is called repeatedly. + 1. This first triggers steps 3 and 4 (See compute_final_commands in TLL + called from step in ./environment.py) + 2. Features are computed (see compute_all_features in TLL called from + step in ./environment.py), triggering steps 1 and 2. + """ + + @property + @abc.abstractmethod + def name(self) -> str: + """Unique string identifier for this object.""" + + def record_measurements( + self, measurements: Mapping[str, gdmr_types.ArrayType] + ) -> None: + """Called once with all the measurements from the DACL.""" + + def record_features( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> None: + """Called once with all the features computed in the Task Layer.""" + + def record_final_commands( + self, commands: Mapping[str, gdmr_types.ArrayType] + ) -> None: + """Called once with the final commands sent to the DACL.""" + + def record_commands_processing( + self, + name: str, + consumed_commands: Mapping[str, gdmr_types.ArrayType], + produced_commands: Mapping[str, gdmr_types.ArrayType], + ) -> None: + """Called once per call to `process_commands` for each CommandsProcessor. + + Args: + name: Name of the `CommandsProcessor`. + consumed_commands: The commands consumed by the current + `CommandsProcessor`. + produced_commands: The commands produced by the current + `CommandsProcessor`. + """ diff --git a/src/experimental/reaf/core/numpy_mock_assertions.py b/src/experimental/reaf/core/numpy_mock_assertions.py new file mode 100644 index 00000000..310a918e --- /dev/null +++ b/src/experimental/reaf/core/numpy_mock_assertions.py @@ -0,0 +1,98 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Testing functions for asserting on Mock objects with numpy structures.""" + +from collections.abc import Sequence +from unittest import mock +import numpy as np + + +def assert_called_once_with(mock_obj: mock.Mock, *args, **kwargs) -> None: + if mock_obj.call_count != 1: + raise AssertionError( + f"Expected exactly one call to {mock_obj}, got {mock_obj.call_count}" + ) + + assert_called_with(mock_obj, *args, **kwargs) + + +def assert_called_with(mock_obj: mock.Mock, *args, **kwargs) -> None: + """Asserts that the last call to mock_obj had the specified arguments.""" + if mock_obj.call_args is None: + raise AssertionError( + f"Mock object {mock_obj} not called. Expected one call." + ) + call_args, call_kwargs = mock_obj.call_args + np.testing.assert_equal(call_args, args) + np.testing.assert_equal(call_kwargs, kwargs) + + +def assert_has_calls( + mock_obj: mock.Mock, calls: Sequence[mock._Call], any_order: bool = False +) -> None: + """Asserts that mock_obj has been called with the specified calls.""" + mock_calls = mock_obj.mock_calls + + # Check that there are at least enough calls. + if mock_obj.call_count < len(calls): + raise AssertionError( + f"Expected at least {len(calls)} calls to {mock_obj}, got" + f" {mock_obj.call_count}" + ) + + def _calls_are_equal(actual: mock._Call, expected: mock._Call) -> bool: + _, actual_args, actual_kwargs = actual + _, expected_args, expected_kwargs = expected + # Quickest way to transform the assertion into a comparator. + try: + np.testing.assert_equal(actual_args, expected_args) + np.testing.assert_equal(actual_kwargs, expected_kwargs) + return True + except AssertionError: + return False + + if any_order: + # We just check for the calls to be contained. + for expected_call in calls: + for actual_call in mock_calls: + if _calls_are_equal(actual_call, expected_call): + break + raise AssertionError( + f"Expected call {expected_call} not found in mock calls {mock_calls}." + ) + return + + # We need to check in order, but first find the first call. + starting_index = -1 + first_expected_call = calls[0] + for index, actual_call in enumerate(mock_calls): + if _calls_are_equal(actual_call, first_expected_call): + starting_index = index + break + if starting_index == -1: + raise AssertionError(f"Calls {calls} not found in mock calls {mock_calls}.") + + non_matching_calls = [] + + # We have the first element. Now we need to compare element wise. + for index, expected_call in enumerate(calls): + actual_call = mock_calls[starting_index + index] + if not _calls_are_equal(actual_call, expected_call): + non_matching_calls.append((index, expected_call, actual_call)) + + if non_matching_calls: + raise AssertionError( + f"Calls {calls} do not match mock calls {mock_calls}. Mismatch (index," + f" expected, actual): {non_matching_calls}" + ) diff --git a/src/experimental/reaf/core/observation_space_adapter.py b/src/experimental/reaf/core/observation_space_adapter.py new file mode 100644 index 00000000..ca4a8d86 --- /dev/null +++ b/src/experimental/reaf/core/observation_space_adapter.py @@ -0,0 +1,42 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Adapts REAF features into observations exposed by the environment.""" + +import abc +from collections.abc import Mapping +from gdm_robotics.interfaces import types as gdmr_types +import tree + + +class ObservationSpaceAdapter(abc.ABC): + """Adapts REAF features into observations exposed by the environment. + + Implementations of this interface are responsible for converting the features + generated by the REAF task layer logic (i.e. dictionary of tensors) into the + more generic `observation` structure exposed by the environment. + """ + + @abc.abstractmethod + def observations_from_features( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> tree.Structure[gdmr_types.ArrayType]: + """Converts the REAF features into the environment observations.""" + + @abc.abstractmethod + def observation_spec(self) -> gdmr_types.ObservationSpec: + """Returns the observation spec.""" + + @abc.abstractmethod + def task_features_keys(self) -> set[str]: + """Returns the task features keys that will be converted by this adapter.""" diff --git a/src/experimental/reaf/core/pass_through_action_space_adapter.py b/src/experimental/reaf/core/pass_through_action_space_adapter.py new file mode 100644 index 00000000..16b28a81 --- /dev/null +++ b/src/experimental/reaf/core/pass_through_action_space_adapter.py @@ -0,0 +1,55 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Adapter that passes the commands spec through.""" + +from collections.abc import Mapping +from gdm_robotics.interfaces import types as gdmr_types +from reaf.core import action_space_adapter + + +class PassThroughActionSpaceAdapter(action_space_adapter.ActionSpaceAdapter): + """Adapter that passes the commands spec through. + + NB the resulting environment will expose a dictionary as the action spec. + """ + + def __init__(self, commands_spec: Mapping[str, gdmr_types.AnyArraySpec]): + self._commands_spec = commands_spec + + def commands_from_environment_action( + self, environment_action: gdmr_types.ActionType + ) -> Mapping[str, gdmr_types.ArrayType]: + """Returns commands accepted by REAF. + + commands_from_environment_action usually accepts a gdmr_types.ActionType but + since this adapter passes the same action as the commands, it needs to be a + dict type in order to pass it through as a dict. + + Args: + environment_action: The environment action(s) to pass as REAF commands. + """ + if not isinstance(environment_action, dict): + raise ValueError( + 'environment_action must be a dict, but got: ' + f'{type(environment_action)}.' + ) + return environment_action + + def action_spec(self) -> gdmr_types.ActionSpec: + """Returns the action spec exposed by the environment.""" + return self._commands_spec + + def task_commands_keys(self) -> set[str]: + """Returns the keys for the commands exposed to the task layer.""" + return set(self._commands_spec.keys()) diff --git a/src/experimental/reaf/core/reward_provider.py b/src/experimental/reaf/core/reward_provider.py new file mode 100644 index 00000000..3a8ec655 --- /dev/null +++ b/src/experimental/reaf/core/reward_provider.py @@ -0,0 +1,292 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Computes the reward.""" + +import abc +from collections.abc import Mapping +import operator +from typing import Callable, TypeAlias, TypeVar, Union + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types +import numpy as np +import tree + + +RewardValue: TypeAlias = tree.Structure[gdmr_types.ArrayType] +RewardSpec: TypeAlias = tree.Structure[specs.Array] + + +class _RewardProvider(abc.ABC): + """Computes the reward. + + Defines the interface for a reward provider. + + Important: Users should not inherit from this class directly. Instead, use the + RewardProvider class later in this file. + """ + + @abc.abstractmethod + def name(self) -> str: + """Returns a unique string identifier for this object.""" + + @abc.abstractmethod + def compute_reward( + self, required_features: Mapping[str, gdmr_types.ArrayType] + ) -> RewardValue: + """Computes the reward. + + Args: + required_features: Measurements and features computed by the task logic + that are required by this provider, i.e. that have keys specified by + `required_features_keys`. + + Returns the computed reward. + """ + + @abc.abstractmethod + def reward_spec(self) -> RewardSpec: + """Returns the spec of the reward.""" + + @abc.abstractmethod + def required_features_keys(self) -> set[str]: + """Returns the feature keys that are required to compute the reward.""" + + def reset(self) -> None: + """Resets the internal state of the reward provider.""" + ... + + +RewardProviderOrValue: TypeAlias = Union['RewardProvider', RewardValue] + + +S = TypeVar('S') +T = TypeVar('T') +UnaryOperator: TypeAlias = Callable[[S], S] +BinaryOperator: TypeAlias = Callable[[S | T, S | T], S | T] + + +class RewardProvider(_RewardProvider): + """Computes the reward. + + Important: Users should inherit from this class and implement the abstract + methods defined in the interface _RewardProvider. + """ + + def __add__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.add, self, other) + + def __radd__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.add, other, self) + + def __sub__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.sub, self, other) + + def __rsub__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.sub, other, self) + + def __mul__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.mul, self, other) + + def __rmul__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.mul, other, self) + + def __truediv__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.truediv, self, other) + + def __rtruediv__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.truediv, other, self) + + def __floordiv__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.floordiv, self, other) + + def __rfloordiv__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.floordiv, other, self) + + def __pow__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.pow, self, other) + + def __rpow__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.pow, other, self) + + def __getitem__(self, index: slice): + return GetItemOperationRewardProvider(self, index) + + def __neg__(self): + return UnaryOperationRewardProvider(operator.neg, self) + + +class ConstantRewardProvider(RewardProvider): + """A RewardProvider that always returns the same reward.""" + + def __init__(self, reward: RewardValue): + super().__init__() + self._reward = reward + + def name(self) -> str: + return str(self._reward) + + def compute_reward( + self, required_features: Mapping[str, gdmr_types.ArrayType] + ) -> RewardValue: + return self._reward + + def reward_spec(self) -> RewardSpec: + return tree.map_structure( + lambda v: specs.Array(v.shape, v.dtype), self._reward + ) + + def required_features_keys(self) -> set[str]: + return set() + + +class BinaryOperationRewardProvider(RewardProvider): + """Applies a binary operator to the result of two reward providers.""" + + def __init__( + self, + op: BinaryOperator, + first_reward_provider: RewardProviderOrValue, + second_reward_provider: RewardProviderOrValue, + ): + super().__init__() + if not isinstance(first_reward_provider, RewardProvider): + first_reward_provider = ConstantRewardProvider(first_reward_provider) + if not isinstance(second_reward_provider, RewardProvider): + second_reward_provider = ConstantRewardProvider(second_reward_provider) + first_spec = first_reward_provider.reward_spec() + second_spec = second_reward_provider.reward_spec() + tree.assert_same_structure(first_spec, second_spec) + assert all( + tree.flatten( + tree.map_structure( + lambda s1, s2: s1.shape == s2.shape and s1.dtype == s2.dtype, + first_spec, + second_spec, + ) + ) + ) + self._op = op + self._first_reward_provider = first_reward_provider + self._second_reward_provider = second_reward_provider + self._reward_spec = first_reward_provider.reward_spec() + self._first_required_features_keys = ( + first_reward_provider.required_features_keys() + ) + self._second_required_features_keys = ( + second_reward_provider.required_features_keys() + ) + + def name(self) -> str: + op_name = getattr(self._op, '__name__', str(self._op)) + return ( + f'{op_name}({self._first_reward_provider.name()},' + f' {self._second_reward_provider.name()})' + ) + + def compute_reward( + self, required_features: Mapping[str, gdmr_types.ArrayType] + ) -> RewardValue: + first_required_features = { + k: v + for k, v in required_features.items() + if k in self._first_required_features_keys + } + second_required_features = { + k: v + for k, v in required_features.items() + if k in self._second_required_features_keys + } + return tree.map_structure( + self._op, + self._first_reward_provider.compute_reward(first_required_features), + self._second_reward_provider.compute_reward(second_required_features), + ) + + def reward_spec(self) -> RewardSpec: + return self._reward_spec + + def required_features_keys(self) -> set[str]: + return ( + self._first_required_features_keys | self._second_required_features_keys + ) + + def reset(self) -> None: + self._first_reward_provider.reset() + self._second_reward_provider.reset() + + +class GetItemOperationRewardProvider(RewardProvider): + """Extracts a slice from the result of a reward provider.""" + + def __init__(self, reward_provider: RewardProviderOrValue, index: slice): + super().__init__() + if not isinstance(reward_provider, RewardProvider): + reward_provider = ConstantRewardProvider(reward_provider) + self._reward_provider = reward_provider + self._index = index + + def name(self) -> str: + return f'{self._reward_provider.name}[{self._index}]' + + def compute_reward( + self, required_features: Mapping[str, gdmr_types.ArrayType] + ) -> RewardValue: + return tree.map_structure( + lambda v: v[self._index], + self._reward_provider.compute_reward(required_features), + ) + + def reward_spec(self) -> RewardSpec: + return tree.map_structure( + lambda s: specs.Array(np.empty(s.shape)[self._index].shape, s.dtype), + self._reward_provider.reward_spec(), + ) + + def required_features_keys(self) -> set[str]: + return self._reward_provider.required_features_keys() + + def reset(self) -> None: + self._reward_provider.reset() + + +class UnaryOperationRewardProvider(RewardProvider): + """Applies a unary operator to the result of a reward provider.""" + + def __init__(self, op: UnaryOperator, reward_provider: RewardProviderOrValue): + super().__init__() + if not isinstance(reward_provider, RewardProvider): + reward_provider = ConstantRewardProvider(reward_provider) + self._op = op + self._reward_provider = reward_provider + + def name(self) -> str: + op_name = getattr(self._op, '__name__', str(self._op)) + return f'{op_name}({self._reward_provider.name()})' + + def compute_reward( + self, required_features: Mapping[str, gdmr_types.ArrayType] + ) -> RewardValue: + return tree.map_structure( + self._op, self._reward_provider.compute_reward(required_features) + ) + + def reward_spec(self) -> RewardSpec: + return self._reward_provider.reward_spec() + + def required_features_keys(self) -> set[str]: + return self._reward_provider.required_features_keys() + + def reset(self) -> None: + self._reward_provider.reset() diff --git a/src/experimental/reaf/core/substep_commands_processor.py b/src/experimental/reaf/core/substep_commands_processor.py new file mode 100644 index 00000000..b63f4388 --- /dev/null +++ b/src/experimental/reaf/core/substep_commands_processor.py @@ -0,0 +1,104 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Protocol for substep commands manipulation in REAF-sim.""" + +from collections.abc import Mapping +import typing + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types + + +class SubstepCommandsProcessor(typing.Protocol): + """Processes substep commands, propagating them through a pipeline. + + This processor manipulates substep commands, acting as a node in a pipeline. + It consumes substep commands, performs operations, and produces updated + substep commands for the next stage in the processing chain. + + The processing pipeline starts with commands provided to the SimulationDevice + and progresses towards the substep commands consumed by the individual + entities. Each processor consumes a subset of substep commands and produces + new, potentially transformed, substep commands. The order of operations is + crucial. + + Example Pipeline (conceptual): + + Simulation Device commands --> Processor (1) --> Processor (2) --> Entities + + Specs are propagated starting from the bottom: + 1) In this example assume that the set of entities expect "p3/c1", "p3/c2" and + "p3/c3". + 2) Processor (2) returns ("p3/c1", "p3/c2") from "p2/c1". This means + that the global substep commands spec exposed at this level is "p2/c1" and + the unprocessed "p3/c3". + 3) Processor (1) returns "p2/c1" from ("p1/c1", "p1/c2"). By applying + the same transformation rule, we can obtain the final spec exposed + by the SimulationDevice: "p1/c1", "p1/c2" and "p3/c3". + + ------------------------------------ + | SimulationDevice | + ------------------------------------ + + "p1/c1" "p1/c2" "p3/c3" + | | | + ----------------- | + | P1 | | + ----------------- | + | "p2/c1" | + ----------------- | + | P2 | | + ----------------- | + | "p3/c1" | "p3/c2" | + | | | + ------------------------------------ + | Entities | + ------------------------------------ + """ + + @property + def name(self) -> str: + """Returns a unique string identifier for this object.""" + + def reset(self) -> None: + """Resets the internal state of this processor.""" + + def produced_substep_commands_keys(self) -> set[str]: + """Keys of the substep commands produced by this processor.""" + + def consumed_substep_commands_spec( + self, + ) -> Mapping[str, specs.Array]: + """Spec of the substep commands consumed by this processor.""" + + def process_substep_commands( + self, + model: typing.Any, + data: typing.Any, + consumed_substep_commands: Mapping[str, gdmr_types.ArrayType], + ) -> Mapping[str, gdmr_types.ArrayType]: + """Processes the substep commands and returns a new modified version of it. + + Args: + model: the simulation model. + data: the simulation data. + consumed_substep_commands: the substep commands up in the processing chain + that are required by this processor, i.e. with keys specified by + `consumed_substep_commands_spec`. + + Returns the new substep commands. Note that the (key, value) pairs in + `consumed_substep_commands` are removed from the running substep commands + dictionary. If users want to keep some of the elements it is their + responsibility to retain them in the output dictionary. + """ diff --git a/src/experimental/reaf/core/substep_measurements_processor.py b/src/experimental/reaf/core/substep_measurements_processor.py new file mode 100644 index 00000000..15dc81df --- /dev/null +++ b/src/experimental/reaf/core/substep_measurements_processor.py @@ -0,0 +1,103 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Protocol for substep measurements manipulation in REAF-sim.""" + +from collections.abc import Mapping +import typing + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types + + +class SubstepMeasurementsProcessor(typing.Protocol): + """Processes substep measurements, propagating them through a pipeline. + + This processor manipulates substep measurements, acting as a node in a + pipeline. It consumes substep measurements, performs operations, and produces + updated substep measurements for the next stage in the processing chain. + + The processing pipeline starts with substep measurements produced by Entities + and progresses towards the measurements exposed by the SimulationDevice. Each + processor consumes a subset of substep measurements and produces new, + potentially transformed, substep measurements. The order of operations is + crucial. + + Example Pipeline (conceptual): + + Entities --> Processor (1) --> Processor (2) -> Simulation Device Measurements + + Specs are propagated starting from the bottom: + 1) In this example assume that the set of entities produce "p1/c1", "p1/c2" + and "p1/c3". + 2) Processor (1) returns "p2/c1" from ("p1/c1", "p1/c2"). + 3) Processor (2) returns ("p3/c1", "p3/c2") from "p2/c1". + + This resulting spec exposed by the SimulationDevice: "p3/c1", "p3/c2" + and "p1/c3". + + ------------------------------------ + | SimulationDevice | + ------------------------------------ + + "p3/c1" "p3/c2" "p1/c3" + | | | + ----------------- | + | P2 | | + ----------------- | + | "p2/c1" | + ----------------- | + | P1 | | + ----------------- | + | "p1/c1" | "p1/c2" | + | | | + ------------------------------------ + | Entities | + ------------------------------------ + """ + + @property + def name(self) -> str: + """Returns a unique string identifier for this object.""" + + def reset(self): + """Resets the internal state of this processor.""" + + def produced_substep_measurements_spec( + self, + ) -> Mapping[str, specs.Array]: + """Spec of the substep measurements consumed by this processor.""" + + def consumed_substep_measurements_keys(self) -> set[str]: + """Keys of the substep measurements consumed by this processor.""" + + def process_substep_measurements( + self, + model: typing.Any, + data: typing.Any, + consumed_substep_measurements: Mapping[str, gdmr_types.ArrayType], + ) -> Mapping[str, gdmr_types.ArrayType]: + """Processes the substep measurements and returns a new modified version of it. + + Args: + model: the simulation model. + data: the simulation data. + consumed_substep_measurements: the substep measurements up in the + processing chain that are required by this processor, i.e. with keys + specified by `consumed_substep_measurements_spec`. + + Returns the new substep measurements. Note that the (key, value) pairs in + `consumed_substep_measurements` are removed from the running substep + measurements dictionary. If users want to keep some of the elements it is + their responsibility to retain them in the output dictionary. + """ diff --git a/src/experimental/reaf/core/task_logic_layer.py b/src/experimental/reaf/core/task_logic_layer.py new file mode 100644 index 00000000..51b25639 --- /dev/null +++ b/src/experimental/reaf/core/task_logic_layer.py @@ -0,0 +1,342 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Task logic layer for the Robotics Environment Authoring Framework.""" + +from collections.abc import Mapping, Sequence +import itertools +from typing import Protocol + +from absl import logging +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types +from reaf.core import commands_processor as reaf_commands_processor +from reaf.core import default_discount_provider +from reaf.core import discount_provider as reaf_discount_provider +from reaf.core import features_observer as reaf_features_observers +from reaf.core import features_producer as reaf_features_producer +from reaf.core import logger as reaf_logger +from reaf.core import reward_provider as reaf_reward_provider +from reaf.core import termination_checker as reaf_termination_checker +from reaf.core import zero_reward_provider +import tree + + +class _ResettableObject(Protocol): + """Protocol for an object that can be reset.""" + + def reset(self) -> None: + ... + + +class TaskLogicLayer: + """Task logic layer for the Robotics Environment Authoring Framework.""" + + def __init__( + self, + *, + commands_processors: Sequence[reaf_commands_processor.CommandsProcessor], + features_producers: Sequence[reaf_features_producer.FeaturesProducer], + termination_checkers: Sequence[ + reaf_termination_checker.TerminationChecker + ], + reward_provider: reaf_reward_provider.RewardProvider | None = None, + discount_provider: reaf_discount_provider.DiscountProvider | None = None, + features_observers: Sequence[ + reaf_features_observers.FeaturesObserver + ] = (), + loggers: Sequence[reaf_logger.Logger] = (), + ): + """Initializes the task logic layer. + + Args: + commands_processors: `CommandsProcessor`s that modify the commands before + being sent down to the DACL. They are called sequentially, starting from + the commands supplied by the policy and ending with the commands that + will be sent to the DACL. + features_producers: `FeaturesProducer`s that generate new features. + Measurements collected by the DACL and features produced by these + `FeaturesProducer`s are then merged into the final feature set that is + provided to the `reward_provider`, `termination_checkers`, + `discount_provider`, `features_observers`, and `loggers`. + termination_checkers: `TerminationChecker`s that check the episode + termination based on the final feature set. + reward_provider: `RewardProvider` that computes a reward based on the + final feature set. If None, the ZeroRewardProvider is used and the + reward is set to 0. + discount_provider: `DiscountProvider` that compute a discount based on the + final feature set and final termination state. If None, the + DefaultDiscountProvider is used returning 0 for termination and 1 for + truncation and non-termination. + features_observers: `FeaturesObserver`s that get a view over the final + feature set. + loggers: `Logger`s for logging measurements, features, and commands in the + task layer. + """ + self._commands_processors = commands_processors + self._features_producers = features_producers + self._reward_provider = ( + reward_provider + if reward_provider + else zero_reward_provider.ZeroRewardProvider() + ) + self._termination_checkers = termination_checkers + self._discount_provider = ( + discount_provider + if discount_provider + else default_discount_provider.DefaultDiscountProvider() + ) + self._features_observers = features_observers + self._loggers = list(loggers) + + # We make a set of all resettable objects so that these objects only get + # their resets called once. This is important for e.g. when having a single + # object that derives from two interfaces. + self._resettable_objects: list[_ResettableObject] = [] + unique_ids = set() + for resettable_object in itertools.chain( + self._commands_processors, + self._features_producers, + self._termination_checkers, + [self._reward_provider], + [self._discount_provider], + ): + resettable_object_id = id(resettable_object) + if resettable_object_id not in unique_ids: + unique_ids.add(resettable_object_id) + self._resettable_objects.append(resettable_object) + + def validate_spec( + self, + *, + dacl_commands_spec: Mapping[str, gdmr_types.AnyArraySpec], + dacl_measurements_spec: Mapping[str, specs.Array], + ) -> None: + """Checks that the specs have consistent keys.""" + logging.vlog(3, "Validate features processing") + self._validate_features_spec(dacl_measurements_spec) + self._validate_commands_spec(dacl_commands_spec) + + def features_spec( + self, + dacl_measurements_spec: Mapping[str, specs.Array], + ) -> Mapping[str, specs.Array]: + """Returns the features spec as exposed by the task layer.""" + spec = dict(dacl_measurements_spec) + for features_producer in self._features_producers: + spec.update(features_producer.produced_features_spec()) + + return spec + + def commands_spec( + self, dacl_commands_spec: Mapping[str, gdmr_types.AnyArraySpec] + ) -> Mapping[str, gdmr_types.AnyArraySpec]: + """Returns the commands spec exposed by the task layer.""" + # Each processor consumes commands (as described by its + # `consumed_commands_spec`) and outputs a potentially different set of + # commands (as described by its `produced_commands_keys`). + # Starting with the DACL command spec, we iterate in reverse order (i.e. in + # the direction DACL -> Policy) through every processor to remove the + # `produced_commands_keys` from the spec, and add their + # `consumed_commands_spec` to the spec. + spec: Mapping[str, gdmr_types.AnyArraySpec] = dict(dacl_commands_spec) + for processor in reversed(self._commands_processors): + processor_produced_keys = processor.produced_commands_keys() + spec = { + key: value + for key, value in spec.items() + if key not in processor_produced_keys + } + spec.update(processor.consumed_commands_spec()) + return spec + + def reward_spec(self) -> tree.Structure[specs.Array]: + return self._reward_provider.reward_spec() + + def discount_spec(self) -> tree.Structure[specs.Array]: + return self._discount_provider.discount_spec() + + def perform_reset(self) -> None: + """Reset the internal state of the task logic layer.""" + for resettable_object in self._resettable_objects: + resettable_object.reset() + + def compute_all_features( + self, measurements: Mapping[str, gdmr_types.ArrayType] + ) -> Mapping[str, gdmr_types.ArrayType]: + """Computes all the task logic features given the current measurements.""" + for logger in self._loggers: + logger.record_measurements(measurements) + + # Produce all the features. + current_features = dict(measurements) + for feature_producer in self._features_producers: + required_features = { + key: current_features[key] + for key in feature_producer.required_features_keys() + } + current_features.update( + feature_producer.produce_features(required_features) + ) + + # Observe the features. + for feature_observer in self._features_observers: + feature_observer.observe_features(current_features) + + # Log the resulting features. + for logger in self._loggers: + logger.record_features(current_features) + return current_features + + def compute_final_commands( + self, + policy_commands: Mapping[str, gdmr_types.ArrayType], + ) -> Mapping[str, gdmr_types.ArrayType]: + """Processes the policy commands and returns the final processed commands.""" + current_commands = dict(policy_commands) + for processor in self._commands_processors: + # Get commands to be consumed by the processor and remove the commands + # from the current_commands.. They correspond to the + # `consumed_command_spec`. + consumed_commands = { + key: current_commands.pop(key) + for key in processor.consumed_commands_spec().keys() + } + produced_commands = processor.process_commands(consumed_commands) + current_commands.update(produced_commands) + + # Log the modification. + for logger in self._loggers: + logger.record_commands_processing( + processor.name, consumed_commands, produced_commands + ) + + for logger in self._loggers: + logger.record_final_commands(current_commands) + return current_commands + + def compute_reward( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> tree.Structure[gdmr_types.ArrayType]: + """Computes the reward given the features.""" + return self._reward_provider.compute_reward({ + key: features[key] + for key in self._reward_provider.required_features_keys() + }) + + def check_for_termination( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> reaf_termination_checker.TerminationResult: + """Checks for termination.""" + current_state = reaf_termination_checker.TerminationResult.DO_NOT_TERMINATE + for termination_checker in self._termination_checkers: + current_state = reaf_termination_checker.TerminationResult.combine( + current_state, + termination_checker.check_termination({ + key: features[key] + for key in termination_checker.required_features_keys() + }), + ) + return current_state + + def compute_discount( + self, + features: Mapping[str, gdmr_types.ArrayType], + termination_state: reaf_termination_checker.TerminationResult, + ) -> tree.Structure[gdmr_types.ArrayType]: + """Computes the discount given the features and termination state.""" + return self._discount_provider.compute_discount( + { + key: features[key] + for key in self._discount_provider.required_features_keys() + }, + termination_state, + ) + + def add_logger(self, logger: reaf_logger.Logger) -> None: + self._loggers.append(logger) + + def remove_logger(self, logger: reaf_logger.Logger) -> None: + self._loggers.remove(logger) + + def _validate_features_spec( + self, dacl_measurements_spec: Mapping[str, specs.Array] + ) -> None: + """Validates the features spec.""" + # Check measurements/features path. + current_key_set = set(dacl_measurements_spec.keys()) + logging.vlog(4, "DACL measurements keys: %s", current_key_set) + + for producer in self._features_producers: + logging.vlog( + 4, + "Producer %s requires %s.", + producer.name, + producer.required_features_keys(), + ) + # Check required features are available. + if not producer.required_features_keys().issubset(current_key_set): + raise ValueError( + "Failed to validate feature specs for feature producer" + f" {producer.name}. Missing keys:" + f" {producer.required_features_keys() - current_key_set}" + ) + # Check that there are not duplicates in the output. + if not current_key_set.isdisjoint( + producer.produced_features_spec().keys() + ): + raise ValueError( + "Failed to validate feature specs for feature producer" + f" {producer.name}. Duplicate keys:" + f" {current_key_set & producer.produced_features_spec().keys()}" + ) + # Now extend the spec. + logging.vlog( + 4, + "Update available keys (from producer %s) with %s.", + producer.name, + producer.produced_features_spec().keys(), + ) + current_key_set.update(producer.produced_features_spec().keys()) + logging.vlog(4, "Available features keys %s.", current_key_set) + + def _validate_commands_spec( + self, dacl_commands_spec: Mapping[str, gdmr_types.AnyArraySpec] + ) -> None: + """Validates the commands spec.""" + # Check commands. Starting from the DACL command specs we propagate up in + # the chain. + logging.vlog(3, "Validate commands processing from DACL to Policy.") + current_key_set = set(dacl_commands_spec.keys()) + logging.vlog(4, "DACL commands keys: %s", current_key_set) + + for processor in reversed(self._commands_processors): + produced_command_keys = processor.produced_commands_keys() + + logging.vlog( + 4, + "Processor %s: specs (accepted keys) %s. Exposes %s.", + processor.name, + processor.consumed_commands_spec().keys(), + produced_command_keys, + ) + if not produced_command_keys.issubset(current_key_set): + raise ValueError( + "Failed to validate commands specs for commands processor" + f" {processor.name}. Missing (consumable) keys:" + f" {produced_command_keys - current_key_set}" + ) + # Remove the produced keys and add the consumed commands specs (as the + # processor is mutable). + current_key_set = current_key_set - produced_command_keys + current_key_set.update(processor.consumed_commands_spec().keys()) diff --git a/src/experimental/reaf/core/termination_checker.py b/src/experimental/reaf/core/termination_checker.py new file mode 100644 index 00000000..754c250d --- /dev/null +++ b/src/experimental/reaf/core/termination_checker.py @@ -0,0 +1,94 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Checks if the episode should terminate.""" + +import abc +from collections.abc import Mapping +import enum +from typing import Self + +from gdm_robotics.interfaces import types as gdmr_types + + +class TerminationResult(enum.IntFlag): + """The result of an episode termination check. + + The TerminationResult refers to the possibility for an episode to terminate. + For more details on the concept of termination we refer the readers to + https://github.com/google-deepmind/dm_env/blob/master/docs/index.md#environment-api-and-semantics. + + Note that this enum does not refer to the possible causes of termination but + only how the termination impacts the learning process. + + The result can be one of the following options: + - DO_NOT_TERMINATE: The episode should not terminate. + - TRUNCATE: The epsisode should terminate. Truncation implies a non-failure + final state. Usually this is associated with a non-zero discount. + - TERMINATE: The episode should terminate as the environment is in some + final state. Usually this is associated with a zero discount for e.g. + finite-horizon RL. + """ + + DO_NOT_TERMINATE = 0 + TRUNCATE = 2**0 + TERMINATE = 2**1 + + def is_terminated(self) -> bool: + return self == TerminationResult.TERMINATE + + def is_truncated(self) -> bool: + return self == TerminationResult.TRUNCATE + + def combine(self, other: Self) -> Self: + # TERMINATE has precedence over TRUNCATE, which in turn has precedence over + # DO_NOT_TERMINATE. Given the definitions above, this can be implemented as + # a maximum operator. To also enable tracing with JAX, we implement this in + # a branchless manner using bitwise operations that preserve the type. + # Note that JAX will trace TerminationResult values as ints. + # Approach: + # - self ^ (self ^ other) == other + # - (-1 * (self < other)) will be bitmask of all 1s iff self < other. + # - AND with (self ^ other) will result in either update or no-op bitmask. + return self ^ ((self ^ other) & (-1 * (self < other))) + + +class TerminationChecker(abc.ABC): + """Checks if the episode should terminate.""" + + @abc.abstractmethod + def name(self) -> str: + """Returns a unique string identifier for this object.""" + + @abc.abstractmethod + def check_termination( + self, required_features: Mapping[str, gdmr_types.ArrayType] + ) -> TerminationResult: + """Checks if the episode should terminate. + + Args: + required_features: Measurements and features computed by the task logic + that are required by this checker, i.e. that have keys specified by + `required_features_keys`. + + Returns if the episode should terminate (and if so, what kind of + termination). + """ + + @abc.abstractmethod + def required_features_keys(self) -> set[str]: + """Returns the feature keys that are required to check the termination.""" + + def reset(self) -> None: + """Resets the internal state of the termination checker.""" + ... diff --git a/src/experimental/reaf/core/trigger.py b/src/experimental/reaf/core/trigger.py new file mode 100644 index 00000000..20901873 --- /dev/null +++ b/src/experimental/reaf/core/trigger.py @@ -0,0 +1,29 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Defines an event-based waiting behaviour.""" + +import abc + + +class Trigger(abc.ABC): + """Defines an event-based waiting behaviour.""" + + @property + @abc.abstractmethod + def name(self) -> str: + """Returns the name of the trigger.""" + + @abc.abstractmethod + def wait_for_event(self) -> None: + """Blocks until the next event.""" diff --git a/src/experimental/reaf/core/zero_reward_provider.py b/src/experimental/reaf/core/zero_reward_provider.py new file mode 100644 index 00000000..c5d2267a --- /dev/null +++ b/src/experimental/reaf/core/zero_reward_provider.py @@ -0,0 +1,48 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Reward provider which provides a zero reward.""" + +from collections.abc import Mapping +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types +import numpy as np +from reaf.core import reward_provider +import tree + + +class ZeroRewardProvider(reward_provider.RewardProvider): + """Reward provider which provides a zero reward.""" + + def __init__(self, name: str = 'zero_reward_provider'): + self._name = name + + def name(self) -> str: + return self._name + + def compute_reward( + self, required_features: Mapping[str, gdmr_types.ArrayType] + ) -> tree.Structure[gdmr_types.ArrayType]: + """Returns a zero reward.""" + return np.zeros(1) + + def reward_spec(self) -> tree.Structure[specs.Array]: + """Returns the spec for a constant zero reward.""" + return specs.Array(shape=(1,), dtype=float) + + def required_features_keys(self) -> set[str]: + """Returns empty set. + + There are no feature keys that are required to compute the reward. + """ + return set() From c08d181d5275acee725c3f09dd10253a2b5411b3 Mon Sep 17 00:00:00 2001 From: Tom Erez Date: Fri, 10 Apr 2026 15:08:31 -0700 Subject: [PATCH 041/251] Add protocols for authoring simulation environments. This is for preview only, we discourage users from using this in production code. PiperOrigin-RevId: 897908600 Change-Id: Ib2fa1ca8527665a99a8f31e9320afb5f54c31035 --- doc/SKILL.md | 876 ------------------ .../reaf/core/action_space_adapter.py | 43 - .../reaf/core/commands_processor.py | 91 -- .../data_acquisition_and_control_layer.py | 170 ---- .../reaf/core/default_discount_provider.py | 79 -- .../core/default_observation_space_adapter.py | 231 ----- src/experimental/reaf/core/device.py | 54 -- .../reaf/core/device_coordinator.py | 87 -- .../reaf/core/discount_provider.py | 61 -- src/experimental/reaf/core/entity.py | 65 -- src/experimental/reaf/core/environment.py | 490 ---------- .../reaf/core/features_observer.py | 34 - .../reaf/core/features_producer.py | 56 -- src/experimental/reaf/core/logger.py | 80 -- .../reaf/core/numpy_mock_assertions.py | 98 -- .../reaf/core/observation_space_adapter.py | 42 - .../core/pass_through_action_space_adapter.py | 55 -- src/experimental/reaf/core/reward_provider.py | 292 ------ .../reaf/core/substep_commands_processor.py | 104 --- .../core/substep_measurements_processor.py | 103 -- .../reaf/core/task_logic_layer.py | 342 ------- .../reaf/core/termination_checker.py | 94 -- src/experimental/reaf/core/trigger.py | 29 - .../reaf/core/zero_reward_provider.py | 48 - 24 files changed, 3624 deletions(-) delete mode 100644 doc/SKILL.md delete mode 100644 src/experimental/reaf/core/action_space_adapter.py delete mode 100644 src/experimental/reaf/core/commands_processor.py delete mode 100644 src/experimental/reaf/core/data_acquisition_and_control_layer.py delete mode 100644 src/experimental/reaf/core/default_discount_provider.py delete mode 100644 src/experimental/reaf/core/default_observation_space_adapter.py delete mode 100644 src/experimental/reaf/core/device.py delete mode 100644 src/experimental/reaf/core/device_coordinator.py delete mode 100644 src/experimental/reaf/core/discount_provider.py delete mode 100644 src/experimental/reaf/core/entity.py delete mode 100644 src/experimental/reaf/core/environment.py delete mode 100644 src/experimental/reaf/core/features_observer.py delete mode 100644 src/experimental/reaf/core/features_producer.py delete mode 100644 src/experimental/reaf/core/logger.py delete mode 100644 src/experimental/reaf/core/numpy_mock_assertions.py delete mode 100644 src/experimental/reaf/core/observation_space_adapter.py delete mode 100644 src/experimental/reaf/core/pass_through_action_space_adapter.py delete mode 100644 src/experimental/reaf/core/reward_provider.py delete mode 100644 src/experimental/reaf/core/substep_commands_processor.py delete mode 100644 src/experimental/reaf/core/substep_measurements_processor.py delete mode 100644 src/experimental/reaf/core/task_logic_layer.py delete mode 100644 src/experimental/reaf/core/termination_checker.py delete mode 100644 src/experimental/reaf/core/trigger.py delete mode 100644 src/experimental/reaf/core/zero_reward_provider.py diff --git a/doc/SKILL.md b/doc/SKILL.md deleted file mode 100644 index ecd4f464..00000000 --- a/doc/SKILL.md +++ /dev/null @@ -1,876 +0,0 @@ ---- -name: mujoco-python -description: > - Build, manipulate, and simulate MuJoCo physics models using the Python - bindings (MjSpec, MjModel, MjData). Covers choosing between compute - backends (C++ for full features and noslip, MJWarp for GPU batch RL). Use when constructing scenes - programmatically via the spec API, compiling and stepping simulations, - reading sensor/body/geom data, attaching sub-models, composing specs with - prefixed names, using contact sensors for fixed-size observation spaces, - configuring collision filtering, offscreen rendering (context management, - cameras, depth/segmentation), or performing spatial math (quaternion, pose, - rotation conversions via mju_). Covers gotchas around compilation - lifecycle, named indexing vs bind, geom size semantics, camera conventions, - and orientation representations. ---- - -# MuJoCo Python Bindings - -## Compilation Lifecycle - -``` -MjSpec ──spec.compile()──▶ MjModel ──MjData(model)──▶ MjData - │ │ │ - │ (mutable blueprint) │ (compiled, mostly frozen) │ (simulation state) - │ │ │ - └── spec.recompile(m, d) ─────┴────────────────────────────┘ -``` - -1. **MjSpec** — mutable data structure you edit to define the simulation. -2. **`spec.compile()`** — produces `MjModel` + you create `MjData(model)`. - After this, changing the spec has **no effect** until you recompile. -3. **Most `MjModel` fields are unsafe to mutate.** Changing them requires - `spec.recompile(model, data)`, which returns **new** model and data objects - (preserving physics state for existing elements). - -```python -import mujoco - -spec = mujoco.MjSpec() -body = spec.worldbody.add_body(pos=[0, 0, 1]) -geom = body.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE, size=[0.1]) -body.add_freejoint() - -model = spec.compile() -data = mujoco.MjData(model) -mujoco.mj_forward(model, data) - -# Later: add another body, recompile keeping state -body2 = spec.worldbody.add_body(pos=[1, 0, 1]) -body2.add_geom(size=[0.1]) -body2.add_freejoint() -model, data = spec.recompile(model, data) # state preserved -``` - -> [!CAUTION] -> `recompile` returns **new** objects. Always reassign: `model, data = spec.recompile(model, data)`. - -### Loading and Serializing - -```python -spec = mujoco.MjSpec() # empty -spec = mujoco.MjSpec.from_string(xml_string) # from XML string -spec = mujoco.MjSpec.from_file('/path/to.xml') # from file -model = mujoco.MjModel.from_xml_string(xml) # direct to model (no spec) - -xml_out = spec.to_xml() # serialize back -``` - -### Compile Error Debugging - -Use the `.info` field on spec elements for traceability: - -```python -geom = spec.worldbody.add_geom() -geom.info = 'created at my_file.py:42' -spec.compile() # Error: "size 0 must be positive in geom\nElement name '', id 0, created at my_file.py:42" -``` - ---- - -## Compute Backends - -MuJoCo has two compute backends. **Choose early** — the backend determines -which features, solvers, and APIs are available. - -| | C++ (default) | MJWarp (NVIDIA GPU) | -|---|---|---| -| **Import** | `import mujoco` | `import mujoco_warp as mjw` | -| **Optimized for** | Latency (single scene) | Throughput (big batches) | -| **Hardware** | CPU | NVIDIA GPU | -| **Solvers** | All (Newton, CG, PGS, **noslip**) | All except PGS, **noslip**, islands | -| **Plugins** | ✅ All | SDF only | -| **Precision** | float64 | float32 | -| **Named access / bind** | ✅ | Via wrapper libraries or MJX `bind()` | -| **Contact sensors** | ✅ | ✅ | -| **Sparse Jacobians** | ✅ | ❌ (dense only) | -| **Batch rendering** | ❌ | ✅ (BVH ray tracing) | - -### When to use which - -- **C++ (default)**: Real-time control, model predictive control, interactive - visualization, any workflow needing full feature support (noslip solver, - PGS, islands, plugins, ellipsoidal fluid model, sparse Jacobians). Also the - only backend with native `bind()`. Use this unless you need massive - parallelism. (For MJWarp, named access is available via wrapper - libraries or MJX `bind()`.) - -- **MJWarp**: Reinforcement learning with large batch sizes on NVIDIA GPUs. - Scales better for contact-rich scenes and large meshes than the legacy - MJX-JAX backend. Not differentiable. May degrade for scenes beyond ~60 DoFs. - -> [!IMPORTANT] -> The `noslip` solver (post-constraint velocity correction for exact zero -> slip at contacts) is **only available in the C++ backend**. If your task -> requires accurate friction modeling without any tangential sliding at -> contacts, you must use C++. - -> [!WARNING] -> MJWarp uses **float32**, which can cause numerical differences vs C++ -> (float64). Solver convergence, small friction values, and long rollouts -> may be sensitive to this. If you see NaNs or instability on GPU, try -> increasing solver iterations or simplifying the model. - ---- - -## Building Models with MjSpec - -### Adding elements - -Most `add_*` methods accept keyword arguments matching MJCF XML attributes: - -```python -spec = mujoco.MjSpec() - -body = spec.worldbody.add_body(name='arm', pos=[0, 0, 1], quat=[1, 0, 0, 0]) -geom = body.add_geom( - name='arm_geom', - type=mujoco.mjtGeom.mjGEOM_CAPSULE, - size=[0.05, 0.3], - rgba=[1, 0, 0, 1], -) -joint = body.add_joint( - name='hinge1', - type=mujoco.mjtJoint.mjJNT_HINGE, - axis=[0, 1, 0], - range=[-1.57, 1.57], -) -site = body.add_site(name='sensor_site', pos=[0, 0, 0.3]) -cam = body.add_camera(name='arm_cam', pos=[0, -2, 0], xyaxes=[1,0,0, 0,0,1]) -``` - -### Orientation alternatives - -In addition to `quat`, you can specify orientation with `euler`, `axisangle`, -`xyaxes`, or `zaxis`. Only one can be set at a time: - -```python -body.add_geom(euler=[0, 90, 0]) # Euler angles (degrees by default) -body.add_geom(axisangle=[0, 1, 0, 1.57]) # axis + angle -body.add_geom(zaxis=[0, 1, 0]) # minimal rotation to align Z -body.add_geom(xyaxes=[1,0,0, 0,0,1]) # explicit X and Y axes -``` - -### Top-level elements - -Sensors, actuators, tendons, materials, textures, and meshes are added directly -to the spec (not to bodies): - -```python -spec.add_material(name='red', rgba=[1, 0, 0, 1]) -spec.add_actuator(name='motor', joint=joint.name, gear=[1, 0, 0, 0, 0, 0]) -spec.add_sensor( - name='joint_pos', - type=mujoco.mjtSensor.mjSENS_JOINTPOS, - objtype=mujoco.mjtObj.mjOBJ_JOINT, - objname=joint.name, -) -``` - -> [!IMPORTANT] -> Always use `element.name` (e.g., `joint.name`, `geom.name`, `site.name`) -> instead of hardcoded strings when referencing spec elements. This keeps -> references correct if the element is renamed or attached with a prefix. - -### Geom size semantics - -| Type | Size params | -| --------- | ----------------------------------------------- | -| sphere | `[radius]` | -| capsule | `[radius, half_length]` or `[radius]` + fromto | -| cylinder | `[radius, half_length]` or `[radius]` + fromto | -| box | `[half_x, half_y, half_z]` | -| ellipsoid | `[radius_x, radius_y, radius_z]` | -| plane | `[half_x, half_y, grid_spacing]` | - -> [!WARNING] -> Capsule/cylinder `size` changes meaning with `fromto`. Without `fromto`, -> `size=[radius, half_length]`. With `fromto`, `size=[radius]` only — the -> length is computed from the two endpoints. - ---- - -## Accessing Compiled Data: Named Access vs Bind - -There are **two** recommended ways to read/write compiled model and data fields. -**Prefer `bind`** when working with spec elements; use **named access** otherwise. - -### 1. Named Access (on MjModel / MjData) - -```python -model.geom('my_geom').size # → numpy view of geom_size for 'my_geom' -data.body('torso').xpos # → numpy view of body_xpos -data.joint('knee').qpos # → shape depends on joint type -data.actuator('motor').ctrl = 1.0 # writable view -``` - -Aliases: `joint` / `jnt`, `camera` / `cam`, `tendon` / `ten`, `material` / `mat`, -`texture` / `tex`, `equality` / `eq`, `keyframe` / `key`. - -> [!WARNING] -> Named access returns **views, not copies.** After `mj_step`, old references -> reflect new values. Use `.copy()` when logging: -> `positions.append(data.body('torso').xpos.copy())` - -### 2. Bind (bridges MjSpec elements → MjModel / MjData) - -`bind()` connects spec elements (or lists of them) to their compiled -counterparts. **Use `.set()` to write through bind:** - -```python -geom = spec.worldbody.add_geom(name='ball', size=[0.1], type=mujoco.mjtGeom.mjGEOM_SPHERE) -joint = body.add_joint(name='j1', type=mujoco.mjtJoint.mjJNT_HINGE) -model = spec.compile() -data = mujoco.MjData(model) -mujoco.mj_forward(model, data) - -# Reading via bind -model.bind(geom).size # → array([0.1, 0., 0.]) -data.bind(geom).xpos # → array([0., 0., 0.]) - -# Writing via bind — always use .set() -data.bind(joint).set('qpos', 1.5) # sets the joint's qpos - -# Bind a list of spec elements -joints = [spec.joint('j1'), spec.joint('j2')] -data.bind(joints).qpos # → concatenated array -data.bind(joints).set('qpos', np.array([0.5, 1.0])) # write to both -``` - -> [!CAUTION] -> The spec must match the compiled model. If you modify the spec after -> `compile()`, you must recompile before calling `bind()`, or you get: -> `ValueError: 'The mjSpec does not match mjModel. Please recompile the mjSpec.'` - ---- - -## Attachments: Composing Specs - -Attach child specs/bodies to parent specs via frames or sites: - -```python -parent = mujoco.MjSpec() -child = mujoco.MjSpec() -child_body = child.worldbody.add_body(name='arm') -child_body.add_geom(name='arm_geom', size=[0.05, 0.3], type=mujoco.mjtGeom.mjGEOM_CAPSULE) -child_body.add_joint(name='arm_joint', type=mujoco.mjtJoint.mjJNT_HINGE) - -frame = parent.worldbody.add_frame(pos=[0, 0, 1]) -frame.attach_body(child_body, prefix='left_') -# 'arm' → 'left_arm', 'arm_geom' → 'left_arm_geom', 'arm_joint' → 'left_arm_joint' - -# Or attach entire spec to a site -site = parent.worldbody.add_site(name='attach_point', pos=[0, 0, 2]) -parent.attach(child, site=site, prefix='right_', suffix='_v2') -``` - -> [!IMPORTANT] -> **Cross-spec references require a shared parent.** -> If you need to create an element (e.g., an equality constraint) that -> references elements from *two different child specs*, you must first -> attach both children to the same parent, then add the cross-referencing -> element to the **parent** spec using the final prefixed/suffixed names: - -```python -# Two robot arms, each defined as a separate spec -arm_spec = mujoco.MjSpec() -arm_body = arm_spec.worldbody.add_body(name='hand') -arm_body.add_geom(name='hand_geom', size=[0.05]) -wrist_joint = arm_body.add_joint(name='wrist', type=mujoco.mjtJoint.mjJNT_HINGE) - -# Attach both to the parent with different prefixes -parent = mujoco.MjSpec() -left_prefix, right_prefix = 'left_', 'right_' - -frame_l = parent.worldbody.add_frame(pos=[-0.5, 0, 1]) -frame_l.attach_body(arm_body, prefix=left_prefix) # left_wrist, left_hand, ... - -frame_r = parent.worldbody.add_frame(pos=[0.5, 0, 1]) -frame_r.attach_body(arm_body, prefix=right_prefix) # right_wrist, right_hand, ... - -# NOW add a constraint linking both arms — look up the prefixed joints -# from the parent spec, don't hardcode the names -left_wrist = parent.joint(f'{left_prefix}{wrist_joint.name}') -right_wrist = parent.joint(f'{right_prefix}{wrist_joint.name}') -parent.add_equality(type=mujoco.mjtEq.mjEQ_JOINT, - name1=left_wrist.name, name2=right_wrist.name) -model = parent.compile() -``` - -### Attachment Transforms - -When attaching to a site or frame, the child body's position is transformed -relative to the parent's attachment point. Attachment also handles unit -conversion (degrees vs radians) between parent and child specs automatically. - -### Assets Get Renamed Too - -Prefix/suffix changes apply to asset filenames: -```python -child.assets = {'mesh.obj': data} -parent.attach(child, prefix='robot_') -# Asset key becomes 'robot_mesh.obj' in parent -``` - ---- - -## Cameras - -### Orientation - -MuJoCo cameras look down the **negative Z axis**. The camera frame is: -- **-Z** → forward (viewing direction) -- **+X** → right -- **+Y** → up - -To point a camera downward (looking at the ground), set its Z axis to `[0, 0, 1]`: - -```python -body.add_camera( - name='overhead', - xyaxes=[1, 0, 0, 0, 1, 0], # x=[1,0,0], y=[0,1,0] → z=[0,0,1] → looks DOWN (-z) - pos=[0, 0, 5], -) -``` - -### Geom Group Visibility - -Each camera/viewer has 6 geom groups (0–5). Default visibility: - -| Group | Default Visible | Typical Use | -|-------|----------------|-------------| -| 0 | ✅ Yes | Standard geoms (default group for new geoms) | -| 1 | ✅ Yes | Secondary visual geoms | -| 2 | ✅ Yes | Tertiary visual geoms | -| 3 | ❌ No | Collision-only or debug geoms | -| 4 | ❌ No | Hidden geoms | -| 5 | ❌ No | Hidden geoms | - -A newly created geom is in **group 0** by default. Toggle visibility at runtime -via `mjvOption.geomgroup[i]`. The same 3-on/3-off default applies to sites, -joints, tendons, actuators, flexes, and skins. - ---- - -## Contacts: Use Sensors, Not the Contact Array - -### The problem with `data.contact` - -`data.contact` is a **variable-length** array that changes size every timestep -depending on what's colliding. Iterating over it directly is fragile and -**incompatible with learning-based agents** and fixed-size observation spaces. - -```python -# ❌ WRONG — don't iterate data.contact for reward/observation logic -for c in data.contact: - if c.geom1 == target_geom_id: - force = ... # fragile, variable-length, non-deterministic order -``` - -> [!CAUTION] -> Never iterate `data.contact` to build observations or compute rewards. -> The array's length and ordering can change between timesteps and even -> between MuJoCo versions. Use **contact sensors** instead. - -### Contact sensors: fixed-size, declarative contact queries - -A `` sensor selects contacts via declarative matching criteria, reduces -them to a fixed number of slots, and extracts requested data fields into -`data.sensordata` — always the same size, every timestep. - -The pipeline has three stages: -1. **Matching** — filter contacts by geom, body, subtree, or site volume -2. **Reduction** — keep the top `num` contacts (by order, min distance, max force, or net force) -3. **Extraction** — copy requested fields (`found`, `force`, `torque`, `dist`, `pos`, `normal`, `tangent`) - -### Example: detect contact force between a gripper and an object - -```python -import mujoco -import numpy as np - -spec = mujoco.MjSpec() - -# Build a simple scene: floor + falling object -floor = spec.worldbody.add_geom( - name='floor', type=mujoco.mjtGeom.mjGEOM_PLANE, size=[1, 1, 0.01] -) -obj_body = spec.worldbody.add_body(name='obj', pos=[0, 0, 0.5]) -obj_body.add_freejoint() -obj_geom = obj_body.add_geom( - name='obj_geom', type=mujoco.mjtGeom.mjGEOM_SPHERE, - size=[0.05], mass=0.1, -) - -# Add a contact sensor: report force for contacts involving obj_geom -contact_sensor = spec.add_sensor( - name='obj_contact', - type=mujoco.mjtSensor.mjSENS_CONTACT, - # Match any contact involving this geom — use .name, not a literal string: - objname=obj_geom.name, objtype=mujoco.mjtObj.mjOBJ_GEOM, -) - -model = spec.compile() -data = mujoco.MjData(model) - -# Step the simulation until the object lands -mujoco.mj_step(model, data, nstep=500) -mujoco.mj_forward(model, data) - -# Read the contact sensor via bind — always fixed-size in data.sensordata -contact_data = data.bind(contact_sensor).sensordata -print(f'Contact sensor output: {contact_data}') -``` - -### XML-based contact sensor (common pattern) - -When loading from XML, contact sensors are even cleaner: - -```xml - - - - - - - -``` - -The output size is deterministic: `num × size(data fields)`. For `"found force -normal"` with `num=3`, you get 3 × (1+3+3) = 21 numbers every timestep, padded -with zeros if fewer contacts match. - -### Touch sensor: simpler alternative for scalar normal force - -If you only need a scalar "how hard is something pressing on this site", use a -`touch` sensor instead: - -```python -site = body.add_site(name='fingertip', pos=[0, 0, 0.05], size=[0.02]) -spec.add_sensor( - name='fingertip_touch', - type=mujoco.mjtSensor.mjSENS_TOUCH, - objname=site.name, objtype=mujoco.mjtObj.mjOBJ_SITE, -) -``` - -The touch sensor sums normal contact forces within the site volume — one scalar -output, always present in `sensordata`. - ---- - -## Spatial Math Utilities (mju_) - -MuJoCo ships a library of spatial computation functions under the `mju_` -namespace — quaternion algebra, rotation conversions, pose composition, and -coordinate transforms. **Always check for an existing `mju_` function before -implementing spatial math from scratch.** For basic vector arithmetic (add, -subtract, dot product, norm), just use NumPy/JAX/Torch directly. - -### Quaternion Operations - -```python -res = np.zeros(3) -mujoco.mju_rotVecQuat(res, vec, quat) # rotate vector by quaternion - -quat = np.zeros(4) -mujoco.mju_mat2Quat(quat, mat3x3) # 3x3 rotation matrix → quaternion -mujoco.mju_quat2Mat(mat, quat) # quaternion → 3x3 matrix -mujoco.mju_axisAngle2Quat(quat, axis, angle) # axis-angle → quaternion -mujoco.mju_euler2Quat(quat, euler, 'xyz') # Euler angles → quaternion -mujoco.mju_mulQuat(res, q1, q2) # multiply quaternions -mujoco.mju_negQuat(res, quat) # conjugate -mujoco.mju_quatZ2Vec(quat, vec) # quat that rotates z-axis to vec -mujoco.mju_quatIntegrate(quat, vel, scale) # integrate quat with angular velocity -``` - -> [!TIP] -> `mju_quatZ2Vec` is particularly useful: given a target direction vector, it -> returns the quaternion that rotates the Z-axis to point in that direction. - -### Pose Operations - -```python -mujoco.mju_mulPose(pos_res, quat_res, pos1, quat1, pos2, quat2) # compose poses -mujoco.mju_negPose(pos_res, quat_res, pos, quat) # invert pose -mujoco.mju_trnVecPose(res, pos, quat, vec) # transform vector by pose -``` - ---- - -## Common Gotchas - -### 1. Computed fields are read-only - -`data.xpos`, `data.xmat`, `data.xquat`, `data.geom_xpos` are **output** fields -computed by `mj_forward()`. You cannot assign to them directly. Instead, modify -input fields (`data.qpos`, `data.qvel`, `data.ctrl`) and call `mj_forward()` or -`mj_step()`. - -### 2. Duplicate names are forbidden - -```python -spec.add_material(name='yellow') -spec.add_material(name='yellow') # ValueError: "repeated name 'yellow' in material" -``` - -Names must be unique within each element type. - -### 3. Orientation keywords are mutually exclusive - -```python -body.add_geom(axisangle=[1, 0, 0, 1.57], euler=[0, 0, 0]) -# ValueError: 'Only one of: axisangle, xyaxes, zaxis, or euler can be set.' -``` - -Pick one orientation representation. Quaternion (`quat`) is the native format. - -### 4. `size` must be positive for geoms - -A geom with `size[0] == 0` will fail compilation. Always set at least -`size=[radius]` for spheres/capsules, or `size=[hx, hy, hz]` for boxes. - -### 5. `mj_step` with `nstep` repeats the same control - -```python -mujoco.mj_step(model, data, nstep=100) # 100 steps, same ctrl each step -``` - -This is much faster than a Python loop and is fine for passive simulation or -constant-control scenarios. But if you need to update `data.ctrl` between steps, -you must step one at a time. - -### 6. Euler sequence matters - -`mju_euler2Quat` takes a 3-character sequence string. Lowercase = intrinsic -rotations, uppercase = extrinsic: - -```python -mujoco.mju_euler2Quat(quat, [roll, pitch, yaw], 'xyz') # intrinsic x-y-z -mujoco.mju_euler2Quat(quat, [roll, pitch, yaw], 'XYZ') # extrinsic X-Y-Z -``` - -The sequence must be exactly 3 characters from `xyzXYZ`. - -### 7. `copy()` vs view semantics - -NumPy arrays from MjModel/MjData are **views** into C memory. `mj_step` changes -them in-place. Always `.copy()` when storing values for later comparison. - -### 8. Default class handling - -```python -main = spec.default # global default class (always named 'main') -child_class = spec.add_default('high_friction', main) -child_class.geom.friction = [1.5, 0.005, 0.0001] - -geom = body.add_geom(child_class) # use specific default class -geom = body.add_geom() # uses 'main' class implicitly -``` - -### 9. Gravity is -Z by default - -MuJoCo convention: **+Z is up**, gravity is `[0, 0, -9.81]`. The viewer and -all built-in models assume this. Don't fight it — orient your scene accordingly. - -### 10. Capsule/cylinder size with and without fromto - -```python -# With explicit pos/quat: size = [radius, half_length] -body.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE, size=[0.05, 0.3]) - -# With fromto: size = [radius] only — length is inferred from endpoints -body.add_geom( - type=mujoco.mjtGeom.mjGEOM_CAPSULE, - size=[0.05], - fromto=[0, 0, 0, 0, 0, 0.6], -) -``` - -### 11. Collision filtering with contype/conaffinity - -Two geoms collide only if `(g1.contype & g2.conaffinity) || (g2.contype & g1.conaffinity)`. -By default both are `1`, so everything collides with everything. - -```python -# Visual-only geom: set contype=0, conaffinity=0 to disable collisions -body.add_geom(size=[0.1], contype=0, conaffinity=0, group=1) - -# Separate collision groups using bitmasks: -robot_geom = body.add_geom(size=[0.05], contype=1, conaffinity=2) -tool_geom = body.add_geom(size=[0.03], contype=2, conaffinity=1) -# Robot and tool collide (1&1=0, but 2&2=0… wait): -# contype=1 & conaffinity=1 → collide; contype=2 & conaffinity=2 → collide -``` - -> [!TIP] -> **`condim` and `friction` interact.** Each geom has `friction=[tangential, torsional, rolling]` -> (default `[1, 0.005, 0.0001]`). The `condim` value controls which friction coefficients are -> *active* in a contact: -> -> | condim | Active friction | Geom `friction` indices used | -> |--------|----------------|------------------------------| -> | 1 | None (frictionless, normal force only) | — | -> | 3 | Tangential (opposes sliding) | `friction[0]` | -> | 4 | Tangential + torsional (opposes sliding and twisting around contact normal) | `friction[0:2]` | -> | 6 | Tangential + torsional + rolling (also opposes rolling around tangent axes) | `friction[0:3]` | -> -> Torsional friction models a surface contact patch resisting twist — useful for soft fingers. -> Rolling friction dissipates energy from local deformations — useful for stopping balls from rolling -> forever. Both torsional and rolling coefficients have **units of length** (roughly the contact -> patch diameter or deformation depth). -> -> ```python -> # A soft finger pad: enable torsional friction for stable grasping -> finger_geom = body.add_geom( -> type=mujoco.mjtGeom.mjGEOM_CAPSULE, -> size=[0.01, 0.02], -> condim=4, -> friction=[1.0, 0.01, 0.0001], # tangential=1.0, torsional=0.01 -> ) -> -> # A ball that should stop rolling on a surface -> ball_geom = body.add_geom( -> type=mujoco.mjtGeom.mjGEOM_SPHERE, -> size=[0.05], -> condim=6, -> friction=[0.8, 0.005, 0.002], # tangential=0.8, torsional=0.005, rolling=0.002 -> ) -> ``` - ---- - -## Offscreen Rendering - -Offscreen rendering produces images (RGB, depth, segmentation) without a -display. It requires an OpenGL context — MuJoCo auto-detects the best -available backend (EGL on headless Linux, GLFW on desktop, OSMesa as -fallback). - -### The `Renderer` class - -`mujoco.Renderer` wraps GL context creation, scene management, and buffer -readback. **Always use it as a context manager** to ensure GPU resources are -freed: - -```python -import mujoco -import numpy as np - -# Define a camera in the spec and keep a reference -overhead_cam = spec.worldbody.add_camera( - name='overhead', - pos=[0, 0, 3], - quat=[0.707, 0.707, 0, 0], # looking down - fovy=60, -) - -model = spec.compile() -data = mujoco.MjData(model) - -# Create renderer — width/height must not exceed offscreen buffer (see below) -with mujoco.Renderer(model, height=480, width=640) as renderer: - mujoco.mj_forward(model, data) - - # Use the spec element's .name — never a literal string - renderer.update_scene(data, camera=overhead_cam.name) - rgb = renderer.render() # → np.ndarray (H, W, 3), dtype=uint8 - - # Depth rendering - renderer.enable_depth_rendering() - renderer.update_scene(data, camera=overhead_cam.name) - depth = renderer.render() # → np.ndarray (H, W), dtype=float32 (meters) - renderer.disable_depth_rendering() - - # Segmentation rendering - renderer.enable_segmentation_rendering() - renderer.update_scene(data, camera=overhead_cam.name) - seg = renderer.render() # → np.ndarray (H, W, 2), dtype=int32 - # seg[:,:,0] = object ID, seg[:,:,1] = object type; background = (-1, -1) - renderer.disable_segmentation_rendering() -``` - -> [!WARNING] -> Forgetting to close the renderer (or not using `with`) leaks GPU memory and -> GL contexts. In loops, create the renderer **once** outside the loop. - -### What the `Renderer` holds internally - -When you create `mujoco.Renderer(model, height, width)`, it allocates three -internal objects that must be freed together: - -1. **`GLContext`** — an offscreen OpenGL context (EGL, GLFW, or OSMesa, - auto-detected). Created with the requested `width × height`. -2. **`MjrContext`** — MuJoCo's GPU rendering resources (shaders, textures, - framebuffers), bound to the GLContext. Set to the offscreen framebuffer. -3. **`MjvScene`** — geometry buffer holding the scene snapshot passed to the - GPU each frame. - -The context manager (`with Renderer(...) as r:`) calls `r.close()` on exit, -which frees the MjrContext first and then the GLContext — **order matters**. -If you use the renderer without `with`, call `renderer.close()` manually. - -> [!CAUTION] -> Internally, `MjrContext.free()` must be called **before** `GLContext.free()`. -> Reversing the order leaks GPU resources or segfaults. The `Renderer` class -> handles this automatically — prefer it over manual context management. - -### Offscreen framebuffer size - -The renderer cannot exceed the offscreen buffer dimensions. The defaults are -640×480. Set larger buffers **before compilation** via `spec.visual`: - -```python -spec.visual.global_.offwidth = 1920 -spec.visual.global_.offheight = 1080 -model = spec.compile() - -# Now you can render up to 1920×1080 -with mujoco.Renderer(model, height=1080, width=1920) as renderer: - ... -``` - -> [!IMPORTANT] -> Increasing offscreen buffer size consumes GPU memory. For batch rendering -> of many cameras, keep the per-frame resolution modest. - -### Cameras - -MuJoCo has two camera systems: **fixed cameras** defined in the model, and -the **free camera** for interactive viewing. - -#### Defining cameras in MjSpec - -Always store the return value of `add_camera` and use its `.name` or `.id` -to reference the camera later — never hardcode literal strings: - -```python -# Fixed camera on worldbody — good for evaluation/recording -overhead_cam = spec.worldbody.add_camera( - name='overhead', - pos=[0, 0, 3], - quat=[0.707, 0.707, 0, 0], - fovy=60, -) - -# Camera attached to a body — moves with the body -wrist_cam = wrist_body.add_camera( - name='wrist_cam', - pos=[0.05, 0, 0], - xyaxes=[0, -1, 0, 0, 0, -1], - fovy=90, -) -``` - -#### Selecting a camera for rendering - -`update_scene` accepts a camera **name** (str), **id** (int), or an -`MjvCamera` object. Always derive from the spec element: - -```python -# By name via spec element (recommended — survives recompilation) -renderer.update_scene(data, camera=overhead_cam.name) - -# By id via spec element (after compile; matches model.cam_* arrays) -renderer.update_scene(data, camera=overhead_cam.id) - -# Free camera (default) — no camera argument needed -renderer.update_scene(data) - -# Custom free camera with explicit lookat/distance/angles -cam = mujoco.MjvCamera() -cam.type = mujoco.mjtCamera.mjCAMERA_FREE -cam.lookat[:] = [0, 0, 0.5] -cam.distance = 3.0 -cam.azimuth = 135 -cam.elevation = -25 -renderer.update_scene(data, camera=cam) -``` - -#### Camera properties reference - -| Property | Type | Description | -|----------|------|-------------| -| `pos` | `real(3)` | Position in parent body frame | -| `quat` | `real(4)` | Orientation quaternion (w, x, y, z) | -| `xyaxes` | `real(6)` | Alternative orientation: `[x_axis(3), y_axis(3)]` | -| `fovy` | `real` | Vertical field of view (degrees, default 45) | -| `resolution` | `int(2)` | Sensor resolution — only for camera-based sensors | -| `targetbody` | `str` | Track this body (camera always looks at it) | -| `mode` | `str` | `"fixed"`, `"track"`, `"trackcom"`, `"targetbody"`, `"targetbodycom"` | - -### Scene options - -Control what is visualized via `MjvOption`: - -```python -scene_option = mujoco.MjvOption() -# geomgroup is a bool array indexed by group number (0–5). -# Each geom's `group` attribute (default 0) assigns it to a group. -# Toggle visibility of each group: -scene_option.geomgroup[:] = False # hide all groups -scene_option.geomgroup[0] = True # show group 0 (e.g. ground plane) -scene_option.geomgroup[3] = True # show group 3 (e.g. visualization geoms) - -# Toggle rendering flags -scene_option.flags[mujoco.mjtVisFlag.mjVIS_CONTACTFORCE] = True -scene_option.flags[mujoco.mjtVisFlag.mjVIS_JOINT] = True - -renderer.update_scene(data, camera=overhead_cam.name, scene_option=scene_option) -``` - -### Filament backend (experimental) - -MuJoCo's default renderer uses OpenGL. An alternative **Filament** backend -(Vulkan-based) is available experimentally and provides higher-quality -rendering. Filament does **not** require vertical flip (`np.flipud` is a -no-op). It is selected via build flags — see the MuJoCo Filament -[source](../src/experimental/filament) for details. - ---- - -## Key References - -### Documentation - -| Document | Description | -|----------|-------------| -| [XMLreference.rst](XMLreference.rst) | Complete MJCF XML element and attribute reference | -| [python.rst](python.rst) | Python bindings API: named access, bind, enums, callbacks | -| [modeling.rst](modeling.rst) | MJCF modeling guide: coordinate frames, defaults, attachments | -| [simulation.rst](programming/simulation.rst) | Simulation loop, state, forward/inverse dynamics | -| [modeledit.rst](programming/modeledit.rst) | Procedural model editing with MjSpec | -| [visualization.rst](programming/visualization.rst) | Rendering, cameras, scene management | -| [APIfunctions.rst](APIreference/APIfunctions.rst) | C API function reference (mj_, mju_, mjv_, mjr_) | -| [APItypes.rst](APIreference/APItypes.rst) | All MuJoCo structs and enums | - -### Test Files (Executable Examples) - -| Test file | Key patterns demonstrated | -|-----------|--------------------------| -| [specs_test.py](../../py/mujoco/specs_test.py) | MjSpec API: compile, recompile, attach, bind, defaults, delete, actuator shortcuts | -| [bindings_test.py](../../py/mujoco/bindings_test.py) | Named indexing, mju_ functions, copy/pickle, contacts, mj_step | -| [support_test.py](../../py/mujoco/mjx/_src/support_test.py) | MJX bind `.set()` pattern, JAX functional updates | - -### Source Code - -| File | Description | -|------|-------------| -| [mujoco.h](../include/mujoco.h) | Main C API header with all mju_ function signatures | -| [XMLschema.rst](XMLschema.rst) | Schema-level XML structure documentation | diff --git a/src/experimental/reaf/core/action_space_adapter.py b/src/experimental/reaf/core/action_space_adapter.py deleted file mode 100644 index 014e02eb..00000000 --- a/src/experimental/reaf/core/action_space_adapter.py +++ /dev/null @@ -1,43 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Adapts environment action into suitable commands format accepted by REAF.""" - -import abc -from collections.abc import Mapping - -from gdm_robotics.interfaces import types as gdmr_types - - -class ActionSpaceAdapter(abc.ABC): - """Adapts environment action into suitable commands format accepted by REAF. - - Implementations of this interface are responsible for converting the more - generic action accepted by the environment (e.g. a flat numpy array) into the - more constraining format accepted as commands by REAF, i.e. a dictionary of - string to tensors. - """ - - @abc.abstractmethod - def commands_from_environment_action( - self, environment_action: gdmr_types.ActionType - ) -> Mapping[str, gdmr_types.ArrayType]: - """Converts the environment action into commands accepted by REAF.""" - - @abc.abstractmethod - def action_spec(self) -> gdmr_types.ActionSpec: - """Returns the action spec exposed by the environment.""" - - @abc.abstractmethod - def task_commands_keys(self) -> set[str]: - """Returns the keys for the commands exposed to the task layer.""" diff --git a/src/experimental/reaf/core/commands_processor.py b/src/experimental/reaf/core/commands_processor.py deleted file mode 100644 index 4cc12b4a..00000000 --- a/src/experimental/reaf/core/commands_processor.py +++ /dev/null @@ -1,91 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Abstract class for commands manipulation in the task logic layer.""" - -import abc -from collections.abc import Mapping - -from gdm_robotics.interfaces import types as gdmr_types - - -class CommandsProcessor(abc.ABC): - """Perform commands manipulation. - - The following describes the processing pipeline starting from the top (closer - to the policy) to the bottom (interfacing with the DACL commands spec). - - Assume that we have two processing units: - Processor 1) has a consumed_commands_spec for two keys: "p1/c1" and "p1/c2". - Its produced_commands_keys are "p2/c1". - Processor 2) has a consumed_commands_spec for "p2/c1". Its - produced_commands_keys are "p3/c1" and "p3/c2". - - Specs are propagated starting from the bottom: - 1) In this example assume that the DACL exposes "p3/c1", "p3/c2" and "p3/c3". - 2) Processor 2) returns ("p3/c1", "p3/c2") from input "p2/c1". This means that - the global commands spec exposed at this level is "p2/c1" and the - unprocessed "p3/c3". - 3) Processor 1) returns "p2/c1" from input ("p1/c1", "p1/c2"). By applying the - same transformation rule, we can obtain the final commands spec exposed by - the full processing pipeline: "p1/c1", "p1/c2" and "p3/c3". - - "p1/c1" "p1/c2" "p3/c3" - | | | - ----------------- | - | P1 | | - ----------------- | - | "p2/c1" | - ----------------- | - | P2 | | - ----------------- | - | "p3/c1" | "p3/c2" | - | | | - ------------------------------------ - | DACL | - ------------------------------------ - """ - - @property - @abc.abstractmethod - def name(self) -> str: - """Returns a unique string identifier for this object.""" - - @abc.abstractmethod - def process_commands( - self, consumed_commands: Mapping[str, gdmr_types.ArrayType] - ) -> Mapping[str, gdmr_types.ArrayType]: - """Processes the commands and returns a new modified version of it. - - Args: - consumed_commands: the commands up in the processing chain (or provided by - the Environment) that are required by this processor, i.e. with keys - specified by `consumed_commands_spec`. - - Returns the new commands. Note that the data in consumed_commands is removed - from the global commands dictionary. If users want to keep some of the - elements it is their responsibility to retain them in the output - dictionary. - """ - - @abc.abstractmethod - def consumed_commands_spec(self) -> Mapping[str, gdmr_types.AnyArraySpec]: - """Spec of the commands consumed by this processor.""" - - @abc.abstractmethod - def produced_commands_keys(self) -> set[str]: - """Keys of the commands produced by this processor.""" - - def reset(self) -> None: - """Resets the internal state of the command processor.""" - ... diff --git a/src/experimental/reaf/core/data_acquisition_and_control_layer.py b/src/experimental/reaf/core/data_acquisition_and_control_layer.py deleted file mode 100644 index e8fcc8ca..00000000 --- a/src/experimental/reaf/core/data_acquisition_and_control_layer.py +++ /dev/null @@ -1,170 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""REAF data acquisition and control layer to interface with the robotic setup.""" - -from collections.abc import Iterable, Mapping - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types -from reaf.core import device as reaf_device -from reaf.core import device_coordinator as reaf_coordinator -from reaf.core import trigger - - -class DataAcquisitionAndControlLayer: - """REAF data acquisition and control layer. - - The DACL is responsible to provide an interface for the robotic setup. - """ - - def __init__( - self, - *, - device_coordinator: reaf_coordinator.DeviceCoordinator, - commands_trigger: trigger.Trigger | None, - measurements_trigger: trigger.Trigger | None, - ): - """Initializes the DataAcquisitionAndControlLayer. - - Args: - device_coordinator: The coordinator representing a specific robotic setup. - Note that callers need to explicitly initialize and finalise the - coordinator. - commands_trigger: A trigger to unblock processing commands during a call - to `step`. - measurements_trigger: A trigger to unblock processing measurements during - a call to `step`. - """ - self._coordinator = device_coordinator - self._devices = self._coordinator.get_devices() - # The following checks that names of the devices are unique and their keys - # are "mergeable". - self._check_device_names_and_keys(self._devices) - - self._commands_trigger = commands_trigger - self._measurements_trigger = measurements_trigger - - # Create a map of supported commands keys for each Device. - self._commands_for_device = { - device.name: device.commands_spec().keys() for device in self._devices - } - - def begin_stepping(self) -> Mapping[str, gdmr_types.ArrayType]: - """Begins stepping the DACL and returns the current measurements.""" - self._coordinator.on_begin_stepping() - - # Wait for the first trigger to happen before collecting the measurements. - if self._measurements_trigger is not None: - self._measurements_trigger.wait_for_event() - return self._get_measurements() - - def end_stepping(self) -> None: - """Ends stepping the data acquisition and control layer.""" - self._coordinator.on_end_stepping() - - def _set_commands(self, commands: Mapping[str, gdmr_types.ArrayType]) -> None: - """Sets the commands of the data acquisition and control layer.""" - self._coordinator.before_set_commands() - for device in self._devices: - device_commands = { - k: v - for k, v in commands.items() - if k in self._commands_for_device[device.name] - } - device.set_commands(device_commands) - self._coordinator.after_set_commands() - - def _get_measurements(self) -> Mapping[str, gdmr_types.ArrayType]: - """Gets the measurements of the data acquisition and control layer.""" - measurements = {} - self._coordinator.before_get_measurements() - for device in self._devices: - measurements.update(device.get_measurements()) - - return measurements - - def step( - self, commands: Mapping[str, gdmr_types.ArrayType] - ) -> Mapping[str, gdmr_types.ArrayType]: - """Steps the data acquisition and control layer.""" - if self._commands_trigger is not None: - self._commands_trigger.wait_for_event() - self._set_commands(commands) - - if self._measurements_trigger is not None: - self._measurements_trigger.wait_for_event() - return self._get_measurements() - - def commands_spec(self) -> Mapping[str, gdmr_types.AnyArraySpec]: - """Returns the specs for the commands.""" - spec = {} - for device in self._devices: - spec.update(device.commands_spec()) - return spec - - def measurements_spec(self) -> Mapping[str, specs.Array]: - """Returns the specs for the measurements.""" - spec = {} - for device in self._devices: - spec.update(device.measurements_spec()) - return spec - - @property - def device_coordinator(self) -> reaf_coordinator.DeviceCoordinator: - return self._coordinator - - def _check_keys_have_been_formatted_correctly( - self, current_key_set: Iterable[str] - ) -> None: - """Check that keys haven't been left unformatted.""" - for key in current_key_set: - if key.find("{}") != -1: - raise ValueError( - "Keys should not contain '{}'. Did you mean to use format()?" - ) - - def _check_device_names_and_keys( - self, devices: Iterable[reaf_device.Device] - ) -> None: - """Raises error if device names are not unique or keys are not exclusive.""" - # Check names first. - all_names = [device.name for device in devices] - unique_names = set(all_names) - if len(unique_names) != len(all_names): - raise RuntimeError(f"Duplicate names when checking devices: {all_names}") - - # Check commands. - devices = tuple(devices) - current_specs = set() - for device in devices: - device_keys = device.commands_spec().keys() - self._check_keys_have_been_formatted_correctly(device_keys) - if not current_specs.isdisjoint(device_keys): - raise RuntimeError( - f"Duplicate keys when checking device {device.name}:" - f" {current_specs.intersection(device_keys)}" - ) - current_specs.update(device_keys) - - # Check measurements. - current_specs = set() - for device in devices: - device_keys = device.measurements_spec().keys() - self._check_keys_have_been_formatted_correctly(device_keys) - if not current_specs.isdisjoint(device_keys): - raise RuntimeError( - f"Duplicate keys when checking device {device.name}:" - f" {current_specs.intersection(device_keys)}" - ) - current_specs.update(device_keys) diff --git a/src/experimental/reaf/core/default_discount_provider.py b/src/experimental/reaf/core/default_discount_provider.py deleted file mode 100644 index ff216b7c..00000000 --- a/src/experimental/reaf/core/default_discount_provider.py +++ /dev/null @@ -1,79 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Computes a constant discount given the termination state. - -This provider returns a discount of 0.0 in case of termination and 1.0 -otherwise (i.e. for truncation and not termination). - -It is usually safe to use this discount provider for environments that return -strictly positive rewards. -""" - -from collections.abc import Mapping - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types -import numpy as np -from reaf.core import discount_provider -from reaf.core import termination_checker -import tree - - -class DefaultDiscountProvider(discount_provider.DiscountProvider): - """Computes a constant discount given the termination state. - - This provider returns a discount of 0.0 in case of termination and 1.0 - otherwise (i.e. for truncation and not termination). - - It is usually safe to use this discount provider for environments that return - strictly positive rewards. - """ - - def __init__(self, name: str = "default_discount_provider"): - self._name = name - self._spec = specs.BoundedArray( - shape=(), dtype=np.float64, minimum=0.0, maximum=1.0, name="discount" - ) - - def name(self) -> str: - """Returns a unique string identifier for this object.""" - return self._name - - def compute_discount( - self, - unused_required_features: Mapping[str, gdmr_types.ArrayType], - termination_state: termination_checker.TerminationResult, - ) -> tree.Structure[gdmr_types.ArrayType]: - """Computes the discount. - - Args: - unused_required_features: Unused - termination_state: The termination state as computed by the termination - checkers. Returns the discount. - - Returns: - The discount. - """ - if termination_state == termination_state.TERMINATE: - return np.asarray(0).astype(self._spec.dtype) - else: # TRUNCATION or DO_NOT_TERMINATE - return np.asarray(1.0).astype(self._spec.dtype) - - def discount_spec(self) -> tree.Structure[specs.Array]: - """Returns the spec of the discount.""" - return self._spec - - def required_features_keys(self) -> set[str]: - """Returns the feature keys that are required to compute the discount.""" - return set() diff --git a/src/experimental/reaf/core/default_observation_space_adapter.py b/src/experimental/reaf/core/default_observation_space_adapter.py deleted file mode 100644 index 1261b3ca..00000000 --- a/src/experimental/reaf/core/default_observation_space_adapter.py +++ /dev/null @@ -1,231 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""ObservationSpaceAdapter supporting filtering, renaming and type conversion.""" - -import abc -from collections.abc import Iterable, Mapping -import dataclasses - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types -import numpy as np -import numpy.typing as npt -from reaf.core import observation_space_adapter -import tree - - -@dataclasses.dataclass(frozen=True, kw_only=True) -class RenameInfo: - original_key: str - renamed_key: str - - -class ObservationTypeMapper(abc.ABC): - """Maps from REAF features and specs into corresponding environment types.""" - - @abc.abstractmethod - def to_observation_spec( - self, features_spec: Mapping[str, specs.Array] - ) -> gdmr_types.ObservationSpec: - """Convert the features spec into the environment observation spec.""" - - @abc.abstractmethod - def to_observations( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> tree.Structure[gdmr_types.ArrayType]: - """Convert the features into the environment observations.""" - - -class _DefaultObservationTypeMapper(ObservationTypeMapper): - """An ObservationTypeMapper that returns the input features specs and dict. - - This `ObservationTypeMapper` maps observations from the more constrained - `Mapping[str, ArrayType]` used in the task layer to the more generic - `tree.Structure[ArrayType]` exposed by the GDM Environment. - """ - - def to_observation_spec( - self, features_spec: Mapping[str, specs.Array] - ) -> gdmr_types.ObservationSpec: - """Returns the features spec, unmodified, as a `gdmr_types.ObservationSpec`.""" - return features_spec - - def to_observations( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> tree.Structure[gdmr_types.ArrayType]: - """Returns the features, unmodified, as a `tree.Structure`.""" - return features - - -class DefaultObservationSpaceAdapter( - observation_space_adapter.ObservationSpaceAdapter -): - """Observation adapter supporting filtering, renaming and type conversion. - - This adapter supports filtering, renaming, and converting REAF features into - environment observations. - - The order of operations is the following: - 1) Filtering, i.e. feature selection. - 2) Downcasting floats to max_float_dtype. - 3) Renaming. - 4) Type conversion. - - Please refer to the constructor documentation for more information. - """ - - def __init__( - self, - *, - task_features_spec: Mapping[str, specs.Array], - selected_features: Iterable[str] | None, - renamed_features: Iterable[RenameInfo] | None, - observation_type_mapper: ObservationTypeMapper | None, - max_float_dtype: npt.DTypeLike = np.float64, - ): - """Initializes the observation space adapter. - - Args: - task_features_spec: The spec of all the features exposed by the task - layer. - selected_features: The features that will be exposed as observations. If - None, all features will be exposed, i.e. no filtering. - renamed_features: `RenameInfo` objects specifying which features should be - renamed and the corresponding new name. If empty or None, no renaming - will occur. - observation_type_mapper: An `ObservationTypeMapper` specifying how to - convert the task layer features data type (i.e. a Mapping[str, - ArrayType]) into the more generic type exposed by the GDM Environment - (i.e. a tree.Structure[ArrayType]). If None, an instance of - `_DefaultObservationTypeMapper` is used which converts the task logic - layer features dictionary to the more generic type (i.e. - `tree.Structure[ArrayType])` exposed by the environment. - max_float_dtype: The maximum float dtype to use for downcasting floats. - """ - if not np.issubdtype(max_float_dtype, np.floating): - raise ValueError( - 'max_float_dtype must be a floating point dtype. Got' - f' {max_float_dtype}' - ) - self._max_float_dtype = max_float_dtype - self._max_bits = np.finfo(self._max_float_dtype).bits - self._task_features_spec = task_features_spec - self._selected_filter = selected_features - self._renamed_features = renamed_features or () - self._observation_type_mapper = ( - observation_type_mapper or _DefaultObservationTypeMapper() - ) - self._check_specs_consistency() - # Compute the observation spec only once. - self._observation_spec = self._compute_observation_spec() - - def _check_specs_consistency(self) -> None: - # Check that filter keys are present in the spec. - if self._selected_filter is not None: - all_features = self._task_features_spec.keys() - features = set() - for feature in self._selected_filter: - if feature not in all_features: - raise ValueError(f'Feature {feature} is not present in the spec.') - features.add(feature) - else: - # No filter applied. Select all features. - features = set(self._task_features_spec.keys()) - - # Check renaming. - for rename_info in self._renamed_features: - if rename_info.original_key not in features: - raise ValueError( - f'Feature {rename_info.original_key} is not present in the spec.' - ) - - def observations_from_features( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> tree.Structure[gdmr_types.ArrayType]: - """Converts the features into the final environment observations.""" - # 1. Filter the observations. - if (selected_features := self._selected_filter) is None: - # No filter. Expose all observations. - filtered_features = dict(features) - else: - filtered_features = { - k: v for k, v in features.items() if k in selected_features # pytype: disable=unsupported-operands - } - - # 2. Downcast floats to max_float_dtype. - filtered_features = { - k: self._downcast_if_necessary(v) for k, v in filtered_features.items() - } - - # 3. Rename. - for rename_info in self._renamed_features: - # Rename the feature. - value = filtered_features[rename_info.original_key] - del filtered_features[rename_info.original_key] - filtered_features[rename_info.renamed_key] = value - - # 4. Convert type. - return self._observation_type_mapper.to_observations(filtered_features) - - def _compute_observation_spec(self) -> gdmr_types.ObservationSpec: - """Computes the observation spec.""" - # 1. Filter the specs - if (features_to_filter := self._selected_filter) is None: - # The observation spec corresponds to the task features spec. - filtered_specs = dict(self._task_features_spec) - else: - filtered_specs = { - k: v - for k, v in self._task_features_spec.items() - if k in features_to_filter # pytype: disable=unsupported-operands - } - - # 2. Downcast floats to max_float_dtype. - for k, v in filtered_specs.items(): - if self._dtype_needs_downcast(v.dtype): - filtered_specs[k] = v.replace(dtype=self._max_float_dtype) - - # 3. Rename. - for rename_info in self._renamed_features: - # Rename the feature. - value = filtered_specs[rename_info.original_key] - del filtered_specs[rename_info.original_key] - filtered_specs[rename_info.renamed_key] = value - - # 4. Convert the type. - return self._observation_type_mapper.to_observation_spec(filtered_specs) - - def observation_spec(self) -> gdmr_types.ObservationSpec: - """Returns the observation spec.""" - return self._observation_spec - - def task_features_keys(self) -> set[str]: - """Returns the task features keys that will be converted by this adapter.""" - return set(self._task_features_spec.keys()) - - def _downcast_if_necessary( - self, value: gdmr_types.ArrayType - ) -> gdmr_types.ArrayType: - if ( - hasattr(value, 'dtype') and self._dtype_needs_downcast(value.dtype) - ) or self._dtype_needs_downcast(type(value)): - return np.asarray(value).astype(self._max_float_dtype) - else: - return value - - def _dtype_needs_downcast(self, dtype: npt.DTypeLike) -> bool: - return ( - np.issubdtype(dtype, np.floating) - and np.finfo(dtype).bits > self._max_bits - ) diff --git a/src/experimental/reaf/core/device.py b/src/experimental/reaf/core/device.py deleted file mode 100644 index cc53a0ca..00000000 --- a/src/experimental/reaf/core/device.py +++ /dev/null @@ -1,54 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""REAF basic device to interface with the robotic setup.""" - -import abc -from collections.abc import Mapping -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types - - -class Device(abc.ABC): - """REAF basic device to interface with the robotic setup. - - A device defines a single piece in the robotic setup. It should be - hermetic, that is, not depending on other Devices. The coordination of the - devices is responsibility of the DeviceCoordinator. - - Important: a Device should return the commands and measurements specs - immediately after initialisation without the need for any explicit - initialisation, nor for resource acquisition (e.g. connecting to the - hardware). - """ - - @property - @abc.abstractmethod - def name(self) -> str: - """Returns the name of this device.""" - - @abc.abstractmethod - def commands_spec(self) -> Mapping[str, gdmr_types.AnyArraySpec]: - """Returns the commands specs for this device.""" - - @abc.abstractmethod - def measurements_spec(self) -> Mapping[str, specs.Array]: - """Returns the measurements specs for this device.""" - - @abc.abstractmethod - def set_commands(self, commands: Mapping[str, gdmr_types.ArrayType]) -> None: - """Sets the commands for this device.""" - - @abc.abstractmethod - def get_measurements(self) -> Mapping[str, gdmr_types.ArrayType]: - """Returns the measurements provided by this device.""" diff --git a/src/experimental/reaf/core/device_coordinator.py b/src/experimental/reaf/core/device_coordinator.py deleted file mode 100644 index 233705bb..00000000 --- a/src/experimental/reaf/core/device_coordinator.py +++ /dev/null @@ -1,87 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Coordinates the devices composing a robotic setup.""" - -import abc -from collections.abc import Iterable -from reaf.core import device - - -class DeviceCoordinator(abc.ABC): - """Coordinates the devices composing a robotic setup. - - The `DeviceCoordinator` object is responsible for coordinating all the - devices constituting the robotic setup. Whilst the Device is hermetic, - the coordinator is responsible for passing information from one device to - the other if required. For example in a bimanual setup the coordinator is - charged with passing the position of each robot to the other so we can ensure - proper and safe interaction such as for example collision avoidance. - - The `DeviceCoordinator` can be configurable to enable different - properties on the robotic setup, e.g. adding or not adding a `Device` or - forwarding configuration to each `Device`. - - At the very least, the coordinator must implement `get_devices` - to return all the devices. We also provide `on_begin_stepping` and - `on_end_stepping` methods that will be called before the start of an episode - and after the end of the episode respectively. Note that resource acquisition - and subsequent release is completely up to the implementation. - - Finally, `before_set_commands`/`before_get_measurements` can be implemented to - coordinate devices behaviour before their corresponding functions are - called. - """ - - @property - @abc.abstractmethod - def name(self) -> str: - """Returns the name of the coordinator.""" - - @abc.abstractmethod - def get_devices(self) -> Iterable[device.Device]: - """Returns the devices composing the embodiment.""" - - # Lifecycle methods. - - def on_begin_stepping(self) -> None: - """Prepares the coordinator for having its devices called repeatedly. - - After `on_begin_stepping` the devices returned by `get_devices` will have - their `set_commands` and `get_measurements` called repeatedly until - `on_end_stepping` is called on this coordinator. - """ - - def on_end_stepping(self) -> None: - """Notifies the coordinator that the devices are no longer called. - - After `on_end_stepping` the devices returned by `get_devices` will not have - their `set_commands` and `get_measurements` called anymore until this - coordinator `on_begin_stepping` method is notified again. - """ - - # Step hooks methods. - - def before_set_commands(self) -> None: - """Prepares the coordinator to have its devices set_commands called.""" - - def after_set_commands(self) -> None: - """Notifies the coordinator that its devices got `set_commands` called.""" - - def before_get_measurements(self) -> None: - """Prepares the coordinator to have its devices get_measurements called. - - This method gets called immediately before the devices `get_measurements` - method is called and can be used to customise the devices state given the - whole setup state. - """ diff --git a/src/experimental/reaf/core/discount_provider.py b/src/experimental/reaf/core/discount_provider.py deleted file mode 100644 index df1b22fa..00000000 --- a/src/experimental/reaf/core/discount_provider.py +++ /dev/null @@ -1,61 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Computes the discount.""" - -import abc -from collections.abc import Mapping - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types -from reaf.core import termination_checker -import tree - - -class DiscountProvider(abc.ABC): - """Computes the discount.""" - - @abc.abstractmethod - def name(self) -> str: - """Returns a unique string identifier for this object.""" - - @abc.abstractmethod - def compute_discount( - self, - required_features: Mapping[str, gdmr_types.ArrayType], - termination_state: termination_checker.TerminationResult, - ) -> tree.Structure[gdmr_types.ArrayType]: - """Computes the discount. - - Args: - required_features: Measurements and features computed by the task logic - that are required by this provider, i.e. that have keys specified by - `required_features_keys`. - termination_state: The termination state as computed by the termination - checkers. Returns the discount. - - Returns: - The discount. - """ - - @abc.abstractmethod - def discount_spec(self) -> tree.Structure[specs.Array]: - """Returns the spec of the discount.""" - - @abc.abstractmethod - def required_features_keys(self) -> set[str]: - """Returns the feature keys that are required to compute the discount.""" - - def reset(self) -> None: - """Resets the internal state of the discount provider.""" - ... diff --git a/src/experimental/reaf/core/entity.py b/src/experimental/reaf/core/entity.py deleted file mode 100644 index 68c2e38d..00000000 --- a/src/experimental/reaf/core/entity.py +++ /dev/null @@ -1,65 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Basic REAF-sim protocol to interface with the simulation.""" - -from collections.abc import Mapping -import typing - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types - - -class Entity(typing.Protocol): - """Basic REAF component to interface with the simulation. - - An entity defines a single component in the simulation that consumes substep - commands and outputs substep measurements at every simulation substep. It - should be hermetic, that is, not depending on other Entities. - - Important: an Entity should return the substep commands and substep - measurements specs immediately after initialisation without the need for any - explicit initialisation. - """ - - @property - def name(self) -> str: - """Instance name.""" - - def reset(self): - """Resets the entity.""" - - def substep_commands_spec( - self, - ) -> Mapping[str, specs.Array]: - """Spec for the substep commands.""" - - def substep_measurements_spec( - self, - ) -> Mapping[str, specs.Array]: - """Spec for the substep measurements.""" - - def set_substep_commands( - self, - model: typing.Any, - data: typing.Any, - consumed_substep_commands: Mapping[str, gdmr_types.ArrayType], - ) -> None: - """Sets the substep commands.""" - - def get_substep_measurements( - self, - model: typing.Any, - data: typing.Any, - ) -> Mapping[str, gdmr_types.ArrayType]: - """Returns the substep measurements.""" diff --git a/src/experimental/reaf/core/environment.py b/src/experimental/reaf/core/environment.py deleted file mode 100644 index dca306f0..00000000 --- a/src/experimental/reaf/core/environment.py +++ /dev/null @@ -1,490 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""The Robotics Environment Authoring Framework (REAF) Environment class.""" - -import abc -from collections.abc import Mapping -import enum -from typing import Generic - -from absl import logging -import dm_env -from dm_env import specs -from gdm_robotics.interfaces import environment as gdmr_env -from gdm_robotics.interfaces import types as gdmr_types -import numpy as np -from reaf.core import action_space_adapter as reaf_action_space_adapter -from reaf.core import data_acquisition_and_control_layer as reaf_dacl -from reaf.core import default_observation_space_adapter -from reaf.core import logger as reaf_logger -from reaf.core import observation_space_adapter as reaf_observation_space_adapter -from reaf.core import pass_through_action_space_adapter -from reaf.core import task_logic_layer as reaf_tll -import tree - - -class ActionSpecEnforcementOption(enum.StrEnum): - """Options for action spec enforcement.""" - - CLIP_TO_SPEC = "clip_to_spec" - IGNORE = "ignore" - WARNING = "warning" - RAISE_ERROR = "raise_error" - - -class EnvironmentReset(abc.ABC, Generic[gdmr_env.ResetOptions]): - """Support for general resets adhering to the GDM environment API.""" - - @abc.abstractmethod - def do_reset( - self, - config: gdmr_env.ResetOptions, - ) -> None: - """Resets the environment.""" - - def default_reset_configuration(self) -> gdmr_env.ResetOptions: - """Returns the default reset configuration.""" - return gdmr_env.Options() - - -class EndOfEpisodeHandler: - """Handler called after the last episode step.""" - - def on_end_of_episode_stepping(self, final_timestep: dm_env.TimeStep) -> None: - """Called when the episode has ended stepping. - - This will be called at the end of every episode, after all other triggers - have been resolved. Episodes can end either due to truncation or - termination, i.e. `timestep.step_type` is `StepType.LAST`, or due to an - early call to `Environment.reset()`. To verify whether it has indeed - ended due to truncation or termination, the implementer should test - `timestep.last()`. - - Note that the first reset after environment construction will not trigger - this handler, but it will be triggered before resolving any subsequent - environment resets, either implicit or explicit. - - Args: - final_timestep: The final timestep of the episode that ended stepping. - """ - - -class EnvironmentCloser(abc.ABC): - """Handler called when the environment is closed.""" - - @abc.abstractmethod - def close(self) -> None: - """Releases resources when the environment is closed. - - This method is called automatically when exiting the environment's - context manager (`with` statement). - """ - - -class Environment(gdmr_env.Environment): - """The Robotics Environment Authoring Framework (REAF) Environment class.""" - - def __init__( - self, - *, - data_acquisition_and_control_layer: reaf_dacl.DataAcquisitionAndControlLayer, - task_logic_layer: reaf_tll.TaskLogicLayer, - environment_reset: EnvironmentReset, - action_space_adapter: ( - reaf_action_space_adapter.ActionSpaceAdapter | None - ) = None, - observation_space_adapter: ( - reaf_observation_space_adapter.ObservationSpaceAdapter | None - ) = None, - end_of_episode_handler: EndOfEpisodeHandler | None = None, - environment_closer: EnvironmentCloser | None = None, - action_spec_enforcement_option: ActionSpecEnforcementOption = ActionSpecEnforcementOption.RAISE_ERROR, - ): - """Creates an environment. - - Args: - data_acquisition_and_control_layer: The layer for communicating with the - specific robotic setup. - task_logic_layer: The layer in charge of defining the task. - environment_reset: The `EnvironmentReset` specifying the function to be - called at environment reset and the default environment reset - configuration. - action_space_adapter: Adapter from the agent action space to the flattened - commands accepted by the task layer. If None the - PassThroughActionSpaceAdapter is used, meaning the entirety of the - commands dictionary is exposed to the agent. - observation_space_adapter: Adapter from the computed features to the - observations that are exposed to the agent. If None the - DefaultObservationSpaceAdapter is used, meaning all the features are - exposed to the agent as observations. - end_of_episode_handler: Called at the end of an episode, after the last - step. - environment_closer: Specifies the handler to be called when the - environment is closed. This is called automatically on exit if the - environment is used as a context manager. If None, no action is - performed at close. - action_spec_enforcement_option: How to enforce the action spec. If - `CLIP_TO_SPEC`, the action will be clipped to the spec. If `WARNING`, an - warning logged if the action is outside the spec. If `RAISE_ERROR`, an - error will be raised if the action is outside the spec. If `IGNORE`, - the action will be passed through. Default is `RAISE_ERROR`. - """ - - self._data_acquisition_and_control_layer = ( - data_acquisition_and_control_layer - ) - self._task_logic_layer = task_logic_layer - self._end_of_episode_handler = ( - end_of_episode_handler or EndOfEpisodeHandler() - ) - self._environment_reset = environment_reset - self._environment_closer = environment_closer - self._action_spec_enforcement_option = action_spec_enforcement_option - - # Before assigning the adapters, validate the specs on the task logic layer - # and the DACL. - self._validate_dacl_and_ttl_specs() - - ttl_commands_spec = self._task_logic_layer.commands_spec( - self._data_acquisition_and_control_layer.commands_spec() - ) - ttl_features_spec = self._task_logic_layer.features_spec( - self._data_acquisition_and_control_layer.measurements_spec() - ) - - if action_space_adapter is None: - action_space_adapter = ( - pass_through_action_space_adapter.PassThroughActionSpaceAdapter( - commands_spec=ttl_commands_spec - ) - ) - self._action_space_adapter = action_space_adapter - - if observation_space_adapter is None: - observation_space_adapter = ( - default_observation_space_adapter.DefaultObservationSpaceAdapter( - task_features_spec=ttl_features_spec, - selected_features=None, - renamed_features=None, - observation_type_mapper=None, - ) - ) - self._observation_space_adapter = observation_space_adapter - - # Now we can validate the adapters. - self._validate_adapters_specs() - - self._last_timestep: dm_env.TimeStep | None = None - self._should_finalize_episode = False - self._timestep_spec = gdmr_types.TimeStepSpec( - step_type=gdmr_types.STEP_TYPE_SPEC, - reward=self._task_logic_layer.reward_spec(), - discount=self._task_logic_layer.discount_spec(), - # The observation spec corresponds to the one exposed by the adapter. - observation=self._observation_space_adapter.observation_spec(), - ) - - self._zero_reward, self._zero_discount = tree.map_structure( - _read_only_zeros_like_spec, - (self._timestep_spec.reward, self._timestep_spec.discount), - ) - - def close(self) -> None: - """Frees any resources used by the environment.""" - if self._environment_closer is not None: - self._environment_closer.close() - - def default_reset_options(self) -> gdmr_env.ResetOptions: - return self._environment_reset.default_reset_configuration() - - def reset_with_options( - self, - *, - options: gdmr_env.ResetOptions, - ) -> dm_env.TimeStep: - """Starts a new sequence and returns the first `TimeStep`.""" - if self._should_finalize_episode: - self._finalize_episode() - self._environment_reset.do_reset(options) - self._task_logic_layer.perform_reset() - measurements = self._data_acquisition_and_control_layer.begin_stepping() - features = self._task_logic_layer.compute_all_features(measurements) - observations = self._compute_observations_from_features(features) - - self._last_timestep = self._restart(observation=observations) - # Make sure any early reset after this one triggers `_finalize_episode`. - self._should_finalize_episode = True - return self._last_timestep - - def action_spec(self) -> gdmr_types.ActionSpec: - """Defines the actions that should be provided to `step`.""" - # The action spec corresponds to the one exposed by the adapter. - return self._action_space_adapter.action_spec() - - def timestep_spec(self) -> gdmr_types.TimeStepSpec: - """Returns the spec associated to the returned TimeStep.""" - return self._timestep_spec - - def step(self, action: gdmr_types.ActionType) -> dm_env.TimeStep: - """Updates the environment according to action and returns a `TimeStep`.""" - - action = self._enforce_action_spec(action) - if self._last_timestep is None or self._last_timestep.last(): - return self.reset() - - # Process the action to obtain a command. - commands = self._compute_commands_from_agent_action(action) - commands = self._task_logic_layer.compute_final_commands(commands) - measurements = self._data_acquisition_and_control_layer.step(commands) - - # Compute all the features. - features = self._task_logic_layer.compute_all_features(measurements) - - # Compute the elements of the timestep. - reward = self._task_logic_layer.compute_reward(features) - termination_state = self._task_logic_layer.check_for_termination(features) - discount = self._task_logic_layer.compute_discount( - features, termination_state - ) - - observations = self._compute_observations_from_features(features) - - if termination_state.is_terminated(): - self._last_timestep = self._termination( - reward=reward, observation=observations - ) - elif termination_state.is_truncated(): - self._last_timestep = self._truncation( - reward=reward, observation=observations, discount=discount - ) - else: - self._last_timestep = self._transition( - reward=reward, observation=observations, discount=discount - ) - - if self._last_timestep.last(): - self._finalize_episode() - return self._last_timestep - - def _finalize_episode(self) -> None: - self._data_acquisition_and_control_layer.end_stepping() - # It's crucial to call `end_stepping` on the dacl before invoking the end - # of episode handler. This ensures no further `set_command` or - # `get_measurements` calls are made. In contrast, the end of episode - # handler might interact with devices, requiring them to be informed - # beforehand. - self._end_of_episode_handler.on_end_of_episode_stepping(self._last_timestep) - self._should_finalize_episode = False - - @property - def data_acquisition_and_control_layer( - self, - ) -> reaf_dacl.DataAcquisitionAndControlLayer: - return self._data_acquisition_and_control_layer - - @property - def task_logic_layer(self) -> reaf_tll.TaskLogicLayer: - return self._task_logic_layer - - @property - def environment_reset(self) -> EnvironmentReset: - return self._environment_reset - - @environment_reset.setter - def environment_reset(self, environment_reset: EnvironmentReset) -> None: - self._environment_reset = environment_reset - - def add_logger(self, logger: reaf_logger.Logger) -> None: - self._task_logic_layer.add_logger(logger) - - def remove_logger(self, logger: reaf_logger.Logger) -> None: - self._task_logic_layer.remove_logger(logger) - - def _validate_dacl_and_ttl_specs(self) -> None: - """Validates the specs on the task logic layer.""" - # Validate the spec on the task logic layer. - self._task_logic_layer.validate_spec( - dacl_commands_spec=( - self._data_acquisition_and_control_layer.commands_spec() - ), - dacl_measurements_spec=( - self._data_acquisition_and_control_layer.measurements_spec() - ), - ) - - def _validate_adapters_specs(self) -> None: - # Collect the full commands and features spec and validate them against - # the adapters. - commands_spec = set( - self._task_logic_layer.commands_spec( - self._data_acquisition_and_control_layer.commands_spec() - ).keys() - ) - features_spec = set( - self._task_logic_layer.features_spec( - self._data_acquisition_and_control_layer.measurements_spec() - ) - ) - - # Check the action space adapter. - adapter_keys = self._action_space_adapter.task_commands_keys() - - if adapter_keys != commands_spec: - raise ValueError( - "Mismatch between commands exposed by the action space adapter:" - f" {adapter_keys} and commands spec expected by the task layer:" - f" {commands_spec}." - ) - - # Check the observation spec adapter. - adapter_keys = self._observation_space_adapter.task_features_keys() - if not adapter_keys.issubset(features_spec): - raise ValueError( - "Failed to validate observation space adapter specs. Missing keys:" - f" {adapter_keys - features_spec}" - ) - - def _compute_observations_from_features( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> tree.Structure[gdmr_types.ArrayType]: - return self._observation_space_adapter.observations_from_features(features) - - def _compute_commands_from_agent_action( - self, agent_action: gdmr_types.ActionType - ) -> Mapping[str, gdmr_types.ArrayType]: - return self._action_space_adapter.commands_from_environment_action( - agent_action - ) - - def _restart( - self, - observation: tree.Structure[gdmr_types.ArrayType], - ) -> dm_env.TimeStep: - """Returns a `TimeStep` with `step_type` set to `StepType.FIRST`.""" - return dm_env.TimeStep( - step_type=np.asarray(dm_env.StepType.FIRST, dtype=np.uint8), - observation=observation, - reward=self._zero_reward, - discount=self._zero_discount, - ) - - def _transition( - self, - reward: tree.Structure[gdmr_types.ArrayType], - observation: tree.Structure[gdmr_types.ArrayType], - discount: tree.Structure[gdmr_types.ArrayType], - ) -> dm_env.TimeStep: - """Returns a `TimeStep` with `step_type` set to `StepType.MID`.""" - return dm_env.TimeStep( - step_type=np.asarray(dm_env.StepType.MID, dtype=np.uint8), - observation=observation, - reward=reward, - discount=discount, - ) - - def _termination( - self, - reward: tree.Structure[gdmr_types.ArrayType], - observation: tree.Structure[gdmr_types.ArrayType], - ) -> dm_env.TimeStep: - """Returns a `TimeStep` with `step_type` set to `StepType.LAST`.""" - return dm_env.TimeStep( - step_type=np.asarray(dm_env.StepType.LAST, dtype=np.uint8), - observation=observation, - reward=reward, - discount=self._zero_discount, - ) - - def _truncation( - self, - reward: tree.Structure[gdmr_types.ArrayType], - observation: tree.Structure[gdmr_types.ArrayType], - discount: tree.Structure[gdmr_types.ArrayType], - ) -> dm_env.TimeStep: - """Returns a `TimeStep` with `step_type` set to `StepType.LAST`.""" - return dm_env.TimeStep( - step_type=np.asarray(dm_env.StepType.LAST, dtype=np.uint8), - observation=observation, - reward=reward, - discount=discount, - ) - - def _enforce_action_spec( - self, action: gdmr_types.ActionType - ) -> gdmr_types.ActionType: - """Enforces the action spec.""" - match self._action_spec_enforcement_option: - case ActionSpecEnforcementOption.IGNORE: - pass - case ActionSpecEnforcementOption.CLIP_TO_SPEC: - try: - - def clip_to_spec(a, s): - if isinstance(s, specs.BoundedArray): - return np.clip(a, s.minimum, s.maximum) - return a - - action = tree.map_structure( - clip_to_spec, - action, - self._action_space_adapter.action_spec(), - ) - except ValueError as e: - raise ValueError( - "Failed to clip action to spec. Action:" - f" {action} and spec: {self._action_space_adapter.action_spec()}" - ) from e - case ActionSpecEnforcementOption.WARNING: - - def _validate_without_raising(a, s): - dtype_ok = s.dtype == a.dtype - shape_ok = s.shape == a.shape - minimum_ok = True - maximum_ok = True - if isinstance(s, specs.BoundedArray): - minimum_ok = (s.minimum <= a).all() - maximum_ok = (a <= s.maximum).all() - return dtype_ok and shape_ok and minimum_ok and maximum_ok - - if not all( - tree.flatten( - tree.map_structure( - _validate_without_raising, - action, - self._action_space_adapter.action_spec(), - ) - ) - ): - logging.warning( - "Failed to validate action against spec. Action: %r and spec: %r", - action, - self._action_space_adapter.action_spec(), - ) - case ActionSpecEnforcementOption.RAISE_ERROR: - action = tree.map_structure( - lambda a, spec: spec.validate(a), action, self.action_spec() - ) - case _: - raise ValueError( - "Unknown action spec enforcement option:" - f" {self._action_spec_enforcement_option}" - ) - return action - - -def _read_only_zeros_like_spec(spec: specs.Array) -> np.ndarray: - """Returns a zero array matching the specified spec.""" - arr = np.zeros(shape=spec.shape, dtype=spec.dtype) - arr.flags.writeable = False - return arr diff --git a/src/experimental/reaf/core/features_observer.py b/src/experimental/reaf/core/features_observer.py deleted file mode 100644 index cc87f0d3..00000000 --- a/src/experimental/reaf/core/features_observer.py +++ /dev/null @@ -1,34 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Observe all the produced features and measurements.""" - -import abc -from collections.abc import Mapping - -from gdm_robotics.interfaces import types as gdmr_types - - -class FeaturesObserver(abc.ABC): - """Observe all the produced features and measurements.""" - - @property - @abc.abstractmethod - def name(self) -> str: - """Returns a unique string identifier for this object.""" - - @abc.abstractmethod - def observe_features( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> None: - """Observes all the features and measurements.""" diff --git a/src/experimental/reaf/core/features_producer.py b/src/experimental/reaf/core/features_producer.py deleted file mode 100644 index 8ce44e94..00000000 --- a/src/experimental/reaf/core/features_producer.py +++ /dev/null @@ -1,56 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Produces additional features to be exposed by the task logic layer.""" - -import abc -from collections.abc import Mapping - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types - - -class FeaturesProducer(abc.ABC): - """Produces additional features to be exposed by the task logic layer.""" - - @property - @abc.abstractmethod - def name(self) -> str: - """Returns a unique string identifier for this object.""" - - @abc.abstractmethod - def produce_features( - self, required_features: Mapping[str, gdmr_types.ArrayType] - ) -> Mapping[str, gdmr_types.ArrayType]: - """Produces additional features for the environment. - - Args: - required_features: Measurements and features generated by previous - producers in the processing chain that are required by this processor, - i.e. with keys specified by `required_features_keys`. - - Returns additional features that will be added to the global measurements - and features dictionary. - """ - - @abc.abstractmethod - def produced_features_spec(self) -> Mapping[str, specs.Array]: - """Returns the spec of the features produced by this producer.""" - - @abc.abstractmethod - def required_features_keys(self) -> set[str]: - """Returns the keys that are required to produce the new features.""" - - def reset(self) -> None: - """Resets the internal state of the feature producer.""" - ... diff --git a/src/experimental/reaf/core/logger.py b/src/experimental/reaf/core/logger.py deleted file mode 100644 index 63bb5f33..00000000 --- a/src/experimental/reaf/core/logger.py +++ /dev/null @@ -1,80 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Support logging inside the task logic layer.""" - -import abc -from collections.abc import Mapping - -from gdm_robotics.interfaces import types as gdmr_types - - -class Logger(abc.ABC): - """Support logging inside the task logic layer. - - Lifecycle - For each environment step, these member functions are called in this order: - 1. `record_measurements` is called with raw measurements from the sensors. - 2. `record_features` is called with features derived from the measurements. - 3. `record_commands_processing` is called for each - `CommandsProcessor.process_commands` invocation, tracking the - transformation of commands. - 4. `record_final_commands` is called once with the final commands sent to - the DACL. - - Notes: - An environment is first reset(). This triggers the first two steps above. - See reset_with_options in ./environment.py. - - After reset, step is called repeatedly. - 1. This first triggers steps 3 and 4 (See compute_final_commands in TLL - called from step in ./environment.py) - 2. Features are computed (see compute_all_features in TLL called from - step in ./environment.py), triggering steps 1 and 2. - """ - - @property - @abc.abstractmethod - def name(self) -> str: - """Unique string identifier for this object.""" - - def record_measurements( - self, measurements: Mapping[str, gdmr_types.ArrayType] - ) -> None: - """Called once with all the measurements from the DACL.""" - - def record_features( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> None: - """Called once with all the features computed in the Task Layer.""" - - def record_final_commands( - self, commands: Mapping[str, gdmr_types.ArrayType] - ) -> None: - """Called once with the final commands sent to the DACL.""" - - def record_commands_processing( - self, - name: str, - consumed_commands: Mapping[str, gdmr_types.ArrayType], - produced_commands: Mapping[str, gdmr_types.ArrayType], - ) -> None: - """Called once per call to `process_commands` for each CommandsProcessor. - - Args: - name: Name of the `CommandsProcessor`. - consumed_commands: The commands consumed by the current - `CommandsProcessor`. - produced_commands: The commands produced by the current - `CommandsProcessor`. - """ diff --git a/src/experimental/reaf/core/numpy_mock_assertions.py b/src/experimental/reaf/core/numpy_mock_assertions.py deleted file mode 100644 index 310a918e..00000000 --- a/src/experimental/reaf/core/numpy_mock_assertions.py +++ /dev/null @@ -1,98 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Testing functions for asserting on Mock objects with numpy structures.""" - -from collections.abc import Sequence -from unittest import mock -import numpy as np - - -def assert_called_once_with(mock_obj: mock.Mock, *args, **kwargs) -> None: - if mock_obj.call_count != 1: - raise AssertionError( - f"Expected exactly one call to {mock_obj}, got {mock_obj.call_count}" - ) - - assert_called_with(mock_obj, *args, **kwargs) - - -def assert_called_with(mock_obj: mock.Mock, *args, **kwargs) -> None: - """Asserts that the last call to mock_obj had the specified arguments.""" - if mock_obj.call_args is None: - raise AssertionError( - f"Mock object {mock_obj} not called. Expected one call." - ) - call_args, call_kwargs = mock_obj.call_args - np.testing.assert_equal(call_args, args) - np.testing.assert_equal(call_kwargs, kwargs) - - -def assert_has_calls( - mock_obj: mock.Mock, calls: Sequence[mock._Call], any_order: bool = False -) -> None: - """Asserts that mock_obj has been called with the specified calls.""" - mock_calls = mock_obj.mock_calls - - # Check that there are at least enough calls. - if mock_obj.call_count < len(calls): - raise AssertionError( - f"Expected at least {len(calls)} calls to {mock_obj}, got" - f" {mock_obj.call_count}" - ) - - def _calls_are_equal(actual: mock._Call, expected: mock._Call) -> bool: - _, actual_args, actual_kwargs = actual - _, expected_args, expected_kwargs = expected - # Quickest way to transform the assertion into a comparator. - try: - np.testing.assert_equal(actual_args, expected_args) - np.testing.assert_equal(actual_kwargs, expected_kwargs) - return True - except AssertionError: - return False - - if any_order: - # We just check for the calls to be contained. - for expected_call in calls: - for actual_call in mock_calls: - if _calls_are_equal(actual_call, expected_call): - break - raise AssertionError( - f"Expected call {expected_call} not found in mock calls {mock_calls}." - ) - return - - # We need to check in order, but first find the first call. - starting_index = -1 - first_expected_call = calls[0] - for index, actual_call in enumerate(mock_calls): - if _calls_are_equal(actual_call, first_expected_call): - starting_index = index - break - if starting_index == -1: - raise AssertionError(f"Calls {calls} not found in mock calls {mock_calls}.") - - non_matching_calls = [] - - # We have the first element. Now we need to compare element wise. - for index, expected_call in enumerate(calls): - actual_call = mock_calls[starting_index + index] - if not _calls_are_equal(actual_call, expected_call): - non_matching_calls.append((index, expected_call, actual_call)) - - if non_matching_calls: - raise AssertionError( - f"Calls {calls} do not match mock calls {mock_calls}. Mismatch (index," - f" expected, actual): {non_matching_calls}" - ) diff --git a/src/experimental/reaf/core/observation_space_adapter.py b/src/experimental/reaf/core/observation_space_adapter.py deleted file mode 100644 index ca4a8d86..00000000 --- a/src/experimental/reaf/core/observation_space_adapter.py +++ /dev/null @@ -1,42 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Adapts REAF features into observations exposed by the environment.""" - -import abc -from collections.abc import Mapping -from gdm_robotics.interfaces import types as gdmr_types -import tree - - -class ObservationSpaceAdapter(abc.ABC): - """Adapts REAF features into observations exposed by the environment. - - Implementations of this interface are responsible for converting the features - generated by the REAF task layer logic (i.e. dictionary of tensors) into the - more generic `observation` structure exposed by the environment. - """ - - @abc.abstractmethod - def observations_from_features( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> tree.Structure[gdmr_types.ArrayType]: - """Converts the REAF features into the environment observations.""" - - @abc.abstractmethod - def observation_spec(self) -> gdmr_types.ObservationSpec: - """Returns the observation spec.""" - - @abc.abstractmethod - def task_features_keys(self) -> set[str]: - """Returns the task features keys that will be converted by this adapter.""" diff --git a/src/experimental/reaf/core/pass_through_action_space_adapter.py b/src/experimental/reaf/core/pass_through_action_space_adapter.py deleted file mode 100644 index 16b28a81..00000000 --- a/src/experimental/reaf/core/pass_through_action_space_adapter.py +++ /dev/null @@ -1,55 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Adapter that passes the commands spec through.""" - -from collections.abc import Mapping -from gdm_robotics.interfaces import types as gdmr_types -from reaf.core import action_space_adapter - - -class PassThroughActionSpaceAdapter(action_space_adapter.ActionSpaceAdapter): - """Adapter that passes the commands spec through. - - NB the resulting environment will expose a dictionary as the action spec. - """ - - def __init__(self, commands_spec: Mapping[str, gdmr_types.AnyArraySpec]): - self._commands_spec = commands_spec - - def commands_from_environment_action( - self, environment_action: gdmr_types.ActionType - ) -> Mapping[str, gdmr_types.ArrayType]: - """Returns commands accepted by REAF. - - commands_from_environment_action usually accepts a gdmr_types.ActionType but - since this adapter passes the same action as the commands, it needs to be a - dict type in order to pass it through as a dict. - - Args: - environment_action: The environment action(s) to pass as REAF commands. - """ - if not isinstance(environment_action, dict): - raise ValueError( - 'environment_action must be a dict, but got: ' - f'{type(environment_action)}.' - ) - return environment_action - - def action_spec(self) -> gdmr_types.ActionSpec: - """Returns the action spec exposed by the environment.""" - return self._commands_spec - - def task_commands_keys(self) -> set[str]: - """Returns the keys for the commands exposed to the task layer.""" - return set(self._commands_spec.keys()) diff --git a/src/experimental/reaf/core/reward_provider.py b/src/experimental/reaf/core/reward_provider.py deleted file mode 100644 index 3a8ec655..00000000 --- a/src/experimental/reaf/core/reward_provider.py +++ /dev/null @@ -1,292 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Computes the reward.""" - -import abc -from collections.abc import Mapping -import operator -from typing import Callable, TypeAlias, TypeVar, Union - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types -import numpy as np -import tree - - -RewardValue: TypeAlias = tree.Structure[gdmr_types.ArrayType] -RewardSpec: TypeAlias = tree.Structure[specs.Array] - - -class _RewardProvider(abc.ABC): - """Computes the reward. - - Defines the interface for a reward provider. - - Important: Users should not inherit from this class directly. Instead, use the - RewardProvider class later in this file. - """ - - @abc.abstractmethod - def name(self) -> str: - """Returns a unique string identifier for this object.""" - - @abc.abstractmethod - def compute_reward( - self, required_features: Mapping[str, gdmr_types.ArrayType] - ) -> RewardValue: - """Computes the reward. - - Args: - required_features: Measurements and features computed by the task logic - that are required by this provider, i.e. that have keys specified by - `required_features_keys`. - - Returns the computed reward. - """ - - @abc.abstractmethod - def reward_spec(self) -> RewardSpec: - """Returns the spec of the reward.""" - - @abc.abstractmethod - def required_features_keys(self) -> set[str]: - """Returns the feature keys that are required to compute the reward.""" - - def reset(self) -> None: - """Resets the internal state of the reward provider.""" - ... - - -RewardProviderOrValue: TypeAlias = Union['RewardProvider', RewardValue] - - -S = TypeVar('S') -T = TypeVar('T') -UnaryOperator: TypeAlias = Callable[[S], S] -BinaryOperator: TypeAlias = Callable[[S | T, S | T], S | T] - - -class RewardProvider(_RewardProvider): - """Computes the reward. - - Important: Users should inherit from this class and implement the abstract - methods defined in the interface _RewardProvider. - """ - - def __add__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.add, self, other) - - def __radd__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.add, other, self) - - def __sub__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.sub, self, other) - - def __rsub__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.sub, other, self) - - def __mul__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.mul, self, other) - - def __rmul__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.mul, other, self) - - def __truediv__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.truediv, self, other) - - def __rtruediv__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.truediv, other, self) - - def __floordiv__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.floordiv, self, other) - - def __rfloordiv__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.floordiv, other, self) - - def __pow__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.pow, self, other) - - def __rpow__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.pow, other, self) - - def __getitem__(self, index: slice): - return GetItemOperationRewardProvider(self, index) - - def __neg__(self): - return UnaryOperationRewardProvider(operator.neg, self) - - -class ConstantRewardProvider(RewardProvider): - """A RewardProvider that always returns the same reward.""" - - def __init__(self, reward: RewardValue): - super().__init__() - self._reward = reward - - def name(self) -> str: - return str(self._reward) - - def compute_reward( - self, required_features: Mapping[str, gdmr_types.ArrayType] - ) -> RewardValue: - return self._reward - - def reward_spec(self) -> RewardSpec: - return tree.map_structure( - lambda v: specs.Array(v.shape, v.dtype), self._reward - ) - - def required_features_keys(self) -> set[str]: - return set() - - -class BinaryOperationRewardProvider(RewardProvider): - """Applies a binary operator to the result of two reward providers.""" - - def __init__( - self, - op: BinaryOperator, - first_reward_provider: RewardProviderOrValue, - second_reward_provider: RewardProviderOrValue, - ): - super().__init__() - if not isinstance(first_reward_provider, RewardProvider): - first_reward_provider = ConstantRewardProvider(first_reward_provider) - if not isinstance(second_reward_provider, RewardProvider): - second_reward_provider = ConstantRewardProvider(second_reward_provider) - first_spec = first_reward_provider.reward_spec() - second_spec = second_reward_provider.reward_spec() - tree.assert_same_structure(first_spec, second_spec) - assert all( - tree.flatten( - tree.map_structure( - lambda s1, s2: s1.shape == s2.shape and s1.dtype == s2.dtype, - first_spec, - second_spec, - ) - ) - ) - self._op = op - self._first_reward_provider = first_reward_provider - self._second_reward_provider = second_reward_provider - self._reward_spec = first_reward_provider.reward_spec() - self._first_required_features_keys = ( - first_reward_provider.required_features_keys() - ) - self._second_required_features_keys = ( - second_reward_provider.required_features_keys() - ) - - def name(self) -> str: - op_name = getattr(self._op, '__name__', str(self._op)) - return ( - f'{op_name}({self._first_reward_provider.name()},' - f' {self._second_reward_provider.name()})' - ) - - def compute_reward( - self, required_features: Mapping[str, gdmr_types.ArrayType] - ) -> RewardValue: - first_required_features = { - k: v - for k, v in required_features.items() - if k in self._first_required_features_keys - } - second_required_features = { - k: v - for k, v in required_features.items() - if k in self._second_required_features_keys - } - return tree.map_structure( - self._op, - self._first_reward_provider.compute_reward(first_required_features), - self._second_reward_provider.compute_reward(second_required_features), - ) - - def reward_spec(self) -> RewardSpec: - return self._reward_spec - - def required_features_keys(self) -> set[str]: - return ( - self._first_required_features_keys | self._second_required_features_keys - ) - - def reset(self) -> None: - self._first_reward_provider.reset() - self._second_reward_provider.reset() - - -class GetItemOperationRewardProvider(RewardProvider): - """Extracts a slice from the result of a reward provider.""" - - def __init__(self, reward_provider: RewardProviderOrValue, index: slice): - super().__init__() - if not isinstance(reward_provider, RewardProvider): - reward_provider = ConstantRewardProvider(reward_provider) - self._reward_provider = reward_provider - self._index = index - - def name(self) -> str: - return f'{self._reward_provider.name}[{self._index}]' - - def compute_reward( - self, required_features: Mapping[str, gdmr_types.ArrayType] - ) -> RewardValue: - return tree.map_structure( - lambda v: v[self._index], - self._reward_provider.compute_reward(required_features), - ) - - def reward_spec(self) -> RewardSpec: - return tree.map_structure( - lambda s: specs.Array(np.empty(s.shape)[self._index].shape, s.dtype), - self._reward_provider.reward_spec(), - ) - - def required_features_keys(self) -> set[str]: - return self._reward_provider.required_features_keys() - - def reset(self) -> None: - self._reward_provider.reset() - - -class UnaryOperationRewardProvider(RewardProvider): - """Applies a unary operator to the result of a reward provider.""" - - def __init__(self, op: UnaryOperator, reward_provider: RewardProviderOrValue): - super().__init__() - if not isinstance(reward_provider, RewardProvider): - reward_provider = ConstantRewardProvider(reward_provider) - self._op = op - self._reward_provider = reward_provider - - def name(self) -> str: - op_name = getattr(self._op, '__name__', str(self._op)) - return f'{op_name}({self._reward_provider.name()})' - - def compute_reward( - self, required_features: Mapping[str, gdmr_types.ArrayType] - ) -> RewardValue: - return tree.map_structure( - self._op, self._reward_provider.compute_reward(required_features) - ) - - def reward_spec(self) -> RewardSpec: - return self._reward_provider.reward_spec() - - def required_features_keys(self) -> set[str]: - return self._reward_provider.required_features_keys() - - def reset(self) -> None: - self._reward_provider.reset() diff --git a/src/experimental/reaf/core/substep_commands_processor.py b/src/experimental/reaf/core/substep_commands_processor.py deleted file mode 100644 index b63f4388..00000000 --- a/src/experimental/reaf/core/substep_commands_processor.py +++ /dev/null @@ -1,104 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Protocol for substep commands manipulation in REAF-sim.""" - -from collections.abc import Mapping -import typing - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types - - -class SubstepCommandsProcessor(typing.Protocol): - """Processes substep commands, propagating them through a pipeline. - - This processor manipulates substep commands, acting as a node in a pipeline. - It consumes substep commands, performs operations, and produces updated - substep commands for the next stage in the processing chain. - - The processing pipeline starts with commands provided to the SimulationDevice - and progresses towards the substep commands consumed by the individual - entities. Each processor consumes a subset of substep commands and produces - new, potentially transformed, substep commands. The order of operations is - crucial. - - Example Pipeline (conceptual): - - Simulation Device commands --> Processor (1) --> Processor (2) --> Entities - - Specs are propagated starting from the bottom: - 1) In this example assume that the set of entities expect "p3/c1", "p3/c2" and - "p3/c3". - 2) Processor (2) returns ("p3/c1", "p3/c2") from "p2/c1". This means - that the global substep commands spec exposed at this level is "p2/c1" and - the unprocessed "p3/c3". - 3) Processor (1) returns "p2/c1" from ("p1/c1", "p1/c2"). By applying - the same transformation rule, we can obtain the final spec exposed - by the SimulationDevice: "p1/c1", "p1/c2" and "p3/c3". - - ------------------------------------ - | SimulationDevice | - ------------------------------------ - - "p1/c1" "p1/c2" "p3/c3" - | | | - ----------------- | - | P1 | | - ----------------- | - | "p2/c1" | - ----------------- | - | P2 | | - ----------------- | - | "p3/c1" | "p3/c2" | - | | | - ------------------------------------ - | Entities | - ------------------------------------ - """ - - @property - def name(self) -> str: - """Returns a unique string identifier for this object.""" - - def reset(self) -> None: - """Resets the internal state of this processor.""" - - def produced_substep_commands_keys(self) -> set[str]: - """Keys of the substep commands produced by this processor.""" - - def consumed_substep_commands_spec( - self, - ) -> Mapping[str, specs.Array]: - """Spec of the substep commands consumed by this processor.""" - - def process_substep_commands( - self, - model: typing.Any, - data: typing.Any, - consumed_substep_commands: Mapping[str, gdmr_types.ArrayType], - ) -> Mapping[str, gdmr_types.ArrayType]: - """Processes the substep commands and returns a new modified version of it. - - Args: - model: the simulation model. - data: the simulation data. - consumed_substep_commands: the substep commands up in the processing chain - that are required by this processor, i.e. with keys specified by - `consumed_substep_commands_spec`. - - Returns the new substep commands. Note that the (key, value) pairs in - `consumed_substep_commands` are removed from the running substep commands - dictionary. If users want to keep some of the elements it is their - responsibility to retain them in the output dictionary. - """ diff --git a/src/experimental/reaf/core/substep_measurements_processor.py b/src/experimental/reaf/core/substep_measurements_processor.py deleted file mode 100644 index 15dc81df..00000000 --- a/src/experimental/reaf/core/substep_measurements_processor.py +++ /dev/null @@ -1,103 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Protocol for substep measurements manipulation in REAF-sim.""" - -from collections.abc import Mapping -import typing - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types - - -class SubstepMeasurementsProcessor(typing.Protocol): - """Processes substep measurements, propagating them through a pipeline. - - This processor manipulates substep measurements, acting as a node in a - pipeline. It consumes substep measurements, performs operations, and produces - updated substep measurements for the next stage in the processing chain. - - The processing pipeline starts with substep measurements produced by Entities - and progresses towards the measurements exposed by the SimulationDevice. Each - processor consumes a subset of substep measurements and produces new, - potentially transformed, substep measurements. The order of operations is - crucial. - - Example Pipeline (conceptual): - - Entities --> Processor (1) --> Processor (2) -> Simulation Device Measurements - - Specs are propagated starting from the bottom: - 1) In this example assume that the set of entities produce "p1/c1", "p1/c2" - and "p1/c3". - 2) Processor (1) returns "p2/c1" from ("p1/c1", "p1/c2"). - 3) Processor (2) returns ("p3/c1", "p3/c2") from "p2/c1". - - This resulting spec exposed by the SimulationDevice: "p3/c1", "p3/c2" - and "p1/c3". - - ------------------------------------ - | SimulationDevice | - ------------------------------------ - - "p3/c1" "p3/c2" "p1/c3" - | | | - ----------------- | - | P2 | | - ----------------- | - | "p2/c1" | - ----------------- | - | P1 | | - ----------------- | - | "p1/c1" | "p1/c2" | - | | | - ------------------------------------ - | Entities | - ------------------------------------ - """ - - @property - def name(self) -> str: - """Returns a unique string identifier for this object.""" - - def reset(self): - """Resets the internal state of this processor.""" - - def produced_substep_measurements_spec( - self, - ) -> Mapping[str, specs.Array]: - """Spec of the substep measurements consumed by this processor.""" - - def consumed_substep_measurements_keys(self) -> set[str]: - """Keys of the substep measurements consumed by this processor.""" - - def process_substep_measurements( - self, - model: typing.Any, - data: typing.Any, - consumed_substep_measurements: Mapping[str, gdmr_types.ArrayType], - ) -> Mapping[str, gdmr_types.ArrayType]: - """Processes the substep measurements and returns a new modified version of it. - - Args: - model: the simulation model. - data: the simulation data. - consumed_substep_measurements: the substep measurements up in the - processing chain that are required by this processor, i.e. with keys - specified by `consumed_substep_measurements_spec`. - - Returns the new substep measurements. Note that the (key, value) pairs in - `consumed_substep_measurements` are removed from the running substep - measurements dictionary. If users want to keep some of the elements it is - their responsibility to retain them in the output dictionary. - """ diff --git a/src/experimental/reaf/core/task_logic_layer.py b/src/experimental/reaf/core/task_logic_layer.py deleted file mode 100644 index 51b25639..00000000 --- a/src/experimental/reaf/core/task_logic_layer.py +++ /dev/null @@ -1,342 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Task logic layer for the Robotics Environment Authoring Framework.""" - -from collections.abc import Mapping, Sequence -import itertools -from typing import Protocol - -from absl import logging -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types -from reaf.core import commands_processor as reaf_commands_processor -from reaf.core import default_discount_provider -from reaf.core import discount_provider as reaf_discount_provider -from reaf.core import features_observer as reaf_features_observers -from reaf.core import features_producer as reaf_features_producer -from reaf.core import logger as reaf_logger -from reaf.core import reward_provider as reaf_reward_provider -from reaf.core import termination_checker as reaf_termination_checker -from reaf.core import zero_reward_provider -import tree - - -class _ResettableObject(Protocol): - """Protocol for an object that can be reset.""" - - def reset(self) -> None: - ... - - -class TaskLogicLayer: - """Task logic layer for the Robotics Environment Authoring Framework.""" - - def __init__( - self, - *, - commands_processors: Sequence[reaf_commands_processor.CommandsProcessor], - features_producers: Sequence[reaf_features_producer.FeaturesProducer], - termination_checkers: Sequence[ - reaf_termination_checker.TerminationChecker - ], - reward_provider: reaf_reward_provider.RewardProvider | None = None, - discount_provider: reaf_discount_provider.DiscountProvider | None = None, - features_observers: Sequence[ - reaf_features_observers.FeaturesObserver - ] = (), - loggers: Sequence[reaf_logger.Logger] = (), - ): - """Initializes the task logic layer. - - Args: - commands_processors: `CommandsProcessor`s that modify the commands before - being sent down to the DACL. They are called sequentially, starting from - the commands supplied by the policy and ending with the commands that - will be sent to the DACL. - features_producers: `FeaturesProducer`s that generate new features. - Measurements collected by the DACL and features produced by these - `FeaturesProducer`s are then merged into the final feature set that is - provided to the `reward_provider`, `termination_checkers`, - `discount_provider`, `features_observers`, and `loggers`. - termination_checkers: `TerminationChecker`s that check the episode - termination based on the final feature set. - reward_provider: `RewardProvider` that computes a reward based on the - final feature set. If None, the ZeroRewardProvider is used and the - reward is set to 0. - discount_provider: `DiscountProvider` that compute a discount based on the - final feature set and final termination state. If None, the - DefaultDiscountProvider is used returning 0 for termination and 1 for - truncation and non-termination. - features_observers: `FeaturesObserver`s that get a view over the final - feature set. - loggers: `Logger`s for logging measurements, features, and commands in the - task layer. - """ - self._commands_processors = commands_processors - self._features_producers = features_producers - self._reward_provider = ( - reward_provider - if reward_provider - else zero_reward_provider.ZeroRewardProvider() - ) - self._termination_checkers = termination_checkers - self._discount_provider = ( - discount_provider - if discount_provider - else default_discount_provider.DefaultDiscountProvider() - ) - self._features_observers = features_observers - self._loggers = list(loggers) - - # We make a set of all resettable objects so that these objects only get - # their resets called once. This is important for e.g. when having a single - # object that derives from two interfaces. - self._resettable_objects: list[_ResettableObject] = [] - unique_ids = set() - for resettable_object in itertools.chain( - self._commands_processors, - self._features_producers, - self._termination_checkers, - [self._reward_provider], - [self._discount_provider], - ): - resettable_object_id = id(resettable_object) - if resettable_object_id not in unique_ids: - unique_ids.add(resettable_object_id) - self._resettable_objects.append(resettable_object) - - def validate_spec( - self, - *, - dacl_commands_spec: Mapping[str, gdmr_types.AnyArraySpec], - dacl_measurements_spec: Mapping[str, specs.Array], - ) -> None: - """Checks that the specs have consistent keys.""" - logging.vlog(3, "Validate features processing") - self._validate_features_spec(dacl_measurements_spec) - self._validate_commands_spec(dacl_commands_spec) - - def features_spec( - self, - dacl_measurements_spec: Mapping[str, specs.Array], - ) -> Mapping[str, specs.Array]: - """Returns the features spec as exposed by the task layer.""" - spec = dict(dacl_measurements_spec) - for features_producer in self._features_producers: - spec.update(features_producer.produced_features_spec()) - - return spec - - def commands_spec( - self, dacl_commands_spec: Mapping[str, gdmr_types.AnyArraySpec] - ) -> Mapping[str, gdmr_types.AnyArraySpec]: - """Returns the commands spec exposed by the task layer.""" - # Each processor consumes commands (as described by its - # `consumed_commands_spec`) and outputs a potentially different set of - # commands (as described by its `produced_commands_keys`). - # Starting with the DACL command spec, we iterate in reverse order (i.e. in - # the direction DACL -> Policy) through every processor to remove the - # `produced_commands_keys` from the spec, and add their - # `consumed_commands_spec` to the spec. - spec: Mapping[str, gdmr_types.AnyArraySpec] = dict(dacl_commands_spec) - for processor in reversed(self._commands_processors): - processor_produced_keys = processor.produced_commands_keys() - spec = { - key: value - for key, value in spec.items() - if key not in processor_produced_keys - } - spec.update(processor.consumed_commands_spec()) - return spec - - def reward_spec(self) -> tree.Structure[specs.Array]: - return self._reward_provider.reward_spec() - - def discount_spec(self) -> tree.Structure[specs.Array]: - return self._discount_provider.discount_spec() - - def perform_reset(self) -> None: - """Reset the internal state of the task logic layer.""" - for resettable_object in self._resettable_objects: - resettable_object.reset() - - def compute_all_features( - self, measurements: Mapping[str, gdmr_types.ArrayType] - ) -> Mapping[str, gdmr_types.ArrayType]: - """Computes all the task logic features given the current measurements.""" - for logger in self._loggers: - logger.record_measurements(measurements) - - # Produce all the features. - current_features = dict(measurements) - for feature_producer in self._features_producers: - required_features = { - key: current_features[key] - for key in feature_producer.required_features_keys() - } - current_features.update( - feature_producer.produce_features(required_features) - ) - - # Observe the features. - for feature_observer in self._features_observers: - feature_observer.observe_features(current_features) - - # Log the resulting features. - for logger in self._loggers: - logger.record_features(current_features) - return current_features - - def compute_final_commands( - self, - policy_commands: Mapping[str, gdmr_types.ArrayType], - ) -> Mapping[str, gdmr_types.ArrayType]: - """Processes the policy commands and returns the final processed commands.""" - current_commands = dict(policy_commands) - for processor in self._commands_processors: - # Get commands to be consumed by the processor and remove the commands - # from the current_commands.. They correspond to the - # `consumed_command_spec`. - consumed_commands = { - key: current_commands.pop(key) - for key in processor.consumed_commands_spec().keys() - } - produced_commands = processor.process_commands(consumed_commands) - current_commands.update(produced_commands) - - # Log the modification. - for logger in self._loggers: - logger.record_commands_processing( - processor.name, consumed_commands, produced_commands - ) - - for logger in self._loggers: - logger.record_final_commands(current_commands) - return current_commands - - def compute_reward( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> tree.Structure[gdmr_types.ArrayType]: - """Computes the reward given the features.""" - return self._reward_provider.compute_reward({ - key: features[key] - for key in self._reward_provider.required_features_keys() - }) - - def check_for_termination( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> reaf_termination_checker.TerminationResult: - """Checks for termination.""" - current_state = reaf_termination_checker.TerminationResult.DO_NOT_TERMINATE - for termination_checker in self._termination_checkers: - current_state = reaf_termination_checker.TerminationResult.combine( - current_state, - termination_checker.check_termination({ - key: features[key] - for key in termination_checker.required_features_keys() - }), - ) - return current_state - - def compute_discount( - self, - features: Mapping[str, gdmr_types.ArrayType], - termination_state: reaf_termination_checker.TerminationResult, - ) -> tree.Structure[gdmr_types.ArrayType]: - """Computes the discount given the features and termination state.""" - return self._discount_provider.compute_discount( - { - key: features[key] - for key in self._discount_provider.required_features_keys() - }, - termination_state, - ) - - def add_logger(self, logger: reaf_logger.Logger) -> None: - self._loggers.append(logger) - - def remove_logger(self, logger: reaf_logger.Logger) -> None: - self._loggers.remove(logger) - - def _validate_features_spec( - self, dacl_measurements_spec: Mapping[str, specs.Array] - ) -> None: - """Validates the features spec.""" - # Check measurements/features path. - current_key_set = set(dacl_measurements_spec.keys()) - logging.vlog(4, "DACL measurements keys: %s", current_key_set) - - for producer in self._features_producers: - logging.vlog( - 4, - "Producer %s requires %s.", - producer.name, - producer.required_features_keys(), - ) - # Check required features are available. - if not producer.required_features_keys().issubset(current_key_set): - raise ValueError( - "Failed to validate feature specs for feature producer" - f" {producer.name}. Missing keys:" - f" {producer.required_features_keys() - current_key_set}" - ) - # Check that there are not duplicates in the output. - if not current_key_set.isdisjoint( - producer.produced_features_spec().keys() - ): - raise ValueError( - "Failed to validate feature specs for feature producer" - f" {producer.name}. Duplicate keys:" - f" {current_key_set & producer.produced_features_spec().keys()}" - ) - # Now extend the spec. - logging.vlog( - 4, - "Update available keys (from producer %s) with %s.", - producer.name, - producer.produced_features_spec().keys(), - ) - current_key_set.update(producer.produced_features_spec().keys()) - logging.vlog(4, "Available features keys %s.", current_key_set) - - def _validate_commands_spec( - self, dacl_commands_spec: Mapping[str, gdmr_types.AnyArraySpec] - ) -> None: - """Validates the commands spec.""" - # Check commands. Starting from the DACL command specs we propagate up in - # the chain. - logging.vlog(3, "Validate commands processing from DACL to Policy.") - current_key_set = set(dacl_commands_spec.keys()) - logging.vlog(4, "DACL commands keys: %s", current_key_set) - - for processor in reversed(self._commands_processors): - produced_command_keys = processor.produced_commands_keys() - - logging.vlog( - 4, - "Processor %s: specs (accepted keys) %s. Exposes %s.", - processor.name, - processor.consumed_commands_spec().keys(), - produced_command_keys, - ) - if not produced_command_keys.issubset(current_key_set): - raise ValueError( - "Failed to validate commands specs for commands processor" - f" {processor.name}. Missing (consumable) keys:" - f" {produced_command_keys - current_key_set}" - ) - # Remove the produced keys and add the consumed commands specs (as the - # processor is mutable). - current_key_set = current_key_set - produced_command_keys - current_key_set.update(processor.consumed_commands_spec().keys()) diff --git a/src/experimental/reaf/core/termination_checker.py b/src/experimental/reaf/core/termination_checker.py deleted file mode 100644 index 754c250d..00000000 --- a/src/experimental/reaf/core/termination_checker.py +++ /dev/null @@ -1,94 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Checks if the episode should terminate.""" - -import abc -from collections.abc import Mapping -import enum -from typing import Self - -from gdm_robotics.interfaces import types as gdmr_types - - -class TerminationResult(enum.IntFlag): - """The result of an episode termination check. - - The TerminationResult refers to the possibility for an episode to terminate. - For more details on the concept of termination we refer the readers to - https://github.com/google-deepmind/dm_env/blob/master/docs/index.md#environment-api-and-semantics. - - Note that this enum does not refer to the possible causes of termination but - only how the termination impacts the learning process. - - The result can be one of the following options: - - DO_NOT_TERMINATE: The episode should not terminate. - - TRUNCATE: The epsisode should terminate. Truncation implies a non-failure - final state. Usually this is associated with a non-zero discount. - - TERMINATE: The episode should terminate as the environment is in some - final state. Usually this is associated with a zero discount for e.g. - finite-horizon RL. - """ - - DO_NOT_TERMINATE = 0 - TRUNCATE = 2**0 - TERMINATE = 2**1 - - def is_terminated(self) -> bool: - return self == TerminationResult.TERMINATE - - def is_truncated(self) -> bool: - return self == TerminationResult.TRUNCATE - - def combine(self, other: Self) -> Self: - # TERMINATE has precedence over TRUNCATE, which in turn has precedence over - # DO_NOT_TERMINATE. Given the definitions above, this can be implemented as - # a maximum operator. To also enable tracing with JAX, we implement this in - # a branchless manner using bitwise operations that preserve the type. - # Note that JAX will trace TerminationResult values as ints. - # Approach: - # - self ^ (self ^ other) == other - # - (-1 * (self < other)) will be bitmask of all 1s iff self < other. - # - AND with (self ^ other) will result in either update or no-op bitmask. - return self ^ ((self ^ other) & (-1 * (self < other))) - - -class TerminationChecker(abc.ABC): - """Checks if the episode should terminate.""" - - @abc.abstractmethod - def name(self) -> str: - """Returns a unique string identifier for this object.""" - - @abc.abstractmethod - def check_termination( - self, required_features: Mapping[str, gdmr_types.ArrayType] - ) -> TerminationResult: - """Checks if the episode should terminate. - - Args: - required_features: Measurements and features computed by the task logic - that are required by this checker, i.e. that have keys specified by - `required_features_keys`. - - Returns if the episode should terminate (and if so, what kind of - termination). - """ - - @abc.abstractmethod - def required_features_keys(self) -> set[str]: - """Returns the feature keys that are required to check the termination.""" - - def reset(self) -> None: - """Resets the internal state of the termination checker.""" - ... diff --git a/src/experimental/reaf/core/trigger.py b/src/experimental/reaf/core/trigger.py deleted file mode 100644 index 20901873..00000000 --- a/src/experimental/reaf/core/trigger.py +++ /dev/null @@ -1,29 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Defines an event-based waiting behaviour.""" - -import abc - - -class Trigger(abc.ABC): - """Defines an event-based waiting behaviour.""" - - @property - @abc.abstractmethod - def name(self) -> str: - """Returns the name of the trigger.""" - - @abc.abstractmethod - def wait_for_event(self) -> None: - """Blocks until the next event.""" diff --git a/src/experimental/reaf/core/zero_reward_provider.py b/src/experimental/reaf/core/zero_reward_provider.py deleted file mode 100644 index c5d2267a..00000000 --- a/src/experimental/reaf/core/zero_reward_provider.py +++ /dev/null @@ -1,48 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Reward provider which provides a zero reward.""" - -from collections.abc import Mapping -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types -import numpy as np -from reaf.core import reward_provider -import tree - - -class ZeroRewardProvider(reward_provider.RewardProvider): - """Reward provider which provides a zero reward.""" - - def __init__(self, name: str = 'zero_reward_provider'): - self._name = name - - def name(self) -> str: - return self._name - - def compute_reward( - self, required_features: Mapping[str, gdmr_types.ArrayType] - ) -> tree.Structure[gdmr_types.ArrayType]: - """Returns a zero reward.""" - return np.zeros(1) - - def reward_spec(self) -> tree.Structure[specs.Array]: - """Returns the spec for a constant zero reward.""" - return specs.Array(shape=(1,), dtype=float) - - def required_features_keys(self) -> set[str]: - """Returns empty set. - - There are no feature keys that are required to compute the reward. - """ - return set() From 239aa1f8569e0dc6cf42765ccadb4b7b47288b2d Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sun, 12 Apr 2026 13:59:58 -0700 Subject: [PATCH 042/251] Minor updates to documentation across multiple sections. PiperOrigin-RevId: 898652783 Change-Id: Idb671910c18ed3406722b8a1307f97ae9a194139 --- doc/XMLreference.rst | 130 ++++++++++++++---------------- doc/computation/index.rst | 102 ++++++++++++++--------- doc/mjx.rst | 12 +-- doc/modeling.rst | 120 +++++++++++---------------- doc/overview.rst | 108 +++++++++++++++---------- doc/programming/extension.rst | 6 +- doc/programming/index.rst | 21 ++--- doc/programming/modeledit.rst | 30 +++---- doc/programming/simulation.rst | 26 +++--- doc/programming/visualization.rst | 93 +++++++++------------ doc/python.rst | 7 +- 11 files changed, 322 insertions(+), 333 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index a1443753..a5486dec 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -266,7 +266,7 @@ This element does not strictly belong to MJCF. Instead it is a meta-element, use files in a single document object model (DOM) before parsing. The included file must be a valid XML file with a unique top-level element. This top-level element is removed by the parser, and the elements below it are inserted at the location of the :el:`include` element. At least one element must be inserted as a result of this procedure. The -:el:`include` element can be used where ever an XML element is expected in the MJCF file. Nested includes are allowed, +:el:`include` element can be used wherever an XML element is expected in the MJCF file. Nested includes are allowed, however a given XML file can be included at most once in the entire model. After all the included XML files have been assembled into a single DOM, it must correspond to a valid MJCF model. Other than that, it is up to the user to decide how to use includes and how to modularize large files if desired. @@ -746,10 +746,8 @@ has any effect. The settings here are global and apply to the entire model. .. _compiler-coordinate: :at:`coordinate`: :at-val:`[local, global], "local"` - In previous versions, this attribute could be used to specify whether frame positions and orientations are expressed - in local or global coordinates, but the "global" option has since been removed, and will cause an error to be - generated. In order to convert older models which used the "global" option, load and save them in MuJoCo 2.3.3 or - older. + This attribute specifies whether frame positions and orientations are expressed in local coordinates. The "global" + option is no longer supported and will cause an error. .. _compiler-angle: @@ -826,8 +824,8 @@ has any effect. The settings here are global and apply to the entire model. .. _compiler-usethread: :at:`usethread`: :at-val:`[false, true], "true"` - If this attribute is "true", the model compiler will run in multi-threaded mode. Currently multi-threading is used - for computing the length ranges of actuators and for parallel loading and processing of meshes. + If this attribute is "true", the model compiler will run in multi-threaded mode. Multi-threading is used for + computing the length ranges of actuators and for parallel loading and processing of meshes. .. _compiler-fusestatic: @@ -995,25 +993,24 @@ compilation. .. _size-njmax: :at:`njmax`: :at-val:`int, "-1"` |nbsp| |nbsp| |nbsp| (legacy) - This is a deprecated legacy attribute. In versions prior to 2.3.0, it determined the maximum allowed number - of constraints. Currently it means "allocate as much memory as would have previously been required for this number of + This is a deprecated legacy attribute. It previously determined the maximum allowed number of constraints. + Currently it means "allocate as much memory as would have previously been required for this number of constraints". Specifying both :at:`njmax` and :at:`memory` leads to an error. .. _size-nconmax: :at:`nconmax`: :at-val:`int, "-1"` |nbsp| |nbsp| |nbsp| (legacy) This attribute specifies the maximum number of contacts that will be generated at runtime. If the number of active - contacts is about to exceed this value, the extra contacts are discarded and a warning is generated. This is a - deprecated legacy attribute which prior to version 2.3.0 affected memory allocation. It is kept for backwards - compatibility and debugging purposes. + contacts is about to exceed this value, the extra contacts are discarded and a warning is generated. This is a + deprecated legacy attribute which previously affected memory allocation. It is kept for backwards compatibility + and debugging purposes. .. _size-nstack: :at:`nstack`: :at-val:`int, "-1"` |nbsp| |nbsp| |nbsp| (legacy) - This is a deprecated legacy attribute. In versions prior to 2.3.0, it determined the maximum size of the - :ref:`stack `. After version 2.3.0, if :at:`nstack` is specified, then the size of ``mjData.narena`` is - ``nstack * sizeof(mjtNum)`` bytes, plus an additional space for the constraint solver. Specifying both :at:`nstack` - and :at:`memory` leads to an error. + This is a deprecated legacy attribute. It previously determined the maximum size of the :ref:`stack `. + If :at:`nstack` is specified, then the size of ``mjData.narena`` is ``nstack * sizeof(mjtNum)`` bytes, plus an + additional space for the constraint solver. Specifying both :at:`nstack` and :at:`memory` leads to an error. .. _size-nuserdata: @@ -1290,8 +1287,8 @@ The full list of processing steps applied by the compiler to each mesh is as fol :at:`inertia`: :at-val:`[convex, exact, legacy, shell], "legacy"` This attribute controls how the mesh is used when mass and inertia are - :ref:`inferred from geometry`. The current default value :at-val:`legacy` will be changed - to :at-val:`convex` in a future release. + :ref:`inferred from geometry`. The default value is :at-val:`legacy` for backward + compatibility, but :at-val:`convex` is recommended. :at-val:`convex`: Use the mesh's convex hull to compute volume and inertia, assuming uniform density. @@ -1602,8 +1599,8 @@ also known as terrain map, is a 2D matrix of elevation data. The data can be spe .. _asset-skin-rgba: .. _asset-skin-group: -:ref:`Skins` have been moved under the new grouping element :ref:`deformable`. They can -still be specified here but this functionality is now deprecated and will be removed in the future. +:ref:`Skins` are grouped under the :ref:`deformable` element. Specifying them here is +deprecated. @@ -1618,7 +1615,7 @@ The texture data can be loaded from files or can be generated by the compiler as different texture types require different parameters, only a subset of the attributes below are used for any given texture. Provisions are provided for loading cube and skybox textures from individual image files. -Currently, three file formats are supported for loading textures: PNG, KTX, and a custom MuJoCo texture format. The +Three file formats are supported for loading textures: PNG, KTX, and a custom MuJoCo texture format. The loader will use the extension of the file name to determine which format to use, defaulting to the custom format if the extension is not recognized. Alternatively, the content_type attribute can be used to specify the format explicitly. Only ``image/png``, ``image/ktx``, or ``image/vnd.mujoco.texture`` are supported. @@ -1917,8 +1914,8 @@ properties are grouped together. This attribute should be in the range [0 1]. If the value is greater than 0, and the material is applied to a plane or a box geom, the renderer will simulate reflectance. The larger the value, the stronger the reflectance. For boxes, only the face in the direction of the local +Z axis is reflective. Simulating reflectance properly requires - ray-tracing which cannot (yet) be done in real-time. We are using the stencil buffer and suitable projections - instead. Only the first reflective geom in the model is rendered as such. This adds one extra rendering pass through + ray-tracing. This renderer uses the stencil buffer and suitable projections instead to approximate it. Only the + first reflective geom in the model is rendered as such. This adds one extra rendering pass through all geoms, in addition to the extra rendering pass added by each shadow-casting light. .. _asset-material-metallic: @@ -2183,7 +2180,7 @@ between the body where it is defined and the body's parent. If multiple joints a corresponding spatial transformations (of the body frame relative to the parent frame) are applied in order. If no joints are defined, the body is welded to its parent. Joints cannot be defined in the world body. At runtime the positions and orientations of all joints defined in the model are stored in the vector ``mjData.qpos``, in the order in -which the appear in the kinematic tree. The linear and angular velocities are stored in the vector ``mjData.qvel``. +which they appear in the kinematic tree. The linear and angular velocities are stored in the vector ``mjData.qvel``. These two vectors have different dimensionality when free or ball joints are used, because such joints represent rotations as unit quaternions. @@ -2479,7 +2476,7 @@ helps clarify the role of bodies and geoms in MuJoCo. .. _body-geom-type: :at:`type`: :at-val:`[plane, hfield, sphere, capsule, ellipsoid, cylinder, box, mesh, sdf], "sphere"` - Type of geometric shape. The keywords have the following meaning: The **plane** type defines a plane which is + Type of geometric shape. The keywords have the following meaning: The **plane** type defines a surface which is infinite for collision detection purposes. It can only be attached to the world body or static children of the world. The plane passes through a point specified via the pos attribute. It is normal to the Z axis of the geom's local frame. The +Z direction corresponds to empty space. Thus the position and orientation defaults of (0,0,0) and @@ -3218,9 +3215,9 @@ object. These elements are bodies (with their own joints and geoms) that become the macro. The macro expansion is done by the model compiler. If the resulting model is then saved, the macro will be replaced with the actual model elements. The defaults mechanism used in the rest of MJCF does not apply here, even if the parent body has a childclass attribute defined. Instead there are internal defaults adjusted automatically for each -composite object type. See :ref:`CComposite` in the modeling guide for more detailed explanation. Note that there used -to be several composite types, but they have incrementally replaced by :ref:`replicate` (for repeated -objects) and :ref:`flexcomp` (for soft objects). Therefore, the only supported composite type is now +composite object type. See :ref:`CComposite` in the modeling guide for more detailed explanation. Note that several +legacy composite types have been replaced by :ref:`replicate` (for repeated objects) and +:ref:`flexcomp` (for soft objects). Therefore, the only supported composite type is now cable, which produces an inextensible chain of bodies connected with ball joints. .. _body-composite-prefix: @@ -3237,8 +3234,8 @@ cable, which produces an inextensible chain of bodies connected with ball joints The **cable** type creates a 1D chain of bodies connected with ball joints, each having a geom with user-defined type (cylinder, capsule or box). The geometry can either be defined with an array of 3D vertex coordinates :at:`vertex` - or with prescribed functions with the option :at:`curve`. Currently, only linear and trigonometric functions are - supported. For example, an helix can be obtained with curve="cos(s) sin(s) s". The size is set with the option + or with prescribed functions with the option :at:`curve`. Only linear and trigonometric functions are supported. For + example, an helix can be obtained with curve="cos(s) sin(s) s". The size is set with the option :at:`size`, resulting in :math:`f(s)=\{\text{size}[1]\cdot\cos(2\pi\cdot\text{size}[2]),\; \text{size}[1]\cdot\sin(2\pi\cdot\text{size}[2]),\; \text{size}[0]\cdot s\}`. @@ -3360,7 +3357,7 @@ joints should be created, as well as to adjust the attributes of both automatic '''''''''''''''''''''''''''''''''''''''' This sub-element adjusts the attributes of the geoms in the composite object. The default attributes are the same as in -the rest of MJCF (except that user-defined defaults have no effect here). Note that the geom sub-element can appears +the rest of MJCF (except that user-defined defaults have no effect here). Note that the geom sub-element can appear only once, unlike joint and tendon sub-elements which can appear multiple times. This is because different kinds of joints and tendons have different sets of attributes, while all geoms in the composite object are identical. @@ -3500,8 +3497,8 @@ Associate this composite with an :ref:`engine plugin`. Either :at:`plu :el-prefix:`body/` |-| **flexcomp** |*| ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Similar to :el:`composite`, this element (new in MuJoCo 3.0) is not a model element, but rather a macro which expands -into multiple model elements representing a deformable entity. In particular this macro creates one +Similar to :el:`composite`, this element is not a model element, but rather a macro which expands into multiple +model elements representing a deformable entity. In particular this macro creates one :ref:`flex` element, a number of bodies that are children of the body in which the :el:`flexcomp` is defined, and optionally one :ref:`flex equality` which constrains all flex edges to their initial length. A number of attributes are specified here and then passed through to the automatically-constructed flex. The primary @@ -3518,9 +3515,9 @@ flexcomp point is not pinned, a new child body is created at the coordinates of parent body), and then the coordinates of the flex vertex within that new body are (0,0,0). The mechanism for :ref:`pinning` flexcomp points is explained below. -Composite objects (available prior to MuJoCo 3.0) needed bodies with geoms for collisions, and sites for connecting -tendons which generated shape-preserving forces. In contrast, flexes generate their own collisions and shape-preserving -forces (as well as rendering), thus the bodies created here are much simpler: no geoms, sites or tendons are needed. +While :el:`composite` objects need bodies with geoms for collisions and sites for connecting tendons, flexes +generate their own collisions and shape-preserving forces. Thus the bodies created here are much simpler: no geoms, +sites or tendons are needed. Most of the bodies created here have 3 orthogonal slider joints, corresponding to freely moving point masses. In some cases we generate radial slider joints, allowing only expansion and contraction. Since no geoms are generated, the bodies need to have explicit inertial parameters. @@ -4142,8 +4139,8 @@ This is a grouping element and does not have any attributes. It groups elements :el-prefix:`deformable/` |-| **flex** |*| ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Flexible objects (or flexes) were added in MuJoCo 3.0. These are collections of massless stretchable geometric elements -(capsules, triangles or tetrahedra) connecting vertices that are defined within different moving body frames. These +Flexible objects (or flexes) are collections of massless stretchable geometric elements (capsules, triangles or +tetrahedra) connecting vertices that are defined within different moving body frames. These stretchable elements support collisions and contact forces, which are then distributed to all the interconnected bodies. Flexes also generate passive and constraint forces as needed to simulate deformable entities with the desired material properties. The modeling of flexes is automated and simplified by the :ref:`flexcomp` element. In most @@ -4321,9 +4318,8 @@ extensions specific to flexes. flex. The pre-defined vertex-element pairs are generated by the model compiler automatically. In 3D, internal collision checks are performed within each tetraheron: each vertex is collided with the plane corresponding to the opposing triangle face (again using the flex radius). The resulting contacts are always created with condim 1, gap 0, - margin 0. Note that internal contacts modify the behavior implied by the :ref:`elasticity - parameters` and is recommended only for flexes where element inversion cannot be prevented. The - default value of this attribute was changed from "true" to "false" in version 3.3.1. + margin 0. Note that internal contacts modify the behavior implied by the :ref:`elasticity parameters` + and is recommended only for flexes where element inversion cannot be prevented. .. _flex-contact-selfcollide: @@ -4369,7 +4365,7 @@ extensions specific to flexes. :at:`passive`: :at-val:`[true, false], "false"` When enabled, the contact is not added to the contact solver but it is instead used to compute passive (spring-damper) contact forces. All contacts, regardless of the specified condim, are frictionless (condim 1). This - is an experimental feature and might change in future releases. + is an experimental feature. .. _deformable-skin: @@ -4867,13 +4863,7 @@ constraint type is only supported for dimension 3 flexes with trilinear or quadr Name of the flex whose strain is being constrained. -.. _equality-distance: -:el-prefix:`equality/` |-| **distance** |*| -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -Distance equality constraints were removed in MuJoCo version 2.2.2. If you are using an earlier version, please switch -to the corresponding version of the documentation. .. _tendon: @@ -5157,7 +5147,7 @@ illustrated the use of pulleys. This element creates an abstract tendon whose length is defined as a linear combination of joint positions. Recall that the tendon length and its gradient are the only quantities needed for simulation. Thus we could define any scalar -function of joint positions, call it "tendon", and plug it in MuJoCo. Presently the only such function is a fixed linear +function of joint positions, call it "tendon", and use it in MuJoCo. The only such function supported is a fixed linear combination. The attributes of fixed tendons are a subset of the attributes of spatial tendons and have the same meaning as above. @@ -5574,8 +5564,8 @@ specify them independently. :el-prefix:`actuator/` |-| **motor** |*| ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -This and the next three elements are the :ref:`Actuator shortcuts ` discussed earlier. When a -such shortcut is encountered, the parser creates a :el:`general` actuator and sets its dynprm, gainprm and biasprm +This and the next three elements are the :ref:`Actuator shortcuts ` discussed earlier. When +such a shortcut is encountered, the parser creates a :el:`general` actuator and sets its dynprm, gainprm and biasprm attributes to the internal defaults shown above, regardless of any default settings. It then adjusts dyntype, gaintype and biastype depending on the shortcut, parses any custom attributes (beyond the common ones), and translates them into regular attributes (i.e., attributes of the :el:`general` actuator type) as explained here. @@ -5775,7 +5765,7 @@ This element has one custom attribute in addition to the common attributes: :el-prefix:`actuator/` |-| **velocity** |*| ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -This element creates a velocity servo. Note that in order create a PD controller, one has to define two actuators: a +This element creates a velocity servo. Note that in order to create a PD controller, one has to define two actuators: a position servo and a velocity servo. This is because MuJoCo actuators are SISO while a PD controller takes two control inputs (reference position and reference velocity). When using this actuator, it is recommended to use the implicitfast or implicit :ref:`integrators`. @@ -6204,7 +6194,7 @@ This element has nine custom attributes in addition to the common attributes: :at:`tausmooth`: :at-val:`real, "0"` Width of smooth transition between activation and deactivation time constants. Units of ctrl, must be - nonegative. + nonnegative. .. _actuator-muscle-range: @@ -6598,7 +6588,7 @@ computations. In addition to the sensors created with the elements below, the top-level function :ref:`mj_step` computes the quantities mjData.cacc, mjData.cfrc_int and mjData.crfc_ext corresponding to body accelerations and interaction forces. Some of these quantities are used to compute the output of -certain sensors (force, acceleration etc.) but even if no such sensors are defined in the model, these quantities +certain sensors (force, acceleration, etc.) but even if no such sensors are defined in the model, these quantities themselves are "features" that could be of interest to the user. @@ -6611,7 +6601,7 @@ This element creates a touch sensor. The active sensor zone is defined by a site site's volume, and involves a geom attached to the same body as the site, the corresponding contact force is included in the sensor reading. If a contact point falls outside the sensor zone, but the normal ray intersects the sensor zone, it is also included. This re-projection feature is needed because, without it, the contact point may leave the sensor zone -from the back (due to soft contacts) and cause an erroneous force reading. The output of this sensor is non-negative +from the back (due to soft contacts) and cause an erroneous force reading. The output of this sensor is a non-negative scalar. It is computed by adding up the (scalar) normal forces from all included contacts. .. _sensor-touch-name: @@ -6878,10 +6868,10 @@ defined as geoms whose rgba (or whose material rgba) has alpha=0, are also exclu invisible in the visualizer by disabling their geom group are not excluded; this is because sensor calculations are independent of the visualizer. -The image on the right (click to see the model being visualized) shows two rangefinder sensors attached to a perspective and -an orthographic camera, with frustums visualized. Both cameras have 4x4 resolution, for 16 rays each. The rangefinder -sensors report :at:`data` = :at-val:`"dist point normal"` (see below), so we can see the rays (lines), the intersection -points (spheres) and the surface normals (arrows). +The image on the right (click to see the model being visualized) shows two rangefinder sensors attached to a +perspective and an orthographic camera, with frustums visualized. Both cameras have 4x4 resolution, for 16 rays +each. The rangefinder sensors report :at:`data` = :at-val:`"dist point normal"` (see below), so we can see the rays +(lines), the intersection points (spheres) and the surface normals (arrows). .. _sensor-rangefinder-data: @@ -8328,7 +8318,7 @@ sensor reports information that was discovered during the collision and constrai from ``mjData.{contact, efc_force}``, ignoring contacts that were filtered out by the :ref:`standard` mechanism and produce no force. -Contact sensor output involves three stages: **matching**, **reduction** and **extraction**. +Contact sensor output involves three stages: **matching**, **reduction**, and **extraction**. Matching Selects a set of contacts from ``mjData.contact`` using criteria defined by :ref:`geom1`, @@ -8346,7 +8336,7 @@ Matching Reduction Reduces the number of matched contacts to exactly :ref:`num` sub-arrays, or "slots". If less than :at:`num` contacts match, the remaining slots are set to be identically zero. Note that the default, - "unsorted" reduction criterion is potentitally non-deterministic. See :ref:`reduce` below. + "unsorted" reduction criterion is potentially non-deterministic. See :ref:`reduce` below. Extraction Copies the set of fields specified by the user into each slot, see :ref:`data`. @@ -8400,7 +8390,7 @@ Extraction Importantly, the :at:`data` attribute can contain **multiple sequential data types**, as long as the relative order---as listed above---is maintained. For example, :at:`data` = :at-val:`"found force dist"` will return 5 numbers - per contact (the concateneated values of [found, force, dist]), while :at:`data` = :at-val:`"force found dist"` is an + per contact (the concatenated values of [found, force, dist]), while :at:`data` = :at-val:`"force found dist"` is an error because :at-val:`found` must come before :at-val:`force`. Missing contacts @@ -8599,8 +8589,8 @@ This element creates a user sensor. MuJoCo does not know how to compute the outp should install the callback :ref:`mjcb_sensor` which is expected to fill in the sensor data in ``mjData.sensordata``. The specification in the XML is used to allocate space for this sensor, and also determine which MuJoCo object it is attached to and what stage of computation it needs before the data can be computed. Note that the MuJoCo object -referenced here can be a tuple, which in turn can reference a custom collection of MuJoCo objects -- for example several -bodies whose center of mass is of interest. +referenced here can be a tuple, which in turn can reference a custom collection of MuJoCo objects -- for example +several bodies whose center of mass is of interest. If a user sensor is of :ref:`stage` "vel" or "acc", then :ref:`mj_subtreeVel` or :ref:`mj_rnePostConstraint` will be triggered, respectively. @@ -8877,7 +8867,7 @@ visualization should somehow be simplified. .. _visual-quality-shadowsize: :at:`shadowsize`: :at-val:`int, "4096"` - This attribute specifies the size of the square texture used for shadow mapping. Higher values result is smoother + This attribute specifies the size of the square texture used for shadow mapping. Higher values result in smoother shadows. The size of the area over which a :ref:`light ` can cast shadows also affects smoothness, so these settings should be adjusted jointly. The default here is somewhat conservative. Most modern GPUs are able to handle significantly larger textures without slowing down. @@ -9246,7 +9236,7 @@ disables the rendering of the corresponding object. .. _visual-rgba-contactgap: -:at:`contactgap`: :at-val:`real(4), "0.5, 0.8, 0.9, 1"` +:at:`contactgap`: :at-val:`real(4), "0.5 0.8 0.9 1"` Color of contacts that fall in the contact gap (and are thereby excluded from contact force computations). .. _visual-rgba-rangefinder: @@ -9315,7 +9305,7 @@ if omitted. ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ | This element sets the attributes of the dummy :ref:`mesh ` element of the defaults class. -| The available attributes are: :ref:`scale ` and :ref:`scale `. +| The available attributes are: :ref:`scale ` and :ref:`maxhullvert `. .. _default-material: @@ -9766,8 +9756,8 @@ if omitted. ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This and the next three elements set the attributes of the :ref:`general ` element using -:ref:`Actuator shortcuts `. It does not make sense to use more than one such shortcut in the same defaults -class, because they set the same underlying attributes, replacing any previous settings. All +:ref:`Actuator shortcuts `. It does not make sense to use more than one such shortcut in the same +defaults class, because they set the same underlying attributes, replacing any previous settings. All :ref:`motor ` attributes are available here except: name, class, joint, jointinparent, site, refsite, tendon, slidersite, cranksite. @@ -10228,7 +10218,7 @@ See :ref:`exPlugin` for more details. :el-prefix:`plugin/` |-| **instance** |*| ''''''''''''''''''''''''''''''''''''''''' -Declares a plugin instance. Explicit instances declaration is required when multiple elements are backed by the same +Declares a plugin instance. Explicit instance declaration is required when multiple elements are backed by the same plugin, or when global plugin configuration is desired. See plugin :ref:`declaration` and :ref:`configuration` for more details. diff --git a/doc/computation/index.rst b/doc/computation/index.rst index b3122212..67b1623d 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -202,7 +202,7 @@ earlier arm model :ref:`example ` the model has :math:`\nv = 13` degre for each of the 4 hinge joints, and 6 for the free-floating object. They appear in the same order in all system-level vectors and matrices whose dimensionality is :math:`\nv`. The data corresponding to a given model element can be recovered via indexing operations as illustrated in the :ref:`Clarifications` section in the Overview chapter. Vectors -and matrices with dimensionality :math:`\nq` are somewhat different because the active :ref:`constraints ` +and matrices with dimensionality :math:`\nc` are somewhat different because the active :ref:`constraints ` change at runtime. In that case there is still a fixed enumeration order (corresponding to the order in which the model elements appear in ``mjModel``) but any inactive constraints are omitted. @@ -524,13 +524,13 @@ the *new* velocity. *Implicit* Euler means: \end{aligned} Comparing :eq:`eq_semimplicit` and :eq:`eq_implicit`, we see that the acceleration :math:`a_{t+h}=\dot{v}_{t+h}` on the -right hand side of the velocity update is evaluated at the *next time step*. While evaluating the next acceleration +right-hand side of the velocity update is evaluated at the *next time step*. While evaluating the next acceleration is not possible without stepping, we can use a first-order Taylor expansion to approximate this quantity, and take a single step of Newton's method. When the expansion is only with respect to velocity (and not position), the integrator is known as *implicit-in-velocity* Euler. This approach is particularly effective in systems where instabilities are caused by velocity-dependent forces: multi-joint pendulums, bodies tumbling through space, systems with lift and drag forces, and systems with substantial damping in tendons and actuators. Writing the -acceleration as a function of velocity: :math:`a_t = a(v_t)`, the velocity update we aim to approximate is +acceleration as a function of velocity, :math:`a_t = a(v_t)`, the velocity update we aim to approximate is .. math:: v_{t+h} = v_t + h a(v_{t+h}) @@ -550,7 +550,7 @@ Thus we define the derivative D &\equiv {\partial \over \partial v} \Big(\tau(v) - c (v) + J^T f(v)\Big) \end{aligned} -The velocity update corresponding to Newton's method is as follows. First, we expand the right hand side to first order +The velocity update corresponding to Newton's method is as follows. First, we expand the right-hand side to first order .. math:: \begin{aligned} @@ -585,7 +585,7 @@ Semi-implicit with implicit joint damping (``Euler``) For this method, :math:`D` only includes derivatives of joint damping. Note that in this case :math:`D` is diagonal and :math:`\widehat{M}` is symmetric, so :math:`L^TL` decomposition (a variant of Cholesky) can be used. This factorization is stored in ``mjData.qH``. If the model has no joint damping or the - :ref:`eulerdamp` disable-flag is set, implicit damping is disabled and the semi-implicit + :ref:`eulerdamp` disable flag is set, implicit damping is disabled and the semi-implicit update :eq:`eq_semimplicit` is used, rather than :eq:`eq_implicit_update`, avoiding the additional factorization of :math:`\widehat{M}` (*additional* because :math:`M` is already factorized for the acceleration update :eq:`eq_forward`). @@ -612,12 +612,12 @@ Fast implicit-in-velocity (``implicitfast``) 4th-order Runge-Kutta (``RK4``) One advantage of our continuous-time formulation is that we can use higher order integrators such as Runge-Kutta or - multistep methods. The only such integrator currently implemented is the fixed-step `4th-order Runge-Kutta method + multistep methods. MuJoCo implements the fixed-step `4th-order Runge-Kutta method `__, though users can easily implement other integrators by calling :ref:`mj_forward` and integrating accelerations themselves. We have observed that for energy-conserving systems (`example <../_static/pendulum.xml>`__), RK4 is qualitatively better than the single-step methods, both in terms of stability and accuracy, even when the timestep is decreased by - a factor of 4 (so the computational effort is identical). In the presence of large velocity- dependent forces, if the + a factor of 4 (so the computational effort is identical). In the presence of large velocity-dependent forces, if the chosen single-step method integrates those forces implicitly, single-step methods can be significantly more stable than RK4. @@ -683,8 +683,8 @@ Constraint model MuJoCo has a very flexible constraint model, which is nevertheless handled in a uniform way by the :ref:`solver ` described later. Here we explain what the individual constraints are conceptually, and how they -are laid out in the system-level vector and matrices with dimensionality :math:`\nq`. Each conceptual constraint can -contribute one or more scalar constraints towards the total count :math:`\nq`, and each scalar constraint has a +are laid out in the system-level vector and matrices with dimensionality :math:`\nc`. Each conceptual constraint can +contribute one or more scalar constraints towards the total count :math:`\nc`, and each scalar constraint has a corresponding row in the constraint Jacobian :math:`J`. Active constraints are ordered by type in the order in which the types are described below, and then by model element within each type. The types are: equality, friction loss, limit, contact. Limits are handled as frictionless contacts by the solver and are not treated as a separate type internally. We @@ -698,7 +698,7 @@ Equality MuJoCo can model equality constraints in the general form :math:`r(q) = 0` where :math:`r` can be any differentiable scalar or vector function of the position vector :math:`q`. It has the semantics of a residual. The solver can actually work with non-holonomic constraints as well, but we do not yet have such constraint types defined. Each equality -constraint contributes :math:`\dim(r)` elements to the total constraint count :math:`\nq`. The corresponding block in +constraint contributes :math:`\dim(r)` elements to the total constraint count :math:`\nc`. The corresponding block in :math:`J` is simply the Jacobian of the residual, namely :math:`\partial r / \partial q`. Note that due to the properties of quaternions, differentiation with respect to :math:`q` produces vectors of size :math:`\nv` rather than :math:`\nq`. @@ -1002,34 +1002,34 @@ We will use the following notation beyond the notation introduced earlier: - Size - Description * - :math:`z` - - :math:`\nq` + - :math:`\nc` - constraint deformations * - :math:`\omega` - - :math:`\nq` + - :math:`\nc` - velocity of constraint deformations * - :math:`k` - - :math:`\nq` + - :math:`\nc` - virtual constraint stiffness * - :math:`b` - - :math:`\nq` + - :math:`\nc` - virtual constraint damping * - :math:`d` - - :math:`\nq` + - :math:`\nc` - constraint impedance * - :math:`A(q)` - - :math:`\nq \times \nq` + - :math:`\nc \times \nc` - inverse inertia in constraint space * - :math:`R(q)` - - :math:`\nq \times \nq` + - :math:`\nc \times \nc` - diagonal regularizer in constraint space * - :math:`\ar` - - :math:`\nq` + - :math:`\nc` - reference acceleration in constraint space * - :math:`\au(q, v, \tau)` - - :math:`\nq` + - :math:`\nc` - unconstrained acceleration in constraint space * - :math:`\ac(q, v, \dot{v})` - - :math:`\nq` + - :math:`\nc` - constrained acceleration in constraint space * - :math:`\mathcal{K}(q)` - @@ -1066,7 +1066,8 @@ explain what it means and why it makes sense. That problem is :label: eq:primal The new players here are the diagonal regularizer :math:`R > 0` which makes the constraints soft, and the reference -acceleration :math:`\ar` which stabilizes the constraints. The latter is similar in spirit to Baumgarte stabilization, +acceleration :math:`\ar` which stabilizes the constraints; the latter is a spring-damper defined in the +:ref:`Parameters ` section below. It is similar in spirit to Baumgarte stabilization, but instead of adding a constraint force directly, it modifies the optimization problem whose solution is the constraint force. Since this problem is itself constrained, the relation between :math:`\ar` and :math:`f` is generally non-linear. The quantities :math:`R` and :math:`\ar` are computed from the solver :ref:`parameters ` as described @@ -1254,13 +1255,15 @@ implementation, we do not actually compute the acceleration term :math:`\dot{J} problems depend on differences of constraint-space accelerations, and so this term would cancel out even if we were to compute it. -Note that the quadratic term in the inverse problem is weighted by :math:`R` instead of :math:`A+R`. This tells us two -things. First, in the limit :math:`R \to 0` corresponding to hard constraints the inverse is no longer defined, as one -would expect. Second and more useful, the inverse problem is diagonal, i.e., it decouples into independent optimization -problems over the individual constraint forces. The only remaining coupling is due to the constraint set :math:`\Omega`, -but that set is also decoupled over the conceptual constraints discussed earlier. It turns out that all these -independent optimization problems can be solved analytically. The only non-trivial case is the elliptic friction cone -model; we have shown how it can be handled in the above-referenced +Note that the quadratic term in the inverse problem is weighted by :math:`R` instead of :math:`A+R`. This is the key +structural insight: the :math:`A` matrix cancels entirely, leaving only :math:`R` in the quadratic term. Two +consequences follow. First, in the limit :math:`R \to 0` corresponding to hard constraints the inverse is no longer +defined, as one would expect. Second, the inverse problem is diagonal, i.e., it decouples into independent optimization +problems over the individual constraint forces. Since :math:`R` is diagonal, no matrix inversion or factorization is +needed -- the inverse dynamics require no optimization at all, only analytical formulas. The only remaining coupling is +due to the constraint set :math:`\Omega`, but that set is also decoupled over the conceptual constraints discussed +earlier. It turns out that all these independent optimization problems can be solved analytically. The only non-trivial +case is the elliptic friction cone model; we have shown how it can be handled in the above-referenced `paper `__. It requires a certain coupling of the diagonal values of :math:`R`, which is automatically enforced by MuJoCo so as to enable an exact analytical inverse for every model. @@ -1287,12 +1290,15 @@ Each solver algorithm can be used with both pyramidal and elliptic friction cone representations of the constraint Jacobian and related matrices. **CG** : conjugate gradient method - This algorithm uses the non-linear conjugate gradient method with the Polak-Ribiere-Plus formula. Line-search is - exact, using Newton's method in one dimension, with analytical second derivatives. + This algorithm uses the non-linear conjugate gradient method with the Polak-Ribiere-Plus formula (non-negative + :math:`\beta`). Line-search is exact, using Newton's method in one dimension with analytical second derivatives on + the piecewise-quadratic cost. CG has no setup cost. **Newton** : Newton's method This algorithm implements the exact Newton method, with analytical second-order derivatives and Cholesky - factorization of the Hessian. The line-search is the same as in the CG method. It is the default solver. + factorization of the Hessian. The line-search is the same as in the CG method. When constraint states change between + iterations (e.g., a constraint transitions from quadratic to linear), the Hessian factorization is updated + incrementally via rank-1 Cholesky updates, avoiding full refactorization. It is the default solver. **PGS** : Projected Gauss-Seidel method This is the most common algorithm used in physics simulators, and used to be the default in MuJoCo, until we @@ -1327,6 +1333,21 @@ representations of the constraint Jacobian and related matrices. handle elliptic cones without approximating them. It does more work per contact, however the contact dimensionality is smaller, and these two factors roughly balance each other. +**NoSlip** : post-processing pass + This is not a standalone solver but a post-processing step, enabled by setting ``noslip_iterations`` to a positive + value in :ref:`option + )"; + + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + + // set nonzero velocity and ctrl + d->qvel[0] = 1.0; + d->ctrl[0] = 0.5; + + // forward to compute act_dot, etc. + mj_forward(m, d); + + // compute analytical derivatives + mjd_smooth_vel(m, d, /* flg_bias = */ 1); + + // extract diagonal of qDeriv + mjtNum qDeriv_diag = d->qDeriv[m->D_rowadr[0] + m->D_rownnz[0] - 1]; + + // expected: K*(dVdw - K)*(1 - exp(-h/te))/R + // with K=2, R=0.5, te=0.001, h=0.002, kd=5, dVdw=-5 + mjtNum K = 2.0, R = 0.5, te = 0.001, h = 0.002, kd = 5.0; + mjtNum expected = K * (-kd - K) * (1 - mju_exp(-h / te)) / R; + EXPECT_NEAR(qDeriv_diag, expected, 1e-10) + << "stateful DC motor derivative should match analytical formula"; + + mj_deleteData(d); + mj_deleteModel(m); +} + + +// verify that stateful DC motor derivative converges to stateless as te -> 0 +TEST_F(DerivativeTest, DCMotorStatefulConvergesToStateless) { + // stateless DC motor with position controller + static constexpr char xml_stateless[] = R"( + + + )"; + + // stateful DC motor with very small te + static constexpr char xml_stateful[] = R"( + + + )"; + + char error[1024]; + mjModel* m_sl = LoadModelFromString(xml_stateless, error, sizeof(error)); + ASSERT_THAT(m_sl, NotNull()) << error; + mjData* d_sl = mj_makeData(m_sl); + + mjModel* m_sf = LoadModelFromString(xml_stateful, error, sizeof(error)); + ASSERT_THAT(m_sf, NotNull()) << error; + mjData* d_sf = mj_makeData(m_sf); + + // set identical state + d_sl->qvel[0] = d_sf->qvel[0] = 1.0; + d_sl->ctrl[0] = d_sf->ctrl[0] = 0.5; + + // forward and compute derivatives + mj_forward(m_sl, d_sl); + mj_forward(m_sf, d_sf); + mjd_smooth_vel(m_sl, d_sl, 1); + mjd_smooth_vel(m_sf, d_sf, 1); + + // extract diagonals + mjtNum diag_sl = d_sl->qDeriv[m_sl->D_rowadr[0] + m_sl->D_rownnz[0] - 1]; + mjtNum diag_sf = d_sf->qDeriv[m_sf->D_rowadr[0] + m_sf->D_rownnz[0] - 1]; + + EXPECT_NEAR(diag_sf, diag_sl, 1e-6) + << "stateful derivative should converge to stateless as te -> 0"; + + mj_deleteData(d_sf); + mj_deleteModel(m_sf); + mj_deleteData(d_sl); + mj_deleteModel(m_sl); +} + // Utility: Rotate flex grid void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name, double angle) { diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 3c9a05d4..fce6cf68 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -1805,8 +1805,12 @@ TEST_F(DCMotorTest, StatefulPositionWithCurrentMode) { // di/dt = (1.485 - 0.0125 - 0.5) / 0.5 = 0.9725 / 0.5 = 1.945 EXPECT_NEAR(data->act_dot[adr+2], 1.945, MjTol(1e-12, 1e-5)); - // Force is just K * current since current is stateful - EXPECT_NEAR(data->actuator_force[0], 0.05 * 0.5, MjTol(1e-12, 1e-5)); + // Force is K * next_activation (actearly is always on for DC motors) + // Inline mj_nextActivation for te = 0.5 + mjtNum te = 0.5; + mjtNum h = model->opt.timestep; + mjtNum next_i = 0.5 + data->act_dot[adr+2] * te * (1 - mju_exp(-h / te)); + EXPECT_NEAR(data->actuator_force[0], 0.05 * next_i, MjTol(1e-12, 1e-5)); mj_deleteData(data); mj_deleteModel(model); @@ -1902,7 +1906,11 @@ TEST_F(DCMotorTest, CurrentPlusThermal) { data->ctrl[0] = V; mj_forward(model, data); - EXPECT_NEAR(data->actuator_force[0], K * current, MjTol(1e-12, 1e-5)); + // Force uses next_activation (actearly is always on for DC motors) + // Inline mj_nextActivation for te = 0.01 / R = 0.005 + mjtNum h = model->opt.timestep; + mjtNum next_i = current + data->act_dot[adr+1] * te * (1 - mju_exp(-h / te)); + EXPECT_NEAR(data->actuator_force[0], K * next_i, MjTol(1e-12, 1e-5)); double R_hot = R * (1 + 0.004 * dT); double T_dot = (R_hot * current * current - dT / RT) / C; diff --git a/test/engine/testdata/derivative/dcmotor.xml b/test/engine/testdata/derivative/dcmotor.xml index a1053bb6..3d9e895a 100644 --- a/test/engine/testdata/derivative/dcmotor.xml +++ b/test/engine/testdata/derivative/dcmotor.xml @@ -16,6 +16,18 @@ + + + + + + + + + + + + @@ -31,5 +43,19 @@ + + + + + + + + + From f55aeb04fa308b66c4dec5c134d191e84b28c10f Mon Sep 17 00:00:00 2001 From: Tom Erez Date: Mon, 13 Apr 2026 08:20:32 -0700 Subject: [PATCH 048/251] Add protocols for authoring simulation environments. This is for preview only, we discourage users from using this in production code. PiperOrigin-RevId: 899009633 Change-Id: I2de08c5cc4ece69fa135f3fd02df7542bf66d7d0 --- .../reaf/core/action_space_adapter.py | 43 ++ .../reaf/core/commands_processor.py | 91 ++++ .../data_acquisition_and_control_layer.py | 170 ++++++ .../reaf/core/default_discount_provider.py | 79 +++ .../core/default_observation_space_adapter.py | 231 +++++++++ src/experimental/reaf/core/device.py | 54 ++ .../reaf/core/device_coordinator.py | 87 ++++ .../reaf/core/discount_provider.py | 61 +++ src/experimental/reaf/core/entity.py | 65 +++ src/experimental/reaf/core/environment.py | 490 ++++++++++++++++++ .../reaf/core/features_observer.py | 34 ++ .../reaf/core/features_producer.py | 56 ++ src/experimental/reaf/core/logger.py | 80 +++ .../reaf/core/numpy_mock_assertions.py | 98 ++++ .../reaf/core/observation_space_adapter.py | 42 ++ .../core/pass_through_action_space_adapter.py | 55 ++ src/experimental/reaf/core/reward_provider.py | 292 +++++++++++ .../reaf/core/substep_commands_processor.py | 104 ++++ .../core/substep_measurements_processor.py | 103 ++++ .../reaf/core/task_logic_layer.py | 342 ++++++++++++ .../reaf/core/termination_checker.py | 94 ++++ src/experimental/reaf/core/trigger.py | 29 ++ .../reaf/core/zero_reward_provider.py | 48 ++ 23 files changed, 2748 insertions(+) create mode 100644 src/experimental/reaf/core/action_space_adapter.py create mode 100644 src/experimental/reaf/core/commands_processor.py create mode 100644 src/experimental/reaf/core/data_acquisition_and_control_layer.py create mode 100644 src/experimental/reaf/core/default_discount_provider.py create mode 100644 src/experimental/reaf/core/default_observation_space_adapter.py create mode 100644 src/experimental/reaf/core/device.py create mode 100644 src/experimental/reaf/core/device_coordinator.py create mode 100644 src/experimental/reaf/core/discount_provider.py create mode 100644 src/experimental/reaf/core/entity.py create mode 100644 src/experimental/reaf/core/environment.py create mode 100644 src/experimental/reaf/core/features_observer.py create mode 100644 src/experimental/reaf/core/features_producer.py create mode 100644 src/experimental/reaf/core/logger.py create mode 100644 src/experimental/reaf/core/numpy_mock_assertions.py create mode 100644 src/experimental/reaf/core/observation_space_adapter.py create mode 100644 src/experimental/reaf/core/pass_through_action_space_adapter.py create mode 100644 src/experimental/reaf/core/reward_provider.py create mode 100644 src/experimental/reaf/core/substep_commands_processor.py create mode 100644 src/experimental/reaf/core/substep_measurements_processor.py create mode 100644 src/experimental/reaf/core/task_logic_layer.py create mode 100644 src/experimental/reaf/core/termination_checker.py create mode 100644 src/experimental/reaf/core/trigger.py create mode 100644 src/experimental/reaf/core/zero_reward_provider.py diff --git a/src/experimental/reaf/core/action_space_adapter.py b/src/experimental/reaf/core/action_space_adapter.py new file mode 100644 index 00000000..014e02eb --- /dev/null +++ b/src/experimental/reaf/core/action_space_adapter.py @@ -0,0 +1,43 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Adapts environment action into suitable commands format accepted by REAF.""" + +import abc +from collections.abc import Mapping + +from gdm_robotics.interfaces import types as gdmr_types + + +class ActionSpaceAdapter(abc.ABC): + """Adapts environment action into suitable commands format accepted by REAF. + + Implementations of this interface are responsible for converting the more + generic action accepted by the environment (e.g. a flat numpy array) into the + more constraining format accepted as commands by REAF, i.e. a dictionary of + string to tensors. + """ + + @abc.abstractmethod + def commands_from_environment_action( + self, environment_action: gdmr_types.ActionType + ) -> Mapping[str, gdmr_types.ArrayType]: + """Converts the environment action into commands accepted by REAF.""" + + @abc.abstractmethod + def action_spec(self) -> gdmr_types.ActionSpec: + """Returns the action spec exposed by the environment.""" + + @abc.abstractmethod + def task_commands_keys(self) -> set[str]: + """Returns the keys for the commands exposed to the task layer.""" diff --git a/src/experimental/reaf/core/commands_processor.py b/src/experimental/reaf/core/commands_processor.py new file mode 100644 index 00000000..4cc12b4a --- /dev/null +++ b/src/experimental/reaf/core/commands_processor.py @@ -0,0 +1,91 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Abstract class for commands manipulation in the task logic layer.""" + +import abc +from collections.abc import Mapping + +from gdm_robotics.interfaces import types as gdmr_types + + +class CommandsProcessor(abc.ABC): + """Perform commands manipulation. + + The following describes the processing pipeline starting from the top (closer + to the policy) to the bottom (interfacing with the DACL commands spec). + + Assume that we have two processing units: + Processor 1) has a consumed_commands_spec for two keys: "p1/c1" and "p1/c2". + Its produced_commands_keys are "p2/c1". + Processor 2) has a consumed_commands_spec for "p2/c1". Its + produced_commands_keys are "p3/c1" and "p3/c2". + + Specs are propagated starting from the bottom: + 1) In this example assume that the DACL exposes "p3/c1", "p3/c2" and "p3/c3". + 2) Processor 2) returns ("p3/c1", "p3/c2") from input "p2/c1". This means that + the global commands spec exposed at this level is "p2/c1" and the + unprocessed "p3/c3". + 3) Processor 1) returns "p2/c1" from input ("p1/c1", "p1/c2"). By applying the + same transformation rule, we can obtain the final commands spec exposed by + the full processing pipeline: "p1/c1", "p1/c2" and "p3/c3". + + "p1/c1" "p1/c2" "p3/c3" + | | | + ----------------- | + | P1 | | + ----------------- | + | "p2/c1" | + ----------------- | + | P2 | | + ----------------- | + | "p3/c1" | "p3/c2" | + | | | + ------------------------------------ + | DACL | + ------------------------------------ + """ + + @property + @abc.abstractmethod + def name(self) -> str: + """Returns a unique string identifier for this object.""" + + @abc.abstractmethod + def process_commands( + self, consumed_commands: Mapping[str, gdmr_types.ArrayType] + ) -> Mapping[str, gdmr_types.ArrayType]: + """Processes the commands and returns a new modified version of it. + + Args: + consumed_commands: the commands up in the processing chain (or provided by + the Environment) that are required by this processor, i.e. with keys + specified by `consumed_commands_spec`. + + Returns the new commands. Note that the data in consumed_commands is removed + from the global commands dictionary. If users want to keep some of the + elements it is their responsibility to retain them in the output + dictionary. + """ + + @abc.abstractmethod + def consumed_commands_spec(self) -> Mapping[str, gdmr_types.AnyArraySpec]: + """Spec of the commands consumed by this processor.""" + + @abc.abstractmethod + def produced_commands_keys(self) -> set[str]: + """Keys of the commands produced by this processor.""" + + def reset(self) -> None: + """Resets the internal state of the command processor.""" + ... diff --git a/src/experimental/reaf/core/data_acquisition_and_control_layer.py b/src/experimental/reaf/core/data_acquisition_and_control_layer.py new file mode 100644 index 00000000..e8fcc8ca --- /dev/null +++ b/src/experimental/reaf/core/data_acquisition_and_control_layer.py @@ -0,0 +1,170 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""REAF data acquisition and control layer to interface with the robotic setup.""" + +from collections.abc import Iterable, Mapping + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types +from reaf.core import device as reaf_device +from reaf.core import device_coordinator as reaf_coordinator +from reaf.core import trigger + + +class DataAcquisitionAndControlLayer: + """REAF data acquisition and control layer. + + The DACL is responsible to provide an interface for the robotic setup. + """ + + def __init__( + self, + *, + device_coordinator: reaf_coordinator.DeviceCoordinator, + commands_trigger: trigger.Trigger | None, + measurements_trigger: trigger.Trigger | None, + ): + """Initializes the DataAcquisitionAndControlLayer. + + Args: + device_coordinator: The coordinator representing a specific robotic setup. + Note that callers need to explicitly initialize and finalise the + coordinator. + commands_trigger: A trigger to unblock processing commands during a call + to `step`. + measurements_trigger: A trigger to unblock processing measurements during + a call to `step`. + """ + self._coordinator = device_coordinator + self._devices = self._coordinator.get_devices() + # The following checks that names of the devices are unique and their keys + # are "mergeable". + self._check_device_names_and_keys(self._devices) + + self._commands_trigger = commands_trigger + self._measurements_trigger = measurements_trigger + + # Create a map of supported commands keys for each Device. + self._commands_for_device = { + device.name: device.commands_spec().keys() for device in self._devices + } + + def begin_stepping(self) -> Mapping[str, gdmr_types.ArrayType]: + """Begins stepping the DACL and returns the current measurements.""" + self._coordinator.on_begin_stepping() + + # Wait for the first trigger to happen before collecting the measurements. + if self._measurements_trigger is not None: + self._measurements_trigger.wait_for_event() + return self._get_measurements() + + def end_stepping(self) -> None: + """Ends stepping the data acquisition and control layer.""" + self._coordinator.on_end_stepping() + + def _set_commands(self, commands: Mapping[str, gdmr_types.ArrayType]) -> None: + """Sets the commands of the data acquisition and control layer.""" + self._coordinator.before_set_commands() + for device in self._devices: + device_commands = { + k: v + for k, v in commands.items() + if k in self._commands_for_device[device.name] + } + device.set_commands(device_commands) + self._coordinator.after_set_commands() + + def _get_measurements(self) -> Mapping[str, gdmr_types.ArrayType]: + """Gets the measurements of the data acquisition and control layer.""" + measurements = {} + self._coordinator.before_get_measurements() + for device in self._devices: + measurements.update(device.get_measurements()) + + return measurements + + def step( + self, commands: Mapping[str, gdmr_types.ArrayType] + ) -> Mapping[str, gdmr_types.ArrayType]: + """Steps the data acquisition and control layer.""" + if self._commands_trigger is not None: + self._commands_trigger.wait_for_event() + self._set_commands(commands) + + if self._measurements_trigger is not None: + self._measurements_trigger.wait_for_event() + return self._get_measurements() + + def commands_spec(self) -> Mapping[str, gdmr_types.AnyArraySpec]: + """Returns the specs for the commands.""" + spec = {} + for device in self._devices: + spec.update(device.commands_spec()) + return spec + + def measurements_spec(self) -> Mapping[str, specs.Array]: + """Returns the specs for the measurements.""" + spec = {} + for device in self._devices: + spec.update(device.measurements_spec()) + return spec + + @property + def device_coordinator(self) -> reaf_coordinator.DeviceCoordinator: + return self._coordinator + + def _check_keys_have_been_formatted_correctly( + self, current_key_set: Iterable[str] + ) -> None: + """Check that keys haven't been left unformatted.""" + for key in current_key_set: + if key.find("{}") != -1: + raise ValueError( + "Keys should not contain '{}'. Did you mean to use format()?" + ) + + def _check_device_names_and_keys( + self, devices: Iterable[reaf_device.Device] + ) -> None: + """Raises error if device names are not unique or keys are not exclusive.""" + # Check names first. + all_names = [device.name for device in devices] + unique_names = set(all_names) + if len(unique_names) != len(all_names): + raise RuntimeError(f"Duplicate names when checking devices: {all_names}") + + # Check commands. + devices = tuple(devices) + current_specs = set() + for device in devices: + device_keys = device.commands_spec().keys() + self._check_keys_have_been_formatted_correctly(device_keys) + if not current_specs.isdisjoint(device_keys): + raise RuntimeError( + f"Duplicate keys when checking device {device.name}:" + f" {current_specs.intersection(device_keys)}" + ) + current_specs.update(device_keys) + + # Check measurements. + current_specs = set() + for device in devices: + device_keys = device.measurements_spec().keys() + self._check_keys_have_been_formatted_correctly(device_keys) + if not current_specs.isdisjoint(device_keys): + raise RuntimeError( + f"Duplicate keys when checking device {device.name}:" + f" {current_specs.intersection(device_keys)}" + ) + current_specs.update(device_keys) diff --git a/src/experimental/reaf/core/default_discount_provider.py b/src/experimental/reaf/core/default_discount_provider.py new file mode 100644 index 00000000..ff216b7c --- /dev/null +++ b/src/experimental/reaf/core/default_discount_provider.py @@ -0,0 +1,79 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Computes a constant discount given the termination state. + +This provider returns a discount of 0.0 in case of termination and 1.0 +otherwise (i.e. for truncation and not termination). + +It is usually safe to use this discount provider for environments that return +strictly positive rewards. +""" + +from collections.abc import Mapping + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types +import numpy as np +from reaf.core import discount_provider +from reaf.core import termination_checker +import tree + + +class DefaultDiscountProvider(discount_provider.DiscountProvider): + """Computes a constant discount given the termination state. + + This provider returns a discount of 0.0 in case of termination and 1.0 + otherwise (i.e. for truncation and not termination). + + It is usually safe to use this discount provider for environments that return + strictly positive rewards. + """ + + def __init__(self, name: str = "default_discount_provider"): + self._name = name + self._spec = specs.BoundedArray( + shape=(), dtype=np.float64, minimum=0.0, maximum=1.0, name="discount" + ) + + def name(self) -> str: + """Returns a unique string identifier for this object.""" + return self._name + + def compute_discount( + self, + unused_required_features: Mapping[str, gdmr_types.ArrayType], + termination_state: termination_checker.TerminationResult, + ) -> tree.Structure[gdmr_types.ArrayType]: + """Computes the discount. + + Args: + unused_required_features: Unused + termination_state: The termination state as computed by the termination + checkers. Returns the discount. + + Returns: + The discount. + """ + if termination_state == termination_state.TERMINATE: + return np.asarray(0).astype(self._spec.dtype) + else: # TRUNCATION or DO_NOT_TERMINATE + return np.asarray(1.0).astype(self._spec.dtype) + + def discount_spec(self) -> tree.Structure[specs.Array]: + """Returns the spec of the discount.""" + return self._spec + + def required_features_keys(self) -> set[str]: + """Returns the feature keys that are required to compute the discount.""" + return set() diff --git a/src/experimental/reaf/core/default_observation_space_adapter.py b/src/experimental/reaf/core/default_observation_space_adapter.py new file mode 100644 index 00000000..1261b3ca --- /dev/null +++ b/src/experimental/reaf/core/default_observation_space_adapter.py @@ -0,0 +1,231 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""ObservationSpaceAdapter supporting filtering, renaming and type conversion.""" + +import abc +from collections.abc import Iterable, Mapping +import dataclasses + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types +import numpy as np +import numpy.typing as npt +from reaf.core import observation_space_adapter +import tree + + +@dataclasses.dataclass(frozen=True, kw_only=True) +class RenameInfo: + original_key: str + renamed_key: str + + +class ObservationTypeMapper(abc.ABC): + """Maps from REAF features and specs into corresponding environment types.""" + + @abc.abstractmethod + def to_observation_spec( + self, features_spec: Mapping[str, specs.Array] + ) -> gdmr_types.ObservationSpec: + """Convert the features spec into the environment observation spec.""" + + @abc.abstractmethod + def to_observations( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> tree.Structure[gdmr_types.ArrayType]: + """Convert the features into the environment observations.""" + + +class _DefaultObservationTypeMapper(ObservationTypeMapper): + """An ObservationTypeMapper that returns the input features specs and dict. + + This `ObservationTypeMapper` maps observations from the more constrained + `Mapping[str, ArrayType]` used in the task layer to the more generic + `tree.Structure[ArrayType]` exposed by the GDM Environment. + """ + + def to_observation_spec( + self, features_spec: Mapping[str, specs.Array] + ) -> gdmr_types.ObservationSpec: + """Returns the features spec, unmodified, as a `gdmr_types.ObservationSpec`.""" + return features_spec + + def to_observations( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> tree.Structure[gdmr_types.ArrayType]: + """Returns the features, unmodified, as a `tree.Structure`.""" + return features + + +class DefaultObservationSpaceAdapter( + observation_space_adapter.ObservationSpaceAdapter +): + """Observation adapter supporting filtering, renaming and type conversion. + + This adapter supports filtering, renaming, and converting REAF features into + environment observations. + + The order of operations is the following: + 1) Filtering, i.e. feature selection. + 2) Downcasting floats to max_float_dtype. + 3) Renaming. + 4) Type conversion. + + Please refer to the constructor documentation for more information. + """ + + def __init__( + self, + *, + task_features_spec: Mapping[str, specs.Array], + selected_features: Iterable[str] | None, + renamed_features: Iterable[RenameInfo] | None, + observation_type_mapper: ObservationTypeMapper | None, + max_float_dtype: npt.DTypeLike = np.float64, + ): + """Initializes the observation space adapter. + + Args: + task_features_spec: The spec of all the features exposed by the task + layer. + selected_features: The features that will be exposed as observations. If + None, all features will be exposed, i.e. no filtering. + renamed_features: `RenameInfo` objects specifying which features should be + renamed and the corresponding new name. If empty or None, no renaming + will occur. + observation_type_mapper: An `ObservationTypeMapper` specifying how to + convert the task layer features data type (i.e. a Mapping[str, + ArrayType]) into the more generic type exposed by the GDM Environment + (i.e. a tree.Structure[ArrayType]). If None, an instance of + `_DefaultObservationTypeMapper` is used which converts the task logic + layer features dictionary to the more generic type (i.e. + `tree.Structure[ArrayType])` exposed by the environment. + max_float_dtype: The maximum float dtype to use for downcasting floats. + """ + if not np.issubdtype(max_float_dtype, np.floating): + raise ValueError( + 'max_float_dtype must be a floating point dtype. Got' + f' {max_float_dtype}' + ) + self._max_float_dtype = max_float_dtype + self._max_bits = np.finfo(self._max_float_dtype).bits + self._task_features_spec = task_features_spec + self._selected_filter = selected_features + self._renamed_features = renamed_features or () + self._observation_type_mapper = ( + observation_type_mapper or _DefaultObservationTypeMapper() + ) + self._check_specs_consistency() + # Compute the observation spec only once. + self._observation_spec = self._compute_observation_spec() + + def _check_specs_consistency(self) -> None: + # Check that filter keys are present in the spec. + if self._selected_filter is not None: + all_features = self._task_features_spec.keys() + features = set() + for feature in self._selected_filter: + if feature not in all_features: + raise ValueError(f'Feature {feature} is not present in the spec.') + features.add(feature) + else: + # No filter applied. Select all features. + features = set(self._task_features_spec.keys()) + + # Check renaming. + for rename_info in self._renamed_features: + if rename_info.original_key not in features: + raise ValueError( + f'Feature {rename_info.original_key} is not present in the spec.' + ) + + def observations_from_features( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> tree.Structure[gdmr_types.ArrayType]: + """Converts the features into the final environment observations.""" + # 1. Filter the observations. + if (selected_features := self._selected_filter) is None: + # No filter. Expose all observations. + filtered_features = dict(features) + else: + filtered_features = { + k: v for k, v in features.items() if k in selected_features # pytype: disable=unsupported-operands + } + + # 2. Downcast floats to max_float_dtype. + filtered_features = { + k: self._downcast_if_necessary(v) for k, v in filtered_features.items() + } + + # 3. Rename. + for rename_info in self._renamed_features: + # Rename the feature. + value = filtered_features[rename_info.original_key] + del filtered_features[rename_info.original_key] + filtered_features[rename_info.renamed_key] = value + + # 4. Convert type. + return self._observation_type_mapper.to_observations(filtered_features) + + def _compute_observation_spec(self) -> gdmr_types.ObservationSpec: + """Computes the observation spec.""" + # 1. Filter the specs + if (features_to_filter := self._selected_filter) is None: + # The observation spec corresponds to the task features spec. + filtered_specs = dict(self._task_features_spec) + else: + filtered_specs = { + k: v + for k, v in self._task_features_spec.items() + if k in features_to_filter # pytype: disable=unsupported-operands + } + + # 2. Downcast floats to max_float_dtype. + for k, v in filtered_specs.items(): + if self._dtype_needs_downcast(v.dtype): + filtered_specs[k] = v.replace(dtype=self._max_float_dtype) + + # 3. Rename. + for rename_info in self._renamed_features: + # Rename the feature. + value = filtered_specs[rename_info.original_key] + del filtered_specs[rename_info.original_key] + filtered_specs[rename_info.renamed_key] = value + + # 4. Convert the type. + return self._observation_type_mapper.to_observation_spec(filtered_specs) + + def observation_spec(self) -> gdmr_types.ObservationSpec: + """Returns the observation spec.""" + return self._observation_spec + + def task_features_keys(self) -> set[str]: + """Returns the task features keys that will be converted by this adapter.""" + return set(self._task_features_spec.keys()) + + def _downcast_if_necessary( + self, value: gdmr_types.ArrayType + ) -> gdmr_types.ArrayType: + if ( + hasattr(value, 'dtype') and self._dtype_needs_downcast(value.dtype) + ) or self._dtype_needs_downcast(type(value)): + return np.asarray(value).astype(self._max_float_dtype) + else: + return value + + def _dtype_needs_downcast(self, dtype: npt.DTypeLike) -> bool: + return ( + np.issubdtype(dtype, np.floating) + and np.finfo(dtype).bits > self._max_bits + ) diff --git a/src/experimental/reaf/core/device.py b/src/experimental/reaf/core/device.py new file mode 100644 index 00000000..cc53a0ca --- /dev/null +++ b/src/experimental/reaf/core/device.py @@ -0,0 +1,54 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""REAF basic device to interface with the robotic setup.""" + +import abc +from collections.abc import Mapping +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types + + +class Device(abc.ABC): + """REAF basic device to interface with the robotic setup. + + A device defines a single piece in the robotic setup. It should be + hermetic, that is, not depending on other Devices. The coordination of the + devices is responsibility of the DeviceCoordinator. + + Important: a Device should return the commands and measurements specs + immediately after initialisation without the need for any explicit + initialisation, nor for resource acquisition (e.g. connecting to the + hardware). + """ + + @property + @abc.abstractmethod + def name(self) -> str: + """Returns the name of this device.""" + + @abc.abstractmethod + def commands_spec(self) -> Mapping[str, gdmr_types.AnyArraySpec]: + """Returns the commands specs for this device.""" + + @abc.abstractmethod + def measurements_spec(self) -> Mapping[str, specs.Array]: + """Returns the measurements specs for this device.""" + + @abc.abstractmethod + def set_commands(self, commands: Mapping[str, gdmr_types.ArrayType]) -> None: + """Sets the commands for this device.""" + + @abc.abstractmethod + def get_measurements(self) -> Mapping[str, gdmr_types.ArrayType]: + """Returns the measurements provided by this device.""" diff --git a/src/experimental/reaf/core/device_coordinator.py b/src/experimental/reaf/core/device_coordinator.py new file mode 100644 index 00000000..233705bb --- /dev/null +++ b/src/experimental/reaf/core/device_coordinator.py @@ -0,0 +1,87 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Coordinates the devices composing a robotic setup.""" + +import abc +from collections.abc import Iterable +from reaf.core import device + + +class DeviceCoordinator(abc.ABC): + """Coordinates the devices composing a robotic setup. + + The `DeviceCoordinator` object is responsible for coordinating all the + devices constituting the robotic setup. Whilst the Device is hermetic, + the coordinator is responsible for passing information from one device to + the other if required. For example in a bimanual setup the coordinator is + charged with passing the position of each robot to the other so we can ensure + proper and safe interaction such as for example collision avoidance. + + The `DeviceCoordinator` can be configurable to enable different + properties on the robotic setup, e.g. adding or not adding a `Device` or + forwarding configuration to each `Device`. + + At the very least, the coordinator must implement `get_devices` + to return all the devices. We also provide `on_begin_stepping` and + `on_end_stepping` methods that will be called before the start of an episode + and after the end of the episode respectively. Note that resource acquisition + and subsequent release is completely up to the implementation. + + Finally, `before_set_commands`/`before_get_measurements` can be implemented to + coordinate devices behaviour before their corresponding functions are + called. + """ + + @property + @abc.abstractmethod + def name(self) -> str: + """Returns the name of the coordinator.""" + + @abc.abstractmethod + def get_devices(self) -> Iterable[device.Device]: + """Returns the devices composing the embodiment.""" + + # Lifecycle methods. + + def on_begin_stepping(self) -> None: + """Prepares the coordinator for having its devices called repeatedly. + + After `on_begin_stepping` the devices returned by `get_devices` will have + their `set_commands` and `get_measurements` called repeatedly until + `on_end_stepping` is called on this coordinator. + """ + + def on_end_stepping(self) -> None: + """Notifies the coordinator that the devices are no longer called. + + After `on_end_stepping` the devices returned by `get_devices` will not have + their `set_commands` and `get_measurements` called anymore until this + coordinator `on_begin_stepping` method is notified again. + """ + + # Step hooks methods. + + def before_set_commands(self) -> None: + """Prepares the coordinator to have its devices set_commands called.""" + + def after_set_commands(self) -> None: + """Notifies the coordinator that its devices got `set_commands` called.""" + + def before_get_measurements(self) -> None: + """Prepares the coordinator to have its devices get_measurements called. + + This method gets called immediately before the devices `get_measurements` + method is called and can be used to customise the devices state given the + whole setup state. + """ diff --git a/src/experimental/reaf/core/discount_provider.py b/src/experimental/reaf/core/discount_provider.py new file mode 100644 index 00000000..df1b22fa --- /dev/null +++ b/src/experimental/reaf/core/discount_provider.py @@ -0,0 +1,61 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Computes the discount.""" + +import abc +from collections.abc import Mapping + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types +from reaf.core import termination_checker +import tree + + +class DiscountProvider(abc.ABC): + """Computes the discount.""" + + @abc.abstractmethod + def name(self) -> str: + """Returns a unique string identifier for this object.""" + + @abc.abstractmethod + def compute_discount( + self, + required_features: Mapping[str, gdmr_types.ArrayType], + termination_state: termination_checker.TerminationResult, + ) -> tree.Structure[gdmr_types.ArrayType]: + """Computes the discount. + + Args: + required_features: Measurements and features computed by the task logic + that are required by this provider, i.e. that have keys specified by + `required_features_keys`. + termination_state: The termination state as computed by the termination + checkers. Returns the discount. + + Returns: + The discount. + """ + + @abc.abstractmethod + def discount_spec(self) -> tree.Structure[specs.Array]: + """Returns the spec of the discount.""" + + @abc.abstractmethod + def required_features_keys(self) -> set[str]: + """Returns the feature keys that are required to compute the discount.""" + + def reset(self) -> None: + """Resets the internal state of the discount provider.""" + ... diff --git a/src/experimental/reaf/core/entity.py b/src/experimental/reaf/core/entity.py new file mode 100644 index 00000000..68c2e38d --- /dev/null +++ b/src/experimental/reaf/core/entity.py @@ -0,0 +1,65 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Basic REAF-sim protocol to interface with the simulation.""" + +from collections.abc import Mapping +import typing + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types + + +class Entity(typing.Protocol): + """Basic REAF component to interface with the simulation. + + An entity defines a single component in the simulation that consumes substep + commands and outputs substep measurements at every simulation substep. It + should be hermetic, that is, not depending on other Entities. + + Important: an Entity should return the substep commands and substep + measurements specs immediately after initialisation without the need for any + explicit initialisation. + """ + + @property + def name(self) -> str: + """Instance name.""" + + def reset(self): + """Resets the entity.""" + + def substep_commands_spec( + self, + ) -> Mapping[str, specs.Array]: + """Spec for the substep commands.""" + + def substep_measurements_spec( + self, + ) -> Mapping[str, specs.Array]: + """Spec for the substep measurements.""" + + def set_substep_commands( + self, + model: typing.Any, + data: typing.Any, + consumed_substep_commands: Mapping[str, gdmr_types.ArrayType], + ) -> None: + """Sets the substep commands.""" + + def get_substep_measurements( + self, + model: typing.Any, + data: typing.Any, + ) -> Mapping[str, gdmr_types.ArrayType]: + """Returns the substep measurements.""" diff --git a/src/experimental/reaf/core/environment.py b/src/experimental/reaf/core/environment.py new file mode 100644 index 00000000..dca306f0 --- /dev/null +++ b/src/experimental/reaf/core/environment.py @@ -0,0 +1,490 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""The Robotics Environment Authoring Framework (REAF) Environment class.""" + +import abc +from collections.abc import Mapping +import enum +from typing import Generic + +from absl import logging +import dm_env +from dm_env import specs +from gdm_robotics.interfaces import environment as gdmr_env +from gdm_robotics.interfaces import types as gdmr_types +import numpy as np +from reaf.core import action_space_adapter as reaf_action_space_adapter +from reaf.core import data_acquisition_and_control_layer as reaf_dacl +from reaf.core import default_observation_space_adapter +from reaf.core import logger as reaf_logger +from reaf.core import observation_space_adapter as reaf_observation_space_adapter +from reaf.core import pass_through_action_space_adapter +from reaf.core import task_logic_layer as reaf_tll +import tree + + +class ActionSpecEnforcementOption(enum.StrEnum): + """Options for action spec enforcement.""" + + CLIP_TO_SPEC = "clip_to_spec" + IGNORE = "ignore" + WARNING = "warning" + RAISE_ERROR = "raise_error" + + +class EnvironmentReset(abc.ABC, Generic[gdmr_env.ResetOptions]): + """Support for general resets adhering to the GDM environment API.""" + + @abc.abstractmethod + def do_reset( + self, + config: gdmr_env.ResetOptions, + ) -> None: + """Resets the environment.""" + + def default_reset_configuration(self) -> gdmr_env.ResetOptions: + """Returns the default reset configuration.""" + return gdmr_env.Options() + + +class EndOfEpisodeHandler: + """Handler called after the last episode step.""" + + def on_end_of_episode_stepping(self, final_timestep: dm_env.TimeStep) -> None: + """Called when the episode has ended stepping. + + This will be called at the end of every episode, after all other triggers + have been resolved. Episodes can end either due to truncation or + termination, i.e. `timestep.step_type` is `StepType.LAST`, or due to an + early call to `Environment.reset()`. To verify whether it has indeed + ended due to truncation or termination, the implementer should test + `timestep.last()`. + + Note that the first reset after environment construction will not trigger + this handler, but it will be triggered before resolving any subsequent + environment resets, either implicit or explicit. + + Args: + final_timestep: The final timestep of the episode that ended stepping. + """ + + +class EnvironmentCloser(abc.ABC): + """Handler called when the environment is closed.""" + + @abc.abstractmethod + def close(self) -> None: + """Releases resources when the environment is closed. + + This method is called automatically when exiting the environment's + context manager (`with` statement). + """ + + +class Environment(gdmr_env.Environment): + """The Robotics Environment Authoring Framework (REAF) Environment class.""" + + def __init__( + self, + *, + data_acquisition_and_control_layer: reaf_dacl.DataAcquisitionAndControlLayer, + task_logic_layer: reaf_tll.TaskLogicLayer, + environment_reset: EnvironmentReset, + action_space_adapter: ( + reaf_action_space_adapter.ActionSpaceAdapter | None + ) = None, + observation_space_adapter: ( + reaf_observation_space_adapter.ObservationSpaceAdapter | None + ) = None, + end_of_episode_handler: EndOfEpisodeHandler | None = None, + environment_closer: EnvironmentCloser | None = None, + action_spec_enforcement_option: ActionSpecEnforcementOption = ActionSpecEnforcementOption.RAISE_ERROR, + ): + """Creates an environment. + + Args: + data_acquisition_and_control_layer: The layer for communicating with the + specific robotic setup. + task_logic_layer: The layer in charge of defining the task. + environment_reset: The `EnvironmentReset` specifying the function to be + called at environment reset and the default environment reset + configuration. + action_space_adapter: Adapter from the agent action space to the flattened + commands accepted by the task layer. If None the + PassThroughActionSpaceAdapter is used, meaning the entirety of the + commands dictionary is exposed to the agent. + observation_space_adapter: Adapter from the computed features to the + observations that are exposed to the agent. If None the + DefaultObservationSpaceAdapter is used, meaning all the features are + exposed to the agent as observations. + end_of_episode_handler: Called at the end of an episode, after the last + step. + environment_closer: Specifies the handler to be called when the + environment is closed. This is called automatically on exit if the + environment is used as a context manager. If None, no action is + performed at close. + action_spec_enforcement_option: How to enforce the action spec. If + `CLIP_TO_SPEC`, the action will be clipped to the spec. If `WARNING`, an + warning logged if the action is outside the spec. If `RAISE_ERROR`, an + error will be raised if the action is outside the spec. If `IGNORE`, + the action will be passed through. Default is `RAISE_ERROR`. + """ + + self._data_acquisition_and_control_layer = ( + data_acquisition_and_control_layer + ) + self._task_logic_layer = task_logic_layer + self._end_of_episode_handler = ( + end_of_episode_handler or EndOfEpisodeHandler() + ) + self._environment_reset = environment_reset + self._environment_closer = environment_closer + self._action_spec_enforcement_option = action_spec_enforcement_option + + # Before assigning the adapters, validate the specs on the task logic layer + # and the DACL. + self._validate_dacl_and_ttl_specs() + + ttl_commands_spec = self._task_logic_layer.commands_spec( + self._data_acquisition_and_control_layer.commands_spec() + ) + ttl_features_spec = self._task_logic_layer.features_spec( + self._data_acquisition_and_control_layer.measurements_spec() + ) + + if action_space_adapter is None: + action_space_adapter = ( + pass_through_action_space_adapter.PassThroughActionSpaceAdapter( + commands_spec=ttl_commands_spec + ) + ) + self._action_space_adapter = action_space_adapter + + if observation_space_adapter is None: + observation_space_adapter = ( + default_observation_space_adapter.DefaultObservationSpaceAdapter( + task_features_spec=ttl_features_spec, + selected_features=None, + renamed_features=None, + observation_type_mapper=None, + ) + ) + self._observation_space_adapter = observation_space_adapter + + # Now we can validate the adapters. + self._validate_adapters_specs() + + self._last_timestep: dm_env.TimeStep | None = None + self._should_finalize_episode = False + self._timestep_spec = gdmr_types.TimeStepSpec( + step_type=gdmr_types.STEP_TYPE_SPEC, + reward=self._task_logic_layer.reward_spec(), + discount=self._task_logic_layer.discount_spec(), + # The observation spec corresponds to the one exposed by the adapter. + observation=self._observation_space_adapter.observation_spec(), + ) + + self._zero_reward, self._zero_discount = tree.map_structure( + _read_only_zeros_like_spec, + (self._timestep_spec.reward, self._timestep_spec.discount), + ) + + def close(self) -> None: + """Frees any resources used by the environment.""" + if self._environment_closer is not None: + self._environment_closer.close() + + def default_reset_options(self) -> gdmr_env.ResetOptions: + return self._environment_reset.default_reset_configuration() + + def reset_with_options( + self, + *, + options: gdmr_env.ResetOptions, + ) -> dm_env.TimeStep: + """Starts a new sequence and returns the first `TimeStep`.""" + if self._should_finalize_episode: + self._finalize_episode() + self._environment_reset.do_reset(options) + self._task_logic_layer.perform_reset() + measurements = self._data_acquisition_and_control_layer.begin_stepping() + features = self._task_logic_layer.compute_all_features(measurements) + observations = self._compute_observations_from_features(features) + + self._last_timestep = self._restart(observation=observations) + # Make sure any early reset after this one triggers `_finalize_episode`. + self._should_finalize_episode = True + return self._last_timestep + + def action_spec(self) -> gdmr_types.ActionSpec: + """Defines the actions that should be provided to `step`.""" + # The action spec corresponds to the one exposed by the adapter. + return self._action_space_adapter.action_spec() + + def timestep_spec(self) -> gdmr_types.TimeStepSpec: + """Returns the spec associated to the returned TimeStep.""" + return self._timestep_spec + + def step(self, action: gdmr_types.ActionType) -> dm_env.TimeStep: + """Updates the environment according to action and returns a `TimeStep`.""" + + action = self._enforce_action_spec(action) + if self._last_timestep is None or self._last_timestep.last(): + return self.reset() + + # Process the action to obtain a command. + commands = self._compute_commands_from_agent_action(action) + commands = self._task_logic_layer.compute_final_commands(commands) + measurements = self._data_acquisition_and_control_layer.step(commands) + + # Compute all the features. + features = self._task_logic_layer.compute_all_features(measurements) + + # Compute the elements of the timestep. + reward = self._task_logic_layer.compute_reward(features) + termination_state = self._task_logic_layer.check_for_termination(features) + discount = self._task_logic_layer.compute_discount( + features, termination_state + ) + + observations = self._compute_observations_from_features(features) + + if termination_state.is_terminated(): + self._last_timestep = self._termination( + reward=reward, observation=observations + ) + elif termination_state.is_truncated(): + self._last_timestep = self._truncation( + reward=reward, observation=observations, discount=discount + ) + else: + self._last_timestep = self._transition( + reward=reward, observation=observations, discount=discount + ) + + if self._last_timestep.last(): + self._finalize_episode() + return self._last_timestep + + def _finalize_episode(self) -> None: + self._data_acquisition_and_control_layer.end_stepping() + # It's crucial to call `end_stepping` on the dacl before invoking the end + # of episode handler. This ensures no further `set_command` or + # `get_measurements` calls are made. In contrast, the end of episode + # handler might interact with devices, requiring them to be informed + # beforehand. + self._end_of_episode_handler.on_end_of_episode_stepping(self._last_timestep) + self._should_finalize_episode = False + + @property + def data_acquisition_and_control_layer( + self, + ) -> reaf_dacl.DataAcquisitionAndControlLayer: + return self._data_acquisition_and_control_layer + + @property + def task_logic_layer(self) -> reaf_tll.TaskLogicLayer: + return self._task_logic_layer + + @property + def environment_reset(self) -> EnvironmentReset: + return self._environment_reset + + @environment_reset.setter + def environment_reset(self, environment_reset: EnvironmentReset) -> None: + self._environment_reset = environment_reset + + def add_logger(self, logger: reaf_logger.Logger) -> None: + self._task_logic_layer.add_logger(logger) + + def remove_logger(self, logger: reaf_logger.Logger) -> None: + self._task_logic_layer.remove_logger(logger) + + def _validate_dacl_and_ttl_specs(self) -> None: + """Validates the specs on the task logic layer.""" + # Validate the spec on the task logic layer. + self._task_logic_layer.validate_spec( + dacl_commands_spec=( + self._data_acquisition_and_control_layer.commands_spec() + ), + dacl_measurements_spec=( + self._data_acquisition_and_control_layer.measurements_spec() + ), + ) + + def _validate_adapters_specs(self) -> None: + # Collect the full commands and features spec and validate them against + # the adapters. + commands_spec = set( + self._task_logic_layer.commands_spec( + self._data_acquisition_and_control_layer.commands_spec() + ).keys() + ) + features_spec = set( + self._task_logic_layer.features_spec( + self._data_acquisition_and_control_layer.measurements_spec() + ) + ) + + # Check the action space adapter. + adapter_keys = self._action_space_adapter.task_commands_keys() + + if adapter_keys != commands_spec: + raise ValueError( + "Mismatch between commands exposed by the action space adapter:" + f" {adapter_keys} and commands spec expected by the task layer:" + f" {commands_spec}." + ) + + # Check the observation spec adapter. + adapter_keys = self._observation_space_adapter.task_features_keys() + if not adapter_keys.issubset(features_spec): + raise ValueError( + "Failed to validate observation space adapter specs. Missing keys:" + f" {adapter_keys - features_spec}" + ) + + def _compute_observations_from_features( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> tree.Structure[gdmr_types.ArrayType]: + return self._observation_space_adapter.observations_from_features(features) + + def _compute_commands_from_agent_action( + self, agent_action: gdmr_types.ActionType + ) -> Mapping[str, gdmr_types.ArrayType]: + return self._action_space_adapter.commands_from_environment_action( + agent_action + ) + + def _restart( + self, + observation: tree.Structure[gdmr_types.ArrayType], + ) -> dm_env.TimeStep: + """Returns a `TimeStep` with `step_type` set to `StepType.FIRST`.""" + return dm_env.TimeStep( + step_type=np.asarray(dm_env.StepType.FIRST, dtype=np.uint8), + observation=observation, + reward=self._zero_reward, + discount=self._zero_discount, + ) + + def _transition( + self, + reward: tree.Structure[gdmr_types.ArrayType], + observation: tree.Structure[gdmr_types.ArrayType], + discount: tree.Structure[gdmr_types.ArrayType], + ) -> dm_env.TimeStep: + """Returns a `TimeStep` with `step_type` set to `StepType.MID`.""" + return dm_env.TimeStep( + step_type=np.asarray(dm_env.StepType.MID, dtype=np.uint8), + observation=observation, + reward=reward, + discount=discount, + ) + + def _termination( + self, + reward: tree.Structure[gdmr_types.ArrayType], + observation: tree.Structure[gdmr_types.ArrayType], + ) -> dm_env.TimeStep: + """Returns a `TimeStep` with `step_type` set to `StepType.LAST`.""" + return dm_env.TimeStep( + step_type=np.asarray(dm_env.StepType.LAST, dtype=np.uint8), + observation=observation, + reward=reward, + discount=self._zero_discount, + ) + + def _truncation( + self, + reward: tree.Structure[gdmr_types.ArrayType], + observation: tree.Structure[gdmr_types.ArrayType], + discount: tree.Structure[gdmr_types.ArrayType], + ) -> dm_env.TimeStep: + """Returns a `TimeStep` with `step_type` set to `StepType.LAST`.""" + return dm_env.TimeStep( + step_type=np.asarray(dm_env.StepType.LAST, dtype=np.uint8), + observation=observation, + reward=reward, + discount=discount, + ) + + def _enforce_action_spec( + self, action: gdmr_types.ActionType + ) -> gdmr_types.ActionType: + """Enforces the action spec.""" + match self._action_spec_enforcement_option: + case ActionSpecEnforcementOption.IGNORE: + pass + case ActionSpecEnforcementOption.CLIP_TO_SPEC: + try: + + def clip_to_spec(a, s): + if isinstance(s, specs.BoundedArray): + return np.clip(a, s.minimum, s.maximum) + return a + + action = tree.map_structure( + clip_to_spec, + action, + self._action_space_adapter.action_spec(), + ) + except ValueError as e: + raise ValueError( + "Failed to clip action to spec. Action:" + f" {action} and spec: {self._action_space_adapter.action_spec()}" + ) from e + case ActionSpecEnforcementOption.WARNING: + + def _validate_without_raising(a, s): + dtype_ok = s.dtype == a.dtype + shape_ok = s.shape == a.shape + minimum_ok = True + maximum_ok = True + if isinstance(s, specs.BoundedArray): + minimum_ok = (s.minimum <= a).all() + maximum_ok = (a <= s.maximum).all() + return dtype_ok and shape_ok and minimum_ok and maximum_ok + + if not all( + tree.flatten( + tree.map_structure( + _validate_without_raising, + action, + self._action_space_adapter.action_spec(), + ) + ) + ): + logging.warning( + "Failed to validate action against spec. Action: %r and spec: %r", + action, + self._action_space_adapter.action_spec(), + ) + case ActionSpecEnforcementOption.RAISE_ERROR: + action = tree.map_structure( + lambda a, spec: spec.validate(a), action, self.action_spec() + ) + case _: + raise ValueError( + "Unknown action spec enforcement option:" + f" {self._action_spec_enforcement_option}" + ) + return action + + +def _read_only_zeros_like_spec(spec: specs.Array) -> np.ndarray: + """Returns a zero array matching the specified spec.""" + arr = np.zeros(shape=spec.shape, dtype=spec.dtype) + arr.flags.writeable = False + return arr diff --git a/src/experimental/reaf/core/features_observer.py b/src/experimental/reaf/core/features_observer.py new file mode 100644 index 00000000..cc87f0d3 --- /dev/null +++ b/src/experimental/reaf/core/features_observer.py @@ -0,0 +1,34 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Observe all the produced features and measurements.""" + +import abc +from collections.abc import Mapping + +from gdm_robotics.interfaces import types as gdmr_types + + +class FeaturesObserver(abc.ABC): + """Observe all the produced features and measurements.""" + + @property + @abc.abstractmethod + def name(self) -> str: + """Returns a unique string identifier for this object.""" + + @abc.abstractmethod + def observe_features( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> None: + """Observes all the features and measurements.""" diff --git a/src/experimental/reaf/core/features_producer.py b/src/experimental/reaf/core/features_producer.py new file mode 100644 index 00000000..8ce44e94 --- /dev/null +++ b/src/experimental/reaf/core/features_producer.py @@ -0,0 +1,56 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Produces additional features to be exposed by the task logic layer.""" + +import abc +from collections.abc import Mapping + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types + + +class FeaturesProducer(abc.ABC): + """Produces additional features to be exposed by the task logic layer.""" + + @property + @abc.abstractmethod + def name(self) -> str: + """Returns a unique string identifier for this object.""" + + @abc.abstractmethod + def produce_features( + self, required_features: Mapping[str, gdmr_types.ArrayType] + ) -> Mapping[str, gdmr_types.ArrayType]: + """Produces additional features for the environment. + + Args: + required_features: Measurements and features generated by previous + producers in the processing chain that are required by this processor, + i.e. with keys specified by `required_features_keys`. + + Returns additional features that will be added to the global measurements + and features dictionary. + """ + + @abc.abstractmethod + def produced_features_spec(self) -> Mapping[str, specs.Array]: + """Returns the spec of the features produced by this producer.""" + + @abc.abstractmethod + def required_features_keys(self) -> set[str]: + """Returns the keys that are required to produce the new features.""" + + def reset(self) -> None: + """Resets the internal state of the feature producer.""" + ... diff --git a/src/experimental/reaf/core/logger.py b/src/experimental/reaf/core/logger.py new file mode 100644 index 00000000..63bb5f33 --- /dev/null +++ b/src/experimental/reaf/core/logger.py @@ -0,0 +1,80 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Support logging inside the task logic layer.""" + +import abc +from collections.abc import Mapping + +from gdm_robotics.interfaces import types as gdmr_types + + +class Logger(abc.ABC): + """Support logging inside the task logic layer. + + Lifecycle + For each environment step, these member functions are called in this order: + 1. `record_measurements` is called with raw measurements from the sensors. + 2. `record_features` is called with features derived from the measurements. + 3. `record_commands_processing` is called for each + `CommandsProcessor.process_commands` invocation, tracking the + transformation of commands. + 4. `record_final_commands` is called once with the final commands sent to + the DACL. + + Notes: + An environment is first reset(). This triggers the first two steps above. + See reset_with_options in ./environment.py. + + After reset, step is called repeatedly. + 1. This first triggers steps 3 and 4 (See compute_final_commands in TLL + called from step in ./environment.py) + 2. Features are computed (see compute_all_features in TLL called from + step in ./environment.py), triggering steps 1 and 2. + """ + + @property + @abc.abstractmethod + def name(self) -> str: + """Unique string identifier for this object.""" + + def record_measurements( + self, measurements: Mapping[str, gdmr_types.ArrayType] + ) -> None: + """Called once with all the measurements from the DACL.""" + + def record_features( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> None: + """Called once with all the features computed in the Task Layer.""" + + def record_final_commands( + self, commands: Mapping[str, gdmr_types.ArrayType] + ) -> None: + """Called once with the final commands sent to the DACL.""" + + def record_commands_processing( + self, + name: str, + consumed_commands: Mapping[str, gdmr_types.ArrayType], + produced_commands: Mapping[str, gdmr_types.ArrayType], + ) -> None: + """Called once per call to `process_commands` for each CommandsProcessor. + + Args: + name: Name of the `CommandsProcessor`. + consumed_commands: The commands consumed by the current + `CommandsProcessor`. + produced_commands: The commands produced by the current + `CommandsProcessor`. + """ diff --git a/src/experimental/reaf/core/numpy_mock_assertions.py b/src/experimental/reaf/core/numpy_mock_assertions.py new file mode 100644 index 00000000..310a918e --- /dev/null +++ b/src/experimental/reaf/core/numpy_mock_assertions.py @@ -0,0 +1,98 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Testing functions for asserting on Mock objects with numpy structures.""" + +from collections.abc import Sequence +from unittest import mock +import numpy as np + + +def assert_called_once_with(mock_obj: mock.Mock, *args, **kwargs) -> None: + if mock_obj.call_count != 1: + raise AssertionError( + f"Expected exactly one call to {mock_obj}, got {mock_obj.call_count}" + ) + + assert_called_with(mock_obj, *args, **kwargs) + + +def assert_called_with(mock_obj: mock.Mock, *args, **kwargs) -> None: + """Asserts that the last call to mock_obj had the specified arguments.""" + if mock_obj.call_args is None: + raise AssertionError( + f"Mock object {mock_obj} not called. Expected one call." + ) + call_args, call_kwargs = mock_obj.call_args + np.testing.assert_equal(call_args, args) + np.testing.assert_equal(call_kwargs, kwargs) + + +def assert_has_calls( + mock_obj: mock.Mock, calls: Sequence[mock._Call], any_order: bool = False +) -> None: + """Asserts that mock_obj has been called with the specified calls.""" + mock_calls = mock_obj.mock_calls + + # Check that there are at least enough calls. + if mock_obj.call_count < len(calls): + raise AssertionError( + f"Expected at least {len(calls)} calls to {mock_obj}, got" + f" {mock_obj.call_count}" + ) + + def _calls_are_equal(actual: mock._Call, expected: mock._Call) -> bool: + _, actual_args, actual_kwargs = actual + _, expected_args, expected_kwargs = expected + # Quickest way to transform the assertion into a comparator. + try: + np.testing.assert_equal(actual_args, expected_args) + np.testing.assert_equal(actual_kwargs, expected_kwargs) + return True + except AssertionError: + return False + + if any_order: + # We just check for the calls to be contained. + for expected_call in calls: + for actual_call in mock_calls: + if _calls_are_equal(actual_call, expected_call): + break + raise AssertionError( + f"Expected call {expected_call} not found in mock calls {mock_calls}." + ) + return + + # We need to check in order, but first find the first call. + starting_index = -1 + first_expected_call = calls[0] + for index, actual_call in enumerate(mock_calls): + if _calls_are_equal(actual_call, first_expected_call): + starting_index = index + break + if starting_index == -1: + raise AssertionError(f"Calls {calls} not found in mock calls {mock_calls}.") + + non_matching_calls = [] + + # We have the first element. Now we need to compare element wise. + for index, expected_call in enumerate(calls): + actual_call = mock_calls[starting_index + index] + if not _calls_are_equal(actual_call, expected_call): + non_matching_calls.append((index, expected_call, actual_call)) + + if non_matching_calls: + raise AssertionError( + f"Calls {calls} do not match mock calls {mock_calls}. Mismatch (index," + f" expected, actual): {non_matching_calls}" + ) diff --git a/src/experimental/reaf/core/observation_space_adapter.py b/src/experimental/reaf/core/observation_space_adapter.py new file mode 100644 index 00000000..ca4a8d86 --- /dev/null +++ b/src/experimental/reaf/core/observation_space_adapter.py @@ -0,0 +1,42 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Adapts REAF features into observations exposed by the environment.""" + +import abc +from collections.abc import Mapping +from gdm_robotics.interfaces import types as gdmr_types +import tree + + +class ObservationSpaceAdapter(abc.ABC): + """Adapts REAF features into observations exposed by the environment. + + Implementations of this interface are responsible for converting the features + generated by the REAF task layer logic (i.e. dictionary of tensors) into the + more generic `observation` structure exposed by the environment. + """ + + @abc.abstractmethod + def observations_from_features( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> tree.Structure[gdmr_types.ArrayType]: + """Converts the REAF features into the environment observations.""" + + @abc.abstractmethod + def observation_spec(self) -> gdmr_types.ObservationSpec: + """Returns the observation spec.""" + + @abc.abstractmethod + def task_features_keys(self) -> set[str]: + """Returns the task features keys that will be converted by this adapter.""" diff --git a/src/experimental/reaf/core/pass_through_action_space_adapter.py b/src/experimental/reaf/core/pass_through_action_space_adapter.py new file mode 100644 index 00000000..16b28a81 --- /dev/null +++ b/src/experimental/reaf/core/pass_through_action_space_adapter.py @@ -0,0 +1,55 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Adapter that passes the commands spec through.""" + +from collections.abc import Mapping +from gdm_robotics.interfaces import types as gdmr_types +from reaf.core import action_space_adapter + + +class PassThroughActionSpaceAdapter(action_space_adapter.ActionSpaceAdapter): + """Adapter that passes the commands spec through. + + NB the resulting environment will expose a dictionary as the action spec. + """ + + def __init__(self, commands_spec: Mapping[str, gdmr_types.AnyArraySpec]): + self._commands_spec = commands_spec + + def commands_from_environment_action( + self, environment_action: gdmr_types.ActionType + ) -> Mapping[str, gdmr_types.ArrayType]: + """Returns commands accepted by REAF. + + commands_from_environment_action usually accepts a gdmr_types.ActionType but + since this adapter passes the same action as the commands, it needs to be a + dict type in order to pass it through as a dict. + + Args: + environment_action: The environment action(s) to pass as REAF commands. + """ + if not isinstance(environment_action, dict): + raise ValueError( + 'environment_action must be a dict, but got: ' + f'{type(environment_action)}.' + ) + return environment_action + + def action_spec(self) -> gdmr_types.ActionSpec: + """Returns the action spec exposed by the environment.""" + return self._commands_spec + + def task_commands_keys(self) -> set[str]: + """Returns the keys for the commands exposed to the task layer.""" + return set(self._commands_spec.keys()) diff --git a/src/experimental/reaf/core/reward_provider.py b/src/experimental/reaf/core/reward_provider.py new file mode 100644 index 00000000..3a8ec655 --- /dev/null +++ b/src/experimental/reaf/core/reward_provider.py @@ -0,0 +1,292 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Computes the reward.""" + +import abc +from collections.abc import Mapping +import operator +from typing import Callable, TypeAlias, TypeVar, Union + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types +import numpy as np +import tree + + +RewardValue: TypeAlias = tree.Structure[gdmr_types.ArrayType] +RewardSpec: TypeAlias = tree.Structure[specs.Array] + + +class _RewardProvider(abc.ABC): + """Computes the reward. + + Defines the interface for a reward provider. + + Important: Users should not inherit from this class directly. Instead, use the + RewardProvider class later in this file. + """ + + @abc.abstractmethod + def name(self) -> str: + """Returns a unique string identifier for this object.""" + + @abc.abstractmethod + def compute_reward( + self, required_features: Mapping[str, gdmr_types.ArrayType] + ) -> RewardValue: + """Computes the reward. + + Args: + required_features: Measurements and features computed by the task logic + that are required by this provider, i.e. that have keys specified by + `required_features_keys`. + + Returns the computed reward. + """ + + @abc.abstractmethod + def reward_spec(self) -> RewardSpec: + """Returns the spec of the reward.""" + + @abc.abstractmethod + def required_features_keys(self) -> set[str]: + """Returns the feature keys that are required to compute the reward.""" + + def reset(self) -> None: + """Resets the internal state of the reward provider.""" + ... + + +RewardProviderOrValue: TypeAlias = Union['RewardProvider', RewardValue] + + +S = TypeVar('S') +T = TypeVar('T') +UnaryOperator: TypeAlias = Callable[[S], S] +BinaryOperator: TypeAlias = Callable[[S | T, S | T], S | T] + + +class RewardProvider(_RewardProvider): + """Computes the reward. + + Important: Users should inherit from this class and implement the abstract + methods defined in the interface _RewardProvider. + """ + + def __add__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.add, self, other) + + def __radd__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.add, other, self) + + def __sub__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.sub, self, other) + + def __rsub__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.sub, other, self) + + def __mul__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.mul, self, other) + + def __rmul__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.mul, other, self) + + def __truediv__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.truediv, self, other) + + def __rtruediv__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.truediv, other, self) + + def __floordiv__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.floordiv, self, other) + + def __rfloordiv__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.floordiv, other, self) + + def __pow__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.pow, self, other) + + def __rpow__(self, other: RewardProviderOrValue): + return BinaryOperationRewardProvider(operator.pow, other, self) + + def __getitem__(self, index: slice): + return GetItemOperationRewardProvider(self, index) + + def __neg__(self): + return UnaryOperationRewardProvider(operator.neg, self) + + +class ConstantRewardProvider(RewardProvider): + """A RewardProvider that always returns the same reward.""" + + def __init__(self, reward: RewardValue): + super().__init__() + self._reward = reward + + def name(self) -> str: + return str(self._reward) + + def compute_reward( + self, required_features: Mapping[str, gdmr_types.ArrayType] + ) -> RewardValue: + return self._reward + + def reward_spec(self) -> RewardSpec: + return tree.map_structure( + lambda v: specs.Array(v.shape, v.dtype), self._reward + ) + + def required_features_keys(self) -> set[str]: + return set() + + +class BinaryOperationRewardProvider(RewardProvider): + """Applies a binary operator to the result of two reward providers.""" + + def __init__( + self, + op: BinaryOperator, + first_reward_provider: RewardProviderOrValue, + second_reward_provider: RewardProviderOrValue, + ): + super().__init__() + if not isinstance(first_reward_provider, RewardProvider): + first_reward_provider = ConstantRewardProvider(first_reward_provider) + if not isinstance(second_reward_provider, RewardProvider): + second_reward_provider = ConstantRewardProvider(second_reward_provider) + first_spec = first_reward_provider.reward_spec() + second_spec = second_reward_provider.reward_spec() + tree.assert_same_structure(first_spec, second_spec) + assert all( + tree.flatten( + tree.map_structure( + lambda s1, s2: s1.shape == s2.shape and s1.dtype == s2.dtype, + first_spec, + second_spec, + ) + ) + ) + self._op = op + self._first_reward_provider = first_reward_provider + self._second_reward_provider = second_reward_provider + self._reward_spec = first_reward_provider.reward_spec() + self._first_required_features_keys = ( + first_reward_provider.required_features_keys() + ) + self._second_required_features_keys = ( + second_reward_provider.required_features_keys() + ) + + def name(self) -> str: + op_name = getattr(self._op, '__name__', str(self._op)) + return ( + f'{op_name}({self._first_reward_provider.name()},' + f' {self._second_reward_provider.name()})' + ) + + def compute_reward( + self, required_features: Mapping[str, gdmr_types.ArrayType] + ) -> RewardValue: + first_required_features = { + k: v + for k, v in required_features.items() + if k in self._first_required_features_keys + } + second_required_features = { + k: v + for k, v in required_features.items() + if k in self._second_required_features_keys + } + return tree.map_structure( + self._op, + self._first_reward_provider.compute_reward(first_required_features), + self._second_reward_provider.compute_reward(second_required_features), + ) + + def reward_spec(self) -> RewardSpec: + return self._reward_spec + + def required_features_keys(self) -> set[str]: + return ( + self._first_required_features_keys | self._second_required_features_keys + ) + + def reset(self) -> None: + self._first_reward_provider.reset() + self._second_reward_provider.reset() + + +class GetItemOperationRewardProvider(RewardProvider): + """Extracts a slice from the result of a reward provider.""" + + def __init__(self, reward_provider: RewardProviderOrValue, index: slice): + super().__init__() + if not isinstance(reward_provider, RewardProvider): + reward_provider = ConstantRewardProvider(reward_provider) + self._reward_provider = reward_provider + self._index = index + + def name(self) -> str: + return f'{self._reward_provider.name}[{self._index}]' + + def compute_reward( + self, required_features: Mapping[str, gdmr_types.ArrayType] + ) -> RewardValue: + return tree.map_structure( + lambda v: v[self._index], + self._reward_provider.compute_reward(required_features), + ) + + def reward_spec(self) -> RewardSpec: + return tree.map_structure( + lambda s: specs.Array(np.empty(s.shape)[self._index].shape, s.dtype), + self._reward_provider.reward_spec(), + ) + + def required_features_keys(self) -> set[str]: + return self._reward_provider.required_features_keys() + + def reset(self) -> None: + self._reward_provider.reset() + + +class UnaryOperationRewardProvider(RewardProvider): + """Applies a unary operator to the result of a reward provider.""" + + def __init__(self, op: UnaryOperator, reward_provider: RewardProviderOrValue): + super().__init__() + if not isinstance(reward_provider, RewardProvider): + reward_provider = ConstantRewardProvider(reward_provider) + self._op = op + self._reward_provider = reward_provider + + def name(self) -> str: + op_name = getattr(self._op, '__name__', str(self._op)) + return f'{op_name}({self._reward_provider.name()})' + + def compute_reward( + self, required_features: Mapping[str, gdmr_types.ArrayType] + ) -> RewardValue: + return tree.map_structure( + self._op, self._reward_provider.compute_reward(required_features) + ) + + def reward_spec(self) -> RewardSpec: + return self._reward_provider.reward_spec() + + def required_features_keys(self) -> set[str]: + return self._reward_provider.required_features_keys() + + def reset(self) -> None: + self._reward_provider.reset() diff --git a/src/experimental/reaf/core/substep_commands_processor.py b/src/experimental/reaf/core/substep_commands_processor.py new file mode 100644 index 00000000..b63f4388 --- /dev/null +++ b/src/experimental/reaf/core/substep_commands_processor.py @@ -0,0 +1,104 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Protocol for substep commands manipulation in REAF-sim.""" + +from collections.abc import Mapping +import typing + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types + + +class SubstepCommandsProcessor(typing.Protocol): + """Processes substep commands, propagating them through a pipeline. + + This processor manipulates substep commands, acting as a node in a pipeline. + It consumes substep commands, performs operations, and produces updated + substep commands for the next stage in the processing chain. + + The processing pipeline starts with commands provided to the SimulationDevice + and progresses towards the substep commands consumed by the individual + entities. Each processor consumes a subset of substep commands and produces + new, potentially transformed, substep commands. The order of operations is + crucial. + + Example Pipeline (conceptual): + + Simulation Device commands --> Processor (1) --> Processor (2) --> Entities + + Specs are propagated starting from the bottom: + 1) In this example assume that the set of entities expect "p3/c1", "p3/c2" and + "p3/c3". + 2) Processor (2) returns ("p3/c1", "p3/c2") from "p2/c1". This means + that the global substep commands spec exposed at this level is "p2/c1" and + the unprocessed "p3/c3". + 3) Processor (1) returns "p2/c1" from ("p1/c1", "p1/c2"). By applying + the same transformation rule, we can obtain the final spec exposed + by the SimulationDevice: "p1/c1", "p1/c2" and "p3/c3". + + ------------------------------------ + | SimulationDevice | + ------------------------------------ + + "p1/c1" "p1/c2" "p3/c3" + | | | + ----------------- | + | P1 | | + ----------------- | + | "p2/c1" | + ----------------- | + | P2 | | + ----------------- | + | "p3/c1" | "p3/c2" | + | | | + ------------------------------------ + | Entities | + ------------------------------------ + """ + + @property + def name(self) -> str: + """Returns a unique string identifier for this object.""" + + def reset(self) -> None: + """Resets the internal state of this processor.""" + + def produced_substep_commands_keys(self) -> set[str]: + """Keys of the substep commands produced by this processor.""" + + def consumed_substep_commands_spec( + self, + ) -> Mapping[str, specs.Array]: + """Spec of the substep commands consumed by this processor.""" + + def process_substep_commands( + self, + model: typing.Any, + data: typing.Any, + consumed_substep_commands: Mapping[str, gdmr_types.ArrayType], + ) -> Mapping[str, gdmr_types.ArrayType]: + """Processes the substep commands and returns a new modified version of it. + + Args: + model: the simulation model. + data: the simulation data. + consumed_substep_commands: the substep commands up in the processing chain + that are required by this processor, i.e. with keys specified by + `consumed_substep_commands_spec`. + + Returns the new substep commands. Note that the (key, value) pairs in + `consumed_substep_commands` are removed from the running substep commands + dictionary. If users want to keep some of the elements it is their + responsibility to retain them in the output dictionary. + """ diff --git a/src/experimental/reaf/core/substep_measurements_processor.py b/src/experimental/reaf/core/substep_measurements_processor.py new file mode 100644 index 00000000..15dc81df --- /dev/null +++ b/src/experimental/reaf/core/substep_measurements_processor.py @@ -0,0 +1,103 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Protocol for substep measurements manipulation in REAF-sim.""" + +from collections.abc import Mapping +import typing + +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types + + +class SubstepMeasurementsProcessor(typing.Protocol): + """Processes substep measurements, propagating them through a pipeline. + + This processor manipulates substep measurements, acting as a node in a + pipeline. It consumes substep measurements, performs operations, and produces + updated substep measurements for the next stage in the processing chain. + + The processing pipeline starts with substep measurements produced by Entities + and progresses towards the measurements exposed by the SimulationDevice. Each + processor consumes a subset of substep measurements and produces new, + potentially transformed, substep measurements. The order of operations is + crucial. + + Example Pipeline (conceptual): + + Entities --> Processor (1) --> Processor (2) -> Simulation Device Measurements + + Specs are propagated starting from the bottom: + 1) In this example assume that the set of entities produce "p1/c1", "p1/c2" + and "p1/c3". + 2) Processor (1) returns "p2/c1" from ("p1/c1", "p1/c2"). + 3) Processor (2) returns ("p3/c1", "p3/c2") from "p2/c1". + + This resulting spec exposed by the SimulationDevice: "p3/c1", "p3/c2" + and "p1/c3". + + ------------------------------------ + | SimulationDevice | + ------------------------------------ + + "p3/c1" "p3/c2" "p1/c3" + | | | + ----------------- | + | P2 | | + ----------------- | + | "p2/c1" | + ----------------- | + | P1 | | + ----------------- | + | "p1/c1" | "p1/c2" | + | | | + ------------------------------------ + | Entities | + ------------------------------------ + """ + + @property + def name(self) -> str: + """Returns a unique string identifier for this object.""" + + def reset(self): + """Resets the internal state of this processor.""" + + def produced_substep_measurements_spec( + self, + ) -> Mapping[str, specs.Array]: + """Spec of the substep measurements consumed by this processor.""" + + def consumed_substep_measurements_keys(self) -> set[str]: + """Keys of the substep measurements consumed by this processor.""" + + def process_substep_measurements( + self, + model: typing.Any, + data: typing.Any, + consumed_substep_measurements: Mapping[str, gdmr_types.ArrayType], + ) -> Mapping[str, gdmr_types.ArrayType]: + """Processes the substep measurements and returns a new modified version of it. + + Args: + model: the simulation model. + data: the simulation data. + consumed_substep_measurements: the substep measurements up in the + processing chain that are required by this processor, i.e. with keys + specified by `consumed_substep_measurements_spec`. + + Returns the new substep measurements. Note that the (key, value) pairs in + `consumed_substep_measurements` are removed from the running substep + measurements dictionary. If users want to keep some of the elements it is + their responsibility to retain them in the output dictionary. + """ diff --git a/src/experimental/reaf/core/task_logic_layer.py b/src/experimental/reaf/core/task_logic_layer.py new file mode 100644 index 00000000..51b25639 --- /dev/null +++ b/src/experimental/reaf/core/task_logic_layer.py @@ -0,0 +1,342 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Task logic layer for the Robotics Environment Authoring Framework.""" + +from collections.abc import Mapping, Sequence +import itertools +from typing import Protocol + +from absl import logging +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types +from reaf.core import commands_processor as reaf_commands_processor +from reaf.core import default_discount_provider +from reaf.core import discount_provider as reaf_discount_provider +from reaf.core import features_observer as reaf_features_observers +from reaf.core import features_producer as reaf_features_producer +from reaf.core import logger as reaf_logger +from reaf.core import reward_provider as reaf_reward_provider +from reaf.core import termination_checker as reaf_termination_checker +from reaf.core import zero_reward_provider +import tree + + +class _ResettableObject(Protocol): + """Protocol for an object that can be reset.""" + + def reset(self) -> None: + ... + + +class TaskLogicLayer: + """Task logic layer for the Robotics Environment Authoring Framework.""" + + def __init__( + self, + *, + commands_processors: Sequence[reaf_commands_processor.CommandsProcessor], + features_producers: Sequence[reaf_features_producer.FeaturesProducer], + termination_checkers: Sequence[ + reaf_termination_checker.TerminationChecker + ], + reward_provider: reaf_reward_provider.RewardProvider | None = None, + discount_provider: reaf_discount_provider.DiscountProvider | None = None, + features_observers: Sequence[ + reaf_features_observers.FeaturesObserver + ] = (), + loggers: Sequence[reaf_logger.Logger] = (), + ): + """Initializes the task logic layer. + + Args: + commands_processors: `CommandsProcessor`s that modify the commands before + being sent down to the DACL. They are called sequentially, starting from + the commands supplied by the policy and ending with the commands that + will be sent to the DACL. + features_producers: `FeaturesProducer`s that generate new features. + Measurements collected by the DACL and features produced by these + `FeaturesProducer`s are then merged into the final feature set that is + provided to the `reward_provider`, `termination_checkers`, + `discount_provider`, `features_observers`, and `loggers`. + termination_checkers: `TerminationChecker`s that check the episode + termination based on the final feature set. + reward_provider: `RewardProvider` that computes a reward based on the + final feature set. If None, the ZeroRewardProvider is used and the + reward is set to 0. + discount_provider: `DiscountProvider` that compute a discount based on the + final feature set and final termination state. If None, the + DefaultDiscountProvider is used returning 0 for termination and 1 for + truncation and non-termination. + features_observers: `FeaturesObserver`s that get a view over the final + feature set. + loggers: `Logger`s for logging measurements, features, and commands in the + task layer. + """ + self._commands_processors = commands_processors + self._features_producers = features_producers + self._reward_provider = ( + reward_provider + if reward_provider + else zero_reward_provider.ZeroRewardProvider() + ) + self._termination_checkers = termination_checkers + self._discount_provider = ( + discount_provider + if discount_provider + else default_discount_provider.DefaultDiscountProvider() + ) + self._features_observers = features_observers + self._loggers = list(loggers) + + # We make a set of all resettable objects so that these objects only get + # their resets called once. This is important for e.g. when having a single + # object that derives from two interfaces. + self._resettable_objects: list[_ResettableObject] = [] + unique_ids = set() + for resettable_object in itertools.chain( + self._commands_processors, + self._features_producers, + self._termination_checkers, + [self._reward_provider], + [self._discount_provider], + ): + resettable_object_id = id(resettable_object) + if resettable_object_id not in unique_ids: + unique_ids.add(resettable_object_id) + self._resettable_objects.append(resettable_object) + + def validate_spec( + self, + *, + dacl_commands_spec: Mapping[str, gdmr_types.AnyArraySpec], + dacl_measurements_spec: Mapping[str, specs.Array], + ) -> None: + """Checks that the specs have consistent keys.""" + logging.vlog(3, "Validate features processing") + self._validate_features_spec(dacl_measurements_spec) + self._validate_commands_spec(dacl_commands_spec) + + def features_spec( + self, + dacl_measurements_spec: Mapping[str, specs.Array], + ) -> Mapping[str, specs.Array]: + """Returns the features spec as exposed by the task layer.""" + spec = dict(dacl_measurements_spec) + for features_producer in self._features_producers: + spec.update(features_producer.produced_features_spec()) + + return spec + + def commands_spec( + self, dacl_commands_spec: Mapping[str, gdmr_types.AnyArraySpec] + ) -> Mapping[str, gdmr_types.AnyArraySpec]: + """Returns the commands spec exposed by the task layer.""" + # Each processor consumes commands (as described by its + # `consumed_commands_spec`) and outputs a potentially different set of + # commands (as described by its `produced_commands_keys`). + # Starting with the DACL command spec, we iterate in reverse order (i.e. in + # the direction DACL -> Policy) through every processor to remove the + # `produced_commands_keys` from the spec, and add their + # `consumed_commands_spec` to the spec. + spec: Mapping[str, gdmr_types.AnyArraySpec] = dict(dacl_commands_spec) + for processor in reversed(self._commands_processors): + processor_produced_keys = processor.produced_commands_keys() + spec = { + key: value + for key, value in spec.items() + if key not in processor_produced_keys + } + spec.update(processor.consumed_commands_spec()) + return spec + + def reward_spec(self) -> tree.Structure[specs.Array]: + return self._reward_provider.reward_spec() + + def discount_spec(self) -> tree.Structure[specs.Array]: + return self._discount_provider.discount_spec() + + def perform_reset(self) -> None: + """Reset the internal state of the task logic layer.""" + for resettable_object in self._resettable_objects: + resettable_object.reset() + + def compute_all_features( + self, measurements: Mapping[str, gdmr_types.ArrayType] + ) -> Mapping[str, gdmr_types.ArrayType]: + """Computes all the task logic features given the current measurements.""" + for logger in self._loggers: + logger.record_measurements(measurements) + + # Produce all the features. + current_features = dict(measurements) + for feature_producer in self._features_producers: + required_features = { + key: current_features[key] + for key in feature_producer.required_features_keys() + } + current_features.update( + feature_producer.produce_features(required_features) + ) + + # Observe the features. + for feature_observer in self._features_observers: + feature_observer.observe_features(current_features) + + # Log the resulting features. + for logger in self._loggers: + logger.record_features(current_features) + return current_features + + def compute_final_commands( + self, + policy_commands: Mapping[str, gdmr_types.ArrayType], + ) -> Mapping[str, gdmr_types.ArrayType]: + """Processes the policy commands and returns the final processed commands.""" + current_commands = dict(policy_commands) + for processor in self._commands_processors: + # Get commands to be consumed by the processor and remove the commands + # from the current_commands.. They correspond to the + # `consumed_command_spec`. + consumed_commands = { + key: current_commands.pop(key) + for key in processor.consumed_commands_spec().keys() + } + produced_commands = processor.process_commands(consumed_commands) + current_commands.update(produced_commands) + + # Log the modification. + for logger in self._loggers: + logger.record_commands_processing( + processor.name, consumed_commands, produced_commands + ) + + for logger in self._loggers: + logger.record_final_commands(current_commands) + return current_commands + + def compute_reward( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> tree.Structure[gdmr_types.ArrayType]: + """Computes the reward given the features.""" + return self._reward_provider.compute_reward({ + key: features[key] + for key in self._reward_provider.required_features_keys() + }) + + def check_for_termination( + self, features: Mapping[str, gdmr_types.ArrayType] + ) -> reaf_termination_checker.TerminationResult: + """Checks for termination.""" + current_state = reaf_termination_checker.TerminationResult.DO_NOT_TERMINATE + for termination_checker in self._termination_checkers: + current_state = reaf_termination_checker.TerminationResult.combine( + current_state, + termination_checker.check_termination({ + key: features[key] + for key in termination_checker.required_features_keys() + }), + ) + return current_state + + def compute_discount( + self, + features: Mapping[str, gdmr_types.ArrayType], + termination_state: reaf_termination_checker.TerminationResult, + ) -> tree.Structure[gdmr_types.ArrayType]: + """Computes the discount given the features and termination state.""" + return self._discount_provider.compute_discount( + { + key: features[key] + for key in self._discount_provider.required_features_keys() + }, + termination_state, + ) + + def add_logger(self, logger: reaf_logger.Logger) -> None: + self._loggers.append(logger) + + def remove_logger(self, logger: reaf_logger.Logger) -> None: + self._loggers.remove(logger) + + def _validate_features_spec( + self, dacl_measurements_spec: Mapping[str, specs.Array] + ) -> None: + """Validates the features spec.""" + # Check measurements/features path. + current_key_set = set(dacl_measurements_spec.keys()) + logging.vlog(4, "DACL measurements keys: %s", current_key_set) + + for producer in self._features_producers: + logging.vlog( + 4, + "Producer %s requires %s.", + producer.name, + producer.required_features_keys(), + ) + # Check required features are available. + if not producer.required_features_keys().issubset(current_key_set): + raise ValueError( + "Failed to validate feature specs for feature producer" + f" {producer.name}. Missing keys:" + f" {producer.required_features_keys() - current_key_set}" + ) + # Check that there are not duplicates in the output. + if not current_key_set.isdisjoint( + producer.produced_features_spec().keys() + ): + raise ValueError( + "Failed to validate feature specs for feature producer" + f" {producer.name}. Duplicate keys:" + f" {current_key_set & producer.produced_features_spec().keys()}" + ) + # Now extend the spec. + logging.vlog( + 4, + "Update available keys (from producer %s) with %s.", + producer.name, + producer.produced_features_spec().keys(), + ) + current_key_set.update(producer.produced_features_spec().keys()) + logging.vlog(4, "Available features keys %s.", current_key_set) + + def _validate_commands_spec( + self, dacl_commands_spec: Mapping[str, gdmr_types.AnyArraySpec] + ) -> None: + """Validates the commands spec.""" + # Check commands. Starting from the DACL command specs we propagate up in + # the chain. + logging.vlog(3, "Validate commands processing from DACL to Policy.") + current_key_set = set(dacl_commands_spec.keys()) + logging.vlog(4, "DACL commands keys: %s", current_key_set) + + for processor in reversed(self._commands_processors): + produced_command_keys = processor.produced_commands_keys() + + logging.vlog( + 4, + "Processor %s: specs (accepted keys) %s. Exposes %s.", + processor.name, + processor.consumed_commands_spec().keys(), + produced_command_keys, + ) + if not produced_command_keys.issubset(current_key_set): + raise ValueError( + "Failed to validate commands specs for commands processor" + f" {processor.name}. Missing (consumable) keys:" + f" {produced_command_keys - current_key_set}" + ) + # Remove the produced keys and add the consumed commands specs (as the + # processor is mutable). + current_key_set = current_key_set - produced_command_keys + current_key_set.update(processor.consumed_commands_spec().keys()) diff --git a/src/experimental/reaf/core/termination_checker.py b/src/experimental/reaf/core/termination_checker.py new file mode 100644 index 00000000..754c250d --- /dev/null +++ b/src/experimental/reaf/core/termination_checker.py @@ -0,0 +1,94 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Checks if the episode should terminate.""" + +import abc +from collections.abc import Mapping +import enum +from typing import Self + +from gdm_robotics.interfaces import types as gdmr_types + + +class TerminationResult(enum.IntFlag): + """The result of an episode termination check. + + The TerminationResult refers to the possibility for an episode to terminate. + For more details on the concept of termination we refer the readers to + https://github.com/google-deepmind/dm_env/blob/master/docs/index.md#environment-api-and-semantics. + + Note that this enum does not refer to the possible causes of termination but + only how the termination impacts the learning process. + + The result can be one of the following options: + - DO_NOT_TERMINATE: The episode should not terminate. + - TRUNCATE: The epsisode should terminate. Truncation implies a non-failure + final state. Usually this is associated with a non-zero discount. + - TERMINATE: The episode should terminate as the environment is in some + final state. Usually this is associated with a zero discount for e.g. + finite-horizon RL. + """ + + DO_NOT_TERMINATE = 0 + TRUNCATE = 2**0 + TERMINATE = 2**1 + + def is_terminated(self) -> bool: + return self == TerminationResult.TERMINATE + + def is_truncated(self) -> bool: + return self == TerminationResult.TRUNCATE + + def combine(self, other: Self) -> Self: + # TERMINATE has precedence over TRUNCATE, which in turn has precedence over + # DO_NOT_TERMINATE. Given the definitions above, this can be implemented as + # a maximum operator. To also enable tracing with JAX, we implement this in + # a branchless manner using bitwise operations that preserve the type. + # Note that JAX will trace TerminationResult values as ints. + # Approach: + # - self ^ (self ^ other) == other + # - (-1 * (self < other)) will be bitmask of all 1s iff self < other. + # - AND with (self ^ other) will result in either update or no-op bitmask. + return self ^ ((self ^ other) & (-1 * (self < other))) + + +class TerminationChecker(abc.ABC): + """Checks if the episode should terminate.""" + + @abc.abstractmethod + def name(self) -> str: + """Returns a unique string identifier for this object.""" + + @abc.abstractmethod + def check_termination( + self, required_features: Mapping[str, gdmr_types.ArrayType] + ) -> TerminationResult: + """Checks if the episode should terminate. + + Args: + required_features: Measurements and features computed by the task logic + that are required by this checker, i.e. that have keys specified by + `required_features_keys`. + + Returns if the episode should terminate (and if so, what kind of + termination). + """ + + @abc.abstractmethod + def required_features_keys(self) -> set[str]: + """Returns the feature keys that are required to check the termination.""" + + def reset(self) -> None: + """Resets the internal state of the termination checker.""" + ... diff --git a/src/experimental/reaf/core/trigger.py b/src/experimental/reaf/core/trigger.py new file mode 100644 index 00000000..20901873 --- /dev/null +++ b/src/experimental/reaf/core/trigger.py @@ -0,0 +1,29 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Defines an event-based waiting behaviour.""" + +import abc + + +class Trigger(abc.ABC): + """Defines an event-based waiting behaviour.""" + + @property + @abc.abstractmethod + def name(self) -> str: + """Returns the name of the trigger.""" + + @abc.abstractmethod + def wait_for_event(self) -> None: + """Blocks until the next event.""" diff --git a/src/experimental/reaf/core/zero_reward_provider.py b/src/experimental/reaf/core/zero_reward_provider.py new file mode 100644 index 00000000..c5d2267a --- /dev/null +++ b/src/experimental/reaf/core/zero_reward_provider.py @@ -0,0 +1,48 @@ +# Copyright 2025 Google LLC +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +"""Reward provider which provides a zero reward.""" + +from collections.abc import Mapping +from dm_env import specs +from gdm_robotics.interfaces import types as gdmr_types +import numpy as np +from reaf.core import reward_provider +import tree + + +class ZeroRewardProvider(reward_provider.RewardProvider): + """Reward provider which provides a zero reward.""" + + def __init__(self, name: str = 'zero_reward_provider'): + self._name = name + + def name(self) -> str: + return self._name + + def compute_reward( + self, required_features: Mapping[str, gdmr_types.ArrayType] + ) -> tree.Structure[gdmr_types.ArrayType]: + """Returns a zero reward.""" + return np.zeros(1) + + def reward_spec(self) -> tree.Structure[specs.Array]: + """Returns the spec for a constant zero reward.""" + return specs.Array(shape=(1,), dtype=float) + + def required_features_keys(self) -> set[str]: + """Returns empty set. + + There are no feature keys that are required to compute the reward. + """ + return set() From 7ae07d81579bd999ea14706cbae04d1cb3105b02 Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Mon, 13 Apr 2026 08:21:13 -0700 Subject: [PATCH 049/251] URDF parsing no longer strips filepaths by default. This default forces users to use the `spec.assets` dictionary or to do other filepath gymnastics when loading URDF. Instead we encourage users to just modify file paths directly if necessary. PiperOrigin-RevId: 899009853 Change-Id: I4bb84606a95b65be79ccc77ccbe78062d6de8773 --- doc/XMLreference.rst | 2 +- doc/changelog.rst | 10 ++++++++++ src/xml/xml.cc | 1 - 3 files changed, 11 insertions(+), 2 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index a5486dec..8cf40b72 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -739,7 +739,7 @@ has any effect. The settings here are global and apply to the entire model. .. _compiler-strippath: -:at:`strippath`: :at-val:`[false, true], "false" for MJCF, "true" for URDF` +:at:`strippath`: :at-val:`[false, true], "false"` When this attribute is "true", the parser will remove any path information in file names specified in the model. This is useful for loading models created on a different system using a different directory structure. diff --git a/doc/changelog.rst b/doc/changelog.rst index 82fae188..e8dbb0ed 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -52,6 +52,16 @@ General - The :ref:`mjtWarning` enum value ``mjWARN_VGEOMFULL`` is removed. Exhaustion of visual geoms is now handled internally by the :ref:`mjvScene`. + - URDF parsing no longer hardcodes :ref:`strippath` to "true". The setting is now respected and + the default is "false". Setting this is attribute is now the responsibility of the user. + + **Migration:** Set :ref:`strippath` to "true" in MJCF or programmatically using + + .. code-block:: python + + spec = mujoco.MjSpec.from_file("path/to/model.urdf") + spec.compiler.strippath = True + Bug fixes ^^^^^^^^^ diff --git a/src/xml/xml.cc b/src/xml/xml.cc index 639fc587..654d0a57 100644 --- a/src/xml/xml.cc +++ b/src/xml/xml.cc @@ -296,7 +296,6 @@ mjSpec* SpecFromXML(std::string_view xml, std::string_view dir, // set reasonable default for parsing a URDF // this is separate from the Parser to allow multiple URDFs to be loaded. - spec->strippath = true; spec->compiler.fusestatic = true; spec->compiler.discardvisual = true; From 0c337799bd9f293485bb13d409ff8335d722a2c2 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 13 Apr 2026 09:37:51 -0700 Subject: [PATCH 050/251] Implement midpoint integrator for free bodies. PiperOrigin-RevId: 899043541 Change-Id: I0bb38f6ad94e189b45ab16777a04ad6fefc6adf7 --- doc/XMLreference.rst | 23 +- doc/changelog.rst | 9 +- doc/computation/index.rst | 56 +++- mjx/mujoco/mjx/_src/forward_test.py | 2 + src/engine/engine_forward.c | 353 +++++++++++++++++++++++++- src/engine/engine_forward.h | 9 +- src/engine/engine_util_solve.c | 31 +++ src/engine/engine_util_solve.h | 4 + test/engine/engine_derivative_test.cc | 80 +++++- test/engine/engine_forward_test.cc | 305 ++++++++++++++++++++++ test/engine/engine_inverse_test.cc | 17 +- test/engine/engine_sleep_test.cc | 53 ++++ 12 files changed, 913 insertions(+), 29 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 8cf40b72..a8ebff10 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -657,13 +657,22 @@ from its default. .. _option-flag-invdiscrete: :at:`invdiscrete`: :at-val:`[disable, enable], "disable"` - This flag enables discrete-time inverse dynamics with :ref:`mj_inverse` for all - :ref:`integrators` other than ``RK4``. Recall from the - :ref:`numerical integration` section that the one-step integrators (``Euler``, ``implicit`` and - ``implicitfast``), modify the mass matrix :math:`M \rightarrow M-hD`. This implies that finite-differenced - accelerations :math:`(v_{t+h} - v_t)/h` will not correspond to the continuous-time acceleration ``mjData.qacc``. - When this flag is enabled, :ref:`mj_inverse` will interpret ``qacc`` as having been computed from the difference of - two sequential velocities, and undo the above modification. + This dual-purpose flag enables discrete-time inverse dynamics and disables :ref:`midpoint integration`. + + Enable discrete-time inverse dynamics + This flag **enables** discrete-time inverse dynamics with :ref:`mj_inverse` for all + :ref:`integrators` other than ``RK4``. Recall from the :ref:`numerical + integration` section that the one-step integrators (``Euler``, ``implicit`` and ``implicitfast``), + modify the mass matrix :math:`M \rightarrow M-hD`. This implies that finite-differenced accelerations + :math:`(v_{t+h} - v_t)/h` will not correspond to the continuous-time acceleration ``mjData.qacc``. When this flag + is enabled, :ref:`mj_inverse` will interpret ``qacc`` as having been computed from the difference of two sequential + velocities, and undo the above modification. + + Disable midpoint integration + Additionally and relatedly, this flag **disables** :ref:`midpoint integration` for free bodies, which + would otherwise break the linear relationship between finite-differenced velocities and forces assumed by discrete + inverse dynamics. Note that disabling midpoint integration might be useful for debugging or for other reasons, + regardless or whether inverse dynamics are used. .. _option-flag-multiccd: diff --git a/doc/changelog.rst b/doc/changelog.rst index e8dbb0ed..d664df1c 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -27,8 +27,13 @@ General (``jnt_stiffness``, ``dof_damping``, etc.) continue to hold the linear coefficient and are unchanged. The polynomial order is defined by the new constant :ref:`mjNPOLY`. A future breaking C-API change may unify the linear and higher-order coefficients into a single array. - -- Introduced :ref:`mjpEncoder`, the counterpart to :ref:`mjpDecoder` for encoding of :ref:`mjSpec` and :ref:`mjModel` into :ref:`mjResource`. +- Added :ref:`midpoint integration` for standalone free bodies in ``implicit`` and ``implicitfast`` + :ref:`integrators`. This applies the implicit midpoint rule to the rotational dynamics of free bodies + with no children, exactly conserving kinetic energy and angular momentum in the absence of external torques. The + :ref:`invdiscrete` flag now also disables midpoint integration, providing an opt-out + mechanism. +- Introduced :ref:`mjpEncoder`, the counterpart to :ref:`mjpDecoder` for encoding of :ref:`mjSpec` and :ref:`mjModel` + into :ref:`mjResource`. - Added :ref:`mj_encode`, :ref:`mjp_registerEncoder`, :ref:`mjp_defaultEncoder`, and :ref:`mjp_findEncoder`. diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 67b1623d..74421918 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -573,6 +573,49 @@ Solving for :math:`v_{t+h}`, we obtain the implicit-in-velocity update \widehat{M} &\equiv M-h D \end{aligned} +.. _geMidpoint: + +Midpoint integration for free bodies + The implicit-in-velocity update :eq:`eq_implicit_update` treats the acceleration as a function of velocity and + linearizes. While effective for damping-like forces, it is sub-optimal for rotational dynamics, where + Coriolis and gyroscopic forces are *quadratic* in angular velocity. For this case, a better approach is to directly + discretize the rotational equations of motion using the *midpoint method*. + + Consider a rigid body rotating in its principal-axis frame with angular velocity + :math:`\omega \in \mathbb{R}^3` and diagonal inertia tensor :math:`I = \text{diag}(I_1, I_2, I_3)`. The rotational + dynamics are given by `Euler's rotation equation + `__: + + .. math:: + I \dot{\omega} + \omega \times I\omega = \tau + + where :math:`\tau` is the external torque in the principal-axis frame. + Evaluating the velocities at the midpoint, :math:`\omega_\text{mid} = (\omega_t + \omega_{t+h})/2`, gives: + + .. math:: + \frac{2}{h} I (\omega_\text{mid} - \omega_t) + \omega_\text{mid} \times I \omega_\text{mid} = \tau + + This is a system of 3 nonlinear equations in 3 unknowns :math:`\omega_\text{mid}`, solved at each timestep using + Newton's method with a backtracking line search. After solving, the new velocity is recovered as + :math:`\omega_{t+h} = 2\omega_\text{mid} - \omega_t`. + + **Properties.** The midpoint method preserves all `quadratic first integrals + `__ of the ODE. For Euler's equations, these are the + kinetic energy :math:`H = \frac{1}{2}\omega^T I\omega` and the squared angular momentum + :math:`C = \frac{1}{2}|I\omega|^2`, both conserved exactly in the absence of external torque. Since :math:`C` is the + Casimir function of the `Lie-Poisson `__ structure, the midpoint + method is a symmetric (time-reversible) and second-order accurate *Poisson integrator*. + + **Eligibility.** Midpoint integration is only applied to free bodies with no child bodies. + + **Performance.** While the midpoint method carries computational overhead, we've found it to be + negligible compared to the rest of the pipeline, on the order of 1% in the worst case. + + **Disabling.** Because midpoint integration solves a nonlinear equation for the next velocity, it breaks the linear + relationship between finite-differenced velocities and forces assumed by discrete inverse dynamics. Therefore, + setting the :ref:`invdiscrete` flag disables midpoint integration, and also provides a + general opt-out mechanism for this integrator. + .. _geIntegrators: Integrators @@ -610,6 +653,9 @@ Fast implicit-in-velocity (``implicitfast``) derivatives are also the main source of asymmetry of :math:`D`, by dropping them and symmetrizing, we can use the faster :math:`L^TL` rather than :math:`LU` decomposition. +Both ``implicit`` and ``implicitfast`` apply :ref:`midpoint integration` to eligible free bodies, +providing exact energy conservation for spinning objects at negligible additional cost. + 4th-order Runge-Kutta (``RK4``) One advantage of our continuous-time formulation is that we can use higher order integrators such as Runge-Kutta or multistep methods. MuJoCo implements the fixed-step `4th-order Runge-Kutta method @@ -641,11 +687,11 @@ Fast implicit-in-velocity (``implicitfast``) The ``implicitfast`` integrator has similar computational cost to ``Euler``, yet provides increased stability, and is therefore a strict improvement. It is the recommended integrator for most models. **implicit**: - The benefit over ``implicitfast`` is the implicit integration of Coriolis and centripetal forces, including - gyroscopic forces. The most common case where integrating such forces implicitly leads to noticeable improvement is - when free objects with asymmetric inertia are spinning quickly. `gyroscopic.xml <../_static/gyroscopic.xml>`__ - shows an ellipsoid rolling on an inclined plane which quickly diverges with ``implicitfast`` but is stable with - ``implicit``. + The benefit over ``implicitfast`` is the implicit integration of Coriolis and centripetal forces for *coupled* + rotational systems such as multi-link pendula. Both ``implicitfast`` and ``implicit`` apply :ref:`midpoint + integration` to eligible free bodies with no children, for example + `gyroscopic.xml <../_static/gyroscopic.xml>`__ shows an ellipsoid rolling on an + inclined plane; both ``implicitfast`` and ``implicit`` handle this case well, while ``Euler`` quickly diverges. **RK4**: This integrator is best for systems which are energy conserving, or almost energy-conserving. `pendulum.xml <../_static/pendulum.xml>`__ shows a complicated pendulum mechanism which diverges quickly using ``Euler`` or diff --git a/mjx/mujoco/mjx/_src/forward_test.py b/mjx/mujoco/mjx/_src/forward_test.py index ba200b72..751aeb70 100644 --- a/mjx/mujoco/mjx/_src/forward_test.py +++ b/mjx/mujoco/mjx/_src/forward_test.py @@ -78,6 +78,8 @@ class ForwardTest(absltest.TestCase): # implicitfast m.opt.integrator = mujoco.mjtIntegrator.mjINT_IMPLICITFAST + # TODO(team): remove this override when the mjx feature matches mujoco + m.opt.enableflags |= mujoco.mjtEnableBit.mjENBL_INVDISCRETE dx = jax.jit(mjx.implicit)(mx, mjx.put_data(m, d)) mujoco.mj_implicit(m, d) _assert_attr_eq(d, dx, 'qpos') diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index e8e08f00..b0d15b16 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -38,6 +38,7 @@ #include "engine/engine_sleep.h" #include "engine/engine_solver.h" #include "engine/engine_support.h" +#include "engine/engine_inline.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" @@ -1043,7 +1044,10 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) { //-------------------------- state advancement and integration ------------------------------------ -// advance state and time given activation derivatives, acceleration, and optional velocity +// advance state and time +// act_dot: activation derivatives +// qacc: acceleration used to update d->qvel (d->qvel += h*qacc) +// qvel: optional velocity used for position integration; if NULL, use d->qvel static void mj_advance(const mjModel* m, mjData* d, const mjtNum* act_dot, const mjtNum* qacc, const mjtNum* qvel) { int nu = m->nu, nsensor = m->nsensor; @@ -1546,6 +1550,293 @@ static void flexInterp_solve(const mjModel* m, mjData* d, const FlexInterpContex } +// return 1 if free joint is eligible for midpoint quaternion integration: +// standalone 6-DOF tree with no children +static int midpoint_eligible(const mjModel* m, int jnt) { + if (m->jnt_type[jnt] == mjJNT_FREE) { + int body = m->jnt_bodyid[jnt]; + int treeid = m->dof_treeid[m->jnt_dofadr[jnt]]; + return m->tree_dofnum[treeid] == 6 && + m->body_subtreemass[body] == m->body_mass[body]; + } + + return 0; +} + + +// return 1 if the body's CoM is at the joint origin (no translational-rotational coupling) +static int midpoint_aligned(const mjModel* m, int jnt) { + int body = m->jnt_bodyid[jnt]; + return m->body_ipos[3*body+0] == 0 && + m->body_ipos[3*body+1] == 0 && + m->body_ipos[3*body+2] == 0; +} + + +// implicit midpoint integration for 3D rotation of a single body +// +// solves the Euler rigid body equation in the inertial frame: +// I * (w_new - w) / h = tau - w_mid x (I*w_mid) +// where w_mid = (w + w_new) / 2 is solved via Newton iteration. +// +// inputs: +// inertia: principal moments of inertia (3) +// w: initial angular velocity in principal axes frame (3) +// tau: external torque in principal axes frame (3) +// h: timestep +// outputs: +// w_mid: midpoint angular velocity in principal axes frame (3) +// returns: number of Newton iterations +static int midpointNewton(const mjtNum inertia[3], const mjtNum w[3], + const mjtNum tau[3], mjtNum h, mjtNum w_mid[3]) { + // precompute constants + mjtNum i2h = 2.0 / h; + mjtNum dI[3] = {inertia[2]-inertia[1], inertia[0]-inertia[2], inertia[1]-inertia[0]}; + mjtNum i2h_I[3] = {i2h*inertia[0], i2h*inertia[1], i2h*inertia[2]}; + + // initialize solution to previous angular velocity + mji_copy3(w_mid, w); + + // Newton iteration + int niter; + for (niter=0; niter < 100; niter++) { + // compute Coriolis term + mjtNum Iw[3] = {inertia[0]*w_mid[0], inertia[1]*w_mid[1], inertia[2]*w_mid[2]}; + mjtNum coriolis[3]; + mji_cross(coriolis, w_mid, Iw); + + // residual: f = i2h*I*(w_mid - w) + w_mid x (I*w_mid) - tau + mjtNum f[3]; + for (int k=0; k < 3; k++) { + f[k] = i2h_I[k]*(w_mid[k] - w[k]) + coriolis[k] - tau[k]; + } + + // check convergence + mjtNum fnorm = mju_norm3(f); +#ifndef mjUSESINGLE + mjtNum tol = 1e-13; +#else + mjtNum tol = 1e-6f; +#endif + if (fnorm < tol*(1 + i2h*mju_norm3(Iw))) break; + + // Jacobian: J = i2h*diag(I) + d(w x Iw)/dw + mjtNum J[9]; + J[0] = i2h_I[0]; J[1] = w_mid[2]*dI[0]; J[2] = w_mid[1]*dI[0]; + J[3] = w_mid[2]*dI[1]; J[4] = i2h_I[1]; J[5] = w_mid[0]*dI[1]; + J[6] = w_mid[1]*dI[2]; J[7] = w_mid[0]*dI[2]; J[8] = i2h_I[2]; + + // solve J*delta = -f for search direction delta + mjtNum neg_f[3] = {-f[0], -f[1], -f[2]}; + mjtNum delta[3]; + mju_solve3(delta, J, neg_f); + + // backtracking line search + mjtNum step = 1.0; + for (int ls=0; ls < 20; ls++) { + // candidate step + mjtNum w_try[3], Iw_try[3]; + for (int k=0; k < 3; k++) { + w_try[k] = w_mid[k] + step*delta[k]; + Iw_try[k] = inertia[k]*w_try[k]; + } + mjtNum coriolis_try[3]; + mji_cross(coriolis_try, w_try, Iw_try); + + // residual at candidate step + mjtNum f_try[3]; + for (int k=0; k < 3; k++) { + f_try[k] = i2h_I[k]*(w_try[k] - w[k]) + coriolis_try[k] - tau[k]; + } + + // accept step if residual decreased, otherwise backtrack + if (mju_norm3(f_try) < fnorm) { + mji_copy3(w_mid, w_try); + break; + } + step *= 0.5; + } + } + + return niter; +} + + +// implicit midpoint integration for one free body +// +// solves the Euler rigid body equation in the inertial frame: +// I * dw/dt = tau - w x (I*w) +// using the implicit midpoint rule: +// I * (w_new - w_old) / h = tau_mid - w_mid x (I*w_mid) +// where w_mid = (w_old + w_new) / 2 is solved via Newton iteration. +// +// inputs: +// mass: body mass +// inertia: principal moments of inertia +// ipos: CoM offset from joint origin, in body frame +// iquat: inertial quaternion (body_iquat) +// xquat: body orientation in world frame +// qvel_old: current velocity (lin in world : rot in body) +// qfrc: external force (lin in world : rot in body) +// gravity: gravitational acceleration in world frame (NULL: no gravity) +// h: timestep +// outputs: +// qvel_new: next velocity (lin in world : rot in body) +int mj_midpoint(mjtNum mass, const mjtNum inertia[3], const mjtNum ipos[3], + const mjtNum iquat[4], const mjtNum xquat[4], const mjtNum qvel_old[6], + const mjtNum qfrc[6], const mjtNum gravity[3], mjtNum h, + mjtNum qvel_new[6]) { + // transform angular velocity and torque to inertial frame + mjtNum iquat_neg[4], w[3], tau[3]; + mji_negQuat(iquat_neg, iquat); + mji_rotVecQuat(w, qvel_old+3, iquat_neg); // qvel+3 (angular) is in body frame + mji_rotVecQuat(tau, qfrc+3, iquat_neg); // qfrc+3 (angular) is in body frame + + // check for translational-rotational coupling + int aligned = (ipos[0] == 0 && ipos[1] == 0 && ipos[2] == 0); + + mjtNum r_com[3]; // joint-to-CoM vector in inertial frame + mjtNum tau_com[3]; // torque at CoM in inertial frame + mjtNum rot_x2i[4]; // quaternion rotation from world to inertial frame + mjtNum force[3]; // external force in inertial frame + + // compute torque at CoM in inertial frame + if (aligned) { + mji_copy3(tau_com, tau); + } else { + // rotation from world to inertial frame + mjtNum xquat_neg[4]; + mji_negQuat(xquat_neg, xquat); + mji_mulQuat(rot_x2i, iquat_neg, xquat_neg); + + // force and CoM offset in inertial frame + mji_rotVecQuat(force, qfrc, rot_x2i); + mji_rotVecQuat(r_com, ipos, iquat_neg); + + // torque at CoM in inertial frame + mjtNum rxf[3]; + mji_cross(rxf, r_com, force); + mji_sub3(tau_com, tau, rxf); + } + + // solve for midpoint angular velocity + mjtNum w_mid[3]; + int niter = midpointNewton(inertia, w, tau_com, h, w_mid); + + // next and mid angular velocities in inertial frame, rotate both to body frame + mjtNum w_new[3], w_new_body[3], w_mid_body[3]; + for (int k=0; k < 3; k++) { + w_new[k] = 2.0*w_mid[k] - w[k]; + } + mji_rotVecQuat(w_new_body, w_new, iquat); + mji_rotVecQuat(w_mid_body, w_mid, iquat); + mji_copy3(qvel_new+3, w_new_body); + + // === aligned: return + if (aligned) { + return niter; + } + + // === non-aligned: solve for translational velocity + + // rotate linear velocity to inertial frame + mjtNum v[3]; + mji_rotVecQuat(v, qvel_old, rot_x2i); + + // current CoM velocities (rot, lin) in inertial frame + mjtNum wxr[3]; + mji_cross(wxr, w, r_com); + mjtNum vcom[3]; + mji_add3(vcom, v, wxr); + + // right-hand side for midpoint CoM velocity + mjtNum i2h = 2.0 / h; + mjtNum b[3]; + for (int k=0; k < 3; k++) { + b[k] = force[k]/mass + i2h*vcom[k]; + } + + // add gravity, if any + if (gravity) { + mjtNum g_inertial[3]; + mji_rotVecQuat(g_inertial, gravity, rot_x2i); + mji_addTo3(b, g_inertial); + } + + // analytic solution for (i2h*Id + [w_mid]x) * vcom_mid = b + mjtNum wnorm2 = mju_dot3(w_mid, w_mid); + mjtNum denom = i2h*i2h + wnorm2; + mjtNum w_dot_b = mju_dot3(w_mid, b); + mjtNum w_cross_b[3]; + mji_cross(w_cross_b, w_mid, b); + mjtNum vcom_mid[3]; + for (int k=0; k < 3; k++) { + vcom_mid[k] = (i2h*b[k] + (w_dot_b/i2h)*w_mid[k] - w_cross_b[k]) / denom; + } + + // recover midpoint and new joint velocity in inertial frame + mjtNum wxr_mid[3]; + mji_cross(wxr_mid, w_mid, r_com); + mjtNum v_mid[3], v_new[3]; + for (int k=0; k < 3; k++) { + v_mid[k] = vcom_mid[k] - wxr_mid[k]; + v_new[k] = 2.0*v_mid[k] - v[k]; + } + + // estimate new orientation + mjtNum axis[3]; + mji_copy3(axis, w_mid_body); + mjtNum wnorm = mju_normalize3(axis); + mjtNum qrot_new[4]; + mji_axisAngle2Quat(qrot_new, axis, h*wnorm); + mjtNum xquat_new[4]; + mji_mulQuat(xquat_new, xquat, qrot_new); + + // v_new (linear): inertial → body → world using new orientation + mjtNum v_body[3]; + mji_rotVecQuat(v_body, v_new, iquat); + mji_rotVecQuat(qvel_new, v_body, xquat_new); + + return niter; +} + + +// compute next velocities via midpoint integration for eligible free bodies +// qfrc: total force (qfrc_smooth + qfrc_constraint) +// free_jntid: list of eligible free joint IDs +// nfree: number of eligible free joints +// qvel_old: output array for old velocities (6 per joint) +// qvel_new: output array for new velocities (6 per joint) +// dofadr: output array for DOF addresses (1 per joint) +static void midpoint(const mjModel* m, const mjData* d, const mjtNum* qfrc, + const int* free_jntid, int nfree, + mjtNum* qvel_old, mjtNum* qvel_new, int* dofadr) { + for (int i=0; i < nfree; i++) { + int j = free_jntid[i]; + int body = m->jnt_bodyid[j]; + + // save DOF address + int adr = m->jnt_dofadr[j]; + dofadr[i] = adr; + + // save old (current) velocity, needed after mj_advance (which overwrites qvel) + mju_copy(qvel_old+6*i, d->qvel+adr, 6); + + // compute external force = qfrc + qfrc_bias (undo bias subtraction) + mjtNum qfrc_total[6]; + mju_add(qfrc_total, qfrc+adr, d->qfrc_bias+adr, 6); + + // gravity handled inside mj_midpoint (accelerating frame of reference) + const mjtNum* gravity = mjDISABLED(mjDSBL_GRAVITY) ? NULL : m->opt.gravity; + + // midpoint solver for free joint j + mj_midpoint(m->body_mass[body], m->body_inertia+3*body, m->body_ipos+3*body, + m->body_iquat+4*body, d->xquat+4*body, + d->qvel+adr, qfrc_total, gravity, m->opt.timestep, qvel_new+6*i); + } +} + + // fully implicit in velocity, possibly skipping factorization void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { TM_START; @@ -1640,8 +1931,64 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { flexInterp_solve(m, d, &flex, qacc, qfrc, nv); } - // advance state and time - mj_advance(m, d, d->act_dot, qacc, NULL); + // count and list joints of free bodies eligible for midpoint integration + int nfree = 0; + int* free_jntid = NULL; + if (!mjENABLED(mjENBL_INVDISCRETE)) { + free_jntid = mjSTACKALLOC(d, m->njnt, int); + for (int j=0; j < m->njnt; j++) { + // add to list if eligible and awake + if (midpoint_eligible(m, j) && d->tree_awake[m->dof_treeid[m->jnt_dofadr[j]]]) { + free_jntid[nfree++] = j; + } + } + } + + // compute midpoint velocities (used to update positions) + int* dofadr = NULL; + mjtNum* qvel_old = NULL; + mjtNum* qvel_new = NULL; + mjtNum* qvel_mid = NULL; + if (nfree) { + // allocate arrays, call midpoint solver for all eligible free joints + dofadr = mjSTACKALLOC(d, nfree, int); + qvel_new = mjSTACKALLOC(d, 6*nfree, mjtNum); + qvel_old = mjSTACKALLOC(d, 6*nfree, mjtNum); + midpoint(m, d, qfrc, free_jntid, nfree, qvel_old, qvel_new, dofadr); + + // build qvel_mid = d->qvel + h*qacc for all DOFs, then overwrite midpoint DOFs + qvel_mid = mjSTACKALLOC(d, m->nv, mjtNum); + mju_addScl(qvel_mid, d->qvel, qacc, m->opt.timestep, m->nv); + for (int i=0; i < nfree; i++) { + int adr = dofadr[i]; + int start = midpoint_aligned(m, free_jntid[i]) ? 3 : 0; + for (int k=start; k < 6; k++) { + qvel_mid[adr+k] = 0.5*(qvel_new[6*i+k] + qvel_old[6*i+k]); + } + } + } + + // advance state and time (use qvel_mid if allocated, NULL otherwise) + mj_advance(m, d, d->act_dot, qacc, qvel_mid); + + // overwrite midpoint DOFs with true next velocity and acceleration + if (nfree) { + mjtNum h_inv = 1.0 / m->opt.timestep; + for (int i=0; i < nfree; i++) { + // skip sleeping tree (may have been put to sleep during mj_advance) + int adr = dofadr[i]; + if (!d->tree_awake[m->dof_treeid[adr]]) { + continue; + } + + // overwrite 3 or 6 midpoint DOFs with true next velocity and acceleration + int start = midpoint_aligned(m, free_jntid[i]) ? 3 : 0; + for (int k=start; k < 6; k++) { + d->qvel[adr+k] = qvel_new[6*i+k]; + d->qacc[adr+k] = (qvel_new[6*i+k] - qvel_old[6*i+k]) * h_inv; + } + } + } mj_freeStack(d); diff --git a/src/engine/engine_forward.h b/src/engine/engine_forward.h index 1735ca6c..0a3fd5e0 100644 --- a/src/engine/engine_forward.h +++ b/src/engine/engine_forward.h @@ -18,6 +18,7 @@ #include #include #include +#include #ifdef __cplusplus extern "C" { @@ -46,7 +47,6 @@ MJAPI void mj_forward(const mjModel* m, mjData* d); MJAPI void mj_forwardSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor); - //-------------------------------- integrators ----------------------------------------------------- // Runge Kutta explicit order-N integrator @@ -64,6 +64,13 @@ MJAPI void mj_implicit(const mjModel *m, mjData *d); // fully implicit in velocity, possibly skipping factorization MJAPI void mj_implicitSkip(const mjModel *m, mjData *d, int skipfactor); +// implicit midpoint integration for 6 DOFs (translation + rotation) of a single body +// returns number of Newton iterations +MJAPI int mj_midpoint(mjtNum mass, const mjtNum inertia[3], const mjtNum ipos[3], + const mjtNum iquat[4], const mjtNum xquat[4], const mjtNum qvel[6], + const mjtNum qfrc[6], const mjtNum gravity[3], mjtNum h, + mjtNum qvel_new[6]); + //-------------------------------- solver components ----------------------------------------------- diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 426f214e..259f7c59 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -883,6 +883,37 @@ void mju_solveLUSparse(mjtNum* res, const mjtNum* LU, const mjtNum* vec, int n, } +//--------------------------- 3x3 linear solve ----------------------------------------------------- + +// solve 3x3 linear system A*x = b using Gaussian elimination +void mju_solve3(mjtNum x[3], const mjtNum A[9], const mjtNum b[3]) { + mjtNum M[3][4] = { + {A[0], A[1], A[2], b[0]}, + {A[3], A[4], A[5], b[1]}, + {A[6], A[7], A[8], b[2]} + }; + + for (int i=0; i<3; i++) { + mjtNum pivot = M[i][i]; + for (int j=i; j<4; j++) { + M[i][j] /= pivot; + } + + for (int k=0; k<3; k++) { + if (k != i) { + mjtNum factor = M[k][i]; + for (int j=i; j<4; j++) { + M[k][j] -= factor * M[i][j]; + } + } + } + } + x[0] = M[0][3]; + x[1] = M[1][3]; + x[2] = M[2][3]; +} + + //--------------------------- eigen decomposition -------------------------------------------------- // eigenvalue decomposition of symmetric 3x3 matrix diff --git a/src/engine/engine_util_solve.h b/src/engine/engine_util_solve.h index 523bc04a..a50ab4ee 100644 --- a/src/engine/engine_util_solve.h +++ b/src/engine/engine_util_solve.h @@ -17,6 +17,7 @@ #include #include +#include #ifdef __cplusplus extern "C" { @@ -103,6 +104,9 @@ void mju_solveLUSparse(mjtNum *res, const mjtNum *LU, const mjtNum* vec, int n, const int *rownnz, const int *rowadr, const int* diag, const int *colind, const int *index); +// solve 3x3 linear system A*x = b using Gaussian elimination +void mju_solve3(mjtNum x[3], const mjtNum A[9], const mjtNum b[3]); + // eigenvalue decomposition of symmetric 3x3 matrix MJAPI int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum mat[9]); diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 18ae65b3..e5978c9b 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -320,17 +320,19 @@ TEST_F(DerivativeTest, PassiveDvel) { mj_forward(model, data); // get analytic derivatives + mju_zero(data->qDeriv, model->nD); + mjd_passive_vel(model, data); mju_copy(qDerivAnalytic, data->qDeriv, nD); // clear qDeriv, get finite-difference derivatives mju_zero(data->qDeriv, nD); mju_zero(qDerivFD, nD); - mjtNum eps = MjTol(1e-6, 1e-3); + mjtNum eps = MjTol(1e-6, 1e-4); mjd_passive_velFD(model, data, eps); // expect FD and analytic derivatives to be similar to tol precision EXPECT_THAT(AsVector(data->qDeriv, nD), - Pointwise(MjNear(1e-4, 1e-3), AsVector(qDerivAnalytic, nD))); + Pointwise(MjNear(1e-6, 1e-4), AsVector(qDerivAnalytic, nD))); } mju_free(qDerivFD); @@ -1733,5 +1735,79 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) { mj_deleteModel(model); } +TEST_F(DerivativeTest, MidpointFluidAccuracy) { + const std::string xml_path = + GetTestDataFilePath(kTumblingThinObjectEllipsoidPath); + char error[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + + mjtNum dt_small = 1e-4; + mjtNum dt_large = m->opt.timestep; // 2e-3, the default + mjtNum duration = 0.5; + + mjData* d_ref = mj_makeData(m); + mjData* d_midpoint = mj_makeData(m); + mjData* d_nomidpoint = mj_makeData(m); + + // give initial angular velocity for tumbling + mj_resetData(m, d_ref); + mj_resetData(m, d_midpoint); + mj_resetData(m, d_nomidpoint); + d_ref->qvel[3] = 5; + d_ref->qvel[4] = 3; + d_ref->qvel[5] = 1; + d_midpoint->qvel[3] = 5; + d_midpoint->qvel[4] = 3; + d_midpoint->qvel[5] = 1; + d_nomidpoint->qvel[3] = 5; + d_nomidpoint->qvel[4] = 3; + d_nomidpoint->qvel[5] = 1; + + int nsteps_large = static_cast(duration / dt_large); + int substeps = static_cast(dt_large / dt_small); + + mjtNum error_midpoint = 0; + mjtNum error_nomidpoint = 0; + + for (int i = 0; i < nsteps_large; i++) { + // reference: RK4 at small timestep + m->opt.integrator = mjINT_RK4; + m->opt.timestep = dt_small; + m->opt.enableflags &= ~mjENBL_INVDISCRETE; + for (int j = 0; j < substeps; j++) { + mj_step(m, d_ref); + } + + // implicit with midpoint (default) + m->opt.integrator = mjINT_IMPLICIT; + m->opt.timestep = dt_large; + m->opt.enableflags &= ~mjENBL_INVDISCRETE; + mj_step(m, d_midpoint); + + // implicit without midpoint + m->opt.enableflags |= mjENBL_INVDISCRETE; + mj_step(m, d_nomidpoint); + + // accumulate position errors + for (int k = 0; k < 7; k++) { + mjtNum diff_mid = d_ref->qpos[k] - d_midpoint->qpos[k]; + mjtNum diff_nomid = d_ref->qpos[k] - d_nomidpoint->qpos[k]; + error_midpoint += diff_mid * diff_mid; + error_nomidpoint += diff_nomid * diff_nomid; + } + } + + // expect midpoint to be more accurate + EXPECT_LT(error_midpoint, error_nomidpoint) + << "implicit midpoint should be more accurate than implicit without " + << "midpoint for a free body with fluid forces"; + + mj_deleteData(d_nomidpoint); + mj_deleteData(d_midpoint); + mj_deleteData(d_ref); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index fce6cf68..4b0caf9c 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -17,6 +17,7 @@ #include "src/engine/engine_forward.h" #include "src/engine/engine_derivative.h" +#include #include #include #include @@ -478,6 +479,310 @@ TEST_F(ImplicitIntegratorTest, EnergyConservation) { mj_deleteModel(model); } +// Energy and angmom conservation for free body with implicitfast (IMR) +TEST_F(ImplicitIntegratorTest, ConservationMidpoint) { + // aligned: CoM at joint origin + static constexpr char xml1[] = R"( + + + + + + + + + + )"; + + // auto-aligned: CoM at joint origin + static constexpr char xml2[] = R"( + + + + + + + + + + )"; + + // non-aligned: CoM offset from joint origin + static constexpr char xml3[] = R"( + + + + + + + + + + )"; + int xml_idx = 1; + for (auto xml : {xml1, xml2, xml3}) { + SCOPED_TRACE(testing::Message() << "XML case " << xml_idx++); + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + + const int nstep = 500; + mjtNum energy_drift[2], angmom_drift[2]; // [0]=midpoint, [1]=rk4 + + for (int integrator : {mjINT_IMPLICITFAST, mjINT_RK4}) { + int idx = (integrator == mjINT_IMPLICITFAST) ? 0 : 1; + model->opt.integrator = integrator; + + // reset + mj_resetData(model, data); + data->qvel[3] = 1.0; + data->qvel[4] = 2.0; + data->qvel[5] = 3.0; + mj_forward(model, data); + mjtNum initial_energy = data->energy[1]; + mjtNum initial_angmom[3]; + mj_subtreeVel(model, data); + mju_copy3(initial_angmom, data->subtree_angmom); + + for (int i=0; i < nstep; i++) { + mj_step(model, data); + } + + energy_drift[idx] = fabs(data->energy[1] - initial_energy); + mj_subtreeVel(model, data); + mjtNum angmom_err[3]; + mju_sub3(angmom_err, data->subtree_angmom, initial_angmom); + angmom_drift[idx] = mju_norm3(angmom_err); + } + + // midpoint should conserve energy better than RK4 (double only) +#ifndef mjUSESINGLE + EXPECT_LT(energy_drift[0], energy_drift[1]); +#endif + + // both should conserve angular momentum well + EXPECT_LT(angmom_drift[0], MjTol(1e-3, 1e-2)); + EXPECT_LT(angmom_drift[1], MjTol(1e-3, 1e-2)); + + mj_deleteData(data); + mj_deleteModel(model); + } +} + +// verify second-order convergence of midpoint integration +TEST_F(ImplicitIntegratorTest, MidpointConvergenceOrder) { + // aligned: CoM at joint origin + static constexpr char xml1[] = R"( + + + + + + + + + + )"; + + // non-aligned: CoM offset from joint origin + static constexpr char xml2[] = R"( + + + + + + + + + + )"; + + int xml_idx = 1; + for (auto xml : {xml1, xml2}) { + SCOPED_TRACE(testing::Message() << "XML case " << xml_idx++); + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + + mjtNum T = 1.0; + mjtNum h_coarse = 0.02; + mjtNum quat_coarse[4], quat_fine[4], quat_ref[4]; + + auto run = [&](mjtNum h, mjtNum quat_out[4]) { + model->opt.timestep = h; + mjData* data = mj_makeData(model); + + data->qvel[3] = 1.0; + data->qvel[4] = 2.0; + data->qvel[5] = 3.0; + + int nstep = (int)(T / h + 0.5); + for (int i = 0; i < nstep; i++) { + mj_step(model, data); + } + + mju_copy4(quat_out, data->qpos + 3); + mj_deleteData(data); + }; + + run(h_coarse, quat_coarse); + run(h_coarse / 2, quat_fine); + run(h_coarse / 16, quat_ref); + + // quaternion distance: ||quat - quat_ref|| (handles sign ambiguity) + auto quat_dist = [](const mjtNum a[4], const mjtNum b[4]) -> mjtNum { + mjtNum pos = 0, neg = 0; + for (int i = 0; i < 4; i++) { + pos += (a[i] - b[i]) * (a[i] - b[i]); + neg += (a[i] + b[i]) * (a[i] + b[i]); + } + return mju_sqrt(mju_min(pos, neg)); + }; + + mjtNum err_coarse = quat_dist(quat_coarse, quat_ref); + mjtNum err_fine = quat_dist(quat_fine, quat_ref); + + // second-order: error ratio should be ~4 when halving timestep + mjtNum ratio = err_coarse / err_fine; + EXPECT_GT(ratio, 3.5); + EXPECT_LT(ratio, 4.5); + + mj_deleteModel(model); + } +} + +// verify that Newton iteration in mj_midpoint converges quickly (aligned case) +TEST_F(ImplicitIntegratorTest, MidpointNewtonConvergence) { + // inertia ratios: symmetric, mildly asymmetric, extremely asymmetric + mjtNum inertias[][3] = { + {1.0, 1.0, 1.0}, + {1.0, 2.0, 3.0}, + {0.01, 1.0, 100.0}, + {1.0, 1.0, 1000.0}, + }; + + mjtNum timesteps[] = {0.001, 0.01, 0.1}; + + mjtNum velocities[][3] = { + {1.0, 2.0, 3.0}, + {100.0, 0.0, 0.0}, + {10.0, 10.0, 10.0}, + {0.01, 0.01, 100.0}, + }; + + mjtNum q_identity[4] = {1, 0, 0, 0}; + mjtNum torques[][3] = { + {0, 0, 0}, + {10.0, 20.0, 30.0}, + {100.0, 0.0, 0.0}, + {0.0, 0.0, 100.0}, + }; + + int max_iter = 0; + int total_iter = 0; + int ncases = 0; + + for (auto& I : inertias) { + for (mjtNum h : timesteps) { + for (auto& w : velocities) { + for (auto& tau : torques) { + mjtNum vel[6] = {0, 0, 0, w[0], w[1], w[2]}; + mjtNum tau_ext[6] = {0, 0, 0, tau[0], tau[1], tau[2]}; + mjtNum v_new[6]; + mjtNum ipos[3] = {0, 0, 0}; + int niter = mj_midpoint(1.0, I, ipos, q_identity, q_identity, vel, + tau_ext, NULL, h, v_new); + EXPECT_LT(niter, 10) + << "Failed for I=(" << I[0] << "," << I[1] << "," << I[2] << ")" + << " h=" << h + << " w=(" << w[0] << "," << w[1] << "," << w[2] << ")" + << " tau=(" << tau[0] << "," << tau[1] << "," << tau[2] << ")"; + max_iter = std::max(max_iter, niter); + total_iter += niter; + ncases++; + } + } + } + } + + EXPECT_LE(max_iter, 4); + EXPECT_LT((mjtNum)total_iter / ncases, 2.0); +} + +// verify that Newton iteration in mj_midpoint converges quickly (non-aligned) +TEST_F(ImplicitIntegratorTest, MidpointFullNewtonConvergence) { + mjtNum masses[] = {0.1, 1.0, 10.0}; + + mjtNum inertias[][3] = { + {1.0, 1.0, 1.0}, + {1.0, 2.0, 3.0}, + {0.01, 1.0, 100.0}, + }; + + mjtNum offsets[][3] = { + {0.1, 0.0, 0.0}, + {0.05, 0.03, 0.02}, + {0.0, 0.0, 0.5}, + }; + + mjtNum timesteps[] = {0.001, 0.01, 0.1}; + + mjtNum velocities[][6] = { + {1.0, 0.0, 0.0, 1.0, 2.0, 3.0}, + {0.0, 0.0, 0.0, 10.0, 10.0, 10.0}, + {5.0, 5.0, 5.0, 0.01, 0.01, 100.0}, + }; + + mjtNum q_identity[4] = {1, 0, 0, 0}; + mjtNum forces[][6] = { + {0, 0, 0, 0, 0, 0}, + {10.0, 20.0, 30.0, 1.0, 2.0, 3.0}, + }; + + int max_iter = 0; + int total_iter = 0; + int ncases = 0; + + for (mjtNum mass : masses) { + for (auto& I : inertias) { + for (auto& r : offsets) { + for (mjtNum h : timesteps) { + for (auto& vel : velocities) { + for (auto& frc : forces) { + mjtNum v_new[6]; + int niter = mj_midpoint(mass, I, r, q_identity, q_identity, + vel, frc, NULL, h, v_new); + EXPECT_LT(niter, 10) + << "Failed for mass=" << mass + << " I=(" << I[0] << "," << I[1] << "," << I[2] << ")" + << " r=(" << r[0] << "," << r[1] << "," << r[2] << ")" + << " h=" << h; + max_iter = std::max(max_iter, niter); + total_iter += niter; + ncases++; + } + } + } + } + } + } + + EXPECT_LE(max_iter, 6); + EXPECT_LT((mjtNum)total_iter / ncases, 3.0); +} + TEST_F(ForwardTest, ControlClamping) { static constexpr char xml[] = R"( diff --git a/test/engine/engine_inverse_test.cc b/test/engine/engine_inverse_test.cc index 2bfc2d6d..988e73e6 100644 --- a/test/engine/engine_inverse_test.cc +++ b/test/engine/engine_inverse_test.cc @@ -73,9 +73,16 @@ TEST_F(InverseTest, DiscreteInverseMatch) { mjtNum* qvel_next = (mjtNum*)mju_malloc(nv * sizeof(mjtNum)); mjtNum* qacc_fd = (mjtNum*)mju_malloc(nv * sizeof(mjtNum)); - for (auto integrator : {mjINT_EULER, mjINT_IMPLICIT, mjINT_IMPLICITFAST}) { + for (auto integrator : {mjINT_EULER, mjINT_IMPLICIT}) { model->opt.integrator = integrator; for (bool invdiscrete : {false, true}) { + // set/unset mjENBL_INVDISCRETE flag (affects both forward and inverse) + if (invdiscrete) { + model->opt.enableflags |= mjENBL_INVDISCRETE; + } else { + model->opt.enableflags &= ~mjENBL_INVDISCRETE; + } + // simulate mj_resetData(model, data); for (int i = 0; i < kSteps; ++i) { @@ -98,17 +105,9 @@ TEST_F(InverseTest, DiscreteInverseMatch) { mj_forward(model, data); mju_copy(data->qacc, qacc_fd, nv); - // set/unset mjENBL_INVDISCRETE flag - if (invdiscrete) { - model->opt.enableflags |= mjENBL_INVDISCRETE; - } else { - model->opt.enableflags &= ~mjENBL_INVDISCRETE; - } - // call built-in testing function mj_compareFwdInv(model, data); - // depending on mjENBL_INVDISCRETE flag, expect mismatch to be small/large if (invdiscrete) { mjtNum epsilon = MjTol(1e-9, 0.05); EXPECT_LT(data->solver_fwdinv[0], epsilon); diff --git a/test/engine/engine_sleep_test.cc b/test/engine/engine_sleep_test.cc index 88067eff..1245dc83 100644 --- a/test/engine/engine_sleep_test.cc +++ b/test/engine/engine_sleep_test.cc @@ -533,6 +533,59 @@ TEST_F(SleepTest, Equality) { mj_deleteModel(m); } +// Test that the midpoint integrator doesn't break the sleep qvel=0 invariant. +// A standalone free body (eligible for midpoint) with high viscosity should +// eventually go to sleep, and after sleeping, qvel/qacc must be exactly zero. +TEST_F(SleepTest, MidpointSleepZeroVelocity) { + static constexpr char xml[] = R"( + + + + + + + + + + )"; + + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + + // give initial velocity (both translational and angular) + d->qvel[0] = 0.5; + d->qvel[1] = 0.5; + d->qvel[2] = 0.5; + d->qvel[3] = 1.0; + d->qvel[4] = 2.0; + d->qvel[5] = 3.0; + + // step until body goes to sleep + for (int step = 0; step < 1000; step++) { + mj_step(m, d); + if (d->ntree_awake == 0) break; + } + + // body should have gone to sleep + ASSERT_EQ(d->ntree_awake, 0) << "body did not go to sleep"; + + // qvel and qacc must be exactly zero for sleeping body + for (int i = 0; i < 6; i++) { + EXPECT_EQ(d->qvel[i], 0.0) << "qvel[" << i << "] not zero after sleep"; + EXPECT_EQ(d->qacc[i], 0.0) << "qacc[" << i << "] not zero after sleep"; + } + + mj_deleteData(d); + mj_deleteModel(m); +} + static const char* const kInitIslandFailModel = "engine/testdata/sleep/init_island_fail.xml"; From 412cee20596353aededb991a0896ee295a6ca25f Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 13 Apr 2026 12:47:17 -0700 Subject: [PATCH 051/251] Add Jdot correction for connect and weld constaints Measured reduction in constraint violations before/after this change: | Model | Correction ON (Avg Viol) | Correction OFF (Avg Viol) | Reduction | | :--- | :--- | :--- | :--- | | `jdotv_connect_2d.xml` | 3.959e-4 | 1.699e-3 | **76.7%** | | `jdotv_connect_3d.xml` | 1.399e-3 | 5.493e-3 | **74.5%** | | `jdotv_weld_3d.xml` | 9.472e-3 | 1.148e-2 | **17.5%** | PiperOrigin-RevId: 899137525 Change-Id: Ic3e33764ebd64239bab916289c23d80c3da0b51b --- doc/changelog.rst | 6 +- doc/computation/index.rst | 11 +- mjx/mujoco/mjx/_src/constraint_test.py | 3 + mjx/mujoco/mjx/_src/forward_test.py | 6 + src/engine/engine_core_constraint.c | 273 ++++++++++++++++-- src/engine/engine_core_constraint.h | 4 + src/engine/engine_util_sparse.h | 5 + test/engine/engine_core_constraint_test.cc | 110 +++++++ test/engine/engine_solver_test.cc | 17 +- .../core_constraint/jdotv_connect_2d.xml | 29 ++ .../core_constraint/jdotv_connect_3d.xml | 29 ++ .../core_constraint/jdotv_weld_3d.xml | 29 ++ wasm/tests/bindings_test.ts | 2 +- 13 files changed, 481 insertions(+), 43 deletions(-) create mode 100644 test/engine/testdata/core_constraint/jdotv_connect_2d.xml create mode 100644 test/engine/testdata/core_constraint/jdotv_connect_3d.xml create mode 100644 test/engine/testdata/core_constraint/jdotv_weld_3d.xml diff --git a/doc/changelog.rst b/doc/changelog.rst index d664df1c..a0fe7a7f 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -29,9 +29,13 @@ General may unify the linear and higher-order coefficients into a single array. - Added :ref:`midpoint integration` for standalone free bodies in ``implicit`` and ``implicitfast`` :ref:`integrators`. This applies the implicit midpoint rule to the rotational dynamics of free bodies - with no children, exactly conserving kinetic energy and angular momentum in the absence of external torques. The + with no children, conserving kinetic energy to machine precision in the absence of external torques. The :ref:`invdiscrete` flag now also disables midpoint integration, providing an opt-out mechanism. +- Added the centripetal/Coriolis acceleration term :math:`\dot{J}v` to the constraint solver bias for + :ref:`connect` and :ref:`weld` equality constaints. This significantly improves the + stability of constrained mechanisms like four-bar linkages. See :ref:`Dual problem` for details. + - Introduced :ref:`mjpEncoder`, the counterpart to :ref:`mjpDecoder` for encoding of :ref:`mjSpec` and :ref:`mjModel` into :ref:`mjResource`. diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 74421918..01b06886 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -1296,10 +1296,13 @@ when This key identity is essentially Newton's second law projected in constraint space. It is derived by moving the term :math:`c` in the equations of motion :eq:`eq:motion` to the right hand side, multiplying by :math:`J M^{-1}` from the -left, adding :math:`\dot{J} v` to both sides, and substituting the above definitions of :math:`A, \au, \ac`. In terms of -implementation, we do not actually compute the acceleration term :math:`\dot{J} v`. This is because our optimization -problems depend on differences of constraint-space accelerations, and so this term would cancel out even if we were to -compute it. +left, adding :math:`\dot{J} v` to both sides, and substituting the above definitions of :math:`A, \au, \ac`. Computing +:math:`\dot{J} v` requires differentiating the constraint Jacobian with respect to time, which is nontrivial. +Although this term cancels in the identity :eq:`eq:identity` and so does not affect the forward-inverse comparison, its +omission in the forward dynamics introduces a velocity-dependent bias for any constraint whose Jacobian varies with +configuration. We compute this term for equality constraints (connect and weld) where Jacobian differentiation +is tractable. For contacts, the term remains omitted due to the complexity of differentiating the contact frame through +the collision pipeline. Note that the quadratic term in the inverse problem is weighted by :math:`R` instead of :math:`A+R`. This is the key structural insight: the :math:`A` matrix cancels entirely, leaving only :math:`R` in the quadratic term. Two diff --git a/mjx/mujoco/mjx/_src/constraint_test.py b/mjx/mujoco/mjx/_src/constraint_test.py index f94873ff..ab17745f 100644 --- a/mjx/mujoco/mjx/_src/constraint_test.py +++ b/mjx/mujoco/mjx/_src/constraint_test.py @@ -60,6 +60,9 @@ class ConstraintTest(parameterized.TestCase): # sample a mix of active/inactive constraints at different timesteps for key in range(3): mujoco.mj_resetDataKeyframe(m, d, key) + # scale down velocities to minimize Jdotv effect (not in MJX) + # TODO(team): remove this change when mjx supports this feature + d.qvel[:] *= 1e-2 if rand_eq_active: d.eq_active[:] = np.random.randint(0, 2, size=m.neq) mujoco.mj_forward(m, d) diff --git a/mjx/mujoco/mjx/_src/forward_test.py b/mjx/mujoco/mjx/_src/forward_test.py index 751aeb70..abeacea0 100644 --- a/mjx/mujoco/mjx/_src/forward_test.py +++ b/mjx/mujoco/mjx/_src/forward_test.py @@ -49,6 +49,9 @@ class ForwardTest(absltest.TestCase): d.xfrc_applied[0, 2] = 0.1 # torque d.xfrc_applied[1, 4] = 0.3 # linear force mujoco.mj_step(m, d, 20) # get some dynamics going + # scale down velocities to minimize Jdotv effect (not in MJX) + # TODO(team): remove this change when mjx supports this feature + d.qvel[:] *= 1e-2 mujoco.mj_forward(m, d) mx = mjx.put_model(m) @@ -92,6 +95,9 @@ class ForwardTest(absltest.TestCase): d.xfrc_applied[0, 2] = 0.1 # torque d.xfrc_applied[1, 4] = 0.3 # linear force mujoco.mj_step(m, d, 20) # get some dynamics going + # scale down velocities to minimize Jdotv effect (not in MJX) + # TODO(team): remove this change when mjx supports this feature + d.qvel[:] *= 1e-2 dx = jax.jit(mjx.step)(mjx.put_model(m), mjx.put_data(m, d)) mujoco.mj_step(m, d) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 179c4477..9bbece11 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -612,6 +612,39 @@ void mj_mulJacTVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* } +// compute global anchor points for connect/weld equality constraints +static void mj_equalityAnchors(const mjModel* m, const mjData* d, int eq_id, + mjtNum pos1[3], mjtNum pos2[3], + int* body1, int* body2) { + mjtEq type = (mjtEq) m->eq_type[eq_id]; + int obj1 = m->eq_obj1id[eq_id]; + int obj2 = m->eq_obj2id[eq_id]; + + if (m->eq_objtype[eq_id] == mjOBJ_BODY) { + const mjtNum* data = m->eq_data + mjNEQDATA*eq_id; + if (type == mjEQ_CONNECT) { + mju_mulMatVec3(pos1, d->xmat + 9*obj1, data); + mju_addTo3(pos1, d->xpos + 3*obj1); + mju_mulMatVec3(pos2, d->xmat + 9*obj2, data + 3); + mju_addTo3(pos2, d->xpos + 3*obj2); + } else { + // weld uses data+3*(1-j) for anchor + mju_mulMatVec3(pos1, d->xmat + 9*obj1, data + 3); + mju_addTo3(pos1, d->xpos + 3*obj1); + mju_mulMatVec3(pos2, d->xmat + 9*obj2, data); + mju_addTo3(pos2, d->xpos + 3*obj2); + } + *body1 = obj1; + *body2 = obj2; + } else { + mju_copy3(pos1, d->site_xpos + 3*obj1); + mju_copy3(pos2, d->site_xpos + 3*obj2); + *body1 = m->site_bodyid[obj1]; + *body2 = m->site_bodyid[obj2]; + } +} + + //--------------------- instantiate constraints by type -------------------------------------------- // equality constraints @@ -670,21 +703,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { switch ((mjtEq) m->eq_type[i]) { case mjEQ_CONNECT: // connect bodies with ball joint // find global points, body semantic - if (m->eq_objtype[i] == mjOBJ_BODY) { - for (int j=0; j < 2; j++) { - mju_mulMatVec3(pos[j], d->xmat + 9*id[j], data + 3*j); - mju_addTo3(pos[j], d->xpos + 3*id[j]); - body_id[j] = id[j]; - } - } - - // find global points, site semantic - else { - for (int j=0; j < 2; j++) { - mju_copy3(pos[j], d->site_xpos + 3*id[j]); - body_id[j] = m->site_bodyid[id[j]]; - } - } + mj_equalityAnchors(m, d, i, pos[0], pos[1], body_id, body_id + 1); // compute position error mju_sub3(cpos, pos[0], pos[1]); @@ -702,22 +721,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { case mjEQ_WELD: // fix relative position and orientation // find global points, body semantic - if (m->eq_objtype[i] == mjOBJ_BODY) { - for (int j=0; j < 2; j++) { - mjtNum* anchor = data + 3*(1-j); - mju_mulMatVec3(pos[j], d->xmat + 9*id[j], anchor); - mju_addTo3(pos[j], d->xpos + 3*id[j]); - body_id[j] = id[j]; - } - } - - // find global points, site semantic - else { - for (int j=0; j < 2; j++) { - mju_copy3(pos[j], d->site_xpos + 3*id[j]); - body_id[j] = m->site_bodyid[id[j]]; - } - } + mj_equalityAnchors(m, d, i, pos[0], pos[1], body_id, body_id + 1); // compute position error mju_sub3(cpos, pos[0], pos[1]); @@ -1133,6 +1137,208 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mj_freeStack(d); } +// subtract Jdot*v correction from result vector for equality constraints +void mj_Jdotv(const mjModel* m, mjData* d, mjtNum* result) { + int nv = m->nv, ne = d->ne; + + // nothing to do + if (!ne || !nv) { + return; + } + + int issparse = mj_isSparse(m); + + mj_markStack(d); + + // allocate scratch for jacDot matrices (translational and rotational) + int* chain = issparse ? mjSTACKALLOC(d, nv, int) : NULL; + mjtNum* jacdot1 = NULL; + mjtNum* jacdot2 = NULL; + mjtNum* jacrdot1 = NULL; + mjtNum* jacrdot2 = NULL; + + // iterate over equality constraint efc rows + int row = 0; + while (row < ne) { + int eq_id = d->efc_id[row]; + mjtEq type = (mjtEq) m->eq_type[eq_id]; + + // connect or weld: compute Jdot*v for translational part + if (type == mjEQ_CONNECT || type == mjEQ_WELD) { + mjtNum* data = m->eq_data + mjNEQDATA*eq_id; + + // allocate translational scratch on first connect or weld + if (!jacdot1) { + jacdot1 = mjSTACKALLOC(d, 3*nv, mjtNum); + jacdot2 = mjSTACKALLOC(d, 3*nv, mjtNum); + } + + // allocate rotational scratch on first weld + if (type == mjEQ_WELD && !jacrdot1) { + jacrdot1 = mjSTACKALLOC(d, 3*nv, mjtNum); + jacrdot2 = mjSTACKALLOC(d, 3*nv, mjtNum); + } + + // compute global anchor points and body ids + int obj1 = m->eq_obj1id[eq_id]; + int obj2 = m->eq_obj2id[eq_id]; + mjtNum pos1[3], pos2[3]; + int body1, body2; + mj_equalityAnchors(m, d, eq_id, pos1, pos2, &body1, &body2); + + // compute jacDot*v for each body point + mjtNum jdv1[3], jdv2[3]; + mjtNum jrdv1[3] = {0}, jrdv2[3] = {0}; + if (issparse) { + // get merged chain for the two bodies + int NV = mj_mergeChain(m, chain, body1, body2, /*flg_skipcommon=*/0); + + if (NV) { + // sparse: translational and rotational + mjtNum* jacr1 = (type == mjEQ_WELD) ? jacrdot1 : NULL; + mjtNum* jacr2 = (type == mjEQ_WELD) ? jacrdot2 : NULL; + mj_jacDotSparse(m, d, jacdot1, jacr1, pos1, body1, NV, chain); + mj_jacDotSparse(m, d, jacdot2, jacr2, pos2, body2, NV, chain); + + // translational jdv = jacDot * qvel + mju_dotSparseX3(jdv1, jdv1+1, jdv1+2, jacdot1, jacdot1+NV, jacdot1+2*NV, + d->qvel, NV, chain); + mju_dotSparseX3(jdv2, jdv2+1, jdv2+2, jacdot2, jacdot2+NV, jacdot2+2*NV, + d->qvel, NV, chain); + + // rotational jdv for welds + if (type == mjEQ_WELD) { + mju_dotSparseX3(jrdv1, jrdv1+1, jrdv1+2, jacrdot1, jacrdot1+NV, jacrdot1+2*NV, + d->qvel, NV, chain); + mju_dotSparseX3(jrdv2, jrdv2+1, jrdv2+2, jacrdot2, jacrdot2+NV, jacrdot2+2*NV, + d->qvel, NV, chain); + } + } else { + mju_zero3(jdv1); + mju_zero3(jdv2); + } + } else { + // dense: translational and rotational + mjtNum* jacr1 = (type == mjEQ_WELD) ? jacrdot1 : NULL; + mjtNum* jacr2 = (type == mjEQ_WELD) ? jacrdot2 : NULL; + mj_jacDot(m, d, jacdot1, jacr1, pos1, body1); + mj_jacDot(m, d, jacdot2, jacr2, pos2, body2); + + // translational jdv = jacDot * qvel + mju_mulMatVec(jdv1, jacdot1, d->qvel, 3, nv); + mju_mulMatVec(jdv2, jacdot2, d->qvel, 3, nv); + + // rotational jdv for welds + if (type == mjEQ_WELD) { + mju_mulMatVec(jrdv1, jacrdot1, d->qvel, 3, nv); + mju_mulMatVec(jrdv2, jacrdot2, d->qvel, 3, nv); + } + } + + // subtract translational Jdot*v + result[row+0] -= jdv1[0] - jdv2[0]; + result[row+1] -= jdv1[1] - jdv2[1]; + result[row+2] -= jdv1[2] - jdv2[2]; + + // advance past translational rows + row += 3; + + // weld: compute rotational Jdot*v + if (type == mjEQ_WELD) { + mjtNum torquescale = data[10]; + + // get body quaternions and relpose, following mj_instantiateEquality + mjtNum q0r[4], negq1[4]; // q0r = q0*relpose, negq1 = neg(q1) + if (m->eq_objtype[eq_id] == mjOBJ_BODY) { + mjtNum* relpose = data+6; + mju_mulQuat(q0r, d->xquat+4*body1, relpose); + mju_negQuat(negq1, d->xquat+4*body2); + } else { + mju_mulQuat(q0r, d->xquat+4*body1, m->site_quat+4*obj1); + mjtNum qsite1[4]; + mju_mulQuat(qsite1, d->xquat+4*body2, m->site_quat+4*obj2); + mju_negQuat(negq1, qsite1); + } + + // angular velocities from cvel (first 3 components are angular) + const mjtNum* omega1 = d->cvel+6*body1; + const mjtNum* omega2 = d->cvel+6*body2; + + // relative angular velocity: domega = omega1 - omega2 + mjtNum domega[3]; + mju_sub3(domega, omega1, omega2); + + // quaternion derivatives: qdot = 0.5 * q * (0, omega) + mjtNum qdot0[4]; + if (m->eq_objtype[eq_id] == mjOBJ_BODY) { + mju_derivQuat(qdot0, d->xquat+4*body1, omega1); + } else { + mjtNum qfull0[4]; + mju_mulQuat(qfull0, d->xquat+4*body1, m->site_quat+4*obj1); + mju_derivQuat(qdot0, qfull0, omega1); + } + mjtNum qdot0r[4]; // d/dt(q0 * relpose) = qdot0 * relpose + if (m->eq_objtype[eq_id] == mjOBJ_BODY) { + mju_mulQuat(qdot0r, qdot0, data+6); + } else { + mju_copy4(qdot0r, qdot0); + } + + // neg(qdot1): d/dt(neg(q1)) = neg(qdot1) + mjtNum negqdot1[4]; + if (m->eq_objtype[eq_id] == mjOBJ_BODY) { + mjtNum qdot1[4]; + mju_derivQuat(qdot1, d->xquat+4*body2, omega2); + mju_negQuat(negqdot1, qdot1); + } else { + mjtNum qfull1[4], qdot1[4]; + mju_mulQuat(qfull1, d->xquat+4*body2, m->site_quat+4*obj2); + mju_derivQuat(qdot1, qfull1, omega2); + mju_negQuat(negqdot1, qdot1); + } + + // Jdot_rot * v differentiates: 0.5 * neg(q1) * (J0-J1)*v * q0*relpose + // three terms from product rule: + + // djrdv = Jrdot0*v - Jrdot1*v (rotational jacDot difference * v) + mjtNum djrdv[3]; + mju_sub3(djrdv, jrdv1, jrdv2); + + // term1: neg(qdot1) * domega * q0r + mjtNum t1a[4], t1[4]; + mju_mulQuatAxis(t1a, negqdot1, domega); + mju_mulQuat(t1, t1a, q0r); + + // term2: neg(q1) * djrdv * q0r + mjtNum t2a[4], t2[4]; + mju_mulQuatAxis(t2a, negq1, djrdv); + mju_mulQuat(t2, t2a, q0r); + + // term3: neg(q1) * domega * qdot0r + mjtNum t3a[4], t3[4]; + mju_mulQuatAxis(t3a, negq1, domega); + mju_mulQuat(t3, t3a, qdot0r); + + // combine: 0.5 * (term1 + term2 + term3), take vector part, scale + result[row+0] -= 0.5 * (t1[1] + t2[1] + t3[1]) * torquescale; + result[row+1] -= 0.5 * (t1[2] + t2[2] + t3[2]) * torquescale; + result[row+2] -= 0.5 * (t1[3] + t2[3] + t3[3]) * torquescale; + + row += 3; + } + } + + // other types: advance past all rows with this efc_id + else { + while (row < ne && d->efc_id[row] == eq_id) { + row++; + } + } + } + + mj_freeStack(d); +} + // return number of constraint non-zeros, handle dense and dof-less cases static inline int mj_addConstraintCount(const mjModel* m, int size, int NV) { @@ -2838,6 +3044,11 @@ void mj_referenceConstraint(const mjModel* m, mjData* d) { d->efc_aref[i] = -KBIP[4*i+1]*d->efc_vel[i] -KBIP[4*i]*KBIP[4*i+2]*(d->efc_pos[i]-d->efc_margin[i]); } + + // subtract Jdot*v correction for connect/weld equality constraints + if (d->ne > 0) { + mj_Jdotv(m, d, d->efc_aref); + } } diff --git a/src/engine/engine_core_constraint.h b/src/engine/engine_core_constraint.h index 9557fbd9..15c03b6b 100644 --- a/src/engine/engine_core_constraint.h +++ b/src/engine/engine_core_constraint.h @@ -18,6 +18,7 @@ #include #include #include +#include #ifdef __cplusplus extern "C" { @@ -35,6 +36,9 @@ MJAPI void mj_mulJacVec(const mjModel* m, const mjData* d, mjtNum* res, const mj // multiply JacobianT by vector MJAPI void mj_mulJacTVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec); +// subtract Jdot*v correction from result vector +MJAPI void mj_Jdotv(const mjModel* m, mjData* d, mjtNum* result); + //-------------------------- utility functions ----------------------------------------------------- diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index 9c5e07cc..4168cb8b 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -32,6 +32,11 @@ extern "C" { MJAPI mjtNum mju_dotSparse2(const mjtNum* vec1, const int* ind1, int nnz1, const mjtNum* vec2, const int* ind2, int nnz2); +// dot-productX3, first vector is sparse; supernode of size 3 +void mju_dotSparseX3(mjtNum* res0, mjtNum* res1, mjtNum* res2, + const mjtNum* vec10, const mjtNum* vec11, const mjtNum* vec12, + const mjtNum* vec2, int nnz1, const int* ind1); + // convert matrix from dense to sparse // nnz is size of res and colind, return 1 if too small, 0 otherwise MJAPI int mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc, diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index b5ab5e0e..7b055936 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -787,6 +787,116 @@ TEST_F(CoreConstraintTest, ContactSharedDofJacobian) { mj_deleteModel(model); } +static const char* const kJdotvConnect2dPath = + "engine/testdata/core_constraint/jdotv_connect_2d.xml"; +static const char* const kJdotvConnect3dPath = + "engine/testdata/core_constraint/jdotv_connect_3d.xml"; +static const char* const kJdotvWeld3dPath = + "engine/testdata/core_constraint/jdotv_weld_3d.xml"; + +// validate mj_Jdotv against finite-differenced constraint Jacobian +TEST_F(CoreConstraintTest, JdotvFiniteDifference) { + + for (const char* path : {kJdotvConnect2dPath, + kJdotvConnect3dPath, + kJdotvWeld3dPath}) { + const std::string xml_path = GetTestDataFilePath(path); + char err[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, err, sizeof(err)); + ASSERT_THAT(m, NotNull()) << err << " for " << path; + int nv = m->nv; + mjData* d = mj_makeData(m); + + // simulate for 1 second to accumulate velocity + while (d->time < 1.0) { + mj_step(m, d); + } + + // forward to populate constraints + mj_forward(m, d); + ASSERT_GT(d->ne, 0) << "no equality constraints for " << path; + int ne = d->ne; + + // get dense J_0 (ne x nv) + std::vector J0(ne * nv); + if (mj_isSparse(m)) { + mju_sparse2dense(J0.data(), d->efc_J, ne, nv, + d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); + } else { + mju_copy(J0.data(), d->efc_J, ne * nv); + } + + // compute mj_Jdotv at current state + std::vector jdv(ne, 0); + mj_Jdotv(m, d, jdv.data()); + + // save qpos and qvel + std::vector qpos0(m->nq), qvel0(nv); + mju_copy(qpos0.data(), d->qpos, m->nq); + mju_copy(qvel0.data(), d->qvel, nv); + + // integrate qpos forward by h using qvel + const mjtNum h = MjTol(1e-7, 5e-4); + mj_integratePos(m, d->qpos, d->qvel, h); + mj_forward(m, d); + + // get dense J_h (ne x nv) + ASSERT_EQ(d->ne, ne) << "constraint count changed after integration"; + std::vector Jh(ne * nv); + if (mj_isSparse(m)) { + mju_sparse2dense(Jh.data(), d->efc_J, ne, nv, + d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); + } else { + mju_copy(Jh.data(), d->efc_J, ne * nv); + } + + // FD: Jdotv_fd[i] = -sum_j (Jh[i,j] - J0[i,j]) / h * qvel[j] + // (negated because mj_Jdotv subtracts) + std::vector jdv_fd(ne, 0); + for (int i = 0; i < ne; i++) { + for (int j = 0; j < nv; j++) { + jdv_fd[i] -= (Jh[i*nv+j] - J0[i*nv+j]) / h * qvel0[j]; + } + } + + // compare + EXPECT_THAT(AsVector(jdv.data(), ne), + Pointwise(MjNear(1e-4, 1e-2), AsVector(jdv_fd.data(), ne))) + << "Jdotv FD mismatch for " << path; + + mj_deleteData(d); + mj_deleteModel(m); + } +} + +// Test 2: forward-inverse identity preserved with Jdot*v correction +TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) { + for (const char* path : {kJdotvConnect2dPath, + kJdotvConnect3dPath, + kJdotvWeld3dPath}) { + const std::string xml_path = GetTestDataFilePath(path); + char err[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, err, sizeof(err)); + ASSERT_THAT(m, NotNull()) << err; + mjData* d = mj_makeData(m); + + // give initial velocity + for (int i = 0; i < m->nv; i++) d->qvel[i] = 0.5 * (i + 1); + + // forward (with correction ON by default) + mj_forward(m, d); + mj_compareFwdInv(m, d); + mjtNum fwdinv = d->solver_fwdinv[0]; + + mjtNum epsilon = MjTol(1e-10, 1e-2); + EXPECT_LT(fwdinv, epsilon) + << "fwdinv broken for " << path + << " (fwdinv=" << fwdinv << ")"; + + mj_deleteData(d); + mj_deleteModel(m); + } +} } // namespace } // namespace mujoco diff --git a/test/engine/engine_solver_test.cc b/test/engine/engine_solver_test.cc index 4b46ddf8..523dfd33 100644 --- a/test/engine/engine_solver_test.cc +++ b/test/engine/engine_solver_test.cc @@ -55,16 +55,21 @@ TEST_F(SolverTest, IslandsEquivalent) { mjData* data_island = mj_makeData(model); mjData* data_noisland = mj_makeData(model); - // Below are 3 tolerances associated with 3 different iteration counts, - // they are only moderately tight, 12x higher than x86-64 failure on Linux, - // i.e. in that case the test fails with rtol smaller than {6e-3, 6e-4, 6e-5}. + constexpr int kNumTol = 3; + mjtNum maxiter[kNumTol] = {30, 40, 60}; + // Below are 3 tolerances associated with 3 different iteration counts. + // Tolerances are set to be ~12x higher than failure thresholds. + // For float32, failure thresholds are ~6000x larger than for float64. + // Line 99 adds a 500x factor for float32, so we need another ~12x in rtol. // The point of this test is to show that CG convergence is actually not very // precise, simply changing whether islands are used changes the solution by // quite a lot, even at high iteration count and zero {ls_}tolerance. // Increasing the iteration count higher than 60 does not improve convergence. - constexpr int kNumTol = 3; - mjtNum maxiter[kNumTol] = {30, 40, 60}; - mjtNum rtol[kNumTol] = {6e-2, 6e-3, 6e-4}; + mjtNum rtol[kNumTol] = { + MjTol(6e-2, 7.2e-1), + MjTol(6e-3, 7.2e-2), + MjTol(6e-4, 7.2e-3) + }; for (int i = 0; i < kNumTol; ++i) { model->opt.iterations = maxiter[i]; diff --git a/test/engine/testdata/core_constraint/jdotv_connect_2d.xml b/test/engine/testdata/core_constraint/jdotv_connect_2d.xml new file mode 100644 index 00000000..18daa136 --- /dev/null +++ b/test/engine/testdata/core_constraint/jdotv_connect_2d.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/core_constraint/jdotv_connect_3d.xml b/test/engine/testdata/core_constraint/jdotv_connect_3d.xml new file mode 100644 index 00000000..5f404df4 --- /dev/null +++ b/test/engine/testdata/core_constraint/jdotv_connect_3d.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/engine/testdata/core_constraint/jdotv_weld_3d.xml b/test/engine/testdata/core_constraint/jdotv_weld_3d.xml new file mode 100644 index 00000000..5a533de9 --- /dev/null +++ b/test/engine/testdata/core_constraint/jdotv_weld_3d.xml @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/wasm/tests/bindings_test.ts b/wasm/tests/bindings_test.ts index 08d9864a..516cf507 100644 --- a/wasm/tests/bindings_test.ts +++ b/wasm/tests/bindings_test.ts @@ -365,7 +365,7 @@ describe('MuJoCo WASM Bindings', () => { mujoco.mj_constraintUpdate( model!, data!, res.GetView(), cost, /*flg_coneHessian=*/ 1); - expect(cost.GetView()[0]).toBeCloseTo(3355.837); + expect(cost.GetView()[0]).toBeCloseTo(3357.584); res.delete(); cost.delete(); From 8135465779368bc4fdf5af0bb3d0fcd64d72fe67 Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Mon, 13 Apr 2026 17:30:26 -0700 Subject: [PATCH 052/251] Remove usages of `texture2D` and other FL0 features that are obsolete due to FL0 now going through `spirv-cross`. This is an early upload of https://github.com/google/filament/pull/9867 and necessary material changes Original PR commits: FL0 materials are always compile via spirv-cross postfx materials now support sRGB emulation in FL0 PiperOrigin-RevId: 899262890 Change-Id: I52c002d60790d84269674c1f0d0c5f7d6a58000c --- src/experimental/filament/assets/unlit_ui.mat | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/experimental/filament/assets/unlit_ui.mat b/src/experimental/filament/assets/unlit_ui.mat index 2b4f402e..80d308a2 100644 --- a/src/experimental/filament/assets/unlit_ui.mat +++ b/src/experimental/filament/assets/unlit_ui.mat @@ -36,7 +36,7 @@ fragment { prepareMaterial(material); vec2 uv = getUV0(); uv.y = 1.0 - uv.y; - vec4 tex_color = texture2D(materialParams_glyph, uv); + vec4 tex_color = texture(materialParams_glyph, uv); material.baseColor = getColor() * tex_color; material.baseColor.rgb *= material.baseColor.a; } From 82e021fcdae3cbd824c0f8d0cc38ce82b3d92205 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 14 Apr 2026 00:13:00 -0700 Subject: [PATCH 053/251] Fix UI material; it no longer requires featureLevel 0. PiperOrigin-RevId: 899400551 Change-Id: I7a2d42cd5342ed079599eef50e85c50aa340aecc --- src/experimental/filament/assets/unlit_ui.mat | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/experimental/filament/assets/unlit_ui.mat b/src/experimental/filament/assets/unlit_ui.mat index 80d308a2..0148f28e 100644 --- a/src/experimental/filament/assets/unlit_ui.mat +++ b/src/experimental/filament/assets/unlit_ui.mat @@ -27,8 +27,7 @@ material { shadingModel : unlit, culling : none, depthCulling: false, - blending : transparent, - featureLevel : 0 + blending : transparent } fragment { From 171e6dc1775d8effe5f05af36ee5b36915aabe20 Mon Sep 17 00:00:00 2001 From: Michael Moss Date: Tue, 14 Apr 2026 01:15:49 -0700 Subject: [PATCH 054/251] Automated rollback of PiperOrigin-RevId: 899009633. *** Original change description *** Add protocols for authoring simulation environments. This is for preview only, we discourage users from using this in production code. PiperOrigin-RevId: 899425542 Change-Id: Ifb6c5d3b762789d821bfa9826f5d6d983e8af7d3 --- .../reaf/core/action_space_adapter.py | 43 -- .../reaf/core/commands_processor.py | 91 ---- .../data_acquisition_and_control_layer.py | 170 ------ .../reaf/core/default_discount_provider.py | 79 --- .../core/default_observation_space_adapter.py | 231 --------- src/experimental/reaf/core/device.py | 54 -- .../reaf/core/device_coordinator.py | 87 ---- .../reaf/core/discount_provider.py | 61 --- src/experimental/reaf/core/entity.py | 65 --- src/experimental/reaf/core/environment.py | 490 ------------------ .../reaf/core/features_observer.py | 34 -- .../reaf/core/features_producer.py | 56 -- src/experimental/reaf/core/logger.py | 80 --- .../reaf/core/numpy_mock_assertions.py | 98 ---- .../reaf/core/observation_space_adapter.py | 42 -- .../core/pass_through_action_space_adapter.py | 55 -- src/experimental/reaf/core/reward_provider.py | 292 ----------- .../reaf/core/substep_commands_processor.py | 104 ---- .../core/substep_measurements_processor.py | 103 ---- .../reaf/core/task_logic_layer.py | 342 ------------ .../reaf/core/termination_checker.py | 94 ---- src/experimental/reaf/core/trigger.py | 29 -- .../reaf/core/zero_reward_provider.py | 48 -- 23 files changed, 2748 deletions(-) delete mode 100644 src/experimental/reaf/core/action_space_adapter.py delete mode 100644 src/experimental/reaf/core/commands_processor.py delete mode 100644 src/experimental/reaf/core/data_acquisition_and_control_layer.py delete mode 100644 src/experimental/reaf/core/default_discount_provider.py delete mode 100644 src/experimental/reaf/core/default_observation_space_adapter.py delete mode 100644 src/experimental/reaf/core/device.py delete mode 100644 src/experimental/reaf/core/device_coordinator.py delete mode 100644 src/experimental/reaf/core/discount_provider.py delete mode 100644 src/experimental/reaf/core/entity.py delete mode 100644 src/experimental/reaf/core/environment.py delete mode 100644 src/experimental/reaf/core/features_observer.py delete mode 100644 src/experimental/reaf/core/features_producer.py delete mode 100644 src/experimental/reaf/core/logger.py delete mode 100644 src/experimental/reaf/core/numpy_mock_assertions.py delete mode 100644 src/experimental/reaf/core/observation_space_adapter.py delete mode 100644 src/experimental/reaf/core/pass_through_action_space_adapter.py delete mode 100644 src/experimental/reaf/core/reward_provider.py delete mode 100644 src/experimental/reaf/core/substep_commands_processor.py delete mode 100644 src/experimental/reaf/core/substep_measurements_processor.py delete mode 100644 src/experimental/reaf/core/task_logic_layer.py delete mode 100644 src/experimental/reaf/core/termination_checker.py delete mode 100644 src/experimental/reaf/core/trigger.py delete mode 100644 src/experimental/reaf/core/zero_reward_provider.py diff --git a/src/experimental/reaf/core/action_space_adapter.py b/src/experimental/reaf/core/action_space_adapter.py deleted file mode 100644 index 014e02eb..00000000 --- a/src/experimental/reaf/core/action_space_adapter.py +++ /dev/null @@ -1,43 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Adapts environment action into suitable commands format accepted by REAF.""" - -import abc -from collections.abc import Mapping - -from gdm_robotics.interfaces import types as gdmr_types - - -class ActionSpaceAdapter(abc.ABC): - """Adapts environment action into suitable commands format accepted by REAF. - - Implementations of this interface are responsible for converting the more - generic action accepted by the environment (e.g. a flat numpy array) into the - more constraining format accepted as commands by REAF, i.e. a dictionary of - string to tensors. - """ - - @abc.abstractmethod - def commands_from_environment_action( - self, environment_action: gdmr_types.ActionType - ) -> Mapping[str, gdmr_types.ArrayType]: - """Converts the environment action into commands accepted by REAF.""" - - @abc.abstractmethod - def action_spec(self) -> gdmr_types.ActionSpec: - """Returns the action spec exposed by the environment.""" - - @abc.abstractmethod - def task_commands_keys(self) -> set[str]: - """Returns the keys for the commands exposed to the task layer.""" diff --git a/src/experimental/reaf/core/commands_processor.py b/src/experimental/reaf/core/commands_processor.py deleted file mode 100644 index 4cc12b4a..00000000 --- a/src/experimental/reaf/core/commands_processor.py +++ /dev/null @@ -1,91 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Abstract class for commands manipulation in the task logic layer.""" - -import abc -from collections.abc import Mapping - -from gdm_robotics.interfaces import types as gdmr_types - - -class CommandsProcessor(abc.ABC): - """Perform commands manipulation. - - The following describes the processing pipeline starting from the top (closer - to the policy) to the bottom (interfacing with the DACL commands spec). - - Assume that we have two processing units: - Processor 1) has a consumed_commands_spec for two keys: "p1/c1" and "p1/c2". - Its produced_commands_keys are "p2/c1". - Processor 2) has a consumed_commands_spec for "p2/c1". Its - produced_commands_keys are "p3/c1" and "p3/c2". - - Specs are propagated starting from the bottom: - 1) In this example assume that the DACL exposes "p3/c1", "p3/c2" and "p3/c3". - 2) Processor 2) returns ("p3/c1", "p3/c2") from input "p2/c1". This means that - the global commands spec exposed at this level is "p2/c1" and the - unprocessed "p3/c3". - 3) Processor 1) returns "p2/c1" from input ("p1/c1", "p1/c2"). By applying the - same transformation rule, we can obtain the final commands spec exposed by - the full processing pipeline: "p1/c1", "p1/c2" and "p3/c3". - - "p1/c1" "p1/c2" "p3/c3" - | | | - ----------------- | - | P1 | | - ----------------- | - | "p2/c1" | - ----------------- | - | P2 | | - ----------------- | - | "p3/c1" | "p3/c2" | - | | | - ------------------------------------ - | DACL | - ------------------------------------ - """ - - @property - @abc.abstractmethod - def name(self) -> str: - """Returns a unique string identifier for this object.""" - - @abc.abstractmethod - def process_commands( - self, consumed_commands: Mapping[str, gdmr_types.ArrayType] - ) -> Mapping[str, gdmr_types.ArrayType]: - """Processes the commands and returns a new modified version of it. - - Args: - consumed_commands: the commands up in the processing chain (or provided by - the Environment) that are required by this processor, i.e. with keys - specified by `consumed_commands_spec`. - - Returns the new commands. Note that the data in consumed_commands is removed - from the global commands dictionary. If users want to keep some of the - elements it is their responsibility to retain them in the output - dictionary. - """ - - @abc.abstractmethod - def consumed_commands_spec(self) -> Mapping[str, gdmr_types.AnyArraySpec]: - """Spec of the commands consumed by this processor.""" - - @abc.abstractmethod - def produced_commands_keys(self) -> set[str]: - """Keys of the commands produced by this processor.""" - - def reset(self) -> None: - """Resets the internal state of the command processor.""" - ... diff --git a/src/experimental/reaf/core/data_acquisition_and_control_layer.py b/src/experimental/reaf/core/data_acquisition_and_control_layer.py deleted file mode 100644 index e8fcc8ca..00000000 --- a/src/experimental/reaf/core/data_acquisition_and_control_layer.py +++ /dev/null @@ -1,170 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""REAF data acquisition and control layer to interface with the robotic setup.""" - -from collections.abc import Iterable, Mapping - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types -from reaf.core import device as reaf_device -from reaf.core import device_coordinator as reaf_coordinator -from reaf.core import trigger - - -class DataAcquisitionAndControlLayer: - """REAF data acquisition and control layer. - - The DACL is responsible to provide an interface for the robotic setup. - """ - - def __init__( - self, - *, - device_coordinator: reaf_coordinator.DeviceCoordinator, - commands_trigger: trigger.Trigger | None, - measurements_trigger: trigger.Trigger | None, - ): - """Initializes the DataAcquisitionAndControlLayer. - - Args: - device_coordinator: The coordinator representing a specific robotic setup. - Note that callers need to explicitly initialize and finalise the - coordinator. - commands_trigger: A trigger to unblock processing commands during a call - to `step`. - measurements_trigger: A trigger to unblock processing measurements during - a call to `step`. - """ - self._coordinator = device_coordinator - self._devices = self._coordinator.get_devices() - # The following checks that names of the devices are unique and their keys - # are "mergeable". - self._check_device_names_and_keys(self._devices) - - self._commands_trigger = commands_trigger - self._measurements_trigger = measurements_trigger - - # Create a map of supported commands keys for each Device. - self._commands_for_device = { - device.name: device.commands_spec().keys() for device in self._devices - } - - def begin_stepping(self) -> Mapping[str, gdmr_types.ArrayType]: - """Begins stepping the DACL and returns the current measurements.""" - self._coordinator.on_begin_stepping() - - # Wait for the first trigger to happen before collecting the measurements. - if self._measurements_trigger is not None: - self._measurements_trigger.wait_for_event() - return self._get_measurements() - - def end_stepping(self) -> None: - """Ends stepping the data acquisition and control layer.""" - self._coordinator.on_end_stepping() - - def _set_commands(self, commands: Mapping[str, gdmr_types.ArrayType]) -> None: - """Sets the commands of the data acquisition and control layer.""" - self._coordinator.before_set_commands() - for device in self._devices: - device_commands = { - k: v - for k, v in commands.items() - if k in self._commands_for_device[device.name] - } - device.set_commands(device_commands) - self._coordinator.after_set_commands() - - def _get_measurements(self) -> Mapping[str, gdmr_types.ArrayType]: - """Gets the measurements of the data acquisition and control layer.""" - measurements = {} - self._coordinator.before_get_measurements() - for device in self._devices: - measurements.update(device.get_measurements()) - - return measurements - - def step( - self, commands: Mapping[str, gdmr_types.ArrayType] - ) -> Mapping[str, gdmr_types.ArrayType]: - """Steps the data acquisition and control layer.""" - if self._commands_trigger is not None: - self._commands_trigger.wait_for_event() - self._set_commands(commands) - - if self._measurements_trigger is not None: - self._measurements_trigger.wait_for_event() - return self._get_measurements() - - def commands_spec(self) -> Mapping[str, gdmr_types.AnyArraySpec]: - """Returns the specs for the commands.""" - spec = {} - for device in self._devices: - spec.update(device.commands_spec()) - return spec - - def measurements_spec(self) -> Mapping[str, specs.Array]: - """Returns the specs for the measurements.""" - spec = {} - for device in self._devices: - spec.update(device.measurements_spec()) - return spec - - @property - def device_coordinator(self) -> reaf_coordinator.DeviceCoordinator: - return self._coordinator - - def _check_keys_have_been_formatted_correctly( - self, current_key_set: Iterable[str] - ) -> None: - """Check that keys haven't been left unformatted.""" - for key in current_key_set: - if key.find("{}") != -1: - raise ValueError( - "Keys should not contain '{}'. Did you mean to use format()?" - ) - - def _check_device_names_and_keys( - self, devices: Iterable[reaf_device.Device] - ) -> None: - """Raises error if device names are not unique or keys are not exclusive.""" - # Check names first. - all_names = [device.name for device in devices] - unique_names = set(all_names) - if len(unique_names) != len(all_names): - raise RuntimeError(f"Duplicate names when checking devices: {all_names}") - - # Check commands. - devices = tuple(devices) - current_specs = set() - for device in devices: - device_keys = device.commands_spec().keys() - self._check_keys_have_been_formatted_correctly(device_keys) - if not current_specs.isdisjoint(device_keys): - raise RuntimeError( - f"Duplicate keys when checking device {device.name}:" - f" {current_specs.intersection(device_keys)}" - ) - current_specs.update(device_keys) - - # Check measurements. - current_specs = set() - for device in devices: - device_keys = device.measurements_spec().keys() - self._check_keys_have_been_formatted_correctly(device_keys) - if not current_specs.isdisjoint(device_keys): - raise RuntimeError( - f"Duplicate keys when checking device {device.name}:" - f" {current_specs.intersection(device_keys)}" - ) - current_specs.update(device_keys) diff --git a/src/experimental/reaf/core/default_discount_provider.py b/src/experimental/reaf/core/default_discount_provider.py deleted file mode 100644 index ff216b7c..00000000 --- a/src/experimental/reaf/core/default_discount_provider.py +++ /dev/null @@ -1,79 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Computes a constant discount given the termination state. - -This provider returns a discount of 0.0 in case of termination and 1.0 -otherwise (i.e. for truncation and not termination). - -It is usually safe to use this discount provider for environments that return -strictly positive rewards. -""" - -from collections.abc import Mapping - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types -import numpy as np -from reaf.core import discount_provider -from reaf.core import termination_checker -import tree - - -class DefaultDiscountProvider(discount_provider.DiscountProvider): - """Computes a constant discount given the termination state. - - This provider returns a discount of 0.0 in case of termination and 1.0 - otherwise (i.e. for truncation and not termination). - - It is usually safe to use this discount provider for environments that return - strictly positive rewards. - """ - - def __init__(self, name: str = "default_discount_provider"): - self._name = name - self._spec = specs.BoundedArray( - shape=(), dtype=np.float64, minimum=0.0, maximum=1.0, name="discount" - ) - - def name(self) -> str: - """Returns a unique string identifier for this object.""" - return self._name - - def compute_discount( - self, - unused_required_features: Mapping[str, gdmr_types.ArrayType], - termination_state: termination_checker.TerminationResult, - ) -> tree.Structure[gdmr_types.ArrayType]: - """Computes the discount. - - Args: - unused_required_features: Unused - termination_state: The termination state as computed by the termination - checkers. Returns the discount. - - Returns: - The discount. - """ - if termination_state == termination_state.TERMINATE: - return np.asarray(0).astype(self._spec.dtype) - else: # TRUNCATION or DO_NOT_TERMINATE - return np.asarray(1.0).astype(self._spec.dtype) - - def discount_spec(self) -> tree.Structure[specs.Array]: - """Returns the spec of the discount.""" - return self._spec - - def required_features_keys(self) -> set[str]: - """Returns the feature keys that are required to compute the discount.""" - return set() diff --git a/src/experimental/reaf/core/default_observation_space_adapter.py b/src/experimental/reaf/core/default_observation_space_adapter.py deleted file mode 100644 index 1261b3ca..00000000 --- a/src/experimental/reaf/core/default_observation_space_adapter.py +++ /dev/null @@ -1,231 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""ObservationSpaceAdapter supporting filtering, renaming and type conversion.""" - -import abc -from collections.abc import Iterable, Mapping -import dataclasses - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types -import numpy as np -import numpy.typing as npt -from reaf.core import observation_space_adapter -import tree - - -@dataclasses.dataclass(frozen=True, kw_only=True) -class RenameInfo: - original_key: str - renamed_key: str - - -class ObservationTypeMapper(abc.ABC): - """Maps from REAF features and specs into corresponding environment types.""" - - @abc.abstractmethod - def to_observation_spec( - self, features_spec: Mapping[str, specs.Array] - ) -> gdmr_types.ObservationSpec: - """Convert the features spec into the environment observation spec.""" - - @abc.abstractmethod - def to_observations( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> tree.Structure[gdmr_types.ArrayType]: - """Convert the features into the environment observations.""" - - -class _DefaultObservationTypeMapper(ObservationTypeMapper): - """An ObservationTypeMapper that returns the input features specs and dict. - - This `ObservationTypeMapper` maps observations from the more constrained - `Mapping[str, ArrayType]` used in the task layer to the more generic - `tree.Structure[ArrayType]` exposed by the GDM Environment. - """ - - def to_observation_spec( - self, features_spec: Mapping[str, specs.Array] - ) -> gdmr_types.ObservationSpec: - """Returns the features spec, unmodified, as a `gdmr_types.ObservationSpec`.""" - return features_spec - - def to_observations( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> tree.Structure[gdmr_types.ArrayType]: - """Returns the features, unmodified, as a `tree.Structure`.""" - return features - - -class DefaultObservationSpaceAdapter( - observation_space_adapter.ObservationSpaceAdapter -): - """Observation adapter supporting filtering, renaming and type conversion. - - This adapter supports filtering, renaming, and converting REAF features into - environment observations. - - The order of operations is the following: - 1) Filtering, i.e. feature selection. - 2) Downcasting floats to max_float_dtype. - 3) Renaming. - 4) Type conversion. - - Please refer to the constructor documentation for more information. - """ - - def __init__( - self, - *, - task_features_spec: Mapping[str, specs.Array], - selected_features: Iterable[str] | None, - renamed_features: Iterable[RenameInfo] | None, - observation_type_mapper: ObservationTypeMapper | None, - max_float_dtype: npt.DTypeLike = np.float64, - ): - """Initializes the observation space adapter. - - Args: - task_features_spec: The spec of all the features exposed by the task - layer. - selected_features: The features that will be exposed as observations. If - None, all features will be exposed, i.e. no filtering. - renamed_features: `RenameInfo` objects specifying which features should be - renamed and the corresponding new name. If empty or None, no renaming - will occur. - observation_type_mapper: An `ObservationTypeMapper` specifying how to - convert the task layer features data type (i.e. a Mapping[str, - ArrayType]) into the more generic type exposed by the GDM Environment - (i.e. a tree.Structure[ArrayType]). If None, an instance of - `_DefaultObservationTypeMapper` is used which converts the task logic - layer features dictionary to the more generic type (i.e. - `tree.Structure[ArrayType])` exposed by the environment. - max_float_dtype: The maximum float dtype to use for downcasting floats. - """ - if not np.issubdtype(max_float_dtype, np.floating): - raise ValueError( - 'max_float_dtype must be a floating point dtype. Got' - f' {max_float_dtype}' - ) - self._max_float_dtype = max_float_dtype - self._max_bits = np.finfo(self._max_float_dtype).bits - self._task_features_spec = task_features_spec - self._selected_filter = selected_features - self._renamed_features = renamed_features or () - self._observation_type_mapper = ( - observation_type_mapper or _DefaultObservationTypeMapper() - ) - self._check_specs_consistency() - # Compute the observation spec only once. - self._observation_spec = self._compute_observation_spec() - - def _check_specs_consistency(self) -> None: - # Check that filter keys are present in the spec. - if self._selected_filter is not None: - all_features = self._task_features_spec.keys() - features = set() - for feature in self._selected_filter: - if feature not in all_features: - raise ValueError(f'Feature {feature} is not present in the spec.') - features.add(feature) - else: - # No filter applied. Select all features. - features = set(self._task_features_spec.keys()) - - # Check renaming. - for rename_info in self._renamed_features: - if rename_info.original_key not in features: - raise ValueError( - f'Feature {rename_info.original_key} is not present in the spec.' - ) - - def observations_from_features( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> tree.Structure[gdmr_types.ArrayType]: - """Converts the features into the final environment observations.""" - # 1. Filter the observations. - if (selected_features := self._selected_filter) is None: - # No filter. Expose all observations. - filtered_features = dict(features) - else: - filtered_features = { - k: v for k, v in features.items() if k in selected_features # pytype: disable=unsupported-operands - } - - # 2. Downcast floats to max_float_dtype. - filtered_features = { - k: self._downcast_if_necessary(v) for k, v in filtered_features.items() - } - - # 3. Rename. - for rename_info in self._renamed_features: - # Rename the feature. - value = filtered_features[rename_info.original_key] - del filtered_features[rename_info.original_key] - filtered_features[rename_info.renamed_key] = value - - # 4. Convert type. - return self._observation_type_mapper.to_observations(filtered_features) - - def _compute_observation_spec(self) -> gdmr_types.ObservationSpec: - """Computes the observation spec.""" - # 1. Filter the specs - if (features_to_filter := self._selected_filter) is None: - # The observation spec corresponds to the task features spec. - filtered_specs = dict(self._task_features_spec) - else: - filtered_specs = { - k: v - for k, v in self._task_features_spec.items() - if k in features_to_filter # pytype: disable=unsupported-operands - } - - # 2. Downcast floats to max_float_dtype. - for k, v in filtered_specs.items(): - if self._dtype_needs_downcast(v.dtype): - filtered_specs[k] = v.replace(dtype=self._max_float_dtype) - - # 3. Rename. - for rename_info in self._renamed_features: - # Rename the feature. - value = filtered_specs[rename_info.original_key] - del filtered_specs[rename_info.original_key] - filtered_specs[rename_info.renamed_key] = value - - # 4. Convert the type. - return self._observation_type_mapper.to_observation_spec(filtered_specs) - - def observation_spec(self) -> gdmr_types.ObservationSpec: - """Returns the observation spec.""" - return self._observation_spec - - def task_features_keys(self) -> set[str]: - """Returns the task features keys that will be converted by this adapter.""" - return set(self._task_features_spec.keys()) - - def _downcast_if_necessary( - self, value: gdmr_types.ArrayType - ) -> gdmr_types.ArrayType: - if ( - hasattr(value, 'dtype') and self._dtype_needs_downcast(value.dtype) - ) or self._dtype_needs_downcast(type(value)): - return np.asarray(value).astype(self._max_float_dtype) - else: - return value - - def _dtype_needs_downcast(self, dtype: npt.DTypeLike) -> bool: - return ( - np.issubdtype(dtype, np.floating) - and np.finfo(dtype).bits > self._max_bits - ) diff --git a/src/experimental/reaf/core/device.py b/src/experimental/reaf/core/device.py deleted file mode 100644 index cc53a0ca..00000000 --- a/src/experimental/reaf/core/device.py +++ /dev/null @@ -1,54 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""REAF basic device to interface with the robotic setup.""" - -import abc -from collections.abc import Mapping -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types - - -class Device(abc.ABC): - """REAF basic device to interface with the robotic setup. - - A device defines a single piece in the robotic setup. It should be - hermetic, that is, not depending on other Devices. The coordination of the - devices is responsibility of the DeviceCoordinator. - - Important: a Device should return the commands and measurements specs - immediately after initialisation without the need for any explicit - initialisation, nor for resource acquisition (e.g. connecting to the - hardware). - """ - - @property - @abc.abstractmethod - def name(self) -> str: - """Returns the name of this device.""" - - @abc.abstractmethod - def commands_spec(self) -> Mapping[str, gdmr_types.AnyArraySpec]: - """Returns the commands specs for this device.""" - - @abc.abstractmethod - def measurements_spec(self) -> Mapping[str, specs.Array]: - """Returns the measurements specs for this device.""" - - @abc.abstractmethod - def set_commands(self, commands: Mapping[str, gdmr_types.ArrayType]) -> None: - """Sets the commands for this device.""" - - @abc.abstractmethod - def get_measurements(self) -> Mapping[str, gdmr_types.ArrayType]: - """Returns the measurements provided by this device.""" diff --git a/src/experimental/reaf/core/device_coordinator.py b/src/experimental/reaf/core/device_coordinator.py deleted file mode 100644 index 233705bb..00000000 --- a/src/experimental/reaf/core/device_coordinator.py +++ /dev/null @@ -1,87 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Coordinates the devices composing a robotic setup.""" - -import abc -from collections.abc import Iterable -from reaf.core import device - - -class DeviceCoordinator(abc.ABC): - """Coordinates the devices composing a robotic setup. - - The `DeviceCoordinator` object is responsible for coordinating all the - devices constituting the robotic setup. Whilst the Device is hermetic, - the coordinator is responsible for passing information from one device to - the other if required. For example in a bimanual setup the coordinator is - charged with passing the position of each robot to the other so we can ensure - proper and safe interaction such as for example collision avoidance. - - The `DeviceCoordinator` can be configurable to enable different - properties on the robotic setup, e.g. adding or not adding a `Device` or - forwarding configuration to each `Device`. - - At the very least, the coordinator must implement `get_devices` - to return all the devices. We also provide `on_begin_stepping` and - `on_end_stepping` methods that will be called before the start of an episode - and after the end of the episode respectively. Note that resource acquisition - and subsequent release is completely up to the implementation. - - Finally, `before_set_commands`/`before_get_measurements` can be implemented to - coordinate devices behaviour before their corresponding functions are - called. - """ - - @property - @abc.abstractmethod - def name(self) -> str: - """Returns the name of the coordinator.""" - - @abc.abstractmethod - def get_devices(self) -> Iterable[device.Device]: - """Returns the devices composing the embodiment.""" - - # Lifecycle methods. - - def on_begin_stepping(self) -> None: - """Prepares the coordinator for having its devices called repeatedly. - - After `on_begin_stepping` the devices returned by `get_devices` will have - their `set_commands` and `get_measurements` called repeatedly until - `on_end_stepping` is called on this coordinator. - """ - - def on_end_stepping(self) -> None: - """Notifies the coordinator that the devices are no longer called. - - After `on_end_stepping` the devices returned by `get_devices` will not have - their `set_commands` and `get_measurements` called anymore until this - coordinator `on_begin_stepping` method is notified again. - """ - - # Step hooks methods. - - def before_set_commands(self) -> None: - """Prepares the coordinator to have its devices set_commands called.""" - - def after_set_commands(self) -> None: - """Notifies the coordinator that its devices got `set_commands` called.""" - - def before_get_measurements(self) -> None: - """Prepares the coordinator to have its devices get_measurements called. - - This method gets called immediately before the devices `get_measurements` - method is called and can be used to customise the devices state given the - whole setup state. - """ diff --git a/src/experimental/reaf/core/discount_provider.py b/src/experimental/reaf/core/discount_provider.py deleted file mode 100644 index df1b22fa..00000000 --- a/src/experimental/reaf/core/discount_provider.py +++ /dev/null @@ -1,61 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Computes the discount.""" - -import abc -from collections.abc import Mapping - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types -from reaf.core import termination_checker -import tree - - -class DiscountProvider(abc.ABC): - """Computes the discount.""" - - @abc.abstractmethod - def name(self) -> str: - """Returns a unique string identifier for this object.""" - - @abc.abstractmethod - def compute_discount( - self, - required_features: Mapping[str, gdmr_types.ArrayType], - termination_state: termination_checker.TerminationResult, - ) -> tree.Structure[gdmr_types.ArrayType]: - """Computes the discount. - - Args: - required_features: Measurements and features computed by the task logic - that are required by this provider, i.e. that have keys specified by - `required_features_keys`. - termination_state: The termination state as computed by the termination - checkers. Returns the discount. - - Returns: - The discount. - """ - - @abc.abstractmethod - def discount_spec(self) -> tree.Structure[specs.Array]: - """Returns the spec of the discount.""" - - @abc.abstractmethod - def required_features_keys(self) -> set[str]: - """Returns the feature keys that are required to compute the discount.""" - - def reset(self) -> None: - """Resets the internal state of the discount provider.""" - ... diff --git a/src/experimental/reaf/core/entity.py b/src/experimental/reaf/core/entity.py deleted file mode 100644 index 68c2e38d..00000000 --- a/src/experimental/reaf/core/entity.py +++ /dev/null @@ -1,65 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Basic REAF-sim protocol to interface with the simulation.""" - -from collections.abc import Mapping -import typing - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types - - -class Entity(typing.Protocol): - """Basic REAF component to interface with the simulation. - - An entity defines a single component in the simulation that consumes substep - commands and outputs substep measurements at every simulation substep. It - should be hermetic, that is, not depending on other Entities. - - Important: an Entity should return the substep commands and substep - measurements specs immediately after initialisation without the need for any - explicit initialisation. - """ - - @property - def name(self) -> str: - """Instance name.""" - - def reset(self): - """Resets the entity.""" - - def substep_commands_spec( - self, - ) -> Mapping[str, specs.Array]: - """Spec for the substep commands.""" - - def substep_measurements_spec( - self, - ) -> Mapping[str, specs.Array]: - """Spec for the substep measurements.""" - - def set_substep_commands( - self, - model: typing.Any, - data: typing.Any, - consumed_substep_commands: Mapping[str, gdmr_types.ArrayType], - ) -> None: - """Sets the substep commands.""" - - def get_substep_measurements( - self, - model: typing.Any, - data: typing.Any, - ) -> Mapping[str, gdmr_types.ArrayType]: - """Returns the substep measurements.""" diff --git a/src/experimental/reaf/core/environment.py b/src/experimental/reaf/core/environment.py deleted file mode 100644 index dca306f0..00000000 --- a/src/experimental/reaf/core/environment.py +++ /dev/null @@ -1,490 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""The Robotics Environment Authoring Framework (REAF) Environment class.""" - -import abc -from collections.abc import Mapping -import enum -from typing import Generic - -from absl import logging -import dm_env -from dm_env import specs -from gdm_robotics.interfaces import environment as gdmr_env -from gdm_robotics.interfaces import types as gdmr_types -import numpy as np -from reaf.core import action_space_adapter as reaf_action_space_adapter -from reaf.core import data_acquisition_and_control_layer as reaf_dacl -from reaf.core import default_observation_space_adapter -from reaf.core import logger as reaf_logger -from reaf.core import observation_space_adapter as reaf_observation_space_adapter -from reaf.core import pass_through_action_space_adapter -from reaf.core import task_logic_layer as reaf_tll -import tree - - -class ActionSpecEnforcementOption(enum.StrEnum): - """Options for action spec enforcement.""" - - CLIP_TO_SPEC = "clip_to_spec" - IGNORE = "ignore" - WARNING = "warning" - RAISE_ERROR = "raise_error" - - -class EnvironmentReset(abc.ABC, Generic[gdmr_env.ResetOptions]): - """Support for general resets adhering to the GDM environment API.""" - - @abc.abstractmethod - def do_reset( - self, - config: gdmr_env.ResetOptions, - ) -> None: - """Resets the environment.""" - - def default_reset_configuration(self) -> gdmr_env.ResetOptions: - """Returns the default reset configuration.""" - return gdmr_env.Options() - - -class EndOfEpisodeHandler: - """Handler called after the last episode step.""" - - def on_end_of_episode_stepping(self, final_timestep: dm_env.TimeStep) -> None: - """Called when the episode has ended stepping. - - This will be called at the end of every episode, after all other triggers - have been resolved. Episodes can end either due to truncation or - termination, i.e. `timestep.step_type` is `StepType.LAST`, or due to an - early call to `Environment.reset()`. To verify whether it has indeed - ended due to truncation or termination, the implementer should test - `timestep.last()`. - - Note that the first reset after environment construction will not trigger - this handler, but it will be triggered before resolving any subsequent - environment resets, either implicit or explicit. - - Args: - final_timestep: The final timestep of the episode that ended stepping. - """ - - -class EnvironmentCloser(abc.ABC): - """Handler called when the environment is closed.""" - - @abc.abstractmethod - def close(self) -> None: - """Releases resources when the environment is closed. - - This method is called automatically when exiting the environment's - context manager (`with` statement). - """ - - -class Environment(gdmr_env.Environment): - """The Robotics Environment Authoring Framework (REAF) Environment class.""" - - def __init__( - self, - *, - data_acquisition_and_control_layer: reaf_dacl.DataAcquisitionAndControlLayer, - task_logic_layer: reaf_tll.TaskLogicLayer, - environment_reset: EnvironmentReset, - action_space_adapter: ( - reaf_action_space_adapter.ActionSpaceAdapter | None - ) = None, - observation_space_adapter: ( - reaf_observation_space_adapter.ObservationSpaceAdapter | None - ) = None, - end_of_episode_handler: EndOfEpisodeHandler | None = None, - environment_closer: EnvironmentCloser | None = None, - action_spec_enforcement_option: ActionSpecEnforcementOption = ActionSpecEnforcementOption.RAISE_ERROR, - ): - """Creates an environment. - - Args: - data_acquisition_and_control_layer: The layer for communicating with the - specific robotic setup. - task_logic_layer: The layer in charge of defining the task. - environment_reset: The `EnvironmentReset` specifying the function to be - called at environment reset and the default environment reset - configuration. - action_space_adapter: Adapter from the agent action space to the flattened - commands accepted by the task layer. If None the - PassThroughActionSpaceAdapter is used, meaning the entirety of the - commands dictionary is exposed to the agent. - observation_space_adapter: Adapter from the computed features to the - observations that are exposed to the agent. If None the - DefaultObservationSpaceAdapter is used, meaning all the features are - exposed to the agent as observations. - end_of_episode_handler: Called at the end of an episode, after the last - step. - environment_closer: Specifies the handler to be called when the - environment is closed. This is called automatically on exit if the - environment is used as a context manager. If None, no action is - performed at close. - action_spec_enforcement_option: How to enforce the action spec. If - `CLIP_TO_SPEC`, the action will be clipped to the spec. If `WARNING`, an - warning logged if the action is outside the spec. If `RAISE_ERROR`, an - error will be raised if the action is outside the spec. If `IGNORE`, - the action will be passed through. Default is `RAISE_ERROR`. - """ - - self._data_acquisition_and_control_layer = ( - data_acquisition_and_control_layer - ) - self._task_logic_layer = task_logic_layer - self._end_of_episode_handler = ( - end_of_episode_handler or EndOfEpisodeHandler() - ) - self._environment_reset = environment_reset - self._environment_closer = environment_closer - self._action_spec_enforcement_option = action_spec_enforcement_option - - # Before assigning the adapters, validate the specs on the task logic layer - # and the DACL. - self._validate_dacl_and_ttl_specs() - - ttl_commands_spec = self._task_logic_layer.commands_spec( - self._data_acquisition_and_control_layer.commands_spec() - ) - ttl_features_spec = self._task_logic_layer.features_spec( - self._data_acquisition_and_control_layer.measurements_spec() - ) - - if action_space_adapter is None: - action_space_adapter = ( - pass_through_action_space_adapter.PassThroughActionSpaceAdapter( - commands_spec=ttl_commands_spec - ) - ) - self._action_space_adapter = action_space_adapter - - if observation_space_adapter is None: - observation_space_adapter = ( - default_observation_space_adapter.DefaultObservationSpaceAdapter( - task_features_spec=ttl_features_spec, - selected_features=None, - renamed_features=None, - observation_type_mapper=None, - ) - ) - self._observation_space_adapter = observation_space_adapter - - # Now we can validate the adapters. - self._validate_adapters_specs() - - self._last_timestep: dm_env.TimeStep | None = None - self._should_finalize_episode = False - self._timestep_spec = gdmr_types.TimeStepSpec( - step_type=gdmr_types.STEP_TYPE_SPEC, - reward=self._task_logic_layer.reward_spec(), - discount=self._task_logic_layer.discount_spec(), - # The observation spec corresponds to the one exposed by the adapter. - observation=self._observation_space_adapter.observation_spec(), - ) - - self._zero_reward, self._zero_discount = tree.map_structure( - _read_only_zeros_like_spec, - (self._timestep_spec.reward, self._timestep_spec.discount), - ) - - def close(self) -> None: - """Frees any resources used by the environment.""" - if self._environment_closer is not None: - self._environment_closer.close() - - def default_reset_options(self) -> gdmr_env.ResetOptions: - return self._environment_reset.default_reset_configuration() - - def reset_with_options( - self, - *, - options: gdmr_env.ResetOptions, - ) -> dm_env.TimeStep: - """Starts a new sequence and returns the first `TimeStep`.""" - if self._should_finalize_episode: - self._finalize_episode() - self._environment_reset.do_reset(options) - self._task_logic_layer.perform_reset() - measurements = self._data_acquisition_and_control_layer.begin_stepping() - features = self._task_logic_layer.compute_all_features(measurements) - observations = self._compute_observations_from_features(features) - - self._last_timestep = self._restart(observation=observations) - # Make sure any early reset after this one triggers `_finalize_episode`. - self._should_finalize_episode = True - return self._last_timestep - - def action_spec(self) -> gdmr_types.ActionSpec: - """Defines the actions that should be provided to `step`.""" - # The action spec corresponds to the one exposed by the adapter. - return self._action_space_adapter.action_spec() - - def timestep_spec(self) -> gdmr_types.TimeStepSpec: - """Returns the spec associated to the returned TimeStep.""" - return self._timestep_spec - - def step(self, action: gdmr_types.ActionType) -> dm_env.TimeStep: - """Updates the environment according to action and returns a `TimeStep`.""" - - action = self._enforce_action_spec(action) - if self._last_timestep is None or self._last_timestep.last(): - return self.reset() - - # Process the action to obtain a command. - commands = self._compute_commands_from_agent_action(action) - commands = self._task_logic_layer.compute_final_commands(commands) - measurements = self._data_acquisition_and_control_layer.step(commands) - - # Compute all the features. - features = self._task_logic_layer.compute_all_features(measurements) - - # Compute the elements of the timestep. - reward = self._task_logic_layer.compute_reward(features) - termination_state = self._task_logic_layer.check_for_termination(features) - discount = self._task_logic_layer.compute_discount( - features, termination_state - ) - - observations = self._compute_observations_from_features(features) - - if termination_state.is_terminated(): - self._last_timestep = self._termination( - reward=reward, observation=observations - ) - elif termination_state.is_truncated(): - self._last_timestep = self._truncation( - reward=reward, observation=observations, discount=discount - ) - else: - self._last_timestep = self._transition( - reward=reward, observation=observations, discount=discount - ) - - if self._last_timestep.last(): - self._finalize_episode() - return self._last_timestep - - def _finalize_episode(self) -> None: - self._data_acquisition_and_control_layer.end_stepping() - # It's crucial to call `end_stepping` on the dacl before invoking the end - # of episode handler. This ensures no further `set_command` or - # `get_measurements` calls are made. In contrast, the end of episode - # handler might interact with devices, requiring them to be informed - # beforehand. - self._end_of_episode_handler.on_end_of_episode_stepping(self._last_timestep) - self._should_finalize_episode = False - - @property - def data_acquisition_and_control_layer( - self, - ) -> reaf_dacl.DataAcquisitionAndControlLayer: - return self._data_acquisition_and_control_layer - - @property - def task_logic_layer(self) -> reaf_tll.TaskLogicLayer: - return self._task_logic_layer - - @property - def environment_reset(self) -> EnvironmentReset: - return self._environment_reset - - @environment_reset.setter - def environment_reset(self, environment_reset: EnvironmentReset) -> None: - self._environment_reset = environment_reset - - def add_logger(self, logger: reaf_logger.Logger) -> None: - self._task_logic_layer.add_logger(logger) - - def remove_logger(self, logger: reaf_logger.Logger) -> None: - self._task_logic_layer.remove_logger(logger) - - def _validate_dacl_and_ttl_specs(self) -> None: - """Validates the specs on the task logic layer.""" - # Validate the spec on the task logic layer. - self._task_logic_layer.validate_spec( - dacl_commands_spec=( - self._data_acquisition_and_control_layer.commands_spec() - ), - dacl_measurements_spec=( - self._data_acquisition_and_control_layer.measurements_spec() - ), - ) - - def _validate_adapters_specs(self) -> None: - # Collect the full commands and features spec and validate them against - # the adapters. - commands_spec = set( - self._task_logic_layer.commands_spec( - self._data_acquisition_and_control_layer.commands_spec() - ).keys() - ) - features_spec = set( - self._task_logic_layer.features_spec( - self._data_acquisition_and_control_layer.measurements_spec() - ) - ) - - # Check the action space adapter. - adapter_keys = self._action_space_adapter.task_commands_keys() - - if adapter_keys != commands_spec: - raise ValueError( - "Mismatch between commands exposed by the action space adapter:" - f" {adapter_keys} and commands spec expected by the task layer:" - f" {commands_spec}." - ) - - # Check the observation spec adapter. - adapter_keys = self._observation_space_adapter.task_features_keys() - if not adapter_keys.issubset(features_spec): - raise ValueError( - "Failed to validate observation space adapter specs. Missing keys:" - f" {adapter_keys - features_spec}" - ) - - def _compute_observations_from_features( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> tree.Structure[gdmr_types.ArrayType]: - return self._observation_space_adapter.observations_from_features(features) - - def _compute_commands_from_agent_action( - self, agent_action: gdmr_types.ActionType - ) -> Mapping[str, gdmr_types.ArrayType]: - return self._action_space_adapter.commands_from_environment_action( - agent_action - ) - - def _restart( - self, - observation: tree.Structure[gdmr_types.ArrayType], - ) -> dm_env.TimeStep: - """Returns a `TimeStep` with `step_type` set to `StepType.FIRST`.""" - return dm_env.TimeStep( - step_type=np.asarray(dm_env.StepType.FIRST, dtype=np.uint8), - observation=observation, - reward=self._zero_reward, - discount=self._zero_discount, - ) - - def _transition( - self, - reward: tree.Structure[gdmr_types.ArrayType], - observation: tree.Structure[gdmr_types.ArrayType], - discount: tree.Structure[gdmr_types.ArrayType], - ) -> dm_env.TimeStep: - """Returns a `TimeStep` with `step_type` set to `StepType.MID`.""" - return dm_env.TimeStep( - step_type=np.asarray(dm_env.StepType.MID, dtype=np.uint8), - observation=observation, - reward=reward, - discount=discount, - ) - - def _termination( - self, - reward: tree.Structure[gdmr_types.ArrayType], - observation: tree.Structure[gdmr_types.ArrayType], - ) -> dm_env.TimeStep: - """Returns a `TimeStep` with `step_type` set to `StepType.LAST`.""" - return dm_env.TimeStep( - step_type=np.asarray(dm_env.StepType.LAST, dtype=np.uint8), - observation=observation, - reward=reward, - discount=self._zero_discount, - ) - - def _truncation( - self, - reward: tree.Structure[gdmr_types.ArrayType], - observation: tree.Structure[gdmr_types.ArrayType], - discount: tree.Structure[gdmr_types.ArrayType], - ) -> dm_env.TimeStep: - """Returns a `TimeStep` with `step_type` set to `StepType.LAST`.""" - return dm_env.TimeStep( - step_type=np.asarray(dm_env.StepType.LAST, dtype=np.uint8), - observation=observation, - reward=reward, - discount=discount, - ) - - def _enforce_action_spec( - self, action: gdmr_types.ActionType - ) -> gdmr_types.ActionType: - """Enforces the action spec.""" - match self._action_spec_enforcement_option: - case ActionSpecEnforcementOption.IGNORE: - pass - case ActionSpecEnforcementOption.CLIP_TO_SPEC: - try: - - def clip_to_spec(a, s): - if isinstance(s, specs.BoundedArray): - return np.clip(a, s.minimum, s.maximum) - return a - - action = tree.map_structure( - clip_to_spec, - action, - self._action_space_adapter.action_spec(), - ) - except ValueError as e: - raise ValueError( - "Failed to clip action to spec. Action:" - f" {action} and spec: {self._action_space_adapter.action_spec()}" - ) from e - case ActionSpecEnforcementOption.WARNING: - - def _validate_without_raising(a, s): - dtype_ok = s.dtype == a.dtype - shape_ok = s.shape == a.shape - minimum_ok = True - maximum_ok = True - if isinstance(s, specs.BoundedArray): - minimum_ok = (s.minimum <= a).all() - maximum_ok = (a <= s.maximum).all() - return dtype_ok and shape_ok and minimum_ok and maximum_ok - - if not all( - tree.flatten( - tree.map_structure( - _validate_without_raising, - action, - self._action_space_adapter.action_spec(), - ) - ) - ): - logging.warning( - "Failed to validate action against spec. Action: %r and spec: %r", - action, - self._action_space_adapter.action_spec(), - ) - case ActionSpecEnforcementOption.RAISE_ERROR: - action = tree.map_structure( - lambda a, spec: spec.validate(a), action, self.action_spec() - ) - case _: - raise ValueError( - "Unknown action spec enforcement option:" - f" {self._action_spec_enforcement_option}" - ) - return action - - -def _read_only_zeros_like_spec(spec: specs.Array) -> np.ndarray: - """Returns a zero array matching the specified spec.""" - arr = np.zeros(shape=spec.shape, dtype=spec.dtype) - arr.flags.writeable = False - return arr diff --git a/src/experimental/reaf/core/features_observer.py b/src/experimental/reaf/core/features_observer.py deleted file mode 100644 index cc87f0d3..00000000 --- a/src/experimental/reaf/core/features_observer.py +++ /dev/null @@ -1,34 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Observe all the produced features and measurements.""" - -import abc -from collections.abc import Mapping - -from gdm_robotics.interfaces import types as gdmr_types - - -class FeaturesObserver(abc.ABC): - """Observe all the produced features and measurements.""" - - @property - @abc.abstractmethod - def name(self) -> str: - """Returns a unique string identifier for this object.""" - - @abc.abstractmethod - def observe_features( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> None: - """Observes all the features and measurements.""" diff --git a/src/experimental/reaf/core/features_producer.py b/src/experimental/reaf/core/features_producer.py deleted file mode 100644 index 8ce44e94..00000000 --- a/src/experimental/reaf/core/features_producer.py +++ /dev/null @@ -1,56 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Produces additional features to be exposed by the task logic layer.""" - -import abc -from collections.abc import Mapping - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types - - -class FeaturesProducer(abc.ABC): - """Produces additional features to be exposed by the task logic layer.""" - - @property - @abc.abstractmethod - def name(self) -> str: - """Returns a unique string identifier for this object.""" - - @abc.abstractmethod - def produce_features( - self, required_features: Mapping[str, gdmr_types.ArrayType] - ) -> Mapping[str, gdmr_types.ArrayType]: - """Produces additional features for the environment. - - Args: - required_features: Measurements and features generated by previous - producers in the processing chain that are required by this processor, - i.e. with keys specified by `required_features_keys`. - - Returns additional features that will be added to the global measurements - and features dictionary. - """ - - @abc.abstractmethod - def produced_features_spec(self) -> Mapping[str, specs.Array]: - """Returns the spec of the features produced by this producer.""" - - @abc.abstractmethod - def required_features_keys(self) -> set[str]: - """Returns the keys that are required to produce the new features.""" - - def reset(self) -> None: - """Resets the internal state of the feature producer.""" - ... diff --git a/src/experimental/reaf/core/logger.py b/src/experimental/reaf/core/logger.py deleted file mode 100644 index 63bb5f33..00000000 --- a/src/experimental/reaf/core/logger.py +++ /dev/null @@ -1,80 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Support logging inside the task logic layer.""" - -import abc -from collections.abc import Mapping - -from gdm_robotics.interfaces import types as gdmr_types - - -class Logger(abc.ABC): - """Support logging inside the task logic layer. - - Lifecycle - For each environment step, these member functions are called in this order: - 1. `record_measurements` is called with raw measurements from the sensors. - 2. `record_features` is called with features derived from the measurements. - 3. `record_commands_processing` is called for each - `CommandsProcessor.process_commands` invocation, tracking the - transformation of commands. - 4. `record_final_commands` is called once with the final commands sent to - the DACL. - - Notes: - An environment is first reset(). This triggers the first two steps above. - See reset_with_options in ./environment.py. - - After reset, step is called repeatedly. - 1. This first triggers steps 3 and 4 (See compute_final_commands in TLL - called from step in ./environment.py) - 2. Features are computed (see compute_all_features in TLL called from - step in ./environment.py), triggering steps 1 and 2. - """ - - @property - @abc.abstractmethod - def name(self) -> str: - """Unique string identifier for this object.""" - - def record_measurements( - self, measurements: Mapping[str, gdmr_types.ArrayType] - ) -> None: - """Called once with all the measurements from the DACL.""" - - def record_features( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> None: - """Called once with all the features computed in the Task Layer.""" - - def record_final_commands( - self, commands: Mapping[str, gdmr_types.ArrayType] - ) -> None: - """Called once with the final commands sent to the DACL.""" - - def record_commands_processing( - self, - name: str, - consumed_commands: Mapping[str, gdmr_types.ArrayType], - produced_commands: Mapping[str, gdmr_types.ArrayType], - ) -> None: - """Called once per call to `process_commands` for each CommandsProcessor. - - Args: - name: Name of the `CommandsProcessor`. - consumed_commands: The commands consumed by the current - `CommandsProcessor`. - produced_commands: The commands produced by the current - `CommandsProcessor`. - """ diff --git a/src/experimental/reaf/core/numpy_mock_assertions.py b/src/experimental/reaf/core/numpy_mock_assertions.py deleted file mode 100644 index 310a918e..00000000 --- a/src/experimental/reaf/core/numpy_mock_assertions.py +++ /dev/null @@ -1,98 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Testing functions for asserting on Mock objects with numpy structures.""" - -from collections.abc import Sequence -from unittest import mock -import numpy as np - - -def assert_called_once_with(mock_obj: mock.Mock, *args, **kwargs) -> None: - if mock_obj.call_count != 1: - raise AssertionError( - f"Expected exactly one call to {mock_obj}, got {mock_obj.call_count}" - ) - - assert_called_with(mock_obj, *args, **kwargs) - - -def assert_called_with(mock_obj: mock.Mock, *args, **kwargs) -> None: - """Asserts that the last call to mock_obj had the specified arguments.""" - if mock_obj.call_args is None: - raise AssertionError( - f"Mock object {mock_obj} not called. Expected one call." - ) - call_args, call_kwargs = mock_obj.call_args - np.testing.assert_equal(call_args, args) - np.testing.assert_equal(call_kwargs, kwargs) - - -def assert_has_calls( - mock_obj: mock.Mock, calls: Sequence[mock._Call], any_order: bool = False -) -> None: - """Asserts that mock_obj has been called with the specified calls.""" - mock_calls = mock_obj.mock_calls - - # Check that there are at least enough calls. - if mock_obj.call_count < len(calls): - raise AssertionError( - f"Expected at least {len(calls)} calls to {mock_obj}, got" - f" {mock_obj.call_count}" - ) - - def _calls_are_equal(actual: mock._Call, expected: mock._Call) -> bool: - _, actual_args, actual_kwargs = actual - _, expected_args, expected_kwargs = expected - # Quickest way to transform the assertion into a comparator. - try: - np.testing.assert_equal(actual_args, expected_args) - np.testing.assert_equal(actual_kwargs, expected_kwargs) - return True - except AssertionError: - return False - - if any_order: - # We just check for the calls to be contained. - for expected_call in calls: - for actual_call in mock_calls: - if _calls_are_equal(actual_call, expected_call): - break - raise AssertionError( - f"Expected call {expected_call} not found in mock calls {mock_calls}." - ) - return - - # We need to check in order, but first find the first call. - starting_index = -1 - first_expected_call = calls[0] - for index, actual_call in enumerate(mock_calls): - if _calls_are_equal(actual_call, first_expected_call): - starting_index = index - break - if starting_index == -1: - raise AssertionError(f"Calls {calls} not found in mock calls {mock_calls}.") - - non_matching_calls = [] - - # We have the first element. Now we need to compare element wise. - for index, expected_call in enumerate(calls): - actual_call = mock_calls[starting_index + index] - if not _calls_are_equal(actual_call, expected_call): - non_matching_calls.append((index, expected_call, actual_call)) - - if non_matching_calls: - raise AssertionError( - f"Calls {calls} do not match mock calls {mock_calls}. Mismatch (index," - f" expected, actual): {non_matching_calls}" - ) diff --git a/src/experimental/reaf/core/observation_space_adapter.py b/src/experimental/reaf/core/observation_space_adapter.py deleted file mode 100644 index ca4a8d86..00000000 --- a/src/experimental/reaf/core/observation_space_adapter.py +++ /dev/null @@ -1,42 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Adapts REAF features into observations exposed by the environment.""" - -import abc -from collections.abc import Mapping -from gdm_robotics.interfaces import types as gdmr_types -import tree - - -class ObservationSpaceAdapter(abc.ABC): - """Adapts REAF features into observations exposed by the environment. - - Implementations of this interface are responsible for converting the features - generated by the REAF task layer logic (i.e. dictionary of tensors) into the - more generic `observation` structure exposed by the environment. - """ - - @abc.abstractmethod - def observations_from_features( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> tree.Structure[gdmr_types.ArrayType]: - """Converts the REAF features into the environment observations.""" - - @abc.abstractmethod - def observation_spec(self) -> gdmr_types.ObservationSpec: - """Returns the observation spec.""" - - @abc.abstractmethod - def task_features_keys(self) -> set[str]: - """Returns the task features keys that will be converted by this adapter.""" diff --git a/src/experimental/reaf/core/pass_through_action_space_adapter.py b/src/experimental/reaf/core/pass_through_action_space_adapter.py deleted file mode 100644 index 16b28a81..00000000 --- a/src/experimental/reaf/core/pass_through_action_space_adapter.py +++ /dev/null @@ -1,55 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Adapter that passes the commands spec through.""" - -from collections.abc import Mapping -from gdm_robotics.interfaces import types as gdmr_types -from reaf.core import action_space_adapter - - -class PassThroughActionSpaceAdapter(action_space_adapter.ActionSpaceAdapter): - """Adapter that passes the commands spec through. - - NB the resulting environment will expose a dictionary as the action spec. - """ - - def __init__(self, commands_spec: Mapping[str, gdmr_types.AnyArraySpec]): - self._commands_spec = commands_spec - - def commands_from_environment_action( - self, environment_action: gdmr_types.ActionType - ) -> Mapping[str, gdmr_types.ArrayType]: - """Returns commands accepted by REAF. - - commands_from_environment_action usually accepts a gdmr_types.ActionType but - since this adapter passes the same action as the commands, it needs to be a - dict type in order to pass it through as a dict. - - Args: - environment_action: The environment action(s) to pass as REAF commands. - """ - if not isinstance(environment_action, dict): - raise ValueError( - 'environment_action must be a dict, but got: ' - f'{type(environment_action)}.' - ) - return environment_action - - def action_spec(self) -> gdmr_types.ActionSpec: - """Returns the action spec exposed by the environment.""" - return self._commands_spec - - def task_commands_keys(self) -> set[str]: - """Returns the keys for the commands exposed to the task layer.""" - return set(self._commands_spec.keys()) diff --git a/src/experimental/reaf/core/reward_provider.py b/src/experimental/reaf/core/reward_provider.py deleted file mode 100644 index 3a8ec655..00000000 --- a/src/experimental/reaf/core/reward_provider.py +++ /dev/null @@ -1,292 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Computes the reward.""" - -import abc -from collections.abc import Mapping -import operator -from typing import Callable, TypeAlias, TypeVar, Union - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types -import numpy as np -import tree - - -RewardValue: TypeAlias = tree.Structure[gdmr_types.ArrayType] -RewardSpec: TypeAlias = tree.Structure[specs.Array] - - -class _RewardProvider(abc.ABC): - """Computes the reward. - - Defines the interface for a reward provider. - - Important: Users should not inherit from this class directly. Instead, use the - RewardProvider class later in this file. - """ - - @abc.abstractmethod - def name(self) -> str: - """Returns a unique string identifier for this object.""" - - @abc.abstractmethod - def compute_reward( - self, required_features: Mapping[str, gdmr_types.ArrayType] - ) -> RewardValue: - """Computes the reward. - - Args: - required_features: Measurements and features computed by the task logic - that are required by this provider, i.e. that have keys specified by - `required_features_keys`. - - Returns the computed reward. - """ - - @abc.abstractmethod - def reward_spec(self) -> RewardSpec: - """Returns the spec of the reward.""" - - @abc.abstractmethod - def required_features_keys(self) -> set[str]: - """Returns the feature keys that are required to compute the reward.""" - - def reset(self) -> None: - """Resets the internal state of the reward provider.""" - ... - - -RewardProviderOrValue: TypeAlias = Union['RewardProvider', RewardValue] - - -S = TypeVar('S') -T = TypeVar('T') -UnaryOperator: TypeAlias = Callable[[S], S] -BinaryOperator: TypeAlias = Callable[[S | T, S | T], S | T] - - -class RewardProvider(_RewardProvider): - """Computes the reward. - - Important: Users should inherit from this class and implement the abstract - methods defined in the interface _RewardProvider. - """ - - def __add__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.add, self, other) - - def __radd__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.add, other, self) - - def __sub__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.sub, self, other) - - def __rsub__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.sub, other, self) - - def __mul__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.mul, self, other) - - def __rmul__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.mul, other, self) - - def __truediv__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.truediv, self, other) - - def __rtruediv__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.truediv, other, self) - - def __floordiv__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.floordiv, self, other) - - def __rfloordiv__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.floordiv, other, self) - - def __pow__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.pow, self, other) - - def __rpow__(self, other: RewardProviderOrValue): - return BinaryOperationRewardProvider(operator.pow, other, self) - - def __getitem__(self, index: slice): - return GetItemOperationRewardProvider(self, index) - - def __neg__(self): - return UnaryOperationRewardProvider(operator.neg, self) - - -class ConstantRewardProvider(RewardProvider): - """A RewardProvider that always returns the same reward.""" - - def __init__(self, reward: RewardValue): - super().__init__() - self._reward = reward - - def name(self) -> str: - return str(self._reward) - - def compute_reward( - self, required_features: Mapping[str, gdmr_types.ArrayType] - ) -> RewardValue: - return self._reward - - def reward_spec(self) -> RewardSpec: - return tree.map_structure( - lambda v: specs.Array(v.shape, v.dtype), self._reward - ) - - def required_features_keys(self) -> set[str]: - return set() - - -class BinaryOperationRewardProvider(RewardProvider): - """Applies a binary operator to the result of two reward providers.""" - - def __init__( - self, - op: BinaryOperator, - first_reward_provider: RewardProviderOrValue, - second_reward_provider: RewardProviderOrValue, - ): - super().__init__() - if not isinstance(first_reward_provider, RewardProvider): - first_reward_provider = ConstantRewardProvider(first_reward_provider) - if not isinstance(second_reward_provider, RewardProvider): - second_reward_provider = ConstantRewardProvider(second_reward_provider) - first_spec = first_reward_provider.reward_spec() - second_spec = second_reward_provider.reward_spec() - tree.assert_same_structure(first_spec, second_spec) - assert all( - tree.flatten( - tree.map_structure( - lambda s1, s2: s1.shape == s2.shape and s1.dtype == s2.dtype, - first_spec, - second_spec, - ) - ) - ) - self._op = op - self._first_reward_provider = first_reward_provider - self._second_reward_provider = second_reward_provider - self._reward_spec = first_reward_provider.reward_spec() - self._first_required_features_keys = ( - first_reward_provider.required_features_keys() - ) - self._second_required_features_keys = ( - second_reward_provider.required_features_keys() - ) - - def name(self) -> str: - op_name = getattr(self._op, '__name__', str(self._op)) - return ( - f'{op_name}({self._first_reward_provider.name()},' - f' {self._second_reward_provider.name()})' - ) - - def compute_reward( - self, required_features: Mapping[str, gdmr_types.ArrayType] - ) -> RewardValue: - first_required_features = { - k: v - for k, v in required_features.items() - if k in self._first_required_features_keys - } - second_required_features = { - k: v - for k, v in required_features.items() - if k in self._second_required_features_keys - } - return tree.map_structure( - self._op, - self._first_reward_provider.compute_reward(first_required_features), - self._second_reward_provider.compute_reward(second_required_features), - ) - - def reward_spec(self) -> RewardSpec: - return self._reward_spec - - def required_features_keys(self) -> set[str]: - return ( - self._first_required_features_keys | self._second_required_features_keys - ) - - def reset(self) -> None: - self._first_reward_provider.reset() - self._second_reward_provider.reset() - - -class GetItemOperationRewardProvider(RewardProvider): - """Extracts a slice from the result of a reward provider.""" - - def __init__(self, reward_provider: RewardProviderOrValue, index: slice): - super().__init__() - if not isinstance(reward_provider, RewardProvider): - reward_provider = ConstantRewardProvider(reward_provider) - self._reward_provider = reward_provider - self._index = index - - def name(self) -> str: - return f'{self._reward_provider.name}[{self._index}]' - - def compute_reward( - self, required_features: Mapping[str, gdmr_types.ArrayType] - ) -> RewardValue: - return tree.map_structure( - lambda v: v[self._index], - self._reward_provider.compute_reward(required_features), - ) - - def reward_spec(self) -> RewardSpec: - return tree.map_structure( - lambda s: specs.Array(np.empty(s.shape)[self._index].shape, s.dtype), - self._reward_provider.reward_spec(), - ) - - def required_features_keys(self) -> set[str]: - return self._reward_provider.required_features_keys() - - def reset(self) -> None: - self._reward_provider.reset() - - -class UnaryOperationRewardProvider(RewardProvider): - """Applies a unary operator to the result of a reward provider.""" - - def __init__(self, op: UnaryOperator, reward_provider: RewardProviderOrValue): - super().__init__() - if not isinstance(reward_provider, RewardProvider): - reward_provider = ConstantRewardProvider(reward_provider) - self._op = op - self._reward_provider = reward_provider - - def name(self) -> str: - op_name = getattr(self._op, '__name__', str(self._op)) - return f'{op_name}({self._reward_provider.name()})' - - def compute_reward( - self, required_features: Mapping[str, gdmr_types.ArrayType] - ) -> RewardValue: - return tree.map_structure( - self._op, self._reward_provider.compute_reward(required_features) - ) - - def reward_spec(self) -> RewardSpec: - return self._reward_provider.reward_spec() - - def required_features_keys(self) -> set[str]: - return self._reward_provider.required_features_keys() - - def reset(self) -> None: - self._reward_provider.reset() diff --git a/src/experimental/reaf/core/substep_commands_processor.py b/src/experimental/reaf/core/substep_commands_processor.py deleted file mode 100644 index b63f4388..00000000 --- a/src/experimental/reaf/core/substep_commands_processor.py +++ /dev/null @@ -1,104 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Protocol for substep commands manipulation in REAF-sim.""" - -from collections.abc import Mapping -import typing - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types - - -class SubstepCommandsProcessor(typing.Protocol): - """Processes substep commands, propagating them through a pipeline. - - This processor manipulates substep commands, acting as a node in a pipeline. - It consumes substep commands, performs operations, and produces updated - substep commands for the next stage in the processing chain. - - The processing pipeline starts with commands provided to the SimulationDevice - and progresses towards the substep commands consumed by the individual - entities. Each processor consumes a subset of substep commands and produces - new, potentially transformed, substep commands. The order of operations is - crucial. - - Example Pipeline (conceptual): - - Simulation Device commands --> Processor (1) --> Processor (2) --> Entities - - Specs are propagated starting from the bottom: - 1) In this example assume that the set of entities expect "p3/c1", "p3/c2" and - "p3/c3". - 2) Processor (2) returns ("p3/c1", "p3/c2") from "p2/c1". This means - that the global substep commands spec exposed at this level is "p2/c1" and - the unprocessed "p3/c3". - 3) Processor (1) returns "p2/c1" from ("p1/c1", "p1/c2"). By applying - the same transformation rule, we can obtain the final spec exposed - by the SimulationDevice: "p1/c1", "p1/c2" and "p3/c3". - - ------------------------------------ - | SimulationDevice | - ------------------------------------ - - "p1/c1" "p1/c2" "p3/c3" - | | | - ----------------- | - | P1 | | - ----------------- | - | "p2/c1" | - ----------------- | - | P2 | | - ----------------- | - | "p3/c1" | "p3/c2" | - | | | - ------------------------------------ - | Entities | - ------------------------------------ - """ - - @property - def name(self) -> str: - """Returns a unique string identifier for this object.""" - - def reset(self) -> None: - """Resets the internal state of this processor.""" - - def produced_substep_commands_keys(self) -> set[str]: - """Keys of the substep commands produced by this processor.""" - - def consumed_substep_commands_spec( - self, - ) -> Mapping[str, specs.Array]: - """Spec of the substep commands consumed by this processor.""" - - def process_substep_commands( - self, - model: typing.Any, - data: typing.Any, - consumed_substep_commands: Mapping[str, gdmr_types.ArrayType], - ) -> Mapping[str, gdmr_types.ArrayType]: - """Processes the substep commands and returns a new modified version of it. - - Args: - model: the simulation model. - data: the simulation data. - consumed_substep_commands: the substep commands up in the processing chain - that are required by this processor, i.e. with keys specified by - `consumed_substep_commands_spec`. - - Returns the new substep commands. Note that the (key, value) pairs in - `consumed_substep_commands` are removed from the running substep commands - dictionary. If users want to keep some of the elements it is their - responsibility to retain them in the output dictionary. - """ diff --git a/src/experimental/reaf/core/substep_measurements_processor.py b/src/experimental/reaf/core/substep_measurements_processor.py deleted file mode 100644 index 15dc81df..00000000 --- a/src/experimental/reaf/core/substep_measurements_processor.py +++ /dev/null @@ -1,103 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Protocol for substep measurements manipulation in REAF-sim.""" - -from collections.abc import Mapping -import typing - -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types - - -class SubstepMeasurementsProcessor(typing.Protocol): - """Processes substep measurements, propagating them through a pipeline. - - This processor manipulates substep measurements, acting as a node in a - pipeline. It consumes substep measurements, performs operations, and produces - updated substep measurements for the next stage in the processing chain. - - The processing pipeline starts with substep measurements produced by Entities - and progresses towards the measurements exposed by the SimulationDevice. Each - processor consumes a subset of substep measurements and produces new, - potentially transformed, substep measurements. The order of operations is - crucial. - - Example Pipeline (conceptual): - - Entities --> Processor (1) --> Processor (2) -> Simulation Device Measurements - - Specs are propagated starting from the bottom: - 1) In this example assume that the set of entities produce "p1/c1", "p1/c2" - and "p1/c3". - 2) Processor (1) returns "p2/c1" from ("p1/c1", "p1/c2"). - 3) Processor (2) returns ("p3/c1", "p3/c2") from "p2/c1". - - This resulting spec exposed by the SimulationDevice: "p3/c1", "p3/c2" - and "p1/c3". - - ------------------------------------ - | SimulationDevice | - ------------------------------------ - - "p3/c1" "p3/c2" "p1/c3" - | | | - ----------------- | - | P2 | | - ----------------- | - | "p2/c1" | - ----------------- | - | P1 | | - ----------------- | - | "p1/c1" | "p1/c2" | - | | | - ------------------------------------ - | Entities | - ------------------------------------ - """ - - @property - def name(self) -> str: - """Returns a unique string identifier for this object.""" - - def reset(self): - """Resets the internal state of this processor.""" - - def produced_substep_measurements_spec( - self, - ) -> Mapping[str, specs.Array]: - """Spec of the substep measurements consumed by this processor.""" - - def consumed_substep_measurements_keys(self) -> set[str]: - """Keys of the substep measurements consumed by this processor.""" - - def process_substep_measurements( - self, - model: typing.Any, - data: typing.Any, - consumed_substep_measurements: Mapping[str, gdmr_types.ArrayType], - ) -> Mapping[str, gdmr_types.ArrayType]: - """Processes the substep measurements and returns a new modified version of it. - - Args: - model: the simulation model. - data: the simulation data. - consumed_substep_measurements: the substep measurements up in the - processing chain that are required by this processor, i.e. with keys - specified by `consumed_substep_measurements_spec`. - - Returns the new substep measurements. Note that the (key, value) pairs in - `consumed_substep_measurements` are removed from the running substep - measurements dictionary. If users want to keep some of the elements it is - their responsibility to retain them in the output dictionary. - """ diff --git a/src/experimental/reaf/core/task_logic_layer.py b/src/experimental/reaf/core/task_logic_layer.py deleted file mode 100644 index 51b25639..00000000 --- a/src/experimental/reaf/core/task_logic_layer.py +++ /dev/null @@ -1,342 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Task logic layer for the Robotics Environment Authoring Framework.""" - -from collections.abc import Mapping, Sequence -import itertools -from typing import Protocol - -from absl import logging -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types -from reaf.core import commands_processor as reaf_commands_processor -from reaf.core import default_discount_provider -from reaf.core import discount_provider as reaf_discount_provider -from reaf.core import features_observer as reaf_features_observers -from reaf.core import features_producer as reaf_features_producer -from reaf.core import logger as reaf_logger -from reaf.core import reward_provider as reaf_reward_provider -from reaf.core import termination_checker as reaf_termination_checker -from reaf.core import zero_reward_provider -import tree - - -class _ResettableObject(Protocol): - """Protocol for an object that can be reset.""" - - def reset(self) -> None: - ... - - -class TaskLogicLayer: - """Task logic layer for the Robotics Environment Authoring Framework.""" - - def __init__( - self, - *, - commands_processors: Sequence[reaf_commands_processor.CommandsProcessor], - features_producers: Sequence[reaf_features_producer.FeaturesProducer], - termination_checkers: Sequence[ - reaf_termination_checker.TerminationChecker - ], - reward_provider: reaf_reward_provider.RewardProvider | None = None, - discount_provider: reaf_discount_provider.DiscountProvider | None = None, - features_observers: Sequence[ - reaf_features_observers.FeaturesObserver - ] = (), - loggers: Sequence[reaf_logger.Logger] = (), - ): - """Initializes the task logic layer. - - Args: - commands_processors: `CommandsProcessor`s that modify the commands before - being sent down to the DACL. They are called sequentially, starting from - the commands supplied by the policy and ending with the commands that - will be sent to the DACL. - features_producers: `FeaturesProducer`s that generate new features. - Measurements collected by the DACL and features produced by these - `FeaturesProducer`s are then merged into the final feature set that is - provided to the `reward_provider`, `termination_checkers`, - `discount_provider`, `features_observers`, and `loggers`. - termination_checkers: `TerminationChecker`s that check the episode - termination based on the final feature set. - reward_provider: `RewardProvider` that computes a reward based on the - final feature set. If None, the ZeroRewardProvider is used and the - reward is set to 0. - discount_provider: `DiscountProvider` that compute a discount based on the - final feature set and final termination state. If None, the - DefaultDiscountProvider is used returning 0 for termination and 1 for - truncation and non-termination. - features_observers: `FeaturesObserver`s that get a view over the final - feature set. - loggers: `Logger`s for logging measurements, features, and commands in the - task layer. - """ - self._commands_processors = commands_processors - self._features_producers = features_producers - self._reward_provider = ( - reward_provider - if reward_provider - else zero_reward_provider.ZeroRewardProvider() - ) - self._termination_checkers = termination_checkers - self._discount_provider = ( - discount_provider - if discount_provider - else default_discount_provider.DefaultDiscountProvider() - ) - self._features_observers = features_observers - self._loggers = list(loggers) - - # We make a set of all resettable objects so that these objects only get - # their resets called once. This is important for e.g. when having a single - # object that derives from two interfaces. - self._resettable_objects: list[_ResettableObject] = [] - unique_ids = set() - for resettable_object in itertools.chain( - self._commands_processors, - self._features_producers, - self._termination_checkers, - [self._reward_provider], - [self._discount_provider], - ): - resettable_object_id = id(resettable_object) - if resettable_object_id not in unique_ids: - unique_ids.add(resettable_object_id) - self._resettable_objects.append(resettable_object) - - def validate_spec( - self, - *, - dacl_commands_spec: Mapping[str, gdmr_types.AnyArraySpec], - dacl_measurements_spec: Mapping[str, specs.Array], - ) -> None: - """Checks that the specs have consistent keys.""" - logging.vlog(3, "Validate features processing") - self._validate_features_spec(dacl_measurements_spec) - self._validate_commands_spec(dacl_commands_spec) - - def features_spec( - self, - dacl_measurements_spec: Mapping[str, specs.Array], - ) -> Mapping[str, specs.Array]: - """Returns the features spec as exposed by the task layer.""" - spec = dict(dacl_measurements_spec) - for features_producer in self._features_producers: - spec.update(features_producer.produced_features_spec()) - - return spec - - def commands_spec( - self, dacl_commands_spec: Mapping[str, gdmr_types.AnyArraySpec] - ) -> Mapping[str, gdmr_types.AnyArraySpec]: - """Returns the commands spec exposed by the task layer.""" - # Each processor consumes commands (as described by its - # `consumed_commands_spec`) and outputs a potentially different set of - # commands (as described by its `produced_commands_keys`). - # Starting with the DACL command spec, we iterate in reverse order (i.e. in - # the direction DACL -> Policy) through every processor to remove the - # `produced_commands_keys` from the spec, and add their - # `consumed_commands_spec` to the spec. - spec: Mapping[str, gdmr_types.AnyArraySpec] = dict(dacl_commands_spec) - for processor in reversed(self._commands_processors): - processor_produced_keys = processor.produced_commands_keys() - spec = { - key: value - for key, value in spec.items() - if key not in processor_produced_keys - } - spec.update(processor.consumed_commands_spec()) - return spec - - def reward_spec(self) -> tree.Structure[specs.Array]: - return self._reward_provider.reward_spec() - - def discount_spec(self) -> tree.Structure[specs.Array]: - return self._discount_provider.discount_spec() - - def perform_reset(self) -> None: - """Reset the internal state of the task logic layer.""" - for resettable_object in self._resettable_objects: - resettable_object.reset() - - def compute_all_features( - self, measurements: Mapping[str, gdmr_types.ArrayType] - ) -> Mapping[str, gdmr_types.ArrayType]: - """Computes all the task logic features given the current measurements.""" - for logger in self._loggers: - logger.record_measurements(measurements) - - # Produce all the features. - current_features = dict(measurements) - for feature_producer in self._features_producers: - required_features = { - key: current_features[key] - for key in feature_producer.required_features_keys() - } - current_features.update( - feature_producer.produce_features(required_features) - ) - - # Observe the features. - for feature_observer in self._features_observers: - feature_observer.observe_features(current_features) - - # Log the resulting features. - for logger in self._loggers: - logger.record_features(current_features) - return current_features - - def compute_final_commands( - self, - policy_commands: Mapping[str, gdmr_types.ArrayType], - ) -> Mapping[str, gdmr_types.ArrayType]: - """Processes the policy commands and returns the final processed commands.""" - current_commands = dict(policy_commands) - for processor in self._commands_processors: - # Get commands to be consumed by the processor and remove the commands - # from the current_commands.. They correspond to the - # `consumed_command_spec`. - consumed_commands = { - key: current_commands.pop(key) - for key in processor.consumed_commands_spec().keys() - } - produced_commands = processor.process_commands(consumed_commands) - current_commands.update(produced_commands) - - # Log the modification. - for logger in self._loggers: - logger.record_commands_processing( - processor.name, consumed_commands, produced_commands - ) - - for logger in self._loggers: - logger.record_final_commands(current_commands) - return current_commands - - def compute_reward( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> tree.Structure[gdmr_types.ArrayType]: - """Computes the reward given the features.""" - return self._reward_provider.compute_reward({ - key: features[key] - for key in self._reward_provider.required_features_keys() - }) - - def check_for_termination( - self, features: Mapping[str, gdmr_types.ArrayType] - ) -> reaf_termination_checker.TerminationResult: - """Checks for termination.""" - current_state = reaf_termination_checker.TerminationResult.DO_NOT_TERMINATE - for termination_checker in self._termination_checkers: - current_state = reaf_termination_checker.TerminationResult.combine( - current_state, - termination_checker.check_termination({ - key: features[key] - for key in termination_checker.required_features_keys() - }), - ) - return current_state - - def compute_discount( - self, - features: Mapping[str, gdmr_types.ArrayType], - termination_state: reaf_termination_checker.TerminationResult, - ) -> tree.Structure[gdmr_types.ArrayType]: - """Computes the discount given the features and termination state.""" - return self._discount_provider.compute_discount( - { - key: features[key] - for key in self._discount_provider.required_features_keys() - }, - termination_state, - ) - - def add_logger(self, logger: reaf_logger.Logger) -> None: - self._loggers.append(logger) - - def remove_logger(self, logger: reaf_logger.Logger) -> None: - self._loggers.remove(logger) - - def _validate_features_spec( - self, dacl_measurements_spec: Mapping[str, specs.Array] - ) -> None: - """Validates the features spec.""" - # Check measurements/features path. - current_key_set = set(dacl_measurements_spec.keys()) - logging.vlog(4, "DACL measurements keys: %s", current_key_set) - - for producer in self._features_producers: - logging.vlog( - 4, - "Producer %s requires %s.", - producer.name, - producer.required_features_keys(), - ) - # Check required features are available. - if not producer.required_features_keys().issubset(current_key_set): - raise ValueError( - "Failed to validate feature specs for feature producer" - f" {producer.name}. Missing keys:" - f" {producer.required_features_keys() - current_key_set}" - ) - # Check that there are not duplicates in the output. - if not current_key_set.isdisjoint( - producer.produced_features_spec().keys() - ): - raise ValueError( - "Failed to validate feature specs for feature producer" - f" {producer.name}. Duplicate keys:" - f" {current_key_set & producer.produced_features_spec().keys()}" - ) - # Now extend the spec. - logging.vlog( - 4, - "Update available keys (from producer %s) with %s.", - producer.name, - producer.produced_features_spec().keys(), - ) - current_key_set.update(producer.produced_features_spec().keys()) - logging.vlog(4, "Available features keys %s.", current_key_set) - - def _validate_commands_spec( - self, dacl_commands_spec: Mapping[str, gdmr_types.AnyArraySpec] - ) -> None: - """Validates the commands spec.""" - # Check commands. Starting from the DACL command specs we propagate up in - # the chain. - logging.vlog(3, "Validate commands processing from DACL to Policy.") - current_key_set = set(dacl_commands_spec.keys()) - logging.vlog(4, "DACL commands keys: %s", current_key_set) - - for processor in reversed(self._commands_processors): - produced_command_keys = processor.produced_commands_keys() - - logging.vlog( - 4, - "Processor %s: specs (accepted keys) %s. Exposes %s.", - processor.name, - processor.consumed_commands_spec().keys(), - produced_command_keys, - ) - if not produced_command_keys.issubset(current_key_set): - raise ValueError( - "Failed to validate commands specs for commands processor" - f" {processor.name}. Missing (consumable) keys:" - f" {produced_command_keys - current_key_set}" - ) - # Remove the produced keys and add the consumed commands specs (as the - # processor is mutable). - current_key_set = current_key_set - produced_command_keys - current_key_set.update(processor.consumed_commands_spec().keys()) diff --git a/src/experimental/reaf/core/termination_checker.py b/src/experimental/reaf/core/termination_checker.py deleted file mode 100644 index 754c250d..00000000 --- a/src/experimental/reaf/core/termination_checker.py +++ /dev/null @@ -1,94 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Checks if the episode should terminate.""" - -import abc -from collections.abc import Mapping -import enum -from typing import Self - -from gdm_robotics.interfaces import types as gdmr_types - - -class TerminationResult(enum.IntFlag): - """The result of an episode termination check. - - The TerminationResult refers to the possibility for an episode to terminate. - For more details on the concept of termination we refer the readers to - https://github.com/google-deepmind/dm_env/blob/master/docs/index.md#environment-api-and-semantics. - - Note that this enum does not refer to the possible causes of termination but - only how the termination impacts the learning process. - - The result can be one of the following options: - - DO_NOT_TERMINATE: The episode should not terminate. - - TRUNCATE: The epsisode should terminate. Truncation implies a non-failure - final state. Usually this is associated with a non-zero discount. - - TERMINATE: The episode should terminate as the environment is in some - final state. Usually this is associated with a zero discount for e.g. - finite-horizon RL. - """ - - DO_NOT_TERMINATE = 0 - TRUNCATE = 2**0 - TERMINATE = 2**1 - - def is_terminated(self) -> bool: - return self == TerminationResult.TERMINATE - - def is_truncated(self) -> bool: - return self == TerminationResult.TRUNCATE - - def combine(self, other: Self) -> Self: - # TERMINATE has precedence over TRUNCATE, which in turn has precedence over - # DO_NOT_TERMINATE. Given the definitions above, this can be implemented as - # a maximum operator. To also enable tracing with JAX, we implement this in - # a branchless manner using bitwise operations that preserve the type. - # Note that JAX will trace TerminationResult values as ints. - # Approach: - # - self ^ (self ^ other) == other - # - (-1 * (self < other)) will be bitmask of all 1s iff self < other. - # - AND with (self ^ other) will result in either update or no-op bitmask. - return self ^ ((self ^ other) & (-1 * (self < other))) - - -class TerminationChecker(abc.ABC): - """Checks if the episode should terminate.""" - - @abc.abstractmethod - def name(self) -> str: - """Returns a unique string identifier for this object.""" - - @abc.abstractmethod - def check_termination( - self, required_features: Mapping[str, gdmr_types.ArrayType] - ) -> TerminationResult: - """Checks if the episode should terminate. - - Args: - required_features: Measurements and features computed by the task logic - that are required by this checker, i.e. that have keys specified by - `required_features_keys`. - - Returns if the episode should terminate (and if so, what kind of - termination). - """ - - @abc.abstractmethod - def required_features_keys(self) -> set[str]: - """Returns the feature keys that are required to check the termination.""" - - def reset(self) -> None: - """Resets the internal state of the termination checker.""" - ... diff --git a/src/experimental/reaf/core/trigger.py b/src/experimental/reaf/core/trigger.py deleted file mode 100644 index 20901873..00000000 --- a/src/experimental/reaf/core/trigger.py +++ /dev/null @@ -1,29 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Defines an event-based waiting behaviour.""" - -import abc - - -class Trigger(abc.ABC): - """Defines an event-based waiting behaviour.""" - - @property - @abc.abstractmethod - def name(self) -> str: - """Returns the name of the trigger.""" - - @abc.abstractmethod - def wait_for_event(self) -> None: - """Blocks until the next event.""" diff --git a/src/experimental/reaf/core/zero_reward_provider.py b/src/experimental/reaf/core/zero_reward_provider.py deleted file mode 100644 index c5d2267a..00000000 --- a/src/experimental/reaf/core/zero_reward_provider.py +++ /dev/null @@ -1,48 +0,0 @@ -# Copyright 2025 Google LLC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -"""Reward provider which provides a zero reward.""" - -from collections.abc import Mapping -from dm_env import specs -from gdm_robotics.interfaces import types as gdmr_types -import numpy as np -from reaf.core import reward_provider -import tree - - -class ZeroRewardProvider(reward_provider.RewardProvider): - """Reward provider which provides a zero reward.""" - - def __init__(self, name: str = 'zero_reward_provider'): - self._name = name - - def name(self) -> str: - return self._name - - def compute_reward( - self, required_features: Mapping[str, gdmr_types.ArrayType] - ) -> tree.Structure[gdmr_types.ArrayType]: - """Returns a zero reward.""" - return np.zeros(1) - - def reward_spec(self) -> tree.Structure[specs.Array]: - """Returns the spec for a constant zero reward.""" - return specs.Array(shape=(1,), dtype=float) - - def required_features_keys(self) -> set[str]: - """Returns empty set. - - There are no feature keys that are required to compute the reward. - """ - return set() From fd5a7004f84cb113b96b90c67573ef629d390bc6 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 14 Apr 2026 03:13:11 -0700 Subject: [PATCH 055/251] Remove Drawable abstraction. Instead, use a set of functions to create/update a Renderable from an mjvGeom. This completely isolates mjvScene/mjvGeom from SceneView, making SceneView a more general purpose class. PiperOrigin-RevId: 899475268 Change-Id: I48a3a45bc8fff3e1f514847c7a1ca8ab3a1ff16e --- src/experimental/filament/CMakeLists.txt | 4 +- src/experimental/filament/filament/drawable.h | 70 ----- .../filament/filament/scene_bridge.cc | 39 +-- .../filament/filament/scene_bridge.h | 4 +- .../{drawable.cc => scene_geom_util.cc} | 249 ++++++++++-------- .../filament/filament/scene_geom_util.h | 36 +++ .../filament/filament/scene_view.cc | 53 ++-- .../filament/filament/scene_view.h | 18 +- 8 files changed, 227 insertions(+), 246 deletions(-) delete mode 100644 src/experimental/filament/filament/drawable.h rename src/experimental/filament/filament/{drawable.cc => scene_geom_util.cc} (72%) create mode 100644 src/experimental/filament/filament/scene_geom_util.h diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index ebd544f2..d1105e06 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -27,8 +27,6 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/builtins.h filament/color_grading_options.cc filament/color_grading_options.h - filament/drawable.cc - filament/drawable.h filament/filament_context.cc filament/filament_context.h filament/filament_platform_factory.cc @@ -56,6 +54,8 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/renderable.h filament/scene_bridge.cc filament/scene_bridge.h + filament/scene_geom_util.cc + filament/scene_geom_util.h filament/scene_view.cc filament/scene_view.h filament/texture.cc diff --git a/src/experimental/filament/filament/drawable.h b/src/experimental/filament/filament/drawable.h deleted file mode 100644 index 29829c78..00000000 --- a/src/experimental/filament/filament/drawable.h +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2025 DeepMind Technologies Limited -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_DRAWABLE_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_DRAWABLE_H_ - -#include -#include -#include -#include -#include "experimental/filament/filament/material.h" -#include "experimental/filament/filament/model_objects.h" -#include "experimental/filament/filament/object_manager.h" -#include "experimental/filament/filament/renderable.h" - -namespace mujoco { - -// Manages the filament Entities and MaterialInstances for a single mjvGeom. -class Drawable { - public: - Drawable(ModelObjects* model_objects, const mjvScene* scene, - const mjvGeom& geom); - ~Drawable() noexcept = default; - - Drawable(const Drawable&) = delete; - Drawable& operator=(const Drawable&) = delete; - - // Updates the transform of the drawable for rendering. - void SetTransform(const mjvGeom& geom); - - // Updates the material parameters of the drawable for rendering. - void UpdateMaterial(const mjModel* model, const mjvGeom& geom, - ModelObjects* model_objs, const float headpos[3], - const mjtByte render_flags[mjNRNDFLAG], - ObjectManager::MaterialType* out_material_type); - - // Returns the transform of the drawable. - const filament::math::mat4& GetTransform() const { return transform_; } - - // Returns the renderable for the drawable. - Renderable& GetRenderable() { return renderable_; } - - // Returns the material for the drawable. - Material& GetMaterial() { return renderable_.GetMaterial(); } - - private: - void AddMesh(ModelObjects* model_objs, int data_id); - void AddGeom(ModelObjects* model_objs, const mjvScene* scene, - const mjvGeom& geom); - void AddHeightField(ModelObjects* model_objs, int hfield_id); - void AddShape(ModelObjects* model_objs, ModelObjects::ShapeType shape_type); - - Renderable renderable_; - filament::math::mat4 transform_; -}; - -} // namespace mujoco - -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_DRAWABLE_H_ diff --git a/src/experimental/filament/filament/scene_bridge.cc b/src/experimental/filament/filament/scene_bridge.cc index acd71d31..8ac95d60 100644 --- a/src/experimental/filament/filament/scene_bridge.cc +++ b/src/experimental/filament/filament/scene_bridge.cc @@ -29,7 +29,6 @@ #include #include #include -#include #include #include #include @@ -38,7 +37,6 @@ #include #include #include "experimental/filament/filament/color_grading_options.h" -#include "experimental/filament/filament/drawable.h" #include "experimental/filament/filament/gui_view.h" #include "experimental/filament/filament/light.h" #include "experimental/filament/filament/material.h" @@ -46,6 +44,8 @@ #include "experimental/filament/filament/model_objects.h" #include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/filament/renderable.h" +#include "experimental/filament/filament/scene_geom_util.h" #include "experimental/filament/filament/scene_view.h" namespace mujoco { @@ -164,10 +164,10 @@ SceneBridge::~SceneBridge() { } lights_.clear(); - for (auto& iter : drawables_) { + for (auto& iter : renderables_) { scene_view_->RemoveFromScene(iter.get()); } - drawables_.clear(); + renderables_.clear(); } void SceneBridge::SetEnvironmentLight(std::string_view filename, @@ -305,16 +305,15 @@ void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { mju_n2f(headpos, hpos, 3); mju_n2f(gazedir, hfwd, 3); - const mjModel* model = model_objects_->GetModel(); const mjvGLCamera gl_camera = mjv_averageCamera(scene->camera, scene->camera + 1); clip_from_world_ = CalculateClipFromWorld(viewport, gl_camera); // Remove all drawables from previous render and prepare new ones. - for (auto& iter : drawables_) { + for (auto& iter : renderables_) { scene_view_->RemoveFromScene(iter.get()); } - drawables_.clear(); + renderables_.clear(); for (int i = 0; i < scene->ngeom; ++i) { const mjvGeom* geom = scene->geoms + i; @@ -324,28 +323,12 @@ void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { } } - auto drawable = - std::make_unique(model_objects_.get(), scene, *geom); - drawable->SetTransform(*geom); + std::unique_ptr renderable = + CreateGeomRenderable(*geom, scene, object_mgr_, model_objects_.get(), + headpos, &fallback_textures_); - ObjectManager::MaterialType material_type = ObjectManager::kNumMaterials; - drawable->UpdateMaterial(model, *geom, model_objects_.get(), headpos, - scene->flags, &material_type); - - Material& material = drawable->GetMaterial(); - material.SetFallbackTextures(&fallback_textures_); - material.SetMaterial( - Material::DrawMode::kNormal, - object_mgr_->GetMaterial(material_type)); - material.SetMaterial( - Material::DrawMode::kDepth, - object_mgr_->GetMaterial(ObjectManager::kUnlitDepth)); - material.SetMaterial( - Material::DrawMode::kSegmentation, - object_mgr_->GetMaterial(ObjectManager::kUnlitSegmentation)); - - scene_view_->AddToScene(drawable.get()); - drawables_.push_back(std::move(drawable)); + scene_view_->AddToScene(renderable.get()); + renderables_.push_back(std::move(renderable)); } bool headlight_enabled = false; diff --git a/src/experimental/filament/filament/scene_bridge.h b/src/experimental/filament/filament/scene_bridge.h index e3d4917d..e6fdd765 100644 --- a/src/experimental/filament/filament/scene_bridge.h +++ b/src/experimental/filament/filament/scene_bridge.h @@ -24,11 +24,11 @@ #include #include #include -#include "experimental/filament/filament/drawable.h" #include "experimental/filament/filament/light.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/model_objects.h" #include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/scene_view.h" namespace mujoco { @@ -72,7 +72,7 @@ class SceneBridge { ObjectManager* object_mgr_ = nullptr; std::unique_ptr model_objects_; std::vector> lights_; - std::vector> drawables_; + std::vector> renderables_; filament::math::mat4 clip_from_world_; int default_shadow_map_size_ = 2048; float default_vsm_blur_width_ = 0.0f; diff --git a/src/experimental/filament/filament/drawable.cc b/src/experimental/filament/filament/scene_geom_util.cc similarity index 72% rename from src/experimental/filament/filament/drawable.cc rename to src/experimental/filament/filament/scene_geom_util.cc index a2bff095..468519f0 100644 --- a/src/experimental/filament/filament/drawable.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/filament/filament/drawable.h" +#include "experimental/filament/filament/scene_geom_util.h" #include #include +#include #include #include @@ -81,80 +82,111 @@ static float GetPlaneTileSize(const mjModel* model, int matid, } static bool IsBehind(const float* headpos, const float* pos, const float* mat) { - return ((headpos[0] - pos[0]) * mat[2] + - (headpos[1] - pos[1]) * mat[5] + - (headpos[2] - pos[2]) * mat[8] < 0.0f); + return ((headpos[0] - pos[0]) * mat[2] + (headpos[1] - pos[1]) * mat[5] + + (headpos[2] - pos[2]) * mat[8] < + 0.0f); } -Drawable::Drawable(ModelObjects* model_objects, const mjvScene* scene, - const mjvGeom& geom) - : renderable_(model_objects->GetEngine()) { - if (geom.category == mjCAT_DECOR) { - renderable_.SetCastShadows(false); - renderable_.SetReceiveShadows(false); +static void AddMesh(Renderable& renderable, ModelObjects* model_objs, + int data_id) { + const Mesh* mesh = model_objs->GetMeshBuffer(data_id); + if (mesh == nullptr) { + mju_error("Unknown mesh %d", data_id); } + renderable.Append(mesh); +} +static void AddGeom(Renderable& renderable, ModelObjects* model_objs, + const mjvScene* scene, const mjvGeom& geom) { + if (geom.type == mjGEOM_FLEX) { + renderable.Append(model_objs->CreateFlexMesh(scene, geom)); + } else if (geom.type == mjGEOM_SKIN) { + renderable.Append(model_objs->CreateSkinMesh(scene, geom)); + } +} + +static void AddHeightField(Renderable& renderable, ModelObjects* model_objs, + int hfield_id) { + const Mesh* mesh = model_objs->GetHeightFieldBuffer(hfield_id); + if (mesh == nullptr) { + mju_error("Unknown height field %d", hfield_id); + } + renderable.Append(mesh); +} + +static void AddShape(Renderable& renderable, ModelObjects* model_objs, + ModelObjects::ShapeType shape_type) { + const Mesh* mesh = model_objs->GetShapeBuffer(shape_type); + if (mesh == nullptr) { + mju_error("Unknown shape %d", shape_type); + } + renderable.Append(mesh); +} + +static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, + const mjvScene* scene, + ModelObjects* model_objects) { switch ((mjtGeom)geom.type) { case mjGEOM_MESH: - AddMesh(model_objects, geom.dataid); + AddMesh(renderable, model_objects, geom.dataid); break; case mjGEOM_HFIELD: - AddHeightField(model_objects, geom.dataid); + AddHeightField(renderable, model_objects, geom.dataid); break; case mjGEOM_PLANE: - AddShape(model_objects, ModelObjects::kPlane); + AddShape(renderable, model_objects, ModelObjects::kPlane); break; case mjGEOM_SPHERE: - AddShape(model_objects, ModelObjects::kSphere); + AddShape(renderable, model_objects, ModelObjects::kSphere); break; case mjGEOM_ELLIPSOID: - AddShape(model_objects, ModelObjects::kSphere); + AddShape(renderable, model_objects, ModelObjects::kSphere); break; case mjGEOM_BOX: - AddShape(model_objects, ModelObjects::kBox); + AddShape(renderable, model_objects, ModelObjects::kBox); break; case mjGEOM_CAPSULE: - AddShape(model_objects, ModelObjects::kTube); - AddShape(model_objects, ModelObjects::kDome); - AddShape(model_objects, ModelObjects::kDome); + AddShape(renderable, model_objects, ModelObjects::kTube); + AddShape(renderable, model_objects, ModelObjects::kDome); + AddShape(renderable, model_objects, ModelObjects::kDome); break; case mjGEOM_CYLINDER: - AddShape(model_objects, ModelObjects::kTube); - AddShape(model_objects, ModelObjects::kDisk); - AddShape(model_objects, ModelObjects::kDisk); + AddShape(renderable, model_objects, ModelObjects::kTube); + AddShape(renderable, model_objects, ModelObjects::kDisk); + AddShape(renderable, model_objects, ModelObjects::kDisk); break; case mjGEOM_ARROW: - AddShape(model_objects, ModelObjects::kTube); - AddShape(model_objects, ModelObjects::kCone); - AddShape(model_objects, ModelObjects::kDisk); + AddShape(renderable, model_objects, ModelObjects::kTube); + AddShape(renderable, model_objects, ModelObjects::kCone); + AddShape(renderable, model_objects, ModelObjects::kDisk); break; case mjGEOM_ARROW1: - AddShape(model_objects, ModelObjects::kTube); - AddShape(model_objects, ModelObjects::kCone); - AddShape(model_objects, ModelObjects::kDisk); - AddShape(model_objects, ModelObjects::kDisk); + AddShape(renderable, model_objects, ModelObjects::kTube); + AddShape(renderable, model_objects, ModelObjects::kCone); + AddShape(renderable, model_objects, ModelObjects::kDisk); + AddShape(renderable, model_objects, ModelObjects::kDisk); break; case mjGEOM_ARROW2: - AddShape(model_objects, ModelObjects::kTube); - AddShape(model_objects, ModelObjects::kCone); - AddShape(model_objects, ModelObjects::kCone); - AddShape(model_objects, ModelObjects::kDisk); - AddShape(model_objects, ModelObjects::kDisk); + AddShape(renderable, model_objects, ModelObjects::kTube); + AddShape(renderable, model_objects, ModelObjects::kCone); + AddShape(renderable, model_objects, ModelObjects::kCone); + AddShape(renderable, model_objects, ModelObjects::kDisk); + AddShape(renderable, model_objects, ModelObjects::kDisk); break; case mjGEOM_LINE: - AddShape(model_objects, ModelObjects::kLine); + AddShape(renderable, model_objects, ModelObjects::kLine); break; case mjGEOM_LINEBOX: - AddShape(model_objects, ModelObjects::kLineBox); + AddShape(renderable, model_objects, ModelObjects::kLineBox); break; case mjGEOM_TRIANGLE: - AddShape(model_objects, ModelObjects::kTriangle); + AddShape(renderable, model_objects, ModelObjects::kTriangle); break; case mjGEOM_FLEX: - AddGeom(model_objects, scene, geom); + AddGeom(renderable, model_objects, scene, geom); break; case mjGEOM_SKIN: - AddGeom(model_objects, scene, geom); + AddGeom(renderable, model_objects, scene, geom); break; case mjGEOM_NONE: case mjGEOM_LABEL: @@ -167,56 +199,22 @@ Drawable::Drawable(ModelObjects* model_objects, const mjvScene* scene, } } -void Drawable::AddMesh(ModelObjects* model_objs, int data_id) { - const Mesh* mesh = model_objs->GetMeshBuffer(data_id); - if (mesh == nullptr) { - mju_error("Unknown mesh %d", data_id); - } - renderable_.Append(mesh); -} - -void Drawable::AddGeom(ModelObjects* model_objs, const mjvScene* scene, - const mjvGeom& geom) { - if (geom.type == mjGEOM_FLEX) { - renderable_.Append(model_objs->CreateFlexMesh(scene, geom)); - } else if (geom.type == mjGEOM_SKIN) { - renderable_.Append(model_objs->CreateSkinMesh(scene, geom)); - } -} - -void Drawable::AddHeightField(ModelObjects* model_objs, int hfield_id) { - const Mesh* mesh = model_objs->GetHeightFieldBuffer(hfield_id); - if (mesh == nullptr) { - mju_error("Unknown height field %d", hfield_id); - } - renderable_.Append(mesh); -} - -void Drawable::AddShape(ModelObjects* model_objs, - ModelObjects::ShapeType shape_type) { - const Mesh* mesh = model_objs->GetShapeBuffer(shape_type); - if (mesh == nullptr) { - mju_error("Unknown shape %d", shape_type); - } - renderable_.Append(mesh); -} - -void Drawable::SetTransform(const mjvGeom& geom) { +static void SetGeomTransform(Renderable& renderable, const mjvGeom& geom) { // Flex and skin geometries are in global space. if (geom.type == mjGEOM_FLEX || geom.type == mjGEOM_SKIN) { return; } - transform_ = mat4(ReadMat3(geom.mat), ReadFloat3(geom.pos)); + mat4 transform = mat4(ReadMat3(geom.mat), ReadFloat3(geom.pos)); float3 size = ReadFloat3(geom.size); filament::TransformManager& tm = - renderable_.GetEngine()->getTransformManager(); - for (int j = 0; j < renderable_.GetNumEntities(); ++j) { - const utils::Entity& entity = renderable_[j]; + renderable.GetEngine()->getTransformManager(); + for (int j = 0; j < renderable.GetNumEntities(); ++j) { + const utils::Entity& entity = renderable[j]; // Update object transform. - mat4 entity_transform = transform_; + mat4 entity_transform = transform; // Some built-in drawables are composed of multiple entities. For example, // capsules are a combination of a open tube and two dome end caps. @@ -318,27 +316,34 @@ void Drawable::SetTransform(const mjvGeom& geom) { } } -void Drawable::UpdateMaterial(const mjModel* model, const mjvGeom& geom, - ModelObjects* model_objs, const float headpos[3], - const mjtByte render_flags[mjNRNDFLAG], - ObjectManager::MaterialType* out_material_type) { - const bool use_segid_color = render_flags[mjRND_IDCOLOR]; - const bool enable_reflection = render_flags[mjRND_REFLECTION]; +static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, + const mjvScene* scene, ModelObjects* model_objs, + ObjectManager* object_mgr, + const float headpos[3]) { + const mjModel* model = model_objs->GetModel(); + Material& material = renderable.GetMaterial(); + + const bool use_segid_color = scene->flags[mjRND_IDCOLOR]; + const bool enable_reflection = scene->flags[mjRND_REFLECTION]; Material::Params params; params.color = ReadFloat4(geom.rgba); if (geom.type == mjGEOM_PLANE) { if (IsBehind(headpos, geom.pos, geom.mat)) { params.color[3] *= 0.3; - renderable_.SetReceiveShadows(false); + renderable.SetReceiveShadows(false); params.reflective = false; } else { - renderable_.SetReceiveShadows(true); + renderable.SetReceiveShadows(true); params.reflective = enable_reflection && geom.reflectance > 0 && params.color.a == 1.0f; } } - renderable_.SetWireframe(render_flags[mjRND_WIREFRAME]); + renderable.SetWireframe(scene->flags[mjRND_WIREFRAME]); + if (geom.category == mjCAT_DECOR) { + renderable.SetCastShadows(false); + renderable.SetReceiveShadows(false); + } Material::Textures textures; if (geom.matid >= 0) { @@ -351,25 +356,26 @@ void Drawable::UpdateMaterial(const mjModel* model, const mjvGeom& geom, model_objs->GetTexture(geom.matid, mjTEXROLE_ROUGHNESS); textures.occlusion = model_objs->GetTexture(geom.matid, mjTEXROLE_OCCLUSION); - GetMaterial().UpdateTextures(textures); + material.UpdateTextures(textures); } + ObjectManager::MaterialType material_type = ObjectManager::kNumMaterials; if (geom.type == mjGEOM_LINE || geom.type == mjGEOM_LINEBOX) { - *out_material_type = ObjectManager::kUnlitLine; + material_type = ObjectManager::kUnlitLine; } else { bool material_assigned = false; if (geom.matid >= 0) { material_assigned = true; if (textures.orm) { - *out_material_type = ObjectManager::kPbrPacked; + material_type = ObjectManager::kPbrPacked; } else if (textures.metallic) { - *out_material_type = ObjectManager::kPbr; + material_type = ObjectManager::kPbr; } else if (textures.roughness) { - *out_material_type = ObjectManager::kPbr; + material_type = ObjectManager::kPbr; } else if (model->mat_metallic[geom.matid] >= 0) { - *out_material_type = ObjectManager::kPbr; + material_type = ObjectManager::kPbr; } else if (model->mat_roughness[geom.matid] >= 0) { - *out_material_type = ObjectManager::kPbr; + material_type = ObjectManager::kPbr; } else { material_assigned = false; } @@ -388,36 +394,36 @@ void Drawable::UpdateMaterial(const mjModel* model, const mjvGeom& geom, if (textures.color == nullptr) { if (params.color.a < 1.0f) { - *out_material_type = ObjectManager::kPhongColorFade; + material_type = ObjectManager::kPhongColorFade; } else if (params.reflective) { - *out_material_type = ObjectManager::kPhongColorReflect; + material_type = ObjectManager::kPhongColorReflect; } else { - *out_material_type = ObjectManager::kPhongColor; + material_type = ObjectManager::kPhongColor; } } else if (textures.color->GetFilamentTexture()->getTarget() == - filament::Texture::Sampler::SAMPLER_CUBEMAP) { + filament::Texture::Sampler::SAMPLER_CUBEMAP) { if (params.color.a < 1.0f) { - *out_material_type = ObjectManager::kPhongCubeFade; + material_type = ObjectManager::kPhongCubeFade; } else if (params.reflective) { - *out_material_type = ObjectManager::kPhongCubeReflect; + material_type = ObjectManager::kPhongCubeReflect; } else { - *out_material_type = ObjectManager::kPhongCube; + material_type = ObjectManager::kPhongCube; } } else if (has_texcoords) { if (params.color.a < 1.0f) { - *out_material_type = ObjectManager::kPhong2dUvFade; + material_type = ObjectManager::kPhong2dUvFade; } else if (params.reflective) { - *out_material_type = ObjectManager::kPhong2dUvReflect; + material_type = ObjectManager::kPhong2dUvReflect; } else { - *out_material_type = ObjectManager::kPhong2dUv; + material_type = ObjectManager::kPhong2dUv; } } else { if (params.color.a < 1.0f) { - *out_material_type = ObjectManager::kPhong2dFade; + material_type = ObjectManager::kPhong2dFade; } else if (params.reflective) { - *out_material_type = ObjectManager::kPhong2dReflect; + material_type = ObjectManager::kPhong2dReflect; } else { - *out_material_type = ObjectManager::kPhong2d; + material_type = ObjectManager::kPhong2d; } } } @@ -522,7 +528,32 @@ void Drawable::UpdateMaterial(const mjModel* model, const mjvGeom& geom, params.emissive *= model_objs->GetEmissiveMultiplier(); params.specular *= model_objs->GetSpecularMultiplier(); params.glossiness *= model_objs->GetShininessMultiplier(); + material.UpdateParams(params); - GetMaterial().UpdateParams(params); + material.SetMaterial( + Material::DrawMode::kNormal, + object_mgr->GetMaterial(material_type)); + material.SetMaterial( + Material::DrawMode::kDepth, + object_mgr->GetMaterial(ObjectManager::kUnlitDepth)); + material.SetMaterial( + Material::DrawMode::kSegmentation, + object_mgr->GetMaterial(ObjectManager::kUnlitSegmentation)); +} + +std::unique_ptr CreateGeomRenderable( + const mjvGeom& geom, const mjvScene* scene, ObjectManager* object_mgr, + ModelObjects* model_objs, const float headpos[3], + Material::Textures* fallback_textures) { + auto renderable = std::make_unique(model_objs->GetEngine()); + + // The order of these calls is important. e.g. We need to create the filament + // renderable entities before we can set their transform. + PrepareGeomMeshes(*renderable, geom, scene, model_objs); + SetGeomTransform(*renderable, geom); + renderable->GetMaterial().SetFallbackTextures(fallback_textures); + UpdateGeomMaterial(*renderable, geom, scene, model_objs, object_mgr, headpos); + + return renderable; } } // namespace mujoco diff --git a/src/experimental/filament/filament/scene_geom_util.h b/src/experimental/filament/filament/scene_geom_util.h new file mode 100644 index 00000000..20311cc2 --- /dev/null +++ b/src/experimental/filament/filament/scene_geom_util.h @@ -0,0 +1,36 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_SCENE_GEOM_UTIL_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_SCENE_GEOM_UTIL_H_ + +#include + +#include +#include "experimental/filament/filament/material.h" +#include "experimental/filament/filament/model_objects.h" +#include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/filament/renderable.h" + +namespace mujoco { + +// Creates a Renderable from the given mjvGeom. +std::unique_ptr CreateGeomRenderable( + const mjvGeom& geom, const mjvScene* scene, ObjectManager* object_mgr, + ModelObjects* model_objs, const float headpos[3], + Material::Textures* fallback_textures); + +} // namespace mujoco + +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_SCENE_GEOM_UTIL_H_ diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 9ea46dcb..e8c20c30 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -40,7 +40,6 @@ #include #include #include "experimental/filament/filament/color_grading_options.h" -#include "experimental/filament/filament/drawable.h" #include "experimental/filament/filament/light.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/math_util.h" @@ -162,11 +161,11 @@ SceneView::~SceneView() { for (auto& light : lights_) { light->RemoveFromScene(scene_); } - for (auto& drawable : drawables_) { - drawable->GetRenderable().RemoveFromScene(scene_); + for (auto& renderable : renderables_) { + renderable->RemoveFromScene(scene_); } lights_.clear(); - drawables_.clear(); + renderables_.clear(); reflect_targets_.clear(); engine_->destroyCameraComponent(reflect_camera_->getEntity()); engine_->destroy(reflect_view_); @@ -192,22 +191,22 @@ void SceneView::RemoveFromScene(Light* light) { } } -void SceneView::AddToScene(Drawable* drawable) { - if (drawables_.insert(drawable).second) { - drawable->GetRenderable().AddToScene(scene_); - if (drawable->GetMaterial().GetParams().reflective) { - AddReflectiveDrawable(drawable); +void SceneView::AddToScene(Renderable* renderable) { + if (renderables_.insert(renderable).second) { + renderable->AddToScene(scene_); + if (renderable->GetMaterial().GetParams().reflective) { + AddReflectiveRenderable(renderable); } } } -void SceneView::RemoveFromScene(Drawable* drawable) { - if (drawables_.erase(drawable)) { - auto it = std::find(reflectives_.begin(), reflectives_.end(), drawable); +void SceneView::RemoveFromScene(Renderable* renderable) { + if (renderables_.erase(renderable)) { + auto it = std::find(reflectives_.begin(), reflectives_.end(), renderable); if (it != reflectives_.end()) { reflectives_.erase(it); } - drawable->GetRenderable().RemoveFromScene(scene_); + renderable->RemoveFromScene(scene_); } } @@ -246,11 +245,9 @@ void SceneView::Render(filament::Renderer* renderer, SetupCamera(request.camera, viewport, camera_); - for (auto& iter : drawables_) { + for (auto& iter : renderables_) { Material& material = iter->GetMaterial(); - Renderable& renderable = iter->GetRenderable(); - renderable.SetMaterialInstance( - material.GetMaterialInstance(request.draw_mode)); + iter->SetMaterialInstance(material.GetMaterialInstance(request.draw_mode)); } filament::View* view = views_[static_cast(request.draw_mode)]; @@ -266,13 +263,17 @@ void SceneView::Render(filament::Renderer* renderer, // Render reflection passes. if (request.draw_mode == DrawMode::kNormal) { + filament::TransformManager& tm = engine_->getTransformManager(); for (size_t i = 0; i < reflectives_.size(); ++i) { - Drawable* drawable = reflectives_[i]; + Renderable* renderable = reflectives_[i]; - SetupReflectionCamera(drawable->GetTransform(), camera_, reflect_camera_); + // We assume the 0th entity is the reflective entity. + const utils::Entity entity = (*renderable)[0]; + const mat4 transform(tm.getTransform(tm.getInstance(entity))); + SetupReflectionCamera(transform, camera_, reflect_camera_); // Hide reflective surface from its own reflection pass. - drawable->GetRenderable().SetLayerMask(0x00); + renderable->SetLayerMask(0x00); // Render the reflection to its render target. reflect_view_->setRenderTarget( @@ -280,7 +281,7 @@ void SceneView::Render(filament::Renderer* renderer, renderer->render(reflect_view_); // Unhide the reflective surface. - drawable->GetRenderable().SetLayerMask(0x01); + renderable->SetLayerMask(0x01); } } @@ -293,24 +294,24 @@ void SceneView::Render(filament::Renderer* renderer, } } -void SceneView::AddReflectiveDrawable(Drawable* drawable) { +void SceneView::AddReflectiveRenderable(Renderable* renderable) { const int index = reflectives_.size(); - reflectives_.push_back(drawable); + reflectives_.push_back(renderable); // Ensure we have the same number of render targets as we do reflective - // drawables. + // renderables. while (reflect_targets_.size() < reflectives_.size()) { reflect_targets_.push_back(std::make_unique( engine_, RenderTargetTextureType::kReflectionColor, RenderTargetTextureType::kDepth)); } - // Prepare a render target for the reflective drawable. + // Prepare a render target for the reflective renderable. auto viewport = reflect_view_->getViewport(); auto& target = reflect_targets_[index]; target->Prepare(viewport.width, viewport.height); - Material& material = drawable->GetMaterial(); + Material& material = renderable->GetMaterial(); Material::Textures textures = material.GetTextures(); textures.reflection = target->GetColorTexture(); material.UpdateTextures(textures); diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index a7d7c3aa..fff29dd7 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -27,9 +27,9 @@ #include #include #include "experimental/filament/filament/color_grading_options.h" -#include "experimental/filament/filament/drawable.h" #include "experimental/filament/filament/light.h" #include "experimental/filament/filament/material.h" +#include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/render_target.h" namespace mujoco { @@ -47,8 +47,8 @@ class SceneView { // Adds/removes entities from the scene. void AddToScene(Light* light); void RemoveFromScene(Light* light); - void AddToScene(Drawable* drawable); - void RemoveFromScene(Drawable* drawable); + void AddToScene(Renderable* renderable); + void RemoveFromScene(Renderable* renderable); void AddToScene(filament::Skybox* skybox); void RemoveFromScene(filament::Skybox* skybox); void AddToScene(filament::IndirectLight* indirect_light); @@ -85,9 +85,9 @@ class SceneView { SceneView& operator=(const SceneView&) = delete; private: - // Marks a drawable as reflective. Reflective drawables have to be rendered - // in their own passes to create the reflective texture. - void AddReflectiveDrawable(Drawable* drawable); + // Marks a renderable as reflective. Reflective renderables have to be + // rendered in their own passes to create the reflective texture. + void AddReflectiveRenderable(Renderable* renderable); filament::Engine* engine_ = nullptr; filament::Scene* scene_ = nullptr; @@ -99,7 +99,7 @@ class SceneView { // Scene objects. std::unordered_set lights_; - std::unordered_set drawables_; + std::unordered_set renderables_; filament::Skybox* skybox_ = nullptr; filament::IndirectLight* indirect_light_ = nullptr; @@ -107,8 +107,8 @@ class SceneView { filament::View* reflect_view_ = nullptr; filament::Camera* reflect_camera_ = nullptr; - // The list of reflective drawables and their corresponding render targets. - std::vector reflectives_; + // The list of reflective renderables and their corresponding render targets. + std::vector reflectives_; std::vector> reflect_targets_; }; } // namespace mujoco From aef0589442098a7cfe95d2826ae39cfc087aa08e Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 14 Apr 2026 03:48:41 -0700 Subject: [PATCH 056/251] Allow Light objects to be image based lights. PiperOrigin-RevId: 899488765 Change-Id: I3b56fc6499404bb011f9361c03dba23b736ad4bc --- src/experimental/filament/filament/light.cc | 120 +++++++++++++----- src/experimental/filament/filament/light.h | 12 +- .../filament/filament/object_manager.cc | 35 +---- .../filament/filament/object_manager.h | 7 +- .../filament/filament/scene_bridge.cc | 99 ++++++++++----- .../filament/filament/scene_bridge.h | 1 + .../filament/filament/scene_view.cc | 13 -- .../filament/filament/scene_view.h | 3 - 8 files changed, 165 insertions(+), 125 deletions(-) diff --git a/src/experimental/filament/filament/light.cc b/src/experimental/filament/filament/light.cc index cbc68092..d00f6097 100644 --- a/src/experimental/filament/filament/light.cc +++ b/src/experimental/filament/filament/light.cc @@ -17,30 +17,50 @@ #include #include +#include #include #include +#include #include #include #include #include +#include "experimental/filament/filament/texture.h" namespace mujoco { +using filament::math::float3; +using filament::math::mat3f; + Light::Light(filament::Engine* engine, const Params& params) : engine_(engine), params_(params) { + // Filament treats image-based lights (IBLs) as separate objects (i.e. + // filament::IndirectLight) and so we need to handle IBLs specially. + if (params.type == mjLIGHT_IMAGE) { + filament::IndirectLight::Builder builder; + if (params.texture) { + // Allow null textures for fallback lights. + builder.reflections(params.texture->GetFilamentTexture()); + const Texture::SphericalHarmonics* spherical_harmonics = + params.texture->GetSphericalHarmonics(); + if (spherical_harmonics != nullptr) { + builder.irradiance(3, *spherical_harmonics); + } + } + builder.intensity(params.intensity); + // Rotate the light to match mujoco's Z-up convention. + builder.rotation(mat3f::rotation(std::numbers::pi / 2, float3{1, 0, 0})); + ibl_ = builder.build(*engine_); + return; + } + filament::LightManager::Type type; switch (params.type) { case mjLIGHT_SPOT: type = filament::LightManager::Type::FOCUSED_SPOT; break; case mjLIGHT_DIRECTIONAL: - // We break with the spec here slightly and use a spot light for the head - // light instead of a directional params. This is because filament only - // supports a single directional light, and we'd rather allow a scene - // light to be that directional params. It's also a bit odd for a - // directional light to move with the camera. - type = params.headlight ? filament::LightManager::Type::FOCUSED_SPOT - : filament::LightManager::Type::DIRECTIONAL; + type = filament::LightManager::Type::DIRECTIONAL; break; case mjLIGHT_POINT: type = filament::LightManager::Type::POINT; @@ -55,12 +75,8 @@ Light::Light(filament::Engine* engine, const Params& params) builder.intensityCandela(params.intensity); builder.castShadows(params.castshadow); if (type == filament::LightManager::Type::FOCUSED_SPOT) { - if (params.headlight) { - builder.spotLightCone(0, std::numbers::pi / 2.0f); - } else { - builder.spotLightCone(0, - params.spot_cone_angle * std::numbers::pi / 180.0f); - } + builder.spotLightCone(0, + params.spot_cone_angle * std::numbers::pi / 180.0f); } if (type != filament::LightManager::Type::DIRECTIONAL) { builder.falloff(params.range); @@ -86,52 +102,86 @@ Light::Light(filament::Engine* engine, const Params& params) } Light::~Light() noexcept { - utils::EntityManager& em = utils::EntityManager::get(); - if (!entity_.isNull()) { - engine_->destroy(entity_); - em.destroy(entity_); + if (ibl_) { + engine_->destroy(ibl_); + } else { + utils::EntityManager& em = utils::EntityManager::get(); + if (!entity_.isNull()) { + engine_->destroy(entity_); + em.destroy(entity_); + } } } -void Light::AddToScene(filament::Scene* scene) { scene->addEntity(entity_); } +void Light::AddToScene(filament::Scene* scene) { + if (ibl_) { + scene->setIndirectLight(ibl_); + } else { + scene->addEntity(entity_); + } +} -void Light::RemoveFromScene(filament::Scene* scene) { scene->remove(entity_); } +void Light::RemoveFromScene(filament::Scene* scene) { + if (ibl_) { + scene->setIndirectLight(nullptr); + } else { + scene->remove(entity_); + } +} void Light::SetTransform(filament::math::float3 position, filament::math::float3 direction) { - filament::LightManager& lm = engine_->getLightManager(); - const filament::LightManager::Instance li = lm.getInstance(entity_); - lm.setPosition(li, position); - lm.setDirection(li, direction); + if (!ibl_) { + filament::LightManager& lm = engine_->getLightManager(); + const filament::LightManager::Instance li = lm.getInstance(entity_); + lm.setPosition(li, position); + lm.setDirection(li, direction); + } } void Light::SetColor(const filament::math::float3& color) { - filament::LightManager& lm = engine_->getLightManager(); - const filament::LightManager::Instance li = lm.getInstance(entity_); - lm.setColor(li, color); + if (!ibl_) { + params_.color = color; + filament::LightManager& lm = engine_->getLightManager(); + const filament::LightManager::Instance li = lm.getInstance(entity_); + lm.setColor(li, color); + } } void Light::SetIntensity(float intensity) { - filament::LightManager& lm = engine_->getLightManager(); - const filament::LightManager::Instance li = lm.getInstance(entity_); - lm.setIntensityCandela(li, intensity); + params_.intensity = intensity; + if (ibl_) { + ibl_->setIntensity(intensity); + } else { + filament::LightManager& lm = engine_->getLightManager(); + const filament::LightManager::Instance li = lm.getInstance(entity_); + lm.setIntensityCandela(li, intensity); + } } void Light::Enable() { if (!enabled_) { enabled_ = true; - filament::LightManager& lm = engine_->getLightManager(); - const filament::LightManager::Instance li = lm.getInstance(entity_); - lm.setLightChannel(li, 0, enabled_); + if (ibl_) { + ibl_->setIntensity(params_.intensity); + } else { + filament::LightManager& lm = engine_->getLightManager(); + const filament::LightManager::Instance li = lm.getInstance(entity_); + lm.setLightChannel(li, 0, enabled_); + } } } void Light::Disable() { if (enabled_) { enabled_ = false; - filament::LightManager& lm = engine_->getLightManager(); - const filament::LightManager::Instance li = lm.getInstance(entity_); - lm.setLightChannel(li, 0, enabled_); + if (ibl_) { + ibl_->setIntensity(0.f); + } else { + filament::LightManager& lm = engine_->getLightManager(); + const filament::LightManager::Instance li = lm.getInstance(entity_); + lm.setLightChannel(li, 0, enabled_); + } } } diff --git a/src/experimental/filament/filament/light.h b/src/experimental/filament/filament/light.h index 858b0ef8..93974972 100644 --- a/src/experimental/filament/filament/light.h +++ b/src/experimental/filament/filament/light.h @@ -20,6 +20,7 @@ #include #include #include +#include "experimental/filament/filament/texture.h" namespace mujoco { @@ -30,6 +31,8 @@ class Light { struct Params { // The type of light (e.g. spot, point, directional, etc.) mjtLightType type; + // The texture to use for image lights. + const Texture* texture = nullptr; // The color of the light. filament::math::float3 color = {0, 0, 0}; // The intensity of the light, in candela. @@ -46,8 +49,6 @@ class Light { int shadow_map_size = 2048; // Blur width for EL VSM. float vsm_blur_width = 0.0f; - // Whether or not the light is a headlight. - bool headlight = false; }; Light(filament::Engine* engine, const Params& params); @@ -72,15 +73,16 @@ class Light { // Sets the intensity of the light in candela. void SetIntensity(float intensity); + // Returns the type of the light. + mjtLightType GetType() const { return params_.type; } + // Enables/disables the light in the scene. void Enable(); void Disable(); - // Returns true if the light is a headlight. - bool IsHeadlight() const { return params_.headlight; } - private: filament::Engine* engine_ = nullptr; + filament::IndirectLight* ibl_ = nullptr; utils::Entity entity_; bool enabled_ = true; Params params_; diff --git a/src/experimental/filament/filament/object_manager.cc b/src/experimental/filament/filament/object_manager.cc index 80679284..03e636ee 100644 --- a/src/experimental/filament/filament/object_manager.cc +++ b/src/experimental/filament/filament/object_manager.cc @@ -127,14 +127,10 @@ ObjectManager::ObjectManager(filament::Engine* engine) fallback_textures_[mjTEXROLE_EMISSIVE] = fallback_black_.get(); fallback_textures_[mjTEXROLE_ORM] = fallback_orm_.get(); - LoadFallbackIndirectLight("ibl.ktx", 1.0f); + LoadFallbackIndirectLight("ibl.ktx"); } ObjectManager::~ObjectManager() { - if (fallback_indirect_light_) { - engine_->destroy(fallback_indirect_light_); - } - fallback_indirect_light_texture_.reset(); for (auto& iter : materials_) { engine_->destroy(iter); } @@ -155,17 +151,12 @@ const Texture* ObjectManager::GetFallbackTexture( return fallback_textures_[role]; } -filament::IndirectLight* ObjectManager::GetFallbackIndirectLight() { - return fallback_indirect_light_; +const Texture* ObjectManager::GetFallbackIndirectLightTexture() { + return fallback_indirect_light_texture_.get(); } -void ObjectManager::LoadFallbackIndirectLight( - std::string_view filename, float intensity) { +void ObjectManager::LoadFallbackIndirectLight(std::string_view filename) { fallback_indirect_light_texture_.reset(); - if (fallback_indirect_light_ != nullptr) { - engine_->destroy(fallback_indirect_light_); - fallback_indirect_light_ = nullptr; - } Asset* asset = new Asset(filename); auto release_asset = +[](void* user_data) { @@ -194,23 +185,5 @@ void ObjectManager::LoadFallbackIndirectLight( payload.user_data = asset; fallback_indirect_light_texture_->Upload(payload); - if (fallback_indirect_light_texture_ == nullptr) { - return; - } - - const Texture::SphericalHarmonics* spherical_harmonics = - fallback_indirect_light_texture_->GetSphericalHarmonics(); - - // Build the indirect light. - filament::IndirectLight::Builder builder; - builder.reflections(fallback_indirect_light_texture_->GetFilamentTexture()); - if (spherical_harmonics) { - builder.irradiance(3, *spherical_harmonics); - } - builder.intensity(intensity); - // Rotate the light to match mujoco's Z-up convention. - builder.rotation(filament::math::mat3f::rotation( - filament::math::f::PI / 2, filament::math::float3{1, 0, 0})); - fallback_indirect_light_ = builder.build(*engine_); } } // namespace mujoco diff --git a/src/experimental/filament/filament/object_manager.h b/src/experimental/filament/filament/object_manager.h index 52320347..83d40e74 100644 --- a/src/experimental/filament/filament/object_manager.h +++ b/src/experimental/filament/filament/object_manager.h @@ -65,10 +65,10 @@ class ObjectManager { const Texture* GetFallbackTexture(mjtTextureRole role) const; // Returns the fallback IndirectLight. - filament::IndirectLight* GetFallbackIndirectLight(); + const Texture* GetFallbackIndirectLightTexture(); // Loads an indirect light from a file, setting it to the fallback. - void LoadFallbackIndirectLight(std::string_view filename, float intensity); + void LoadFallbackIndirectLight(std::string_view filename); ObjectManager(const ObjectManager&) = delete; ObjectManager& operator=(const ObjectManager&) = delete; @@ -81,8 +81,7 @@ class ObjectManager { std::unique_ptr fallback_black_ = nullptr; std::unique_ptr fallback_normal_ = nullptr; std::unique_ptr fallback_orm_ = nullptr; - std::unique_ptr fallback_indirect_light_texture_ = nullptr; - filament::IndirectLight* fallback_indirect_light_ = nullptr; + std::unique_ptr fallback_indirect_light_texture_; }; } // namespace mujoco diff --git a/src/experimental/filament/filament/scene_bridge.cc b/src/experimental/filament/filament/scene_bridge.cc index 8ac95d60..4a9c17bd 100644 --- a/src/experimental/filament/filament/scene_bridge.cc +++ b/src/experimental/filament/filament/scene_bridge.cc @@ -148,13 +148,6 @@ SceneBridge::SceneBridge(ObjectManager* object_mgr, const mjModel* model, fallback_textures_.orm = object_mgr_->GetFallbackTexture(mjTEXROLE_ORM); fallback_textures_.emissive = object_mgr_->GetFallbackTexture(mjTEXROLE_EMISSIVE); fallback_textures_.reflection = object_mgr_->GetFallbackTexture(mjTEXROLE_USER); - - // Create an empty/black indirect light to ensure that the skybox is oriented - // to respect mujoco's Z-up convention. - filament::IndirectLight* empty_ibl = - model_objects_->CreateIndirectLight(-1, 100000); - scene_view_->AddToScene(empty_ibl); - PrepareLights(); } @@ -172,11 +165,26 @@ SceneBridge::~SceneBridge() { void SceneBridge::SetEnvironmentLight(std::string_view filename, float intensity) { - filament::IndirectLight* ibl = nullptr; - scene_view_->AddToScene(ibl); - object_mgr_->LoadFallbackIndirectLight(filename, intensity); - ibl = object_mgr_->GetFallbackIndirectLight(); - scene_view_->AddToScene(ibl); + for (auto& light : lights_) { + if (light->GetType() == mjLIGHT_IMAGE) { + scene_view_->RemoveFromScene(light.get()); + light.reset(); + break; + } + } + if (fallback_ibl_) { + scene_view_->RemoveFromScene(fallback_ibl_.get()); + fallback_ibl_.reset(); + } + + object_mgr_->LoadFallbackIndirectLight(filename); + + Light::Params params; + params.type = mjLIGHT_IMAGE; + params.texture = object_mgr_->GetFallbackIndirectLightTexture(); + params.intensity = intensity; + fallback_ibl_ = std::make_unique(object_mgr_->GetEngine(), params); + scene_view_->AddToScene(fallback_ibl_.get()); } std::optional SceneBridge::ClipFromWorld(const float3& pos) const{ @@ -190,24 +198,21 @@ std::optional SceneBridge::ClipFromWorld(const float3& pos) const{ void SceneBridge::PrepareLights() { filament::Engine* engine = object_mgr_->GetEngine(); const mjModel* model = model_objects_->GetModel(); - filament::Skybox* skybox = model_objects_->CreateSkybox(); - if (skybox) { - scene_view_->AddToScene(skybox); - } + bool has_image_based_light = false; float total_light_intensity = 0.0f; - for (int i = 0; i < model->nlight; ++i) { total_light_intensity += model->light_intensity[i]; if (model->light_type[i] == mjLIGHT_IMAGE) { - auto* indirect_light = model_objects_->CreateIndirectLight( - model->light_texid[i], model->light_intensity[i]); - if (indirect_light) { - scene_view_->AddToScene(indirect_light); - } - // Add an nullptr as a placeholder so that our indices still match. - lights_.emplace_back(nullptr); + Light::Params params; + params.type = mjLIGHT_IMAGE; + params.texture = model_objects_->GetTexture(model->light_texid[i]); + params.intensity = model->light_intensity[i]; + auto light_obj = std::make_unique(engine, params); + scene_view_->AddToScene(light_obj.get()); + lights_.emplace_back(std::move(light_obj)); + has_image_based_light = true; } else { Light::Params params; params.color = ReadFloat3(model->light_diffuse); @@ -236,10 +241,15 @@ void SceneBridge::PrepareLights() { { Light::Params params; params.color = float3(0, 0, 0); - params.headlight = true; - params.type = mjLIGHT_DIRECTIONAL; + // We break with the spec here slightly and use a spot light for the head + // light instead of a directional params. This is because filament only + // supports a single directional light, and we'd rather allow a scene + // light to be that directional params. It's also a bit odd for a + // directional light to move with the camera. + params.type = mjLIGHT_SPOT; params.castshadow = 0; - params.intensity = 0; + params.intensity = 0.0f; + params.spot_cone_angle = 90.0f; auto light_obj = std::make_unique(engine, params); #ifndef __EMSCRIPTEN__ // TODO(b/458045799): Re-enable when lights work on glinux and chromebook. @@ -248,23 +258,44 @@ void SceneBridge::PrepareLights() { lights_.emplace_back(std::move(light_obj)); } + if (!has_image_based_light && total_light_intensity > 0.0f) { + // Create a black indirect light to ensure that the skybox is + // oriented to respect mujoco's Z-up convention. + filament::Engine* engine = object_mgr_->GetEngine(); + Light::Params params; + params.type = mjLIGHT_IMAGE; + params.intensity = 10.0f; + fallback_ibl_ = std::make_unique(engine, params); + scene_view_->AddToScene(fallback_ibl_.get()); + } + // There are no "physical" lights in the scene which means we're likely // dealing with a "classic renderer" scene. In this case, let's add a // default environment light and set the light intensity ourselves. if (total_light_intensity == 0.0f) { - auto* ibl = object_mgr_->GetFallbackIndirectLight(); - if (ibl) { - ibl->setIntensity(fallback_environment_light_intensity_); - scene_view_->AddToScene(ibl); - } + // Create a fallback environment light. + Light::Params params; + params.type = mjLIGHT_IMAGE; + params.texture = object_mgr_->GetFallbackIndirectLightTexture(); + params.intensity = fallback_environment_light_intensity_; + fallback_ibl_ = std::make_unique(engine, params); + scene_view_->AddToScene(fallback_ibl_.get()); + + // Distribute the fallback scene light intensity among the lights. const float intensity = fallback_scene_light_intensity_ / lights_.size(); for (auto& light : lights_) { if (light) { - light->SetIntensity( - light->IsHeadlight() ? fallback_head_light_intensity_ : intensity); + const bool is_headlight = (light == lights_.back()); + light->SetIntensity(is_headlight ? fallback_head_light_intensity_ + : intensity); } } } + + filament::Skybox* skybox = model_objects_->CreateSkybox(); + if (skybox) { + scene_view_->AddToScene(skybox); + } } filament::math::mat4 CalculateClipFromWorld(const mjrRect& viewport, diff --git a/src/experimental/filament/filament/scene_bridge.h b/src/experimental/filament/filament/scene_bridge.h index e6fdd765..8e847953 100644 --- a/src/experimental/filament/filament/scene_bridge.h +++ b/src/experimental/filament/filament/scene_bridge.h @@ -71,6 +71,7 @@ class SceneBridge { SceneView* scene_view_ = nullptr; ObjectManager* object_mgr_ = nullptr; std::unique_ptr model_objects_; + std::unique_ptr fallback_ibl_; std::vector> lights_; std::vector> renderables_; filament::math::mat4 clip_from_world_; diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index e8c20c30..607cab2f 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -20,7 +20,6 @@ #include #include -#include #include #include #include @@ -222,18 +221,6 @@ void SceneView::RemoveFromScene(filament::Skybox* skybox) { } } -void SceneView::AddToScene(filament::IndirectLight* indirect_light) { - indirect_light_ = indirect_light; - scene_->setIndirectLight(indirect_light); -} - -void SceneView::RemoveFromScene(filament::IndirectLight* indirect_light) { - if (indirect_light_ == indirect_light) { - indirect_light_ = nullptr; - scene_->setIndirectLight(nullptr); - } -} - void SceneView::Render(filament::Renderer* renderer, const RenderRequest& request) { filament::Viewport viewport(request.viewport.left, request.viewport.bottom, diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index fff29dd7..d7f02021 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -51,8 +51,6 @@ class SceneView { void RemoveFromScene(Renderable* renderable); void AddToScene(filament::Skybox* skybox); void RemoveFromScene(filament::Skybox* skybox); - void AddToScene(filament::IndirectLight* indirect_light); - void RemoveFromScene(filament::IndirectLight* indirect_light); // Parameters for rendering the scene. using DrawMode = Material::DrawMode; @@ -101,7 +99,6 @@ class SceneView { std::unordered_set lights_; std::unordered_set renderables_; filament::Skybox* skybox_ = nullptr; - filament::IndirectLight* indirect_light_ = nullptr; // Custom view and camera for reflective surfaces. filament::View* reflect_view_ = nullptr; From 72cb2b210da666617924de709406d6aadbe60c71 Mon Sep 17 00:00:00 2001 From: Michael Moss Date: Tue, 14 Apr 2026 04:09:54 -0700 Subject: [PATCH 057/251] Update changelog for the 3.7.0 release. PiperOrigin-RevId: 899497703 Change-Id: Ib9ec75550615dc245bc66af60c877e8adae17e67 --- doc/changelog.rst | 94 +++++++++++++++++++++++------------------------ 1 file changed, 47 insertions(+), 47 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index a0fe7a7f..880cd3a8 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,80 +2,80 @@ Changelog ========= -Upcoming version (not yet released) ------------------------------------ +Version 3.7.0 (April 14, 2026) +------------------------------ General ^^^^^^^ -- Added the :ref:`dcmotor` actuator for modeling DC motors. Supports optional - electrical dynamics (inductance), cogging torque, thermal resistance variation, and LuGre friction. See the - `technical note <_static/dcmotor.pdf>`__ for more details. -- Actuators with joint or tendon transmissions can now contribute - :ref:`damping` and :ref:`armature` to their transmission target. - These are applied during the passive force and inertia computations, respectively, and are scaled by gear\ :sup:`2` - ("reflected" damping/inertia). +1. Added the :ref:`dcmotor` actuator for modeling DC motors. Supports optional + electrical dynamics (inductance), cogging torque, thermal resistance variation, and LuGre friction. See the + `technical note <_static/dcmotor.pdf>`__ for more details. +2. Actuators with joint or tendon transmissions can now contribute + :ref:`damping` and :ref:`armature` to their transmission target. + These are applied during the passive force and inertia computations, respectively, and are scaled by gear\ :sup:`2` + ("reflected" damping/inertia). .. youtube:: aKa3ZlEF9_Y :align: right :width: 35% -- Stiffness in :ref:`joints` and :ref:`tendons` and damping in - :ref:`joints` and :ref:`tendons` now support nonlinear polynomial - :ref:`force profiles`. New ``mjModel`` arrays (``jnt_stiffnesspoly``, ``tendon_stiffnesspoly``, - ``dof_dampingpoly``, ``tendon_dampingpoly``) hold higher-order coefficients. The existing scalar arrays - (``jnt_stiffness``, ``dof_damping``, etc.) continue to hold the linear coefficient and are unchanged. - The polynomial order is defined by the new constant :ref:`mjNPOLY`. A future breaking C-API change - may unify the linear and higher-order coefficients into a single array. -- Added :ref:`midpoint integration` for standalone free bodies in ``implicit`` and ``implicitfast`` - :ref:`integrators`. This applies the implicit midpoint rule to the rotational dynamics of free bodies - with no children, conserving kinetic energy to machine precision in the absence of external torques. The - :ref:`invdiscrete` flag now also disables midpoint integration, providing an opt-out - mechanism. -- Added the centripetal/Coriolis acceleration term :math:`\dot{J}v` to the constraint solver bias for - :ref:`connect` and :ref:`weld` equality constaints. This significantly improves the - stability of constrained mechanisms like four-bar linkages. See :ref:`Dual problem` for details. +3. Stiffness in :ref:`joints` and :ref:`tendons` and damping in + :ref:`joints` and :ref:`tendons` now support nonlinear polynomial + :ref:`force profiles`. New ``mjModel`` arrays (``jnt_stiffnesspoly``, ``tendon_stiffnesspoly``, + ``dof_dampingpoly``, ``tendon_dampingpoly``) hold higher-order coefficients. The existing scalar arrays + (``jnt_stiffness``, ``dof_damping``, etc.) continue to hold the linear coefficient and are unchanged. + The polynomial order is defined by the new constant :ref:`mjNPOLY`. A future breaking C-API change + may unify the linear and higher-order coefficients into a single array. +4. Added :ref:`midpoint integration` for standalone free bodies in ``implicit`` and ``implicitfast`` + :ref:`integrators`. This applies the implicit midpoint rule to the rotational dynamics of free bodies + with no children, conserving kinetic energy to machine precision in the absence of external torques. The + :ref:`invdiscrete` flag now also disables midpoint integration, providing an opt-out + mechanism. +5. Added the centripetal/Coriolis acceleration term :math:`\dot{J}v` to the constraint solver bias for + :ref:`connect` and :ref:`weld` equality constaints. This significantly improves the + stability of constrained mechanisms like four-bar linkages. See :ref:`Dual problem` for details. -- Introduced :ref:`mjpEncoder`, the counterpart to :ref:`mjpDecoder` for encoding of :ref:`mjSpec` and :ref:`mjModel` - into :ref:`mjResource`. +6. Introduced :ref:`mjpEncoder`, the counterpart to :ref:`mjpDecoder` for encoding of :ref:`mjSpec` and :ref:`mjModel` + into :ref:`mjResource`. - - Added :ref:`mj_encode`, :ref:`mjp_registerEncoder`, :ref:`mjp_defaultEncoder`, and :ref:`mjp_findEncoder`. +7. Added :ref:`mj_encode`, :ref:`mjp_registerEncoder`, :ref:`mjp_defaultEncoder`, and :ref:`mjp_findEncoder`. .. admonition:: Breaking API changes :class: attention - - The ``mjs`` layer fields ``stiffness`` and ``damping`` in :ref:`mjsJoint` and :ref:`mjsTendon` have - been widened from ``mjtNum`` scalars to ``mjtNum[mjNPOLY+1]`` arrays. The first element is the linear coefficient - (previously the scalar), and subsequent elements are the higher-order :ref:`polynomial` coefficients. + 8. The ``mjs`` layer fields ``stiffness`` and ``damping`` in :ref:`mjsJoint` and :ref:`mjsTendon` have + been widened from ``mjtNum`` scalars to ``mjtNum[mjNPOLY+1]`` arrays. The first element is the linear coefficient + (previously the scalar), and subsequent elements are the higher-order :ref:`polynomial` coefficients. - **Migration:** Replace assignments like ``joint.stiffness = val`` with ``joint.stiffness[0] = val``. - - ``.obj`` and ``.stl`` decoders are now included as source when building MuJoCo with CMake. This fixes the - behaviour from the previous release where it required downstream code to load these plugins explicitly. + **Migration:** Replace assignments like ``joint.stiffness = val`` with ``joint.stiffness[0] = val``. + 9. ``.obj`` and ``.stl`` decoders are now included as source when building MuJoCo with CMake. This fixes the + behaviour from the previous release where it required downstream code to load these plugins explicitly. - - The ``vertcollide`` field in :ref:`mjsFlex` has been removed. It is no longer required since - :doc:`MuJoCo Warp ` supports native flex collisions. + 10. The ``vertcollide`` field in :ref:`mjsFlex` has been removed. It is no longer required since + :doc:`MuJoCo Warp ` supports native flex collisions. - - :ref:`mjPLUGIN_LIB_INIT` macro now requires a name argument to avoid initialization function name collisions. - When building with MSVC, we now use the C runtime initialization section to initialize plugins instead of - ``DllMain``. See :ref:`plugin registration` for more details. + 11. :ref:`mjPLUGIN_LIB_INIT` macro now requires a name argument to avoid initialization function name collisions. + When building with MSVC, we now use the C runtime initialization section to initialize plugins instead of + ``DllMain``. See :ref:`plugin registration` for more details. - - The :ref:`mjtWarning` enum value ``mjWARN_VGEOMFULL`` is removed. Exhaustion of visual geoms is now handled - internally by the :ref:`mjvScene`. - - URDF parsing no longer hardcodes :ref:`strippath` to "true". The setting is now respected and - the default is "false". Setting this is attribute is now the responsibility of the user. + 12. The :ref:`mjtWarning` enum value ``mjWARN_VGEOMFULL`` is removed. Exhaustion of visual geoms is now handled + internally by the :ref:`mjvScene`. + 13. URDF parsing no longer hardcodes :ref:`strippath` to "true". The setting is now respected and + the default is "false". Setting this is attribute is now the responsibility of the user. - **Migration:** Set :ref:`strippath` to "true" in MJCF or programmatically using + **Migration:** Set :ref:`strippath` to "true" in MJCF or programmatically using - .. code-block:: python + .. code-block:: python - spec = mujoco.MjSpec.from_file("path/to/model.urdf") - spec.compiler.strippath = True + spec = mujoco.MjSpec.from_file("path/to/model.urdf") + spec.compiler.strippath = True Bug fixes ^^^^^^^^^ -- The compiler now correctly accounts for negative scaling when loading user specified mesh data. +14. The compiler now correctly accounts for negative scaling when loading user specified mesh data. Version 3.6.0 (March 10, 2026) ------------------------------ From 08bb6e66f15b5fd8f0bb2b010850c94352c9520c Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 14 Apr 2026 06:50:16 -0700 Subject: [PATCH 058/251] Small fixes to decor rendering. Use an unlit material for decorative elements and do not render them in the reflection pass. PiperOrigin-RevId: 899560291 Change-Id: I3b035d556432c850ce8240e0c4c4c2d6645d7bf2 --- src/experimental/filament/CMakeLists.txt | 1 + .../filament/assets/unlit_decor.mat | 29 +++++++++++++++++++ .../filament/filament/object_manager.cc | 1 + .../filament/filament/object_manager.h | 1 + .../filament/filament/renderable.cc | 8 +++-- .../filament/filament/renderable.h | 4 +-- .../filament/filament/scene_geom_util.cc | 3 ++ .../filament/filament/scene_view.cc | 7 +++-- 8 files changed, 48 insertions(+), 6 deletions(-) create mode 100644 src/experimental/filament/assets/unlit_decor.mat diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index d1105e06..7e6dfbd8 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -114,6 +114,7 @@ set(MATERIAL_FILES phong_cube_fade.mat phong_cube.mat phong_cube_reflect.mat + unlit_decor.mat unlit_depth.mat unlit_line.mat unlit_segmentation.mat diff --git a/src/experimental/filament/assets/unlit_decor.mat b/src/experimental/filament/assets/unlit_decor.mat new file mode 100644 index 00000000..73a08e50 --- /dev/null +++ b/src/experimental/filament/assets/unlit_decor.mat @@ -0,0 +1,29 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +material { + name : unlit_decor, + shadingModel : unlit, + culling: none, + parameters : [ + { type : float4, name : BaseColorFactor } + ] +} + +fragment { + void material(inout MaterialInputs material) { + prepareMaterial(material); + material.baseColor = materialParams.BaseColorFactor; + } +} diff --git a/src/experimental/filament/filament/object_manager.cc b/src/experimental/filament/filament/object_manager.cc index 03e636ee..1d7bafa8 100644 --- a/src/experimental/filament/filament/object_manager.cc +++ b/src/experimental/filament/filament/object_manager.cc @@ -84,6 +84,7 @@ ObjectManager::ObjectManager(filament::Engine* engine) materials_[kPhongCubeReflect] = LoadMaterial("phong_cube_reflect.filamat"); materials_[kUnlitSegmentation] = LoadMaterial("unlit_segmentation.filamat"); materials_[kUnlitLine] = LoadMaterial("unlit_line.filamat"); + materials_[kUnlitDecor] = LoadMaterial("unlit_decor.filamat"); materials_[kUnlitDepth] = LoadMaterial("unlit_depth.filamat"); materials_[kUnlitUi] = LoadMaterial("unlit_ui.filamat"); diff --git a/src/experimental/filament/filament/object_manager.h b/src/experimental/filament/filament/object_manager.h index 83d40e74..6502b7b9 100644 --- a/src/experimental/filament/filament/object_manager.h +++ b/src/experimental/filament/filament/object_manager.h @@ -49,6 +49,7 @@ class ObjectManager { kPhongCubeFade, kPhongCubeReflect, kUnlitSegmentation, + kUnlitDecor, kUnlitDepth, kUnlitLine, kUnlitUi, diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index e25739b1..32fd66d9 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -183,7 +183,8 @@ void Renderable::SetMaterialInstance(filament::MaterialInstance* instance) { } } -void Renderable::SetLayerMask(std::uint8_t mask) { +std::uint8_t Renderable::SetLayerMask(std::uint8_t mask) { + std::uint8_t prev = layer_mask_; if (mask != layer_mask_) { layer_mask_ = mask; @@ -192,9 +193,11 @@ void Renderable::SetLayerMask(std::uint8_t mask) { rm.setLayerMask(rm.getInstance(entity), 0xff, layer_mask_); } } + return prev; } -void Renderable::SetPriority(std::uint8_t priority) { +std::uint8_t Renderable::SetPriority(std::uint8_t priority) { + std::uint8_t prev = priority_; if (priority != priority_) { priority_ = priority; @@ -203,6 +206,7 @@ void Renderable::SetPriority(std::uint8_t priority) { rm.setPriority(rm.getInstance(entity), priority_); } } + return prev; } void Renderable::SetCastShadows(bool cast_shadows) { diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 7cdcd762..74394980 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -57,10 +57,10 @@ class Renderable { int GetNumEntities() const { return entities_.size(); } // Hides all managed entities. - void SetLayerMask(std::uint8_t mask); + std::uint8_t SetLayerMask(std::uint8_t mask); // Sets the priority of all managed entities. - void SetPriority(std::uint8_t priority); + std::uint8_t SetPriority(std::uint8_t priority); // Disables the renderables from casting shadows. void SetCastShadows(bool cast_shadows); diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index 468519f0..a62d3749 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -206,6 +206,7 @@ static void SetGeomTransform(Renderable& renderable, const mjvGeom& geom) { } mat4 transform = mat4(ReadMat3(geom.mat), ReadFloat3(geom.pos)); + renderable.SetLayerMask(geom.category); float3 size = ReadFloat3(geom.size); filament::TransformManager& tm = @@ -362,6 +363,8 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, ObjectManager::MaterialType material_type = ObjectManager::kNumMaterials; if (geom.type == mjGEOM_LINE || geom.type == mjGEOM_LINEBOX) { material_type = ObjectManager::kUnlitLine; + } else if (geom.category == mjCAT_DECOR) { + material_type = ObjectManager::kUnlitSegmentation; } else { bool material_assigned = false; if (geom.matid >= 0) { diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 607cab2f..5c8f38b1 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -135,6 +136,7 @@ SceneView::SceneView(filament::Engine* engine) : engine_(engine) { view = engine->createView(); view->setScene(scene_); view->setCamera(camera_); + view->setVisibleLayers(0xff, mjCAT_ALL); } reflect_view_ = engine->createView(); @@ -142,6 +144,7 @@ SceneView::SceneView(filament::Engine* engine) : engine_(engine) { reflect_view_->setCamera(reflect_camera_); reflect_view_->setShadowingEnabled(false); reflect_view_->setPostProcessingEnabled(false); + reflect_view_->setVisibleLayers(0xff, mjCAT_DYNAMIC | mjCAT_STATIC); // Disable post processing for the depth and segmentation views to preserve // the values. @@ -260,7 +263,7 @@ void SceneView::Render(filament::Renderer* renderer, SetupReflectionCamera(transform, camera_, reflect_camera_); // Hide reflective surface from its own reflection pass. - renderable->SetLayerMask(0x00); + std::uint8_t previous_layer_mask = renderable->SetLayerMask(0x00); // Render the reflection to its render target. reflect_view_->setRenderTarget( @@ -268,7 +271,7 @@ void SceneView::Render(filament::Renderer* renderer, renderer->render(reflect_view_); // Unhide the reflective surface. - renderable->SetLayerMask(0x01); + renderable->SetLayerMask(previous_layer_mask); } } From 57241585934cd646c8f99eca400577dc57c791aa Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 14 Apr 2026 09:38:17 -0700 Subject: [PATCH 059/251] Refactor flex stiffness storage to support variable sizes. This change introduces `nflexstiffness` and `flex_stiffnessadr` to allow flex stiffness matrices to have sizes other than the fixed 21 per element. Higher-order flexes can now store larger stiffness matrices based on their number of nodes. The `flex_stiffnessadr` array provides the starting index for each flex's stiffness data within the `flex_stiffness` array. PiperOrigin-RevId: 899632263 Change-Id: Ie49182c46c3777acf0c6b492345edfcf62fb5e44 --- doc/includes/references.h | 4 ++- include/mujoco/mjmodel.h | 4 ++- include/mujoco/mjxmacro.h | 4 ++- python/mujoco/introspect/structs.py | 15 +++++++++- src/engine/engine_derivative.c | 2 +- src/engine/engine_io.c | 42 +++++++++++++--------------- src/engine/engine_io.h | 2 +- src/engine/engine_passive.c | 2 +- src/user/user_mesh.cc | 13 +++++---- src/user/user_model.cc | 36 ++++++++++++++++++------ src/user/user_model.h | 1 + test/user/user_flex_test.cc | 8 ++++-- unity/Runtime/Bindings/MjBindings.cs | 2 ++ wasm/codegen/generated/bindings.cc | 13 ++++++++- 14 files changed, 100 insertions(+), 48 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index bea7f4d9..7f23a52d 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1040,6 +1040,7 @@ struct mjModel_ { mjtSize nflexedge; // number of edges in all flexes mjtSize nflexelem; // number of elements in all flexes mjtSize nflexelemdata; // number of element vertex ids in all flexes + mjtSize nflexstiffness; // number of stiffness parameters in all flexes mjtSize nflexelemedge; // number of element edge ids in all flexes mjtSize nflexshelldata; // number of shell fragment vertex ids in all flexes mjtSize nflexevpair; // number of element-vertex pairs in all flexes @@ -1323,6 +1324,7 @@ struct mjModel_ { int* flex_elemadr; // first element address (nflex x 1) int* flex_elemnum; // number of elements (nflex x 1) int* flex_elemdataadr; // first element vertex id address (nflex x 1) + int* flex_stiffnessadr; // stiffness matrix address (nflex x 1) int* flex_elemedgeadr; // first element edge id address (nflex x 1) int* flex_shellnum; // number of shells (nflex x 1) int* flex_shelldataadr; // first shell data address (nflex x 1) @@ -1351,7 +1353,7 @@ struct mjModel_ { mjtNum* flexedge_invweight0; // edge inv. weight in qpos0 (nflexedge x 1) mjtNum* flex_radius; // radius around primitive element (nflex x 1) mjtNum* flex_size; // vertex bounding box half sizes in qpos0 (nflex x 3) - mjtNum* flex_stiffness; // finite element stiffness matrix (nflexelem x 21) + mjtNum* flex_stiffness; // finite element stiffness matrix (nflexstiffness x 1) mjtNum* flex_bending; // bending stiffness (nflexedge x 17) mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1) mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 49cfee0b..8a45f67b 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -703,6 +703,7 @@ struct mjModel_ { mjtSize nflexedge; // number of edges in all flexes mjtSize nflexelem; // number of elements in all flexes mjtSize nflexelemdata; // number of element vertex ids in all flexes + mjtSize nflexstiffness; // number of stiffness parameters in all flexes mjtSize nflexelemedge; // number of element edge ids in all flexes mjtSize nflexshelldata; // number of shell fragment vertex ids in all flexes mjtSize nflexevpair; // number of element-vertex pairs in all flexes @@ -986,6 +987,7 @@ struct mjModel_ { int* flex_elemadr; // first element address (nflex x 1) int* flex_elemnum; // number of elements (nflex x 1) int* flex_elemdataadr; // first element vertex id address (nflex x 1) + int* flex_stiffnessadr; // stiffness matrix address (nflex x 1) int* flex_elemedgeadr; // first element edge id address (nflex x 1) int* flex_shellnum; // number of shells (nflex x 1) int* flex_shelldataadr; // first shell data address (nflex x 1) @@ -1014,7 +1016,7 @@ struct mjModel_ { mjtNum* flexedge_invweight0; // edge inv. weight in qpos0 (nflexedge x 1) mjtNum* flex_radius; // radius around primitive element (nflex x 1) mjtNum* flex_size; // vertex bounding box half sizes in qpos0 (nflex x 3) - mjtNum* flex_stiffness; // finite element stiffness matrix (nflexelem x 21) + mjtNum* flex_stiffness; // finite element stiffness matrix (nflexstiffness x 1) mjtNum* flex_bending; // bending stiffness (nflexedge x 17) mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1) mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 318e76da..970ba622 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -185,6 +185,7 @@ X( nflexedge ) \ X( nflexelem ) \ X( nflexelemdata ) \ + X( nflexstiffness ) \ X( nflexelemedge ) \ X( nflexshelldata ) \ X( nflexevpair ) \ @@ -462,6 +463,7 @@ X ( int, flex_elemadr, nflex, 1 ) \ X ( int, flex_elemnum, nflex, 1 ) \ X ( int, flex_elemdataadr, nflex, 1 ) \ + X ( int, flex_stiffnessadr, nflex, 1 ) \ X ( int, flex_elemedgeadr, nflex, 1 ) \ X ( int, flex_shellnum, nflex, 1 ) \ X ( int, flex_shelldataadr, nflex, 1 ) \ @@ -490,7 +492,7 @@ X ( mjtNum, flexedge_invweight0, nflexedge, 1 ) \ X ( mjtNum, flex_radius, nflex, 1 ) \ X ( mjtNum, flex_size, nflex, 3 ) \ - X ( mjtNum, flex_stiffness, nflexelem, 21 ) \ + X ( mjtNum, flex_stiffness, nflexstiffness, 1 ) \ X ( mjtNum, flex_bending, nflexedge, 17 ) \ X ( mjtNum, flex_damping, nflex, 1 ) \ X ( mjtNum, flex_edgestiffness, nflex, 1 ) \ diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 57a0476e..4dded269 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -972,6 +972,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='mjtSize'), doc='number of element vertex ids in all flexes', ), + StructFieldDecl( + name='nflexstiffness', + type=ValueType(name='mjtSize'), + doc='number of stiffness parameters in all flexes', + ), StructFieldDecl( name='nflexelemedge', type=ValueType(name='mjtSize'), @@ -2735,6 +2740,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='first element vertex id address', array_extent=('nflex',), ), + StructFieldDecl( + name='flex_stiffnessadr', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='stiffness matrix address', + array_extent=('nflex',), + ), StructFieldDecl( name='flex_elemedgeadr', type=PointerType( @@ -2965,7 +2978,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc='finite element stiffness matrix', - array_extent=('nflexelem', 21), + array_extent=('nflexstiffness',), ), StructFieldDecl( name='flex_bending', diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 23781e1d..5f1058d0 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -899,7 +899,7 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, } // get stiffness and damping - mjtNum* k = m->flex_stiffness + 21*m->flex_elemadr[f]; + mjtNum* k = m->flex_stiffness + m->flex_stiffnessadr[f]; // skip if rigid or no stiffness if (m->flex_rigid[f] || k[0] == 0) { diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 8a8e59b0..33fafe6c 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -205,11 +205,11 @@ void mj_makeModel(mjModel** dest, mjtSize nbvhdynamic, mjtSize noct, mjtSize njnt, mjtSize ntree, mjtSize nM, mjtSize nB, mjtSize nC, mjtSize nD, mjtSize ngeom, mjtSize nsite, mjtSize ncam, mjtSize nlight, mjtSize nflex, mjtSize nflexnode, mjtSize nflexvert, mjtSize nflexedge, mjtSize nflexelem, - mjtSize nflexelemdata, mjtSize nflexelemedge, mjtSize nflexshelldata, mjtSize nflexevpair, - mjtSize nflextexcoord, mjtSize nJfe, mjtSize nJfv, mjtSize nmesh, mjtSize nmeshvert, - mjtSize nmeshnormal, mjtSize nmeshtexcoord, mjtSize nmeshface, mjtSize nmeshgraph, - mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap, mjtSize nskin, - mjtSize nskinvert, mjtSize nskintexvert, mjtSize nskinface, mjtSize nskinbone, + mjtSize nflexelemdata, mjtSize nflexstiffness, mjtSize nflexelemedge, mjtSize nflexshelldata, + mjtSize nflexevpair, mjtSize nflextexcoord, mjtSize nJfe, mjtSize nJfv, mjtSize nmesh, + mjtSize nmeshvert, mjtSize nmeshnormal, mjtSize nmeshtexcoord, mjtSize nmeshface, + mjtSize nmeshgraph, mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap, + mjtSize nskin, mjtSize nskinvert, mjtSize nskintexvert, mjtSize nskinface, mjtSize nskinbone, mjtSize nskinbonevert, mjtSize nhfield, mjtSize nhfielddata, mjtSize ntex, mjtSize ntexdata, mjtSize nmat, mjtSize npair, mjtSize nexclude, mjtSize neq, mjtSize ntendon, mjtSize nJten, mjtSize nwrap, mjtSize nsensor, mjtSize nnumeric, mjtSize nnumericdata, mjtSize ntext, @@ -293,6 +293,7 @@ void mj_makeModel(mjModel** dest, m->nflexedge = nflexedge; m->nflexelem = nflexelem; m->nflexelemdata = nflexelemdata; + m->nflexstiffness = nflexstiffness; m->nflexelemedge = nflexelemedge; m->nflexshelldata = nflexshelldata; m->nflexevpair = nflexevpair; @@ -400,22 +401,19 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) { // allocate new model if needed if (!dest) { mj_makeModel( - &dest, src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh, - src->nbvhstatic, src->nbvhdynamic, src->noct, src->njnt, src->ntree, - src->nM, src->nB, src->nC, src->nD, src->ngeom, src->nsite, src->ncam, - src->nlight, src->nflex, src->nflexnode, src->nflexvert, src->nflexedge, - src->nflexelem, src->nflexelemdata, src->nflexelemedge, src->nflexshelldata, - src->nflexevpair, src->nflextexcoord, src->nJfe, src->nJfv, src->nmesh, - src->nmeshvert, src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, - src->nmeshgraph, src->nmeshpoly, src->nmeshpolyvert, src->nmeshpolymap, - src->nskin, src->nskinvert, src->nskintexvert, src->nskinface, - src->nskinbone, src->nskinbonevert, src->nhfield, src->nhfielddata, - src->ntex, src->ntexdata, src->nmat, src->npair, src->nexclude, - src->neq, src->ntendon, src->nJten, src->nwrap, src->nsensor, - src->nnumeric, src->nnumericdata, src->ntext, src->ntextdata, - src->ntuple, src->ntupledata, src->nkey, src->nmocap, src->nplugin, - src->npluginattr, src->nuser_body, src->nuser_jnt, src->nuser_geom, - src->nuser_site, src->nuser_cam, src->nuser_tendon, src->nuser_actuator, + &dest, src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh, src->nbvhstatic, + src->nbvhdynamic, src->noct, src->njnt, src->ntree, src->nM, src->nB, src->nC, src->nD, + src->ngeom, src->nsite, src->ncam, src->nlight, src->nflex, src->nflexnode, src->nflexvert, + src->nflexedge, src->nflexelem, src->nflexelemdata, src->nflexstiffness, + src->nflexelemedge, src->nflexshelldata, src->nflexevpair, src->nflextexcoord, src->nJfe, + src->nJfv, src->nmesh, src->nmeshvert, src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, + src->nmeshgraph, src->nmeshpoly, src->nmeshpolyvert, src->nmeshpolymap, src->nskin, + src->nskinvert, src->nskintexvert, src->nskinface, src->nskinbone, src->nskinbonevert, + src->nhfield, src->nhfielddata, src->ntex, src->ntexdata, src->nmat, src->npair, + src->nexclude, src->neq, src->ntendon, src->nJten, src->nwrap, src->nsensor, src->nnumeric, + src->nnumericdata, src->ntext, src->ntextdata, src->ntuple, src->ntupledata, src->nkey, + src->nmocap, src->nplugin, src->npluginattr, src->nuser_body, src->nuser_jnt, + src->nuser_geom, src->nuser_site, src->nuser_cam, src->nuser_tendon, src->nuser_actuator, src->nuser_sensor, src->nnames, src->npaths); } if (!dest) { @@ -599,7 +597,7 @@ mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) { sizes[56], sizes[57], sizes[58], sizes[59], sizes[60], sizes[61], sizes[62], sizes[63], sizes[64], sizes[65], sizes[66], sizes[67], sizes[68], sizes[69], sizes[70], sizes[71], sizes[72], sizes[73], sizes[74], sizes[75], sizes[76], - sizes[77]); + sizes[77], sizes[78]); // mj_makeModel may fail if the input buffer has invalid sizes if (!m) { diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index 0af1058f..1aedaf86 100644 --- a/src/engine/engine_io.h +++ b/src/engine/engine_io.h @@ -52,7 +52,7 @@ void mj_makeModel(mjModel** dest, mjtSize nbvhdynamic, mjtSize noct, mjtSize njnt, mjtSize ntree, mjtSize nM, mjtSize nB, mjtSize nC, mjtSize nD, mjtSize ngeom, mjtSize nsite, mjtSize ncam, mjtSize nlight, mjtSize nflex, mjtSize nflexnode, mjtSize nflexvert, mjtSize nflexedge, mjtSize nflexelem, - mjtSize nflexelemdata, mjtSize nflexelemedge, mjtSize nflexshelldata, mjtSize nflexevpair, + mjtSize nflexelemdata, mjtSize nflexstiffness, mjtSize nflexelemedge, mjtSize nflexshelldata, mjtSize nflexevpair, mjtSize nflextexcoord, mjtSize nJfe, mjtSize nJfv, mjtSize nmesh, mjtSize nmeshvert, mjtSize nmeshnormal, mjtSize nmeshtexcoord, mjtSize nmeshface, mjtSize nmeshgraph, mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap, mjtSize nskin, diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index d27f615b..d67f8683 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -201,7 +201,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) { // flex elasticity for (int f=0; f < m->nflex; f++) { - mjtNum* k = m->flex_stiffness + 21*m->flex_elemadr[f]; + mjtNum* k = m->flex_stiffness + m->flex_stiffnessadr[f]; mjtNum* b = m->flex_bending + 17*m->flex_edgeadr[f]; int dim = m->flex_dim[f]; int nodenum = m->flex_nodenum[f]; diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index db852729..dc2e5257 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4271,12 +4271,8 @@ void mjCFlex::Compile(const mjVFS* vfs) { } // linear elasticity - stiffness.assign(21*nelem, 0); - if (interpolated) { - int min_size = ceil(nodexpos.size()*nodexpos.size() / 21); - if (min_size > nelem) { - throw mjCError(this, "Trilinear dofs are require at least %d elements", "", min_size); - } + if (!interpolated) { + stiffness.assign(21 * nelem, 0); } // geometrically nonlinear elasticity @@ -4333,6 +4329,11 @@ void mjCFlex::Compile(const mjVFS* vfs) { } if (!stiffness_cached && young > 0 && interpolated) { + int n = pow(order_ + 1, 3); + int ndof = 3 * n; + if (stiffness.size() < ndof * ndof) { + stiffness.resize(ndof * ndof, 0); + } ComputeLinearStiffness(stiffness, nodexpos.data(), young, poisson, order_); } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 2af21381..f3206d13 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1194,6 +1194,7 @@ void mjCModel::Clear() { nflexedge = 0; nflexelem = 0; nflexelemdata = 0; + nflexstiffness = 0; nflexelemedge = 0; nflexshelldata = 0; nflexevpair = 0; @@ -2177,6 +2178,7 @@ void mjCModel::SetSizes() { } nbvh = nbvhstatic + nbvhdynamic; + int extra_stiffness_size = 0; // flex counts for (int i=0; i < nflex; i++) { nflexnode += flexes_[i]->nnode; @@ -2188,6 +2190,9 @@ void mjCModel::SetSizes() { nflexshelldata += (int)flexes_[i]->shell.size(); nflexevpair += (int)flexes_[i]->evpair.size()/2; nflextexcoord += (flexes_[i]->HasTexcoord() ? flexes_[i]->get_texcoord().size()/2 : 0); + if (flexes_[i]->order_ != 0) { + extra_stiffness_size += (3 * flexes_[i]->nnode) * (3 * flexes_[i]->nnode); + } if (flexes_[i]->interpolated || flexes_[i]->rigid) { continue; } @@ -2237,6 +2242,9 @@ void mjCModel::SetSizes() { } } } + // TODO: This can be compacted further when we update mjwarp to not rely on + // 21*elem_adr for non-interpolated flexes. + nflexstiffness = nflexelem * 21 + extra_stiffness_size; // mesh counts for (int i=0; i < nmesh; i++) { @@ -3435,6 +3443,8 @@ void mjCModel::CopyObjects(mjModel* m) { shelldata_adr = 0; evpair_adr = 0; texcoord_adr = 0; + int standard_stiffness_size = 21 * m->nflexelem; + int current_extra_stiffness_adr = standard_stiffness_size; for (int i=0; i < nflex; i++) { // get pointer mjCFlex* pfl = flexes_[i]; @@ -3457,10 +3467,18 @@ void mjCModel::CopyObjects(mjModel* m) { mjuu_copyvec(m->flex_rgba + 4 * i, pfl->rgba, 4); // elasticity - if (!pfl->stiffness.empty()) { - mjuu_copyvec(m->flex_stiffness + 21 * elem_adr, pfl->stiffness.data(), pfl->stiffness.size()); + if (pfl->order_ == 0) { + m->flex_stiffnessadr[i] = 21 * elem_adr; } else { - mjuu_zerovec(m->flex_stiffness + 21 * elem_adr, 21 * pfl->nelem); + m->flex_stiffnessadr[i] = current_extra_stiffness_adr; + current_extra_stiffness_adr += (3 * pfl->nnode) * (3 * pfl->nnode); + } + + if (!pfl->stiffness.empty()) { + mjuu_copyvec(m->flex_stiffness + m->flex_stiffnessadr[i], pfl->stiffness.data(), pfl->stiffness.size()); + } else { + int size = (pfl->order_ == 0) ? 21 * pfl->nelem : (3 * pfl->nnode) * (3 * pfl->nnode); + mjuu_zerovec(m->flex_stiffness + m->flex_stiffnessadr[i], size); } if (!pfl->bending.empty()) { mjuu_copyvec(m->flex_bending + 17 * edge_adr, pfl->bending.data(), pfl->bending.size()); @@ -5125,12 +5143,12 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { mj_makeModel(&m, nq, nv, nu, na, nbody, nbvh, nbvhstatic, nbvhdynamic, noct, njnt, ntree, nM, nB, nC, nD, ngeom, nsite, ncam, nlight, nflex, nflexnode, nflexvert, nflexedge, nflexelem, - nflexelemdata, nflexelemedge, nflexshelldata, nflexevpair, nflextexcoord, nJfe, nJfv, - nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph, nmeshpoly, - nmeshpolyvert, nmeshpolymap, nskin, nskinvert, nskintexvert, nskinface, nskinbone, - nskinbonevert, nhfield, nhfielddata, ntex, ntexdata, nmat, npair, nexclude, - neq, ntendon, nJten, nwrap, nsensor, nnumeric, nnumericdata, ntext, ntextdata, - ntuple, ntupledata, nkey, nmocap, nplugin, npluginattr, + nflexelemdata, nflexstiffness, nflexelemedge, nflexshelldata, nflexevpair, + nflextexcoord, nJfe, nJfv, nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, + nmeshgraph, nmeshpoly, nmeshpolyvert, nmeshpolymap, nskin, nskinvert, nskintexvert, + nskinface, nskinbone, nskinbonevert, nhfield, nhfielddata, ntex, ntexdata, nmat, + npair, nexclude, neq, ntendon, nJten, nwrap, nsensor, nnumeric, nnumericdata, ntext, + ntextdata, ntuple, ntupledata, nkey, nmocap, nplugin, npluginattr, nuser_body, nuser_jnt, nuser_geom, nuser_site, nuser_cam, nuser_tendon, nuser_actuator, nuser_sensor, nnames, npaths); if (!m) { diff --git a/src/user/user_model.h b/src/user/user_model.h index 90feb1ed..9513b27b 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -96,6 +96,7 @@ class mjCModel_ : public mjsElement { mjtSize nflexedge; // number of edges in all flexes mjtSize nflexelem; // number of elements in all flexes mjtSize nflexelemdata; // number of element vertex ids in all flexes + mjtSize nflexstiffness; // number of stiffness parameters in all flexes mjtSize nflexelemedge; // number of element edges in all flexes mjtSize nflexshelldata; // number of shell fragment vertex ids in all flexes mjtSize nflexevpair; // number of element-vertex pairs in all flexes diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index 017bb320..478b23e0 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -447,7 +447,7 @@ TEST_F(UserFlexTest, StiffnessMatrix) { std::array error; mjModel* m = LoadModelFromString(xml, error.data(), error.size()); ASSERT_THAT(m, NotNull()) << error.data(); - EXPECT_NE(m->flex_stiffness[0], 0); + EXPECT_NE(m->flex_stiffness[m->flex_stiffnessadr[0]], 0); EXPECT_EQ(m->nflexnode, 8); // constants are in the kernel @@ -456,7 +456,8 @@ TEST_F(UserFlexTest, StiffnessMatrix) { zeros[i] = 0; ones[i] = 1; } - mju_mulMatVec(res, m->flex_stiffness, ones, 3*m->nflexnode, 3*m->nflexnode); + mju_mulMatVec(res, m->flex_stiffness + m->flex_stiffnessadr[0], ones, + 3 * m->nflexnode, 3 * m->nflexnode); EXPECT_THAT(res, Pointwise(MjNear(1e-8, 1e-4), zeros)); mj_deleteModel(m); @@ -496,7 +497,8 @@ TEST_F(UserFlexTest, StiffnessCacheDiffersByGeometry) { // Same number of nodes but different stiffness due to different geometry EXPECT_EQ(m_small->nflexnode, m_large->nflexnode); - EXPECT_NE(m_small->flex_stiffness[0], m_large->flex_stiffness[0]); + EXPECT_NE(m_small->flex_stiffness[m_small->flex_stiffnessadr[0]], + m_large->flex_stiffness[m_large->flex_stiffnessadr[0]]); mj_deleteModel(m_small); mj_deleteModel(m_large); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index ca287bd6..72db845d 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -962,6 +962,7 @@ public unsafe struct mjModel_ { public UInt64 nflexedge; public UInt64 nflexelem; public UInt64 nflexelemdata; + public UInt64 nflexstiffness; public UInt64 nflexelemedge; public UInt64 nflexshelldata; public UInt64 nflexevpair; @@ -1208,6 +1209,7 @@ public unsafe struct mjModel_ { public int* flex_elemadr; public int* flex_elemnum; public int* flex_elemdataadr; + public int* flex_stiffnessadr; public int* flex_elemedgeadr; public int* flex_shellnum; public int* flex_shelldataadr; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 410cef5a..3fcacd7d 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -3737,6 +3737,12 @@ struct MjModel { void set_nflexelemdata(int value) { ptr_->nflexelemdata = static_cast(value); } + int nflexstiffness() const { + return static_cast(ptr_->nflexstiffness); + } + void set_nflexstiffness(int value) { + ptr_->nflexstiffness = static_cast(value); + } int nflexelemedge() const { return static_cast(ptr_->nflexelemedge); } @@ -4661,6 +4667,9 @@ struct MjModel { emscripten::val flex_elemdataadr() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nflex, ptr_->flex_elemdataadr)); } + emscripten::val flex_stiffnessadr() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nflex, ptr_->flex_stiffnessadr)); + } emscripten::val flex_elemedgeadr() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nflex, ptr_->flex_elemedgeadr)); } @@ -4746,7 +4755,7 @@ struct MjModel { return emscripten::val(emscripten::typed_memory_view(ptr_->nflex * 3, ptr_->flex_size)); } emscripten::val flex_stiffness() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nflexelem * 21, ptr_->flex_stiffness)); + return emscripten::val(emscripten::typed_memory_view(ptr_->nflexstiffness, ptr_->flex_stiffness)); } emscripten::val flex_bending() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nflexedge * 17, ptr_->flex_bending)); @@ -11848,6 +11857,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("flex_solmix", &MjModel::flex_solmix) .property("flex_solref", &MjModel::flex_solref) .property("flex_stiffness", &MjModel::flex_stiffness) + .property("flex_stiffnessadr", &MjModel::flex_stiffnessadr) .property("flex_texcoord", &MjModel::flex_texcoord) .property("flex_texcoordadr", &MjModel::flex_texcoordadr) .property("flex_vert", &MjModel::flex_vert) @@ -12045,6 +12055,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("nflexevpair", &MjModel::nflexevpair, &MjModel::set_nflexevpair, reference()) .property("nflexnode", &MjModel::nflexnode, &MjModel::set_nflexnode, reference()) .property("nflexshelldata", &MjModel::nflexshelldata, &MjModel::set_nflexshelldata, reference()) + .property("nflexstiffness", &MjModel::nflexstiffness, &MjModel::set_nflexstiffness, reference()) .property("nflextexcoord", &MjModel::nflextexcoord, &MjModel::set_nflextexcoord, reference()) .property("nflexvert", &MjModel::nflexvert, &MjModel::set_nflexvert, reference()) .property("ngeom", &MjModel::ngeom, &MjModel::set_ngeom, reference()) From 8bf11166b1403a36d1ca7c66298822f8d9950423 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Tue, 14 Apr 2026 12:55:06 -0700 Subject: [PATCH 060/251] Import google-deepmind/mujoco_warp from GitHub. PiperOrigin-RevId: 899731493 Change-Id: Ib563023047e85db1aff56e78724dc28d769ef91e --- mjx/mujoco/mjx/_src/io.py | 3 + .../third_party/mujoco_warp/_src/benchmark.py | 12 +- .../mujoco_warp/_src/block_cholesky.py | 10 +- .../mjx/third_party/mujoco_warp/_src/bvh.py | 148 +- .../mujoco_warp/_src/collision_convex.py | 377 ++-- .../mujoco_warp/_src/collision_core.py | 126 +- .../mujoco_warp/_src/collision_driver.py | 108 +- .../mujoco_warp/_src/collision_flex.py | 276 +-- .../mujoco_warp/_src/collision_gjk.py | 142 +- .../mujoco_warp/_src/collision_primitive.py | 496 ++--- .../mujoco_warp/_src/collision_sdf.py | 182 +- .../mujoco_warp/_src/constraint.py | 800 ++++---- .../mujoco_warp/_src/derivative.py | 106 +- .../third_party/mujoco_warp/_src/forward.py | 226 +-- .../third_party/mujoco_warp/_src/inverse.py | 20 +- .../mjx/third_party/mujoco_warp/_src/io.py | 312 ++-- .../third_party/mujoco_warp/_src/island.py | 44 +- .../third_party/mujoco_warp/_src/passive.py | 174 +- .../mjx/third_party/mujoco_warp/_src/ray.py | 226 +-- .../third_party/mujoco_warp/_src/render.py | 216 +-- .../mujoco_warp/_src/render_util.py | 28 +- .../third_party/mujoco_warp/_src/sensor.py | 884 ++++----- .../third_party/mujoco_warp/_src/smooth.py | 942 +++++----- .../third_party/mujoco_warp/_src/solver.py | 648 +++---- .../third_party/mujoco_warp/_src/support.py | 196 +- .../mjx/third_party/mujoco_warp/_src/types.py | 122 +- .../third_party/mujoco_warp/pyproject.toml | 1 + .../mjx/third_party/mujoco_warp/viewer.py | 114 +- mjx/mujoco/mjx/warp/bvh.py | 39 +- mjx/mujoco/mjx/warp/collision_driver.py | 185 +- mjx/mujoco/mjx/warp/forward.py | 1617 +++++++++-------- mjx/mujoco/mjx/warp/render.py | 59 +- mjx/mujoco/mjx/warp/smooth.py | 181 +- mjx/mujoco/mjx/warp/types.py | 4 +- 34 files changed, 4556 insertions(+), 4468 deletions(-) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index d9ef50b3..74937bc0 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -456,6 +456,9 @@ def _put_model_warp( if not hasattr(mw, k) or k in ('stat', 'opt'): continue field = _wp_to_np_type(getattr(mw, k), k) + if k == 'geom_dataid' and field.ndim > 1: + # Batched geom_dataid is not supported in MJX. + field = field[0] fields[k] = field impl_fields = {} diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/benchmark.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/benchmark.py index 5712b81f..293aca63 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/benchmark.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/benchmark.py @@ -41,18 +41,18 @@ def _sum(stack1, stack2): @wp.kernel def ctrl_noise( # Model: - opt_timestep: wp.array(dtype=float), - actuator_ctrllimited: wp.array(dtype=bool), - actuator_ctrlrange: wp.array2d(dtype=wp.vec2), + opt_timestep: wp.array[float], + actuator_ctrllimited: wp.array[bool], + actuator_ctrlrange: wp.array2d[wp.vec2], # Data in: - ctrl_in: wp.array2d(dtype=float), + ctrl_in: wp.array2d[float], # In: - ctrl_center: wp.array1d(dtype=float), + ctrl_center: wp.array[float], step: int, ctrlnoisestd: float, ctrlnoiserate: float, # Data out: - ctrl_out: wp.array2d(dtype=float), + ctrl_out: wp.array2d[float], ): worldid, actid = wp.tid() diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/block_cholesky.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/block_cholesky.py index e354000a..628ad5a0 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/block_cholesky.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/block_cholesky.py @@ -23,10 +23,10 @@ def create_blocked_cholesky_func(block_size: int): @wp.func def blocked_cholesky_func( # In: - A: wp.array(dtype=float, ndim=2), + A: wp.array2d[float], matrix_size: int, # Out: - L: wp.array(dtype=float, ndim=2), + L: wp.array2d[float], ): """Computes the Cholesky factorization of a symmetric positive definite matrix A in blocks. @@ -68,11 +68,11 @@ def create_blocked_cholesky_solve_func(block_size: int, matrix_size_static: int) @wp.func def blocked_cholesky_solve_func( # In: - L: wp.array(dtype=float, ndim=2), - b: wp.array(dtype=float, ndim=2), + L: wp.array2d[float], + b: wp.array2d[float], matrix_size: int, # Out: - x: wp.array(dtype=float, ndim=2), + x: wp.array2d[float], ): """Block Cholesky factorization and solve. diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/bvh.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/bvh.py index 58f7b221..32899e07 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/bvh.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/bvh.py @@ -173,21 +173,21 @@ def _compute_cylinder_bounds( @wp.kernel def _compute_bvh_bounds( # Model: - geom_type: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), + geom_type: wp.array[int], + geom_dataid: wp.array2d[int], + geom_size: wp.array2d[wp.vec3], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], # In: bvh_ngeom: int, - enabled_geom_ids: wp.array(dtype=int), - mesh_bounds_size: wp.array(dtype=wp.vec3), - hfield_bounds_size: wp.array(dtype=wp.vec3), + enabled_geom_ids: wp.array[int], + mesh_bounds_size: wp.array[wp.vec3], + hfield_bounds_size: wp.array[wp.vec3], # Out: - lower_out: wp.array(dtype=wp.vec3), - upper_out: wp.array(dtype=wp.vec3), - group_out: wp.array(dtype=int), + lower_out: wp.array[wp.vec3], + upper_out: wp.array[wp.vec3], + group_out: wp.array[int], ): worldid, geom_local_id = wp.tid() geom_id = enabled_geom_ids[geom_local_id] @@ -205,8 +205,13 @@ def _compute_bvh_bounds( elif type == GeomType.PLANE: lower_bound, upper_bound = _compute_plane_bounds(pos, rot, size) elif type == GeomType.MESH: - size = mesh_bounds_size[geom_dataid[geom_id]] - lower_bound, upper_bound = _compute_box_bounds(pos, rot, size) + did = geom_dataid[worldid % geom_dataid.shape[0], geom_id] + if did >= 0: + size = mesh_bounds_size[did] + lower_bound, upper_bound = _compute_box_bounds(pos, rot, size) + else: + lower_bound = pos + upper_bound = pos elif type == GeomType.ELLIPSOID: lower_bound, upper_bound = _compute_ellipsoid_bounds(pos, rot, size) elif type == GeomType.CYLINDER: @@ -214,9 +219,14 @@ def _compute_bvh_bounds( elif type == GeomType.BOX: lower_bound, upper_bound = _compute_box_bounds(pos, rot, size) elif type == GeomType.HFIELD: - size = hfield_bounds_size[geom_dataid[geom_id]] - hfield_center = pos + rot[:, 2] * size[2] - lower_bound, upper_bound = _compute_box_bounds(hfield_center, rot, size) + did = geom_dataid[worldid % geom_dataid.shape[0], geom_id] + if did >= 0: + size = hfield_bounds_size[did] + hfield_center = pos + rot[:, 2] * size[2] + lower_bound, upper_bound = _compute_box_bounds(hfield_center, rot, size) + else: + lower_bound = pos + upper_bound = pos lower_out[worldid * bvh_ngeom + geom_local_id] = lower_bound upper_out[worldid * bvh_ngeom + geom_local_id] = upper_bound @@ -228,7 +238,7 @@ def compute_bvh_group_roots( # In: bvh_id: wp.uint64, # Out: - group_root_out: wp.array(dtype=int), + group_root_out: wp.array[int], ): tid = wp.tid() root = wp.bvh_get_group_root(bvh_id, tid) @@ -238,21 +248,21 @@ def compute_bvh_group_roots( @wp.kernel def _compute_flex_bvh_bounds( # Model: - flex_vertadr: wp.array(dtype=int), - flex_vertnum: wp.array(dtype=int), - flex_edge: wp.array(dtype=wp.vec2i), - flex_radius: wp.array(dtype=float), + flex_vertadr: wp.array[int], + flex_vertnum: wp.array[int], + flex_edge: wp.array[wp.vec2i], + flex_radius: wp.array[float], # Data in: - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + flexvert_xpos_in: wp.array2d[wp.vec3], # In: - flex_geom_flexid: wp.array(dtype=int), - flex_geom_edgeid: wp.array(dtype=int), + flex_geom_flexid: wp.array[int], + flex_geom_edgeid: wp.array[int], bvh_ngeom: int, total_bvh_size: int, # Out: - lower_out: wp.array(dtype=wp.vec3), - upper_out: wp.array(dtype=wp.vec3), - group_out: wp.array(dtype=int), + lower_out: wp.array[wp.vec3], + upper_out: wp.array[wp.vec3], + group_out: wp.array[int], ): worldid, flexlocalid = wp.tid() @@ -289,7 +299,7 @@ def build_scene_bvh(mjm: mujoco.MjModel, mjd: mujoco.MjData, rc: RenderContext, total_bvh_size = rc.bvh_ngeom + rc.bvh_nflexgeom geom_type = wp.array(mjm.geom_type, dtype=int) - geom_dataid = wp.array(mjm.geom_dataid, dtype=int) + geom_dataid = wp.array(np.tile(mjm.geom_dataid, (nworld, 1)), dtype=int) geom_size = wp.array(np.tile(mjm.geom_size[np.newaxis, :, :], (nworld, 1, 1)), dtype=wp.vec3) geom_xpos = wp.array(np.tile(mjd.geom_xpos[np.newaxis, :, :], (nworld, 1, 1)), dtype=wp.vec3) geom_xmat = wp.array(np.tile(mjd.geom_xmat.reshape(mjm.ngeom, 3, 3)[np.newaxis, :, :, :], (nworld, 1, 1, 1)), dtype=wp.mat33) @@ -599,16 +609,16 @@ def build_hfield_bvh( def accumulate_flex_vertex_normals( # Model: nflex: int, - flex_dim: wp.array(dtype=int), - flex_vertadr: wp.array(dtype=int), - flex_elemadr: wp.array(dtype=int), - flex_elemnum: wp.array(dtype=int), - flex_elemdataadr: wp.array(dtype=int), - flex_elem: wp.array(dtype=int), + flex_dim: wp.array[int], + flex_vertadr: wp.array[int], + flex_elemadr: wp.array[int], + flex_elemnum: wp.array[int], + flex_elemdataadr: wp.array[int], + flex_elem: wp.array[int], # Data in: - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + flexvert_xpos_in: wp.array2d[wp.vec3], # Out: - flexvert_norm_out: wp.array2d(dtype=wp.vec3), + flexvert_norm_out: wp.array2d[wp.vec3], ): """Accumulate per-vertex normals by summing adjacent face normals.""" worldid, elemid = wp.tid() @@ -644,7 +654,7 @@ def accumulate_flex_vertex_normals( @wp.kernel def normalize_vertex_normals( # Out: - flexvert_norm_out: wp.array2d(dtype=wp.vec3), + flexvert_norm_out: wp.array2d[wp.vec3], ): """Normalize accumulated vertex normals.""" worldid, vertid = wp.tid() @@ -654,20 +664,20 @@ def normalize_vertex_normals( @wp.kernel def _build_flex_2d_elements( # Model: - flex_elem: wp.array(dtype=int), + flex_elem: wp.array[int], # Data in: - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + flexvert_xpos_in: wp.array2d[wp.vec3], # In: - flexvert_norm_in: wp.array2d(dtype=wp.vec3), + flexvert_norm_in: wp.array2d[wp.vec3], elem_adr: int, vert_adr: int, face_offset: int, radius: float, nfaces: int, # Out: - face_point_out: wp.array(dtype=wp.vec3), - face_index_out: wp.array(dtype=int), - group_out: wp.array(dtype=int), + face_point_out: wp.array[wp.vec3], + face_index_out: wp.array[int], + group_out: wp.array[int], ): """Create faces from 2D flex elements (triangles). @@ -728,20 +738,20 @@ def _build_flex_2d_elements( @wp.kernel def _build_flex_2d_sides( # Model: - flex_shell: wp.array(dtype=int), + flex_shell: wp.array[int], # Data in: - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + flexvert_xpos_in: wp.array2d[wp.vec3], # In: - flexvert_norm_in: wp.array2d(dtype=wp.vec3), + flexvert_norm_in: wp.array2d[wp.vec3], shell_adr: int, vert_adr: int, face_offset: int, radius: float, nface: int, # Out: - face_point_out: wp.array(dtype=wp.vec3), - face_index_out: wp.array(dtype=int), - group_out: wp.array(dtype=int), + face_point_out: wp.array[wp.vec3], + face_index_out: wp.array[int], + group_out: wp.array[int], ): """Create side faces from 2D flex shell fragments. @@ -790,18 +800,18 @@ def _build_flex_2d_sides( @wp.kernel def _build_flex_3d_shells( # Model: - flex_shell: wp.array(dtype=int), + flex_shell: wp.array[int], # Data in: - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + flexvert_xpos_in: wp.array2d[wp.vec3], # In: shell_adr: int, vert_adr: int, face_offset: int, nface: int, # Out: - face_point_out: wp.array(dtype=wp.vec3), - face_index_out: wp.array(dtype=int), - group_out: wp.array(dtype=int), + face_point_out: wp.array[wp.vec3], + face_index_out: wp.array[int], + group_out: wp.array[int], ): """Create faces from 3D flex shell fragments (triangles). @@ -836,22 +846,22 @@ def _build_flex_3d_shells( @wp.kernel def _update_flex_2d_face_points( # Model: - flex_vertadr: wp.array(dtype=int), - flex_elemnum: wp.array(dtype=int), - flex_elemdataadr: wp.array(dtype=int), - flex_shelldataadr: wp.array(dtype=int), - flex_elem: wp.array(dtype=int), - flex_shell: wp.array(dtype=int), - flex_radius: wp.array(dtype=float), + flex_vertadr: wp.array[int], + flex_elemnum: wp.array[int], + flex_elemdataadr: wp.array[int], + flex_shelldataadr: wp.array[int], + flex_elem: wp.array[int], + flex_shell: wp.array[int], + flex_radius: wp.array[float], # Data in: - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + flexvert_xpos_in: wp.array2d[wp.vec3], # In: - flexvert_norm_in: wp.array2d(dtype=wp.vec3), + flexvert_norm_in: wp.array2d[wp.vec3], flex_id: int, nface: int, smooth: bool, # Out: - face_point_out: wp.array(dtype=wp.vec3), + face_point_out: wp.array[wp.vec3], ): worldid, workid = wp.tid() @@ -935,16 +945,16 @@ def _update_flex_2d_face_points( @wp.kernel def _update_flex_3d_face_points( # Model: - flex_vertadr: wp.array(dtype=int), - flex_shelldataadr: wp.array(dtype=int), - flex_shell: wp.array(dtype=int), + flex_vertadr: wp.array[int], + flex_shelldataadr: wp.array[int], + flex_shell: wp.array[int], # Data in: - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + flexvert_xpos_in: wp.array2d[wp.vec3], # In: flex_id: int, nface: int, # Out: - face_point_out: wp.array(dtype=wp.vec3), + face_point_out: wp.array[wp.vec3], ): worldid, shellid = wp.tid() diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py index 12d9824a..4f6b21c5 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py @@ -55,20 +55,20 @@ mat_maxconpair = wp.types.matrix(shape=(MJ_MAXCONPAIR, 3), dtype=float) @wp.func def _hfield_filter( # Model: - geom_type: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - geom_rbound: wp.array2d(dtype=float), - geom_margin: wp.array2d(dtype=float), - mesh_vertadr: wp.array(dtype=int), - mesh_vertnum: wp.array(dtype=int), - mesh_graphadr: wp.array(dtype=int), - mesh_vert: wp.array(dtype=wp.vec3), - mesh_graph: wp.array(dtype=int), - hfield_size: wp.array(dtype=wp.vec4), + geom_type: wp.array[int], + geom_dataid: wp.array2d[int], + geom_size: wp.array2d[wp.vec3], + geom_rbound: wp.array2d[float], + geom_margin: wp.array2d[float], + mesh_vertadr: wp.array[int], + mesh_vertnum: wp.array[int], + mesh_graphadr: wp.array[int], + mesh_vert: wp.array[wp.vec3], + mesh_graph: wp.array[int], + hfield_size: wp.array[wp.vec4], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], # In: worldid: int, g1: int, @@ -79,7 +79,8 @@ def _hfield_filter( See MuJoCo mjc_ConvexHField. """ # height field info - hfdataid = geom_dataid[g1] + dataid_setid = worldid % geom_dataid.shape[0] + hfdataid = geom_dataid[dataid_setid, g1] size1 = hfield_size[hfdataid] # geom info @@ -124,7 +125,7 @@ def _hfield_filter( # load mesh vertex data for support function queries if geomtype2 == GeomType.MESH: - dataid = geom_dataid[g2] + dataid = geom_dataid[dataid_setid, g2] geom2.vertadr = wp.where(dataid >= 0, mesh_vertadr[dataid], -1) geom2.vertnum = wp.where(dataid >= 0, mesh_vertnum[dataid], -1) geom2.graphadr = wp.where(dataid >= 0, mesh_graphadr[dataid], -1) @@ -167,78 +168,78 @@ def ccd_hfield_kernel_builder( @wp.kernel(module="unique", enable_backward=False) def ccd_hfield_kernel( # Model: - opt_ccd_tolerance: wp.array(dtype=float), - geom_type: wp.array(dtype=int), - geom_condim: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_priority: wp.array(dtype=int), - geom_solmix: wp.array2d(dtype=float), - geom_solref: wp.array2d(dtype=wp.vec2), - geom_solimp: wp.array2d(dtype=vec5), - geom_size: wp.array2d(dtype=wp.vec3), - geom_rbound: wp.array2d(dtype=float), - geom_friction: wp.array2d(dtype=wp.vec3), - geom_margin: wp.array2d(dtype=float), - geom_gap: wp.array2d(dtype=float), - mesh_vertadr: wp.array(dtype=int), - mesh_vertnum: wp.array(dtype=int), - mesh_graphadr: wp.array(dtype=int), - mesh_vert: wp.array(dtype=wp.vec3), - mesh_graph: wp.array(dtype=int), - mesh_polynum: wp.array(dtype=int), - mesh_polyadr: wp.array(dtype=int), - mesh_polynormal: wp.array(dtype=wp.vec3), - mesh_polyvertadr: wp.array(dtype=int), - mesh_polyvertnum: wp.array(dtype=int), - mesh_polyvert: wp.array(dtype=int), - mesh_polymapadr: wp.array(dtype=int), - mesh_polymapnum: wp.array(dtype=int), - mesh_polymap: wp.array(dtype=int), - hfield_size: wp.array(dtype=wp.vec4), - hfield_nrow: wp.array(dtype=int), - hfield_ncol: wp.array(dtype=int), - hfield_adr: wp.array(dtype=int), - hfield_data: wp.array(dtype=float), - pair_dim: wp.array(dtype=int), - pair_solref: wp.array2d(dtype=wp.vec2), - pair_solreffriction: wp.array2d(dtype=wp.vec2), - pair_solimp: wp.array2d(dtype=vec5), - pair_margin: wp.array2d(dtype=float), - pair_gap: wp.array2d(dtype=float), - pair_friction: wp.array2d(dtype=vec5), + opt_ccd_tolerance: wp.array[float], + geom_type: wp.array[int], + geom_condim: wp.array[int], + geom_dataid: wp.array2d[int], + geom_priority: wp.array[int], + geom_solmix: wp.array2d[float], + geom_solref: wp.array2d[wp.vec2], + geom_solimp: wp.array2d[vec5], + geom_size: wp.array2d[wp.vec3], + geom_rbound: wp.array2d[float], + geom_friction: wp.array2d[wp.vec3], + geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], + mesh_vertadr: wp.array[int], + mesh_vertnum: wp.array[int], + mesh_graphadr: wp.array[int], + mesh_vert: wp.array[wp.vec3], + mesh_graph: wp.array[int], + mesh_polynum: wp.array[int], + mesh_polyadr: wp.array[int], + mesh_polynormal: wp.array[wp.vec3], + mesh_polyvertadr: wp.array[int], + mesh_polyvertnum: wp.array[int], + mesh_polyvert: wp.array[int], + mesh_polymapadr: wp.array[int], + mesh_polymapnum: wp.array[int], + mesh_polymap: wp.array[int], + hfield_size: wp.array[wp.vec4], + hfield_nrow: wp.array[int], + hfield_ncol: wp.array[int], + hfield_adr: wp.array[int], + hfield_data: wp.array[float], + pair_dim: wp.array[int], + pair_solref: wp.array2d[wp.vec2], + pair_solreffriction: wp.array2d[wp.vec2], + pair_solimp: wp.array2d[vec5], + pair_margin: wp.array2d[float], + pair_gap: wp.array2d[float], + pair_friction: wp.array2d[vec5], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], naconmax_in: int, naccdmax_in: int, - ncollision_in: wp.array(dtype=int), + ncollision_in: wp.array[int], # In: - collision_pair_in: wp.array(dtype=wp.vec2i), - collision_pairid_in: wp.array(dtype=wp.vec2i), - collision_worldid_in: wp.array(dtype=int), - epa_vert_in: wp.array2d(dtype=wp.vec3), - epa_vert_index_in: wp.array2d(dtype=int), - epa_face_in: wp.array2d(dtype=int), - epa_pr_in: wp.array2d(dtype=wp.vec3), - epa_norm2_in: wp.array2d(dtype=float), - epa_horizon_in: wp.array2d(dtype=int), - nccd_in: wp.array(dtype=int), + collision_pair_in: wp.array[wp.vec2i], + collision_pairid_in: wp.array[wp.vec2i], + collision_worldid_in: wp.array[int], + epa_vert_in: wp.array2d[wp.vec3], + epa_vert_index_in: wp.array2d[int], + epa_face_in: wp.array2d[int], + epa_pr_in: wp.array2d[wp.vec3], + epa_norm2_in: wp.array2d[float], + epa_horizon_in: wp.array2d[int], + nccd_in: wp.array[int], # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): collisionid = wp.tid() if collisionid >= ncollision_in[0]: @@ -339,7 +340,7 @@ def ccd_hfield_kernel_builder( geom1.rot = wp.identity(n=3, dtype=float) # see MuJoCo mjc_ConvexHField - geom1_dataid = geom_dataid[g1] + geom1_dataid = geom_dataid[worldid % geom_dataid.shape[0], g1] # height field subgrid nrow = hfield_nrow[geom1_dataid] @@ -710,27 +711,27 @@ def ccd_kernel_builder( @wp.func def eval_ccd_write_contact( # Model: - opt_ccd_tolerance: wp.array(dtype=float), + opt_ccd_tolerance: wp.array[float], # Data in: naconmax_in: int, # In: - epa_vert_in: wp.array2d(dtype=wp.vec3), - epa_vert_index_in: wp.array2d(dtype=int), - epa_face_in: wp.array2d(dtype=int), - epa_pr_in: wp.array2d(dtype=wp.vec3), - epa_norm2_in: wp.array2d(dtype=float), - epa_horizon_in: wp.array2d(dtype=int), - multiccd_polygon_in: wp.array2d(dtype=wp.vec3), - multiccd_clipped_in: wp.array2d(dtype=wp.vec3), - multiccd_pnormal_in: wp.array2d(dtype=wp.vec3), - multiccd_pdist_in: wp.array2d(dtype=float), - multiccd_idx1_in: wp.array2d(dtype=int), - multiccd_idx2_in: wp.array2d(dtype=int), - multiccd_n1_in: wp.array2d(dtype=wp.vec3), - multiccd_n2_in: wp.array2d(dtype=wp.vec3), - multiccd_endvert_in: wp.array2d(dtype=wp.vec3), - multiccd_face1_in: wp.array2d(dtype=wp.vec3), - multiccd_face2_in: wp.array2d(dtype=wp.vec3), + epa_vert_in: wp.array2d[wp.vec3], + epa_vert_index_in: wp.array2d[int], + epa_face_in: wp.array2d[int], + epa_pr_in: wp.array2d[wp.vec3], + epa_norm2_in: wp.array2d[float], + epa_horizon_in: wp.array2d[int], + multiccd_polygon_in: wp.array2d[wp.vec3], + multiccd_clipped_in: wp.array2d[wp.vec3], + multiccd_pnormal_in: wp.array2d[wp.vec3], + multiccd_pdist_in: wp.array2d[float], + multiccd_idx1_in: wp.array2d[int], + multiccd_idx2_in: wp.array2d[int], + multiccd_n1_in: wp.array2d[wp.vec3], + multiccd_n2_in: wp.array2d[wp.vec3], + multiccd_endvert_in: wp.array2d[wp.vec3], + multiccd_face1_in: wp.array2d[wp.vec3], + multiccd_face2_in: wp.array2d[wp.vec3], geom1: Geom, geom2: Geom, geoms: wp.vec2i, @@ -747,21 +748,21 @@ def ccd_kernel_builder( x2: wp.vec3, pairid: wp.vec2i, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ) -> int: points = mat43() witness1 = mat43() @@ -892,83 +893,83 @@ def ccd_kernel_builder( @wp.kernel(module="unique", enable_backward=False) def ccd_kernel( # Model: - opt_ccd_tolerance: wp.array(dtype=float), - geom_type: wp.array(dtype=int), - geom_condim: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_priority: wp.array(dtype=int), - geom_solmix: wp.array2d(dtype=float), - geom_solref: wp.array2d(dtype=wp.vec2), - geom_solimp: wp.array2d(dtype=vec5), - geom_size: wp.array2d(dtype=wp.vec3), - geom_friction: wp.array2d(dtype=wp.vec3), - geom_margin: wp.array2d(dtype=float), - geom_gap: wp.array2d(dtype=float), - mesh_vertadr: wp.array(dtype=int), - mesh_vertnum: wp.array(dtype=int), - mesh_graphadr: wp.array(dtype=int), - mesh_vert: wp.array(dtype=wp.vec3), - mesh_graph: wp.array(dtype=int), - mesh_polynum: wp.array(dtype=int), - mesh_polyadr: wp.array(dtype=int), - mesh_polynormal: wp.array(dtype=wp.vec3), - mesh_polyvertadr: wp.array(dtype=int), - mesh_polyvertnum: wp.array(dtype=int), - mesh_polyvert: wp.array(dtype=int), - mesh_polymapadr: wp.array(dtype=int), - mesh_polymapnum: wp.array(dtype=int), - mesh_polymap: wp.array(dtype=int), - pair_dim: wp.array(dtype=int), - pair_solref: wp.array2d(dtype=wp.vec2), - pair_solreffriction: wp.array2d(dtype=wp.vec2), - pair_solimp: wp.array2d(dtype=vec5), - pair_margin: wp.array2d(dtype=float), - pair_gap: wp.array2d(dtype=float), - pair_friction: wp.array2d(dtype=vec5), + opt_ccd_tolerance: wp.array[float], + geom_type: wp.array[int], + geom_condim: wp.array[int], + geom_dataid: wp.array2d[int], + geom_priority: wp.array[int], + geom_solmix: wp.array2d[float], + geom_solref: wp.array2d[wp.vec2], + geom_solimp: wp.array2d[vec5], + geom_size: wp.array2d[wp.vec3], + geom_friction: wp.array2d[wp.vec3], + geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], + mesh_vertadr: wp.array[int], + mesh_vertnum: wp.array[int], + mesh_graphadr: wp.array[int], + mesh_vert: wp.array[wp.vec3], + mesh_graph: wp.array[int], + mesh_polynum: wp.array[int], + mesh_polyadr: wp.array[int], + mesh_polynormal: wp.array[wp.vec3], + mesh_polyvertadr: wp.array[int], + mesh_polyvertnum: wp.array[int], + mesh_polyvert: wp.array[int], + mesh_polymapadr: wp.array[int], + mesh_polymapnum: wp.array[int], + mesh_polymap: wp.array[int], + pair_dim: wp.array[int], + pair_solref: wp.array2d[wp.vec2], + pair_solreffriction: wp.array2d[wp.vec2], + pair_solimp: wp.array2d[vec5], + pair_margin: wp.array2d[float], + pair_gap: wp.array2d[float], + pair_friction: wp.array2d[vec5], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], naconmax_in: int, naccdmax_in: int, - ncollision_in: wp.array(dtype=int), + ncollision_in: wp.array[int], # In: - collision_pair_in: wp.array(dtype=wp.vec2i), - collision_pairid_in: wp.array(dtype=wp.vec2i), - collision_worldid_in: wp.array(dtype=int), - epa_vert_in: wp.array2d(dtype=wp.vec3), - epa_vert_index_in: wp.array2d(dtype=int), - epa_face_in: wp.array2d(dtype=int), - epa_pr_in: wp.array2d(dtype=wp.vec3), - epa_norm2_in: wp.array2d(dtype=float), - epa_horizon_in: wp.array2d(dtype=int), - multiccd_polygon_in: wp.array2d(dtype=wp.vec3), - multiccd_clipped_in: wp.array2d(dtype=wp.vec3), - multiccd_pnormal_in: wp.array2d(dtype=wp.vec3), - multiccd_pdist_in: wp.array2d(dtype=float), - multiccd_idx1_in: wp.array2d(dtype=int), - multiccd_idx2_in: wp.array2d(dtype=int), - multiccd_n1_in: wp.array2d(dtype=wp.vec3), - multiccd_n2_in: wp.array2d(dtype=wp.vec3), - multiccd_endvert_in: wp.array2d(dtype=wp.vec3), - multiccd_face1_in: wp.array2d(dtype=wp.vec3), - multiccd_face2_in: wp.array2d(dtype=wp.vec3), - nccd_in: wp.array(dtype=int), + collision_pair_in: wp.array[wp.vec2i], + collision_pairid_in: wp.array[wp.vec2i], + collision_worldid_in: wp.array[int], + epa_vert_in: wp.array2d[wp.vec3], + epa_vert_index_in: wp.array2d[int], + epa_face_in: wp.array2d[int], + epa_pr_in: wp.array2d[wp.vec3], + epa_norm2_in: wp.array2d[float], + epa_horizon_in: wp.array2d[int], + multiccd_polygon_in: wp.array2d[wp.vec3], + multiccd_clipped_in: wp.array2d[wp.vec3], + multiccd_pnormal_in: wp.array2d[wp.vec3], + multiccd_pdist_in: wp.array2d[float], + multiccd_idx1_in: wp.array2d[int], + multiccd_idx2_in: wp.array2d[int], + multiccd_n1_in: wp.array2d[wp.vec3], + multiccd_n2_in: wp.array2d[wp.vec3], + multiccd_endvert_in: wp.array2d[wp.vec3], + multiccd_face1_in: wp.array2d[wp.vec3], + multiccd_face2_in: wp.array2d[wp.vec3], + nccd_in: wp.array[int], # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): collisionid = wp.tid() if collisionid >= ncollision_in[0]: diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_core.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_core.py index b7affa7d..96da38e0 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_core.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_core.py @@ -47,44 +47,44 @@ class Geom: hfprism: mat63 vertadr: int vertnum: int - vert: wp.array(dtype=wp.vec3) + vert: wp.array[wp.vec3] graphadr: int - graph: wp.array(dtype=int) + graph: wp.array[int] mesh_polynum: int mesh_polyadr: int - mesh_polynormal: wp.array(dtype=wp.vec3) - mesh_polyvertadr: wp.array(dtype=int) - mesh_polyvertnum: wp.array(dtype=int) - mesh_polyvert: wp.array(dtype=int) - mesh_polymapadr: wp.array(dtype=int) - mesh_polymapnum: wp.array(dtype=int) - mesh_polymap: wp.array(dtype=int) + mesh_polynormal: wp.array[wp.vec3] + mesh_polyvertadr: wp.array[int] + mesh_polyvertnum: wp.array[int] + mesh_polyvert: wp.array[int] + mesh_polymapadr: wp.array[int] + mesh_polymapnum: wp.array[int] + mesh_polymap: wp.array[int] index: int @wp.func def geom_collision_pair( # Model: - geom_type: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - mesh_vertadr: wp.array(dtype=int), - mesh_vertnum: wp.array(dtype=int), - mesh_graphadr: wp.array(dtype=int), - mesh_vert: wp.array(dtype=wp.vec3), - mesh_graph: wp.array(dtype=int), - mesh_polynum: wp.array(dtype=int), - mesh_polyadr: wp.array(dtype=int), - mesh_polynormal: wp.array(dtype=wp.vec3), - mesh_polyvertadr: wp.array(dtype=int), - mesh_polyvertnum: wp.array(dtype=int), - mesh_polyvert: wp.array(dtype=int), - mesh_polymapadr: wp.array(dtype=int), - mesh_polymapnum: wp.array(dtype=int), - mesh_polymap: wp.array(dtype=int), + geom_type: wp.array[int], + geom_dataid: wp.array2d[int], + geom_size: wp.array2d[wp.vec3], + mesh_vertadr: wp.array[int], + mesh_vertnum: wp.array[int], + mesh_graphadr: wp.array[int], + mesh_vert: wp.array[wp.vec3], + mesh_graph: wp.array[int], + mesh_polynum: wp.array[int], + mesh_polyadr: wp.array[int], + mesh_polynormal: wp.array[wp.vec3], + mesh_polyvertadr: wp.array[int], + mesh_polyvertnum: wp.array[int], + mesh_polyvert: wp.array[int], + mesh_polymapadr: wp.array[int], + mesh_polymapnum: wp.array[int], + mesh_polymap: wp.array[int], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], # In: geoms: wp.vec2i, worldid: int, @@ -109,8 +109,10 @@ def geom_collision_pair( # z-axis of the rotation matrix, used as the surface normal for plane collisions geom2.normal = wp.vec3(geom2.rot[0, 2], geom2.rot[1, 2], geom2.rot[2, 2]) + dataid_setid = worldid % geom_dataid.shape[0] + if geom_type1 == GeomType.MESH: - dataid = geom_dataid[g1] + dataid = geom_dataid[dataid_setid, g1] geom1.vertadr = wp.where(dataid >= 0, mesh_vertadr[dataid], -1) geom1.vertnum = wp.where(dataid >= 0, mesh_vertnum[dataid], -1) geom1.graphadr = wp.where(dataid >= 0, mesh_graphadr[dataid], -1) @@ -128,7 +130,7 @@ def geom_collision_pair( geom1.mesh_polymap = mesh_polymap if geom_type2 == GeomType.MESH: - dataid = geom_dataid[g2] + dataid = geom_dataid[dataid_setid, g2] geom2.vertadr = wp.where(dataid >= 0, mesh_vertadr[dataid], -1) geom2.vertnum = wp.where(dataid >= 0, mesh_vertnum[dataid], -1) geom2.graphadr = wp.where(dataid >= 0, mesh_graphadr[dataid], -1) @@ -174,21 +176,21 @@ def write_contact( pairid_in: wp.vec2i, worldid_in: int, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ) -> int: """Atomically write a detected contact into the contact output arrays. @@ -233,24 +235,24 @@ def write_contact( @wp.func def contact_params( # Model: - geom_condim: wp.array(dtype=int), - geom_priority: wp.array(dtype=int), - geom_solmix: wp.array2d(dtype=float), - geom_solref: wp.array2d(dtype=wp.vec2), - geom_solimp: wp.array2d(dtype=vec5), - geom_friction: wp.array2d(dtype=wp.vec3), - geom_margin: wp.array2d(dtype=float), - geom_gap: wp.array2d(dtype=float), - pair_dim: wp.array(dtype=int), - pair_solref: wp.array2d(dtype=wp.vec2), - pair_solreffriction: wp.array2d(dtype=wp.vec2), - pair_solimp: wp.array2d(dtype=vec5), - pair_margin: wp.array2d(dtype=float), - pair_gap: wp.array2d(dtype=float), - pair_friction: wp.array2d(dtype=vec5), + geom_condim: wp.array[int], + geom_priority: wp.array[int], + geom_solmix: wp.array2d[float], + geom_solref: wp.array2d[wp.vec2], + geom_solimp: wp.array2d[vec5], + geom_friction: wp.array2d[wp.vec3], + geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], + pair_dim: wp.array[int], + pair_solref: wp.array2d[wp.vec2], + pair_solreffriction: wp.array2d[wp.vec2], + pair_solimp: wp.array2d[vec5], + pair_margin: wp.array2d[float], + pair_gap: wp.array2d[float], + pair_friction: wp.array2d[vec5], # In: - collision_pair_in: wp.array(dtype=wp.vec2i), - collision_pairid_in: wp.array(dtype=wp.vec2i), + collision_pair_in: wp.array[wp.vec2i], + collision_pairid_in: wp.array[wp.vec2i], cid: int, worldid: int, ): diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py index 786ee9b8..c87b21f1 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py @@ -80,8 +80,8 @@ MJ_COLLISION_TABLE = { @wp.kernel def _zero_nacon_ncollision( # Data out: - nacon_out: wp.array(dtype=int), - ncollision_out: wp.array(dtype=int), + nacon_out: wp.array[int], + ncollision_out: wp.array[int], ): ncollision_out[0] = 0 nacon_out[0] = 0 @@ -275,12 +275,12 @@ def _broadphase_filter(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound @wp.func def func( # Model: - geom_aabb: wp.array3d(dtype=wp.vec3), - geom_rbound: wp.array2d(dtype=float), - geom_margin: wp.array2d(dtype=float), + geom_aabb: wp.array3d[wp.vec3], + geom_rbound: wp.array2d[float], + geom_margin: wp.array2d[float], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], # In: geom1: int, geom2: int, @@ -324,8 +324,8 @@ def _broadphase_filter(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound @wp.func def _add_geom_pair( # Model: - geom_type: wp.array(dtype=int), - nxn_pairid: wp.array(dtype=wp.vec2i), + geom_type: wp.array[int], + nxn_pairid: wp.array[wp.vec2i], # Data in: naconmax_in: int, # In: @@ -334,11 +334,11 @@ def _add_geom_pair( worldid: int, nxnid: int, # Data out: - ncollision_out: wp.array(dtype=int), + ncollision_out: wp.array[int], # Out: - collision_pair_out: wp.array(dtype=wp.vec2i), - collision_pairid_out: wp.array(dtype=wp.vec2i), - collision_worldid_out: wp.array(dtype=int), + collision_pair_out: wp.array[wp.vec2i], + collision_pairid_out: wp.array[wp.vec2i], + collision_worldid_out: wp.array[int], ): pairid = wp.atomic_add(ncollision_out, 0, 1) @@ -359,7 +359,7 @@ def _add_geom_pair( @wp.func -def _binary_search(values: wp.array(dtype=Any), value: Any, lower: int, upper: int) -> int: +def _binary_search(values: wp.array[Any], value: Any, lower: int, upper: int) -> int: while lower < upper: mid = (lower + upper) >> 1 if values[mid] > value: @@ -375,18 +375,18 @@ def _sap_project(opt_broadphase: int): def sap_project( # Model: ngeom: int, - geom_rbound: wp.array2d(dtype=float), - geom_margin: wp.array2d(dtype=float), + geom_rbound: wp.array2d[float], + geom_margin: wp.array2d[float], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xpos_in: wp.array2d[wp.vec3], nworld_in: int, # In: direction_in: wp.vec3, # Out: - projection_lower_out: wp.array2d(dtype=float), - projection_upper_out: wp.array2d(dtype=float), - sort_index_out: wp.array2d(dtype=int), - segmented_index_out: wp.array(dtype=int), + projection_lower_out: wp.array2d[float], + projection_upper_out: wp.array2d[float], + sort_index_out: wp.array2d[int], + segmented_index_out: wp.array[int], ): worldid, geomid = wp.tid() @@ -422,11 +422,11 @@ def _sap_range( # Model: ngeom: int, # In: - projection_lower_in: wp.array2d(dtype=float), - projection_upper_in: wp.array2d(dtype=float), - sort_index_in: wp.array2d(dtype=int), + projection_lower_in: wp.array2d[float], + projection_upper_in: wp.array2d[float], + sort_index_in: wp.array2d[int], # Out: - range_out: wp.array2d(dtype=int), + range_out: wp.array2d[int], ): worldid, geomid = wp.tid() @@ -448,26 +448,26 @@ def _sap_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: i def kernel( # Model: ngeom: int, - geom_type: wp.array(dtype=int), - geom_aabb: wp.array3d(dtype=wp.vec3), - geom_rbound: wp.array2d(dtype=float), - geom_margin: wp.array2d(dtype=float), - nxn_pairid: wp.array(dtype=wp.vec2i), + geom_type: wp.array[int], + geom_aabb: wp.array3d[wp.vec3], + geom_rbound: wp.array2d[float], + geom_margin: wp.array2d[float], + nxn_pairid: wp.array[wp.vec2i], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], nworld_in: int, naconmax_in: int, # In: - sort_index_in: wp.array2d(dtype=int), - cumulative_sum_in: wp.array(dtype=int), + sort_index_in: wp.array2d[int], + cumulative_sum_in: wp.array[int], nsweep_in: int, # Data out: - ncollision_out: wp.array(dtype=int), + ncollision_out: wp.array[int], # Out: - collision_pair_out: wp.array(dtype=wp.vec2i), - collision_pairid_out: wp.array(dtype=wp.vec2i), - collision_worldid_out: wp.array(dtype=int), + collision_pair_out: wp.array[wp.vec2i], + collision_pairid_out: wp.array[wp.vec2i], + collision_worldid_out: wp.array[int], ): worldgeomid = wp.tid() @@ -528,11 +528,11 @@ def _segmented_sort(tile_size: int): @wp.kernel(module="unique") def segmented_sort( # In: - projection_lower_in: wp.array2d(dtype=float), - sort_index_in: wp.array2d(dtype=int), + projection_lower_in: wp.array2d[float], + sort_index_in: wp.array2d[int], # Out: - projection_lower_out: wp.array2d(dtype=float), - sort_index_out: wp.array2d(dtype=int), + projection_lower_out: wp.array2d[float], + sort_index_out: wp.array2d[int], ): worldid = wp.tid() @@ -648,22 +648,22 @@ def _nxn_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: i @wp.kernel(module="unique", enable_backward=False) def kernel( # Model: - geom_type: wp.array(dtype=int), - geom_aabb: wp.array3d(dtype=wp.vec3), - geom_rbound: wp.array2d(dtype=float), - geom_margin: wp.array2d(dtype=float), - nxn_geom_pair: wp.array(dtype=wp.vec2i), - nxn_pairid: wp.array(dtype=wp.vec2i), + geom_type: wp.array[int], + geom_aabb: wp.array3d[wp.vec3], + geom_rbound: wp.array2d[float], + geom_margin: wp.array2d[float], + nxn_geom_pair: wp.array[wp.vec2i], + nxn_pairid: wp.array[wp.vec2i], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], naconmax_in: int, # Data out: - ncollision_out: wp.array(dtype=int), + ncollision_out: wp.array[int], # Out: - collision_pair_out: wp.array(dtype=wp.vec2i), - collision_pairid_out: wp.array(dtype=wp.vec2i), - collision_worldid_out: wp.array(dtype=int), + collision_pair_out: wp.array[wp.vec2i], + collision_pairid_out: wp.array[wp.vec2i], + collision_worldid_out: wp.array[int], ): worldid, elementid = wp.tid() diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_flex.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_flex.py index 215423e9..cd5c9087 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_flex.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_flex.py @@ -46,22 +46,22 @@ def _write_flex_contact( vertid: int, worldid: int, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_flex_out: wp.array(dtype=wp.vec2i), - contact_vert_out: wp.array(dtype=wp.vec2i), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_flex_out: wp.array[wp.vec2i], + contact_vert_out: wp.array[wp.vec2i], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): if dist >= margin or dist >= MJ_MAXVAL: return @@ -110,22 +110,22 @@ def _collide_geom_triangle( vertex_id: int, worldid: int, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_flex_out: wp.array(dtype=wp.vec2i), - contact_vert_out: wp.array(dtype=wp.vec2i), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_flex_out: wp.array[wp.vec2i], + contact_vert_out: wp.array[wp.vec2i], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): if gtype == int(GeomType.SPHERE): sphere_radius = size_val[0] @@ -262,41 +262,41 @@ def _flex_plane_narrowphase( # Model: ngeom: int, nflexvert: int, - geom_type: wp.array(dtype=int), - geom_condim: wp.array(dtype=int), - geom_solref: wp.array2d(dtype=wp.vec2), - geom_solimp: wp.array2d(dtype=vec5), - geom_friction: wp.array2d(dtype=wp.vec3), - geom_margin: wp.array2d(dtype=float), - flex_condim: wp.array(dtype=int), - flex_friction: wp.array(dtype=wp.vec3), - flex_margin: wp.array(dtype=float), - flex_vertadr: wp.array(dtype=int), - flex_radius: wp.array(dtype=float), - flex_vertflexid: wp.array(dtype=int), + geom_type: wp.array[int], + geom_condim: wp.array[int], + geom_solref: wp.array2d[wp.vec2], + geom_solimp: wp.array2d[vec5], + geom_friction: wp.array2d[wp.vec3], + geom_margin: wp.array2d[float], + flex_condim: wp.array[int], + flex_friction: wp.array[wp.vec3], + flex_margin: wp.array[float], + flex_vertadr: wp.array[int], + flex_radius: wp.array[float], + flex_vertflexid: wp.array[int], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], + flexvert_xpos_in: wp.array2d[wp.vec3], nworld_in: int, naconmax_in: int, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_flex_out: wp.array(dtype=wp.vec2i), - contact_vert_out: wp.array(dtype=wp.vec2i), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_flex_out: wp.array[wp.vec2i], + contact_vert_out: wp.array[wp.vec2i], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): worldid, vertid = wp.tid() @@ -382,48 +382,48 @@ def _flex_narrowphase_dim2( # Model: ngeom: int, nflex: int, - geom_type: wp.array(dtype=int), - geom_contype: wp.array(dtype=int), - geom_conaffinity: wp.array(dtype=int), - geom_condim: wp.array(dtype=int), - geom_solref: wp.array2d(dtype=wp.vec2), - geom_solimp: wp.array2d(dtype=vec5), - geom_size: wp.array2d(dtype=wp.vec3), - geom_friction: wp.array2d(dtype=wp.vec3), - geom_margin: wp.array2d(dtype=float), - flex_contype: wp.array(dtype=int), - flex_conaffinity: wp.array(dtype=int), - flex_margin: wp.array(dtype=float), - flex_dim: wp.array(dtype=int), - flex_vertadr: wp.array(dtype=int), - flex_elemadr: wp.array(dtype=int), - flex_elemnum: wp.array(dtype=int), - flex_elemdataadr: wp.array(dtype=int), - flex_elem: wp.array(dtype=int), - flex_radius: wp.array(dtype=float), + geom_type: wp.array[int], + geom_contype: wp.array[int], + geom_conaffinity: wp.array[int], + geom_condim: wp.array[int], + geom_solref: wp.array2d[wp.vec2], + geom_solimp: wp.array2d[vec5], + geom_size: wp.array2d[wp.vec3], + geom_friction: wp.array2d[wp.vec3], + geom_margin: wp.array2d[float], + flex_contype: wp.array[int], + flex_conaffinity: wp.array[int], + flex_margin: wp.array[float], + flex_dim: wp.array[int], + flex_vertadr: wp.array[int], + flex_elemadr: wp.array[int], + flex_elemnum: wp.array[int], + flex_elemdataadr: wp.array[int], + flex_elem: wp.array[int], + flex_radius: wp.array[float], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], + flexvert_xpos_in: wp.array2d[wp.vec3], nworld_in: int, naconmax_in: int, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_flex_out: wp.array(dtype=wp.vec2i), - contact_vert_out: wp.array(dtype=wp.vec2i), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_flex_out: wp.array[wp.vec2i], + contact_vert_out: wp.array[wp.vec2i], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): worldid, elemid = wp.tid() @@ -533,47 +533,47 @@ def _flex_narrowphase_dim3( # Model: ngeom: int, nflex: int, - geom_type: wp.array(dtype=int), - geom_contype: wp.array(dtype=int), - geom_conaffinity: wp.array(dtype=int), - geom_condim: wp.array(dtype=int), - geom_solref: wp.array2d(dtype=wp.vec2), - geom_solimp: wp.array2d(dtype=vec5), - geom_size: wp.array2d(dtype=wp.vec3), - geom_friction: wp.array2d(dtype=wp.vec3), - geom_margin: wp.array2d(dtype=float), - flex_contype: wp.array(dtype=int), - flex_conaffinity: wp.array(dtype=int), - flex_margin: wp.array(dtype=float), - flex_dim: wp.array(dtype=int), - flex_vertadr: wp.array(dtype=int), - flex_shellnum: wp.array(dtype=int), - flex_shelldataadr: wp.array(dtype=int), - flex_shell: wp.array(dtype=int), - flex_radius: wp.array(dtype=float), + geom_type: wp.array[int], + geom_contype: wp.array[int], + geom_conaffinity: wp.array[int], + geom_condim: wp.array[int], + geom_solref: wp.array2d[wp.vec2], + geom_solimp: wp.array2d[vec5], + geom_size: wp.array2d[wp.vec3], + geom_friction: wp.array2d[wp.vec3], + geom_margin: wp.array2d[float], + flex_contype: wp.array[int], + flex_conaffinity: wp.array[int], + flex_margin: wp.array[float], + flex_dim: wp.array[int], + flex_vertadr: wp.array[int], + flex_shellnum: wp.array[int], + flex_shelldataadr: wp.array[int], + flex_shell: wp.array[int], + flex_radius: wp.array[float], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], + flexvert_xpos_in: wp.array2d[wp.vec3], nworld_in: int, naconmax_in: int, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_flex_out: wp.array(dtype=wp.vec2i), - contact_vert_out: wp.array(dtype=wp.vec2i), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_flex_out: wp.array[wp.vec2i], + contact_vert_out: wp.array[wp.vec2i], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): worldid, shellid = wp.tid() diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_gjk.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_gjk.py index fe1c4445..21e61b7c 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_gjk.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_gjk.py @@ -63,20 +63,20 @@ class Polytope: status: int # vertices in polytope (packed geom1 followed by geom2) - vert: wp.array(dtype=wp.vec3) - vert_index: wp.array(dtype=int) + vert: wp.array[wp.vec3] + vert_index: wp.array[int] nvert: int # faces in polytope # 10 bits per each vertex index, while the last significant bits are for # invalid and deleted face - face: wp.array(dtype=int) - face_pr: wp.array(dtype=wp.vec3) - face_norm2: wp.array(dtype=float) + face: wp.array[int] + face_pr: wp.array[wp.vec3] + face_norm2: wp.array[float] nface: int # edges that make up the horizon when adding new vertices to polytope - horizon: wp.array(dtype=int) + horizon: wp.array[int] nhorizon: int @@ -1335,7 +1335,7 @@ def _area4(a: wp.vec3, b: wp.vec3, c: wp.vec3, d: wp.vec3) -> float: @wp.func -def _polygon_quad(polygon: wp.array(dtype=wp.vec3), npolygon: int) -> wp.vec4i: +def _polygon_quad(polygon: wp.array[wp.vec3], npolygon: int) -> wp.vec4i: """Returns the indices of a quadrilateral of maximum area in a convex polygon (npolygon > 4).""" b = int(1) c = int(2) @@ -1376,7 +1376,7 @@ def _polygon_quad(polygon: wp.array(dtype=wp.vec3), npolygon: int) -> wp.vec4i: # return number (1, 2 or 3) of dimensions of a simplex; reorder vertices if necessary @wp.func def _feature_dim( - face: wp.vec3i, vert_index: wp.array(dtype=int), vert: wp.array(dtype=wp.vec3), offset: int + face: wp.vec3i, vert_index: wp.array[int], vert: wp.array[wp.vec3], offset: int ) -> Tuple[int, wp.vec3i, wp.mat33]: v1i = vert_index[2 * face[0] + offset] v2i = vert_index[2 * face[1] + offset] @@ -1401,9 +1401,7 @@ def _feature_dim( # find two normals that are facing each other within a tolerance, return 1 if found @wp.func -def _aligned_faces( - vert1: wp.array(dtype=wp.vec3), len1: int, vert2: wp.array(dtype=wp.vec3), len2: int -) -> Tuple[int, wp.vec2i]: +def _aligned_faces(vert1: wp.array[wp.vec3], len1: int, vert2: wp.array[wp.vec3], len2: int) -> Tuple[int, wp.vec2i]: res = wp.vec2i() for i in range(len1): for j in range(len2): @@ -1417,9 +1415,7 @@ def _aligned_faces( # find two normals that are perpendicular to each other within a tolerance # return 1 if found @wp.func -def _aligned_face_edge( - edge: wp.array(dtype=wp.vec3), nedge: int, face: wp.array(dtype=wp.vec3), nface: int -) -> Tuple[int, wp.vec2i]: +def _aligned_face_edge(edge: wp.array[wp.vec3], nedge: int, face: wp.array[wp.vec3], nface: int) -> Tuple[int, wp.vec2i]: res = wp.vec2i() for i in range(nface): for j in range(nedge): @@ -1432,9 +1428,7 @@ def _aligned_face_edge( # find up to n <= 2 common integers of two arrays, return n @wp.func -def _intersect1( - a1: wp.array(dtype=int), a2: wp.array(dtype=int), start1: int, start2: int, len1: int, len2: int -) -> Tuple[int, wp.vec2i]: +def _intersect1(a1: wp.array[int], a2: wp.array[int], start1: int, start2: int, len1: int, len2: int) -> Tuple[int, wp.vec2i]: count = int(0) res = wp.vec2i() for i in range(start1, start1 + len1): @@ -1448,7 +1442,7 @@ def _intersect1( @wp.func -def _intersect2(a1: wp.vec2i, a2: wp.array(dtype=int), start2: int, len1: int, len2: int) -> Tuple[int, wp.vec2i]: +def _intersect2(a1: wp.vec2i, a2: wp.array[int], start2: int, len1: int, len2: int) -> Tuple[int, wp.vec2i]: count = int(0) res = wp.vec2i() for i in range(len1): @@ -1470,13 +1464,13 @@ def _mesh_normals( mat: wp.mat33, vertadr: int, polyadr: int, - polynormal: wp.array(dtype=wp.vec3), - polymapadr: wp.array(dtype=int), - polymapnum: wp.array(dtype=int), - polymap: wp.array(dtype=int), + polynormal: wp.array[wp.vec3], + polymapadr: wp.array[int], + polymapnum: wp.array[int], + polymap: wp.array[int], # Out: - normals_out: wp.array(dtype=wp.vec3), - indices_out: wp.array(dtype=int), + normals_out: wp.array[wp.vec3], + indices_out: wp.array[int], ) -> int: v1 = feature_index[0] v2 = feature_index[1] @@ -1540,19 +1534,19 @@ def _mesh_edge_normals( pos: wp.vec3, vertadr: int, polyadr: int, - vert: wp.array(dtype=wp.vec3), - polyvertadr: wp.array(dtype=int), - polyvertnum: wp.array(dtype=int), - polyvert: wp.array(dtype=int), - polymapadr: wp.array(dtype=int), - polymapnum: wp.array(dtype=int), - polymap: wp.array(dtype=int), + vert: wp.array[wp.vec3], + polyvertadr: wp.array[int], + polyvertnum: wp.array[int], + polyvert: wp.array[int], + polymapadr: wp.array[int], + polymapnum: wp.array[int], + polymap: wp.array[int], v1: wp.vec3, v2: wp.vec3, v1i: int, # Out: - normals_out: wp.array(dtype=wp.vec3), - endverts_out: wp.array(dtype=wp.vec3), + normals_out: wp.array[wp.vec3], + endverts_out: wp.array[wp.vec3], ) -> int: # only one edge if dim == 2: @@ -1586,8 +1580,8 @@ def _box_normals2( mat: wp.mat33, n: wp.vec3, # Out: - normal_out: wp.array(dtype=wp.vec3), - index_out: wp.array(dtype=int), + normal_out: wp.array[wp.vec3], + index_out: wp.array[int], ) -> int: # list of box face normals face_normals = mat63(1.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, -1.0) @@ -1621,8 +1615,8 @@ def _box_normals( mat: wp.mat33, dir: wp.vec3, # Out: - normal_out: wp.array(dtype=wp.vec3), - index_out: wp.array(dtype=int), + normal_out: wp.array[wp.vec3], + index_out: wp.array[int], ) -> int: v1 = feature_index[0] v2 = feature_index[1] @@ -1669,7 +1663,7 @@ def _box_normals( # c is 1 if edge is diagonal of a box face # c is 2 if edge is an external edge of box if c == 1 or c == 2: - return 2 + return c return _box_normals2(mat, dir, normal_out, index_out) if feature_dim == 1: @@ -1698,8 +1692,8 @@ def _box_edge_normals( v2: wp.vec3, v1i: int, # Out: - normal_out: wp.array(dtype=wp.vec3), - endvert_out: wp.array(dtype=wp.vec3), + normal_out: wp.array[wp.vec3], + endvert_out: wp.array[wp.vec3], ) -> int: if dim == 2: endvert_out[0] = v2 @@ -1726,7 +1720,7 @@ def _box_edge_normals( # recover face of a box from its index @wp.func -def _box_face(mat: wp.mat33, pos: wp.vec3, size: wp.vec3, idx: int, face_out: wp.array(dtype=wp.vec3)) -> int: +def _box_face(mat: wp.mat33, pos: wp.vec3, size: wp.vec3, idx: int, face_out: wp.array[wp.vec3]) -> int: # compute global coordinates of the box face and face normal if idx == 0: # right face_out[0] = mat @ wp.vec3(size[0], size[1], size[2]) + pos @@ -1775,13 +1769,13 @@ def _mesh_face( pos: wp.vec3, vertadr: int, polyadr: int, - vert: wp.array(dtype=wp.vec3), - polyvertadr: wp.array(dtype=int), - polyvertnum: wp.array(dtype=int), - polyvert: wp.array(dtype=int), + vert: wp.array[wp.vec3], + polyvertadr: wp.array[int], + polyvertnum: wp.array[int], + polyvert: wp.array[int], idx: int, # Out: - face_out: wp.array(dtype=wp.vec3), + face_out: wp.array[wp.vec3], ) -> int: adr = polyvertadr[polyadr + idx] j = int(0) @@ -1821,17 +1815,17 @@ def _plane_intersect(pn: wp.vec3, pd: float, a: wp.vec3, b: wp.vec3) -> float: @wp.func def _polygon_clip( # In: - plane_normal: wp.array(dtype=wp.vec3), - plane_dist: wp.array(dtype=float), - face1: wp.array(dtype=wp.vec3), + plane_normal: wp.array[wp.vec3], + plane_dist: wp.array[float], + face1: wp.array[wp.vec3], nface1: int, - face2: wp.array(dtype=wp.vec3), + face2: wp.array[wp.vec3], nface2: int, n: wp.vec3, dir: wp.vec3, # Out: - polygon_out: wp.array(dtype=wp.vec3), - clipped_out: wp.array(dtype=wp.vec3), + polygon_out: wp.array[wp.vec3], + clipped_out: wp.array[wp.vec3], ) -> Tuple[int, mat43, mat43]: witness1 = mat43() witness2 = mat43() @@ -1918,13 +1912,13 @@ def _polygon_clip( @wp.func def _set_edge( # In: - vert1: wp.array(dtype=wp.vec3), - vert2: wp.array(dtype=wp.vec3), + vert1: wp.array[wp.vec3], + vert2: wp.array[wp.vec3], start: int, end: int, offset: int, # Out: - face_out: wp.array(dtype=wp.vec3), + face_out: wp.array[wp.vec3], ) -> int: face_out[0] = vert1[2 * start + offset] face_out[1] = vert2[end] @@ -1935,19 +1929,19 @@ def _set_edge( @wp.func def multicontact( # In: - polygon: wp.array(dtype=wp.vec3), - clipped: wp.array(dtype=wp.vec3), - plane_normal: wp.array(dtype=wp.vec3), - plane_dist: wp.array(dtype=float), - idx1: wp.array(dtype=int), - idx2: wp.array(dtype=int), - n1: wp.array(dtype=wp.vec3), - n2: wp.array(dtype=wp.vec3), - endvert: wp.array(dtype=wp.vec3), - face1: wp.array(dtype=wp.vec3), - face2: wp.array(dtype=wp.vec3), - epa_vert: wp.array(dtype=wp.vec3), - epa_vert_index: wp.array(dtype=int), + polygon: wp.array[wp.vec3], + clipped: wp.array[wp.vec3], + plane_normal: wp.array[wp.vec3], + plane_dist: wp.array[float], + idx1: wp.array[int], + idx2: wp.array[int], + n1: wp.array[wp.vec3], + n2: wp.array[wp.vec3], + endvert: wp.array[wp.vec3], + face1: wp.array[wp.vec3], + face2: wp.array[wp.vec3], + epa_vert: wp.array[wp.vec3], + epa_vert_index: wp.array[int], epa_face: int, x1: wp.vec3, x2: wp.vec3, @@ -2215,12 +2209,12 @@ def ccd( geomtype2: int, x_1: wp.vec3, x_2: wp.vec3, - vert: wp.array(dtype=wp.vec3), - vert_index: wp.array(dtype=int), - face: wp.array(dtype=int), - face_pr: wp.array(dtype=wp.vec3), - face_norm2: wp.array(dtype=float), - horizon: wp.array(dtype=int), + vert: wp.array[wp.vec3], + vert_index: wp.array[int], + face: wp.array[int], + face_pr: wp.array[wp.vec3], + face_norm2: wp.array[float], + horizon: wp.array[int], ) -> Tuple[float, int, wp.vec3, wp.vec3, int]: """General convex collision detection via GJK/EPA.""" full_margin1 = 0.0 diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive.py index f1829de4..1002c727 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_primitive.py @@ -295,21 +295,21 @@ def plane_sphere_wrapper( geoms: wp.vec2i, pairid: wp.vec2i, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): """Calculates contact between a sphere and a plane.""" normal = plane.normal @@ -367,21 +367,21 @@ def sphere_sphere_wrapper( geoms: wp.vec2i, pairid: wp.vec2i, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): """Calculates contact between two spheres.""" dist, pos, normal = sphere_sphere(sphere1.pos, sphere1.size[0], sphere2.pos, sphere2.size[0]) @@ -438,21 +438,21 @@ def sphere_capsule_wrapper( geoms: wp.vec2i, pairid: wp.vec2i, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): """Calculates one contact between a sphere and a capsule.""" # capsule axis @@ -512,21 +512,21 @@ def capsule_capsule_wrapper( geoms: wp.vec2i, pairid: wp.vec2i, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): """Calculates contacts between two capsules.""" # capsule axes @@ -598,21 +598,21 @@ def plane_capsule_wrapper( geoms: wp.vec2i, pairid: wp.vec2i, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): """Calculates contacts between a capsule and a plane.""" # capsule axis @@ -680,21 +680,21 @@ def plane_ellipsoid_wrapper( geoms: wp.vec2i, pairid: wp.vec2i, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): """Calculates contacts between an ellipsoid and a plane.""" dist, pos, normal = plane_ellipsoid(plane.normal, plane.pos, ellipsoid.pos, ellipsoid.rot, ellipsoid.size) @@ -751,21 +751,21 @@ def plane_box_wrapper( geoms: wp.vec2i, pairid: wp.vec2i, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): """Calculates contacts between a box and a plane.""" dist, pos, normal = plane_box(plane.normal, plane.pos, box.pos, box.rot, box.size) @@ -824,21 +824,21 @@ def plane_convex_wrapper( geoms: wp.vec2i, pairid: wp.vec2i, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): """Calculates contacts between a plane and a convex object.""" dist, pos, normal = plane_convex(plane.normal, plane.pos, convex) @@ -897,21 +897,21 @@ def sphere_cylinder_wrapper( geoms: wp.vec2i, pairid: wp.vec2i, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): """Calculates contacts between a sphere and a cylinder.""" # cylinder axis @@ -978,21 +978,21 @@ def plane_cylinder_wrapper( geoms: wp.vec2i, pairid: wp.vec2i, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): """Calculates contacts between a cylinder and a plane.""" # cylinder axis @@ -1061,21 +1061,21 @@ def sphere_box_wrapper( geoms: wp.vec2i, pairid: wp.vec2i, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): dist, pos, normal = sphere_box(sphere.pos, sphere.size[0], box.pos, box.rot, box.size) @@ -1131,21 +1131,21 @@ def capsule_box_wrapper( geoms: wp.vec2i, pairid: wp.vec2i, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): """Calculates contacts between a capsule and a box.""" # Extract capsule axis @@ -1216,21 +1216,21 @@ def box_box_wrapper( geoms: wp.vec2i, pairid: wp.vec2i, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): """Calculates contacts between two boxes.""" # Call the core function to get contact geometry @@ -1302,63 +1302,63 @@ def _primitive_narrowphase(primitive_collisions_types, primitive_collisions_func @wp.kernel(module="unique", enable_backward=False) def primitive_narrowphase( # Model: - geom_type: wp.array(dtype=int), - geom_condim: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_priority: wp.array(dtype=int), - geom_solmix: wp.array2d(dtype=float), - geom_solref: wp.array2d(dtype=wp.vec2), - geom_solimp: wp.array2d(dtype=vec5), - geom_size: wp.array2d(dtype=wp.vec3), - geom_friction: wp.array2d(dtype=wp.vec3), - geom_margin: wp.array2d(dtype=float), - geom_gap: wp.array2d(dtype=float), - mesh_vertadr: wp.array(dtype=int), - mesh_vertnum: wp.array(dtype=int), - mesh_graphadr: wp.array(dtype=int), - mesh_vert: wp.array(dtype=wp.vec3), - mesh_graph: wp.array(dtype=int), - mesh_polynum: wp.array(dtype=int), - mesh_polyadr: wp.array(dtype=int), - mesh_polynormal: wp.array(dtype=wp.vec3), - mesh_polyvertadr: wp.array(dtype=int), - mesh_polyvertnum: wp.array(dtype=int), - mesh_polyvert: wp.array(dtype=int), - mesh_polymapadr: wp.array(dtype=int), - mesh_polymapnum: wp.array(dtype=int), - mesh_polymap: wp.array(dtype=int), - pair_dim: wp.array(dtype=int), - pair_solref: wp.array2d(dtype=wp.vec2), - pair_solreffriction: wp.array2d(dtype=wp.vec2), - pair_solimp: wp.array2d(dtype=vec5), - pair_margin: wp.array2d(dtype=float), - pair_gap: wp.array2d(dtype=float), - pair_friction: wp.array2d(dtype=vec5), + geom_type: wp.array[int], + geom_condim: wp.array[int], + geom_dataid: wp.array2d[int], + geom_priority: wp.array[int], + geom_solmix: wp.array2d[float], + geom_solref: wp.array2d[wp.vec2], + geom_solimp: wp.array2d[vec5], + geom_size: wp.array2d[wp.vec3], + geom_friction: wp.array2d[wp.vec3], + geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], + mesh_vertadr: wp.array[int], + mesh_vertnum: wp.array[int], + mesh_graphadr: wp.array[int], + mesh_vert: wp.array[wp.vec3], + mesh_graph: wp.array[int], + mesh_polynum: wp.array[int], + mesh_polyadr: wp.array[int], + mesh_polynormal: wp.array[wp.vec3], + mesh_polyvertadr: wp.array[int], + mesh_polyvertnum: wp.array[int], + mesh_polyvert: wp.array[int], + mesh_polymapadr: wp.array[int], + mesh_polymapnum: wp.array[int], + mesh_polymap: wp.array[int], + pair_dim: wp.array[int], + pair_solref: wp.array2d[wp.vec2], + pair_solreffriction: wp.array2d[wp.vec2], + pair_solimp: wp.array2d[vec5], + pair_margin: wp.array2d[float], + pair_gap: wp.array2d[float], + pair_friction: wp.array2d[vec5], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], naconmax_in: int, - ncollision_in: wp.array(dtype=int), + ncollision_in: wp.array[int], # In: - collision_pair_in: wp.array(dtype=wp.vec2i), - collision_pairid_in: wp.array(dtype=wp.vec2i), - collision_worldid_in: wp.array(dtype=int), + collision_pair_in: wp.array[wp.vec2i], + collision_pairid_in: wp.array[wp.vec2i], + collision_worldid_in: wp.array[int], # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): tid = wp.tid() diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_sdf.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_sdf.py index 4decf33c..5937cf6e 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_sdf.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_sdf.py @@ -54,9 +54,9 @@ class AABB: class VolumeData: center: wp.vec3 half_size: wp.vec3 - oct_aabb: wp.array2d(dtype=wp.vec3) - oct_child: wp.array(dtype=vec8i) - oct_coeff: wp.array(dtype=vec8) + oct_aabb: wp.array2d[wp.vec3] + oct_child: wp.array[vec8i] + oct_coeff: wp.array[vec8] root: int = 0 valid: bool = False @@ -64,10 +64,10 @@ class VolumeData: @wp.struct class MeshData: nmeshface: int - mesh_vertadr: wp.array(dtype=int) - mesh_vert: wp.array(dtype=wp.vec3) - mesh_faceadr: wp.array(dtype=int) - mesh_face: wp.array(dtype=wp.vec3i) + mesh_vertadr: wp.array[int] + mesh_vert: wp.array[wp.vec3] + mesh_faceadr: wp.array[int] + mesh_face: wp.array[wp.vec3i] data_id: int pos: wp.vec3 mat: wp.mat33 @@ -80,12 +80,12 @@ class MeshData: @wp.func def get_sdf_params( # Model: - oct_child: wp.array(dtype=vec8i), - oct_aabb: wp.array2d(dtype=wp.vec3), - oct_coeff: wp.array(dtype=vec8), - mesh_octadr: wp.array(dtype=int), - plugin: wp.array(dtype=int), - plugin_attr: wp.array(dtype=vec_pluginattr), + oct_child: wp.array[vec8i], + oct_aabb: wp.array2d[wp.vec3], + oct_coeff: wp.array[vec8], + mesh_octadr: wp.array[int], + plugin: wp.array[int], + plugin_attr: wp.array[vec_pluginattr], # In: g_type: int, g_size: wp.vec3, @@ -252,7 +252,7 @@ def user_sdf_grad(p: wp.vec3, attr: vec_pluginattr, sdf_type: int) -> wp.vec3: @wp.func def find_oct( - oct_child: wp.array(dtype=vec8i), oct_aabb: wp.array2d(dtype=wp.vec3), p: wp.vec3, grad: bool, root: int + oct_child: wp.array[vec8i], oct_aabb: wp.array2d[wp.vec3], p: wp.vec3, grad: bool, root: int ) -> Tuple[int, Tuple[vec8, vec8, vec8]]: stack = root niter = int(100) @@ -665,75 +665,75 @@ def gradient_descent( def _sdf_narrowphase( # Model: nmeshface: int, - oct_child: wp.array(dtype=vec8i), - oct_aabb: wp.array2d(dtype=wp.vec3), - oct_coeff: wp.array(dtype=vec8), - geom_type: wp.array(dtype=int), - geom_condim: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_priority: wp.array(dtype=int), - geom_solmix: wp.array2d(dtype=float), - geom_solref: wp.array2d(dtype=wp.vec2), - geom_solimp: wp.array2d(dtype=vec5), - geom_size: wp.array2d(dtype=wp.vec3), - geom_aabb: wp.array3d(dtype=wp.vec3), - geom_friction: wp.array2d(dtype=wp.vec3), - geom_margin: wp.array2d(dtype=float), - geom_gap: wp.array2d(dtype=float), - mesh_vertadr: wp.array(dtype=int), - mesh_vertnum: wp.array(dtype=int), - mesh_faceadr: wp.array(dtype=int), - mesh_octadr: wp.array(dtype=int), - mesh_graphadr: wp.array(dtype=int), - mesh_vert: wp.array(dtype=wp.vec3), - mesh_face: wp.array(dtype=wp.vec3i), - mesh_graph: wp.array(dtype=int), - mesh_polynum: wp.array(dtype=int), - mesh_polyadr: wp.array(dtype=int), - mesh_polynormal: wp.array(dtype=wp.vec3), - mesh_polyvertadr: wp.array(dtype=int), - mesh_polyvertnum: wp.array(dtype=int), - mesh_polyvert: wp.array(dtype=int), - mesh_polymapadr: wp.array(dtype=int), - mesh_polymapnum: wp.array(dtype=int), - mesh_polymap: wp.array(dtype=int), - pair_dim: wp.array(dtype=int), - pair_solref: wp.array2d(dtype=wp.vec2), - pair_solreffriction: wp.array2d(dtype=wp.vec2), - pair_solimp: wp.array2d(dtype=vec5), - pair_margin: wp.array2d(dtype=float), - pair_gap: wp.array2d(dtype=float), - pair_friction: wp.array2d(dtype=vec5), - plugin: wp.array(dtype=int), - plugin_attr: wp.array(dtype=vec_pluginattr), - geom_plugin_index: wp.array(dtype=int), + oct_child: wp.array[vec8i], + oct_aabb: wp.array2d[wp.vec3], + oct_coeff: wp.array[vec8], + geom_type: wp.array[int], + geom_condim: wp.array[int], + geom_dataid: wp.array2d[int], + geom_priority: wp.array[int], + geom_solmix: wp.array2d[float], + geom_solref: wp.array2d[wp.vec2], + geom_solimp: wp.array2d[vec5], + geom_size: wp.array2d[wp.vec3], + geom_aabb: wp.array3d[wp.vec3], + geom_friction: wp.array2d[wp.vec3], + geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], + mesh_vertadr: wp.array[int], + mesh_vertnum: wp.array[int], + mesh_faceadr: wp.array[int], + mesh_octadr: wp.array[int], + mesh_graphadr: wp.array[int], + mesh_vert: wp.array[wp.vec3], + mesh_face: wp.array[wp.vec3i], + mesh_graph: wp.array[int], + mesh_polynum: wp.array[int], + mesh_polyadr: wp.array[int], + mesh_polynormal: wp.array[wp.vec3], + mesh_polyvertadr: wp.array[int], + mesh_polyvertnum: wp.array[int], + mesh_polyvert: wp.array[int], + mesh_polymapadr: wp.array[int], + mesh_polymapnum: wp.array[int], + mesh_polymap: wp.array[int], + pair_dim: wp.array[int], + pair_solref: wp.array2d[wp.vec2], + pair_solreffriction: wp.array2d[wp.vec2], + pair_solimp: wp.array2d[vec5], + pair_margin: wp.array2d[float], + pair_gap: wp.array2d[float], + pair_friction: wp.array2d[vec5], + plugin: wp.array[int], + plugin_attr: wp.array[vec_pluginattr], + geom_plugin_index: wp.array[int], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], naconmax_in: int, - ncollision_in: wp.array(dtype=int), + ncollision_in: wp.array[int], # In: - collision_pair_in: wp.array(dtype=wp.vec2i), - collision_pairid_in: wp.array(dtype=wp.vec2i), - collision_worldid_in: wp.array(dtype=int), + collision_pair_in: wp.array[wp.vec2i], + collision_pairid_in: wp.array[wp.vec2i], + collision_worldid_in: wp.array[int], sdf_initpoints: int, sdf_iterations: int, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), - nacon_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], + nacon_out: wp.array[int], ): i, contact_tid = wp.tid() if i >= sdf_initpoints: @@ -817,12 +817,32 @@ def _sdf_narrowphase( pos1 = geom1.pos rot1 = geom1.rot + dataid_setid = worldid % geom_dataid.shape[0] + attr1, g1_plugin_id, volume_data1, mesh_data1 = get_sdf_params( - oct_child, oct_aabb, oct_coeff, mesh_octadr, plugin, plugin_attr, type1, geom1.size, g1_plugin, geom_dataid[g1] + oct_child, + oct_aabb, + oct_coeff, + mesh_octadr, + plugin, + plugin_attr, + type1, + geom1.size, + g1_plugin, + geom_dataid[dataid_setid, g1], ) attr2, g2_plugin_id, volume_data2, mesh_data2 = get_sdf_params( - oct_child, oct_aabb, oct_coeff, mesh_octadr, plugin, plugin_attr, type2, geom2.size, g2_plugin, geom_dataid[g2] + oct_child, + oct_aabb, + oct_coeff, + mesh_octadr, + plugin, + plugin_attr, + type2, + geom2.size, + g2_plugin, + geom_dataid[dataid_setid, g2], ) mesh_data1.nmeshface = nmeshface @@ -830,7 +850,7 @@ def _sdf_narrowphase( mesh_data1.mesh_vert = mesh_vert mesh_data1.mesh_faceadr = mesh_faceadr mesh_data1.mesh_face = mesh_face - mesh_data1.data_id = geom_dataid[g1] + mesh_data1.data_id = geom_dataid[dataid_setid, g1] mesh_data1.pos = geom1.pos mesh_data1.mat = geom1.rot mesh_data1.size = geom1.size @@ -843,7 +863,7 @@ def _sdf_narrowphase( mesh_data2.mesh_vert = mesh_vert mesh_data2.mesh_faceadr = mesh_faceadr mesh_data2.mesh_face = mesh_face - mesh_data2.data_id = geom_dataid[g2] + mesh_data2.data_id = geom_dataid[dataid_setid, g2] mesh_data2.pos = geom2.pos mesh_data2.mat = geom2.rot mesh_data2.size = geom2.size diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/constraint.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/constraint.py index eec47583..788f1af5 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/constraint.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/constraint.py @@ -32,12 +32,12 @@ wp.set_module_options({"enable_backward": False}) @wp.kernel def _zero_constraint_counts( # Data out: - ne_out: wp.array(dtype=int), - nf_out: wp.array(dtype=int), - nl_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), + ne_out: wp.array[int], + nf_out: wp.array[int], + nl_out: wp.array[int], + nefc_out: wp.array[int], # Out: - efc_nnz_out: wp.array(dtype=int), + efc_nnz_out: wp.array[int], ): worldid = wp.tid() @@ -68,14 +68,14 @@ def _efc_row( type: int, id: int, # Out: - type_out: wp.array2d(dtype=int), - id_out: wp.array2d(dtype=int), - pos_out: wp.array2d(dtype=float), - margin_out: wp.array2d(dtype=float), - D_out: wp.array2d(dtype=float), - vel_out: wp.array2d(dtype=float), - aref_out: wp.array2d(dtype=float), - frictionloss_out: wp.array2d(dtype=float), + type_out: wp.array2d[int], + id_out: wp.array2d[int], + pos_out: wp.array2d[float], + margin_out: wp.array2d[float], + D_out: wp.array2d[float], + vel_out: wp.array2d[float], + aref_out: wp.array2d[float], + frictionloss_out: wp.array2d[float], ): # calculate kbi timeconst = solref[0] @@ -126,52 +126,52 @@ def _equality_connect( # Model: nv: int, nsite: int, - opt_timestep: wp.array(dtype=float), + opt_timestep: wp.array[float], opt_disableflags: int, - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - body_weldid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - body_invweight0: wp.array2d(dtype=wp.vec2), - dof_bodyid: wp.array(dtype=int), - dof_parentid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - eq_obj1id: wp.array(dtype=int), - eq_obj2id: wp.array(dtype=int), - eq_objtype: wp.array(dtype=int), - eq_solref: wp.array2d(dtype=wp.vec2), - eq_solimp: wp.array2d(dtype=vec5), - eq_data: wp.array2d(dtype=vec11), + body_parentid: wp.array[int], + body_rootid: wp.array[int], + body_weldid: wp.array[int], + body_dofnum: wp.array[int], + body_dofadr: wp.array[int], + body_invweight0: wp.array2d[wp.vec2], + dof_bodyid: wp.array[int], + dof_parentid: wp.array[int], + site_bodyid: wp.array[int], + eq_obj1id: wp.array[int], + eq_obj2id: wp.array[int], + eq_objtype: wp.array[int], + eq_solref: wp.array2d[wp.vec2], + eq_solimp: wp.array2d[vec5], + eq_data: wp.array2d[vec11], is_sparse: bool, - eq_connect_adr: wp.array(dtype=int), + eq_connect_adr: wp.array[int], # Data in: - qvel_in: wp.array2d(dtype=float), - eq_active_in: wp.array2d(dtype=bool), - xpos_in: wp.array2d(dtype=wp.vec3), - xmat_in: wp.array2d(dtype=wp.mat33), - site_xpos_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), + qvel_in: wp.array2d[float], + eq_active_in: wp.array2d[bool], + xpos_in: wp.array2d[wp.vec3], + xmat_in: wp.array2d[wp.mat33], + site_xpos_in: wp.array2d[wp.vec3], + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], njmax_in: int, njmax_nnz_in: int, # Data out: - ne_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + ne_out: wp.array[int], + nefc_out: wp.array[int], + efc_type_out: wp.array2d[int], + efc_id_out: wp.array2d[int], + efc_J_rownnz_out: wp.array2d[int], + efc_J_rowadr_out: wp.array2d[int], + efc_J_colind_out: wp.array3d[int], + efc_J_out: wp.array3d[float], + efc_pos_out: wp.array2d[float], + efc_margin_out: wp.array2d[float], + efc_D_out: wp.array2d[float], + efc_vel_out: wp.array2d[float], + efc_aref_out: wp.array2d[float], + efc_frictionloss_out: wp.array2d[float], # Out: - efc_nnz_out: wp.array(dtype=int), + efc_nnz_out: wp.array[int], ): """Calculates constraint rows for connect equality constraints.""" worldid, eqconnectid = wp.tid() @@ -368,42 +368,42 @@ def _equality_connect( def _equality_joint( # Model: nv: int, - opt_timestep: wp.array(dtype=float), + opt_timestep: wp.array[float], opt_disableflags: int, - qpos0: wp.array2d(dtype=float), - jnt_qposadr: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - dof_invweight0: wp.array2d(dtype=float), - eq_obj1id: wp.array(dtype=int), - eq_obj2id: wp.array(dtype=int), - eq_solref: wp.array2d(dtype=wp.vec2), - eq_solimp: wp.array2d(dtype=vec5), - eq_data: wp.array2d(dtype=vec11), + qpos0: wp.array2d[float], + jnt_qposadr: wp.array[int], + jnt_dofadr: wp.array[int], + dof_invweight0: wp.array2d[float], + eq_obj1id: wp.array[int], + eq_obj2id: wp.array[int], + eq_solref: wp.array2d[wp.vec2], + eq_solimp: wp.array2d[vec5], + eq_data: wp.array2d[vec11], is_sparse: bool, - eq_jnt_adr: wp.array(dtype=int), + eq_jnt_adr: wp.array[int], # Data in: - qpos_in: wp.array2d(dtype=float), - qvel_in: wp.array2d(dtype=float), - eq_active_in: wp.array2d(dtype=bool), + qpos_in: wp.array2d[float], + qvel_in: wp.array2d[float], + eq_active_in: wp.array2d[bool], njmax_in: int, njmax_nnz_in: int, # Data out: - ne_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + ne_out: wp.array[int], + nefc_out: wp.array[int], + efc_type_out: wp.array2d[int], + efc_id_out: wp.array2d[int], + efc_J_rownnz_out: wp.array2d[int], + efc_J_rowadr_out: wp.array2d[int], + efc_J_colind_out: wp.array3d[int], + efc_J_out: wp.array3d[float], + efc_pos_out: wp.array2d[float], + efc_margin_out: wp.array2d[float], + efc_D_out: wp.array2d[float], + efc_vel_out: wp.array2d[float], + efc_aref_out: wp.array2d[float], + efc_frictionloss_out: wp.array2d[float], # Out: - efc_nnz_out: wp.array(dtype=int), + efc_nnz_out: wp.array[int], ): worldid, eqjntid = wp.tid() eqid = eq_jnt_adr[eqjntid] @@ -499,44 +499,44 @@ def _equality_joint( def _equality_tendon( # Model: nv: int, - opt_timestep: wp.array(dtype=float), + opt_timestep: wp.array[float], opt_disableflags: int, - eq_obj1id: wp.array(dtype=int), - eq_obj2id: wp.array(dtype=int), - eq_solref: wp.array2d(dtype=wp.vec2), - eq_solimp: wp.array2d(dtype=vec5), - eq_data: wp.array2d(dtype=vec11), - ten_J_rownnz: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - tendon_length0: wp.array2d(dtype=float), - tendon_invweight0: wp.array2d(dtype=float), + eq_obj1id: wp.array[int], + eq_obj2id: wp.array[int], + eq_solref: wp.array2d[wp.vec2], + eq_solimp: wp.array2d[vec5], + eq_data: wp.array2d[vec11], + ten_J_rownnz: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_colind: wp.array[int], + tendon_length0: wp.array2d[float], + tendon_invweight0: wp.array2d[float], is_sparse: bool, - eq_ten_adr: wp.array(dtype=int), + eq_ten_adr: wp.array[int], # Data in: - qvel_in: wp.array2d(dtype=float), - eq_active_in: wp.array2d(dtype=bool), - ten_J_in: wp.array2d(dtype=float), - ten_length_in: wp.array2d(dtype=float), + qvel_in: wp.array2d[float], + eq_active_in: wp.array2d[bool], + ten_J_in: wp.array2d[float], + ten_length_in: wp.array2d[float], njmax_in: int, njmax_nnz_in: int, # Data out: - ne_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + ne_out: wp.array[int], + nefc_out: wp.array[int], + efc_type_out: wp.array2d[int], + efc_id_out: wp.array2d[int], + efc_J_rownnz_out: wp.array2d[int], + efc_J_rowadr_out: wp.array2d[int], + efc_J_colind_out: wp.array3d[int], + efc_J_out: wp.array3d[float], + efc_pos_out: wp.array2d[float], + efc_margin_out: wp.array2d[float], + efc_D_out: wp.array2d[float], + efc_vel_out: wp.array2d[float], + efc_aref_out: wp.array2d[float], + efc_frictionloss_out: wp.array2d[float], # Out: - efc_nnz_out: wp.array(dtype=int), + efc_nnz_out: wp.array[int], ): worldid, eqtenid = wp.tid() eqid = eq_ten_adr[eqtenid] @@ -679,42 +679,42 @@ def _equality_flex(is_sparse: bool): def kernel( # Model: nv: int, - opt_timestep: wp.array(dtype=float), + opt_timestep: wp.array[float], opt_disableflags: int, - flex_edgeadr: wp.array(dtype=int), - flex_edgenum: wp.array(dtype=int), - flexedge_length0: wp.array(dtype=float), - flexedge_invweight0: wp.array(dtype=float), - flexedge_J_rownnz: wp.array(dtype=int), - flexedge_J_rowadr: wp.array(dtype=int), - flexedge_J_colind: wp.array(dtype=int), - eq_obj1id: wp.array(dtype=int), - eq_solref: wp.array2d(dtype=wp.vec2), - eq_solimp: wp.array2d(dtype=vec5), - eq_flex_adr: wp.array(dtype=int), + flex_edgeadr: wp.array[int], + flex_edgenum: wp.array[int], + flexedge_length0: wp.array[float], + flexedge_invweight0: wp.array[float], + flexedge_J_rownnz: wp.array[int], + flexedge_J_rowadr: wp.array[int], + flexedge_J_colind: wp.array[int], + eq_obj1id: wp.array[int], + eq_solref: wp.array2d[wp.vec2], + eq_solimp: wp.array2d[vec5], + eq_flex_adr: wp.array[int], # Data in: - qvel_in: wp.array2d(dtype=float), - flexedge_J_in: wp.array2d(dtype=float), - flexedge_length_in: wp.array2d(dtype=float), + qvel_in: wp.array2d[float], + flexedge_J_in: wp.array2d[float], + flexedge_length_in: wp.array2d[float], njmax_in: int, njmax_nnz_in: int, # Data out: - ne_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + ne_out: wp.array[int], + nefc_out: wp.array[int], + efc_type_out: wp.array2d[int], + efc_id_out: wp.array2d[int], + efc_J_rownnz_out: wp.array2d[int], + efc_J_rowadr_out: wp.array2d[int], + efc_J_colind_out: wp.array3d[int], + efc_J_out: wp.array3d[float], + efc_pos_out: wp.array2d[float], + efc_margin_out: wp.array2d[float], + efc_D_out: wp.array2d[float], + efc_vel_out: wp.array2d[float], + efc_aref_out: wp.array2d[float], + efc_frictionloss_out: wp.array2d[float], # Out: - efc_nnz_out: wp.array(dtype=int), + efc_nnz_out: wp.array[int], ): worldid, eqflexid, edgeid = wp.tid() eqid = eq_flex_adr[eqflexid] @@ -794,54 +794,54 @@ def _equality_weld( # Model: nv: int, nsite: int, - opt_timestep: wp.array(dtype=float), + opt_timestep: wp.array[float], opt_disableflags: int, - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - body_weldid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - body_invweight0: wp.array2d(dtype=wp.vec2), - dof_bodyid: wp.array(dtype=int), - dof_parentid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - site_quat: wp.array2d(dtype=wp.quat), - eq_obj1id: wp.array(dtype=int), - eq_obj2id: wp.array(dtype=int), - eq_objtype: wp.array(dtype=int), - eq_solref: wp.array2d(dtype=wp.vec2), - eq_solimp: wp.array2d(dtype=vec5), - eq_data: wp.array2d(dtype=vec11), + body_parentid: wp.array[int], + body_rootid: wp.array[int], + body_weldid: wp.array[int], + body_dofnum: wp.array[int], + body_dofadr: wp.array[int], + body_invweight0: wp.array2d[wp.vec2], + dof_bodyid: wp.array[int], + dof_parentid: wp.array[int], + site_bodyid: wp.array[int], + site_quat: wp.array2d[wp.quat], + eq_obj1id: wp.array[int], + eq_obj2id: wp.array[int], + eq_objtype: wp.array[int], + eq_solref: wp.array2d[wp.vec2], + eq_solimp: wp.array2d[vec5], + eq_data: wp.array2d[vec11], is_sparse: bool, - eq_wld_adr: wp.array(dtype=int), + eq_wld_adr: wp.array[int], # Data in: - qvel_in: wp.array2d(dtype=float), - eq_active_in: wp.array2d(dtype=bool), - xpos_in: wp.array2d(dtype=wp.vec3), - xquat_in: wp.array2d(dtype=wp.quat), - xmat_in: wp.array2d(dtype=wp.mat33), - site_xpos_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), + qvel_in: wp.array2d[float], + eq_active_in: wp.array2d[bool], + xpos_in: wp.array2d[wp.vec3], + xquat_in: wp.array2d[wp.quat], + xmat_in: wp.array2d[wp.mat33], + site_xpos_in: wp.array2d[wp.vec3], + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], njmax_in: int, njmax_nnz_in: int, # Data out: - ne_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + ne_out: wp.array[int], + nefc_out: wp.array[int], + efc_type_out: wp.array2d[int], + efc_id_out: wp.array2d[int], + efc_J_rownnz_out: wp.array2d[int], + efc_J_rowadr_out: wp.array2d[int], + efc_J_colind_out: wp.array3d[int], + efc_J_out: wp.array3d[float], + efc_pos_out: wp.array2d[float], + efc_margin_out: wp.array2d[float], + efc_D_out: wp.array2d[float], + efc_vel_out: wp.array2d[float], + efc_aref_out: wp.array2d[float], + efc_frictionloss_out: wp.array2d[float], # Out: - efc_nnz_out: wp.array(dtype=int), + efc_nnz_out: wp.array[int], ): worldid, eqweldid = wp.tid() eqid = eq_wld_adr[eqweldid] @@ -1114,34 +1114,34 @@ def _equality_weld( def _friction_dof( # Model: nv: int, - opt_timestep: wp.array(dtype=float), + opt_timestep: wp.array[float], opt_disableflags: int, - dof_solref: wp.array2d(dtype=wp.vec2), - dof_solimp: wp.array2d(dtype=vec5), - dof_frictionloss: wp.array2d(dtype=float), - dof_invweight0: wp.array2d(dtype=float), + dof_solref: wp.array2d[wp.vec2], + dof_solimp: wp.array2d[vec5], + dof_frictionloss: wp.array2d[float], + dof_invweight0: wp.array2d[float], is_sparse: bool, # Data in: - qvel_in: wp.array2d(dtype=float), + qvel_in: wp.array2d[float], njmax_in: int, njmax_nnz_in: int, # Data out: - nf_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + nf_out: wp.array[int], + nefc_out: wp.array[int], + efc_type_out: wp.array2d[int], + efc_id_out: wp.array2d[int], + efc_J_rownnz_out: wp.array2d[int], + efc_J_rowadr_out: wp.array2d[int], + efc_J_colind_out: wp.array3d[int], + efc_J_out: wp.array3d[float], + efc_pos_out: wp.array2d[float], + efc_margin_out: wp.array2d[float], + efc_D_out: wp.array2d[float], + efc_vel_out: wp.array2d[float], + efc_aref_out: wp.array2d[float], + efc_frictionloss_out: wp.array2d[float], # Out: - efc_nnz_out: wp.array(dtype=int), + efc_nnz_out: wp.array[int], ): worldid, dofid = wp.tid() @@ -1204,38 +1204,38 @@ def _friction_dof( def _friction_tendon( # Model: nv: int, - opt_timestep: wp.array(dtype=float), + opt_timestep: wp.array[float], opt_disableflags: int, - ten_J_rownnz: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - tendon_solref_fri: wp.array2d(dtype=wp.vec2), - tendon_solimp_fri: wp.array2d(dtype=vec5), - tendon_frictionloss: wp.array2d(dtype=float), - tendon_invweight0: wp.array2d(dtype=float), + ten_J_rownnz: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_colind: wp.array[int], + tendon_solref_fri: wp.array2d[wp.vec2], + tendon_solimp_fri: wp.array2d[vec5], + tendon_frictionloss: wp.array2d[float], + tendon_invweight0: wp.array2d[float], is_sparse: bool, # Data in: - qvel_in: wp.array2d(dtype=float), - ten_J_in: wp.array2d(dtype=float), + qvel_in: wp.array2d[float], + ten_J_in: wp.array2d[float], njmax_in: int, njmax_nnz_in: int, # Data out: - nf_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + nf_out: wp.array[int], + nefc_out: wp.array[int], + efc_type_out: wp.array2d[int], + efc_id_out: wp.array2d[int], + efc_J_rownnz_out: wp.array2d[int], + efc_J_rowadr_out: wp.array2d[int], + efc_J_colind_out: wp.array3d[int], + efc_J_out: wp.array3d[float], + efc_pos_out: wp.array2d[float], + efc_margin_out: wp.array2d[float], + efc_D_out: wp.array2d[float], + efc_vel_out: wp.array2d[float], + efc_aref_out: wp.array2d[float], + efc_frictionloss_out: wp.array2d[float], # Out: - efc_nnz_out: wp.array(dtype=int), + efc_nnz_out: wp.array[int], ): worldid, tenid = wp.tid() @@ -1317,39 +1317,39 @@ def _friction_tendon( def _limit_slide_hinge( # Model: nv: int, - opt_timestep: wp.array(dtype=float), + opt_timestep: wp.array[float], opt_disableflags: int, - jnt_qposadr: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - jnt_solref: wp.array2d(dtype=wp.vec2), - jnt_solimp: wp.array2d(dtype=vec5), - jnt_range: wp.array2d(dtype=wp.vec2), - jnt_margin: wp.array2d(dtype=float), - dof_invweight0: wp.array2d(dtype=float), + jnt_qposadr: wp.array[int], + jnt_dofadr: wp.array[int], + jnt_solref: wp.array2d[wp.vec2], + jnt_solimp: wp.array2d[vec5], + jnt_range: wp.array2d[wp.vec2], + jnt_margin: wp.array2d[float], + dof_invweight0: wp.array2d[float], is_sparse: bool, - jnt_limited_slide_hinge_adr: wp.array(dtype=int), + jnt_limited_slide_hinge_adr: wp.array[int], # Data in: - qpos_in: wp.array2d(dtype=float), - qvel_in: wp.array2d(dtype=float), + qpos_in: wp.array2d[float], + qvel_in: wp.array2d[float], njmax_in: int, njmax_nnz_in: int, # Data out: - nl_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + nl_out: wp.array[int], + nefc_out: wp.array[int], + efc_type_out: wp.array2d[int], + efc_id_out: wp.array2d[int], + efc_J_rownnz_out: wp.array2d[int], + efc_J_rowadr_out: wp.array2d[int], + efc_J_colind_out: wp.array3d[int], + efc_J_out: wp.array3d[float], + efc_pos_out: wp.array2d[float], + efc_margin_out: wp.array2d[float], + efc_D_out: wp.array2d[float], + efc_vel_out: wp.array2d[float], + efc_aref_out: wp.array2d[float], + efc_frictionloss_out: wp.array2d[float], # Out: - efc_nnz_out: wp.array(dtype=int), + efc_nnz_out: wp.array[int], ): worldid, jntlimitedid = wp.tid() jntid = jnt_limited_slide_hinge_adr[jntlimitedid] @@ -1422,39 +1422,39 @@ def _limit_slide_hinge( def _limit_ball( # Model: nv: int, - opt_timestep: wp.array(dtype=float), + opt_timestep: wp.array[float], opt_disableflags: int, - jnt_qposadr: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - jnt_solref: wp.array2d(dtype=wp.vec2), - jnt_solimp: wp.array2d(dtype=vec5), - jnt_range: wp.array2d(dtype=wp.vec2), - jnt_margin: wp.array2d(dtype=float), - dof_invweight0: wp.array2d(dtype=float), + jnt_qposadr: wp.array[int], + jnt_dofadr: wp.array[int], + jnt_solref: wp.array2d[wp.vec2], + jnt_solimp: wp.array2d[vec5], + jnt_range: wp.array2d[wp.vec2], + jnt_margin: wp.array2d[float], + dof_invweight0: wp.array2d[float], is_sparse: bool, - jnt_limited_ball_adr: wp.array(dtype=int), + jnt_limited_ball_adr: wp.array[int], # Data in: - qpos_in: wp.array2d(dtype=float), - qvel_in: wp.array2d(dtype=float), + qpos_in: wp.array2d[float], + qvel_in: wp.array2d[float], njmax_in: int, njmax_nnz_in: int, # Data out: - nl_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + nl_out: wp.array[int], + nefc_out: wp.array[int], + efc_type_out: wp.array2d[int], + efc_id_out: wp.array2d[int], + efc_J_rownnz_out: wp.array2d[int], + efc_J_rowadr_out: wp.array2d[int], + efc_J_colind_out: wp.array3d[int], + efc_J_out: wp.array3d[float], + efc_pos_out: wp.array2d[float], + efc_margin_out: wp.array2d[float], + efc_D_out: wp.array2d[float], + efc_vel_out: wp.array2d[float], + efc_aref_out: wp.array2d[float], + efc_frictionloss_out: wp.array2d[float], # Out: - efc_nnz_out: wp.array(dtype=int), + efc_nnz_out: wp.array[int], ): worldid, jntlimitedid = wp.tid() jntid = jnt_limited_ball_adr[jntlimitedid] @@ -1547,41 +1547,41 @@ def _limit_ball( def _limit_tendon( # Model: nv: int, - opt_timestep: wp.array(dtype=float), + opt_timestep: wp.array[float], opt_disableflags: int, - ten_J_rownnz: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - tendon_solref_lim: wp.array2d(dtype=wp.vec2), - tendon_solimp_lim: wp.array2d(dtype=vec5), - tendon_range: wp.array2d(dtype=wp.vec2), - tendon_margin: wp.array2d(dtype=float), - tendon_invweight0: wp.array2d(dtype=float), + ten_J_rownnz: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_colind: wp.array[int], + tendon_solref_lim: wp.array2d[wp.vec2], + tendon_solimp_lim: wp.array2d[vec5], + tendon_range: wp.array2d[wp.vec2], + tendon_margin: wp.array2d[float], + tendon_invweight0: wp.array2d[float], is_sparse: bool, - tendon_limited_adr: wp.array(dtype=int), + tendon_limited_adr: wp.array[int], # Data in: - qvel_in: wp.array2d(dtype=float), - ten_J_in: wp.array2d(dtype=float), - ten_length_in: wp.array2d(dtype=float), + qvel_in: wp.array2d[float], + ten_J_in: wp.array2d[float], + ten_length_in: wp.array2d[float], njmax_in: int, njmax_nnz_in: int, # Data out: - nl_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + nl_out: wp.array[int], + nefc_out: wp.array[int], + efc_type_out: wp.array2d[int], + efc_id_out: wp.array2d[int], + efc_J_rownnz_out: wp.array2d[int], + efc_J_rowadr_out: wp.array2d[int], + efc_J_colind_out: wp.array3d[int], + efc_J_out: wp.array3d[float], + efc_pos_out: wp.array2d[float], + efc_margin_out: wp.array2d[float], + efc_D_out: wp.array2d[float], + efc_vel_out: wp.array2d[float], + efc_aref_out: wp.array2d[float], + efc_frictionloss_out: wp.array2d[float], # Out: - efc_nnz_out: wp.array(dtype=int), + efc_nnz_out: wp.array[int], ): worldid, tenlimitedid = wp.tid() tenid = tendon_limited_adr[tenlimitedid] @@ -1669,59 +1669,59 @@ def _limit_tendon( def _contact_pyramidal( # Model: nv: int, - opt_timestep: wp.array(dtype=float), + opt_timestep: wp.array[float], opt_disableflags: int, - opt_impratio_invsqrt: wp.array(dtype=float), - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - body_weldid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - body_invweight0: wp.array2d(dtype=wp.vec2), - dof_bodyid: wp.array(dtype=int), - dof_parentid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - flex_vertadr: wp.array(dtype=int), - flex_vertbodyid: wp.array(dtype=int), + opt_impratio_invsqrt: wp.array[float], + body_parentid: wp.array[int], + body_rootid: wp.array[int], + body_weldid: wp.array[int], + body_dofnum: wp.array[int], + body_dofadr: wp.array[int], + body_invweight0: wp.array2d[wp.vec2], + dof_bodyid: wp.array[int], + dof_parentid: wp.array[int], + geom_bodyid: wp.array[int], + flex_vertadr: wp.array[int], + flex_vertbodyid: wp.array[int], is_sparse: bool, # Data in: - qvel_in: wp.array2d(dtype=float), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), + qvel_in: wp.array2d[float], + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], njmax_in: int, njmax_nnz_in: int, - nacon_in: wp.array(dtype=int), + nacon_in: wp.array[int], # In: - dist_in: wp.array(dtype=float), - condim_in: wp.array(dtype=int), - includemargin_in: wp.array(dtype=float), - worldid_in: wp.array(dtype=int), - geom_in: wp.array(dtype=wp.vec2i), - flex_in: wp.array(dtype=wp.vec2i), - vert_in: wp.array(dtype=wp.vec2i), - pos_in: wp.array(dtype=wp.vec3), - frame_in: wp.array(dtype=wp.mat33), - friction_in: wp.array(dtype=vec5), - solref_in: wp.array(dtype=wp.vec2), - solimp_in: wp.array(dtype=vec5), - type_in: wp.array(dtype=int), + dist_in: wp.array[float], + condim_in: wp.array[int], + includemargin_in: wp.array[float], + worldid_in: wp.array[int], + geom_in: wp.array[wp.vec2i], + flex_in: wp.array[wp.vec2i], + vert_in: wp.array[wp.vec2i], + pos_in: wp.array[wp.vec3], + frame_in: wp.array[wp.mat33], + friction_in: wp.array[vec5], + solref_in: wp.array[wp.vec2], + solimp_in: wp.array[vec5], + type_in: wp.array[int], # Data out: - nefc_out: wp.array(dtype=int), - contact_efc_address_out: wp.array2d(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + nefc_out: wp.array[int], + contact_efc_address_out: wp.array2d[int], + efc_type_out: wp.array2d[int], + efc_id_out: wp.array2d[int], + efc_J_rownnz_out: wp.array2d[int], + efc_J_rowadr_out: wp.array2d[int], + efc_J_colind_out: wp.array3d[int], + efc_J_out: wp.array3d[float], + efc_pos_out: wp.array2d[float], + efc_margin_out: wp.array2d[float], + efc_D_out: wp.array2d[float], + efc_vel_out: wp.array2d[float], + efc_aref_out: wp.array2d[float], + efc_frictionloss_out: wp.array2d[float], # Out: - efc_nnz_out: wp.array(dtype=int), + efc_nnz_out: wp.array[int], ): conid, dimid = wp.tid() @@ -1940,60 +1940,60 @@ def _contact_pyramidal( def _contact_elliptic( # Model: nv: int, - opt_timestep: wp.array(dtype=float), + opt_timestep: wp.array[float], opt_disableflags: int, - opt_impratio_invsqrt: wp.array(dtype=float), - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - body_weldid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - body_invweight0: wp.array2d(dtype=wp.vec2), - dof_bodyid: wp.array(dtype=int), - dof_parentid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - flex_vertadr: wp.array(dtype=int), - flex_vertbodyid: wp.array(dtype=int), + opt_impratio_invsqrt: wp.array[float], + body_parentid: wp.array[int], + body_rootid: wp.array[int], + body_weldid: wp.array[int], + body_dofnum: wp.array[int], + body_dofadr: wp.array[int], + body_invweight0: wp.array2d[wp.vec2], + dof_bodyid: wp.array[int], + dof_parentid: wp.array[int], + geom_bodyid: wp.array[int], + flex_vertadr: wp.array[int], + flex_vertbodyid: wp.array[int], is_sparse: bool, # Data in: - qvel_in: wp.array2d(dtype=float), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), + qvel_in: wp.array2d[float], + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], njmax_in: int, njmax_nnz_in: int, - nacon_in: wp.array(dtype=int), + nacon_in: wp.array[int], # In: - dist_in: wp.array(dtype=float), - condim_in: wp.array(dtype=int), - includemargin_in: wp.array(dtype=float), - worldid_in: wp.array(dtype=int), - geom_in: wp.array(dtype=wp.vec2i), - flex_in: wp.array(dtype=wp.vec2i), - vert_in: wp.array(dtype=wp.vec2i), - pos_in: wp.array(dtype=wp.vec3), - frame_in: wp.array(dtype=wp.mat33), - friction_in: wp.array(dtype=vec5), - solref_in: wp.array(dtype=wp.vec2), - solreffriction_in: wp.array(dtype=wp.vec2), - solimp_in: wp.array(dtype=vec5), - type_in: wp.array(dtype=int), + dist_in: wp.array[float], + condim_in: wp.array[int], + includemargin_in: wp.array[float], + worldid_in: wp.array[int], + geom_in: wp.array[wp.vec2i], + flex_in: wp.array[wp.vec2i], + vert_in: wp.array[wp.vec2i], + pos_in: wp.array[wp.vec3], + frame_in: wp.array[wp.mat33], + friction_in: wp.array[vec5], + solref_in: wp.array[wp.vec2], + solreffriction_in: wp.array[wp.vec2], + solimp_in: wp.array[vec5], + type_in: wp.array[int], # Data out: - nefc_out: wp.array(dtype=int), - contact_efc_address_out: wp.array2d(dtype=int), - efc_type_out: wp.array2d(dtype=int), - efc_id_out: wp.array2d(dtype=int), - efc_J_rownnz_out: wp.array2d(dtype=int), - efc_J_rowadr_out: wp.array2d(dtype=int), - efc_J_colind_out: wp.array3d(dtype=int), - efc_J_out: wp.array3d(dtype=float), - efc_pos_out: wp.array2d(dtype=float), - efc_margin_out: wp.array2d(dtype=float), - efc_D_out: wp.array2d(dtype=float), - efc_vel_out: wp.array2d(dtype=float), - efc_aref_out: wp.array2d(dtype=float), - efc_frictionloss_out: wp.array2d(dtype=float), + nefc_out: wp.array[int], + contact_efc_address_out: wp.array2d[int], + efc_type_out: wp.array2d[int], + efc_id_out: wp.array2d[int], + efc_J_rownnz_out: wp.array2d[int], + efc_J_rowadr_out: wp.array2d[int], + efc_J_colind_out: wp.array3d[int], + efc_J_out: wp.array3d[float], + efc_pos_out: wp.array2d[float], + efc_margin_out: wp.array2d[float], + efc_D_out: wp.array2d[float], + efc_vel_out: wp.array2d[float], + efc_aref_out: wp.array2d[float], + efc_frictionloss_out: wp.array2d[float], # Out: - efc_nnz_out: wp.array(dtype=int), + efc_nnz_out: wp.array[int], ): conid, dimid = wp.tid() diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/derivative.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/derivative.py index 20da751a..bdcd81cb 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/derivative.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/derivative.py @@ -31,27 +31,27 @@ wp.set_module_options({"enable_backward": False}) @wp.kernel def _qderiv_actuator_passive_vel( # Model: - opt_timestep: wp.array(dtype=float), - actuator_dyntype: wp.array(dtype=int), - actuator_gaintype: wp.array(dtype=int), - actuator_biastype: wp.array(dtype=int), - actuator_actadr: wp.array(dtype=int), - actuator_actnum: wp.array(dtype=int), - actuator_forcelimited: wp.array(dtype=bool), - actuator_actlimited: wp.array(dtype=bool), - actuator_dynprm: wp.array2d(dtype=vec10f), - actuator_gainprm: wp.array2d(dtype=vec10f), - actuator_biasprm: wp.array2d(dtype=vec10f), - actuator_actearly: wp.array(dtype=bool), - actuator_forcerange: wp.array2d(dtype=wp.vec2), - actuator_actrange: wp.array2d(dtype=wp.vec2), + opt_timestep: wp.array[float], + actuator_dyntype: wp.array[int], + actuator_gaintype: wp.array[int], + actuator_biastype: wp.array[int], + actuator_actadr: wp.array[int], + actuator_actnum: wp.array[int], + actuator_forcelimited: wp.array[bool], + actuator_actlimited: wp.array[bool], + actuator_dynprm: wp.array2d[vec10f], + actuator_gainprm: wp.array2d[vec10f], + actuator_biasprm: wp.array2d[vec10f], + actuator_actearly: wp.array[bool], + actuator_forcerange: wp.array2d[wp.vec2], + actuator_actrange: wp.array2d[wp.vec2], # Data in: - act_in: wp.array2d(dtype=float), - ctrl_in: wp.array2d(dtype=float), - act_dot_in: wp.array2d(dtype=float), - actuator_force_in: wp.array2d(dtype=float), + act_in: wp.array2d[float], + ctrl_in: wp.array2d[float], + act_dot_in: wp.array2d[float], + actuator_force_in: wp.array2d[float], # Out: - vel_out: wp.array2d(dtype=float), + vel_out: wp.array2d[float], ): worldid, actid = wp.tid() @@ -121,16 +121,16 @@ def _qderiv_actuator_passive_actuation_dense( # Model: nu: int, # Data in: - moment_rownnz_in: wp.array2d(dtype=int), - moment_rowadr_in: wp.array2d(dtype=int), - moment_colind_in: wp.array2d(dtype=int), - actuator_moment_in: wp.array2d(dtype=float), + moment_rownnz_in: wp.array2d[int], + moment_rowadr_in: wp.array2d[int], + moment_colind_in: wp.array2d[int], + actuator_moment_in: wp.array2d[float], # In: - vel_in: wp.array2d(dtype=float), - qMi: wp.array(dtype=int), - qMj: wp.array(dtype=int), + vel_in: wp.array2d[float], + qMi: wp.array[int], + qMj: wp.array[int], # Out: - qDeriv_out: wp.array3d(dtype=float), + qDeriv_out: wp.array3d[float], ): worldid, elemid = wp.tid() @@ -171,18 +171,18 @@ def _qderiv_actuator_passive_actuation_dense( @wp.kernel def _qderiv_actuator_passive_actuation_sparse( # Model: - M_rownnz: wp.array(dtype=int), - M_rowadr: wp.array(dtype=int), + M_rownnz: wp.array[int], + M_rowadr: wp.array[int], # Data in: - moment_rownnz_in: wp.array2d(dtype=int), - moment_rowadr_in: wp.array2d(dtype=int), - moment_colind_in: wp.array2d(dtype=int), - actuator_moment_in: wp.array2d(dtype=float), + moment_rownnz_in: wp.array2d[int], + moment_rowadr_in: wp.array2d[int], + moment_colind_in: wp.array2d[int], + actuator_moment_in: wp.array2d[float], # In: - vel_in: wp.array2d(dtype=float), - qMj: wp.array(dtype=int), + vel_in: wp.array2d[float], + qMj: wp.array[int], # Out: - qDeriv_out: wp.array3d(dtype=float), + qDeriv_out: wp.array3d[float], ): worldid, actid = wp.tid() @@ -225,18 +225,18 @@ def _qderiv_actuator_passive_actuation_sparse( @wp.kernel def _qderiv_actuator_passive( # Model: - opt_timestep: wp.array(dtype=float), + opt_timestep: wp.array[float], opt_disableflags: int, - dof_damping: wp.array2d(dtype=float), + dof_damping: wp.array2d[float], is_sparse: bool, # Data in: - qM_in: wp.array3d(dtype=float), + qM_in: wp.array3d[float], # In: - qMi: wp.array(dtype=int), - qMj: wp.array(dtype=int), - qDeriv_in: wp.array3d(dtype=float), + qMi: wp.array[int], + qMj: wp.array[int], + qDeriv_in: wp.array3d[float], # Out: - qDeriv_out: wp.array3d(dtype=float), + qDeriv_out: wp.array3d[float], ): worldid, elemid = wp.tid() @@ -267,19 +267,19 @@ def _qderiv_actuator_passive( def _qderiv_tendon_damping( # Model: ntendon: int, - opt_timestep: wp.array(dtype=float), - ten_J_rownnz: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - tendon_damping: wp.array2d(dtype=float), + opt_timestep: wp.array[float], + ten_J_rownnz: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_colind: wp.array[int], + tendon_damping: wp.array2d[float], is_sparse: bool, # Data in: - ten_J_in: wp.array2d(dtype=float), + ten_J_in: wp.array2d[float], # In: - qMi: wp.array(dtype=int), - qMj: wp.array(dtype=int), + qMi: wp.array[int], + qMj: wp.array[int], # Out: - qDeriv_out: wp.array3d(dtype=float), + qDeriv_out: wp.array3d[float], ): worldid, elemid = wp.tid() dofiid = qMi[elemid] @@ -318,7 +318,7 @@ def _qderiv_tendon_damping( @event_scope -def deriv_smooth_vel(m: Model, d: Data, out: wp.array2d(dtype=float)): +def deriv_smooth_vel(m: Model, d: Data, out: wp.array2d[float]): """Analytical derivative of smooth forces w.r.t. velocities. Args: diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/forward.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/forward.py index 64bdd91f..55c7f357 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/forward.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/forward.py @@ -51,17 +51,17 @@ wp.set_module_options({"enable_backward": False}) @wp.kernel def _next_position( # Model: - opt_timestep: wp.array(dtype=float), - jnt_type: wp.array(dtype=int), - jnt_qposadr: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), + opt_timestep: wp.array[float], + jnt_type: wp.array[int], + jnt_qposadr: wp.array[int], + jnt_dofadr: wp.array[int], # Data in: - qpos_in: wp.array2d(dtype=float), - qvel_in: wp.array2d(dtype=float), + qpos_in: wp.array2d[float], + qvel_in: wp.array2d[float], # In: qvel_scale_in: float, # Data out: - qpos_out: wp.array2d(dtype=float), + qpos_out: wp.array2d[float], ): worldid, jntid = wp.tid() timestep = opt_timestep[worldid % opt_timestep.shape[0]] @@ -115,14 +115,14 @@ def _next_position( @wp.kernel def _next_velocity( # Model: - opt_timestep: wp.array(dtype=float), + opt_timestep: wp.array[float], # Data in: - qvel_in: wp.array2d(dtype=float), - qacc_in: wp.array2d(dtype=float), + qvel_in: wp.array2d[float], + qacc_in: wp.array2d[float], # In: qacc_scale_in: float, # Data out: - qvel_out: wp.array2d(dtype=float), + qvel_out: wp.array2d[float], ): worldid, dofid = wp.tid() timestep = opt_timestep[worldid % opt_timestep.shape[0]] @@ -132,21 +132,21 @@ def _next_velocity( @wp.kernel def _next_activation( # Model: - opt_timestep: wp.array(dtype=float), - actuator_dyntype: wp.array(dtype=int), - actuator_actadr: wp.array(dtype=int), - actuator_actnum: wp.array(dtype=int), - actuator_actlimited: wp.array(dtype=bool), - actuator_dynprm: wp.array2d(dtype=vec10f), - actuator_actrange: wp.array2d(dtype=wp.vec2), + opt_timestep: wp.array[float], + actuator_dyntype: wp.array[int], + actuator_actadr: wp.array[int], + actuator_actnum: wp.array[int], + actuator_actlimited: wp.array[bool], + actuator_dynprm: wp.array2d[vec10f], + actuator_actrange: wp.array2d[wp.vec2], # Data in: - act_in: wp.array2d(dtype=float), - act_dot_in: wp.array2d(dtype=float), + act_in: wp.array2d[float], + act_dot_in: wp.array2d[float], # In: act_dot_scale: float, limit: bool, # Data out: - act_out: wp.array2d(dtype=float), + act_out: wp.array2d[float], ): worldid, uid = wp.tid() opt_timestep_id = worldid % opt_timestep.shape[0] @@ -171,21 +171,21 @@ def _next_activation( @wp.kernel def _next_time( # Model: - opt_timestep: wp.array(dtype=float), + opt_timestep: wp.array[float], is_sparse: bool, # Data in: - nefc_in: wp.array(dtype=int), - time_in: wp.array(dtype=float), - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), + nefc_in: wp.array[int], + time_in: wp.array[float], + efc_J_rownnz_in: wp.array2d[int], + efc_J_rowadr_in: wp.array2d[int], nworld_in: int, naconmax_in: int, njmax_in: int, njmax_nnz_in: int, - nacon_in: wp.array(dtype=int), - ncollision_in: wp.array(dtype=int), + nacon_in: wp.array[int], + ncollision_in: wp.array[int], # Data out: - time_out: wp.array(dtype=float), + time_out: wp.array[float], ): worldid = wp.tid() time_out[worldid] = time_in[worldid] + opt_timestep[worldid % opt_timestep.shape[0]] @@ -277,11 +277,11 @@ def _advance(m: Model, d: Data, qacc: wp.array, qvel: Optional[wp.array] = None) @wp.kernel def _euler_damp_qfrc_sparse( # Model: - opt_timestep: wp.array(dtype=float), - dof_Madr: wp.array(dtype=int), - dof_damping: wp.array2d(dtype=float), + opt_timestep: wp.array[float], + dof_Madr: wp.array[int], + dof_damping: wp.array2d[float], # Out: - qM_integration_out: wp.array3d(dtype=float), + qM_integration_out: wp.array3d[float], ): worldid, tid = wp.tid() timestep = opt_timestep[worldid % opt_timestep.shape[0]] @@ -295,15 +295,15 @@ def _tile_euler_dense(tile: TileSet): @wp.kernel(module="unique", enable_backward=False) def euler_dense( # Model: - opt_timestep: wp.array(dtype=float), - dof_damping: wp.array2d(dtype=float), + opt_timestep: wp.array[float], + dof_damping: wp.array2d[float], # Data in: - qM_in: wp.array3d(dtype=float), - efc_Ma_in: wp.array2d(dtype=float), + qM_in: wp.array3d[float], + efc_Ma_in: wp.array2d[float], # In: - adr_in: wp.array(dtype=int), + adr_in: wp.array[int], # Data out: - qacc_out: wp.array2d(dtype=float), + qacc_out: wp.array2d[float], ): worldid, nodeid = wp.tid() timestep = opt_timestep[worldid % opt_timestep.shape[0]] @@ -358,8 +358,8 @@ def _rk_perturb_state( m: Model, d: Data, scale: float, - qpos_t0: wp.array2d(dtype=float), - qvel_t0: wp.array2d(dtype=float), + qpos_t0: wp.array2d[float], + qvel_t0: wp.array2d[float], act_t0: Optional[wp.array] = None, ): # position @@ -403,13 +403,13 @@ def _rk_perturb_state( @wp.kernel def _rk_accumulate_velocity_acceleration( # Data in: - qvel_in: wp.array2d(dtype=float), - qacc_in: wp.array2d(dtype=float), + qvel_in: wp.array2d[float], + qacc_in: wp.array2d[float], # In: scale: float, # Data out: - qvel_out: wp.array2d(dtype=float), - qacc_out: wp.array2d(dtype=float), + qvel_out: wp.array2d[float], + qacc_out: wp.array2d[float], ): worldid, dofid = wp.tid() qvel_out[worldid, dofid] += scale * qvel_in[worldid, dofid] @@ -419,11 +419,11 @@ def _rk_accumulate_velocity_acceleration( @wp.kernel def _rk_accumulate_activation_velocity( # Data in: - act_dot_in: wp.array2d(dtype=float), + act_dot_in: wp.array2d[float], # In: scale: float, # Data out: - act_dot_out: wp.array2d(dtype=float), + act_dot_out: wp.array2d[float], ): worldid, actid = wp.tid() act_dot_out[worldid, actid] += scale * act_dot_in[worldid, actid] @@ -433,8 +433,8 @@ def _rk_accumulate( m: Model, d: Data, scale: float, - qvel_rk: wp.array2d(dtype=float), - qacc_rk: wp.array2d(dtype=float), + qvel_rk: wp.array2d[float], + qacc_rk: wp.array2d[float], act_dot_rk: Optional[wp.array] = None, ): """Computes one term of 1/6 k_1 + 1/3 k_2 + 1/3 k_3 + 1/6 k_4.""" @@ -540,13 +540,13 @@ def fwd_position(m: Model, d: Data, factorize: bool = True): @wp.kernel def _actuator_velocity( # Data in: - qvel_in: wp.array2d(dtype=float), - moment_rownnz_in: wp.array2d(dtype=int), - moment_rowadr_in: wp.array2d(dtype=int), - moment_colind_in: wp.array2d(dtype=int), - actuator_moment_in: wp.array2d(dtype=float), + qvel_in: wp.array2d[float], + moment_rownnz_in: wp.array2d[int], + moment_rowadr_in: wp.array2d[int], + moment_colind_in: wp.array2d[int], + actuator_moment_in: wp.array2d[float], # Data out: - actuator_velocity_out: wp.array2d(dtype=float), + actuator_velocity_out: wp.array2d[float], ): worldid, actid = wp.tid() @@ -565,14 +565,14 @@ def _actuator_velocity( @wp.kernel def _tendon_velocity( # Model: - ten_J_rownnz: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), + ten_J_rownnz: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_colind: wp.array[int], # Data in: - qvel_in: wp.array2d(dtype=float), - ten_J_in: wp.array2d(dtype=float), + qvel_in: wp.array2d[float], + ten_J_in: wp.array2d[float], # Data out: - ten_velocity_out: wp.array2d(dtype=float), + ten_velocity_out: wp.array2d[float], ): worldid, tenid = wp.tid() @@ -617,34 +617,34 @@ def fwd_velocity(m: Model, d: Data): def _actuator_force( # Model: na: int, - opt_timestep: wp.array(dtype=float), - actuator_dyntype: wp.array(dtype=int), - actuator_gaintype: wp.array(dtype=int), - actuator_biastype: wp.array(dtype=int), - actuator_actadr: wp.array(dtype=int), - actuator_actnum: wp.array(dtype=int), - actuator_ctrllimited: wp.array(dtype=bool), - actuator_forcelimited: wp.array(dtype=bool), - actuator_actlimited: wp.array(dtype=bool), - actuator_dynprm: wp.array2d(dtype=vec10f), - actuator_gainprm: wp.array2d(dtype=vec10f), - actuator_biasprm: wp.array2d(dtype=vec10f), - actuator_actearly: wp.array(dtype=bool), - actuator_ctrlrange: wp.array2d(dtype=wp.vec2), - actuator_forcerange: wp.array2d(dtype=wp.vec2), - actuator_actrange: wp.array2d(dtype=wp.vec2), - actuator_acc0: wp.array2d(dtype=float), - actuator_lengthrange: wp.array2d(dtype=wp.vec2), + opt_timestep: wp.array[float], + actuator_dyntype: wp.array[int], + actuator_gaintype: wp.array[int], + actuator_biastype: wp.array[int], + actuator_actadr: wp.array[int], + actuator_actnum: wp.array[int], + actuator_ctrllimited: wp.array[bool], + actuator_forcelimited: wp.array[bool], + actuator_actlimited: wp.array[bool], + actuator_dynprm: wp.array2d[vec10f], + actuator_gainprm: wp.array2d[vec10f], + actuator_biasprm: wp.array2d[vec10f], + actuator_actearly: wp.array[bool], + actuator_ctrlrange: wp.array2d[wp.vec2], + actuator_forcerange: wp.array2d[wp.vec2], + actuator_actrange: wp.array2d[wp.vec2], + actuator_acc0: wp.array2d[float], + actuator_lengthrange: wp.array2d[wp.vec2], # Data in: - act_in: wp.array2d(dtype=float), - ctrl_in: wp.array2d(dtype=float), - actuator_length_in: wp.array2d(dtype=float), - actuator_velocity_in: wp.array2d(dtype=float), + act_in: wp.array2d[float], + ctrl_in: wp.array2d[float], + actuator_length_in: wp.array2d[float], + actuator_velocity_in: wp.array2d[float], # In: dsbl_clampctrl: int, # Data out: - act_dot_out: wp.array2d(dtype=float), - actuator_force_out: wp.array2d(dtype=float), + act_dot_out: wp.array2d[float], + actuator_force_out: wp.array2d[float], ): worldid, uid = wp.tid() @@ -738,12 +738,12 @@ def _actuator_force( @wp.kernel def _tendon_actuator_force( # Model: - actuator_trntype: wp.array(dtype=int), - actuator_trnid: wp.array(dtype=wp.vec2i), + actuator_trntype: wp.array[int], + actuator_trnid: wp.array[wp.vec2i], # Data in: - actuator_force_in: wp.array2d(dtype=float), + actuator_force_in: wp.array2d[float], # Out: - ten_actfrc_out: wp.array2d(dtype=float), + ten_actfrc_out: wp.array2d[float], ): worldid, actid = wp.tid() @@ -756,14 +756,14 @@ def _tendon_actuator_force( @wp.kernel def _tendon_actuator_force_clamp( # Model: - tendon_actfrclimited: wp.array(dtype=bool), - tendon_actfrcrange: wp.array2d(dtype=wp.vec2), - actuator_trntype: wp.array(dtype=int), - actuator_trnid: wp.array(dtype=wp.vec2i), + tendon_actfrclimited: wp.array[bool], + tendon_actfrcrange: wp.array2d[wp.vec2], + actuator_trntype: wp.array[int], + actuator_trnid: wp.array[wp.vec2i], # In: - ten_actfrc_in: wp.array2d(dtype=float), + ten_actfrc_in: wp.array2d[float], # Data out: - actuator_force_out: wp.array2d(dtype=float), + actuator_force_out: wp.array2d[float], ): worldid, actid = wp.tid() @@ -782,13 +782,13 @@ def _tendon_actuator_force_clamp( @wp.kernel def _qfrc_actuator( # Data in: - moment_rownnz_in: wp.array2d(dtype=int), - moment_rowadr_in: wp.array2d(dtype=int), - moment_colind_in: wp.array2d(dtype=int), - actuator_moment_in: wp.array2d(dtype=float), - actuator_force_in: wp.array2d(dtype=float), + moment_rownnz_in: wp.array2d[int], + moment_rowadr_in: wp.array2d[int], + moment_colind_in: wp.array2d[int], + actuator_moment_in: wp.array2d[float], + actuator_force_in: wp.array2d[float], # Data out: - qfrc_actuator_out: wp.array2d(dtype=float), + qfrc_actuator_out: wp.array2d[float], ): worldid, actid = wp.tid() @@ -806,15 +806,15 @@ def _qfrc_actuator( def _qfrc_actuator_gravcomp_limits( # Model: ngravcomp: int, - jnt_actfrclimited: wp.array(dtype=bool), - jnt_actgravcomp: wp.array(dtype=int), - jnt_actfrcrange: wp.array2d(dtype=wp.vec2), - dof_jntid: wp.array(dtype=int), + jnt_actfrclimited: wp.array[bool], + jnt_actgravcomp: wp.array[int], + jnt_actfrcrange: wp.array2d[wp.vec2], + dof_jntid: wp.array[int], # Data in: - qfrc_gravcomp_in: wp.array2d(dtype=float), - qfrc_actuator_in: wp.array2d(dtype=float), + qfrc_gravcomp_in: wp.array2d[float], + qfrc_actuator_in: wp.array2d[float], # Data out: - qfrc_actuator_out: wp.array2d(dtype=float), + qfrc_actuator_out: wp.array2d[float], ): worldid, dofid = wp.tid() jntid = dof_jntid[dofid] @@ -930,12 +930,12 @@ def fwd_actuation(m: Model, d: Data): @wp.kernel def _qfrc_smooth( # Data in: - qfrc_applied_in: wp.array2d(dtype=float), - qfrc_bias_in: wp.array2d(dtype=float), - qfrc_passive_in: wp.array2d(dtype=float), - qfrc_actuator_in: wp.array2d(dtype=float), + qfrc_applied_in: wp.array2d[float], + qfrc_bias_in: wp.array2d[float], + qfrc_passive_in: wp.array2d[float], + qfrc_actuator_in: wp.array2d[float], # Data out: - qfrc_smooth_out: wp.array2d(dtype=float), + qfrc_smooth_out: wp.array2d[float], ): worldid, dofid = wp.tid() qfrc_smooth_out[worldid, dofid] = ( diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/inverse.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/inverse.py index 9fb9242b..065afde0 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/inverse.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/inverse.py @@ -34,12 +34,12 @@ wp.set_module_options({"enable_backward": False}) @wp.kernel def _qfrc_eulerdamp( # Model: - opt_timestep: wp.array(dtype=float), - dof_damping: wp.array2d(dtype=float), + opt_timestep: wp.array[float], + dof_damping: wp.array2d[float], # Data in: - qacc_in: wp.array2d(dtype=float), + qacc_in: wp.array2d[float], # Out: - qfrc_out: wp.array2d(dtype=float), + qfrc_out: wp.array2d[float], ): worldid, dofid = wp.tid() timestep = opt_timestep[worldid % opt_timestep.shape[0]] @@ -49,13 +49,13 @@ def _qfrc_eulerdamp( @wp.kernel def _qfrc_inverse( # Data in: - qfrc_bias_in: wp.array2d(dtype=float), - qfrc_passive_in: wp.array2d(dtype=float), - qfrc_constraint_in: wp.array2d(dtype=float), + qfrc_bias_in: wp.array2d[float], + qfrc_passive_in: wp.array2d[float], + qfrc_constraint_in: wp.array2d[float], # In: - Ma: wp.array2d(dtype=float), + Ma: wp.array2d[float], # Data out: - qfrc_inverse_out: wp.array2d(dtype=float), + qfrc_inverse_out: wp.array2d[float], ): worldid, dofid = wp.tid() @@ -67,7 +67,7 @@ def _qfrc_inverse( qfrc_inverse_out[worldid, dofid] = qfrc_inverse -def discrete_acc(m: Model, d: Data, qacc: wp.array2d(dtype=float)): +def discrete_acc(m: Model, d: Data, qacc: wp.array2d[float]): """Convert discrete-time qacc to continuous-time qacc. Args: diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py index 0b53094b..4a743681 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py @@ -34,14 +34,20 @@ from mujoco.mjx.third_party.mujoco_warp._src.types import vec10 from mujoco.mjx.third_party.mujoco_warp._src.util_pkg import check_version -def _create_array(data: Any, spec: wp.array, sizes: dict[str, int]) -> wp.array | None: +def _is_array_spec(typ) -> bool: + """Check if a type annotation is an array spec (wp.array instance or bracket annotation).""" + return isinstance(typ, wp.array) or type(typ).__name__ == "_ArrayAnnotation" + + +def _create_array(data: Any, spec, sizes: dict[str, int]) -> wp.array | None: """Creates a warp array and populates it with data. The array shape is determined by a field spec referencing MjModel / MjData array sizes. """ + spec_shape = getattr(spec, "shape", (0,)) shape = None - if spec.shape != (0,): - shape = tuple(sizes[dim] if isinstance(dim, str) else dim for dim in spec.shape) + if spec_shape != (0,): + shape = tuple(sizes[dim] if isinstance(dim, str) else dim for dim in spec_shape) if data is None and shape is None: return None # nothing to do @@ -50,7 +56,7 @@ def _create_array(data: Any, spec: wp.array, sizes: dict[str, int]) -> wp.array else: array = wp.array(np.array(data), dtype=spec.dtype, shape=shape) - if spec.shape[0] == "*": + if spec_shape and spec_shape[0] == "*": # add private attribute for JAX to determine which fields are batched array._is_batched = True # also set stride 0 to 0 which is expected legacy behavior (but is deprecated) @@ -194,7 +200,7 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: # place opt on device for f in dataclasses.fields(types.Option): - if isinstance(f.type, wp.array): + if _is_array_spec(f.type): setattr(opt, f.name, _create_array(getattr(opt, f.name), f.type, {"*": 1})) else: setattr(opt, f.name, f.type(getattr(opt, f.name))) @@ -635,7 +641,7 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: # place m on device sizes = dict({"*": 1}, **{f.name: getattr(m, f.name) for f in dataclasses.fields(types.Model) if f.type is int}) for f in dataclasses.fields(types.Model): - if isinstance(f.type, wp.array): + if _is_array_spec(f.type): setattr(m, f.name, _create_array(getattr(m, f.name), f.type, sizes)) return m @@ -1459,7 +1465,7 @@ def reset_data(m: types.Model, d: types.Data, reset: Optional[wp.array] = None): """ @wp.kernel(module="unique", enable_backward=False) - def reset_xfrc_applied(reset_in: wp.array(dtype=bool), xfrc_applied_out: wp.array2d(dtype=wp.spatial_vector)): + def reset_xfrc_applied(reset_in: wp.array[bool], xfrc_applied_out: wp.array2d[wp.spatial_vector]): worldid, bodyid, elemid = wp.tid() if wp.static(reset is not None): @@ -1469,7 +1475,7 @@ def reset_data(m: types.Model, d: types.Data, reset: Optional[wp.array] = None): xfrc_applied_out[worldid, bodyid][elemid] = 0.0 @wp.kernel(module="unique", enable_backward=False) - def reset_qM(reset_in: wp.array(dtype=bool), qM_out: wp.array3d(dtype=float)): + def reset_qM(reset_in: wp.array[bool], qM_out: wp.array3d[float]): worldid, elemid1, elemid2 = wp.tid() if wp.static(reset is not None): @@ -1487,31 +1493,31 @@ def reset_data(m: types.Model, d: types.Data, reset: Optional[wp.array] = None): na: int, neq: int, nsensordata: int, - qpos0: wp.array2d(dtype=float), - eq_active0: wp.array(dtype=bool), + qpos0: wp.array2d[float], + eq_active0: wp.array[bool], # Data in: nworld_in: int, # In: - reset_in: wp.array(dtype=bool), + reset_in: wp.array[bool], # Data out: - solver_niter_out: wp.array(dtype=int), - ne_out: wp.array(dtype=int), - nf_out: wp.array(dtype=int), - nl_out: wp.array(dtype=int), - nefc_out: wp.array(dtype=int), - time_out: wp.array(dtype=float), - energy_out: wp.array(dtype=wp.vec2), - qpos_out: wp.array2d(dtype=float), - qvel_out: wp.array2d(dtype=float), - act_out: wp.array2d(dtype=float), - qacc_warmstart_out: wp.array2d(dtype=float), - ctrl_out: wp.array2d(dtype=float), - qfrc_applied_out: wp.array2d(dtype=float), - eq_active_out: wp.array2d(dtype=bool), - qacc_out: wp.array2d(dtype=float), - act_dot_out: wp.array2d(dtype=float), - sensordata_out: wp.array2d(dtype=float), - nacon_out: wp.array(dtype=int), + solver_niter_out: wp.array[int], + ne_out: wp.array[int], + nf_out: wp.array[int], + nl_out: wp.array[int], + nefc_out: wp.array[int], + time_out: wp.array[float], + energy_out: wp.array[wp.vec2], + qpos_out: wp.array2d[float], + qvel_out: wp.array2d[float], + act_out: wp.array2d[float], + qacc_warmstart_out: wp.array2d[float], + ctrl_out: wp.array2d[float], + qfrc_applied_out: wp.array2d[float], + eq_active_out: wp.array2d[bool], + qacc_out: wp.array2d[float], + act_dot_out: wp.array2d[float], + sensordata_out: wp.array2d[float], + nacon_out: wp.array[int], ): worldid = wp.tid() @@ -1549,14 +1555,14 @@ def reset_data(m: types.Model, d: types.Data, reset: Optional[wp.array] = None): @wp.kernel(module="unique", enable_backward=False) def reset_mocap( # Model: - body_mocapid: wp.array(dtype=int), - body_pos: wp.array2d(dtype=wp.vec3), - body_quat: wp.array2d(dtype=wp.quat), + body_mocapid: wp.array[int], + body_pos: wp.array2d[wp.vec3], + body_quat: wp.array2d[wp.quat], # In: - reset_in: wp.array(dtype=bool), + reset_in: wp.array[bool], # Data out: - mocap_pos_out: wp.array2d(dtype=wp.vec3), - mocap_quat_out: wp.array2d(dtype=wp.quat), + mocap_pos_out: wp.array2d[wp.vec3], + mocap_quat_out: wp.array2d[wp.quat], ): worldid, bodyid = wp.tid() @@ -1573,27 +1579,27 @@ def reset_data(m: types.Model, d: types.Data, reset: Optional[wp.array] = None): @wp.kernel(module="unique", enable_backward=False) def reset_contact( # Data in: - nacon_in: wp.array(dtype=int), + nacon_in: wp.array[int], # In: - reset_in: wp.array(dtype=bool), + reset_in: wp.array[bool], nefcaddress: int, # Data out: - contact_dist_out: wp.array(dtype=float), - contact_pos_out: wp.array(dtype=wp.vec3), - contact_frame_out: wp.array(dtype=wp.mat33), - contact_includemargin_out: wp.array(dtype=float), - contact_friction_out: wp.array(dtype=types.vec5), - contact_solref_out: wp.array(dtype=wp.vec2), - contact_solreffriction_out: wp.array(dtype=wp.vec2), - contact_solimp_out: wp.array(dtype=types.vec5), - contact_dim_out: wp.array(dtype=int), - contact_geom_out: wp.array(dtype=wp.vec2i), - contact_flex_out: wp.array(dtype=wp.vec2i), - contact_vert_out: wp.array(dtype=wp.vec2i), - contact_efc_address_out: wp.array2d(dtype=int), - contact_worldid_out: wp.array(dtype=int), - contact_type_out: wp.array(dtype=int), - contact_geomcollisionid_out: wp.array(dtype=int), + contact_dist_out: wp.array[float], + contact_pos_out: wp.array[wp.vec3], + contact_frame_out: wp.array[wp.mat33], + contact_includemargin_out: wp.array[float], + contact_friction_out: wp.array[types.vec5], + contact_solref_out: wp.array[wp.vec2], + contact_solreffriction_out: wp.array[wp.vec2], + contact_solimp_out: wp.array[types.vec5], + contact_dim_out: wp.array[int], + contact_geom_out: wp.array[wp.vec2i], + contact_flex_out: wp.array[wp.vec2i], + contact_vert_out: wp.array[wp.vec2i], + contact_efc_address_out: wp.array2d[int], + contact_worldid_out: wp.array[int], + contact_type_out: wp.array[int], + contact_geomcollisionid_out: wp.array[int], ): conid = wp.tid() @@ -1697,8 +1703,8 @@ def reset_data(m: types.Model, d: types.Data, reset: Optional[wp.array] = None): # kernel_analyzer: off @wp.kernel def _init_subtreemass( - body_mass_in: wp.array2d(dtype=float), - body_subtreemass_out: wp.array2d(dtype=float), + body_mass_in: wp.array2d[float], + body_subtreemass_out: wp.array2d[float], ): worldid, bodyid = wp.tid() body_mass_id = worldid % body_mass_in.shape[0] @@ -1708,9 +1714,9 @@ def _init_subtreemass( @wp.kernel def _accumulate_subtreemass( - body_parentid: wp.array(dtype=int), - body_subtreemass_io: wp.array2d(dtype=float), - body_tree_: wp.array(dtype=int), + body_parentid: wp.array[int], + body_subtreemass_io: wp.array2d[float], + body_tree_: wp.array[int], ): worldid, nodeid = wp.tid() body_subtreemass_id = worldid % body_subtreemass_io.shape[0] @@ -1722,8 +1728,8 @@ def _accumulate_subtreemass( @wp.kernel def _copy_qpos0_to_qpos( - qpos0: wp.array2d(dtype=float), - qpos_out: wp.array2d(dtype=float), + qpos0: wp.array2d[float], + qpos_out: wp.array2d[float], ): worldid, i = wp.tid() qpos0_id = worldid % qpos0.shape[0] @@ -1732,8 +1738,8 @@ def _copy_qpos0_to_qpos( @wp.kernel def _copy_tendon_length0( - ten_length_in: wp.array2d(dtype=float), - tendon_length0_out: wp.array2d(dtype=float), + ten_length_in: wp.array2d[float], + tendon_length0_out: wp.array2d[float], ): worldid, tenid = wp.tid() tendon_length0_id = worldid % tendon_length0_out.shape[0] @@ -1744,9 +1750,9 @@ def _copy_tendon_length0( def _compute_meaninertia( nv: int, is_sparse: bool, - dof_Madr_in: wp.array(dtype=int), - qM_in: wp.array3d(dtype=float), - meaninertia_out: wp.array(dtype=float), + dof_Madr_in: wp.array[int], + qM_in: wp.array3d[float], + meaninertia_out: wp.array[float], ): """Compute mean diagonal inertia from qM at qpos0.""" worldid = wp.tid() @@ -1771,7 +1777,7 @@ def _compute_meaninertia( @wp.kernel def _set_unit_vector( dofid_target: int, - unit_vec_out: wp.array2d(dtype=float), + unit_vec_out: wp.array2d[float], ): worldid = wp.tid() nv = unit_vec_out.shape[1] @@ -1785,8 +1791,8 @@ def _set_unit_vector( @wp.kernel def _extract_dof_A_diag( dofid: int, - result_vec_in: wp.array2d(dtype=float), - dof_A_diag_out: wp.array2d(dtype=float), + result_vec_in: wp.array2d[float], + dof_A_diag_out: wp.array2d[float], ): worldid = wp.tid() dof_A_diag_id = worldid % dof_A_diag_out.shape[0] @@ -1795,11 +1801,11 @@ def _extract_dof_A_diag( @wp.kernel def _finalize_dof_invweight0( - dof_jntid: wp.array(dtype=int), - jnt_type: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - dof_A_diag_in: wp.array2d(dtype=float), - dof_invweight0_out: wp.array2d(dtype=float), + dof_jntid: wp.array[int], + jnt_type: wp.array[int], + jnt_dofadr: wp.array[int], + dof_A_diag_in: wp.array2d[float], + dof_invweight0_out: wp.array2d[float], ): worldid, dofid = wp.tid() dof_invweight0_id = worldid % dof_invweight0_out.shape[0] @@ -1842,15 +1848,15 @@ def _compute_body_jac_row( nv: int, bodyid_target: int, row_idx: int, - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - dof_parentid: wp.array(dtype=int), - subtree_com_in: wp.array2d(dtype=wp.vec3), - xipos_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), - body_jac_row_out: wp.array2d(dtype=float), + body_parentid: wp.array[int], + body_rootid: wp.array[int], + body_dofadr: wp.array[int], + body_dofnum: wp.array[int], + dof_parentid: wp.array[int], + subtree_com_in: wp.array2d[wp.vec3], + xipos_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], + body_jac_row_out: wp.array2d[float], ): worldid = wp.tid() @@ -1901,9 +1907,9 @@ def _compute_body_A_diag_entry( nv: int, bodyid_target: int, row_idx: int, - body_jac_row_in: wp.array2d(dtype=float), - result_vec_in: wp.array2d(dtype=float), - body_A_diag_out: wp.array3d(dtype=float), + body_jac_row_in: wp.array2d[float], + result_vec_in: wp.array2d[float], + body_A_diag_out: wp.array3d[float], ): worldid = wp.tid() body_A_diag_id = worldid % body_A_diag_out.shape[0] @@ -1916,9 +1922,9 @@ def _compute_body_A_diag_entry( @wp.kernel def _finalize_body_invweight0( - body_weldid: wp.array(dtype=int), - body_A_diag_in: wp.array3d(dtype=float), - body_invweight0_out: wp.array2d(dtype=wp.vec2), + body_weldid: wp.array[int], + body_A_diag_in: wp.array3d[float], + body_invweight0_out: wp.array2d[wp.vec2], ): worldid, bodyid = wp.tid() body_invweight0_id = worldid % body_invweight0_out.shape[0] @@ -1953,11 +1959,11 @@ def _finalize_body_invweight0( @wp.kernel def _copy_tendon_jacobian( tenid_target: int, - ten_J_rownnz: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - ten_J_in: wp.array2d(dtype=float), - ten_J_vec_out: wp.array2d(dtype=float), + ten_J_rownnz: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_colind: wp.array[int], + ten_J_in: wp.array2d[float], + ten_J_vec_out: wp.array2d[float], ): worldid = wp.tid() nv = ten_J_in.shape[2] @@ -1971,15 +1977,15 @@ def _copy_tendon_jacobian( @wp.kernel def _compute_tendon_dot_product( # Model: - ten_J_rownnz: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), + ten_J_rownnz: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_colind: wp.array[int], # In: tenid_target: int, - ten_J_in: wp.array2d(dtype=float), - result_vec_in: wp.array2d(dtype=float), + ten_J_in: wp.array2d[float], + result_vec_in: wp.array2d[float], # Out: - tendon_invweight0_out: wp.array2d(dtype=float), + tendon_invweight0_out: wp.array2d[float], ): worldid = wp.tid() tendon_invweight0_id = worldid % tendon_invweight0_out.shape[0] @@ -1997,15 +2003,15 @@ def _compute_tendon_dot_product( @wp.kernel def _compute_cam_pos0( - cam_bodyid: wp.array(dtype=int), - cam_targetbodyid: wp.array(dtype=int), - cam_xpos_in: wp.array2d(dtype=wp.vec3), - cam_xmat_in: wp.array2d(dtype=wp.mat33), - xpos_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cam_pos0_out: wp.array2d(dtype=wp.vec3), - cam_poscom0_out: wp.array2d(dtype=wp.vec3), - cam_mat0_out: wp.array2d(dtype=wp.mat33), + cam_bodyid: wp.array[int], + cam_targetbodyid: wp.array[int], + cam_xpos_in: wp.array2d[wp.vec3], + cam_xmat_in: wp.array2d[wp.mat33], + xpos_in: wp.array2d[wp.vec3], + subtree_com_in: wp.array2d[wp.vec3], + cam_pos0_out: wp.array2d[wp.vec3], + cam_poscom0_out: wp.array2d[wp.vec3], + cam_mat0_out: wp.array2d[wp.mat33], ): worldid, camid = wp.tid() cam_pos0_id = worldid % cam_pos0_out.shape[0] @@ -2023,15 +2029,15 @@ def _compute_cam_pos0( @wp.kernel def _compute_light_pos0( - light_bodyid: wp.array(dtype=int), - light_targetbodyid: wp.array(dtype=int), - light_xpos_in: wp.array2d(dtype=wp.vec3), - light_xdir_in: wp.array2d(dtype=wp.vec3), - xpos_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), - light_pos0_out: wp.array2d(dtype=wp.vec3), - light_poscom0_out: wp.array2d(dtype=wp.vec3), - light_dir0_out: wp.array2d(dtype=wp.vec3), + light_bodyid: wp.array[int], + light_targetbodyid: wp.array[int], + light_xpos_in: wp.array2d[wp.vec3], + light_xdir_in: wp.array2d[wp.vec3], + xpos_in: wp.array2d[wp.vec3], + subtree_com_in: wp.array2d[wp.vec3], + light_pos0_out: wp.array2d[wp.vec3], + light_poscom0_out: wp.array2d[wp.vec3], + light_dir0_out: wp.array2d[wp.vec3], ): worldid, lightid = wp.tid() light_pos0_id = worldid % light_pos0_out.shape[0] @@ -2050,11 +2056,11 @@ def _compute_light_pos0( @wp.kernel def _copy_actuator_moment( actid_target: int, - moment_rownnz_in: wp.array2d(dtype=int), - moment_rowadr_in: wp.array2d(dtype=int), - moment_colind_in: wp.array2d(dtype=int), - actuator_moment_in: wp.array2d(dtype=float), - act_moment_vec_out: wp.array2d(dtype=float), + moment_rownnz_in: wp.array2d[int], + moment_rowadr_in: wp.array2d[int], + moment_colind_in: wp.array2d[int], + actuator_moment_in: wp.array2d[float], + act_moment_vec_out: wp.array2d[float], ): worldid = wp.tid() nv = act_moment_vec_out.shape[1] @@ -2072,8 +2078,8 @@ def _copy_actuator_moment( def _compute_actuator_acc0( actid_target: int, nv: int, - result_vec_in: wp.array2d(dtype=float), - actuator_acc0_out: wp.array2d(dtype=float), + result_vec_in: wp.array2d[float], + actuator_acc0_out: wp.array2d[float], ): worldid = wp.tid() norm_sq = float(0.0) @@ -2084,11 +2090,11 @@ def _compute_actuator_acc0( @wp.kernel def _compute_dof_M0( - dof_bodyid: wp.array(dtype=int), - dof_armature: wp.array2d(dtype=float), - cdof_in: wp.array2d(dtype=wp.spatial_vector), - crb_in: wp.array2d(dtype=vec10), - dof_M0_out: wp.array2d(dtype=float), + dof_bodyid: wp.array[int], + dof_armature: wp.array2d[float], + cdof_in: wp.array2d[wp.spatial_vector], + crb_in: wp.array2d[vec10], + dof_M0_out: wp.array2d[float], ): worldid, dofid = wp.tid() bodyid = dof_bodyid[dofid] @@ -2099,15 +2105,15 @@ def _compute_dof_M0( @wp.kernel def _resolve_dampratio( - actuator_biastype: wp.array(dtype=int), - actuator_gainprm: wp.array2d(dtype=types.vec10f), - moment_rownnz_in: wp.array2d(dtype=int), - moment_rowadr_in: wp.array2d(dtype=int), - moment_colind_in: wp.array2d(dtype=int), - actuator_moment_in: wp.array2d(dtype=float), - dof_M0_in: wp.array2d(dtype=float), + actuator_biastype: wp.array[int], + actuator_gainprm: wp.array2d[types.vec10f], + moment_rownnz_in: wp.array2d[int], + moment_rowadr_in: wp.array2d[int], + moment_colind_in: wp.array2d[int], + actuator_moment_in: wp.array2d[float], + dof_M0_in: wp.array2d[float], nv: int, - actuator_biasprm: wp.array2d(dtype=types.vec10f), + actuator_biasprm: wp.array2d[types.vec10f], ): worldid, actid = wp.tid() biastype = actuator_biastype[actid] @@ -2150,15 +2156,15 @@ def _resolve_dampratio( @wp.kernel def _set_length_range( - actuator_trntype: wp.array(dtype=int), - actuator_trnid: wp.array(dtype=wp.vec2i), - actuator_gear: wp.array2d(dtype=wp.spatial_vector), - jnt_limited: wp.array(dtype=int), - jnt_range: wp.array2d(dtype=wp.vec2), - tendon_limited: wp.array(dtype=int), - tendon_range: wp.array2d(dtype=wp.vec2), + actuator_trntype: wp.array[int], + actuator_trnid: wp.array[wp.vec2i], + actuator_gear: wp.array2d[wp.spatial_vector], + jnt_limited: wp.array[int], + jnt_range: wp.array2d[wp.vec2], + tendon_limited: wp.array[int], + tendon_range: wp.array2d[wp.vec2], ntendon: int, - actuator_lengthrange_out: wp.array2d(dtype=wp.vec2), + actuator_lengthrange_out: wp.array2d[wp.vec2], ): worldid, actid = wp.tid() trntype = actuator_trntype[actid] @@ -2515,7 +2521,13 @@ def override_model(model: types.Model | mujoco.MjModel, overrides: dict[str, Any "AUTO": mujoco.mjtJacobian.mjJAC_AUTO, }, } - mjw_only_fields = {"opt.broadphase", "opt.broadphase_filter", "opt.ls_parallel", "opt.graph_conditional"} + mjw_only_fields = { + "opt.broadphase", + "opt.broadphase_filter", + "opt.ls_parallel", + "opt.graph_conditional", + "opt.contact_sensor_maxmatch", + } mj_only_fields = {"opt.jacobian"} if not isinstance(overrides, dict): @@ -2626,7 +2638,7 @@ def _build_rays( intrinsic: wp.vec4, znear: float, # Out: - ray_out: wp.array(dtype=wp.vec3), + ray_out: wp.array[wp.vec3], ): xid, yid = wp.tid() ray_out[offset + xid + yid * img_w] = render_util.compute_ray( @@ -2682,18 +2694,16 @@ def create_render_context( if callable(_cubql_avail) and _cubql_avail(): constructor = "cubql" - # Mesh BVHs + # Mesh BVHs – build for all meshes so per-world variants are available nmesh = mjm.nmesh geom_enabled_mask = np.isin(mjm.geom_group, list(enabled_geom_groups)) - mesh_geom_mask = geom_enabled_mask & (mjm.geom_type == types.GeomType.MESH) & (mjm.geom_dataid >= 0) - used_mesh_id = set(mjm.geom_dataid[mesh_geom_mask].astype(int)) geom_enabled_idx = np.nonzero(geom_enabled_mask)[0] mesh_registry = {} mesh_bvh_id = [wp.uint64(0) for _ in range(nmesh)] mesh_bounds_size = [wp.vec3(0.0, 0.0, 0.0) for _ in range(nmesh)] - for mid in used_mesh_id: + for mid in range(nmesh): mesh, half = bvh.build_mesh_bvh(mjm, mid, constructor=constructor) mesh_registry[mesh.id] = mesh mesh_bvh_id[mid] = mesh.id diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/island.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/island.py index c021db3f..f7fb3a10 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/island.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/island.py @@ -26,24 +26,24 @@ from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope def _tree_edges( # Model: nv: int, - body_treeid: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - dof_treeid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - eq_type: wp.array(dtype=int), - eq_obj1id: wp.array(dtype=int), - eq_obj2id: wp.array(dtype=int), - eq_objtype: wp.array(dtype=int), + body_treeid: wp.array[int], + jnt_dofadr: wp.array[int], + dof_treeid: wp.array[int], + geom_bodyid: wp.array[int], + site_bodyid: wp.array[int], + eq_type: wp.array[int], + eq_obj1id: wp.array[int], + eq_obj2id: wp.array[int], + eq_objtype: wp.array[int], # Data in: - nefc_in: wp.array(dtype=int), - contact_geom_in: wp.array(dtype=wp.vec2i), - efc_type_in: wp.array2d(dtype=int), - efc_id_in: wp.array2d(dtype=int), - efc_J_in: wp.array3d(dtype=float), + nefc_in: wp.array[int], + contact_geom_in: wp.array[wp.vec2i], + efc_type_in: wp.array2d[int], + efc_id_in: wp.array2d[int], + efc_J_in: wp.array3d[float], njmax_in: int, # Out: - tree_tree: wp.array3d(dtype=int), # kernel_analyzer: off + tree_tree: wp.array3d[int], # kernel_analyzer: off ): """Find tree edges.""" worldid, efcid = wp.tid() @@ -151,7 +151,7 @@ def _tree_edges( wp.atomic_max(tree_tree, worldid, first_tree, first_tree, 1) -def tree_edges(m: types.Model, d: types.Data, tree_tree: wp.array3d(dtype=int)): +def tree_edges(m: types.Model, d: types.Data, tree_tree: wp.array3d[int]): """Compute tree-tree adjacency matrix.""" tree_tree.zero_() wp.launch( @@ -184,14 +184,14 @@ def _flood_fill( # Model: ntree: int, # In: - tree_tree_in: wp.array3d(dtype=int), - labels_in: wp.array2d(dtype=int), - stack_in: wp.array2d(dtype=int), + tree_tree_in: wp.array3d[int], + labels_in: wp.array2d[int], + stack_in: wp.array2d[int], # Data out: - nisland_out: wp.array(dtype=int), - tree_island_out: wp.array2d(dtype=int), + nisland_out: wp.array[int], + tree_island_out: wp.array2d[int], # Out: - stack_out: wp.array2d(dtype=int), + stack_out: wp.array2d[int], ): """DFS flood fill to discover islands using tree_tree matrix.""" worldid = wp.tid() diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py index 4abcff26..4c76d1f8 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py @@ -71,18 +71,18 @@ def _ellipsoid_max_moment(size: wp.vec3, dir: int) -> float: def _spring_damper_dof_passive( # Model: opt_disableflags: int, - qpos_spring: wp.array2d(dtype=float), - jnt_type: wp.array(dtype=int), - jnt_qposadr: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - jnt_stiffness: wp.array2d(dtype=float), - dof_damping: wp.array2d(dtype=float), + qpos_spring: wp.array2d[float], + jnt_type: wp.array[int], + jnt_qposadr: wp.array[int], + jnt_dofadr: wp.array[int], + jnt_stiffness: wp.array2d[float], + dof_damping: wp.array2d[float], # Data in: - qpos_in: wp.array2d(dtype=float), - qvel_in: wp.array2d(dtype=float), + qpos_in: wp.array2d[float], + qvel_in: wp.array2d[float], # Data out: - qfrc_spring_out: wp.array2d(dtype=float), - qfrc_damper_out: wp.array2d(dtype=float), + qfrc_spring_out: wp.array2d[float], + qfrc_damper_out: wp.array2d[float], ): worldid, jntid = wp.tid() dofid = jnt_dofadr[jntid] @@ -182,22 +182,22 @@ def _spring_damper_dof_passive( @wp.kernel def _spring_damper_tendon_passive( # Model: - ten_J_rownnz: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - tendon_stiffness: wp.array2d(dtype=float), - tendon_damping: wp.array2d(dtype=float), - tendon_lengthspring: wp.array2d(dtype=wp.vec2), + ten_J_rownnz: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_colind: wp.array[int], + tendon_stiffness: wp.array2d[float], + tendon_damping: wp.array2d[float], + tendon_lengthspring: wp.array2d[wp.vec2], # Data in: - ten_J_in: wp.array2d(dtype=float), - ten_length_in: wp.array2d(dtype=float), - ten_velocity_in: wp.array2d(dtype=float), + ten_J_in: wp.array2d[float], + ten_length_in: wp.array2d[float], + ten_velocity_in: wp.array2d[float], # In: dsbl_spring: bool, dsbl_damper: bool, # Data out: - qfrc_spring_out: wp.array2d(dtype=float), - qfrc_damper_out: wp.array2d(dtype=float), + qfrc_spring_out: wp.array2d[float], + qfrc_damper_out: wp.array2d[float], ): worldid, tenid, dofid_sparse = wp.tid() @@ -246,18 +246,18 @@ def _spring_damper_tendon_passive( @wp.kernel def _gravity_force( # Model: - opt_gravity: wp.array(dtype=wp.vec3), - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - body_mass: wp.array2d(dtype=float), - body_gravcomp: wp.array2d(dtype=float), - dof_bodyid: wp.array(dtype=int), + opt_gravity: wp.array[wp.vec3], + body_parentid: wp.array[int], + body_rootid: wp.array[int], + body_mass: wp.array2d[float], + body_gravcomp: wp.array2d[float], + dof_bodyid: wp.array[int], # Data in: - xipos_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), + xipos_in: wp.array2d[wp.vec3], + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], # Data out: - qfrc_gravcomp_out: wp.array2d(dtype=float), + qfrc_gravcomp_out: wp.array2d[float], ): worldid, bodyid, dofid = wp.tid() bodyid += 1 # skip world body @@ -275,27 +275,27 @@ def _gravity_force( @wp.kernel def _fluid_force( # Model: - opt_wind: wp.array(dtype=wp.vec3), - opt_density: wp.array(dtype=float), - opt_viscosity: wp.array(dtype=float), - body_rootid: wp.array(dtype=int), - body_geomnum: wp.array(dtype=int), - body_geomadr: wp.array(dtype=int), - body_mass: wp.array2d(dtype=float), - body_inertia: wp.array2d(dtype=wp.vec3), - geom_type: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - geom_fluid: wp.array2d(dtype=float), - body_fluid_ellipsoid: wp.array(dtype=bool), + opt_wind: wp.array[wp.vec3], + opt_density: wp.array[float], + opt_viscosity: wp.array[float], + body_rootid: wp.array[int], + body_geomnum: wp.array[int], + body_geomadr: wp.array[int], + body_mass: wp.array2d[float], + body_inertia: wp.array2d[wp.vec3], + geom_type: wp.array[int], + geom_size: wp.array2d[wp.vec3], + geom_fluid: wp.array2d[float], + body_fluid_ellipsoid: wp.array[bool], # Data in: - xipos_in: wp.array2d(dtype=wp.vec3), - ximat_in: wp.array2d(dtype=wp.mat33), - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cvel_in: wp.array2d(dtype=wp.spatial_vector), + xipos_in: wp.array2d[wp.vec3], + ximat_in: wp.array2d[wp.mat33], + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], + subtree_com_in: wp.array2d[wp.vec3], + cvel_in: wp.array2d[wp.spatial_vector], # Out: - fluid_applied_out: wp.array2d(dtype=wp.spatial_vector), + fluid_applied_out: wp.array2d[wp.spatial_vector], ): """Computes body-space fluid forces for both inertia-box and ellipsoid models.""" worldid, bodyid = wp.tid() @@ -535,18 +535,18 @@ def _fluid(m: Model, d: Data): @wp.kernel def _qfrc_passive( # Model: - jnt_actgravcomp: wp.array(dtype=int), - dof_jntid: wp.array(dtype=int), + jnt_actgravcomp: wp.array[int], + dof_jntid: wp.array[int], has_fluid: bool, # Data in: - qfrc_spring_in: wp.array2d(dtype=float), - qfrc_damper_in: wp.array2d(dtype=float), - qfrc_gravcomp_in: wp.array2d(dtype=float), - qfrc_fluid_in: wp.array2d(dtype=float), + qfrc_spring_in: wp.array2d[float], + qfrc_damper_in: wp.array2d[float], + qfrc_gravcomp_in: wp.array2d[float], + qfrc_fluid_in: wp.array2d[float], # In: gravcomp: bool, # Data out: - qfrc_passive_out: wp.array2d(dtype=float), + qfrc_passive_out: wp.array2d[float], ): worldid, dofid = wp.tid() qfrc_passive = qfrc_spring_in[worldid, dofid] @@ -567,29 +567,29 @@ def _qfrc_passive( def _flex_elasticity( # Model: nflex: int, - opt_timestep: wp.array(dtype=float), - body_dofadr: wp.array(dtype=int), - flex_dim: wp.array(dtype=int), - flex_vertadr: wp.array(dtype=int), - flex_edgeadr: wp.array(dtype=int), - flex_elemadr: wp.array(dtype=int), - flex_elemnum: wp.array(dtype=int), - flex_elemdataadr: wp.array(dtype=int), - flex_elemedgeadr: wp.array(dtype=int), - flex_vertbodyid: wp.array(dtype=int), - flex_elem: wp.array(dtype=int), - flex_elemedge: wp.array(dtype=int), - flexedge_length0: wp.array(dtype=float), - flex_stiffness: wp.array2d(dtype=float), - flex_damping: wp.array(dtype=float), + opt_timestep: wp.array[float], + body_dofadr: wp.array[int], + flex_dim: wp.array[int], + flex_vertadr: wp.array[int], + flex_edgeadr: wp.array[int], + flex_elemadr: wp.array[int], + flex_elemnum: wp.array[int], + flex_elemdataadr: wp.array[int], + flex_elemedgeadr: wp.array[int], + flex_vertbodyid: wp.array[int], + flex_elem: wp.array[int], + flex_elemedge: wp.array[int], + flexedge_length0: wp.array[float], + flex_stiffness: wp.array2d[float], + flex_damping: wp.array[float], # Data in: - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), - flexedge_length_in: wp.array2d(dtype=float), - flexedge_velocity_in: wp.array2d(dtype=float), + flexvert_xpos_in: wp.array2d[wp.vec3], + flexedge_length_in: wp.array2d[float], + flexedge_velocity_in: wp.array2d[float], # In: dsbl_damper: bool, # Data out: - qfrc_spring_out: wp.array2d(dtype=float), + qfrc_spring_out: wp.array2d[float], ): worldid, elemid = wp.tid() timestep = opt_timestep[worldid % opt_timestep.shape[0]] @@ -665,19 +665,19 @@ def _flex_elasticity( def _flex_bending( # Model: nflex: int, - body_dofadr: wp.array(dtype=int), - flex_dim: wp.array(dtype=int), - flex_vertadr: wp.array(dtype=int), - flex_edgeadr: wp.array(dtype=int), - flex_edgenum: wp.array(dtype=int), - flex_vertbodyid: wp.array(dtype=int), - flex_edge: wp.array(dtype=wp.vec2i), - flex_edgeflap: wp.array(dtype=wp.vec2i), - flex_bending: wp.array2d(dtype=float), + body_dofadr: wp.array[int], + flex_dim: wp.array[int], + flex_vertadr: wp.array[int], + flex_edgeadr: wp.array[int], + flex_edgenum: wp.array[int], + flex_vertbodyid: wp.array[int], + flex_edge: wp.array[wp.vec2i], + flex_edgeflap: wp.array[wp.vec2i], + flex_bending: wp.array2d[float], # Data in: - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + flexvert_xpos_in: wp.array2d[wp.vec3], # Data out: - qfrc_spring_out: wp.array2d(dtype=float), + qfrc_spring_out: wp.array2d[float], ): worldid, edgeid = wp.tid() nvert = 4 diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/ray.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/ray.py index 57d6a5ba..a320644a 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/ray.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/ray.py @@ -52,12 +52,12 @@ def _ray_map(pos: wp.vec3, mat: wp.mat33, pnt: wp.vec3, vec: wp.vec3) -> Tuple[w @wp.func def _ray_eliminate( # Model: - body_weldid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - geom_matid: wp.array(dtype=int), # kernel_analyzer: ignore - geom_group: wp.array(dtype=int), - geom_rgba: wp.array(dtype=wp.vec4), # kernel_analyzer: ignore - mat_rgba: wp.array(dtype=wp.vec4), # kernel_analyzer: ignore + body_weldid: wp.array[int], + geom_bodyid: wp.array[int], + geom_matid: wp.array[int], # kernel_analyzer: ignore + geom_group: wp.array[int], + geom_rgba: wp.array[wp.vec4], # kernel_analyzer: ignore + mat_rgba: wp.array[wp.vec4], # kernel_analyzer: ignore # In: geomid: int, geomgroup: vec6, @@ -451,26 +451,27 @@ def ray_box(pos: wp.vec3, mat: wp.mat33, size: wp.vec3, pnt: wp.vec3, vec: wp.ve @wp.func def ray_hfield( # Model: - geom_type: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - hfield_size: wp.array(dtype=wp.vec4), - hfield_nrow: wp.array(dtype=int), - hfield_ncol: wp.array(dtype=int), - hfield_adr: wp.array(dtype=int), - hfield_data: wp.array(dtype=float), + geom_type: wp.array[int], + geom_dataid: wp.array2d[int], + hfield_size: wp.array[wp.vec4], + hfield_nrow: wp.array[int], + hfield_ncol: wp.array[int], + hfield_adr: wp.array[int], + hfield_data: wp.array[float], # In: pos: wp.vec3, mat: wp.mat33, pnt: wp.vec3, vec: wp.vec3, id: int, + worldid: int, ) -> Tuple[float, wp.vec3]: # check geom type if geom_type[id] != GeomType.HFIELD: return -1.0, wp.vec3() # hfield id and dimensions - hid = geom_dataid[id] + hid = geom_dataid[worldid % geom_dataid.shape[0], id] nrow = hfield_nrow[hid] ncol = hfield_ncol[hid] @@ -622,10 +623,10 @@ def ray_hfield( def ray_mesh( # Model: nmeshface: int, - mesh_vertadr: wp.array(dtype=int), - mesh_faceadr: wp.array(dtype=int), - mesh_vert: wp.array(dtype=wp.vec3), - mesh_face: wp.array(dtype=wp.vec3i), + mesh_vertadr: wp.array[int], + mesh_faceadr: wp.array[int], + mesh_vert: wp.array[wp.vec3], + mesh_face: wp.array[wp.vec3i], # In: data_id: int, pos: wp.vec3, @@ -699,7 +700,7 @@ def ray_mesh( @wp.func def ray_mesh_with_bvh( # In: - mesh_bvh_id: wp.array(dtype=wp.uint64), + mesh_bvh_id: wp.array[wp.uint64], mesh_geom_id: int, pos: wp.vec3, mat: wp.mat33, @@ -732,7 +733,7 @@ def ray_mesh_with_bvh( @wp.func def ray_mesh_with_bvh_anyhit( # In: - mesh_bvh_id: wp.array(dtype=wp.uint64), + mesh_bvh_id: wp.array[wp.uint64], mesh_geom_id: int, pos: wp.vec3, mat: wp.mat33, @@ -752,7 +753,7 @@ def ray_mesh_with_bvh_anyhit( @wp.func def ray_flex_with_bvh( # In: - flex_bvh_id: wp.array(dtype=wp.uint64), + flex_bvh_id: wp.array[wp.uint64], flexid: int, group_root: int, pnt: wp.vec3, @@ -781,7 +782,7 @@ def ray_flex_with_bvh( @wp.func def ray_flex_with_bvh_anyhit( # In: - flex_bvh_id: wp.array(dtype=wp.uint64), + flex_bvh_id: wp.array[wp.uint64], flexid: int, group_root: int, pnt: wp.vec3, @@ -823,27 +824,27 @@ def ray_geom(pos: wp.vec3, mat: wp.mat33, size: wp.vec3, pnt: wp.vec3, vec: wp.v def _ray_geom_mesh( # Model: nmeshface: int, - body_weldid: wp.array(dtype=int), - geom_type: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_matid: wp.array2d(dtype=int), - geom_group: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - geom_rgba: wp.array2d(dtype=wp.vec4), - mesh_vertadr: wp.array(dtype=int), - mesh_faceadr: wp.array(dtype=int), - mesh_vert: wp.array(dtype=wp.vec3), - mesh_face: wp.array(dtype=wp.vec3i), - hfield_size: wp.array(dtype=wp.vec4), - hfield_nrow: wp.array(dtype=int), - hfield_ncol: wp.array(dtype=int), - hfield_adr: wp.array(dtype=int), - hfield_data: wp.array(dtype=float), - mat_rgba: wp.array2d(dtype=wp.vec4), + body_weldid: wp.array[int], + geom_type: wp.array[int], + geom_bodyid: wp.array[int], + geom_dataid: wp.array2d[int], + geom_matid: wp.array2d[int], + geom_group: wp.array[int], + geom_size: wp.array2d[wp.vec3], + geom_rgba: wp.array2d[wp.vec4], + mesh_vertadr: wp.array[int], + mesh_faceadr: wp.array[int], + mesh_vert: wp.array[wp.vec3], + mesh_face: wp.array[wp.vec3i], + hfield_size: wp.array[wp.vec4], + hfield_nrow: wp.array[int], + hfield_ncol: wp.array[int], + hfield_adr: wp.array[int], + hfield_data: wp.array[float], + mat_rgba: wp.array2d[wp.vec4], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], # In: worldid: int, pnt: wp.vec3, @@ -876,7 +877,7 @@ def _ray_geom_mesh( mesh_faceadr, mesh_vert, mesh_face, - geom_dataid[geomid], + geom_dataid[worldid % geom_dataid.shape[0], geomid], pos, mat, geom_size[worldid % geom_size.shape[0], geomid], @@ -897,6 +898,7 @@ def _ray_geom_mesh( pnt, vec, geomid, + worldid, ) else: return ray_geom(pos, mat, geom_size[worldid % geom_size.shape[0], geomid], pnt, vec, type) @@ -909,37 +911,37 @@ def _ray( # Model: ngeom: int, nmeshface: int, - body_weldid: wp.array(dtype=int), - geom_type: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_matid: wp.array2d(dtype=int), - geom_group: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - geom_rgba: wp.array2d(dtype=wp.vec4), - mesh_vertadr: wp.array(dtype=int), - mesh_faceadr: wp.array(dtype=int), - mesh_vert: wp.array(dtype=wp.vec3), - mesh_face: wp.array(dtype=wp.vec3i), - hfield_size: wp.array(dtype=wp.vec4), - hfield_nrow: wp.array(dtype=int), - hfield_ncol: wp.array(dtype=int), - hfield_adr: wp.array(dtype=int), - hfield_data: wp.array(dtype=float), - mat_rgba: wp.array2d(dtype=wp.vec4), + body_weldid: wp.array[int], + geom_type: wp.array[int], + geom_bodyid: wp.array[int], + geom_dataid: wp.array2d[int], + geom_matid: wp.array2d[int], + geom_group: wp.array[int], + geom_size: wp.array2d[wp.vec3], + geom_rgba: wp.array2d[wp.vec4], + mesh_vertadr: wp.array[int], + mesh_faceadr: wp.array[int], + mesh_vert: wp.array[wp.vec3], + mesh_face: wp.array[wp.vec3i], + hfield_size: wp.array[wp.vec4], + hfield_nrow: wp.array[int], + hfield_ncol: wp.array[int], + hfield_adr: wp.array[int], + hfield_data: wp.array[float], + mat_rgba: wp.array2d[wp.vec4], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], # In: - pnt: wp.array2d(dtype=wp.vec3), - vec: wp.array2d(dtype=wp.vec3), + pnt: wp.array2d[wp.vec3], + vec: wp.array2d[wp.vec3], geomgroup: vec6, flg_static: bool, - bodyexclude: wp.array(dtype=int), + bodyexclude: wp.array[int], # Out: - dist_out: wp.array2d(dtype=float), - geomid_out: wp.array2d(dtype=int), - normal_out: wp.array2d(dtype=wp.vec3), + dist_out: wp.array2d[float], + geomid_out: wp.array2d[int], + normal_out: wp.array2d[wp.vec3], ): worldid, rayid, tid = wp.tid() @@ -1011,18 +1013,18 @@ def _ray( @wp.func def _ray_geom_mesh_bvh( # Model: - body_weldid: wp.array(dtype=int), - geom_type: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_matid: wp.array2d(dtype=int), - geom_group: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - geom_rgba: wp.array2d(dtype=wp.vec4), - mat_rgba: wp.array2d(dtype=wp.vec4), + body_weldid: wp.array[int], + geom_type: wp.array[int], + geom_bodyid: wp.array[int], + geom_dataid: wp.array2d[int], + geom_matid: wp.array2d[int], + geom_group: wp.array[int], + geom_size: wp.array2d[wp.vec3], + geom_rgba: wp.array2d[wp.vec4], + mat_rgba: wp.array2d[wp.vec4], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], # In: worldid: int, pnt: wp.vec3, @@ -1031,8 +1033,8 @@ def _ray_geom_mesh_bvh( flg_static: bool, bodyexclude: int, geomid: int, - mesh_bvh_id: wp.array(dtype=wp.uint64), - hfield_bvh_id: wp.array(dtype=wp.uint64), + mesh_bvh_id: wp.array[wp.uint64], + hfield_bvh_id: wp.array[wp.uint64], min_dist: float, ) -> Tuple[float, wp.vec3]: if not _ray_eliminate( @@ -1055,7 +1057,7 @@ def _ray_geom_mesh_bvh( bvh_ids = mesh_bvh_id if gtype == GeomType.MESH else hfield_bvh_id t, n, u, v, f, geom_mesh_id = ray_mesh_with_bvh( bvh_ids, - geom_dataid[geomid], + geom_dataid[worldid % geom_dataid.shape[0], geomid], pos, mat, pnt, @@ -1081,33 +1083,33 @@ def _ray_geom_mesh_bvh( def _ray_bvh( # Model: ngeom: int, - body_weldid: wp.array(dtype=int), - geom_type: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_matid: wp.array2d(dtype=int), - geom_group: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - geom_rgba: wp.array2d(dtype=wp.vec4), - mat_rgba: wp.array2d(dtype=wp.vec4), + body_weldid: wp.array[int], + geom_type: wp.array[int], + geom_bodyid: wp.array[int], + geom_dataid: wp.array2d[int], + geom_matid: wp.array2d[int], + geom_group: wp.array[int], + geom_size: wp.array2d[wp.vec3], + geom_rgba: wp.array2d[wp.vec4], + mat_rgba: wp.array2d[wp.vec4], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], # In: - pnt: wp.array2d(dtype=wp.vec3), - vec: wp.array2d(dtype=wp.vec3), + pnt: wp.array2d[wp.vec3], + vec: wp.array2d[wp.vec3], geomgroup: vec6, flg_static: bool, - bodyexclude: wp.array(dtype=int), + bodyexclude: wp.array[int], bvh_id: wp.uint64, - group_root: wp.array(dtype=int), - enabled_geom_ids: wp.array(dtype=int), - mesh_bvh_id: wp.array(dtype=wp.uint64), - hfield_bvh_id: wp.array(dtype=wp.uint64), + group_root: wp.array[int], + enabled_geom_ids: wp.array[int], + mesh_bvh_id: wp.array[wp.uint64], + hfield_bvh_id: wp.array[wp.uint64], # Out: - dist_out: wp.array2d(dtype=float), - geomid_out: wp.array2d(dtype=int), - normal_out: wp.array2d(dtype=wp.vec3), + dist_out: wp.array2d[float], + geomid_out: wp.array2d[int], + normal_out: wp.array2d[wp.vec3], ): worldid, rayid = wp.tid() @@ -1166,8 +1168,8 @@ def _ray_bvh( def ray( m: Model, d: Data, - pnt: wp.array2d(dtype=wp.vec3), - vec: wp.array2d(dtype=wp.vec3), + pnt: wp.array2d[wp.vec3], + vec: wp.array2d[wp.vec3], geomgroup: vec6 | None = None, flg_static: bool = True, bodyexclude: int = -1, @@ -1210,14 +1212,14 @@ def ray( def rays( m: Model, d: Data, - pnt: wp.array2d(dtype=wp.vec3), - vec: wp.array2d(dtype=wp.vec3), + pnt: wp.array2d[wp.vec3], + vec: wp.array2d[wp.vec3], geomgroup: vec6, flg_static: bool, - bodyexclude: wp.array(dtype=int), - dist: wp.array2d(dtype=float), - geomid: wp.array2d(dtype=int), - normal: wp.array2d(dtype=wp.vec3), + bodyexclude: wp.array[int], + dist: wp.array2d[float], + geomid: wp.array2d[int], + normal: wp.array2d[wp.vec3], rc: RenderContext | None = None, ): """Ray intersection for multiple worlds and multiple rays. diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render.py index bc8d16c3..371032e1 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render.py @@ -43,17 +43,17 @@ wp.set_module_options({"enable_backward": False}) @wp.func def sample_texture( # Model: - geom_type: wp.array(dtype=int), - mesh_faceadr: wp.array(dtype=int), + geom_type: wp.array[int], + mesh_faceadr: wp.array[int], # In: geom_id: int, tex_repeat: wp.vec2, tex: wp.Texture2D, pos: wp.vec3, rot: wp.mat33, - mesh_facetexcoord: wp.array(dtype=wp.vec3i), - mesh_texcoord: wp.array(dtype=wp.vec2), - mesh_texcoord_offsets: wp.array(dtype=int), + mesh_facetexcoord: wp.array[wp.vec3i], + mesh_texcoord: wp.array[wp.vec2], + mesh_texcoord_offsets: wp.array[int], hit_point: wp.vec3, bary_u: float, bary_v: float, @@ -88,29 +88,29 @@ def sample_texture( @wp.func def cast_ray( # Model: - geom_type: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - flex_vertadr: wp.array(dtype=int), - flex_edge: wp.array(dtype=wp.vec2i), - flex_radius: wp.array(dtype=float), + geom_type: wp.array[int], + geom_dataid: wp.array2d[int], + geom_size: wp.array2d[wp.vec3], + flex_vertadr: wp.array[int], + flex_edge: wp.array[wp.vec2i], + flex_radius: wp.array[float], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], + flexvert_xpos_in: wp.array2d[wp.vec3], # In: bvh_id: wp.uint64, group_root: int, worldid: int, bvh_ngeom: int, flex_bvh_ngeom: int, - enabled_geom_ids: wp.array(dtype=int), - mesh_bvh_id: wp.array(dtype=wp.uint64), - hfield_bvh_id: wp.array(dtype=wp.uint64), - flex_geom_flexid: wp.array(dtype=int), - flex_geom_edgeid: wp.array(dtype=int), - flex_bvh_id: wp.array(dtype=wp.uint64), - flex_group_root: wp.array2d(dtype=int), + enabled_geom_ids: wp.array[int], + mesh_bvh_id: wp.array[wp.uint64], + hfield_bvh_id: wp.array[wp.uint64], + flex_geom_flexid: wp.array[int], + flex_geom_edgeid: wp.array[int], + flex_bvh_id: wp.array[wp.uint64], + flex_group_root: wp.array2d[int], ray_origin_world: wp.vec3, ray_dir_world: wp.vec3, ) -> Tuple[int, float, wp.vec3, float, float, int, int]: @@ -159,7 +159,7 @@ def cast_ray( if gtype == GeomType.HFIELD: d, n, u, v, f, geom_hfield_id = ray_mesh_with_bvh( hfield_bvh_id, - geom_dataid[gi], + geom_dataid[worldid % geom_dataid.shape[0], gi], geom_xpos_in[worldid, gi], geom_xmat_in[worldid, gi], ray_origin_world, @@ -208,7 +208,7 @@ def cast_ray( if gtype == GeomType.MESH: d, n, u, v, f, hit_mesh_id = ray_mesh_with_bvh( mesh_bvh_id, - geom_dataid[gi], + geom_dataid[worldid % geom_dataid.shape[0], gi], geom_xpos_in[worldid, gi], geom_xmat_in[worldid, gi], ray_origin_world, @@ -256,29 +256,29 @@ def cast_ray( @wp.func def cast_ray_first_hit( # Model: - geom_type: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - flex_vertadr: wp.array(dtype=int), - flex_edge: wp.array(dtype=wp.vec2i), - flex_radius: wp.array(dtype=float), + geom_type: wp.array[int], + geom_dataid: wp.array2d[int], + geom_size: wp.array2d[wp.vec3], + flex_vertadr: wp.array[int], + flex_edge: wp.array[wp.vec2i], + flex_radius: wp.array[float], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], + flexvert_xpos_in: wp.array2d[wp.vec3], # In: bvh_id: wp.uint64, group_root: int, worldid: int, bvh_ngeom: int, bvh_nflexgeom: int, - enabled_geom_ids: wp.array(dtype=int), - mesh_bvh_id: wp.array(dtype=wp.uint64), - hfield_bvh_id: wp.array(dtype=wp.uint64), - flex_geom_flexid: wp.array(dtype=int), - flex_geom_edgeid: wp.array(dtype=int), - flex_bvh_id: wp.array(dtype=wp.uint64), - flex_group_root: wp.array2d(dtype=int), + enabled_geom_ids: wp.array[int], + mesh_bvh_id: wp.array[wp.uint64], + hfield_bvh_id: wp.array[wp.uint64], + flex_geom_flexid: wp.array[int], + flex_geom_edgeid: wp.array[int], + flex_bvh_id: wp.array[wp.uint64], + flex_group_root: wp.array2d[int], ray_origin_world: wp.vec3, ray_dir_world: wp.vec3, max_dist: float, @@ -314,7 +314,7 @@ def cast_ray_first_hit( if gtype == GeomType.HFIELD: d, n, u, v, f, geom_hfield_id = ray_mesh_with_bvh( hfield_bvh_id, - geom_dataid[gi], + geom_dataid[worldid % geom_dataid.shape[0], gi], geom_xpos_in[worldid, gi], geom_xmat_in[worldid, gi], ray_origin_world, @@ -363,7 +363,7 @@ def cast_ray_first_hit( if gtype == GeomType.MESH: hit = ray_mesh_with_bvh_anyhit( mesh_bvh_id, - geom_dataid[gi], + geom_dataid[worldid % geom_dataid.shape[0], gi], geom_xpos_in[worldid, gi], geom_xmat_in[worldid, gi], ray_origin_world, @@ -409,30 +409,30 @@ def cast_ray_first_hit( @wp.func def compute_lighting( # Model: - geom_type: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - flex_vertadr: wp.array(dtype=int), - flex_edge: wp.array(dtype=wp.vec2i), - flex_radius: wp.array(dtype=float), + geom_type: wp.array[int], + geom_dataid: wp.array2d[int], + geom_size: wp.array2d[wp.vec3], + flex_vertadr: wp.array[int], + flex_edge: wp.array[wp.vec2i], + flex_radius: wp.array[float], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], + flexvert_xpos_in: wp.array2d[wp.vec3], # In: use_shadows: bool, bvh_id: wp.uint64, group_root: int, bvh_ngeom: int, bvh_nflexgeom: int, - enabled_geom_ids: wp.array(dtype=int), + enabled_geom_ids: wp.array[int], worldid: int, - mesh_bvh_id: wp.array(dtype=wp.uint64), - hfield_bvh_id: wp.array(dtype=wp.uint64), - flex_geom_flexid: wp.array(dtype=int), - flex_geom_edgeid: wp.array(dtype=int), - flex_bvh_id: wp.array(dtype=wp.uint64), - flex_group_root: wp.array2d(dtype=int), + mesh_bvh_id: wp.array[wp.uint64], + hfield_bvh_id: wp.array[wp.uint64], + flex_geom_flexid: wp.array[int], + flex_geom_edgeid: wp.array[int], + flex_bvh_id: wp.array[wp.uint64], + flex_group_root: wp.array2d[int], lightactive: bool, lighttype: int, lightcastshadow: bool, @@ -530,65 +530,65 @@ def render(m: Model, d: Data, rc: RenderContext): @wp.kernel(module="unique", enable_backward=False) def _render_megakernel( # Model: - geom_type: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_matid: wp.array2d(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - geom_rgba: wp.array2d(dtype=wp.vec4), - cam_projection: wp.array(dtype=int), - cam_fovy: wp.array2d(dtype=float), - cam_sensorsize: wp.array(dtype=wp.vec2), - cam_intrinsic: wp.array2d(dtype=wp.vec4), - light_type: wp.array2d(dtype=int), - light_castshadow: wp.array2d(dtype=bool), - light_active: wp.array2d(dtype=bool), - flex_vertadr: wp.array(dtype=int), - flex_edge: wp.array(dtype=wp.vec2i), - flex_radius: wp.array(dtype=float), - mesh_faceadr: wp.array(dtype=int), - mat_texid: wp.array3d(dtype=int), - mat_texrepeat: wp.array2d(dtype=wp.vec2), - mat_rgba: wp.array2d(dtype=wp.vec4), + geom_type: wp.array[int], + geom_dataid: wp.array2d[int], + geom_matid: wp.array2d[int], + geom_size: wp.array2d[wp.vec3], + geom_rgba: wp.array2d[wp.vec4], + cam_projection: wp.array[int], + cam_fovy: wp.array2d[float], + cam_sensorsize: wp.array[wp.vec2], + cam_intrinsic: wp.array2d[wp.vec4], + light_type: wp.array2d[int], + light_castshadow: wp.array2d[bool], + light_active: wp.array2d[bool], + flex_vertadr: wp.array[int], + flex_edge: wp.array[wp.vec2i], + flex_radius: wp.array[float], + mesh_faceadr: wp.array[int], + mat_texid: wp.array3d[int], + mat_texrepeat: wp.array2d[wp.vec2], + mat_rgba: wp.array2d[wp.vec4], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - cam_xpos_in: wp.array2d(dtype=wp.vec3), - cam_xmat_in: wp.array2d(dtype=wp.mat33), - light_xpos_in: wp.array2d(dtype=wp.vec3), - light_xdir_in: wp.array2d(dtype=wp.vec3), - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], + cam_xpos_in: wp.array2d[wp.vec3], + cam_xmat_in: wp.array2d[wp.mat33], + light_xpos_in: wp.array2d[wp.vec3], + light_xdir_in: wp.array2d[wp.vec3], + flexvert_xpos_in: wp.array2d[wp.vec3], # In: nrender: int, use_shadows: bool, bvh_ngeom: int, bvh_nflexgeom: int, - cam_res: wp.array(dtype=wp.vec2i), - cam_id_map: wp.array(dtype=int), - ray: wp.array(dtype=wp.vec3), - rgb_adr: wp.array(dtype=int), - depth_adr: wp.array(dtype=int), - seg_adr: wp.array(dtype=int), - render_rgb: wp.array(dtype=bool), - render_depth: wp.array(dtype=bool), - render_seg: wp.array(dtype=bool), + cam_res: wp.array[wp.vec2i], + cam_id_map: wp.array[int], + ray: wp.array[wp.vec3], + rgb_adr: wp.array[int], + depth_adr: wp.array[int], + seg_adr: wp.array[int], + render_rgb: wp.array[bool], + render_depth: wp.array[bool], + render_seg: wp.array[bool], bvh_id: wp.uint64, - group_root: wp.array(dtype=int), - flex_bvh_id: wp.array(dtype=wp.uint64), - flex_group_root: wp.array2d(dtype=int), - enabled_geom_ids: wp.array(dtype=int), - mesh_bvh_id: wp.array(dtype=wp.uint64), - mesh_facetexcoord: wp.array(dtype=wp.vec3i), - mesh_texcoord: wp.array(dtype=wp.vec2), - mesh_texcoord_offsets: wp.array(dtype=int), - hfield_bvh_id: wp.array(dtype=wp.uint64), - flex_rgba: wp.array(dtype=wp.vec4), - flex_geom_flexid: wp.array(dtype=int), - flex_geom_edgeid: wp.array(dtype=int), - textures: wp.array(dtype=wp.Texture2D), + group_root: wp.array[int], + flex_bvh_id: wp.array[wp.uint64], + flex_group_root: wp.array2d[int], + enabled_geom_ids: wp.array[int], + mesh_bvh_id: wp.array[wp.uint64], + mesh_facetexcoord: wp.array[wp.vec3i], + mesh_texcoord: wp.array[wp.vec2], + mesh_texcoord_offsets: wp.array[int], + hfield_bvh_id: wp.array[wp.uint64], + flex_rgba: wp.array[wp.vec4], + flex_geom_flexid: wp.array[int], + flex_geom_edgeid: wp.array[int], + textures: wp.array[wp.Texture2D], # Out: - rgb_out: wp.array2d(dtype=wp.uint32), - depth_out: wp.array2d(dtype=float), - seg_out: wp.array2d(dtype=int), + rgb_out: wp.array2d[wp.uint32], + depth_out: wp.array2d[float], + seg_out: wp.array2d[int], ): worldid, rayid = wp.tid() diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render_util.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render_util.py index 36958f8e..c59da005 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render_util.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render_util.py @@ -28,9 +28,9 @@ def _convert_texture_data( width: int, adr: int, nc: int, - tex_data_in: wp.array(dtype=wp.uint8), + tex_data_in: wp.array[wp.uint8], # Out: - tex_data_out: wp.array3d(dtype=float), + tex_data_out: wp.array3d[float], ): """Convert uint8 texture data to vec4 format for efficient sampling.""" x, y = wp.tid() @@ -134,11 +134,11 @@ def pack_rgba_to_uint32(r: float, g: float, b: float, a: float) -> wp.uint32: @wp.kernel def unpack_rgb_kernel( # In: - packed: wp.array2d(dtype=wp.uint32), - rgb_adr: wp.array(dtype=int), + packed: wp.array2d[wp.uint32], + rgb_adr: wp.array[int], camera_index: int, # Out: - rgb_out: wp.array3d(dtype=wp.vec3), + rgb_out: wp.array3d[wp.vec3], ): """Unpack ABGR uint32 packed pixel data into separate R, G, and B channels.""" worldid, pixelid = wp.tid() @@ -157,12 +157,12 @@ def unpack_rgb_kernel( @wp.kernel def extract_depth_kernel( # In: - depth_data: wp.array2d(dtype=float), - depth_adr: wp.array(dtype=int), + depth_data: wp.array2d[float], + depth_adr: wp.array[int], camera_index: int, depth_scale: float, # Out: - depth_out: wp.array3d(dtype=float), + depth_out: wp.array3d[float], ): """Extract the depth data from the render context buffers for a given camera index.""" worldid, pixelid = wp.tid() @@ -174,7 +174,7 @@ def extract_depth_kernel( depth_out[worldid, yid, xid] = wp.clamp(val / depth_scale, 0.0, 1.0) -def get_rgb(rc: RenderContext, camera_index: int, rgb_out: wp.array3d(dtype=wp.vec3)): +def get_rgb(rc: RenderContext, camera_index: int, rgb_out: wp.array3d[wp.vec3]): """Get the RGB data output from the render context buffers for a given camera index. Args: @@ -190,7 +190,7 @@ def get_rgb(rc: RenderContext, camera_index: int, rgb_out: wp.array3d(dtype=wp.v ) -def get_depth(rc: RenderContext, camera_index: int, depth_scale: float, depth_out: wp.array3d(dtype=float)): +def get_depth(rc: RenderContext, camera_index: int, depth_scale: float, depth_out: wp.array3d[float]): """Get the depth data output from the render context buffers for a given camera index. Args: @@ -211,11 +211,11 @@ def get_depth(rc: RenderContext, camera_index: int, depth_scale: float, depth_ou @wp.kernel def _extract_seg_kernel( # In: - seg_data: wp.array2d(dtype=int), - seg_adr: wp.array(dtype=int), + seg_data: wp.array2d[int], + seg_adr: wp.array[int], camera_index: int, # Out: - seg_out: wp.array3d(dtype=int), + seg_out: wp.array3d[int], ): """Extract per-pixel geom IDs from the render context buffers for a given camera index.""" worldid, pixelid = wp.tid() @@ -226,7 +226,7 @@ def _extract_seg_kernel( seg_out[worldid, yid, xid] = seg_data[worldid, seg_adr_offset + pixelid] -def get_segmentation(rc: RenderContext, camera_index: int, seg_out: wp.array3d(dtype=int)): +def get_segmentation(rc: RenderContext, camera_index: int, seg_out: wp.array3d[int]): """Get the segmentation data from the render context buffers for a given camera index. Each pixel contains the MuJoCo geom ID of the geometry hit by the ray, -1 for diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py index 2c8177b8..b615d20c 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py @@ -53,15 +53,15 @@ wp.set_module_options({"enable_backward": False}) @wp.func def _write_scalar( # Model: - sensor_type: wp.array(dtype=int), - sensor_datatype: wp.array(dtype=int), - sensor_adr: wp.array(dtype=int), - sensor_cutoff: wp.array(dtype=float), + sensor_type: wp.array[int], + sensor_datatype: wp.array[int], + sensor_adr: wp.array[int], + sensor_cutoff: wp.array[float], # In: sensorid: int, sensor: Any, # Out: - out: wp.array(dtype=float), + out: wp.array[float], ): adr = sensor_adr[sensorid] cutoff = sensor_cutoff[sensorid] @@ -81,16 +81,16 @@ def _write_scalar( @wp.func def _write_vector( # Model: - sensor_type: wp.array(dtype=int), - sensor_datatype: wp.array(dtype=int), - sensor_adr: wp.array(dtype=int), - sensor_cutoff: wp.array(dtype=float), + sensor_type: wp.array[int], + sensor_datatype: wp.array[int], + sensor_adr: wp.array[int], + sensor_cutoff: wp.array[float], # In: sensorid: int, sensordim: int, sensor: Any, # Out: - out: wp.array(dtype=float), + out: wp.array[float], ): adr = sensor_adr[sensorid] cutoff = sensor_cutoff[sensorid] @@ -113,9 +113,9 @@ def _write_vector( @wp.func def _magnetometer( # Model: - opt_magnetic: wp.array(dtype=wp.vec3), + opt_magnetic: wp.array[wp.vec3], # Data in: - site_xmat_in: wp.array2d(dtype=wp.mat33), + site_xmat_in: wp.array2d[wp.mat33], # In: worldid: int, objid: int, @@ -127,14 +127,14 @@ def _magnetometer( @wp.func def _cam_projection( # Model: - cam_fovy: wp.array2d(dtype=float), - cam_resolution: wp.array(dtype=wp.vec2i), - cam_sensorsize: wp.array(dtype=wp.vec2), - cam_intrinsic: wp.array2d(dtype=wp.vec4), + cam_fovy: wp.array2d[float], + cam_resolution: wp.array[wp.vec2i], + cam_sensorsize: wp.array[wp.vec2], + cam_intrinsic: wp.array2d[wp.vec4], # Data in: - site_xpos_in: wp.array2d(dtype=wp.vec3), - cam_xpos_in: wp.array2d(dtype=wp.vec3), - cam_xmat_in: wp.array2d(dtype=wp.mat33), + site_xpos_in: wp.array2d[wp.vec3], + cam_xpos_in: wp.array2d[wp.vec3], + cam_xmat_in: wp.array2d[wp.mat33], # In: worldid: int, objid: int, @@ -194,14 +194,14 @@ def _cam_projection( @wp.kernel def _sensor_rangefinder_init( # Model: - sensor_objid: wp.array(dtype=int), - sensor_rangefinder_adr: wp.array(dtype=int), + sensor_objid: wp.array[int], + sensor_rangefinder_adr: wp.array[int], # Data in: - site_xpos_in: wp.array2d(dtype=wp.vec3), - site_xmat_in: wp.array2d(dtype=wp.mat33), + site_xpos_in: wp.array2d[wp.vec3], + site_xmat_in: wp.array2d[wp.mat33], # Out: - pnt_out: wp.array2d(dtype=wp.vec3), - vec_out: wp.array2d(dtype=wp.vec3), + pnt_out: wp.array2d[wp.vec3], + vec_out: wp.array2d[wp.vec3], ): worldid, rfid = wp.tid() sensorid = sensor_rangefinder_adr[rfid] @@ -214,22 +214,22 @@ def _sensor_rangefinder_init( @wp.func -def _joint_pos(jnt_qposadr: wp.array(dtype=int), qpos_in: wp.array2d(dtype=float), worldid: int, objid: int) -> float: +def _joint_pos(jnt_qposadr: wp.array[int], qpos_in: wp.array2d[float], worldid: int, objid: int) -> float: return qpos_in[worldid, jnt_qposadr[objid]] @wp.func -def _tendon_pos(ten_length_in: wp.array2d(dtype=float), worldid: int, objid: int) -> float: +def _tendon_pos(ten_length_in: wp.array2d[float], worldid: int, objid: int) -> float: return ten_length_in[worldid, objid] @wp.func -def _actuator_pos(actuator_length_in: wp.array2d(dtype=float), worldid: int, objid: int) -> float: +def _actuator_pos(actuator_length_in: wp.array2d[float], worldid: int, objid: int) -> float: return actuator_length_in[worldid, objid] @wp.func -def _ball_quat(jnt_qposadr: wp.array(dtype=int), qpos_in: wp.array2d(dtype=float), worldid: int, objid: int) -> wp.quat: +def _ball_quat(jnt_qposadr: wp.array[int], qpos_in: wp.array2d[float], worldid: int, objid: int) -> wp.quat: adr = jnt_qposadr[objid] quat = wp.quat( qpos_in[worldid, adr + 0], @@ -243,22 +243,22 @@ def _ball_quat(jnt_qposadr: wp.array(dtype=int), qpos_in: wp.array2d(dtype=float @wp.kernel def _limit_pos( # Model: - sensor_type: wp.array(dtype=int), - sensor_datatype: wp.array(dtype=int), - sensor_objid: wp.array(dtype=int), - sensor_adr: wp.array(dtype=int), - sensor_cutoff: wp.array(dtype=float), - sensor_limitpos_adr: wp.array(dtype=int), + sensor_type: wp.array[int], + sensor_datatype: wp.array[int], + sensor_objid: wp.array[int], + sensor_adr: wp.array[int], + sensor_cutoff: wp.array[float], + sensor_limitpos_adr: wp.array[int], # Data in: - ne_in: wp.array(dtype=int), - nf_in: wp.array(dtype=int), - nl_in: wp.array(dtype=int), - efc_type_in: wp.array2d(dtype=int), - efc_id_in: wp.array2d(dtype=int), - efc_pos_in: wp.array2d(dtype=float), - efc_margin_in: wp.array2d(dtype=float), + ne_in: wp.array[int], + nf_in: wp.array[int], + nl_in: wp.array[int], + efc_type_in: wp.array2d[int], + efc_id_in: wp.array2d[int], + efc_pos_in: wp.array2d[float], + efc_margin_in: wp.array2d[float], # Data out: - sensordata_out: wp.array2d(dtype=float), + sensordata_out: wp.array2d[float], ): worldid, efcid, limitposid = wp.tid() @@ -281,16 +281,16 @@ def _limit_pos( @wp.func def _frame_pos( # Data in: - xpos_in: wp.array2d(dtype=wp.vec3), - xmat_in: wp.array2d(dtype=wp.mat33), - xipos_in: wp.array2d(dtype=wp.vec3), - ximat_in: wp.array2d(dtype=wp.mat33), - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - site_xpos_in: wp.array2d(dtype=wp.vec3), - site_xmat_in: wp.array2d(dtype=wp.mat33), - cam_xpos_in: wp.array2d(dtype=wp.vec3), - cam_xmat_in: wp.array2d(dtype=wp.mat33), + xpos_in: wp.array2d[wp.vec3], + xmat_in: wp.array2d[wp.mat33], + xipos_in: wp.array2d[wp.vec3], + ximat_in: wp.array2d[wp.mat33], + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], + site_xpos_in: wp.array2d[wp.vec3], + site_xmat_in: wp.array2d[wp.mat33], + cam_xpos_in: wp.array2d[wp.vec3], + cam_xmat_in: wp.array2d[wp.mat33], # In: worldid: int, objid: int, @@ -340,11 +340,11 @@ def _frame_pos( @wp.func def _frame_axis( # Data in: - xmat_in: wp.array2d(dtype=wp.mat33), - ximat_in: wp.array2d(dtype=wp.mat33), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - site_xmat_in: wp.array2d(dtype=wp.mat33), - cam_xmat_in: wp.array2d(dtype=wp.mat33), + xmat_in: wp.array2d[wp.mat33], + ximat_in: wp.array2d[wp.mat33], + geom_xmat_in: wp.array2d[wp.mat33], + site_xmat_in: wp.array2d[wp.mat33], + cam_xmat_in: wp.array2d[wp.mat33], # In: worldid: int, objid: int, @@ -393,15 +393,15 @@ def _frame_axis( @wp.func def _frame_quat( # Model: - body_iquat: wp.array2d(dtype=wp.quat), - geom_bodyid: wp.array(dtype=int), - geom_quat: wp.array2d(dtype=wp.quat), - site_bodyid: wp.array(dtype=int), - site_quat: wp.array2d(dtype=wp.quat), - cam_bodyid: wp.array(dtype=int), - cam_quat: wp.array2d(dtype=wp.quat), + body_iquat: wp.array2d[wp.quat], + geom_bodyid: wp.array[int], + geom_quat: wp.array2d[wp.quat], + site_bodyid: wp.array[int], + site_quat: wp.array2d[wp.quat], + cam_bodyid: wp.array[int], + cam_quat: wp.array2d[wp.quat], # Data in: - xquat_in: wp.array2d(dtype=wp.quat), + xquat_in: wp.array2d[wp.quat], # In: worldid: int, objid: int, @@ -446,12 +446,12 @@ def _frame_quat( @wp.func -def _subtree_com(subtree_com_in: wp.array2d(dtype=wp.vec3), worldid: int, objid: int) -> wp.vec3: +def _subtree_com(subtree_com_in: wp.array2d[wp.vec3], worldid: int, objid: int) -> wp.vec3: return subtree_com_in[worldid, objid] @wp.func -def _clock(time_in: wp.array(dtype=float), worldid: int) -> float: +def _clock(time_in: wp.array[float], worldid: int) -> float: return time_in[worldid] @@ -459,58 +459,58 @@ def _clock(time_in: wp.array(dtype=float), worldid: int) -> float: def _sensor_pos( # Model: ngeom: int, - opt_magnetic: wp.array(dtype=wp.vec3), - body_geomnum: wp.array(dtype=int), - body_geomadr: wp.array(dtype=int), - body_iquat: wp.array2d(dtype=wp.quat), - jnt_qposadr: wp.array(dtype=int), - geom_type: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - geom_quat: wp.array2d(dtype=wp.quat), - site_type: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - site_size: wp.array(dtype=wp.vec3), - site_quat: wp.array2d(dtype=wp.quat), - cam_bodyid: wp.array(dtype=int), - cam_quat: wp.array2d(dtype=wp.quat), - cam_fovy: wp.array2d(dtype=float), - cam_resolution: wp.array(dtype=wp.vec2i), - cam_sensorsize: wp.array(dtype=wp.vec2), - cam_intrinsic: wp.array2d(dtype=wp.vec4), - sensor_type: wp.array(dtype=int), - sensor_datatype: wp.array(dtype=int), - sensor_objtype: wp.array(dtype=int), - sensor_objid: wp.array(dtype=int), - sensor_reftype: wp.array(dtype=int), - sensor_refid: wp.array(dtype=int), - sensor_adr: wp.array(dtype=int), - sensor_cutoff: wp.array(dtype=float), - nxn_pairid: wp.array(dtype=wp.vec2i), - sensor_pos_adr: wp.array(dtype=int), - rangefinder_sensor_adr: wp.array(dtype=int), + opt_magnetic: wp.array[wp.vec3], + body_geomnum: wp.array[int], + body_geomadr: wp.array[int], + body_iquat: wp.array2d[wp.quat], + jnt_qposadr: wp.array[int], + geom_type: wp.array[int], + geom_bodyid: wp.array[int], + geom_quat: wp.array2d[wp.quat], + site_type: wp.array[int], + site_bodyid: wp.array[int], + site_size: wp.array[wp.vec3], + site_quat: wp.array2d[wp.quat], + cam_bodyid: wp.array[int], + cam_quat: wp.array2d[wp.quat], + cam_fovy: wp.array2d[float], + cam_resolution: wp.array[wp.vec2i], + cam_sensorsize: wp.array[wp.vec2], + cam_intrinsic: wp.array2d[wp.vec4], + sensor_type: wp.array[int], + sensor_datatype: wp.array[int], + sensor_objtype: wp.array[int], + sensor_objid: wp.array[int], + sensor_reftype: wp.array[int], + sensor_refid: wp.array[int], + sensor_adr: wp.array[int], + sensor_cutoff: wp.array[float], + nxn_pairid: wp.array[wp.vec2i], + sensor_pos_adr: wp.array[int], + rangefinder_sensor_adr: wp.array[int], # Data in: - time_in: wp.array(dtype=float), - energy_in: wp.array(dtype=wp.vec2), - qpos_in: wp.array2d(dtype=float), - xpos_in: wp.array2d(dtype=wp.vec3), - xquat_in: wp.array2d(dtype=wp.quat), - xmat_in: wp.array2d(dtype=wp.mat33), - xipos_in: wp.array2d(dtype=wp.vec3), - ximat_in: wp.array2d(dtype=wp.mat33), - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - site_xpos_in: wp.array2d(dtype=wp.vec3), - site_xmat_in: wp.array2d(dtype=wp.mat33), - cam_xpos_in: wp.array2d(dtype=wp.vec3), - cam_xmat_in: wp.array2d(dtype=wp.mat33), - subtree_com_in: wp.array2d(dtype=wp.vec3), - ten_length_in: wp.array2d(dtype=float), - actuator_length_in: wp.array2d(dtype=float), + time_in: wp.array[float], + energy_in: wp.array[wp.vec2], + qpos_in: wp.array2d[float], + xpos_in: wp.array2d[wp.vec3], + xquat_in: wp.array2d[wp.quat], + xmat_in: wp.array2d[wp.mat33], + xipos_in: wp.array2d[wp.vec3], + ximat_in: wp.array2d[wp.mat33], + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], + site_xpos_in: wp.array2d[wp.vec3], + site_xmat_in: wp.array2d[wp.mat33], + cam_xpos_in: wp.array2d[wp.vec3], + cam_xmat_in: wp.array2d[wp.mat33], + subtree_com_in: wp.array2d[wp.vec3], + ten_length_in: wp.array2d[float], + actuator_length_in: wp.array2d[float], # In: - rangefinder_dist_in: wp.array2d(dtype=float), - sensor_collision_in: wp.array4d(dtype=float), + rangefinder_dist_in: wp.array2d[float], + sensor_collision_in: wp.array4d[float], # Data out: - sensordata_out: wp.array2d(dtype=float), + sensordata_out: wp.array2d[float], ): worldid, posid = wp.tid() sensorid = sensor_pos_adr[posid] @@ -710,18 +710,18 @@ def _sensor_pos( def _sensor_collision( # Model: ngeom: int, - nxn_pairid: wp.array(dtype=wp.vec2i), + nxn_pairid: wp.array[wp.vec2i], # Data in: - contact_dist_in: wp.array(dtype=float), - contact_pos_in: wp.array(dtype=wp.vec3), - contact_frame_in: wp.array(dtype=wp.mat33), - contact_geom_in: wp.array(dtype=wp.vec2i), - contact_worldid_in: wp.array(dtype=int), - contact_type_in: wp.array(dtype=int), - contact_geomcollisionid_in: wp.array(dtype=int), - nacon_in: wp.array(dtype=int), + contact_dist_in: wp.array[float], + contact_pos_in: wp.array[wp.vec3], + contact_frame_in: wp.array[wp.mat33], + contact_geom_in: wp.array[wp.vec2i], + contact_worldid_in: wp.array[int], + contact_type_in: wp.array[int], + contact_geomcollisionid_in: wp.array[int], + nacon_in: wp.array[int], # Out: - sensor_collision_out: wp.array4d(dtype=float), + sensor_collision_out: wp.array4d[float], ): conid = wp.tid() @@ -908,13 +908,13 @@ def sensor_pos(m: Model, d: Data): @wp.func def _velocimeter( # Model: - body_rootid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), + body_rootid: wp.array[int], + site_bodyid: wp.array[int], # Data in: - site_xpos_in: wp.array2d(dtype=wp.vec3), - site_xmat_in: wp.array2d(dtype=wp.mat33), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cvel_in: wp.array2d(dtype=wp.spatial_vector), + site_xpos_in: wp.array2d[wp.vec3], + site_xmat_in: wp.array2d[wp.mat33], + subtree_com_in: wp.array2d[wp.vec3], + cvel_in: wp.array2d[wp.spatial_vector], # In: worldid: int, objid: int, @@ -933,10 +933,10 @@ def _velocimeter( @wp.func def _gyro( # Model: - site_bodyid: wp.array(dtype=int), + site_bodyid: wp.array[int], # Data in: - site_xmat_in: wp.array2d(dtype=wp.mat33), - cvel_in: wp.array2d(dtype=wp.spatial_vector), + site_xmat_in: wp.array2d[wp.mat33], + cvel_in: wp.array2d[wp.spatial_vector], # In: worldid: int, objid: int, @@ -949,22 +949,22 @@ def _gyro( @wp.func -def _joint_vel(jnt_dofadr: wp.array(dtype=int), qvel_in: wp.array2d(dtype=float), worldid: int, objid: int) -> float: +def _joint_vel(jnt_dofadr: wp.array[int], qvel_in: wp.array2d[float], worldid: int, objid: int) -> float: return qvel_in[worldid, jnt_dofadr[objid]] @wp.func -def _tendon_vel(ten_velocity_in: wp.array2d(dtype=float), worldid: int, objid: int) -> float: +def _tendon_vel(ten_velocity_in: wp.array2d[float], worldid: int, objid: int) -> float: return ten_velocity_in[worldid, objid] @wp.func -def _actuator_vel(actuator_velocity_in: wp.array2d(dtype=float), worldid: int, objid: int) -> float: +def _actuator_vel(actuator_velocity_in: wp.array2d[float], worldid: int, objid: int) -> float: return actuator_velocity_in[worldid, objid] @wp.func -def _ball_ang_vel(jnt_dofadr: wp.array(dtype=int), qvel_in: wp.array2d(dtype=float), worldid: int, objid: int) -> wp.vec3: +def _ball_ang_vel(jnt_dofadr: wp.array[int], qvel_in: wp.array2d[float], worldid: int, objid: int) -> wp.vec3: adr = jnt_dofadr[objid] return wp.vec3(qvel_in[worldid, adr + 0], qvel_in[worldid, adr + 1], qvel_in[worldid, adr + 2]) @@ -972,21 +972,21 @@ def _ball_ang_vel(jnt_dofadr: wp.array(dtype=int), qvel_in: wp.array2d(dtype=flo @wp.kernel def _limit_vel( # Model: - sensor_type: wp.array(dtype=int), - sensor_datatype: wp.array(dtype=int), - sensor_objid: wp.array(dtype=int), - sensor_adr: wp.array(dtype=int), - sensor_cutoff: wp.array(dtype=float), - sensor_limitvel_adr: wp.array(dtype=int), + sensor_type: wp.array[int], + sensor_datatype: wp.array[int], + sensor_objid: wp.array[int], + sensor_adr: wp.array[int], + sensor_cutoff: wp.array[float], + sensor_limitvel_adr: wp.array[int], # Data in: - ne_in: wp.array(dtype=int), - nf_in: wp.array(dtype=int), - nl_in: wp.array(dtype=int), - efc_type_in: wp.array2d(dtype=int), - efc_id_in: wp.array2d(dtype=int), - efc_vel_in: wp.array2d(dtype=float), + ne_in: wp.array[int], + nf_in: wp.array[int], + nl_in: wp.array[int], + efc_type_in: wp.array2d[int], + efc_id_in: wp.array2d[int], + efc_vel_in: wp.array2d[float], # Data out: - sensordata_out: wp.array2d(dtype=float), + sensordata_out: wp.array2d[float], ): worldid, efcid, limitvelid = wp.tid() @@ -1010,18 +1010,18 @@ def _limit_vel( @wp.func def _cvel_offset( # Model: - body_rootid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - cam_bodyid: wp.array(dtype=int), + body_rootid: wp.array[int], + geom_bodyid: wp.array[int], + site_bodyid: wp.array[int], + cam_bodyid: wp.array[int], # Data in: - xpos_in: wp.array2d(dtype=wp.vec3), - xipos_in: wp.array2d(dtype=wp.vec3), - geom_xpos_in: wp.array2d(dtype=wp.vec3), - site_xpos_in: wp.array2d(dtype=wp.vec3), - cam_xpos_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cvel_in: wp.array2d(dtype=wp.spatial_vector), + xpos_in: wp.array2d[wp.vec3], + xipos_in: wp.array2d[wp.vec3], + geom_xpos_in: wp.array2d[wp.vec3], + site_xpos_in: wp.array2d[wp.vec3], + cam_xpos_in: wp.array2d[wp.vec3], + subtree_com_in: wp.array2d[wp.vec3], + cvel_in: wp.array2d[wp.spatial_vector], # In: worldid: int, objtype: int, @@ -1052,23 +1052,23 @@ def _cvel_offset( @wp.func def _frame_linvel( # Model: - body_rootid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - cam_bodyid: wp.array(dtype=int), + body_rootid: wp.array[int], + geom_bodyid: wp.array[int], + site_bodyid: wp.array[int], + cam_bodyid: wp.array[int], # Data in: - xpos_in: wp.array2d(dtype=wp.vec3), - xmat_in: wp.array2d(dtype=wp.mat33), - xipos_in: wp.array2d(dtype=wp.vec3), - ximat_in: wp.array2d(dtype=wp.mat33), - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - site_xpos_in: wp.array2d(dtype=wp.vec3), - site_xmat_in: wp.array2d(dtype=wp.mat33), - cam_xpos_in: wp.array2d(dtype=wp.vec3), - cam_xmat_in: wp.array2d(dtype=wp.mat33), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cvel_in: wp.array2d(dtype=wp.spatial_vector), + xpos_in: wp.array2d[wp.vec3], + xmat_in: wp.array2d[wp.mat33], + xipos_in: wp.array2d[wp.vec3], + ximat_in: wp.array2d[wp.mat33], + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], + site_xpos_in: wp.array2d[wp.vec3], + site_xmat_in: wp.array2d[wp.mat33], + cam_xpos_in: wp.array2d[wp.vec3], + cam_xmat_in: wp.array2d[wp.mat33], + subtree_com_in: wp.array2d[wp.vec3], + cvel_in: wp.array2d[wp.spatial_vector], # In: worldid: int, objid: int, @@ -1158,23 +1158,23 @@ def _frame_linvel( @wp.func def _frame_angvel( # Model: - body_rootid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - cam_bodyid: wp.array(dtype=int), + body_rootid: wp.array[int], + geom_bodyid: wp.array[int], + site_bodyid: wp.array[int], + cam_bodyid: wp.array[int], # Data in: - xpos_in: wp.array2d(dtype=wp.vec3), - xmat_in: wp.array2d(dtype=wp.mat33), - xipos_in: wp.array2d(dtype=wp.vec3), - ximat_in: wp.array2d(dtype=wp.mat33), - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - site_xpos_in: wp.array2d(dtype=wp.vec3), - site_xmat_in: wp.array2d(dtype=wp.mat33), - cam_xpos_in: wp.array2d(dtype=wp.vec3), - cam_xmat_in: wp.array2d(dtype=wp.mat33), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cvel_in: wp.array2d(dtype=wp.spatial_vector), + xpos_in: wp.array2d[wp.vec3], + xmat_in: wp.array2d[wp.mat33], + xipos_in: wp.array2d[wp.vec3], + ximat_in: wp.array2d[wp.mat33], + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], + site_xpos_in: wp.array2d[wp.vec3], + site_xmat_in: wp.array2d[wp.mat33], + cam_xpos_in: wp.array2d[wp.vec3], + cam_xmat_in: wp.array2d[wp.mat33], + subtree_com_in: wp.array2d[wp.vec3], + cvel_in: wp.array2d[wp.spatial_vector], # In: worldid: int, objid: int, @@ -1238,52 +1238,52 @@ def _frame_angvel( @wp.func -def _subtree_linvel(subtree_linvel_in: wp.array2d(dtype=wp.vec3), worldid: int, objid: int) -> wp.vec3: +def _subtree_linvel(subtree_linvel_in: wp.array2d[wp.vec3], worldid: int, objid: int) -> wp.vec3: return subtree_linvel_in[worldid, objid] @wp.func -def _subtree_angmom(subtree_angmom_in: wp.array2d(dtype=wp.vec3), worldid: int, objid: int) -> wp.vec3: +def _subtree_angmom(subtree_angmom_in: wp.array2d[wp.vec3], worldid: int, objid: int) -> wp.vec3: return subtree_angmom_in[worldid, objid] @wp.kernel def _sensor_vel( # Model: - body_rootid: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - cam_bodyid: wp.array(dtype=int), - sensor_type: wp.array(dtype=int), - sensor_datatype: wp.array(dtype=int), - sensor_objtype: wp.array(dtype=int), - sensor_objid: wp.array(dtype=int), - sensor_reftype: wp.array(dtype=int), - sensor_refid: wp.array(dtype=int), - sensor_adr: wp.array(dtype=int), - sensor_cutoff: wp.array(dtype=float), - sensor_vel_adr: wp.array(dtype=int), + body_rootid: wp.array[int], + jnt_dofadr: wp.array[int], + geom_bodyid: wp.array[int], + site_bodyid: wp.array[int], + cam_bodyid: wp.array[int], + sensor_type: wp.array[int], + sensor_datatype: wp.array[int], + sensor_objtype: wp.array[int], + sensor_objid: wp.array[int], + sensor_reftype: wp.array[int], + sensor_refid: wp.array[int], + sensor_adr: wp.array[int], + sensor_cutoff: wp.array[float], + sensor_vel_adr: wp.array[int], # Data in: - qvel_in: wp.array2d(dtype=float), - xpos_in: wp.array2d(dtype=wp.vec3), - xmat_in: wp.array2d(dtype=wp.mat33), - xipos_in: wp.array2d(dtype=wp.vec3), - ximat_in: wp.array2d(dtype=wp.mat33), - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - site_xpos_in: wp.array2d(dtype=wp.vec3), - site_xmat_in: wp.array2d(dtype=wp.mat33), - cam_xpos_in: wp.array2d(dtype=wp.vec3), - cam_xmat_in: wp.array2d(dtype=wp.mat33), - subtree_com_in: wp.array2d(dtype=wp.vec3), - ten_velocity_in: wp.array2d(dtype=float), - actuator_velocity_in: wp.array2d(dtype=float), - cvel_in: wp.array2d(dtype=wp.spatial_vector), - subtree_linvel_in: wp.array2d(dtype=wp.vec3), - subtree_angmom_in: wp.array2d(dtype=wp.vec3), + qvel_in: wp.array2d[float], + xpos_in: wp.array2d[wp.vec3], + xmat_in: wp.array2d[wp.mat33], + xipos_in: wp.array2d[wp.vec3], + ximat_in: wp.array2d[wp.mat33], + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], + site_xpos_in: wp.array2d[wp.vec3], + site_xmat_in: wp.array2d[wp.mat33], + cam_xpos_in: wp.array2d[wp.vec3], + cam_xmat_in: wp.array2d[wp.mat33], + subtree_com_in: wp.array2d[wp.vec3], + ten_velocity_in: wp.array2d[float], + actuator_velocity_in: wp.array2d[float], + cvel_in: wp.array2d[wp.spatial_vector], + subtree_linvel_in: wp.array2d[wp.vec3], + subtree_angmom_in: wp.array2d[wp.vec3], # Data out: - sensordata_out: wp.array2d(dtype=float), + sensordata_out: wp.array2d[float], ): worldid, velid = wp.tid() sensorid = sensor_vel_adr[velid] @@ -1450,14 +1450,14 @@ def sensor_vel(m: Model, d: Data): @wp.func def _accelerometer( # Model: - body_rootid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), + body_rootid: wp.array[int], + site_bodyid: wp.array[int], # Data in: - site_xpos_in: wp.array2d(dtype=wp.vec3), - site_xmat_in: wp.array2d(dtype=wp.mat33), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cvel_in: wp.array2d(dtype=wp.spatial_vector), - cacc_in: wp.array2d(dtype=wp.spatial_vector), + site_xpos_in: wp.array2d[wp.vec3], + site_xmat_in: wp.array2d[wp.mat33], + subtree_com_in: wp.array2d[wp.vec3], + cvel_in: wp.array2d[wp.spatial_vector], + cacc_in: wp.array2d[wp.spatial_vector], # In: worldid: int, objid: int, @@ -1482,10 +1482,10 @@ def _accelerometer( @wp.func def _force( # Model: - site_bodyid: wp.array(dtype=int), + site_bodyid: wp.array[int], # Data in: - site_xmat_in: wp.array2d(dtype=wp.mat33), - cfrc_int_in: wp.array2d(dtype=wp.spatial_vector), + site_xmat_in: wp.array2d[wp.mat33], + cfrc_int_in: wp.array2d[wp.spatial_vector], # In: worldid: int, objid: int, @@ -1499,13 +1499,13 @@ def _force( @wp.func def _torque( # Model: - body_rootid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), + body_rootid: wp.array[int], + site_bodyid: wp.array[int], # Data in: - site_xpos_in: wp.array2d(dtype=wp.vec3), - site_xmat_in: wp.array2d(dtype=wp.mat33), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cfrc_int_in: wp.array2d(dtype=wp.spatial_vector), + site_xpos_in: wp.array2d[wp.vec3], + site_xmat_in: wp.array2d[wp.mat33], + subtree_com_in: wp.array2d[wp.vec3], + cfrc_int_in: wp.array2d[wp.spatial_vector], # In: worldid: int, objid: int, @@ -1518,16 +1518,16 @@ def _torque( @wp.func -def _actuator_force(actuator_force_in: wp.array2d(dtype=float), worldid: int, objid: int) -> float: +def _actuator_force(actuator_force_in: wp.array2d[float], worldid: int, objid: int) -> float: return actuator_force_in[worldid, objid] @wp.func def _joint_actuator_force( # Model: - jnt_dofadr: wp.array(dtype=int), + jnt_dofadr: wp.array[int], # Data in: - qfrc_actuator_in: wp.array2d(dtype=float), + qfrc_actuator_in: wp.array2d[float], # In: worldid: int, objid: int, @@ -1538,15 +1538,15 @@ def _joint_actuator_force( @wp.kernel def _tendon_actuator_force( # Model: - actuator_trntype: wp.array(dtype=int), - actuator_trnid: wp.array(dtype=wp.vec2i), - sensor_objid: wp.array(dtype=int), - sensor_adr: wp.array(dtype=int), - sensor_tendonactfrc_adr: wp.array(dtype=int), + actuator_trntype: wp.array[int], + actuator_trnid: wp.array[wp.vec2i], + sensor_objid: wp.array[int], + sensor_adr: wp.array[int], + sensor_tendonactfrc_adr: wp.array[int], # Data in: - actuator_force_in: wp.array2d(dtype=float), + actuator_force_in: wp.array2d[float], # Data out: - sensordata_out: wp.array2d(dtype=float), + sensordata_out: wp.array2d[float], ): worldid, tenactfrcid, actid = wp.tid() sensorid = sensor_tendonactfrc_adr[tenactfrcid] @@ -1559,15 +1559,15 @@ def _tendon_actuator_force( @wp.kernel def _tendon_actuator_force_cutoff( # Model: - sensor_type: wp.array(dtype=int), - sensor_datatype: wp.array(dtype=int), - sensor_adr: wp.array(dtype=int), - sensor_cutoff: wp.array(dtype=float), - sensor_tendonactfrc_adr: wp.array(dtype=int), + sensor_type: wp.array[int], + sensor_datatype: wp.array[int], + sensor_adr: wp.array[int], + sensor_cutoff: wp.array[float], + sensor_tendonactfrc_adr: wp.array[int], # Data in: - sensordata_in: wp.array2d(dtype=float), + sensordata_in: wp.array2d[float], # Data out: - sensordata_out: wp.array2d(dtype=float), + sensordata_out: wp.array2d[float], ): worldid, tenactfrcid = wp.tid() sensorid = sensor_tendonactfrc_adr[tenactfrcid] @@ -1580,21 +1580,21 @@ def _tendon_actuator_force_cutoff( @wp.kernel def _limit_frc( # Model: - sensor_type: wp.array(dtype=int), - sensor_datatype: wp.array(dtype=int), - sensor_objid: wp.array(dtype=int), - sensor_adr: wp.array(dtype=int), - sensor_cutoff: wp.array(dtype=float), - sensor_limitfrc_adr: wp.array(dtype=int), + sensor_type: wp.array[int], + sensor_datatype: wp.array[int], + sensor_objid: wp.array[int], + sensor_adr: wp.array[int], + sensor_cutoff: wp.array[float], + sensor_limitfrc_adr: wp.array[int], # Data in: - ne_in: wp.array(dtype=int), - nf_in: wp.array(dtype=int), - nl_in: wp.array(dtype=int), - efc_type_in: wp.array2d(dtype=int), - efc_id_in: wp.array2d(dtype=int), - efc_force_in: wp.array2d(dtype=float), + ne_in: wp.array[int], + nf_in: wp.array[int], + nl_in: wp.array[int], + efc_type_in: wp.array2d[int], + efc_id_in: wp.array2d[int], + efc_force_in: wp.array2d[float], # Data out: - sensordata_out: wp.array2d(dtype=float), + sensordata_out: wp.array2d[float], ): worldid, efcid, limitfrcid = wp.tid() @@ -1618,19 +1618,19 @@ def _limit_frc( @wp.func def _framelinacc( # Model: - body_rootid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - cam_bodyid: wp.array(dtype=int), + body_rootid: wp.array[int], + geom_bodyid: wp.array[int], + site_bodyid: wp.array[int], + cam_bodyid: wp.array[int], # Data in: - xpos_in: wp.array2d(dtype=wp.vec3), - xipos_in: wp.array2d(dtype=wp.vec3), - geom_xpos_in: wp.array2d(dtype=wp.vec3), - site_xpos_in: wp.array2d(dtype=wp.vec3), - cam_xpos_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cvel_in: wp.array2d(dtype=wp.spatial_vector), - cacc_in: wp.array2d(dtype=wp.spatial_vector), + xpos_in: wp.array2d[wp.vec3], + xipos_in: wp.array2d[wp.vec3], + geom_xpos_in: wp.array2d[wp.vec3], + site_xpos_in: wp.array2d[wp.vec3], + cam_xpos_in: wp.array2d[wp.vec3], + subtree_com_in: wp.array2d[wp.vec3], + cvel_in: wp.array2d[wp.spatial_vector], + cacc_in: wp.array2d[wp.spatial_vector], # In: worldid: int, objid: int, @@ -1669,11 +1669,11 @@ def _framelinacc( @wp.func def _frameangacc( # Model: - geom_bodyid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - cam_bodyid: wp.array(dtype=int), + geom_bodyid: wp.array[int], + site_bodyid: wp.array[int], + cam_bodyid: wp.array[int], # Data in: - cacc_in: wp.array2d(dtype=wp.spatial_vector), + cacc_in: wp.array2d[wp.spatial_vector], # In: worldid: int, objid: int, @@ -1697,49 +1697,49 @@ def _frameangacc( def _sensor_acc( # Model: opt_cone: int, - body_rootid: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - cam_bodyid: wp.array(dtype=int), - sensor_type: wp.array(dtype=int), - sensor_datatype: wp.array(dtype=int), - sensor_objtype: wp.array(dtype=int), - sensor_objid: wp.array(dtype=int), - sensor_intprm: wp.array2d(dtype=int), - sensor_dim: wp.array(dtype=int), - sensor_adr: wp.array(dtype=int), - sensor_cutoff: wp.array(dtype=float), - sensor_acc_adr: wp.array(dtype=int), - sensor_adr_to_contact_adr: wp.array(dtype=int), + body_rootid: wp.array[int], + jnt_dofadr: wp.array[int], + geom_bodyid: wp.array[int], + site_bodyid: wp.array[int], + cam_bodyid: wp.array[int], + sensor_type: wp.array[int], + sensor_datatype: wp.array[int], + sensor_objtype: wp.array[int], + sensor_objid: wp.array[int], + sensor_intprm: wp.array2d[int], + sensor_dim: wp.array[int], + sensor_adr: wp.array[int], + sensor_cutoff: wp.array[float], + sensor_acc_adr: wp.array[int], + sensor_adr_to_contact_adr: wp.array[int], # Data in: - xpos_in: wp.array2d(dtype=wp.vec3), - xipos_in: wp.array2d(dtype=wp.vec3), - geom_xpos_in: wp.array2d(dtype=wp.vec3), - site_xpos_in: wp.array2d(dtype=wp.vec3), - site_xmat_in: wp.array2d(dtype=wp.mat33), - cam_xpos_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cvel_in: wp.array2d(dtype=wp.spatial_vector), - actuator_force_in: wp.array2d(dtype=float), - qfrc_actuator_in: wp.array2d(dtype=float), - cacc_in: wp.array2d(dtype=wp.spatial_vector), - cfrc_int_in: wp.array2d(dtype=wp.spatial_vector), - contact_dist_in: wp.array(dtype=float), - contact_pos_in: wp.array(dtype=wp.vec3), - contact_frame_in: wp.array(dtype=wp.mat33), - contact_friction_in: wp.array(dtype=vec5), - contact_dim_in: wp.array(dtype=int), - contact_efc_address_in: wp.array2d(dtype=int), - efc_force_in: wp.array2d(dtype=float), + xpos_in: wp.array2d[wp.vec3], + xipos_in: wp.array2d[wp.vec3], + geom_xpos_in: wp.array2d[wp.vec3], + site_xpos_in: wp.array2d[wp.vec3], + site_xmat_in: wp.array2d[wp.mat33], + cam_xpos_in: wp.array2d[wp.vec3], + subtree_com_in: wp.array2d[wp.vec3], + cvel_in: wp.array2d[wp.spatial_vector], + actuator_force_in: wp.array2d[float], + qfrc_actuator_in: wp.array2d[float], + cacc_in: wp.array2d[wp.spatial_vector], + cfrc_int_in: wp.array2d[wp.spatial_vector], + contact_dist_in: wp.array[float], + contact_pos_in: wp.array[wp.vec3], + contact_frame_in: wp.array[wp.mat33], + contact_friction_in: wp.array[vec5], + contact_dim_in: wp.array[int], + contact_efc_address_in: wp.array2d[int], + efc_force_in: wp.array2d[float], njmax_in: int, - nacon_in: wp.array(dtype=int), + nacon_in: wp.array[int], # In: - sensor_contact_nmatch_in: wp.array2d(dtype=int), - sensor_contact_matchid_in: wp.array3d(dtype=int), - sensor_contact_direction_in: wp.array3d(dtype=float), + sensor_contact_nmatch_in: wp.array2d[int], + sensor_contact_matchid_in: wp.array3d[int], + sensor_contact_direction_in: wp.array3d[float], # Data out: - sensordata_out: wp.array2d(dtype=float), + sensordata_out: wp.array2d[float], ): worldid, accid = wp.tid() sensorid = sensor_acc_adr[accid] @@ -2001,26 +2001,26 @@ def _sensor_acc( def _sensor_touch( # Model: opt_cone: int, - geom_bodyid: wp.array(dtype=int), - site_type: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - site_size: wp.array(dtype=wp.vec3), - sensor_objid: wp.array(dtype=int), - sensor_adr: wp.array(dtype=int), - sensor_touch_adr: wp.array(dtype=int), + geom_bodyid: wp.array[int], + site_type: wp.array[int], + site_bodyid: wp.array[int], + site_size: wp.array[wp.vec3], + sensor_objid: wp.array[int], + sensor_adr: wp.array[int], + sensor_touch_adr: wp.array[int], # Data in: - site_xpos_in: wp.array2d(dtype=wp.vec3), - site_xmat_in: wp.array2d(dtype=wp.mat33), - contact_pos_in: wp.array(dtype=wp.vec3), - contact_frame_in: wp.array(dtype=wp.mat33), - contact_dim_in: wp.array(dtype=int), - contact_geom_in: wp.array(dtype=wp.vec2i), - contact_efc_address_in: wp.array2d(dtype=int), - contact_worldid_in: wp.array(dtype=int), - efc_force_in: wp.array2d(dtype=float), - nacon_in: wp.array(dtype=int), + site_xpos_in: wp.array2d[wp.vec3], + site_xmat_in: wp.array2d[wp.mat33], + contact_pos_in: wp.array[wp.vec3], + contact_frame_in: wp.array[wp.mat33], + contact_dim_in: wp.array[int], + contact_geom_in: wp.array[wp.vec2i], + contact_efc_address_in: wp.array2d[int], + contact_worldid_in: wp.array[int], + efc_force_in: wp.array2d[float], + nacon_in: wp.array[int], # Data out: - sensordata_out: wp.array2d(dtype=float), + sensordata_out: wp.array2d[float], ): conid, sensortouchadrid = wp.tid() @@ -2084,15 +2084,15 @@ def _transform_spatial(vec: wp.spatial_vector, dif: wp.vec3) -> wp.vec3: @wp.kernel def _preprocess_tactile_contacts( # Model: - body_weldid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), + body_weldid: wp.array[int], + geom_bodyid: wp.array[int], # Data in: - contact_geom_in: wp.array(dtype=wp.vec2i), - contact_worldid_in: wp.array(dtype=int), - nacon_in: wp.array(dtype=int), + contact_geom_in: wp.array[wp.vec2i], + contact_worldid_in: wp.array[int], + nacon_in: wp.array[int], # Out: - weld_geom_count_out: wp.array2d(dtype=int), - weld_geom_list_out: wp.array3d(dtype=int), + weld_geom_count_out: wp.array2d[int], + weld_geom_list_out: wp.array3d[int], ): conid = wp.tid() ncon = nacon_in[0] @@ -2121,42 +2121,42 @@ def _preprocess_tactile_contacts( @wp.kernel def _sensor_tactile( # Model: - body_rootid: wp.array(dtype=int), - body_weldid: wp.array(dtype=int), - oct_child: wp.array(dtype=vec8i), - oct_aabb: wp.array2d(dtype=wp.vec3), - oct_coeff: wp.array(dtype=vec8), - geom_type: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - mesh_vertadr: wp.array(dtype=int), - mesh_vertnum: wp.array(dtype=int), - mesh_octadr: wp.array(dtype=int), - mesh_normaladr: wp.array(dtype=int), - mesh_normalnum: wp.array(dtype=int), - mesh_vert: wp.array(dtype=wp.vec3), - mesh_normal: wp.array(dtype=wp.vec3), - mesh_quat: wp.array(dtype=wp.quat), - sensor_objid: wp.array(dtype=int), - sensor_refid: wp.array(dtype=int), - sensor_dim: wp.array(dtype=int), - sensor_adr: wp.array(dtype=int), - plugin: wp.array(dtype=int), - plugin_attr: wp.array(dtype=vec_pluginattr), - geom_plugin_index: wp.array(dtype=int), - taxel_vertadr: wp.array(dtype=int), - taxel_sensorid: wp.array(dtype=int), + body_rootid: wp.array[int], + body_weldid: wp.array[int], + oct_child: wp.array[vec8i], + oct_aabb: wp.array2d[wp.vec3], + oct_coeff: wp.array[vec8], + geom_type: wp.array[int], + geom_bodyid: wp.array[int], + geom_dataid: wp.array2d[int], + geom_size: wp.array2d[wp.vec3], + mesh_vertadr: wp.array[int], + mesh_vertnum: wp.array[int], + mesh_octadr: wp.array[int], + mesh_normaladr: wp.array[int], + mesh_normalnum: wp.array[int], + mesh_vert: wp.array[wp.vec3], + mesh_normal: wp.array[wp.vec3], + mesh_quat: wp.array[wp.quat], + sensor_objid: wp.array[int], + sensor_refid: wp.array[int], + sensor_dim: wp.array[int], + sensor_adr: wp.array[int], + plugin: wp.array[int], + plugin_attr: wp.array[vec_pluginattr], + geom_plugin_index: wp.array[int], + taxel_vertadr: wp.array[int], + taxel_sensorid: wp.array[int], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cvel_in: wp.array2d(dtype=wp.spatial_vector), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], + subtree_com_in: wp.array2d[wp.vec3], + cvel_in: wp.array2d[wp.spatial_vector], # In: - weld_geom_count_in: wp.array2d(dtype=int), - weld_geom_list_in: wp.array3d(dtype=int), + weld_geom_count_in: wp.array2d[int], + weld_geom_list_in: wp.array3d[int], # Data out: - sensordata_out: wp.array2d(dtype=float), + sensordata_out: wp.array2d[float], ): worldid, taxelid = wp.tid() @@ -2223,7 +2223,7 @@ def _sensor_tactile( contact_type, geom_size[worldid % geom_size.shape[0], geom], plugin_id, - geom_dataid[geom], + geom_dataid[worldid % geom_dataid.shape[0], geom], ) depth = wp.min(sdf(contact_type, lpos, plugin_attributes, plugin_index, volume_data, mesh_data), 0.0) @@ -2253,7 +2253,7 @@ def _sensor_tactile( @wp.func -def _check_match(body_parentid: wp.array(dtype=int), body: int, geom: int, objtype: int, objid: int) -> bool: +def _check_match(body_parentid: wp.array[int], body: int, geom: int, objtype: int, objid: int) -> bool: """Check if a contact body/geom matches a sensor spec (objtype, objid).""" if objtype == ObjType.UNKNOWN: return True @@ -2276,36 +2276,36 @@ def _contact_match( # Model: opt_cone: int, opt_contact_sensor_maxmatch: int, - body_parentid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - site_type: wp.array(dtype=int), - site_size: wp.array(dtype=wp.vec3), - sensor_objtype: wp.array(dtype=int), - sensor_objid: wp.array(dtype=int), - sensor_reftype: wp.array(dtype=int), - sensor_refid: wp.array(dtype=int), - sensor_intprm: wp.array2d(dtype=int), - sensor_contact_adr: wp.array(dtype=int), + body_parentid: wp.array[int], + geom_bodyid: wp.array[int], + site_type: wp.array[int], + site_size: wp.array[wp.vec3], + sensor_objtype: wp.array[int], + sensor_objid: wp.array[int], + sensor_reftype: wp.array[int], + sensor_refid: wp.array[int], + sensor_intprm: wp.array2d[int], + sensor_contact_adr: wp.array[int], # Data in: - site_xpos_in: wp.array2d(dtype=wp.vec3), - site_xmat_in: wp.array2d(dtype=wp.mat33), - contact_dist_in: wp.array(dtype=float), - contact_pos_in: wp.array(dtype=wp.vec3), - contact_frame_in: wp.array(dtype=wp.mat33), - contact_friction_in: wp.array(dtype=vec5), - contact_dim_in: wp.array(dtype=int), - contact_geom_in: wp.array(dtype=wp.vec2i), - contact_efc_address_in: wp.array2d(dtype=int), - contact_worldid_in: wp.array(dtype=int), - contact_type_in: wp.array(dtype=int), - efc_force_in: wp.array2d(dtype=float), + site_xpos_in: wp.array2d[wp.vec3], + site_xmat_in: wp.array2d[wp.mat33], + contact_dist_in: wp.array[float], + contact_pos_in: wp.array[wp.vec3], + contact_frame_in: wp.array[wp.mat33], + contact_friction_in: wp.array[vec5], + contact_dim_in: wp.array[int], + contact_geom_in: wp.array[wp.vec2i], + contact_efc_address_in: wp.array2d[int], + contact_worldid_in: wp.array[int], + contact_type_in: wp.array[int], + efc_force_in: wp.array2d[float], njmax_in: int, - nacon_in: wp.array(dtype=int), + nacon_in: wp.array[int], # Out: - sensor_contact_nmatch_out: wp.array2d(dtype=int), - sensor_contact_matchid_out: wp.array3d(dtype=int), - sensor_contact_criteria_out: wp.array3d(dtype=float), - sensor_contact_direction_out: wp.array3d(dtype=float), + sensor_contact_nmatch_out: wp.array2d[int], + sensor_contact_matchid_out: wp.array3d[int], + sensor_contact_criteria_out: wp.array3d[float], + sensor_contact_direction_out: wp.array3d[float], ): contactsensorid, contactid = wp.tid() sensorid = sensor_contact_adr[contactsensorid] @@ -2411,14 +2411,14 @@ def _contact_sort(maxmatch: int): @wp.kernel(module="unique", enable_backward=False) def contact_sort( # Model: - sensor_intprm: wp.array2d(dtype=int), - sensor_contact_adr: wp.array(dtype=int), + sensor_intprm: wp.array2d[int], + sensor_contact_adr: wp.array[int], # In: - sensor_contact_nmatch_in: wp.array2d(dtype=int), - sensor_contact_matchid_in: wp.array3d(dtype=int), - sensor_contact_criteria_in: wp.array3d(dtype=float), + sensor_contact_nmatch_in: wp.array2d[int], + sensor_contact_matchid_in: wp.array3d[int], + sensor_contact_criteria_in: wp.array3d[float], # Out: - sensor_contact_matchid_out: wp.array3d(dtype=int), + sensor_contact_matchid_out: wp.array3d[int], ): worldid, contactsensorid = wp.tid() @@ -2700,7 +2700,7 @@ def sensor_acc(m: Model, d: Data): @wp.kernel def _energy_pos_zero( # Data out: - energy_out: wp.array(dtype=wp.vec2), + energy_out: wp.array[wp.vec2], ): worldid = wp.tid() energy_out[worldid][0] = 0.0 @@ -2709,12 +2709,12 @@ def _energy_pos_zero( @wp.kernel def _energy_pos_gravity( # Model: - opt_gravity: wp.array(dtype=wp.vec3), - body_mass: wp.array2d(dtype=float), + opt_gravity: wp.array[wp.vec3], + body_mass: wp.array2d[float], # Data in: - xipos_in: wp.array2d(dtype=wp.vec3), + xipos_in: wp.array2d[wp.vec3], # Data out: - energy_out: wp.array(dtype=wp.vec2), + energy_out: wp.array[wp.vec2], ): worldid, bodyid = wp.tid() gravity = opt_gravity[worldid % opt_gravity.shape[0]] @@ -2731,14 +2731,14 @@ def _energy_pos_gravity( @wp.kernel def _energy_pos_passive_joint( # Model: - qpos_spring: wp.array2d(dtype=float), - jnt_type: wp.array(dtype=int), - jnt_qposadr: wp.array(dtype=int), - jnt_stiffness: wp.array2d(dtype=float), + qpos_spring: wp.array2d[float], + jnt_type: wp.array[int], + jnt_qposadr: wp.array[int], + jnt_stiffness: wp.array2d[float], # Data in: - qpos_in: wp.array2d(dtype=float), + qpos_in: wp.array2d[float], # Data out: - energy_out: wp.array(dtype=wp.vec2), + energy_out: wp.array[wp.vec2], ): worldid, jntid = wp.tid() jnt_stiffness_id = worldid % jnt_stiffness.shape[0] @@ -2817,12 +2817,12 @@ def _energy_pos_passive_joint( @wp.kernel def _energy_pos_passive_tendon( # Model: - tendon_stiffness: wp.array2d(dtype=float), - tendon_lengthspring: wp.array2d(dtype=wp.vec2), + tendon_stiffness: wp.array2d[float], + tendon_lengthspring: wp.array2d[wp.vec2], # Data in: - ten_length_in: wp.array2d(dtype=float), + ten_length_in: wp.array2d[float], # Data out: - energy_out: wp.array(dtype=wp.vec2), + energy_out: wp.array[wp.vec2], ): worldid, tenid = wp.tid() @@ -2897,11 +2897,11 @@ def _energy_vel_kinetic(nv: int): @wp.kernel(module="unique", enable_backward=False) def energy_vel_kinetic( # Data in: - qvel_in: wp.array2d(dtype=float), + qvel_in: wp.array2d[float], # In: - Mqvel: wp.array2d(dtype=float), + Mqvel: wp.array2d[float], # Data out: - energy_out: wp.array(dtype=wp.vec2), + energy_out: wp.array[wp.vec2], ): worldid = wp.tid() diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/smooth.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/smooth.py index 51dfadb4..bdb90ecd 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/smooth.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/smooth.py @@ -44,28 +44,28 @@ wp.set_module_options({"enable_backward": False}) @wp.kernel def _kinematics_branch( # Model: - qpos0: wp.array2d(dtype=float), - body_parentid: wp.array(dtype=int), - body_mocapid: wp.array(dtype=int), - body_jntnum: wp.array(dtype=int), - body_jntadr: wp.array(dtype=int), - body_pos: wp.array2d(dtype=wp.vec3), - body_quat: wp.array2d(dtype=wp.quat), - jnt_type: wp.array(dtype=int), - jnt_qposadr: wp.array(dtype=int), - jnt_pos: wp.array2d(dtype=wp.vec3), - jnt_axis: wp.array2d(dtype=wp.vec3), - body_branches: wp.array(dtype=int), - body_branch_start: wp.array(dtype=int), + qpos0: wp.array2d[float], + body_parentid: wp.array[int], + body_mocapid: wp.array[int], + body_jntnum: wp.array[int], + body_jntadr: wp.array[int], + body_pos: wp.array2d[wp.vec3], + body_quat: wp.array2d[wp.quat], + jnt_type: wp.array[int], + jnt_qposadr: wp.array[int], + jnt_pos: wp.array2d[wp.vec3], + jnt_axis: wp.array2d[wp.vec3], + body_branches: wp.array[int], + body_branch_start: wp.array[int], # Data in: - qpos_in: wp.array2d(dtype=float), - mocap_pos_in: wp.array2d(dtype=wp.vec3), - mocap_quat_in: wp.array2d(dtype=wp.quat), + qpos_in: wp.array2d[float], + mocap_pos_in: wp.array2d[wp.vec3], + mocap_quat_in: wp.array2d[wp.quat], # Data out: - xpos_out: wp.array2d(dtype=wp.vec3), - xquat_out: wp.array2d(dtype=wp.quat), - xanchor_out: wp.array2d(dtype=wp.vec3), - xaxis_out: wp.array2d(dtype=wp.vec3), + xpos_out: wp.array2d[wp.vec3], + xquat_out: wp.array2d[wp.quat], + xanchor_out: wp.array2d[wp.vec3], + xaxis_out: wp.array2d[wp.vec3], ): worldid, branchid = wp.tid() @@ -146,14 +146,14 @@ def _kinematics_branch( @wp.kernel def _compute_body_inertial_frames( # Model: - body_ipos: wp.array2d(dtype=wp.vec3), - body_iquat: wp.array2d(dtype=wp.quat), + body_ipos: wp.array2d[wp.vec3], + body_iquat: wp.array2d[wp.quat], # Data in: - xpos_in: wp.array2d(dtype=wp.vec3), - xquat_in: wp.array2d(dtype=wp.quat), + xpos_in: wp.array2d[wp.vec3], + xquat_in: wp.array2d[wp.quat], # Data out: - xipos_out: wp.array2d(dtype=wp.vec3), - ximat_out: wp.array2d(dtype=wp.mat33), + xipos_out: wp.array2d[wp.vec3], + ximat_out: wp.array2d[wp.mat33], ): worldid, bodyid = wp.tid() xpos = xpos_in[worldid, bodyid] @@ -165,9 +165,9 @@ def _compute_body_inertial_frames( @wp.kernel def _compute_body_matrices( # Data in: - xquat_in: wp.array2d(dtype=wp.quat), + xquat_in: wp.array2d[wp.quat], # Data out: - xmat_out: wp.array2d(dtype=wp.mat33), + xmat_out: wp.array2d[wp.mat33], ): worldid, bodyid = wp.tid() xmat_out[worldid, bodyid] = math.quat_to_mat(xquat_in[worldid, bodyid]) @@ -176,18 +176,18 @@ def _compute_body_matrices( @wp.kernel def _geom_local_to_global( # Model: - body_rootid: wp.array(dtype=int), - body_weldid: wp.array(dtype=int), - body_mocapid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - geom_pos: wp.array2d(dtype=wp.vec3), - geom_quat: wp.array2d(dtype=wp.quat), + body_rootid: wp.array[int], + body_weldid: wp.array[int], + body_mocapid: wp.array[int], + geom_bodyid: wp.array[int], + geom_pos: wp.array2d[wp.vec3], + geom_quat: wp.array2d[wp.quat], # Data in: - xpos_in: wp.array2d(dtype=wp.vec3), - xquat_in: wp.array2d(dtype=wp.quat), + xpos_in: wp.array2d[wp.vec3], + xquat_in: wp.array2d[wp.quat], # Data out: - geom_xpos_out: wp.array2d(dtype=wp.vec3), - geom_xmat_out: wp.array2d(dtype=wp.mat33), + geom_xpos_out: wp.array2d[wp.vec3], + geom_xmat_out: wp.array2d[wp.mat33], ): worldid, geomid = wp.tid() bodyid = geom_bodyid[geomid] @@ -206,15 +206,15 @@ def _geom_local_to_global( @wp.kernel def _site_local_to_global( # Model: - site_bodyid: wp.array(dtype=int), - site_pos: wp.array2d(dtype=wp.vec3), - site_quat: wp.array2d(dtype=wp.quat), + site_bodyid: wp.array[int], + site_pos: wp.array2d[wp.vec3], + site_quat: wp.array2d[wp.quat], # Data in: - xpos_in: wp.array2d(dtype=wp.vec3), - xquat_in: wp.array2d(dtype=wp.quat), + xpos_in: wp.array2d[wp.vec3], + xquat_in: wp.array2d[wp.quat], # Data out: - site_xpos_out: wp.array2d(dtype=wp.vec3), - site_xmat_out: wp.array2d(dtype=wp.mat33), + site_xpos_out: wp.array2d[wp.vec3], + site_xmat_out: wp.array2d[wp.mat33], ): worldid, siteid = wp.tid() bodyid = site_bodyid[siteid] @@ -228,16 +228,16 @@ def _site_local_to_global( def _flex_vertices( # Model: nflex: int, - flex_vertadr: wp.array(dtype=int), - flex_vertnum: wp.array(dtype=int), - flex_vertbodyid: wp.array(dtype=int), - flex_vert: wp.array(dtype=wp.vec3), - flex_centered: wp.array(dtype=bool), + flex_vertadr: wp.array[int], + flex_vertnum: wp.array[int], + flex_vertbodyid: wp.array[int], + flex_vert: wp.array[wp.vec3], + flex_centered: wp.array[bool], # Data in: - xpos_in: wp.array2d(dtype=wp.vec3), - xmat_in: wp.array2d(dtype=wp.mat33), + xpos_in: wp.array2d[wp.vec3], + xmat_in: wp.array2d[wp.mat33], # Data out: - flexvert_xpos_out: wp.array2d(dtype=wp.vec3), + flexvert_xpos_out: wp.array2d[wp.vec3], ): worldid, vertid = wp.tid() @@ -261,25 +261,25 @@ def _flex_vertices( def _flex_edges( # Model: nflex: int, - body_rootid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - flex_vertadr: wp.array(dtype=int), - flex_edgeadr: wp.array(dtype=int), - flex_edgenum: wp.array(dtype=int), - flex_vertbodyid: wp.array(dtype=int), - flex_edge: wp.array(dtype=wp.vec2i), - flexedge_J_rowadr: wp.array(dtype=int), - flexedge_J_colind: wp.array(dtype=int), + body_rootid: wp.array[int], + body_dofnum: wp.array[int], + body_dofadr: wp.array[int], + flex_vertadr: wp.array[int], + flex_edgeadr: wp.array[int], + flex_edgenum: wp.array[int], + flex_vertbodyid: wp.array[int], + flex_edge: wp.array[wp.vec2i], + flexedge_J_rowadr: wp.array[int], + flexedge_J_colind: wp.array[int], # Data in: - qvel_in: wp.array2d(dtype=float), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), - flexvert_xpos_in: wp.array2d(dtype=wp.vec3), + qvel_in: wp.array2d[float], + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], + flexvert_xpos_in: wp.array2d[wp.vec3], # Data out: - flexedge_J_out: wp.array2d(dtype=float), - flexedge_length_out: wp.array2d(dtype=float), - flexedge_velocity_out: wp.array2d(dtype=float), + flexedge_J_out: wp.array2d[float], + flexedge_length_out: wp.array2d[float], + flexedge_velocity_out: wp.array2d[float], ): worldid, edgeid = wp.tid() for i in range(nflex): @@ -463,11 +463,11 @@ def flex(m: Model, d: Data): @wp.kernel def _subtree_com_init( # Model: - body_mass: wp.array2d(dtype=float), + body_mass: wp.array2d[float], # Data in: - xipos_in: wp.array2d(dtype=wp.vec3), + xipos_in: wp.array2d[wp.vec3], # Data out: - subtree_com_out: wp.array2d(dtype=wp.vec3), + subtree_com_out: wp.array2d[wp.vec3], ): worldid, bodyid = wp.tid() subtree_com_out[worldid, bodyid] = xipos_in[worldid, bodyid] * body_mass[worldid % body_mass.shape[0], bodyid] @@ -476,13 +476,13 @@ def _subtree_com_init( @wp.kernel def _subtree_com_acc( # Model: - body_parentid: wp.array(dtype=int), + body_parentid: wp.array[int], # Data in: - subtree_com_in: wp.array2d(dtype=wp.vec3), + subtree_com_in: wp.array2d[wp.vec3], # In: - body_tree_: wp.array(dtype=int), + body_tree_: wp.array[int], # Data out: - subtree_com_out: wp.array2d(dtype=wp.vec3), + subtree_com_out: wp.array2d[wp.vec3], ): worldid, nodeid = wp.tid() bodyid = body_tree_[nodeid] @@ -494,11 +494,11 @@ def _subtree_com_acc( @wp.kernel def _subtree_div( # Model: - body_subtreemass: wp.array2d(dtype=float), + body_subtreemass: wp.array2d[float], # Data in: - subtree_com_in: wp.array2d(dtype=wp.vec3), + subtree_com_in: wp.array2d[wp.vec3], # Data out: - subtree_com_out: wp.array2d(dtype=wp.vec3), + subtree_com_out: wp.array2d[wp.vec3], ): worldid, bodyid = wp.tid() com = subtree_com_in[worldid, bodyid] @@ -510,15 +510,15 @@ def _subtree_div( @wp.kernel def _cinert( # Model: - body_rootid: wp.array(dtype=int), - body_mass: wp.array2d(dtype=float), - body_inertia: wp.array2d(dtype=wp.vec3), + body_rootid: wp.array[int], + body_mass: wp.array2d[float], + body_inertia: wp.array2d[wp.vec3], # Data in: - xipos_in: wp.array2d(dtype=wp.vec3), - ximat_in: wp.array2d(dtype=wp.mat33), - subtree_com_in: wp.array2d(dtype=wp.vec3), + xipos_in: wp.array2d[wp.vec3], + ximat_in: wp.array2d[wp.mat33], + subtree_com_in: wp.array2d[wp.vec3], # Data out: - cinert_out: wp.array2d(dtype=vec10), + cinert_out: wp.array2d[vec10], ): worldid, bodyid = wp.tid() mat = ximat_in[worldid, bodyid] @@ -556,17 +556,17 @@ def _cinert( @wp.kernel def _cdof( # Model: - body_rootid: wp.array(dtype=int), - jnt_type: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - jnt_bodyid: wp.array(dtype=int), + body_rootid: wp.array[int], + jnt_type: wp.array[int], + jnt_dofadr: wp.array[int], + jnt_bodyid: wp.array[int], # Data in: - xmat_in: wp.array2d(dtype=wp.mat33), - xanchor_in: wp.array2d(dtype=wp.vec3), - xaxis_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), + xmat_in: wp.array2d[wp.mat33], + xanchor_in: wp.array2d[wp.vec3], + xaxis_in: wp.array2d[wp.vec3], + subtree_com_in: wp.array2d[wp.vec3], # Data out: - cdof_out: wp.array2d(dtype=wp.spatial_vector), + cdof_out: wp.array2d[wp.spatial_vector], ): worldid, jntid = wp.tid() bodyid = jnt_bodyid[jntid] @@ -635,21 +635,21 @@ def com_pos(m: Model, d: Data): @wp.kernel def _cam_local_to_global( # Model: - cam_mode: wp.array(dtype=int), - cam_bodyid: wp.array(dtype=int), - cam_targetbodyid: wp.array(dtype=int), - cam_pos: wp.array2d(dtype=wp.vec3), - cam_quat: wp.array2d(dtype=wp.quat), - cam_poscom0: wp.array2d(dtype=wp.vec3), - cam_pos0: wp.array2d(dtype=wp.vec3), - cam_mat0: wp.array2d(dtype=wp.mat33), + cam_mode: wp.array[int], + cam_bodyid: wp.array[int], + cam_targetbodyid: wp.array[int], + cam_pos: wp.array2d[wp.vec3], + cam_quat: wp.array2d[wp.quat], + cam_poscom0: wp.array2d[wp.vec3], + cam_pos0: wp.array2d[wp.vec3], + cam_mat0: wp.array2d[wp.mat33], # Data in: - xpos_in: wp.array2d(dtype=wp.vec3), - xquat_in: wp.array2d(dtype=wp.quat), - subtree_com_in: wp.array2d(dtype=wp.vec3), + xpos_in: wp.array2d[wp.vec3], + xquat_in: wp.array2d[wp.quat], + subtree_com_in: wp.array2d[wp.vec3], # Data out: - cam_xpos_out: wp.array2d(dtype=wp.vec3), - cam_xmat_out: wp.array2d(dtype=wp.mat33), + cam_xpos_out: wp.array2d[wp.vec3], + cam_xmat_out: wp.array2d[wp.mat33], ): worldid, camid = wp.tid() cam_pos_id = worldid % cam_pos.shape[0] @@ -702,21 +702,21 @@ def _cam_local_to_global( @wp.kernel def _light_local_to_global( # Model: - light_mode: wp.array(dtype=int), - light_bodyid: wp.array(dtype=int), - light_targetbodyid: wp.array(dtype=int), - light_pos: wp.array2d(dtype=wp.vec3), - light_dir: wp.array2d(dtype=wp.vec3), - light_poscom0: wp.array2d(dtype=wp.vec3), - light_pos0: wp.array2d(dtype=wp.vec3), - light_dir0: wp.array2d(dtype=wp.vec3), + light_mode: wp.array[int], + light_bodyid: wp.array[int], + light_targetbodyid: wp.array[int], + light_pos: wp.array2d[wp.vec3], + light_dir: wp.array2d[wp.vec3], + light_poscom0: wp.array2d[wp.vec3], + light_pos0: wp.array2d[wp.vec3], + light_dir0: wp.array2d[wp.vec3], # Data in: - xpos_in: wp.array2d(dtype=wp.vec3), - xquat_in: wp.array2d(dtype=wp.quat), - subtree_com_in: wp.array2d(dtype=wp.vec3), + xpos_in: wp.array2d[wp.vec3], + xquat_in: wp.array2d[wp.quat], + subtree_com_in: wp.array2d[wp.vec3], # Data out: - light_xpos_out: wp.array2d(dtype=wp.vec3), - light_xdir_out: wp.array2d(dtype=wp.vec3), + light_xpos_out: wp.array2d[wp.vec3], + light_xdir_out: wp.array2d[wp.vec3], ): worldid, lightid = wp.tid() light_pos_id = worldid % light_pos.shape[0] @@ -806,13 +806,13 @@ def camlight(m: Model, d: Data): @wp.kernel def _crb_accumulate( # Model: - body_parentid: wp.array(dtype=int), + body_parentid: wp.array[int], # Data in: - crb_in: wp.array2d(dtype=vec10), + crb_in: wp.array2d[vec10], # In: - body_tree_: wp.array(dtype=int), + body_tree_: wp.array[int], # Data out: - crb_out: wp.array2d(dtype=vec10), + crb_out: wp.array2d[vec10], ): worldid, nodeid = wp.tid() bodyid = body_tree_[nodeid] @@ -825,15 +825,15 @@ def _crb_accumulate( @wp.kernel def _qM_sparse( # Model: - dof_bodyid: wp.array(dtype=int), - dof_parentid: wp.array(dtype=int), - dof_Madr: wp.array(dtype=int), - dof_armature: wp.array2d(dtype=float), + dof_bodyid: wp.array[int], + dof_parentid: wp.array[int], + dof_Madr: wp.array[int], + dof_armature: wp.array2d[float], # Data in: - cdof_in: wp.array2d(dtype=wp.spatial_vector), - crb_in: wp.array2d(dtype=vec10), + cdof_in: wp.array2d[wp.spatial_vector], + crb_in: wp.array2d[vec10], # Data out: - qM_out: wp.array3d(dtype=float), + qM_out: wp.array3d[float], ): worldid, dofid = wp.tid() madr_ij = dof_Madr[dofid] # dof_Madr is not batched @@ -855,14 +855,14 @@ def _qM_sparse( @wp.kernel def _qM_dense( # Model: - dof_bodyid: wp.array(dtype=int), - dof_parentid: wp.array(dtype=int), - dof_armature: wp.array2d(dtype=float), + dof_bodyid: wp.array[int], + dof_parentid: wp.array[int], + dof_armature: wp.array2d[float], # Data in: - cdof_in: wp.array2d(dtype=wp.spatial_vector), - crb_in: wp.array2d(dtype=vec10), + cdof_in: wp.array2d[wp.spatial_vector], + crb_in: wp.array2d[vec10], # Data out: - qM_out: wp.array3d(dtype=float), + qM_out: wp.array3d[float], ): worldid, dofid = wp.tid() bodyid = dof_bodyid[dofid] @@ -915,17 +915,17 @@ def crb(m: Model, d: Data): @wp.kernel def _tendon_armature( # Model: - dof_parentid: wp.array(dtype=int), - dof_Madr: wp.array(dtype=int), - ten_J_rownnz: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - tendon_armature: wp.array2d(dtype=float), + dof_parentid: wp.array[int], + dof_Madr: wp.array[int], + ten_J_rownnz: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_colind: wp.array[int], + tendon_armature: wp.array2d[float], is_sparse: bool, # Data in: - ten_J_in: wp.array2d(dtype=float), + ten_J_in: wp.array2d[float], # Data out: - qM_out: wp.array3d(dtype=float), + qM_out: wp.array3d[float], ): worldid, tenid, dofid = wp.tid() @@ -1003,11 +1003,11 @@ def tendon_armature(m: Model, d: Data): @wp.kernel def _copy_CSR( # Model: - mapM2M: wp.array(dtype=int), + mapM2M: wp.array[int], # In: - M_in: wp.array3d(dtype=float), + M_in: wp.array3d[float], # Out: - L_out: wp.array3d(dtype=float), + L_out: wp.array3d[float], ): worldid, ind = wp.tid() L_out[worldid, 0, ind] = M_in[worldid, 0, mapM2M[ind]] @@ -1016,13 +1016,13 @@ def _copy_CSR( @wp.kernel def _qLD_acc( # Model: - M_rownnz: wp.array(dtype=int), - M_rowadr: wp.array(dtype=int), + M_rownnz: wp.array[int], + M_rowadr: wp.array[int], # In: - qLD_updates_: wp.array(dtype=wp.vec3i), - L_in: wp.array3d(dtype=float), + qLD_updates_: wp.array[wp.vec3i], + L_in: wp.array3d[float], # Out: - L_out: wp.array3d(dtype=float), + L_out: wp.array3d[float], ): worldid, nodeid = wp.tid() update = qLD_updates_[nodeid] @@ -1041,19 +1041,19 @@ def _qLD_acc( @wp.kernel def _qLDiag_div( # Model: - M_rownnz: wp.array(dtype=int), - M_rowadr: wp.array(dtype=int), + M_rownnz: wp.array[int], + M_rowadr: wp.array[int], # In: - L_in: wp.array3d(dtype=float), + L_in: wp.array3d[float], # Out: - D_out: wp.array2d(dtype=float), + D_out: wp.array2d[float], ): worldid, dofid = wp.tid() diag_i = M_rowadr[dofid] + M_rownnz[dofid] - 1 # Address of diagonal element of i D_out[worldid, dofid] = 1.0 / L_in[worldid, 0, diag_i] -def _factor_i_sparse(m: Model, d: Data, M: wp.array3d(dtype=float), L: wp.array3d(dtype=float), D: wp.array2d(dtype=float)): +def _factor_i_sparse(m: Model, d: Data, M: wp.array3d[float], L: wp.array3d[float], D: wp.array2d[float]): """Sparse L'*D*L factorization of inertia-like matrix M, assumed spd.""" wp.launch(_copy_CSR, dim=(d.nworld, m.nC), inputs=[m.mapM2M, M], outputs=[L]) @@ -1071,11 +1071,11 @@ def _tile_cholesky_factorize(tile: TileSet): @wp.kernel(module="unique", enable_backward=False) def cholesky_factorize( # Data in: - qM_in: wp.array3d(dtype=float), + qM_in: wp.array3d[float], # In: - adr: wp.array(dtype=int), + adr: wp.array[int], # Out: - L_out: wp.array3d(dtype=float), + L_out: wp.array3d[float], ): worldid, nodeid = wp.tid() TILE_SIZE = wp.static(tile.size) @@ -1112,9 +1112,9 @@ def factor_m(m: Model, d: Data): @wp.kernel def _cacc_world( # In: - gravity: wp.array(dtype=wp.vec3), + gravity: wp.array[wp.vec3], # Data out: - cacc_out: wp.array2d(dtype=wp.spatial_vector), + cacc_out: wp.array2d[wp.spatial_vector], ): worldid = wp.tid() cacc_out[worldid, 0] = wp.spatial_vector(wp.vec3(0.0), -gravity[worldid % gravity.shape[0]]) @@ -1130,20 +1130,20 @@ def _rne_cacc_world(m: Model, d: Data): @wp.kernel def _cacc_branch( # Model: - body_parentid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - body_branches: wp.array(dtype=int), - body_branch_start: wp.array(dtype=int), + body_parentid: wp.array[int], + body_dofnum: wp.array[int], + body_dofadr: wp.array[int], + body_branches: wp.array[int], + body_branch_start: wp.array[int], # Data in: - qvel_in: wp.array2d(dtype=float), - qacc_in: wp.array2d(dtype=float), - cdof_in: wp.array2d(dtype=wp.spatial_vector), - cdof_dot_in: wp.array2d(dtype=wp.spatial_vector), + qvel_in: wp.array2d[float], + qacc_in: wp.array2d[float], + cdof_in: wp.array2d[wp.spatial_vector], + cdof_dot_in: wp.array2d[wp.spatial_vector], # In: flg_acc: bool, # Data out: - cacc_out: wp.array2d(dtype=wp.spatial_vector), + cacc_out: wp.array2d[wp.spatial_vector], ): worldid, branchid = wp.tid() @@ -1187,14 +1187,14 @@ def _rne_cacc_forward(m: Model, d: Data, flg_acc: bool = False): @wp.kernel def _cfrc( # Data in: - cinert_in: wp.array2d(dtype=vec10), - cvel_in: wp.array2d(dtype=wp.spatial_vector), - cacc_in: wp.array2d(dtype=wp.spatial_vector), - cfrc_ext_in: wp.array2d(dtype=wp.spatial_vector), + cinert_in: wp.array2d[vec10], + cvel_in: wp.array2d[wp.spatial_vector], + cacc_in: wp.array2d[wp.spatial_vector], + cfrc_ext_in: wp.array2d[wp.spatial_vector], # In: flg_cfrc_ext: bool, # Data out: - cfrc_int_out: wp.array2d(dtype=wp.spatial_vector), + cfrc_int_out: wp.array2d[wp.spatial_vector], ): worldid, bodyid = wp.tid() bodyid += 1 # skip world body @@ -1218,13 +1218,13 @@ def _rne_cfrc(m: Model, d: Data, flg_cfrc_ext: bool = False): @wp.kernel def _cfrc_backward( # Model: - body_parentid: wp.array(dtype=int), + body_parentid: wp.array[int], # Data in: - cfrc_int_in: wp.array2d(dtype=wp.spatial_vector), + cfrc_int_in: wp.array2d[wp.spatial_vector], # In: - body_tree_: wp.array(dtype=int), + body_tree_: wp.array[int], # Data out: - cfrc_int_out: wp.array2d(dtype=wp.spatial_vector), + cfrc_int_out: wp.array2d[wp.spatial_vector], ): worldid, nodeid = wp.tid() bodyid = body_tree_[nodeid] @@ -1243,12 +1243,12 @@ def _rne_cfrc_backward(m: Model, d: Data): @wp.kernel def _qfrc_bias( # Model: - dof_bodyid: wp.array(dtype=int), + dof_bodyid: wp.array[int], # Data in: - cdof_in: wp.array2d(dtype=wp.spatial_vector), - cfrc_int_in: wp.array2d(dtype=wp.spatial_vector), + cdof_in: wp.array2d[wp.spatial_vector], + cfrc_int_in: wp.array2d[wp.spatial_vector], # Data out: - qfrc_bias_out: wp.array2d(dtype=float), + qfrc_bias_out: wp.array2d[float], ): worldid, dofid = wp.tid() bodyid = dof_bodyid[dofid] @@ -1277,13 +1277,13 @@ def rne(m: Model, d: Data, flg_acc: bool = False): @wp.kernel def _cfrc_ext( # Model: - body_rootid: wp.array(dtype=int), + body_rootid: wp.array[int], # Data in: - xfrc_applied_in: wp.array2d(dtype=wp.spatial_vector), - xipos_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), + xfrc_applied_in: wp.array2d[wp.spatial_vector], + xipos_in: wp.array2d[wp.vec3], + subtree_com_in: wp.array2d[wp.vec3], # Data out: - cfrc_ext_out: wp.array2d(dtype=wp.spatial_vector), + cfrc_ext_out: wp.array2d[wp.spatial_vector], ): worldid, bodyid = wp.tid() if bodyid == 0: @@ -1298,14 +1298,14 @@ def _cfrc_ext( @wp.kernel def _count_equality_constraints( # Model: - eq_type: wp.array(dtype=int), + eq_type: wp.array[int], # Data in: - ne_in: wp.array(dtype=int), - efc_type_in: wp.array2d(dtype=int), - efc_id_in: wp.array2d(dtype=int), + ne_in: wp.array[int], + efc_type_in: wp.array2d[int], + efc_id_in: wp.array2d[int], # Out: - ne_connect_out: wp.array(dtype=int), - ne_weld_out: wp.array(dtype=int), + ne_connect_out: wp.array[int], + ne_weld_out: wp.array[int], ): """Counts connect and weld equality constraints from efc data.""" worldid, efcid = wp.tid() @@ -1328,24 +1328,24 @@ def _count_equality_constraints( @wp.kernel def _cfrc_ext_equality( # Model: - body_rootid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - site_pos: wp.array2d(dtype=wp.vec3), - eq_obj1id: wp.array(dtype=int), - eq_obj2id: wp.array(dtype=int), - eq_objtype: wp.array(dtype=int), - eq_data: wp.array2d(dtype=vec11), + body_rootid: wp.array[int], + site_bodyid: wp.array[int], + site_pos: wp.array2d[wp.vec3], + eq_obj1id: wp.array[int], + eq_obj2id: wp.array[int], + eq_objtype: wp.array[int], + eq_data: wp.array2d[vec11], # Data in: - xpos_in: wp.array2d(dtype=wp.vec3), - xmat_in: wp.array2d(dtype=wp.mat33), - subtree_com_in: wp.array2d(dtype=wp.vec3), - efc_id_in: wp.array2d(dtype=int), - efc_force_in: wp.array2d(dtype=float), + xpos_in: wp.array2d[wp.vec3], + xmat_in: wp.array2d[wp.mat33], + subtree_com_in: wp.array2d[wp.vec3], + efc_id_in: wp.array2d[int], + efc_force_in: wp.array2d[float], # In: - ne_connect_in: wp.array(dtype=int), - ne_weld_in: wp.array(dtype=int), + ne_connect_in: wp.array[int], + ne_weld_in: wp.array[int], # Data out: - cfrc_ext_out: wp.array2d(dtype=wp.spatial_vector), + cfrc_ext_out: wp.array2d[wp.spatial_vector], ): worldid, eqid = wp.tid() @@ -1439,22 +1439,22 @@ def transform_force(force: wp.vec3, torque: wp.vec3, offset: wp.vec3) -> wp.spat def _cfrc_ext_contact( # Model: opt_cone: int, - body_rootid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), + body_rootid: wp.array[int], + geom_bodyid: wp.array[int], # Data in: - subtree_com_in: wp.array2d(dtype=wp.vec3), - contact_pos_in: wp.array(dtype=wp.vec3), - contact_frame_in: wp.array(dtype=wp.mat33), - contact_friction_in: wp.array(dtype=vec5), - contact_dim_in: wp.array(dtype=int), - contact_geom_in: wp.array(dtype=wp.vec2i), - contact_efc_address_in: wp.array2d(dtype=int), - contact_worldid_in: wp.array(dtype=int), - efc_force_in: wp.array2d(dtype=float), + subtree_com_in: wp.array2d[wp.vec3], + contact_pos_in: wp.array[wp.vec3], + contact_frame_in: wp.array[wp.mat33], + contact_friction_in: wp.array[vec5], + contact_dim_in: wp.array[int], + contact_geom_in: wp.array[wp.vec2i], + contact_efc_address_in: wp.array2d[int], + contact_worldid_in: wp.array[int], + efc_force_in: wp.array2d[float], njmax_in: int, - nacon_in: wp.array(dtype=int), + nacon_in: wp.array[int], # Data out: - cfrc_ext_out: wp.array2d(dtype=wp.spatial_vector), + cfrc_ext_out: wp.array2d[wp.spatial_vector], ): contactid = wp.tid() @@ -1584,17 +1584,17 @@ def rne_postconstraint(m: Model, d: Data): @wp.func def _accumulate_jac_dot_chain( # Model: - body_parentid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - jnt_type: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - dof_jntid: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), + body_parentid: wp.array[int], + body_dofnum: wp.array[int], + body_dofadr: wp.array[int], + jnt_type: wp.array[int], + jnt_dofadr: wp.array[int], + dof_jntid: wp.array[int], + ten_J_colind: wp.array[int], # Data in: - cdof_in: wp.array2d(dtype=wp.spatial_vector), - cvel_in: wp.array2d(dtype=wp.spatial_vector), - cdof_dot_in: wp.array2d(dtype=wp.spatial_vector), + cdof_in: wp.array2d[wp.spatial_vector], + cvel_in: wp.array2d[wp.spatial_vector], + cdof_dot_in: wp.array2d[wp.spatial_vector], # In: offset: wp.vec3, pvel_lin: wp.vec3, @@ -1606,7 +1606,7 @@ def _accumulate_jac_dot_chain( scale: float, worldid: int, # Out: - ten_Jdot_out: wp.array2d(dtype=float), + ten_Jdot_out: wp.array2d[float], ): """Walk body chain from bodyid to root, accumulate Jdot contributions.""" ptr = rownnz - 1 @@ -1655,31 +1655,31 @@ def _accumulate_jac_dot_chain( @wp.kernel def _tendon_dot( # Model: - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - jnt_type: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - dof_jntid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - tendon_adr: wp.array(dtype=int), - tendon_num: wp.array(dtype=int), - ten_J_rownnz: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - tendon_armature: wp.array2d(dtype=float), - wrap_type: wp.array(dtype=int), - wrap_objid: wp.array(dtype=int), - wrap_prm: wp.array(dtype=float), + body_parentid: wp.array[int], + body_rootid: wp.array[int], + body_dofnum: wp.array[int], + body_dofadr: wp.array[int], + jnt_type: wp.array[int], + jnt_dofadr: wp.array[int], + dof_jntid: wp.array[int], + site_bodyid: wp.array[int], + tendon_adr: wp.array[int], + tendon_num: wp.array[int], + ten_J_rownnz: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_colind: wp.array[int], + tendon_armature: wp.array2d[float], + wrap_type: wp.array[int], + wrap_objid: wp.array[int], + wrap_prm: wp.array[float], # Data in: - site_xpos_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), - cvel_in: wp.array2d(dtype=wp.spatial_vector), - cdof_dot_in: wp.array2d(dtype=wp.spatial_vector), + site_xpos_in: wp.array2d[wp.vec3], + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], + cvel_in: wp.array2d[wp.spatial_vector], + cdof_dot_in: wp.array2d[wp.spatial_vector], # Out: - ten_Jdot_out: wp.array2d(dtype=float), + ten_Jdot_out: wp.array2d[float], ): worldid, tenid = wp.tid() @@ -1809,16 +1809,16 @@ def _tendon_dot( @wp.kernel def _tendon_bias_coef( # Model: - ten_J_rownnz: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - tendon_armature: wp.array2d(dtype=float), + ten_J_rownnz: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_colind: wp.array[int], + tendon_armature: wp.array2d[float], # Data in: - qvel_in: wp.array2d(dtype=float), + qvel_in: wp.array2d[float], # In: - ten_Jdot_in: wp.array2d(dtype=float), + ten_Jdot_in: wp.array2d[float], # Out: - ten_bias_coef_out: wp.array2d(dtype=float), + ten_bias_coef_out: wp.array2d[float], ): worldid, tenid, dofid_sparse = wp.tid() @@ -1842,16 +1842,16 @@ def _tendon_bias_coef( @wp.kernel def _tendon_bias_qfrc( # Model: - ten_J_rownnz: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - tendon_armature: wp.array2d(dtype=float), + ten_J_rownnz: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_colind: wp.array[int], + tendon_armature: wp.array2d[float], # Data in: - ten_J_in: wp.array2d(dtype=float), + ten_J_in: wp.array2d[float], # In: - ten_bias_coef_in: wp.array2d(dtype=float), + ten_bias_coef_in: wp.array2d[float], # Out: - qfrc_out: wp.array2d(dtype=float), + qfrc_out: wp.array2d[float], ): worldid, tenid, dofid = wp.tid() @@ -1875,7 +1875,7 @@ def _tendon_bias_qfrc( @event_scope -def tendon_bias(m: Model, d: Data, qfrc: wp.array2d(dtype=float)): +def tendon_bias(m: Model, d: Data, qfrc: wp.array2d[float]): """Add bias force due to tendon armature. Args: @@ -1933,7 +1933,7 @@ def tendon_bias(m: Model, d: Data, qfrc: wp.array2d(dtype=float)): @wp.kernel -def _comvel_root(cvel_out: wp.array2d(dtype=wp.spatial_vector)): +def _comvel_root(cvel_out: wp.array2d[wp.spatial_vector]): worldid, elementid = wp.tid() cvel_out[worldid, 0][elementid] = 0.0 @@ -1941,19 +1941,19 @@ def _comvel_root(cvel_out: wp.array2d(dtype=wp.spatial_vector)): @wp.kernel def _comvel_branch( # Model: - body_parentid: wp.array(dtype=int), - body_jntnum: wp.array(dtype=int), - body_jntadr: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - jnt_type: wp.array(dtype=int), - body_branches: wp.array(dtype=int), - body_branch_start: wp.array(dtype=int), + body_parentid: wp.array[int], + body_jntnum: wp.array[int], + body_jntadr: wp.array[int], + body_dofadr: wp.array[int], + jnt_type: wp.array[int], + body_branches: wp.array[int], + body_branch_start: wp.array[int], # Data in: - qvel_in: wp.array2d(dtype=float), - cdof_in: wp.array2d(dtype=wp.spatial_vector), + qvel_in: wp.array2d[float], + cdof_in: wp.array2d[wp.spatial_vector], # Data out: - cvel_out: wp.array2d(dtype=wp.spatial_vector), - cdof_dot_out: wp.array2d(dtype=wp.spatial_vector), + cvel_out: wp.array2d[wp.spatial_vector], + cdof_dot_out: wp.array2d[wp.spatial_vector], ): worldid, branchid = wp.tid() @@ -2042,42 +2042,42 @@ def com_vel(m: Model, d: Data): def _transmission( # Model: nv: int, - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - body_weldid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - jnt_type: wp.array(dtype=int), - jnt_qposadr: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - dof_bodyid: wp.array(dtype=int), - dof_parentid: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - site_quat: wp.array2d(dtype=wp.quat), - ten_J_rownnz: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - actuator_trntype: wp.array(dtype=int), - actuator_trnid: wp.array(dtype=wp.vec2i), - actuator_gear: wp.array2d(dtype=wp.spatial_vector), - actuator_cranklength: wp.array2d(dtype=float), + body_parentid: wp.array[int], + body_rootid: wp.array[int], + body_weldid: wp.array[int], + body_dofnum: wp.array[int], + body_dofadr: wp.array[int], + jnt_type: wp.array[int], + jnt_qposadr: wp.array[int], + jnt_dofadr: wp.array[int], + dof_bodyid: wp.array[int], + dof_parentid: wp.array[int], + site_bodyid: wp.array[int], + site_quat: wp.array2d[wp.quat], + ten_J_rownnz: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_colind: wp.array[int], + actuator_trntype: wp.array[int], + actuator_trnid: wp.array[wp.vec2i], + actuator_gear: wp.array2d[wp.spatial_vector], + actuator_cranklength: wp.array2d[float], # Data in: - qpos_in: wp.array2d(dtype=float), - xquat_in: wp.array2d(dtype=wp.quat), - site_xpos_in: wp.array2d(dtype=wp.vec3), - site_xmat_in: wp.array2d(dtype=wp.mat33), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), - ten_J_in: wp.array2d(dtype=float), - ten_length_in: wp.array2d(dtype=float), + qpos_in: wp.array2d[float], + xquat_in: wp.array2d[wp.quat], + site_xpos_in: wp.array2d[wp.vec3], + site_xmat_in: wp.array2d[wp.mat33], + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], + ten_J_in: wp.array2d[float], + ten_length_in: wp.array2d[float], # In: - moment_nnz: wp.array(dtype=int), + moment_nnz: wp.array[int], # Data out: - actuator_length_out: wp.array2d(dtype=float), - moment_rownnz_out: wp.array2d(dtype=int), - moment_rowadr_out: wp.array2d(dtype=int), - moment_colind_out: wp.array2d(dtype=int), - actuator_moment_out: wp.array2d(dtype=float), + actuator_length_out: wp.array2d[float], + moment_rownnz_out: wp.array2d[int], + moment_rowadr_out: wp.array2d[int], + moment_colind_out: wp.array2d[int], + actuator_moment_out: wp.array2d[float], ): worldid, actid = wp.tid() trntype = actuator_trntype[actid] @@ -2448,35 +2448,35 @@ def _transmission( def _transmission_body_moment( # Model: opt_cone: int, - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - dof_bodyid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - actuator_trnid: wp.array(dtype=wp.vec2i), - actuator_trntype_body_adr: wp.array(dtype=int), + body_parentid: wp.array[int], + body_rootid: wp.array[int], + dof_bodyid: wp.array[int], + geom_bodyid: wp.array[int], + actuator_trnid: wp.array[wp.vec2i], + actuator_trntype_body_adr: wp.array[int], # Data in: - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), - moment_rowadr_in: wp.array2d(dtype=int), - contact_dist_in: wp.array(dtype=float), - contact_pos_in: wp.array(dtype=wp.vec3), - contact_frame_in: wp.array(dtype=wp.mat33), - contact_includemargin_in: wp.array(dtype=float), - contact_dim_in: wp.array(dtype=int), - contact_geom_in: wp.array(dtype=wp.vec2i), - contact_efc_address_in: wp.array2d(dtype=int), - contact_worldid_in: wp.array(dtype=int), - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), - efc_J_colind_in: wp.array3d(dtype=int), - efc_J_in: wp.array3d(dtype=float), - nacon_in: wp.array(dtype=int), + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], + moment_rowadr_in: wp.array2d[int], + contact_dist_in: wp.array[float], + contact_pos_in: wp.array[wp.vec3], + contact_frame_in: wp.array[wp.mat33], + contact_includemargin_in: wp.array[float], + contact_dim_in: wp.array[int], + contact_geom_in: wp.array[wp.vec2i], + contact_efc_address_in: wp.array2d[int], + contact_worldid_in: wp.array[int], + efc_J_rownnz_in: wp.array2d[int], + efc_J_rowadr_in: wp.array2d[int], + efc_J_colind_in: wp.array3d[int], + efc_J_in: wp.array3d[float], + nacon_in: wp.array[int], # In: efc_is_sparse: bool, # Data out: - actuator_moment_out: wp.array2d(dtype=float), + actuator_moment_out: wp.array2d[float], # Out: - actuator_trntype_body_ncon_out: wp.array2d(dtype=int), + actuator_trntype_body_ncon_out: wp.array2d[int], ): trnbodyid, conid, dofid = wp.tid() actid = actuator_trntype_body_adr[trnbodyid] @@ -2583,13 +2583,13 @@ def _transmission_body_moment( @wp.kernel def _transmission_body_moment_scale( # Model: - actuator_trntype_body_adr: wp.array(dtype=int), + actuator_trntype_body_adr: wp.array[int], # Data in: - moment_rowadr_in: wp.array2d(dtype=int), + moment_rowadr_in: wp.array2d[int], # In: - actuator_trntype_body_ncon_in: wp.array2d(dtype=int), + actuator_trntype_body_ncon_in: wp.array2d[int], # Data out: - actuator_moment_out: wp.array2d(dtype=float), + actuator_moment_out: wp.array2d[float], ): worldid, trnbodyid, dofid = wp.tid() @@ -2704,13 +2704,13 @@ def _solve_LD_sparse_fused(nv: int, nlevels: int): @wp.kernel(module="unique", enable_backward=False) def kernel( # In: - L: wp.array3d(dtype=float), - D: wp.array2d(dtype=float), - all_updates: wp.array(dtype=wp.vec3i), - level_offsets: wp.array(dtype=int), - y: wp.array2d(dtype=float), + L: wp.array3d[float], + D: wp.array2d[float], + all_updates: wp.array[wp.vec3i], + level_offsets: wp.array[int], + y: wp.array2d[float], # Out: - x_out: wp.array2d(dtype=float), + x_out: wp.array2d[float], ): worldid, tid = wp.tid() NV = wp.static(nv) @@ -2757,10 +2757,10 @@ def _solve_LD_sparse_fused(nv: int, nlevels: int): def _solve_LD_sparse( m: Model, d: Data, - L: wp.array3d(dtype=float), - D: wp.array2d(dtype=float), - x: wp.array2d(dtype=float), - y: wp.array2d(dtype=float), + L: wp.array3d[float], + D: wp.array2d[float], + x: wp.array2d[float], + y: wp.array2d[float], ): """Computes sparse backsubstitution: x = inv(L'*D*L)*y.""" nlevels = len(m.qLD_updates) @@ -2786,11 +2786,11 @@ def _tile_cholesky_solve(tile: TileSet): @wp.kernel(module="unique", enable_backward=False) def cholesky_solve( # In: - L: wp.array3d(dtype=float), - y: wp.array2d(dtype=float), - adr: wp.array(dtype=int), + L: wp.array3d[float], + y: wp.array2d[float], + adr: wp.array[int], # Out: - x: wp.array2d(dtype=float), + x: wp.array2d[float], ): worldid, nodeid = wp.tid() TILE_SIZE = wp.static(tile.size) @@ -2804,7 +2804,7 @@ def _tile_cholesky_solve(tile: TileSet): return cholesky_solve -def _solve_LD_dense(m: Model, d: Data, L: wp.array3d(dtype=float), x: wp.array2d(dtype=float), y: wp.array2d(dtype=float)): +def _solve_LD_dense(m: Model, d: Data, L: wp.array3d[float], x: wp.array2d[float], y: wp.array2d[float]): """Computes dense backsubstitution: x = inv(L'*L)*y.""" for tile in m.qM_tiles: wp.launch_tiled( @@ -2819,10 +2819,10 @@ def _solve_LD_dense(m: Model, d: Data, L: wp.array3d(dtype=float), x: wp.array2d def solve_LD( m: Model, d: Data, - L: wp.array3d(dtype=float), - D: wp.array2d(dtype=float), - x: wp.array2d(dtype=float), - y: wp.array2d(dtype=float), + L: wp.array3d[float], + D: wp.array2d[float], + x: wp.array2d[float], + y: wp.array2d[float], ): """Computes backsubstitution to solve a linear system of the form x = inv(L'*D*L) * y. @@ -2845,7 +2845,7 @@ def solve_LD( @event_scope -def solve_m(m: Model, d: Data, x: wp.array2d(dtype=float), y: wp.array2d(dtype=float)): +def solve_m(m: Model, d: Data, x: wp.array2d[float], y: wp.array2d[float]): """Computes backsubstitution: x = qLD * y. Args: @@ -2864,12 +2864,12 @@ def _tile_cholesky_factorize_solve(tile: TileSet): @wp.kernel(module="unique", enable_backward=False) def cholesky_factorize_solve( # In: - M: wp.array3d(dtype=float), - y: wp.array2d(dtype=float), - adr: wp.array(dtype=int), + M: wp.array3d[float], + y: wp.array2d[float], + adr: wp.array[int], # Out: - x: wp.array2d(dtype=float), - L: wp.array3d(dtype=float), + x: wp.array2d[float], + L: wp.array3d[float], ): worldid, nodeid = wp.tid() TILE_SIZE = wp.static(tile.size) @@ -2889,10 +2889,10 @@ def _tile_cholesky_factorize_solve(tile: TileSet): def _factor_solve_i_dense( m: Model, d: Data, - M: wp.array3d(dtype=float), - x: wp.array2d(dtype=float), - y: wp.array2d(dtype=float), - L: wp.array3d(dtype=float), + M: wp.array3d[float], + x: wp.array2d[float], + y: wp.array2d[float], + L: wp.array3d[float], ): for tile in m.qM_tiles: wp.launch_tiled( @@ -2931,19 +2931,19 @@ def factor_solve_i(m, d, M, L, D, x, y): @wp.kernel def _subtree_vel_forward( # Model: - body_rootid: wp.array(dtype=int), - body_mass: wp.array2d(dtype=float), - body_inertia: wp.array2d(dtype=wp.vec3), + body_rootid: wp.array[int], + body_mass: wp.array2d[float], + body_inertia: wp.array2d[wp.vec3], # Data in: - xipos_in: wp.array2d(dtype=wp.vec3), - ximat_in: wp.array2d(dtype=wp.mat33), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cvel_in: wp.array2d(dtype=wp.spatial_vector), + xipos_in: wp.array2d[wp.vec3], + ximat_in: wp.array2d[wp.mat33], + subtree_com_in: wp.array2d[wp.vec3], + cvel_in: wp.array2d[wp.spatial_vector], # Data out: - subtree_linvel_out: wp.array2d(dtype=wp.vec3), - subtree_angmom_out: wp.array2d(dtype=wp.vec3), + subtree_linvel_out: wp.array2d[wp.vec3], + subtree_angmom_out: wp.array2d[wp.vec3], # Out: - subtree_bodyvel_out: wp.array2d(dtype=wp.spatial_vector), + subtree_bodyvel_out: wp.array2d[wp.spatial_vector], ): worldid, bodyid = wp.tid() body_mass_id = worldid % body_mass.shape[0] @@ -2971,14 +2971,14 @@ def _subtree_vel_forward( @wp.kernel def _linear_momentum( # Model: - body_parentid: wp.array(dtype=int), - body_subtreemass: wp.array2d(dtype=float), + body_parentid: wp.array[int], + body_subtreemass: wp.array2d[float], # Data in: - subtree_linvel_in: wp.array2d(dtype=wp.vec3), + subtree_linvel_in: wp.array2d[wp.vec3], # In: - body_tree_: wp.array(dtype=int), + body_tree_: wp.array[int], # Data out: - subtree_linvel_out: wp.array2d(dtype=wp.vec3), + subtree_linvel_out: wp.array2d[wp.vec3], ): worldid, nodeid = wp.tid() bodyid = body_tree_[nodeid] @@ -2991,18 +2991,18 @@ def _linear_momentum( @wp.kernel def _angular_momentum( # Model: - body_parentid: wp.array(dtype=int), - body_mass: wp.array2d(dtype=float), - body_subtreemass: wp.array2d(dtype=float), + body_parentid: wp.array[int], + body_mass: wp.array2d[float], + body_subtreemass: wp.array2d[float], # Data in: - xipos_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), - subtree_linvel_in: wp.array2d(dtype=wp.vec3), + xipos_in: wp.array2d[wp.vec3], + subtree_com_in: wp.array2d[wp.vec3], + subtree_linvel_in: wp.array2d[wp.vec3], # In: - subtree_bodyvel_in: wp.array2d(dtype=wp.spatial_vector), - body_tree_: wp.array(dtype=int), + subtree_bodyvel_in: wp.array2d[wp.spatial_vector], + body_tree_: wp.array[int], # Data out: - subtree_angmom_out: wp.array2d(dtype=wp.vec3), + subtree_angmom_out: wp.array2d[wp.vec3], ): worldid, nodeid = wp.tid() bodyid = body_tree_[nodeid] @@ -3087,20 +3087,20 @@ def subtree_vel(m: Model, d: Data): @wp.kernel def _joint_tendon( # Model: - jnt_qposadr: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - ten_J_rownnz: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - wrap_objid: wp.array(dtype=int), - wrap_prm: wp.array(dtype=float), - tendon_jnt_adr: wp.array(dtype=int), - wrap_jnt_adr: wp.array(dtype=int), + jnt_qposadr: wp.array[int], + jnt_dofadr: wp.array[int], + ten_J_rownnz: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_colind: wp.array[int], + wrap_objid: wp.array[int], + wrap_prm: wp.array[float], + tendon_jnt_adr: wp.array[int], + wrap_jnt_adr: wp.array[int], # Data in: - qpos_in: wp.array2d(dtype=float), + qpos_in: wp.array2d[float], # Data out: - ten_J_out: wp.array2d(dtype=float), - ten_length_out: wp.array2d(dtype=float), + ten_J_out: wp.array2d[float], + ten_length_out: wp.array2d[float], ): worldid, wrapid = wp.tid() @@ -3126,12 +3126,12 @@ def _joint_tendon( @wp.func def _accumulate_jac_chain( # Model: - body_parentid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), + body_parentid: wp.array[int], + body_dofnum: wp.array[int], + body_dofadr: wp.array[int], + ten_J_colind: wp.array[int], # Data in: - cdof_in: wp.array2d(dtype=wp.spatial_vector), + cdof_in: wp.array2d[wp.spatial_vector], # In: offset: wp.vec3, vec: wp.vec3, @@ -3141,7 +3141,7 @@ def _accumulate_jac_chain( scale: float, worldid: int, # Data out: - ten_J_out: wp.array2d(dtype=float), + ten_J_out: wp.array2d[float], ): """Walk body chain from bodyid to root, accumulate Jacobian contributions.""" ptr = rownnz - 1 @@ -3172,25 +3172,25 @@ def _accumulate_jac_chain( @wp.kernel def _spatial_site_tendon( # Model: - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - ten_J_rownnz: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - wrap_objid: wp.array(dtype=int), - tendon_site_pair_adr: wp.array(dtype=int), - wrap_site_pair_adr: wp.array(dtype=int), - wrap_pulley_scale: wp.array(dtype=float), + body_parentid: wp.array[int], + body_rootid: wp.array[int], + body_dofnum: wp.array[int], + body_dofadr: wp.array[int], + site_bodyid: wp.array[int], + ten_J_rownnz: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_colind: wp.array[int], + wrap_objid: wp.array[int], + tendon_site_pair_adr: wp.array[int], + wrap_site_pair_adr: wp.array[int], + wrap_pulley_scale: wp.array[float], # Data in: - site_xpos_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), + site_xpos_in: wp.array2d[wp.vec3], + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], # Data out: - ten_J_out: wp.array2d(dtype=float), - ten_length_out: wp.array2d(dtype=float), + ten_J_out: wp.array2d[float], + ten_length_out: wp.array2d[float], ): worldid, elementid = wp.tid() @@ -3255,33 +3255,33 @@ def _spatial_site_tendon( @wp.kernel def _spatial_geom_tendon( # Model: - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - site_bodyid: wp.array(dtype=int), - ten_J_rownnz: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - wrap_type: wp.array(dtype=int), - wrap_objid: wp.array(dtype=int), - wrap_prm: wp.array(dtype=float), - tendon_geom_adr: wp.array(dtype=int), - wrap_geom_adr: wp.array(dtype=int), - wrap_pulley_scale: wp.array(dtype=float), + body_parentid: wp.array[int], + body_rootid: wp.array[int], + body_dofnum: wp.array[int], + body_dofadr: wp.array[int], + geom_bodyid: wp.array[int], + geom_size: wp.array2d[wp.vec3], + site_bodyid: wp.array[int], + ten_J_rownnz: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_colind: wp.array[int], + wrap_type: wp.array[int], + wrap_objid: wp.array[int], + wrap_prm: wp.array[float], + tendon_geom_adr: wp.array[int], + wrap_geom_adr: wp.array[int], + wrap_pulley_scale: wp.array[float], # Data in: - geom_xpos_in: wp.array2d(dtype=wp.vec3), - geom_xmat_in: wp.array2d(dtype=wp.mat33), - site_xpos_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), + geom_xpos_in: wp.array2d[wp.vec3], + geom_xmat_in: wp.array2d[wp.mat33], + site_xpos_in: wp.array2d[wp.vec3], + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], # Data out: - ten_J_out: wp.array2d(dtype=float), - ten_length_out: wp.array2d(dtype=float), + ten_J_out: wp.array2d[float], + ten_length_out: wp.array2d[float], # Out: - wrap_geom_xpos_out: wp.array2d(dtype=wp.spatial_vector), + wrap_geom_xpos_out: wp.array2d[wp.spatial_vector], ): worldid, elementid = wp.tid() wrap_adr = wrap_geom_adr[elementid] @@ -3468,19 +3468,19 @@ def _spatial_geom_tendon( def _spatial_tendon_wrap( # Model: ntendon: int, - tendon_adr: wp.array(dtype=int), - tendon_num: wp.array(dtype=int), - wrap_type: wp.array(dtype=int), - wrap_objid: wp.array(dtype=int), + tendon_adr: wp.array[int], + tendon_num: wp.array[int], + wrap_type: wp.array[int], + wrap_objid: wp.array[int], # Data in: - site_xpos_in: wp.array2d(dtype=wp.vec3), + site_xpos_in: wp.array2d[wp.vec3], # In: - wrap_geom_xpos_in: wp.array2d(dtype=wp.spatial_vector), + wrap_geom_xpos_in: wp.array2d[wp.spatial_vector], # Data out: - ten_wrapadr_out: wp.array2d(dtype=int), - ten_wrapnum_out: wp.array2d(dtype=int), - wrap_obj_out: wp.array2d(dtype=wp.vec2i), - wrap_xpos_out: wp.array2d(dtype=wp.spatial_vector), + ten_wrapadr_out: wp.array2d[int], + ten_wrapnum_out: wp.array2d[int], + wrap_obj_out: wp.array2d[wp.vec2i], + wrap_xpos_out: wp.array2d[wp.spatial_vector], ): worldid = wp.tid() diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/solver.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/solver.py index 2fabebe0..3964107d 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/solver.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/solver.py @@ -38,43 +38,43 @@ _BLOCK_CHOLESKY_DIM = 32 class InverseContext: """Workspace arrays for inverse dynamics.""" - Jaref: wp.array2d(dtype=float) - search_dot: wp.array(dtype=float) - gauss: wp.array(dtype=float) - cost: wp.array(dtype=float) - prev_cost: wp.array(dtype=float) - done: wp.array(dtype=bool) - changed_efc_ids: wp.array2d(dtype=int) - changed_efc_count: wp.array(dtype=int) + Jaref: wp.array2d[float] + search_dot: wp.array[float] + gauss: wp.array[float] + cost: wp.array[float] + prev_cost: wp.array[float] + done: wp.array[bool] + changed_efc_ids: wp.array2d[int] + changed_efc_count: wp.array[int] @dataclasses.dataclass class SolverContext: """Workspace arrays for constraint solver.""" - Jaref: wp.array2d(dtype=float) - search_dot: wp.array(dtype=float) - gauss: wp.array(dtype=float) - cost: wp.array(dtype=float) - prev_cost: wp.array(dtype=float) - done: wp.array(dtype=bool) - grad: wp.array2d(dtype=float) - grad_dot: wp.array(dtype=float) - Mgrad: wp.array2d(dtype=float) - search: wp.array2d(dtype=float) - mv: wp.array2d(dtype=float) - jv: wp.array2d(dtype=float) - quad: wp.array2d(dtype=wp.vec3) - quad_gauss: wp.array(dtype=wp.vec3) - alpha: wp.array(dtype=float) - prev_grad: wp.array2d(dtype=float) - prev_Mgrad: wp.array2d(dtype=float) - beta: wp.array(dtype=float) - h: wp.array3d(dtype=float) - hfactor: wp.array3d(dtype=float) + Jaref: wp.array2d[float] + search_dot: wp.array[float] + gauss: wp.array[float] + cost: wp.array[float] + prev_cost: wp.array[float] + done: wp.array[bool] + grad: wp.array2d[float] + grad_dot: wp.array[float] + Mgrad: wp.array2d[float] + search: wp.array2d[float] + mv: wp.array2d[float] + jv: wp.array2d[float] + quad: wp.array2d[wp.vec3] + quad_gauss: wp.array[wp.vec3] + alpha: wp.array[float] + prev_grad: wp.array2d[float] + prev_Mgrad: wp.array2d[float] + beta: wp.array[float] + h: wp.array3d[float] + hfactor: wp.array3d[float] # Incremental Hessian update (Newton only) - changed_efc_ids: wp.array2d(dtype=int) - changed_efc_count: wp.array(dtype=int) + changed_efc_ids: wp.array2d[int] + changed_efc_count: wp.array[int] def create_inverse_context(m: types.Model, d: types.Data) -> InverseContext: @@ -331,28 +331,28 @@ def _log_scale(min_value: float, max_value: float, num_values: int, i: int) -> f def linesearch_parallel_fused( # Model: opt_ls_iterations: int, - opt_impratio_invsqrt: wp.array(dtype=float), + opt_impratio_invsqrt: wp.array[float], opt_ls_parallel_min_step: float, # Data in: - ne_in: wp.array(dtype=int), - nf_in: wp.array(dtype=int), - nefc_in: wp.array(dtype=int), - contact_friction_in: wp.array(dtype=types.vec5), - contact_efc_address_in: wp.array2d(dtype=int), - efc_type_in: wp.array2d(dtype=int), - efc_id_in: wp.array2d(dtype=int), - efc_D_in: wp.array2d(dtype=float), - efc_frictionloss_in: wp.array2d(dtype=float), + ne_in: wp.array[int], + nf_in: wp.array[int], + nefc_in: wp.array[int], + contact_friction_in: wp.array[types.vec5], + contact_efc_address_in: wp.array2d[int], + efc_type_in: wp.array2d[int], + efc_id_in: wp.array2d[int], + efc_D_in: wp.array2d[float], + efc_frictionloss_in: wp.array2d[float], njmax_in: int, - nacon_in: wp.array(dtype=int), + nacon_in: wp.array[int], # In: - ctx_Jaref_in: wp.array2d(dtype=float), - ctx_jv_in: wp.array2d(dtype=float), - ctx_quad_in: wp.array2d(dtype=wp.vec3), - ctx_quad_gauss_in: wp.array(dtype=wp.vec3), - ctx_done_in: wp.array(dtype=bool), + ctx_Jaref_in: wp.array2d[float], + ctx_jv_in: wp.array2d[float], + ctx_quad_in: wp.array2d[wp.vec3], + ctx_quad_gauss_in: wp.array[wp.vec3], + ctx_done_in: wp.array[bool], # Out: - cost_out: wp.array2d(dtype=float), + cost_out: wp.array2d[float], ): worldid, alphaid = wp.tid() @@ -457,10 +457,10 @@ def linesearch_parallel_best_alpha( opt_ls_iterations: int, opt_ls_parallel_min_step: float, # In: - ctx_done_in: wp.array(dtype=bool), - cost_in: wp.array2d(dtype=float), + ctx_done_in: wp.array[bool], + cost_in: wp.array2d[float], # Out: - ctx_alpha_out: wp.array(dtype=float), + ctx_alpha_out: wp.array[float], ): worldid = wp.tid() @@ -478,7 +478,7 @@ def linesearch_parallel_best_alpha( ctx_alpha_out[worldid] = _log_scale(opt_ls_parallel_min_step, 1.0, opt_ls_iterations, bestid) -def _linesearch_parallel(m: types.Model, d: types.Data, ctx: SolverContext, cost: wp.array2d(dtype=float)): +def _linesearch_parallel(m: types.Model, d: types.Data, ctx: SolverContext, cost: wp.array2d[float]): """Parallel linesearch with setup and teardown kernels.""" dofs_per_thread = 20 if m.nv > 50 else 50 threads_per_efc = ceil(m.nv / dofs_per_thread) @@ -575,7 +575,7 @@ def _compute_efc_eval_pt_pyramidal( nf: int, # Per-row data: efc_D: float, - efc_frictionloss: wp.array(dtype=float), + efc_frictionloss: wp.array[float], ctx_Jaref: float, ctx_jv: float, ) -> wp.vec3: @@ -607,8 +607,8 @@ def _compute_efc_eval_pt_elliptic( impratio_invsqrt: float, # Per-row data (arrays for deferred load): efc_type: int, - efc_D_in: wp.array(dtype=float), - efc_frictionloss: wp.array(dtype=float), + efc_D_in: wp.array[float], + efc_frictionloss: wp.array[float], ctx_Jaref: float, ctx_jv: float, ctx_quad: wp.vec3, @@ -652,7 +652,7 @@ def _compute_efc_eval_pt_alpha_zero_pyramidal( nf: int, # Per-row data: efc_D: float, - efc_frictionloss: wp.array(dtype=float), + efc_frictionloss: wp.array[float], ctx_Jaref: float, ctx_jv: float, ) -> wp.vec3: @@ -681,8 +681,8 @@ def _compute_efc_eval_pt_alpha_zero_elliptic( impratio_invsqrt: float, # Per-row data (arrays for deferred load): efc_type: int, - efc_D_in: wp.array(dtype=float), - efc_frictionloss: wp.array(dtype=float), + efc_D_in: wp.array[float], + efc_frictionloss: wp.array[float], ctx_Jaref: float, ctx_jv: float, ctx_quad: wp.vec3, @@ -727,7 +727,7 @@ def _compute_efc_eval_pt_3alphas_pyramidal( nf: int, # Per-row data: efc_D: float, - efc_frictionloss: wp.array(dtype=float), + efc_frictionloss: wp.array[float], ctx_Jaref: float, ctx_jv: float, ) -> tuple[wp.vec3, wp.vec3, wp.vec3]: @@ -771,8 +771,8 @@ def _compute_efc_eval_pt_3alphas_elliptic( impratio_invsqrt: float, # Per-row data (arrays for deferred load): efc_type: int, - efc_D_in: wp.array(dtype=float), - efc_frictionloss: wp.array(dtype=float), + efc_D_in: wp.array[float], + efc_frictionloss: wp.array[float], ctx_Jaref: float, ctx_jv: float, ctx_quad: wp.vec3, @@ -917,44 +917,44 @@ def linesearch_iterative(ls_iterations: int, cone_type: types.ConeType, fuse_jv: def kernel( # Model: nv: int, - opt_tolerance: wp.array(dtype=float), - opt_ls_tolerance: wp.array(dtype=float), - opt_impratio_invsqrt: wp.array(dtype=float), - stat_meaninertia: wp.array(dtype=float), + opt_tolerance: wp.array[float], + opt_ls_tolerance: wp.array[float], + opt_impratio_invsqrt: wp.array[float], + stat_meaninertia: wp.array[float], # Data in: - ne_in: wp.array(dtype=int), - nf_in: wp.array(dtype=int), - nefc_in: wp.array(dtype=int), - qfrc_smooth_in: wp.array2d(dtype=float), - contact_friction_in: wp.array(dtype=types.vec5), - contact_dim_in: wp.array(dtype=int), - contact_efc_address_in: wp.array2d(dtype=int), - efc_type_in: wp.array2d(dtype=int), - efc_id_in: wp.array2d(dtype=int), - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), - efc_J_colind_in: wp.array3d(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_D_in: wp.array2d(dtype=float), - efc_frictionloss_in: wp.array2d(dtype=float), + ne_in: wp.array[int], + nf_in: wp.array[int], + nefc_in: wp.array[int], + qfrc_smooth_in: wp.array2d[float], + contact_friction_in: wp.array[types.vec5], + contact_dim_in: wp.array[int], + contact_efc_address_in: wp.array2d[int], + efc_type_in: wp.array2d[int], + efc_id_in: wp.array2d[int], + efc_J_rownnz_in: wp.array2d[int], + efc_J_rowadr_in: wp.array2d[int], + efc_J_colind_in: wp.array3d[int], + efc_J_in: wp.array3d[float], + efc_D_in: wp.array2d[float], + efc_frictionloss_in: wp.array2d[float], njmax_in: int, - nacon_in: wp.array(dtype=int), + nacon_in: wp.array[int], # In: - ctx_Jaref_in: wp.array2d(dtype=float), - ctx_search_in: wp.array2d(dtype=float), - ctx_search_dot_in: wp.array(dtype=float), - ctx_gauss_in: wp.array(dtype=float), - ctx_mv_in: wp.array2d(dtype=float), - ctx_jv_in: wp.array2d(dtype=float), - ctx_quad_in: wp.array2d(dtype=wp.vec3), - ctx_done_in: wp.array(dtype=bool), + ctx_Jaref_in: wp.array2d[float], + ctx_search_in: wp.array2d[float], + ctx_search_dot_in: wp.array[float], + ctx_gauss_in: wp.array[float], + ctx_mv_in: wp.array2d[float], + ctx_jv_in: wp.array2d[float], + ctx_quad_in: wp.array2d[wp.vec3], + ctx_done_in: wp.array[bool], # Data out: - qacc_out: wp.array2d(dtype=float), - efc_Ma_out: wp.array2d(dtype=float), + qacc_out: wp.array2d[float], + efc_Ma_out: wp.array2d[float], # Out: - ctx_Jaref_out: wp.array2d(dtype=float), - ctx_jv_out: wp.array2d(dtype=float), - ctx_quad_out: wp.array2d(dtype=wp.vec3), + ctx_Jaref_out: wp.array2d[float], + ctx_jv_out: wp.array2d[float], + ctx_quad_out: wp.array2d[wp.vec3], ): worldid, tid = wp.tid() @@ -1393,11 +1393,11 @@ def _linesearch_iterative(m: types.Model, d: types.Data, ctx: SolverContext, fus @wp.kernel def linesearch_zero_jv( # Data in: - nefc_in: wp.array(dtype=int), + nefc_in: wp.array[int], # In: - ctx_done_in: wp.array(dtype=bool), + ctx_done_in: wp.array[bool], # Out: - ctx_jv_out: wp.array2d(dtype=float), + ctx_jv_out: wp.array2d[float], ): worldid, efcid = wp.tid() @@ -1415,16 +1415,16 @@ def linesearch_jv_fused(is_sparse: bool, nv: int, dofs_per_thread: int): @wp.kernel(module="unique", enable_backward=False) def kernel( # Data in: - nefc_in: wp.array(dtype=int), - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), - efc_J_colind_in: wp.array3d(dtype=int), - efc_J_in: wp.array3d(dtype=float), + nefc_in: wp.array[int], + efc_J_rownnz_in: wp.array2d[int], + efc_J_rowadr_in: wp.array2d[int], + efc_J_colind_in: wp.array3d[int], + efc_J_in: wp.array3d[float], # In: - ctx_search_in: wp.array2d(dtype=float), - ctx_done_in: wp.array(dtype=bool), + ctx_search_in: wp.array2d[float], + ctx_done_in: wp.array[bool], # Out: - ctx_jv_out: wp.array2d(dtype=float), + ctx_jv_out: wp.array2d[float], ): worldid, efcid, dofstart = wp.tid() @@ -1476,15 +1476,15 @@ def linesearch_prepare_gauss(nv: int, dofs_per_thread: int): @wp.kernel(module="unique", enable_backward=False) def kernel( # Data in: - qfrc_smooth_in: wp.array2d(dtype=float), - efc_Ma_in: wp.array2d(dtype=float), + qfrc_smooth_in: wp.array2d[float], + efc_Ma_in: wp.array2d[float], # In: - ctx_search_in: wp.array2d(dtype=float), - ctx_gauss_in: wp.array(dtype=float), - ctx_mv_in: wp.array2d(dtype=float), - ctx_done_in: wp.array(dtype=bool), + ctx_search_in: wp.array2d[float], + ctx_gauss_in: wp.array[float], + ctx_mv_in: wp.array2d[float], + ctx_done_in: wp.array[bool], # Out: - ctx_quad_gauss_out: wp.array(dtype=wp.vec3), + ctx_quad_gauss_out: wp.array[wp.vec3], ): worldid, dofstart = wp.tid() @@ -1523,22 +1523,22 @@ def linesearch_prepare_gauss(nv: int, dofs_per_thread: int): @wp.kernel def linesearch_prepare_quad( # Model: - opt_impratio_invsqrt: wp.array(dtype=float), + opt_impratio_invsqrt: wp.array[float], # Data in: - nefc_in: wp.array(dtype=int), - contact_friction_in: wp.array(dtype=types.vec5), - contact_dim_in: wp.array(dtype=int), - contact_efc_address_in: wp.array2d(dtype=int), - efc_type_in: wp.array2d(dtype=int), - efc_id_in: wp.array2d(dtype=int), - efc_D_in: wp.array2d(dtype=float), - nacon_in: wp.array(dtype=int), + nefc_in: wp.array[int], + contact_friction_in: wp.array[types.vec5], + contact_dim_in: wp.array[int], + contact_efc_address_in: wp.array2d[int], + efc_type_in: wp.array2d[int], + efc_id_in: wp.array2d[int], + efc_D_in: wp.array2d[float], + nacon_in: wp.array[int], # In: - ctx_Jaref_in: wp.array2d(dtype=float), - ctx_jv_in: wp.array2d(dtype=float), - ctx_done_in: wp.array(dtype=bool), + ctx_Jaref_in: wp.array2d[float], + ctx_jv_in: wp.array2d[float], + ctx_done_in: wp.array[bool], # Out: - ctx_quad_out: wp.array2d(dtype=wp.vec3), + ctx_quad_out: wp.array2d[wp.vec3], ): worldid, efcid = wp.tid() @@ -1619,13 +1619,13 @@ def linesearch_prepare_quad( @wp.kernel def linesearch_qacc_ma( # In: - ctx_search_in: wp.array2d(dtype=float), - ctx_mv_in: wp.array2d(dtype=float), - ctx_alpha_in: wp.array(dtype=float), - ctx_done_in: wp.array(dtype=bool), + ctx_search_in: wp.array2d[float], + ctx_mv_in: wp.array2d[float], + ctx_alpha_in: wp.array[float], + ctx_done_in: wp.array[bool], # Data out: - qacc_out: wp.array2d(dtype=float), - efc_Ma_out: wp.array2d(dtype=float), + qacc_out: wp.array2d[float], + efc_Ma_out: wp.array2d[float], ): worldid, dofid = wp.tid() @@ -1640,13 +1640,13 @@ def linesearch_qacc_ma( @wp.kernel def linesearch_jaref( # Data in: - nefc_in: wp.array(dtype=int), + nefc_in: wp.array[int], # In: - ctx_jv_in: wp.array2d(dtype=float), - ctx_alpha_in: wp.array(dtype=float), - ctx_done_in: wp.array(dtype=bool), + ctx_jv_in: wp.array2d[float], + ctx_alpha_in: wp.array[float], + ctx_done_in: wp.array[bool], # Out: - ctx_Jaref_out: wp.array2d(dtype=float), + ctx_Jaref_out: wp.array2d[float], ): worldid, efcid = wp.tid() @@ -1660,7 +1660,7 @@ def linesearch_jaref( @event_scope -def _linesearch(m: types.Model, d: types.Data, ctx: SolverContext, cost: wp.array2d(dtype=float)): +def _linesearch(m: types.Model, d: types.Data, ctx: SolverContext, cost: wp.array2d[float]): """Linesearch for constraint solver. Args: @@ -1706,11 +1706,11 @@ def _linesearch(m: types.Model, d: types.Data, ctx: SolverContext, cost: wp.arra @wp.kernel def solve_init_efc( # Data out: - solver_niter_out: wp.array(dtype=int), + solver_niter_out: wp.array[int], # Out: - ctx_search_dot_out: wp.array(dtype=float), - ctx_cost_out: wp.array(dtype=float), - ctx_done_out: wp.array(dtype=bool), + ctx_search_dot_out: wp.array[float], + ctx_cost_out: wp.array[float], + ctx_done_out: wp.array[bool], ): worldid = wp.tid() ctx_cost_out[worldid] = types.MJ_MAXVAL @@ -1724,15 +1724,15 @@ def solve_init_jaref(is_sparse: bool, nv: int, dofs_per_thread: int): @wp.kernel(module="unique", enable_backward=False) def kernel( # Data in: - nefc_in: wp.array(dtype=int), - qacc_in: wp.array2d(dtype=float), - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), - efc_J_colind_in: wp.array3d(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_aref_in: wp.array2d(dtype=float), + nefc_in: wp.array[int], + qacc_in: wp.array2d[float], + efc_J_rownnz_in: wp.array2d[int], + efc_J_rowadr_in: wp.array2d[int], + efc_J_colind_in: wp.array3d[int], + efc_J_in: wp.array3d[float], + efc_aref_in: wp.array2d[float], # Out: - ctx_Jaref_out: wp.array2d(dtype=float), + ctx_Jaref_out: wp.array2d[float], ): worldid, efcid, dofstart = wp.tid() @@ -1771,10 +1771,10 @@ def solve_init_jaref(is_sparse: bool, nv: int, dofs_per_thread: int): @wp.kernel def solve_init_search( # In: - ctx_Mgrad_in: wp.array2d(dtype=float), + ctx_Mgrad_in: wp.array2d[float], # Out: - ctx_search_out: wp.array2d(dtype=float), - ctx_search_dot_out: wp.array(dtype=float), + ctx_search_out: wp.array2d[float], + ctx_search_dot_out: wp.array[float], ): worldid, dofid = wp.tid() search = -1.0 * ctx_Mgrad_in[worldid, dofid] @@ -1785,12 +1785,12 @@ def solve_init_search( @wp.kernel def update_constraint_init_cost( # In: - ctx_cost_in: wp.array(dtype=float), - ctx_done_in: wp.array(dtype=bool), + ctx_cost_in: wp.array[float], + ctx_done_in: wp.array[bool], # Out: - ctx_gauss_out: wp.array(dtype=float), - ctx_cost_out: wp.array(dtype=float), - ctx_prev_cost_out: wp.array(dtype=float), + ctx_gauss_out: wp.array[float], + ctx_cost_out: wp.array[float], + ctx_prev_cost_out: wp.array[float], ): worldid = wp.tid() @@ -1809,29 +1809,29 @@ def update_constraint_efc(track_changes: bool): @wp.kernel(module="unique", enable_backward=False) def kernel( # Model: - opt_impratio_invsqrt: wp.array(dtype=float), + opt_impratio_invsqrt: wp.array[float], # Data in: - ne_in: wp.array(dtype=int), - nf_in: wp.array(dtype=int), - nefc_in: wp.array(dtype=int), - contact_friction_in: wp.array(dtype=types.vec5), - contact_dim_in: wp.array(dtype=int), - contact_efc_address_in: wp.array2d(dtype=int), - efc_type_in: wp.array2d(dtype=int), - efc_id_in: wp.array2d(dtype=int), - efc_D_in: wp.array2d(dtype=float), - efc_frictionloss_in: wp.array2d(dtype=float), - nacon_in: wp.array(dtype=int), + ne_in: wp.array[int], + nf_in: wp.array[int], + nefc_in: wp.array[int], + contact_friction_in: wp.array[types.vec5], + contact_dim_in: wp.array[int], + contact_efc_address_in: wp.array2d[int], + efc_type_in: wp.array2d[int], + efc_id_in: wp.array2d[int], + efc_D_in: wp.array2d[float], + efc_frictionloss_in: wp.array2d[float], + nacon_in: wp.array[int], # In: - ctx_Jaref_in: wp.array2d(dtype=float), - ctx_done_in: wp.array(dtype=bool), + ctx_Jaref_in: wp.array2d[float], + ctx_done_in: wp.array[bool], # Data out: - efc_force_out: wp.array2d(dtype=float), - efc_state_out: wp.array2d(dtype=int), + efc_force_out: wp.array2d[float], + efc_state_out: wp.array2d[int], # Out: - ctx_cost_out: wp.array(dtype=float), - changed_ids_out: wp.array2d(dtype=int), - changed_count_out: wp.array(dtype=int), + ctx_cost_out: wp.array[float], + changed_ids_out: wp.array2d[int], + changed_count_out: wp.array[int], ): worldid, efcid = wp.tid() @@ -1954,16 +1954,16 @@ def update_constraint_efc(track_changes: bool): @wp.kernel def update_constraint_init_qfrc_constraint_sparse( # Data in: - nefc_in: wp.array(dtype=int), - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), - efc_J_colind_in: wp.array3d(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_force_in: wp.array2d(dtype=float), + nefc_in: wp.array[int], + efc_J_rownnz_in: wp.array2d[int], + efc_J_rowadr_in: wp.array2d[int], + efc_J_colind_in: wp.array3d[int], + efc_J_in: wp.array3d[float], + efc_force_in: wp.array2d[float], # In: - ctx_done_in: wp.array(dtype=bool), + ctx_done_in: wp.array[bool], # Data out: - qfrc_constraint_out: wp.array2d(dtype=float), + qfrc_constraint_out: wp.array2d[float], ): worldid, efcid = wp.tid() @@ -1987,14 +1987,14 @@ def update_constraint_init_qfrc_constraint_sparse( @wp.kernel def update_constraint_init_qfrc_constraint_dense( # Data in: - nefc_in: wp.array(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_force_in: wp.array2d(dtype=float), + nefc_in: wp.array[int], + efc_J_in: wp.array3d[float], + efc_force_in: wp.array2d[float], njmax_in: int, # In: - ctx_done_in: wp.array(dtype=bool), + ctx_done_in: wp.array[bool], # Data out: - qfrc_constraint_out: wp.array2d(dtype=float), + qfrc_constraint_out: wp.array2d[float], ): worldid, dofid = wp.tid() @@ -2015,15 +2015,15 @@ def update_constraint_gauss_cost(nv: int, dofs_per_thread: int): @wp.kernel(module="unique", enable_backward=False) def kernel( # Data in: - qacc_in: wp.array2d(dtype=float), - qfrc_smooth_in: wp.array2d(dtype=float), - qacc_smooth_in: wp.array2d(dtype=float), - efc_Ma_in: wp.array2d(dtype=float), + qacc_in: wp.array2d[float], + qfrc_smooth_in: wp.array2d[float], + qacc_smooth_in: wp.array2d[float], + efc_Ma_in: wp.array2d[float], # In: - ctx_done_in: wp.array(dtype=bool), + ctx_done_in: wp.array[bool], # Out: - ctx_gauss_out: wp.array(dtype=float), - ctx_cost_out: wp.array(dtype=float), + ctx_gauss_out: wp.array[float], + ctx_cost_out: wp.array[float], ): worldid, dofstart = wp.tid() @@ -2054,14 +2054,14 @@ def update_constraint_gauss_cost(nv: int, dofs_per_thread: int): @wp.kernel def update_gradient_h_incremental( # Data in: - efc_J_in: wp.array3d(dtype=float), - efc_D_in: wp.array2d(dtype=float), - efc_state_in: wp.array2d(dtype=int), + efc_J_in: wp.array3d[float], + efc_D_in: wp.array2d[float], + efc_state_in: wp.array2d[int], # In: - changed_ids_in: wp.array2d(dtype=int), - changed_count_in: wp.array(dtype=int), + changed_ids_in: wp.array2d[int], + changed_count_in: wp.array[int], # Out: - ctx_h_out: wp.array3d(dtype=float), + ctx_h_out: wp.array3d[float], ): """Incrementally update lower triangle of H for changed constraints. @@ -2101,17 +2101,17 @@ def update_gradient_h_incremental( @wp.kernel def update_gradient_h_incremental_sparse( # Data in: - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), - efc_J_colind_in: wp.array3d(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_D_in: wp.array2d(dtype=float), - efc_state_in: wp.array2d(dtype=int), + efc_J_rownnz_in: wp.array2d[int], + efc_J_rowadr_in: wp.array2d[int], + efc_J_colind_in: wp.array3d[int], + efc_J_in: wp.array3d[float], + efc_D_in: wp.array2d[float], + efc_state_in: wp.array2d[int], # In: - changed_ids_in: wp.array2d(dtype=int), - changed_count_in: wp.array(dtype=int), + changed_ids_in: wp.array2d[int], + changed_count_in: wp.array[int], # Out: - ctx_h_out: wp.array3d(dtype=float), + ctx_h_out: wp.array3d[float], ): """Incrementally update lower triangle of H for changed constraints (sparse J).""" worldid, change_idx = wp.tid() @@ -2222,9 +2222,9 @@ def _update_constraint(m: types.Model, d: types.Data, ctx: SolverContext | Inver @wp.kernel def update_gradient_zero_grad_dot( # In: - ctx_done_in: wp.array(dtype=bool), + ctx_done_in: wp.array[bool], # Out: - ctx_grad_dot_out: wp.array(dtype=float), + ctx_grad_dot_out: wp.array[float], ): worldid = wp.tid() @@ -2237,14 +2237,14 @@ def update_gradient_zero_grad_dot( @wp.kernel def update_gradient_grad( # Data in: - qfrc_smooth_in: wp.array2d(dtype=float), - qfrc_constraint_in: wp.array2d(dtype=float), - efc_Ma_in: wp.array2d(dtype=float), + qfrc_smooth_in: wp.array2d[float], + qfrc_constraint_in: wp.array2d[float], + efc_Ma_in: wp.array2d[float], # In: - ctx_done_in: wp.array(dtype=bool), + ctx_done_in: wp.array[bool], # Out: - ctx_grad_out: wp.array2d(dtype=float), - ctx_grad_dot_out: wp.array(dtype=float), + ctx_grad_out: wp.array2d[float], + ctx_grad_dot_out: wp.array[float], ): worldid, dofid = wp.tid() @@ -2259,14 +2259,14 @@ def update_gradient_grad( @wp.kernel def update_gradient_set_h_qM_lower_sparse( # Model: - qM_fullm_i: wp.array(dtype=int), - qM_fullm_j: wp.array(dtype=int), + qM_fullm_i: wp.array[int], + qM_fullm_j: wp.array[int], # Data in: - qM_in: wp.array3d(dtype=float), + qM_in: wp.array3d[float], # In: - ctx_done_in: wp.array(dtype=bool), + ctx_done_in: wp.array[bool], # Out: - ctx_h_out: wp.array3d(dtype=float), + ctx_h_out: wp.array3d[float], ): worldid, elementid = wp.tid() @@ -2301,14 +2301,14 @@ def update_gradient_JTDAJ_sparse_tiled(tile_size: int, njmax: int): @wp.kernel(module="unique", enable_backward=False) def kernel( # Data in: - nefc_in: wp.array(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_D_in: wp.array2d(dtype=float), - efc_state_in: wp.array2d(dtype=int), + nefc_in: wp.array[int], + efc_J_in: wp.array3d[float], + efc_D_in: wp.array2d[float], + efc_state_in: wp.array2d[int], # In: - ctx_done_in: wp.array(dtype=bool), + ctx_done_in: wp.array[bool], # Out: - ctx_h_out: wp.array3d(dtype=float), + ctx_h_out: wp.array3d[float], ): worldid, elementid = wp.tid() @@ -2374,15 +2374,15 @@ def update_gradient_JTDAJ_dense_tiled(nv_pad: int, tile_size: int, njmax: int): @wp.kernel(module="unique", enable_backward=False) def kernel( # Data in: - nefc_in: wp.array(dtype=int), - qM_in: wp.array3d(dtype=float), - efc_J_in: wp.array3d(dtype=float), - efc_D_in: wp.array2d(dtype=float), - efc_state_in: wp.array2d(dtype=int), + nefc_in: wp.array[int], + qM_in: wp.array3d[float], + efc_J_in: wp.array3d[float], + efc_D_in: wp.array2d[float], + efc_state_in: wp.array2d[int], # In: - ctx_done_in: wp.array(dtype=bool), + ctx_done_in: wp.array[bool], # Out: - ctx_h_out: wp.array3d(dtype=float), + ctx_h_out: wp.array3d[float], ): worldid = wp.tid() @@ -2429,31 +2429,31 @@ def update_gradient_JTDAJ_dense_tiled(nv_pad: int, tile_size: int, njmax: int): @wp.kernel def update_gradient_JTCJ_sparse( # Model: - opt_impratio_invsqrt: wp.array(dtype=float), - dof_tri_row: wp.array(dtype=int), - dof_tri_col: wp.array(dtype=int), + opt_impratio_invsqrt: wp.array[float], + dof_tri_row: wp.array[int], + dof_tri_col: wp.array[int], # Data in: - contact_dist_in: wp.array(dtype=float), - contact_includemargin_in: wp.array(dtype=float), - contact_friction_in: wp.array(dtype=types.vec5), - contact_dim_in: wp.array(dtype=int), - contact_efc_address_in: wp.array2d(dtype=int), - contact_worldid_in: wp.array(dtype=int), - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), - efc_J_colind_in: wp.array3d(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_D_in: wp.array2d(dtype=float), - efc_state_in: wp.array2d(dtype=int), + contact_dist_in: wp.array[float], + contact_includemargin_in: wp.array[float], + contact_friction_in: wp.array[types.vec5], + contact_dim_in: wp.array[int], + contact_efc_address_in: wp.array2d[int], + contact_worldid_in: wp.array[int], + efc_J_rownnz_in: wp.array2d[int], + efc_J_rowadr_in: wp.array2d[int], + efc_J_colind_in: wp.array3d[int], + efc_J_in: wp.array3d[float], + efc_D_in: wp.array2d[float], + efc_state_in: wp.array2d[int], naconmax_in: int, - nacon_in: wp.array(dtype=int), + nacon_in: wp.array[int], # In: - ctx_Jaref_in: wp.array2d(dtype=float), - ctx_done_in: wp.array(dtype=bool), + ctx_Jaref_in: wp.array2d[float], + ctx_done_in: wp.array[bool], nblocks_perblock: int, dim_block: int, # Out: - ctx_h_out: wp.array3d(dtype=float), + ctx_h_out: wp.array3d[float], ): conid_start, elementid = wp.tid() @@ -2591,28 +2591,28 @@ def update_gradient_JTCJ_sparse( @wp.kernel def update_gradient_JTCJ_dense( # Model: - opt_impratio_invsqrt: wp.array(dtype=float), - dof_tri_row: wp.array(dtype=int), - dof_tri_col: wp.array(dtype=int), + opt_impratio_invsqrt: wp.array[float], + dof_tri_row: wp.array[int], + dof_tri_col: wp.array[int], # Data in: - contact_dist_in: wp.array(dtype=float), - contact_includemargin_in: wp.array(dtype=float), - contact_friction_in: wp.array(dtype=types.vec5), - contact_dim_in: wp.array(dtype=int), - contact_efc_address_in: wp.array2d(dtype=int), - contact_worldid_in: wp.array(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_D_in: wp.array2d(dtype=float), - efc_state_in: wp.array2d(dtype=int), + contact_dist_in: wp.array[float], + contact_includemargin_in: wp.array[float], + contact_friction_in: wp.array[types.vec5], + contact_dim_in: wp.array[int], + contact_efc_address_in: wp.array2d[int], + contact_worldid_in: wp.array[int], + efc_J_in: wp.array3d[float], + efc_D_in: wp.array2d[float], + efc_state_in: wp.array2d[int], naconmax_in: int, - nacon_in: wp.array(dtype=int), + nacon_in: wp.array[int], # In: - ctx_Jaref_in: wp.array2d(dtype=float), - ctx_done_in: wp.array(dtype=bool), + ctx_Jaref_in: wp.array2d[float], + ctx_done_in: wp.array[bool], nblocks_perblock: int, dim_block: int, # Out: - ctx_h_out: wp.array3d(dtype=float), + ctx_h_out: wp.array3d[float], ): conid_start, elementid = wp.tid() @@ -2733,11 +2733,11 @@ def update_gradient_cholesky(tile_size: int): @wp.kernel(module="unique", enable_backward=False) def kernel( # In: - ctx_grad_in: wp.array2d(dtype=float), - h_in: wp.array3d(dtype=float), - ctx_done_in: wp.array(dtype=bool), + ctx_grad_in: wp.array2d[float], + h_in: wp.array3d[float], + ctx_done_in: wp.array[bool], # Out: - ctx_Mgrad_out: wp.array2d(dtype=float), + ctx_Mgrad_out: wp.array2d[float], ): worldid = wp.tid() TILE_SIZE = wp.static(tile_size) @@ -2759,12 +2759,12 @@ def update_gradient_cholesky_blocked(tile_size: int, matrix_size: int): @wp.kernel(module="unique", enable_backward=False) def kernel( # In: - ctx_done_in: wp.array(dtype=bool), - ctx_grad_in: wp.array3d(dtype=float), - ctx_h_in: wp.array3d(dtype=float), - ctx_hfactor: wp.array3d(dtype=float), + ctx_done_in: wp.array[bool], + ctx_grad_in: wp.array3d[float], + ctx_h_in: wp.array3d[float], + ctx_hfactor: wp.array3d[float], # Out: - ctx_Mgrad_out: wp.array3d(dtype=float), + ctx_Mgrad_out: wp.array3d[float], ): worldid = wp.tid() TILE_SIZE = wp.static(tile_size) @@ -2786,7 +2786,7 @@ def update_gradient_cholesky_blocked(tile_size: int, matrix_size: int): @wp.kernel -def padding_h(nv: int, ctx_done_in: wp.array(dtype=bool), ctx_h_out: wp.array3d(dtype=float)): +def padding_h(nv: int, ctx_done_in: wp.array[bool], ctx_h_out: wp.array3d[float]): worldid, elementid = wp.tid() if ctx_done_in[worldid]: @@ -2826,17 +2826,17 @@ def _cholesky_factorize_solve(m: types.Model, d: types.Data, ctx: SolverContext) @wp.kernel def _JTDAJ_sparse( # Data in: - nefc_in: wp.array(dtype=int), - efc_J_rownnz_in: wp.array2d(dtype=int), - efc_J_rowadr_in: wp.array2d(dtype=int), - efc_J_colind_in: wp.array3d(dtype=int), - efc_J_in: wp.array3d(dtype=float), - efc_D_in: wp.array2d(dtype=float), - efc_state_in: wp.array2d(dtype=int), + nefc_in: wp.array[int], + efc_J_rownnz_in: wp.array2d[int], + efc_J_rowadr_in: wp.array2d[int], + efc_J_colind_in: wp.array3d[int], + efc_J_in: wp.array3d[float], + efc_D_in: wp.array2d[float], + efc_state_in: wp.array2d[int], # In: - ctx_done_in: wp.array(dtype=bool), + ctx_done_in: wp.array[bool], # Out: - h_out: wp.array3d(dtype=float), + h_out: wp.array3d[float], ): worldid, efcid = wp.tid() @@ -2870,10 +2870,10 @@ def _JTDAJ_sparse( colindj = efc_J_colind_in[worldid, 0, sparseidj] h = Ji * Jj * efc_D - wp.atomic_add(h_out[worldid, colindi], colindj, h) - - if i != j: - wp.atomic_add(h_out[worldid, colindj], colindi, h) + # Store in lower triangle only: ensure row >= col + row = wp.max(colindi, colindj) + col = wp.min(colindi, colindj) + wp.atomic_add(h_out[worldid, row], col, h) def _update_gradient(m: types.Model, d: types.Data, ctx: SolverContext): @@ -3061,12 +3061,12 @@ def _update_gradient_incremental(m: types.Model, d: types.Data, ctx: SolverConte @wp.kernel def solve_prev_grad_Mgrad( # In: - ctx_grad_in: wp.array2d(dtype=float), - ctx_Mgrad_in: wp.array2d(dtype=float), - ctx_done_in: wp.array(dtype=bool), + ctx_grad_in: wp.array2d[float], + ctx_Mgrad_in: wp.array2d[float], + ctx_done_in: wp.array[bool], # Out: - ctx_prev_grad_out: wp.array2d(dtype=float), - ctx_prev_Mgrad_out: wp.array2d(dtype=float), + ctx_prev_grad_out: wp.array2d[float], + ctx_prev_Mgrad_out: wp.array2d[float], ): worldid, dofid = wp.tid() @@ -3082,13 +3082,13 @@ def solve_beta( # Model: nv: int, # In: - ctx_grad_in: wp.array2d(dtype=float), - ctx_Mgrad_in: wp.array2d(dtype=float), - ctx_prev_grad_in: wp.array2d(dtype=float), - ctx_prev_Mgrad_in: wp.array2d(dtype=float), - ctx_done_in: wp.array(dtype=bool), + ctx_grad_in: wp.array2d[float], + ctx_Mgrad_in: wp.array2d[float], + ctx_prev_grad_in: wp.array2d[float], + ctx_prev_Mgrad_in: wp.array2d[float], + ctx_done_in: wp.array[bool], # Out: - ctx_beta_out: wp.array(dtype=float), + ctx_beta_out: wp.array[float], ): worldid = wp.tid() @@ -3108,9 +3108,9 @@ def solve_beta( @wp.kernel def solve_zero_search_dot( # In: - ctx_done_in: wp.array(dtype=bool), + ctx_done_in: wp.array[bool], # Out: - ctx_search_dot_out: wp.array(dtype=float), + ctx_search_dot_out: wp.array[float], ): worldid = wp.tid() @@ -3125,13 +3125,13 @@ def solve_search_update( # Model: opt_solver: int, # In: - ctx_Mgrad_in: wp.array2d(dtype=float), - ctx_search_in: wp.array2d(dtype=float), - ctx_beta_in: wp.array(dtype=float), - ctx_done_in: wp.array(dtype=bool), + ctx_Mgrad_in: wp.array2d[float], + ctx_search_in: wp.array2d[float], + ctx_beta_in: wp.array[float], + ctx_done_in: wp.array[bool], # Out: - ctx_search_out: wp.array2d(dtype=float), - ctx_search_dot_out: wp.array(dtype=float), + ctx_search_out: wp.array2d[float], + ctx_search_dot_out: wp.array[float], ): worldid, dofid = wp.tid() @@ -3151,19 +3151,19 @@ def solve_search_update( def solve_done( # Model: nv: int, - opt_tolerance: wp.array(dtype=float), + opt_tolerance: wp.array[float], opt_iterations: int, - stat_meaninertia: wp.array(dtype=float), + stat_meaninertia: wp.array[float], # In: - ctx_grad_dot_in: wp.array(dtype=float), - ctx_cost_in: wp.array(dtype=float), - ctx_prev_cost_in: wp.array(dtype=float), - ctx_done_in: wp.array(dtype=bool), + ctx_grad_dot_in: wp.array[float], + ctx_cost_in: wp.array[float], + ctx_prev_cost_in: wp.array[float], + ctx_done_in: wp.array[bool], # Data out: - solver_niter_out: wp.array(dtype=int), + solver_niter_out: wp.array[int], # Out: - nsolving_out: wp.array(dtype=int), - ctx_done_out: wp.array(dtype=bool), + nsolving_out: wp.array[int], + ctx_done_out: wp.array[bool], ): worldid = wp.tid() @@ -3189,8 +3189,8 @@ def _solver_iteration( m: types.Model, d: types.Data, ctx: SolverContext, - step_size_cost: wp.array2d(dtype=float), - nsolving: wp.array(dtype=int), + step_size_cost: wp.array2d[float], + nsolving: wp.array[int], ): _linesearch(m, d, ctx, step_size_cost) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/support.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/support.py index 995b2a7c..f6b3fe9d 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/support.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/support.py @@ -69,16 +69,16 @@ def mul_m_sparse(check_skip: bool): @wp.kernel(module="unique") def _mul_m_sparse( # Model: - qM_mulm_rowadr: wp.array(dtype=int), - qM_mulm_col: wp.array(dtype=int), - qM_mulm_madr: wp.array(dtype=int), + qM_mulm_rowadr: wp.array[int], + qM_mulm_col: wp.array[int], + qM_mulm_madr: wp.array[int], # Data in: - qM_in: wp.array3d(dtype=float), + qM_in: wp.array3d[float], # In: - vec: wp.array2d(dtype=float), - skip: wp.array(dtype=bool), + vec: wp.array2d[float], + skip: wp.array[bool], # Out: - res: wp.array2d(dtype=float), + res: wp.array2d[float], ): """Sparse matmul: one thread per DOF, gather-based (no atomics).""" worldid, dofid = wp.tid() @@ -108,12 +108,12 @@ def mul_m_dense(nv: int, check_skip: bool): @wp.kernel(module="unique") def _mul_m_dense( # Data in: - qM_in: wp.array3d(dtype=float), + qM_in: wp.array3d[float], # In: - vec: wp.array2d(dtype=float), - skip: wp.array(dtype=bool), + vec: wp.array2d[float], + skip: wp.array[bool], # Out: - res: wp.array2d(dtype=float), + res: wp.array2d[float], ): worldid, i = wp.tid() @@ -133,8 +133,8 @@ def mul_m_dense(nv: int, check_skip: bool): def mul_m( m: Model, d: Data, - res: wp.array2d(dtype=float), - vec: wp.array2d(dtype=float), + res: wp.array2d[float], + vec: wp.array2d[float], skip: Optional[wp.array] = None, M: Optional[wp.array] = None, ): @@ -175,18 +175,18 @@ def mul_m( def _apply_ft( # Model: nbody: int, - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - dof_bodyid: wp.array(dtype=int), + body_parentid: wp.array[int], + body_rootid: wp.array[int], + dof_bodyid: wp.array[int], # Data in: - xipos_in: wp.array2d(dtype=wp.vec3), - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), + xipos_in: wp.array2d[wp.vec3], + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], # In: - ft_in: wp.array2d(dtype=wp.spatial_vector), + ft_in: wp.array2d[wp.spatial_vector], flg_add: bool, # Out: - qfrc_out: wp.array2d(dtype=float), + qfrc_out: wp.array2d[float], ): worldid, dofid = wp.tid() cdof = cdof_in[worldid, dofid] @@ -216,7 +216,7 @@ def _apply_ft( qfrc_out[worldid, dofid] = accumul -def apply_ft(m: Model, d: Data, ft: wp.array2d(dtype=wp.spatial_vector), qfrc: wp.array2d(dtype=float), flg_add: bool): +def apply_ft(m: Model, d: Data, ft: wp.array2d[wp.spatial_vector], qfrc: wp.array2d[float], flg_add: bool): wp.launch( kernel=_apply_ft, dim=(d.nworld, m.nv), @@ -226,7 +226,7 @@ def apply_ft(m: Model, d: Data, ft: wp.array2d(dtype=wp.spatial_vector), qfrc: w @event_scope -def xfrc_accumulate(m: Model, d: Data, qfrc: wp.array2d(dtype=float)): +def xfrc_accumulate(m: Model, d: Data, qfrc: wp.array2d[float]): """Map applied forces at each body via Jacobians to dof space and accumulate. Args: @@ -238,9 +238,7 @@ def xfrc_accumulate(m: Model, d: Data, qfrc: wp.array2d(dtype=float)): @wp.func -def _decode_pyramid( - njmax_in: int, pyramid: wp.array(dtype=float), efc_address: int, mu: vec5, condim: int -) -> wp.spatial_vector: +def _decode_pyramid(njmax_in: int, pyramid: wp.array[float], efc_address: int, mu: vec5, condim: int) -> wp.spatial_vector: """Converts pyramid representation to contact force.""" force = wp.spatial_vector() @@ -270,13 +268,13 @@ def contact_force_fn( # Model: opt_cone: int, # Data in: - contact_frame_in: wp.array(dtype=wp.mat33), - contact_friction_in: wp.array(dtype=vec5), - contact_dim_in: wp.array(dtype=int), - contact_efc_address_in: wp.array2d(dtype=int), - efc_force_in: wp.array2d(dtype=float), + contact_frame_in: wp.array[wp.mat33], + contact_friction_in: wp.array[vec5], + contact_dim_in: wp.array[int], + contact_efc_address_in: wp.array2d[int], + efc_force_in: wp.array2d[float], njmax_in: int, - nacon_in: wp.array(dtype=int), + nacon_in: wp.array[int], # In: worldid: int, contact_id: int, @@ -315,19 +313,19 @@ def contact_force_kernel( # Model: opt_cone: int, # Data in: - contact_frame_in: wp.array(dtype=wp.mat33), - contact_friction_in: wp.array(dtype=vec5), - contact_dim_in: wp.array(dtype=int), - contact_efc_address_in: wp.array2d(dtype=int), - contact_worldid_in: wp.array(dtype=int), - efc_force_in: wp.array2d(dtype=float), + contact_frame_in: wp.array[wp.mat33], + contact_friction_in: wp.array[vec5], + contact_dim_in: wp.array[int], + contact_efc_address_in: wp.array2d[int], + contact_worldid_in: wp.array[int], + efc_force_in: wp.array2d[float], njmax_in: int, - nacon_in: wp.array(dtype=int), + nacon_in: wp.array[int], # In: - contact_ids: wp.array(dtype=int), + contact_ids: wp.array[int], to_world_frame: bool, # Out: - out: wp.array(dtype=wp.spatial_vector), + out: wp.array[wp.spatial_vector], ): tid = wp.tid() @@ -353,9 +351,7 @@ def contact_force_kernel( ) -def contact_force( - m: Model, d: Data, contact_ids: wp.array(dtype=int), to_world_frame: bool, force: wp.array(dtype=wp.spatial_vector) -): +def contact_force(m: Model, d: Data, contact_ids: wp.array[int], to_world_frame: bool, force: wp.array[wp.spatial_vector]): """Compute forces for contacts in Data. Args: @@ -400,12 +396,12 @@ def transform_force(frc: wp.spatial_vector, offset: wp.vec3) -> wp.spatial_vecto @wp.func def jac_dof( # Model: - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - dof_bodyid: wp.array(dtype=int), + body_parentid: wp.array[int], + body_rootid: wp.array[int], + dof_bodyid: wp.array[int], # Data in: - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], # In: point: wp.vec3, bodyid: int, @@ -441,18 +437,18 @@ def _make_jac_kernel(has_jacp: bool, has_jacr: bool): @wp.kernel(module="unique", enable_backward=False) def _jac( # Model: - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - dof_bodyid: wp.array(dtype=int), + body_parentid: wp.array[int], + body_rootid: wp.array[int], + dof_bodyid: wp.array[int], # Data in: - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], # In: - point_in: wp.array(dtype=wp.vec3), - bodyid_in: wp.array(dtype=int), + point_in: wp.array[wp.vec3], + bodyid_in: wp.array[int], # Out: - jacp_out: wp.array3d(dtype=float), - jacr_out: wp.array3d(dtype=float), + jacp_out: wp.array3d[float], + jacr_out: wp.array3d[float], ): worldid, dofid = wp.tid() @@ -477,10 +473,10 @@ def _make_jac_kernel(has_jacp: bool, has_jacr: bool): def jac( m: Model, d: Data, - jacp: wp.array | None, # wp.array3d(dtype=float) - jacr: wp.array | None, # wp.array3d(dtype=float) - point: wp.array(dtype=wp.vec3), - body: wp.array(dtype=int), + jacp: wp.array | None, # wp.array3d[float] + jacr: wp.array | None, # wp.array3d[float] + point: wp.array[wp.vec3], + body: wp.array[int], ): """Compute translational and rotational Jacobian for point on body. @@ -508,17 +504,17 @@ def jac( @wp.func def jac_dot_dof( # Model: - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - jnt_type: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - dof_bodyid: wp.array(dtype=int), - dof_jntid: wp.array(dtype=int), + body_parentid: wp.array[int], + body_rootid: wp.array[int], + jnt_type: wp.array[int], + jnt_dofadr: wp.array[int], + dof_bodyid: wp.array[int], + dof_jntid: wp.array[int], # Data in: - subtree_com_in: wp.array2d(dtype=wp.vec3), - cdof_in: wp.array2d(dtype=wp.spatial_vector), - cvel_in: wp.array2d(dtype=wp.spatial_vector), - cdof_dot_in: wp.array2d(dtype=wp.spatial_vector), + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], + cvel_in: wp.array2d[wp.spatial_vector], + cdof_dot_in: wp.array2d[wp.spatial_vector], # In: point: wp.vec3, bodyid: int, @@ -573,7 +569,7 @@ def jac_dot_dof( return jacp, jacr -def get_state(m: Model, d: Data, state: wp.array2d(dtype=float), sig: int, active: Optional[wp.array] = None): +def get_state(m: Model, d: Data, state: wp.array2d[float], sig: int, active: Optional[wp.array] = None): """Copy concatenated state components specified by sig from Data into state. The bits of the integer sig correspond to element fields of State. @@ -599,22 +595,22 @@ def get_state(m: Model, d: Data, state: wp.array2d(dtype=float), sig: int, activ neq: int, nmocap: int, # Data in: - time_in: wp.array(dtype=float), - qpos_in: wp.array2d(dtype=float), - qvel_in: wp.array2d(dtype=float), - act_in: wp.array2d(dtype=float), - qacc_warmstart_in: wp.array2d(dtype=float), - ctrl_in: wp.array2d(dtype=float), - qfrc_applied_in: wp.array2d(dtype=float), - xfrc_applied_in: wp.array2d(dtype=wp.spatial_vector), - eq_active_in: wp.array2d(dtype=bool), - mocap_pos_in: wp.array2d(dtype=wp.vec3), - mocap_quat_in: wp.array2d(dtype=wp.quat), + time_in: wp.array[float], + qpos_in: wp.array2d[float], + qvel_in: wp.array2d[float], + act_in: wp.array2d[float], + qacc_warmstart_in: wp.array2d[float], + ctrl_in: wp.array2d[float], + qfrc_applied_in: wp.array2d[float], + xfrc_applied_in: wp.array2d[wp.spatial_vector], + eq_active_in: wp.array2d[bool], + mocap_pos_in: wp.array2d[wp.vec3], + mocap_quat_in: wp.array2d[wp.quat], # In: sig_in: int, - active_in: wp.array(dtype=bool), + active_in: wp.array[bool], # Out: - state_out: wp.array2d(dtype=float), + state_out: wp.array2d[float], ): worldid = wp.tid() @@ -712,7 +708,7 @@ def get_state(m: Model, d: Data, state: wp.array2d(dtype=float), sig: int, activ ) -def set_state(m: Model, d: Data, state: wp.array2d(dtype=float), sig: int, active: Optional[wp.array] = None): +def set_state(m: Model, d: Data, state: wp.array2d[float], sig: int, active: Optional[wp.array] = None): """Copy concatenated state components specified by sig from state into Data. The bits of the integer sig correspond to element fields of State. @@ -739,20 +735,20 @@ def set_state(m: Model, d: Data, state: wp.array2d(dtype=float), sig: int, activ nmocap: int, # In: sig_in: int, - active_in: wp.array(dtype=bool), - state_in: wp.array2d(dtype=float), + active_in: wp.array[bool], + state_in: wp.array2d[float], # Data out: - time_out: wp.array(dtype=float), - qpos_out: wp.array2d(dtype=float), - qvel_out: wp.array2d(dtype=float), - act_out: wp.array2d(dtype=float), - qacc_warmstart_out: wp.array2d(dtype=float), - ctrl_out: wp.array2d(dtype=float), - qfrc_applied_out: wp.array2d(dtype=float), - xfrc_applied_out: wp.array2d(dtype=wp.spatial_vector), - eq_active_out: wp.array2d(dtype=bool), - mocap_pos_out: wp.array2d(dtype=wp.vec3), - mocap_quat_out: wp.array2d(dtype=wp.quat), + time_out: wp.array[float], + qpos_out: wp.array2d[float], + qvel_out: wp.array2d[float], + act_out: wp.array2d[float], + qacc_warmstart_out: wp.array2d[float], + ctrl_out: wp.array2d[float], + qfrc_applied_out: wp.array2d[float], + xfrc_applied_out: wp.array2d[wp.spatial_vector], + eq_active_out: wp.array2d[bool], + mocap_pos_out: wp.array2d[wp.vec3], + mocap_quat_out: wp.array2d[wp.quat], ): worldid = wp.tid() diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py index 11b7a7c0..7a9dc2a5 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py @@ -17,6 +17,7 @@ import enum from typing import Callable import mujoco +import numpy as np import warp as wp MJ_MINVAL = mujoco.mjMINVAL @@ -793,9 +794,18 @@ class TileSet: size: size of all the tiles in this set """ - adr: wp.array(dtype=int) + adr: wp.array[int] size: int + def __eq__(self, other) -> bool: + if self.__class__ is not other.__class__: + return NotImplemented + return self.size == other.size and np.array_equal(np.asarray(self.adr.numpy()), np.asarray(other.adr.numpy())) + + def __hash__(self) -> int: + adr = np.asarray(self.adr.numpy()) + return hash((self.size, adr.dtype.str, adr.shape, adr.tobytes())) + @dataclasses.dataclass class Callback: @@ -933,7 +943,7 @@ class Model: geom_conaffinity: geom contact affinity (ngeom,) geom_condim: contact dimensionality (1, 3, 4, 6) (ngeom,) geom_bodyid: id of geom's body (ngeom,) - geom_dataid: id of geom's mesh/hfield; -1: none (ngeom,) + geom_dataid: id of geom's mesh/hfield; -1: none (*, ngeom) geom_matid: material id for rendering (*, ngeom,) geom_group: geom group inclusion/exclusion mask (ngeom,) geom_priority: geom contact priority (ngeom,) @@ -1322,7 +1332,7 @@ class Model: geom_conaffinity: array("ngeom", int) geom_condim: array("ngeom", int) geom_bodyid: array("ngeom", int) - geom_dataid: array("ngeom", int) + geom_dataid: array("*", "ngeom", int) geom_matid: array("*", "ngeom", int) geom_group: array("ngeom", int) geom_priority: array("ngeom", int) @@ -1527,70 +1537,70 @@ class Model: has_fluid: bool has_sdf_geom: bool block_dim: BlockDim - body_tree: tuple[wp.array(dtype=int), ...] - body_branches: wp.array(dtype=int) - body_branch_start: wp.array(dtype=int) + body_tree: tuple[wp.array[int], ...] + body_branches: wp.array[int] + body_branch_start: wp.array[int] mocap_bodyid: array("nmocap", int) body_fluid_ellipsoid: array("nbody", bool) - jnt_limited_slide_hinge_adr: wp.array(dtype=int) - jnt_limited_ball_adr: wp.array(dtype=int) - dof_tri_row: wp.array(dtype=int) - dof_tri_col: wp.array(dtype=int) - nxn_geom_pair: wp.array(dtype=wp.vec2i) - nxn_geom_pair_filtered: wp.array(dtype=wp.vec2i) - nxn_pairid: wp.array(dtype=wp.vec2i) - nxn_pairid_filtered: wp.array(dtype=wp.vec2i) + jnt_limited_slide_hinge_adr: wp.array[int] + jnt_limited_ball_adr: wp.array[int] + dof_tri_row: wp.array[int] + dof_tri_col: wp.array[int] + nxn_geom_pair: wp.array[wp.vec2i] + nxn_geom_pair_filtered: wp.array[wp.vec2i] + nxn_pairid: wp.array[wp.vec2i] + nxn_pairid_filtered: wp.array[wp.vec2i] geom_pair_type_count: tuple[int, ...] geom_plugin_index: array("ngeom", int) - eq_connect_adr: wp.array(dtype=int) - eq_wld_adr: wp.array(dtype=int) - eq_jnt_adr: wp.array(dtype=int) - eq_ten_adr: wp.array(dtype=int) - eq_flex_adr: wp.array(dtype=int) - tendon_jnt_adr: wp.array(dtype=int) - tendon_site_pair_adr: wp.array(dtype=int) - tendon_geom_adr: wp.array(dtype=int) - tendon_limited_adr: wp.array(dtype=int) + eq_connect_adr: wp.array[int] + eq_wld_adr: wp.array[int] + eq_jnt_adr: wp.array[int] + eq_ten_adr: wp.array[int] + eq_flex_adr: wp.array[int] + tendon_jnt_adr: wp.array[int] + tendon_site_pair_adr: wp.array[int] + tendon_geom_adr: wp.array[int] + tendon_limited_adr: wp.array[int] max_ten_J_rownnz: int - ten_wrapadr_site: wp.array(dtype=int) - ten_wrapnum_site: wp.array(dtype=int) - wrap_jnt_adr: wp.array(dtype=int) - wrap_site_adr: wp.array(dtype=int) - wrap_site_pair_adr: wp.array(dtype=int) - wrap_geom_adr: wp.array(dtype=int) + ten_wrapadr_site: wp.array[int] + ten_wrapnum_site: wp.array[int] + wrap_jnt_adr: wp.array[int] + wrap_site_adr: wp.array[int] + wrap_site_pair_adr: wp.array[int] + wrap_geom_adr: wp.array[int] wrap_pulley_scale: array("nwrap", float) - actuator_trntype_body_adr: wp.array(dtype=int) - sensor_pos_adr: wp.array(dtype=int) - sensor_limitpos_adr: wp.array(dtype=int) - sensor_vel_adr: wp.array(dtype=int) - sensor_limitvel_adr: wp.array(dtype=int) - sensor_acc_adr: wp.array(dtype=int) - sensor_rangefinder_adr: wp.array(dtype=int) - rangefinder_sensor_adr: wp.array(dtype=int) - sensor_collision_start_adr: wp.array(dtype=int) + actuator_trntype_body_adr: wp.array[int] + sensor_pos_adr: wp.array[int] + sensor_limitpos_adr: wp.array[int] + sensor_vel_adr: wp.array[int] + sensor_limitvel_adr: wp.array[int] + sensor_acc_adr: wp.array[int] + sensor_rangefinder_adr: wp.array[int] + rangefinder_sensor_adr: wp.array[int] + sensor_collision_start_adr: wp.array[int] collision_sensor_adr: array("nsensor", int) - sensor_touch_adr: wp.array(dtype=int) - sensor_limitfrc_adr: wp.array(dtype=int) + sensor_touch_adr: wp.array[int] + sensor_limitfrc_adr: wp.array[int] sensor_e_potential: bool sensor_e_kinetic: bool - sensor_tendonactfrc_adr: wp.array(dtype=int) + sensor_tendonactfrc_adr: wp.array[int] sensor_subtree_vel: bool sensor_contact_adr: array("nsensorcontact", int) sensor_adr_to_contact_adr: array("nsensor", int) sensor_rne_postconstraint: bool sensor_rangefinder_bodyid: array("nrangefinder", int) taxel_vertadr: array("nsensortaxel", int) - taxel_sensorid: wp.array(dtype=int) + taxel_sensorid: wp.array[int] qM_tiles: tuple[TileSet, ...] - qLD_updates: tuple[wp.array(dtype=wp.vec3i), ...] - qLD_all_updates: wp.array(dtype=wp.vec3i) - qLD_level_offsets: wp.array(dtype=int) - qM_fullm_i: wp.array(dtype=int) - qM_fullm_j: wp.array(dtype=int) + qLD_updates: tuple[wp.array[wp.vec3i], ...] + qLD_all_updates: wp.array[wp.vec3i] + qLD_level_offsets: wp.array[int] + qM_fullm_i: wp.array[int] + qM_fullm_j: wp.array[int] # Gather-based sparse mul_m indices (thread per DOF, no atomics) - qM_mulm_rowadr: wp.array(dtype=int) # start address for each row [nv+1] - qM_mulm_col: wp.array(dtype=int) # column index to gather from - qM_mulm_madr: wp.array(dtype=int) # matrix address to read + qM_mulm_rowadr: wp.array[int] # start address for each row [nv+1] + qM_mulm_col: wp.array[int] # column index to gather from + qM_mulm_madr: wp.array[int] # matrix address to read class ContactType(enum.IntFlag): @@ -1674,10 +1684,10 @@ class Constraint: type: array("nworld", "njmax", int) id: array("nworld", "njmax", int) - J_rownnz: wp.array2d(dtype=int) - J_rowadr: wp.array2d(dtype=int) - J_colind: wp.array3d(dtype=int) - J: wp.array3d(dtype=float) + J_rownnz: wp.array2d[int] + J_rowadr: wp.array2d[int] + J_colind: wp.array3d[int] + J: wp.array3d[float] pos: array("nworld", "njmax", float) margin: array("nworld", "njmax", float) D: array("nworld", "njmax_pad", float) @@ -1846,8 +1856,8 @@ class Data: moment_colind: array("nworld", "nJmom", int) actuator_moment: array("nworld", "nJmom", float) crb: array("nworld", "nbody", vec10) - qM: wp.array3d(dtype=float) - qLD: wp.array3d(dtype=float) + qM: wp.array3d[float] + qLD: wp.array3d[float] qLDiagInv: array("nworld", "nv", float) flexedge_velocity: array("nworld", "nflexedge", float) ten_velocity: array("nworld", "ntendon", float) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/pyproject.toml b/mjx/mujoco/mjx/third_party/mujoco_warp/pyproject.toml index e8d98eb4..f1c4bc4a 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/pyproject.toml +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/pyproject.toml @@ -57,6 +57,7 @@ dev = [ "lsprotocol>=2023.0.1,<2024.0.0", "mujoco>=3.6.0.dev0", "warp-lang>=1.11.0.dev0", + "mjviser>=0.0.10", ] # TODO(team): cpu and cuda JAX optional dependencies are temporary, remove after we land MJX:Warp cpu = [ diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/viewer.py b/mjx/mujoco/mjx/third_party/mujoco_warp/viewer.py index cf65f02a..8a825da2 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/viewer.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/viewer.py @@ -1,4 +1,4 @@ -# Copyright 2025 The Newton Developers +# Copyright 2026 The Newton Developers # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -62,6 +62,7 @@ _OVERRIDE = flags.DEFINE_multi_string("override", [], "Model overrides (notation _KEYFRAME = flags.DEFINE_integer("keyframe", 0, "keyframe to initialize simulation.") _DEVICE = flags.DEFINE_string("device", None, "override the default Warp device") _REPLAY = flags.DEFINE_string("replay", None, "keyframe sequence to replay, keyframe name must prefix match") +_VIEWER = flags.DEFINE_enum("viewer", "mujoco", ["mujoco", "viser"], "Viewer backend (mujoco native or mjviser web)") _VIEWER_GLOBAL_STATE = {"running": True, "step_once": False} @@ -105,6 +106,70 @@ def _compile_step(m, d): return capture.graph +def _make_warp_step_fn(mjm, m, d, graph, ctrls=None): + ctrlid = 0 + opt = copy.copy(mjm.opt) + + def step_fn(mjm, mjd): + nonlocal ctrlid, opt, m, graph + if ctrls is not None and ctrlid < len(ctrls): + mjd.ctrl[:] = ctrls[ctrlid] + ctrlid += 1 + if mjm.opt != opt: + opt = copy.copy(mjm.opt) + m = mjw.put_model(mjm) + graph = _compile_step(m, d) if wp.get_device().is_cuda else None + wp.copy(d.ctrl, wp.array([mjd.ctrl.astype(np.float32)])) + wp.copy(d.act, wp.array([mjd.act.astype(np.float32)])) + wp.copy(d.xfrc_applied, wp.array([mjd.xfrc_applied.astype(np.float32)])) + wp.copy(d.qpos, wp.array([mjd.qpos.astype(np.float32)])) + wp.copy(d.qvel, wp.array([mjd.qvel.astype(np.float32)])) + wp.copy(d.time, wp.array([mjd.time], dtype=wp.float32)) + if graph is None: + mjw.step(m, d) + else: + wp.capture_launch(graph) + wp.synchronize() + mjw.get_data_into(mjd, mjm, d) + + return step_fn + + +def _make_c_step_fn(ctrls=None): + if ctrls is None: + return mujoco.mj_step + + ctrlid = 0 + + def step_fn(mjm, mjd): + nonlocal ctrlid + if ctrlid < len(ctrls): + mjd.ctrl[:] = ctrls[ctrlid] + ctrlid += 1 + mujoco.mj_step(mjm, mjd) + + return step_fn + + +def _run_viser_viewer(mjm, mjd, step_fn): + from mjviser import Viewer as MjViserViewer + + MjViserViewer(mjm, mjd, step_fn=step_fn).run() + + +def _run_passive_viewer(mjm, mjd, step_fn): + with mujoco.viewer.launch_passive(mjm, mjd, key_callback=key_callback) as viewer: + while True: + start = time.time() + if _VIEWER_GLOBAL_STATE["running"] or _VIEWER_GLOBAL_STATE["step_once"]: + _VIEWER_GLOBAL_STATE["step_once"] = False + step_fn(mjm, mjd) + viewer.sync() + elapsed = time.time() - start + if elapsed < mjm.opt.timestep: + time.sleep(mjm.opt.timestep - elapsed) + + def _main(argv: Sequence[str]) -> None: """Runs viewer app.""" if len(argv) < 2: @@ -115,7 +180,6 @@ def _main(argv: Sequence[str]) -> None: mjm = _load_model(epath.Path(argv[1])) mjd = mujoco.MjData(mjm) ctrls = None - ctrlid = 0 if _REPLAY.value: keys = find_keys(mjm, _REPLAY.value) if not keys: @@ -169,45 +233,15 @@ def _main(argv: Sequence[str]) -> None: print(f"Data\n nworld: {d.nworld} nconmax: {int(d.naconmax / d.nworld)} njmax: {d.njmax}\n") print(f"MuJoCo Warp simulating with dt = {m.opt.timestep.numpy()[0]:.3f}...") - with mujoco.viewer.launch_passive(mjm, mjd, key_callback=key_callback) as viewer: - opt = copy.copy(mjm.opt) + if _ENGINE.value == EngineOptions.WARP: + step_fn = _make_warp_step_fn(mjm, m, d, graph, ctrls) + else: + step_fn = _make_c_step_fn(ctrls) - while True: - start = time.time() - - if ctrls is not None and ctrlid < len(ctrls): - mjd.ctrl[:] = ctrls[ctrlid] - ctrlid += 1 - - if _ENGINE.value == EngineOptions.C: - mujoco.mj_step(mjm, mjd) - else: # mjwarp - wp.copy(d.ctrl, wp.array([mjd.ctrl.astype(np.float32)])) - wp.copy(d.act, wp.array([mjd.act.astype(np.float32)])) - wp.copy(d.xfrc_applied, wp.array([mjd.xfrc_applied.astype(np.float32)])) - wp.copy(d.qpos, wp.array([mjd.qpos.astype(np.float32)])) - wp.copy(d.qvel, wp.array([mjd.qvel.astype(np.float32)])) - wp.copy(d.time, wp.array([mjd.time], dtype=wp.float32)) - # if the user changed an option in the MuJoCo Simulate UI, go ahead and recompile the step - # TODO: update memory tied to option max iterations - if mjm.opt != opt: - opt = copy.copy(mjm.opt) - m = mjw.put_model(mjm) - graph = _compile_step(m, d) if wp.get_device().is_cuda else None - if _VIEWER_GLOBAL_STATE["running"] or _VIEWER_GLOBAL_STATE["step_once"]: - _VIEWER_GLOBAL_STATE["step_once"] = False - if graph is None: - mjw.step(m, d) - else: - wp.capture_launch(graph) - wp.synchronize() - mjw.get_data_into(mjd, mjm, d) - - viewer.sync() - - elapsed = time.time() - start - if elapsed < mjm.opt.timestep: - time.sleep(mjm.opt.timestep - elapsed) + if _VIEWER.value == "viser": + _run_viser_viewer(mjm, mjd, step_fn) + else: + _run_passive_viewer(mjm, mjd, step_fn) def main(): diff --git a/mjx/mujoco/mjx/warp/bvh.py b/mjx/mujoco/mjx/warp/bvh.py index 157315db..edde9732 100644 --- a/mjx/mujoco/mjx/warp/bvh.py +++ b/mjx/mujoco/mjx/warp/bvh.py @@ -48,34 +48,35 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) + @ffi.format_args_for_warp def _refit_bvh_shim( # Model nworld: int, - flex_dim: wp.array(dtype=int), - flex_edge: wp.array(dtype=wp.vec2i), - flex_elem: wp.array(dtype=int), - flex_elemadr: wp.array(dtype=int), - flex_elemdataadr: wp.array(dtype=int), - flex_elemnum: wp.array(dtype=int), - flex_radius: wp.array(dtype=float), - flex_shell: wp.array(dtype=int), - flex_shelldataadr: wp.array(dtype=int), - flex_vertadr: wp.array(dtype=int), - flex_vertnum: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - geom_type: wp.array(dtype=int), + flex_dim: wp.array[int], + flex_edge: wp.array[wp.vec2i], + flex_elem: wp.array[int], + flex_elemadr: wp.array[int], + flex_elemdataadr: wp.array[int], + flex_elemnum: wp.array[int], + flex_radius: wp.array[float], + flex_shell: wp.array[int], + flex_shelldataadr: wp.array[int], + flex_vertadr: wp.array[int], + flex_vertnum: wp.array[int], + geom_dataid: wp.array2d[int], + geom_size: wp.array2d[wp.vec3], + geom_type: wp.array[int], nflex: int, nflexelem: int, # Data - flexvert_xpos: wp.array2d(dtype=wp.vec3), - geom_xmat: wp.array2d(dtype=wp.mat33), - geom_xpos: wp.array2d(dtype=wp.vec3), + flexvert_xpos: wp.array2d[wp.vec3], + geom_xmat: wp.array2d[wp.mat33], + geom_xpos: wp.array2d[wp.vec3], # Registry rc_id: int, # Dummy output - dummy: wp.array(dtype=int), + dummy: wp.array[int], ): _m.stat = _s _m.opt = _o @@ -135,7 +136,7 @@ def _refit_bvh_jax_impl( m._impl.flex_shelldataadr, m.flex_vertadr, m.flex_vertnum, - m.geom_dataid, + jax.numpy.expand_dims(m.geom_dataid, 0), m.geom_size, m.geom_type, m.nflex, diff --git a/mjx/mujoco/mjx/warp/collision_driver.py b/mjx/mujoco/mjx/warp/collision_driver.py index b0330c4b..530de314 100644 --- a/mjx/mujoco/mjx/warp/collision_driver.py +++ b/mjx/mujoco/mjx/warp/collision_driver.py @@ -46,67 +46,68 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) + @ffi.format_args_for_warp def _collision_shim( # Model nworld: int, block_dim: mjwp_types.BlockDim, - flex_conaffinity: wp.array(dtype=int), - flex_condim: wp.array(dtype=int), - flex_contype: wp.array(dtype=int), - flex_dim: wp.array(dtype=int), - flex_elem: wp.array(dtype=int), - flex_elemadr: wp.array(dtype=int), - flex_elemdataadr: wp.array(dtype=int), - flex_elemnum: wp.array(dtype=int), - flex_friction: wp.array(dtype=wp.vec3), - flex_margin: wp.array(dtype=float), - flex_radius: wp.array(dtype=float), - flex_shell: wp.array(dtype=int), - flex_shelldataadr: wp.array(dtype=int), - flex_shellnum: wp.array(dtype=int), - flex_vertadr: wp.array(dtype=int), - flex_vertflexid: wp.array(dtype=int), - geom_aabb: wp.array3d(dtype=wp.vec3), - geom_conaffinity: wp.array(dtype=int), - geom_condim: wp.array(dtype=int), - geom_contype: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_friction: wp.array2d(dtype=wp.vec3), - geom_gap: wp.array2d(dtype=float), - geom_margin: wp.array2d(dtype=float), + flex_conaffinity: wp.array[int], + flex_condim: wp.array[int], + flex_contype: wp.array[int], + flex_dim: wp.array[int], + flex_elem: wp.array[int], + flex_elemadr: wp.array[int], + flex_elemdataadr: wp.array[int], + flex_elemnum: wp.array[int], + flex_friction: wp.array[wp.vec3], + flex_margin: wp.array[float], + flex_radius: wp.array[float], + flex_shell: wp.array[int], + flex_shelldataadr: wp.array[int], + flex_shellnum: wp.array[int], + flex_vertadr: wp.array[int], + flex_vertflexid: wp.array[int], + geom_aabb: wp.array3d[wp.vec3], + geom_conaffinity: wp.array[int], + geom_condim: wp.array[int], + geom_contype: wp.array[int], + geom_dataid: wp.array2d[int], + geom_friction: wp.array2d[wp.vec3], + geom_gap: wp.array2d[float], + geom_margin: wp.array2d[float], geom_pair_type_count: tuple[int, ...], - geom_plugin_index: wp.array(dtype=int), - geom_priority: wp.array(dtype=int), - geom_rbound: wp.array2d(dtype=float), - geom_size: wp.array2d(dtype=wp.vec3), - geom_solimp: wp.array2d(dtype=mjwp_types.vec5), - geom_solmix: wp.array2d(dtype=float), - geom_solref: wp.array2d(dtype=wp.vec2), - geom_type: wp.array(dtype=int), + geom_plugin_index: wp.array[int], + geom_priority: wp.array[int], + geom_rbound: wp.array2d[float], + geom_size: wp.array2d[wp.vec3], + geom_solimp: wp.array2d[mjwp_types.vec5], + geom_solmix: wp.array2d[float], + geom_solref: wp.array2d[wp.vec2], + geom_type: wp.array[int], has_sdf_geom: bool, - hfield_adr: wp.array(dtype=int), - hfield_data: wp.array(dtype=float), - hfield_ncol: wp.array(dtype=int), - hfield_nrow: wp.array(dtype=int), - hfield_size: wp.array(dtype=wp.vec4), - mesh_face: wp.array(dtype=wp.vec3i), - mesh_faceadr: wp.array(dtype=int), - mesh_graph: wp.array(dtype=int), - mesh_graphadr: wp.array(dtype=int), - mesh_octadr: wp.array(dtype=int), - mesh_polyadr: wp.array(dtype=int), - mesh_polymap: wp.array(dtype=int), - mesh_polymapadr: wp.array(dtype=int), - mesh_polymapnum: wp.array(dtype=int), - mesh_polynormal: wp.array(dtype=wp.vec3), - mesh_polynum: wp.array(dtype=int), - mesh_polyvert: wp.array(dtype=int), - mesh_polyvertadr: wp.array(dtype=int), - mesh_polyvertnum: wp.array(dtype=int), - mesh_vert: wp.array(dtype=wp.vec3), - mesh_vertadr: wp.array(dtype=int), - mesh_vertnum: wp.array(dtype=int), + hfield_adr: wp.array[int], + hfield_data: wp.array[float], + hfield_ncol: wp.array[int], + hfield_nrow: wp.array[int], + hfield_size: wp.array[wp.vec4], + mesh_face: wp.array[wp.vec3i], + mesh_faceadr: wp.array[int], + mesh_graph: wp.array[int], + mesh_graphadr: wp.array[int], + mesh_octadr: wp.array[int], + mesh_polyadr: wp.array[int], + mesh_polymap: wp.array[int], + mesh_polymapadr: wp.array[int], + mesh_polymapnum: wp.array[int], + mesh_polynormal: wp.array[wp.vec3], + mesh_polynum: wp.array[int], + mesh_polyvert: wp.array[int], + mesh_polyvertadr: wp.array[int], + mesh_polyvertnum: wp.array[int], + mesh_vert: wp.array[wp.vec3], + mesh_vertadr: wp.array[int], + mesh_vertnum: wp.array[int], nflex: int, nflexelem: int, nflexshelldata: int, @@ -115,25 +116,25 @@ def _collision_shim( nmaxmeshdeg: int, nmaxpolygon: int, nmeshface: int, - nxn_geom_pair_filtered: wp.array(dtype=wp.vec2i), - nxn_pairid: wp.array(dtype=wp.vec2i), - nxn_pairid_filtered: wp.array(dtype=wp.vec2i), - oct_aabb: wp.array2d(dtype=wp.vec3), - oct_child: wp.array(dtype=mjwp_types.vec8i), - oct_coeff: wp.array(dtype=mjwp_types.vec8), - pair_dim: wp.array(dtype=int), - pair_friction: wp.array2d(dtype=mjwp_types.vec5), - pair_gap: wp.array2d(dtype=float), - pair_margin: wp.array2d(dtype=float), - pair_solimp: wp.array2d(dtype=mjwp_types.vec5), - pair_solref: wp.array2d(dtype=wp.vec2), - pair_solreffriction: wp.array2d(dtype=wp.vec2), - plugin: wp.array(dtype=int), - plugin_attr: wp.array(dtype=mjwp_types.vec_pluginattr), + nxn_geom_pair_filtered: wp.array[wp.vec2i], + nxn_pairid: wp.array[wp.vec2i], + nxn_pairid_filtered: wp.array[wp.vec2i], + oct_aabb: wp.array2d[wp.vec3], + oct_child: wp.array[mjwp_types.vec8i], + oct_coeff: wp.array[mjwp_types.vec8], + pair_dim: wp.array[int], + pair_friction: wp.array2d[mjwp_types.vec5], + pair_gap: wp.array2d[float], + pair_margin: wp.array2d[float], + pair_solimp: wp.array2d[mjwp_types.vec5], + pair_solref: wp.array2d[wp.vec2], + pair_solreffriction: wp.array2d[wp.vec2], + plugin: wp.array[int], + plugin_attr: wp.array[mjwp_types.vec_pluginattr], opt__broadphase: int, opt__broadphase_filter: int, opt__ccd_iterations: int, - opt__ccd_tolerance: wp.array(dtype=float), + opt__ccd_tolerance: wp.array[float], opt__disableflags: int, opt__enableflags: int, opt__sdf_initpoints: int, @@ -141,27 +142,27 @@ def _collision_shim( # Data naccdmax: int, naconmax: int, - flexvert_xpos: wp.array2d(dtype=wp.vec3), - geom_xmat: wp.array2d(dtype=wp.mat33), - geom_xpos: wp.array2d(dtype=wp.vec3), - nacon: wp.array(dtype=int), - ncollision: wp.array(dtype=int), - contact__dim: wp.array(dtype=int), - contact__dist: wp.array(dtype=float), - contact__efc_address: wp.array2d(dtype=int), - contact__flex: wp.array(dtype=wp.vec2i), - contact__frame: wp.array(dtype=wp.mat33), - contact__friction: wp.array(dtype=mjwp_types.vec5), - contact__geom: wp.array(dtype=wp.vec2i), - contact__geomcollisionid: wp.array(dtype=int), - contact__includemargin: wp.array(dtype=float), - contact__pos: wp.array(dtype=wp.vec3), - contact__solimp: wp.array(dtype=mjwp_types.vec5), - contact__solref: wp.array(dtype=wp.vec2), - contact__solreffriction: wp.array(dtype=wp.vec2), - contact__type: wp.array(dtype=int), - contact__vert: wp.array(dtype=wp.vec2i), - contact__worldid: wp.array(dtype=int), + flexvert_xpos: wp.array2d[wp.vec3], + geom_xmat: wp.array2d[wp.mat33], + geom_xpos: wp.array2d[wp.vec3], + nacon: wp.array[int], + ncollision: wp.array[int], + contact__dim: wp.array[int], + contact__dist: wp.array[float], + contact__efc_address: wp.array2d[int], + contact__flex: wp.array[wp.vec2i], + contact__frame: wp.array[wp.mat33], + contact__friction: wp.array[mjwp_types.vec5], + contact__geom: wp.array[wp.vec2i], + contact__geomcollisionid: wp.array[int], + contact__includemargin: wp.array[float], + contact__pos: wp.array[wp.vec3], + contact__solimp: wp.array[mjwp_types.vec5], + contact__solref: wp.array[wp.vec2], + contact__solreffriction: wp.array[wp.vec2], + contact__type: wp.array[int], + contact__vert: wp.array[wp.vec2i], + contact__worldid: wp.array[int], ): _m.stat = _s _m.opt = _o @@ -376,7 +377,7 @@ def _collision_jax_impl(m: types.Model, d: types.Data): m.geom_conaffinity, m.geom_condim, m.geom_contype, - m.geom_dataid, + jax.numpy.expand_dims(m.geom_dataid, 0), m.geom_friction, m.geom_gap, m.geom_margin, diff --git a/mjx/mujoco/mjx/warp/forward.py b/mjx/mujoco/mjx/warp/forward.py index 3d12681a..98459fbb 100644 --- a/mjx/mujoco/mjx/warp/forward.py +++ b/mjx/mujoco/mjx/warp/forward.py @@ -46,210 +46,211 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) + @ffi.format_args_for_warp def _forward_shim( # Model nworld: int, - M_rowadr: wp.array(dtype=int), - M_rownnz: wp.array(dtype=int), - actuator_acc0: wp.array2d(dtype=float), - actuator_actadr: wp.array(dtype=int), - actuator_actearly: wp.array(dtype=bool), - actuator_actlimited: wp.array(dtype=bool), - actuator_actnum: wp.array(dtype=int), - actuator_actrange: wp.array2d(dtype=wp.vec2), - actuator_biasprm: wp.array2d(dtype=mjwp_types.vec10f), - actuator_biastype: wp.array(dtype=int), - actuator_cranklength: wp.array2d(dtype=float), - actuator_ctrllimited: wp.array(dtype=bool), - actuator_ctrlrange: wp.array2d(dtype=wp.vec2), - actuator_dynprm: wp.array2d(dtype=mjwp_types.vec10f), - actuator_dyntype: wp.array(dtype=int), - actuator_forcelimited: wp.array(dtype=bool), - actuator_forcerange: wp.array2d(dtype=wp.vec2), - actuator_gainprm: wp.array2d(dtype=mjwp_types.vec10f), - actuator_gaintype: wp.array(dtype=int), - actuator_gear: wp.array2d(dtype=wp.spatial_vector), - actuator_lengthrange: wp.array2d(dtype=wp.vec2), - actuator_trnid: wp.array(dtype=wp.vec2i), - actuator_trntype: wp.array(dtype=int), - actuator_trntype_body_adr: wp.array(dtype=int), + M_rowadr: wp.array[int], + M_rownnz: wp.array[int], + actuator_acc0: wp.array2d[float], + actuator_actadr: wp.array[int], + actuator_actearly: wp.array[bool], + actuator_actlimited: wp.array[bool], + actuator_actnum: wp.array[int], + actuator_actrange: wp.array2d[wp.vec2], + actuator_biasprm: wp.array2d[mjwp_types.vec10f], + actuator_biastype: wp.array[int], + actuator_cranklength: wp.array2d[float], + actuator_ctrllimited: wp.array[bool], + actuator_ctrlrange: wp.array2d[wp.vec2], + actuator_dynprm: wp.array2d[mjwp_types.vec10f], + actuator_dyntype: wp.array[int], + actuator_forcelimited: wp.array[bool], + actuator_forcerange: wp.array2d[wp.vec2], + actuator_gainprm: wp.array2d[mjwp_types.vec10f], + actuator_gaintype: wp.array[int], + actuator_gear: wp.array2d[wp.spatial_vector], + actuator_lengthrange: wp.array2d[wp.vec2], + actuator_trnid: wp.array[wp.vec2i], + actuator_trntype: wp.array[int], + actuator_trntype_body_adr: wp.array[int], block_dim: mjwp_types.BlockDim, - body_branch_start: wp.array(dtype=int), - body_branches: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_fluid_ellipsoid: wp.array(dtype=bool), - body_geomadr: wp.array(dtype=int), - body_geomnum: wp.array(dtype=int), - body_gravcomp: wp.array2d(dtype=float), - body_inertia: wp.array2d(dtype=wp.vec3), - body_invweight0: wp.array2d(dtype=wp.vec2), - body_ipos: wp.array2d(dtype=wp.vec3), - body_iquat: wp.array2d(dtype=wp.quat), - body_jntadr: wp.array(dtype=int), - body_jntnum: wp.array(dtype=int), - body_mass: wp.array2d(dtype=float), - body_mocapid: wp.array(dtype=int), - body_parentid: wp.array(dtype=int), - body_pos: wp.array2d(dtype=wp.vec3), - body_quat: wp.array2d(dtype=wp.quat), - body_rootid: wp.array(dtype=int), - body_subtreemass: wp.array2d(dtype=float), - body_tree: tuple[wp.array(dtype=int), ...], - body_treeid: wp.array(dtype=int), - body_weldid: wp.array(dtype=int), - cam_bodyid: wp.array(dtype=int), - cam_fovy: wp.array2d(dtype=float), - cam_intrinsic: wp.array2d(dtype=wp.vec4), - cam_mat0: wp.array2d(dtype=wp.mat33), - cam_mode: wp.array(dtype=int), - cam_pos: wp.array2d(dtype=wp.vec3), - cam_pos0: wp.array2d(dtype=wp.vec3), - cam_poscom0: wp.array2d(dtype=wp.vec3), - cam_quat: wp.array2d(dtype=wp.quat), - cam_resolution: wp.array(dtype=wp.vec2i), - cam_sensorsize: wp.array(dtype=wp.vec2), - cam_targetbodyid: wp.array(dtype=int), - dof_Madr: wp.array(dtype=int), - dof_armature: wp.array2d(dtype=float), - dof_bodyid: wp.array(dtype=int), - dof_damping: wp.array2d(dtype=float), - dof_frictionloss: wp.array2d(dtype=float), - dof_invweight0: wp.array2d(dtype=float), - dof_jntid: wp.array(dtype=int), - dof_parentid: wp.array(dtype=int), - dof_solimp: wp.array2d(dtype=mjwp_types.vec5), - dof_solref: wp.array2d(dtype=wp.vec2), - dof_treeid: wp.array(dtype=int), - dof_tri_col: wp.array(dtype=int), - dof_tri_row: wp.array(dtype=int), - eq_connect_adr: wp.array(dtype=int), - eq_data: wp.array2d(dtype=mjwp_types.vec11), - eq_flex_adr: wp.array(dtype=int), - eq_jnt_adr: wp.array(dtype=int), - eq_obj1id: wp.array(dtype=int), - eq_obj2id: wp.array(dtype=int), - eq_objtype: wp.array(dtype=int), - eq_solimp: wp.array2d(dtype=mjwp_types.vec5), - eq_solref: wp.array2d(dtype=wp.vec2), - eq_ten_adr: wp.array(dtype=int), - eq_type: wp.array(dtype=int), - eq_wld_adr: wp.array(dtype=int), - flex_bending: wp.array2d(dtype=float), - flex_centered: wp.array(dtype=bool), - flex_conaffinity: wp.array(dtype=int), - flex_condim: wp.array(dtype=int), - flex_contype: wp.array(dtype=int), - flex_damping: wp.array(dtype=float), - flex_dim: wp.array(dtype=int), - flex_edge: wp.array(dtype=wp.vec2i), - flex_edgeadr: wp.array(dtype=int), - flex_edgeflap: wp.array(dtype=wp.vec2i), - flex_edgenum: wp.array(dtype=int), - flex_elem: wp.array(dtype=int), - flex_elemadr: wp.array(dtype=int), - flex_elemdataadr: wp.array(dtype=int), - flex_elemedge: wp.array(dtype=int), - flex_elemedgeadr: wp.array(dtype=int), - flex_elemnum: wp.array(dtype=int), - flex_friction: wp.array(dtype=wp.vec3), - flex_margin: wp.array(dtype=float), - flex_radius: wp.array(dtype=float), - flex_shell: wp.array(dtype=int), - flex_shelldataadr: wp.array(dtype=int), - flex_shellnum: wp.array(dtype=int), - flex_stiffness: wp.array2d(dtype=float), - flex_vert: wp.array(dtype=wp.vec3), - flex_vertadr: wp.array(dtype=int), - flex_vertbodyid: wp.array(dtype=int), - flex_vertflexid: wp.array(dtype=int), - flex_vertnum: wp.array(dtype=int), - flexedge_J_colind: wp.array(dtype=int), - flexedge_J_rowadr: wp.array(dtype=int), - flexedge_J_rownnz: wp.array(dtype=int), - flexedge_invweight0: wp.array(dtype=float), - flexedge_length0: wp.array(dtype=float), - geom_aabb: wp.array3d(dtype=wp.vec3), - geom_bodyid: wp.array(dtype=int), - geom_conaffinity: wp.array(dtype=int), - geom_condim: wp.array(dtype=int), - geom_contype: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_fluid: wp.array2d(dtype=float), - geom_friction: wp.array2d(dtype=wp.vec3), - geom_gap: wp.array2d(dtype=float), - geom_group: wp.array(dtype=int), - geom_margin: wp.array2d(dtype=float), - geom_matid: wp.array2d(dtype=int), + body_branch_start: wp.array[int], + body_branches: wp.array[int], + body_dofadr: wp.array[int], + body_dofnum: wp.array[int], + body_fluid_ellipsoid: wp.array[bool], + body_geomadr: wp.array[int], + body_geomnum: wp.array[int], + body_gravcomp: wp.array2d[float], + body_inertia: wp.array2d[wp.vec3], + body_invweight0: wp.array2d[wp.vec2], + body_ipos: wp.array2d[wp.vec3], + body_iquat: wp.array2d[wp.quat], + body_jntadr: wp.array[int], + body_jntnum: wp.array[int], + body_mass: wp.array2d[float], + body_mocapid: wp.array[int], + body_parentid: wp.array[int], + body_pos: wp.array2d[wp.vec3], + body_quat: wp.array2d[wp.quat], + body_rootid: wp.array[int], + body_subtreemass: wp.array2d[float], + body_tree: tuple[wp.array[int], ...], + body_treeid: wp.array[int], + body_weldid: wp.array[int], + cam_bodyid: wp.array[int], + cam_fovy: wp.array2d[float], + cam_intrinsic: wp.array2d[wp.vec4], + cam_mat0: wp.array2d[wp.mat33], + cam_mode: wp.array[int], + cam_pos: wp.array2d[wp.vec3], + cam_pos0: wp.array2d[wp.vec3], + cam_poscom0: wp.array2d[wp.vec3], + cam_quat: wp.array2d[wp.quat], + cam_resolution: wp.array[wp.vec2i], + cam_sensorsize: wp.array[wp.vec2], + cam_targetbodyid: wp.array[int], + dof_Madr: wp.array[int], + dof_armature: wp.array2d[float], + dof_bodyid: wp.array[int], + dof_damping: wp.array2d[float], + dof_frictionloss: wp.array2d[float], + dof_invweight0: wp.array2d[float], + dof_jntid: wp.array[int], + dof_parentid: wp.array[int], + dof_solimp: wp.array2d[mjwp_types.vec5], + dof_solref: wp.array2d[wp.vec2], + dof_treeid: wp.array[int], + dof_tri_col: wp.array[int], + dof_tri_row: wp.array[int], + eq_connect_adr: wp.array[int], + eq_data: wp.array2d[mjwp_types.vec11], + eq_flex_adr: wp.array[int], + eq_jnt_adr: wp.array[int], + eq_obj1id: wp.array[int], + eq_obj2id: wp.array[int], + eq_objtype: wp.array[int], + eq_solimp: wp.array2d[mjwp_types.vec5], + eq_solref: wp.array2d[wp.vec2], + eq_ten_adr: wp.array[int], + eq_type: wp.array[int], + eq_wld_adr: wp.array[int], + flex_bending: wp.array2d[float], + flex_centered: wp.array[bool], + flex_conaffinity: wp.array[int], + flex_condim: wp.array[int], + flex_contype: wp.array[int], + flex_damping: wp.array[float], + flex_dim: wp.array[int], + flex_edge: wp.array[wp.vec2i], + flex_edgeadr: wp.array[int], + flex_edgeflap: wp.array[wp.vec2i], + flex_edgenum: wp.array[int], + flex_elem: wp.array[int], + flex_elemadr: wp.array[int], + flex_elemdataadr: wp.array[int], + flex_elemedge: wp.array[int], + flex_elemedgeadr: wp.array[int], + flex_elemnum: wp.array[int], + flex_friction: wp.array[wp.vec3], + flex_margin: wp.array[float], + flex_radius: wp.array[float], + flex_shell: wp.array[int], + flex_shelldataadr: wp.array[int], + flex_shellnum: wp.array[int], + flex_stiffness: wp.array2d[float], + flex_vert: wp.array[wp.vec3], + flex_vertadr: wp.array[int], + flex_vertbodyid: wp.array[int], + flex_vertflexid: wp.array[int], + flex_vertnum: wp.array[int], + flexedge_J_colind: wp.array[int], + flexedge_J_rowadr: wp.array[int], + flexedge_J_rownnz: wp.array[int], + flexedge_invweight0: wp.array[float], + flexedge_length0: wp.array[float], + geom_aabb: wp.array3d[wp.vec3], + geom_bodyid: wp.array[int], + geom_conaffinity: wp.array[int], + geom_condim: wp.array[int], + geom_contype: wp.array[int], + geom_dataid: wp.array2d[int], + geom_fluid: wp.array2d[float], + geom_friction: wp.array2d[wp.vec3], + geom_gap: wp.array2d[float], + geom_group: wp.array[int], + geom_margin: wp.array2d[float], + geom_matid: wp.array2d[int], geom_pair_type_count: tuple[int, ...], - geom_plugin_index: wp.array(dtype=int), - geom_pos: wp.array2d(dtype=wp.vec3), - geom_priority: wp.array(dtype=int), - geom_quat: wp.array2d(dtype=wp.quat), - geom_rbound: wp.array2d(dtype=float), - geom_rgba: wp.array2d(dtype=wp.vec4), - geom_size: wp.array2d(dtype=wp.vec3), - geom_solimp: wp.array2d(dtype=mjwp_types.vec5), - geom_solmix: wp.array2d(dtype=float), - geom_solref: wp.array2d(dtype=wp.vec2), - geom_type: wp.array(dtype=int), + geom_plugin_index: wp.array[int], + geom_pos: wp.array2d[wp.vec3], + geom_priority: wp.array[int], + geom_quat: wp.array2d[wp.quat], + geom_rbound: wp.array2d[float], + geom_rgba: wp.array2d[wp.vec4], + geom_size: wp.array2d[wp.vec3], + geom_solimp: wp.array2d[mjwp_types.vec5], + geom_solmix: wp.array2d[float], + geom_solref: wp.array2d[wp.vec2], + geom_type: wp.array[int], has_fluid: bool, has_sdf_geom: bool, - hfield_adr: wp.array(dtype=int), - hfield_data: wp.array(dtype=float), - hfield_ncol: wp.array(dtype=int), - hfield_nrow: wp.array(dtype=int), - hfield_size: wp.array(dtype=wp.vec4), + hfield_adr: wp.array[int], + hfield_data: wp.array[float], + hfield_ncol: wp.array[int], + hfield_nrow: wp.array[int], + hfield_size: wp.array[wp.vec4], is_sparse: bool, - jnt_actfrclimited: wp.array(dtype=bool), - jnt_actfrcrange: wp.array2d(dtype=wp.vec2), - jnt_actgravcomp: wp.array(dtype=int), - jnt_axis: wp.array2d(dtype=wp.vec3), - jnt_bodyid: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - jnt_limited_ball_adr: wp.array(dtype=int), - jnt_limited_slide_hinge_adr: wp.array(dtype=int), - jnt_margin: wp.array2d(dtype=float), - jnt_pos: wp.array2d(dtype=wp.vec3), - jnt_qposadr: wp.array(dtype=int), - jnt_range: wp.array2d(dtype=wp.vec2), - jnt_solimp: wp.array2d(dtype=mjwp_types.vec5), - jnt_solref: wp.array2d(dtype=wp.vec2), - jnt_stiffness: wp.array2d(dtype=float), - jnt_type: wp.array(dtype=int), - light_bodyid: wp.array(dtype=int), - light_dir: wp.array2d(dtype=wp.vec3), - light_dir0: wp.array2d(dtype=wp.vec3), - light_mode: wp.array(dtype=int), - light_pos: wp.array2d(dtype=wp.vec3), - light_pos0: wp.array2d(dtype=wp.vec3), - light_poscom0: wp.array2d(dtype=wp.vec3), - light_targetbodyid: wp.array(dtype=int), - mapM2M: wp.array(dtype=int), - mat_rgba: wp.array2d(dtype=wp.vec4), + jnt_actfrclimited: wp.array[bool], + jnt_actfrcrange: wp.array2d[wp.vec2], + jnt_actgravcomp: wp.array[int], + jnt_axis: wp.array2d[wp.vec3], + jnt_bodyid: wp.array[int], + jnt_dofadr: wp.array[int], + jnt_limited_ball_adr: wp.array[int], + jnt_limited_slide_hinge_adr: wp.array[int], + jnt_margin: wp.array2d[float], + jnt_pos: wp.array2d[wp.vec3], + jnt_qposadr: wp.array[int], + jnt_range: wp.array2d[wp.vec2], + jnt_solimp: wp.array2d[mjwp_types.vec5], + jnt_solref: wp.array2d[wp.vec2], + jnt_stiffness: wp.array2d[float], + jnt_type: wp.array[int], + light_bodyid: wp.array[int], + light_dir: wp.array2d[wp.vec3], + light_dir0: wp.array2d[wp.vec3], + light_mode: wp.array[int], + light_pos: wp.array2d[wp.vec3], + light_pos0: wp.array2d[wp.vec3], + light_poscom0: wp.array2d[wp.vec3], + light_targetbodyid: wp.array[int], + mapM2M: wp.array[int], + mat_rgba: wp.array2d[wp.vec4], max_ten_J_rownnz: int, - mesh_face: wp.array(dtype=wp.vec3i), - mesh_faceadr: wp.array(dtype=int), - mesh_graph: wp.array(dtype=int), - mesh_graphadr: wp.array(dtype=int), - mesh_normal: wp.array(dtype=wp.vec3), - mesh_normaladr: wp.array(dtype=int), - mesh_normalnum: wp.array(dtype=int), - mesh_octadr: wp.array(dtype=int), - mesh_polyadr: wp.array(dtype=int), - mesh_polymap: wp.array(dtype=int), - mesh_polymapadr: wp.array(dtype=int), - mesh_polymapnum: wp.array(dtype=int), - mesh_polynormal: wp.array(dtype=wp.vec3), - mesh_polynum: wp.array(dtype=int), - mesh_polyvert: wp.array(dtype=int), - mesh_polyvertadr: wp.array(dtype=int), - mesh_polyvertnum: wp.array(dtype=int), - mesh_quat: wp.array(dtype=wp.quat), - mesh_vert: wp.array(dtype=wp.vec3), - mesh_vertadr: wp.array(dtype=int), - mesh_vertnum: wp.array(dtype=int), + mesh_face: wp.array[wp.vec3i], + mesh_faceadr: wp.array[int], + mesh_graph: wp.array[int], + mesh_graphadr: wp.array[int], + mesh_normal: wp.array[wp.vec3], + mesh_normaladr: wp.array[int], + mesh_normalnum: wp.array[int], + mesh_octadr: wp.array[int], + mesh_polyadr: wp.array[int], + mesh_polymap: wp.array[int], + mesh_polymapadr: wp.array[int], + mesh_polymapnum: wp.array[int], + mesh_polynormal: wp.array[wp.vec3], + mesh_polynum: wp.array[int], + mesh_polyvert: wp.array[int], + mesh_polyvertadr: wp.array[int], + mesh_polyvertnum: wp.array[int], + mesh_quat: wp.array[wp.quat], + mesh_vert: wp.array[wp.vec3], + mesh_vertadr: wp.array[int], + mesh_vertnum: wp.array[int], nC: int, nJten: int, na: int, @@ -283,241 +284,241 @@ def _forward_shim( nv: int, nv_pad: int, nwrap: int, - nxn_geom_pair_filtered: wp.array(dtype=wp.vec2i), - nxn_pairid: wp.array(dtype=wp.vec2i), - nxn_pairid_filtered: wp.array(dtype=wp.vec2i), - oct_aabb: wp.array2d(dtype=wp.vec3), - oct_child: wp.array(dtype=mjwp_types.vec8i), - oct_coeff: wp.array(dtype=mjwp_types.vec8), - pair_dim: wp.array(dtype=int), - pair_friction: wp.array2d(dtype=mjwp_types.vec5), - pair_gap: wp.array2d(dtype=float), - pair_margin: wp.array2d(dtype=float), - pair_solimp: wp.array2d(dtype=mjwp_types.vec5), - pair_solref: wp.array2d(dtype=wp.vec2), - pair_solreffriction: wp.array2d(dtype=wp.vec2), - plugin: wp.array(dtype=int), - plugin_attr: wp.array(dtype=mjwp_types.vec_pluginattr), - qLD_all_updates: wp.array(dtype=wp.vec3i), - qLD_level_offsets: wp.array(dtype=int), - qLD_updates: tuple[wp.array(dtype=wp.vec3i), ...], - qM_fullm_i: wp.array(dtype=int), - qM_fullm_j: wp.array(dtype=int), - qM_mulm_col: wp.array(dtype=int), - qM_mulm_madr: wp.array(dtype=int), - qM_mulm_rowadr: wp.array(dtype=int), + nxn_geom_pair_filtered: wp.array[wp.vec2i], + nxn_pairid: wp.array[wp.vec2i], + nxn_pairid_filtered: wp.array[wp.vec2i], + oct_aabb: wp.array2d[wp.vec3], + oct_child: wp.array[mjwp_types.vec8i], + oct_coeff: wp.array[mjwp_types.vec8], + pair_dim: wp.array[int], + pair_friction: wp.array2d[mjwp_types.vec5], + pair_gap: wp.array2d[float], + pair_margin: wp.array2d[float], + pair_solimp: wp.array2d[mjwp_types.vec5], + pair_solref: wp.array2d[wp.vec2], + pair_solreffriction: wp.array2d[wp.vec2], + plugin: wp.array[int], + plugin_attr: wp.array[mjwp_types.vec_pluginattr], + qLD_all_updates: wp.array[wp.vec3i], + qLD_level_offsets: wp.array[int], + qLD_updates: tuple[wp.array[wp.vec3i], ...], + qM_fullm_i: wp.array[int], + qM_fullm_j: wp.array[int], + qM_mulm_col: wp.array[int], + qM_mulm_madr: wp.array[int], + qM_mulm_rowadr: wp.array[int], qM_tiles: tuple[mjwp_types.TileSet, ...], - qpos0: wp.array2d(dtype=float), - qpos_spring: wp.array2d(dtype=float), - rangefinder_sensor_adr: wp.array(dtype=int), - sensor_acc_adr: wp.array(dtype=int), - sensor_adr: wp.array(dtype=int), - sensor_adr_to_contact_adr: wp.array(dtype=int), - sensor_contact_adr: wp.array(dtype=int), - sensor_cutoff: wp.array(dtype=float), - sensor_datatype: wp.array(dtype=int), - sensor_dim: wp.array(dtype=int), + qpos0: wp.array2d[float], + qpos_spring: wp.array2d[float], + rangefinder_sensor_adr: wp.array[int], + sensor_acc_adr: wp.array[int], + sensor_adr: wp.array[int], + sensor_adr_to_contact_adr: wp.array[int], + sensor_contact_adr: wp.array[int], + sensor_cutoff: wp.array[float], + sensor_datatype: wp.array[int], + sensor_dim: wp.array[int], sensor_e_kinetic: bool, sensor_e_potential: bool, - sensor_intprm: wp.array2d(dtype=int), - sensor_limitfrc_adr: wp.array(dtype=int), - sensor_limitpos_adr: wp.array(dtype=int), - sensor_limitvel_adr: wp.array(dtype=int), - sensor_objid: wp.array(dtype=int), - sensor_objtype: wp.array(dtype=int), - sensor_pos_adr: wp.array(dtype=int), - sensor_rangefinder_adr: wp.array(dtype=int), - sensor_rangefinder_bodyid: wp.array(dtype=int), - sensor_refid: wp.array(dtype=int), - sensor_reftype: wp.array(dtype=int), + sensor_intprm: wp.array2d[int], + sensor_limitfrc_adr: wp.array[int], + sensor_limitpos_adr: wp.array[int], + sensor_limitvel_adr: wp.array[int], + sensor_objid: wp.array[int], + sensor_objtype: wp.array[int], + sensor_pos_adr: wp.array[int], + sensor_rangefinder_adr: wp.array[int], + sensor_rangefinder_bodyid: wp.array[int], + sensor_refid: wp.array[int], + sensor_reftype: wp.array[int], sensor_rne_postconstraint: bool, sensor_subtree_vel: bool, - sensor_tendonactfrc_adr: wp.array(dtype=int), - sensor_touch_adr: wp.array(dtype=int), - sensor_type: wp.array(dtype=int), - sensor_vel_adr: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - site_pos: wp.array2d(dtype=wp.vec3), - site_quat: wp.array2d(dtype=wp.quat), - site_size: wp.array(dtype=wp.vec3), - site_type: wp.array(dtype=int), - taxel_sensorid: wp.array(dtype=int), - taxel_vertadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_rownnz: wp.array(dtype=int), - tendon_actfrclimited: wp.array(dtype=bool), - tendon_actfrcrange: wp.array2d(dtype=wp.vec2), - tendon_adr: wp.array(dtype=int), - tendon_armature: wp.array2d(dtype=float), - tendon_damping: wp.array2d(dtype=float), - tendon_frictionloss: wp.array2d(dtype=float), - tendon_geom_adr: wp.array(dtype=int), - tendon_invweight0: wp.array2d(dtype=float), - tendon_jnt_adr: wp.array(dtype=int), - tendon_length0: wp.array2d(dtype=float), - tendon_lengthspring: wp.array2d(dtype=wp.vec2), - tendon_limited_adr: wp.array(dtype=int), - tendon_margin: wp.array2d(dtype=float), - tendon_num: wp.array(dtype=int), - tendon_range: wp.array2d(dtype=wp.vec2), - tendon_site_pair_adr: wp.array(dtype=int), - tendon_solimp_fri: wp.array2d(dtype=mjwp_types.vec5), - tendon_solimp_lim: wp.array2d(dtype=mjwp_types.vec5), - tendon_solref_fri: wp.array2d(dtype=wp.vec2), - tendon_solref_lim: wp.array2d(dtype=wp.vec2), - tendon_stiffness: wp.array2d(dtype=float), - wrap_geom_adr: wp.array(dtype=int), - wrap_jnt_adr: wp.array(dtype=int), - wrap_objid: wp.array(dtype=int), - wrap_prm: wp.array(dtype=float), - wrap_pulley_scale: wp.array(dtype=float), - wrap_site_pair_adr: wp.array(dtype=int), - wrap_type: wp.array(dtype=int), + sensor_tendonactfrc_adr: wp.array[int], + sensor_touch_adr: wp.array[int], + sensor_type: wp.array[int], + sensor_vel_adr: wp.array[int], + site_bodyid: wp.array[int], + site_pos: wp.array2d[wp.vec3], + site_quat: wp.array2d[wp.quat], + site_size: wp.array[wp.vec3], + site_type: wp.array[int], + taxel_sensorid: wp.array[int], + taxel_vertadr: wp.array[int], + ten_J_colind: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_rownnz: wp.array[int], + tendon_actfrclimited: wp.array[bool], + tendon_actfrcrange: wp.array2d[wp.vec2], + tendon_adr: wp.array[int], + tendon_armature: wp.array2d[float], + tendon_damping: wp.array2d[float], + tendon_frictionloss: wp.array2d[float], + tendon_geom_adr: wp.array[int], + tendon_invweight0: wp.array2d[float], + tendon_jnt_adr: wp.array[int], + tendon_length0: wp.array2d[float], + tendon_lengthspring: wp.array2d[wp.vec2], + tendon_limited_adr: wp.array[int], + tendon_margin: wp.array2d[float], + tendon_num: wp.array[int], + tendon_range: wp.array2d[wp.vec2], + tendon_site_pair_adr: wp.array[int], + tendon_solimp_fri: wp.array2d[mjwp_types.vec5], + tendon_solimp_lim: wp.array2d[mjwp_types.vec5], + tendon_solref_fri: wp.array2d[wp.vec2], + tendon_solref_lim: wp.array2d[wp.vec2], + tendon_stiffness: wp.array2d[float], + wrap_geom_adr: wp.array[int], + wrap_jnt_adr: wp.array[int], + wrap_objid: wp.array[int], + wrap_prm: wp.array[float], + wrap_pulley_scale: wp.array[float], + wrap_site_pair_adr: wp.array[int], + wrap_type: wp.array[int], opt__broadphase: int, opt__broadphase_filter: int, opt__ccd_iterations: int, - opt__ccd_tolerance: wp.array(dtype=float), + opt__ccd_tolerance: wp.array[float], opt__cone: int, opt__contact_sensor_maxmatch: int, - opt__density: wp.array(dtype=float), + opt__density: wp.array[float], opt__disableflags: int, opt__enableflags: int, opt__graph_conditional: bool, - opt__gravity: wp.array(dtype=wp.vec3), - opt__impratio_invsqrt: wp.array(dtype=float), + opt__gravity: wp.array[wp.vec3], + opt__impratio_invsqrt: wp.array[float], opt__iterations: int, opt__ls_iterations: int, opt__ls_parallel: bool, opt__ls_parallel_min_step: float, - opt__ls_tolerance: wp.array(dtype=float), - opt__magnetic: wp.array(dtype=wp.vec3), + opt__ls_tolerance: wp.array[float], + opt__magnetic: wp.array[wp.vec3], opt__run_collision_detection: bool, opt__sdf_initpoints: int, opt__sdf_iterations: int, opt__solver: int, - opt__timestep: wp.array(dtype=float), - opt__tolerance: wp.array(dtype=float), - opt__viscosity: wp.array(dtype=float), - opt__wind: wp.array(dtype=wp.vec3), - stat__meaninertia: wp.array(dtype=float), + opt__timestep: wp.array[float], + opt__tolerance: wp.array[float], + opt__viscosity: wp.array[float], + opt__wind: wp.array[wp.vec3], + stat__meaninertia: wp.array[float], # Data naccdmax: int, naconmax: int, njmax: int, njmax_nnz: int, - act: wp.array2d(dtype=float), - act_dot: wp.array2d(dtype=float), - actuator_force: wp.array2d(dtype=float), - actuator_length: wp.array2d(dtype=float), - actuator_moment: wp.array2d(dtype=float), - actuator_velocity: wp.array2d(dtype=float), - cacc: wp.array2d(dtype=wp.spatial_vector), - cam_xmat: wp.array2d(dtype=wp.mat33), - cam_xpos: wp.array2d(dtype=wp.vec3), - cdof: wp.array2d(dtype=wp.spatial_vector), - cdof_dot: wp.array2d(dtype=wp.spatial_vector), - cfrc_ext: wp.array2d(dtype=wp.spatial_vector), - cfrc_int: wp.array2d(dtype=wp.spatial_vector), - cinert: wp.array2d(dtype=mjwp_types.vec10), - crb: wp.array2d(dtype=mjwp_types.vec10), - ctrl: wp.array2d(dtype=float), - cvel: wp.array2d(dtype=wp.spatial_vector), - energy: wp.array(dtype=wp.vec2), - eq_active: wp.array2d(dtype=bool), - flexedge_J: wp.array2d(dtype=float), - flexedge_length: wp.array2d(dtype=float), - flexedge_velocity: wp.array2d(dtype=float), - flexvert_xpos: wp.array2d(dtype=wp.vec3), - geom_xmat: wp.array2d(dtype=wp.mat33), - geom_xpos: wp.array2d(dtype=wp.vec3), - light_xdir: wp.array2d(dtype=wp.vec3), - light_xpos: wp.array2d(dtype=wp.vec3), - mocap_pos: wp.array2d(dtype=wp.vec3), - mocap_quat: wp.array2d(dtype=wp.quat), - moment_colind: wp.array2d(dtype=int), - moment_rowadr: wp.array2d(dtype=int), - moment_rownnz: wp.array2d(dtype=int), - nacon: wp.array(dtype=int), - ncollision: wp.array(dtype=int), - ne: wp.array(dtype=int), - nefc: wp.array(dtype=int), - nf: wp.array(dtype=int), - nisland: wp.array(dtype=int), - nl: wp.array(dtype=int), - qLD: wp.array3d(dtype=float), - qLDiagInv: wp.array2d(dtype=float), - qM: wp.array3d(dtype=float), - qacc: wp.array2d(dtype=float), - qacc_smooth: wp.array2d(dtype=float), - qacc_warmstart: wp.array2d(dtype=float), - qfrc_actuator: wp.array2d(dtype=float), - qfrc_applied: wp.array2d(dtype=float), - qfrc_bias: wp.array2d(dtype=float), - qfrc_constraint: wp.array2d(dtype=float), - qfrc_damper: wp.array2d(dtype=float), - qfrc_fluid: wp.array2d(dtype=float), - qfrc_gravcomp: wp.array2d(dtype=float), - qfrc_passive: wp.array2d(dtype=float), - qfrc_smooth: wp.array2d(dtype=float), - qfrc_spring: wp.array2d(dtype=float), - qpos: wp.array2d(dtype=float), - qvel: wp.array2d(dtype=float), - sensordata: wp.array2d(dtype=float), - site_xmat: wp.array2d(dtype=wp.mat33), - site_xpos: wp.array2d(dtype=wp.vec3), - solver_niter: wp.array(dtype=int), - subtree_angmom: wp.array2d(dtype=wp.vec3), - subtree_com: wp.array2d(dtype=wp.vec3), - subtree_linvel: wp.array2d(dtype=wp.vec3), - ten_J: wp.array2d(dtype=float), - ten_length: wp.array2d(dtype=float), - ten_velocity: wp.array2d(dtype=float), - ten_wrapadr: wp.array2d(dtype=int), - ten_wrapnum: wp.array2d(dtype=int), - time: wp.array(dtype=float), - tree_island: wp.array2d(dtype=int), - wrap_obj: wp.array2d(dtype=wp.vec2i), - wrap_xpos: wp.array2d(dtype=wp.spatial_vector), - xanchor: wp.array2d(dtype=wp.vec3), - xaxis: wp.array2d(dtype=wp.vec3), - xfrc_applied: wp.array2d(dtype=wp.spatial_vector), - ximat: wp.array2d(dtype=wp.mat33), - xipos: wp.array2d(dtype=wp.vec3), - xmat: wp.array2d(dtype=wp.mat33), - xpos: wp.array2d(dtype=wp.vec3), - xquat: wp.array2d(dtype=wp.quat), - contact__dim: wp.array(dtype=int), - contact__dist: wp.array(dtype=float), - contact__efc_address: wp.array2d(dtype=int), - contact__flex: wp.array(dtype=wp.vec2i), - contact__frame: wp.array(dtype=wp.mat33), - contact__friction: wp.array(dtype=mjwp_types.vec5), - contact__geom: wp.array(dtype=wp.vec2i), - contact__geomcollisionid: wp.array(dtype=int), - contact__includemargin: wp.array(dtype=float), - contact__pos: wp.array(dtype=wp.vec3), - contact__solimp: wp.array(dtype=mjwp_types.vec5), - contact__solref: wp.array(dtype=wp.vec2), - contact__solreffriction: wp.array(dtype=wp.vec2), - contact__type: wp.array(dtype=int), - contact__vert: wp.array(dtype=wp.vec2i), - contact__worldid: wp.array(dtype=int), - efc__D: wp.array2d(dtype=float), - efc__J: wp.array3d(dtype=float), - efc__J_colind: wp.array3d(dtype=int), - efc__J_rowadr: wp.array2d(dtype=int), - efc__J_rownnz: wp.array2d(dtype=int), - efc__Ma: wp.array2d(dtype=float), - efc__aref: wp.array2d(dtype=float), - efc__force: wp.array2d(dtype=float), - efc__frictionloss: wp.array2d(dtype=float), - efc__id: wp.array2d(dtype=int), - efc__margin: wp.array2d(dtype=float), - efc__pos: wp.array2d(dtype=float), - efc__state: wp.array2d(dtype=int), - efc__type: wp.array2d(dtype=int), - efc__vel: wp.array2d(dtype=float), + act: wp.array2d[float], + act_dot: wp.array2d[float], + actuator_force: wp.array2d[float], + actuator_length: wp.array2d[float], + actuator_moment: wp.array2d[float], + actuator_velocity: wp.array2d[float], + cacc: wp.array2d[wp.spatial_vector], + cam_xmat: wp.array2d[wp.mat33], + cam_xpos: wp.array2d[wp.vec3], + cdof: wp.array2d[wp.spatial_vector], + cdof_dot: wp.array2d[wp.spatial_vector], + cfrc_ext: wp.array2d[wp.spatial_vector], + cfrc_int: wp.array2d[wp.spatial_vector], + cinert: wp.array2d[mjwp_types.vec10], + crb: wp.array2d[mjwp_types.vec10], + ctrl: wp.array2d[float], + cvel: wp.array2d[wp.spatial_vector], + energy: wp.array[wp.vec2], + eq_active: wp.array2d[bool], + flexedge_J: wp.array2d[float], + flexedge_length: wp.array2d[float], + flexedge_velocity: wp.array2d[float], + flexvert_xpos: wp.array2d[wp.vec3], + geom_xmat: wp.array2d[wp.mat33], + geom_xpos: wp.array2d[wp.vec3], + light_xdir: wp.array2d[wp.vec3], + light_xpos: wp.array2d[wp.vec3], + mocap_pos: wp.array2d[wp.vec3], + mocap_quat: wp.array2d[wp.quat], + moment_colind: wp.array2d[int], + moment_rowadr: wp.array2d[int], + moment_rownnz: wp.array2d[int], + nacon: wp.array[int], + ncollision: wp.array[int], + ne: wp.array[int], + nefc: wp.array[int], + nf: wp.array[int], + nisland: wp.array[int], + nl: wp.array[int], + qLD: wp.array3d[float], + qLDiagInv: wp.array2d[float], + qM: wp.array3d[float], + qacc: wp.array2d[float], + qacc_smooth: wp.array2d[float], + qacc_warmstart: wp.array2d[float], + qfrc_actuator: wp.array2d[float], + qfrc_applied: wp.array2d[float], + qfrc_bias: wp.array2d[float], + qfrc_constraint: wp.array2d[float], + qfrc_damper: wp.array2d[float], + qfrc_fluid: wp.array2d[float], + qfrc_gravcomp: wp.array2d[float], + qfrc_passive: wp.array2d[float], + qfrc_smooth: wp.array2d[float], + qfrc_spring: wp.array2d[float], + qpos: wp.array2d[float], + qvel: wp.array2d[float], + sensordata: wp.array2d[float], + site_xmat: wp.array2d[wp.mat33], + site_xpos: wp.array2d[wp.vec3], + solver_niter: wp.array[int], + subtree_angmom: wp.array2d[wp.vec3], + subtree_com: wp.array2d[wp.vec3], + subtree_linvel: wp.array2d[wp.vec3], + ten_J: wp.array2d[float], + ten_length: wp.array2d[float], + ten_velocity: wp.array2d[float], + ten_wrapadr: wp.array2d[int], + ten_wrapnum: wp.array2d[int], + time: wp.array[float], + tree_island: wp.array2d[int], + wrap_obj: wp.array2d[wp.vec2i], + wrap_xpos: wp.array2d[wp.spatial_vector], + xanchor: wp.array2d[wp.vec3], + xaxis: wp.array2d[wp.vec3], + xfrc_applied: wp.array2d[wp.spatial_vector], + ximat: wp.array2d[wp.mat33], + xipos: wp.array2d[wp.vec3], + xmat: wp.array2d[wp.mat33], + xpos: wp.array2d[wp.vec3], + xquat: wp.array2d[wp.quat], + contact__dim: wp.array[int], + contact__dist: wp.array[float], + contact__efc_address: wp.array2d[int], + contact__flex: wp.array[wp.vec2i], + contact__frame: wp.array[wp.mat33], + contact__friction: wp.array[mjwp_types.vec5], + contact__geom: wp.array[wp.vec2i], + contact__geomcollisionid: wp.array[int], + contact__includemargin: wp.array[float], + contact__pos: wp.array[wp.vec3], + contact__solimp: wp.array[mjwp_types.vec5], + contact__solref: wp.array[wp.vec2], + contact__solreffriction: wp.array[wp.vec2], + contact__type: wp.array[int], + contact__vert: wp.array[wp.vec2i], + contact__worldid: wp.array[int], + efc__D: wp.array2d[float], + efc__J: wp.array3d[float], + efc__J_colind: wp.array3d[int], + efc__J_rowadr: wp.array2d[int], + efc__J_rownnz: wp.array2d[int], + efc__Ma: wp.array2d[float], + efc__aref: wp.array2d[float], + efc__force: wp.array2d[float], + efc__frictionloss: wp.array2d[float], + efc__id: wp.array2d[int], + efc__margin: wp.array2d[float], + efc__pos: wp.array2d[float], + efc__state: wp.array2d[int], + efc__type: wp.array2d[int], + efc__vel: wp.array2d[float], ): _m.stat = _s _m.opt = _o @@ -1512,7 +1513,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m.geom_conaffinity, m.geom_condim, m.geom_contype, - m.geom_dataid, + jax.numpy.expand_dims(m.geom_dataid, 0), m.geom_fluid, m.geom_friction, m.geom_gap, @@ -1979,206 +1980,206 @@ def forward_vmap(unused_axis_size, is_batched, m: types.Model, d: types.Data): def _step_shim( # Model nworld: int, - M_rowadr: wp.array(dtype=int), - M_rownnz: wp.array(dtype=int), - actuator_acc0: wp.array2d(dtype=float), - actuator_actadr: wp.array(dtype=int), - actuator_actearly: wp.array(dtype=bool), - actuator_actlimited: wp.array(dtype=bool), - actuator_actnum: wp.array(dtype=int), - actuator_actrange: wp.array2d(dtype=wp.vec2), - actuator_biasprm: wp.array2d(dtype=mjwp_types.vec10f), - actuator_biastype: wp.array(dtype=int), - actuator_cranklength: wp.array2d(dtype=float), - actuator_ctrllimited: wp.array(dtype=bool), - actuator_ctrlrange: wp.array2d(dtype=wp.vec2), - actuator_dynprm: wp.array2d(dtype=mjwp_types.vec10f), - actuator_dyntype: wp.array(dtype=int), - actuator_forcelimited: wp.array(dtype=bool), - actuator_forcerange: wp.array2d(dtype=wp.vec2), - actuator_gainprm: wp.array2d(dtype=mjwp_types.vec10f), - actuator_gaintype: wp.array(dtype=int), - actuator_gear: wp.array2d(dtype=wp.spatial_vector), - actuator_lengthrange: wp.array2d(dtype=wp.vec2), - actuator_trnid: wp.array(dtype=wp.vec2i), - actuator_trntype: wp.array(dtype=int), - actuator_trntype_body_adr: wp.array(dtype=int), + M_rowadr: wp.array[int], + M_rownnz: wp.array[int], + actuator_acc0: wp.array2d[float], + actuator_actadr: wp.array[int], + actuator_actearly: wp.array[bool], + actuator_actlimited: wp.array[bool], + actuator_actnum: wp.array[int], + actuator_actrange: wp.array2d[wp.vec2], + actuator_biasprm: wp.array2d[mjwp_types.vec10f], + actuator_biastype: wp.array[int], + actuator_cranklength: wp.array2d[float], + actuator_ctrllimited: wp.array[bool], + actuator_ctrlrange: wp.array2d[wp.vec2], + actuator_dynprm: wp.array2d[mjwp_types.vec10f], + actuator_dyntype: wp.array[int], + actuator_forcelimited: wp.array[bool], + actuator_forcerange: wp.array2d[wp.vec2], + actuator_gainprm: wp.array2d[mjwp_types.vec10f], + actuator_gaintype: wp.array[int], + actuator_gear: wp.array2d[wp.spatial_vector], + actuator_lengthrange: wp.array2d[wp.vec2], + actuator_trnid: wp.array[wp.vec2i], + actuator_trntype: wp.array[int], + actuator_trntype_body_adr: wp.array[int], block_dim: mjwp_types.BlockDim, - body_branch_start: wp.array(dtype=int), - body_branches: wp.array(dtype=int), - body_dofadr: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_fluid_ellipsoid: wp.array(dtype=bool), - body_geomadr: wp.array(dtype=int), - body_geomnum: wp.array(dtype=int), - body_gravcomp: wp.array2d(dtype=float), - body_inertia: wp.array2d(dtype=wp.vec3), - body_invweight0: wp.array2d(dtype=wp.vec2), - body_ipos: wp.array2d(dtype=wp.vec3), - body_iquat: wp.array2d(dtype=wp.quat), - body_jntadr: wp.array(dtype=int), - body_jntnum: wp.array(dtype=int), - body_mass: wp.array2d(dtype=float), - body_mocapid: wp.array(dtype=int), - body_parentid: wp.array(dtype=int), - body_pos: wp.array2d(dtype=wp.vec3), - body_quat: wp.array2d(dtype=wp.quat), - body_rootid: wp.array(dtype=int), - body_subtreemass: wp.array2d(dtype=float), - body_tree: tuple[wp.array(dtype=int), ...], - body_treeid: wp.array(dtype=int), - body_weldid: wp.array(dtype=int), - cam_bodyid: wp.array(dtype=int), - cam_fovy: wp.array2d(dtype=float), - cam_intrinsic: wp.array2d(dtype=wp.vec4), - cam_mat0: wp.array2d(dtype=wp.mat33), - cam_mode: wp.array(dtype=int), - cam_pos: wp.array2d(dtype=wp.vec3), - cam_pos0: wp.array2d(dtype=wp.vec3), - cam_poscom0: wp.array2d(dtype=wp.vec3), - cam_quat: wp.array2d(dtype=wp.quat), - cam_resolution: wp.array(dtype=wp.vec2i), - cam_sensorsize: wp.array(dtype=wp.vec2), - cam_targetbodyid: wp.array(dtype=int), - dof_Madr: wp.array(dtype=int), - dof_armature: wp.array2d(dtype=float), - dof_bodyid: wp.array(dtype=int), - dof_damping: wp.array2d(dtype=float), - dof_frictionloss: wp.array2d(dtype=float), - dof_invweight0: wp.array2d(dtype=float), - dof_jntid: wp.array(dtype=int), - dof_parentid: wp.array(dtype=int), - dof_solimp: wp.array2d(dtype=mjwp_types.vec5), - dof_solref: wp.array2d(dtype=wp.vec2), - dof_treeid: wp.array(dtype=int), - dof_tri_col: wp.array(dtype=int), - dof_tri_row: wp.array(dtype=int), - eq_connect_adr: wp.array(dtype=int), - eq_data: wp.array2d(dtype=mjwp_types.vec11), - eq_flex_adr: wp.array(dtype=int), - eq_jnt_adr: wp.array(dtype=int), - eq_obj1id: wp.array(dtype=int), - eq_obj2id: wp.array(dtype=int), - eq_objtype: wp.array(dtype=int), - eq_solimp: wp.array2d(dtype=mjwp_types.vec5), - eq_solref: wp.array2d(dtype=wp.vec2), - eq_ten_adr: wp.array(dtype=int), - eq_type: wp.array(dtype=int), - eq_wld_adr: wp.array(dtype=int), - flex_bending: wp.array2d(dtype=float), - flex_centered: wp.array(dtype=bool), - flex_conaffinity: wp.array(dtype=int), - flex_condim: wp.array(dtype=int), - flex_contype: wp.array(dtype=int), - flex_damping: wp.array(dtype=float), - flex_dim: wp.array(dtype=int), - flex_edge: wp.array(dtype=wp.vec2i), - flex_edgeadr: wp.array(dtype=int), - flex_edgeflap: wp.array(dtype=wp.vec2i), - flex_edgenum: wp.array(dtype=int), - flex_elem: wp.array(dtype=int), - flex_elemadr: wp.array(dtype=int), - flex_elemdataadr: wp.array(dtype=int), - flex_elemedge: wp.array(dtype=int), - flex_elemedgeadr: wp.array(dtype=int), - flex_elemnum: wp.array(dtype=int), - flex_friction: wp.array(dtype=wp.vec3), - flex_margin: wp.array(dtype=float), - flex_radius: wp.array(dtype=float), - flex_shell: wp.array(dtype=int), - flex_shelldataadr: wp.array(dtype=int), - flex_shellnum: wp.array(dtype=int), - flex_stiffness: wp.array2d(dtype=float), - flex_vert: wp.array(dtype=wp.vec3), - flex_vertadr: wp.array(dtype=int), - flex_vertbodyid: wp.array(dtype=int), - flex_vertflexid: wp.array(dtype=int), - flex_vertnum: wp.array(dtype=int), - flexedge_J_colind: wp.array(dtype=int), - flexedge_J_rowadr: wp.array(dtype=int), - flexedge_J_rownnz: wp.array(dtype=int), - flexedge_invweight0: wp.array(dtype=float), - flexedge_length0: wp.array(dtype=float), - geom_aabb: wp.array3d(dtype=wp.vec3), - geom_bodyid: wp.array(dtype=int), - geom_conaffinity: wp.array(dtype=int), - geom_condim: wp.array(dtype=int), - geom_contype: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_fluid: wp.array2d(dtype=float), - geom_friction: wp.array2d(dtype=wp.vec3), - geom_gap: wp.array2d(dtype=float), - geom_group: wp.array(dtype=int), - geom_margin: wp.array2d(dtype=float), - geom_matid: wp.array2d(dtype=int), + body_branch_start: wp.array[int], + body_branches: wp.array[int], + body_dofadr: wp.array[int], + body_dofnum: wp.array[int], + body_fluid_ellipsoid: wp.array[bool], + body_geomadr: wp.array[int], + body_geomnum: wp.array[int], + body_gravcomp: wp.array2d[float], + body_inertia: wp.array2d[wp.vec3], + body_invweight0: wp.array2d[wp.vec2], + body_ipos: wp.array2d[wp.vec3], + body_iquat: wp.array2d[wp.quat], + body_jntadr: wp.array[int], + body_jntnum: wp.array[int], + body_mass: wp.array2d[float], + body_mocapid: wp.array[int], + body_parentid: wp.array[int], + body_pos: wp.array2d[wp.vec3], + body_quat: wp.array2d[wp.quat], + body_rootid: wp.array[int], + body_subtreemass: wp.array2d[float], + body_tree: tuple[wp.array[int], ...], + body_treeid: wp.array[int], + body_weldid: wp.array[int], + cam_bodyid: wp.array[int], + cam_fovy: wp.array2d[float], + cam_intrinsic: wp.array2d[wp.vec4], + cam_mat0: wp.array2d[wp.mat33], + cam_mode: wp.array[int], + cam_pos: wp.array2d[wp.vec3], + cam_pos0: wp.array2d[wp.vec3], + cam_poscom0: wp.array2d[wp.vec3], + cam_quat: wp.array2d[wp.quat], + cam_resolution: wp.array[wp.vec2i], + cam_sensorsize: wp.array[wp.vec2], + cam_targetbodyid: wp.array[int], + dof_Madr: wp.array[int], + dof_armature: wp.array2d[float], + dof_bodyid: wp.array[int], + dof_damping: wp.array2d[float], + dof_frictionloss: wp.array2d[float], + dof_invweight0: wp.array2d[float], + dof_jntid: wp.array[int], + dof_parentid: wp.array[int], + dof_solimp: wp.array2d[mjwp_types.vec5], + dof_solref: wp.array2d[wp.vec2], + dof_treeid: wp.array[int], + dof_tri_col: wp.array[int], + dof_tri_row: wp.array[int], + eq_connect_adr: wp.array[int], + eq_data: wp.array2d[mjwp_types.vec11], + eq_flex_adr: wp.array[int], + eq_jnt_adr: wp.array[int], + eq_obj1id: wp.array[int], + eq_obj2id: wp.array[int], + eq_objtype: wp.array[int], + eq_solimp: wp.array2d[mjwp_types.vec5], + eq_solref: wp.array2d[wp.vec2], + eq_ten_adr: wp.array[int], + eq_type: wp.array[int], + eq_wld_adr: wp.array[int], + flex_bending: wp.array2d[float], + flex_centered: wp.array[bool], + flex_conaffinity: wp.array[int], + flex_condim: wp.array[int], + flex_contype: wp.array[int], + flex_damping: wp.array[float], + flex_dim: wp.array[int], + flex_edge: wp.array[wp.vec2i], + flex_edgeadr: wp.array[int], + flex_edgeflap: wp.array[wp.vec2i], + flex_edgenum: wp.array[int], + flex_elem: wp.array[int], + flex_elemadr: wp.array[int], + flex_elemdataadr: wp.array[int], + flex_elemedge: wp.array[int], + flex_elemedgeadr: wp.array[int], + flex_elemnum: wp.array[int], + flex_friction: wp.array[wp.vec3], + flex_margin: wp.array[float], + flex_radius: wp.array[float], + flex_shell: wp.array[int], + flex_shelldataadr: wp.array[int], + flex_shellnum: wp.array[int], + flex_stiffness: wp.array2d[float], + flex_vert: wp.array[wp.vec3], + flex_vertadr: wp.array[int], + flex_vertbodyid: wp.array[int], + flex_vertflexid: wp.array[int], + flex_vertnum: wp.array[int], + flexedge_J_colind: wp.array[int], + flexedge_J_rowadr: wp.array[int], + flexedge_J_rownnz: wp.array[int], + flexedge_invweight0: wp.array[float], + flexedge_length0: wp.array[float], + geom_aabb: wp.array3d[wp.vec3], + geom_bodyid: wp.array[int], + geom_conaffinity: wp.array[int], + geom_condim: wp.array[int], + geom_contype: wp.array[int], + geom_dataid: wp.array2d[int], + geom_fluid: wp.array2d[float], + geom_friction: wp.array2d[wp.vec3], + geom_gap: wp.array2d[float], + geom_group: wp.array[int], + geom_margin: wp.array2d[float], + geom_matid: wp.array2d[int], geom_pair_type_count: tuple[int, ...], - geom_plugin_index: wp.array(dtype=int), - geom_pos: wp.array2d(dtype=wp.vec3), - geom_priority: wp.array(dtype=int), - geom_quat: wp.array2d(dtype=wp.quat), - geom_rbound: wp.array2d(dtype=float), - geom_rgba: wp.array2d(dtype=wp.vec4), - geom_size: wp.array2d(dtype=wp.vec3), - geom_solimp: wp.array2d(dtype=mjwp_types.vec5), - geom_solmix: wp.array2d(dtype=float), - geom_solref: wp.array2d(dtype=wp.vec2), - geom_type: wp.array(dtype=int), + geom_plugin_index: wp.array[int], + geom_pos: wp.array2d[wp.vec3], + geom_priority: wp.array[int], + geom_quat: wp.array2d[wp.quat], + geom_rbound: wp.array2d[float], + geom_rgba: wp.array2d[wp.vec4], + geom_size: wp.array2d[wp.vec3], + geom_solimp: wp.array2d[mjwp_types.vec5], + geom_solmix: wp.array2d[float], + geom_solref: wp.array2d[wp.vec2], + geom_type: wp.array[int], has_fluid: bool, has_sdf_geom: bool, - hfield_adr: wp.array(dtype=int), - hfield_data: wp.array(dtype=float), - hfield_ncol: wp.array(dtype=int), - hfield_nrow: wp.array(dtype=int), - hfield_size: wp.array(dtype=wp.vec4), + hfield_adr: wp.array[int], + hfield_data: wp.array[float], + hfield_ncol: wp.array[int], + hfield_nrow: wp.array[int], + hfield_size: wp.array[wp.vec4], is_sparse: bool, - jnt_actfrclimited: wp.array(dtype=bool), - jnt_actfrcrange: wp.array2d(dtype=wp.vec2), - jnt_actgravcomp: wp.array(dtype=int), - jnt_axis: wp.array2d(dtype=wp.vec3), - jnt_bodyid: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - jnt_limited_ball_adr: wp.array(dtype=int), - jnt_limited_slide_hinge_adr: wp.array(dtype=int), - jnt_margin: wp.array2d(dtype=float), - jnt_pos: wp.array2d(dtype=wp.vec3), - jnt_qposadr: wp.array(dtype=int), - jnt_range: wp.array2d(dtype=wp.vec2), - jnt_solimp: wp.array2d(dtype=mjwp_types.vec5), - jnt_solref: wp.array2d(dtype=wp.vec2), - jnt_stiffness: wp.array2d(dtype=float), - jnt_type: wp.array(dtype=int), - light_bodyid: wp.array(dtype=int), - light_dir: wp.array2d(dtype=wp.vec3), - light_dir0: wp.array2d(dtype=wp.vec3), - light_mode: wp.array(dtype=int), - light_pos: wp.array2d(dtype=wp.vec3), - light_pos0: wp.array2d(dtype=wp.vec3), - light_poscom0: wp.array2d(dtype=wp.vec3), - light_targetbodyid: wp.array(dtype=int), - mapM2M: wp.array(dtype=int), - mat_rgba: wp.array2d(dtype=wp.vec4), + jnt_actfrclimited: wp.array[bool], + jnt_actfrcrange: wp.array2d[wp.vec2], + jnt_actgravcomp: wp.array[int], + jnt_axis: wp.array2d[wp.vec3], + jnt_bodyid: wp.array[int], + jnt_dofadr: wp.array[int], + jnt_limited_ball_adr: wp.array[int], + jnt_limited_slide_hinge_adr: wp.array[int], + jnt_margin: wp.array2d[float], + jnt_pos: wp.array2d[wp.vec3], + jnt_qposadr: wp.array[int], + jnt_range: wp.array2d[wp.vec2], + jnt_solimp: wp.array2d[mjwp_types.vec5], + jnt_solref: wp.array2d[wp.vec2], + jnt_stiffness: wp.array2d[float], + jnt_type: wp.array[int], + light_bodyid: wp.array[int], + light_dir: wp.array2d[wp.vec3], + light_dir0: wp.array2d[wp.vec3], + light_mode: wp.array[int], + light_pos: wp.array2d[wp.vec3], + light_pos0: wp.array2d[wp.vec3], + light_poscom0: wp.array2d[wp.vec3], + light_targetbodyid: wp.array[int], + mapM2M: wp.array[int], + mat_rgba: wp.array2d[wp.vec4], max_ten_J_rownnz: int, - mesh_face: wp.array(dtype=wp.vec3i), - mesh_faceadr: wp.array(dtype=int), - mesh_graph: wp.array(dtype=int), - mesh_graphadr: wp.array(dtype=int), - mesh_normal: wp.array(dtype=wp.vec3), - mesh_normaladr: wp.array(dtype=int), - mesh_normalnum: wp.array(dtype=int), - mesh_octadr: wp.array(dtype=int), - mesh_polyadr: wp.array(dtype=int), - mesh_polymap: wp.array(dtype=int), - mesh_polymapadr: wp.array(dtype=int), - mesh_polymapnum: wp.array(dtype=int), - mesh_polynormal: wp.array(dtype=wp.vec3), - mesh_polynum: wp.array(dtype=int), - mesh_polyvert: wp.array(dtype=int), - mesh_polyvertadr: wp.array(dtype=int), - mesh_polyvertnum: wp.array(dtype=int), - mesh_quat: wp.array(dtype=wp.quat), - mesh_vert: wp.array(dtype=wp.vec3), - mesh_vertadr: wp.array(dtype=int), - mesh_vertnum: wp.array(dtype=int), + mesh_face: wp.array[wp.vec3i], + mesh_faceadr: wp.array[int], + mesh_graph: wp.array[int], + mesh_graphadr: wp.array[int], + mesh_normal: wp.array[wp.vec3], + mesh_normaladr: wp.array[int], + mesh_normalnum: wp.array[int], + mesh_octadr: wp.array[int], + mesh_polyadr: wp.array[int], + mesh_polymap: wp.array[int], + mesh_polymapadr: wp.array[int], + mesh_polymapnum: wp.array[int], + mesh_polynormal: wp.array[wp.vec3], + mesh_polynum: wp.array[int], + mesh_polyvert: wp.array[int], + mesh_polyvertadr: wp.array[int], + mesh_polyvertnum: wp.array[int], + mesh_quat: wp.array[wp.quat], + mesh_vert: wp.array[wp.vec3], + mesh_vertadr: wp.array[int], + mesh_vertnum: wp.array[int], nC: int, nJten: int, nM: int, @@ -2213,242 +2214,242 @@ def _step_shim( nv: int, nv_pad: int, nwrap: int, - nxn_geom_pair_filtered: wp.array(dtype=wp.vec2i), - nxn_pairid: wp.array(dtype=wp.vec2i), - nxn_pairid_filtered: wp.array(dtype=wp.vec2i), - oct_aabb: wp.array2d(dtype=wp.vec3), - oct_child: wp.array(dtype=mjwp_types.vec8i), - oct_coeff: wp.array(dtype=mjwp_types.vec8), - pair_dim: wp.array(dtype=int), - pair_friction: wp.array2d(dtype=mjwp_types.vec5), - pair_gap: wp.array2d(dtype=float), - pair_margin: wp.array2d(dtype=float), - pair_solimp: wp.array2d(dtype=mjwp_types.vec5), - pair_solref: wp.array2d(dtype=wp.vec2), - pair_solreffriction: wp.array2d(dtype=wp.vec2), - plugin: wp.array(dtype=int), - plugin_attr: wp.array(dtype=mjwp_types.vec_pluginattr), - qLD_all_updates: wp.array(dtype=wp.vec3i), - qLD_level_offsets: wp.array(dtype=int), - qLD_updates: tuple[wp.array(dtype=wp.vec3i), ...], - qM_fullm_i: wp.array(dtype=int), - qM_fullm_j: wp.array(dtype=int), - qM_mulm_col: wp.array(dtype=int), - qM_mulm_madr: wp.array(dtype=int), - qM_mulm_rowadr: wp.array(dtype=int), + nxn_geom_pair_filtered: wp.array[wp.vec2i], + nxn_pairid: wp.array[wp.vec2i], + nxn_pairid_filtered: wp.array[wp.vec2i], + oct_aabb: wp.array2d[wp.vec3], + oct_child: wp.array[mjwp_types.vec8i], + oct_coeff: wp.array[mjwp_types.vec8], + pair_dim: wp.array[int], + pair_friction: wp.array2d[mjwp_types.vec5], + pair_gap: wp.array2d[float], + pair_margin: wp.array2d[float], + pair_solimp: wp.array2d[mjwp_types.vec5], + pair_solref: wp.array2d[wp.vec2], + pair_solreffriction: wp.array2d[wp.vec2], + plugin: wp.array[int], + plugin_attr: wp.array[mjwp_types.vec_pluginattr], + qLD_all_updates: wp.array[wp.vec3i], + qLD_level_offsets: wp.array[int], + qLD_updates: tuple[wp.array[wp.vec3i], ...], + qM_fullm_i: wp.array[int], + qM_fullm_j: wp.array[int], + qM_mulm_col: wp.array[int], + qM_mulm_madr: wp.array[int], + qM_mulm_rowadr: wp.array[int], qM_tiles: tuple[mjwp_types.TileSet, ...], - qpos0: wp.array2d(dtype=float), - qpos_spring: wp.array2d(dtype=float), - rangefinder_sensor_adr: wp.array(dtype=int), - sensor_acc_adr: wp.array(dtype=int), - sensor_adr: wp.array(dtype=int), - sensor_adr_to_contact_adr: wp.array(dtype=int), - sensor_contact_adr: wp.array(dtype=int), - sensor_cutoff: wp.array(dtype=float), - sensor_datatype: wp.array(dtype=int), - sensor_dim: wp.array(dtype=int), + qpos0: wp.array2d[float], + qpos_spring: wp.array2d[float], + rangefinder_sensor_adr: wp.array[int], + sensor_acc_adr: wp.array[int], + sensor_adr: wp.array[int], + sensor_adr_to_contact_adr: wp.array[int], + sensor_contact_adr: wp.array[int], + sensor_cutoff: wp.array[float], + sensor_datatype: wp.array[int], + sensor_dim: wp.array[int], sensor_e_kinetic: bool, sensor_e_potential: bool, - sensor_intprm: wp.array2d(dtype=int), - sensor_limitfrc_adr: wp.array(dtype=int), - sensor_limitpos_adr: wp.array(dtype=int), - sensor_limitvel_adr: wp.array(dtype=int), - sensor_objid: wp.array(dtype=int), - sensor_objtype: wp.array(dtype=int), - sensor_pos_adr: wp.array(dtype=int), - sensor_rangefinder_adr: wp.array(dtype=int), - sensor_rangefinder_bodyid: wp.array(dtype=int), - sensor_refid: wp.array(dtype=int), - sensor_reftype: wp.array(dtype=int), + sensor_intprm: wp.array2d[int], + sensor_limitfrc_adr: wp.array[int], + sensor_limitpos_adr: wp.array[int], + sensor_limitvel_adr: wp.array[int], + sensor_objid: wp.array[int], + sensor_objtype: wp.array[int], + sensor_pos_adr: wp.array[int], + sensor_rangefinder_adr: wp.array[int], + sensor_rangefinder_bodyid: wp.array[int], + sensor_refid: wp.array[int], + sensor_reftype: wp.array[int], sensor_rne_postconstraint: bool, sensor_subtree_vel: bool, - sensor_tendonactfrc_adr: wp.array(dtype=int), - sensor_touch_adr: wp.array(dtype=int), - sensor_type: wp.array(dtype=int), - sensor_vel_adr: wp.array(dtype=int), - site_bodyid: wp.array(dtype=int), - site_pos: wp.array2d(dtype=wp.vec3), - site_quat: wp.array2d(dtype=wp.quat), - site_size: wp.array(dtype=wp.vec3), - site_type: wp.array(dtype=int), - taxel_sensorid: wp.array(dtype=int), - taxel_vertadr: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_rownnz: wp.array(dtype=int), - tendon_actfrclimited: wp.array(dtype=bool), - tendon_actfrcrange: wp.array2d(dtype=wp.vec2), - tendon_adr: wp.array(dtype=int), - tendon_armature: wp.array2d(dtype=float), - tendon_damping: wp.array2d(dtype=float), - tendon_frictionloss: wp.array2d(dtype=float), - tendon_geom_adr: wp.array(dtype=int), - tendon_invweight0: wp.array2d(dtype=float), - tendon_jnt_adr: wp.array(dtype=int), - tendon_length0: wp.array2d(dtype=float), - tendon_lengthspring: wp.array2d(dtype=wp.vec2), - tendon_limited_adr: wp.array(dtype=int), - tendon_margin: wp.array2d(dtype=float), - tendon_num: wp.array(dtype=int), - tendon_range: wp.array2d(dtype=wp.vec2), - tendon_site_pair_adr: wp.array(dtype=int), - tendon_solimp_fri: wp.array2d(dtype=mjwp_types.vec5), - tendon_solimp_lim: wp.array2d(dtype=mjwp_types.vec5), - tendon_solref_fri: wp.array2d(dtype=wp.vec2), - tendon_solref_lim: wp.array2d(dtype=wp.vec2), - tendon_stiffness: wp.array2d(dtype=float), - wrap_geom_adr: wp.array(dtype=int), - wrap_jnt_adr: wp.array(dtype=int), - wrap_objid: wp.array(dtype=int), - wrap_prm: wp.array(dtype=float), - wrap_pulley_scale: wp.array(dtype=float), - wrap_site_pair_adr: wp.array(dtype=int), - wrap_type: wp.array(dtype=int), + sensor_tendonactfrc_adr: wp.array[int], + sensor_touch_adr: wp.array[int], + sensor_type: wp.array[int], + sensor_vel_adr: wp.array[int], + site_bodyid: wp.array[int], + site_pos: wp.array2d[wp.vec3], + site_quat: wp.array2d[wp.quat], + site_size: wp.array[wp.vec3], + site_type: wp.array[int], + taxel_sensorid: wp.array[int], + taxel_vertadr: wp.array[int], + ten_J_colind: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_rownnz: wp.array[int], + tendon_actfrclimited: wp.array[bool], + tendon_actfrcrange: wp.array2d[wp.vec2], + tendon_adr: wp.array[int], + tendon_armature: wp.array2d[float], + tendon_damping: wp.array2d[float], + tendon_frictionloss: wp.array2d[float], + tendon_geom_adr: wp.array[int], + tendon_invweight0: wp.array2d[float], + tendon_jnt_adr: wp.array[int], + tendon_length0: wp.array2d[float], + tendon_lengthspring: wp.array2d[wp.vec2], + tendon_limited_adr: wp.array[int], + tendon_margin: wp.array2d[float], + tendon_num: wp.array[int], + tendon_range: wp.array2d[wp.vec2], + tendon_site_pair_adr: wp.array[int], + tendon_solimp_fri: wp.array2d[mjwp_types.vec5], + tendon_solimp_lim: wp.array2d[mjwp_types.vec5], + tendon_solref_fri: wp.array2d[wp.vec2], + tendon_solref_lim: wp.array2d[wp.vec2], + tendon_stiffness: wp.array2d[float], + wrap_geom_adr: wp.array[int], + wrap_jnt_adr: wp.array[int], + wrap_objid: wp.array[int], + wrap_prm: wp.array[float], + wrap_pulley_scale: wp.array[float], + wrap_site_pair_adr: wp.array[int], + wrap_type: wp.array[int], opt__broadphase: int, opt__broadphase_filter: int, opt__ccd_iterations: int, - opt__ccd_tolerance: wp.array(dtype=float), + opt__ccd_tolerance: wp.array[float], opt__cone: int, opt__contact_sensor_maxmatch: int, - opt__density: wp.array(dtype=float), + opt__density: wp.array[float], opt__disableflags: int, opt__enableflags: int, opt__graph_conditional: bool, - opt__gravity: wp.array(dtype=wp.vec3), - opt__impratio_invsqrt: wp.array(dtype=float), + opt__gravity: wp.array[wp.vec3], + opt__impratio_invsqrt: wp.array[float], opt__integrator: int, opt__iterations: int, opt__ls_iterations: int, opt__ls_parallel: bool, opt__ls_parallel_min_step: float, - opt__ls_tolerance: wp.array(dtype=float), - opt__magnetic: wp.array(dtype=wp.vec3), + opt__ls_tolerance: wp.array[float], + opt__magnetic: wp.array[wp.vec3], opt__run_collision_detection: bool, opt__sdf_initpoints: int, opt__sdf_iterations: int, opt__solver: int, - opt__timestep: wp.array(dtype=float), - opt__tolerance: wp.array(dtype=float), - opt__viscosity: wp.array(dtype=float), - opt__wind: wp.array(dtype=wp.vec3), - stat__meaninertia: wp.array(dtype=float), + opt__timestep: wp.array[float], + opt__tolerance: wp.array[float], + opt__viscosity: wp.array[float], + opt__wind: wp.array[wp.vec3], + stat__meaninertia: wp.array[float], # Data naccdmax: int, naconmax: int, njmax: int, njmax_nnz: int, - act: wp.array2d(dtype=float), - act_dot: wp.array2d(dtype=float), - actuator_force: wp.array2d(dtype=float), - actuator_length: wp.array2d(dtype=float), - actuator_moment: wp.array2d(dtype=float), - actuator_velocity: wp.array2d(dtype=float), - cacc: wp.array2d(dtype=wp.spatial_vector), - cam_xmat: wp.array2d(dtype=wp.mat33), - cam_xpos: wp.array2d(dtype=wp.vec3), - cdof: wp.array2d(dtype=wp.spatial_vector), - cdof_dot: wp.array2d(dtype=wp.spatial_vector), - cfrc_ext: wp.array2d(dtype=wp.spatial_vector), - cfrc_int: wp.array2d(dtype=wp.spatial_vector), - cinert: wp.array2d(dtype=mjwp_types.vec10), - crb: wp.array2d(dtype=mjwp_types.vec10), - ctrl: wp.array2d(dtype=float), - cvel: wp.array2d(dtype=wp.spatial_vector), - energy: wp.array(dtype=wp.vec2), - eq_active: wp.array2d(dtype=bool), - flexedge_J: wp.array2d(dtype=float), - flexedge_length: wp.array2d(dtype=float), - flexedge_velocity: wp.array2d(dtype=float), - flexvert_xpos: wp.array2d(dtype=wp.vec3), - geom_xmat: wp.array2d(dtype=wp.mat33), - geom_xpos: wp.array2d(dtype=wp.vec3), - light_xdir: wp.array2d(dtype=wp.vec3), - light_xpos: wp.array2d(dtype=wp.vec3), - mocap_pos: wp.array2d(dtype=wp.vec3), - mocap_quat: wp.array2d(dtype=wp.quat), - moment_colind: wp.array2d(dtype=int), - moment_rowadr: wp.array2d(dtype=int), - moment_rownnz: wp.array2d(dtype=int), - nacon: wp.array(dtype=int), - ncollision: wp.array(dtype=int), - ne: wp.array(dtype=int), - nefc: wp.array(dtype=int), - nf: wp.array(dtype=int), - nisland: wp.array(dtype=int), - nl: wp.array(dtype=int), - qLD: wp.array3d(dtype=float), - qLDiagInv: wp.array2d(dtype=float), - qM: wp.array3d(dtype=float), - qacc: wp.array2d(dtype=float), - qacc_smooth: wp.array2d(dtype=float), - qacc_warmstart: wp.array2d(dtype=float), - qfrc_actuator: wp.array2d(dtype=float), - qfrc_applied: wp.array2d(dtype=float), - qfrc_bias: wp.array2d(dtype=float), - qfrc_constraint: wp.array2d(dtype=float), - qfrc_damper: wp.array2d(dtype=float), - qfrc_fluid: wp.array2d(dtype=float), - qfrc_gravcomp: wp.array2d(dtype=float), - qfrc_passive: wp.array2d(dtype=float), - qfrc_smooth: wp.array2d(dtype=float), - qfrc_spring: wp.array2d(dtype=float), - qpos: wp.array2d(dtype=float), - qvel: wp.array2d(dtype=float), - sensordata: wp.array2d(dtype=float), - site_xmat: wp.array2d(dtype=wp.mat33), - site_xpos: wp.array2d(dtype=wp.vec3), - solver_niter: wp.array(dtype=int), - subtree_angmom: wp.array2d(dtype=wp.vec3), - subtree_com: wp.array2d(dtype=wp.vec3), - subtree_linvel: wp.array2d(dtype=wp.vec3), - ten_J: wp.array2d(dtype=float), - ten_length: wp.array2d(dtype=float), - ten_velocity: wp.array2d(dtype=float), - ten_wrapadr: wp.array2d(dtype=int), - ten_wrapnum: wp.array2d(dtype=int), - time: wp.array(dtype=float), - tree_island: wp.array2d(dtype=int), - wrap_obj: wp.array2d(dtype=wp.vec2i), - wrap_xpos: wp.array2d(dtype=wp.spatial_vector), - xanchor: wp.array2d(dtype=wp.vec3), - xaxis: wp.array2d(dtype=wp.vec3), - xfrc_applied: wp.array2d(dtype=wp.spatial_vector), - ximat: wp.array2d(dtype=wp.mat33), - xipos: wp.array2d(dtype=wp.vec3), - xmat: wp.array2d(dtype=wp.mat33), - xpos: wp.array2d(dtype=wp.vec3), - xquat: wp.array2d(dtype=wp.quat), - contact__dim: wp.array(dtype=int), - contact__dist: wp.array(dtype=float), - contact__efc_address: wp.array2d(dtype=int), - contact__flex: wp.array(dtype=wp.vec2i), - contact__frame: wp.array(dtype=wp.mat33), - contact__friction: wp.array(dtype=mjwp_types.vec5), - contact__geom: wp.array(dtype=wp.vec2i), - contact__geomcollisionid: wp.array(dtype=int), - contact__includemargin: wp.array(dtype=float), - contact__pos: wp.array(dtype=wp.vec3), - contact__solimp: wp.array(dtype=mjwp_types.vec5), - contact__solref: wp.array(dtype=wp.vec2), - contact__solreffriction: wp.array(dtype=wp.vec2), - contact__type: wp.array(dtype=int), - contact__vert: wp.array(dtype=wp.vec2i), - contact__worldid: wp.array(dtype=int), - efc__D: wp.array2d(dtype=float), - efc__J: wp.array3d(dtype=float), - efc__J_colind: wp.array3d(dtype=int), - efc__J_rowadr: wp.array2d(dtype=int), - efc__J_rownnz: wp.array2d(dtype=int), - efc__Ma: wp.array2d(dtype=float), - efc__aref: wp.array2d(dtype=float), - efc__force: wp.array2d(dtype=float), - efc__frictionloss: wp.array2d(dtype=float), - efc__id: wp.array2d(dtype=int), - efc__margin: wp.array2d(dtype=float), - efc__pos: wp.array2d(dtype=float), - efc__state: wp.array2d(dtype=int), - efc__type: wp.array2d(dtype=int), - efc__vel: wp.array2d(dtype=float), + act: wp.array2d[float], + act_dot: wp.array2d[float], + actuator_force: wp.array2d[float], + actuator_length: wp.array2d[float], + actuator_moment: wp.array2d[float], + actuator_velocity: wp.array2d[float], + cacc: wp.array2d[wp.spatial_vector], + cam_xmat: wp.array2d[wp.mat33], + cam_xpos: wp.array2d[wp.vec3], + cdof: wp.array2d[wp.spatial_vector], + cdof_dot: wp.array2d[wp.spatial_vector], + cfrc_ext: wp.array2d[wp.spatial_vector], + cfrc_int: wp.array2d[wp.spatial_vector], + cinert: wp.array2d[mjwp_types.vec10], + crb: wp.array2d[mjwp_types.vec10], + ctrl: wp.array2d[float], + cvel: wp.array2d[wp.spatial_vector], + energy: wp.array[wp.vec2], + eq_active: wp.array2d[bool], + flexedge_J: wp.array2d[float], + flexedge_length: wp.array2d[float], + flexedge_velocity: wp.array2d[float], + flexvert_xpos: wp.array2d[wp.vec3], + geom_xmat: wp.array2d[wp.mat33], + geom_xpos: wp.array2d[wp.vec3], + light_xdir: wp.array2d[wp.vec3], + light_xpos: wp.array2d[wp.vec3], + mocap_pos: wp.array2d[wp.vec3], + mocap_quat: wp.array2d[wp.quat], + moment_colind: wp.array2d[int], + moment_rowadr: wp.array2d[int], + moment_rownnz: wp.array2d[int], + nacon: wp.array[int], + ncollision: wp.array[int], + ne: wp.array[int], + nefc: wp.array[int], + nf: wp.array[int], + nisland: wp.array[int], + nl: wp.array[int], + qLD: wp.array3d[float], + qLDiagInv: wp.array2d[float], + qM: wp.array3d[float], + qacc: wp.array2d[float], + qacc_smooth: wp.array2d[float], + qacc_warmstart: wp.array2d[float], + qfrc_actuator: wp.array2d[float], + qfrc_applied: wp.array2d[float], + qfrc_bias: wp.array2d[float], + qfrc_constraint: wp.array2d[float], + qfrc_damper: wp.array2d[float], + qfrc_fluid: wp.array2d[float], + qfrc_gravcomp: wp.array2d[float], + qfrc_passive: wp.array2d[float], + qfrc_smooth: wp.array2d[float], + qfrc_spring: wp.array2d[float], + qpos: wp.array2d[float], + qvel: wp.array2d[float], + sensordata: wp.array2d[float], + site_xmat: wp.array2d[wp.mat33], + site_xpos: wp.array2d[wp.vec3], + solver_niter: wp.array[int], + subtree_angmom: wp.array2d[wp.vec3], + subtree_com: wp.array2d[wp.vec3], + subtree_linvel: wp.array2d[wp.vec3], + ten_J: wp.array2d[float], + ten_length: wp.array2d[float], + ten_velocity: wp.array2d[float], + ten_wrapadr: wp.array2d[int], + ten_wrapnum: wp.array2d[int], + time: wp.array[float], + tree_island: wp.array2d[int], + wrap_obj: wp.array2d[wp.vec2i], + wrap_xpos: wp.array2d[wp.spatial_vector], + xanchor: wp.array2d[wp.vec3], + xaxis: wp.array2d[wp.vec3], + xfrc_applied: wp.array2d[wp.spatial_vector], + ximat: wp.array2d[wp.mat33], + xipos: wp.array2d[wp.vec3], + xmat: wp.array2d[wp.mat33], + xpos: wp.array2d[wp.vec3], + xquat: wp.array2d[wp.quat], + contact__dim: wp.array[int], + contact__dist: wp.array[float], + contact__efc_address: wp.array2d[int], + contact__flex: wp.array[wp.vec2i], + contact__frame: wp.array[wp.mat33], + contact__friction: wp.array[mjwp_types.vec5], + contact__geom: wp.array[wp.vec2i], + contact__geomcollisionid: wp.array[int], + contact__includemargin: wp.array[float], + contact__pos: wp.array[wp.vec3], + contact__solimp: wp.array[mjwp_types.vec5], + contact__solref: wp.array[wp.vec2], + contact__solreffriction: wp.array[wp.vec2], + contact__type: wp.array[int], + contact__vert: wp.array[wp.vec2i], + contact__worldid: wp.array[int], + efc__D: wp.array2d[float], + efc__J: wp.array3d[float], + efc__J_colind: wp.array3d[int], + efc__J_rowadr: wp.array2d[int], + efc__J_rownnz: wp.array2d[int], + efc__Ma: wp.array2d[float], + efc__aref: wp.array2d[float], + efc__force: wp.array2d[float], + efc__frictionloss: wp.array2d[float], + efc__id: wp.array2d[int], + efc__margin: wp.array2d[float], + efc__pos: wp.array2d[float], + efc__state: wp.array2d[int], + efc__type: wp.array2d[int], + efc__vel: wp.array2d[float], ): _m.stat = _s _m.opt = _o @@ -3457,7 +3458,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): m.geom_conaffinity, m.geom_condim, m.geom_contype, - m.geom_dataid, + jax.numpy.expand_dims(m.geom_dataid, 0), m.geom_fluid, m.geom_friction, m.geom_gap, diff --git a/mjx/mujoco/mjx/warp/render.py b/mjx/mujoco/mjx/warp/render.py index 6a3f66ab..c97003f0 100644 --- a/mjx/mujoco/mjx/warp/render.py +++ b/mjx/mujoco/mjx/warp/render.py @@ -48,42 +48,43 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) + @ffi.format_args_for_warp def _render_shim( # Model nworld: int, - cam_fovy: wp.array2d(dtype=float), - cam_intrinsic: wp.array2d(dtype=wp.vec4), - cam_projection: wp.array(dtype=int), - cam_sensorsize: wp.array(dtype=wp.vec2), - flex_edge: wp.array(dtype=wp.vec2i), - flex_radius: wp.array(dtype=float), - flex_vertadr: wp.array(dtype=int), - geom_dataid: wp.array(dtype=int), - geom_matid: wp.array2d(dtype=int), - geom_rgba: wp.array2d(dtype=wp.vec4), - geom_size: wp.array2d(dtype=wp.vec3), - geom_type: wp.array(dtype=int), - light_active: wp.array2d(dtype=bool), - light_castshadow: wp.array2d(dtype=bool), - light_type: wp.array2d(dtype=int), - mat_rgba: wp.array2d(dtype=wp.vec4), - mat_texid: wp.array3d(dtype=int), - mat_texrepeat: wp.array2d(dtype=wp.vec2), - mesh_faceadr: wp.array(dtype=int), + cam_fovy: wp.array2d[float], + cam_intrinsic: wp.array2d[wp.vec4], + cam_projection: wp.array[int], + cam_sensorsize: wp.array[wp.vec2], + flex_edge: wp.array[wp.vec2i], + flex_radius: wp.array[float], + flex_vertadr: wp.array[int], + geom_dataid: wp.array2d[int], + geom_matid: wp.array2d[int], + geom_rgba: wp.array2d[wp.vec4], + geom_size: wp.array2d[wp.vec3], + geom_type: wp.array[int], + light_active: wp.array2d[bool], + light_castshadow: wp.array2d[bool], + light_type: wp.array2d[int], + mat_rgba: wp.array2d[wp.vec4], + mat_texid: wp.array3d[int], + mat_texrepeat: wp.array2d[wp.vec2], + mesh_faceadr: wp.array[int], nlight: int, # Data - cam_xmat: wp.array2d(dtype=wp.mat33), - cam_xpos: wp.array2d(dtype=wp.vec3), - flexvert_xpos: wp.array2d(dtype=wp.vec3), - geom_xmat: wp.array2d(dtype=wp.mat33), - geom_xpos: wp.array2d(dtype=wp.vec3), - light_xdir: wp.array2d(dtype=wp.vec3), - light_xpos: wp.array2d(dtype=wp.vec3), + cam_xmat: wp.array2d[wp.mat33], + cam_xpos: wp.array2d[wp.vec3], + flexvert_xpos: wp.array2d[wp.vec3], + geom_xmat: wp.array2d[wp.mat33], + geom_xpos: wp.array2d[wp.vec3], + light_xdir: wp.array2d[wp.vec3], + light_xpos: wp.array2d[wp.vec3], # Registry rc_id: int, - rgb: wp.array2d(dtype=wp.uint32), - depth: wp.array2d(dtype=wp.float32), + rgb: wp.array2d[wp.uint32], + depth: wp.array2d[wp.float32], ): _m.stat = _s _m.opt = _o @@ -164,7 +165,7 @@ def _render_jax_impl(m: types.Model, d: types.Data, ctx: RenderContextPytree): m._impl.flex_edge, m._impl.flex_radius, m.flex_vertadr, - m.geom_dataid, + jax.numpy.expand_dims(m.geom_dataid, 0), m.geom_matid, m.geom_rgba, m.geom_size, diff --git a/mjx/mujoco/mjx/warp/smooth.py b/mjx/mujoco/mjx/warp/smooth.py index 209dc6b0..020b9392 100644 --- a/mjx/mujoco/mjx/warp/smooth.py +++ b/mjx/mujoco/mjx/warp/smooth.py @@ -46,52 +46,53 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) + @ffi.format_args_for_warp def _kinematics_shim( # Model nworld: int, - body_branch_start: wp.array(dtype=int), - body_branches: wp.array(dtype=int), - body_ipos: wp.array2d(dtype=wp.vec3), - body_iquat: wp.array2d(dtype=wp.quat), - body_jntadr: wp.array(dtype=int), - body_jntnum: wp.array(dtype=int), - body_mocapid: wp.array(dtype=int), - body_parentid: wp.array(dtype=int), - body_pos: wp.array2d(dtype=wp.vec3), - body_quat: wp.array2d(dtype=wp.quat), - body_rootid: wp.array(dtype=int), - body_weldid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - geom_pos: wp.array2d(dtype=wp.vec3), - geom_quat: wp.array2d(dtype=wp.quat), - jnt_axis: wp.array2d(dtype=wp.vec3), - jnt_pos: wp.array2d(dtype=wp.vec3), - jnt_qposadr: wp.array(dtype=int), - jnt_type: wp.array(dtype=int), + body_branch_start: wp.array[int], + body_branches: wp.array[int], + body_ipos: wp.array2d[wp.vec3], + body_iquat: wp.array2d[wp.quat], + body_jntadr: wp.array[int], + body_jntnum: wp.array[int], + body_mocapid: wp.array[int], + body_parentid: wp.array[int], + body_pos: wp.array2d[wp.vec3], + body_quat: wp.array2d[wp.quat], + body_rootid: wp.array[int], + body_weldid: wp.array[int], + geom_bodyid: wp.array[int], + geom_pos: wp.array2d[wp.vec3], + geom_quat: wp.array2d[wp.quat], + jnt_axis: wp.array2d[wp.vec3], + jnt_pos: wp.array2d[wp.vec3], + jnt_qposadr: wp.array[int], + jnt_type: wp.array[int], nbody: int, nbranch: int, ngeom: int, nsite: int, - qpos0: wp.array2d(dtype=float), - site_bodyid: wp.array(dtype=int), - site_pos: wp.array2d(dtype=wp.vec3), - site_quat: wp.array2d(dtype=wp.quat), + qpos0: wp.array2d[float], + site_bodyid: wp.array[int], + site_pos: wp.array2d[wp.vec3], + site_quat: wp.array2d[wp.quat], # Data - geom_xmat: wp.array2d(dtype=wp.mat33), - geom_xpos: wp.array2d(dtype=wp.vec3), - mocap_pos: wp.array2d(dtype=wp.vec3), - mocap_quat: wp.array2d(dtype=wp.quat), - qpos: wp.array2d(dtype=float), - site_xmat: wp.array2d(dtype=wp.mat33), - site_xpos: wp.array2d(dtype=wp.vec3), - xanchor: wp.array2d(dtype=wp.vec3), - xaxis: wp.array2d(dtype=wp.vec3), - ximat: wp.array2d(dtype=wp.mat33), - xipos: wp.array2d(dtype=wp.vec3), - xmat: wp.array2d(dtype=wp.mat33), - xpos: wp.array2d(dtype=wp.vec3), - xquat: wp.array2d(dtype=wp.quat), + geom_xmat: wp.array2d[wp.mat33], + geom_xpos: wp.array2d[wp.vec3], + mocap_pos: wp.array2d[wp.vec3], + mocap_quat: wp.array2d[wp.quat], + qpos: wp.array2d[float], + site_xmat: wp.array2d[wp.mat33], + site_xpos: wp.array2d[wp.vec3], + xanchor: wp.array2d[wp.vec3], + xaxis: wp.array2d[wp.vec3], + ximat: wp.array2d[wp.mat33], + xipos: wp.array2d[wp.vec3], + xmat: wp.array2d[wp.mat33], + xpos: wp.array2d[wp.vec3], + xquat: wp.array2d[wp.quat], ): _m.stat = _s _m.opt = _o @@ -297,45 +298,45 @@ def kinematics_vmap( def _tendon_shim( # Model nworld: int, - body_dofadr: wp.array(dtype=int), - body_dofnum: wp.array(dtype=int), - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - geom_bodyid: wp.array(dtype=int), - geom_size: wp.array2d(dtype=wp.vec3), - jnt_dofadr: wp.array(dtype=int), - jnt_qposadr: wp.array(dtype=int), + body_dofadr: wp.array[int], + body_dofnum: wp.array[int], + body_parentid: wp.array[int], + body_rootid: wp.array[int], + geom_bodyid: wp.array[int], + geom_size: wp.array2d[wp.vec3], + jnt_dofadr: wp.array[int], + jnt_qposadr: wp.array[int], ntendon: int, nwrap: int, - site_bodyid: wp.array(dtype=int), - ten_J_colind: wp.array(dtype=int), - ten_J_rowadr: wp.array(dtype=int), - ten_J_rownnz: wp.array(dtype=int), - tendon_adr: wp.array(dtype=int), - tendon_geom_adr: wp.array(dtype=int), - tendon_jnt_adr: wp.array(dtype=int), - tendon_num: wp.array(dtype=int), - tendon_site_pair_adr: wp.array(dtype=int), - wrap_geom_adr: wp.array(dtype=int), - wrap_jnt_adr: wp.array(dtype=int), - wrap_objid: wp.array(dtype=int), - wrap_prm: wp.array(dtype=float), - wrap_pulley_scale: wp.array(dtype=float), - wrap_site_pair_adr: wp.array(dtype=int), - wrap_type: wp.array(dtype=int), + site_bodyid: wp.array[int], + ten_J_colind: wp.array[int], + ten_J_rowadr: wp.array[int], + ten_J_rownnz: wp.array[int], + tendon_adr: wp.array[int], + tendon_geom_adr: wp.array[int], + tendon_jnt_adr: wp.array[int], + tendon_num: wp.array[int], + tendon_site_pair_adr: wp.array[int], + wrap_geom_adr: wp.array[int], + wrap_jnt_adr: wp.array[int], + wrap_objid: wp.array[int], + wrap_prm: wp.array[float], + wrap_pulley_scale: wp.array[float], + wrap_site_pair_adr: wp.array[int], + wrap_type: wp.array[int], # Data - cdof: wp.array2d(dtype=wp.spatial_vector), - geom_xmat: wp.array2d(dtype=wp.mat33), - geom_xpos: wp.array2d(dtype=wp.vec3), - qpos: wp.array2d(dtype=float), - site_xpos: wp.array2d(dtype=wp.vec3), - subtree_com: wp.array2d(dtype=wp.vec3), - ten_J: wp.array2d(dtype=float), - ten_length: wp.array2d(dtype=float), - ten_wrapadr: wp.array2d(dtype=int), - ten_wrapnum: wp.array2d(dtype=int), - wrap_obj: wp.array2d(dtype=wp.vec2i), - wrap_xpos: wp.array2d(dtype=wp.spatial_vector), + cdof: wp.array2d[wp.spatial_vector], + geom_xmat: wp.array2d[wp.mat33], + geom_xpos: wp.array2d[wp.vec3], + qpos: wp.array2d[float], + site_xpos: wp.array2d[wp.vec3], + subtree_com: wp.array2d[wp.vec3], + ten_J: wp.array2d[float], + ten_length: wp.array2d[float], + ten_wrapadr: wp.array2d[int], + ten_wrapnum: wp.array2d[int], + wrap_obj: wp.array2d[wp.vec2i], + wrap_xpos: wp.array2d[wp.spatial_vector], ): _m.stat = _s _m.opt = _o @@ -489,26 +490,26 @@ def tendon_vmap(unused_axis_size, is_batched, m: types.Model, d: types.Data): def _com_pos_shim( # Model nworld: int, - body_inertia: wp.array2d(dtype=wp.vec3), - body_mass: wp.array2d(dtype=float), - body_parentid: wp.array(dtype=int), - body_rootid: wp.array(dtype=int), - body_subtreemass: wp.array2d(dtype=float), - body_tree: tuple[wp.array(dtype=int), ...], - jnt_bodyid: wp.array(dtype=int), - jnt_dofadr: wp.array(dtype=int), - jnt_type: wp.array(dtype=int), + body_inertia: wp.array2d[wp.vec3], + body_mass: wp.array2d[float], + body_parentid: wp.array[int], + body_rootid: wp.array[int], + body_subtreemass: wp.array2d[float], + body_tree: tuple[wp.array[int], ...], + jnt_bodyid: wp.array[int], + jnt_dofadr: wp.array[int], + jnt_type: wp.array[int], nbody: int, njnt: int, # Data - cdof: wp.array2d(dtype=wp.spatial_vector), - cinert: wp.array2d(dtype=mjwp_types.vec10), - subtree_com: wp.array2d(dtype=wp.vec3), - xanchor: wp.array2d(dtype=wp.vec3), - xaxis: wp.array2d(dtype=wp.vec3), - ximat: wp.array2d(dtype=wp.mat33), - xipos: wp.array2d(dtype=wp.vec3), - xmat: wp.array2d(dtype=wp.mat33), + cdof: wp.array2d[wp.spatial_vector], + cinert: wp.array2d[mjwp_types.vec10], + subtree_com: wp.array2d[wp.vec3], + xanchor: wp.array2d[wp.vec3], + xaxis: wp.array2d[wp.vec3], + ximat: wp.array2d[wp.mat33], + xipos: wp.array2d[wp.vec3], + xmat: wp.array2d[wp.mat33], ): _m.stat = _s _m.opt = _o diff --git a/mjx/mujoco/mjx/warp/types.py b/mjx/mujoco/mjx/warp/types.py index 564fe57e..14e416a8 100644 --- a/mjx/mujoco/mjx/warp/types.py +++ b/mjx/mujoco/mjx/warp/types.py @@ -672,7 +672,7 @@ _NDIM = { 'geom_conaffinity': 1, 'geom_condim': 1, 'geom_contype': 1, - 'geom_dataid': 1, + 'geom_dataid': 2, 'geom_fluid': 2, 'geom_friction': 3, 'geom_gap': 2, @@ -1238,7 +1238,7 @@ _BATCH_DIM = { 'geom_conaffinity': False, 'geom_condim': False, 'geom_contype': False, - 'geom_dataid': False, + 'geom_dataid': True, 'geom_fluid': False, 'geom_friction': True, 'geom_gap': True, From 4a98cc8bff73556bee7d9ccba57ede7a5d39471f Mon Sep 17 00:00:00 2001 From: Michael Moss Date: Tue, 14 Apr 2026 16:48:13 -0700 Subject: [PATCH 061/251] Update MuJoCo version to 3.7.1 following the 3.7.0 release PiperOrigin-RevId: 899835826 Change-Id: Ibe247b921e7dad7184acef69ee3199fec79c6e24 --- CMakeLists.txt | 2 +- dist/mujoco.rc | 8 ++++---- dist/simulate.rc | 8 ++++---- doc/APIreference/APIglobals.rst | 2 +- doc/unity.rst | 4 ++-- include/mujoco/mujoco.h | 2 +- mjx/pyproject.toml | 8 ++++---- python/mujoco/CMakeLists.txt | 4 ++-- python/mujoco/mjpython/Info.plist | 8 ++++---- python/pyproject.toml | 6 +++--- sample/CMakeLists.txt | 2 +- simulate/CMakeLists.txt | 2 +- src/engine/engine_support.c | 4 ++-- unity/Editor/Bindings/MujocoBinaryRetriever.cs | 4 ++-- unity/Runtime/Bindings/MjBindings.cs | 2 +- unity/package.json | 2 +- 16 files changed, 34 insertions(+), 34 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2ade5c70..645ea60d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco - VERSION 3.7.0 + VERSION 3.7.1 DESCRIPTION "MuJoCo Physics Simulator" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/dist/mujoco.rc b/dist/mujoco.rc index 25f2e83e..2f55a201 100644 --- a/dist/mujoco.rc +++ b/dist/mujoco.rc @@ -1,6 +1,6 @@ 1 VERSIONINFO -FILEVERSION 3,7,0,0 -PRODUCTVERSION 3,7,0,0 +FILEVERSION 3,7,1,0 +PRODUCTVERSION 3,7,1,0 FILEOS 0x4 FILETYPE 0x1 { @@ -9,9 +9,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.7.0" + VALUE "ProductVersion", "3.7.1" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.7.0" + VALUE "FileVersion", "3.7.1" VALUE "InternalName", "mujoco.dll" VALUE "OriginalFilename", "mujoco.dll" VALUE "CompanyName", "Google DeepMind" diff --git a/dist/simulate.rc b/dist/simulate.rc index 0626bc62..a1517c13 100644 --- a/dist/simulate.rc +++ b/dist/simulate.rc @@ -1,8 +1,8 @@ MUJOCO ICON "mujoco.ico" 1 VERSIONINFO -FILEVERSION 3,7,0,0 -PRODUCTVERSION 3,7,0,0 +FILEVERSION 3,7,1,0 +PRODUCTVERSION 3,7,1,0 FILEOS 0x4 FILETYPE 0x1 { @@ -11,9 +11,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.7.0" + VALUE "ProductVersion", "3.7.1" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.7.0" + VALUE "FileVersion", "3.7.1" VALUE "InternalName", "simulate.exe" VALUE "OriginalFilename", "simulate.exe" VALUE "CompanyName", "Google DeepMind" diff --git a/doc/APIreference/APIglobals.rst b/doc/APIreference/APIglobals.rst index c2e614d8..72f8e741 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -388,7 +388,7 @@ Defined in `mujoco.h diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index 60063b70..fb3c5a5d 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name="mujoco-mjx" -version = "3.7.0" +version = "3.7.1" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -29,7 +29,7 @@ dependencies = [ "etils[epath]", "jax", "jaxlib", - "mujoco>=3.7.0.dev0", + "mujoco>=3.7.1.dev0", "scipy", "trimesh", ] @@ -45,9 +45,9 @@ mjx-viewer = "mujoco.mjx.viewer:main" [project.urls] Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Documentation = "https://mujoco.readthedocs.io/en/3.7.0" +Documentation = "https://mujoco.readthedocs.io/en/3.7.1" Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Changelog = "https://mujoco.readthedocs.io/en/3.7.0/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.7.1/changelog.html" [tool.isort] force_single_line = true diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index cd1289c2..6a6e100f 100644 --- a/python/mujoco/CMakeLists.txt +++ b/python/mujoco/CMakeLists.txt @@ -86,7 +86,7 @@ if(NOT TARGET mujoco) if(MUJOCO_FRAMEWORK) message("MuJoCo framework is at ${MUJOCO_FRAMEWORK}/mujoco.framework") set(MUJOCO_LIBRARY - ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.7.0.dylib + ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.7.1.dylib ) target_compile_options(mujoco INTERFACE -F${MUJOCO_FRAMEWORK}) endif() @@ -94,7 +94,7 @@ if(NOT TARGET mujoco) if(NOT MUJOCO_FRAMEWORK) find_library( - MUJOCO_LIBRARY mujoco mujoco.3.7.0 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED + MUJOCO_LIBRARY mujoco mujoco.3.7.1 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED ) find_path(MUJOCO_INCLUDE mujoco/mujoco.h HINTS ${MUJOCO_INCLUDE_DIR} REQUIRED) message("MuJoCo is at ${MUJOCO_LIBRARY}") diff --git a/python/mujoco/mjpython/Info.plist b/python/mujoco/mjpython/Info.plist index 2cf7f5ed..1c3b5e7c 100644 --- a/python/mujoco/mjpython/Info.plist +++ b/python/mujoco/mjpython/Info.plist @@ -7,13 +7,13 @@ CFBundleIdentifier org.mujoco.mjpython CFBundleVersion - 3.7.0 + 3.7.1 CFBundleGetInfoString - 3.7.0 + 3.7.1 CFBundleLongVersionString - 3.7.0 + 3.7.1 CFBundleShortVersionString - 3.7.0 + 3.7.1 CFBundleExecutable mjpython CFBundleIconFile diff --git a/python/pyproject.toml b/python/pyproject.toml index 157d9898..136e7822 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "mujoco" -version = "3.7.0" +version = "3.7.1" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -34,9 +34,9 @@ dynamic = ["readme", "scripts"] [project.urls] Homepage = "https://github.com/google-deepmind/mujoco" -Documentation = "https://mujoco.readthedocs.io/en/3.7.0" +Documentation = "https://mujoco.readthedocs.io/en/3.7.1" Repository = "https://github.com/google-deepmind/mujoco" -Changelog = "https://mujoco.readthedocs.io/en/3.7.0/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.7.1/changelog.html" [tool.setuptools] include-package-data = false diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt index 09c20cc0..41902f1e 100644 --- a/sample/CMakeLists.txt +++ b/sample/CMakeLists.txt @@ -24,7 +24,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco_samples - VERSION 3.7.0 + VERSION 3.7.1 DESCRIPTION "MuJoCo samples binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/simulate/CMakeLists.txt b/simulate/CMakeLists.txt index 640ac5ad..cbb2cab8 100644 --- a/simulate/CMakeLists.txt +++ b/simulate/CMakeLists.txt @@ -29,7 +29,7 @@ set(MUJOCO_DEP_VERSION_lodepng project( mujoco_simulate - VERSION 3.7.0 + VERSION 3.7.1 DESCRIPTION "MuJoCo simulate binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 81c3b6ca..c1ee7785 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -43,8 +43,8 @@ //-------------------------- Constants ------------------------------------------------------------- - #define mjVERSION 3007000 -#define mjVERSIONSTRING "3.7.0" + #define mjVERSION 3007001 +#define mjVERSIONSTRING "3.7.1" // names of disable flags const char* mjDISABLESTRING[mjNDISABLE] = { diff --git a/unity/Editor/Bindings/MujocoBinaryRetriever.cs b/unity/Editor/Bindings/MujocoBinaryRetriever.cs index b3840a7e..b3ee6cee 100644 --- a/unity/Editor/Bindings/MujocoBinaryRetriever.cs +++ b/unity/Editor/Bindings/MujocoBinaryRetriever.cs @@ -37,7 +37,7 @@ public class MujocoBinaryRetriever { if (AssetDatabase.LoadMainAssetAtPath(mujocoPath + "/mujoco.dylib") == null) { File.Copy( "/Applications/MuJoCo.app/Contents/Frameworks" + - "/mujoco.framework/Versions/Current/libmujoco.3.7.0.dylib", + "/mujoco.framework/Versions/Current/libmujoco.3.7.1.dylib", mujocoPath + "/mujoco.dylib"); AssetDatabase.Refresh(); } @@ -45,7 +45,7 @@ public class MujocoBinaryRetriever { if (AssetDatabase.LoadMainAssetAtPath(mujocoPath + "/libmujoco.so") == null) { File.Copy( Environment.GetFolderPath(Environment.SpecialFolder.UserProfile) + - "/.mujoco/mujoco-3.7.0/lib/libmujoco.so.3.7.0", + "/.mujoco/mujoco-3.7.1/lib/libmujoco.so.3.7.1", mujocoPath + "/libmujoco.so"); AssetDatabase.Refresh(); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 72db845d..94d15988 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -113,7 +113,7 @@ public const int mjMAXLINEPNT = 1001; public const int mjMAXPLANEGRID = 200; public const bool THIRD_PARTY_MUJOCO_MJXMACRO_H_ = true; public const bool THIRD_PARTY_MUJOCO_MUJOCO_H_ = true; -public const int mjVERSION_HEADER = 3007000; +public const int mjVERSION_HEADER = 3007001; // ------------------------------------Enums------------------------------------ diff --git a/unity/package.json b/unity/package.json index 67cf4330..3f32a5c5 100644 --- a/unity/package.json +++ b/unity/package.json @@ -1,7 +1,7 @@ { "name": "org.mujoco", "displayName": "MuJoCo", - "version": "3.7.0", + "version": "3.7.1", "description": "MuJoCo importer and runtime plug-in", "dependencies": {}, "author": { From d913e0b1e9e4eaf67c27a0148062bf0f6100211a Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 15 Apr 2026 01:39:25 -0700 Subject: [PATCH 062/251] Add ReadPixels function to RenderTarget. PiperOrigin-RevId: 900017779 Change-Id: I0632b66124632558ea19157e4ca319e4b0d10617 --- .../filament/filament/filament_context.cc | 28 ++----------- .../filament/filament/render_target.cc | 40 ++++++++++++++++++- .../filament/filament/render_target.h | 11 +++-- 3 files changed, 49 insertions(+), 30 deletions(-) diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index cbc79c24..c01a95b2 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -209,28 +209,6 @@ void FilamentContext::DestroyRenderTargets() { color_target_.reset(); } -static void ReadColorPixels(filament::Renderer* renderer, - RenderTarget* target, mjrRect viewport, - unsigned char* buffer, size_t num_bytes) { - filament::backend::PixelBufferDescriptor descriptor( - buffer, num_bytes, filament::backend::PixelDataFormat::RGB, - filament::backend::PixelDataType::UBYTE); - renderer->readPixels(target->GetFilamentRenderTarget(), viewport.left, - viewport.bottom, viewport.width, viewport.height, - std::move(descriptor)); -} - -static void ReadDepthPixels(filament::Renderer* renderer, - RenderTarget* target, mjrRect viewport, - float* buffer, size_t num_bytes) { - filament::backend::PixelBufferDescriptor descriptor( - buffer, num_bytes, filament::backend::PixelDataFormat::R, - filament::backend::PixelDataType::FLOAT); - renderer->readPixels(target->GetFilamentRenderTarget(), viewport.left, - viewport.bottom, viewport.width, viewport.height, - std::move(descriptor)); -} - void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb, float* depth) { if (scene_swap_chain_target_ != kOffscreenSwapChain) { @@ -261,7 +239,7 @@ void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb, } const size_t num_bytes = viewport.width * viewport.height * 3; - ReadColorPixels(renderer_, color_target_.get(), viewport, rgb, num_bytes); + color_target_->ReadColorPixels(renderer_, rgb, num_bytes); renderer_->endFrame(); } @@ -277,8 +255,8 @@ void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb, scene_view_->Render(renderer_, request); const size_t num_bytes = viewport.width * viewport.height * sizeof(float); - ReadDepthPixels(renderer_, depth_target_.get(), viewport, depth, - num_bytes); + depth_target_->ReadColorPixels( + renderer_, reinterpret_cast(depth), num_bytes); renderer_->endFrame(); } diff --git a/src/experimental/filament/filament/render_target.cc b/src/experimental/filament/filament/render_target.cc index dec68a82..eb2d3490 100644 --- a/src/experimental/filament/filament/render_target.cc +++ b/src/experimental/filament/filament/render_target.cc @@ -14,18 +14,25 @@ #include "experimental/filament/filament/render_target.h" +#include +#include #include +#include +#include +#include #include +#include #include #include +#include #include "experimental/filament/filament/texture.h" namespace mujoco { RenderTarget::RenderTarget(filament::Engine* engine, - RenderTargetTextureType color, - RenderTargetTextureType depth) + RenderTargetTextureType color, + RenderTargetTextureType depth) : engine_(engine), color_type_(color), depth_type_(depth) {} RenderTarget::~RenderTarget() noexcept { @@ -53,6 +60,35 @@ void RenderTarget::Prepare(int width, int height) { render_target_ = builder.build(*engine_); } +void RenderTarget::ReadColorPixels(filament::Renderer* renderer, uint8_t* bytes, + size_t num_bytes) { + filament::backend::PixelDataFormat format; + filament::backend::PixelDataType type; + size_t expected_num_bytes = 0; + switch (color_type_) { + case RenderTargetTextureType::kColor: + format = filament::backend::PixelDataFormat::RGB; + type = filament::backend::PixelDataType::UBYTE; + expected_num_bytes = width_ * height_ * 3; + break; + case RenderTargetTextureType::kDepthColor: + format = filament::backend::PixelDataFormat::R; + type = filament::backend::PixelDataType::FLOAT; + expected_num_bytes = width_ * height_ * sizeof(float); + break; + default: + mju_error("Unsupported pixel format: %d", color_type_); + return; + } + if (num_bytes != expected_num_bytes) { + mju_error("Invalid number of bytes."); + return; + } + + filament::backend::PixelBufferDescriptor desc(bytes, num_bytes, format, type); + renderer->readPixels(render_target_, 0, 0, width_, height_, std::move(desc)); +} + void RenderTarget::Destroy() { if (render_target_) { engine_->destroy(render_target_); diff --git a/src/experimental/filament/filament/render_target.h b/src/experimental/filament/filament/render_target.h index 3e5530cc..ee033de9 100644 --- a/src/experimental/filament/filament/render_target.h +++ b/src/experimental/filament/filament/render_target.h @@ -15,6 +15,8 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDER_TARGET_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDER_TARGET_H_ +#include +#include #include #include @@ -28,9 +30,8 @@ class RenderTarget { public: // Defines the types of textures to create for the color and depth // attachments. - RenderTarget(filament::Engine* engine, - RenderTargetTextureType color, - RenderTargetTextureType depth); + RenderTarget(filament::Engine* engine, RenderTargetTextureType color, + RenderTargetTextureType depth); ~RenderTarget() noexcept; RenderTarget(const RenderTarget&) = delete; @@ -40,6 +41,10 @@ class RenderTarget { // the last time the render target was prepared. void Prepare(int width, int height); + // Reads the pixels from the render target texture. + void ReadColorPixels(filament::Renderer* renderer, uint8_t* bytes, + size_t num_bytes); + // Returns the color texture. Texture* GetColorTexture() const; From 9289905c9aa8ab700395fe2a80e032a4a07bb3ec Mon Sep 17 00:00:00 2001 From: Matija Kecman Date: Wed, 15 Apr 2026 02:47:40 -0700 Subject: [PATCH 063/251] Make StepControlGui a reusable component and refactor/simplify the implementation PiperOrigin-RevId: 900045404 Change-Id: I9976ece042710ddce0a7c5d99a6ac85a0ef06d2f --- src/experimental/platform/sim/step_control.cc | 8 +- src/experimental/platform/sim/step_control.h | 5 +- src/experimental/platform/ux/gui.cc | 83 ++++++++++++ src/experimental/platform/ux/gui.h | 18 +++ src/experimental/studio/app.cc | 128 ++---------------- src/experimental/studio/app.h | 3 - 6 files changed, 122 insertions(+), 123 deletions(-) diff --git a/src/experimental/platform/sim/step_control.cc b/src/experimental/platform/sim/step_control.cc index 7dbb9340..1f5b54c3 100644 --- a/src/experimental/platform/sim/step_control.cc +++ b/src/experimental/platform/sim/step_control.cc @@ -83,10 +83,14 @@ void StepControl::SetNoiseParameters(float ctrl_noise_scale, ctrl_noise_rate_ = ctrl_noise_rate; } -void StepControl::SetPauseState(PauseState state, mjModel* m) { +void StepControl::SetPauseState(PauseState state) { pause_state_ = state; } +StepControl::PauseState StepControl::GetPauseState() const { + return pause_state_; +} + StepControl::Status StepControl::Advance(mjModel* m, mjData* d) { if (!m) { return Status::kOk; @@ -184,7 +188,7 @@ StepControl::Status StepControl::Advance(mjModel* m, mjData* d) { for (mjtWarning w : kDivergedWarnings) { if (d->warning[w].number > 0) { // Stop stepping if the simulation diverged. - pause_state_ = PauseState::kNormalPaused; + SetPauseState(PauseState::kNormalPaused); return Status::kDiverged; } } diff --git a/src/experimental/platform/sim/step_control.h b/src/experimental/platform/sim/step_control.h index 0fec3025..1f34a810 100644 --- a/src/experimental/platform/sim/step_control.h +++ b/src/experimental/platform/sim/step_control.h @@ -70,11 +70,10 @@ class StepControl { enum class PauseState { kUnpaused, kNormalPaused, kViscousPaused }; // Sets the pause state of the simulation. - // m must be non-null for viscous pausing. - void SetPauseState(PauseState state, mjModel* m = nullptr); + void SetPauseState(PauseState state); // Gets the current pause state of the simulation. - PauseState GetPauseState() const { return pause_state_; } + PauseState GetPauseState() const; // If the simulation is paused, will perform a single step on the next // Advance() call. diff --git a/src/experimental/platform/ux/gui.cc b/src/experimental/platform/ux/gui.cc index a5405cb1..b20993ee 100644 --- a/src/experimental/platform/ux/gui.cc +++ b/src/experimental/platform/ux/gui.cc @@ -15,10 +15,12 @@ #include "experimental/platform/ux/gui.h" #include +#include #include #include #include #include +#include #include #include @@ -26,10 +28,23 @@ #include #include #include "experimental/platform/helpers.h" +#include "experimental/platform/sim/step_control.h" #include "experimental/platform/ux/imgui_widgets.h" #include "experimental/platform/ux/interaction.h" namespace mujoco::platform { +namespace { +struct SpeedStatus { + bool misaligned; + float measured; +}; + +static SpeedStatus IsSpeedMisaligned(const StepControl& step_control) { + const float desired = step_control.GetSpeed(); + const float measured = step_control.GetSpeedMeasured(); + return {std::abs(measured - desired) > 0.1f * desired, measured}; +} +} // namespace static ImVec2 GetFlexElementSize(int num_cols) { const float width = (ImGui::GetContentRegionAvail().x / num_cols) - @@ -317,6 +332,74 @@ ImVec4 ConfigureDockingLayout() { return ImVec4(workspace_x, workspace_y, workspace_w, workspace_h); } +void StepControlGui(const mjModel* model, StepControl* step_control, + int& speed_index) { + platform::ScopedStyle style; + style.Var(ImGuiStyleVar_FrameRounding, 2.f); + + const ImColor yellow(255, 215, 0, 255); + const ImColor green(40, 180, 40, 255); + const float scale = ImGui::GetWindowDpiScale(); + ImVec2 button_size(48.f * scale, 32.f * scale); + + auto make_button = [&](const char* icon, StepControl::PauseState target_state, + ImColor color, const char* tooltip = "", + float hover_alpha = 1.f) { + bool active = step_control->GetPauseState() == target_state; + if (ImGui_ColorButton(icon, active, color, button_size, hover_alpha)) { + step_control->SetPauseState(target_state); + } + if (!std::string_view(tooltip).empty()) { + ImGui::SetItemTooltip("%s", tooltip); + } + }; + + make_button(ICON_FA_PAUSE, StepControl::PauseState::kNormalPaused, yellow, + "Pause"); + ImGui::SameLine(0.f, 0.f); + make_button(ICON_FA_MAGIC, StepControl::PauseState::kViscousPaused, yellow, + "Viscous Pause"); + ImGui::SameLine(0.f, 0.f); + make_button(ICON_FA_PLAY, StepControl::PauseState::kUnpaused, green, "", .6f); + + // Speed selection. + ImGui::SameLine(); + const float pad_y = (button_size.y - ImGui::GetFontSize()) * .5f; + ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, + ImVec2(ImGui::GetStyle().FramePadding.x + 5.f, pad_y)); + + const auto [misaligned, measured] = IsSpeedMisaligned(*step_control); + char speed_preview[64]; + if (misaligned) { + snprintf(speed_preview, sizeof(speed_preview), "%s%s (%-4.1f%%)", + ICON_FA_TACHOMETER, kPercentRealTime[speed_index], measured); + } else { + snprintf(speed_preview, sizeof(speed_preview), "%s%s", ICON_FA_TACHOMETER, + kPercentRealTime[speed_index]); + } + + ImGui::SetNextItemWidth(ImGui::CalcTextSize(speed_preview).x + + ImGui::GetStyle().FramePadding.x * 2.f); + if (ImGui::BeginCombo("##Speed", speed_preview, + ImGuiComboFlags_NoArrowButton)) { + for (int n = 0; n < kPercentRealTime.size(); n++) { + if (ImGui::Selectable(kPercentRealTime[n], (speed_index == n))) { + speed_index = std::clamp(n, 0, kPercentRealTime.size() - 1); + float speed = std::stof(kPercentRealTime[speed_index]); + step_control->SetSpeed(speed); + } + } + ImGui::EndCombo(); + } + + ImGui::PopStyleVar(); + if (misaligned) { + ImGui::SetItemTooltip("%s", "Desired Speed (Measured Speed)"); + } else { + ImGui::SetItemTooltip("%s", "Desired Speed"); + } +} + bool ThemeSelectGui(GuiTheme* theme) { static constexpr const char* ICON_DARKMODE = ICON_FA_CIRCLE; static constexpr const char* ICON_LIGHTMODE = ICON_FA_CIRCLE_O; diff --git a/src/experimental/platform/ux/gui.h b/src/experimental/platform/ux/gui.h index cfa8fd80..938466bb 100644 --- a/src/experimental/platform/ux/gui.h +++ b/src/experimental/platform/ux/gui.h @@ -23,10 +23,12 @@ // by the caller. In most cases, this is already stored in mjModel, mjData, // mjvOption, etc. But, some functions take additional arguments as needed. +#include #include #include #include +#include "experimental/platform/sim/step_control.h" namespace mujoco::platform { @@ -62,6 +64,22 @@ void SetupTheme(GuiTheme theme); // be used to place additional elements (e.g. floating charts). ImVec4 ConfigureDockingLayout(); +// logarithmically spaced real-time slow-down coefficients (percent) +// clang-format off +static constexpr std::array kPercentRealTime = { +"100.0 ", " 80.0 ", " 66.0 ", " 50.0 ", " 40.0 ", " 33.0 ", " 25.0 ", " 20.0 ", " 16.0 ", " 13.0 ", +" 10.0 ", " 8.0 ", " 6.6 ", " 5.0 ", " 4.0 ", " 3.3 ", " 2.5 ", " 2.0 ", " 1.6 ", " 1.3 ", +" 1.0 ", " 0.8 ", " 0.7 ", " 0.5 ", " 0.4 ", " 0.33", " 0.25", " 0.2 ", " 0.16", " 0.13", +" 0.1 ", +}; +// clang-format on + +// UX for controlling the simulation stepping. `speed_index` is an index into +// kPercentRealTime, an array of available speeds (indices in range [0, 30] map +// to real-time percentages in range [100%, 0.1%]). +void StepControlGui(const mjModel* model, StepControl* step_control, + int& speed_index); + // UX for selecting the GUI theme. bool ThemeSelectGui(GuiTheme* theme); diff --git a/src/experimental/studio/app.cc b/src/experimental/studio/app.cc index b9cf91fb..9937e8ab 100644 --- a/src/experimental/studio/app.cc +++ b/src/experimental/studio/app.cc @@ -17,7 +17,6 @@ #include #include #include -#include #include #include #include @@ -72,9 +71,6 @@ static void SelectParentPerturb(const mjModel* model, mjvPerturb& perturb) { // TODO: update selected element! } -static constexpr const char* ICON_PLAY = platform::ICON_FA_PLAY; -static constexpr const char* ICON_PAUSE = platform::ICON_FA_PAUSE; -static constexpr const char* ICON_VISCOUS_PAUSE = platform::ICON_FA_MAGIC; static constexpr const char* ICON_COPY_CAMERA = platform::ICON_FA_COPY; static constexpr const char* ICON_UNLOAD_MODEL = platform::ICON_FA_EJECT; static constexpr const char* ICON_RELOAD_MODEL = platform::ICON_FA_REFRESH; @@ -82,21 +78,10 @@ static constexpr const char* ICON_RESET_MODEL = platform::ICON_FA_UNDO; static constexpr const char* ICON_PREV_FRAME = platform::ICON_FA_CARET_LEFT; static constexpr const char* ICON_NEXT_FRAME = platform::ICON_FA_CARET_RIGHT; static constexpr const char* ICON_CURR_FRAME = platform::ICON_FA_FAST_FORWARD; -static constexpr const char* ICON_SPEED = platform::ICON_FA_TACHOMETER; static constexpr const char* ICON_RELOAD_SPEC = platform::ICON_FA_REFRESH; static constexpr const char* ICON_UNDO_SPEC = platform::ICON_FA_UNDO; static constexpr const char* ICON_REDO_SPEC = platform::ICON_FA_REPEAT; -// logarithmically spaced real-time slow-down coefficients (percent) -// clang-format off -static constexpr std::array kPercentRealTime = { -"100.0 ", " 80.0 ", " 66.0 ", " 50.0 ", " 40.0 ", " 33.0 ", " 25.0 ", " 20.0 ", " 16.0 ", " 13.0 ", -" 10.0 ", " 8.0 ", " 6.6 ", " 5.0 ", " 4.0 ", " 3.3 ", " 2.5 ", " 2.0 ", " 1.6 ", " 1.3 ", -" 1.0 ", " 0.8 ", " 0.7 ", " 0.5 ", " 0.4 ", " 0.33", " 0.25", " 0.2 ", " 0.16", " 0.13", -" 0.1 ", -}; -// clang-format on - App::App(Config config) : ini_path_(std::move(config.ini_path)), gfx_mode_(config.gfx_mode) { SwitchGraphicsMode(config.width, config.height, config.gfx_mode); @@ -221,8 +206,8 @@ void App::OnModelLoaded(std::string filename, ModelKind model_kind) { // Initialize the speed based on the model's default real-time setting. float min_error = FLT_MAX; const float desired = mju_log(100 * model->vis.global.realtime); - for (int i = 0; i < kPercentRealTime.size(); ++i) { - const float speed = std::stof(kPercentRealTime[i]); + for (int i = 0; i < platform::kPercentRealTime.size(); ++i) { + const float speed = std::stof(platform::kPercentRealTime[i]); const float error = mju_abs(mju_log(speed) - desired); if (error < min_error) { min_error = error; @@ -613,14 +598,13 @@ void App::HandleKeyboardEvents() { } } else if (ImGui_IsChordJustPressed(ImGuiMod_Ctrl | ImGuiKey_Space)) { if (step_control_.GetPauseState() == PauseState::kViscousPaused) { - step_control_.SetPauseState(PauseState::kUnpaused, model()); + step_control_.SetPauseState(PauseState::kUnpaused); } else { - step_control_.SetPauseState(PauseState::kViscousPaused, model()); - tmp_.viscous_pause_time = ImGui::GetTime(); + step_control_.SetPauseState(PauseState::kViscousPaused); } } else if (ImGui_IsChordJustPressed(ImGuiKey_Space)) { if (step_control_.GetPauseState() == PauseState::kViscousPaused) { - step_control_.SetPauseState(PauseState::kNormalPaused, model()); + step_control_.SetPauseState(PauseState::kNormalPaused); } else if (step_control_.GetPauseState() == PauseState::kUnpaused) { step_control_.SetPauseState(PauseState::kNormalPaused); } else { @@ -817,12 +801,13 @@ void App::SaveSettings() { } void App::SetSpeedIndex(int idx) { - if (idx == tmp_.speed_index || kPercentRealTime.empty()) { + if (idx == tmp_.speed_index || platform::kPercentRealTime.empty()) { return; } - tmp_.speed_index = std::clamp(idx, 0, kPercentRealTime.size() - 1); - float speed = std::stof(kPercentRealTime[tmp_.speed_index]); + tmp_.speed_index = + std::clamp(idx, 0, platform::kPercentRealTime.size() - 1); + float speed = std::stof(platform::kPercentRealTime[tmp_.speed_index]); step_control_.SetSpeed(speed); } @@ -1407,25 +1392,10 @@ void App::HelpGui() { ImGui::Columns(); } -struct SpeedStatus { - bool misaligned; - float measured; -}; - -static SpeedStatus IsSpeedMisaligned( - const platform::StepControl& step_control) { - const float desired = step_control.GetSpeed(); - const float measured = step_control.GetSpeedMeasured(); - return {std::abs(measured - desired) > 0.1f * desired, measured}; -} - void App::ToolBarGui() { if (ImGui::BeginTable("##ToolBarTable", 2)) { platform::ScopedStyle style; const ImColor red(220, 40, 40, 255); - const ImColor green(40, 180, 40, 255); - const ImColor yellow(250, 230, 10, 255); - const int combo_flags = ImGuiComboFlags_NoArrowButton; const float scale = ImGui::GetWindowDpiScale(); const ImVec2 button_size(48.f * scale, 32.f * scale); @@ -1481,82 +1451,10 @@ void App::ToolBarGui() { } ImGui::SetItemTooltip("%s", "Reset"); - // Combined (Normal Pause, Viscous Pause, Play) widget - { - style.Var(ImGuiStyleVar_FrameRounding, 2.0f); - // Normal pause button. - ImGui::SameLine(0, separator_width); - ImColor paused_color = yellow; - bool paused = step_control_.GetPauseState() == PauseState::kNormalPaused; - if (platform::ImGui_ColorButton(ICON_PAUSE, paused, paused_color, - button_size)) { - if (!paused) { - step_control_.SetPauseState(PauseState::kNormalPaused, model()); - } - } - ImGui::SetItemTooltip("%s", "Pause"); - - // Viscous pause button. - ImGui::SameLine(0, 0); - ImColor vpaused_color = green; - float t = 0.f; - bool vpaused = - step_control_.GetPauseState() == PauseState::kViscousPaused; - if (vpaused) { - t = ImGui::GetTime() - tmp_.viscous_pause_time; - t = std::sqrt(std::min(t / 0.75f, 1.0f)); - vpaused_color = ImColor(ImLerp(green.Value, yellow.Value, t)); - } - if (platform::ImGui_ColorButton(ICON_VISCOUS_PAUSE, vpaused, - vpaused_color, button_size, - t < 1.0f ? 1.0f : 0.5f)) { - if (!vpaused) { - step_control_.SetPauseState(PauseState::kViscousPaused, model()); - tmp_.viscous_pause_time = ImGui::GetTime(); - } - } - ImGui::SetItemTooltip("%s", "Viscous Pause"); - - // Play button. - ImGui::SameLine(0, 0); - if (platform::ImGui_ColorButton( - ICON_PLAY, step_control_.GetPauseState() == PauseState::kUnpaused, - green, button_size, 0.6f)) { - step_control_.SetPauseState(PauseState::kUnpaused, model()); - } - } - - // Speed selection. - ImGui::SameLine(); - float pad_y = (button_size.y - ImGui::GetFontSize()) * 0.5f; - ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, - ImVec2(ImGui::GetStyle().FramePadding.x + 5.f, pad_y)); - const auto [misaligned, measured] = IsSpeedMisaligned(step_control_); - char speed_preview[64]; - if (misaligned) { - snprintf(speed_preview, sizeof(speed_preview), "%s%s (%-4.1f%%)", - ICON_SPEED, kPercentRealTime[tmp_.speed_index], measured); - } else { - snprintf(speed_preview, sizeof(speed_preview), "%s%s", ICON_SPEED, - kPercentRealTime[tmp_.speed_index]); - } - ImGui::SetNextItemWidth(ImGui::CalcTextSize(speed_preview).x + - ImGui::GetStyle().FramePadding.x * 2); - if (ImGui::BeginCombo("##Speed", speed_preview, combo_flags)) { - for (int n = 0; n < kPercentRealTime.size(); n++) { - if (ImGui::Selectable(kPercentRealTime[n], (tmp_.speed_index == n))) { - SetSpeedIndex(n); - } - } - ImGui::EndCombo(); - } - ImGui::PopStyleVar(); - if (misaligned) { - ImGui::SetItemTooltip("%s", "Desired Speed (Measured Speed)"); - } else { - ImGui::SetItemTooltip("%s", "Desired Speed"); - } + // Combined (Normal Pause, Viscous Pause, Play) widget and Speed selection. + ImGui::SameLine(0, separator_width); + platform::StepControlGui(model(), &step_control_, tmp_.speed_index); ImGui::TableNextColumn(); ImGui::SetCursorPosY(ImGui::GetCursorPosY() + @@ -1709,7 +1607,7 @@ void App::MainMenuGui() { if (step_control_.GetPauseState() != PauseState::kNormalPaused) { step_control_.SetPauseState(PauseState::kNormalPaused); } else { - step_control_.SetPauseState(PauseState::kUnpaused, model()); + step_control_.SetPauseState(PauseState::kUnpaused); } } if (ImGui::MenuItem("Reset", "Backspace")) { diff --git a/src/experimental/studio/app.h b/src/experimental/studio/app.h index 222cdf73..2a1c7326 100644 --- a/src/experimental/studio/app.h +++ b/src/experimental/studio/app.h @@ -133,9 +133,6 @@ class App { // Controls. bool perturb_active = false; - // Time at which viscous pause was activated, for the green→yellow button - // color animation. - double viscous_pause_time = 0; int speed_index = 0; float cam_speed = 0.0f; From f24f9ef44d040837b33bde5420f8c9d7b6dde54b Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 15 Apr 2026 03:25:09 -0700 Subject: [PATCH 064/251] Allow submeshes to be set when adding meshes to a Renderable. Also add support for blend order. PiperOrigin-RevId: 900061778 Change-Id: I601757a8672fed23e45defd4cdbe4e9526ea3b70 --- .../filament/filament/renderable.cc | 105 ++++++++++++------ .../filament/filament/renderable.h | 89 ++++++++++----- .../filament/filament/scene_geom_util.cc | 12 +- 3 files changed, 135 insertions(+), 71 deletions(-) diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 32fd66d9..ea39df1a 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -31,11 +31,11 @@ Renderable::Renderable(filament::Engine* engine) : material_(engine) {} Renderable::~Renderable() noexcept { while (!entities_.empty()) { - RemoveLast(); + RemoveLastEntity(); } } -void Renderable::RemoveLast() { +void Renderable::RemoveLastEntity() { if (entities_.empty()) { return; } @@ -53,37 +53,32 @@ void Renderable::RemoveLast() { meshes_.pop_back(); } -void Renderable::Update(int index, const Mesh* mesh) { - if (index < 0 || index >= entities_.size()) { - mju_error("Invalid index %d for renderable.", index); - } - utils::Entity& entity = entities_[index]; - UpdateEntity(entity, mesh); - UpdateMeshes(index, mesh); +void Renderable::UpdateMesh(int index, const Mesh* mesh, int elem_offset, + int elem_count) { + MeshInfo& mesh_info = SetMesh(index, mesh, nullptr, elem_offset, elem_count); + UpdateEntity(index, mesh_info); } -void Renderable::Update(int index, MeshPtr mesh) { - if (index < 0 || index >= entities_.size()) { - mju_error("Invalid index %d for renderable.", index); - } - utils::Entity& entity = entities_[index]; - UpdateEntity(entity, mesh.get()); - UpdateMeshes(index, mesh.get(), std::move(mesh)); +void Renderable::UpdateMesh(int index, MeshPtr mesh, int elem_offset, + int elem_count) { + MeshInfo& mesh_info = + SetMesh(index, mesh.get(), std::move(mesh), elem_offset, elem_count); + UpdateEntity(index, mesh_info); } -void Renderable::Append(const Mesh* mesh) { - utils::Entity entity = CreateEntity(mesh); - entities_.push_back(entity); - meshes_.push_back({nullptr, mesh}); +void Renderable::AppendMesh(const Mesh* mesh, int elem_offset, int elem_count) { + MeshInfo& mesh_info = SetMesh(-1, mesh, nullptr, elem_offset, elem_count); + AppendEntity(mesh_info); } -void Renderable::Append(MeshPtr mesh) { - utils::Entity entity = CreateEntity(mesh.get()); - entities_.push_back(entity); - meshes_.push_back({std::move(mesh), mesh.get()}); +void Renderable::AppendMesh(MeshPtr mesh, int elem_offset, int elem_count) { + MeshInfo& mesh_info = + SetMesh(-1, mesh.get(), std::move(mesh), elem_offset, elem_count); + AppendEntity(mesh_info); } -utils::Entity Renderable::CreateEntity(const Mesh* mesh) { +void Renderable::AppendEntity(const MeshInfo& mesh_info) { + const Mesh* mesh = mesh_info.mesh; filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer(); if (vertex_buffer == nullptr) { mju_error("Invalid (null) vertex buffer."); @@ -100,7 +95,8 @@ utils::Entity Renderable::CreateEntity(const Mesh* mesh) { } filament::RenderableManager::Builder builder(1); - builder.geometry(0, mesh->GetPrimitiveType(), vertex_buffer, index_buffer); + builder.geometry(0, mesh->GetPrimitiveType(), vertex_buffer, index_buffer, + mesh_info.elem_offset, mesh_info.elem_count); if (mesh->HasBounds()) { builder.boundingBox(mesh->GetBounds()); } else { @@ -113,17 +109,23 @@ utils::Entity Renderable::CreateEntity(const Mesh* mesh) { builder.receiveShadows(receive_shadows_); builder.layerMask(0xff, layer_mask_); builder.priority(priority_); + builder.blendOrder(0, blend_order_); builder.screenSpaceContactShadows(true); - ; builder.build(*GetEngine(), entity); if (assigned_scene_) { assigned_scene_->addEntity(entity); } - return entity; + entities_.push_back(entity); } -void Renderable::UpdateEntity(utils::Entity entity, const Mesh* mesh) { +void Renderable::UpdateEntity(int index, const MeshInfo& mesh_info) { + if (index < 0 || index >= entities_.size()) { + mju_error("Invalid index %d for renderable.", index); + } + utils::Entity entity = entities_[index]; + + const Mesh* mesh = mesh_info.mesh; filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer(); if (vertex_buffer == nullptr) { mju_error("Invalid (null) vertex buffer."); @@ -136,16 +138,32 @@ void Renderable::UpdateEntity(utils::Entity entity, const Mesh* mesh) { filament::RenderableManager& rm = GetEngine()->getRenderableManager(); rm.setGeometryAt(rm.getInstance(entity), 0, mesh->GetPrimitiveType(), - vertex_buffer, index_buffer, 0, - index_buffer->getIndexCount()); + vertex_buffer, index_buffer, mesh_info.elem_offset, + mesh_info.elem_count); } -void Renderable::UpdateMeshes(int index, const Mesh* mesh, MeshPtr owned_mesh) { - if (index < 0 || index >= meshes_.size()) { +Renderable::MeshInfo& Renderable::SetMesh(int index, const Mesh* mesh, + MeshPtr owned_mesh, int elem_offset, + int elem_count) { + if (index == -1) { + index = meshes_.size(); + meshes_.emplace_back(); + } + if (index < 0 || index >= static_cast(meshes_.size())) { mju_error("Invalid index %d for renderable.", index); } - meshes_[index].owned_mesh = std::move(owned_mesh); - meshes_[index].mesh = mesh; + + MeshInfo* mesh_info = &meshes_[index]; + mesh_info->owned_mesh = std::move(owned_mesh); + mesh_info->mesh = mesh; + mesh_info->elem_offset = elem_offset; + mesh_info->elem_count = elem_count; + if (mesh_info->elem_count == 0) { + const int total = + mesh_info->mesh->GetFilamentIndexBuffer()->getIndexCount(); + mesh_info->elem_count = total - mesh_info->elem_offset; + } + return *mesh_info; } void Renderable::AddToScene(filament::Scene* scene) { @@ -209,6 +227,19 @@ std::uint8_t Renderable::SetPriority(std::uint8_t priority) { return prev; } +std::uint16_t Renderable::SetBlendOrder(std::uint16_t blend_order) { + std::uint16_t prev = blend_order_; + if (blend_order != blend_order_) { + blend_order_ = blend_order; + + filament::RenderableManager& rm = GetEngine()->getRenderableManager(); + for (utils::Entity& entity : entities_) { + rm.setBlendOrderAt(rm.getInstance(entity), 0, blend_order_); + } + } + return prev; +} + void Renderable::SetCastShadows(bool cast_shadows) { if (cast_shadows_ != cast_shadows) { cast_shadows_ = cast_shadows; @@ -246,8 +277,8 @@ void Renderable::SetWireframe(bool wireframe) { filament::IndexBuffer* index_buffer = mesh->GetFilamentIndexBuffer(); rm.setGeometryAt(rm.getInstance(entity), 0, wireframe_ ? kWireframeType : mesh->GetPrimitiveType(), - vertex_buffer, index_buffer, 0, - index_buffer->getIndexCount()); + vertex_buffer, index_buffer, meshes_[i].elem_offset, + meshes_[i].elem_count); } } } diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 74394980..32577beb 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -26,7 +26,14 @@ namespace mujoco { -// Manages a collection of related filament Renderable Entities. +// A collection of meshes and a material that, together, define an object that +// can be rendered in a scene. +// +// Meshes can be added to the Renderable either by unique_ptr or raw pointer. +// This determines whether or not the Renderable takes ownership of the mesh. +// +// Internally, the Renderable creates a filament::Entity for each mesh and +// assigns the same material instance to all of them. class Renderable { public: // Default filament values for priority and layer mask. @@ -39,42 +46,51 @@ class Renderable { Renderable(const Renderable&) = delete; Renderable& operator=(const Renderable&) = delete; - // Appends a new renderable entity built from the given mesh. - void Append(const Mesh* mesh); - void Append(MeshPtr mesh); + // Appends a mesh to the renderable. The elem_offset and elem_count parameters + // can be used to specify a submesh to append. If elem_count is 0, assumes + // the entire mesh should be appended. + void AppendMesh(const Mesh* mesh, int elem_offset = 0, int elem_count = 0); + void AppendMesh(MeshPtr mesh, int elem_offset = 0, int elem_count = 0); - // Updates the entity at the index with new mesh. - void Update(int index, const Mesh* mesh); - void Update(int index, MeshPtr mesh); + // Replaces the mesh at the index with a new mesh. The elem_offset and + // elem_count parameters can be used to specify a submesh to append. If + // elem_count is 0, assumes the entire mesh should be appended. + void UpdateMesh(int index, const Mesh* mesh, int elem_offset = 0, + int elem_count = 0); + void UpdateMesh(int index, MeshPtr mesh, int elem_offset = 0, + int elem_count = 0); - // Removes the last entity. - void RemoveLast(); + // Returns the number of meshes that define the renderable. + int GetNumMeshes() const { return meshes_.size(); } - // Returns the entity at the given index. - utils::Entity operator[](int index) { return entities_[index]; } - - // Returns the number of Entities that make up this renderable. - int GetNumEntities() const { return entities_.size(); } - - // Hides all managed entities. + // Sets the layer mask for the managed filament Entities. Layer masks can be + // used to show/hide the renderable in different views. Returns the previous + // layer mask. std::uint8_t SetLayerMask(std::uint8_t mask); - // Sets the priority of all managed entities. + // Sets the priority for the managed filament Entities. The priority + // determines the order in which renderables are rendered. Returns the + // previous priority. std::uint8_t SetPriority(std::uint8_t priority); - // Disables the renderables from casting shadows. + // Sets the blend order of the managed filament entities. This determines the + // order in which renderables are blended together. Returns the previous blend + // order. + std::uint16_t SetBlendOrder(std::uint16_t blend_order); + + // Disables the renderable from casting shadows. void SetCastShadows(bool cast_shadows); - // Disables the renderables from receiving shadows. + // Disables the renderable from receiving shadows. void SetReceiveShadows(bool receive_shadows); - // If true, forces all entities to be rendered as lines. + // If true, forces all meshes to be rendered using Lines primitives. void SetWireframe(bool wireframe); - // Adds all managed entities to the given filament Scene. + // Adds the renderable to the given filament Scene. void AddToScene(filament::Scene* scene); - // Removes all managed entities from the given filament Scene. + // Removes the renderable from the given filament Scene. void RemoveFromScene(filament::Scene* scene); // Sets the material instance for all managed entities. @@ -86,23 +102,40 @@ class Renderable { // Returns the filament Engine managing the renderables. filament::Engine* GetEngine(); - private: - utils::Entity CreateEntity(const Mesh* mesh); - void UpdateEntity(utils::Entity entity, const Mesh* mesh); - void UpdateMeshes(int index, const Mesh* mesh, MeshPtr owned_mesh = nullptr); + // Returns the underlying filament::entity for the given mesh. + utils::Entity operator[](int index) { return entities_[index]; } - struct MeshWrapper { + private: + struct MeshInfo { MeshPtr owned_mesh; const Mesh* mesh = nullptr; + int elem_offset = 0; + int elem_count = 0; }; + // Sets the mesh information for the mesh at the given index. If index is -1, + // a new mesh will be appended to the renderable. + MeshInfo& SetMesh(int index, const Mesh* mesh, MeshPtr owned_mesh, + int elem_offset, int elem_count); + + // Appends a new filament::Entity to the renderable, configured to use the + // given mesh. + void AppendEntity(const MeshInfo& mesh_info); + + // Updates the filament::Entity at the given index to use the given mesh. + void UpdateEntity(int index, const MeshInfo& mesh_info); + + // Removes the last filament::Entity from the renderable. + void RemoveLastEntity(); + Material material_; filament::Scene* assigned_scene_ = nullptr; filament::MaterialInstance* material_instance_ = nullptr; std::vector entities_; - std::vector meshes_; + std::vector meshes_; std::uint8_t priority_ = kDefaultPriority; std::uint8_t layer_mask_ = kDefaultLayerMask; + std::uint16_t blend_order_ = 0; bool wireframe_ = false; bool cast_shadows_ = true; bool receive_shadows_ = true; diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index a62d3749..f8bf04f5 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -93,15 +93,15 @@ static void AddMesh(Renderable& renderable, ModelObjects* model_objs, if (mesh == nullptr) { mju_error("Unknown mesh %d", data_id); } - renderable.Append(mesh); + renderable.AppendMesh(mesh); } static void AddGeom(Renderable& renderable, ModelObjects* model_objs, const mjvScene* scene, const mjvGeom& geom) { if (geom.type == mjGEOM_FLEX) { - renderable.Append(model_objs->CreateFlexMesh(scene, geom)); + renderable.AppendMesh(model_objs->CreateFlexMesh(scene, geom)); } else if (geom.type == mjGEOM_SKIN) { - renderable.Append(model_objs->CreateSkinMesh(scene, geom)); + renderable.AppendMesh(model_objs->CreateSkinMesh(scene, geom)); } } @@ -111,7 +111,7 @@ static void AddHeightField(Renderable& renderable, ModelObjects* model_objs, if (mesh == nullptr) { mju_error("Unknown height field %d", hfield_id); } - renderable.Append(mesh); + renderable.AppendMesh(mesh); } static void AddShape(Renderable& renderable, ModelObjects* model_objs, @@ -120,7 +120,7 @@ static void AddShape(Renderable& renderable, ModelObjects* model_objs, if (mesh == nullptr) { mju_error("Unknown shape %d", shape_type); } - renderable.Append(mesh); + renderable.AppendMesh(mesh); } static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, @@ -211,7 +211,7 @@ static void SetGeomTransform(Renderable& renderable, const mjvGeom& geom) { float3 size = ReadFloat3(geom.size); filament::TransformManager& tm = renderable.GetEngine()->getTransformManager(); - for (int j = 0; j < renderable.GetNumEntities(); ++j) { + for (int j = 0; j < renderable.GetNumMeshes(); ++j) { const utils::Entity& entity = renderable[j]; // Update object transform. From 8f3ed662eb9d951d17b2311a956a221398d5e85c Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 15 Apr 2026 04:22:16 -0700 Subject: [PATCH 065/251] Hoist solver stack allocations out of iteration loop PiperOrigin-RevId: 900085248 Change-Id: I2bde8fafc8a4801a92c39aa9a13af0ea7475843d --- src/engine/engine_solver.c | 33 ++++++++++++++------------------- 1 file changed, 14 insertions(+), 19 deletions(-) diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 4fb1224d..b03ad913 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -813,6 +813,8 @@ typedef struct { // Newton arrays, known-size (PrimalAllocate) mjtNum* D; // constraint inertia (nefc x 1) + mjtNum* cholupd; // scratch for rank-1 Cholesky updates (nv x 1) + mjtNum* LTJ; // L'*J for cone Cholesky updates (6 x nv) int* H_rowadr; // Hessian row addresses (nv x 1) int* H_rownnz; // Hessian row nonzeros (nv x 1) int* HT_rownnz; // Hessian transpose row nonzeros (nv x 1) @@ -988,6 +990,10 @@ static void PrimalAllocate(mjData* d, mjPrimalContext* ctx, int flg_Newton) { // Newton only, known-size arrays if (flg_Newton) { ctx->D = mjSTACKALLOC(d, nefc, mjtNum); + ctx->cholupd = mjSTACKALLOC(d, nv, mjtNum); + if (ctx->is_elliptic) { + ctx->LTJ = mjSTACKALLOC(d, 6*nv, mjtNum); + } // sparse Newton only if (ctx->is_sparse) { @@ -1678,10 +1684,7 @@ static void HessianCone(mjData* d, mjPrimalContext* ctx) { // start with Hcone = H mju_copy(ctx->Lcone, ctx->L, ctx->nL); - mj_markStack(d); - - // storage for L'*J - mjtNum* LTJ = mjSTACKALLOC(d, 6*nv, mjtNum); + mjtNum* LTJ = ctx->LTJ; // add contributions for (int i=0; i < nefc; i++) { @@ -1737,18 +1740,13 @@ static void HessianCone(mjData* d, mjPrimalContext* ctx) { i += (dim-1); } } - - mj_freeStack(d); } // incremental update to Hessian factor due to changes in efc_state static void HessianIncremental(mjData* d, mjPrimalContext* ctx, const int* oldstate) { int rank, nv = ctx->nv, nefc = ctx->nefc; - mj_markStack(d); - - // local space - mjtNum* vec = mjSTACKALLOC(d, nv, mjtNum); + mjtNum* cholupd = ctx->cholupd; // clear update counter ctx->nupdate = 0; @@ -1769,27 +1767,26 @@ static void HessianIncremental(mjData* d, mjPrimalContext* ctx, const int* oldst // perform update if flagged if (flag_update != -1) { - // update with vec = J(i,:)*sqrt(D[i])) + // update with cholupd = J(i,:)*sqrt(D[i])) if (ctx->is_sparse) { // get nnz and adr of row i const int nnz = ctx->J_rownnz[i], adr = ctx->J_rowadr[i]; - // scale vec - mju_scl(vec, ctx->J+adr, mju_sqrt(ctx->efc_D[i]), nnz); + // scale cholupd + mju_scl(cholupd, ctx->J+adr, mju_sqrt(ctx->efc_D[i]), nnz); // sparse update or downdate - rank = mju_cholUpdateSparse(ctx->L, vec, nv, flag_update, + rank = mju_cholUpdateSparse(ctx->L, cholupd, nv, flag_update, ctx->L_rownnz, ctx->L_rowadr, ctx->L_colind, nnz, ctx->J_colind+adr, d); } else { - mju_scl(vec, ctx->J+i*nv, mju_sqrt(ctx->efc_D[i]), nv); - rank = mju_cholUpdate(ctx->L, vec, nv, flag_update); + mju_scl(cholupd, ctx->J+i*nv, mju_sqrt(ctx->efc_D[i]), nv); + rank = mju_cholUpdate(ctx->L, cholupd, nv, flag_update); } ctx->nupdate++; // recompute H directly if accuracy lost if (rank < nv) { - mj_freeStack(d); FactorizeHessian(d, ctx, /*flg_recompute=*/1); // nothing else to do @@ -1802,8 +1799,6 @@ static void HessianIncremental(mjData* d, mjPrimalContext* ctx, const int* oldst if (ctx->ncone) { HessianCone(d, ctx); } - - mj_freeStack(d); } From 62cffb153637ee5037f127b2dde8317dc8580ab1 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 15 Apr 2026 06:55:33 -0700 Subject: [PATCH 066/251] Update GuiView to use SceneView and Renderables. PiperOrigin-RevId: 900145167 Change-Id: I613c9fe171e97f8a4cd08c0c63e9706e09cbd202 --- src/experimental/filament/assets/unlit_ui.mat | 4 +- .../filament/filament/filament_context.cc | 18 +- .../filament/filament/gui_view.cc | 190 ++++++------------ src/experimental/filament/filament/gui_view.h | 49 ++--- .../filament/filament/material.cc | 5 + src/experimental/filament/filament/material.h | 1 + .../filament/filament/scene_view.cc | 37 ++++ .../filament/filament/scene_view.h | 17 +- 8 files changed, 147 insertions(+), 174 deletions(-) diff --git a/src/experimental/filament/assets/unlit_ui.mat b/src/experimental/filament/assets/unlit_ui.mat index 0148f28e..4659b002 100644 --- a/src/experimental/filament/assets/unlit_ui.mat +++ b/src/experimental/filament/assets/unlit_ui.mat @@ -17,7 +17,7 @@ material { parameters : [ { type : sampler2d, - name : glyph + name : BaseColor } ], requires : [ @@ -35,7 +35,7 @@ fragment { prepareMaterial(material); vec2 uv = getUV0(); uv.y = 1.0 - uv.y; - vec4 tex_color = texture(materialParams_glyph, uv); + vec4 tex_color = texture(materialParams_BaseColor, uv); material.baseColor = getColor() * tex_color; material.baseColor.rgb *= material.baseColor.a; } diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index c01a95b2..a7824f4f 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -97,7 +97,7 @@ void FilamentContext::Init(const mjModel* model) { scene_bridge_ = std::make_unique(object_manager_.get(), model, scene_view_.get()); gui_view_ = std::make_unique( - engine_, object_manager_->GetMaterial(ObjectManager::kUnlitUi)); + scene_view_.get(), object_manager_->GetMaterial(ObjectManager::kUnlitUi)); // Set clear options. filament::Renderer::ClearOptions opts; @@ -131,7 +131,7 @@ void FilamentContext::Render(const mjrRect& viewport, const mjvScene* scene) { // Prepare the filament Renderable that contains the GUI draw commands. We // must call this function even if we do not plan on rendering the GUI to // ensure the ImGui state is updated. - gui_view_->UpdateRenderable(); + gui_view_->Update(); } last_render_mode_ = SceneView::DrawMode::kNormal; @@ -154,12 +154,9 @@ void FilamentContext::Render(const mjrRect& viewport, const mjvScene* scene) { request.draw_mode = last_render_mode_; request.viewport = viewport; request.camera = last_camera_; + request.enable_ux = (gui_swap_chain_target_ == kWindowSwapChain); + request.gui_scale = gui_view_ ? gui_view_->GetScale() : 1.0f; scene_view_->Render(renderer_, request); - - if (gui_view_ && gui_swap_chain_target_ == kWindowSwapChain) { - gui_view_->Render(renderer_); - } - renderer_->endFrame(); } @@ -231,13 +228,10 @@ void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb, request.viewport = viewport; request.target = color_target_.get(); request.camera = last_camera_; + request.enable_ux = (gui_swap_chain_target_ == kOffscreenSwapChain); + request.gui_scale = gui_view_ ? gui_view_->GetScale() : 1.0f; scene_view_->Render(renderer_, request); - // Render the GUI to the texture as well if requested. - if (gui_view_ && gui_swap_chain_target_ == kOffscreenSwapChain) { - gui_view_->Render(renderer_, color_target_.get()); - } - const size_t num_bytes = viewport.width * viewport.height * 3; color_target_->ReadColorPixels(renderer_, rgb, num_bytes); diff --git a/src/experimental/filament/filament/gui_view.cc b/src/experimental/filament/filament/gui_view.cc index a6572366..5802b06d 100644 --- a/src/experimental/filament/filament/gui_view.cc +++ b/src/experimental/filament/filament/gui_view.cc @@ -18,70 +18,26 @@ #include #include #include -#include #include #include -#include -#include -#include -#include -#include +#include #include -#include -#include #include +#include "experimental/filament/filament/material.h" #include "experimental/filament/filament/mesh.h" -#include "experimental/filament/filament/render_target.h" +#include "experimental/filament/filament/renderable.h" +#include "experimental/filament/filament/scene_view.h" #include "experimental/filament/filament/texture.h" namespace mujoco { -using filament::math::float4; - -static constexpr auto kTriangles = - filament::RenderableManager::PrimitiveType::TRIANGLES; - -GuiView::GuiView(filament::Engine* engine, filament::Material* ui_material) - : engine_(engine), material_(ui_material) { - auto& em = utils::EntityManager::get(); - scene_ = engine_->createScene(); - camera_ = engine_->createCamera(em.create()); - view_ = engine_->createView(); - renderable_ = em.create(); - view_->setScene(scene_); - view_->setCamera(camera_); - view_->setPostProcessingEnabled(false); +GuiView::GuiView(SceneView* scene_view, filament::Material* ui_material) + : scene_view_(scene_view), material_(ui_material) { } GuiView::~GuiView() { - if (num_elements_ > 0) { - scene_->remove(renderable_); - auto& rm = engine_->getRenderableManager(); - rm.destroy(renderable_); - } - auto& em = utils::EntityManager::get(); - em.destroy(renderable_); - meshes_.clear(); - for (auto& instance : instances_) { - engine_->destroy(instance); - } - textures_.clear(); - engine_->destroyCameraComponent(camera_->getEntity()); - engine_->destroy(view_); - engine_->destroy(scene_); -} - -void GuiView::ResetRenderable() { - auto& em = utils::EntityManager::get(); - if (!renderable_.isNull()) { - scene_->remove(renderable_); - auto& rm = engine_->getRenderableManager(); - rm.destroy(renderable_); - em.destroy(renderable_); - renderable_ = utils::Entity(); - } - meshes_.clear(); + PrepareRenderables(0); } uintptr_t GuiView::UploadImage(uintptr_t tex_id, const uint8_t* pixels, @@ -116,7 +72,7 @@ uintptr_t GuiView::UploadImage(uintptr_t tex_id, const uint8_t* pixels, config.target = mjTEXTURE_2D; config.format = bpp == 4 ? mjPIXEL_FORMAT_RGBA8 : mjPIXEL_FORMAT_RGB8; config.color_space = mjCOLORSPACE_LINEAR; - texture = std::make_unique(engine_, config); + texture = std::make_unique(scene_view_->GetEngine(), config); } // Create a copy of the image to pass it to filament as we don't know the @@ -153,7 +109,8 @@ void GuiView::CreateTexture(ImTextureData* data) { config.color_space = mjCOLORSPACE_LINEAR; const uintptr_t tex_id = textures_.size() + 1; - textures_[tex_id] = std::make_unique(engine_, config); + textures_[tex_id] = + std::make_unique(scene_view_->GetEngine(), config); data->SetTexID((ImTextureID)tex_id); UpdateTexture(data); } @@ -183,21 +140,22 @@ void GuiView::DestroyTexture(ImTextureData* data) { } } -void GuiView::UpdateRenderable() { +void GuiView::Update() { if (!ImGui::GetCurrentContext()) { + PrepareRenderables(0); return; } // Prepare the imgui draw commands. We must call this function even if we do // not plan on rendering anything to ensure imgui state is updated. ImGui::Render(); - auto& rm = engine_->getRenderableManager(); ImGuiIO& io = ImGui::GetIO(); const ImVec2& size = io.DisplaySize; const ImVec2& scale = io.DisplayFramebufferScale; ImDrawData* commands = ImGui::GetDrawData(); - if (!commands) { + if (!commands || size.x == 0 || size.y == 0) { + PrepareRenderables(0); return; } commands->ScaleClipRects(scale); @@ -243,39 +201,13 @@ void GuiView::UpdateRenderable() { } } - if (size.x == 0 || size.y == 0 || num_elements == 0) { - if (num_elements_ > 0) { - scene_->remove(renderable_); - rm.destroy(renderable_); - } - num_elements_ = 0; + PrepareRenderables(num_elements); + if (num_elements == 0) { return; } - view_->setViewport( - filament::Viewport(0.f, 0.f, size.x * scale.x, size.y * scale.y)); - camera_->setProjection(filament::Camera::Projection::ORTHO, 0.0, size.x, - size.y, 0.0, 0.0, 1.0); - - if (num_elements != num_elements_) { - if (num_elements_ > 0) { - scene_->remove(renderable_); - rm.destroy(renderable_); - } - - num_elements_ = num_elements; - - filament::RenderableManager::Builder builder(num_elements_); - builder.boundingBox({{-100, -100, -100}, {100, 100, 100}}); - builder.culling(false); - builder.build(*engine_, renderable_); - scene_->addEntity(renderable_); - } meshes_.clear(); - - auto ri = rm.getInstance(renderable_); - - int drawable_index = 0; + int renderable_index = 0; for (int n = 0; n < commands->CmdListsCount; ++n) { const ImDrawList* cmds = commands->CmdLists[n]; @@ -297,70 +229,70 @@ void GuiView::UpdateRenderable() { data.indices = cmds->IdxBuffer.Data; data.index_type = mjINDEX_TYPE_USHORT; data.primitive_type = mjPRIM_TYPE_TRIANGLES; - meshes_.push_back(std::make_unique(engine_, data)); - const auto& mesh = meshes_.back(); + meshes_.push_back(std::make_unique(scene_view_->GetEngine(), data)); + + const Mesh* mesh = meshes_.back().get(); int index_offset = 0; for (const ImDrawCmd& command : cmds->CmdBuffer) { const int width = size.x * scale.x; const int height = size.y * scale.y; - int clip_left = command.ClipRect.x; - int clip_bottom = height - command.ClipRect.w; - int clip_width = command.ClipRect.z - command.ClipRect.x; - int clip_height = command.ClipRect.w - command.ClipRect.y; + auto& renderable = renderables_[renderable_index]; + if (renderable->GetNumMeshes() == 0) { + renderable->AppendMesh(mesh, index_offset, command.ElemCount); + } else { + renderable->UpdateMesh(0, mesh, index_offset, command.ElemCount); + } + + Material::Textures textures; + textures.color = textures_[command.GetTexID()].get(); + renderable->GetMaterial().UpdateTextures(textures); + + Material::Params properties; + properties.scissor[0] = command.ClipRect.x; + properties.scissor[1] = height - command.ClipRect.w; + properties.scissor[2] = command.ClipRect.z - command.ClipRect.x; + properties.scissor[3] = command.ClipRect.w - command.ClipRect.y; // Modal dialogs try to cover the whole window, but also a little outside // of it. This doesn't work well with filament's scissor test, so we clip // them to the window. - if (clip_left < 0 || clip_bottom < 0) { - clip_left = 0; - clip_bottom = 0; - clip_width = width; - clip_height = height; + if (properties.scissor[0] < 0 || properties.scissor[1] < 0) { + properties.scissor[0] = 0; + properties.scissor[1] = 0; + properties.scissor[2] = width; + properties.scissor[3] = height; } - - mjrRect clip_rect{clip_left, clip_bottom, clip_width, clip_height}; - rm.setMaterialInstanceAt( - ri, drawable_index, - GetMaterialInstance(drawable_index, clip_rect, command.GetTexID())); - rm.setGeometryAt( - ri, drawable_index, kTriangles, mesh->GetFilamentVertexBuffer(), - mesh->GetFilamentIndexBuffer(), index_offset, command.ElemCount); - rm.setBlendOrderAt(ri, drawable_index, drawable_index); + renderable->GetMaterial().UpdateParams(properties); index_offset += command.ElemCount; - ++drawable_index; + ++renderable_index; } } } -filament::MaterialInstance* GuiView::GetMaterialInstance(int index, - mjrRect rect, - uintptr_t texture_id) { - while (index >= instances_.size()) { - instances_.push_back(material_->createInstance()); - } +void GuiView::PrepareRenderables(int count) { + while (renderables_.size() < count) { + auto& r = renderables_.emplace_back( + std::make_unique(scene_view_->GetEngine())); + r->SetCastShadows(false); + r->SetReceiveShadows(false); + r->SetBlendOrder(static_cast(renderables_.size())); - auto iter = textures_.find(texture_id); - if (iter == textures_.end()) { - mju_error("Texture not found: %lu", texture_id); + Material& material = r->GetMaterial(); + Material::DrawMode mode = Material::DrawMode::kNormal; + material.SetMaterial(mode, material_); + r->SetMaterialInstance(material.GetMaterialInstance(mode)); + scene_view_->AddToUxScene(r.get()); + } + while (renderables_.size() > count) { + scene_view_->RemoveFromUxScene(renderables_.back().get()); + renderables_.pop_back(); } - - filament::MaterialInstance* instance = instances_[index]; - instance->setParameter("glyph", iter->second->GetFilamentTexture(), - filament::TextureSampler()); - instance->setScissor(rect.left, rect.bottom, rect.width, rect.height); - return instance; } -void GuiView::Render(filament::Renderer* renderer, RenderTarget* target) { - if (num_elements_ == 0) { - return; - } - - view_->setRenderTarget(target ? target->GetFilamentRenderTarget() : nullptr); - renderer->render(view_); - view_->setRenderTarget(nullptr); +float GuiView::GetScale() const { + return ImGui::GetIO().DisplayFramebufferScale.x; } static ImVec2 ClipSpaceToWindowCoordinates(float x, float y) { diff --git a/src/experimental/filament/filament/gui_view.h b/src/experimental/filament/filament/gui_view.h index 0346ca57..25dd8530 100644 --- a/src/experimental/filament/filament/gui_view.h +++ b/src/experimental/filament/filament/gui_view.h @@ -21,60 +21,49 @@ #include #include -#include -#include #include -#include -#include -#include -#include -#include #include "experimental/filament/filament/mesh.h" -#include "experimental/filament/filament/render_target.h" +#include "experimental/filament/filament/renderable.h" +#include "experimental/filament/filament/scene_view.h" #include "experimental/filament/filament/texture.h" namespace mujoco { -// A filament::View that contains a filament::Scene used for rendering the GUI. +// Manages Renderables that will be added a SceneView's UX scene. class GuiView { public: - GuiView(filament::Engine* engine, filament::Material* ui_material); + GuiView(SceneView* scene_view, filament::Material* ui_material); ~GuiView(); - // Prepares the UX scene renderable using data from the current ImGui state. - // This function must be called once per frame to ensure ImGui state is - // correctly synced. - void UpdateRenderable(); + // Prepares the Renderables using data from the current ImGui state. This + // function must be called once per frame to ensure ImGui state is correctly + // synced. + void Update(); - void Render(filament::Renderer* renderer, RenderTarget* target = nullptr); + // Returns the current ImGui scale factor. + float GetScale() const; // Uploads texture to be used with ImGui's Image and ImageButton functions. uintptr_t UploadImage(uintptr_t tex_id, const uint8_t* pixels, int width, int height, int bpp); + GuiView(const GuiView&) = delete; + GuiView& operator=(const GuiView&) = delete; + private: + // Ensures exactly `count` Renderables exist, creating or destroying them as + // needed. + void PrepareRenderables(int count); + void CreateTexture(ImTextureData* data); void UpdateTexture(ImTextureData* data); void DestroyTexture(ImTextureData* data); - // Returns the filament::MaterialInstance configured to draw into the given - // scissor rect. - filament::MaterialInstance* GetMaterialInstance(int index, mjrRect rect, - uintptr_t texture_id); - - // Clears the filament::Scene of the UX renderable and releases all buffers. - void ResetRenderable(); - - filament::Engine* engine_ = nullptr; - filament::Scene* scene_ = nullptr; - filament::Camera* camera_ = nullptr; - filament::View* view_ = nullptr; + SceneView* scene_view_ = nullptr; filament::Material* material_ = nullptr; - utils::Entity renderable_; + std::vector> renderables_; std::vector meshes_; - std::vector instances_; std::unordered_map> textures_; - int num_elements_ = 0; }; // Draws text at the given screen coordinates in clip space (i.e. [-1,-1,-1] to diff --git a/src/experimental/filament/filament/material.cc b/src/experimental/filament/filament/material.cc index 10538d28..9dc7863c 100644 --- a/src/experimental/filament/filament/material.cc +++ b/src/experimental/filament/filament/material.cc @@ -72,6 +72,11 @@ void Material::UpdateMaterialInstances() { return; } + if (params_.scissor[2] != 0 && params_.scissor[3] != 0) { + instance->setScissor(params_.scissor[0], params_.scissor[1], + params_.scissor[2], params_.scissor[3]); + } + const filament::Material* material = instance->getMaterial(); if (material->hasParameter("BaseColorFactor")) { instance->setParameter("BaseColorFactor", filament::RgbaType::sRGB, diff --git a/src/experimental/filament/filament/material.h b/src/experimental/filament/filament/material.h index 72024aa9..eaba6103 100644 --- a/src/experimental/filament/filament/material.h +++ b/src/experimental/filament/filament/material.h @@ -55,6 +55,7 @@ class Material { filament::math::float2 tex_repeat = {1, 1}; filament::math::float3 uv_scale = {1, 1, 1}; filament::math::float3 uv_offset = {0, 0, 0}; + filament::math::float4 scissor = {0, 0, 0, 0}; float specular = -1.0f; float glossiness = -1.0f; float metallic = -1.0f; diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 5c8f38b1..41f3d3ad 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -129,7 +129,9 @@ static void SetupReflectionCamera(const mat4& surface_xform, SceneView::SceneView(filament::Engine* engine) : engine_(engine) { scene_ = engine->createScene(); + ux_scene_ = engine->createScene(); camera_ = engine->createCamera(utils::EntityManager::get().create()); + ux_camera_ = engine->createCamera(utils::EntityManager::get().create()); reflect_camera_ = engine->createCamera(utils::EntityManager::get().create()); for (auto& view : views_) { @@ -139,6 +141,12 @@ SceneView::SceneView(filament::Engine* engine) : engine_(engine) { view->setVisibleLayers(0xff, mjCAT_ALL); } + ux_view_ = engine->createView(); + ux_view_->setScene(ux_scene_); + ux_view_->setCamera(ux_camera_); + ux_view_->setPostProcessingEnabled(false); + ux_view_->setShadowingEnabled(false); + reflect_view_ = engine->createView(); reflect_view_->setScene(scene_); reflect_view_->setCamera(reflect_camera_); @@ -166,16 +174,22 @@ SceneView::~SceneView() { for (auto& renderable : renderables_) { renderable->RemoveFromScene(scene_); } + for (auto& renderable : ux_renderables_) { + renderable->RemoveFromScene(ux_scene_); + } lights_.clear(); renderables_.clear(); reflect_targets_.clear(); engine_->destroyCameraComponent(reflect_camera_->getEntity()); engine_->destroy(reflect_view_); + engine_->destroyCameraComponent(ux_camera_->getEntity()); + engine_->destroy(ux_view_); engine_->destroyCameraComponent(camera_->getEntity()); if (color_grading_) { engine_->destroy(color_grading_); } engine_->destroy(scene_); + engine_->destroy(ux_scene_); for (auto& view : views_) { engine_->destroy(view); } @@ -212,6 +226,18 @@ void SceneView::RemoveFromScene(Renderable* renderable) { } } +void SceneView::AddToUxScene(Renderable* renderable) { + if (ux_renderables_.insert(renderable).second) { + renderable->AddToScene(ux_scene_); + } +} + +void SceneView::RemoveFromUxScene(Renderable* renderable) { + if (ux_renderables_.erase(renderable)) { + renderable->RemoveFromScene(ux_scene_); + } +} + void SceneView::AddToScene(filament::Skybox* skybox) { skybox_ = skybox; scene_->setSkybox(skybox); @@ -231,6 +257,7 @@ void SceneView::Render(filament::Renderer* renderer, for (auto& view : views_) { view->setViewport(viewport); } + ux_view_->setViewport(viewport); reflect_view_->setViewport(viewport); SetupCamera(request.camera, viewport, camera_); @@ -279,6 +306,16 @@ void SceneView::Render(filament::Renderer* renderer, renderer->render(view); view->setRenderTarget(nullptr); + if (request.enable_ux) { + ux_camera_->setProjection(filament::Camera::Projection::ORTHO, 0.0f, + viewport.width / request.gui_scale, + viewport.height / request.gui_scale, 0.0f, 0.0f, + 1.0f); + ux_view_->setRenderTarget(render_target); + renderer->render(ux_view_); + ux_view_->setRenderTarget(nullptr); + } + if (request.target) { view->setMultiSampleAntiAliasingOptions(options); } diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index d7f02021..d6546d7c 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -38,7 +38,8 @@ namespace mujoco { // // The filament Scene is populated with the objects (e.g. lights, renderables, // skybox, etc.). It manages multiple views to support a variety of draw modes -// (e.g. normal, depth, segmentation, etc.) as well as reflective surfaces. +// (e.g. normal, depth, segmentation, etc.) as well as reflective surfaces. It +// also manages a separate scene and view for UX rendering. class SceneView { public: SceneView(filament::Engine* engine); @@ -52,6 +53,10 @@ class SceneView { void AddToScene(filament::Skybox* skybox); void RemoveFromScene(filament::Skybox* skybox); + // Adds/removes entities from the UX scene, which is rendered separately. + void AddToUxScene(Renderable* renderable); + void RemoveFromUxScene(Renderable* renderable); + // Parameters for rendering the scene. using DrawMode = Material::DrawMode; struct RenderRequest { @@ -63,6 +68,10 @@ class SceneView { mjvGLCamera camera; // An optional render target into which the scene will be rendered. RenderTarget* target = nullptr; + // Whether or not to render the UX as a separate pass. + bool enable_ux = false; + // The scale factor to use for UX rendering. + float gui_scale = 1.0f; }; // Renders the scene. @@ -89,6 +98,7 @@ class SceneView { filament::Engine* engine_ = nullptr; filament::Scene* scene_ = nullptr; + filament::Scene* ux_scene_ = nullptr; filament::Camera* camera_ = nullptr; filament::ColorGrading* color_grading_ = nullptr; ColorGradingOptions color_grading_options_; @@ -100,6 +110,11 @@ class SceneView { std::unordered_set renderables_; filament::Skybox* skybox_ = nullptr; + // Custom view for UX. + filament::View* ux_view_ = nullptr; + filament::Camera* ux_camera_ = nullptr; + std::unordered_set ux_renderables_; + // Custom view and camera for reflective surfaces. filament::View* reflect_view_ = nullptr; filament::Camera* reflect_camera_ = nullptr; From a2d0e33c0ff16c69815876feb16b6f9bcb6764cf Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 15 Apr 2026 07:19:03 -0700 Subject: [PATCH 067/251] 2-3x speedup of sparse matrix squaring. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Split symbolic and numeric phases for sparse `M'*diag*M` computation. Microseconds per call for the monolithic vs the split approach for the 100_humanoids and 2humanoid100 models: ``` +-------+------+----------+------------+---------+ | Model | Arch | Col (µs) | Split (µs) | Speedup | +-------+------+----------+------------+---------+ | 2H100 | x86 | 238.3 | 74.5 | 3.2x | +-------+------+----------+------------+---------+ | | ARM | 111.6 | 53.2 | 2.1x | +-------+------+----------+------------+---------+ | 100H | x86 | 1325.3 | 656.2 | 2.0x | +-------+------+----------+------------+---------+ | | ARM | 594.8 | 306.6 | 1.9x | +-------+------+----------+------------+---------+ ``` PiperOrigin-RevId: 900154308 Change-Id: Ia6e9b8e196e2ed37b723a0faf60e9731303a9619 --- src/engine/engine_core_constraint.c | 26 +- src/engine/engine_solver.c | 44 +- src/engine/engine_util_sparse.c | 316 ++++++- src/engine/engine_util_sparse.h | 24 +- test/benchmark/chol_benchmark_test.cc | 37 +- .../engine_util_sparse_benchmark_test.cc | 257 ++---- test/benchmark/sqrmat_benchmark_test.cc | 423 +++++++++ test/engine/engine_util_sparse_test.cc | 800 ++++++++++-------- 8 files changed, 1301 insertions(+), 626 deletions(-) create mode 100644 test/benchmark/sqrmat_benchmark_test.cc diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 9bbece11..170d1f3c 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -2965,10 +2965,11 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { return; } - // pre-count A nonzeros (compute AR_rownnz, AR_rowadr) - d->nA = mju_sqrMatTDSparseCount(d->efc_AR_rownnz, d->efc_AR_rowadr, nefc, - BT_rownnz, BT_rowadr, BT_colind, - B_rownnz, B_rowadr, B_colind, B_rowsuper, d, /*flg_upper=*/1); + int* diagind = mjSTACKALLOC(d, nefc, int); + d->nA = mju_sqrMatTDSparseSymbolic( + d->efc_AR_rownnz, d->efc_AR_rowadr, NULL, diagind, + nv, nefc, BT_rownnz, BT_rowadr, BT_colind, + B_rownnz, B_rowadr, B_colind, B_rowsuper, d); // allocate A values and column indices on arena d->efc_AR = mj_arenaAllocByte(d, sizeof(mjtNum) * d->nA, _Alignof(mjtNum)); @@ -2981,12 +2982,17 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { return; } - // A = B * B' - int* diagind = mjSTACKALLOC(d, nefc, int); - mju_sqrMatTDSparse(d->efc_AR, BT, B, NULL, nv, nefc, - d->efc_AR_rownnz, d->efc_AR_rowadr, d->efc_AR_colind, - BT_rownnz, BT_rowadr, BT_colind, NULL, - B_rownnz, B_rowadr, B_colind, B_rowsuper, d, diagind); + // A = B * B': symbolic phase + mju_sqrMatTDSparseSymbolic( + d->efc_AR_rownnz, d->efc_AR_rowadr, d->efc_AR_colind, diagind, + nv, nefc, BT_rownnz, BT_rowadr, BT_colind, + B_rownnz, B_rowadr, B_colind, B_rowsuper, d); + + // A = B * B': numeric phase + mju_sqrMatTDSparseNumeric( + d->efc_AR, nefc, d->efc_AR_rownnz, d->efc_AR_rowadr, + d->efc_AR_colind, diagind, BT, BT_rownnz, BT_rowadr, + BT_colind, B, B_rownnz, B_rowadr, B_colind, B_rowsuper, NULL, d); // AR = A + diag(R) for (int i=0; i < nefc; i++) { diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index b03ad913..b02302fc 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -1530,10 +1530,10 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { // sparse if (ctx->is_sparse) { // initialize Hessian rowadr, rownnz; get total nonzeros - ctx->nH = mju_sqrMatTDSparseCount(ctx->H_rownnz, ctx->H_rowadr, nv, - ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind, - ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, - ctx->JT_rowsuper, d, /*flg_upper=*/0); + ctx->nH = mju_sqrMatTDSparseSymbolic( + ctx->H_rownnz, ctx->H_rowadr, NULL, NULL, + nefc, nv, ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind, + ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, ctx->JT_rowsuper, d); // add M nonzeros to Hessian total (unavoidable overcounting since H_colind is still unknown) ctx->nH += ctx->M_rowadr[nv - 1] + ctx->M_rownnz[nv - 1]; @@ -1549,12 +1549,18 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { ctx->H_colind = mjSTACKALLOC(d, ctx->nH, int); ctx->H = mjSTACKALLOC(d, ctx->nH, mjtNum); - // compute H = J'*D*J - mju_sqrMatTDSparse(ctx->H, ctx->J, ctx->JT, ctx->D, nefc, nv, - ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, - ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind, NULL, - ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, ctx->JT_rowsuper, - d, /*diagind=*/NULL); + // compute H = J'*D*J: symbolic phase + mju_sqrMatTDSparseSymbolic( + ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, NULL, + nefc, nv, ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind, + ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, ctx->JT_rowsuper, d); + + // compute H = J'*D*J: numeric phase + mju_sqrMatTDSparseNumeric( + ctx->H, nv, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, + NULL, ctx->J, ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind, + ctx->JT, ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, + ctx->JT_rowsuper, ctx->D, d); // add mass matrix: H = J'*D*J + C mju_addToMatSparse(ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, nv, @@ -1626,12 +1632,18 @@ static void FactorizeHessian(mjData* d, mjPrimalContext* ctx, int flg_recompute) if (ctx->is_sparse) { // maybe compute H = M + J'*D*J if (flg_recompute) { - // compute H = J'*D*J - mju_sqrMatTDSparse(ctx->H, ctx->J, ctx->JT, ctx->D, nefc, nv, - ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, - ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind, NULL, - ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, ctx->JT_rowsuper, - d, /*diagind=*/NULL); + // compute H = J'*D*J: symbolic phase + mju_sqrMatTDSparseSymbolic( + ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, NULL, + nefc, nv, ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind, + ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, ctx->JT_rowsuper, d); + + // compute H = J'*D*J: numeric phase + mju_sqrMatTDSparseNumeric( + ctx->H, nv, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, + NULL, ctx->J, ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind, + ctx->JT, ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, + ctx->JT_rowsuper, ctx->D, d); // add mass matrix: H = J'*D*J + C mju_addToMatSparse(ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, nv, diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 9d78e409..05960567 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -13,8 +13,6 @@ // limitations under the License. #include "engine/engine_util_sparse.h" -#include "engine/engine_util_sparse_avx.h" // IWYU pragma: keep - #include #include @@ -23,7 +21,7 @@ #include "engine/engine_memory.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_misc.h" - +#include "engine/engine_util_sparse_avx.h" // IWYU pragma: keep //------------------------------ sparse operations ------------------------------------------------- @@ -723,9 +721,317 @@ void mju_sqrMatTDUncompressedInit(int* res_rowadr, int nc) { } -// max number of supernodes handled +// max number of supernodes handled by column-based matrix squaring functions #define mjMAXSUPER 8 +// column-based symbolic phase for sparse matrix squaring: compute sparsity pattern of M'*M +// if res_colind is NULL: count mode, fill res_rownnz/res_rowadr, return nnz +// if res_colind is not NULL: fill mode, write sorted column indices +// if res_diagind is not NULL: also fill upper triangle and output diagonal indices +int mju_sqrMatTDSparseSymbolic( + int* restrict res_rownnz, int* restrict res_rowadr, + int* restrict res_colind, int* restrict res_diagind, int nr, int nc, + const int* rownnz, const int* rowadr, const int* colind, + const int* rownnzT, const int* rowadrT, const int* colindT, const int* rowsuperT, mjData* d) { + mj_markStack(d); + + // reinterpret M^T as CSC + const int* colnnz = rownnzT; + const int* coladr = rowadrT; + const int* rowind = colindT; + const int* colsuper = rowsuperT; + + // reinterpret M as CSC + const int* colnnzT = rownnz; + const int* coladrT = rowadr; + const int* rowindT = colind; + + // marker[j] = 1 if row j has been visited in current column batch + int* marker = mjSTACKALLOC(d, nc, int); + mju_zeroInt(marker, nc); + + // buffer_idx: list of row indices with nonzeros in current column batch + int* buffer_idx = mjSTACKALLOC(d, nc, int); + + // rowstart[r]: first index in row r of M where column > current result column + int* rowstart = mjSTACKALLOC(d, nr, int); + mju_zeroInt(rowstart, nr); + + // clear res_rownnz (used for both counting and filling) + mju_zeroInt(res_rownnz, nc); + + // process result columns c = 0, 1, ..., nc-1 + int ns; // set in the loop + for (int c = 0; c < nc; c += ns) { + int buffer_nnz = 0; + + // column c of M^T + int nnz_c = colnnz[c]; + int adr_c = coladr[c]; + const int* ind_c = rowind + adr_c; + + // supernode size: how many consecutive columns share the same sparsity pattern + ns = 1; + int cs; + if (colsuper && (cs = colsuper[c])) { + ns += mjMIN(cs, mjMAXSUPER - 1); + } + + // for each row r where M^T[r, c] != 0, look at row r of M + for (int i = 0; i < nnz_c; i++) { + int r = ind_c[i]; + int adrT = coladrT[r]; + int nnzT = colnnzT[r]; + const int* indT = rowindT + adrT; + + // scan row r of M, starting from rowstart[r] + for (int k = rowstart[r]; k < nnzT; k++) { + int j = indT[k]; + + // skip if j <= c: only fill the strict lower triangle + if (j <= c) { + rowstart[r]++; + continue; + } + + // new nonzero in row j of result + if (!marker[j]) { + marker[j] = 1; + buffer_idx[buffer_nnz++] = j; + } + } + } + + // scatter: update result rows j > c that have nonzeros in this column batch + + // fill mode: write column indices, clear markers + if (res_colind) { + for (int i = 0; i < buffer_nnz; i++) { + int j = buffer_idx[i]; + marker[j] = 0; + int nm = mjMIN(ns, j - c); + int adr_j = res_rowadr[j] + res_rownnz[j]; + for (int s = 0; s < nm; s++) { + res_colind[adr_j + s] = c + s; + } + res_rownnz[j] += nm; + } + + // write diagonal entries + for (int s = 0; s < ns; s++) { + int col = c + s; + if (colnnz[col]) { + res_colind[res_rowadr[col] + res_rownnz[col]] = col; + res_rownnz[col]++; + } + } + } + + // count mode: just count and clear markers + else { + for (int i = 0; i < buffer_nnz; i++) { + int j = buffer_idx[i]; + marker[j] = 0; + int nm = mjMIN(ns, j - c); + res_rownnz[j] += nm; + if (res_diagind) { + for (int s = 0; s < nm; s++) { + res_rownnz[c + s]++; + } + } + } + + // count diagonal entries + for (int s = 0; s < ns; s++) { + int col = c + s; + if (colnnz[col]) { + res_rownnz[col]++; + } + } + } + } + + // count mode: compute res_rowadr from res_rownnz + if (!res_colind) { + res_rowadr[0] = 0; + for (int r = 1; r < nc; r++) { + res_rowadr[r] = res_rowadr[r - 1] + res_rownnz[r - 1]; + } + } + + // fill mode with upper triangle: record diagonal positions and mirror from lower + if (res_colind && res_diagind) { + // save current counts (lower + diagonal) + int* lower_nnz = mjSTACKALLOC(d, nc, int); + mju_copyInt(lower_nnz, res_rownnz, nc); + + // save diagonal indices + for (int r = 0; r < nc; r++) { + res_diagind[r] = res_rowadr[r] + lower_nnz[r] - 1; + } + + // fill upper triangle: for each (r, c) with c < r, write to (c, r) + for (int r = 0; r < nc; r++) { + int adr = res_rowadr[r]; + int nnz = lower_nnz[r]; + for (int j = 0; j < nnz; j++) { + int col = res_colind[adr + j]; + if (col < r) { + res_colind[res_rowadr[col] + res_rownnz[col]++] = r; + } + } + } + } + + mj_freeStack(d); + + return res_rowadr[nc - 1] + res_rownnz[nc - 1]; +} + + +// numeric phase for sparse matrix squaring: compute values given pre-computed sparsity +// diagind can be NULL, otherwise fills upper triangle and saves diagonal indices +void mju_sqrMatTDSparseNumeric( + mjtNum* restrict res, int nc, + const int* res_rownnz, const int* res_rowadr, const int* res_colind, const int* res_diagind, + const mjtNum* mat, const int* rownnz, const int* rowadr, const int* colind, + const mjtNum* matT, const int* rownnzT, const int* rowadrT, const int* colindT, + const int* rowsuperT, const mjtNum* diag, mjData* d) { + mj_markStack(d); + + // dense accumulator for current result row (or batch of rows) + mjtNum* restrict buffer = mjSTACKALLOC(d, nc * mjMAXSUPER, mjtNum); + mju_zero(buffer, nc * mjMAXSUPER); + + // process result rows + int ns; // set in the loop + for (int r = 0; r < nc; r += ns) { + // determine supernode size + ns = 1; + if (rowsuperT) { + ns = rowsuperT[r] + 1; + if (ns > mjMAXSUPER) ns = mjMAXSUPER; + } + + // single row + if (ns == 1) { + int nnzT_r = rownnzT[r]; + int adr_r = rowadrT[r]; + + // accumulate: res[r, :] = sum over k in M'[r, :] of diag[k] * M'[r, k] * M[k, :] + for (int i = 0; i < nnzT_r; i++) { + int k = colindT[adr_r + i]; + mjtNum valT = matT[adr_r + i]; + mjtNum scale = diag ? diag[k] * valT : valT; + if (scale == 0) continue; + + int adr_k = rowadr[k]; + int nnz_k = rownnz[k]; + const int* ind_k = colind + adr_k; + const mjtNum* val_k = mat + adr_k; + + for (int j = 0; j < nnz_k; j++) { + int c = ind_k[j]; + if (c > r) break; + buffer[c] += scale * val_k[j]; + } + } + + // scatter from dense buffer to sparse result + int res_adr = res_rowadr[r]; + int res_nnz = res_rownnz[r]; + const int* res_ind = res_colind + res_adr; + mjtNum* res_val = res + res_adr; + + for (int j = 0; j < res_nnz; j++) { + int c = res_ind[j]; + res_val[j] = buffer[c]; + buffer[c] = 0; + } + } + + // supernode: ns > 1 rows share the same sparsity pattern + else { + int nnzT_r = rownnzT[r]; + int adr_r = rowadrT[r]; + + // accumulate for ns rows + for (int i = 0; i < nnzT_r; i++) { + int k = colindT[adr_r + i]; + + // compute scale for all rows + mjtNum scale[mjMAXSUPER]; + if (diag) { + mjtNum dk = diag[k]; + if (dk == 0) continue; + for (int s = 0; s < ns; s++) { + scale[s] = dk * matT[rowadrT[r + s] + i]; + } + } else { + for (int s = 0; s < ns; s++) { + scale[s] = matT[rowadrT[r + s] + i]; + } + } + + int adr_k = rowadr[k]; + int nnz_k = rownnz[k]; + const int* ind_k = colind + adr_k; + const mjtNum* val_k = mat + adr_k; + + for (int j = 0; j < nnz_k; j++) { + int c = ind_k[j]; + if (c > r + ns - 1) break; // skip if beyond block + mjtNum v = val_k[j]; + + for (int s = 0; s < ns; s++) { + if (c <= r + s) { + buffer[s * nc + c] += scale[s] * v; + } + } + } + } + + // scatter + for (int s = 0; s < ns; s++) { + int row = r + s; + int res_adr = res_rowadr[row]; + int res_nnz = res_rownnz[row]; + const int* res_ind = res_colind + res_adr; + mjtNum* res_val = res + res_adr; + for (int j = 0; j < res_nnz; j++) { + int c = res_ind[j]; + res_val[j] = buffer[s*nc + c]; + buffer[s*nc + c] = 0; + } + } + } + } + + // fill upper triangle: mirror values from lower triangle + if (res_diagind) { + // initialize write positions after diagonal + int* upper_pos = mjSTACKALLOC(d, nc, int); + for (int r = 0; r < nc; r++) { + upper_pos[r] = res_diagind[r] + 1; + } + + // for each (r, c) with c < r, write r to row c + for (int r = 0; r < nc; r++) { + int adr = res_rowadr[r]; + int lower_nnz = res_diagind[r] - adr + 1; + for (int j = 0; j < lower_nnz; j++) { + int c = res_colind[adr + j]; + if (c < r) { + res[upper_pos[c]++] = res[adr + j]; + } + } + } + } + + mj_freeStack(d); +} + + // compute sparse M'*diag*M (diag=NULL: compute M'*M), res_rowadr must be precomputed void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT, const mjtNum* diag, int nr, int nc, @@ -1160,7 +1466,7 @@ void mju_blockDiagSparse(mjtNum* restrict res, int* restrict res_rownnz, } // end of block reached: update block counter, column offset, next row - if (r + 1 >= row_next && block + 1 < nb ) { + if (r + 1 >= row_next && block + 1 < nb) { block++; col_offset = block_c[block]; row_next = block + 1 < nb ? block_r[block + 1] : nr; diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index 4168cb8b..1350ccff 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -82,7 +82,7 @@ MJAPI void mju_mulSymVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec MJAPI int mju_compressSparse(mjtNum* mat, int nr, int nc, int* rownnz, int* rowadr, int* colind, mjtNum minval); -// count the number of non-zeros in the sum of two sparse vectors +// count the number of nonzeros in the sum of two sparse vectors MJAPI int mju_combineSparseCount(int a_nnz, int b_nnz, const int* a_ind, const int* b_ind); // incomplete combine sparse: dst = a*dst + b*src at common indices @@ -138,6 +138,24 @@ MJAPI int mju_sqrMatTDSparseCount(int* res_rownnz, int* res_rowadr, int nr, const int* rownnzT, const int* rowadrT, const int* colindT, const int* rowsuperT, mjData* d, int flg_upper); +// symbolic phase for mju_sqrMatTDSparse: compute sparsity pattern of M'*M +// if res_colind is NULL: count mode, fill res_rownnz/res_rowadr, return nnz +// if res_colind is not NULL: fill mode, write sorted column indices +// if res_diagind is not NULL: also fill upper triangle and output diagonal indices +MJAPI int mju_sqrMatTDSparseSymbolic( + int* res_rownnz, int* res_rowadr, int* res_colind, int* res_diagind, int nr, int nc, + const int* rownnz, const int* rowadr, const int* colind, + const int* rownnzT, const int* rowadrT, const int* colindT, const int* rowsuperT, mjData* d); + +// numeric phase for mju_sqrMatTDSparse: compute values given pre-computed sparsity +// res_colind, res_rownnz, res_rowadr must be pre-computed by mju_sqrMatTDSparseSymbolic +MJAPI void mju_sqrMatTDSparseNumeric( + mjtNum* res, int nc, + const int* res_rownnz, const int* res_rowadr, const int* res_colind, const int* res_diagind, + const mjtNum* mat, const int* rownnz, const int* rowadr, const int* colind, + const mjtNum* matT, const int* rownnzT, const int* rowadrT, const int* colindT, + const int* rowsuperT, const mjtNum* diag, mjData* d); + // precompute res_rowadr for mju_sqrMatTDSparse using uncompressed memory MJAPI void mju_sqrMatTDUncompressedInit(int* res_rowadr, int nc); @@ -146,7 +164,7 @@ MJAPI void mju_blockDiag(mjtNum* res, const mjtNum* mat, int nc_mat, int nc_res, int nb, const int* perm_r, const int* perm_c, const int* block_nr, const int* block_nc, - const int* blockadr_r, const int* blockadr_c); + const int* block_r, const int* block_c); // block-diagonalize a sparse matrix MJAPI void mju_blockDiagSparse( @@ -238,7 +256,7 @@ int mj_mergeSorted(int* merge, const int* chain1, int n1, const int* chain2, int } else if (c1 > c2) { merge[k++] = c2; j++; - } else { // c1 == c2 + } else { // c1 == c2 merge[k++] = c1; i++; j++; diff --git a/test/benchmark/chol_benchmark_test.cc b/test/benchmark/chol_benchmark_test.cc index 769f990d..004e0c2a 100644 --- a/test/benchmark/chol_benchmark_test.cc +++ b/test/benchmark/chol_benchmark_test.cc @@ -68,13 +68,15 @@ struct HessianData { // D diagonal std::vector D; + int nefc; + void Setup(const mjModel* m, mjData* d) { // initialize simulation state mj_resetDataKeyframe(m, d, 0); mj_forward(m, d); nv = m->nv; - int nefc = d->nefc; + nefc = d->nefc; // compute D corresponding to quad states D.resize(nefc); @@ -205,14 +207,27 @@ mjModel* GetModel() { return m; } +template +HessianData& GetHessianData() { + static HessianData data; + static bool initialized = false; + if (!initialized) { + mjModel* m = GetModel(); + mjData* d = mj_makeData(m); + data.Setup(m, d); + mj_deleteData(d); + initialized = true; + } + return data; +} + // old implementation benchmark template static void BM_chol_old(benchmark::State& state) { mjModel* m = GetModel(); mjData* d = mj_makeData(m); - HessianData hd; - hd.Setup(m, d); + HessianData& hd = GetHessianData(); std::vector L_work(hd.nL); std::vector L_colind_work(hd.nL); @@ -239,8 +254,7 @@ static void BM_chol_symbolic(benchmark::State& state) { mjModel* m = GetModel(); mjData* d = mj_makeData(m); - HessianData hd; - hd.Setup(m, d); + HessianData& hd = GetHessianData(); std::vector L_colind_work(hd.nL); std::vector LT_rownnz_work(hd.nv); @@ -266,8 +280,7 @@ static void BM_chol_numeric(benchmark::State& state) { mjModel* m = GetModel(); mjData* d = mj_makeData(m); - HessianData hd; - hd.Setup(m, d); + HessianData& hd = GetHessianData(); std::vector L_work(hd.nL); std::vector L_colind_work(hd.nL); @@ -371,9 +384,10 @@ template static void BM_update_old(benchmark::State& state) { mjModel* m = GetModel(); mjData* d = mj_makeData(m); + mj_resetDataKeyframe(m, d, 0); + mj_forward(m, d); - HessianData hd; - hd.Setup(m, d); + HessianData& hd = GetHessianData(); int nv = hd.nv; @@ -433,9 +447,10 @@ template static void BM_update_new(benchmark::State& state) { mjModel* m = GetModel(); mjData* d = mj_makeData(m); + mj_resetDataKeyframe(m, d, 0); + mj_forward(m, d); - HessianData hd; - hd.Setup(m, d); + HessianData& hd = GetHessianData(); int nv = hd.nv; diff --git a/test/benchmark/engine_util_sparse_benchmark_test.cc b/test/benchmark/engine_util_sparse_benchmark_test.cc index 1537c398..a82be98e 100644 --- a/test/benchmark/engine_util_sparse_benchmark_test.cc +++ b/test/benchmark/engine_util_sparse_benchmark_test.cc @@ -38,116 +38,7 @@ static const int kNumWarmupSteps = 500; // ----------------------------- old functions -------------------------------- -void ABSL_ATTRIBUTE_NOINLINE mju_sqrMatTDSparse_baseline( - mjtNum* res, const mjtNum* mat, const mjtNum* matT, const mjtNum* diag, - int nr, int nc, int* res_rownnz, int* res_rowadr, int* res_colind, - const int* rownnz, const int* rowadr, const int* colind, - const int* rowsuper, const int* rownnzT, const int* rowadrT, - const int* colindT, const int* rowsuperT, mjData* d, int* unused) { - mj_markStack(d); - int* chain = mj_stackAllocInt(d, 2 * nc); - mjtNum* buffer = mj_stackAllocNum(d, nc); - for (int r = 0; r < nc; r++) { - res_rowadr[r] = r * nc; - } - - for (int r = 0; r < nc; r++) { - if (rowsuperT && r > 0 && rowsuperT[r - 1] > 0) { - res_rownnz[r] = res_rownnz[r - 1]; - memcpy(res_colind + res_rowadr[r], res_colind + res_rowadr[r - 1], - res_rownnz[r] * sizeof(int)); - - if (rownnzT[r]) { - res_colind[res_rowadr[r] + res_rownnz[r]] = r; - res_rownnz[r]++; - } - } else { - int nchain = 0; - int inew = 0, iold = nc; - int lastadded = -1; - for (int i = 0; i < rownnzT[r]; i++) { - int c = colindT[rowadrT[r] + i]; - if (rowsuper && lastadded >= 0 && - (c - lastadded) <= rowsuper[lastadded]) { - continue; - } else { - lastadded = c; - } - - int adr = inew; - inew = iold; - iold = adr; - - int nnewchain = 0; - adr = 0; - int end = rowadr[c] + rownnz[c]; - for (int adr1 = rowadr[c]; adr1 < end; adr1++) { - int col_mat = colind[adr1]; - while (adr < nchain && chain[iold + adr] < col_mat && - chain[iold + adr] <= r) { - chain[inew + nnewchain++] = chain[iold + adr++]; - } - - if (col_mat > r) { - break; - } - - if (adr < nchain && chain[iold + adr] == col_mat) { - adr++; - } - chain[inew + nnewchain++] = col_mat; - } - - while (adr < nchain && chain[iold + adr] <= r) { - chain[inew + nnewchain++] = chain[iold + adr++]; - } - nchain = nnewchain; - } - res_rownnz[r] = nchain; - if (nchain) { - memcpy(res_colind + res_rowadr[r], chain + inew, nchain * sizeof(int)); - } - } - } - - for (int r = 0; r < nc; r++) { - int adr = res_rowadr[r]; - for (int i = 0; i < res_rownnz[r]; i++) { - buffer[res_colind[adr + i]] = 0; - } - for (int i = 0; i < rownnzT[r]; i++) { - int c = colindT[rowadrT[r] + i]; - mjtNum matTrc = matT[rowadrT[r] + i]; - if (diag) { - matTrc *= diag[c]; - } - - int end = rowadr[c] + rownnz[c]; - for (int adr = rowadr[c]; adr < end; adr++) { - int adr1; - if ((adr1 = colind[adr]) > r) { - break; - } - buffer[adr1] += matTrc * mat[adr]; - } - } - adr = res_rowadr[r]; - for (int i = 0; i < res_rownnz[r]; i++) { - res[adr + i] = buffer[res_colind[adr + i]]; - } - } - for (int r = 1; r < nc; r++) { - int end = res_rowadr[r] + res_rownnz[r] - 1; - for (int adr = res_rowadr[r]; adr < end; adr++) { - int adr1 = res_rowadr[res_colind[adr]] + res_rownnz[res_colind[adr]]++; - res[adr1] = res[adr]; - res_colind[adr1] = r; - } - } - - mj_freeStack(d); -} // transpose sparse matrix (uncompressed) void ABSL_ATTRIBUTE_NOINLINE transposeSparse_baseline( @@ -506,15 +397,27 @@ void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_combineSparse_old( } BENCHMARK(BM_combineSparse_old); +enum class Size { H2_100, H100 }; + +template +const char* ModelPath() { + if constexpr (S == Size::H2_100) { + return "../test/benchmark/testdata/2humanoid100_chol.xml"; + } else { + return "../test/benchmark/testdata/100_humanoids_chol.xml"; + } +} + enum class Supernode { None, PostProcess, Inline }; +template static void BM_transposeSparse(benchmark::State& state, TransposeFuncPtr func, Supernode super) { - static mjModel* m = LoadModelFromPath("humanoid/humanoid100.xml"); + static mjModel* m = LoadModelFromPath(ModelPath()); // force use of sparse matrices m->opt.jacobian = mjJAC_SPARSE; @@ -553,131 +456,67 @@ static void BM_transposeSparse(benchmark::State& state, TransposeFuncPtr func, } void ABSL_ATTRIBUTE_NO_TAIL_CALL -BM_transposeSparse_old(benchmark::State& state) { +BM_transposeSparse_2H100_old(benchmark::State& state) { MujocoErrorTestGuard guard; - BM_transposeSparse(state, &transposeSparse_baseline, Supernode::None); + BM_transposeSparse(state, &transposeSparse_baseline, + Supernode::None); } -BENCHMARK(BM_transposeSparse_old); +BENCHMARK(BM_transposeSparse_2H100_old); void ABSL_ATTRIBUTE_NO_TAIL_CALL -BM_transposeSparse_new(benchmark::State& state) { +BM_transposeSparse_2H100_new(benchmark::State& state) { MujocoErrorTestGuard guard; - BM_transposeSparse(state, &mju_transposeSparse, Supernode::None); + BM_transposeSparse(state, &mju_transposeSparse, + Supernode::None); } -BENCHMARK(BM_transposeSparse_new); +BENCHMARK(BM_transposeSparse_2H100_new); void ABSL_ATTRIBUTE_NO_TAIL_CALL -BM_transposeSparse_superpost(benchmark::State& state) { +BM_transposeSparse_2H100_superpost(benchmark::State& state) { MujocoErrorTestGuard guard; - BM_transposeSparse(state, &mju_transposeSparse, Supernode::PostProcess); + BM_transposeSparse(state, &mju_transposeSparse, + Supernode::PostProcess); } -BENCHMARK(BM_transposeSparse_superpost); +BENCHMARK(BM_transposeSparse_2H100_superpost); void ABSL_ATTRIBUTE_NO_TAIL_CALL -BM_transposeSparse_superinline(benchmark::State& state) { +BM_transposeSparse_2H100_superinline(benchmark::State& state) { MujocoErrorTestGuard guard; - BM_transposeSparse(state, &mju_transposeSparse, Supernode::Inline); -} -BENCHMARK(BM_transposeSparse_superinline); - -static void BM_sqrMatTDSparse(benchmark::State& state, SqrMatTDFuncPtr func) { - static mjModel* m = - LoadModelFromPath("../test/benchmark/testdata/2humanoid100.xml"); - - // force use of sparse matrices, Newton solver, no islands - m->opt.jacobian = mjJAC_SPARSE; - m->opt.solver = mjSOL_NEWTON; - m->opt.disableflags |= mjDSBL_ISLAND; - - mjData* d = mj_makeData(m); - - // warm-up rollout to get a typical state - while (d->time < 2) { - mj_step(m, d); - } - - // allocate - mj_markStack(d); - mjtNum* H = mj_stackAllocNum(d, m->nv * m->nv); - int* rownnz = mj_stackAllocInt(d, m->nv); - int* rowadr = mj_stackAllocInt(d, m->nv); - int* colind = mj_stackAllocInt(d, m->nv * m->nv); - int* diagind = mj_stackAllocInt(d, m->nv); - - // compute D corresponding to quad states - mjtNum* D = mj_stackAllocNum(d, d->nefc); - for (int i = 0; i < d->nefc; i++) { - if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) { - D[i] = d->efc_D[i]; - } else { - D[i] = 0; - } - } - - int* JT_rownnz = mj_stackAllocInt(d, m->nv); - int* JT_rowadr = mj_stackAllocInt(d, m->nv); - int* JT_rowsuper = mj_stackAllocInt(d, m->nv); - int* JT_colind = mj_stackAllocInt(d, d->nJ); - mjtNum* JT = mj_stackAllocNum(d, d->nJ); - mju_transposeSparse(JT, d->efc_J, d->nefc, m->nv, - JT_rownnz, JT_rowadr, JT_colind, JT_rowsuper, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); - - // time benchmark - if (func) { - mju_sqrMatTDSparseCount(rownnz, rowadr, m->nv, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, - JT_rownnz, JT_rowadr, - JT_colind, nullptr, d, 1); - - for (auto s : state) { - // compute H = J'*D*J, compressed layout - func(H, d->efc_J, JT, D, d->nefc, m->nv, rownnz, rowadr, colind, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, NULL, - JT_rownnz, JT_rowadr, JT_colind, - JT_rowsuper, d, diagind); - } - } else { - for (auto s : state) { - // baseline depends on efc_J_rowsuper - mju_superSparse(d->nefc, d->efc_J_rowsuper, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); - - // compute H = J'*D*J, uncompressed layout - mju_sqrMatTDSparse_baseline( - H, d->efc_J, JT, D, d->nefc, m->nv, rownnz, rowadr, colind, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind, d->efc_J_rowsuper, - JT_rownnz, JT_rowadr, JT_colind, - JT_rowsuper, d, /*unused=*/nullptr); - } - } - - // finalize - mj_freeStack(d); - mj_deleteData(d); - state.SetItemsProcessed(state.iterations()); + BM_transposeSparse(state, &mju_transposeSparse, + Supernode::Inline); } +BENCHMARK(BM_transposeSparse_2H100_superinline); void ABSL_ATTRIBUTE_NO_TAIL_CALL -BM_sqrMatTDSparse_col(benchmark::State& state) { +BM_transposeSparse_100H_old(benchmark::State& state) { MujocoErrorTestGuard guard; - BM_sqrMatTDSparse(state, &mju_sqrMatTDSparse); + BM_transposeSparse(state, &transposeSparse_baseline, + Supernode::None); } -BENCHMARK(BM_sqrMatTDSparse_col); +BENCHMARK(BM_transposeSparse_100H_old); void ABSL_ATTRIBUTE_NO_TAIL_CALL -BM_sqrMatTDSparse_row(benchmark::State& state) { +BM_transposeSparse_100H_new(benchmark::State& state) { MujocoErrorTestGuard guard; - BM_sqrMatTDSparse(state, &mju_sqrMatTDSparse_row); + BM_transposeSparse(state, &mju_transposeSparse, Supernode::None); } -BENCHMARK(BM_sqrMatTDSparse_row); +BENCHMARK(BM_transposeSparse_100H_new); void ABSL_ATTRIBUTE_NO_TAIL_CALL -BM_sqrMatTDSparse_uncompressed(benchmark::State& state) { +BM_transposeSparse_100H_superpost(benchmark::State& state) { MujocoErrorTestGuard guard; - BM_sqrMatTDSparse(state, nullptr); + BM_transposeSparse(state, &mju_transposeSparse, + Supernode::PostProcess); } -BENCHMARK(BM_sqrMatTDSparse_uncompressed); +BENCHMARK(BM_transposeSparse_100H_superpost); + +void ABSL_ATTRIBUTE_NO_TAIL_CALL +BM_transposeSparse_100H_superinline(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_transposeSparse(state, &mju_transposeSparse, + Supernode::Inline); +} +BENCHMARK(BM_transposeSparse_100H_superinline); } // namespace } // namespace mujoco diff --git a/test/benchmark/sqrmat_benchmark_test.cc b/test/benchmark/sqrmat_benchmark_test.cc new file mode 100644 index 00000000..9e6896d4 --- /dev/null +++ b/test/benchmark/sqrmat_benchmark_test.cc @@ -0,0 +1,423 @@ +// Copyright 2026 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Benchmarks for sparse matrix operations. + +#include +#include + +#include "benchmark/benchmark.h" +#include +#include +#include +#include "src/engine/engine_util_sparse.h" +#include "test/fixture.h" + +namespace mujoco { +namespace { + +// ================================ Test Data ================================== +// Stores pre-computed sparse matrix inputs extracted from MuJoCo simulations. +// Each benchmark computes its own outputs (H, L, etc.) from these inputs. + +struct SparseTestData { + // Dimensions + int nv; // number of DoFs + int nefc; // number of constraint rows + int nJ; // nnz in J + + // J (Jacobian) - nefc x nv sparse + std::vector J; + std::vector J_rownnz, J_rowadr, J_colind, J_rowsuper; + + // J' (transpose) + std::vector JT; + std::vector JT_rownnz, JT_rowadr, JT_colind, JT_rowsuper; + + // D (diagonal weights for constraints) + std::vector D; + + // M structure (mass matrix, lower triangle) + std::vector M_rownnz, M_rowadr, M_colind; + + void Setup(const mjModel* m, mjData* d) { + // initialize simulation state + mj_resetDataKeyframe(m, d, 0); + mj_step(m, d); + mj_forward(m, d); + nv = m->nv; + nefc = d->nefc; + nJ = d->nJ; + + // copy J + J.assign(d->efc_J, d->efc_J + nJ); + J_rownnz.assign(d->efc_J_rownnz, d->efc_J_rownnz + nefc); + J_rowadr.assign(d->efc_J_rowadr, d->efc_J_rowadr + nefc); + J_colind.assign(d->efc_J_colind, d->efc_J_colind + nJ); + J_rowsuper.assign(d->efc_J_rowsuper, d->efc_J_rowsuper + nefc); + + // transpose J + JT.assign(nJ, 0); + JT_rownnz.assign(nv, 0); + JT_rowadr.assign(nv, 0); + JT_colind.assign(nJ, 0); + JT_rowsuper.assign(nv, 0); + mju_transposeSparse(JT.data(), J.data(), nefc, nv, JT_rownnz.data(), + JT_rowadr.data(), JT_colind.data(), JT_rowsuper.data(), + J_rownnz.data(), J_rowadr.data(), J_colind.data()); + + // compute D corresponding to quadratic constraint states + D.resize(nefc); + for (int i = 0; i < nefc; i++) { + if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) { + D[i] = d->efc_D[i]; + } else { + D[i] = 0; + } + } + + // copy M structure + M_rownnz.assign(m->M_rownnz, m->M_rownnz + nv); + M_rowadr.assign(m->M_rowadr, m->M_rowadr + nv); + int nM = M_rowadr[nv - 1] + M_rownnz[nv - 1]; + M_colind.assign(m->M_colind, m->M_colind + nM); + } +}; + +// ================================ Model Sizes ================================ + +enum class Size { H2_100, H100 }; + +template +const char* ModelPath() { + if constexpr (S == Size::H2_100) { + return "../test/benchmark/testdata/2humanoid100_chol.xml"; + } else { + return "../test/benchmark/testdata/100_humanoids_chol.xml"; + } +} + +template +mjModel* GetModel() { + static mjModel* m = LoadModelFromPath(ModelPath()); + m->opt.jacobian = mjJAC_SPARSE; + m->opt.solver = mjSOL_NEWTON; + m->opt.disableflags |= mjDSBL_ISLAND; + return m; +} + +template +SparseTestData& GetData() { + static SparseTestData data; + static bool initialized = false; + if (!initialized) { + mjModel* m = GetModel(); + mjData* d = mj_makeData(m); + data.Setup(m, d); + mj_deleteData(d); + initialized = true; + } + return data; +} + +// ========================== Baseline Implementations ========================= + + + +// Baseline sqrMatTD (uncompressed layout, from old implementation) +void ABSL_ATTRIBUTE_NOINLINE mju_sqrMatTDSparse_baseline( + mjtNum* res, const mjtNum* mat, const mjtNum* matT, const mjtNum* diag, + int nr, int nc, int* res_rownnz, int* res_rowadr, int* res_colind, + const int* rownnz, const int* rowadr, const int* colind, + const int* rowsuper, const int* rownnzT, const int* rowadrT, + const int* colindT, const int* rowsuperT, mjData* d) { + mj_markStack(d); + int* chain = mj_stackAllocInt(d, 2 * nc); + mjtNum* buffer = mj_stackAllocNum(d, nc); + + for (int r = 0; r < nc; r++) { + res_rowadr[r] = r * nc; + } + + for (int r = 0; r < nc; r++) { + if (rowsuperT && r > 0 && rowsuperT[r - 1] > 0) { + res_rownnz[r] = res_rownnz[r - 1]; + memcpy(res_colind + res_rowadr[r], res_colind + res_rowadr[r - 1], + res_rownnz[r] * sizeof(int)); + if (rownnzT[r]) { + res_colind[res_rowadr[r] + res_rownnz[r]] = r; + res_rownnz[r]++; + } + } else { + int nchain = 0; + int inew = 0, iold = nc; + int lastadded = -1; + for (int i = 0; i < rownnzT[r]; i++) { + int c = colindT[rowadrT[r] + i]; + if (rowsuper && lastadded >= 0 && + (c - lastadded) <= rowsuper[lastadded]) { + continue; + } else { + lastadded = c; + } + + int adr = inew; + inew = iold; + iold = adr; + + int nnewchain = 0; + adr = 0; + int end = rowadr[c] + rownnz[c]; + for (int adr1 = rowadr[c]; adr1 < end; adr1++) { + int col_mat = colind[adr1]; + while (adr < nchain && chain[iold + adr] < col_mat && + chain[iold + adr] <= r) { + chain[inew + nnewchain++] = chain[iold + adr++]; + } + if (col_mat > r) { + break; + } + if (adr < nchain && chain[iold + adr] == col_mat) { + adr++; + } + chain[inew + nnewchain++] = col_mat; + } + + while (adr < nchain && chain[iold + adr] <= r) { + chain[inew + nnewchain++] = chain[iold + adr++]; + } + nchain = nnewchain; + } + res_rownnz[r] = nchain; + if (nchain) { + memcpy(res_colind + res_rowadr[r], chain + inew, nchain * sizeof(int)); + } + } + } + + for (int r = 0; r < nc; r++) { + int adr = res_rowadr[r]; + for (int i = 0; i < res_rownnz[r]; i++) { + buffer[res_colind[adr + i]] = 0; + } + for (int i = 0; i < rownnzT[r]; i++) { + int c = colindT[rowadrT[r] + i]; + mjtNum matTrc = matT[rowadrT[r] + i]; + if (diag) { + matTrc *= diag[c]; + } + + int end = rowadr[c] + rownnz[c]; + for (int adr2 = rowadr[c]; adr2 < end; adr2++) { + int adr1; + if ((adr1 = colind[adr2]) > r) { + break; + } + buffer[adr1] += matTrc * mat[adr2]; + } + } + adr = res_rowadr[r]; + for (int i = 0; i < res_rownnz[r]; i++) { + res[adr + i] = buffer[res_colind[adr + i]]; + } + } + for (int r = 1; r < nc; r++) { + int end = res_rowadr[r] + res_rownnz[r] - 1; + for (int adr = res_rowadr[r]; adr < end; adr++) { + int adr1 = res_rowadr[res_colind[adr]] + res_rownnz[res_colind[adr]]++; + res[adr1] = res[adr]; + res_colind[adr1] = r; + } + } + + mj_freeStack(d); +} + + + +// ========================== SqrMatTD Benchmarks ============================== + +enum class SqrMatTDVariant { + kBaseline, + kRow, + kCol, + kSplitCol +}; + +template +static void BM_sqrMatTD_impl(benchmark::State& state, SqrMatTDVariant variant) { + SparseTestData& data = GetData(); + mjModel* m = GetModel(); + mjData* d = mj_makeData(m); + + int nv = data.nv; + + // nothing to benchmark if no constraints + if (data.nefc == 0) { + for (auto s : state) {} + mj_deleteData(d); + return; + } + + // allocate H output (uncompressed for baseline, compressed for others) + int max_nnz = (variant == SqrMatTDVariant::kBaseline) ? nv * nv : 0; + std::vector H; + std::vector H_rownnz(nv); + std::vector H_rowadr(nv); + std::vector H_colind; + std::vector diagind(nv); + + if (variant == SqrMatTDVariant::kBaseline) { + H.resize(max_nnz); + H_colind.resize(max_nnz); + } else if (variant == SqrMatTDVariant::kSplitCol || + variant == SqrMatTDVariant::kCol) { + // use symbolic to count nnz + int nH = mju_sqrMatTDSparseSymbolic( + H_rownnz.data(), H_rowadr.data(), nullptr, nullptr, + data.nefc, nv, data.J_rownnz.data(), data.J_rowadr.data(), + data.J_colind.data(), data.JT_rownnz.data(), data.JT_rowadr.data(), + data.JT_colind.data(), data.JT_rowsuper.data(), d); + H.resize(nH); + H_colind.resize(nH); + } else { + // row: use Count (lower triangle only) + mju_sqrMatTDSparseCount( + H_rownnz.data(), H_rowadr.data(), nv, data.J_rownnz.data(), + data.J_rowadr.data(), data.J_colind.data(), data.JT_rownnz.data(), + data.JT_rowadr.data(), data.JT_colind.data(), nullptr, d, 0); + int nH = H_rowadr[nv - 1] + H_rownnz[nv - 1]; + H.resize(nH); + H_colind.resize(nH); + } + + for (auto s : state) { + switch (variant) { + case SqrMatTDVariant::kBaseline: + mju_superSparse(data.nefc, data.J_rowsuper.data(), data.J_rownnz.data(), + data.J_rowadr.data(), data.J_colind.data()); + mju_sqrMatTDSparse_baseline( + H.data(), data.J.data(), data.JT.data(), data.D.data(), data.nefc, + nv, H_rownnz.data(), H_rowadr.data(), H_colind.data(), + data.J_rownnz.data(), data.J_rowadr.data(), data.J_colind.data(), + data.J_rowsuper.data(), data.JT_rownnz.data(), + data.JT_rowadr.data(), data.JT_colind.data(), + data.JT_rowsuper.data(), d); + break; + case SqrMatTDVariant::kRow: + mju_sqrMatTDSparseCount( + H_rownnz.data(), H_rowadr.data(), nv, data.J_rownnz.data(), + data.J_rowadr.data(), data.J_colind.data(), data.JT_rownnz.data(), + data.JT_rowadr.data(), data.JT_colind.data(), nullptr, d, 0); + mju_sqrMatTDSparse_row( + H.data(), data.J.data(), data.JT.data(), data.D.data(), data.nefc, + nv, H_rownnz.data(), H_rowadr.data(), H_colind.data(), + data.J_rownnz.data(), data.J_rowadr.data(), data.J_colind.data(), + nullptr, data.JT_rownnz.data(), data.JT_rowadr.data(), + data.JT_colind.data(), data.JT_rowsuper.data(), d, nullptr); + break; + case SqrMatTDVariant::kCol: + mju_sqrMatTDSparseCount( + H_rownnz.data(), H_rowadr.data(), nv, data.J_rownnz.data(), + data.J_rowadr.data(), data.J_colind.data(), data.JT_rownnz.data(), + data.JT_rowadr.data(), data.JT_colind.data(), nullptr, d, 0); + mju_sqrMatTDSparse( + H.data(), data.J.data(), data.JT.data(), data.D.data(), data.nefc, + nv, H_rownnz.data(), H_rowadr.data(), H_colind.data(), + data.J_rownnz.data(), data.J_rowadr.data(), data.J_colind.data(), + nullptr, data.JT_rownnz.data(), data.JT_rowadr.data(), + data.JT_colind.data(), data.JT_rowsuper.data(), d, nullptr); + break; + + case SqrMatTDVariant::kSplitCol: + mju_sqrMatTDSparseSymbolic( + H_rownnz.data(), H_rowadr.data(), nullptr, nullptr, + data.nefc, nv, data.J_rownnz.data(), data.J_rowadr.data(), + data.J_colind.data(), data.JT_rownnz.data(), data.JT_rowadr.data(), + data.JT_colind.data(), data.JT_rowsuper.data(), d); + mju_sqrMatTDSparseSymbolic( + H_rownnz.data(), H_rowadr.data(), H_colind.data(), nullptr, + data.nefc, nv, data.J_rownnz.data(), data.J_rowadr.data(), + data.J_colind.data(), data.JT_rownnz.data(), data.JT_rowadr.data(), + data.JT_colind.data(), data.JT_rowsuper.data(), d); + mju_sqrMatTDSparseNumeric( + H.data(), nv, H_rownnz.data(), H_rowadr.data(), + H_colind.data(), nullptr, data.J.data(), data.J_rownnz.data(), + data.J_rowadr.data(), data.J_colind.data(), data.JT.data(), + data.JT_rownnz.data(), data.JT_rowadr.data(), data.JT_colind.data(), + data.JT_rowsuper.data(), data.D.data(), d); + break; + } + } + + mj_deleteData(d); + state.SetItemsProcessed(state.iterations()); +} + +void BM_sqrMatTD_2H100_baseline(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_sqrMatTD_impl(state, SqrMatTDVariant::kBaseline); +} +BENCHMARK(BM_sqrMatTD_2H100_baseline); + +void BM_sqrMatTD_2H100_row(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_sqrMatTD_impl(state, SqrMatTDVariant::kRow); +} +BENCHMARK(BM_sqrMatTD_2H100_row); + +void BM_sqrMatTD_2H100_col(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_sqrMatTD_impl(state, SqrMatTDVariant::kCol); +} +BENCHMARK(BM_sqrMatTD_2H100_col); + +void BM_sqrMatTD_2H100_splitCol(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_sqrMatTD_impl(state, SqrMatTDVariant::kSplitCol); +} +BENCHMARK(BM_sqrMatTD_2H100_splitCol); + +void BM_sqrMatTD_100H_baseline(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_sqrMatTD_impl(state, SqrMatTDVariant::kBaseline); +} +BENCHMARK(BM_sqrMatTD_100H_baseline); + +void BM_sqrMatTD_100H_row(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_sqrMatTD_impl(state, SqrMatTDVariant::kRow); +} +BENCHMARK(BM_sqrMatTD_100H_row); + +void BM_sqrMatTD_100H_col(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_sqrMatTD_impl(state, SqrMatTDVariant::kCol); +} +BENCHMARK(BM_sqrMatTD_100H_col); + +void BM_sqrMatTD_100H_splitCol(benchmark::State& state) { + MujocoErrorTestGuard guard; + BM_sqrMatTD_impl(state, SqrMatTDVariant::kSplitCol); +} +BENCHMARK(BM_sqrMatTD_100H_splitCol); + +} // namespace +} // namespace mujoco + +int main(int argc, char** argv) { + benchmark::Initialize(&argc, argv); + benchmark::RunSpecifiedBenchmarks(); + return 0; +} diff --git a/test/engine/engine_util_sparse_test.cc b/test/engine/engine_util_sparse_test.cc index be74f9dd..67eadbbc 100644 --- a/test/engine/engine_util_sparse_test.cc +++ b/test/engine/engine_util_sparse_test.cc @@ -14,10 +14,11 @@ // Tests for engine/engine_util_sparse.c -#include - #include "src/engine/engine_util_sparse.h" +#include +#include + #include #include #include @@ -360,6 +361,45 @@ TEST_F(EngineUtilSparseTest, MjuCompressSparse) { EXPECT_EQ(AsVector(dense, 6), AsVector(dense_expected_minval1, 6)); } +// helper: run split-col approach and return dense result +static void SqrMatTDSplitCol( + std::vector& dense_result, int nr, int nc, + const mjtNum* mat, const int* rownnz, const int* rowadr, const int* colind, + const mjtNum* matT, const int* rownnzT, const int* rowadrT, + const int* colindT, const int* rowsuperT, const mjtNum* diag, + int* out_diagind, mjData* d) { + // count mode + std::vector H_rownnz(nc, 0); + std::vector H_rowadr(nc, 0); + int nnz = mju_sqrMatTDSparseSymbolic( + H_rownnz.data(), H_rowadr.data(), nullptr, + out_diagind, nr, nc, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, rowsuperT, d); + + // fill mode + std::vector H_colind(nnz); + mju_sqrMatTDSparseSymbolic( + H_rownnz.data(), H_rowadr.data(), H_colind.data(), + out_diagind, nr, nc, rownnz, rowadr, colind, + rownnzT, rowadrT, colindT, rowsuperT, d); + + // numeric phase + std::vector H(nnz, 0); + mju_sqrMatTDSparseNumeric( + H.data(), nc, H_rownnz.data(), H_rowadr.data(), + H_colind.data(), out_diagind, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, rowsuperT, diag, d); + + // densify + dense_result.assign(nc * nc, 0); + for (int r = 0; r < nc; r++) { + for (int j = 0; j < H_rownnz[r]; j++) { + int c = H_colind[H_rowadr[r] + j]; + dense_result[r*nc + c] = H[H_rowadr[r] + j]; + } + } +} + TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse1) { // 0 0 0 // M = 0 0 0 @@ -378,29 +418,13 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse1) { int rownnzT[] = {3, 3, 3}; int rowadrT[] = {0, 3, 6}; - mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int rownnzH[] = {0, 0, 0}; - int rowadrH[] = {0, 0, 0}; - int diagindH[] = {0, 0, 0}; + int diagindH[3]; + std::vector dense; + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, nullptr, nullptr, + diagindH, data); - // test precount - mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); - - EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); - - // test computation - mju_sqrMatTDUncompressedInit(rowadrH, 3); - mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, - rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, diagindH); - - EXPECT_THAT(matH, ElementsAre(0, 0, 0, 0, 0, 0, 0, 0, 0)); - EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); - EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); + EXPECT_THAT(dense, ElementsAre(0, 0, 0, 0, 0, 0, 0, 0, 0)); mj_deleteData(data); mj_deleteModel(model); @@ -424,27 +448,12 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseLower) { int rownnzT[] = {3, 3, 3}; int rowadrT[] = {0, 3, 6}; - mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int rownnzH[] = {0, 0, 0}; - int rowadrH[] = {0, 0, 0}; + std::vector dense; + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, nullptr, nullptr, + nullptr, data); - // test precount - mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 0); - EXPECT_THAT(rownnzH, ElementsAre(1, 2, 3)); - EXPECT_THAT(rowadrH, ElementsAre(0, 1, 3)); - - // test computation - mju_sqrMatTDUncompressedInit(rowadrH, 3); - mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, - rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, nullptr); - - EXPECT_THAT(matH, ElementsAre(12, 0, 0, 0, 6, 0, 12, 3, 14)); - EXPECT_THAT(colindH, ElementsAre(0, 0, 0, 0, 1, 0, 0, 1, 2)); - EXPECT_THAT(rownnzH, ElementsAre(1, 2, 3)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); + EXPECT_THAT(dense, ElementsAre(12, 0, 0, 0, 6, 0, 12, 3, 14)); mj_deleteData(data); mj_deleteModel(model); @@ -468,31 +477,13 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse2) { int rownnzT[] = {3, 3, 3}; int rowadrT[] = {0, 3, 6}; - mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int rownnzH[] = {0, 0, 0}; - int rowadrH[] = {0, 0, 0}; - int diagindH[] = {0, 0, 0}; + int diagindH[3]; + std::vector dense; + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, nullptr, nullptr, + diagindH, data); - // test precount - mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); - - EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); - - - // test computation - mju_sqrMatTDUncompressedInit(rowadrH, 3); - mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, - rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, diagindH); - - EXPECT_THAT(matH, ElementsAre(12, 0, 12, 0, 6, 3, 12, 3, 14)); - EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); - EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); - EXPECT_THAT(diagindH, ElementsAre(0, 4, 8)); + EXPECT_THAT(dense, ElementsAre(12, 0, 12, 0, 6, 3, 12, 3, 14)); mj_deleteData(data); mj_deleteModel(model); @@ -516,31 +507,15 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3) { int rownnzT[] = {2, 2, 0}; int rowadrT[] = {0, 2, 4}; - mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int rownnzH[] = {0, 0, 0}; - int rowadrH[] = {0, 0, 0}; - int diagindH[] = {0, 0, 0}; - mjtNum diag[] = {2, 3, 4}; - // test precount - mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); + int diagindH[3]; + std::vector dense; + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, nullptr, diag, + diagindH, data); - EXPECT_THAT(rownnzH, ElementsAre(2, 2, 0)); - EXPECT_THAT(rowadrH, ElementsAre(0, 2, 4)); - - // test computation - mju_sqrMatTDUncompressedInit(rowadrH, 3); - mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, - rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, diagindH); - - EXPECT_THAT(matH, ElementsAre(66, 4, 0, 4, 35, 0, 0, 0, 0)); - EXPECT_THAT(colindH, ElementsAre(0, 1, 0, 0, 1, 0, 2, 0, 0)); - EXPECT_THAT(rownnzH, ElementsAre(2, 2, 1)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); + EXPECT_THAT(dense, ElementsAre(66, 4, 0, 4, 35, 0, 0, 0, 0)); mj_deleteData(data); mj_deleteModel(model); @@ -564,32 +539,15 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse3b) { int rownnzT[] = {2, 2, 1}; int rowadrT[] = {0, 2, 4}; - mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int rownnzH[] = {0, 0, 0}; - int rowadrH[] = {0, 0, 0}; - int diagindH[] = {0, 0, 0}; - mjtNum diag[] = {1, 1, 1}; - // test precount - mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); + int diagindH[3]; + std::vector dense; + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, nullptr, diag, + diagindH, data); - EXPECT_THAT(rownnzH, ElementsAre(2, 3, 2)); - EXPECT_THAT(rowadrH, ElementsAre(0, 2, 5)); - - // test computation - mju_sqrMatTDUncompressedInit(rowadrH, 3); - mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, - rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, diagindH); - - EXPECT_THAT(matH, ElementsAre(26, 2, 0, 2, 13, 12, 12, 16, 0)); - EXPECT_THAT(colindH, ElementsAre(0, 1, 0, 0, 1, 2, 1, 2, 0)); - EXPECT_THAT(rownnzH, ElementsAre(2, 3, 2)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); - EXPECT_THAT(diagindH, ElementsAre(0, 4, 7)); + EXPECT_THAT(dense, ElementsAre(26, 2, 0, 2, 13, 12, 0, 12, 16)); mj_deleteData(data); mj_deleteModel(model); @@ -613,32 +571,15 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse4) { int rownnzT[] = {2, 0, 2}; int rowadrT[] = {0, 2, 2}; - mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int rownnzH[] = {0, 0, 0}; - int rowadrH[] = {0, 0, 0}; - int diagindH[] = {0, 0, 0}; - mjtNum diag[] = {2, 3, 4}; + int diagindH[3]; + std::vector dense; + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, nullptr, diag, + diagindH, data); - // test precount - mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); - - EXPECT_THAT(rownnzH, ElementsAre(2, 0, 2)); - EXPECT_THAT(rowadrH, ElementsAre(0, 2, 2)); - - // test computation - mju_sqrMatTDUncompressedInit(rowadrH, 3); - mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, - rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, diagindH); - - EXPECT_THAT(matH, ElementsAre(66, 4, 0, 0, 0, 0, 4, 35, 0)); - EXPECT_THAT(colindH, ElementsAre(0, 2, 0, 1, 0, 0, 0, 2, 0)); - EXPECT_THAT(rownnzH, ElementsAre(2, 1, 2)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); + EXPECT_THAT(dense, ElementsAre(66, 0, 4, 0, 0, 0, 4, 0, 35)); mj_deleteData(data); mj_deleteModel(model); @@ -662,30 +603,13 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse5) { int rownnzT[] = {2, 1, 1}; int rowadrT[] = {0, 2, 3}; - mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int rownnzH[] = {0, 0, 0}; - int rowadrH[] = {0, 0, 0}; - int diagindH[] = {0, 0, 0}; + int diagindH[3]; + std::vector dense; + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, nullptr, nullptr, + diagindH, data); - - // test precount - mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); - - EXPECT_THAT(rownnzH, ElementsAre(3, 2, 2)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 5)); - - // test computation - mju_sqrMatTDUncompressedInit(rowadrH, 3); - mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, - rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, diagindH); - - EXPECT_THAT(matH, ElementsAre(5, 6, 4, 6, 9, 0, 4, 16, 0)); - EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 0, 0, 2, 0)); - EXPECT_THAT(rownnzH, ElementsAre(3, 2, 2)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); + EXPECT_THAT(dense, ElementsAre(5, 6, 4, 6, 9, 0, 4, 0, 16)); mj_deleteData(data); mj_deleteModel(model); @@ -709,30 +633,13 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse6) { int rownnzT[] = {1, 1, 2}; int rowadrT[] = {0, 1, 2}; - mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int rownnzH[] = {0, 0, 0}; - int rowadrH[] = {0, 0, 0}; - int diagindH[] = {0, 0, 0}; + int diagindH[3]; + std::vector dense; + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, nullptr, nullptr, + diagindH, data); - // test precount - mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); - - EXPECT_THAT(rownnzH, ElementsAre(2, 1, 2)); - EXPECT_THAT(rowadrH, ElementsAre(0, 2, 3)); - - // test computation - mju_sqrMatTDUncompressedInit(rowadrH, 3); - mju_sqrMatTDSparse(matH, mat, matT, nullptr, 3, 3, rownnzH, rowadrH, colindH, - rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, diagindH); - - EXPECT_THAT(matH, ElementsAre(1, 2, 0, 4, 0, 0, 2, 13, 0)); - EXPECT_THAT(colindH, ElementsAre(0, 2, 0, 1, 0, 0, 0, 2, 0)); - EXPECT_THAT(rownnzH, ElementsAre(2, 1, 2)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); - EXPECT_THAT(diagindH, ElementsAre(0, 3, 7)); + EXPECT_THAT(dense, ElementsAre(1, 0, 2, 0, 4, 0, 2, 0, 13)); mj_deleteData(data); mj_deleteModel(model); @@ -756,31 +663,15 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse7) { int rownnzT[] = {2, 2}; int rowadrT[] = {0, 2}; - mjtNum matH[] = {0, 0, 0, 0}; - int colindH[] = {0, 0, 0, 0}; - int rownnzH[] = {0, 0}; - int rowadrH[] = {0, 0}; - int diagindH[] = {0, 0}; - mjtNum diag[] = {2, 3, 4}; - // test precount - mju_sqrMatTDSparseCount(rownnzH, rowadrH, 2, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); + int diagindH[2]; + std::vector dense; + SqrMatTDSplitCol(dense, 3, 2, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, nullptr, diag, + diagindH, data); - EXPECT_THAT(rownnzH, ElementsAre(2, 2)); - EXPECT_THAT(rowadrH, ElementsAre(0, 2)); - - // test computation - mju_sqrMatTDUncompressedInit(rowadrH, 2); - mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 2, rownnzH, rowadrH, colindH, - rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, diagindH); - - EXPECT_THAT(matH, ElementsAre(66, 4, 4, 35)); - EXPECT_THAT(colindH, ElementsAre(0, 1, 0, 1)); - EXPECT_THAT(rownnzH, ElementsAre(2, 2)); - EXPECT_THAT(rowadrH, ElementsAre(0, 2)); + EXPECT_THAT(dense, ElementsAre(66, 4, 4, 35)); mj_deleteData(data); mj_deleteModel(model); @@ -803,31 +694,15 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse8) { int rownnzT[] = {2, 1, 1}; int rowadrT[] = {0, 2, 3}; - mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int rownnzH[] = {0, 0, 0}; - int rowadrH[] = {0, 0, 0}; - int diagindH[] = {0, 0, 0}; - mjtNum diag[] = {2, 3}; - // test precount - mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); + int diagindH[3]; + std::vector dense; + SqrMatTDSplitCol(dense, 2, 3, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, nullptr, diag, + diagindH, data); - EXPECT_THAT(rownnzH, ElementsAre(3, 2, 2)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 5)); - - // test computation - mju_sqrMatTDUncompressedInit(rowadrH, 3); - mju_sqrMatTDSparse(matH, mat, matT, diag, 2, 3, rownnzH, rowadrH, colindH, - rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, diagindH); - - EXPECT_THAT(matH, ElementsAre(14, 18, 8, 18, 27, 0, 8, 32, 0)); - EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 0, 0, 2, 0)); - EXPECT_THAT(rownnzH, ElementsAre(3, 2, 2)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); + EXPECT_THAT(dense, ElementsAre(14, 18, 8, 18, 27, 0, 8, 0, 32)); mj_deleteData(data); mj_deleteModel(model); @@ -851,31 +726,15 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse9) { int rownnzT[] = {3, 3, 3}; int rowadrT[] = {0, 3, 6}; - mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int rownnzH[] = {0, 0, 0}; - int rowadrH[] = {0, 0, 0}; - int diagindH[] = {0, 0, 0}; - mjtNum diag[] = {2, 3, 4}; - // test precount - mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, nullptr, data, 1); + int diagindH[3]; + std::vector dense; + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, nullptr, diag, + diagindH, data); - EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); - - // test computation - mju_sqrMatTDUncompressedInit(rowadrH, 3); - mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, - rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - nullptr, data, diagindH); - - EXPECT_THAT(matH, ElementsAre(69, 77, 80, 77, 99, 108, 80, 108, 120)); - EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); - EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); + EXPECT_THAT(dense, ElementsAre(69, 77, 80, 77, 99, 108, 80, 108, 120)); mj_deleteData(data); mj_deleteModel(model); @@ -900,31 +759,15 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse10) { int rowadrT[] = {0, 3, 6}; int rowsuperT[] = {2, 1, 0}; - mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int rownnzH[] = {0, 0, 0}; - int rowadrH[] = {0, 0, 0}; - int diagindH[] = {0, 0, 0}; - mjtNum diag[] = {1, 2, 1}; - // test precount - mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data, 1); + int diagindH[3]; + std::vector dense; + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, rowsuperT, diag, + diagindH, data); - EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); - - // test computation - mju_sqrMatTDUncompressedInit(rowadrH, 3); - mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, - rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data, diagindH); - - EXPECT_THAT(matH, ElementsAre(18, 17, 14, 17, 23, 19, 14, 19, 18)); - EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); - EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); + EXPECT_THAT(dense, ElementsAre(18, 17, 14, 17, 23, 19, 14, 19, 18)); mj_deleteData(data); mj_deleteModel(model); @@ -949,31 +792,15 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse11) { int rowadrT[] = {0, 1, 3}; int rowsuperT[] = {0, 1, 0}; - mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; - int rownnzH[] = {0, 0, 0}; - int rowadrH[] = {0, 0, 0}; - int diagindH[] = {0, 0, 0}; - mjtNum diag[] = {1, 1, 1}; - // test precount - mju_sqrMatTDSparseCount(rownnzH, rowadrH, 3, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data, 1); + int diagindH[3]; + std::vector dense; + SqrMatTDSplitCol(dense, 3, 3, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, rowsuperT, diag, + diagindH, data); - EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); - - // test computation - mju_sqrMatTDUncompressedInit(rowadrH, 3); - mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 3, rownnzH, rowadrH, colindH, - rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data, diagindH); - - EXPECT_THAT(matH, ElementsAre(1, 1, 1, 1, 10, 10, 1, 10, 10)); - EXPECT_THAT(colindH, ElementsAre(0, 1, 2, 0, 1, 2, 0, 1, 2)); - EXPECT_THAT(rownnzH, ElementsAre(3, 3, 3)); - EXPECT_THAT(rowadrH, ElementsAre(0, 3, 6)); + EXPECT_THAT(dense, ElementsAre(1, 1, 1, 1, 10, 10, 1, 10, 10)); mj_deleteData(data); mj_deleteModel(model); @@ -998,33 +825,16 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse12) { int rowadrT[] = {0, 1, 2, 4}; int rowsuperT[] = {1, 0, 1, 0}; - mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - int rownnzH[] = {0, 0, 0, 0}; - int rowadrH[] = {0, 0, 0, 0}; - int diagindH[] = {0, 0, 0, 0}; - mjtNum diag[] = {1, 1, 1}; - // test precount - mju_sqrMatTDSparseCount(rownnzH, rowadrH, 4, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data, 1); + int diagindH[4]; + std::vector dense; + SqrMatTDSplitCol(dense, 3, 4, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, rowsuperT, diag, + diagindH, data); - EXPECT_THAT(rownnzH, ElementsAre(4, 4, 4, 4)); - EXPECT_THAT(rowadrH, ElementsAre(0, 4, 8, 12)); - - // test computation - mju_sqrMatTDUncompressedInit(rowadrH, 4); - mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 4, rownnzH, rowadrH, colindH, - rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data, diagindH); - - EXPECT_THAT(matH, + EXPECT_THAT(dense, ElementsAre(1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 10, 10, 1, 1, 10, 10)); - EXPECT_THAT(colindH, - ElementsAre(0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3, 0, 1, 2, 3)); - EXPECT_THAT(rownnzH, ElementsAre(4, 4, 4, 4)); - EXPECT_THAT(rowadrH, ElementsAre(0, 4, 8, 12)); mj_deleteData(data); mj_deleteModel(model); @@ -1049,35 +859,16 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse13) { int rowadrT[] = {0, 3, 6, 6, 6}; int rowsuperT[] = {1, 0, 2, 1, 0}; - mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - int rownnzH[] = {0, 0, 0, 0, 0}; - int rowadrH[] = {0, 0, 0, 0, 0}; - int diagindH[] = {0, 0, 0, 0, 0}; - mjtNum diag[] = {1, 1, 1}; - // test precount - mju_sqrMatTDSparseCount(rownnzH, rowadrH, 5, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data, 1); + int diagindH[5]; + std::vector dense; + SqrMatTDSplitCol(dense, 3, 5, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, rowsuperT, diag, + diagindH, data); - EXPECT_THAT(rownnzH, ElementsAre(2, 2, 0, 0, 0)); - EXPECT_THAT(rowadrH, ElementsAre(0, 2, 4, 4, 4)); - - // test computation - mju_sqrMatTDUncompressedInit(rowadrH, 5); - mju_sqrMatTDSparse(matH, mat, matT, diag, 3, 5, rownnzH, rowadrH, colindH, - rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data, diagindH); - - EXPECT_THAT(matH, ElementsAre(3, 3, 0, 0, 0, 3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0)); - EXPECT_THAT(colindH, ElementsAre(0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 2, 0, 0, 0, 0, - 3, 0, 0, 0, 0, 4, 0, 0, 0, 0)); - EXPECT_THAT(rownnzH, ElementsAre(2, 2, 1, 1, 1)); - EXPECT_THAT(rowadrH, ElementsAre(0, 5, 10, 15, 20)); + EXPECT_THAT(dense, ElementsAre(3, 3, 0, 0, 0, 3, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0)); mj_deleteData(data); mj_deleteModel(model); @@ -1100,40 +891,305 @@ TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparse14) { int rowadrT[] = {0, 1, 2, 3, 4, 5, 6}; int rowsuperT[] = {3, 2, 1, 0, 2, 1, 0}; - mjtNum matH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - int colindH[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - int rownnzH[] = {0, 0, 0, 0, 0, 0, 0}; - int rowadrH[] = {0, 0, 0, 0, 0, 0, 0}; - int diagindH[] = {0, 0, 0, 0, 0, 0, 0}; - - // test precount - mju_sqrMatTDSparseCount(rownnzH, rowadrH, 7, rownnz, rowadr, colind, - rownnzT, rowadrT, colindT, rowsuperT, data, 1); - - EXPECT_THAT(rownnzH, ElementsAre(7, 7, 7, 7, 7, 7, 7)); - EXPECT_THAT(rowadrH, ElementsAre(0, 7, 14, 21, 28, 35, 42)); - - // test computation - mju_sqrMatTDUncompressedInit(rowadrH, 7); - mju_sqrMatTDSparse(matH, mat, matT, nullptr, 1, 7, rownnzH, rowadrH, colindH, - rownnz, rowadr, colind, nullptr, rownnzT, rowadrT, colindT, - rowsuperT, data, diagindH); + int diagindH[7]; + std::vector dense; + SqrMatTDSplitCol(dense, 1, 7, mat, rownnz, rowadr, colind, + matT, rownnzT, rowadrT, colindT, rowsuperT, nullptr, + diagindH, data); EXPECT_THAT( - matH, ElementsAre(1, 1, 1, 1, 2, 2, 2, 1, 1, 1, 1, 2, 2, 2, 1, 1, 1, 1, 2, - 2, 2, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 4, 4, 4, 2, 2, 2, - 2, 4, 4, 4, 2, 2, 2, 2, 4, 4, 4)); - EXPECT_THAT(colindH, - ElementsAre(0, 1, 2, 3, 4, 5, 6, 0, 1, 2, 3, 4, 5, 6, 0, 1, 2, 3, - 4, 5, 6, 0, 1, 2, 3, 4, 5, 6, 0, 1, 2, 3, 4, 5, 6, 0, - 1, 2, 3, 4, 5, 6, 0, 1, 2, 3, 4, 5, 6)); + dense, ElementsAre(1, 1, 1, 1, 2, 2, 2, 1, 1, 1, 1, 2, 2, 2, 1, 1, 1, + 1, 2, 2, 2, 1, 1, 1, 1, 2, 2, 2, 2, 2, 2, 2, 4, 4, + 4, 2, 2, 2, 2, 4, 4, 4, 2, 2, 2, 2, 4, 4, 4)); - EXPECT_THAT(rownnzH, ElementsAre(7, 7, 7, 7, 7, 7, 7)); - EXPECT_THAT(rowadrH, ElementsAre(0, 7, 14, 21, 28, 35, 42)); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolic) { + // Simple dense 2x2 matrix: + // 1 2 + // M = 3 4 + // + // M'M (lower triangle) should have 3 elements: (0,0), (1,0), (1,1) + + mjModel* model = LoadModelFromString(""); + mjData* data = mj_makeData(model); + + // M in CSR: row 0 has cols 0,1; row 1 has cols 0,1 + int colind[] = {0, 1, 0, 1}; + int rownnz[] = {2, 2}; + int rowadr[] = {0, 2}; + + // compute transpose using mju_transposeSparse + mjtNum mat[] = {1, 2, 3, 4}; + mjtNum matT[4]; + int colindT[4]; + int rownnzT[2]; + int rowadrT[2]; + mju_transposeSparse(matT, mat, 2, 2, rownnzT, rowadrT, colindT, nullptr, + rownnz, rowadr, colind); + + // use old function as ground truth + int rownnzH_expected[] = {0, 0}; + int rowadrH_expected[] = {0, 0}; + int nnz_expected = mju_sqrMatTDSparseCount( + rownnzH_expected, rowadrH_expected, 2, rownnz, rowadr, colind, rownnzT, + rowadrT, colindT, nullptr, data, /*flg_upper=*/0); + + // verify: lower triangle should have 3 elements: (0,0), (1,0), (1,1) + EXPECT_EQ(nnz_expected, 3); + EXPECT_THAT(rownnzH_expected, ElementsAre(1, 2)); + EXPECT_THAT(rowadrH_expected, ElementsAre(0, 1)); + + // test count mode of new function + int rownnzH[] = {0, 0}; + int rowadrH[] = {0, 0}; + + int nnz = mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, nullptr, nullptr, + 2, 2, rownnz, rowadr, colind, rownnzT, + rowadrT, colindT, nullptr, data); + + EXPECT_EQ(nnz, nnz_expected); + EXPECT_THAT(rownnzH, ElementsAre(rownnzH_expected[0], rownnzH_expected[1])); + EXPECT_THAT(rowadrH, ElementsAre(rowadrH_expected[0], rowadrH_expected[1])); + + // test fill mode + std::vector colindH(nnz, -1); + + mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, colindH.data(), nullptr, 2, 2, + rownnz, rowadr, colind, rownnzT, rowadrT, + colindT, nullptr, data); + + // verify: row 0 should have {0}, row 1 should have {0, 1} + EXPECT_THAT(colindH, ElementsAre(0, 0, 1)); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolicUpper) { + // Test flg_upper=1: count both lower and upper triangle + // Same matrix as previous test + + mjModel* model = LoadModelFromString(""); + mjData* data = mj_makeData(model); + + int colind[] = {0, 1, 0, 1}; + int rownnz[] = {2, 2}; + int rowadr[] = {0, 2}; + + mjtNum mat[] = {1, 2, 3, 4}; + mjtNum matT[4]; + int colindT[4]; + int rownnzT[2]; + int rowadrT[2]; + mju_transposeSparse(matT, mat, 2, 2, rownnzT, rowadrT, colindT, nullptr, + rownnz, rowadr, colind); + + // use old function as ground truth with flg_upper=1 + int rownnzH_expected[] = {0, 0}; + int rowadrH_expected[] = {0, 0}; + int nnz_expected = mju_sqrMatTDSparseCount( + rownnzH_expected, rowadrH_expected, 2, rownnz, rowadr, colind, rownnzT, + rowadrT, colindT, nullptr, data, /*flg_upper=*/1); + + // test new function with diagind (upper triangle) + int rownnzH[] = {0, 0}; + int rowadrH[] = {0, 0}; + int diagindH[] = {0, 0}; + int nnz = mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, nullptr, diagindH, + 2, 2, rownnz, rowadr, colind, rownnzT, + rowadrT, colindT, nullptr, data); + + EXPECT_EQ(nnz, nnz_expected); + EXPECT_THAT(rownnzH, ElementsAre(rownnzH_expected[0], rownnzH_expected[1])); + EXPECT_THAT(rowadrH, ElementsAre(rowadrH_expected[0], rowadrH_expected[1])); + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseSymbolicSupernode) { + // Test supernode exploitation with a matrix that has supernodes + // M has two rows with identical sparsity pattern + + mjModel* model = LoadModelFromString(""); + mjData* data = mj_makeData(model); + + // 3x2 matrix where rows 1 and 2 have same pattern + // 1 0 + // M = 2 3 + // 4 5 + int colind[] = {0, 0, 1, 0, 1}; + int rownnz[] = {1, 2, 2}; + int rowadr[] = {0, 1, 3}; + + mjtNum mat[] = {1, 2, 3, 4, 5}; + mjtNum matT[5]; + int colindT[5]; + int rownnzT[2]; + int rowadrT[2]; + mju_transposeSparse(matT, mat, 3, 2, rownnzT, rowadrT, colindT, nullptr, + rownnz, rowadr, colind); + + // compute rowsuperT + int rowsuperT[2]; + mju_superSparse(2, rowsuperT, rownnzT, rowadrT, colindT); + + // use old function as ground truth + int rownnzH_expected[] = {0, 0}; + int rowadrH_expected[] = {0, 0}; + int nnz_expected = mju_sqrMatTDSparseCount( + rownnzH_expected, rowadrH_expected, 2, rownnz, rowadr, colind, rownnzT, + rowadrT, colindT, rowsuperT, data, /*flg_upper=*/0); + + // test new function with supernodes + int rownnzH[] = {0, 0}; + int rowadrH[] = {0, 0}; + int nnz = mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, nullptr, nullptr, + 3, 2, rownnz, rowadr, colind, rownnzT, + rowadrT, colindT, rowsuperT, data); + + EXPECT_EQ(nnz, nnz_expected); + EXPECT_THAT(rownnzH, ElementsAre(rownnzH_expected[0], rownnzH_expected[1])); + EXPECT_THAT(rowadrH, ElementsAre(rowadrH_expected[0], rowadrH_expected[1])); + + // test fill mode with supernodes + std::vector colindH(nnz, -1); + mju_sqrMatTDSparseSymbolic(rownnzH, rowadrH, colindH.data(), nullptr, 3, 2, + rownnz, rowadr, colind, rownnzT, rowadrT, + colindT, rowsuperT, data); + + // verify all filled + for (int i = 0; i < nnz; i++) { + EXPECT_GE(colindH[i], 0) << "colindH[" << i << "] not filled"; + } + + // verify numeric phase with supernodes + std::vector resH(nnz); + mjtNum diag[] = {1, 1, 1, 1, 1}; // dummy diagonal + mju_sqrMatTDSparseNumeric(resH.data(), 2, rownnzH, rowadrH, colindH.data(), + nullptr, mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, rowsuperT, diag, data); + + // ground truth numeric + std::vector res_expected(4); + std::vector colindH_expected(4); + int rownnzH_exp[] = {0, 0}; + int rowadrH_exp[] = {0, 2}; + mju_sqrMatTDSparse(res_expected.data(), mat, matT, diag, 3, 2, rownnzH_exp, + rowadrH_exp, colindH_expected.data(), rownnz, rowadr, + colind, nullptr, rownnzT, rowadrT, colindT, rowsuperT, + data, nullptr); + + // compare values (sparse result vs sparse ground truth) + for (int r = 0; r < 2; r++) { + for (int i = 0; i < rownnzH[r]; i++) { + // find matching col in ground truth + int c = colindH[rowadrH[r] + i]; + mjtNum val = resH[rowadrH[r] + i]; + + bool found = false; + for (int j = 0; j < rownnzH_exp[r]; j++) { + if (colindH_expected[rowadrH_exp[r] + j] == c) { + EXPECT_NEAR(val, res_expected[rowadrH_exp[r] + j], 1e-14); + found = true; + break; + } + } + EXPECT_TRUE(found) << "Column " << c + << " not found in ground truth for row " << r; + } + } + + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(EngineUtilSparseTest, MjuSqrMatTDSparseNumeric) { + // Test numeric phase using symbolic phase + existing function as ground truth + // 1 2 + // M = 3 4 + + mjModel* model = LoadModelFromString(""); + mjData* data = mj_makeData(model); + + int colind[] = {0, 1, 0, 1}; + int rownnz[] = {2, 2}; + int rowadr[] = {0, 2}; + mjtNum mat[] = {1, 2, 3, 4}; + + // compute transpose + mjtNum matT[4]; + int colindT[4]; + int rownnzT[2]; + int rowadrT[2]; + mju_transposeSparse(matT, mat, 2, 2, rownnzT, rowadrT, colindT, nullptr, + rownnz, rowadr, colind); + + // compute supernodes + int rowsuperT[2]; + mju_superSparse(2, rowsuperT, rownnzT, rowadrT, colindT); + + mjtNum diag[] = {2, 3}; // diagonal weighting matrix + + // test both diagind cases: lower-only (diagind=NULL) and both triangles + // (diagind!=NULL) + for (int use_diagind = 0; use_diagind <= 1; use_diagind++) { + // compute sparsity pattern using symbolic phase + int rownnzH[] = {0, 0}; + int rowadrH[] = {0, 0}; + int diagindH[] = {0, 0}; + int nnz = mju_sqrMatTDSparseSymbolic( + rownnzH, rowadrH, nullptr, use_diagind ? diagindH : nullptr, 2, 2, + rownnz, rowadr, colind, rownnzT, rowadrT, colindT, nullptr, data); + + std::vector colindH(nnz); + mju_sqrMatTDSparseSymbolic( + rownnzH, rowadrH, colindH.data(), use_diagind ? diagindH : nullptr, 2, + 2, rownnz, rowadr, colind, rownnzT, rowadrT, colindT, nullptr, data); + + // compute values using numeric phase + std::vector resH(nnz); + mju_sqrMatTDSparseNumeric(resH.data(), 2, rownnzH, rowadrH, + colindH.data(), use_diagind ? diagindH : nullptr, + mat, rownnz, rowadr, colind, matT, rownnzT, + rowadrT, colindT, rowsuperT, diag, data); + + // compute ground truth using existing mju_sqrMatTDSparse + // use uncompressed storage to give the old function enough room + std::vector res_expected(4); // 2x2 uncompressed + std::vector colindH_expected(4); + int rownnzH_exp[] = {0, 0}; + int rowadrH_exp[] = {0, 2}; + int diagind_exp[] = {0, 0}; + mju_sqrMatTDSparse(res_expected.data(), mat, matT, diag, 2, 2, rownnzH_exp, + rowadrH_exp, colindH_expected.data(), rownnz, rowadr, + colind, nullptr, rownnzT, rowadrT, colindT, nullptr, + data, use_diagind ? diagind_exp : nullptr); + + // check that rownnz matches (nnz may differ due to compressed vs + // uncompressed storage) + EXPECT_EQ(rownnzH[0], rownnzH_exp[0]) + << "rownnz[0] mismatch for use_diagind=" << use_diagind; + EXPECT_EQ(rownnzH[1], rownnzH_exp[1]) + << "rownnz[1] mismatch for use_diagind=" << use_diagind; + + // compare column indices and values for each row + for (int r = 0; r < 2; r++) { + for (int j = 0; j < rownnzH[r]; j++) { + int idx = rowadrH[r] + j; + int idx_exp = rowadrH_exp[r] + j; + EXPECT_EQ(colindH[idx], colindH_expected[idx_exp]) + << "colind mismatch at row " << r << " pos " << j + << " for use_diagind=" << use_diagind; + EXPECT_NEAR(resH[idx], res_expected[idx_exp], 1e-10) + << "value mismatch at row " << r << " pos " << j + << " for use_diagind=" << use_diagind; + } + } + } mj_deleteData(data); mj_deleteModel(model); From e047f1101ae10843fa1e4c12e263477a939c4b65 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 15 Apr 2026 08:43:21 -0700 Subject: [PATCH 068/251] Rename GuiView to ImguiBridge. PiperOrigin-RevId: 900186368 Change-Id: Ie8f08d081dbc9a2b5638b5e799788e81891c908a --- .../filament/filament/filament_context.cc | 18 +++++----- .../filament/filament/filament_context.h | 4 +-- .../filament/{gui_view.cc => imgui_bridge.cc} | 35 ++++++++----------- .../filament/{gui_view.h => imgui_bridge.h} | 16 ++++----- .../filament/filament/scene_bridge.cc | 2 +- 5 files changed, 35 insertions(+), 40 deletions(-) rename src/experimental/filament/filament/{gui_view.cc => imgui_bridge.cc} (92%) rename src/experimental/filament/filament/{gui_view.h => imgui_bridge.h} (84%) diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index a7824f4f..5d667406 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -39,7 +39,7 @@ #include #include #include "experimental/filament/filament/filament_platform_factory.h" -#include "experimental/filament/filament/gui_view.h" +#include "experimental/filament/filament/imgui_bridge.h" #include "experimental/filament/filament/imgui_editor.h" #include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/object_manager.h" @@ -82,7 +82,7 @@ FilamentContext::FilamentContext(const mjrFilamentConfig* config) FilamentContext::~FilamentContext() { DestroyRenderTargets(); - gui_view_.reset(); + imgui_bridge_.reset(); scene_bridge_.reset(); scene_view_.reset(); object_manager_.reset(); @@ -96,7 +96,7 @@ void FilamentContext::Init(const mjModel* model) { scene_view_ = std::make_unique(engine_); scene_bridge_ = std::make_unique(object_manager_.get(), model, scene_view_.get()); - gui_view_ = std::make_unique( + imgui_bridge_ = std::make_unique( scene_view_.get(), object_manager_->GetMaterial(ObjectManager::kUnlitUi)); // Set clear options. @@ -127,11 +127,11 @@ void FilamentContext::Render(const mjrRect& viewport, const mjvScene* scene) { scene_bridge_->Update(viewport, scene); // Update the UX renderable entity after processing the scene in case there // are any elements in the scene which generate UX draw calls (e.g. labels). - if (gui_view_ && gui_swap_chain_target_ == scene_swap_chain_target_) { + if (imgui_bridge_ && gui_swap_chain_target_ == scene_swap_chain_target_) { // Prepare the filament Renderable that contains the GUI draw commands. We // must call this function even if we do not plan on rendering the GUI to // ensure the ImGui state is updated. - gui_view_->Update(); + imgui_bridge_->Update(); } last_render_mode_ = SceneView::DrawMode::kNormal; @@ -155,7 +155,7 @@ void FilamentContext::Render(const mjrRect& viewport, const mjvScene* scene) { request.viewport = viewport; request.camera = last_camera_; request.enable_ux = (gui_swap_chain_target_ == kWindowSwapChain); - request.gui_scale = gui_view_ ? gui_view_->GetScale() : 1.0f; + request.gui_scale = imgui_bridge_ ? imgui_bridge_->GetScale() : 1.0f; scene_view_->Render(renderer_, request); renderer_->endFrame(); } @@ -229,7 +229,7 @@ void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb, request.target = color_target_.get(); request.camera = last_camera_; request.enable_ux = (gui_swap_chain_target_ == kOffscreenSwapChain); - request.gui_scale = gui_view_ ? gui_view_->GetScale() : 1.0f; + request.gui_scale = imgui_bridge_ ? imgui_bridge_->GetScale() : 1.0f; scene_view_->Render(renderer_, request); const size_t num_bytes = viewport.width * viewport.height * 3; @@ -288,8 +288,8 @@ void FilamentContext::UploadHeightField(const mjModel* model, int id) { uintptr_t FilamentContext::UploadGuiImage(uintptr_t tex_id, const uint8_t* pixels, int width, int height, int bpp) { - if (gui_view_) { - return gui_view_->UploadImage(tex_id, pixels, width, height, bpp); + if (imgui_bridge_) { + return imgui_bridge_->UploadImage(tex_id, pixels, width, height, bpp); } return 0; } diff --git a/src/experimental/filament/filament/filament_context.h b/src/experimental/filament/filament/filament_context.h index 77c22a88..935ca708 100644 --- a/src/experimental/filament/filament/filament_context.h +++ b/src/experimental/filament/filament/filament_context.h @@ -24,7 +24,7 @@ #include #include #include -#include "experimental/filament/filament/gui_view.h" +#include "experimental/filament/filament/imgui_bridge.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/render_target.h" #include "experimental/filament/filament/scene_bridge.h" @@ -89,7 +89,7 @@ class FilamentContext { std::unique_ptr object_manager_; std::unique_ptr scene_view_; std::unique_ptr scene_bridge_; - std::unique_ptr gui_view_; + std::unique_ptr imgui_bridge_; int window_width_ = 0; int window_height_ = 0; }; diff --git a/src/experimental/filament/filament/gui_view.cc b/src/experimental/filament/filament/imgui_bridge.cc similarity index 92% rename from src/experimental/filament/filament/gui_view.cc rename to src/experimental/filament/filament/imgui_bridge.cc index 5802b06d..cb99e868 100644 --- a/src/experimental/filament/filament/gui_view.cc +++ b/src/experimental/filament/filament/imgui_bridge.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/filament/filament/gui_view.h" +#include "experimental/filament/filament/imgui_bridge.h" #include #include @@ -32,16 +32,13 @@ namespace mujoco { -GuiView::GuiView(SceneView* scene_view, filament::Material* ui_material) - : scene_view_(scene_view), material_(ui_material) { -} +ImguiBridge::ImguiBridge(SceneView* scene_view, filament::Material* ui_material) + : scene_view_(scene_view), material_(ui_material) {} -GuiView::~GuiView() { - PrepareRenderables(0); -} +ImguiBridge::~ImguiBridge() { PrepareRenderables(0); } -uintptr_t GuiView::UploadImage(uintptr_t tex_id, const uint8_t* pixels, - int width, int height, int bpp) { +uintptr_t ImguiBridge::UploadImage(uintptr_t tex_id, const uint8_t* pixels, + int width, int height, int bpp) { if (bpp != 4 && bpp != 3) { mju_error("Unsupported image bpp. Got %d, wanted 3 or 4", bpp); } @@ -79,9 +76,8 @@ uintptr_t GuiView::UploadImage(uintptr_t tex_id, const uint8_t* pixels, // lifetime of the data. const size_t num_bytes = width * height * bpp; std::byte* bytes = new std::byte[num_bytes]; - const auto callback = +[](void* user) { - delete[] reinterpret_cast(user); - }; + const auto callback = + +[](void* user) { delete[] reinterpret_cast(user); }; TextureData texture_data; DefaultTextureData(&texture_data); @@ -95,7 +91,7 @@ uintptr_t GuiView::UploadImage(uintptr_t tex_id, const uint8_t* pixels, return tex_id; } -void GuiView::CreateTexture(ImTextureData* data) { +void ImguiBridge::CreateTexture(ImTextureData* data) { if (data->Format != ImTextureFormat_RGBA32) { mju_error("Unsupported texture format."); } @@ -115,7 +111,7 @@ void GuiView::CreateTexture(ImTextureData* data) { UpdateTexture(data); } -void GuiView::UpdateTexture(ImTextureData* data) { +void ImguiBridge::UpdateTexture(ImTextureData* data) { auto iter = textures_.find(data->TexID); if (iter == textures_.end()) { mju_error("Texture not found: %llu", data->TexID); @@ -131,7 +127,7 @@ void GuiView::UpdateTexture(ImTextureData* data) { data->SetStatus(ImTextureStatus_OK); } -void GuiView::DestroyTexture(ImTextureData* data) { +void ImguiBridge::DestroyTexture(ImTextureData* data) { auto iter = textures_.find(data->TexID); if (iter != textures_.end()) { textures_.erase(data->TexID); @@ -140,7 +136,7 @@ void GuiView::DestroyTexture(ImTextureData* data) { } } -void GuiView::Update() { +void ImguiBridge::Update() { if (!ImGui::GetCurrentContext()) { PrepareRenderables(0); return; @@ -271,7 +267,7 @@ void GuiView::Update() { } } -void GuiView::PrepareRenderables(int count) { +void ImguiBridge::PrepareRenderables(int count) { while (renderables_.size() < count) { auto& r = renderables_.emplace_back( std::make_unique(scene_view_->GetEngine())); @@ -291,7 +287,7 @@ void GuiView::PrepareRenderables(int count) { } } -float GuiView::GetScale() const { +float ImguiBridge::GetScale() const { return ImGui::GetIO().DisplayFramebufferScale.x; } @@ -315,8 +311,7 @@ void DrawTextAt(const char* text, float x, float y, float z) { const int flags = ImGuiWindowFlags_NoBringToFrontOnFocus | ImGuiWindowFlags_NoFocusOnAppearing | ImGuiWindowFlags_NoBackground | - ImGuiWindowFlags_NoDecoration | - ImGuiWindowFlags_NoInputs | + ImGuiWindowFlags_NoDecoration | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_NoNav; ImGui::Begin("labels", nullptr, flags); diff --git a/src/experimental/filament/filament/gui_view.h b/src/experimental/filament/filament/imgui_bridge.h similarity index 84% rename from src/experimental/filament/filament/gui_view.h rename to src/experimental/filament/filament/imgui_bridge.h index 25dd8530..f8a711a8 100644 --- a/src/experimental/filament/filament/gui_view.h +++ b/src/experimental/filament/filament/imgui_bridge.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_GUI_VIEW_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_GUI_VIEW_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_IMGUI_BRIDGE_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_IMGUI_BRIDGE_H_ #include #include @@ -30,10 +30,10 @@ namespace mujoco { // Manages Renderables that will be added a SceneView's UX scene. -class GuiView { +class ImguiBridge { public: - GuiView(SceneView* scene_view, filament::Material* ui_material); - ~GuiView(); + ImguiBridge(SceneView* scene_view, filament::Material* ui_material); + ~ImguiBridge(); // Prepares the Renderables using data from the current ImGui state. This // function must be called once per frame to ensure ImGui state is correctly @@ -47,8 +47,8 @@ class GuiView { uintptr_t UploadImage(uintptr_t tex_id, const uint8_t* pixels, int width, int height, int bpp); - GuiView(const GuiView&) = delete; - GuiView& operator=(const GuiView&) = delete; + ImguiBridge(const ImguiBridge&) = delete; + ImguiBridge& operator=(const ImguiBridge&) = delete; private: // Ensures exactly `count` Renderables exist, creating or destroying them as @@ -72,4 +72,4 @@ void DrawTextAt(const char* text, float x, float y, float z); } // namespace mujoco -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_GUI_VIEW_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_IMGUI_BRIDGE_H_ diff --git a/src/experimental/filament/filament/scene_bridge.cc b/src/experimental/filament/filament/scene_bridge.cc index 4a9c17bd..229aba90 100644 --- a/src/experimental/filament/filament/scene_bridge.cc +++ b/src/experimental/filament/filament/scene_bridge.cc @@ -37,7 +37,7 @@ #include #include #include "experimental/filament/filament/color_grading_options.h" -#include "experimental/filament/filament/gui_view.h" +#include "experimental/filament/filament/imgui_bridge.h" #include "experimental/filament/filament/light.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/math_util.h" From 7ac30a39fe0bcc5d047bd496539419cafa403833 Mon Sep 17 00:00:00 2001 From: Matija Kecman Date: Wed, 15 Apr 2026 08:56:35 -0700 Subject: [PATCH 069/251] Fix mj_loadXML hang in single-threaded WASM builds The top-level CMakeLists.txt unconditionally added -pthread for all Emscripten builds, but PTHREAD_POOL_SIZE was only set when MUJOCO_WASM_THREADS=ON. This gave the ST build a live pthread runtime with zero pre-spawned workers, causing CompileMeshesAndTextures to deadlock when it tried to spawn a ThreadPool on the main thread. Fix by making -pthread conditional on MUJOCO_WASM_THREADS. Without -pthread, hardware_concurrency() returns 0 and the existing nthread < 2 guards naturally select the serial compilation path. PiperOrigin-RevId: 900191420 Change-Id: I0481b81fc6f7cccb52c7d7592dcf9dd317b5bad1 --- CMakeLists.txt | 8 ++++++-- wasm/CMakeLists.txt | 2 -- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 645ea60d..2c349167 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,8 +53,12 @@ endif() if(EMSCRIPTEN) option(MUJOCO_BUILD_TESTS_WASM "Build tests for WASM bindings" ON) - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -pthread") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20 -O3 -pthread -fexceptions") + option(MUJOCO_WASM_THREADS "Build with multi-threading support" ON) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20 -O3 -fexceptions") + if(MUJOCO_WASM_THREADS) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -pthread") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") + endif() endif() if(APPLE AND (MUJOCO_BUILD_EXAMPLES OR MUJOCO_BUILD_SIMULATE)) diff --git a/wasm/CMakeLists.txt b/wasm/CMakeLists.txt index 08004839..80ed4feb 100644 --- a/wasm/CMakeLists.txt +++ b/wasm/CMakeLists.txt @@ -31,8 +31,6 @@ if(NOT MUJOCO_WASM_FILES) message(FATAL_ERROR "No source files found in codegen/generated/") endif() -option(MUJOCO_WASM_THREADS "Build with multi-threading support" ON) - # Set Emscripten linker flags set(EMCC_LINKER_FLAGS "--bind" From d9b3faf8f48544f7d649c5b73eba9f8d255ed656 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Wed, 15 Apr 2026 09:19:06 -0700 Subject: [PATCH 070/251] Remove thread_local EPA data in favor of using mjData stack. PiperOrigin-RevId: 900201369 Change-Id: I82fbe9bf0ef9ea117c2124353d876dd42a57fb3a --- src/engine/engine_collision_convex.c | 18 ++----- src/engine/engine_collision_driver.c | 3 +- src/engine/engine_collision_gjk.c | 65 ++++++++---------------- src/engine/engine_collision_gjk.h | 14 ++--- src/engine/engine_support.c | 3 ++ test/engine/engine_collision_gjk_test.cc | 14 ++--- 6 files changed, 37 insertions(+), 80 deletions(-) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index f9c60231..a847a099 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -34,17 +34,6 @@ #define mjMINVAL2 (mjMINVAL * mjMINVAL) -// allocate callback for EPA in nativeccd -static void* ccd_allocate(void* data, size_t nbytes) { - mj_markStack((mjData*)data); - return mj_stackAllocByte((mjData*)data, nbytes, sizeof(mjtNum)); -} - -// free callback for EPA in nativeccd -static void ccd_free(void* data, void* buffer) { - mj_freeStack((mjData*)data); -} - // ccd prism first dir static void prism_firstdir(const void* o1, const void* o2, ccd_vec3_t *vec) { ccdVec3Set(vec, 0, 0, 1); @@ -98,15 +87,15 @@ static int mjc_penetration(const mjModel* m, mjData* d, mjCCDObj* obj1, mjCCDObj mjtNum dist; // set config + mj_markStack(d); config.max_iterations = m->opt.ccd_iterations; config.tolerance = m->opt.ccd_tolerance; config.max_contacts = ncon; config.dist_cutoff = 0; // no geom distances needed - config.context = (void*)d; - config.alloc = ccd_allocate; - config.free = ccd_free; + config.buffer = mj_stackAllocByte(d, mjc_ccdSize(config.max_iterations), sizeof(mjtNum)); if ((dist = mjc_ccd(&config, &status, obj1, obj2)) < 0) { + mj_freeStack(d); int nwitness = status.nx; for (int i = 0; i < nwitness; i++, con++) { con->dist = margin + dist; @@ -119,6 +108,7 @@ static int mjc_penetration(const mjModel* m, mjData* d, mjCCDObj* obj1, mjCCDObj } return nwitness; } + mj_freeStack(d); return 0; } diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index cd8bb5e4..eb092942 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -516,6 +516,7 @@ void mj_collision(const mjModel* m, mjData* d) { } } } + mj_freeStack(d); // finish merging predefined geom pairs for (; pairadr < npair; pairadr++) { @@ -572,8 +573,6 @@ void mj_collision(const mjModel* m, mjData* d) { // end narrowphase and midphase timer TM_END(mjTIMER_COL_NARROW); - - mj_freeStack(d); TM_END1(mjTIMER_POS_COLLISION); } diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 2d97eece..4b84695c 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -22,7 +22,6 @@ #include #include #include "engine/engine_collision_convex.h" -#include "engine/engine_macro.h" #include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" @@ -2208,16 +2207,17 @@ static inline void inflate(mjCCDStatus* status, mjtNum margin1, mjtNum margin2) } +// return size in bytes of the buffer needed for mjc_ccd for a given number of iterations +size_t mjc_ccdSize(int iterations) { + return (sizeof(Face) * 6 * iterations) // faces in polytope + + (sizeof(Face*) * 6 * iterations) // map in polytope + + (sizeof(Vertex) * (5 + iterations)) // vertices in polytope + + 2 * (24 * sizeof(int)); // horizon data +} + + // general convex collision detection mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2) { - // pre-allocate static memory for low iterations - void* buffer = NULL; - static mjTHREADLOCAL Vertex vert_data[5 + mjMAX_EPA_ITERATIONS]; - static mjTHREADLOCAL Face face_data[6 * mjMAX_EPA_ITERATIONS]; - static mjTHREADLOCAL Face* map_data[6 * mjMAX_EPA_ITERATIONS]; - static mjTHREADLOCAL int index_data[6 + mjMAX_EPA_ITERATIONS]; - static mjTHREADLOCAL int edge_data[6 + mjMAX_EPA_ITERATIONS]; - // setup obj1->center(status->x1, obj1); obj2->center(status->x2, obj2); @@ -2295,42 +2295,24 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m return status->dist; } - if (status->dist <= config->tolerance && status->nsimplex > 1) { + if (status->dist <= config->tolerance && status->nsimplex > 1 && config->buffer) { status->dist = 0; // assume touching Polytope pt; pt.nfaces = pt.nmap = pt.nverts = pt.horizon.nedges = 0; - // allocate memory via static thread-local storage + // allocate memory for polytope int N = config->max_iterations; - if (N <= mjMAX_EPA_ITERATIONS) { - pt.maxfaces = 6 * mjMAX_EPA_ITERATIONS; - pt.verts = vert_data; - pt.faces = face_data; - pt.map = map_data; - pt.horizon.indices = index_data; - pt.horizon.edges = edge_data; - } - - // static storage insufficient, allocate with callback - else { - size_t nbytes = (sizeof(Face) * 6 * N) // faces in polytope - + (sizeof(Face*) * 6 * N) // map in polytope - + (sizeof(Vertex) * (5 + N)) // vertices in polytope - + 2*(sizeof(int) * (6 + N)); // horizon data - - pt.maxfaces = 6 * N; - buffer = config->alloc(config->context, nbytes); - uint8_t* bbuffer = (uint8_t*)buffer; - pt.verts = (Vertex*)bbuffer; - bbuffer += sizeof(Vertex) * (5 + N); - pt.faces = (Face*)bbuffer; - bbuffer += sizeof(Face) * (6 * N); - pt.map = (Face**)bbuffer; - bbuffer += sizeof(Face*) * (6 * N); - pt.horizon.indices = (int*)bbuffer; - bbuffer += sizeof(int) * (6 + N); - pt.horizon.edges = (int*)bbuffer; - } + pt.maxfaces = 6 * N; + uint8_t* buffer = config->buffer; + pt.verts = (Vertex*)buffer; + buffer += sizeof(Vertex) * (5 + N); + pt.faces = (Face*)buffer; + buffer += sizeof(Face) * (6 * N); + pt.map = (Face**)buffer; + buffer += sizeof(Face*) * (6 * N); + pt.horizon.indices = (int*)buffer; + buffer += sizeof(int) * 24; + pt.horizon.edges = (int*)buffer; int ret; if (status->nsimplex == 2) { @@ -2350,9 +2332,6 @@ mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, m } } } - if (buffer) { - config->free(config->context, buffer); - } return status->dist; } diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h index 55ee5f8b..35cbf475 100644 --- a/src/engine/engine_collision_gjk.h +++ b/src/engine/engine_collision_gjk.h @@ -35,9 +35,6 @@ extern "C" { #define mjMAX_LIMIT FLT_MAX #endif -// max number of EPA iterations -#define mjMAX_EPA_ITERATIONS 170 - // tolerance for normal alignment of two faces (cosine of 1.6e-3) #define mjFACE_TOL 0.99999872 @@ -77,13 +74,7 @@ typedef struct { mjtNum tolerance; // tolerance used by GJK and EPA int max_contacts; // set to max number of contact points to recover mjtNum dist_cutoff; // set to max geom distance to recover - void* context; // opaque data pointer passed to callbacks - - // callback to allocate memory for polytope (only needed for penetration recovery) - void*(*alloc)(void* context, size_t nbytes); - - // callback to free memory from alloc callback - void(*free)(void* context, void* buffer); + void* buffer; // buffer memory for polytope (should be sized given by mjc_ccdSize) } mjCCDConfig; // data produced from running GJK and EPA @@ -108,6 +99,9 @@ typedef struct { int nsimplex; } mjCCDStatus; +// return size in bytes of the buffer needed for mjc_ccd for a given number of iterations +MJAPI size_t mjc_ccdSize(int iterations); + // run general convex collision detection, returns positive for distance, negative for penetration MJAPI mjtNum mjc_ccd(const mjCCDConfig* config, mjCCDStatus* status, mjCCDObj* obj1, mjCCDObj* obj2); #ifdef __cplusplus diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index c1ee7785..27537d39 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -526,6 +526,7 @@ void mj_xfrcAccumulate(const mjModel* m, mjData* d, mjtNum* qfrc) { // returns the smallest distance between two geoms (using nativeccd) static mjtNum mj_geomDistanceCCD(const mjModel* m, mjData* d, int g1, int g2, mjtNum distmax, mjtNum fromto[6]) { + mj_markStack(d); mjCCDConfig config; mjCCDStatus status; @@ -534,12 +535,14 @@ static mjtNum mj_geomDistanceCCD(const mjModel* m, mjData* d, int g1, int g2, config.tolerance = m->opt.ccd_tolerance; config.max_contacts = 1; // want contacts config.dist_cutoff = distmax; // want geom distances + config.buffer = mj_stackAllocByte(d, mjc_ccdSize(config.max_iterations), sizeof(mjtNum)); mjCCDObj obj1, obj2; mjc_initCCDObj(&obj1, m, d, g1, 0); mjc_initCCDObj(&obj2, m, d, g2, 0); mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2); + mj_freeStack(d); // witness points are only computed if dist <= distmax if (fromto && status.nx > 0) { diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 09e2c58e..653abbb4 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -61,14 +61,6 @@ constexpr char kEllipsoidXml[] = R"( )"; -void* CCDAllocate(void* data, std::size_t nbytes) { - return new std::byte[nbytes]; -} - -void CCDFree(void* data, void* buffer) { - delete [] (std::byte*)buffer; -} - mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], mjtNum x2[3], mjtNum cutoff = mjMAX_LIMIT) { mjCCDConfig config; @@ -79,6 +71,7 @@ mjtNum GeomDist(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], config.tolerance = kTolerance, config.max_contacts = 0; // no geom contacts needed config.dist_cutoff = cutoff; + config.buffer = nullptr; mjCCDObj obj1, obj2; mjc_initCCDObj(&obj1, m, d, g1, 0); @@ -129,14 +122,13 @@ int Penetration(mjCCDStatus& status, mjtNum& depth, std::vector& dir, mjCCDConfig config; // set config + auto buffer = std::vector(mjc_ccdSize(kMaxIterations)); config.max_iterations = kMaxIterations; config.tolerance = kTolerance; config.max_contacts = max_contacts; config.dist_cutoff = 0; // no geom distances needed config.max_contacts = max_contacts; - config.context = nullptr; - config.alloc = CCDAllocate; - config.free = CCDFree; + config.buffer = buffer.data(); mjtNum dist = mjc_ccd(&config, &status, &obj1, &obj2); if (dist < 0) { From eacbce95f440a64b545280bdf1892b4cfdb6e20d Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 15 Apr 2026 09:36:24 -0700 Subject: [PATCH 071/251] Fix CMakeLists file. gui_view was renamed imgui_bridge PiperOrigin-RevId: 900208849 Change-Id: I305a8a90306038c236256c21d04d2bad7144d83a --- src/experimental/filament/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index 7e6dfbd8..f5809b3c 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -31,8 +31,8 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/filament_context.h filament/filament_platform_factory.cc filament/filament_platform_factory.h - filament/gui_view.cc - filament/gui_view.h + filament/imgui_bridge.cc + filament/imgui_bridge.h filament/imgui_editor.cc filament/imgui_editor.h filament/light.cc From 564007204cebfd1c6d5e3a4656a6f24cbf6c1ed3 Mon Sep 17 00:00:00 2001 From: devshahofficial Date: Wed, 15 Apr 2026 12:15:24 -0700 Subject: [PATCH 072/251] Fix AttributeError in Renderer.__del__ on partial construction. If Renderer.__init__ raises before the rendering contexts are assigned (e.g. width > offwidth raises ValueError, or MjrContext construction fails), __del__ calls close() which accesses self._gl_context and self._mjr_context unconditionally, raising AttributeError. This masks the real __init__ failure with a noisy "Exception ignored in..." message during garbage collection. Pre-initialize both attributes to None at the top of __init__ so that close() is safe on a partially-constructed instance. Add a regression test that captures sys.unraisablehook and asserts that __del__ raises nothing when __init__ fails via the width > offwidth path. Fixes #3213. --- python/mujoco/renderer_test.py | 44 +++++++++++++++++++++ python/mujoco/rendering/classic/renderer.py | 8 +++- 2 files changed, 50 insertions(+), 2 deletions(-) diff --git a/python/mujoco/renderer_test.py b/python/mujoco/renderer_test.py index bb5f62f1..055a9476 100644 --- a/python/mujoco/renderer_test.py +++ b/python/mujoco/renderer_test.py @@ -14,6 +14,9 @@ # ============================================================================== """Tests for the MuJoCo renderer.""" +import gc +import sys + from absl.testing import absltest from absl.testing import parameterized import mujoco @@ -157,6 +160,47 @@ class MuJoCoRendererTest(parameterized.TestCase): with self.assertRaises(ValueError): renderer.render(out=np.zeros((*failing_render_size, 3), np.uint8)) + def test_renderer_del_safe_when_init_fails_early(self): + """Regression test for #3213. + + Renderer.__del__ must be safe on a partially-constructed instance when + __init__ raises before the rendering contexts are assigned. Previously, + AttributeError from __del__ masked the real __init__ exception. + """ + xml = """ + + + + + + +""" + model = mujoco.MjModel.from_xml_string(xml) + + # Capture any exception raised from __del__ on the partially-constructed + # Renderer. Without the fix, __del__ raises AttributeError, which is + # funneled through sys.unraisablehook. + unraisable = [] + old_hook = sys.unraisablehook + sys.unraisablehook = lambda args: unraisable.append(args) + try: + # width > offwidth raises ValueError in __init__ before + # self._gl_context is assigned. + with self.assertRaises(ValueError): + mujoco.Renderer(model, height=50, width=200) + gc.collect() + finally: + sys.unraisablehook = old_hook + + self.assertEqual( + [u.exc_type.__name__ for u in unraisable], + [], + msg=( + 'Renderer.__del__ raised on a partially-constructed instance; ' + 'see #3213.' + ), + ) + if __name__ == '__main__': absltest.main() diff --git a/python/mujoco/rendering/classic/renderer.py b/python/mujoco/rendering/classic/renderer.py index e2069db8..0e472e58 100644 --- a/python/mujoco/rendering/classic/renderer.py +++ b/python/mujoco/rendering/classic/renderer.py @@ -49,6 +49,11 @@ class Renderer: ValueError: If `camera_id` is outside the valid range, or if `width` or `height` exceed the dimensions of MuJoCo's offscreen framebuffer. """ + # Pre-initialize context attributes so __del__ -> close() is safe even if + # __init__ raises below before they are assigned. See #3213. + self._gl_context = None # type: ignore + self._mjr_context = None + buffer_width = model.vis.global_.offwidth buffer_height = model.vis.global_.offheight if width > buffer_width: @@ -80,9 +85,8 @@ the clause: # Create render contexts. # TODO(nimrod): Figure out why pytype doesn't like gl_context.GLContext - self._gl_context = None # type: ignore if gl_context.GLContext is not None: - self._gl_context = gl_context.GLContext(width, height) + self._gl_context = gl_context.GLContext(width, height) # type: ignore if self._gl_context: self._gl_context.make_current() self._mjr_context = mujoco.MjrContext(model, font_scale.value) From d39b2f4f9d31f940e7e18591c31e70a8db3afe95 Mon Sep 17 00:00:00 2001 From: Tarik Kelestemur Date: Wed, 15 Apr 2026 18:22:46 -0400 Subject: [PATCH 073/251] mjx segmentation --- doc/mjx.rst | 13 +- mjx/mujoco/mjx/__init__.py | 4 +- mjx/mujoco/mjx/_src/render.py | 24 +++- mjx/mujoco/mjx/_src/render_util.py | 90 +++++++++---- mjx/mujoco/mjx/_src/render_util_test.py | 86 ++++++++++++- mjx/mujoco/mjx/warp/io.py | 10 +- mjx/mujoco/mjx/warp/render.py | 162 ++++++++++++++++++++---- mjx/mujoco/mjx/warp/render_test.py | 91 ++++++++++++- 8 files changed, 414 insertions(+), 66 deletions(-) diff --git a/doc/mjx.rst b/doc/mjx.rst index 0d7f0b2d..247dafa8 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -233,6 +233,7 @@ pytree that should be passed into ``jit``/``vmap``-compiled functions: use_shadows=True, render_rgb=[True] * ncam, render_depth=[False] * ncam, + render_seg=[True] * ncam, enabled_geom_groups=[0, 1, 2], ) @@ -246,21 +247,23 @@ volume hierarchy (BVH) and executing the raycaster: .. code-block:: python from mujoco.mjx import get_rgb + from mujoco.mjx import get_segmentation @jax.jit def render_fn(mx, d, rc_pytree): # 1. Update the BVH for the current scene state d = mjx.refit_bvh(mx, d, rc_pytree) - # 2. Render all configured cameras - pixels, _ = mjx.render(mx, d, rc_pytree) + # 2. Render all configured cameras, including segmentation + pixels, _, segmentation = mjx.render_with_segmentation(mx, d, rc_pytree) - # 3. Extract the RGB tensor for the first camera (index 0) + # 3. Extract the RGB tensor and geom IDs for the first camera (index 0) rgb = get_rgb(rc_pytree, 0, pixels) + seg = get_segmentation(rc_pytree, 0, segmentation) - return rgb, d + return rgb, seg, d - rgb, d = render_fn(mx, d, rc.pytree()) + rgb, seg, d = render_fn(mx, d, rc.pytree()) .. WARNING:: The batch dimension ``nworld`` is fixed when the render context is created via diff --git a/mjx/mujoco/mjx/__init__.py b/mjx/mujoco/mjx/__init__.py index a0ee6dd4..4a7733c1 100644 --- a/mjx/mujoco/mjx/__init__.py +++ b/mjx/mujoco/mjx/__init__.py @@ -20,9 +20,9 @@ from mujoco.mjx._src.types import Model from mujoco.mjx._src.types import Data # isort: on +from mujoco.mjx._src.bvh import refit_bvh # pylint:disable=g-importing-member from mujoco.mjx._src.collision_driver import collision -from mujoco.mjx._src.bvh import refit_bvh from mujoco.mjx._src.constraint import make_constraint from mujoco.mjx._src.derivative import deriv_smooth_vel from mujoco.mjx._src.forward import euler @@ -46,8 +46,10 @@ from mujoco.mjx._src.io import state_size from mujoco.mjx._src.passive import passive from mujoco.mjx._src.ray import ray from mujoco.mjx._src.render import render +from mujoco.mjx._src.render import render_with_segmentation from mujoco.mjx._src.render_util import get_depth from mujoco.mjx._src.render_util import get_rgb +from mujoco.mjx._src.render_util import get_segmentation from mujoco.mjx._src.sensor import sensor_acc from mujoco.mjx._src.sensor import sensor_pos from mujoco.mjx._src.sensor import sensor_vel diff --git a/mjx/mujoco/mjx/_src/render.py b/mjx/mujoco/mjx/_src/render.py index 45f72aed..d6126529 100644 --- a/mjx/mujoco/mjx/_src/render.py +++ b/mjx/mujoco/mjx/_src/render.py @@ -15,6 +15,7 @@ """Render helpers for MJX.""" from typing import Any + # pylint: disable=g-importing-member from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import Impl @@ -26,8 +27,8 @@ import mujoco.mjx.warp as mjxw def render(m: Model, d: Data, ctx: Any) -> Data: """Render.""" if m.impl == Impl.WARP and d.impl == Impl.WARP and mjxw.WARP_INSTALLED: - import mujoco.mjx.warp.render_context as mjxw_rc # pylint: disable=g-import-not-at-top # pytype: disable=import-error - from mujoco.mjx.warp import render as mjxw_render # pylint: disable=g-import-not-at-top # pytype: disable=import-error + from mujoco.mjx.warp import render as mjxw_render + from mujoco.mjx.warp import render_context as mjxw_rc if not isinstance(ctx, mjxw_rc.RenderContextPytree): raise TypeError( @@ -38,3 +39,22 @@ def render(m: Model, d: Data, ctx: Any) -> Data: return mjxw_render.render(m, d, ctx) raise NotImplementedError('render only implemented for MuJoCo Warp.') + + +def render_with_segmentation(m: Model, d: Data, ctx: Any) -> Data: + """Render and return RGB, depth, and packed segmentation outputs.""" + if m.impl == Impl.WARP and d.impl == Impl.WARP and mjxw.WARP_INSTALLED: + from mujoco.mjx.warp import render as mjxw_render + from mujoco.mjx.warp import render_context as mjxw_rc + + if not isinstance(ctx, mjxw_rc.RenderContextPytree): + raise TypeError( + f'Expected RenderContextPytree, got {type(ctx).__name__}.' + ' Use rc.pytree() to get the JAX-compatible handle.' + ) + + return mjxw_render.render_with_segmentation(m, d, ctx) + + raise NotImplementedError( + 'render_with_segmentation only implemented for MuJoCo Warp.' + ) diff --git a/mjx/mujoco/mjx/_src/render_util.py b/mjx/mujoco/mjx/_src/render_util.py index 098291c3..56c7e6ad 100644 --- a/mjx/mujoco/mjx/_src/render_util.py +++ b/mjx/mujoco/mjx/_src/render_util.py @@ -25,6 +25,30 @@ if TYPE_CHECKING: from mujoco.mjx.warp.render_context import RenderContextPytree +def _get_warp_render_context(rc: 'RenderContextPytree'): + """Validates and returns the backing Warp render context.""" + if not mjxw.WARP_INSTALLED: + raise RuntimeError('Warp not installed.') + + from mujoco.mjx.warp import render_context as mjxw_rc + + if not isinstance(rc, mjxw_rc.RenderContextPytree): + raise TypeError( + f'Expected RenderContextPytree, got {type(rc).__name__}.' + ' Use rc.pytree() to get the JAX-compatible handle.' + ) + + # pylint: disable=protected-access + return mjxw_rc._MJX_RENDER_CONTEXT_BUFFERS[(rc.key, None)] + + +def _get_camera_resolution(warp_rc, cam_id: int) -> tuple[int, int]: + """Returns the render resolution for a given camera.""" + width = int(warp_rc.cam_res.numpy()[cam_id][0]) + height = int(warp_rc.cam_res.numpy()[cam_id][1]) + return width, height + + def get_rgb( rc: 'RenderContextPytree', cam_id: int, @@ -44,21 +68,9 @@ def get_rgb( Raises: RuntimeError: If Warp is not installed. """ - if not mjxw.WARP_INSTALLED: - raise RuntimeError('Warp not installed.') - - import mujoco.mjx.warp.render_context as mjxw_rc # pylint: disable=g-import-not-at-top # pytype: disable=import-error - - if not isinstance(rc, mjxw_rc.RenderContextPytree): - raise TypeError( - f'Expected RenderContextPytree, got {type(rc).__name__}.' - ' Use rc.pytree() to get the JAX-compatible handle.' - ) - - warp_rc = mjxw_rc._MJX_RENDER_CONTEXT_BUFFERS[(rc.key, None)] # pylint: disable=protected-access + warp_rc = _get_warp_render_context(rc) rgb_adr = int(warp_rc.rgb_adr.numpy()[cam_id]) - width = int(warp_rc.cam_res.numpy()[cam_id][0]) - height = int(warp_rc.cam_res.numpy()[cam_id][1]) + width, height = _get_camera_resolution(warp_rc, cam_id) packed = jax.lax.dynamic_slice_in_dim( rgb_data, rgb_adr, width * height, axis=rgb_data.ndim - 1 @@ -92,21 +104,9 @@ def get_depth( Raises: RuntimeError: If Warp is not installed. """ - if not mjxw.WARP_INSTALLED: - raise RuntimeError('Warp not installed.') - - import mujoco.mjx.warp.render_context as mjxw_rc # pylint: disable=g-import-not-at-top # pytype: disable=import-error - - if not isinstance(rc, mjxw_rc.RenderContextPytree): - raise TypeError( - f'Expected RenderContextPytree, got {type(rc).__name__}.' - ' Use rc.pytree() to get the JAX-compatible handle.' - ) - - warp_rc = mjxw_rc._MJX_RENDER_CONTEXT_BUFFERS[(rc.key, None)] # pylint: disable=protected-access + warp_rc = _get_warp_render_context(rc) depth_adr = int(warp_rc.depth_adr.numpy()[cam_id]) - width = int(warp_rc.cam_res.numpy()[cam_id][0]) - height = int(warp_rc.cam_res.numpy()[cam_id][1]) + width, height = _get_camera_resolution(warp_rc, cam_id) raw = jax.lax.dynamic_slice_in_dim( depth_data, depth_adr, width * height, axis=depth_data.ndim - 1 @@ -114,3 +114,37 @@ def get_depth( depth = jnp.clip(raw / depth_scale, 0.0, 1.0) return depth.reshape(raw.shape[:-1] + (height, width, 1)) + + +def get_segmentation( + rc: 'RenderContextPytree', + cam_id: int, + seg_data: jax.Array, +) -> jax.Array: + """Extract raw geom IDs for a camera. + + Args: + rc: RenderContextPytree. + cam_id: Camera index to extract. + seg_data: Packed segmentation output, shape (..., total_pixels) as integers. + + Returns: + Integer segmentation array with shape (..., H, W). + Any leading batch axes in `seg_data` are preserved. + + Raises: + RuntimeError: If Warp is not installed. + ValueError: If segmentation is not enabled for the selected camera. + """ + warp_rc = _get_warp_render_context(rc) + seg_adr = int(warp_rc.seg_adr.numpy()[cam_id]) + if seg_adr < 0: + raise ValueError( + f'Camera {cam_id} was not configured with segmentation rendering.' + ) + + width, height = _get_camera_resolution(warp_rc, cam_id) + packed = jax.lax.dynamic_slice_in_dim( + seg_data, seg_adr, width * height, axis=seg_data.ndim - 1 + ) + return packed.reshape(packed.shape[:-1] + (height, width)) diff --git a/mjx/mujoco/mjx/_src/render_util_test.py b/mjx/mujoco/mjx/_src/render_util_test.py index f97c1e0a..87065dcc 100644 --- a/mjx/mujoco/mjx/_src/render_util_test.py +++ b/mjx/mujoco/mjx/_src/render_util_test.py @@ -28,14 +28,23 @@ from mujoco.mjx.warp.render_context import RenderContextPytree _FORCE_TEST = os.environ.get('MJX_WARP_FORCE_TEST', '0') == '1' -def _fake_render_context(ncam, width, height): +def _fake_render_context(ncam, width, height, render_seg=True): """Fake RenderContext for testing.""" rc = mock.MagicMock() rgb_adr = np.arange(ncam, dtype=np.int32) * width * height depth_adr = np.arange(ncam, dtype=np.int32) * width * height + if isinstance(render_seg, bool): + render_seg = [render_seg] * ncam + seg_adr = np.full(ncam, -1, dtype=np.int32) + seg_offset = 0 + for i, enabled in enumerate(render_seg): + if enabled: + seg_adr[i] = seg_offset + seg_offset += width * height cam_res = np.tile([width, height], (ncam, 1)).astype(np.int32) rc.rgb_adr.numpy.return_value = rgb_adr rc.depth_adr.numpy.return_value = depth_adr + rc.seg_adr.numpy.return_value = seg_adr rc.cam_res.numpy.return_value = cam_res return rc @@ -154,6 +163,81 @@ class RenderUtilTest(absltest.TestCase): self.assertEqual(depth.shape, (nworld, height, width, 1)) + def test_get_segmentation(self): + width, height = 4, 4 + warp_rc = _fake_render_context(1, width, height) + rc = mock.MagicMock(spec=RenderContextPytree, key=0) + seg_data = jnp.arange(width * height, dtype=jnp.int32) + + with mock.patch.dict( + 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', + {(0, None): warp_rc}, + ): + segmentation = jax.jit( + render_util.get_segmentation, static_argnums=(0, 1) + )(rc, 0, seg_data) + + self.assertEqual(segmentation.shape, (height, width)) + np.testing.assert_array_equal( + np.asarray(segmentation), + np.arange(width * height, dtype=np.int32).reshape(height, width), + ) + + def test_get_segmentation_preserves_leading_dims(self): + width, height = 4, 4 + warp_rc = _fake_render_context(1, width, height) + rc = mock.MagicMock(spec=RenderContextPytree, key=0) + + with mock.patch.dict( + 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', + {(0, None): warp_rc}, + ): + for leading_shape in ((1,), (3,), (2, 3)): + with self.subTest(leading_shape=leading_shape): + seg_data = jnp.arange( + np.prod(leading_shape) * width * height, dtype=jnp.int32 + ).reshape(leading_shape + (width * height,)) + segmentation = jax.jit( + render_util.get_segmentation, static_argnums=(0, 1) + )(rc, 0, seg_data) + + self.assertEqual(segmentation.shape, leading_shape + (height, width)) + + def test_get_segmentation_vmap(self): + nworld, width, height = 3, 4, 4 + warp_rc = _fake_render_context(1, width, height) + rc = mock.MagicMock(spec=RenderContextPytree, key=0) + seg_data = jnp.arange(nworld * width * height, dtype=jnp.int32).reshape( + nworld, width * height + ) + + with mock.patch.dict( + 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', + {(0, None): warp_rc}, + ): + segmentation = jax.jit( + jax.vmap(render_util.get_segmentation, in_axes=(None, None, 0)), + static_argnums=(0, 1), + )(rc, 0, seg_data) + + self.assertEqual(segmentation.shape, (nworld, height, width)) + + def test_get_segmentation_raises_for_disabled_camera(self): + width, height = 4, 4 + warp_rc = _fake_render_context(2, width, height, render_seg=[True, False]) + rc = mock.MagicMock(spec=RenderContextPytree, key=0) + seg_data = jnp.arange(width * height, dtype=jnp.int32) + + with mock.patch.dict( + 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', + {(0, None): warp_rc}, + ): + with self.assertRaisesWithLiteralMatch( + ValueError, + 'Camera 1 was not configured with segmentation rendering.', + ): + render_util.get_segmentation(rc, 1, seg_data) + if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/warp/io.py b/mjx/mujoco/mjx/warp/io.py index b32d7122..d3179990 100644 --- a/mjx/mujoco/mjx/warp/io.py +++ b/mjx/mujoco/mjx/warp/io.py @@ -14,11 +14,12 @@ # ============================================================================== """I/O functions for MJX Warp.""" -import mujoco -from mujoco.mjx.warp import render_context -import mujoco.mjx.third_party.mujoco_warp as mjw import warp as wp +import mujoco +import mujoco.mjx.third_party.mujoco_warp as mjw +from mujoco.mjx.warp import render_context + _MJX_RENDER_CONTEXT_COUNTER = 0 @@ -27,8 +28,11 @@ def _create_context(mjm, nworld, device, **kwargs): ctx = mjw.create_render_context(mjm=mjm, nworld=nworld, **kwargs) ctx.rgb_data_shape = ctx.rgb_data.shape ctx.depth_data_shape = ctx.depth_data.shape + ctx.seg_data_shape = ctx.seg_data.shape + ctx.seg_data_buffer = ctx.seg_data ctx.rgb_data = None ctx.depth_data = None + ctx.seg_data = None return ctx diff --git a/mjx/mujoco/mjx/warp/render.py b/mjx/mujoco/mjx/warp/render.py index c97003f0..1361aa11 100644 --- a/mjx/mujoco/mjx/warp/render.py +++ b/mjx/mujoco/mjx/warp/render.py @@ -14,17 +14,19 @@ # ============================================================================== """DO NOT EDIT. This file is auto-generated.""" + import dataclasses import functools + import jax +import warp as wp + from mujoco.mjx._src import types +import mujoco.mjx.third_party.mujoco_warp as mjwarp +from mujoco.mjx.third_party.mujoco_warp._src import types as mjwp_types from mujoco.mjx.warp import ffi from mujoco.mjx.warp.render_context import _MJX_RENDER_CONTEXT_BUFFERS from mujoco.mjx.warp.render_context import RenderContextPytree -import mujoco.mjx.third_party.mujoco_warp as mjwarp -from mujoco.mjx.third_party.mujoco_warp._src import types as mjwp_types -import warp as wp - _m = mjwarp.Model( **{f.name: None for f in dataclasses.fields(mjwarp.Model) if f.init} @@ -122,36 +124,129 @@ def _render_shim( render_context = _MJX_RENDER_CONTEXT_BUFFERS[(rc_id, wp.get_device().ordinal)] render_context.rgb_data = rgb render_context.depth_data = depth + render_context.seg_data = render_context.seg_data_buffer mjwarp.render(_m, _d, render_context) -def _render_jax_impl(m: types.Model, d: types.Data, ctx: RenderContextPytree): +@ffi.format_args_for_warp +def _render_with_segmentation_shim( + # Model + nworld: int, + cam_fovy: wp.array2d[float], + cam_intrinsic: wp.array2d[wp.vec4], + cam_projection: wp.array[int], + cam_sensorsize: wp.array[wp.vec2], + flex_edge: wp.array[wp.vec2i], + flex_radius: wp.array[float], + flex_vertadr: wp.array[int], + geom_dataid: wp.array2d[int], + geom_matid: wp.array2d[int], + geom_rgba: wp.array2d[wp.vec4], + geom_size: wp.array2d[wp.vec3], + geom_type: wp.array[int], + light_active: wp.array2d[bool], + light_castshadow: wp.array2d[bool], + light_type: wp.array2d[int], + mat_rgba: wp.array2d[wp.vec4], + mat_texid: wp.array3d[int], + mat_texrepeat: wp.array2d[wp.vec2], + mesh_faceadr: wp.array[int], + nlight: int, + # Data + cam_xmat: wp.array2d[wp.mat33], + cam_xpos: wp.array2d[wp.vec3], + flexvert_xpos: wp.array2d[wp.vec3], + geom_xmat: wp.array2d[wp.mat33], + geom_xpos: wp.array2d[wp.vec3], + light_xdir: wp.array2d[wp.vec3], + light_xpos: wp.array2d[wp.vec3], + # Registry + rc_id: int, + rgb: wp.array2d[wp.uint32], + depth: wp.array2d[wp.float32], + seg: wp.array2d[int], +): + _m.stat = _s + _m.opt = _o + _m.callback = _cb + _d.efc = _e + _d.contact = _c + _m.cam_fovy = cam_fovy + _m.cam_intrinsic = cam_intrinsic + _m.cam_projection = cam_projection + _m.cam_sensorsize = cam_sensorsize + _m.flex_edge = flex_edge + _m.flex_radius = flex_radius + _m.flex_vertadr = flex_vertadr + _m.geom_dataid = geom_dataid + _m.geom_matid = geom_matid + _m.geom_rgba = geom_rgba + _m.geom_size = geom_size + _m.geom_type = geom_type + _m.light_active = light_active + _m.light_castshadow = light_castshadow + _m.light_type = light_type + _m.mat_rgba = mat_rgba + _m.mat_texid = mat_texid + _m.mat_texrepeat = mat_texrepeat + _m.mesh_faceadr = mesh_faceadr + _m.nlight = nlight + _d.cam_xmat = cam_xmat + _d.cam_xpos = cam_xpos + _d.flexvert_xpos = flexvert_xpos + _d.geom_xmat = geom_xmat + _d.geom_xpos = geom_xpos + _d.light_xdir = light_xdir + _d.light_xpos = light_xpos + _d.nworld = nworld + render_context = _MJX_RENDER_CONTEXT_BUFFERS[(rc_id, wp.get_device().ordinal)] + render_context.rgb_data = rgb + render_context.depth_data = depth + render_context.seg_data = seg + mjwarp.render(_m, _d, render_context) + + +_RENDER_STAGE_IN_ARGNAMES = set([ + 'cam_fovy', + 'cam_intrinsic', + 'cam_xmat', + 'cam_xpos', + 'geom_matid', + 'geom_rgba', + 'geom_size', + 'geom_xmat', + 'geom_xpos', + 'light_castshadow', + 'light_type', + 'mat_rgba', + 'mat_texid', +]) + + +def _render_jax_impl( + m: types.Model, + d: types.Data, + ctx: RenderContextPytree, + with_segmentation: bool = False, +): render_ctx = _MJX_RENDER_CONTEXT_BUFFERS[(ctx.key, None)] output_dims = { 'rgb': render_ctx.rgb_data_shape, 'depth': render_ctx.depth_data_shape, } + render_shim = _render_shim + num_outputs = 2 + if with_segmentation: + output_dims['seg'] = render_ctx.seg_data_shape + render_shim = _render_with_segmentation_shim + num_outputs = 3 jf = ffi.jax_callable_variadic_tuple( - _render_shim, - num_outputs=2, + render_shim, + num_outputs=num_outputs, output_dims=output_dims, vmap_method=None, in_out_argnames=set([]), - stage_in_argnames=set([ - 'cam_fovy', - 'cam_intrinsic', - 'cam_xmat', - 'cam_xpos', - 'geom_matid', - 'geom_rgba', - 'geom_size', - 'geom_xmat', - 'geom_xpos', - 'light_castshadow', - 'light_type', - 'mat_rgba', - 'mat_texid', - ]), + stage_in_argnames=_RENDER_STAGE_IN_ARGNAMES, stage_out_argnames=set([]), graph_mode=m.opt._impl.graph_mode, has_side_effect=False, @@ -208,3 +303,26 @@ def render_vmap( ): out = render(m, d, ctx) return out, [True, True] + + +@jax.custom_batching.custom_vmap +@functools.partial(ffi.marshal_jax_warp_callable, tree_map_output=True) +def render_with_segmentation( + m: types.Model, + d: types.Data, + ctx: RenderContextPytree, +): + return _render_jax_impl(m, d, ctx, with_segmentation=True) + + +@render_with_segmentation.def_vmap +@functools.partial(ffi.marshal_custom_vmap, tree_map_output=True) +def render_with_segmentation_vmap( + unused_axis_size, + is_batched, + m: types.Model, + d: types.Data, + ctx: RenderContextPytree, +): + out = render_with_segmentation(m, d, ctx) + return out, [True, True, True] diff --git a/mjx/mujoco/mjx/warp/render_test.py b/mjx/mujoco/mjx/warp/render_test.py index d4864d9c..fc089617 100644 --- a/mjx/mujoco/mjx/warp/render_test.py +++ b/mjx/mujoco/mjx/warp/render_test.py @@ -19,22 +19,22 @@ from absl.testing import absltest from absl.testing import parameterized import jax from jax import numpy as jp +import numpy as np + import mujoco from mujoco import mjx from mujoco.mjx._src import bvh from mujoco.mjx._src import forward from mujoco.mjx._src import io from mujoco.mjx._src import render -import mujoco.mjx.warp as mjxw from mujoco.mjx.warp import test_util as tu from mujoco.mjx.warp import warp as wp # pylint: disable=g-importing-member -import numpy as np - +import mujoco.mjx.warp as mjxw _FORCE_TEST = os.environ.get('MJX_WARP_FORCE_TEST', '0') == '1' -def _get_model_data_rc(xml, batch_size): +def _get_model_data_rc(xml, batch_size, render_seg=False): m = tu.load_test_file(xml) d = mujoco.MjData(m) mujoco.mj_forward(m, d) @@ -63,6 +63,7 @@ def _get_model_data_rc(xml, batch_size): use_shadows=True, render_rgb=True, render_depth=True, + render_seg=render_seg, enabled_geom_groups=[0, 1, 2], ) return mx, dx_batch, rc @@ -148,6 +149,88 @@ class RenderTest(parameterized.TestCase): self.assertGreater(np.count_nonzero(depth), 0) self.assertNotEqual(np.unique(depth).shape[0], 1) + @parameterized.product( + xml=('humanoid/humanoid.xml',), + batch_size=(1, 16), + ) + def test_render_with_segmentation(self, xml: str, batch_size: int): + """Tests MJX render pipeline with packed segmentation output.""" + self._maybe_skip() + mx, dx_batch, rc = _get_model_data_rc(xml, batch_size, render_seg=True) + + dx_batch = jax.jit(mjx.refit_bvh)(mx, dx_batch, rc.pytree()) + out_batch = jax.jit(mjx.render_with_segmentation)(mx, dx_batch, rc.pytree()) + + rgb = np.asarray(out_batch[0]) + depth = np.asarray(out_batch[1]) + seg = np.asarray(out_batch[2]) + + self.assertGreater(np.count_nonzero(rgb), 0) + self.assertGreater(np.count_nonzero(depth), 0) + self.assertTrue(np.any(seg != -1)) + self.assertGreater(np.unique(seg).shape[0], 1) + + unpacked_seg = jax.vmap(mjx.get_segmentation, in_axes=(None, None, 0))( + rc.pytree(), 0, out_batch[2] + ) + unpacked_seg = np.asarray(unpacked_seg) + width, height = rc._default.cam_res.numpy()[ + 0 + ] # pylint: disable=protected-access + seg_adr = int( + rc._default.seg_adr.numpy()[0] # pylint: disable=protected-access + ) + expected_seg = seg[:, seg_adr : seg_adr + width * height].reshape( + batch_size, height, width + ) + np.testing.assert_array_equal(unpacked_seg, expected_seg) + + @parameterized.product( + xml=('humanoid/humanoid.xml',), + batch_size=(4, 16), + ) + def test_render_with_segmentation_nested_vmap( + self, xml: str, batch_size: int + ): + """Tests MJX render_with_segmentation with nested vmap.""" + self._maybe_skip() + mx, dx_batch, rc = _get_model_data_rc(xml, batch_size, render_seg=True) + + def inner(mx, dx, rc): + dx = jax.vmap(bvh.refit_bvh, in_axes=(None, 0, None))(mx, dx, rc) + out = jax.vmap(render.render_with_segmentation, in_axes=(None, 0, None))( + mx, dx, rc + ) + return out + + dx_batch = jax.vmap(bvh.refit_bvh, in_axes=(None, 0, None))( + mx, dx_batch, rc.pytree() + ) + ref = jax.vmap(render.render_with_segmentation, in_axes=(None, 0, None))( + mx, dx_batch, rc.pytree() + ) + ref_rgb = np.asarray(ref[0]) + ref_depth = np.asarray(ref[1]) + ref_seg = np.asarray(ref[2]) + + def _reshape_batched(x): + if x.shape[0] == batch_size: + return x.reshape(2, batch_size // 2, *x.shape[1:]) + return x + + dx_2d = jax.tree.map(_reshape_batched, dx_batch) + + out_batch = jax.vmap(inner, in_axes=(None, 0, None))(mx, dx_2d, rc.pytree()) + out_batch = jax.tree.map(lambda x: x.reshape(-1, *x.shape[2:]), out_batch) + rgb = np.asarray(out_batch[0]) + depth = np.asarray(out_batch[1]) + seg = np.asarray(out_batch[2]) + + np.testing.assert_array_equal(rgb, ref_rgb) + np.testing.assert_array_equal(depth, ref_depth) + np.testing.assert_array_equal(seg, ref_seg) + self.assertTrue(np.any(seg != -1)) + class RenderContextGarbageCollectionTest(absltest.TestCase): """Tests that RenderContext cleans up buffers on deletion.""" From a6b98e5a78481706cfe5bf147af6d5de14478ef3 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 16 Apr 2026 01:41:13 -0700 Subject: [PATCH 074/251] Keep track of the vertex attributes in a Mesh. PiperOrigin-RevId: 900583030 Change-Id: I5b1ff5ad7b698e3e68fd06f44f2c5d3399d08504 --- src/experimental/filament/filament/mesh.cc | 8 ++++++++ src/experimental/filament/filament/mesh.h | 12 +++++++++++- 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/src/experimental/filament/filament/mesh.cc b/src/experimental/filament/filament/mesh.cc index 8acc2d59..0736cbd9 100644 --- a/src/experimental/filament/filament/mesh.cc +++ b/src/experimental/filament/filament/mesh.cc @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -202,6 +203,7 @@ void Mesh::BuildVertexBuffer(const MeshData& data) { vb_builder.normalized(usage); } offset += VertexAttributeTypeSize(attrib); + attributes_[i] = usage; } vertex_buffer_ = vb_builder.build(*engine_); vertex_buffer_->setBufferAt(*engine_, 0, {bytes, nbytes, callback, this}); @@ -221,7 +223,9 @@ void Mesh::BuildVertexBuffer(const MeshData& data) { if (usage == filament::VertexAttribute::COLOR) { vb_builder.normalized(usage); } + attributes_[i] = usage; } + num_attributes_ = data.nattributes; vertex_buffer_ = vb_builder.build(*engine_); // Assign the individual data buffers. @@ -334,6 +338,10 @@ filament::RenderableManager::PrimitiveType Mesh::GetPrimitiveType() const { return type_; } +std::span Mesh::GetVertexAttributes() const { + return {attributes_.data(), attributes_.data() + num_attributes_}; +} + bool Mesh::HasBounds() const { return bounds_.has_value(); } diff --git a/src/experimental/filament/filament/mesh.h b/src/experimental/filament/filament/mesh.h index 5953d0da..cc30b6d3 100644 --- a/src/experimental/filament/filament/mesh.h +++ b/src/experimental/filament/filament/mesh.h @@ -15,10 +15,12 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MESH_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MESH_H_ +#include #include #include #include #include +#include #include #include @@ -31,6 +33,9 @@ // Functions for creating filament vertex and index buffers. namespace mujoco { +// Maximum number of vertex attributes that can be used by a mesh. +static constexpr int kMaxVertexAttributes = 16; + // The type of data stored in an index buffer. typedef enum mjtIndexType_ { mjINDEX_TYPE_USHORT = 0, @@ -83,7 +88,7 @@ struct MeshData { // Information about each attribute of a vertex in the mesh. See `interleaved` // for more details. - VertexAttribute attributes[16]; + VertexAttribute attributes[kMaxVertexAttributes]; // Whether the vertex attributes are interleaved or not. // @@ -147,6 +152,9 @@ class Mesh { // Returns the primitive type of the mesh. filament::RenderableManager::PrimitiveType GetPrimitiveType() const; + // Returns the vertex attribute usages for the mesh. + std::span GetVertexAttributes() const; + // Returns whether the mesh has bounds. bool HasBounds() const; @@ -173,6 +181,8 @@ class Mesh { filament::RenderableManager::PrimitiveType::TRIANGLES; std::optional bounds_; std::vector> release_callbacks_; + std::array attributes_; + int num_attributes_ = 0; }; using MeshPtr = std::unique_ptr; From 5a0809efaf441d93030941065c679ed1e15a45fb Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 16 Apr 2026 03:23:50 -0700 Subject: [PATCH 075/251] Remove dependency on Texture from ObjectManager. ObjectManger now manages low-level filament Texture and Material objects directly. Moves the IBL Texture to the SceneBridge class. PiperOrigin-RevId: 900626110 Change-Id: Idd39464bfb324188dd54975a500954d4ebb29006 --- .../filament/filament/filament_context.cc | 8 +- .../filament/filament/imgui_bridge.cc | 10 +- .../filament/filament/imgui_bridge.h | 6 +- .../filament/filament/material.cc | 46 +++--- src/experimental/filament/filament/material.h | 11 +- .../filament/filament/object_manager.cc | 133 ++++++------------ .../filament/filament/object_manager.h | 42 ++++-- .../filament/filament/renderable.cc | 3 +- .../filament/filament/renderable.h | 3 +- .../filament/filament/scene_bridge.cc | 60 +++++--- .../filament/filament/scene_bridge.h | 7 +- .../filament/filament/scene_geom_util.cc | 6 +- .../filament/filament/scene_geom_util.h | 3 +- 13 files changed, 163 insertions(+), 175 deletions(-) diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index 5d667406..f43d473f 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -94,10 +94,10 @@ FilamentContext::~FilamentContext() { void FilamentContext::Init(const mjModel* model) { scene_view_ = std::make_unique(engine_); - scene_bridge_ = std::make_unique(object_manager_.get(), model, - scene_view_.get()); - imgui_bridge_ = std::make_unique( - scene_view_.get(), object_manager_->GetMaterial(ObjectManager::kUnlitUi)); + scene_bridge_ = std::make_unique(object_manager_.get(), + scene_view_.get(), model); + imgui_bridge_ = + std::make_unique(object_manager_.get(), scene_view_.get()); // Set clear options. filament::Renderer::ClearOptions opts; diff --git a/src/experimental/filament/filament/imgui_bridge.cc b/src/experimental/filament/filament/imgui_bridge.cc index cb99e868..efdadb07 100644 --- a/src/experimental/filament/filament/imgui_bridge.cc +++ b/src/experimental/filament/filament/imgui_bridge.cc @@ -21,19 +21,19 @@ #include #include -#include #include #include #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/renderable.h" +#include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/scene_view.h" #include "experimental/filament/filament/texture.h" namespace mujoco { -ImguiBridge::ImguiBridge(SceneView* scene_view, filament::Material* ui_material) - : scene_view_(scene_view), material_(ui_material) {} +ImguiBridge::ImguiBridge(ObjectManager* object_mgr, SceneView* scene_view) + : object_mgr_(object_mgr), scene_view_(scene_view) {} ImguiBridge::~ImguiBridge() { PrepareRenderables(0); } @@ -270,14 +270,14 @@ void ImguiBridge::Update() { void ImguiBridge::PrepareRenderables(int count) { while (renderables_.size() < count) { auto& r = renderables_.emplace_back( - std::make_unique(scene_view_->GetEngine())); + std::make_unique(object_mgr_)); r->SetCastShadows(false); r->SetReceiveShadows(false); r->SetBlendOrder(static_cast(renderables_.size())); Material& material = r->GetMaterial(); Material::DrawMode mode = Material::DrawMode::kNormal; - material.SetMaterial(mode, material_); + material.SetMaterial(mode, object_mgr_->GetMaterial(ObjectManager::kUnlitUi)); r->SetMaterialInstance(material.GetMaterialInstance(mode)); scene_view_->AddToUxScene(r.get()); } diff --git a/src/experimental/filament/filament/imgui_bridge.h b/src/experimental/filament/filament/imgui_bridge.h index f8a711a8..03ee4f47 100644 --- a/src/experimental/filament/filament/imgui_bridge.h +++ b/src/experimental/filament/filament/imgui_bridge.h @@ -21,18 +21,18 @@ #include #include -#include #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/scene_view.h" #include "experimental/filament/filament/texture.h" +#include "experimental/filament/filament/object_manager.h" namespace mujoco { // Manages Renderables that will be added a SceneView's UX scene. class ImguiBridge { public: - ImguiBridge(SceneView* scene_view, filament::Material* ui_material); + ImguiBridge(ObjectManager* object_mgr, SceneView* scene_view); ~ImguiBridge(); // Prepares the Renderables using data from the current ImGui state. This @@ -59,8 +59,8 @@ class ImguiBridge { void UpdateTexture(ImTextureData* data); void DestroyTexture(ImTextureData* data); + ObjectManager* object_mgr_ = nullptr; SceneView* scene_view_ = nullptr; - filament::Material* material_ = nullptr; std::vector> renderables_; std::vector meshes_; std::unordered_map> textures_; diff --git a/src/experimental/filament/filament/material.cc b/src/experimental/filament/filament/material.cc index 9dc7863c..864dea32 100644 --- a/src/experimental/filament/filament/material.cc +++ b/src/experimental/filament/filament/material.cc @@ -19,18 +19,20 @@ #include #include #include +#include #include "experimental/filament/filament/texture.h" +#include "experimental/filament/filament/object_manager.h" namespace mujoco { -Material::Material(filament::Engine* engine) - : engine_(engine) { +Material::Material(ObjectManager* object_mgr) + : object_mgr_(object_mgr) { } Material::~Material() noexcept { for (int i = 0; i < kNumDrawModes; ++i) { if (instances_[i]) { - engine_->destroy(instances_[i]); + GetEngine()->destroy(instances_[i]); } } } @@ -43,7 +45,7 @@ void Material::SetMaterial(DrawMode mode, filament::Material* material) { return; } - engine_->destroy(instances_[mode]); + GetEngine()->destroy(instances_[mode]); instances_[mode] = nullptr; } if (material) { @@ -62,10 +64,6 @@ void Material::UpdateTextures(const Textures& textures) { UpdateMaterialInstances(); } -void Material::SetFallbackTextures(const Textures* fallback_textures) { - fallback_textures_ = fallback_textures; -} - void Material::UpdateMaterialInstances() { filament::MaterialInstance* instance = instances_[DrawMode::kNormal]; if (instance == nullptr) { @@ -124,32 +122,24 @@ void Material::UpdateMaterialInstances() { filament::TextureSampler::MinFilter::LINEAR_MIPMAP_LINEAR); auto TrySetTexture = [&](const char* name, const Texture* texture, - const Texture* fallback) { + mjtTextureRole role) { if (material->hasParameter(name)) { - if (texture) { + if (texture != nullptr) { instance->setParameter(name, texture->GetFilamentTexture(), sampler); - } else if (fallback) { - instance->setParameter(name, fallback->GetFilamentTexture(), sampler); + } else { + instance->setParameter(name, object_mgr_->GetFallbackTexture(role), sampler); } } }; - TrySetTexture("BaseColor", textures_.color, - fallback_textures_ ? fallback_textures_->color : nullptr); - TrySetTexture("Normal", textures_.normal, - fallback_textures_ ? fallback_textures_->normal : nullptr); - TrySetTexture("Metallic", textures_.metallic, - fallback_textures_ ? fallback_textures_->metallic : nullptr); - TrySetTexture("Roughness", textures_.roughness, - fallback_textures_ ? fallback_textures_->roughness : nullptr); - TrySetTexture("Occlusion", textures_.occlusion, - fallback_textures_ ? fallback_textures_->occlusion : nullptr); - TrySetTexture("ORM", textures_.orm, - fallback_textures_ ? fallback_textures_->orm : nullptr); - TrySetTexture("Emissive", textures_.emissive, - fallback_textures_ ? fallback_textures_->emissive : nullptr); - TrySetTexture("Reflection", textures_.reflection, - fallback_textures_ ? fallback_textures_->reflection : nullptr); + TrySetTexture("BaseColor", textures_.color, mjTEXROLE_RGB); + TrySetTexture("Normal", textures_.normal, mjTEXROLE_NORMAL); + TrySetTexture("Metallic", textures_.metallic, mjTEXROLE_METALLIC); + TrySetTexture("Roughness", textures_.roughness, mjTEXROLE_ROUGHNESS); + TrySetTexture("Occlusion", textures_.occlusion, mjTEXROLE_OCCLUSION); + TrySetTexture("ORM", textures_.orm, mjTEXROLE_ORM); + TrySetTexture("Emissive", textures_.emissive, mjTEXROLE_EMISSIVE); + TrySetTexture("Reflection", textures_.reflection, mjTEXROLE_USER); } } // namespace mujoco diff --git a/src/experimental/filament/filament/material.h b/src/experimental/filament/filament/material.h index eaba6103..ff8c37af 100644 --- a/src/experimental/filament/filament/material.h +++ b/src/experimental/filament/filament/material.h @@ -21,6 +21,7 @@ #include #include #include "experimental/filament/filament/texture.h" +#include "experimental/filament/filament/object_manager.h" namespace mujoco { @@ -66,7 +67,7 @@ class Material { bool reflective = false; }; - explicit Material(filament::Engine* engine); + explicit Material(ObjectManager* object_mgr); ~Material() noexcept; Material(const Material&) = delete; @@ -75,9 +76,6 @@ class Material { // Assigns a material to the draw mode. void SetMaterial(DrawMode mode, filament::Material* material); - // Sets the fallback textures for the material. - void SetFallbackTextures(const Textures* fallback_textures); - // Updates the parameters for the material. void UpdateParams(const Params& params); @@ -96,16 +94,15 @@ class Material { } // Returns the filament Engine managing the material. - filament::Engine* GetEngine() const { return engine_; } + filament::Engine* GetEngine() const { return object_mgr_->GetEngine(); } private: // Updates the material instances based on the currently set parameters and // textures. void UpdateMaterialInstances(); - filament::Engine* engine_ = nullptr; + ObjectManager* object_mgr_; filament::MaterialInstance* instances_[kNumDrawModes] = {nullptr}; - const Textures* fallback_textures_ = nullptr; Params params_; Textures textures_; }; diff --git a/src/experimental/filament/filament/object_manager.cc b/src/experimental/filament/filament/object_manager.cc index 1d7bafa8..9beb1490 100644 --- a/src/experimental/filament/filament/object_manager.cc +++ b/src/experimental/filament/filament/object_manager.cc @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -24,40 +25,33 @@ #include #include #include -#include -#include -#include +#include #include #include "experimental/filament/filament/texture.h" #include "user/user_resource.h" namespace mujoco { -namespace { -// Loads binary data from a file using mjrFilamentConfig callbacks. -struct Asset { - explicit Asset(std::string_view filename) { - std::string path = "filament:" + std::string(filename); +static std::string GetAssetPath(std::string_view filename) { + std::string path = "filament:" + std::string(filename); + return path; +} - resource = mju_openResource("", path.c_str(), nullptr, nullptr, 0); - size = mju_readResource(resource, const_cast(&payload)); +ObjectManager::Asset::Asset(std::string_view filename) { + std::string path = GetAssetPath(filename); + resource = mju_openResource("", path.c_str(), nullptr, nullptr, 0); + size = mju_readResource(resource, const_cast(&payload)); +} + +ObjectManager::Asset::~Asset() { + if (resource) { + mju_closeResource(resource); } +} - ~Asset() { - if (resource) { - mju_closeResource(resource); - } - } - - Asset(const Asset&) = delete; - Asset& operator=(const Asset&) = delete; - - int size = 0; - void* payload = nullptr; - mjResource* resource = nullptr; -}; - -} // namespace +std::span ObjectManager::Asset::GetBytes() const { + return {reinterpret_cast(payload), size}; +} ObjectManager::ObjectManager(filament::Engine* engine) : engine_(engine) { @@ -93,24 +87,16 @@ ObjectManager::ObjectManager(filament::Engine* engine) static uint8_t normal_data[3] = {128, 128, 255}; static uint8_t orm_data[3] = {0, 255, 0}; - TextureConfig config; - DefaultTextureConfig(&config); - config.width = 1; - config.height = 1; - config.target = mjTEXTURE_2D; - config.format = mjPIXEL_FORMAT_RGB8; - config.color_space = mjCOLORSPACE_LINEAR; - - auto CreateFallbackTexture = [this, &config](uint8_t color[3]) { - auto texture = std::make_unique(engine_, config); - - TextureData payload; - DefaultTextureData(&payload); - payload.bytes = color; - payload.nbytes = 3; - payload.release_callback = nullptr; - payload.user_data = nullptr; - texture->Upload(payload); + auto CreateFallbackTexture = [this](uint8_t color[3]) { + filament::Texture::Builder builder; + builder.width(1); + builder.height(1); + builder.format(filament::Texture::InternalFormat::RGB8); + builder.sampler(filament::Texture::Sampler::SAMPLER_2D); + filament::Texture* texture = builder.build(*engine_); + const filament::Texture::Type type = filament::Texture::Type::UBYTE; + const filament::Texture::Format format = filament::Texture::Format::RGB; + texture->setImage(*engine_, 0, {color, 3, format, type}); return texture; }; @@ -119,19 +105,21 @@ ObjectManager::ObjectManager(filament::Engine* engine) fallback_normal_ = CreateFallbackTexture(normal_data); fallback_orm_ = CreateFallbackTexture(orm_data); - fallback_textures_[mjTEXROLE_USER] = fallback_black_.get(); - fallback_textures_[mjTEXROLE_RGB] = fallback_white_.get(); - fallback_textures_[mjTEXROLE_OCCLUSION] = fallback_white_.get(); - fallback_textures_[mjTEXROLE_ROUGHNESS] = fallback_white_.get(); - fallback_textures_[mjTEXROLE_METALLIC] = fallback_black_.get(); - fallback_textures_[mjTEXROLE_NORMAL] = fallback_normal_.get(); - fallback_textures_[mjTEXROLE_EMISSIVE] = fallback_black_.get(); - fallback_textures_[mjTEXROLE_ORM] = fallback_orm_.get(); - - LoadFallbackIndirectLight("ibl.ktx"); + fallback_textures_[mjTEXROLE_USER] = fallback_black_; + fallback_textures_[mjTEXROLE_RGB] = fallback_white_; + fallback_textures_[mjTEXROLE_OCCLUSION] = fallback_white_; + fallback_textures_[mjTEXROLE_ROUGHNESS] = fallback_white_; + fallback_textures_[mjTEXROLE_METALLIC] = fallback_black_; + fallback_textures_[mjTEXROLE_NORMAL] = fallback_normal_; + fallback_textures_[mjTEXROLE_EMISSIVE] = fallback_black_; + fallback_textures_[mjTEXROLE_ORM] = fallback_orm_; } ObjectManager::~ObjectManager() { + engine_->destroy(fallback_black_); + engine_->destroy(fallback_white_); + engine_->destroy(fallback_normal_); + engine_->destroy(fallback_orm_); for (auto& iter : materials_) { engine_->destroy(iter); } @@ -144,7 +132,7 @@ filament::Material* ObjectManager::GetMaterial(MaterialType type) const { return materials_[type]; } -const Texture* ObjectManager::GetFallbackTexture( +const filament::Texture* ObjectManager::GetFallbackTexture( mjtTextureRole role) const { if (role < 0 || role >= mjNTEXROLE) { mju_error("Invalid texture role: %d", role); @@ -152,39 +140,8 @@ const Texture* ObjectManager::GetFallbackTexture( return fallback_textures_[role]; } -const Texture* ObjectManager::GetFallbackIndirectLightTexture() { - return fallback_indirect_light_texture_.get(); -} - -void ObjectManager::LoadFallbackIndirectLight(std::string_view filename) { - fallback_indirect_light_texture_.reset(); - - Asset* asset = new Asset(filename); - auto release_asset = +[](void* user_data) { - delete static_cast(user_data); - }; - if (asset->size == 0) { - release_asset(asset); - return; - } - - TextureConfig config; - DefaultTextureConfig(&config); - config.width = 1; - config.height = 1; - config.target = mjTEXTURE_CUBE; - config.format = mjPIXEL_FORMAT_KTX; - config.color_space = mjCOLORSPACE_AUTO; - - fallback_indirect_light_texture_ = std::make_unique(engine_, config); - - TextureData payload; - DefaultTextureData(&payload); - payload.bytes = asset->payload; - payload.nbytes = static_cast(asset->size); - payload.release_callback = release_asset; - payload.user_data = asset; - - fallback_indirect_light_texture_->Upload(payload); +std::unique_ptr ObjectManager::LoadAsset( + std::string_view filename) { + return std::unique_ptr(new Asset(filename)); } } // namespace mujoco diff --git a/src/experimental/filament/filament/object_manager.h b/src/experimental/filament/filament/object_manager.h index 6502b7b9..140d08fe 100644 --- a/src/experimental/filament/filament/object_manager.h +++ b/src/experimental/filament/filament/object_manager.h @@ -16,20 +16,39 @@ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_OBJECT_MANAGER_H_ #include +#include #include +#include #include #include #include #include +#include #include -#include "experimental/filament/filament/texture.h" namespace mujoco { // Creates and owns various filament objects based on the data in a mjrContext. class ObjectManager { public: + class Asset { + public: + ~Asset(); + + std::span GetBytes() const; + + Asset(const Asset&) = delete; + Asset& operator=(const Asset&) = delete; + private: + friend class ObjectManager; + explicit Asset(std::string_view filename); + + std::size_t size = 0; + void* payload = nullptr; + mjResource* resource = nullptr; + }; + ObjectManager(filament::Engine* engine); ~ObjectManager(); @@ -63,13 +82,13 @@ class ObjectManager { filament::Material* GetMaterial(MaterialType type) const; // Returns the fallback Texture with the given role. - const Texture* GetFallbackTexture(mjtTextureRole role) const; + const filament::Texture* GetFallbackTexture(mjtTextureRole role) const; - // Returns the fallback IndirectLight. - const Texture* GetFallbackIndirectLightTexture(); + // Loads the given asset from the filament resource directory. + std::unique_ptr LoadAsset(std::string_view filename); - // Loads an indirect light from a file, setting it to the fallback. - void LoadFallbackIndirectLight(std::string_view filename); + // The default environment light to use if no environment light is specified. + static constexpr const char* kDefaultEnvironmentLight = "ibl.ktx"; ObjectManager(const ObjectManager&) = delete; ObjectManager& operator=(const ObjectManager&) = delete; @@ -77,12 +96,11 @@ class ObjectManager { private: filament::Engine* engine_ = nullptr; std::array materials_; - std::array fallback_textures_; - std::unique_ptr fallback_white_ = nullptr; - std::unique_ptr fallback_black_ = nullptr; - std::unique_ptr fallback_normal_ = nullptr; - std::unique_ptr fallback_orm_ = nullptr; - std::unique_ptr fallback_indirect_light_texture_; + std::array fallback_textures_; + filament::Texture* fallback_white_ = nullptr; + filament::Texture* fallback_black_ = nullptr; + filament::Texture* fallback_normal_ = nullptr; + filament::Texture* fallback_orm_ = nullptr; }; } // namespace mujoco diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index ea39df1a..cf4857fa 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -24,10 +24,11 @@ #include #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/mesh.h" +#include "experimental/filament/filament/object_manager.h" namespace mujoco { -Renderable::Renderable(filament::Engine* engine) : material_(engine) {} +Renderable::Renderable(ObjectManager* object_mgr) : material_(object_mgr) {} Renderable::~Renderable() noexcept { while (!entities_.empty()) { diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 32577beb..5dc3b87f 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -23,6 +23,7 @@ #include #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/mesh.h" +#include "experimental/filament/filament/object_manager.h" namespace mujoco { @@ -40,7 +41,7 @@ class Renderable { static constexpr std::uint8_t kDefaultPriority = 4; static constexpr std::uint8_t kDefaultLayerMask = 0x01; - Renderable(filament::Engine* engine); + explicit Renderable(ObjectManager* object_mgr); ~Renderable() noexcept; Renderable(const Renderable&) = delete; diff --git a/src/experimental/filament/filament/scene_bridge.cc b/src/experimental/filament/filament/scene_bridge.cc index 229aba90..e8198b00 100644 --- a/src/experimental/filament/filament/scene_bridge.cc +++ b/src/experimental/filament/filament/scene_bridge.cc @@ -47,6 +47,7 @@ #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/scene_geom_util.h" #include "experimental/filament/filament/scene_view.h" +#include "experimental/filament/filament/texture.h" namespace mujoco { @@ -55,8 +56,39 @@ using filament::math::float4; using filament::math::mat3; using filament::math::mat4; -SceneBridge::SceneBridge(ObjectManager* object_mgr, const mjModel* model, - SceneView* scene_view) +static std::unique_ptr CreateFallbackIndirectLightTexture( + ObjectManager* object_mgr, std::string_view filename = "") { + if (filename.empty()) { + filename = ObjectManager::kDefaultEnvironmentLight; + } + + std::unique_ptr asset = object_mgr->LoadAsset(filename); + + TextureConfig config; + DefaultTextureConfig(&config); + config.width = 1; + config.height = 1; + config.target = mjTEXTURE_CUBE; + config.format = mjPIXEL_FORMAT_KTX; + config.color_space = mjCOLORSPACE_AUTO; + + auto texture = std::make_unique(object_mgr->GetEngine(), config); + + TextureData payload; + DefaultTextureData(&payload); + payload.bytes = (void*)asset->GetBytes().data(); + payload.nbytes = asset->GetBytes().size(); + payload.release_callback = +[](void* user_data) { + delete static_cast(user_data); + }; + payload.user_data = asset.release(); + + texture->Upload(payload); + return texture; +} + +SceneBridge::SceneBridge(ObjectManager* object_mgr, SceneView* scene_view, + const mjModel* model) : scene_view_(scene_view), object_mgr_(object_mgr) { model_objects_ = std::make_unique(model, object_mgr_->GetEngine()); @@ -140,14 +172,6 @@ SceneBridge::SceneBridge(ObjectManager* object_mgr, const mjModel* model, ReadElement(model, "filament.fallback.environment_light_intensity", fallback_environment_light_intensity_); - fallback_textures_.color = object_mgr_->GetFallbackTexture(mjTEXROLE_RGB); - fallback_textures_.normal = object_mgr_->GetFallbackTexture(mjTEXROLE_NORMAL); - fallback_textures_.metallic = object_mgr_->GetFallbackTexture(mjTEXROLE_METALLIC); - fallback_textures_.roughness = object_mgr_->GetFallbackTexture(mjTEXROLE_ROUGHNESS); - fallback_textures_.occlusion = object_mgr_->GetFallbackTexture(mjTEXROLE_OCCLUSION); - fallback_textures_.orm = object_mgr_->GetFallbackTexture(mjTEXROLE_ORM); - fallback_textures_.emissive = object_mgr_->GetFallbackTexture(mjTEXROLE_EMISSIVE); - fallback_textures_.reflection = object_mgr_->GetFallbackTexture(mjTEXROLE_USER); PrepareLights(); } @@ -164,7 +188,7 @@ SceneBridge::~SceneBridge() { } void SceneBridge::SetEnvironmentLight(std::string_view filename, - float intensity) { + float intensity) { for (auto& light : lights_) { if (light->GetType() == mjLIGHT_IMAGE) { scene_view_->RemoveFromScene(light.get()); @@ -177,11 +201,12 @@ void SceneBridge::SetEnvironmentLight(std::string_view filename, fallback_ibl_.reset(); } - object_mgr_->LoadFallbackIndirectLight(filename); + fallback_ibl_texture_ = + CreateFallbackIndirectLightTexture(object_mgr_, filename); Light::Params params; params.type = mjLIGHT_IMAGE; - params.texture = object_mgr_->GetFallbackIndirectLightTexture(); + params.texture = fallback_ibl_texture_.get(); params.intensity = intensity; fallback_ibl_ = std::make_unique(object_mgr_->GetEngine(), params); scene_view_->AddToScene(fallback_ibl_.get()); @@ -274,9 +299,11 @@ void SceneBridge::PrepareLights() { // default environment light and set the light intensity ourselves. if (total_light_intensity == 0.0f) { // Create a fallback environment light. + fallback_ibl_texture_ = CreateFallbackIndirectLightTexture(object_mgr_); + Light::Params params; params.type = mjLIGHT_IMAGE; - params.texture = object_mgr_->GetFallbackIndirectLightTexture(); + params.texture = fallback_ibl_texture_.get(); params.intensity = fallback_environment_light_intensity_; fallback_ibl_ = std::make_unique(engine, params); scene_view_->AddToScene(fallback_ibl_.get()); @@ -354,9 +381,8 @@ void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { } } - std::unique_ptr renderable = - CreateGeomRenderable(*geom, scene, object_mgr_, model_objects_.get(), - headpos, &fallback_textures_); + std::unique_ptr renderable = CreateGeomRenderable( + *geom, scene, object_mgr_, model_objects_.get(), headpos); scene_view_->AddToScene(renderable.get()); renderables_.push_back(std::move(renderable)); diff --git a/src/experimental/filament/filament/scene_bridge.h b/src/experimental/filament/filament/scene_bridge.h index 8e847953..e707d6ed 100644 --- a/src/experimental/filament/filament/scene_bridge.h +++ b/src/experimental/filament/filament/scene_bridge.h @@ -30,14 +30,15 @@ #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/scene_view.h" +#include "experimental/filament/filament/texture.h" namespace mujoco { // Manages all mjModel data and updates a SceneView using an mjvScene. class SceneBridge { public: - SceneBridge(ObjectManager* object_mgr, const mjModel* model, - SceneView* scene_view); + SceneBridge(ObjectManager* object_mgr, SceneView* scene_view, + const mjModel* model); ~SceneBridge(); // Updates the environment light using the KTX image at the given path. @@ -72,6 +73,7 @@ class SceneBridge { ObjectManager* object_mgr_ = nullptr; std::unique_ptr model_objects_; std::unique_ptr fallback_ibl_; + std::unique_ptr fallback_ibl_texture_; std::vector> lights_; std::vector> renderables_; filament::math::mat4 clip_from_world_; @@ -80,7 +82,6 @@ class SceneBridge { float fallback_head_light_intensity_ = 0.f; float fallback_scene_light_intensity_ = 80'000.f; float fallback_environment_light_intensity_ = 5'000.f; - Material::Textures fallback_textures_; }; } // namespace mujoco diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index f8bf04f5..7b75a796 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -546,15 +546,13 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, std::unique_ptr CreateGeomRenderable( const mjvGeom& geom, const mjvScene* scene, ObjectManager* object_mgr, - ModelObjects* model_objs, const float headpos[3], - Material::Textures* fallback_textures) { - auto renderable = std::make_unique(model_objs->GetEngine()); + ModelObjects* model_objs, const float headpos[3]) { + auto renderable = std::make_unique(object_mgr); // The order of these calls is important. e.g. We need to create the filament // renderable entities before we can set their transform. PrepareGeomMeshes(*renderable, geom, scene, model_objs); SetGeomTransform(*renderable, geom); - renderable->GetMaterial().SetFallbackTextures(fallback_textures); UpdateGeomMaterial(*renderable, geom, scene, model_objs, object_mgr, headpos); return renderable; diff --git a/src/experimental/filament/filament/scene_geom_util.h b/src/experimental/filament/filament/scene_geom_util.h index 20311cc2..deef9c58 100644 --- a/src/experimental/filament/filament/scene_geom_util.h +++ b/src/experimental/filament/filament/scene_geom_util.h @@ -28,8 +28,7 @@ namespace mujoco { // Creates a Renderable from the given mjvGeom. std::unique_ptr CreateGeomRenderable( const mjvGeom& geom, const mjvScene* scene, ObjectManager* object_mgr, - ModelObjects* model_objs, const float headpos[3], - Material::Textures* fallback_textures); + ModelObjects* model_objs, const float headpos[3]); } // namespace mujoco From 56e98cc16ccc1410e788054ea90aada390b6e27c Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Thu, 16 Apr 2026 03:24:28 -0700 Subject: [PATCH 076/251] Improve AABB bounds in makeAAMM. MODEL BEFORE(ns) AFTER(ns) boxmesh.xml 874645 744822 box.xml 808226 786226 ellipsoid.xml 1477609 1436058 mixed.xml 1492063 1404848 PiperOrigin-RevId: 900626282 Change-Id: I3df5a6cd8a4cb2e3b9ded1b40a5310a4c14ec624 --- src/engine/engine_collision_driver.c | 32 ++++++++++++++++++++++------ src/engine/engine_inline.h | 15 +++++++++++++ src/engine/engine_util_misc.c | 12 ++--------- 3 files changed, 43 insertions(+), 16 deletions(-) diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index eb092942..b77164f6 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -27,6 +27,7 @@ #include "engine/engine_collision_sdf.h" #include "engine/engine_core_constraint.h" #include "engine/engine_core_util.h" +#include "engine/engine_inline.h" #include "engine/engine_macro.h" #include "engine/engine_memory.h" #include "engine/engine_sort.h" @@ -949,6 +950,7 @@ static void makeAAMM(const mjModel* m, mjData* d, mjtNum* x_max, mjtNum* y_max, mjtNum* z_max, int bf, const mjtNum* frame) { mjtNum aamm[6]; + mjtNum override_margin = mjENABLED(mjENBL_OVERRIDE) ? 0.5 * m->opt.o_margin : 0; // body if (bf < m->nbody) { @@ -958,14 +960,32 @@ static void makeAAMM(const mjModel* m, mjData* d, // process all body geoms (body is collidable, should have geoms) for (int i=0; i < body_geomnum; i++) { int geom = m->body_geomadr[body]+i; - mjtNum margin = mjENABLED(mjENBL_OVERRIDE) ? 0.5*m->opt.o_margin : m->geom_margin[geom]; + mjtNum margin = override_margin ? override_margin : m->geom_margin[geom]; mjtNum _aamm[6]; - // set _aamm for this geom + const mjtNum* aabb = m->geom_aabb + 6*geom; + const mjtNum* size = m->geom_aabb + 6*geom + 3; + const mjtNum* xpos = d->geom_xpos + 3*geom; + const mjtNum* xmat = d->geom_xmat + 9*geom; + + // compute center in global coordinates + mjtNum pos[3]; + mji_mulMatVec3(pos, xmat, aabb); + mju_addTo3(pos, xpos); + + mjtNum axis[9]; + mji_transpose3(axis, xmat); + mjtNum r_half = m->geom_rbound[geom]; + for (int j=0; j < 3; j++) { - mjtNum cen = mju_dot3(d->geom_xpos+3*geom, frame+3*j); - _aamm[j] = cen - m->geom_rbound[geom] - margin; - _aamm[j+3] = cen + m->geom_rbound[geom] + margin; + const mjtNum* frame_j = frame + 3*j; + mjtNum aabb_cen = mju_dot3(pos, frame_j); + mjtNum aabb_half = mju_abs(size[0] * mju_dot3(axis + 0, frame_j)) + + mju_abs(size[1] * mju_dot3(axis + 3, frame_j)) + + mju_abs(size[2] * mju_dot3(axis + 6, frame_j)); + mjtNum r_cen = mju_dot3(xpos, frame_j); + _aamm[j + 0] = mju_max(r_cen - r_half, aabb_cen - aabb_half) - margin; + _aamm[j + 3] = mju_min(r_cen + r_half, aabb_cen + aabb_half) + margin; } // update body aamm @@ -1006,7 +1026,7 @@ static void makeAAMM(const mjModel* m, mjData* d, } // correct for flex radius and margin - mjtNum margin = mjENABLED(mjENBL_OVERRIDE) ? 0.5*m->opt.o_margin : m->flex_margin[f]; + mjtNum margin = override_margin ? override_margin : m->flex_margin[f]; mjtNum bound = m->flex_radius[f] + margin; aamm[0] -= bound; aamm[1] -= bound; diff --git a/src/engine/engine_inline.h b/src/engine/engine_inline.h index bcf07756..dbe4b8f3 100644 --- a/src/engine/engine_inline.h +++ b/src/engine/engine_inline.h @@ -189,6 +189,21 @@ void mji_mulMatTMat3(mjtNum* restrict res, const mjtNum mat1[9], const mjtNum ma } +// transpose 3x3 matrix +static inline +void mji_transpose3(mjtNum* restrict res, const mjtNum mat[9]) { + res[0] = mat[0]; + res[1] = mat[3]; + res[2] = mat[6]; + res[3] = mat[1]; + res[4] = mat[4]; + res[5] = mat[7]; + res[6] = mat[2]; + res[7] = mat[5]; + res[8] = mat[8]; +} + + //------------------------------ 4D vector and matrix-vector operations ---------------------------- // res = vec diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 8a867b1d..abaf7055 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -1304,21 +1304,13 @@ void mju_printMatSparse(const mjtNum* mat, int nr, // min function, avoid re-evaluation mjtNum mju_min(mjtNum a, mjtNum b) { - if (a <= b) { - return a; - } else { - return b; - } + return a <= b ? a : b; } // max function, avoid re-evaluation mjtNum mju_max(mjtNum a, mjtNum b) { - if (a >= b) { - return a; - } else { - return b; - } + return a >= b ? a : b; } From 1117e7db397492e5dfc97e97c0c4e7d0152dd482 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 16 Apr 2026 04:43:39 -0700 Subject: [PATCH 077/251] Move DrawMode into its own header file. PiperOrigin-RevId: 900656695 Change-Id: I4db6f0f74c2109752efe40bc37c7104505139049 --- src/experimental/filament/CMakeLists.txt | 1 + .../filament/filament/draw_mode.h | 36 +++++++++++++++++++ .../filament/filament/filament_context.cc | 9 ++--- .../filament/filament/filament_context.h | 3 +- .../filament/filament/imgui_bridge.cc | 3 +- .../filament/filament/material.cc | 26 +++++++++----- src/experimental/filament/filament/material.h | 15 ++------ .../filament/filament/scene_geom_util.cc | 12 +++---- .../filament/filament/scene_view.cc | 12 +++---- .../filament/filament/scene_view.h | 8 ++--- 10 files changed, 78 insertions(+), 47 deletions(-) create mode 100644 src/experimental/filament/filament/draw_mode.h diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index f5809b3c..fa2081b5 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -27,6 +27,7 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/builtins.h filament/color_grading_options.cc filament/color_grading_options.h + filament/draw_mode.h filament/filament_context.cc filament/filament_context.h filament/filament_platform_factory.cc diff --git a/src/experimental/filament/filament/draw_mode.h b/src/experimental/filament/filament/draw_mode.h new file mode 100644 index 00000000..15bb2bca --- /dev/null +++ b/src/experimental/filament/filament/draw_mode.h @@ -0,0 +1,36 @@ +// Copyright 2026 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_DRAW_MODE_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_DRAW_MODE_H_ + +namespace mujoco { + +// The different modes that can be used to render the scene. +enum class DrawMode { + // Render the scene with "normal" colors and lighting. + Color, + // Render the scene as a grayscale depth map. + Depth, + // Render each object with a unique, uniform (flat) color regardless of + // lighting and texture. + Segmentation, +}; + +static constexpr int kNumDrawModes = 3; + +} // namespace mujoco + + +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_DRAW_MODE_H_ diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index f43d473f..4da0d63b 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -38,6 +38,7 @@ #include #include #include +#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/filament_platform_factory.h" #include "experimental/filament/filament/imgui_bridge.h" #include "experimental/filament/filament/imgui_editor.h" @@ -134,11 +135,11 @@ void FilamentContext::Render(const mjrRect& viewport, const mjvScene* scene) { imgui_bridge_->Update(); } - last_render_mode_ = SceneView::DrawMode::kNormal; + last_render_mode_ = DrawMode::Color; if (scene->flags[mjRND_SEGMENT]) { - last_render_mode_ = SceneView::DrawMode::kSegmentation; + last_render_mode_ = DrawMode::Segmentation; } else if (scene->flags[mjRND_DEPTH]) { - last_render_mode_ = SceneView::DrawMode::kDepth; + last_render_mode_ = DrawMode::Depth; } last_camera_ = mjv_averageCamera(scene->camera, scene->camera + 1); @@ -242,7 +243,7 @@ void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb, if (depth) { if (renderer_->beginFrame(offscreen_swap_chain_)) { SceneView::RenderRequest request; - request.draw_mode = SceneView::DrawMode::kDepth; + request.draw_mode = DrawMode::Depth; request.viewport = viewport; request.target = depth_target_.get(); request.camera = last_camera_; diff --git a/src/experimental/filament/filament/filament_context.h b/src/experimental/filament/filament/filament_context.h index 935ca708..3f4eeb7f 100644 --- a/src/experimental/filament/filament/filament_context.h +++ b/src/experimental/filament/filament/filament_context.h @@ -24,6 +24,7 @@ #include #include #include +#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/imgui_bridge.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/render_target.h" @@ -80,7 +81,7 @@ class FilamentContext { filament::SwapChain* offscreen_swap_chain_ = nullptr; std::unique_ptr platform_; - SceneView::DrawMode last_render_mode_ = SceneView::DrawMode::kNormal; + DrawMode last_render_mode_ = DrawMode::Color; mjvGLCamera last_camera_; SwapChainType scene_swap_chain_target_ = kWindowSwapChain; SwapChainType gui_swap_chain_target_ = kWindowSwapChain; diff --git a/src/experimental/filament/filament/imgui_bridge.cc b/src/experimental/filament/filament/imgui_bridge.cc index efdadb07..7337a5b0 100644 --- a/src/experimental/filament/filament/imgui_bridge.cc +++ b/src/experimental/filament/filament/imgui_bridge.cc @@ -23,6 +23,7 @@ #include #include #include +#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/renderable.h" @@ -276,7 +277,7 @@ void ImguiBridge::PrepareRenderables(int count) { r->SetBlendOrder(static_cast(renderables_.size())); Material& material = r->GetMaterial(); - Material::DrawMode mode = Material::DrawMode::kNormal; + DrawMode mode = DrawMode::Color; material.SetMaterial(mode, object_mgr_->GetMaterial(ObjectManager::kUnlitUi)); r->SetMaterialInstance(material.GetMaterialInstance(mode)); scene_view_->AddToUxScene(r.get()); diff --git a/src/experimental/filament/filament/material.cc b/src/experimental/filament/filament/material.cc index 864dea32..2e274f93 100644 --- a/src/experimental/filament/filament/material.cc +++ b/src/experimental/filament/filament/material.cc @@ -20,6 +20,7 @@ #include #include #include +#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/filament/object_manager.h" @@ -38,22 +39,27 @@ Material::~Material() noexcept { } void Material::SetMaterial(DrawMode mode, filament::Material* material) { - if (instances_[mode]) { + const int index = static_cast(mode); + if (instances_[index]) { const filament::Material* current_material = - instances_[mode]->getMaterial(); + instances_[index]->getMaterial(); if (current_material == material) { return; } - GetEngine()->destroy(instances_[mode]); - instances_[mode] = nullptr; + GetEngine()->destroy(instances_[index]); + instances_[index] = nullptr; } if (material) { - instances_[mode] = material->createInstance(); + instances_[index] = material->createInstance(); UpdateMaterialInstances(); } } +filament::MaterialInstance* Material::GetMaterialInstance(DrawMode mode) { + return instances_[static_cast(mode)]; +} + void Material::UpdateParams(const Params& params) { params_ = params; UpdateMaterialInstances(); @@ -65,7 +71,8 @@ void Material::UpdateTextures(const Textures& textures) { } void Material::UpdateMaterialInstances() { - filament::MaterialInstance* instance = instances_[DrawMode::kNormal]; + filament::MaterialInstance* instance = + instances_[static_cast(DrawMode::Color)]; if (instance == nullptr) { return; } @@ -107,9 +114,10 @@ void Material::UpdateMaterialInstances() { instance->setParameter("Reflectance", params_.reflectance); } - if (instances_[DrawMode::kSegmentation]) { - instances_[DrawMode::kSegmentation]->setParameter( - "BaseColorFactor", params_.segmentation_color); + const int segmentation_index = static_cast(DrawMode::Segmentation); + if (instances_[segmentation_index]) { + instances_[segmentation_index]->setParameter("BaseColorFactor", + params_.segmentation_color); } // All textures use the same default sampler. diff --git a/src/experimental/filament/filament/material.h b/src/experimental/filament/filament/material.h index ff8c37af..cf5854f6 100644 --- a/src/experimental/filament/filament/material.h +++ b/src/experimental/filament/filament/material.h @@ -20,6 +20,7 @@ #include #include #include +#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/filament/object_manager.h" @@ -27,16 +28,6 @@ namespace mujoco { class Material { public: - // The different methods for rendering objects. Each mode uses a different - // material, but all materials "share" the same textures and parameters - // (unless specifically noted otherwise). - enum DrawMode { - kNormal, - kDepth, - kSegmentation, - kNumDrawModes, - }; - // The textures that can be assigned to the drawable's material. struct Textures { const Texture* color = nullptr; @@ -89,9 +80,7 @@ class Material { const Textures& GetTextures() const { return textures_; } // Returns the material instance assigned to the draw mode. - filament::MaterialInstance* GetMaterialInstance(DrawMode mode) { - return instances_[mode]; - } + filament::MaterialInstance* GetMaterialInstance(DrawMode mode); // Returns the filament Engine managing the material. filament::Engine* GetEngine() const { return object_mgr_->GetEngine(); } diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index 7b75a796..3dba24d9 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -31,6 +31,7 @@ #include #include #include +#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" @@ -533,14 +534,11 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, params.glossiness *= model_objs->GetShininessMultiplier(); material.UpdateParams(params); + material.SetMaterial(DrawMode::Color, object_mgr->GetMaterial(material_type)); + material.SetMaterial(DrawMode::Depth, + object_mgr->GetMaterial(ObjectManager::kUnlitDepth)); material.SetMaterial( - Material::DrawMode::kNormal, - object_mgr->GetMaterial(material_type)); - material.SetMaterial( - Material::DrawMode::kDepth, - object_mgr->GetMaterial(ObjectManager::kUnlitDepth)); - material.SetMaterial( - Material::DrawMode::kSegmentation, + DrawMode::Segmentation, object_mgr->GetMaterial(ObjectManager::kUnlitSegmentation)); } diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 41f3d3ad..1f4c4009 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -40,6 +40,7 @@ #include #include #include "experimental/filament/filament/color_grading_options.h" +#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/light.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/math_util.h" @@ -53,12 +54,9 @@ using filament::math::float3; using filament::math::float4; using filament::math::mat4; -static constexpr int kNormalIndex = - static_cast(Material::DrawMode::kNormal); -static constexpr int kDepthIndex = - static_cast(Material::DrawMode::kDepth); -static constexpr int kSegmentIndex = - static_cast(Material::DrawMode::kSegmentation); +static constexpr int kNormalIndex = static_cast(DrawMode::Color); +static constexpr int kDepthIndex = static_cast(DrawMode::Depth); +static constexpr int kSegmentIndex = static_cast(DrawMode::Segmentation); static filament::ColorGrading::Builder ToBuilder( const ColorGradingOptions& opts) { @@ -279,7 +277,7 @@ void SceneView::Render(filament::Renderer* renderer, } // Render reflection passes. - if (request.draw_mode == DrawMode::kNormal) { + if (request.draw_mode == DrawMode::Color) { filament::TransformManager& tm = engine_->getTransformManager(); for (size_t i = 0; i < reflectives_.size(); ++i) { Renderable* renderable = reflectives_[i]; diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index d6546d7c..f50a86fa 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -27,8 +27,8 @@ #include #include #include "experimental/filament/filament/color_grading_options.h" +#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/light.h" -#include "experimental/filament/filament/material.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/render_target.h" @@ -58,10 +58,9 @@ class SceneView { void RemoveFromUxScene(Renderable* renderable); // Parameters for rendering the scene. - using DrawMode = Material::DrawMode; struct RenderRequest { // The draw mode (e.g. normal, depth, segmentation) to render. - DrawMode draw_mode = DrawMode::kNormal; + DrawMode draw_mode = DrawMode::Color; // The target viewport for the rendered image. mjrRect viewport; // The camera from which to render the scene. @@ -102,8 +101,7 @@ class SceneView { filament::Camera* camera_ = nullptr; filament::ColorGrading* color_grading_ = nullptr; ColorGradingOptions color_grading_options_; - std::array views_; - DrawMode active_mode_ = DrawMode::kNumDrawModes; + std::array views_; // Scene objects. std::unordered_set lights_; From 01a043f20b4914e5ddc98df5ac874cf3c0b35a97 Mon Sep 17 00:00:00 2001 From: Tarik Kelestemur Date: Wed, 15 Apr 2026 18:22:46 -0400 Subject: [PATCH 078/251] mjx segmentation --- doc/mjx.rst | 13 +- mjx/mujoco/mjx/__init__.py | 4 +- mjx/mujoco/mjx/_src/render.py | 24 +++- mjx/mujoco/mjx/_src/render_util.py | 90 +++++++++---- mjx/mujoco/mjx/_src/render_util_test.py | 86 ++++++++++++- mjx/mujoco/mjx/warp/io.py | 10 +- mjx/mujoco/mjx/warp/render.py | 162 ++++++++++++++++++++---- mjx/mujoco/mjx/warp/render_test.py | 91 ++++++++++++- 8 files changed, 414 insertions(+), 66 deletions(-) diff --git a/doc/mjx.rst b/doc/mjx.rst index 0d7f0b2d..247dafa8 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -233,6 +233,7 @@ pytree that should be passed into ``jit``/``vmap``-compiled functions: use_shadows=True, render_rgb=[True] * ncam, render_depth=[False] * ncam, + render_seg=[True] * ncam, enabled_geom_groups=[0, 1, 2], ) @@ -246,21 +247,23 @@ volume hierarchy (BVH) and executing the raycaster: .. code-block:: python from mujoco.mjx import get_rgb + from mujoco.mjx import get_segmentation @jax.jit def render_fn(mx, d, rc_pytree): # 1. Update the BVH for the current scene state d = mjx.refit_bvh(mx, d, rc_pytree) - # 2. Render all configured cameras - pixels, _ = mjx.render(mx, d, rc_pytree) + # 2. Render all configured cameras, including segmentation + pixels, _, segmentation = mjx.render_with_segmentation(mx, d, rc_pytree) - # 3. Extract the RGB tensor for the first camera (index 0) + # 3. Extract the RGB tensor and geom IDs for the first camera (index 0) rgb = get_rgb(rc_pytree, 0, pixels) + seg = get_segmentation(rc_pytree, 0, segmentation) - return rgb, d + return rgb, seg, d - rgb, d = render_fn(mx, d, rc.pytree()) + rgb, seg, d = render_fn(mx, d, rc.pytree()) .. WARNING:: The batch dimension ``nworld`` is fixed when the render context is created via diff --git a/mjx/mujoco/mjx/__init__.py b/mjx/mujoco/mjx/__init__.py index a0ee6dd4..4a7733c1 100644 --- a/mjx/mujoco/mjx/__init__.py +++ b/mjx/mujoco/mjx/__init__.py @@ -20,9 +20,9 @@ from mujoco.mjx._src.types import Model from mujoco.mjx._src.types import Data # isort: on +from mujoco.mjx._src.bvh import refit_bvh # pylint:disable=g-importing-member from mujoco.mjx._src.collision_driver import collision -from mujoco.mjx._src.bvh import refit_bvh from mujoco.mjx._src.constraint import make_constraint from mujoco.mjx._src.derivative import deriv_smooth_vel from mujoco.mjx._src.forward import euler @@ -46,8 +46,10 @@ from mujoco.mjx._src.io import state_size from mujoco.mjx._src.passive import passive from mujoco.mjx._src.ray import ray from mujoco.mjx._src.render import render +from mujoco.mjx._src.render import render_with_segmentation from mujoco.mjx._src.render_util import get_depth from mujoco.mjx._src.render_util import get_rgb +from mujoco.mjx._src.render_util import get_segmentation from mujoco.mjx._src.sensor import sensor_acc from mujoco.mjx._src.sensor import sensor_pos from mujoco.mjx._src.sensor import sensor_vel diff --git a/mjx/mujoco/mjx/_src/render.py b/mjx/mujoco/mjx/_src/render.py index 45f72aed..d6126529 100644 --- a/mjx/mujoco/mjx/_src/render.py +++ b/mjx/mujoco/mjx/_src/render.py @@ -15,6 +15,7 @@ """Render helpers for MJX.""" from typing import Any + # pylint: disable=g-importing-member from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import Impl @@ -26,8 +27,8 @@ import mujoco.mjx.warp as mjxw def render(m: Model, d: Data, ctx: Any) -> Data: """Render.""" if m.impl == Impl.WARP and d.impl == Impl.WARP and mjxw.WARP_INSTALLED: - import mujoco.mjx.warp.render_context as mjxw_rc # pylint: disable=g-import-not-at-top # pytype: disable=import-error - from mujoco.mjx.warp import render as mjxw_render # pylint: disable=g-import-not-at-top # pytype: disable=import-error + from mujoco.mjx.warp import render as mjxw_render + from mujoco.mjx.warp import render_context as mjxw_rc if not isinstance(ctx, mjxw_rc.RenderContextPytree): raise TypeError( @@ -38,3 +39,22 @@ def render(m: Model, d: Data, ctx: Any) -> Data: return mjxw_render.render(m, d, ctx) raise NotImplementedError('render only implemented for MuJoCo Warp.') + + +def render_with_segmentation(m: Model, d: Data, ctx: Any) -> Data: + """Render and return RGB, depth, and packed segmentation outputs.""" + if m.impl == Impl.WARP and d.impl == Impl.WARP and mjxw.WARP_INSTALLED: + from mujoco.mjx.warp import render as mjxw_render + from mujoco.mjx.warp import render_context as mjxw_rc + + if not isinstance(ctx, mjxw_rc.RenderContextPytree): + raise TypeError( + f'Expected RenderContextPytree, got {type(ctx).__name__}.' + ' Use rc.pytree() to get the JAX-compatible handle.' + ) + + return mjxw_render.render_with_segmentation(m, d, ctx) + + raise NotImplementedError( + 'render_with_segmentation only implemented for MuJoCo Warp.' + ) diff --git a/mjx/mujoco/mjx/_src/render_util.py b/mjx/mujoco/mjx/_src/render_util.py index 098291c3..56c7e6ad 100644 --- a/mjx/mujoco/mjx/_src/render_util.py +++ b/mjx/mujoco/mjx/_src/render_util.py @@ -25,6 +25,30 @@ if TYPE_CHECKING: from mujoco.mjx.warp.render_context import RenderContextPytree +def _get_warp_render_context(rc: 'RenderContextPytree'): + """Validates and returns the backing Warp render context.""" + if not mjxw.WARP_INSTALLED: + raise RuntimeError('Warp not installed.') + + from mujoco.mjx.warp import render_context as mjxw_rc + + if not isinstance(rc, mjxw_rc.RenderContextPytree): + raise TypeError( + f'Expected RenderContextPytree, got {type(rc).__name__}.' + ' Use rc.pytree() to get the JAX-compatible handle.' + ) + + # pylint: disable=protected-access + return mjxw_rc._MJX_RENDER_CONTEXT_BUFFERS[(rc.key, None)] + + +def _get_camera_resolution(warp_rc, cam_id: int) -> tuple[int, int]: + """Returns the render resolution for a given camera.""" + width = int(warp_rc.cam_res.numpy()[cam_id][0]) + height = int(warp_rc.cam_res.numpy()[cam_id][1]) + return width, height + + def get_rgb( rc: 'RenderContextPytree', cam_id: int, @@ -44,21 +68,9 @@ def get_rgb( Raises: RuntimeError: If Warp is not installed. """ - if not mjxw.WARP_INSTALLED: - raise RuntimeError('Warp not installed.') - - import mujoco.mjx.warp.render_context as mjxw_rc # pylint: disable=g-import-not-at-top # pytype: disable=import-error - - if not isinstance(rc, mjxw_rc.RenderContextPytree): - raise TypeError( - f'Expected RenderContextPytree, got {type(rc).__name__}.' - ' Use rc.pytree() to get the JAX-compatible handle.' - ) - - warp_rc = mjxw_rc._MJX_RENDER_CONTEXT_BUFFERS[(rc.key, None)] # pylint: disable=protected-access + warp_rc = _get_warp_render_context(rc) rgb_adr = int(warp_rc.rgb_adr.numpy()[cam_id]) - width = int(warp_rc.cam_res.numpy()[cam_id][0]) - height = int(warp_rc.cam_res.numpy()[cam_id][1]) + width, height = _get_camera_resolution(warp_rc, cam_id) packed = jax.lax.dynamic_slice_in_dim( rgb_data, rgb_adr, width * height, axis=rgb_data.ndim - 1 @@ -92,21 +104,9 @@ def get_depth( Raises: RuntimeError: If Warp is not installed. """ - if not mjxw.WARP_INSTALLED: - raise RuntimeError('Warp not installed.') - - import mujoco.mjx.warp.render_context as mjxw_rc # pylint: disable=g-import-not-at-top # pytype: disable=import-error - - if not isinstance(rc, mjxw_rc.RenderContextPytree): - raise TypeError( - f'Expected RenderContextPytree, got {type(rc).__name__}.' - ' Use rc.pytree() to get the JAX-compatible handle.' - ) - - warp_rc = mjxw_rc._MJX_RENDER_CONTEXT_BUFFERS[(rc.key, None)] # pylint: disable=protected-access + warp_rc = _get_warp_render_context(rc) depth_adr = int(warp_rc.depth_adr.numpy()[cam_id]) - width = int(warp_rc.cam_res.numpy()[cam_id][0]) - height = int(warp_rc.cam_res.numpy()[cam_id][1]) + width, height = _get_camera_resolution(warp_rc, cam_id) raw = jax.lax.dynamic_slice_in_dim( depth_data, depth_adr, width * height, axis=depth_data.ndim - 1 @@ -114,3 +114,37 @@ def get_depth( depth = jnp.clip(raw / depth_scale, 0.0, 1.0) return depth.reshape(raw.shape[:-1] + (height, width, 1)) + + +def get_segmentation( + rc: 'RenderContextPytree', + cam_id: int, + seg_data: jax.Array, +) -> jax.Array: + """Extract raw geom IDs for a camera. + + Args: + rc: RenderContextPytree. + cam_id: Camera index to extract. + seg_data: Packed segmentation output, shape (..., total_pixels) as integers. + + Returns: + Integer segmentation array with shape (..., H, W). + Any leading batch axes in `seg_data` are preserved. + + Raises: + RuntimeError: If Warp is not installed. + ValueError: If segmentation is not enabled for the selected camera. + """ + warp_rc = _get_warp_render_context(rc) + seg_adr = int(warp_rc.seg_adr.numpy()[cam_id]) + if seg_adr < 0: + raise ValueError( + f'Camera {cam_id} was not configured with segmentation rendering.' + ) + + width, height = _get_camera_resolution(warp_rc, cam_id) + packed = jax.lax.dynamic_slice_in_dim( + seg_data, seg_adr, width * height, axis=seg_data.ndim - 1 + ) + return packed.reshape(packed.shape[:-1] + (height, width)) diff --git a/mjx/mujoco/mjx/_src/render_util_test.py b/mjx/mujoco/mjx/_src/render_util_test.py index f97c1e0a..87065dcc 100644 --- a/mjx/mujoco/mjx/_src/render_util_test.py +++ b/mjx/mujoco/mjx/_src/render_util_test.py @@ -28,14 +28,23 @@ from mujoco.mjx.warp.render_context import RenderContextPytree _FORCE_TEST = os.environ.get('MJX_WARP_FORCE_TEST', '0') == '1' -def _fake_render_context(ncam, width, height): +def _fake_render_context(ncam, width, height, render_seg=True): """Fake RenderContext for testing.""" rc = mock.MagicMock() rgb_adr = np.arange(ncam, dtype=np.int32) * width * height depth_adr = np.arange(ncam, dtype=np.int32) * width * height + if isinstance(render_seg, bool): + render_seg = [render_seg] * ncam + seg_adr = np.full(ncam, -1, dtype=np.int32) + seg_offset = 0 + for i, enabled in enumerate(render_seg): + if enabled: + seg_adr[i] = seg_offset + seg_offset += width * height cam_res = np.tile([width, height], (ncam, 1)).astype(np.int32) rc.rgb_adr.numpy.return_value = rgb_adr rc.depth_adr.numpy.return_value = depth_adr + rc.seg_adr.numpy.return_value = seg_adr rc.cam_res.numpy.return_value = cam_res return rc @@ -154,6 +163,81 @@ class RenderUtilTest(absltest.TestCase): self.assertEqual(depth.shape, (nworld, height, width, 1)) + def test_get_segmentation(self): + width, height = 4, 4 + warp_rc = _fake_render_context(1, width, height) + rc = mock.MagicMock(spec=RenderContextPytree, key=0) + seg_data = jnp.arange(width * height, dtype=jnp.int32) + + with mock.patch.dict( + 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', + {(0, None): warp_rc}, + ): + segmentation = jax.jit( + render_util.get_segmentation, static_argnums=(0, 1) + )(rc, 0, seg_data) + + self.assertEqual(segmentation.shape, (height, width)) + np.testing.assert_array_equal( + np.asarray(segmentation), + np.arange(width * height, dtype=np.int32).reshape(height, width), + ) + + def test_get_segmentation_preserves_leading_dims(self): + width, height = 4, 4 + warp_rc = _fake_render_context(1, width, height) + rc = mock.MagicMock(spec=RenderContextPytree, key=0) + + with mock.patch.dict( + 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', + {(0, None): warp_rc}, + ): + for leading_shape in ((1,), (3,), (2, 3)): + with self.subTest(leading_shape=leading_shape): + seg_data = jnp.arange( + np.prod(leading_shape) * width * height, dtype=jnp.int32 + ).reshape(leading_shape + (width * height,)) + segmentation = jax.jit( + render_util.get_segmentation, static_argnums=(0, 1) + )(rc, 0, seg_data) + + self.assertEqual(segmentation.shape, leading_shape + (height, width)) + + def test_get_segmentation_vmap(self): + nworld, width, height = 3, 4, 4 + warp_rc = _fake_render_context(1, width, height) + rc = mock.MagicMock(spec=RenderContextPytree, key=0) + seg_data = jnp.arange(nworld * width * height, dtype=jnp.int32).reshape( + nworld, width * height + ) + + with mock.patch.dict( + 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', + {(0, None): warp_rc}, + ): + segmentation = jax.jit( + jax.vmap(render_util.get_segmentation, in_axes=(None, None, 0)), + static_argnums=(0, 1), + )(rc, 0, seg_data) + + self.assertEqual(segmentation.shape, (nworld, height, width)) + + def test_get_segmentation_raises_for_disabled_camera(self): + width, height = 4, 4 + warp_rc = _fake_render_context(2, width, height, render_seg=[True, False]) + rc = mock.MagicMock(spec=RenderContextPytree, key=0) + seg_data = jnp.arange(width * height, dtype=jnp.int32) + + with mock.patch.dict( + 'mujoco.mjx.warp.render_context._MJX_RENDER_CONTEXT_BUFFERS', + {(0, None): warp_rc}, + ): + with self.assertRaisesWithLiteralMatch( + ValueError, + 'Camera 1 was not configured with segmentation rendering.', + ): + render_util.get_segmentation(rc, 1, seg_data) + if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/warp/io.py b/mjx/mujoco/mjx/warp/io.py index b32d7122..d3179990 100644 --- a/mjx/mujoco/mjx/warp/io.py +++ b/mjx/mujoco/mjx/warp/io.py @@ -14,11 +14,12 @@ # ============================================================================== """I/O functions for MJX Warp.""" -import mujoco -from mujoco.mjx.warp import render_context -import mujoco.mjx.third_party.mujoco_warp as mjw import warp as wp +import mujoco +import mujoco.mjx.third_party.mujoco_warp as mjw +from mujoco.mjx.warp import render_context + _MJX_RENDER_CONTEXT_COUNTER = 0 @@ -27,8 +28,11 @@ def _create_context(mjm, nworld, device, **kwargs): ctx = mjw.create_render_context(mjm=mjm, nworld=nworld, **kwargs) ctx.rgb_data_shape = ctx.rgb_data.shape ctx.depth_data_shape = ctx.depth_data.shape + ctx.seg_data_shape = ctx.seg_data.shape + ctx.seg_data_buffer = ctx.seg_data ctx.rgb_data = None ctx.depth_data = None + ctx.seg_data = None return ctx diff --git a/mjx/mujoco/mjx/warp/render.py b/mjx/mujoco/mjx/warp/render.py index c97003f0..1361aa11 100644 --- a/mjx/mujoco/mjx/warp/render.py +++ b/mjx/mujoco/mjx/warp/render.py @@ -14,17 +14,19 @@ # ============================================================================== """DO NOT EDIT. This file is auto-generated.""" + import dataclasses import functools + import jax +import warp as wp + from mujoco.mjx._src import types +import mujoco.mjx.third_party.mujoco_warp as mjwarp +from mujoco.mjx.third_party.mujoco_warp._src import types as mjwp_types from mujoco.mjx.warp import ffi from mujoco.mjx.warp.render_context import _MJX_RENDER_CONTEXT_BUFFERS from mujoco.mjx.warp.render_context import RenderContextPytree -import mujoco.mjx.third_party.mujoco_warp as mjwarp -from mujoco.mjx.third_party.mujoco_warp._src import types as mjwp_types -import warp as wp - _m = mjwarp.Model( **{f.name: None for f in dataclasses.fields(mjwarp.Model) if f.init} @@ -122,36 +124,129 @@ def _render_shim( render_context = _MJX_RENDER_CONTEXT_BUFFERS[(rc_id, wp.get_device().ordinal)] render_context.rgb_data = rgb render_context.depth_data = depth + render_context.seg_data = render_context.seg_data_buffer mjwarp.render(_m, _d, render_context) -def _render_jax_impl(m: types.Model, d: types.Data, ctx: RenderContextPytree): +@ffi.format_args_for_warp +def _render_with_segmentation_shim( + # Model + nworld: int, + cam_fovy: wp.array2d[float], + cam_intrinsic: wp.array2d[wp.vec4], + cam_projection: wp.array[int], + cam_sensorsize: wp.array[wp.vec2], + flex_edge: wp.array[wp.vec2i], + flex_radius: wp.array[float], + flex_vertadr: wp.array[int], + geom_dataid: wp.array2d[int], + geom_matid: wp.array2d[int], + geom_rgba: wp.array2d[wp.vec4], + geom_size: wp.array2d[wp.vec3], + geom_type: wp.array[int], + light_active: wp.array2d[bool], + light_castshadow: wp.array2d[bool], + light_type: wp.array2d[int], + mat_rgba: wp.array2d[wp.vec4], + mat_texid: wp.array3d[int], + mat_texrepeat: wp.array2d[wp.vec2], + mesh_faceadr: wp.array[int], + nlight: int, + # Data + cam_xmat: wp.array2d[wp.mat33], + cam_xpos: wp.array2d[wp.vec3], + flexvert_xpos: wp.array2d[wp.vec3], + geom_xmat: wp.array2d[wp.mat33], + geom_xpos: wp.array2d[wp.vec3], + light_xdir: wp.array2d[wp.vec3], + light_xpos: wp.array2d[wp.vec3], + # Registry + rc_id: int, + rgb: wp.array2d[wp.uint32], + depth: wp.array2d[wp.float32], + seg: wp.array2d[int], +): + _m.stat = _s + _m.opt = _o + _m.callback = _cb + _d.efc = _e + _d.contact = _c + _m.cam_fovy = cam_fovy + _m.cam_intrinsic = cam_intrinsic + _m.cam_projection = cam_projection + _m.cam_sensorsize = cam_sensorsize + _m.flex_edge = flex_edge + _m.flex_radius = flex_radius + _m.flex_vertadr = flex_vertadr + _m.geom_dataid = geom_dataid + _m.geom_matid = geom_matid + _m.geom_rgba = geom_rgba + _m.geom_size = geom_size + _m.geom_type = geom_type + _m.light_active = light_active + _m.light_castshadow = light_castshadow + _m.light_type = light_type + _m.mat_rgba = mat_rgba + _m.mat_texid = mat_texid + _m.mat_texrepeat = mat_texrepeat + _m.mesh_faceadr = mesh_faceadr + _m.nlight = nlight + _d.cam_xmat = cam_xmat + _d.cam_xpos = cam_xpos + _d.flexvert_xpos = flexvert_xpos + _d.geom_xmat = geom_xmat + _d.geom_xpos = geom_xpos + _d.light_xdir = light_xdir + _d.light_xpos = light_xpos + _d.nworld = nworld + render_context = _MJX_RENDER_CONTEXT_BUFFERS[(rc_id, wp.get_device().ordinal)] + render_context.rgb_data = rgb + render_context.depth_data = depth + render_context.seg_data = seg + mjwarp.render(_m, _d, render_context) + + +_RENDER_STAGE_IN_ARGNAMES = set([ + 'cam_fovy', + 'cam_intrinsic', + 'cam_xmat', + 'cam_xpos', + 'geom_matid', + 'geom_rgba', + 'geom_size', + 'geom_xmat', + 'geom_xpos', + 'light_castshadow', + 'light_type', + 'mat_rgba', + 'mat_texid', +]) + + +def _render_jax_impl( + m: types.Model, + d: types.Data, + ctx: RenderContextPytree, + with_segmentation: bool = False, +): render_ctx = _MJX_RENDER_CONTEXT_BUFFERS[(ctx.key, None)] output_dims = { 'rgb': render_ctx.rgb_data_shape, 'depth': render_ctx.depth_data_shape, } + render_shim = _render_shim + num_outputs = 2 + if with_segmentation: + output_dims['seg'] = render_ctx.seg_data_shape + render_shim = _render_with_segmentation_shim + num_outputs = 3 jf = ffi.jax_callable_variadic_tuple( - _render_shim, - num_outputs=2, + render_shim, + num_outputs=num_outputs, output_dims=output_dims, vmap_method=None, in_out_argnames=set([]), - stage_in_argnames=set([ - 'cam_fovy', - 'cam_intrinsic', - 'cam_xmat', - 'cam_xpos', - 'geom_matid', - 'geom_rgba', - 'geom_size', - 'geom_xmat', - 'geom_xpos', - 'light_castshadow', - 'light_type', - 'mat_rgba', - 'mat_texid', - ]), + stage_in_argnames=_RENDER_STAGE_IN_ARGNAMES, stage_out_argnames=set([]), graph_mode=m.opt._impl.graph_mode, has_side_effect=False, @@ -208,3 +303,26 @@ def render_vmap( ): out = render(m, d, ctx) return out, [True, True] + + +@jax.custom_batching.custom_vmap +@functools.partial(ffi.marshal_jax_warp_callable, tree_map_output=True) +def render_with_segmentation( + m: types.Model, + d: types.Data, + ctx: RenderContextPytree, +): + return _render_jax_impl(m, d, ctx, with_segmentation=True) + + +@render_with_segmentation.def_vmap +@functools.partial(ffi.marshal_custom_vmap, tree_map_output=True) +def render_with_segmentation_vmap( + unused_axis_size, + is_batched, + m: types.Model, + d: types.Data, + ctx: RenderContextPytree, +): + out = render_with_segmentation(m, d, ctx) + return out, [True, True, True] diff --git a/mjx/mujoco/mjx/warp/render_test.py b/mjx/mujoco/mjx/warp/render_test.py index d4864d9c..fc089617 100644 --- a/mjx/mujoco/mjx/warp/render_test.py +++ b/mjx/mujoco/mjx/warp/render_test.py @@ -19,22 +19,22 @@ from absl.testing import absltest from absl.testing import parameterized import jax from jax import numpy as jp +import numpy as np + import mujoco from mujoco import mjx from mujoco.mjx._src import bvh from mujoco.mjx._src import forward from mujoco.mjx._src import io from mujoco.mjx._src import render -import mujoco.mjx.warp as mjxw from mujoco.mjx.warp import test_util as tu from mujoco.mjx.warp import warp as wp # pylint: disable=g-importing-member -import numpy as np - +import mujoco.mjx.warp as mjxw _FORCE_TEST = os.environ.get('MJX_WARP_FORCE_TEST', '0') == '1' -def _get_model_data_rc(xml, batch_size): +def _get_model_data_rc(xml, batch_size, render_seg=False): m = tu.load_test_file(xml) d = mujoco.MjData(m) mujoco.mj_forward(m, d) @@ -63,6 +63,7 @@ def _get_model_data_rc(xml, batch_size): use_shadows=True, render_rgb=True, render_depth=True, + render_seg=render_seg, enabled_geom_groups=[0, 1, 2], ) return mx, dx_batch, rc @@ -148,6 +149,88 @@ class RenderTest(parameterized.TestCase): self.assertGreater(np.count_nonzero(depth), 0) self.assertNotEqual(np.unique(depth).shape[0], 1) + @parameterized.product( + xml=('humanoid/humanoid.xml',), + batch_size=(1, 16), + ) + def test_render_with_segmentation(self, xml: str, batch_size: int): + """Tests MJX render pipeline with packed segmentation output.""" + self._maybe_skip() + mx, dx_batch, rc = _get_model_data_rc(xml, batch_size, render_seg=True) + + dx_batch = jax.jit(mjx.refit_bvh)(mx, dx_batch, rc.pytree()) + out_batch = jax.jit(mjx.render_with_segmentation)(mx, dx_batch, rc.pytree()) + + rgb = np.asarray(out_batch[0]) + depth = np.asarray(out_batch[1]) + seg = np.asarray(out_batch[2]) + + self.assertGreater(np.count_nonzero(rgb), 0) + self.assertGreater(np.count_nonzero(depth), 0) + self.assertTrue(np.any(seg != -1)) + self.assertGreater(np.unique(seg).shape[0], 1) + + unpacked_seg = jax.vmap(mjx.get_segmentation, in_axes=(None, None, 0))( + rc.pytree(), 0, out_batch[2] + ) + unpacked_seg = np.asarray(unpacked_seg) + width, height = rc._default.cam_res.numpy()[ + 0 + ] # pylint: disable=protected-access + seg_adr = int( + rc._default.seg_adr.numpy()[0] # pylint: disable=protected-access + ) + expected_seg = seg[:, seg_adr : seg_adr + width * height].reshape( + batch_size, height, width + ) + np.testing.assert_array_equal(unpacked_seg, expected_seg) + + @parameterized.product( + xml=('humanoid/humanoid.xml',), + batch_size=(4, 16), + ) + def test_render_with_segmentation_nested_vmap( + self, xml: str, batch_size: int + ): + """Tests MJX render_with_segmentation with nested vmap.""" + self._maybe_skip() + mx, dx_batch, rc = _get_model_data_rc(xml, batch_size, render_seg=True) + + def inner(mx, dx, rc): + dx = jax.vmap(bvh.refit_bvh, in_axes=(None, 0, None))(mx, dx, rc) + out = jax.vmap(render.render_with_segmentation, in_axes=(None, 0, None))( + mx, dx, rc + ) + return out + + dx_batch = jax.vmap(bvh.refit_bvh, in_axes=(None, 0, None))( + mx, dx_batch, rc.pytree() + ) + ref = jax.vmap(render.render_with_segmentation, in_axes=(None, 0, None))( + mx, dx_batch, rc.pytree() + ) + ref_rgb = np.asarray(ref[0]) + ref_depth = np.asarray(ref[1]) + ref_seg = np.asarray(ref[2]) + + def _reshape_batched(x): + if x.shape[0] == batch_size: + return x.reshape(2, batch_size // 2, *x.shape[1:]) + return x + + dx_2d = jax.tree.map(_reshape_batched, dx_batch) + + out_batch = jax.vmap(inner, in_axes=(None, 0, None))(mx, dx_2d, rc.pytree()) + out_batch = jax.tree.map(lambda x: x.reshape(-1, *x.shape[2:]), out_batch) + rgb = np.asarray(out_batch[0]) + depth = np.asarray(out_batch[1]) + seg = np.asarray(out_batch[2]) + + np.testing.assert_array_equal(rgb, ref_rgb) + np.testing.assert_array_equal(depth, ref_depth) + np.testing.assert_array_equal(seg, ref_seg) + self.assertTrue(np.any(seg != -1)) + class RenderContextGarbageCollectionTest(absltest.TestCase): """Tests that RenderContext cleans up buffers on deletion.""" From 506aa79ad89b7681fa9336ccc994e21cdede75fe Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 16 Apr 2026 09:01:35 -0700 Subject: [PATCH 079/251] Add Human-700 to model gallery PiperOrigin-RevId: 900755246 Change-Id: I655c21c21f9b858c907e593d50c26698d90bcfbb --- doc/models.rst | 2 ++ 1 file changed, 2 insertions(+) diff --git a/doc/models.rst b/doc/models.rst index 9ba7e806..1ccd43b5 100644 --- a/doc/models.rst +++ b/doc/models.rst @@ -151,3 +151,5 @@ Biomechanical - Preview * - `Fruitfly `_ - .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/flybody/flybody.png + * - `MS-Human-700 `_ + - .. image:: https://raw.githubusercontent.com/google-deepmind/mujoco_menagerie/main/ms_human_700/ms_human_700.png From f91ce9a62744c781589cee6fcb9e4bc176827b5b Mon Sep 17 00:00:00 2001 From: Matija Kecman Date: Fri, 17 Apr 2026 00:10:42 -0700 Subject: [PATCH 080/251] Remove window dependency from PipGui PiperOrigin-RevId: 901115560 Change-Id: I85183a91fd07682f17f43fcfad593dececdde28c --- src/experimental/platform/ux/picture_gui.cc | 12 +++++------- src/experimental/platform/ux/picture_gui.h | 3 +-- src/experimental/studio/app.cc | 3 ++- 3 files changed, 8 insertions(+), 10 deletions(-) diff --git a/src/experimental/platform/ux/picture_gui.cc b/src/experimental/platform/ux/picture_gui.cc index 7281c3d7..fdf1fa3b 100644 --- a/src/experimental/platform/ux/picture_gui.cc +++ b/src/experimental/platform/ux/picture_gui.cc @@ -20,16 +20,14 @@ #include #include #include "experimental/platform/hal/renderer.h" -#include "experimental/platform/hal/window.h" #include "experimental/platform/ux/imgui_widgets.h" namespace mujoco::platform { // Returns false if the user requests that this picture-in-picture widget be // removed from the GUI. -static bool PipGuiImpl(const mjModel* model, mjData* data, - platform::Window* window, platform::Renderer* renderer, - PipState* pip) { +static bool PipGuiImpl(const mjModel* model, mjData* data, float aspect_ratio, + platform::Renderer* renderer, PipState* pip) { bool result = true; auto get_camera_name = [model](int i) -> const char* { @@ -41,7 +39,7 @@ static bool PipGuiImpl(const mjModel* model, mjData* data, }; const int width = ImGui::GetContentRegionAvail().x; - const int height = width / window->GetAspectRatio(); + const int height = aspect_ratio != 0.f ? width / aspect_ratio : width; std::vector output(width * height * 3); const int combo_width = (width - 30) / 2; @@ -97,7 +95,7 @@ static bool PipGuiImpl(const mjModel* model, mjData* data, return result; } -void PipGui(const mjModel* model, mjData* data, platform::Window* window, +void PipGui(const mjModel* model, mjData* data, float aspect_ratio, platform::Renderer* renderer, std::vector* pips) { if (pips->empty()) { pips->emplace_back(); @@ -106,7 +104,7 @@ void PipGui(const mjModel* model, mjData* data, platform::Window* window, std::vector to_delete; for (int i = 0; i < pips->size(); ++i) { PipState& pip = pips->at(i); - if (PipGuiImpl(model, data, window, renderer, &pip) == false) { + if (PipGuiImpl(model, data, aspect_ratio, renderer, &pip) == false) { to_delete.push_back(i); }; ImGui::Separator(); diff --git a/src/experimental/platform/ux/picture_gui.h b/src/experimental/platform/ux/picture_gui.h index ef80d1f4..ab4f9146 100644 --- a/src/experimental/platform/ux/picture_gui.h +++ b/src/experimental/platform/ux/picture_gui.h @@ -19,7 +19,6 @@ #include #include "experimental/platform/hal/renderer.h" -#include "experimental/platform/hal/window.h" namespace mujoco::platform { @@ -32,7 +31,7 @@ struct PipState { }; // Renders the GUI for a set of picture-in-picture widgets. -void PipGui(const mjModel* model, mjData* data, platform::Window* window, +void PipGui(const mjModel* model, mjData* data, float aspect_ratio, platform::Renderer* renderer, std::vector* pips); } // namespace mujoco::platform diff --git a/src/experimental/studio/app.cc b/src/experimental/studio/app.cc index 9937e8ab..e7ad6c89 100644 --- a/src/experimental/studio/app.cc +++ b/src/experimental/studio/app.cc @@ -902,7 +902,8 @@ void App::BuildGui() { if (tmp_.picture_in_picture) { if (ImGui::Begin("Picture-in-Picture", &tmp_.picture_in_picture)) { - PipGui(model(), data(), window_.get(), renderer_.get(), &tmp_.pips); + platform::PipGui(model(), data(), window_->GetAspectRatio(), + renderer_.get(), &tmp_.pips); } ImGui::End(); } From c23b5e8420f68d875a2fb0cc1391319dd92bd186 Mon Sep 17 00:00:00 2001 From: Matija Kecman Date: Fri, 17 Apr 2026 01:28:40 -0700 Subject: [PATCH 081/251] Fix WASM build issues, improve test coverage and improve cmake files * Ensure compiler.usethread=0 is set before compilation in single-threaded WASM builds * Explicitly enable exceptions for the single-threaded WASM module * Test the single-threaded module in addition to the multi-threaded module in internal and external CI * Consolidate wasm/tests/CMakeLists.txt into wasm/CMakeLists.txt, fixing the single-threaded benchmark test build which was missing shared linker flags PiperOrigin-RevId: 901149728 Change-Id: If4946e29e5116610782d218906cf3f5940c1cdd0 --- .github/workflows/build_steps.sh | 9 ++-- CMakeLists.txt | 3 -- src/user/user_model.cc | 14 ++++++ wasm/CMakeLists.txt | 76 ++++++++++++++++++++++++-------- wasm/tests/CMakeLists.txt | 63 -------------------------- 5 files changed, 78 insertions(+), 87 deletions(-) delete mode 100644 wasm/tests/CMakeLists.txt diff --git a/.github/workflows/build_steps.sh b/.github/workflows/build_steps.sh index 4c55e5d4..c173333f 100755 --- a/.github/workflows/build_steps.sh +++ b/.github/workflows/build_steps.sh @@ -212,25 +212,28 @@ build_test_wasm() { echo "Building and testing WASM bindings..." source emsdk/emsdk_env.sh export PATH="$(pwd)/node_modules/.bin:$PATH" - - echo "Building Multi-Threaded version..." + echo "Build MuJoCo with Emscripten (Multi-Threaded)..." emcmake cmake -B build_wasm_mt \ -DCMAKE_INTERPROCEDURAL_OPTIMIZATION:BOOL=OFF \ -DMUJOCO_WASM_THREADS=ON \ $WASM_CMAKE_ARGS cmake --build build_wasm_mt --parallel $(nproc) + echo "Run bindings tests for Multi-Threaded version..." + npm run test --prefix ./wasm + echo "Moving Multi-Thread version under mt subfolder..." mkdir -p wasm/dist/mt mv wasm/dist/mujoco.* wasm/dist/mt/ - echo "Building Single-Threaded version..." + echo "Build MuJoCo with Emscripten (Single-Threaded)..." emcmake cmake -B build_wasm_st \ -DCMAKE_INTERPROCEDURAL_OPTIMIZATION:BOOL=OFF \ -DMUJOCO_WASM_THREADS=OFF \ $WASM_CMAKE_ARGS cmake --build build_wasm_st --parallel $(nproc) + echo "Run bindings tests for Single-Threaded version..." npm run test --prefix ./wasm } diff --git a/CMakeLists.txt b/CMakeLists.txt index 2c349167..36a64004 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -132,9 +132,6 @@ endif() if(EMSCRIPTEN) add_subdirectory(wasm) - if(MUJOCO_BUILD_TESTS_WASM) - add_subdirectory(wasm/tests) - endif() endif() diff --git a/src/user/user_model.cc b/src/user/user_model.cc index f3206d13..64048736 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -4967,6 +4967,20 @@ void mjCModel::ResolveKeyframes(const mjModel* m) { } void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { +#if defined(__EMSCRIPTEN__) && !defined(MUJOCO_WASM_THREADS) + // The MuJoCo compiler defaults to usethread=1, which causes it to try to + // create pthreads for compilation. In the single-threaded WASM build, this + // crashes because there is no threading support, so we disable threading on + // the internal compiler struct (not the spec) to avoid permanently mutating + // the spec (which would cause usethread="false" to appear in a saved XML). + struct ScopedDisableThreading { + mjtByte& ref; + mjtByte saved; + explicit ScopedDisableThreading(mjtByte& r) : ref(r), saved(r) { ref = 0; } + ~ScopedDisableThreading() { ref = saved; } + } disable_usethread(compiler.usethread); +#endif + // check if nan test works double test = mjNAN; if (mjuu_defined(test)) { diff --git a/wasm/CMakeLists.txt b/wasm/CMakeLists.txt index 80ed4feb..158e6b11 100644 --- a/wasm/CMakeLists.txt +++ b/wasm/CMakeLists.txt @@ -22,16 +22,14 @@ include_directories(${PROJECT_SOURCE_DIR}) link_directories(${CMAKE_BINARY_DIR}/lib) -file(GLOB MUJOCO_WASM_FILES - "codegen/generated/*.cc" - "unpack.cc" -) +# Set Emscripten compile flags. +# -fexceptions is required for val::throw_() (used by ThrowMujocoErrorToJS) to +# actually throw a JS exception. Without it, Emscripten compiles throw as a +# no-op. In the MT build, -pthread implicitly enables exception support, but +# the ST build needs it set explicitly here. +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fexceptions") -if(NOT MUJOCO_WASM_FILES) - message(FATAL_ERROR "No source files found in codegen/generated/") -endif() - -# Set Emscripten linker flags +# Set Emscripten linker flags shared by all WASM targets. set(EMCC_LINKER_FLAGS "--bind" "-s ASSERTIONS=1" @@ -42,9 +40,9 @@ set(EMCC_LINKER_FLAGS "-s EXPORTED_RUNTIME_METHODS=['ccall','cwrap','FS','MEMFS']" "-s EXPORT_NAME=loadMujoco" "-s DISABLE_EXCEPTION_CATCHING=0" + "-fexceptions" "-gsource-map" "-g" - "--emit-tsd mujoco.d.ts" ) if(MUJOCO_WASM_THREADS) list(APPEND EMCC_LINKER_FLAGS @@ -53,7 +51,28 @@ if(MUJOCO_WASM_THREADS) ) add_definitions(-DMUJOCO_WASM_THREADS) endif() -string (REPLACE ";" " " EMCC_LINKER_FLAGS_STR "${EMCC_LINKER_FLAGS}") + +# Common link libraries for all WASM targets. The mujoco library is linked as a +# whole archive to avoid losing plugin registration such as obj_decoder and +# stl_decoder. +set(MUJOCO_WASM_LINK_LIBRARIES + -Wl,--whole-archive mujoco -Wl,--no-whole-archive + ccd lodepng tinyxml2 qhullstatic_r +) + +# --- Main WASM bindings target --- + +file(GLOB MUJOCO_WASM_FILES + "codegen/generated/*.cc" + "unpack.cc" +) + +if(NOT MUJOCO_WASM_FILES) + message(FATAL_ERROR "No source files found in codegen/generated/") +endif() + +set(MUJOCO_WASM_LINKER_FLAGS ${EMCC_LINKER_FLAGS} "--emit-tsd mujoco.d.ts") +string(REPLACE ";" " " MUJOCO_WASM_LINKER_FLAGS_STR "${MUJOCO_WASM_LINKER_FLAGS}") add_executable(mujoco_wasm ${MUJOCO_WASM_FILES}) @@ -61,15 +80,36 @@ add_executable(mujoco_wasm ${MUJOCO_WASM_FILES}) # `mujoco` library target, but emit artifacts named `mujoco.*` by setting the # output name. Also apply the emscripten linker flags to the wasm target. set_target_properties(mujoco_wasm PROPERTIES - LINK_FLAGS "${EMCC_LINKER_FLAGS_STR}" + LINK_FLAGS "${MUJOCO_WASM_LINKER_FLAGS_STR}" OUTPUT_NAME "mujoco" ) -# Link the mujoco library as a whole archive to avoid losing plugin -# registration such as obj_decoder and stl_decoder. -target_link_libraries(mujoco_wasm PRIVATE - -Wl,--whole-archive mujoco -Wl,--no-whole-archive - ccd lodepng tinyxml2 qhullstatic_r -) +target_link_libraries(mujoco_wasm PRIVATE ${MUJOCO_WASM_LINK_LIBRARIES}) install(TARGETS mujoco_wasm DESTINATION ${DIVISIBLE_INSTALL_BIN_DIR}) + +# --- Benchmark target --- + +if(MUJOCO_BUILD_TESTS_WASM) + file(GLOB MUJOCO_WASM_BENCHMARK_FILES + "tests/benchmark_test.cc" + "unpack.cc" + ) + + if(NOT MUJOCO_WASM_BENCHMARK_FILES) + message(FATAL_ERROR "No benchmark source files found") + endif() + + set(BENCHMARK_LINKER_FLAGS ${EMCC_LINKER_FLAGS} "--emit-tsd mujoco_wasm_benchmark.d.ts") + string(REPLACE ";" " " BENCHMARK_LINKER_FLAGS_STR "${BENCHMARK_LINKER_FLAGS}") + + add_executable(mujoco_wasm_benchmark ${MUJOCO_WASM_BENCHMARK_FILES}) + + set_target_properties(mujoco_wasm_benchmark PROPERTIES + LINK_FLAGS "${BENCHMARK_LINKER_FLAGS_STR}" + ) + + target_link_libraries(mujoco_wasm_benchmark PRIVATE ${MUJOCO_WASM_LINK_LIBRARIES}) + + install(TARGETS mujoco_wasm_benchmark DESTINATION ${DIVISIBLE_INSTALL_BIN_DIR}) +endif() diff --git a/wasm/tests/CMakeLists.txt b/wasm/tests/CMakeLists.txt deleted file mode 100644 index f6d74289..00000000 --- a/wasm/tests/CMakeLists.txt +++ /dev/null @@ -1,63 +0,0 @@ -# Copyright 2025 DeepMind Technologies Limited -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_SOURCE_DIR}/wasm/dist") - -set(CMAKE_INSTALL_PREFIX ${PROJECT_SOURCE_DIR}/wasm) - -include_directories(${PROJECT_SOURCE_DIR}/include) -include_directories(${PROJECT_SOURCE_DIR}/src) -include_directories(${PROJECT_SOURCE_DIR}) - -link_directories(${CMAKE_BINARY_DIR}/lib) - -file(GLOB MUJOCO_WASM_FILES - "benchmark_test.cc" - "../unpack.cc" -) - -if(NOT MUJOCO_WASM_FILES) - message(FATAL_ERROR "No source files found") -endif() - -# Set Emscripten linker flags -set(EMCC_LINKER_FLAGS - "--bind" - "-pthread" - "-s PTHREAD_POOL_SIZE=navigator.hardwareConcurrency" - "-s ASSERTIONS=1" - "-s ALLOW_MEMORY_GROWTH=1" - "-s EXPORT_ES6=1" - "-s MODULARIZE=1" - "-s FORCE_FILESYSTEM=1" - "-s EXPORTED_RUNTIME_METHODS=['ccall','cwrap','FS','MEMFS']" - "-s EXPORT_NAME=loadMujoco" - "-gsource-map" - "-g" - "--emit-tsd mujoco_wasm_benchmark.d.ts" -) -string (REPLACE ";" " " EMCC_LINKER_FLAGS_STR "${EMCC_LINKER_FLAGS}") - -add_executable(mujoco_wasm_benchmark ${MUJOCO_WASM_FILES}) - -set_target_properties(mujoco_wasm_benchmark PROPERTIES LINK_FLAGS "${EMCC_LINKER_FLAGS_STR}") - -# Link the mujoco library as a whole archive to avoid losing plugin -# registration such as obj_decoder and stl_decoder. -target_link_libraries(mujoco_wasm_benchmark PRIVATE - -Wl,--whole-archive mujoco -Wl,--no-whole-archive - ccd lodepng tinyxml2 qhullstatic_r -) - -install(TARGETS mujoco_wasm_benchmark DESTINATION ${DIVISIBLE_INSTALL_BIN_DIR}) From 8415dff3075e5556f4f669e084f0b2847b9c0f1f Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Fri, 17 Apr 2026 01:58:27 -0700 Subject: [PATCH 082/251] Merge Material into Renderable. Allows the Renderable to update its own MaterialInstances whenever Material textures or parameters are modified. Each Renderable now also knows how it is intended to be used (i.e. scene objects or ux) which allows it to correctly pick the underlying filament::Material to use. PiperOrigin-RevId: 901161990 Change-Id: Ic280731d39c272c50d6ad100cec7116e683cbc79 --- .../filament/assets/unlit_segmentation.mat | 4 +- .../filament/filament/imgui_bridge.cc | 15 +- .../filament/filament/material.cc | 109 ++++--------- src/experimental/filament/filament/material.h | 104 +++++-------- .../filament/filament/renderable.cc | 144 +++++++++++++++++- .../filament/filament/renderable.h | 36 ++++- .../filament/filament/scene_geom_util.cc | 97 ++---------- .../filament/filament/scene_view.cc | 10 +- 8 files changed, 252 insertions(+), 267 deletions(-) diff --git a/src/experimental/filament/assets/unlit_segmentation.mat b/src/experimental/filament/assets/unlit_segmentation.mat index 359bb319..b57aca03 100644 --- a/src/experimental/filament/assets/unlit_segmentation.mat +++ b/src/experimental/filament/assets/unlit_segmentation.mat @@ -17,13 +17,13 @@ material { shadingModel : unlit, culling: none, parameters : [ - { type : float4, name : BaseColorFactor } + { type : float4, name : SegmentationColor } ] } fragment { void material(inout MaterialInputs material) { prepareMaterial(material); - material.baseColor = materialParams.BaseColorFactor; + material.baseColor = materialParams.SegmentationColor; } } diff --git a/src/experimental/filament/filament/imgui_bridge.cc b/src/experimental/filament/filament/imgui_bridge.cc index 7337a5b0..f5553ea8 100644 --- a/src/experimental/filament/filament/imgui_bridge.cc +++ b/src/experimental/filament/filament/imgui_bridge.cc @@ -23,7 +23,6 @@ #include #include #include -#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/renderable.h" @@ -242,11 +241,10 @@ void ImguiBridge::Update() { renderable->UpdateMesh(0, mesh, index_offset, command.ElemCount); } - Material::Textures textures; + MaterialTextures textures; textures.color = textures_[command.GetTexID()].get(); - renderable->GetMaterial().UpdateTextures(textures); - Material::Params properties; + MaterialParams properties; properties.scissor[0] = command.ClipRect.x; properties.scissor[1] = height - command.ClipRect.w; properties.scissor[2] = command.ClipRect.z - command.ClipRect.x; @@ -260,7 +258,7 @@ void ImguiBridge::Update() { properties.scissor[2] = width; properties.scissor[3] = height; } - renderable->GetMaterial().UpdateParams(properties); + renderable->UpdateMaterial(properties, textures); index_offset += command.ElemCount; ++renderable_index; @@ -271,15 +269,10 @@ void ImguiBridge::Update() { void ImguiBridge::PrepareRenderables(int count) { while (renderables_.size() < count) { auto& r = renderables_.emplace_back( - std::make_unique(object_mgr_)); + std::make_unique(Renderable::Usage::Ux, object_mgr_)); r->SetCastShadows(false); r->SetReceiveShadows(false); r->SetBlendOrder(static_cast(renderables_.size())); - - Material& material = r->GetMaterial(); - DrawMode mode = DrawMode::Color; - material.SetMaterial(mode, object_mgr_->GetMaterial(ObjectManager::kUnlitUi)); - r->SetMaterialInstance(material.GetMaterialInstance(mode)); scene_view_->AddToUxScene(r.get()); } while (renderables_.size() > count) { diff --git a/src/experimental/filament/filament/material.cc b/src/experimental/filament/filament/material.cc index 2e274f93..6522bd31 100644 --- a/src/experimental/filament/filament/material.cc +++ b/src/experimental/filament/filament/material.cc @@ -20,104 +20,54 @@ #include #include #include -#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/filament/object_manager.h" namespace mujoco { -Material::Material(ObjectManager* object_mgr) - : object_mgr_(object_mgr) { -} - -Material::~Material() noexcept { - for (int i = 0; i < kNumDrawModes; ++i) { - if (instances_[i]) { - GetEngine()->destroy(instances_[i]); - } - } -} - -void Material::SetMaterial(DrawMode mode, filament::Material* material) { - const int index = static_cast(mode); - if (instances_[index]) { - const filament::Material* current_material = - instances_[index]->getMaterial(); - if (current_material == material) { - return; - } - - GetEngine()->destroy(instances_[index]); - instances_[index] = nullptr; - } - if (material) { - instances_[index] = material->createInstance(); - UpdateMaterialInstances(); - } -} - -filament::MaterialInstance* Material::GetMaterialInstance(DrawMode mode) { - return instances_[static_cast(mode)]; -} - -void Material::UpdateParams(const Params& params) { - params_ = params; - UpdateMaterialInstances(); -} - -void Material::UpdateTextures(const Textures& textures) { - textures_ = textures; - UpdateMaterialInstances(); -} - -void Material::UpdateMaterialInstances() { - filament::MaterialInstance* instance = - instances_[static_cast(DrawMode::Color)]; - if (instance == nullptr) { - return; - } - - if (params_.scissor[2] != 0 && params_.scissor[3] != 0) { - instance->setScissor(params_.scissor[0], params_.scissor[1], - params_.scissor[2], params_.scissor[3]); +void UpdateMaterialInstance(filament::MaterialInstance* instance, + const MaterialParams& params, + const MaterialTextures& textures, + ObjectManager* object_mgr) { + if (params.scissor[2] != 0 && params.scissor[3] != 0) { + instance->setScissor(params.scissor[0], params.scissor[1], + params.scissor[2], params.scissor[3]); } const filament::Material* material = instance->getMaterial(); if (material->hasParameter("BaseColorFactor")) { instance->setParameter("BaseColorFactor", filament::RgbaType::sRGB, - params_.color); + params.color); + } + if (material->hasParameter("SegmentationColor")) { + instance->setParameter("SegmentationColor", filament::RgbaType::LINEAR, + params.segmentation_color); } if (material->hasParameter("EmissiveFactor")) { - instance->setParameter("EmissiveFactor", params_.emissive); + instance->setParameter("EmissiveFactor", params.emissive); } if (material->hasParameter("SpecularFactor")) { - instance->setParameter("SpecularFactor", params_.specular); + instance->setParameter("SpecularFactor", params.specular); } if (material->hasParameter("GlossinessFactor")) { - instance->setParameter("GlossinessFactor", params_.glossiness); + instance->setParameter("GlossinessFactor", params.glossiness); } if (material->hasParameter("MetallicFactor")) { instance->setParameter("MetallicFactor", - params_.metallic >= 0 ? params_.metallic : 1.0f); + params.metallic >= 0 ? params.metallic : 1.0f); } if (material->hasParameter("RoughnessFactor")) { instance->setParameter("RoughnessFactor", - params_.roughness >= 0 ? params_.roughness : 1.0f); + params.roughness >= 0 ? params.roughness : 1.0f); } if (material->hasParameter("UvScale")) { - instance->setParameter("UvScale", params_.uv_scale); + instance->setParameter("UvScale", params.uv_scale); } if (material->hasParameter("UvOffset")) { - instance->setParameter("UvOffset", params_.uv_offset); + instance->setParameter("UvOffset", params.uv_offset); } if (material->hasParameter("Reflectance")) { - instance->setParameter("Reflectance", params_.reflectance); - } - - const int segmentation_index = static_cast(DrawMode::Segmentation); - if (instances_[segmentation_index]) { - instances_[segmentation_index]->setParameter("BaseColorFactor", - params_.segmentation_color); + instance->setParameter("Reflectance", params.reflectance); } // All textures use the same default sampler. @@ -135,19 +85,20 @@ void Material::UpdateMaterialInstances() { if (texture != nullptr) { instance->setParameter(name, texture->GetFilamentTexture(), sampler); } else { - instance->setParameter(name, object_mgr_->GetFallbackTexture(role), sampler); + instance->setParameter(name, object_mgr->GetFallbackTexture(role), + sampler); } } }; - TrySetTexture("BaseColor", textures_.color, mjTEXROLE_RGB); - TrySetTexture("Normal", textures_.normal, mjTEXROLE_NORMAL); - TrySetTexture("Metallic", textures_.metallic, mjTEXROLE_METALLIC); - TrySetTexture("Roughness", textures_.roughness, mjTEXROLE_ROUGHNESS); - TrySetTexture("Occlusion", textures_.occlusion, mjTEXROLE_OCCLUSION); - TrySetTexture("ORM", textures_.orm, mjTEXROLE_ORM); - TrySetTexture("Emissive", textures_.emissive, mjTEXROLE_EMISSIVE); - TrySetTexture("Reflection", textures_.reflection, mjTEXROLE_USER); + TrySetTexture("BaseColor", textures.color, mjTEXROLE_RGB); + TrySetTexture("Normal", textures.normal, mjTEXROLE_NORMAL); + TrySetTexture("Metallic", textures.metallic, mjTEXROLE_METALLIC); + TrySetTexture("Roughness", textures.roughness, mjTEXROLE_ROUGHNESS); + TrySetTexture("Occlusion", textures.occlusion, mjTEXROLE_OCCLUSION); + TrySetTexture("ORM", textures.orm, mjTEXROLE_ORM); + TrySetTexture("Emissive", textures.emissive, mjTEXROLE_EMISSIVE); + TrySetTexture("Reflection", textures.reflection, mjTEXROLE_USER); } } // namespace mujoco diff --git a/src/experimental/filament/filament/material.h b/src/experimental/filament/filament/material.h index cf5854f6..339b5b5b 100644 --- a/src/experimental/filament/filament/material.h +++ b/src/experimental/filament/filament/material.h @@ -20,82 +20,48 @@ #include #include #include -#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/filament/object_manager.h" namespace mujoco { -class Material { - public: - // The textures that can be assigned to the drawable's material. - struct Textures { - const Texture* color = nullptr; - const Texture* normal = nullptr; - const Texture* metallic = nullptr; - const Texture* roughness = nullptr; - const Texture* occlusion = nullptr; - const Texture* orm = nullptr; - const Texture* emissive = nullptr; - const Texture* reflection = nullptr; - }; - - // The parameters that can be applied to the drawable's material. - struct Params { - filament::math::float4 color = {1, 1, 1, 1}; - filament::math::float4 segmentation_color = {1, 1, 1, 1}; - filament::math::float2 tex_repeat = {1, 1}; - filament::math::float3 uv_scale = {1, 1, 1}; - filament::math::float3 uv_offset = {0, 0, 0}; - filament::math::float4 scissor = {0, 0, 0, 0}; - float specular = -1.0f; - float glossiness = -1.0f; - float metallic = -1.0f; - float roughness = -1.0f; - float emissive = -1.0f; - float reflectance = 0.0f; - bool tex_uniform = false; - bool reflective = false; - }; - - explicit Material(ObjectManager* object_mgr); - ~Material() noexcept; - - Material(const Material&) = delete; - Material& operator=(const Material&) = delete; - - // Assigns a material to the draw mode. - void SetMaterial(DrawMode mode, filament::Material* material); - - // Updates the parameters for the material. - void UpdateParams(const Params& params); - - // Updates the textures for the material. - void UpdateTextures(const Textures& textures); - - // Returns the current material parameters. - const Params& GetParams() const { return params_; } - - // Returns the current material textures. - const Textures& GetTextures() const { return textures_; } - - // Returns the material instance assigned to the draw mode. - filament::MaterialInstance* GetMaterialInstance(DrawMode mode); - - // Returns the filament Engine managing the material. - filament::Engine* GetEngine() const { return object_mgr_->GetEngine(); } - - private: - // Updates the material instances based on the currently set parameters and - // textures. - void UpdateMaterialInstances(); - - ObjectManager* object_mgr_; - filament::MaterialInstance* instances_[kNumDrawModes] = {nullptr}; - Params params_; - Textures textures_; +// The textures that can be assigned to the drawable's material. +struct MaterialTextures { + const Texture* color = nullptr; + const Texture* normal = nullptr; + const Texture* metallic = nullptr; + const Texture* roughness = nullptr; + const Texture* occlusion = nullptr; + const Texture* orm = nullptr; + const Texture* emissive = nullptr; + const Texture* reflection = nullptr; }; +// The parameters that can be applied to the drawable's material. +struct MaterialParams { + filament::math::float4 color = {1, 1, 1, 1}; + filament::math::float4 segmentation_color = {1, 1, 1, 1}; + filament::math::float2 tex_repeat = {1, 1}; + filament::math::float3 uv_scale = {1, 1, 1}; + filament::math::float3 uv_offset = {0, 0, 0}; + filament::math::float4 scissor = {0, 0, 0, 0}; + float specular = -1.0f; + float glossiness = -1.0f; + float metallic = -1.0f; + float roughness = -1.0f; + float emissive = -1.0f; + float reflectance = 0.0f; + bool tex_uniform = false; + bool reflective = false; +}; + +// Updates the material instances based on the currently set parameters and +// textures. +void UpdateMaterialInstance(filament::MaterialInstance* instance, + const MaterialParams& params, + const MaterialTextures& textures, + ObjectManager* object_mgr); + } // namespace mujoco #endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MATERIAL_H_ diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index cf4857fa..6a3da110 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -14,26 +14,36 @@ #include "experimental/filament/filament/renderable.h" +#include #include #include #include +#include #include #include #include #include +#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/object_manager.h" namespace mujoco { -Renderable::Renderable(ObjectManager* object_mgr) : material_(object_mgr) {} +Renderable::Renderable(Usage usage, ObjectManager* object_mgr) + : usage_(usage), object_mgr_(object_mgr) {} Renderable::~Renderable() noexcept { while (!entities_.empty()) { RemoveLastEntity(); } + for (int i = 0; i < kNumDrawModes; ++i) { + if (instances_[i] != nullptr) { + GetEngine()->destroy(instances_[i]); + instances_[i] = nullptr; + } + } } void Renderable::RemoveLastEntity() { @@ -103,8 +113,8 @@ void Renderable::AppendEntity(const MeshInfo& mesh_info) { } else { builder.culling(false); } - if (material_instance_) { - builder.material(0, material_instance_); + if (instances_[static_cast(draw_mode_)] != nullptr) { + builder.material(0, instances_[static_cast(draw_mode_)]); } builder.castShadows(cast_shadows_); builder.receiveShadows(receive_shadows_); @@ -191,15 +201,67 @@ void Renderable::RemoveFromScene(filament::Scene* scene) { assigned_scene_ = nullptr; } -void Renderable::SetMaterialInstance(filament::MaterialInstance* instance) { - if (instance != material_instance_) { +void Renderable::UpdateMaterial(const MaterialParams& params, + const MaterialTextures& textures) { + params_ = params; + textures_ = textures; + + AssignMaterial(DrawMode::Color, GetColorMaterialType()); + if (usage_ == Usage::SceneObject) { + AssignMaterial(DrawMode::Depth, ObjectManager::kUnlitDepth); + AssignMaterial(DrawMode::Segmentation, ObjectManager::kUnlitSegmentation); + } + + for (int i = 0; i < kNumDrawModes; ++i) { + if (instances_[i]) { + UpdateMaterialInstance(instances_[i], params_, textures_, object_mgr_); + } + } + SetDrawMode(draw_mode_); +} + +void Renderable::AssignMaterial(DrawMode mode, + ObjectManager::MaterialType material_type) { + const int index = static_cast(mode); + + filament::Material* material = object_mgr_->GetMaterial(material_type); + if (instances_[index]) { + if (instances_[index]->getMaterial() == material) { + // The correct material is already assigned, do nothing. + return; + } else { + GetEngine()->destroy(instances_[index]); + instances_[index] = nullptr; + } + } + if (material) { + instances_[index] = material->createInstance(); + } +} + +const MaterialParams& Renderable::GetMaterialParams() const { + return params_; +} + +const MaterialTextures& Renderable::GetMaterialTextures() const { + return textures_; +} + +void Renderable::SetDrawMode(DrawMode mode) { + // Only SceneObjects support non-color draw modes. + if (usage_ != Usage::SceneObject) { + mode = DrawMode::Color; + } + + filament::MaterialInstance* instance = instances_[static_cast(mode)]; + if (instance) { filament::RenderableManager& rm = GetEngine()->getRenderableManager(); for (utils::Entity& entity : entities_) { filament::RenderableManager::Instance ri = rm.getInstance(entity); rm.setMaterialInstanceAt(ri, 0, instance); } - material_instance_ = instance; } + draw_mode_ = mode; } std::uint8_t Renderable::SetLayerMask(std::uint8_t mask) { @@ -284,8 +346,74 @@ void Renderable::SetWireframe(bool wireframe) { } } -Material& Renderable::GetMaterial() { return material_; } -filament::Engine* Renderable::GetEngine() { return material_.GetEngine(); } +ObjectManager::MaterialType Renderable::GetColorMaterialType() const { + if (usage_ == Usage::DecorLines) { + return ObjectManager::kUnlitLine; + } else if (usage_ == Usage::Decor) { + return ObjectManager::kUnlitSegmentation; + } else if (usage_ == Usage::Ux) { + return ObjectManager::kUnlitUi; + } else if (textures_.orm) { + return ObjectManager::kPbrPacked; + } else if (textures_.metallic) { + return ObjectManager::kPbr; + } else if (textures_.roughness) { + return ObjectManager::kPbr; + } else if (params_.metallic >= 0) { + return ObjectManager::kPbr; + } else if (params_.roughness >= 0) { + return ObjectManager::kPbr; + } + + // Check to see if we're dealing with a mesh with texture coordinates. + // `data_id` is the id of the mesh in model (i.e. the geom has mesh + // geometry) and `mesh_texcoordadr` stores the address of the mesh uvs if + // it has them. + bool has_texcoords = false; + if (!meshes_.empty()) { + const auto attribs = meshes_[0].mesh->GetVertexAttributes(); + auto it = std::find(attribs.begin(), attribs.end(), + filament::VertexAttribute::UV0); + has_texcoords = (it != attribs.end()); + } + + if (textures_.color == nullptr) { + if (params_.color.a < 1.0f) { + return ObjectManager::kPhongColorFade; + } else if (params_.reflective) { + return ObjectManager::kPhongColorReflect; + } else { + return ObjectManager::kPhongColor; + } + } else if (textures_.color->GetFilamentTexture()->getTarget() == + filament::Texture::Sampler::SAMPLER_CUBEMAP) { + if (params_.color.a < 1.0f) { + return ObjectManager::kPhongCubeFade; + } else if (params_.reflective) { + return ObjectManager::kPhongCubeReflect; + } else { + return ObjectManager::kPhongCube; + } + } else if (has_texcoords) { + if (params_.color.a < 1.0f) { + return ObjectManager::kPhong2dUvFade; + } else if (params_.reflective) { + return ObjectManager::kPhong2dUvReflect; + } else { + return ObjectManager::kPhong2dUv; + } + } else { + if (params_.color.a < 1.0f) { + return ObjectManager::kPhong2dFade; + } else if (params_.reflective) { + return ObjectManager::kPhong2dReflect; + } else { + return ObjectManager::kPhong2d; + } + } +} + +filament::Engine* Renderable::GetEngine() { return object_mgr_->GetEngine(); } } // namespace mujoco diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 5dc3b87f..dbe03498 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -21,6 +21,7 @@ #include #include #include +#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/object_manager.h" @@ -37,11 +38,19 @@ namespace mujoco { // assigns the same material instance to all of them. class Renderable { public: + // How the material is to be used for rendering. + enum class Usage { + SceneObject, + Decor, + DecorLines, + Ux, + }; + // Default filament values for priority and layer mask. static constexpr std::uint8_t kDefaultPriority = 4; static constexpr std::uint8_t kDefaultLayerMask = 0x01; - explicit Renderable(ObjectManager* object_mgr); + Renderable(Usage usage, ObjectManager* object_mgr); ~Renderable() noexcept; Renderable(const Renderable&) = delete; @@ -95,10 +104,17 @@ class Renderable { void RemoveFromScene(filament::Scene* scene); // Sets the material instance for all managed entities. - void SetMaterialInstance(filament::MaterialInstance* material_instance); + void SetDrawMode(DrawMode mode); - // Returns the material for the renderables. - Material& GetMaterial(); + // Updates the parameters for the material. + void UpdateMaterial(const MaterialParams& params, + const MaterialTextures& textures); + + // Returns the current material parameters. + const MaterialParams& GetMaterialParams() const; + + // Returns the current material textures. + const MaterialTextures& GetMaterialTextures() const; // Returns the filament Engine managing the renderables. filament::Engine* GetEngine(); @@ -129,9 +145,17 @@ class Renderable { // Removes the last filament::Entity from the renderable. void RemoveLastEntity(); - Material material_; + void AssignMaterial(DrawMode mode, ObjectManager::MaterialType material_type); + + ObjectManager::MaterialType GetColorMaterialType() const; + + Usage usage_; + ObjectManager* object_mgr_; + filament::MaterialInstance* instances_[kNumDrawModes] = {nullptr}; + MaterialParams params_; + MaterialTextures textures_; + DrawMode draw_mode_ = DrawMode::Color; filament::Scene* assigned_scene_ = nullptr; - filament::MaterialInstance* material_instance_ = nullptr; std::vector entities_; std::vector meshes_; std::uint8_t priority_ = kDefaultPriority; diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index 3dba24d9..e60bacf4 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -31,7 +31,6 @@ #include #include #include -#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" @@ -323,12 +322,10 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, ObjectManager* object_mgr, const float headpos[3]) { const mjModel* model = model_objs->GetModel(); - Material& material = renderable.GetMaterial(); const bool use_segid_color = scene->flags[mjRND_IDCOLOR]; const bool enable_reflection = scene->flags[mjRND_REFLECTION]; - - Material::Params params; + MaterialParams params; params.color = ReadFloat4(geom.rgba); if (geom.type == mjGEOM_PLANE) { if (IsBehind(headpos, geom.pos, geom.mat)) { @@ -347,7 +344,7 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, renderable.SetReceiveShadows(false); } - Material::Textures textures; + MaterialTextures textures; if (geom.matid >= 0) { textures.color = model_objs->GetTexture(geom.matid, mjTEXROLE_RGB); textures.normal = model_objs->GetTexture(geom.matid, mjTEXROLE_NORMAL); @@ -358,79 +355,6 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, model_objs->GetTexture(geom.matid, mjTEXROLE_ROUGHNESS); textures.occlusion = model_objs->GetTexture(geom.matid, mjTEXROLE_OCCLUSION); - material.UpdateTextures(textures); - } - - ObjectManager::MaterialType material_type = ObjectManager::kNumMaterials; - if (geom.type == mjGEOM_LINE || geom.type == mjGEOM_LINEBOX) { - material_type = ObjectManager::kUnlitLine; - } else if (geom.category == mjCAT_DECOR) { - material_type = ObjectManager::kUnlitSegmentation; - } else { - bool material_assigned = false; - if (geom.matid >= 0) { - material_assigned = true; - if (textures.orm) { - material_type = ObjectManager::kPbrPacked; - } else if (textures.metallic) { - material_type = ObjectManager::kPbr; - } else if (textures.roughness) { - material_type = ObjectManager::kPbr; - } else if (model->mat_metallic[geom.matid] >= 0) { - material_type = ObjectManager::kPbr; - } else if (model->mat_roughness[geom.matid] >= 0) { - material_type = ObjectManager::kPbr; - } else { - material_assigned = false; - } - } - - if (!material_assigned) { - // Check to see if we're dealing with a mesh with texture coordinates. - // `data_id` is the id of the mesh in model (i.e. the geom has mesh - // geometry) and `mesh_texcoordadr` stores the address of the mesh uvs if - // it has them. - bool has_texcoords = false; - if ((geom.type == mjGEOM_MESH || geom.type == mjGEOM_SDF) && - geom.dataid >= 0 && model->mesh_texcoordadr[geom.dataid / 2] >= 0) { - has_texcoords = true; - } - - if (textures.color == nullptr) { - if (params.color.a < 1.0f) { - material_type = ObjectManager::kPhongColorFade; - } else if (params.reflective) { - material_type = ObjectManager::kPhongColorReflect; - } else { - material_type = ObjectManager::kPhongColor; - } - } else if (textures.color->GetFilamentTexture()->getTarget() == - filament::Texture::Sampler::SAMPLER_CUBEMAP) { - if (params.color.a < 1.0f) { - material_type = ObjectManager::kPhongCubeFade; - } else if (params.reflective) { - material_type = ObjectManager::kPhongCubeReflect; - } else { - material_type = ObjectManager::kPhongCube; - } - } else if (has_texcoords) { - if (params.color.a < 1.0f) { - material_type = ObjectManager::kPhong2dUvFade; - } else if (params.reflective) { - material_type = ObjectManager::kPhong2dUvReflect; - } else { - material_type = ObjectManager::kPhong2dUv; - } - } else { - if (params.color.a < 1.0f) { - material_type = ObjectManager::kPhong2dFade; - } else if (params.reflective) { - material_type = ObjectManager::kPhong2dReflect; - } else { - material_type = ObjectManager::kPhong2d; - } - } - } } params.reflectance = geom.reflectance; @@ -532,20 +456,21 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, params.emissive *= model_objs->GetEmissiveMultiplier(); params.specular *= model_objs->GetSpecularMultiplier(); params.glossiness *= model_objs->GetShininessMultiplier(); - material.UpdateParams(params); - material.SetMaterial(DrawMode::Color, object_mgr->GetMaterial(material_type)); - material.SetMaterial(DrawMode::Depth, - object_mgr->GetMaterial(ObjectManager::kUnlitDepth)); - material.SetMaterial( - DrawMode::Segmentation, - object_mgr->GetMaterial(ObjectManager::kUnlitSegmentation)); + renderable.UpdateMaterial(params, textures); } std::unique_ptr CreateGeomRenderable( const mjvGeom& geom, const mjvScene* scene, ObjectManager* object_mgr, ModelObjects* model_objs, const float headpos[3]) { - auto renderable = std::make_unique(object_mgr); + Renderable::Usage usage = Renderable::Usage::SceneObject; + if (geom.type == mjGEOM_LINE || geom.type == mjGEOM_LINEBOX) { + usage = Renderable::Usage::DecorLines; + } else if (geom.category == mjCAT_DECOR) { + usage = Renderable::Usage::Decor; + } + + auto renderable = std::make_unique(usage, object_mgr); // The order of these calls is important. e.g. We need to create the filament // renderable entities before we can set their transform. diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 1f4c4009..d6832f6f 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -208,7 +208,7 @@ void SceneView::RemoveFromScene(Light* light) { void SceneView::AddToScene(Renderable* renderable) { if (renderables_.insert(renderable).second) { renderable->AddToScene(scene_); - if (renderable->GetMaterial().GetParams().reflective) { + if (renderable->GetMaterialParams().reflective) { AddReflectiveRenderable(renderable); } } @@ -261,8 +261,7 @@ void SceneView::Render(filament::Renderer* renderer, SetupCamera(request.camera, viewport, camera_); for (auto& iter : renderables_) { - Material& material = iter->GetMaterial(); - iter->SetMaterialInstance(material.GetMaterialInstance(request.draw_mode)); + iter->SetDrawMode(request.draw_mode); } filament::View* view = views_[static_cast(request.draw_mode)]; @@ -336,10 +335,9 @@ void SceneView::AddReflectiveRenderable(Renderable* renderable) { auto& target = reflect_targets_[index]; target->Prepare(viewport.width, viewport.height); - Material& material = renderable->GetMaterial(); - Material::Textures textures = material.GetTextures(); + MaterialTextures textures = renderable->GetMaterialTextures(); textures.reflection = target->GetColorTexture(); - material.UpdateTextures(textures); + renderable->UpdateMaterial(renderable->GetMaterialParams(), textures); } void SceneView::SetColorGradingOptions(const ColorGradingOptions& opts) { From 6c7ed667812bee182642891c12aca9b55b512372 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 17 Apr 2026 04:10:54 -0700 Subject: [PATCH 083/251] Implement multi-cell finite element method for interpolated flexes. This change introduces a `flex_cellcount` field to `mjModel` to specify the number of cells in each dimension for interpolated flexes. The stiffness computation, passive force calculation, and Jacobian derivatives are updated to operate on a per-cell basis, significantly improving performance by localizing computations to the nodes within each cell. PiperOrigin-RevId: 901216393 Change-Id: Ic23132e609de11e71bb7fef8d1f139daad2ec264 --- doc/XMLreference.rst | 21 +- doc/XMLschema.rst | 9 + doc/changelog.rst | 9 + doc/includes/references.h | 3 + doc/modeling.rst | 10 + include/mujoco/mjmodel.h | 1 + include/mujoco/mjspec.h | 2 + include/mujoco/mjxmacro.h | 1 + model/flex/bunny_multicell.xml | 42 ++ python/mujoco/introspect/structs.py | 21 + src/engine/engine_core_constraint.c | 307 +++++---- src/engine/engine_core_smooth.c | 27 +- src/engine/engine_core_util.c | 28 + src/engine/engine_core_util.h | 3 + src/engine/engine_derivative.c | 207 +++--- src/engine/engine_passive.c | 175 ++--- src/engine/engine_passive.h | 4 +- src/engine/engine_util_misc.c | 109 ++- src/engine/engine_util_misc.h | 14 +- src/engine/engine_vis_interact.c | 26 +- src/engine/engine_vis_visualize.c | 33 +- src/user/user_flexcomp.cc | 99 ++- src/user/user_flexcomp.h | 1 + src/user/user_init.c | 3 + src/user/user_mesh.cc | 69 +- src/user/user_model.cc | 38 +- src/user/user_objects.h | 4 +- src/xml/xml_native_reader.cc | 17 +- src/xml/xml_native_writer.cc | 7 + test/engine/engine_core_constraint_test.cc | 129 +--- test/engine/engine_core_util_test.cc | 740 +++++++++++++++++++++ test/engine/engine_forward_test.cc | 4 +- test/engine/engine_support_test.cc | 515 -------------- test/engine/engine_util_misc_test.cc | 122 +++- unity/Runtime/Bindings/MjBindings.cs | 1 + wasm/codegen/generated/bindings.cc | 15 + 36 files changed, 1777 insertions(+), 1039 deletions(-) create mode 100644 model/flex/bunny_multicell.xml create mode 100644 test/engine/engine_core_util_test.cc diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index a8ebff10..9a8eba6c 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -3700,8 +3700,14 @@ saving the XML: .. _body-flexcomp-count: :at:`count`: :at-val:`int(3), "10 10 10"` - The number of automatically generated points in each dimension. This and the next attribute only apply to types grid, - box, cylinder, ellipsoid. + Specifies the number of automatically generated points in each dimension for types **grid**, **box**, **cylinder**, + and **ellipsoid**. + +.. _body-flexcomp-cellcount: + +:at:`cellcount`: :at-val:`int(3), "1 1 1"` + Specifies the number of cells in each dimension for the background interpolation grid when using **trilinear** or + **quadratic** dofs. .. _body-flexcomp-spacing: @@ -4242,6 +4248,17 @@ cases, the user will specify a :el:`flexcomp` which will then automatically cons An array of MuJoCo body names (separated by white space) to which each node belongs. The number of body names should equal the number of nodes (nnode). See the flexcomp :ref:`dof` attribute for more details. +.. _deformable-flex-cellcount: + +:at:`cellcount`: :at-val:`int(3), optional` + When using **trilinear** or **quadratic** dofs, this specifies the number of cells in each dimension for the + background interpolation grid. + +.. _deformable-flex-dof: + +:at:`dof`: :at-val:`[trilinear, quadratic], optional` + Interpolation order for the flex. + .. _flex-edge: :el-prefix:`flex/` |-| **edge** |?| diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 4dbc6c29..60d828be 100755 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -1417,6 +1417,9 @@ .. grid-item:: :ref:`count` + .. grid-item:: + :ref:`cellcount` + .. grid-item:: :ref:`spacing` @@ -1657,6 +1660,12 @@ .. grid-item:: :ref:`node` + .. grid-item:: + :ref:`cellcount` + + .. grid-item:: + :ref:`dof` + .. dropdown:: :ref:`contact` :octicon:`dot` diff --git a/doc/changelog.rst b/doc/changelog.rst index 880cd3a8..b073a3f2 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,6 +2,15 @@ Changelog ========= +Upcoming version (not yet released) +----------------------------------- + +General +^^^^^^^ + +- Added :ref:`multi-cell support` for trilinear and quadratic flexes. Note that the implicit + integrator uses a dense solver for the flex degrees of freedom, which can be slow for multi-cell flexes. + Version 3.7.0 (April 14, 2026) ------------------------------ diff --git a/doc/includes/references.h b/doc/includes/references.h index 7f23a52d..614a7516 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1315,6 +1315,7 @@ struct mjModel_ { int* flex_matid; // material id for rendering (nflex x 1) int* flex_group; // group for visibility (nflex x 1) int* flex_interp; // interpolation (0: vertex, 1: nodes) (nflex x 1) + int* flex_cellnum; // finite cell num per dimension (nflex x 3) int* flex_nodeadr; // first node address (nflex x 1) int* flex_nodenum; // number of nodes (nflex x 1) int* flex_vertadr; // first vertex address (nflex x 1) @@ -2256,6 +2257,8 @@ typedef struct mjsFlex_ { // flex specification double damping; // Rayleigh's damping double thickness; // thickness (2D only) int elastic2d; // 2D passive forces; 0: none, 1: bending, 2: stretching, 3: both + int cellcount[3]; // grid cell count for finite cell method + int order; // interpolation order (1: trilinear, 2: quadratic) // mesh properties mjStringVec* nodebody; // node body names diff --git a/doc/modeling.rst b/doc/modeling.rst index f91f698e..017e68fc 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -1442,6 +1442,16 @@ instead to specify shear and volumetric stiffnesses separately using the `Poisso `__ of the material. For more details, see the `Saint Venant-Kirchhoff `__ hyperelastic model. +**Parametrization types**. + +While the default behavior of :el:`flexcomp` produces a "full" flex where every node corresponds to a MuJoCo body, it +also supports specialized :ref:`parametrizations` for volumetric objects: **trilinear** and +**quadratic**. Instead of directly simulating all nodes, these options define a background grid of cells. The positions +of the interior vertices are computed by interpolating the positions of the cell corners. Trilinear flexes use 8-node +hexahedral cells with linear interpolation along each axis, while quadratic flexes use 27-node cells with quadratic +interpolation, allowing for curved deformation modes. These grid-based parametrizations require fewer degrees of freedom +than full flexes and can result in significantly faster simulation times, especially for large volumetric soft bodies. + **Creation and visualization**. .. code-block:: xml diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 8a45f67b..9b39cbb6 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -978,6 +978,7 @@ struct mjModel_ { int* flex_matid; // material id for rendering (nflex x 1) int* flex_group; // group for visibility (nflex x 1) int* flex_interp; // interpolation (0: vertex, 1: nodes) (nflex x 1) + int* flex_cellnum; // finite cell num per dimension (nflex x 3) int* flex_nodeadr; // first node address (nflex x 1) int* flex_nodenum; // number of nodes (nflex x 1) int* flex_vertadr; // first vertex address (nflex x 1) diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index f886da53..73d7c74f 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -454,6 +454,8 @@ typedef struct mjsFlex_ { // flex specification double damping; // Rayleigh's damping double thickness; // thickness (2D only) int elastic2d; // 2D passive forces; 0: none, 1: bending, 2: stretching, 3: both + int cellcount[3]; // grid cell count for finite cell method + int order; // interpolation order (1: trilinear, 2: quadratic) // mesh properties mjStringVec* nodebody; // node body names diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 970ba622..87a95a31 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -454,6 +454,7 @@ X ( int, flex_matid, nflex, 1 ) \ X ( int, flex_group, nflex, 1 ) \ X ( int, flex_interp, nflex, 1 ) \ + X ( int, flex_cellnum, nflex, 3 ) \ X ( int, flex_nodeadr, nflex, 1 ) \ X ( int, flex_nodenum, nflex, 1 ) \ X ( int, flex_vertadr, nflex, 1 ) \ diff --git a/model/flex/bunny_multicell.xml b/model/flex/bunny_multicell.xml new file mode 100644 index 00000000..a77f5376 --- /dev/null +++ b/model/flex/bunny_multicell.xml @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 4dded269..14a44f32 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -2668,6 +2668,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='interpolation (0: vertex, 1: nodes)', array_extent=('nflex',), ), + StructFieldDecl( + name='flex_cellnum', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='finite cell num per dimension', + array_extent=('nflex', 3), + ), StructFieldDecl( name='flex_nodeadr', type=PointerType( @@ -8237,6 +8245,19 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='2D passive forces; 0: none, 1: bending, 2: stretching, 3: both', # pylint: disable=line-too-long ), + StructFieldDecl( + name='cellcount', + type=ArrayType( + inner_type=ValueType(name='int'), + extents=(3,), + ), + doc='grid cell count for finite cell method', + ), + StructFieldDecl( + name='order', + type=ValueType(name='int'), + doc='interpolation order (1: trilinear, 2: quadratic)', + ), StructFieldDecl( name='nodebody', type=PointerType( diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 170d1f3c..41ca6a39 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -432,21 +432,31 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v, return 0; } + // compute parametric coordinates of the vertex in [0, 1]^3 mjtNum coord[3] = {0, 0, 0}; for (int i = 0; i < nw; i++) { - mju_addToScl3(coord, m->flex_vert0 + 3*v[i], vweight[i]); + mju_addToScl3(coord, m->flex_vert0 + 3*v[i], vweight[i]); } + + int order = m->flex_interp[f]; + int npc = (order+1)*(order+1)*(order+1); // number of nodes per cell + + // cell lookup: get local coords and node indices + mjtNum local[3]; + int nodeindices[27]; // max npc for quadratic: 3^3 = 27 + mju_cellLookup(coord, m->flex_cellnum+3*f, order, local, nodeindices); + + // evaluate basis functions for this cell's local nodes int nstart = m->flex_nodeadr[f]; - int nend = m->flex_nodeadr[f] + m->flex_nodenum[f]; int nb = 0; - for (int i = nstart; i < nend; i++) { - mjtNum w = mju_evalBasis(coord, i-nstart, m->flex_interp[f]); + for (int j = 0; j < npc; j++) { + mjtNum w = mju_evalBasis(local, j, order); if (w < 1e-5) { continue; } if (bweight) bweight[nb] = w; - body[nb++] = m->flex_nodebodyid[i]; + body[nb++] = m->flex_nodebodyid[nstart + nodeindices[j]]; } return nb; @@ -871,6 +881,11 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { break; } + int npc = (order+1)*(order+1)*(order+1); + int cx = m->flex_cellnum[3*f+0]; + int cy = m->flex_cellnum[3*f+1]; + int cz = m->flex_cellnum[3*f+2]; + // allocate stack for node positions and Jacobians mj_markStack(d); mjtNum* xpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); @@ -910,152 +925,164 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { } } - // loop over Gauss points - // get reference positions from m->flex_node0 (Cartesian positions at qpos0) + // reference positions for all nodes int nstart = m->flex_nodeadr[f]; mjtNum* refpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); for (int n = 0; n < nodenum; n++) { mju_copy3(refpos + 3*n, m->flex_node0 + 3*(n + nstart)); } - // B-bar: precompute center-point values for volumetric constraint (trilinear only) - if (order == 1) { - mjtNum center[3] = {0.5, 0.5, 0.5}; - mjtNum Fcur_c[9], Fref_c[9], Fref_inv_center[9], F_center[9]; - - // compute deformation gradient at center - mju_defGradient(Fcur_c, center, xpos, order); - mju_defGradient(Fref_c, center, refpos, order); - mat3_inverse(Fref_c, Fref_inv_center); - mju_mulMatMat3(F_center, Fcur_c, Fref_inv_center); - - // compute C and E at center - mjtNum C_c[9], E_c[9]; - mju_mulMatTMat3(C_c, F_center, F_center); - mju_scl(E_c, C_c, 0.5, 9); - E_c[0] -= 0.5; - E_c[4] -= 0.5; - E_c[8] -= 0.5; - - // J = det(F) at center - mjtNum I1_center = E_c[0] + E_c[4] + E_c[8]; - mjtNum J_center = mat3_det(F_center); - - // compute shape function gradients at center (8 nodes for trilinear) - mjtNum grad_center[8][3]; - shape_gradients(order, center, grad_center); - - // add I1 and J-1 constraints at center (reduced integration for volumetric) - mjtNum* dSdx = mjSTACKALLOC(d, 3*nodenum, mjtNum); - for (int inv = 0; inv < 2; inv++) { - if (inv == 0) { - // I1 = tr(E), dI1/dE = I - cpos[0] = I1_center; - } else { - // J - 1 = det(F) - 1, dJ/dF = cofactor(F) - cpos[0] = J_center - 1.0; - } - - volumetric_dSdx(inv, nodenum, grad_center, F_center, Fref_inv_center, dSdx); - strain_jacobian(nodenum, nv, dSdx, node_jac, strain_jac); - - if (issparse) { - mjtNum* sparse_jac = mjSTACKALLOC(d, combined_nnz, mjtNum); - for (int k = 0; k < combined_nnz; k++) { - sparse_jac[k] = strain_jac[combined_chain[k]]; - } - mj_addConstraint(m, d, sparse_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, - combined_nnz, combined_chain); - } else { - mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); - } - } - } - - // add I1 and J-1 constraints at center (reduced integration for volumetric) + // per-cell arrays + mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); + mjtNum* refpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); + mjtNum* dSdx_local = mjSTACKALLOC(d, 3*npc, mjtNum); mjtNum* dSdx = mjSTACKALLOC(d, 3*nodenum, mjtNum); - for (int g = 0; g < ngauss; g++) { - mjtNum* p = gauss[g]; + int gindices[125]; // max npc = 125 for quadratic - // F = Fcur * Fref_inv - mjtNum Fcur[9], Fref[9], Fref_inv[9], F[9]; - mju_defGradient(Fcur, p, xpos, order); - mju_defGradient(Fref, p, refpos, order); - mat3_inverse(Fref, Fref_inv); - mju_mulMatMat3(F, Fcur, Fref_inv); + // loop over cells + for (int ci = 0; ci < cx; ci++) { + for (int cj = 0; cj < cy; cj++) { + for (int ck = 0; ck < cz; ck++) { + // gather cell-local node positions + mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos, NULL, refpos, xpos_c, NULL, + refpos_c, gindices, NULL); - // compute Green-Lagrange strain E = 0.5*(C - I) - mjtNum C[9], E[9]; - mju_mulMatTMat3(C, F, F); - for (int j = 0; j < 9; j++) { - E[j] = 0.5 * C[j]; - } - E[0] -= 0.5; - E[4] -= 0.5; - E[8] -= 0.5; + // B-bar: center-point volumetric constraints (trilinear) + if (order == 1) { + mjtNum center[3] = {0.5, 0.5, 0.5}; + mjtNum Fcur_c[9], Fref_c[9], Fref_inv_c[9], F_c[9]; - // compute 3 invariants of E - mjtNum I1 = E[0] + E[4] + E[8]; - mjtNum trE2 = E[0]*E[0] + E[1]*E[3] + E[2]*E[6] + - E[3]*E[1] + E[4]*E[4] + E[5]*E[7] + - E[6]*E[2] + E[7]*E[5] + E[8]*E[8]; - mjtNum I2 = 0.5 * (I1*I1 - trE2); - mjtNum I3 = mat3_det(E); + mju_defGradient(Fcur_c, center, xpos_c, order); + mju_defGradient(Fref_c, center, refpos_c, order); + mat3_inverse(Fref_c, Fref_inv_c); + mju_mulMatMat3(F_c, Fcur_c, Fref_inv_c); - // compute shape function gradients at this Gauss point - mjtNum grad[27][3]; - shape_gradients(order, p, grad); + mjtNum C_c[9], E_c[9]; + mju_mulMatTMat3(C_c, F_c, F_c); + mju_scl(E_c, C_c, 0.5, 9); + E_c[0] -= 0.5; E_c[4] -= 0.5; E_c[8] -= 0.5; - // trilinear: 3 constraints per Gauss point (I1, I2, I3 skipped - only shear) - // quadratic: 6 constraints per Gauss point - for (int s = 0; s < 6; s++) { - // skip I1, I2, I3 for trilinear (I1, J-1 at center; I2 is small for small strain) - if (order == 1 && (s == 0 || s == 1 || s == 2)) { - continue; - } + mjtNum I1_c = E_c[0] + E_c[4] + E_c[8]; + mjtNum J_c = mat3_det(F_c); - mjtNum dSdE[9]; - mju_zero(dSdE, 9); + mjtNum grad_c[8][3]; + shape_gradients(order, center, grad_c); - if (s == 0) { - // I1 = tr(E), dI1/dE = I (only for quadratic) - cpos[0] = I1; - dSdE[0] = dSdE[4] = dSdE[8] = 1.0; - } else if (s == 1) { - // I2 = 0.5*(tr(E)^2 - tr(E^2)), dI2/dE = tr(E)*I - E - cpos[0] = I2; - dSdE[0] = I1 - E[0]; - dSdE[4] = I1 - E[4]; - dSdE[8] = I1 - E[8]; - dSdE[1] = -E[1]; dSdE[3] = -E[3]; - dSdE[2] = -E[2]; dSdE[6] = -E[6]; - dSdE[5] = -E[5]; dSdE[7] = -E[7]; - } else if (s == 2) { - // I3 = det(E), dI3/dE = cofactor(E) - cpos[0] = I3; - mat3_cofactor(E, dSdE); - } else { - // off-diagonal entries: s=3->E12, s=4->E13, s=5->E23 - int offdiag_idx[3] = {1, 2, 5}; - int ij = offdiag_idx[s - 3]; - cpos[0] = E[ij]; - dSdE[ij] = 1.0; - } + for (int inv = 0; inv < 2; inv++) { + cpos[0] = (inv == 0) ? I1_c : J_c - 1.0; - // compute dS/dx for all nodes - invariant_dSdx(nodenum, grad, F, Fref_inv, dSdE, dSdx); - strain_jacobian(nodenum, nv, dSdx, node_jac, strain_jac); + // compute local dSdx + volumetric_dSdx(inv, npc, grad_c, F_c, Fref_inv_c, dSdx_local); - // add constraint - if (issparse) { - mjtNum* sparse_jac = mjSTACKALLOC(d, combined_nnz, mjtNum); - for (int k = 0; k < combined_nnz; k++) { - sparse_jac[k] = strain_jac[combined_chain[k]]; + // scatter to global dSdx + mju_zero(dSdx, 3*nodenum); + for (int n = 0; n < npc; n++) { + mju_addTo3(dSdx + 3*gindices[n], dSdx_local + 3*n); + } + + strain_jacobian(nodenum, nv, dSdx, node_jac, strain_jac); + + if (issparse) { + mj_markStack(d); + mjtNum* sj = mjSTACKALLOC(d, combined_nnz, mjtNum); + for (int k = 0; k < combined_nnz; k++) { + sj[k] = strain_jac[combined_chain[k]]; + } + mj_addConstraint(m, d, sj, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, + combined_nnz, combined_chain); + mj_freeStack(d); + } else { + mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); + } + } + } + + // Gauss integration per cell + for (int g = 0; g < ngauss; g++) { + mjtNum* p = gauss[g]; + + // F = Fcur * Fref_inv + mjtNum Fcur[9], Fref[9], Fref_inv[9], F[9]; + mju_defGradient(Fcur, p, xpos_c, order); + mju_defGradient(Fref, p, refpos_c, order); + mat3_inverse(Fref, Fref_inv); + mju_mulMatMat3(F, Fcur, Fref_inv); + + // Green-Lagrange strain E = 0.5*(C - I) + mjtNum C[9], E[9]; + mju_mulMatTMat3(C, F, F); + for (int j = 0; j < 9; j++) { + E[j] = 0.5 * C[j]; + } + E[0] -= 0.5; E[4] -= 0.5; E[8] -= 0.5; + + // 3 invariants of E + mjtNum I1 = E[0] + E[4] + E[8]; + mjtNum trE2 = E[0]*E[0] + E[1]*E[3] + E[2]*E[6] + + E[3]*E[1] + E[4]*E[4] + E[5]*E[7] + + E[6]*E[2] + E[7]*E[5] + E[8]*E[8]; + mjtNum I2 = 0.5 * (I1*I1 - trE2); + mjtNum I3 = mat3_det(E); + + // shape function gradients at Gauss point + mjtNum grad[27][3]; + shape_gradients(order, p, grad); + + for (int s = 0; s < 6; s++) { + // skip I1,I2,I3 for trilinear (B-bar handles vol) + if (order == 1 && (s == 0 || s == 1 || s == 2)) { + continue; + } + + mjtNum dSdE[9]; + mju_zero(dSdE, 9); + + if (s == 0) { + cpos[0] = I1; + dSdE[0] = dSdE[4] = dSdE[8] = 1.0; + } else if (s == 1) { + cpos[0] = I2; + dSdE[0] = I1-E[0]; dSdE[4] = I1-E[4]; + dSdE[8] = I1-E[8]; + dSdE[1] = -E[1]; dSdE[3] = -E[3]; + dSdE[2] = -E[2]; dSdE[6] = -E[6]; + dSdE[5] = -E[5]; dSdE[7] = -E[7]; + } else if (s == 2) { + cpos[0] = I3; + mat3_cofactor(E, dSdE); + } else { + int offdiag_idx[3] = {1, 2, 5}; + int ij = offdiag_idx[s - 3]; + cpos[0] = E[ij]; + dSdE[ij] = 1.0; + } + + // compute local dS/dx for cell nodes + invariant_dSdx(npc, grad, F, Fref_inv, dSdE, + dSdx_local); + + // scatter to global dSdx + mju_zero(dSdx, 3*nodenum); + for (int n = 0; n < npc; n++) { + mju_addTo3(dSdx + 3*gindices[n], dSdx_local + 3*n); + } + + strain_jacobian(nodenum, nv, dSdx, node_jac, strain_jac); + + if (issparse) { + mj_markStack(d); + mjtNum* sj = mjSTACKALLOC(d, combined_nnz, mjtNum); + for (int k = 0; k < combined_nnz; k++) { + sj[k] = strain_jac[combined_chain[k]]; + } + mj_addConstraint(m, d, sj, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, + combined_nnz, combined_chain); + mj_freeStack(d); + } else { + mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); + } + } } - mj_addConstraint(m, d, sparse_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, - combined_nnz, combined_chain); - } else { - mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); } } } @@ -1899,10 +1926,13 @@ void mj_diagApprox(const mjModel* m, mjData* d) { int nstart = m->flex_nodeadr[flex_id]; int order = m->flex_interp[flex_id]; - // compute constraint count: trilinear (2 + 3*8 = 26), quadratic (6*27 = 162) + // compute constraint count per cell, then multiply by ncells int nquad = order + 1; int ngauss = nquad * nquad * nquad; - int nconstraint = (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss); + int ncells = m->flex_cellnum[3*flex_id+0] + * m->flex_cellnum[3*flex_id+1] + * m->flex_cellnum[3*flex_id+2]; + int nconstraint = ncells * ((order == 1) ? (2 + 3 * ngauss) : (6 * ngauss)); mjtNum avg_invweight = 0; for (int n = 0; n < nodenum; n++) { @@ -2510,7 +2540,10 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { } int nquad = order + 1; // 2 for order=1, 3 for order=2 int ngauss = nquad * nquad * nquad; // 8 or 27 - size = (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss); // 26 or 162 + int ncells = m->flex_cellnum[3*id[0]+0] + * m->flex_cellnum[3*id[0]+1] + * m->flex_cellnum[3*id[0]+2]; + size = ncells * ((order == 1) ? (2 + 3 * ngauss) : (6 * ngauss)); if (nnz) { // Count unique DOFs across all node bodies (matching instantiation) diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 2ae4d5b4..8f3412f1 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -580,9 +580,11 @@ void mj_flex(const mjModel* m, mjData* d) { } } - // trilinear interpolation + // trilinear/quadratic interpolation else { - mjtNum nodexpos[3*mjMAXFLEXNODES]; + int nodenum = nend - nstart; + mj_markStack(d); + mjtNum* nodexpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); if (m->flex_centered[f]) { for (int i=nstart; i < nend; i++) { mji_copy3(nodexpos + 3*(i-nstart), d->xpos + 3*m->flex_nodebodyid[i]); @@ -596,14 +598,26 @@ void mj_flex(const mjModel* m, mjData* d) { } int order = m->flex_interp[f]; - if (nend - nstart != (order + 1) * (order + 1) * (order + 1)) { + int cx = m->flex_cellnum[3*f+0]; + int cy = m->flex_cellnum[3*f+1]; + int cz = m->flex_cellnum[3*f+2]; + int nx_g = cx * order + 1; + int ny_g = cy * order + 1; + int nz_g = cz * order + 1; + if (nend - nstart != nx_g * ny_g * nz_g) { mjERROR("flex_interp_order mismatch"); } for (int i=vstart; i < vend; i++) { mju_zero3(d->flexvert_xpos+3*i); - mju_interpolate3D(d->flexvert_xpos+3*i, m->flex_vert0 + 3*i, nodexpos, order); + + // cell lookup: get local coords and node indices + mjtNum local[3]; + int nodeindices[27]; // max npc for quadratic: 3^3 = 27 + mju_cellLookup(m->flex_vert0 + 3*i, m->flex_cellnum+3*f, order, local, nodeindices); + mju_interpolate3D(d->flexvert_xpos+3*i, local, nodexpos, order, nodeindices); } + mj_freeStack(d); } } @@ -2617,7 +2631,10 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { if (order && nodenum) { int nquad = order + 1; int ngauss = nquad * nquad * nquad; - i += (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss); + int ncells = m->flex_cellnum[3*k+0] + * m->flex_cellnum[3*k+1] + * m->flex_cellnum[3*k+2]; + i += ncells * ((order == 1) ? (2 + 3 * ngauss) : (6 * ngauss)); } break; } diff --git a/src/engine/engine_core_util.c b/src/engine/engine_core_util.c index c49f3ee5..bf2b6994 100644 --- a/src/engine/engine_core_util.c +++ b/src/engine/engine_core_util.c @@ -987,6 +987,34 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], //-------------------------- miscellaneous utilities ----------------------------------------------- +// gather global node positions and velocities +void mju_flexGatherState(const mjModel* m, mjData* d, int f, mjtNum* xpos, mjtNum* vel) { + int nodenum = m->flex_nodenum[f]; + int nstart = m->flex_nodeadr[f]; + int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; + + // compute positions + if (m->flex_centered[f]) { + for (int i=0; i < nodenum; i++) { + mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]); + if (vel) { + mju_copy3(vel + 3*i, d->qvel + m->body_dofadr[bodyid[i]]); + } + } + } else { + mjtNum screw[6]; + for (int i=0; i < nodenum; i++) { + mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart)); + mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]); + if (vel) { + mj_objectVelocity(m, d, mjOBJ_BODY, bodyid[i], screw, 0); + mju_copy3(vel + 3*i, screw + 3); + } + } + } +} + + // extract 6D force:torque for one contact, in contact frame void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]) { mjContact* con; diff --git a/src/engine/engine_core_util.h b/src/engine/engine_core_util.h index 39633ff0..949aa257 100644 --- a/src/engine/engine_core_util.h +++ b/src/engine/engine_core_util.h @@ -129,6 +129,9 @@ MJAPI void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], //-------------------------- miscellaneous --------------------------------------------------------- +// gather global node positions and velocities +MJAPI void mju_flexGatherState(const mjModel* m, mjData* d, int f, mjtNum* xpos, mjtNum* vel); + // extract 6D force:torque for one contact, in contact frame MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]); diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 5f1058d0..1eb4df9d 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -915,127 +915,146 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, continue; } + int order = m->flex_interp[f]; + int npc = (order+1)*(order+1)*(order+1); + int cx = m->flex_cellnum[3*f+0]; + int cy = m->flex_cellnum[3*f+1]; + int cz = m->flex_cellnum[3*f+2]; + int nodenum = m->flex_nodenum[f]; int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; // standard stack allocation mj_markStack(d); mjtNum* xpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); - mjtNum* K_rot = mjSTACKALLOC(d, 9*nodenum*nodenum, mjtNum); - // sparse Jacobian allocations - int dim = 3 * nodenum; - int* rownnz = mjSTACKALLOC(d, dim, int); - int* rowadr = mjSTACKALLOC(d, dim, int); - mjtNum* J_val = mjSTACKALLOC(d, dim*nv, mjtNum); - int* J_colind = mjSTACKALLOC(d, dim*nv, int); + // per-cell arrays + int dim_c = 3 * npc; + mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); + mjtNum* K_rot_cell = mjSTACKALLOC(d, dim_c*dim_c, mjtNum); + + // sparse Jacobian for one cell + int* J_rownnz = mjSTACKALLOC(d, dim_c, int); + int* J_rowadr = mjSTACKALLOC(d, dim_c, int); + mjtNum* J_val = mjSTACKALLOC(d, dim_c*nv, mjtNum); + int* J_colind = mjSTACKALLOC(d, dim_c*nv, int); // temp allocations for chain int* chain_colind = mjSTACKALLOC(d, nv, int); mjtNum* blk_jac = mjSTACKALLOC(d, 3*nv, mjtNum); - // compute positions, rotation and Jacobian - mjtNum quat[4] = {1, 0, 0, 0}; - mj_flexInterpState(m, d, f, xpos, NULL, quat); + // gather raw node positions (unrotated) + mju_flexGatherState(m, d, f, xpos, NULL); - // compute generalized stiffness in global frame: K_rot = R * K * R^T - mjtNum R[9]; - mju_quat2Mat(R, quat); // R = R_global2local - mjtNum RT[9]; - mju_transpose(RT, R, 3, 3); // RT = R_local2global + // loop over cells + int cell_idx = 0; + for (int ci = 0; ci < cx; ci++) { + for (int cj = 0; cj < cy; cj++) { + for (int ck = 0; ck < cz; ck++) { + // gather cell-local node positions + int gindices[125]; // max npc = 125 for quadratic + mjtNum quat[4]; + mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos, NULL, NULL, + xpos_c, NULL, NULL, gindices, quat); - // blockwise rotation: K_rot(i,j) = scale * RT * K_local(i,j) * R - // note: k stores -K, so K_rot = scale * (-K_phys) - for (int i=0; i < nodenum; i++) { - for (int j=0; j < nodenum; j++) { - mjtNum blk[9], tmp[9]; + // R = R_global2local, RT = R_local2global + mjtNum R[9], RT[9]; + mju_quat2Mat(R, quat); + mju_transpose(RT, R, 3, 3); - // get K_local(i,j) - int adr = (3*i)*(3*nodenum) + 3*j; - for (int r=0; r < 3; r++) { - for (int c=0; c < 3; c++) { - blk[3*r+c] = k[adr + r*(3*nodenum) + c]; + // get cell stiffness + mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc; + + // compute K_rot_cell = RT * K_cell * R (block-wise) + mju_zero(K_rot_cell, dim_c*dim_c); + for (int a = 0; a < npc; a++) { + for (int b = 0; b < npc; b++) { + mjtNum blk[9], tmp[9]; + + // get K_cell(a,b) 3x3 block + int adr_cell = (3*a)*(3*npc) + 3*b; + for (int r = 0; r < 3; r++) { + for (int c = 0; c < 3; c++) { + blk[3*r+c] = k_cell[adr_cell + r*(3*npc) + c]; + } + } + + // tmp = K * R + mju_mulMatMat3(tmp, blk, R); + // blk = RT * tmp = RT * K * R + mju_mulMatMat3(blk, RT, tmp); + + // store in K_rot_cell at (a, b) + int adr_out = (3*a)*dim_c + 3*b; + for (int r = 0; r < 3; r++) { + for (int c = 0; c < 3; c++) { + K_rot_cell[adr_out + r*dim_c + c] = scale * blk[3*r+c]; + } + } + } } - } - // tmp = K * R - mju_mulMatMat3(tmp, blk, R); + // construct sparse Jacobian for this cell's nodes + int current_adr = 0; + for (int n = 0; n < npc; n++) { + int bid = bodyid[gindices[n]]; + int chain_nnz = mj_bodyChain(m, bid, chain_colind); + mj_jacSparse(m, d, blk_jac, NULL, xpos+3*gindices[n], bid, + chain_nnz, chain_colind, /*flg_skipcommon=*/0); - // blk = RT * tmp = RT * K * R - mju_mulMatMat3(blk, RT, tmp); + for (int r = 0; r < 3; r++) { + int row_idx = 3*n + r; + J_rownnz[row_idx] = chain_nnz; + J_rowadr[row_idx] = current_adr; - // store scaled into K_rot - for (int r=0; r < 3; r++) { - for (int c=0; c < 3; c++) { - K_rot[adr + r*(3*nodenum) + c] = scale * blk[3*r+c]; - } - } - } - } - - // construct sparse Jacobian J_val - int current_adr = 0; - for (int i=0; i < nodenum; i++) { - // get chain for this node - int chain_nnz = mj_bodyChain(m, bodyid[i], chain_colind); - - // compute sparse Jacobian for this node (3 rows) - mj_jacSparse(m, d, blk_jac, NULL, xpos+3*i, bodyid[i], chain_nnz, chain_colind, - /*flg_skipcommon=*/0); - - // copy to sparse structure - for (int r=0; r<3; r++) { - int row_idx = 3*i + r; - rownnz[row_idx] = chain_nnz; - rowadr[row_idx] = current_adr; - - for (int idx=0; idx= 0) { - J_reduced[i*ndof + local_idx] = J_val[adr + idx]; } + } } - } - // H -= J_reduced^T * K_rot * J_reduced - // K_rot * J_reduced (dim x ndof) - mjtNum* KJ = mjSTACKALLOC(d, dim*ndof, mjtNum); - mju_mulMatMat(KJ, K_rot, J_reduced, dim, dim, ndof); + // apply operation with cell's K_rot and J + if (op == mjFLEXOP_VEC) { + addJTBJ_mulSparse(m, d, res, vec, J_rownnz, J_rowadr, J_colind, + J_val, K_rot_cell, dim_c); + } else if (op == mjFLEXOP_ADDH) { + mj_markStack(d); + // H -= J_cell^T * K_rot_cell * J_cell + mjtNum* J_reduced = mjSTACKALLOC(d, dim_c*ndof, mjtNum); + mju_zero(J_reduced, dim_c*ndof); - // H[i, j] -= sum_k J_reduced[k, i] * KJ[k, j] - for (int i=0; i= 0) { + J_reduced[i*ndof + local_idx] = J_val[adr + idx]; + } + } + } + + // KJ = K_rot_cell * J_reduced (dim_c x ndof) + mjtNum* KJ = mjSTACKALLOC(d, dim_c*ndof, mjtNum); + mju_mulMatMat(KJ, K_rot_cell, J_reduced, dim_c, dim_c, ndof); + + // H[i,j] -= J_reduced[k,i] * KJ[k,j] + for (int i = 0; i < ndof; i++) { + for (int j = 0; j < ndof; j++) { + mjtNum val = 0; + for (int dim_idx = 0; dim_idx < dim_c; dim_idx++) { + val += J_reduced[dim_idx*ndof + i] * KJ[dim_idx*ndof + j]; + } + res[i*ndof + j] -= val; + } + } + mj_freeStack(d); } - // res is H - res[i*ndof + j] -= val; + + cell_idx++; } } } diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index d67f8683..e6d3b6fa 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -58,61 +58,7 @@ static void inline GradSquaredLengths(mjtNum gradient[6][2][3], } } -// compute interpolated flex state: xpos, vel, quat -// f: flex index -// xpos: (output) 3*nodenum -// vel: (output) 3*nodenum, can be NULL -// quat: (output) 4, rotation from global to local -void mj_flexInterpState(const mjModel* m, mjData* d, int f, - mjtNum* xpos, mjtNum* vel, mjtNum* quat) { - int nodenum = m->flex_nodenum[f]; - int nstart = m->flex_nodeadr[f]; - int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; - mjtNum com[3] = {0}; - // compute positions - if (m->flex_centered[f]) { - for (int i=0; i < nodenum; i++) { - mji_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]); - if (vel) { - mji_copy3(vel + 3*i, d->qvel + m->body_dofadr[bodyid[i]]); - } - } - } else { - mjtNum screw[6]; - for (int i=0; i < nodenum; i++) { - mji_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart)); - mji_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]); - if (vel) { - mj_objectVelocity(m, d, mjOBJ_BODY, bodyid[i], screw, 0); - mji_copy3(vel + 3*i, screw + 3); - } - } - } - - // compute center of mass - for (int i = 0; i < nodenum; i++) { - mji_addToScl3(com, xpos+3*i, 1.0/nodenum); - } - - // compute the Jacobian at the center of mass - mjtNum mat[9] = {0}; - mjtNum p[3] = {.5, .5, .5}; - mju_defGradient(mat, p, xpos, m->flex_interp[f]); - - // find rotation - mju_mat2Rot(quat, mat); - mju_negQuat(quat, quat); - - // rotate vertices to quat and add reference center of mass - for (int i = 0; i < nodenum; i++) { - mju_rotVecQuat(xpos+3*i, xpos+3*i, quat); - mji_addTo3(xpos+3*i, p); - if (vel) { - mju_rotVecQuat(vel+3*i, vel+3*i, quat); - } - } -} // spring and damper forces static void mj_springdamper(const mjModel* m, mjData* d) { @@ -284,42 +230,111 @@ static void mj_springdamper(const mjModel* m, mjData* d) { } if (m->flex_interp[f]) { + int order = m->flex_interp[f]; + int npc = (order+1)*(order+1)*(order+1); // nodes per cell + int cx = m->flex_cellnum[3*f+0]; + int cy = m->flex_cellnum[3*f+1]; + int cz = m->flex_cellnum[3*f+2]; + int ny_g = cy * order + 1; + int nz_g = cz * order + 1; + mj_markStack(d); - mjtNum* xpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); - mjtNum* displ = mjSTACKALLOC(d, 3*nodenum, mjtNum); - mjtNum* vel = mjSTACKALLOC(d, 3*nodenum, mjtNum); - mjtNum* frc = mjSTACKALLOC(d, 3*nodenum, mjtNum); - mjtNum* dmp = mjSTACKALLOC(d, 3*nodenum, mjtNum); + + // allocate global arrays + mjtNum* xpos_g = mjSTACKALLOC(d, 3*nodenum, mjtNum); + mjtNum* vel_g = mjSTACKALLOC(d, 3*nodenum, mjtNum); + mjtNum* frc_g = mjSTACKALLOC(d, 3*nodenum, mjtNum); + mjtNum* dmp_g = mjSTACKALLOC(d, 3*nodenum, mjtNum); mjtNum* xpos0 = m->flex_node0 + 3*m->flex_nodeadr[f]; int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; - mjtNum quat[4] = {1, 0, 0, 0}; - mj_flexInterpState(m, d, f, xpos, vel, quat); + // gather global node positions and velocities (unrotated) + mju_flexGatherState(m, d, f, xpos_g, vel_g); - // compute displacement - for (int i = 0; i < nodenum; i++) { - mji_addScl3(displ+3*i, xpos+3*i, xpos0+3*i, -1); + // zero global force accumulators + mju_zero(frc_g, 3*nodenum); + mju_zero(dmp_g, 3*nodenum); + + // per-cell arrays + mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); + mjtNum* vel_c = mjSTACKALLOC(d, 3*npc, mjtNum); + mjtNum* xpos0_c = mjSTACKALLOC(d, 3*npc, mjtNum); + mjtNum* displ_c = mjSTACKALLOC(d, 3*npc, mjtNum); + mjtNum* frc_c = mjSTACKALLOC(d, 3*npc, mjtNum); + mjtNum* dmp_c = mjSTACKALLOC(d, 3*npc, mjtNum); + + // loop over cells + int cell_idx = 0; + for (int ci = 0; ci < cx; ci++) { + for (int cj = 0; cj < cy; cj++) { + for (int ck = 0; ck < cz; ck++) { + // gather cell-local node data + mjtNum quat[4]; + mjtNum p[3] = {.5, .5, .5}; + mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, xpos0, + xpos_c, vel_c, xpos0_c, NULL, quat); + + // rotate to corotational frame + for (int n = 0; n < npc; n++) { + mju_rotVecQuat(xpos_c+3*n, xpos_c+3*n, quat); + mji_addTo3(xpos_c+3*n, p); + mju_rotVecQuat(vel_c+3*n, vel_c+3*n, quat); + } + + // compute displacement + for (int n = 0; n < npc; n++) { + mji_addScl3(displ_c+3*n, xpos_c+3*n, xpos0_c+3*n, -1); + } + + // get cell stiffness matrix + mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc; + + // compute force in corotational frame + if (enbl_spring) { + mju_mulMatVec(frc_c, k_cell, displ_c, 3*npc, 3*npc); + } + if (enbl_damper) { + mju_mulMatVec(dmp_c, k_cell, vel_c, 3*npc, 3*npc); + } + + // rotate back to global frame and scatter + mju_negQuat(quat, quat); + int local = 0; + for (int li = 0; li <= order; li++) { + for (int lj = 0; lj <= order; lj++) { + for (int lk = 0; lk <= order; lk++) { + int gi = ci*order + li; + int gj = cj*order + lj; + int gk = ck*order + lk; + int gidx = gi*ny_g*nz_g + gj*nz_g + gk; + mjtNum qfrc[3], qdmp[3]; + mji_rotVecQuat(qfrc, frc_c+3*local, quat); + mji_rotVecQuat(qdmp, dmp_c+3*local, quat); + if (enbl_spring) { + mji_addTo3(frc_g + 3*gidx, qfrc); + } + if (enbl_damper) { + mji_addTo3(dmp_g + 3*gidx, qdmp); + } + local++; + } + } + } + + cell_idx++; + } + } } - // compute force in the stretch frame - if (enbl_spring) mju_mulMatVec(frc, k, displ, 3*nodenum, 3*nodenum); - - // compute damping force in stretch frame - if (enbl_damper) mju_mulMatVec(dmp, k, vel, 3*nodenum, 3*nodenum); - - // rotate forces to global frame and add to qfrc - mju_negQuat(quat, quat); + // apply accumulated forces to bodies for (int i = 0; i < nodenum; i++) { - mjtNum qfrc[3], qdmp[3]; - mji_rotVecQuat(qfrc, frc+3*i, quat); - mji_rotVecQuat(qdmp, dmp+3*i, quat); - mju_scl3(qdmp, qdmp, m->flex_damping[f]); + mju_scl3(dmp_g+3*i, dmp_g+3*i, m->flex_damping[f]); if (m->flex_centered[f]) { - if (enbl_spring) mji_addTo3(d->qfrc_spring+m->body_dofadr[bodyid[i]], qfrc); - if (enbl_damper) mji_addTo3(d->qfrc_damper+m->body_dofadr[bodyid[i]], qdmp); + if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bodyid[i]], frc_g+3*i); + if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bodyid[i]], dmp_g+3*i); } else { - if (enbl_spring) mj_applyFT(m, d, qfrc, 0, xpos+3*i, bodyid[i], d->qfrc_spring); - if (enbl_damper) mj_applyFT(m, d, qdmp, 0, xpos+3*i, bodyid[i], d->qfrc_damper); + if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bodyid[i], d->qfrc_spring); + if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bodyid[i], d->qfrc_damper); } } diff --git a/src/engine/engine_passive.h b/src/engine/engine_passive.h index ba8b3233..096cfefb 100644 --- a/src/engine/engine_passive.h +++ b/src/engine/engine_passive.h @@ -28,9 +28,7 @@ extern "C" { // all passive forces MJAPI void mj_passive(const mjModel* m, mjData* d); -// compute interpolated flex state: xpos, vel, quat -MJAPI void mj_flexInterpState(const mjModel* m, mjData* d, int f, - mjtNum* xpos, mjtNum* vel, mjtNum* quat); + //------------------------- fluid models ----------------------------------------------------------- diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index abaf7055..6ec01f72 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -612,11 +612,116 @@ mjtNum mju_evalBasis(const mjtNum x[3], int i, int order) { } } +// map global parametric coord to cell-local coord and build node indices +// coord: [0,1]^3 parametric coordinates +// cellnum: cell counts (cx, cy, cz) +// order: interpolation order (1=trilinear, 2=triquadratic) +// local: output local parametric coordinates within cell [0,1]^3 +// nodeindices: output array of global node indices for the cell (size (order+1)^3, may be NULL) +// returns: number of nodes per cell (order+1)^3 +int mju_cellLookup(const mjtNum coord[3], const int cellnum[3], int order, mjtNum local[3], + int* nodeindices) { + int cx = cellnum[0], cy = cellnum[1], cz = cellnum[2]; + + // find containing cell + int ci = (int)mju_floor(coord[0] * cx); + int cj = (int)mju_floor(coord[1] * cy); + int ck = (int)mju_floor(coord[2] * cz); + ci = mjMIN(ci, cx - 1); ci = mjMAX(ci, 0); + cj = mjMIN(cj, cy - 1); cj = mjMAX(cj, 0); + ck = mjMIN(ck, cz - 1); ck = mjMAX(ck, 0); + + // local parametric coordinates within cell + local[0] = mju_clip(coord[0] * cx - ci, 0, 1); + local[1] = mju_clip(coord[1] * cy - cj, 0, 1); + local[2] = mju_clip(coord[2] * cz - ck, 0, 1); + + // build node indices for this cell + if (nodeindices) { + int ny_g = cy * order + 1; + int nz_g = cz * order + 1; + int ni = 0; + for (int li = 0; li <= order; li++) { + for (int lj = 0; lj <= order; lj++) { + for (int lk = 0; lk <= order; lk++) { + int gi = ci*order + li; + int gj = cj*order + lj; + int gk = ck*order + lk; + nodeindices[ni++] = gi*ny_g*nz_g + gj*nz_g + gk; + } + } + } + } + + int npc = (order + 1) * (order + 1) * (order + 1); + return npc; +} + + // interpolate a function at x with given interpolation coefficients and order n -void mju_interpolate3D(mjtNum res[3], const mjtNum x[3], const mjtNum* coeff, int order) { +void mju_interpolate3D(mjtNum res[3], const mjtNum x[3], const mjtNum* coeff, int order, + const int* nodeindices) { int npoint = (order + 1) * (order + 1) * (order + 1); for (int j=0; j < npoint; j++) { - mju_addToScl3(res, coeff+3*j, mju_evalBasis(x, j, order)); + int idx = nodeindices ? nodeindices[j] : j; + mju_addToScl3(res, coeff+3*idx, mju_evalBasis(x, j, order)); + } +} + + +static void flexInterpRotation(int order, const mjtNum* xpos_c, + const mjtNum local[3], mjtNum* quat) { + mjtNum mat[9] = {0}; + + if (order > 0) { + mju_defGradient(mat, local, xpos_c, order); + } else { + // order 0: fallback to identity matrix + mat[0] = 1; + mat[4] = 1; + mat[8] = 1; + } + + // find rotation + quat[0] = 1; + quat[1] = 0; + quat[2] = 0; + quat[3] = 0; + mju_mat2Rot(quat, mat); + mju_negQuat(quat, quat); +} + + +// gather cell-local quantities and optionally compute rotation +void mju_flexGatherCellState(int order, int cy, int cz, int ci, int cj, int ck, + const mjtNum* xpos_g, const mjtNum* vel_g, const mjtNum* xpos0_g, + mjtNum* xpos_c, mjtNum* vel_c, mjtNum* xpos0_c, + int* nodeindices, mjtNum* quat) { + int ny_g = cy * order + 1; + int nz_g = cz * order + 1; + + int local = 0; + for (int li = 0; li <= order; li++) { + for (int lj = 0; lj <= order; lj++) { + for (int lk = 0; lk <= order; lk++) { + int gi = ci*order + li; + int gj = cj*order + lj; + int gk = ck*order + lk; + int gidx = gi*ny_g*nz_g + gj*nz_g + gk; + + if (xpos_c && xpos_g) mju_copy3(xpos_c + 3*local, xpos_g + 3*gidx); + if (vel_c && vel_g) mju_copy3(vel_c + 3*local, vel_g + 3*gidx); + if (xpos0_c && xpos0_g) mju_copy3(xpos0_c + 3*local, xpos0_g + 3*gidx); + if (nodeindices) nodeindices[local] = gidx; + + local++; + } + } + } + + if (quat && xpos_c) { + mjtNum p[3] = {.5, .5, .5}; + flexInterpRotation(order, xpos_c, p, quat); } } diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index cac5bed5..b5574670 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -89,8 +89,20 @@ MJAPI void mju_defGradient(mjtNum res[9], const mjtNum p[3], const mjtNum* dof, // evaluate the basis function at x for the i-th node MJAPI mjtNum mju_evalBasis(const mjtNum x[3], int i, int order); +// map global parametric coord to cell-local coord and build node indices +MJAPI int mju_cellLookup(const mjtNum coord[3], const int cellnum[3], int order, mjtNum local[3], + int* nodeindices); + // interpolate a function at x with given interpolation coefficients and order n -MJAPI void mju_interpolate3D(mjtNum res[3], const mjtNum x[3], const mjtNum* coeff, int order); +MJAPI void mju_interpolate3D(mjtNum res[3], const mjtNum x[3], const mjtNum* coeff, int order, + const int* nodeindices); + +// gather cell-local quantities and optionally compute rotation +MJAPI void mju_flexGatherCellState(int order, int cy, int cz, int ci, int cj, int ck, + const mjtNum* xpos_g, const mjtNum* vel_g, + const mjtNum* xpos0_g, mjtNum* xpos_c, mjtNum* vel_c, + mjtNum* xpos0_c, int* nodeindices, mjtNum* quat); + // ----------------------------- Base64 ------------------------------------------------------------ diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 1c7b602c..d1ce7ffe 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -863,24 +863,30 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt, flexdist = newdist; if (m->flex_interp[i]) { mjtNum* coord = m->flex_vert0 + 3*(m->flex_vertadr[i] + vertid); + int order = m->flex_interp[i]; + int npc = (order+1)*(order+1)*(order+1); + + // cell lookup: get local coords and node indices + mjtNum loc[3]; + int nodeindices[27]; // max npc for quadratic: 3^3 = 27 + mju_cellLookup(coord, m->flex_cellnum+3*i, order, loc, nodeindices); + + // find node with largest weight in this cell int nodeid = -1; int nstart = m->flex_nodeadr[i]; - int nend = nstart + m->flex_nodenum[i]; mjtNum w = 0; - for (int j = nstart; j < nend; j++) { - if (mju_evalBasis(coord, j-nstart, m->flex_interp[i]) > w) { - w = mju_evalBasis(coord, j-nstart, m->flex_interp[i]); - nodeid = j; + for (int j = 0; j < npc; j++) { + mjtNum ww = mju_evalBasis(loc, j, order); + if (ww > w) { + w = ww; + nodeid = nodeindices[j]; } } - if (nodeid < 0) { - mjERROR("flex %d: node closest to vertex %d not found", i, vertid); - } - flexbodyid = m->flex_nodebodyid[m->flex_nodeadr[i] + nodeid]; + flexbodyid = m->flex_nodebodyid[nstart + nodeid]; if (m->flex_centered[i]) { mju_copy3(flexpnt, d->xpos + 3*flexbodyid); } else { - mju_mulMatVec3(flexpnt, d->xmat + 9*flexbodyid, m->flex_node + 3*nodeid); + mju_mulMatVec3(flexpnt, d->xmat + 9*flexbodyid, m->flex_node + 3*(nstart + nodeid)); mju_addTo3(flexpnt, d->xpos + 3*flexbodyid); } } else { diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index b1b74d21..c0072dbb 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1434,10 +1434,9 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, } // control points box - mjtNum xpos[mjMAXFLEXNODES]; + mjtNum* xpos = mjSTACKALLOC(d, 3*m->flex_nodenum[f], mjtNum); int nstart = m->flex_nodeadr[f]; int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; - int nnode = m->flex_interp[f]+1; if (m->flex_centered[f]) { for (int i=0; i < m->flex_nodenum[f]; i++) { mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]); @@ -1448,15 +1447,23 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]); } } - for (int i=0; i < nnode; i++) { - for (int j=0; j < nnode; j++) { - for (int k=0; k < nnode; k++) { - int nn = nnode*nnode; - int offset = 3*(nn*(i+0) + nnode*(j+0) + k); - int offset1 = 3*(nn*(i+1) + nnode*(j+0) + k); - int offset2 = 3*(nn*(i+0) + nnode*(j+1) + k); - int offset3 = 3*(nn*(i+0) + nnode*(j+0) + (k+1)); - if (i < nnode-1) { + + int cx = m->flex_cellnum[3*f+0]; + int cy = m->flex_cellnum[3*f+1]; + int cz = m->flex_cellnum[3*f+2]; + int order = m->flex_interp[f]; + int NX = cx * order + 1; + int NY = cy * order + 1; + int NZ = cz * order + 1; + + for (int i=0; i < NX; i++) { + for (int j=0; j < NY; j++) { + for (int k=0; k < NZ; k++) { + int offset = 3*(i*NY*NZ + j*NZ + k); + int offset1 = 3*((i+1)*NY*NZ + j*NZ + k); + int offset2 = 3*(i*NY*NZ + (j+1)*NZ + k); + int offset3 = 3*(i*NY*NZ + j*NZ + (k+1)); + if (i < NX-1) { mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN); if (!thisgeom) { return; @@ -1465,7 +1472,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset1); releaseGeom(&thisgeom, scn); } - if (j < nnode-1) { + if (j < NY-1) { mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN); if (!thisgeom) { return; @@ -1474,7 +1481,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset2); releaseGeom(&thisgeom, scn); } - if (k < nnode-1) { + if (k < NZ-1) { mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN); if (!thisgeom) { return; diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 56fcb615..4035a176 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -77,6 +77,7 @@ bool IsValidElementOrNodeHeader22(const std::string& line) { mjCFlexcomp::mjCFlexcomp(void) { type = mjFCOMPTYPE_GRID; count[0] = count[1] = count[2] = 10; + cellcount[0] = cellcount[1] = cellcount[2] = -1; mjuu_setvec(spacing, 0.02, 0.02, 0.02); mjuu_setvec(scale, 1, 1, 1); mass = 1; @@ -269,10 +270,19 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf // construct pinned array int nnode = 0; - if (doftype == mjFCOMPDOF_TRILINEAR) { - nnode = 8; - } else if (doftype == mjFCOMPDOF_QUADRATIC) { - nnode = 27; + if (doftype == mjFCOMPDOF_TRILINEAR || doftype == mjFCOMPDOF_QUADRATIC) { + int order = doftype == mjFCOMPDOF_TRILINEAR ? 1 : 2; + // multi-cell count for mesh/direct/gmsh, else single cell + int cx = 1, cy = 1, cz = 1; + if (type == mjFCOMPTYPE_MESH || type == mjFCOMPTYPE_DIRECT || + type == mjFCOMPTYPE_GMSH) { + if (cellcount[0] >= 0) { + cx = cellcount[0]; + cy = cellcount[1]; + cz = cellcount[2]; + } + } + nnode = (cx*order+1) * (cy*order+1) * (cz*order+1); } pinned = vector(std::max(npnt, nnode), rigid); @@ -562,36 +572,81 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } } - // create nodal mesh for trilinear interpolation + // create nodal mesh for trilinear/quadratic interpolation if (doftype == mjFCOMPDOF_TRILINEAR || doftype == mjFCOMPDOF_QUADRATIC) { - int order = doftype == mjFCOMPDOF_TRILINEAR ? 1 : 2; - flex->SetOrder(order); - std::vector node(3*(order+1)*(order+1)*(order+1), 0); + flex->spec.order = doftype == mjFCOMPDOF_TRILINEAR ? 1 : 2; + + if (cellcount[0] >= 0) { + flex->spec.cellcount[0] = cellcount[0]; + flex->spec.cellcount[1] = cellcount[1]; + flex->spec.cellcount[2] = cellcount[2]; + } + + // total number of nodes with shared boundaries + int nx = flex->spec.cellcount[0] * flex->spec.order + 1; + int ny = flex->spec.cellcount[1] * flex->spec.order + 1; + int nz = flex->spec.cellcount[2] * flex->spec.order + 1; + int nnode = nx * ny * nz; + + std::vector node(3 * nnode, 0); int idx = 0; - double step = 1.0 / (double)order; + + // Simpson's rule weights for quadratic mass distribution double massP2[3] = {1. / 6., 2. / 3., 1. / 6.}; - for (int i=0; i <= order; i++) { - for (int j=0; j <= order; j++) { - for (int k=0; k <= order; k++) { + + // compute per-node mass for trilinear: + // mass / nnode (uniform), or use Simpson for quadratic + double node_mass_uniform = mass / nnode; + + for (int gi = 0; gi < nx; gi++) { + for (int gj = 0; gj < ny; gj++) { + for (int gk = 0; gk < nz; gk++) { + // parametric position in [0, 1]^3 + double s = (double)gi / (flex->spec.cellcount[0] * flex->spec.order); + double t = (double)gj / (flex->spec.cellcount[1] * flex->spec.order); + double u = (double)gk / (flex->spec.cellcount[2] * flex->spec.order); + + // physical position + double px = minmax[0] + s * (minmax[3] - minmax[0]); + double py = minmax[1] + t * (minmax[4] - minmax[1]); + double pz = minmax[2] + u * (minmax[5] - minmax[2]); + if (pinned[idx]) { - node[3*idx+0] = minmax[0] + i * step * (minmax[3] - minmax[0]); - node[3*idx+1] = minmax[1] + j * step * (minmax[4] - minmax[1]); - node[3*idx+2] = minmax[2] + k * step * (minmax[5] - minmax[2]); - mjs_appendString(pf->nodebody, mjs_getName(body->element)->c_str()); + node[3*idx+0] = px; + node[3*idx+1] = py; + node[3*idx+2] = pz; + mjs_appendString(pf->nodebody, + mjs_getName(body->element)->c_str()); idx++; continue; } mjsBody* pb = mjs_addBody(body, 0); - pb->pos[0] = minmax[0] + i * step * (minmax[3] - minmax[0]); - pb->pos[1] = minmax[1] + j * step * (minmax[4] - minmax[1]); - pb->pos[2] = minmax[2] + k * step * (minmax[5] - minmax[2]); + pb->pos[0] = px; + pb->pos[1] = py; + pb->pos[2] = pz; mjuu_zerovec(pb->ipos, 3); + + // mass distribution if (doftype == mjFCOMPDOF_TRILINEAR) { - pb->mass = mass / 8; + pb->mass = node_mass_uniform; } else { - pb->mass = mass * massP2[i] * massP2[j] * massP2[k]; + // local index within the cell for mass computation + int li = gi % flex->spec.order; + int lj = gj % flex->spec.order; + int lk = gk % flex->spec.order; + // boundary nodes: average mass contribution + int ncells_i = (gi > 0 && gi < nx-1 && li == 0) ? 2 : 1; + int ncells_j = (gj > 0 && gj < ny-1 && lj == 0) ? 2 : 1; + int ncells_k = (gk > 0 && gk < nz-1 && lk == 0) ? 2 : 1; + // use Simpson weights scaled by cell count + double wi = massP2[li == 0 ? 0 : li]; + double wj = massP2[lj == 0 ? 0 : lj]; + double wk = massP2[lk == 0 ? 0 : lk]; + pb->mass = mass * wi * wj * wk * ncells_i * ncells_j * ncells_k + / (flex->spec.cellcount[0] * flex->spec.cellcount[1] * flex->spec.cellcount[2]); } + pb->inertia[0] = pb->mass*(2.0*inertiabox*inertiabox)/3.0; pb->inertia[1] = pb->mass*(2.0*inertiabox*inertiabox)/3.0; pb->inertia[2] = pb->mass*(2.0*inertiabox*inertiabox)/3.0; @@ -607,7 +662,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf // construct node name, add to nodebody char txt[100]; - mju::sprintf_arr(txt, "%s_%d_%d_%d", name.c_str(), i, j, k); + mju::sprintf_arr(txt, "%s_%d_%d_%d", name.c_str(), gi, gj, gk); mjs_setName(pb->element, txt); mjs_appendString(pf->nodebody, mjs_getName(pb->element)->c_str()); diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index 09e13c41..8624be7b 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -78,6 +78,7 @@ class mjCFlexcomp { std::string name; // flex name mjtFcompType type; // flexcomp type int count[3]; // grid count in each dimension + int cellcount[3]; // number of cells for interpolation double spacing[3]; // spacing between grid elements double scale[3]; // scaling for mesh and direct double origin[3]; // origin for generating a 3D mesh from a convex 2D mesh diff --git a/src/user/user_init.c b/src/user/user_init.c index 82db341b..1e522776 100644 --- a/src/user/user_init.c +++ b/src/user/user_init.c @@ -224,6 +224,9 @@ void mjs_defaultFlex(mjsFlex* flex) { // set other defaults flex->dim = 2; flex->radius = 0.005; + flex->cellcount[0] = 1; + flex->cellcount[1] = 1; + flex->cellcount[2] = 1; flex->internal = 0; flex->selfcollide = mjFLEXSELF_AUTO; flex->activelayers = 1; diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index dc2e5257..51a940a4 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -3961,7 +3961,10 @@ std::string mjCFlex::ComputeStiffnessCacheKey() const { combine(std::hash{}(young)); combine(std::hash{}(poisson)); - combine(std::hash{}(order_)); + combine(std::hash{}(spec.order)); + combine(std::hash{}(spec.cellcount[0])); + combine(std::hash{}(spec.cellcount[1])); + combine(std::hash{}(spec.cellcount[2])); // compute bounding box from vertex positions if (!vert_.empty()) { @@ -4086,10 +4089,20 @@ void mjCFlex::Compile(const mjVFS* vfs) { // set nnode nnode = static_cast(nodebody_.size()); - if (nnode && !order_) { - order_ = std::pow(nnode, 1.0 / 3) - 1; - if (nnode != std::pow(order_ + 1, 3)) { - throw mjCError(this, "number of nodes must be %d^3 but it is %d", nullptr, order_, nnode); + if (nnode && !spec.order) { + throw mjCError(this, "Interpolation order must be explicitly specified (dof is missing)"); + } + + // check node compatibility with count and dof + if (spec.order > 0) { + int expected_nodes = (spec.cellcount[0] * spec.order + 1) * + (spec.cellcount[1] * spec.order + 1) * + (spec.cellcount[2] * spec.order + 1); + if (nnode != expected_nodes) { + std::string msg = "number of nodes (" + std::to_string(nnode) + + ") does not match cellcount and dof expected (" + + std::to_string(expected_nodes) + ")"; + throw mjCError(this, msg.c_str()); } } @@ -4329,12 +4342,48 @@ void mjCFlex::Compile(const mjVFS* vfs) { } if (!stiffness_cached && young > 0 && interpolated) { - int n = pow(order_ + 1, 3); - int ndof = 3 * n; - if (stiffness.size() < ndof * ndof) { - stiffness.resize(ndof * ndof, 0); + int npc = pow(spec.order + 1, 3); // nodes per cell + int ndof_cell = 3 * npc; + int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2]; + int ncells = cx * cy * cz; + int ny_global = cy * spec.order + 1; + int nz_global = cz * spec.order + 1; + + // total stiffness = ncells * ndof_cell^2 + stiffness.resize(ncells * ndof_cell * ndof_cell, 0); + + // compute stiffness per cell + for (int ci = 0; ci < cx; ci++) { + for (int cj = 0; cj < cy; cj++) { + for (int ck = 0; ck < cz; ck++) { + int cell_idx = ci * cy * cz + cj * cz + ck; + + // gather cell's local node positions + std::vector cell_pos(3 * npc); + int local = 0; + for (int li = 0; li <= spec.order; li++) { + for (int lj = 0; lj <= spec.order; lj++) { + for (int lk = 0; lk <= spec.order; lk++) { + int gi = ci * spec.order + li; + int gj = cj * spec.order + lj; + int gk = ck * spec.order + lk; + int global = gi * ny_global * nz_global + gj * nz_global + gk; + mjuu_copyvec(cell_pos.data() + 3*local, nodexpos.data() + 3*global, 3); + local++; + } + } + } + + // compute per-cell stiffness + std::vector K_cell(ndof_cell * ndof_cell, 0); + ComputeLinearStiffness(K_cell, cell_pos.data(), young, poisson, spec.order); + + // copy into global stiffness array + mjuu_copyvec(stiffness.data() + cell_idx * ndof_cell * ndof_cell, + K_cell.data(), ndof_cell * ndof_cell); + } + } } - ComputeLinearStiffness(stiffness, nodexpos.data(), young, poisson, order_); } // create bounding volume hierarchy diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 64048736..85245020 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2190,8 +2190,13 @@ void mjCModel::SetSizes() { nflexshelldata += (int)flexes_[i]->shell.size(); nflexevpair += (int)flexes_[i]->evpair.size()/2; nflextexcoord += (flexes_[i]->HasTexcoord() ? flexes_[i]->get_texcoord().size()/2 : 0); - if (flexes_[i]->order_ != 0) { - extra_stiffness_size += (3 * flexes_[i]->nnode) * (3 * flexes_[i]->nnode); + if (flexes_[i]->spec.order != 0) { + int npc = (int)pow(flexes_[i]->spec.order + 1, 3); + int ndof_cell = 3 * npc; + int ncells = flexes_[i]->spec.cellcount[0] * + flexes_[i]->spec.cellcount[1] * + flexes_[i]->spec.cellcount[2]; + extra_stiffness_size += ncells * ndof_cell * ndof_cell; } if (flexes_[i]->interpolated || flexes_[i]->rigid) { continue; @@ -3467,18 +3472,30 @@ void mjCModel::CopyObjects(mjModel* m) { mjuu_copyvec(m->flex_rgba + 4 * i, pfl->rgba, 4); // elasticity - if (pfl->order_ == 0) { + if (pfl->spec.order == 0) { m->flex_stiffnessadr[i] = 21 * elem_adr; } else { m->flex_stiffnessadr[i] = current_extra_stiffness_adr; - current_extra_stiffness_adr += (3 * pfl->nnode) * (3 * pfl->nnode); + int npc = (int)pow(pfl->spec.order + 1, 3); + int ndof_cell = 3 * npc; + int ncells = pfl->spec.cellcount[0] * pfl->spec.cellcount[1] * pfl->spec.cellcount[2]; + current_extra_stiffness_adr += ncells * ndof_cell * ndof_cell; } if (!pfl->stiffness.empty()) { - mjuu_copyvec(m->flex_stiffness + m->flex_stiffnessadr[i], pfl->stiffness.data(), pfl->stiffness.size()); + mjuu_copyvec(m->flex_stiffness + m->flex_stiffnessadr[i], + pfl->stiffness.data(), pfl->stiffness.size()); } else { - int size = (pfl->order_ == 0) ? 21 * pfl->nelem : (3 * pfl->nnode) * (3 * pfl->nnode); - mjuu_zerovec(m->flex_stiffness + m->flex_stiffnessadr[i], size); + int stiff_size; + if (pfl->spec.order == 0) { + stiff_size = 21 * pfl->nelem; + } else { + int npc = (int)pow(pfl->spec.order + 1, 3); + int ndof_cell = 3 * npc; + int ncells = pfl->spec.cellcount[0] * pfl->spec.cellcount[1] * pfl->spec.cellcount[2]; + stiff_size = ncells * ndof_cell * ndof_cell; + } + mjuu_zerovec(m->flex_stiffness + m->flex_stiffnessadr[i], stiff_size); } if (!pfl->bending.empty()) { mjuu_copyvec(m->flex_bending + 17 * edge_adr, pfl->bending.data(), pfl->bending.size()); @@ -3613,7 +3630,12 @@ void mjCModel::CopyObjects(mjModel* m) { } // set interpolation type, only two types for now - m->flex_interp[i] = pfl->order_; + m->flex_interp[i] = pfl->spec.order; + + // set cell count for multi-cell finite cell method + m->flex_cellnum[3*i+0] = pfl->spec.cellcount[0]; + m->flex_cellnum[3*i+1] = pfl->spec.cellcount[1]; + m->flex_cellnum[3*i+2] = pfl->spec.cellcount[2]; // convert edge pairs to int array, set edge rigid for (int k=0; k < pfl->nedge; k++) { diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 232d0470..8ca60124 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1042,7 +1042,7 @@ class mjCFlex: public mjCFlex_, private mjsFlex { static constexpr int kNumEdges[3] = {1, 3, 6}; // number of edges per element indexed by dim - void SetOrder(int order) { order_ = order; } // set interpolation order + private: void Compile(const mjVFS* vfs); // compiler @@ -1052,7 +1052,7 @@ class mjCFlex: public mjCFlex_, private mjsFlex { std::vector vert0_; // vertex positions in [0, 1]^d in the bounding box std::vector node0_; // node Cartesian positions - int order_ = 0; // interpolation order + // stiffness caching std::string ComputeStiffnessCacheKey() const; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index df10509f..d38bde20 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -315,7 +315,7 @@ std::vector MJCF[nMJCF] = { {">"}, {">"}, {"flexcomp", "*", "name", "type", "group", "dim", "dof", - "count", "spacing", "radius", "rigid", "mass", "inertiabox", + "count", "cellcount", "spacing", "radius", "rigid", "mass", "inertiabox", "scale", "file", "point", "element", "texcoord", "material", "rgba", "flatskin", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler", "origin"}, {"<"}, @@ -334,8 +334,8 @@ std::vector MJCF[nMJCF] = { {"deformable", "*"}, {"<"}, - {"flex", "*", "name", "group", "dim", "radius", "material", - "rgba", "flatskin", "body", "vertex", "element", "texcoord", "elemtexcoord", "node"}, + {"flex", "*", "name", "group", "dim", "radius", "material", "rgba", "flatskin", "body", + "vertex", "element", "texcoord", "elemtexcoord", "node", "cellcount", "dof"}, {"<"}, {"contact", "?", "contype", "conaffinity", "condim", "priority", "friction", "solmix", "solref", "solimp", "margin", "gap", @@ -1501,6 +1501,16 @@ void mjXReader::OneFlex(XMLElement* elem, mjsFlex* flex) { ReadAttrInt(elem, "dim", &flex->dim); ReadAttrInt(elem, "group", &flex->group); + flex->cellcount[0] = 1; + flex->cellcount[1] = 1; + flex->cellcount[2] = 1; + ReadAttr(elem, "cellcount", 3, flex->cellcount, text); + + flex->order = 0; + if (MapValue(elem, "dof", &n, fdof_map, mjNFCOMPDOFS)) { + flex->order = (n == mjFCOMPDOF_QUADRATIC) ? 2 : (n == mjFCOMPDOF_TRILINEAR ? 1 : 0); + } + // read data vectors if (ReadAttrTxt(elem, "body", text, true)) { mjs_setStringVec(flex->vertbody, text.c_str()); @@ -2794,6 +2804,7 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) { fcomp.type = (mjtFcompType)n; } ReadAttr(elem, "count", 3, fcomp.count, text); + ReadAttr(elem, "cellcount", 3, fcomp.cellcount, text); ReadAttr(elem, "spacing", 3, fcomp.spacing, text); ReadAttr(elem, "scale", 3, fcomp.scale, text); ReadAttr(elem, "mass", 1, &fcomp.mass, text); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index e9aa9ea7..007a76a3 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -141,6 +141,13 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) { WriteAttrKey(elem, "flatskin", bool_map, 2, flex->flatskin, defflex.flatskin); WriteAttrInt(elem, "dim", flex->dim, defflex.dim); WriteAttrInt(elem, "group", flex->group, defflex.group); + WriteAttr(elem, "cellcount", 3, flex->spec.cellcount, defflex.spec.cellcount); + if (flex->spec.order != defflex.spec.order) { + string dof_str = "full"; + if (flex->spec.order == 1) dof_str = "trilinear"; + else if (flex->spec.order == 2) dof_str = "quadratic"; + WriteAttrTxt(elem, "dof", dof_str); + } // data vectors if (!flex->get_vertbody().empty()) { diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index 7b055936..d795a404 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -35,136 +35,9 @@ namespace { using ::testing::NotNull; using ::testing::Pointwise; + using CoreConstraintTest = MujocoTest; -// compute rotation residual following formula in mj_instantiateEquality -void RotationResidual(const mjModel *model, mjData *data, - const mjtNum qpos[7], const mjtNum dqpos[6], - mjtNum res[3]) { - // copy configuration, compute required quantities with mj_step1 - mju_copy(data->qpos, qpos, 7); - - // perturb configuration if given - if (dqpos) { - mj_integratePos(model, data->qpos, dqpos, 1); - } - - // update relevant quantities - mj_step1(model, data); - - // compute orientation residual - mjtNum quat1[4], quat2[4], quat3[4]; - mju_copy4(quat1, data->xquat+4*1); - mju_negQuat(quat2, data->xquat+4*2); - mju_mulQuat(quat3, quat2, quat1); - mju_copy3(res, quat3+1); -} - -// validate rotational Jacobian used in welds -TEST_F(CoreConstraintTest, WeldRotJacobian) { -#ifdef mjUSESINGLE - GTEST_SKIP() << "FD Jacobian with eps=1e-6 below float32 precision"; -#endif - constexpr char xml[] = R"( - - - )"; - char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, testing::NotNull()) << error; - ASSERT_EQ(model->nq, 7); - ASSERT_EQ(model->nv, 6); - static const int nv = 6; // for increased readability - mjData* data = mj_makeData(model); - - // arbitrary initial values for the ball and hinge joints - mjtNum qpos0[7] = {.5, .5, .5, .5, .7, .8, .9}; - - // compute required quantities using mj_step1 - mj_step1(model, data); - - // get orientation error - mjtNum res[3]; - RotationResidual(model, data, qpos0, NULL, res); - - // compute Jacobian with finite-differencing - mjtNum jacFD[3*nv]; - mjtNum dqpos[nv] = {0}; - mjtNum dres[3]; - const mjtNum eps = 1e-6; - for (int i=0; i < nv; i++) { - // nudge i-th dof - dqpos[i] = eps; - - // get nudged residual - RotationResidual(model, data, qpos0, dqpos, dres); - - // remove nudge - dqpos[i] = 0.0; - - // compute Jacobian column - for (int j=0; j < 3; j++) { - jacFD[nv*j + i] = (dres[j] - res[j]) / eps; - } - } - - // reset mjData to qpos0 - mju_copy(data->qpos, qpos0, 7); - mj_step1(model, data); - - // intermediate quaternions quat1 and quat2 - mjtNum quat1[4], negQuat2[4]; - mju_copy4(quat1, data->xquat+4*1); - mju_negQuat(negQuat2, data->xquat+4*2); - - // get analytical Jacobian following formula in mj_instantiateEquality - mjtNum jacdif[3*nv], jac0[3*nv], jac1[3*nv]; - mjtNum point[3] = {0}; - - // rotational Jacobian difference - mj_jacDifPair(model, data, NULL, 2, 1, point, point, - NULL, NULL, NULL, jac0, jac1, jacdif, mj_isSparse(model), - /*flg_skipcommon=*/0); - - // formula: 0.5 * neg(quat2) * (jac1-jac2) * quat1 - mjtNum axis[3], quat3[4], quat4[4]; - for (int j=0; j < nv; j++) { - // axis = [jac1-jac2]_col(j) - axis[0] = jacdif[0*nv+j]; - axis[1] = jacdif[1*nv+j]; - axis[2] = jacdif[2*nv+j]; - - // apply formula - mju_mulQuatAxis(quat3, negQuat2, axis); - mju_mulQuat(quat4, quat3, quat1); - - // correct Jacobian - jacdif[0*nv+j] = 0.5*quat4[1]; - jacdif[1*nv+j] = 0.5*quat4[2]; - jacdif[2*nv+j] = 0.5*quat4[3]; - } - - // test that analytical and finite-differenced Jacobians match - EXPECT_THAT(AsVector(jacFD, 3*nv), - Pointwise(MjNear(eps, 1e-3), AsVector(jacdif, 3*nv))); - - mj_deleteData(data); - mj_deleteModel(model); -} - // test formulas for penetration at rest TEST_F(CoreConstraintTest, RestPenetration) { constexpr char xml[] = R"( diff --git a/test/engine/engine_core_util_test.cc b/test/engine/engine_core_util_test.cc new file mode 100644 index 00000000..8cd73dca --- /dev/null +++ b/test/engine/engine_core_util_test.cc @@ -0,0 +1,740 @@ +// Copyright 2026 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Tests for engine/engine_core_util.c. + +#include "src/engine/engine_core_util.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include "test/fixture.h" + +namespace mujoco { +namespace { + +using ::testing::NotNull; +using ::testing::Pointwise; + +using FlexGatherStateTest = MujocoTest; + +TEST_F(FlexGatherStateTest, mju_flexGatherState_Grid) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + mjData* data = mj_makeData(model); + mj_forward(model, data); + + ASSERT_EQ(model->nflex, 1); + int f = 0; + int nodenum = model->flex_nodenum[f]; + int nstart = model->flex_nodeadr[f]; + + // Simulate a rotated state (90 degrees around Z axis) + ASSERT_TRUE(model->flex_centered[f]); + for (int i = 0; i < nodenum; i++) { + int b = model->flex_nodebodyid[nstart + i]; + mjtNum x = data->xpos[3*b + 0]; + mjtNum y = data->xpos[3*b + 1]; + mjtNum z = data->xpos[3*b + 2]; + + // Rotate 90 degrees around Z: (x, y, z) -> (-y, x, z) + data->xpos[3*b + 0] = -y; + data->xpos[3*b + 1] = x; + data->xpos[3*b + 2] = z; + } + + std::vector xpos(3 * nodenum); + mju_flexGatherState(model, data, f, xpos.data(), NULL); + + // Verify that gathered xpos matches the rotated data->xpos + for (int i = 0; i < nodenum; i++) { + int b = model->flex_nodebodyid[nstart + i]; + EXPECT_NEAR(xpos[3*i + 0], data->xpos[3*b + 0], 1e-5); + EXPECT_NEAR(xpos[3*i + 1], data->xpos[3*b + 1], 1e-5); + EXPECT_NEAR(xpos[3*i + 2], data->xpos[3*b + 2], 1e-5); + } + + mj_deleteData(data); + mj_deleteModel(model); +} + + +using AngMomMatTest = MujocoTest; + +static constexpr char AngMomTestingModel[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +// compare subtree angular momentum computed in two ways +TEST_F(AngMomMatTest, CompareAngMom) { + char error[1024]; + mjModel* model = + LoadModelFromString(AngMomTestingModel, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + int nv = model->nv; + int bodyid = mj_name2id(model, mjOBJ_BODY, "link1"); + + mjData* data = mj_makeData(model); + + // reset to the keyframe with some angular velocities + mj_resetDataKeyframe(model, data, 0); + mj_forward(model, data); + + // get the reference value of angular momentum + mj_subtreeVel(model, data); + mjtNum angmom_ref[3]; + mju_copy3(angmom_ref, data->subtree_angmom+3*bodyid); + + // compute angular momentum using the angular momentum matrix + mjtNum* angmom_mat = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); + mj_angmomMat(model, data, angmom_mat, bodyid); + mjtNum angmom_test[3]; + mju_mulMatVec(angmom_test, angmom_mat, data->qvel, 3, nv); + + // compare the two angular momentum values + for (int i = 0; i < 3; i++) { + EXPECT_THAT(angmom_ref[i], MjNear(angmom_test[i], 1e-8, 1e-4)); + } + + mju_free(angmom_mat); + mj_deleteData(data); + mj_deleteModel(model); +} + +// compare subtree angular momentum matrix: analytical and findiff +TEST_F(AngMomMatTest, CompareAngMomMats) { + char error[1024]; + mjModel* model = + LoadModelFromString(AngMomTestingModel, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + int nv = model->nv; + int bodyid = mj_name2id(model, mjOBJ_BODY, "link1"); + mjData* data = mj_makeData(model); + mjtNum* angmom_mat = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); + mjtNum* angmom_mat_fd = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); + + // reset to the keyframe with some angular velocities + mj_resetDataKeyframe(model, data, 0); + mj_forward(model, data); + + // compute the angular momentum matrix using the analytical method + mj_angmomMat(model, data, angmom_mat, bodyid); + + // compute the angular momentum matrix using finite differences + static constexpr mjtNum eps = MjTol(1e-6, 1e-3); + for (int i = 0; i < nv; i++) { + // reset vel, forward nudge i-th dof, get angmom + mju_copy(data->qvel, model->key_qvel, model->nv); + data->qvel[i] += eps; + mj_forward(model, data); + mj_subtreeVel(model, data); + mjtNum agmf[3]; + mju_copy3(agmf, data->subtree_angmom+3*bodyid); + + // reset vel, backward nudge i-th dof, get angmom + mju_copy(data->qvel, model->key_qvel, model->nv); + data->qvel[i] -= eps; + mj_forward(model, data); + mj_subtreeVel(model, data); + mjtNum agmb[3]; + mju_copy3(agmb, data->subtree_angmom+3*bodyid); + + // finite-difference the angmom matrix + for (int j = 0; j < 3; j++) { + angmom_mat_fd[nv*j+i] = (agmf[j] - agmb[j]) / (2 * eps); + } + } + + // compare the two matrices + for (int i = 0; i < 3*nv; i++) { + EXPECT_THAT(angmom_mat_fd[i], MjNear(angmom_mat[i], 1e-8, 2e-4)); + } + + mju_free(angmom_mat_fd); + mju_free(angmom_mat); + mj_deleteData(data); + mj_deleteModel(model); +} + +using JacobianTest = MujocoTest; +static const mjtNum max_abs_err = std::numeric_limits::epsilon(); + +static constexpr char kJacobianTestingModel[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +// compare analytic and finite-differenced subtree-com Jacobian +TEST_F(JacobianTest, SubtreeJac) { + char error[1024]; + mjModel* model = + LoadModelFromString(kJacobianTestingModel, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + int nv = model->nv; + int bodyid = mj_name2id(model, mjOBJ_BODY, "main"); + mjData* data = mj_makeData(model); + mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); + mjtNum* qpos = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nq); + mjtNum* nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv); + + // all we need for Jacobians are kinematics and CoM-related quantities + mj_kinematics(model, data); + mj_comPos(model, data); + + // get subtree CoM Jacobian of free body + mj_jacSubtreeCom(model, data, jac_subtree, bodyid); + + // save current subtree-com and qpos, clear nudge + mjtNum subtree_com[3]; + mju_copy3(subtree_com, data->subtree_com+3*bodyid); + mju_copy(qpos, data->qpos, model->nq); + mju_zero(nudge, nv); + + // compare analytic Jacobian to finite-difference approximation + static const mjtNum eps = 1e-6; + for (int i=0; i < nv; i++) { + // reset qpos, nudge i-th dof, update data->qpos, reset nudge + mju_copy(data->qpos, qpos, model->nq); + nudge[i] = 1; + mj_integratePos(model, data->qpos, nudge, eps); + nudge[i] = 0; + + // kinematics and comPos to get nudged com + mj_kinematics(model, data); + mj_comPos(model, data); + + // compare finite-differenced and analytic Jacobian + for (int j=0; j < 3; j++) { + mjtNum findiff = (data->subtree_com[3*bodyid+j] - subtree_com[j]) / eps; + EXPECT_THAT(jac_subtree[nv*j+i], MjNear(findiff, eps, 1e-2)); + } + } + + mju_free(nudge); + mju_free(qpos); + mju_free(jac_subtree); + mj_deleteData(data); + mj_deleteModel(model); +} + +// confirm that applying linear forces via the subtree-com Jacobian only creates +// the expected linear accelerations (no accelerations of internal joints) +TEST_F(JacobianTest, SubtreeJacNoInternalAcc) { + char error[1024]; + mjModel* model = + LoadModelFromString(kJacobianTestingModel, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + int nv = model->nv; + int bodyid = mj_name2id(model, mjOBJ_BODY, "main"); + mjData* data = mj_makeData(model); + mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); + + // all we need for Jacobians are kinematics and CoM-related quantities + mj_kinematics(model, data); + mj_comPos(model, data); + + // get subtree CoM Jacobian of free body + mj_jacSubtreeCom(model, data, jac_subtree, bodyid); + + // uncomment for debugging + // mju_printMat(jac_subtree, 3, nv); + + // call fwdPosition since we'll need the factorised mass matrix in the test + mj_fwdPosition(model, data); + + // treating the subtree Jacobian as the projection of 3 axis-aligned unit + // forces into joint space, solve for the resulting accelerations in-place + mj_solveM(model, data, jac_subtree, jac_subtree, 3); + + // expect to find accelerations of magnitude 1/subtreemass in the first 3 + // coordinates of the free joint and 0s elsewhere, since applying forces to + // the CoM should accelerate the whole mechanism without any internal motion + int body_dofadr = model->body_dofadr[bodyid]; + mjtNum invtreemass = 1.0/model->body_subtreemass[bodyid]; + for (int r = 0; r < 3; r++) { + for (int c = 0; c < nv; c++) { + mjtNum expected = c - body_dofadr == r ? invtreemass : 0.0; + EXPECT_THAT(jac_subtree[nv*r+c], MjNear(expected, max_abs_err, 1e-4)); + } + } + + mju_free(jac_subtree); + mj_deleteData(data); + mj_deleteModel(model); +} + +static constexpr char kQuat[] = R"( + + + + + + + + + + + + +)"; + +static constexpr char kFreeBall[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +static constexpr char kQuatlessPendulum[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +static constexpr char kTelescope[] = R"( + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +static constexpr char kHinge[] = R"( + + + + + + + + + + + + + +)"; + +// compare mj_jacDot with finite-differenced mj_jac +TEST_F(JacobianTest, JacDot) { + for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) { + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + int nv = model->nv; + mjData* data = mj_makeData(model); + + // load keyframe if present, step for a bit + if (model->nkey) mj_resetDataKeyframe(model, data, 0); + while (data->time < 0.1) { + mj_step(model, data); + } + + // minimal call required for mj_jacDot outputs to be valid + mj_kinematics(model, data); + mj_comPos(model, data); + mj_comVel(model, data); + + // get bodyid + int bodyid = mj_name2id(model, mjOBJ_BODY, "query"); + EXPECT_GT(bodyid, 0); + + // get site position + int siteid = mj_name2id(model, mjOBJ_SITE, "query"); + EXPECT_GT(siteid, -1); + mjtNum point[3]; + mju_copy3(point, data->site_xpos+3*siteid); + + // jac, jac_dot + std::vector jacp(3*nv); + std::vector jacr(3*nv); + mj_jac(model, data, jacp.data(), jacr.data(), point, bodyid); + std::vector jacp_dot(3*nv); + std::vector jacr_dot(3*nv); + mj_jacDot(model, data, jacp_dot.data(), jacr_dot.data(), point, bodyid); + + // jac_h: jacobian after integrating qpos with a timestep of h + constexpr mjtNum h = MjTol(1e-7, 5e-4); + mj_integratePos(model, data->qpos, data->qvel, h); + mj_kinematics(model, data); + mj_comPos(model, data); + std::vector jacp_h(3*nv); + std::vector jacr_h(3*nv); + mju_copy3(point, data->site_xpos+3*siteid); // get updated site position + mj_jac(model, data, jacp_h.data(), jacr_h.data(), point, bodyid); + + // jac_dot_h finite-difference approximation + std::vector jacp_dot_h(3*nv); + mju_sub(jacp_dot_h.data(), jacp_h.data(), jacp.data(), 3*nv); + mju_scl(jacp_dot_h.data(), jacp_dot_h.data(), 1/h, 3*nv); + std::vector jacr_dot_h(3*nv); + mju_sub(jacr_dot_h.data(), jacr_h.data(), jacr.data(), 3*nv); + mju_scl(jacr_dot_h.data(), jacr_dot_h.data(), 1/h, 3*nv); + + // compare finite-differenced and analytic + mjtNum tol = 1e-5; + EXPECT_THAT(jacp_dot, Pointwise(MjNear(tol, 5e-2), jacp_dot_h)); + EXPECT_THAT(jacr_dot, Pointwise(MjNear(tol, 5e-2), jacr_dot_h)); + + mj_deleteData(data); + mj_deleteModel(model); + } +} + +// compare mj_jacDotSparse with dense mj_jacDot +TEST_F(JacobianTest, JacDotSparse) { + for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) { + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + int nv = model->nv; + mjData* data = mj_makeData(model); + + // load keyframe if present, step for a bit + if (model->nkey) mj_resetDataKeyframe(model, data, 0); + while (data->time < 0.1) { + mj_step(model, data); + } + + // minimal call required for mj_jacDot outputs to be valid + mj_kinematics(model, data); + mj_comPos(model, data); + mj_comVel(model, data); + + // get bodyid and site position + int bodyid = mj_name2id(model, mjOBJ_BODY, "query"); + EXPECT_GT(bodyid, 0); + int siteid = mj_name2id(model, mjOBJ_SITE, "query"); + EXPECT_GT(siteid, -1); + mjtNum point[3]; + mju_copy3(point, data->site_xpos+3*siteid); + + // dense jacDot + std::vector jacp_dense(3*nv); + std::vector jacr_dense(3*nv); + mj_jacDot(model, data, jacp_dense.data(), jacr_dense.data(), point, bodyid); + + // compute body chain using public mjModel fields + std::vector chain(nv); + int NV = 0; + int weldbody = model->body_weldid[bodyid]; + if (weldbody) { + int da = model->body_dofadr[weldbody] + model->body_dofnum[weldbody] - 1; + while (da >= 0) { + chain[NV++] = da; + da = model->dof_parentid[da]; + } + std::reverse(chain.begin(), chain.begin() + NV); + } + EXPECT_GT(NV, 0); + + // sparse jacDot + std::vector jacp_sparse(3*NV); + std::vector jacr_sparse(3*NV); + mj_jacDotSparse(model, data, jacp_sparse.data(), jacr_sparse.data(), + point, bodyid, NV, chain.data()); + + // expand sparse to dense and compare + std::vector jacp_expanded(3*nv, 0); + std::vector jacr_expanded(3*nv, 0); + for (int ci = 0; ci < NV; ci++) { + int di = chain[ci]; + for (int r = 0; r < 3; r++) { + jacp_expanded[di+r*nv] = jacp_sparse[ci+r*NV]; + jacr_expanded[di+r*nv] = jacr_sparse[ci+r*NV]; + } + } + + // expect bitwise equality + EXPECT_EQ(jacp_expanded, jacp_dense); + EXPECT_EQ(jacr_expanded, jacr_dense); + + mj_deleteData(data); + mj_deleteModel(model); + } +} + + +// validate rotational Jacobian used in welds +TEST_F(JacobianTest, WeldRotJacobian) { +#ifdef mjUSESINGLE + GTEST_SKIP() << "FD Jacobian with eps=1e-6 below float32 precision"; +#endif + constexpr char xml[] = R"( + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, testing::NotNull()) << error; + ASSERT_EQ(model->nq, 7); + ASSERT_EQ(model->nv, 6); + static const int nv = 6; // for increased readability + mjData* data = mj_makeData(model); + + // arbitrary initial values for the ball and hinge joints + mjtNum qpos0[7] = {.5, .5, .5, .5, .7, .8, .9}; + + // compute required quantities using mj_step1 + mj_step1(model, data); + + // get orientation error + mjtNum res[3]; + // compute rotation residual following formula in mj_instantiateEquality + auto RotationResidual = [](const mjModel *model, mjData *data, + const mjtNum qpos[7], const mjtNum dqpos[6], + mjtNum res[3]) { + // copy configuration, compute required quantities with mj_step1 + mju_copy(data->qpos, qpos, 7); + + // perturb configuration if given + if (dqpos) { + mj_integratePos(model, data->qpos, dqpos, 1); + } + + // update relevant quantities + mj_step1(model, data); + + // compute orientation residual + mjtNum quat1[4], quat2[4], quat3[4]; + mju_copy4(quat1, data->xquat+4*1); + mju_negQuat(quat2, data->xquat+4*2); + mju_mulQuat(quat3, quat2, quat1); + mju_copy3(res, quat3+1); + }; + + RotationResidual(model, data, qpos0, NULL, res); + + // compute Jacobian with finite-differencing + mjtNum jacFD[3*nv]; + mjtNum dqpos[nv] = {0}; + mjtNum dres[3]; + const mjtNum eps = 1e-6; + for (int i=0; i < nv; i++) { + // nudge i-th dof + dqpos[i] = eps; + + // get nudged residual + RotationResidual(model, data, qpos0, dqpos, dres); + + // remove nudge + dqpos[i] = 0.0; + + // compute Jacobian column + for (int j=0; j < 3; j++) { + jacFD[nv*j + i] = (dres[j] - res[j]) / eps; + } + } + + // reset mjData to qpos0 + mju_copy(data->qpos, qpos0, 7); + mj_step1(model, data); + + // intermediate quaternions quat1 and quat2 + mjtNum quat1[4], negQuat2[4]; + mju_copy4(quat1, data->xquat+4*1); + mju_negQuat(negQuat2, data->xquat+4*2); + + // get analytical Jacobian following formula in mj_instantiateEquality + mjtNum jacdif[3*nv], jac0[3*nv], jac1[3*nv]; + mjtNum point[3] = {0}; + + // rotational Jacobian difference + mj_jacDifPair(model, data, NULL, 2, 1, point, point, + NULL, NULL, NULL, jac0, jac1, jacdif, mj_isSparse(model), + /*flg_skipcommon=*/0); + + // formula: 0.5 * neg(quat2) * (jac1-jac2) * quat1 + mjtNum axis[3], quat3[4], quat4[4]; + for (int j=0; j < nv; j++) { + // axis = [jac1-jac2]_col(j) + axis[0] = jacdif[0*nv+j]; + axis[1] = jacdif[1*nv+j]; + axis[2] = jacdif[2*nv+j]; + + // apply formula + mju_mulQuatAxis(quat3, negQuat2, axis); + mju_mulQuat(quat4, quat3, quat1); + + // correct Jacobian + jacdif[0*nv+j] = 0.5*quat4[1]; + jacdif[1*nv+j] = 0.5*quat4[2]; + jacdif[2*nv+j] = 0.5*quat4[3]; + } + + // test that analytical and finite-differenced Jacobians match + EXPECT_THAT(AsVector(jacFD, 3*nv), + Pointwise(MjNear(eps, 1e-3), AsVector(jacdif, 3*nv))); + + mj_deleteData(data); + mj_deleteModel(model); +} + +} // namespace +} // namespace mujoco + + diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 4b0caf9c..3e36934f 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -3107,7 +3107,7 @@ TEST_F(ForwardTest, FlexParentCoupling) { - @@ -3152,7 +3152,7 @@ TEST_F(ForwardTest, FlexParentCoupling) { if (diff > max_diff) max_diff = diff; } - EXPECT_LT(max_diff, MjTol(2e-5, 5e-3)) + EXPECT_LT(max_diff, MjTol(2e-5, 1.5e-2)) << "Implicit integrator should match Euler at small timestep"; mj_deleteData(data); diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index 629517ae..f52d374f 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -14,12 +14,9 @@ // Tests for engine/{engine_support.c and engine_core_util.c} -#include "src/engine/engine_core_util.h" #include "src/engine/engine_support.h" -#include #include -#include #include #include #include @@ -41,521 +38,9 @@ using ::testing::Ne; using ::testing::NotNull; using ::testing::Pointwise; -using AngMomMatTest = MujocoTest; -static constexpr char AngMomTestingModel[] = R"( - - - - - - - - - - - - - - - - - - - - - - - - -)"; -// compare subtree angular momentum computed in two ways -TEST_F(AngMomMatTest, CompareAngMom) { - char error[1024]; - mjModel* model = - LoadModelFromString(AngMomTestingModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - int nv = model->nv; - int bodyid = mj_name2id(model, mjOBJ_BODY, "link1"); - mjData* data = mj_makeData(model); - - // reset to the keyframe with some angular velocities - mj_resetDataKeyframe(model, data, 0); - mj_forward(model, data); - - // get the reference value of angular momentum - mj_subtreeVel(model, data); - mjtNum angmom_ref[3]; - mju_copy3(angmom_ref, data->subtree_angmom+3*bodyid); - - // compute angular momentum using the angular momentum matrix - mjtNum* angmom_mat = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); - mj_angmomMat(model, data, angmom_mat, bodyid); - mjtNum angmom_test[3]; - mju_mulMatVec(angmom_test, angmom_mat, data->qvel, 3, nv); - - // compare the two angular momentum values - for (int i = 0; i < 3; i++) { - EXPECT_THAT(angmom_ref[i], MjNear(angmom_test[i], 1e-8, 1e-4)); - } - - mju_free(angmom_mat); - mj_deleteData(data); - mj_deleteModel(model); -} - -// compare subtree angular momentum matrix: analytical and findiff -TEST_F(AngMomMatTest, CompareAngMomMats) { - char error[1024]; - mjModel* model = - LoadModelFromString(AngMomTestingModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - int nv = model->nv; - int bodyid = mj_name2id(model, mjOBJ_BODY, "link1"); - mjData* data = mj_makeData(model); - mjtNum* angmom_mat = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); - mjtNum* angmom_mat_fd = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); - - // reset to the keyframe with some angular velocities - mj_resetDataKeyframe(model, data, 0); - mj_forward(model, data); - - // compute the angular momentum matrix using the analytical method - mj_angmomMat(model, data, angmom_mat, bodyid); - - // compute the angular momentum matrix using finite differences - static constexpr mjtNum eps = MjTol(1e-6, 1e-3); - for (int i = 0; i < nv; i++) { - // reset vel, forward nudge i-th dof, get angmom - mju_copy(data->qvel, model->key_qvel, model->nv); - data->qvel[i] += eps; - mj_forward(model, data); - mj_subtreeVel(model, data); - mjtNum agmf[3]; - mju_copy3(agmf, data->subtree_angmom+3*bodyid); - - // reset vel, backward nudge i-th dof, get angmom - mju_copy(data->qvel, model->key_qvel, model->nv); - data->qvel[i] -= eps; - mj_forward(model, data); - mj_subtreeVel(model, data); - mjtNum agmb[3]; - mju_copy3(agmb, data->subtree_angmom+3*bodyid); - - // finite-difference the angmom matrix - for (int j = 0; j < 3; j++) { - angmom_mat_fd[nv*j+i] = (agmf[j] - agmb[j]) / (2 * eps); - } - } - - // compare the two matrices - for (int i = 0; i < 3*nv; i++) { - EXPECT_THAT(angmom_mat_fd[i], MjNear(angmom_mat[i], 1e-8, 2e-4)); - } - - mju_free(angmom_mat_fd); - mju_free(angmom_mat); - mj_deleteData(data); - mj_deleteModel(model); -} - -using JacobianTest = MujocoTest; -static const mjtNum max_abs_err = std::numeric_limits::epsilon(); - -static constexpr char kJacobianTestingModel[] = R"( - - - - - - - - - - - - - - - - - - - - - - - - - - - - -)"; - -// compare analytic and finite-differenced subtree-com Jacobian -TEST_F(JacobianTest, SubtreeJac) { - char error[1024]; - mjModel* model = - LoadModelFromString(kJacobianTestingModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - int nv = model->nv; - int bodyid = mj_name2id(model, mjOBJ_BODY, "main"); - mjData* data = mj_makeData(model); - mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); - mjtNum* qpos = (mjtNum*) mju_malloc(sizeof(mjtNum)*model->nq); - mjtNum* nudge = (mjtNum*) mju_malloc(sizeof(mjtNum)*nv); - - // all we need for Jacobians are kinematics and CoM-related quantities - mj_kinematics(model, data); - mj_comPos(model, data); - - // get subtree CoM Jacobian of free body - mj_jacSubtreeCom(model, data, jac_subtree, bodyid); - - // save current subtree-com and qpos, clear nudge - mjtNum subtree_com[3]; - mju_copy3(subtree_com, data->subtree_com+3*bodyid); - mju_copy(qpos, data->qpos, model->nq); - mju_zero(nudge, nv); - - // compare analytic Jacobian to finite-difference approximation - static const mjtNum eps = 1e-6; - for (int i=0; i < nv; i++) { - // reset qpos, nudge i-th dof, update data->qpos, reset nudge - mju_copy(data->qpos, qpos, model->nq); - nudge[i] = 1; - mj_integratePos(model, data->qpos, nudge, eps); - nudge[i] = 0; - - // kinematics and comPos to get nudged com - mj_kinematics(model, data); - mj_comPos(model, data); - - // compare finite-differenced and analytic Jacobian - for (int j=0; j < 3; j++) { - mjtNum findiff = (data->subtree_com[3*bodyid+j] - subtree_com[j]) / eps; - EXPECT_THAT(jac_subtree[nv*j+i], MjNear(findiff, eps, 1e-2)); - } - } - - mju_free(nudge); - mju_free(qpos); - mju_free(jac_subtree); - mj_deleteData(data); - mj_deleteModel(model); -} - -// confirm that applying linear forces via the subtree-com Jacobian only creates -// the expected linear accelerations (no accelerations of internal joints) -TEST_F(JacobianTest, SubtreeJacNoInternalAcc) { - char error[1024]; - mjModel* model = - LoadModelFromString(kJacobianTestingModel, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - int nv = model->nv; - int bodyid = mj_name2id(model, mjOBJ_BODY, "main"); - mjData* data = mj_makeData(model); - mjtNum* jac_subtree = (mjtNum*) mju_malloc(sizeof(mjtNum)*3*nv); - - // all we need for Jacobians are kinematics and CoM-related quantities - mj_kinematics(model, data); - mj_comPos(model, data); - - // get subtree CoM Jacobian of free body - mj_jacSubtreeCom(model, data, jac_subtree, bodyid); - - // uncomment for debugging - // mju_printMat(jac_subtree, 3, nv); - - // call fwdPosition since we'll need the factorised mass matrix in the test - mj_fwdPosition(model, data); - - // treating the subtree Jacobian as the projection of 3 axis-aligned unit - // forces into joint space, solve for the resulting accelerations in-place - mj_solveM(model, data, jac_subtree, jac_subtree, 3); - - // expect to find accelerations of magnitude 1/subtreemass in the first 3 - // coordinates of the free joint and 0s elsewhere, since applying forces to - // the CoM should accelerate the whole mechanism without any internal motion - int body_dofadr = model->body_dofadr[bodyid]; - mjtNum invtreemass = 1.0/model->body_subtreemass[bodyid]; - for (int r = 0; r < 3; r++) { - for (int c = 0; c < nv; c++) { - mjtNum expected = c - body_dofadr == r ? invtreemass : 0.0; - EXPECT_THAT(jac_subtree[nv*r+c], MjNear(expected, max_abs_err, 1e-4)); - } - } - - mju_free(jac_subtree); - mj_deleteData(data); - mj_deleteModel(model); -} - -static constexpr char kQuat[] = R"( - - - - - - - - - - - - -)"; - -static constexpr char kFreeBall[] = R"( - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -)"; - -static constexpr char kQuatlessPendulum[] = R"( - - - - - - - - - - - - - - - - - - - - - - - - -)"; - -static constexpr char kTelescope[] = R"( - - - - - - - - - - - - - - - - - - - - - - - - -)"; - -static constexpr char kHinge[] = R"( - - - - - - - - - - - - - -)"; - -// compare mj_jacDot with finite-differenced mj_jac -TEST_F(JacobianTest, JacDot) { - for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) { - char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - int nv = model->nv; - mjData* data = mj_makeData(model); - - // load keyframe if present, step for a bit - if (model->nkey) mj_resetDataKeyframe(model, data, 0); - while (data->time < 0.1) { - mj_step(model, data); - } - - // minimal call required for mj_jacDot outputs to be valid - mj_kinematics(model, data); - mj_comPos(model, data); - mj_comVel(model, data); - - // get bodyid - int bodyid = mj_name2id(model, mjOBJ_BODY, "query"); - EXPECT_GT(bodyid, 0); - - // get site position - int siteid = mj_name2id(model, mjOBJ_SITE, "query"); - EXPECT_GT(siteid, -1); - mjtNum point[3]; - mju_copy3(point, data->site_xpos+3*siteid); - - // jac, jac_dot - vector jacp(3*nv); - vector jacr(3*nv); - mj_jac(model, data, jacp.data(), jacr.data(), point, bodyid); - vector jacp_dot(3*nv); - vector jacr_dot(3*nv); - mj_jacDot(model, data, jacp_dot.data(), jacr_dot.data(), point, bodyid); - - // jac_h: jacobian after integrating qpos with a timestep of h - constexpr mjtNum h = MjTol(1e-7, 5e-4); - mj_integratePos(model, data->qpos, data->qvel, h); - mj_kinematics(model, data); - mj_comPos(model, data); - vector jacp_h(3*nv); - vector jacr_h(3*nv); - mju_copy3(point, data->site_xpos+3*siteid); // get updated site position - mj_jac(model, data, jacp_h.data(), jacr_h.data(), point, bodyid); - - // jac_dot_h finite-difference approximation - vector jacp_dot_h(3*nv); - mju_sub(jacp_dot_h.data(), jacp_h.data(), jacp.data(), 3*nv); - mju_scl(jacp_dot_h.data(), jacp_dot_h.data(), 1/h, 3*nv); - vector jacr_dot_h(3*nv); - mju_sub(jacr_dot_h.data(), jacr_h.data(), jacr.data(), 3*nv); - mju_scl(jacr_dot_h.data(), jacr_dot_h.data(), 1/h, 3*nv); - - // compare finite-differenced and analytic - mjtNum tol = 1e-5; - EXPECT_THAT(jacp_dot, Pointwise(MjNear(tol, 5e-2), jacp_dot_h)); - EXPECT_THAT(jacr_dot, Pointwise(MjNear(tol, 5e-2), jacr_dot_h)); - - mj_deleteData(data); - mj_deleteModel(model); - } -} - -// compare mj_jacDotSparse with dense mj_jacDot -TEST_F(JacobianTest, JacDotSparse) { - for (auto xml : {kHinge, kQuat, kTelescope, kFreeBall, kQuatlessPendulum}) { - char error[1024]; - mjModel* model = LoadModelFromString(xml, error, sizeof(error)); - ASSERT_THAT(model, NotNull()) << error; - int nv = model->nv; - mjData* data = mj_makeData(model); - - // load keyframe if present, step for a bit - if (model->nkey) mj_resetDataKeyframe(model, data, 0); - while (data->time < 0.1) { - mj_step(model, data); - } - - // minimal call required for mj_jacDot outputs to be valid - mj_kinematics(model, data); - mj_comPos(model, data); - mj_comVel(model, data); - - // get bodyid and site position - int bodyid = mj_name2id(model, mjOBJ_BODY, "query"); - EXPECT_GT(bodyid, 0); - int siteid = mj_name2id(model, mjOBJ_SITE, "query"); - EXPECT_GT(siteid, -1); - mjtNum point[3]; - mju_copy3(point, data->site_xpos+3*siteid); - - // dense jacDot - vector jacp_dense(3*nv); - vector jacr_dense(3*nv); - mj_jacDot(model, data, jacp_dense.data(), jacr_dense.data(), point, bodyid); - - // compute body chain using public mjModel fields - vector chain(nv); - int NV = 0; - int weldbody = model->body_weldid[bodyid]; - if (weldbody) { - int da = model->body_dofadr[weldbody] + model->body_dofnum[weldbody] - 1; - while (da >= 0) { - chain[NV++] = da; - da = model->dof_parentid[da]; - } - std::reverse(chain.begin(), chain.begin() + NV); - } - EXPECT_GT(NV, 0); - - // sparse jacDot - vector jacp_sparse(3*NV); - vector jacr_sparse(3*NV); - mj_jacDotSparse(model, data, jacp_sparse.data(), jacr_sparse.data(), - point, bodyid, NV, chain.data()); - - // expand sparse to dense and compare - vector jacp_expanded(3*nv, 0); - vector jacr_expanded(3*nv, 0); - for (int ci = 0; ci < NV; ci++) { - int di = chain[ci]; - for (int r = 0; r < 3; r++) { - jacp_expanded[di+r*nv] = jacp_sparse[ci+r*NV]; - jacr_expanded[di+r*nv] = jacr_sparse[ci+r*NV]; - } - } - - // expect bitwise equality - EXPECT_EQ(jacp_expanded, jacp_dense); - EXPECT_EQ(jacr_expanded, jacr_dense); - - mj_deleteData(data); - mj_deleteModel(model); - } -} using Name2idTest = MujocoTest; diff --git a/test/engine/engine_util_misc_test.cc b/test/engine/engine_util_misc_test.cc index 2acd6125..d1eb8314 100644 --- a/test/engine/engine_util_misc_test.cc +++ b/test/engine/engine_util_misc_test.cc @@ -430,13 +430,90 @@ TEST_F(InterpolationTest, mju_interpolate3D) { expected[0] = quadratic_function_1(sample[0], sample[1], sample[2]); expected[1] = quadratic_function_2(sample[0], sample[1], sample[2]); expected[2] = quadratic_function_3(sample[0], sample[1], sample[2]); - mju_interpolate3D(res, sample, coeff, order); + mju_interpolate3D(res, sample, coeff, order, NULL); EXPECT_NEAR(res[0], expected[0], MjTol(1e-10, 1e-5)); EXPECT_NEAR(res[1], expected[1], MjTol(1e-10, 1e-5)); EXPECT_NEAR(res[2], expected[2], MjTol(1e-10, 1e-5)); } } +TEST_F(InterpolationTest, mju_cellLookup_SingleCell) { + // single cell (1x1x1): local coords should equal global coords + int cellnum[3] = {1, 1, 1}; + mjtNum coord[3] = {0.3, 0.7, 0.5}; + mjtNum local[3]; + int nodeindices[8]; + + int npc = mju_cellLookup(coord, cellnum, 1, local, nodeindices); + EXPECT_EQ(npc, 8); + EXPECT_NEAR(local[0], 0.3, MjTol(1e-12, 1e-6)); + EXPECT_NEAR(local[1], 0.7, MjTol(1e-12, 1e-6)); + EXPECT_NEAR(local[2], 0.5, MjTol(1e-12, 1e-6)); + + // for trilinear 1x1x1: nodes are 0..7 in lexicographic order + for (int i = 0; i < 8; i++) { + EXPECT_EQ(nodeindices[i], i); + } +} + +TEST_F(InterpolationTest, mju_cellLookup_MultiCell) { + // 2x3x4 grid, trilinear: 3x4x5 = 60 nodes + int cellnum[3] = {2, 3, 4}; + int order = 1; + int ny_g = 3*1 + 1; // 4 + int nz_g = 4*1 + 1; // 5 + + // point at (0.75, 0.5, 0.125) -> cell (1, 1, 0) + mjtNum coord[3] = {0.75, 0.5, 0.125}; + mjtNum local[3]; + int nodeindices[8]; + + int npc = mju_cellLookup(coord, cellnum, order, local, nodeindices); + EXPECT_EQ(npc, 8); + + // cell (1,1,0): local = (0.75*2 - 1, 0.5*3 - 1, 0.125*4 - 0) + EXPECT_NEAR(local[0], 0.5, 1e-12); + EXPECT_NEAR(local[1], 0.5, 1e-12); + EXPECT_NEAR(local[2], 0.5, 1e-12); + + // expected node indices for cell (1,1,0), trilinear: + // (gi, gj, gk) for li,lj,lk in {0,1} + // gi = 1+li, gj = 1+lj, gk = 0+lk + // gidx = gi*ny_g*nz_g + gj*nz_g + gk + int expected[8]; + int ni = 0; + for (int li = 0; li <= 1; li++) { + for (int lj = 0; lj <= 1; lj++) { + for (int lk = 0; lk <= 1; lk++) { + expected[ni++] = (1+li)*ny_g*nz_g + (1+lj)*nz_g + lk; + } + } + } + for (int i = 0; i < 8; i++) { + EXPECT_EQ(nodeindices[i], expected[i]); + } +} + +TEST_F(InterpolationTest, mju_cellLookup_Boundary) { + // point exactly at coord=1.0 should clamp to last cell + int cellnum[3] = {3, 3, 3}; + mjtNum coord[3] = {1.0, 1.0, 1.0}; + mjtNum local[3]; + + mju_cellLookup(coord, cellnum, 1, local, NULL); + // cell (2,2,2), local = (1*3 - 2, 1*3 - 2, 1*3 - 2) = (1, 1, 1) + EXPECT_NEAR(local[0], 1.0, 1e-12); + EXPECT_NEAR(local[1], 1.0, 1e-12); + EXPECT_NEAR(local[2], 1.0, 1e-12); + + // point at coord=0.0 should map to first cell + mjtNum coord0[3] = {0.0, 0.0, 0.0}; + mju_cellLookup(coord0, cellnum, 1, local, NULL); + EXPECT_NEAR(local[0], 0.0, 1e-12); + EXPECT_NEAR(local[1], 0.0, 1e-12); + EXPECT_NEAR(local[2], 0.0, 1e-12); +} + TEST_F(InterpolationTest, mju_defGradient) { int order = 1; mjtNum mat[9]; @@ -521,7 +598,48 @@ TEST_F(InterpolationTest, mju_defGradient) { EXPECT_THAT(mat, Pointwise(MjNear(1e-8, 1e-6), rot7)); } -// --------------------------------- Base64 ------------------------------------ +TEST_F(InterpolationTest, mju_flexInterpState_MultiCell) { + int order = 1; // trilinear + int cy = 2; + int cz = 2; + int nodenum = 27; // 3x3x3 + + std::vector xpos(3 * nodenum); + mjtNum quat[4]; + + // Populate xpos directly for a grid centered at origin, rotated 90 deg around + // Z Original grid points: {-0.1, 0.0, 0.1}^3 Rotated: (x, y, z) -> (-y, x, z) + int idx = 0; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + for (int k = 0; k < 3; k++) { + mjtNum x = (i - 1) * 0.1; + mjtNum y = (j - 1) * 0.1; + mjtNum z = (k - 1) * 0.1; + + // Apply rotation + xpos[3*idx + 0] = -y; + xpos[3*idx + 1] = x; + xpos[3*idx + 2] = z; + idx++; + } + } + } + + int npc = (order+1)*(order+1)*(order+1); + std::vector xpos_c(3 * npc); + + mju_flexGatherCellState(order, cy, cz, 0, 0, 0, xpos.data(), NULL, NULL, + xpos_c.data(), NULL, NULL, NULL, quat); + + // Expected quaternion for -90 deg around Z (global to local): + // [sqrt(0.5), 0, 0, -sqrt(0.5)] + mjtNum expected_val = mju_sqrt(0.5); + EXPECT_NEAR(quat[0], expected_val, 1e-5); + EXPECT_NEAR(quat[1], 0.0, 1e-5); + EXPECT_NEAR(quat[2], 0.0, 1e-5); + EXPECT_NEAR(quat[3], -expected_val, 1e-5); +} using Base64Test = MujocoTest; diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 94d15988..d7d4c47d 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -1200,6 +1200,7 @@ public unsafe struct mjModel_ { public int* flex_matid; public int* flex_group; public int* flex_interp; + public int* flex_cellnum; public int* flex_nodeadr; public int* flex_nodenum; public int* flex_vertadr; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 3fcacd7d..80fb63d5 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -2446,6 +2446,15 @@ struct MjsFlex { void set_elastic2d(int value) { ptr_->elastic2d = value; } + emscripten::val cellcount() const { + return emscripten::val(emscripten::typed_memory_view(3, ptr_->cellcount)); + } + int order() const { + return ptr_->order; + } + void set_order(int value) { + ptr_->order = value; + } mjStringVec &nodebody() const { return *(ptr_->nodebody); } @@ -4640,6 +4649,9 @@ struct MjModel { emscripten::val flex_interp() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nflex, ptr_->flex_interp)); } + emscripten::val flex_cellnum() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nflex * 3, ptr_->flex_cellnum)); + } emscripten::val flex_nodeadr() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nflex, ptr_->flex_nodeadr)); } @@ -11806,6 +11818,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("flex_bending", &MjModel::flex_bending) .property("flex_bvhadr", &MjModel::flex_bvhadr) .property("flex_bvhnum", &MjModel::flex_bvhnum) + .property("flex_cellnum", &MjModel::flex_cellnum) .property("flex_centered", &MjModel::flex_centered) .property("flex_conaffinity", &MjModel::flex_conaffinity) .property("flex_condim", &MjModel::flex_condim) @@ -12569,6 +12582,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("info", &MjsExclude::info, &MjsExclude::set_info, reference()); emscripten::class_("MjsFlex") .property("activelayers", &MjsFlex::activelayers, &MjsFlex::set_activelayers, reference()) + .property("cellcount", &MjsFlex::cellcount) .property("conaffinity", &MjsFlex::conaffinity, &MjsFlex::set_conaffinity, reference()) .property("condim", &MjsFlex::condim, &MjsFlex::set_condim, reference()) .property("contype", &MjsFlex::contype, &MjsFlex::set_contype, reference()) @@ -12590,6 +12604,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("material", &MjsFlex::material, &MjsFlex::set_material, reference()) .property("node", &MjsFlex::node, reference()) .property("nodebody", &MjsFlex::nodebody, reference()) + .property("order", &MjsFlex::order, &MjsFlex::set_order, reference()) .property("passive", &MjsFlex::passive, &MjsFlex::set_passive, reference()) .property("poisson", &MjsFlex::poisson, &MjsFlex::set_poisson, reference()) .property("priority", &MjsFlex::priority, &MjsFlex::set_priority, reference()) From b16383dfaf1207cf731d0ab3e0106b0aa94cfaa4 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 17 Apr 2026 07:59:34 -0700 Subject: [PATCH 084/251] Use banded solver for implicit flex integration. The flex interpolation stiffness matrix within the implicit/implicitfast solvers is now built and factorized in a banded format instead of a dense one. This involves: - Calculating the bandwidth based on the sparsity of the mass/damping matrix and the connectivity within flex cells. - Allocating and populating a banded matrix `H`. - Using `mju_cholFactorBand` and `mju_cholSolveBand` for factorization and solving. This change improves performance for flexes with many DOFs but local coupling. PiperOrigin-RevId: 901297952 Change-Id: I3efe06353d1903ea65ab30dc49685cede228bb68 --- doc/includes/references.h | 1 + include/mujoco/mjmodel.h | 1 + include/mujoco/mjxmacro.h | 1 + python/mujoco/introspect/structs.py | 8 ++ src/engine/engine_derivative.c | 25 ++--- src/engine/engine_derivative.h | 2 +- src/engine/engine_forward.c | 55 ++++++++--- src/engine/engine_setconst.c | 130 ++++++++++++++++++++++++++ src/user/user_mesh.cc | 3 + test/engine/engine_derivative_test.cc | 20 +++- test/user/user_flex_test.cc | 21 +++++ unity/Runtime/Bindings/MjBindings.cs | 1 + wasm/codegen/generated/bindings.cc | 4 + 13 files changed, 239 insertions(+), 33 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 614a7516..0ee5a22b 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1315,6 +1315,7 @@ struct mjModel_ { int* flex_matid; // material id for rendering (nflex x 1) int* flex_group; // group for visibility (nflex x 1) int* flex_interp; // interpolation (0: vertex, 1: nodes) (nflex x 1) + int* flex_bandwidth; // precomputed solver bandwidth (nflex x 1) int* flex_cellnum; // finite cell num per dimension (nflex x 3) int* flex_nodeadr; // first node address (nflex x 1) int* flex_nodenum; // number of nodes (nflex x 1) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 9b39cbb6..fba680f9 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -978,6 +978,7 @@ struct mjModel_ { int* flex_matid; // material id for rendering (nflex x 1) int* flex_group; // group for visibility (nflex x 1) int* flex_interp; // interpolation (0: vertex, 1: nodes) (nflex x 1) + int* flex_bandwidth; // precomputed solver bandwidth (nflex x 1) int* flex_cellnum; // finite cell num per dimension (nflex x 3) int* flex_nodeadr; // first node address (nflex x 1) int* flex_nodenum; // number of nodes (nflex x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 87a95a31..4efaf94b 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -454,6 +454,7 @@ X ( int, flex_matid, nflex, 1 ) \ X ( int, flex_group, nflex, 1 ) \ X ( int, flex_interp, nflex, 1 ) \ + X ( int, flex_bandwidth, nflex, 1 ) \ X ( int, flex_cellnum, nflex, 3 ) \ X ( int, flex_nodeadr, nflex, 1 ) \ X ( int, flex_nodenum, nflex, 1 ) \ diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 14a44f32..9bd45295 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -2668,6 +2668,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='interpolation (0: vertex, 1: nodes)', array_extent=('nflex',), ), + StructFieldDecl( + name='flex_bandwidth', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='precomputed solver bandwidth', + array_extent=('nflex',), + ), StructFieldDecl( name='flex_cellnum', type=PointerType( diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 1eb4df9d..8a7eabc7 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -872,12 +872,12 @@ typedef enum { // shared kernel for flex interpolation derivatives, scale = s1 + s2*damping // op: operation type (VEC, or ADDH) -// res: output vector (VEC) or dense H matrix (ADDH) +// res: output vector (VEC) or banded H matrix (ADDH) // vec: input vector for VEC operation, NULL otherwise -// dof_indices, ndof: DOF mapping for ADDH, ignored otherwise +// dof_indices, ndof, nband: DOF mapping and band width for ADDH, ignored otherwise static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, mjtNum* res, const mjtNum* vec, mjtNum s1, mjtNum s2, - const int* dof_indices, int ndof) { + const int* dof_indices, int ndof, int nband) { int nv = m->nv; // build global2local map for ADDH @@ -1021,7 +1021,7 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, J_val, K_rot_cell, dim_c); } else if (op == mjFLEXOP_ADDH) { mj_markStack(d); - // H -= J_cell^T * K_rot_cell * J_cell + // H -= J_cell^T * K_rot_cell * J_cell (banded format) mjtNum* J_reduced = mjSTACKALLOC(d, dim_c*ndof, mjtNum); mju_zero(J_reduced, dim_c*ndof); @@ -1041,14 +1041,14 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, mjtNum* KJ = mjSTACKALLOC(d, dim_c*ndof, mjtNum); mju_mulMatMat(KJ, K_rot_cell, J_reduced, dim_c, dim_c, ndof); - // H[i,j] -= J_reduced[k,i] * KJ[k,j] + // H[i,j] -= J_reduced[k,i] * KJ[k,j], store lower triangle in banded format for (int i = 0; i < ndof; i++) { - for (int j = 0; j < ndof; j++) { + for (int j = mjMAX(0, i-nband+1); j <= i; j++) { mjtNum val = 0; for (int dim_idx = 0; dim_idx < dim_c; dim_idx++) { val += J_reduced[dim_idx*ndof + i] * KJ[dim_idx*ndof + j]; } - res[i*ndof + j] -= val; + res[i*nband + nband-1-(i-j)] -= val; } } mj_freeStack(d); @@ -1072,15 +1072,16 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, // compute res += (h^2 + h*damping) * J'*K*J * vec, for all interpolated flexes void mjd_flexInterp_mulKD(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h) { // s1=h*h, s2=h => scale = h*h + h*damping - mjd_flexInterp_kernel(m, d, mjFLEXOP_VEC, res, vec, h * h, h, NULL, 0); + mjd_flexInterp_kernel(m, d, mjFLEXOP_VEC, res, vec, h * h, h, NULL, 0, 0); } -// add (h^2 + h*damping) * J'*K*J to dense matrix H, for all interpolated flexes -// H: dense ndof x ndof matrix +// add (h^2 + h*damping) * J'*K*J to banded matrix H, for all interpolated flexes +// H: banded ndof x nband matrix (lower triangle, band storage) // dof_indices: maps local indices to global DOFs -void mjd_flexInterp_addH(const mjModel* m, mjData* d, mjtNum* H, const int* dof_indices, int ndof, mjtNum h) { - mjd_flexInterp_kernel(m, d, mjFLEXOP_ADDH, H, NULL, h * h, h, dof_indices, ndof); +void mjd_flexInterp_addH(const mjModel* m, mjData* d, mjtNum* H, const int* dof_indices, + int ndof, int nband, mjtNum h) { + mjd_flexInterp_kernel(m, d, mjFLEXOP_ADDH, H, NULL, h * h, h, dof_indices, ndof, nband); } diff --git a/src/engine/engine_derivative.h b/src/engine/engine_derivative.h index af2aef4c..65a136f5 100644 --- a/src/engine/engine_derivative.h +++ b/src/engine/engine_derivative.h @@ -49,7 +49,7 @@ MJAPI void mjd_flexInterp_mulKD(const mjModel* m, mjData* d, mjtNum* res, const // assemble flex stiffness matrix H_flex: H += h*h*K + h*D // H is a dense matrix of size ndof x ndof, dof_indices maps local rows/cols to global DOFs -MJAPI void mjd_flexInterp_addH(const mjModel* m, mjData* d, mjtNum* H, const int* dof_indices, int ndof, mjtNum h); +MJAPI void mjd_flexInterp_addH(const mjModel* m, mjData* d, mjtNum* H, const int* dof_indices, int ndof, int nband, mjtNum h); #ifdef __cplusplus diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index b0d15b16..dbd3a74e 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -1350,11 +1350,12 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { } -// context for flex interp reduced dense factorization/solve +// context for flex interp reduced banded factorization/solve typedef struct { - mjtNum* H; // dense Cholesky-factored matrix (ndof x ndof) + mjtNum* H; // banded Cholesky-factored matrix (ndof x nband) int* dof_indices; // global DOF index for each local flex DOF int ndof; // number of flex DOFs + int nband; // half-bandwidth + 1 (number of band columns) int ncoupling; // number of off-diagonal coupling terms mjtNum* coupling_val; // coupling coefficient values int* coupling_row; // local flex row index for each coupling term @@ -1391,7 +1392,7 @@ static void flexInterp_collect(const mjModel* m, int f, } -// build and factor the reduced dense matrix for flex interp DOFs +// build and factor the reduced banded matrix for flex interp DOFs // mark/free stack handled by caller static FlexInterpContext flexInterp_factor(const mjModel* m, mjData* d, int nv) { FlexInterpContext ctx = {0}; @@ -1456,19 +1457,36 @@ static FlexInterpContext flexInterp_factor(const mjModel* m, mjData* d, int nv) const int* colind = implicit ? m->D_colind : m->M_colind; const mjtNum* source = implicit ? d->qLU : d->qH; - // count coupling terms (off-diagonal: flex row, non-flex col) + // get precomputed bandwidth + int bandwidth = 0; + for (int f=0; f < m->nflex; f++) { + if (m->flex_interp[f]) { + if (m->flex_bandwidth[f] > bandwidth) { + bandwidth = m->flex_bandwidth[f]; + } + } + } + + // compute ncoupling from sparse matrix entries int ncoupling = 0; for (int i=0; i < ndof; i++) { int row = dof_indices[i]; int start = rowadr[row]; int end = start + rownnz[row]; for (int k=start; k < end; k++) { - if (global2local[colind[k]] < 0) { + int local_j = global2local[colind[k]]; + if (local_j < 0) { ncoupling++; } } } + // nband = bandwidth + 1 (includes diagonal) + int nband = bandwidth + 1; + + // cap nband at ndof (dense fallback for small systems) + if (nband > ndof) nband = ndof; + // allocate coupling storage mjtNum* coupling_val = NULL; int* coupling_row = NULL; @@ -1479,9 +1497,9 @@ static FlexInterpContext flexInterp_factor(const mjModel* m, mjData* d, int nv) coupling_col = mjSTACKALLOC(d, ncoupling, int); } - // build H_flex (dense) from qLU (implicit) or qH (implicitfast) - mjtNum* H = mjSTACKALLOC(d, ndof*ndof, mjtNum); - mju_zero(H, ndof*ndof); + // build H_flex (banded) from qLU (implicit) or qH (implicitfast) + mjtNum* H = mjSTACKALLOC(d, ndof*nband, mjtNum); + mju_zero(H, ndof*nband); int coup_cnt = 0; for (int i=0; i < ndof; i++) { @@ -1492,7 +1510,13 @@ static FlexInterpContext flexInterp_factor(const mjModel* m, mjData* d, int nv) int col = colind[k]; int local_j = global2local[col]; if (local_j >= 0) { - H[i*ndof+local_j] = source[k]; + // store lower triangle only: row i, col local_j, where i >= local_j + if (i >= local_j) { + H[i*nband + nband-1-(i-local_j)] = source[k]; + } else { + // upper triangle entry: store symmetrically in lower triangle + H[local_j*nband + nband-1-(local_j-i)] = source[k]; + } } else if (coup_cnt < ncoupling) { coupling_val[coup_cnt] = source[k]; coupling_row[coup_cnt] = i; @@ -1502,14 +1526,15 @@ static FlexInterpContext flexInterp_factor(const mjModel* m, mjData* d, int nv) } } - // add flex stiffness and factorize - mjd_flexInterp_addH(m, d, H, dof_indices, ndof, m->opt.timestep); - mju_cholFactor(H, ndof, mjMINVAL); + // add flex stiffness in banded format and factorize + mjd_flexInterp_addH(m, d, H, dof_indices, ndof, nband, m->opt.timestep); + mju_cholFactorBand(H, ndof, nband, 0, 0, 0); // store results in context ctx.H = H; ctx.dof_indices = dof_indices; ctx.ndof = ndof; + ctx.nband = nband; ctx.ncoupling = ncoupling; ctx.coupling_val = coupling_val; ctx.coupling_row = coupling_row; @@ -1518,7 +1543,7 @@ static FlexInterpContext flexInterp_factor(const mjModel* m, mjData* d, int nv) } -// solve the reduced dense system for flex interp DOFs, overwrite qacc +// solve the reduced banded system for flex interp DOFs, overwrite qacc static void flexInterp_solve(const mjModel* m, mjData* d, const FlexInterpContext* ctx, mjtNum* qacc, const mjtNum* qfrc, int nv) { int ndof = ctx->ndof; @@ -1544,8 +1569,8 @@ static void flexInterp_solve(const mjModel* m, mjData* d, const FlexInterpContex qfrc_flex[ctx->coupling_row[k]] -= ctx->coupling_val[k] * qacc[ctx->coupling_col[k]]; } - // solve and scatter back - mju_cholSolve(qfrc_flex, ctx->H, qfrc_flex, ndof); + // solve with banded Cholesky and scatter back + mju_cholSolveBand(qfrc_flex, ctx->H, qfrc_flex, ndof, ctx->nband, 0); mju_scatter(qacc, qfrc_flex, ctx->dof_indices, ndof); } diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index e0ffb480..24ef9955 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -636,6 +636,135 @@ static void makeFlexSparse(mjModel* m, mjData* d) { mj_freeStack(d); } +// compute flex bandwidth for trilinear interpolation +static void makeFlexBandwidth(mjModel* m, mjData* d) { + if (!m->nflex) { + return; + } + + mj_markStack(d); + int* chain_dofs = mjSTACKALLOC(d, m->nv, int); + int* seen_dof = mjSTACKALLOC(d, m->nv, int); + int* dof_indices = mjSTACKALLOC(d, m->nv, int); + int* global2local = mjSTACKALLOC(d, m->nv, int); + + mju_zeroInt(seen_dof, m->nv); + for (int i = 0; i < m->nv; i++) { + global2local[i] = -1; + } + + int ndof = 0; + for (int f = 0; f < m->nflex; f++) { + if (m->flex_interp[f]) { + int nodenum = m->flex_nodenum[f]; + int nodeadr = m->flex_nodeadr[f]; + for (int n = 0; n < nodenum; n++) { + int b = m->flex_nodebodyid[nodeadr + n]; + // only the body's own DOFs enter the reduced banded flex system; + // ancestor DOFs are solved by the global factorization and coupled + // via off-diagonal correction (see flexInterp_solve in engine_forward) + int chain_nnz; + if (m->body_dofnum[b] == 0) { + chain_nnz = mj_bodyChain(m, b, chain_dofs); + } else { + chain_nnz = m->body_dofnum[b]; + for (int j = 0; j < chain_nnz; j++) { + chain_dofs[j] = m->body_dofadr[b] + j; + } + } + for (int i = 0; i < chain_nnz; i++) { + int dof = chain_dofs[i]; + if (!seen_dof[dof]) { + seen_dof[dof] = 1; + dof_indices[ndof] = dof; + global2local[dof] = ndof++; + } + } + } + } + } + + int bandwidth = 0; + if (ndof > 0) { + // check sparse matrix coupling (both D and M) + for (int integrator = 0; integrator < 2; integrator++) { + const int* rownnz = (integrator == 0) ? m->D_rownnz : m->M_rownnz; + const int* rowadr = (integrator == 0) ? m->D_rowadr : m->M_rowadr; + const int* colind = (integrator == 0) ? m->D_colind : m->M_colind; + + // D arrays are only allocated for implicit integrators + if (!rownnz) continue; + + for (int i = 0; i < ndof; i++) { + int row = dof_indices[i]; + int start = rowadr[row]; + int end = start + rownnz[row]; + for (int k = start; k < end; k++) { + int local_j = global2local[colind[k]]; + if (local_j >= 0) { + int diff = i - local_j; + if (diff < 0) diff = -diff; + if (diff > bandwidth) bandwidth = diff; + } + } + } + } + + // check stiffness coupling + for (int f = 0; f < m->nflex; f++) { + if (!m->flex_interp[f]) continue; + int order = m->flex_interp[f]; + int nodeadr = m->flex_nodeadr[f]; + int nodenum = m->flex_nodenum[f]; + int cx = m->flex_cellnum[3*f+0]; + int cy = m->flex_cellnum[3*f+1]; + int cz = m->flex_cellnum[3*f+2]; + int ny = cy * order + 1; + int nz = cz * order + 1; + + for (int icx = 0; icx < cx; icx++) { + for (int icy = 0; icy < cy; icy++) { + for (int icz = 0; icz < cz; icz++) { + int min_local = ndof, max_local = -1; + for (int lx = 0; lx <= order; lx++) { + for (int ly = 0; ly <= order; ly++) { + for (int lz = 0; lz <= order; lz++) { + int gx = icx * order + lx; + int gy = icy * order + ly; + int gz = icz * order + lz; + int node_idx = gx * ny * nz + gy * nz + gz; // non-negative by construction + if (node_idx < nodenum) { + int b = m->flex_nodebodyid[nodeadr + node_idx]; + int chain_nnz = mj_bodyChain(m, b, chain_dofs); + for (int i = 0; i < chain_nnz; i++) { + int dof = chain_dofs[i]; + int local = global2local[dof]; + if (local >= 0) { + if (local < min_local) min_local = local; + if (local > max_local) max_local = local; + } + } + } + } + } + } + if (max_local >= 0 && max_local - min_local > bandwidth) { + bandwidth = max_local - min_local; + } + } + } + } + } + } + + // store bandwidth for all flexes (global max) + for (int f = 0; f < m->nflex; f++) { + m->flex_bandwidth[f] = bandwidth; + } + + mj_freeStack(d); +} + // align 2D flexes to the XY plane static void mj_alignFlex(mjModel* m, mjData* d) { for (int f = 0; f < m->nflex; f++) { @@ -687,6 +816,7 @@ static void mj_alignFlex(mjModel* m, mjData* d) { static void set0(mjModel* m, mjData* d) { makeTendonSparse(m); makeFlexSparse(m, d); + makeFlexBandwidth(m, d); mj_alignFlex(m, d); int nv = m->nv; mjtNum A[36] = {0}, pos[3], quat[4]; diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 51a940a4..244a48f2 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4095,6 +4095,9 @@ void mjCFlex::Compile(const mjVFS* vfs) { // check node compatibility with count and dof if (spec.order > 0) { + if (spec.cellcount[0] == 0 || spec.cellcount[1] == 0 || spec.cellcount[2] == 0) { + throw mjCError(this, "cellcount cannot be 0 in any dimension when interpolation order > 0"); + } int expected_nodes = (spec.cellcount[0] * spec.order + 1) * (spec.cellcount[1] * spec.order + 1) * (spec.cellcount[2] * spec.order + 1); diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index e5978c9b..e0b693d3 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -1473,6 +1473,17 @@ void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name, } } +// Helper: assemble flex stiffness into dense matrix via banded addH +// This wraps the banded API and converts to dense for test verification. +static void addH_dense(mjModel* m, mjData* d, mjtNum* H_dense, + const int* dof_indices, int ndof, mjtNum h) { + // use full bandwidth (ndof) for exact dense equivalence + std::vector H_band(ndof * ndof, 0); + mjd_flexInterp_addH(m, d, H_band.data(), dof_indices, ndof, ndof, h); + // convert banded to dense (lower triangle), then symmetrize + mju_band2Dense(H_dense, H_band.data(), ndof, ndof, 0, 1); +} + // compare analytic and fin-diff d_qfrc_passive/d_qvel for flex interp // Combined test for verify mjd_flexInterp_mulK (stiffness) and damping TEST_F(DerivativeTest, FlexInterpDerivatives) { @@ -1525,7 +1536,7 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) { for (int i = 0; i < nv; i++) dof_indices[i] = i; // assemble K into H - mjd_flexInterp_addH(model, data, H.data(), dof_indices.data(), nv, 1.0); + addH_dense(model, data, H.data(), dof_indices.data(), nv, 1.0); // restore damping model->flex_damping[0] = save_damping; @@ -1618,10 +1629,10 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) { for (int i = 0; i < nv; i++) dof_indices[i] = i; vector H1(nv * nv, 0); - mjd_flexInterp_addH(model, data, H1.data(), dof_indices.data(), nv, 1.0); + addH_dense(model, data, H1.data(), dof_indices.data(), nv, 1.0); vector H2(nv * nv, 0); - mjd_flexInterp_addH(model, data, H2.data(), dof_indices.data(), nv, 0.5); + addH_dense(model, data, H2.data(), dof_indices.data(), nv, 0.5); vector D(nv * nv); for (int i = 0; i < nv * nv; i++) { @@ -1695,8 +1706,7 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) { for (int i = 0; i < nv; i++) dof_indices[i] = i; // h=1, damping=0 => adds K to H - mjd_flexInterp_addH(model, data, H_approx.data(), dof_indices.data(), nv, - 1.0); + addH_dense(model, data, H_approx.data(), dof_indices.data(), nv, 1.0); // 2. Compute Finite Difference Jacobian (Ground Truth) // qfrc_passive = -dV/dq diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index 478b23e0..5482568d 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -79,6 +79,27 @@ TEST_F(UserFlexTest, CountTooSmall) { EXPECT_THAT(error.data(), HasSubstr("Count too small")); } +TEST_F(UserFlexTest, CellnumZeroInterpolated) { + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(m, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("cellcount cannot be 0")); +} + TEST_F(UserFlexTest, SpacingGreaterThanGeometry) { static constexpr char xml[] = R"( diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index d7d4c47d..1673c4d0 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -1200,6 +1200,7 @@ public unsafe struct mjModel_ { public int* flex_matid; public int* flex_group; public int* flex_interp; + public int* flex_bandwidth; public int* flex_cellnum; public int* flex_nodeadr; public int* flex_nodenum; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 80fb63d5..185f5bb7 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -4649,6 +4649,9 @@ struct MjModel { emscripten::val flex_interp() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nflex, ptr_->flex_interp)); } + emscripten::val flex_bandwidth() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nflex, ptr_->flex_bandwidth)); + } emscripten::val flex_cellnum() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nflex * 3, ptr_->flex_cellnum)); } @@ -11815,6 +11818,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("eq_type", &MjModel::eq_type) .property("exclude_signature", &MjModel::exclude_signature) .property("flex_activelayers", &MjModel::flex_activelayers) + .property("flex_bandwidth", &MjModel::flex_bandwidth) .property("flex_bending", &MjModel::flex_bending) .property("flex_bvhadr", &MjModel::flex_bvhadr) .property("flex_bvhnum", &MjModel::flex_bvhnum) From a9e61966b5a38a20ff1ba714e6630ebd6c9c1f7c Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Fri, 17 Apr 2026 11:21:47 -0700 Subject: [PATCH 085/251] Import of Warp v1.12.1 from https://github.com/nvidia/warp. PiperOrigin-RevId: 901388190 Change-Id: Id2c5f1bc2ba9050af2c3b7597d84b9525ffa5766 --- mjx/cuda_requirements.txt | 10 +++++----- .../warp/_src/jax_experimental/__init__.py | 12 ------------ .../warp/_src/jax_experimental/custom_call.py | 12 ------------ .../warp/_src/jax_experimental/ffi.py | 16 ++-------------- .../warp/_src/jax_experimental/xla_ffi.py | 12 ------------ mjx/pyproject.toml | 2 +- 6 files changed, 8 insertions(+), 56 deletions(-) diff --git a/mjx/cuda_requirements.txt b/mjx/cuda_requirements.txt index 2ced5d12..c8de6c4a 100644 --- a/mjx/cuda_requirements.txt +++ b/mjx/cuda_requirements.txt @@ -16,8 +16,8 @@ jax-cuda12-pjrt==0.5.3; python_version >= '3.10' \ jax-cuda12-pjrt==0.4.30; python_version == '3.9' \ --hash=sha256:895d0198ad99638fcaf976c47592e2a543eef79ea15fabd24a402d055390c328 \ --hash=sha256:c36fb1e0c236563bf3a87e70f4d1ab28a31d7cf5d722c9ede30c4172116e8bcb -warp-lang==1.12.0 \ - --hash=sha256:c78c3701d5cad86c30ef5017410d294ec46a396bb0d502ee1c98743494f3a62f \ - --hash=sha256:a1436f60a1881cd94f787e751a83fc0987626be2d3e2b4e74c64a6947c6d1266 \ - --hash=sha256:a2d6decba693aba5b828573c4414fd6a3f4c4a934db9c322736ef2b3fa99fe76 \ - --hash=sha256:697248edd2f1e2952f50e3db33b214af76173641a8894aacc467bed6dc247f8a +warp-lang==1.12.1 \ + --hash=sha256:98df3533a6c40a33cce961f8efa991006b30c9d286356e4cd77ea8ce86928f1d \ + --hash=sha256:6bf01f10509488ba8eacaf4ec7fcf7cfbd503118b22e002ecba407b40a17424e \ + --hash=sha256:af6d680e79c1be6e46ddf80ecaa358f222804f882f4683260a7b4abd80a0981b \ + --hash=sha256:826b2f93df8e47eac0c751a8eb5a0533e2fc5434158c8896a63be53bfbd728c7 diff --git a/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/__init__.py b/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/__init__.py index 3159bfe6..1a8431c3 100644 --- a/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/__init__.py +++ b/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/__init__.py @@ -1,14 +1,2 @@ # SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. diff --git a/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/custom_call.py b/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/custom_call.py index 0adf6435..dba3f715 100644 --- a/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/custom_call.py +++ b/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/custom_call.py @@ -1,17 +1,5 @@ # SPDX-FileCopyrightText: Copyright (c) 2024 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. import ctypes from functools import reduce diff --git a/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/ffi.py b/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/ffi.py index e9fe408f..c3f9e6e9 100644 --- a/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/ffi.py +++ b/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/ffi.py @@ -1,17 +1,5 @@ # SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. from __future__ import annotations @@ -223,7 +211,7 @@ class FfiKernel: # register the callback FFI_CCALLFUNC = ctypes.CFUNCTYPE(ctypes.c_void_p, ctypes.POINTER(XLA_FFI_CallFrame)) - self.callback_func = FFI_CCALLFUNC(lambda call_frame: self.ffi_callback(call_frame)) + self.callback_func = FFI_CCALLFUNC(self.ffi_callback) ffi_ccall_address = ctypes.cast(self.callback_func, ctypes.c_void_p) ffi_capsule = jax.ffi.pycapsule(ffi_ccall_address.value) jax.ffi.register_ffi_target(self.name, ffi_capsule, platform="CUDA") @@ -606,7 +594,7 @@ class FfiCallable: # register the callback FFI_CCALLFUNC = ctypes.CFUNCTYPE(ctypes.c_void_p, ctypes.POINTER(XLA_FFI_CallFrame)) - self.callback_func = FFI_CCALLFUNC(lambda call_frame: self.ffi_callback(call_frame)) + self.callback_func = FFI_CCALLFUNC(self.ffi_callback) ffi_ccall_address = ctypes.cast(self.callback_func, ctypes.c_void_p) ffi_capsule = jax.ffi.pycapsule(ffi_ccall_address.value) jax.ffi.register_ffi_target(self.name, ffi_capsule, platform="CUDA") diff --git a/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/xla_ffi.py b/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/xla_ffi.py index 2da12c4e..911ca2f1 100644 --- a/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/xla_ffi.py +++ b/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/xla_ffi.py @@ -1,17 +1,5 @@ # SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. import ctypes import enum diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index fb3c5a5d..e6a48dec 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -36,7 +36,7 @@ dependencies = [ [project.optional-dependencies] warp = [ - "warp-lang==1.12.0", + "warp-lang==1.12.1", ] [project.scripts] From 3d45a33190641bfef58724a21e6bba06613cc608 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Sun, 19 Apr 2026 07:20:49 -0700 Subject: [PATCH 086/251] Refactor flex strain constraints to be per-cell. Each mjEQ_FLEXSTRAIN equality now represents a single cell within a flex. This allows for more efficient sparse Jacobian computation by only considering the degrees of freedom of the nodes within each specific cell. This change gives a speedup of about 10x on a 3x3x3 model. PiperOrigin-RevId: 902164069 Change-Id: I78eedf1d5cf39b8989fe9863c22d164922fc0efb --- doc/XMLreference.rst | 5 +- doc/XMLschema.rst | 3 + doc/changelog.rst | 3 + src/engine/engine_core_constraint.c | 489 ++++++++++++++-------------- src/user/user_flexcomp.cc | 35 +- src/xml/xml_native_reader.cc | 6 +- src/xml/xml_native_writer.cc | 4 + 7 files changed, 289 insertions(+), 256 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 9a8eba6c..7e5cfe5a 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4888,8 +4888,11 @@ constraint type is only supported for dimension 3 flexes with trilinear or quadr :at:`flex`: :at-val:`string, required` Name of the flex whose strain is being constrained. +.. _equality-flexstrain-cell: - +:at:`cell`: :at-val:`int(3), optional` + 3D grid index (i, j, k) identifying the cell in the flex object. The grid size is specified in the :ref:`cellcount + ` attribute. .. _tendon: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 60d828be..f394f2ac 100755 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -2062,6 +2062,9 @@ .. grid-item:: :ref:`flex` + .. grid-item:: + :ref:`cell` + .. grid-item:: :ref:`active` diff --git a/doc/changelog.rst b/doc/changelog.rst index b073a3f2..218bec89 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -10,6 +10,9 @@ General - Added :ref:`multi-cell support` for trilinear and quadratic flexes. Note that the implicit integrator uses a dense solver for the flex degrees of freedom, which can be slow for multi-cell flexes. +- Refactored ``flexstrain`` equality constraints to be instantiated per cell instead of per flex object, reducing the + number of degrees of freedom per constraint row. The equality can be associated with a specific cell with the new + attribute ":ref:`cell ` Version 3.7.0 (April 14, 2026) ------------------------------ diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 41ca6a39..d8c81bda 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -88,75 +88,79 @@ static mjtNum mat3_det(const mjtNum* mat) { } -// compute node positions and Jacobians for flex strain constraints -// xpos: output array of size 3*nodenum (global node positions) -// node_jac: output array of size 3*nodenum*nv (dense Jacobians) -// combined_chain: output array of DOF indices used by any node (sparse mode) -// combined_nnz: output number of entries in combined_chain -static void node_pos_and_jac(const mjModel* m, mjData* d, int f, int nv, int issparse, mjtNum* xpos, - mjtNum* node_jac, int* combined_chain, int* combined_nnz) { - int nodenum = m->flex_nodenum[f]; - int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; - int nstart = m->flex_nodeadr[f]; +// compute cell node Jacobians and combined chain for flex strain constraints +// npc: number of nodes per cell +// gindices: global indices of cell nodes in flex +// cell_node_jac: output array of size 3*npc*cell_nnz (allocated on stack) +// mj_{mark/free}Stack in calling function +static mjtNum* cell_pos_and_jac(const mjModel* m, mjData* d, int flex_id, int npc, const int* gindices, + int nv, const mjtNum* xpos_c, int* cell_chain, int* cell_nnz) { + int* nstart = m->flex_nodeadr + flex_id; + int* bodyid = m->flex_nodebodyid + *nstart; - for (int n = 0; n < nodenum; n++) { - if (m->flex_centered[f]) { - mju_copy3(xpos + 3*n, d->xpos + 3*bodyid[n]); - } else { - mju_mulMatVec3(xpos + 3*n, d->xmat + 9*bodyid[n], m->flex_node + 3*(n + nstart)); - mju_addTo3(xpos + 3*n, d->xpos + 3*bodyid[n]); + // build per-cell sparse chain: union of bodyChain for npc nodes + *cell_nnz = 0; + int* dof_used = mjSTACKALLOC(d, nv, int); + int* temp_chain = mjSTACKALLOC(d, nv, int); + mju_zeroInt(dof_used, nv); + for (int n = 0; n < npc; n++) { + int temp_nnz = mj_bodyChain(m, bodyid[gindices[n]], temp_chain); + for (int k = 0; k < temp_nnz; k++) { + dof_used[temp_chain[k]] = 1; + } + } + for (int q = 0; q < nv; q++) { + if (dof_used[q]) { + cell_chain[(*cell_nnz)++] = q; } } + // build per-cell node Jacobians: 3*npc x cell_nnz + mjtNum* cell_node_jac = mjSTACKALLOC(d, 3*npc*(*cell_nnz), mjtNum); + mju_zero(cell_node_jac, 3*npc*(*cell_nnz)); int* chain_col = mjSTACKALLOC(d, nv, int); mjtNum* blk_jac = mjSTACKALLOC(d, 3*nv, mjtNum); - mju_zero(node_jac, 3*nodenum*nv); - - for (int n = 0; n < nodenum; n++) { - int chain_nnz = mj_bodyChain(m, bodyid[n], chain_col); - mju_zero(blk_jac, 3*nv); - mj_jacSparse(m, d, blk_jac, NULL, xpos + 3*n, bodyid[n], chain_nnz, chain_col, 0); - + for (int n = 0; n < npc; n++) { + int body = bodyid[gindices[n]]; + int chain_n = mj_bodyChain(m, body, chain_col); + mju_zero(blk_jac, 3*chain_n); + mj_jacSparse(m, d, blk_jac, NULL, xpos_c + 3*n, + body, chain_n, chain_col, 0); + // map node's sparse chain into cell_chain indexing for (int r = 0; r < 3; r++) { - for (int k = 0; k < chain_nnz; k++) { - node_jac[(3*n + r)*nv + chain_col[k]] = blk_jac[r*chain_nnz + k]; + for (int k = 0; k < chain_n; k++) { + // find chain_col[k] in cell_chain via linear scan (chain is short) + for (int cc = 0; cc < *cell_nnz; cc++) { + if (cell_chain[cc] == chain_col[k]) { + cell_node_jac[(3*n + r)*(*cell_nnz) + cc] = blk_jac[r*chain_n + k]; + break; + } + } } } } - *combined_nnz = 0; - if (issparse) { - int* dof_used = mjSTACKALLOC(d, nv, int); - mju_zeroInt(dof_used, nv); - for (int n = 0; n < nodenum; n++) { - int temp_chain[200]; - int temp_nnz = mj_bodyChain(m, bodyid[n], temp_chain); - for (int k = 0; k < temp_nnz; k++) { - dof_used[temp_chain[k]] = 1; - } - } - - for (int q = 0; q < nv; q++) { - if (dof_used[q]) { - combined_chain[(*combined_nnz)++] = q; - } - } - } + return cell_node_jac; } -// compute strain Jacobian from strain derivative w.r.t. node positions -// dSdx: input array of size 3*nodenum (dStrain/dNodePosition) -// node_jac: input array of size 3*nodenum*nv (dense Jacobians) -// strain_jac: output array of size nv (dStrain/dq) -static void strain_jacobian(int nodenum, int nv, const mjtNum* dSdx, const mjtNum* node_jac, - mjtNum* strain_jac) { - mju_zero(strain_jac, nv); - for (int n = 0; n < nodenum; n++) { + +// compute strain Jacobian from strain derivative w.r.t. cell-local node positions +// dSdx_local: input array of size 3*npc (dStrain/dNodePosition for cell nodes) +// cell_node_jac: input array of size 3*npc*cell_nnz (sparse Jacobians) +// strain_jac: output array of size cell_nnz (dStrain/dq) +static void cell_strain_jacobian(int npc, int cell_nnz, + const mjtNum* dSdx_local, + const mjtNum* cell_node_jac, + mjtNum* strain_jac) { + mju_zero(strain_jac, cell_nnz); + for (int n = 0; n < npc; n++) { for (int c = 0; c < 3; c++) { + mjtNum w = dSdx_local[3*n + c]; + if (w == 0) continue; int row = 3*n + c; - for (int q = 0; q < nv; q++) { - strain_jac[q] += dSdx[row] * node_jac[row*nv + q]; + for (int k = 0; k < cell_nnz; k++) { + strain_jac[k] += w * cell_node_jac[row*cell_nnz + k]; } } } @@ -872,6 +876,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { break; case mjEQ_FLEXSTRAIN: { + // each constraint represents a single cell; cell index in eq_data int f = id[0]; int nodenum = m->flex_nodenum[f]; int order = m->flex_interp[f]; @@ -881,27 +886,64 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { break; } + // only order 1 (trilinear) and 2 (quadratic) are supported + if (order > 2) { + mjERROR("flex strain constraints only support order 1 and 2, got %d", order); + } + int npc = (order+1)*(order+1)*(order+1); - int cx = m->flex_cellnum[3*f+0]; int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; + int nstart = m->flex_nodeadr[f]; + int* bodyid = m->flex_nodebodyid + nstart; + + // read cell index from eq_data + int ci = (int)data[0]; + int cj = (int)data[1]; + int ck = (int)data[2]; - // allocate stack for node positions and Jacobians mj_markStack(d); - mjtNum* xpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); - mjtNum* node_jac = mjSTACKALLOC(d, 3*nodenum*nv, mjtNum); - int* combined_chain = mjSTACKALLOC(d, nv, int); - mjtNum* strain_jac = mjSTACKALLOC(d, nv, mjtNum); - int combined_nnz = 0; - node_pos_and_jac(m, d, f, nv, issparse, xpos, node_jac, combined_chain, &combined_nnz); + // get cell node indices + int gindices[125]; // max npc = 125 for quadratic + mju_flexGatherCellState(order, cy, cz, ci, cj, ck, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + + // compute positions only for cell nodes (npc << nodenum) + mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); + mjtNum* refpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); + for (int n = 0; n < npc; n++) { + int gn = gindices[n]; + if (m->flex_centered[f]) { + mju_copy3(xpos_c + 3*n, d->xpos + 3*bodyid[gn]); + } else { + mju_mulMatVec3(xpos_c + 3*n, d->xmat + 9*bodyid[gn], m->flex_node + 3*(gn + nstart)); + mju_addTo3(xpos_c + 3*n, d->xpos + 3*bodyid[gn]); + } + mju_copy3(refpos_c + 3*n, m->flex_node0 + 3*(gn + nstart)); + } + + // build per-cell sparse chain and node Jacobians + int* cell_chain = mjSTACKALLOC(d, nv, int); + int cell_nnz = 0; + mjtNum* cell_node_jac = cell_pos_and_jac(m, d, f, npc, gindices, nv, xpos_c, cell_chain, + &cell_nnz); + + + mjtNum* strain_jac = mjSTACKALLOC(d, cell_nnz, mjtNum); + mjtNum* dSdx_local = mjSTACKALLOC(d, 3*npc, mjtNum); + + // for dense mode: allocate and zero a dense Jacobian buffer once + mjtNum* dense_jac = NULL; + if (!issparse) { + dense_jac = mjSTACKALLOC(d, nv, mjtNum); + mju_zero(dense_jac, nv); + } // Gauss-Legendre quadrature points in [0,1]^3 - // order=1: 2x2x2=8 points, order=2: 3x3x3=27 points int nquad = order + 1; int ngauss = nquad * nquad * nquad; - // 1D Gauss points mjtNum gp1d[3]; if (nquad == 2) { gp1d[0] = 0.5 - 0.5/mju_sqrt(3.0); @@ -912,8 +954,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { gp1d[2] = 0.5 + 0.5*mju_sqrt(0.6); } - // build 3D Gauss points array (max 27 points) - mjtNum gauss[27][3]; + mjtNum (*gauss)[3] = (mjtNum (*)[3])mjSTACKALLOC(d, 3*ngauss, mjtNum); for (int gi = 0; gi < nquad; gi++) { for (int gj = 0; gj < nquad; gj++) { for (int gk = 0; gk < nquad; gk++) { @@ -925,163 +966,113 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { } } - // reference positions for all nodes - int nstart = m->flex_nodeadr[f]; - mjtNum* refpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); - for (int n = 0; n < nodenum; n++) { - mju_copy3(refpos + 3*n, m->flex_node0 + 3*(n + nstart)); + // B-bar: center-point volumetric constraints (trilinear) + if (order == 1) { + mjtNum center[3] = {0.5, 0.5, 0.5}; + mjtNum Fcur_c[9], Fref_c[9], Fref_inv_c[9], F_c[9]; + + mju_defGradient(Fcur_c, center, xpos_c, order); + mju_defGradient(Fref_c, center, refpos_c, order); + mat3_inverse(Fref_c, Fref_inv_c); + mju_mulMatMat3(F_c, Fcur_c, Fref_inv_c); + + mjtNum C_c[9], E_c[9]; + mju_mulMatTMat3(C_c, F_c, F_c); + mju_scl(E_c, C_c, 0.5, 9); + E_c[0] -= 0.5; E_c[4] -= 0.5; E_c[8] -= 0.5; + + mjtNum I1_c = E_c[0] + E_c[4] + E_c[8]; + mjtNum J_c = mat3_det(F_c); + + mjtNum grad_c[8][3]; + shape_gradients(order, center, grad_c); + + for (int inv = 0; inv < 2; inv++) { + cpos[0] = (inv == 0) ? I1_c : J_c - 1.0; + volumetric_dSdx(inv, npc, grad_c, F_c, Fref_inv_c, dSdx_local); + cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac); + if (issparse) { + mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, + cell_nnz, cell_chain); + } else { + for (int k = 0; k < cell_nnz; k++) { + dense_jac[cell_chain[k]] = strain_jac[k]; + } + mj_addConstraint(m, d, dense_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); + for (int k = 0; k < cell_nnz; k++) { + dense_jac[cell_chain[k]] = 0; + } + } + } } - // per-cell arrays - mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* refpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* dSdx_local = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* dSdx = mjSTACKALLOC(d, 3*nodenum, mjtNum); - int gindices[125]; // max npc = 125 for quadratic + // Gauss integration + for (int g = 0; g < ngauss; g++) { + mjtNum* p = gauss[g]; - // loop over cells - for (int ci = 0; ci < cx; ci++) { - for (int cj = 0; cj < cy; cj++) { - for (int ck = 0; ck < cz; ck++) { - // gather cell-local node positions - mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos, NULL, refpos, xpos_c, NULL, - refpos_c, gindices, NULL); + mjtNum Fcur[9], Fref[9], Fref_inv[9], F[9]; + mju_defGradient(Fcur, p, xpos_c, order); + mju_defGradient(Fref, p, refpos_c, order); + mat3_inverse(Fref, Fref_inv); + mju_mulMatMat3(F, Fcur, Fref_inv); - // B-bar: center-point volumetric constraints (trilinear) - if (order == 1) { - mjtNum center[3] = {0.5, 0.5, 0.5}; - mjtNum Fcur_c[9], Fref_c[9], Fref_inv_c[9], F_c[9]; + mjtNum C[9], E[9]; + mju_mulMatTMat3(C, F, F); + for (int j = 0; j < 9; j++) { + E[j] = 0.5 * C[j]; + } + E[0] -= 0.5; E[4] -= 0.5; E[8] -= 0.5; - mju_defGradient(Fcur_c, center, xpos_c, order); - mju_defGradient(Fref_c, center, refpos_c, order); - mat3_inverse(Fref_c, Fref_inv_c); - mju_mulMatMat3(F_c, Fcur_c, Fref_inv_c); + mjtNum I1 = E[0] + E[4] + E[8]; + mjtNum trE2 = E[0]*E[0] + E[1]*E[3] + E[2]*E[6] + + E[3]*E[1] + E[4]*E[4] + E[5]*E[7] + + E[6]*E[2] + E[7]*E[5] + E[8]*E[8]; + mjtNum I2 = 0.5 * (I1*I1 - trE2); + mjtNum I3 = mat3_det(E); - mjtNum C_c[9], E_c[9]; - mju_mulMatTMat3(C_c, F_c, F_c); - mju_scl(E_c, C_c, 0.5, 9); - E_c[0] -= 0.5; E_c[4] -= 0.5; E_c[8] -= 0.5; + mjtNum (*grad)[3] = (mjtNum (*)[3])mjSTACKALLOC(d, 3*npc, mjtNum); + shape_gradients(order, p, grad); - mjtNum I1_c = E_c[0] + E_c[4] + E_c[8]; - mjtNum J_c = mat3_det(F_c); + for (int s = 0; s < 6; s++) { + if (order == 1 && (s == 0 || s == 1 || s == 2)) { + continue; + } - mjtNum grad_c[8][3]; - shape_gradients(order, center, grad_c); + mjtNum dSdE[9]; + mju_zero(dSdE, 9); - for (int inv = 0; inv < 2; inv++) { - cpos[0] = (inv == 0) ? I1_c : J_c - 1.0; + if (s == 0) { + cpos[0] = I1; + dSdE[0] = dSdE[4] = dSdE[8] = 1.0; + } else if (s == 1) { + cpos[0] = I2; + dSdE[0] = I1-E[0]; dSdE[4] = I1-E[4]; + dSdE[8] = I1-E[8]; + dSdE[1] = -E[1]; dSdE[3] = -E[3]; + dSdE[2] = -E[2]; dSdE[6] = -E[6]; + dSdE[5] = -E[5]; dSdE[7] = -E[7]; + } else if (s == 2) { + cpos[0] = I3; + mat3_cofactor(E, dSdE); + } else { + int offdiag_idx[3] = {1, 2, 5}; + int ij = offdiag_idx[s - 3]; + cpos[0] = E[ij]; + dSdE[ij] = 1.0; + } - // compute local dSdx - volumetric_dSdx(inv, npc, grad_c, F_c, Fref_inv_c, dSdx_local); - - // scatter to global dSdx - mju_zero(dSdx, 3*nodenum); - for (int n = 0; n < npc; n++) { - mju_addTo3(dSdx + 3*gindices[n], dSdx_local + 3*n); - } - - strain_jacobian(nodenum, nv, dSdx, node_jac, strain_jac); - - if (issparse) { - mj_markStack(d); - mjtNum* sj = mjSTACKALLOC(d, combined_nnz, mjtNum); - for (int k = 0; k < combined_nnz; k++) { - sj[k] = strain_jac[combined_chain[k]]; - } - mj_addConstraint(m, d, sj, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, - combined_nnz, combined_chain); - mj_freeStack(d); - } else { - mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); - } - } + invariant_dSdx(npc, grad, F, Fref_inv, dSdE, dSdx_local); + cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac); + if (issparse) { + mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, + cell_nnz, cell_chain); + } else { + for (int k = 0; k < cell_nnz; k++) { + dense_jac[cell_chain[k]] = strain_jac[k]; } - - // Gauss integration per cell - for (int g = 0; g < ngauss; g++) { - mjtNum* p = gauss[g]; - - // F = Fcur * Fref_inv - mjtNum Fcur[9], Fref[9], Fref_inv[9], F[9]; - mju_defGradient(Fcur, p, xpos_c, order); - mju_defGradient(Fref, p, refpos_c, order); - mat3_inverse(Fref, Fref_inv); - mju_mulMatMat3(F, Fcur, Fref_inv); - - // Green-Lagrange strain E = 0.5*(C - I) - mjtNum C[9], E[9]; - mju_mulMatTMat3(C, F, F); - for (int j = 0; j < 9; j++) { - E[j] = 0.5 * C[j]; - } - E[0] -= 0.5; E[4] -= 0.5; E[8] -= 0.5; - - // 3 invariants of E - mjtNum I1 = E[0] + E[4] + E[8]; - mjtNum trE2 = E[0]*E[0] + E[1]*E[3] + E[2]*E[6] - + E[3]*E[1] + E[4]*E[4] + E[5]*E[7] - + E[6]*E[2] + E[7]*E[5] + E[8]*E[8]; - mjtNum I2 = 0.5 * (I1*I1 - trE2); - mjtNum I3 = mat3_det(E); - - // shape function gradients at Gauss point - mjtNum grad[27][3]; - shape_gradients(order, p, grad); - - for (int s = 0; s < 6; s++) { - // skip I1,I2,I3 for trilinear (B-bar handles vol) - if (order == 1 && (s == 0 || s == 1 || s == 2)) { - continue; - } - - mjtNum dSdE[9]; - mju_zero(dSdE, 9); - - if (s == 0) { - cpos[0] = I1; - dSdE[0] = dSdE[4] = dSdE[8] = 1.0; - } else if (s == 1) { - cpos[0] = I2; - dSdE[0] = I1-E[0]; dSdE[4] = I1-E[4]; - dSdE[8] = I1-E[8]; - dSdE[1] = -E[1]; dSdE[3] = -E[3]; - dSdE[2] = -E[2]; dSdE[6] = -E[6]; - dSdE[5] = -E[5]; dSdE[7] = -E[7]; - } else if (s == 2) { - cpos[0] = I3; - mat3_cofactor(E, dSdE); - } else { - int offdiag_idx[3] = {1, 2, 5}; - int ij = offdiag_idx[s - 3]; - cpos[0] = E[ij]; - dSdE[ij] = 1.0; - } - - // compute local dS/dx for cell nodes - invariant_dSdx(npc, grad, F, Fref_inv, dSdE, - dSdx_local); - - // scatter to global dSdx - mju_zero(dSdx, 3*nodenum); - for (int n = 0; n < npc; n++) { - mju_addTo3(dSdx + 3*gindices[n], dSdx_local + 3*n); - } - - strain_jacobian(nodenum, nv, dSdx, node_jac, strain_jac); - - if (issparse) { - mj_markStack(d); - mjtNum* sj = mjSTACKALLOC(d, combined_nnz, mjtNum); - for (int k = 0; k < combined_nnz; k++) { - sj[k] = strain_jac[combined_chain[k]]; - } - mj_addConstraint(m, d, sj, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, - combined_nnz, combined_chain); - mj_freeStack(d); - } else { - mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); - } - } + mj_addConstraint(m, d, dense_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); + for (int k = 0; k < cell_nnz; k++) { + dense_jac[cell_chain[k]] = 0; } } } @@ -1920,26 +1911,35 @@ void mj_diagApprox(const mjModel* m, mjData* d) { break; case mjEQ_FLEXSTRAIN: { - // strain constraints: use average node inv weight + // strain constraints: per-cell, use avg inv weight of cell's npc nodes int flex_id = m->eq_obj1id[id]; - int nodenum = m->flex_nodenum[flex_id]; int nstart = m->flex_nodeadr[flex_id]; int order = m->flex_interp[flex_id]; + int npc = (order+1)*(order+1)*(order+1); - // compute constraint count per cell, then multiply by ncells + // per-cell constraint count int nquad = order + 1; int ngauss = nquad * nquad * nquad; - int ncells = m->flex_cellnum[3*flex_id+0] - * m->flex_cellnum[3*flex_id+1] - * m->flex_cellnum[3*flex_id+2]; - int nconstraint = ncells * ((order == 1) ? (2 + 3 * ngauss) : (6 * ngauss)); + int nconstraint = (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss); + + // get cell index from eq_data + int eq_id = d->efc_id[i]; + int ci_cell = (int)m->eq_data[mjNEQDATA*eq_id + 0]; + int cj_cell = (int)m->eq_data[mjNEQDATA*eq_id + 1]; + int ck_cell = (int)m->eq_data[mjNEQDATA*eq_id + 2]; + int cy = m->flex_cellnum[3*flex_id+1]; + int cz = m->flex_cellnum[3*flex_id+2]; + + int gindices[125]; + mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); mjtNum avg_invweight = 0; - for (int n = 0; n < nodenum; n++) { - int bodyid = m->flex_nodebodyid[nstart + n]; + for (int n = 0; n < npc; n++) { + int bodyid = m->flex_nodebodyid[nstart + gindices[n]]; avg_invweight += m->body_invweight0[2*bodyid]; } - avg_invweight /= nodenum; + avg_invweight /= npc; for (int c = 0; c < nconstraint; c++) { dA[i++] = avg_invweight; } @@ -2529,34 +2529,35 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { break; case mjEQ_FLEXSTRAIN: { - // strain constraints: - // Q1: B-bar, 2 center (I1, J-1) + 3*8 shear = 26 - // Q2: full 3x3x3 Gauss, 6*27 = 162 - // skip if not interpolated (order == 0 or no nodes) - int order = m->flex_interp[id[0]]; - int nodenum = m->flex_nodenum[id[0]]; - if (!order || !nodenum) { + // per-cell strain constraints: each equality is one cell + int f = id[0]; + int order = m->flex_interp[f]; + if (!order || !m->flex_nodenum[f]) { break; } - int nquad = order + 1; // 2 for order=1, 3 for order=2 - int ngauss = nquad * nquad * nquad; // 8 or 27 - int ncells = m->flex_cellnum[3*id[0]+0] - * m->flex_cellnum[3*id[0]+1] - * m->flex_cellnum[3*id[0]+2]; - size = ncells * ((order == 1) ? (2 + 3 * ngauss) : (6 * ngauss)); + int npc = (order+1)*(order+1)*(order+1); + int nquad = order + 1; + int ngauss = nquad * nquad * nquad; + size = (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss); // per cell if (nnz) { - // Count unique DOFs across all node bodies (matching instantiation) - int nstart = m->flex_nodeadr[id[0]]; - int* nodebodies = mjSTACKALLOC(d, nodenum, int); - for (int n = 0; n < nodenum; n++) { - nodebodies[n] = m->flex_nodebodyid[nstart + n]; + // get cell index from eq_data + int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0]; + int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1]; + int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2]; + int cy = m->flex_cellnum[3*f+1]; + int cz = m->flex_cellnum[3*f+2]; + + // get the npc node body IDs for this cell + int gindices[125]; + mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + int nstart = m->flex_nodeadr[f]; + int* cell_bodies = mjSTACKALLOC(d, npc, int); + for (int n = 0; n < npc; n++) { + cell_bodies[n] = m->flex_nodebodyid[nstart + gindices[n]]; } - - // mj_jacSumCount deduplicates shared DOFs - NV = mj_jacSumCount(m, d, chain, nodenum, nodebodies); - - // each constraint row shares this combined NV + NV = mj_jacSumCount(m, d, chain, npc, cell_bodies); // npc nodes only NV = size * NV; } break; diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 4035a176..a86a2f33 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -680,20 +680,35 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf mjs_setDouble(pf->vert, point.data(), point.size()); } - // create edge equality constraint + // create equality constraints if (equality) { - mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); - mjs_setDefault(pe->element, &model->Default()->spec); // equality 1=edge(mjEQ_FLEX), 2=vert(mjEQ_FLEXVERT), 3=strain(mjEQ_FLEXSTRAIN) - if (equality == 1) { - pe->type = mjEQ_FLEX; - } else if (equality == 2) { - pe->type = mjEQ_FLEXVERT; + if (equality == 1 || equality == 2) { + mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); + mjs_setDefault(pe->element, &model->Default()->spec); + pe->type = (equality == 1) ? mjEQ_FLEX : mjEQ_FLEXVERT; + pe->active = true; + mjs_setString(pe->name1, name.c_str()); } else if (equality == 3) { - pe->type = mjEQ_FLEXSTRAIN; + // create one strain constraint per cell, storing cell index in eq_data + int cell_cx = flex->spec.cellcount[0]; + int cell_cy = flex->spec.cellcount[1]; + int cell_cz = flex->spec.cellcount[2]; + for (int ci = 0; ci < cell_cx; ci++) { + for (int cj = 0; cj < cell_cy; cj++) { + for (int ck = 0; ck < cell_cz; ck++) { + mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); + mjs_setDefault(pe->element, &model->Default()->spec); + pe->type = mjEQ_FLEXSTRAIN; + pe->active = true; + mjs_setString(pe->name1, name.c_str()); + pe->data[0] = ci; + pe->data[1] = cj; + pe->data[2] = ck; + } + } + } } - pe->active = true; - mjs_setString(pe->name1, name.c_str()); } return true; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index d38bde20..360c46be 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -371,7 +371,7 @@ std::vector MJCF[nMJCF] = { "active", "solref", "solimp"}, {"flexvert", "*", "name", "class", "flex", "active", "solref", "solimp"}, - {"flexstrain", "*", "name", "class", "flex", + {"flexstrain", "*", "name", "class", "flex", "cell", "active", "solref", "solimp"}, {">"}, @@ -2245,8 +2245,12 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* equality) { case mjEQ_FLEX: case mjEQ_FLEXVERT: + ReadAttrTxt(elem, "flex", name1, true); + break; + case mjEQ_FLEXSTRAIN: ReadAttrTxt(elem, "flex", name1, true); + ReadAttr(elem, "cell", 3, equality->data, text); break; case mjEQ_DISTANCE: diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 007a76a3..ebe95bab 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -728,8 +728,12 @@ void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDe case mjEQ_FLEX: case mjEQ_FLEXVERT: + WriteAttrTxt(elem, "flex", mjs_getString(equality->name1)); + break; + case mjEQ_FLEXSTRAIN: WriteAttrTxt(elem, "flex", mjs_getString(equality->name1)); + WriteAttr(elem, "cell", 3, equality->data); break; default: From 8262280f5f16a8afd6c74d5ca0487b922a0e893c Mon Sep 17 00:00:00 2001 From: Tarik Kelestemur Date: Sun, 19 Apr 2026 13:12:52 -0400 Subject: [PATCH 087/251] Clean up MJX segmentation rendering --- doc/mjx.rst | 5 ++- mjx/mujoco/mjx/_src/bvh.py | 13 +++---- mjx/mujoco/mjx/_src/render.py | 30 +++++++--------- mjx/mujoco/mjx/_src/render_util.py | 45 +++++++---------------- mjx/mujoco/mjx/_src/warp_context.py | 56 +++++++++++++++++++++++++++++ mjx/mujoco/mjx/warp/render_test.py | 16 +++++++++ 6 files changed, 106 insertions(+), 59 deletions(-) create mode 100644 mjx/mujoco/mjx/_src/warp_context.py diff --git a/doc/mjx.rst b/doc/mjx.rst index 247dafa8..42ad03e8 100644 --- a/doc/mjx.rst +++ b/doc/mjx.rst @@ -257,7 +257,7 @@ volume hierarchy (BVH) and executing the raycaster: # 2. Render all configured cameras, including segmentation pixels, _, segmentation = mjx.render_with_segmentation(mx, d, rc_pytree) - # 3. Extract the RGB tensor and geom IDs for the first camera (index 0) + # 3. Extract the RGB tensor and segmentation IDs for the first camera rgb = get_rgb(rc_pytree, 0, pixels) seg = get_segmentation(rc_pytree, 0, segmentation) @@ -265,6 +265,9 @@ volume hierarchy (BVH) and executing the raycaster: rgb, seg, d = render_fn(mx, d, rc.pytree()) +The segmentation image contains MuJoCo geom IDs per pixel, ``-1`` for +background, and ``-2`` for flex bodies. + .. WARNING:: The batch dimension ``nworld`` is fixed when the render context is created via :func:`~mujoco.mjx.create_render_context` since the underlying Warp render context allocates diff --git a/mjx/mujoco/mjx/_src/bvh.py b/mjx/mujoco/mjx/_src/bvh.py index 1a75a8a0..87e75ae9 100644 --- a/mjx/mujoco/mjx/_src/bvh.py +++ b/mjx/mujoco/mjx/_src/bvh.py @@ -15,26 +15,23 @@ """BVH helpers for MJX.""" from typing import Any + +import mujoco.mjx.warp as mjxw + +from mujoco.mjx._src.warp_context import get_warp_render_context # pylint: disable=g-importing-member from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import Impl from mujoco.mjx._src.types import Model # pylint: enable=g-importing-member -import mujoco.mjx.warp as mjxw def refit_bvh(m: Model, d: Data, ctx: Any): """Refit the scene BVH for the current pose.""" if m.impl == Impl.WARP and d.impl == Impl.WARP and mjxw.WARP_INSTALLED: - import mujoco.mjx.warp.render_context as mjxw_rc # pylint: disable=g-import-not-at-top # pytype: disable=import-error from mujoco.mjx.warp import bvh as mjxw_bvh # pylint: disable=g-import-not-at-top # pytype: disable=import-error - if not isinstance(ctx, mjxw_rc.RenderContextPytree): - raise TypeError( - f'Expected RenderContextPytree, got {type(ctx).__name__}.' - ' Use rc.pytree() to get the JAX-compatible handle.' - ) - + get_warp_render_context(ctx) return mjxw_bvh.refit_bvh(m, d, ctx) raise NotImplementedError('refit_bvh only implemented for MuJoCo Warp.') diff --git a/mjx/mujoco/mjx/_src/render.py b/mjx/mujoco/mjx/_src/render.py index d6126529..56666dc9 100644 --- a/mjx/mujoco/mjx/_src/render.py +++ b/mjx/mujoco/mjx/_src/render.py @@ -16,42 +16,38 @@ from typing import Any +import jax +import mujoco.mjx.warp as mjxw + +from mujoco.mjx._src.warp_context import get_warp_render_context +from mujoco.mjx._src.warp_context import require_segmentation_enabled # pylint: disable=g-importing-member from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import Impl from mujoco.mjx._src.types import Model # pylint: enable=g-importing-member -import mujoco.mjx.warp as mjxw -def render(m: Model, d: Data, ctx: Any) -> Data: - """Render.""" +def render(m: Model, d: Data, ctx: Any) -> tuple[jax.Array, jax.Array]: + """Render packed RGB and depth buffers.""" if m.impl == Impl.WARP and d.impl == Impl.WARP and mjxw.WARP_INSTALLED: from mujoco.mjx.warp import render as mjxw_render - from mujoco.mjx.warp import render_context as mjxw_rc - - if not isinstance(ctx, mjxw_rc.RenderContextPytree): - raise TypeError( - f'Expected RenderContextPytree, got {type(ctx).__name__}.' - ' Use rc.pytree() to get the JAX-compatible handle.' - ) + get_warp_render_context(ctx) return mjxw_render.render(m, d, ctx) raise NotImplementedError('render only implemented for MuJoCo Warp.') -def render_with_segmentation(m: Model, d: Data, ctx: Any) -> Data: +def render_with_segmentation( + m: Model, d: Data, ctx: Any +) -> tuple[jax.Array, jax.Array, jax.Array]: """Render and return RGB, depth, and packed segmentation outputs.""" if m.impl == Impl.WARP and d.impl == Impl.WARP and mjxw.WARP_INSTALLED: from mujoco.mjx.warp import render as mjxw_render - from mujoco.mjx.warp import render_context as mjxw_rc - if not isinstance(ctx, mjxw_rc.RenderContextPytree): - raise TypeError( - f'Expected RenderContextPytree, got {type(ctx).__name__}.' - ' Use rc.pytree() to get the JAX-compatible handle.' - ) + warp_rc = get_warp_render_context(ctx) + require_segmentation_enabled(warp_rc) return mjxw_render.render_with_segmentation(m, d, ctx) diff --git a/mjx/mujoco/mjx/_src/render_util.py b/mjx/mujoco/mjx/_src/render_util.py index 56c7e6ad..78f537b1 100644 --- a/mjx/mujoco/mjx/_src/render_util.py +++ b/mjx/mujoco/mjx/_src/render_util.py @@ -19,36 +19,13 @@ from typing import TYPE_CHECKING import jax import jax.numpy as jnp -import mujoco.mjx.warp as mjxw +from mujoco.mjx._src.warp_context import get_camera_resolution +from mujoco.mjx._src.warp_context import get_warp_render_context if TYPE_CHECKING: from mujoco.mjx.warp.render_context import RenderContextPytree -def _get_warp_render_context(rc: 'RenderContextPytree'): - """Validates and returns the backing Warp render context.""" - if not mjxw.WARP_INSTALLED: - raise RuntimeError('Warp not installed.') - - from mujoco.mjx.warp import render_context as mjxw_rc - - if not isinstance(rc, mjxw_rc.RenderContextPytree): - raise TypeError( - f'Expected RenderContextPytree, got {type(rc).__name__}.' - ' Use rc.pytree() to get the JAX-compatible handle.' - ) - - # pylint: disable=protected-access - return mjxw_rc._MJX_RENDER_CONTEXT_BUFFERS[(rc.key, None)] - - -def _get_camera_resolution(warp_rc, cam_id: int) -> tuple[int, int]: - """Returns the render resolution for a given camera.""" - width = int(warp_rc.cam_res.numpy()[cam_id][0]) - height = int(warp_rc.cam_res.numpy()[cam_id][1]) - return width, height - - def get_rgb( rc: 'RenderContextPytree', cam_id: int, @@ -68,9 +45,9 @@ def get_rgb( Raises: RuntimeError: If Warp is not installed. """ - warp_rc = _get_warp_render_context(rc) + warp_rc = get_warp_render_context(rc) rgb_adr = int(warp_rc.rgb_adr.numpy()[cam_id]) - width, height = _get_camera_resolution(warp_rc, cam_id) + width, height = get_camera_resolution(warp_rc, cam_id) packed = jax.lax.dynamic_slice_in_dim( rgb_data, rgb_adr, width * height, axis=rgb_data.ndim - 1 @@ -104,9 +81,9 @@ def get_depth( Raises: RuntimeError: If Warp is not installed. """ - warp_rc = _get_warp_render_context(rc) + warp_rc = get_warp_render_context(rc) depth_adr = int(warp_rc.depth_adr.numpy()[cam_id]) - width, height = _get_camera_resolution(warp_rc, cam_id) + width, height = get_camera_resolution(warp_rc, cam_id) raw = jax.lax.dynamic_slice_in_dim( depth_data, depth_adr, width * height, axis=depth_data.ndim - 1 @@ -121,7 +98,7 @@ def get_segmentation( cam_id: int, seg_data: jax.Array, ) -> jax.Array: - """Extract raw geom IDs for a camera. + """Extract raw segmentation IDs for a camera. Args: rc: RenderContextPytree. @@ -129,21 +106,23 @@ def get_segmentation( seg_data: Packed segmentation output, shape (..., total_pixels) as integers. Returns: - Integer segmentation array with shape (..., H, W). + Integer segmentation array with shape (..., H, W). Each pixel contains the + MuJoCo geom ID of the hit geometry, ``-1`` for background, or ``-2`` for a + flex body. Any leading batch axes in `seg_data` are preserved. Raises: RuntimeError: If Warp is not installed. ValueError: If segmentation is not enabled for the selected camera. """ - warp_rc = _get_warp_render_context(rc) + warp_rc = get_warp_render_context(rc) seg_adr = int(warp_rc.seg_adr.numpy()[cam_id]) if seg_adr < 0: raise ValueError( f'Camera {cam_id} was not configured with segmentation rendering.' ) - width, height = _get_camera_resolution(warp_rc, cam_id) + width, height = get_camera_resolution(warp_rc, cam_id) packed = jax.lax.dynamic_slice_in_dim( seg_data, seg_adr, width * height, axis=seg_data.ndim - 1 ) diff --git a/mjx/mujoco/mjx/_src/warp_context.py b/mjx/mujoco/mjx/_src/warp_context.py new file mode 100644 index 00000000..3479ccfd --- /dev/null +++ b/mjx/mujoco/mjx/_src/warp_context.py @@ -0,0 +1,56 @@ +# Copyright 2026 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +"""Shared helpers for MJX-Warp render contexts.""" + +from typing import TYPE_CHECKING + +import mujoco.mjx.warp as mjxw + +if TYPE_CHECKING: + from mujoco.mjx.warp.render_context import RenderContextPytree + + +def get_warp_render_context(rc: 'RenderContextPytree'): + """Validates and returns the backing Warp render context.""" + if not mjxw.WARP_INSTALLED: + raise RuntimeError('Warp not installed.') + + from mujoco.mjx.warp import render_context as mjxw_rc + + if not isinstance(rc, mjxw_rc.RenderContextPytree): + raise TypeError( + f'Expected RenderContextPytree, got {type(rc).__name__}.' + ' Use rc.pytree() to get the JAX-compatible handle.' + ) + + # pylint: disable=protected-access + return mjxw_rc._MJX_RENDER_CONTEXT_BUFFERS[(rc.key, None)] + + +def get_camera_resolution(warp_rc, cam_id: int) -> tuple[int, int]: + """Returns the render resolution for a given camera.""" + width = int(warp_rc.cam_res.numpy()[cam_id][0]) + height = int(warp_rc.cam_res.numpy()[cam_id][1]) + return width, height + + +def require_segmentation_enabled(warp_rc) -> None: + """Raises if the render context has no segmentation-enabled cameras.""" + if not (warp_rc.seg_adr.numpy() >= 0).any(): + raise ValueError( + 'Render context was not configured with segmentation rendering. ' + 'Pass render_seg=True or enable it for at least one camera in ' + 'create_render_context.' + ) diff --git a/mjx/mujoco/mjx/warp/render_test.py b/mjx/mujoco/mjx/warp/render_test.py index fc089617..6745c2e6 100644 --- a/mjx/mujoco/mjx/warp/render_test.py +++ b/mjx/mujoco/mjx/warp/render_test.py @@ -185,6 +185,22 @@ class RenderTest(parameterized.TestCase): ) np.testing.assert_array_equal(unpacked_seg, expected_seg) + def test_render_with_segmentation_raises_when_disabled(self): + """Tests render_with_segmentation rejects contexts without seg output.""" + self._maybe_skip() + mx, dx_batch, rc = _get_model_data_rc( + 'humanoid/humanoid.xml', 1, render_seg=False + ) + + dx_batch = jax.jit(mjx.refit_bvh)(mx, dx_batch, rc.pytree()) + with self.assertRaisesWithLiteralMatch( + ValueError, + 'Render context was not configured with segmentation rendering. ' + 'Pass render_seg=True or enable it for at least one camera in ' + 'create_render_context.', + ): + jax.jit(mjx.render_with_segmentation)(mx, dx_batch, rc.pytree()) + @parameterized.product( xml=('humanoid/humanoid.xml',), batch_size=(4, 16), From 6bf31cf68a0f5454f6fa329dc05be793e158c980 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 20 Apr 2026 01:12:03 -0700 Subject: [PATCH 088/251] Update Windows build notes. PiperOrigin-RevId: 902480947 Change-Id: I69f099b3d7eabd6513a62e74e5ef697a5062e7b5 --- doc/programming/index.rst | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/doc/programming/index.rst b/doc/programming/index.rst index 48ef13f2..7b34cdcc 100644 --- a/doc/programming/index.rst +++ b/doc/programming/index.rst @@ -116,9 +116,10 @@ target directory. **Notes:** -- When building on Windows, use Visual Studio 2019 or later and make sure Windows SDK version 10.0.22000 or later is - installed (see :issue:`862` for more details). -- To optimize runtime performance build with ``-DCMAKE_BUILD_TYPE=Release`` +- To optimize runtime performance build with ``-DCMAKE_BUILD_TYPE=Release``. +- When building on Windows with MSVC, use Visual Studio 2019 or later and make sure Windows SDK version 10.0.22000 or + later is installed (see :issue:`862` for more details). +- We've found that performance on Windows is best when building with Clang, rather than MSVC. .. tip:: As a reference, a working build configuration can be found in MuJoCo's From f3f12bfad6a4cf974ac0cfbe5c706a4bcf048eb3 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Mon, 20 Apr 2026 01:23:20 -0700 Subject: [PATCH 089/251] Manage dynamic meshes (skins, flexes) in model_objects. Simplifies Renderable as it no longer has the option of owning any Meshes. Instead, the ModelObjects owns the Meshes for flex and skin geom (just like it owns all the other mjModel Meshes). PiperOrigin-RevId: 902485912 Change-Id: I8b9f9d394de6c46f7e25aa4b3885a374aad05294 --- .../filament/filament/model_objects.cc | 18 +++++++--------- .../filament/filament/model_objects.h | 16 +++++++------- .../filament/filament/renderable.cc | 21 +++---------------- .../filament/filament/renderable.h | 8 ++----- .../filament/filament/scene_bridge.cc | 4 ++++ .../filament/filament/scene_geom_util.cc | 14 +++++-------- 6 files changed, 30 insertions(+), 51 deletions(-) diff --git a/src/experimental/filament/filament/model_objects.cc b/src/experimental/filament/filament/model_objects.cc index 1f88dce0..15121743 100644 --- a/src/experimental/filament/filament/model_objects.cc +++ b/src/experimental/filament/filament/model_objects.cc @@ -630,20 +630,11 @@ void ModelObjects::UploadHeightField(const mjModel* model, int id) { height_fields_[id] = std::make_unique(engine_, data); } -MeshPtr ModelObjects::CreateFlexMesh(const mjvScene* scene, - const mjvGeom& geom) { +void ModelObjects::CreateSkinFlexMesh(const mjvScene* scene, const mjvGeom& geom) { MeshData data; DefaultMeshData(&data); UpdateSkinFlexMeshData(&data, model_, scene, geom); - return std::make_unique(engine_, data); -} - -MeshPtr ModelObjects::CreateSkinMesh(const mjvScene* scene, - const mjvGeom& geom) { - MeshData data; - DefaultMeshData(&data); - UpdateSkinFlexMeshData(&data, model_, scene, geom); - return std::make_unique(engine_, data); + dynamic_meshes_[geom.objid] = std::make_unique(engine_, data); } const Mesh* ModelObjects::GetMeshBuffer(int data_id) const { @@ -672,6 +663,11 @@ const Mesh* ModelObjects::GetShapeBuffer(ShapeType shape) const { return shapes_[shape].get(); } +const Mesh* ModelObjects::GetFlexSkinGeomMesh(int geom_id) const { + auto it = dynamic_meshes_.find(geom_id); + return it != dynamic_meshes_.end() ? it->second.get() : nullptr; +} + const Texture* ModelObjects::GetTexture(int tex_id) const { auto it = textures_.find(tex_id); return it != textures_.end() ? it->second.get() : nullptr; diff --git a/src/experimental/filament/filament/model_objects.h b/src/experimental/filament/filament/model_objects.h index bc6a005b..d85693e3 100644 --- a/src/experimental/filament/filament/model_objects.h +++ b/src/experimental/filament/filament/model_objects.h @@ -15,6 +15,7 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_OBJECTS_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_OBJECTS_H_ +#include #include #include #include @@ -55,6 +56,8 @@ class ModelObjects { void UploadHeightField(const mjModel* model, int id); + void CreateSkinFlexMesh(const mjvScene* scene, const mjvGeom& geom); + // Returns the filament engine used by the ModelObjects to create filament // objects. filament::Engine* GetEngine() const { return engine_; } @@ -63,12 +66,10 @@ class ModelObjects { const Mesh* GetShapeBuffer(ShapeType shape) const; const Mesh* GetMeshBuffer(int data_id) const; const Mesh* GetHeightFieldBuffer(int hfield_id) const; + const Mesh* GetFlexSkinGeomMesh(int geom_id) const; const Texture* GetTexture(int tex_id) const; const Texture* GetTexture(int mat_id, int role) const; - MeshPtr CreateFlexMesh(const mjvScene* scene, const mjvGeom& geom); - MeshPtr CreateSkinMesh(const mjvScene* scene, const mjvGeom& geom); - filament::Skybox* CreateSkybox(); filament::IndirectLight* CreateIndirectLight(int tex_id, float intensity); @@ -86,10 +87,11 @@ class ModelObjects { filament::Engine* engine_ = nullptr; std::vector skyboxes_; std::vector indirect_lights_; - std::array shapes_; - std::unordered_map meshes_; - std::unordered_map convex_hulls_; - std::unordered_map height_fields_; + std::array, kNumShapes> shapes_; + std::unordered_map> meshes_; + std::unordered_map> convex_hulls_; + std::unordered_map> height_fields_; + std::unordered_map> dynamic_meshes_; std::unordered_map> textures_; float specular_multiplier_ = 0.2f; float shininess_multiplier_ = 0.1f; diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 6a3da110..4fc9aaf3 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -66,25 +66,12 @@ void Renderable::RemoveLastEntity() { void Renderable::UpdateMesh(int index, const Mesh* mesh, int elem_offset, int elem_count) { - MeshInfo& mesh_info = SetMesh(index, mesh, nullptr, elem_offset, elem_count); - UpdateEntity(index, mesh_info); -} - -void Renderable::UpdateMesh(int index, MeshPtr mesh, int elem_offset, - int elem_count) { - MeshInfo& mesh_info = - SetMesh(index, mesh.get(), std::move(mesh), elem_offset, elem_count); + MeshInfo& mesh_info = SetMesh(index, mesh, elem_offset, elem_count); UpdateEntity(index, mesh_info); } void Renderable::AppendMesh(const Mesh* mesh, int elem_offset, int elem_count) { - MeshInfo& mesh_info = SetMesh(-1, mesh, nullptr, elem_offset, elem_count); - AppendEntity(mesh_info); -} - -void Renderable::AppendMesh(MeshPtr mesh, int elem_offset, int elem_count) { - MeshInfo& mesh_info = - SetMesh(-1, mesh.get(), std::move(mesh), elem_offset, elem_count); + MeshInfo& mesh_info = SetMesh(-1, mesh, elem_offset, elem_count); AppendEntity(mesh_info); } @@ -154,8 +141,7 @@ void Renderable::UpdateEntity(int index, const MeshInfo& mesh_info) { } Renderable::MeshInfo& Renderable::SetMesh(int index, const Mesh* mesh, - MeshPtr owned_mesh, int elem_offset, - int elem_count) { + int elem_offset, int elem_count) { if (index == -1) { index = meshes_.size(); meshes_.emplace_back(); @@ -165,7 +151,6 @@ Renderable::MeshInfo& Renderable::SetMesh(int index, const Mesh* mesh, } MeshInfo* mesh_info = &meshes_[index]; - mesh_info->owned_mesh = std::move(owned_mesh); mesh_info->mesh = mesh; mesh_info->elem_offset = elem_offset; mesh_info->elem_count = elem_count; diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index dbe03498..016824b8 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -60,15 +60,12 @@ class Renderable { // can be used to specify a submesh to append. If elem_count is 0, assumes // the entire mesh should be appended. void AppendMesh(const Mesh* mesh, int elem_offset = 0, int elem_count = 0); - void AppendMesh(MeshPtr mesh, int elem_offset = 0, int elem_count = 0); // Replaces the mesh at the index with a new mesh. The elem_offset and // elem_count parameters can be used to specify a submesh to append. If // elem_count is 0, assumes the entire mesh should be appended. void UpdateMesh(int index, const Mesh* mesh, int elem_offset = 0, int elem_count = 0); - void UpdateMesh(int index, MeshPtr mesh, int elem_offset = 0, - int elem_count = 0); // Returns the number of meshes that define the renderable. int GetNumMeshes() const { return meshes_.size(); } @@ -124,7 +121,6 @@ class Renderable { private: struct MeshInfo { - MeshPtr owned_mesh; const Mesh* mesh = nullptr; int elem_offset = 0; int elem_count = 0; @@ -132,8 +128,8 @@ class Renderable { // Sets the mesh information for the mesh at the given index. If index is -1, // a new mesh will be appended to the renderable. - MeshInfo& SetMesh(int index, const Mesh* mesh, MeshPtr owned_mesh, - int elem_offset, int elem_count); + MeshInfo& SetMesh(int index, const Mesh* mesh, int elem_offset, + int elem_count); // Appends a new filament::Entity to the renderable, configured to use the // given mesh. diff --git a/src/experimental/filament/filament/scene_bridge.cc b/src/experimental/filament/filament/scene_bridge.cc index e8198b00..3438b8c9 100644 --- a/src/experimental/filament/filament/scene_bridge.cc +++ b/src/experimental/filament/filament/scene_bridge.cc @@ -381,6 +381,10 @@ void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { } } + if (geom->type == mjGEOM_FLEX || geom->type == mjGEOM_SKIN) { + model_objects_->CreateSkinFlexMesh(scene, *geom); + } + std::unique_ptr renderable = CreateGeomRenderable( *geom, scene, object_mgr_, model_objects_.get(), headpos); diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index e60bacf4..1b20fd18 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -96,13 +96,9 @@ static void AddMesh(Renderable& renderable, ModelObjects* model_objs, renderable.AppendMesh(mesh); } -static void AddGeom(Renderable& renderable, ModelObjects* model_objs, - const mjvScene* scene, const mjvGeom& geom) { - if (geom.type == mjGEOM_FLEX) { - renderable.AppendMesh(model_objs->CreateFlexMesh(scene, geom)); - } else if (geom.type == mjGEOM_SKIN) { - renderable.AppendMesh(model_objs->CreateSkinMesh(scene, geom)); - } +static void AddSkinFlexMesh(Renderable& renderable, ModelObjects* model_objs, + int objid) { + renderable.AppendMesh(model_objs->GetFlexSkinGeomMesh(objid)); } static void AddHeightField(Renderable& renderable, ModelObjects* model_objs, @@ -183,10 +179,10 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, AddShape(renderable, model_objects, ModelObjects::kTriangle); break; case mjGEOM_FLEX: - AddGeom(renderable, model_objects, scene, geom); + AddSkinFlexMesh(renderable, model_objects, geom.objid); break; case mjGEOM_SKIN: - AddGeom(renderable, model_objects, scene, geom); + AddSkinFlexMesh(renderable, model_objects, geom.objid); break; case mjGEOM_NONE: case mjGEOM_LABEL: From cf3f6ccf1f5633afb7b0689a680c1b36a1085bbc Mon Sep 17 00:00:00 2001 From: Matija Kecman Date: Mon, 20 Apr 2026 01:26:54 -0700 Subject: [PATCH 090/251] Make file search deterministic in FindFileInPath. Collect all entries from the recursive directory iterator, sort them, and then check for the filename. This ensures that if multiple files with the same name exist in different subdirectories, the one found is always the same, regardless of the filesystem's directory iteration order. PiperOrigin-RevId: 902487291 Change-Id: Ia4c45cd2e3cab4a4e3825c267fc47c3134f99f76 --- src/experimental/platform/helpers.cc | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/experimental/platform/helpers.cc b/src/experimental/platform/helpers.cc index add021f9..e10d8e8d 100644 --- a/src/experimental/platform/helpers.cc +++ b/src/experimental/platform/helpers.cc @@ -14,6 +14,7 @@ #include "experimental/platform/helpers.h" +#include #include #include #include @@ -23,6 +24,7 @@ #include #include #include +#include #include "webp/encode.h" #include "webp/types.h" @@ -77,8 +79,13 @@ std::string ResolveFile(const std::string& filename, return resolved; } + std::vector entries; for (const auto& it : std::filesystem::recursive_directory_iterator(path)) { - resolved = CheckPathForFile(it.path(), filename); + entries.push_back(it.path()); + } + std::sort(entries.begin(), entries.end()); + for (const auto& entry : entries) { + resolved = CheckPathForFile(entry, filename); if (!resolved.empty()) { return resolved; } From a04c2b1b4a12771d7834183bc153494808fd4574 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 20 Apr 2026 01:46:48 -0700 Subject: [PATCH 091/251] Consolidate stack allocation calls in primal solvers down from 30 to 2 (dense) or 6 (sparse). Preparation for atomic allocation calls in threaded mode. PiperOrigin-RevId: 902496039 Change-Id: I39df011951713505c4743c7475d74856ddbdf4f9 --- src/engine/engine_solver.c | 201 ++++++++++++++++++++++--------------- 1 file changed, 118 insertions(+), 83 deletions(-) diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index b02302fc..cc9e01ce 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -810,6 +810,12 @@ typedef struct { mjtNum* Mgrad; // M\grad or H\grad (nv x 1) mjtNum* search; // linesearch vector (nv x 1) mjtNum* quad; // quadratic polynomials for constraint costs (nefc x 3) + int* oldstate; // previous constraint state (nefc x 1) + + // CG arrays (PrimalAllocate, CG only) + mjtNum* gradold; // previous gradient (nv x 1) + mjtNum* Mgradold; // previous preconditioned gradient (nv x 1) + mjtNum* Mgraddif; // gradient difference (nv x 1) // Newton arrays, known-size (PrimalAllocate) mjtNum* D; // constraint inertia (nefc x 1) @@ -960,55 +966,99 @@ static void PrimalPointers(const mjModel* m, const mjData* d, mjPrimalContext* c // allocate fixed-size arrays in mjPrimalContext // mj_{mark/free}Stack in calling function! static void PrimalAllocate(mjData* d, mjPrimalContext* ctx, int flg_Newton) { - // local sizes + // local sizes and flags int nv = ctx->nv; int nefc = ctx->nefc; + int nJ = ctx->is_sparse ? d->nJ : 0; + int is_sparse = ctx->is_sparse; + int is_elliptic = ctx->is_elliptic; - // common arrays - ctx->Jaref = mjSTACKALLOC(d, nefc, mjtNum); - ctx->Jv = mjSTACKALLOC(d, nefc, mjtNum); - ctx->Ma = mjSTACKALLOC(d, nv, mjtNum); - ctx->Mv = mjSTACKALLOC(d, nv, mjtNum); - ctx->grad = mjSTACKALLOC(d, nv, mjtNum); - ctx->Mgrad = mjSTACKALLOC(d, nv, mjtNum); - ctx->search = mjSTACKALLOC(d, nv, mjtNum); - ctx->quad = mjSTACKALLOC(d, nefc*3, mjtNum); + // compute mjtNum block size + size_t nNum = 5*nefc + 5*nv; // common arrays + if (is_sparse) nNum += nJ; // JT + if (flg_Newton) { + nNum += nefc + nv; // D, cholupd + if (is_elliptic) nNum += 6*nv; // LTJ + if (is_sparse) { + nNum += nv; // buf_val + } else { + nNum += nv*nv; // L (dense) + if (is_elliptic) nNum += nv*nv; // Lcone (dense) + } + } else { + nNum += 3*nv; // CG arrays + } - // sparse only, compute Jacobian transpose - if (ctx->is_sparse) { - ctx->JT_rownnz = mjSTACKALLOC(d, nv, int); - ctx->JT_rowadr = mjSTACKALLOC(d, nv, int); - ctx->JT_rowsuper = mjSTACKALLOC(d, nv, int); - ctx->JT_colind = mjSTACKALLOC(d, d->nJ, int); - ctx->JT = mjSTACKALLOC(d, d->nJ, mjtNum); - int offset = ctx->J_rowadr[0]; + // compute int block size + size_t nInt = nefc; // oldstate + if (is_sparse) { + nInt += 3*nv + nJ; // JT sparse + if (flg_Newton) nInt += 9*nv; // Newton sparse + } + + // allocate mjtNum and int blocks + mjtNum* numblock = mjSTACKALLOC(d, nNum, mjtNum); + int* intblock = mjSTACKALLOC(d, nInt, int); + + // carve mjtNum block + ctx->Jaref = numblock; numblock += nefc; + ctx->Jv = numblock; numblock += nefc; + ctx->Ma = numblock; numblock += nv; + ctx->Mv = numblock; numblock += nv; + ctx->grad = numblock; numblock += nv; + ctx->Mgrad = numblock; numblock += nv; + ctx->search = numblock; numblock += nv; + ctx->quad = numblock; numblock += 3*nefc; + if (is_sparse) { + ctx->JT = numblock; numblock += nJ; + } + if (flg_Newton) { + ctx->D = numblock; numblock += nefc; + ctx->cholupd = numblock; numblock += nv; + if (is_elliptic) { + ctx->LTJ = numblock; numblock += 6*nv; + } + if (is_sparse) { + ctx->buf_val = numblock; numblock += nv; + } else { + ctx->nL = nv*nv; + ctx->L = numblock; numblock += ctx->nL; + ctx->Lcone = is_elliptic ? numblock : NULL; + if (is_elliptic) numblock += ctx->nL; + } + } else { + ctx->gradold = numblock; numblock += nv; + ctx->Mgradold = numblock; numblock += nv; + ctx->Mgraddif = numblock; numblock += nv; + } + + // carve int block + ctx->oldstate = intblock; intblock += nefc; + if (is_sparse) { + ctx->JT_rownnz = intblock; intblock += nv; + ctx->JT_rowadr = intblock; intblock += nv; + ctx->JT_rowsuper = intblock; intblock += nv; + ctx->JT_colind = intblock; intblock += nJ; + } + if (flg_Newton && is_sparse) { + ctx->H_rowadr = intblock; intblock += nv; + ctx->H_rownnz = intblock; intblock += nv; + ctx->HT_rownnz = intblock; intblock += nv; + ctx->HT_rowadr = intblock; intblock += nv; + ctx->L_rownnz = intblock; intblock += nv; + ctx->L_rowadr = intblock; intblock += nv; + ctx->LT_rownnz = intblock; intblock += nv; + ctx->LT_rowadr = intblock; intblock += nv; + ctx->buf_ind = intblock; intblock += nv; + } + + // sparse: compute Jacobian transpose + if (is_sparse) { + int offset = ctx->J_rowadr[0]; mju_transposeSparse(ctx->JT, ctx->J + offset, nefc, nv, ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, ctx->JT_rowsuper, ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind + offset); } - - // Newton only, known-size arrays - if (flg_Newton) { - ctx->D = mjSTACKALLOC(d, nefc, mjtNum); - ctx->cholupd = mjSTACKALLOC(d, nv, mjtNum); - if (ctx->is_elliptic) { - ctx->LTJ = mjSTACKALLOC(d, 6*nv, mjtNum); - } - - // sparse Newton only - if (ctx->is_sparse) { - ctx->H_rowadr = mjSTACKALLOC(d, nv, int); - ctx->H_rownnz = mjSTACKALLOC(d, nv, int); - ctx->HT_rownnz = mjSTACKALLOC(d, nv, int); - ctx->HT_rowadr = mjSTACKALLOC(d, nv, int); - ctx->L_rownnz = mjSTACKALLOC(d, nv, int); - ctx->L_rowadr = mjSTACKALLOC(d, nv, int); - ctx->LT_rownnz = mjSTACKALLOC(d, nv, int); - ctx->LT_rowadr = mjSTACKALLOC(d, nv, int); - ctx->buf_val = mjSTACKALLOC(d, nv, mjtNum); - ctx->buf_ind = mjSTACKALLOC(d, nv, int); - } - } } @@ -1529,7 +1579,7 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { // sparse if (ctx->is_sparse) { - // initialize Hessian rowadr, rownnz; get total nonzeros + // count Hessian nonzeros, initialize rowadr, rownnz ctx->nH = mju_sqrMatTDSparseSymbolic( ctx->H_rownnz, ctx->H_rowadr, NULL, NULL, nefc, nv, ctx->J_rownnz, ctx->J_rowadr, ctx->J_colind, @@ -1538,17 +1588,19 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { // add M nonzeros to Hessian total (unavoidable overcounting since H_colind is still unknown) ctx->nH += ctx->M_rowadr[nv - 1] + ctx->M_rownnz[nv - 1]; - // shift H row addresses to make room for C + // nH is known: allocate H, H_colind, HT_colind + ctx->H = mjSTACKALLOC(d, ctx->nH, mjtNum); + int* H_intblock = mjSTACKALLOC(d, 2*ctx->nH, int); + ctx->H_colind = H_intblock; + ctx->HT_colind = H_intblock + ctx->nH; + + // shift H row addresses to make room for M int shift = 0; for (int r = 0; r < nv - 1; r++) { shift += ctx->M_rownnz[r]; ctx->H_rowadr[r + 1] += shift; } - // allocate H_colind and H - ctx->H_colind = mjSTACKALLOC(d, ctx->nH, int); - ctx->H = mjSTACKALLOC(d, ctx->nH, mjtNum); - // compute H = J'*D*J: symbolic phase mju_sqrMatTDSparseSymbolic( ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, NULL, @@ -1562,13 +1614,12 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { ctx->JT, ctx->JT_rownnz, ctx->JT_rowadr, ctx->JT_colind, ctx->JT_rowsuper, ctx->D, d); - // add mass matrix: H = J'*D*J + C + // add mass matrix: H = J'*D*J + M mju_addToMatSparse(ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, nv, ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind, ctx->buf_val, ctx->buf_ind); - // compute H' (upper triangle, required for symbolic Cholesky) - ctx->HT_colind = mjSTACKALLOC(d, ctx->nH, int); + // compute H' sparse structure (upper triangle, required for symbolic Cholesky) mju_transposeSparse(NULL, NULL, nv, nv, ctx->HT_rownnz, ctx->HT_rowadr, ctx->HT_colind, NULL, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind); @@ -1578,16 +1629,16 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { ctx->HT_rownnz, ctx->HT_rowadr, ctx->HT_colind, nv, d); - // allocate L_colind, L, Lcone - ctx->L_colind = mjSTACKALLOC(d, ctx->nL, int); - ctx->L = mjSTACKALLOC(d, ctx->nL, mjtNum); - if (ctx->is_elliptic) { - ctx->Lcone = mjSTACKALLOC(d, ctx->nL, mjtNum); - } - - // allocate LT (CSC representation of L) - ctx->LT_colind = mjSTACKALLOC(d, ctx->nL, int); - ctx->LT_map = mjSTACKALLOC(d, ctx->nL, int); + // nL is known: allocate blocks and carve L_colind, LT_colind, LT_map, L, Lcone + size_t nL_int = 2*ctx->nL + ctx->nL; // L_colind + LT_colind + LT_map + size_t nL_num = ctx->is_elliptic ? 2*ctx->nL : ctx->nL; // L + Lcone + int* L_intblock = mjSTACKALLOC(d, nL_int, int); + mjtNum* L_numblock = mjSTACKALLOC(d, nL_num, mjtNum); + ctx->L_colind = L_intblock; + ctx->LT_colind = L_intblock + ctx->nL; + ctx->LT_map = L_intblock + 2*ctx->nL; + ctx->L = L_numblock; + ctx->Lcone = ctx->is_elliptic ? L_numblock + ctx->nL : NULL; // symbolic Cholesky: populate L_colind and LT structures mju_cholFactorSymbolic(ctx->L_colind, ctx->L_rownnz, ctx->L_rowadr, @@ -1598,13 +1649,6 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { // dense else { - // allocate L, Lcone - ctx->nL = nv*nv; - ctx->L = mjSTACKALLOC(d, ctx->nL, mjtNum); - if (ctx->is_elliptic) { - ctx->Lcone = mjSTACKALLOC(d, ctx->nL, mjtNum); - } - // compute H = M + J'*D*J mju_sqrMatTD_impl(ctx->L, ctx->J, ctx->D, nefc, nv, /*flg_upper=*/ 0); mju_addToSymSparse(ctx->L, ctx->M, ctx->nv, @@ -1691,13 +1735,12 @@ static void FactorizeHessian(mjData* d, mjPrimalContext* ctx, int flg_recompute) // elliptic case: Hcone = H + cone_contributions static void HessianCone(mjData* d, mjPrimalContext* ctx) { int nv = ctx->nv, nefc = ctx->nefc; + mjtNum* LTJ = ctx->LTJ; mjtNum local[36]; // start with Hcone = H mju_copy(ctx->Lcone, ctx->L, ctx->nL); - mjtNum* LTJ = ctx->LTJ; - // add contributions for (int i=0; i < nefc; i++) { if (ctx->efc_state[i] == mjCNSTRSTATE_CONE) { @@ -1818,7 +1861,6 @@ static void HessianIncremental(mjData* d, mjPrimalContext* ctx, const int* oldst static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, int flg_Newton) { int iter = 0; mjtNum alpha, beta; - mjtNum *gradold = NULL, *Mgradold = NULL, *Mgraddif = NULL; mjPrimalContext ctx; mj_markStack(d); @@ -1829,14 +1871,7 @@ static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, i // local copies int nv = ctx.nv; int nefc = ctx.nefc; - - // allocate local storage - if (!flg_Newton) { - gradold = mjSTACKALLOC(d, nv, mjtNum); - Mgradold = mjSTACKALLOC(d, nv, mjtNum); - Mgraddif = mjSTACKALLOC(d, nv, mjtNum); - } - int* oldstate = mjSTACKALLOC(d, nefc, int); + int* oldstate = ctx.oldstate; // compute Ma = M * qacc mju_mulSymVecSparse(ctx.Ma, ctx.M, ctx.qacc, nv, @@ -1895,8 +1930,8 @@ static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, i // save old if (!flg_Newton) { - mju_copy(gradold, ctx.grad, nv); - mju_copy(Mgradold, ctx.Mgrad, nv); + mju_copy(ctx.gradold, ctx.grad, nv); + mju_copy(ctx.Mgradold, ctx.Mgrad, nv); } mju_copyInt(oldstate, ctx.efc_state, nefc); mjtNum oldcost = ctx.cost; @@ -1933,9 +1968,9 @@ static void mj_solPrimal(const mjModel* m, mjData* d, int island, int maxiter, i mju_scl(ctx.search, ctx.Mgrad, -1, nv); } else { // Polak-Ribiere - mju_sub(Mgraddif, ctx.Mgrad, Mgradold, nv); - beta = mju_dot(ctx.grad, Mgraddif, nv) / - mju_max(mjMINVAL, mju_dot(gradold, Mgradold, nv)); + mju_sub(ctx.Mgraddif, ctx.Mgrad, ctx.Mgradold, nv); + beta = mju_dot(ctx.grad, ctx.Mgraddif, nv) / + mju_max(mjMINVAL, mju_dot(ctx.gradold, ctx.Mgradold, nv)); // reset if negative if (beta < 0) { From bf9be2c3127f89f85205419ea593c7b1240ff5a9 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Mon, 20 Apr 2026 02:01:02 -0700 Subject: [PATCH 092/251] Remove MeshPtr type alias. PiperOrigin-RevId: 902502036 Change-Id: I5bd0c185f204518f0cd0d9189741c17c24b51eb6 --- .../filament/filament/builtins.cc | 23 ++++++++++--------- src/experimental/filament/filament/builtins.h | 22 ++++++++++-------- .../filament/filament/imgui_bridge.h | 2 +- src/experimental/filament/filament/mesh.h | 2 -- 4 files changed, 25 insertions(+), 24 deletions(-) diff --git a/src/experimental/filament/filament/builtins.cc b/src/experimental/filament/filament/builtins.cc index 5ee489b3..86a6a5c5 100644 --- a/src/experimental/filament/filament/builtins.cc +++ b/src/experimental/filament/filament/builtins.cc @@ -65,7 +65,8 @@ class BuiltinBuilder : MeshData { virtual ~BuiltinBuilder() = default; template - static MeshPtr Create(filament::Engine* engine, Args&&... args) { + static std::unique_ptr Create(filament::Engine* engine, + Args&&... args) { auto builder = new T(std::forward(args)...); MeshData* mesh_data = builder->PrepareMeshData(); mesh_data->release_callback = +[](void* user_data) { @@ -621,43 +622,43 @@ class DomeBuilder : public BuiltinBuilder { } }; -MeshPtr CreateLine(filament::Engine* engine) { +std::unique_ptr CreateLine(filament::Engine* engine) { return BuiltinBuilder::Create(engine); } -MeshPtr CreatePlane(filament::Engine* engine, int nquad) { +std::unique_ptr CreatePlane(filament::Engine* engine, int nquad) { return BuiltinBuilder::Create(engine, nquad); } -MeshPtr CreateTriangle(filament::Engine* engine) { +std::unique_ptr CreateTriangle(filament::Engine* engine) { return BuiltinBuilder::Create(engine); } -MeshPtr CreateBox(filament::Engine* engine, int nquad) { +std::unique_ptr CreateBox(filament::Engine* engine, int nquad) { return BuiltinBuilder::Create(engine, nquad); } -MeshPtr CreateLineBox(filament::Engine* engine) { +std::unique_ptr CreateLineBox(filament::Engine* engine) { return BuiltinBuilder::Create(engine); } -MeshPtr CreateSphere(filament::Engine* engine, int nstack, int nslice) { +std::unique_ptr CreateSphere(filament::Engine* engine, int nstack, int nslice) { return BuiltinBuilder::Create(engine, nstack, nslice); } -MeshPtr CreateTube(filament::Engine* engine, int nstack, int nslice) { +std::unique_ptr CreateTube(filament::Engine* engine, int nstack, int nslice) { return BuiltinBuilder::Create(engine, nstack, nslice); } -MeshPtr CreateDisk(filament::Engine* engine, int nslice) { +std::unique_ptr CreateDisk(filament::Engine* engine, int nslice) { return BuiltinBuilder::Create(engine, nslice); } -MeshPtr CreateDome(filament::Engine* engine, int nstack, int nslice) { +std::unique_ptr CreateDome(filament::Engine* engine, int nstack, int nslice) { return BuiltinBuilder::Create(engine, nstack, nslice); } -MeshPtr CreateCone(filament::Engine* engine, int nstack, int nslice) { +std::unique_ptr CreateCone(filament::Engine* engine, int nstack, int nslice) { return BuiltinBuilder::Create(engine, nstack, nslice); } diff --git a/src/experimental/filament/filament/builtins.h b/src/experimental/filament/filament/builtins.h index c698bca8..5fd5c8a5 100644 --- a/src/experimental/filament/filament/builtins.h +++ b/src/experimental/filament/filament/builtins.h @@ -15,22 +15,24 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUILTINS_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUILTINS_H_ +#include + #include #include "experimental/filament/filament/mesh.h" // Generates buffers for built-in shapes. namespace mujoco { -MeshPtr CreateLine(filament::Engine* engine); -MeshPtr CreatePlane(filament::Engine* engine, int nquad); -MeshPtr CreateTriangle(filament::Engine* engine); -MeshPtr CreateBox(filament::Engine* engine, int nquad); -MeshPtr CreateLineBox(filament::Engine* engine); -MeshPtr CreateSphere(filament::Engine* engine, int nstack, int nslice); -MeshPtr CreateTube(filament::Engine* engine, int nstack, int nslice); -MeshPtr CreateDisk(filament::Engine* engine, int nslice); -MeshPtr CreateDome(filament::Engine* engine, int nstack, int nslice); -MeshPtr CreateCone(filament::Engine* engine, int nstack, int nslice); +std::unique_ptr CreateLine(filament::Engine* engine); +std::unique_ptr CreatePlane(filament::Engine* engine, int nquad); +std::unique_ptr CreateTriangle(filament::Engine* engine); +std::unique_ptr CreateBox(filament::Engine* engine, int nquad); +std::unique_ptr CreateLineBox(filament::Engine* engine); +std::unique_ptr CreateSphere(filament::Engine* engine, int nstack, int nslice); +std::unique_ptr CreateTube(filament::Engine* engine, int nstack, int nslice); +std::unique_ptr CreateDisk(filament::Engine* engine, int nslice); +std::unique_ptr CreateDome(filament::Engine* engine, int nstack, int nslice); +std::unique_ptr CreateCone(filament::Engine* engine, int nstack, int nslice); } // namespace mujoco diff --git a/src/experimental/filament/filament/imgui_bridge.h b/src/experimental/filament/filament/imgui_bridge.h index 03ee4f47..4e36b333 100644 --- a/src/experimental/filament/filament/imgui_bridge.h +++ b/src/experimental/filament/filament/imgui_bridge.h @@ -62,7 +62,7 @@ class ImguiBridge { ObjectManager* object_mgr_ = nullptr; SceneView* scene_view_ = nullptr; std::vector> renderables_; - std::vector meshes_; + std::vector> meshes_; std::unordered_map> textures_; }; diff --git a/src/experimental/filament/filament/mesh.h b/src/experimental/filament/filament/mesh.h index cc30b6d3..b75ad6de 100644 --- a/src/experimental/filament/filament/mesh.h +++ b/src/experimental/filament/filament/mesh.h @@ -185,8 +185,6 @@ class Mesh { int num_attributes_ = 0; }; -using MeshPtr = std::unique_ptr; - } // namespace mujoco #endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MESH_H_ From 3230cf99f90261e896f26ee683731c91e9bea19c Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 20 Apr 2026 02:02:01 -0700 Subject: [PATCH 093/251] Change flex constraints to eigenmodes of the stiffness matrix. This provides a reduction from 26 to 18 constraints for trilinear and from 162 to 75 for quadratic. The assembly of the constraints becomes trivial. In total the speedup for a trilinear 3x3x3 grid is about 3x. PiperOrigin-RevId: 902502398 Change-Id: I764772c7adef78da5a644f64701f842d36e4b543 --- src/engine/engine_core_constraint.c | 373 ++++----------------- src/engine/engine_derivative.c | 5 + src/engine/engine_passive.c | 7 +- src/user/user_flexcomp.cc | 1 + src/user/user_mesh.cc | 62 +++- src/user/user_model.cc | 2 + src/user/user_objects.h | 1 + src/user/user_util.cc | 99 +++++- src/user/user_util.h | 16 +- test/engine/engine_core_constraint_test.cc | 143 ++++++++ test/user/CMakeLists.txt | 2 + test/user/user_util_test.cc | 139 ++++++++ 12 files changed, 523 insertions(+), 327 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index d8c81bda..5eab4da6 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -47,47 +47,6 @@ //-------------------------- utility functions ----------------------------------------------------- -// compute 3x3 matrix inverse, storing result in out -// assumes matrix is invertible (det != 0) -static void mat3_inverse(const mjtNum* mat, mjtNum* out) { - mjtNum det = mat[0]*(mat[4]*mat[8] - mat[5]*mat[7]) - - mat[1]*(mat[3]*mat[8] - mat[5]*mat[6]) + - mat[2]*(mat[3]*mat[7] - mat[4]*mat[6]); - - out[0] = (mat[4]*mat[8] - mat[5]*mat[7]) / det; - out[1] = -(mat[1]*mat[8] - mat[2]*mat[7]) / det; - out[2] = (mat[1]*mat[5] - mat[2]*mat[4]) / det; - out[3] = -(mat[3]*mat[8] - mat[5]*mat[6]) / det; - out[4] = (mat[0]*mat[8] - mat[2]*mat[6]) / det; - out[5] = -(mat[0]*mat[5] - mat[2]*mat[3]) / det; - out[6] = (mat[3]*mat[7] - mat[4]*mat[6]) / det; - out[7] = -(mat[0]*mat[7] - mat[1]*mat[6]) / det; - out[8] = (mat[0]*mat[4] - mat[1]*mat[3]) / det; -} - - -// compute 3x3 matrix cofactor, storing result in out -static void mat3_cofactor(const mjtNum* mat, mjtNum* out) { - out[0] = mat[4]*mat[8] - mat[5]*mat[7]; - out[1] = -(mat[3]*mat[8] - mat[5]*mat[6]); - out[2] = mat[3]*mat[7] - mat[4]*mat[6]; - out[3] = -(mat[1]*mat[8] - mat[2]*mat[7]); - out[4] = mat[0]*mat[8] - mat[2]*mat[6]; - out[5] = -(mat[0]*mat[7] - mat[1]*mat[6]); - out[6] = mat[1]*mat[5] - mat[2]*mat[4]; - out[7] = -(mat[0]*mat[5] - mat[2]*mat[3]); - out[8] = mat[0]*mat[4] - mat[1]*mat[3]; -} - - -// compute 3x3 matrix determinant -static mjtNum mat3_det(const mjtNum* mat) { - return mat[0]*(mat[4]*mat[8] - mat[5]*mat[7]) - - mat[1]*(mat[3]*mat[8] - mat[5]*mat[6]) + - mat[2]*(mat[3]*mat[7] - mat[4]*mat[6]); -} - - // compute cell node Jacobians and combined chain for flex strain constraints // npc: number of nodes per cell // gindices: global indices of cell nodes in flex @@ -167,133 +126,6 @@ static void cell_strain_jacobian(int npc, int cell_nnz, } -// basis functions for flex strain constraints -static void basis(int order, int i, mjtNum p, mjtNum* phi, mjtNum* dphi) { - if (order == 1) { - *phi = (i == 0 ? 1 - p : p); - *dphi = (i == 0 ? -1 : 1); - } else { - if (i == 0) { - *phi = 2 * p * p - 3 * p + 1; - *dphi = 4 * p - 3; - } else if (i == 1) { - *phi = 4 * (p - p * p); - *dphi = 4 * (1 - 2 * p); - } else { - *phi = 2 * p * p - p; - *dphi = 4 * p - 1; - } - } -} - - -// compute shape function gradients at a parametric point -// grad: output array of size nodenum x 3 (gradient w.r.t. parametric coords) -static void shape_gradients( - int order, const mjtNum* p, mjtNum grad[][3]) { - int npoint = (order + 1) * (order + 1) * (order + 1); - int stride = order + 1; - - for (int n = 0; n < npoint; n++) { - int ix = n / (stride * stride); - int iy = (n / stride) % stride; - int iz = n % stride; - - mjtNum phi_x, phi_y, phi_z, dphi_x, dphi_y, dphi_z; - basis(order, ix, p[0], &phi_x, &dphi_x); - basis(order, iy, p[1], &phi_y, &dphi_y); - basis(order, iz, p[2], &phi_z, &dphi_z); - - grad[n][0] = dphi_x * phi_y * phi_z; - grad[n][1] = phi_x * dphi_y * phi_z; - grad[n][2] = phi_x * phi_y * dphi_z; - } -} - - -// compute dStrain/dNodePosition for volumetric invariants (I1 or J-1) -// dSdx: output array of size 3*nodenum -static void volumetric_dSdx(int invariant_type, int nodenum, mjtNum grad[][3], - const mjtNum* F, const mjtNum* Fref_inv, mjtNum* dSdx) { - mju_zero(dSdx, 3*nodenum); - - if (invariant_type == 0) { - mjtNum dSdE[9] = {1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0}; - - for (int n = 0; n < nodenum; n++) { - for (int c = 0; c < 3; c++) { - mjtNum dS = 0; - for (int ij = 0; ij < 9; ij++) { - int ii = ij / 3; - int jj = ij % 3; - - mjtNum dF_ci = 0; - for (int k = 0; k < 3; k++) { - dF_ci += grad[n][k] * Fref_inv[k*3 + ii]; - } - mjtNum dF_cj = 0; - for (int k = 0; k < 3; k++) { - dF_cj += grad[n][k] * Fref_inv[k*3 + jj]; - } - - mjtNum dC_ij = dF_ci * F[c*3 + jj] + F[c*3 + ii] * dF_cj; - dS += dSdE[ij] * 0.5 * dC_ij; - } - dSdx[3*n + c] = dS; - } - } - } else { - mjtNum cofF[9]; - mat3_cofactor(F, cofF); - - for (int n = 0; n < nodenum; n++) { - for (int c = 0; c < 3; c++) { - mjtNum dJ = 0; - for (int b = 0; b < 3; b++) { - mjtNum dF_cb = 0; - for (int k = 0; k < 3; k++) { - dF_cb += grad[n][k] * Fref_inv[k*3 + b]; - } - dJ += cofF[c*3 + b] * dF_cb; - } - dSdx[3*n + c] = dJ; - } - } - } -} - - -// compute dStrain/dNodePosition for general strain invariants -// dSdx: output array of size 3*nodenum -static void invariant_dSdx(int nodenum, mjtNum grad[][3], const mjtNum* F, - const mjtNum* Fref_inv, const mjtNum* dSdE, mjtNum* dSdx) { - mju_zero(dSdx, 3*nodenum); - - for (int n = 0; n < nodenum; n++) { - for (int c = 0; c < 3; c++) { - mjtNum dS = 0; - for (int ij = 0; ij < 9; ij++) { - int ii = ij / 3; - int jj = ij % 3; - - mjtNum dF_ci = 0; - for (int k = 0; k < 3; k++) { - dF_ci += grad[n][k] * Fref_inv[k*3 + ii]; - } - mjtNum dF_cj = 0; - for (int k = 0; k < 3; k++) { - dF_cj += grad[n][k] * Fref_inv[k*3 + jj]; - } - - mjtNum dC_ij = dF_ci * F[c*3 + jj] + F[c*3 + ii] * dF_cj; - dS += dSdE[ij] * 0.5 * dC_ij; - } - dSdx[3*n + c] = dS; - } - } -} - - // allocate efc arrays on arena, return 1 on success, 0 on failure static int arenaAllocEfc(const mjModel* m, mjData* d) { #undef MJ_M @@ -923,6 +755,16 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mju_copy3(refpos_c + 3*n, m->flex_node0 + 3*(gn + nstart)); } + // compute corotational quaternion from cell-local positions + mjtNum cell_quat[4] = {1, 0, 0, 0}; + { + mjtNum center[3] = {0.5, 0.5, 0.5}; + mjtNum mat[9]; + mju_defGradient(mat, center, xpos_c, order); + mju_mat2Rot(cell_quat, mat); + mju_negQuat(cell_quat, cell_quat); + } + // build per-cell sparse chain and node Jacobians int* cell_chain = mjSTACKALLOC(d, nv, int); int cell_nnz = 0; @@ -940,140 +782,59 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mju_zero(dense_jac, nv); } - // Gauss-Legendre quadrature points in [0,1]^3 - int nquad = order + 1; - int ngauss = nquad * nquad * nquad; + // read eigenmode data from flex_stiffness + int ndof_cell = 3 * npc; + int cell_idx = ci * m->flex_cellnum[3*f+1] * m->flex_cellnum[3*f+2] + + cj * m->flex_cellnum[3*f+2] + ck; + const mjtNum* k_cell = m->flex_stiffness + m->flex_stiffnessadr[f] + + cell_idx * ndof_cell * ndof_cell; + int neig = (int)k_cell[0]; - mjtNum gp1d[3]; - if (nquad == 2) { - gp1d[0] = 0.5 - 0.5/mju_sqrt(3.0); - gp1d[1] = 0.5 + 0.5/mju_sqrt(3.0); - } else { - gp1d[0] = 0.5 - 0.5*mju_sqrt(0.6); - gp1d[1] = 0.5; - gp1d[2] = 0.5 + 0.5*mju_sqrt(0.6); + // compute displacement in corotational frame + mjtNum* displ_c = mjSTACKALLOC(d, ndof_cell, mjtNum); + for (int n = 0; n < npc; n++) { + // rotate xpos_c to corotational frame + mjtNum xrot[3]; + mju_rotVecQuat(xrot, xpos_c + 3*n, cell_quat); + displ_c[3*n + 0] = xrot[0] - refpos_c[3*n + 0]; + displ_c[3*n + 1] = xrot[1] - refpos_c[3*n + 1]; + displ_c[3*n + 2] = xrot[2] - refpos_c[3*n + 2]; } - mjtNum (*gauss)[3] = (mjtNum (*)[3])mjSTACKALLOC(d, 3*ngauss, mjtNum); - for (int gi = 0; gi < nquad; gi++) { - for (int gj = 0; gj < nquad; gj++) { - for (int gk = 0; gk < nquad; gk++) { - int idx = gi*nquad*nquad + gj*nquad + gk; - gauss[idx][0] = gp1d[gi]; - gauss[idx][1] = gp1d[gj]; - gauss[idx][2] = gp1d[gk]; - } + // compute inverse quaternion for rotating eigenvectors to world frame + mjtNum cell_quat_inv[4]; + mju_negQuat(cell_quat_inv, cell_quat); + + // loop over eigenmodes + for (int eig = 0; eig < neig; eig++) { + const mjtNum* eigvec = k_cell + 1 + eig * ndof_cell; + + // constraint residual: dot product of scaled eigenvector with displacement + mjtNum residual = 0; + for (int j = 0; j < ndof_cell; j++) { + residual += eigvec[j] * displ_c[j]; } - } + cpos[0] = residual; - // B-bar: center-point volumetric constraints (trilinear) - if (order == 1) { - mjtNum center[3] = {0.5, 0.5, 0.5}; - mjtNum Fcur_c[9], Fref_c[9], Fref_inv_c[9], F_c[9]; - - mju_defGradient(Fcur_c, center, xpos_c, order); - mju_defGradient(Fref_c, center, refpos_c, order); - mat3_inverse(Fref_c, Fref_inv_c); - mju_mulMatMat3(F_c, Fcur_c, Fref_inv_c); - - mjtNum C_c[9], E_c[9]; - mju_mulMatTMat3(C_c, F_c, F_c); - mju_scl(E_c, C_c, 0.5, 9); - E_c[0] -= 0.5; E_c[4] -= 0.5; E_c[8] -= 0.5; - - mjtNum I1_c = E_c[0] + E_c[4] + E_c[8]; - mjtNum J_c = mat3_det(F_c); - - mjtNum grad_c[8][3]; - shape_gradients(order, center, grad_c); - - for (int inv = 0; inv < 2; inv++) { - cpos[0] = (inv == 0) ? I1_c : J_c - 1.0; - volumetric_dSdx(inv, npc, grad_c, F_c, Fref_inv_c, dSdx_local); - cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac); - if (issparse) { - mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, - cell_nnz, cell_chain); - } else { - for (int k = 0; k < cell_nnz; k++) { - dense_jac[cell_chain[k]] = strain_jac[k]; - } - mj_addConstraint(m, d, dense_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); - for (int k = 0; k < cell_nnz; k++) { - dense_jac[cell_chain[k]] = 0; - } - } + // rotate eigenvector to world frame for Jacobian + // dSdx_local[3*n+c] = Σ_d R_inv[c][d] * eigvec[3*n+d] + for (int n = 0; n < npc; n++) { + mju_rotVecQuat(dSdx_local + 3*n, eigvec + 3*n, cell_quat_inv); } - } - // Gauss integration - for (int g = 0; g < ngauss; g++) { - mjtNum* p = gauss[g]; + // contract with cell_node_jac to get sparse Jacobian + cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac); - mjtNum Fcur[9], Fref[9], Fref_inv[9], F[9]; - mju_defGradient(Fcur, p, xpos_c, order); - mju_defGradient(Fref, p, refpos_c, order); - mat3_inverse(Fref, Fref_inv); - mju_mulMatMat3(F, Fcur, Fref_inv); - - mjtNum C[9], E[9]; - mju_mulMatTMat3(C, F, F); - for (int j = 0; j < 9; j++) { - E[j] = 0.5 * C[j]; - } - E[0] -= 0.5; E[4] -= 0.5; E[8] -= 0.5; - - mjtNum I1 = E[0] + E[4] + E[8]; - mjtNum trE2 = E[0]*E[0] + E[1]*E[3] + E[2]*E[6] - + E[3]*E[1] + E[4]*E[4] + E[5]*E[7] - + E[6]*E[2] + E[7]*E[5] + E[8]*E[8]; - mjtNum I2 = 0.5 * (I1*I1 - trE2); - mjtNum I3 = mat3_det(E); - - mjtNum (*grad)[3] = (mjtNum (*)[3])mjSTACKALLOC(d, 3*npc, mjtNum); - shape_gradients(order, p, grad); - - for (int s = 0; s < 6; s++) { - if (order == 1 && (s == 0 || s == 1 || s == 2)) { - continue; + if (issparse) { + mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, + cell_nnz, cell_chain); + } else { + for (int k = 0; k < cell_nnz; k++) { + dense_jac[cell_chain[k]] = strain_jac[k]; } - - mjtNum dSdE[9]; - mju_zero(dSdE, 9); - - if (s == 0) { - cpos[0] = I1; - dSdE[0] = dSdE[4] = dSdE[8] = 1.0; - } else if (s == 1) { - cpos[0] = I2; - dSdE[0] = I1-E[0]; dSdE[4] = I1-E[4]; - dSdE[8] = I1-E[8]; - dSdE[1] = -E[1]; dSdE[3] = -E[3]; - dSdE[2] = -E[2]; dSdE[6] = -E[6]; - dSdE[5] = -E[5]; dSdE[7] = -E[7]; - } else if (s == 2) { - cpos[0] = I3; - mat3_cofactor(E, dSdE); - } else { - int offdiag_idx[3] = {1, 2, 5}; - int ij = offdiag_idx[s - 3]; - cpos[0] = E[ij]; - dSdE[ij] = 1.0; - } - - invariant_dSdx(npc, grad, F, Fref_inv, dSdE, dSdx_local); - cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac); - if (issparse) { - mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, - cell_nnz, cell_chain); - } else { - for (int k = 0; k < cell_nnz; k++) { - dense_jac[cell_chain[k]] = strain_jac[k]; - } - mj_addConstraint(m, d, dense_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); - for (int k = 0; k < cell_nnz; k++) { - dense_jac[cell_chain[k]] = 0; - } + mj_addConstraint(m, d, dense_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); + for (int k = 0; k < cell_nnz; k++) { + dense_jac[cell_chain[k]] = 0; } } } @@ -2404,6 +2165,9 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { chain2 = mjSTACKALLOC(d, nv, int); } + // pre-allocate buffer for cell body IDs (max npc = 125 for order=2) + int* cell_bodies = nnz ? mjSTACKALLOC(d, 125, int) : NULL; + // find active equality constraints for (int i=0; i < neq; i++) { // skip inactive @@ -2536,24 +2300,25 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { break; } int npc = (order+1)*(order+1)*(order+1); - int nquad = order + 1; - int ngauss = nquad * nquad * nquad; - size = (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss); // per cell + + // read eigenmode count from flex_stiffness + int ndof_cell = 3 * npc; + int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0]; + int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1]; + int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2]; + int cy = m->flex_cellnum[3*f+1]; + int cz = m->flex_cellnum[3*f+2]; + int cell_idx = ci_cell * cy * cz + cj_cell * cz + ck_cell; + const mjtNum* k_cell = m->flex_stiffness + m->flex_stiffnessadr[f] + + cell_idx * ndof_cell * ndof_cell; + size = (int)k_cell[0]; // neig stored as first element if (nnz) { - // get cell index from eq_data - int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0]; - int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1]; - int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2]; - int cy = m->flex_cellnum[3*f+1]; - int cz = m->flex_cellnum[3*f+2]; - // get the npc node body IDs for this cell int gindices[125]; mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell, NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); int nstart = m->flex_nodeadr[f]; - int* cell_bodies = mjSTACKALLOC(d, npc, int); for (int n = 0; n < npc; n++) { cell_bodies[n] = m->flex_nodebodyid[nstart + gindices[n]]; } diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 8a7eabc7..f5db7108 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -906,6 +906,11 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, continue; } + // skip if strain constraints present (stiffness handled by constraint solver) + if (m->flex_edgeequality[f] == 3) { + continue; + } + // compute scale mjtNum damping = m->flex_damping[f]; mjtNum scale = s1 + s2 * damping; diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index e6d3b6fa..bf59900b 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -229,6 +229,11 @@ static void mj_springdamper(const mjModel* m, mjData* d) { continue; } + // skip interpolated flex with strain constraints (stiffness in constraint solver) + if (m->flex_edgeequality[f] == 3) { + continue; + } + if (m->flex_interp[f]) { int order = m->flex_interp[f]; int npc = (order+1)*(order+1)*(order+1); // nodes per cell @@ -270,14 +275,12 @@ static void mj_springdamper(const mjModel* m, mjData* d) { for (int ck = 0; ck < cz; ck++) { // gather cell-local node data mjtNum quat[4]; - mjtNum p[3] = {.5, .5, .5}; mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, xpos0, xpos_c, vel_c, xpos0_c, NULL, quat); // rotate to corotational frame for (int n = 0; n < npc; n++) { mju_rotVecQuat(xpos_c+3*n, xpos_c+3*n, quat); - mji_addTo3(xpos_c+3*n, p); mju_rotVecQuat(vel_c+3*n, vel_c+3*n, quat); } diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index a86a2f33..d4c567d3 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -691,6 +691,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf mjs_setString(pe->name1, name.c_str()); } else if (equality == 3) { // create one strain constraint per cell, storing cell index in eq_data + flex->has_strain_eq = true; int cell_cx = flex->spec.cellcount[0]; int cell_cy = flex->spec.cellcount[1]; int cell_cz = flex->spec.cellcount[2]; diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 244a48f2..3e448fb5 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -3818,6 +3818,48 @@ void inline ComputeLinearStiffness(std::vector& K, } } + +// Eigendecompose cell stiffness matrix and store scaled eigenvectors. +// K_cell is n×n stored (negative convention: K_stored = -K_physical). +// Output layout in `out`: +// [0]: neig (as double) +// [1 .. neig*n]: sqrt(λ_phys_i) * v_i, row-major +// Returns number of retained eigenmodes. +static int EigendecomposeStiffness(const double* K_cell_data, + double* out, int ndof) { + // copy K_cell for in-place decomposition + std::vector mat(K_cell_data, K_cell_data + ndof * ndof); + std::vector eigval(ndof); + std::vector eigvec(ndof * ndof); + + mjuu_eigendecompose(mat.data(), eigval.data(), eigvec.data(), ndof); + + // K_stored = -K_physical, so physical eigenvalue = -eigval[i] + // retain modes where physical eigenvalue > threshold + double max_eigval = 0; + for (int i = 0; i < ndof; i++) { + max_eigval = std::max(max_eigval, std::abs(eigval[i])); + } + double threshold = max_eigval * 1e-8; + + int neig = 0; + for (int i = 0; i < ndof; i++) { + double lambda_phys = -eigval[i]; // negate to get physical eigenvalue + if (lambda_phys > threshold) { + // store sqrt(λ) * eigenvector (column i of eigvec matrix) + double scale = std::sqrt(lambda_phys); + for (int j = 0; j < ndof; j++) { + out[1 + neig * ndof + j] = scale * eigvec[j * ndof + i]; + } + neig++; + } + } + + out[0] = static_cast(neig); + return neig; +} + + //------------------ class mjCFlex implementation -------------------------------------------------- // constructor @@ -4344,7 +4386,11 @@ void mjCFlex::Compile(const mjVFS* vfs) { stiffness_cached = LoadCachedStiffness(); } - if (!stiffness_cached && young > 0 && interpolated) { + if (!stiffness_cached && interpolated && (young > 0 || has_strain_eq)) { + // use young=1 for strain constraints (eigenvectors are geometry-only) + double K_young = has_strain_eq ? 1e1 : young; + double K_poisson = has_strain_eq ? 0.3 : poisson; + int npc = pow(spec.order + 1, 3); // nodes per cell int ndof_cell = 3 * npc; int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2]; @@ -4379,11 +4425,17 @@ void mjCFlex::Compile(const mjVFS* vfs) { // compute per-cell stiffness std::vector K_cell(ndof_cell * ndof_cell, 0); - ComputeLinearStiffness(K_cell, cell_pos.data(), young, poisson, spec.order); + ComputeLinearStiffness(K_cell, cell_pos.data(), K_young, K_poisson, spec.order); + double* out = stiffness.data() + cell_idx * ndof_cell * ndof_cell; - // copy into global stiffness array - mjuu_copyvec(stiffness.data() + cell_idx * ndof_cell * ndof_cell, - K_cell.data(), ndof_cell * ndof_cell); + if (has_strain_eq) { + // eigendecompose: store [neig, sqrt(λ)*v_1, sqrt(λ)*v_2, ...] + std::fill(out, out + ndof_cell * ndof_cell, 0.0); + EigendecomposeStiffness(K_cell.data(), out, ndof_cell); + } else { + // store raw K for passive forces + std::copy(K_cell.begin(), K_cell.end(), out); + } } } } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 85245020..19672803 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -3657,6 +3657,8 @@ void mjCModel::CopyObjects(mjModel* m) { int b1 = pfl->vertbodyid[pfl->edge[k].first]; int b2 = pfl->vertbodyid[pfl->edge[k].second]; m->flexedge_rigid[edge_adr+k] = (bodies_[b1]->weldid == bodies_[b2]->weldid); + } else { + m->flexedge_rigid[edge_adr+k] = 0; } } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 8ca60124..1f80949d 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -983,6 +983,7 @@ class mjCFlex_ : public mjCBase { std::vector edgeidx_; // element edge ids std::vector stiffness; // elasticity stiffness matrix std::vector bending; // bending stiffness matrix + bool has_strain_eq = false; // true if strain constraints reference this flex // variable-size data std::vector vertbody_; // vertex body names diff --git a/src/user/user_util.cc b/src/user/user_util.cc index d9ad6cb6..b7bbc00e 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -754,6 +754,81 @@ int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double m return iter; } + +// Jacobi eigenvalue decomposition of symmetric n×n matrix. +// On output, eigenvalues are in eigval and eigenvectors are columns of eigvec. +// Both arrays must be pre-allocated: eigval[n], eigvec[n*n]. +// The input matrix mat is destroyed. +int mjuu_eigendecompose(double* mat, double* eigval, double* eigvec, int n) { + // initialize eigvec to identity + std::fill(eigvec, eigvec + n*n, 0.0); + for (int i = 0; i < n; i++) { + eigvec[i*n + i] = 1.0; + } + + const int max_sweeps = 200; + const double tol = 1e-12; + + int sweep; + for (sweep = 0; sweep < max_sweeps; sweep++) { + // check convergence: sum of squared off-diagonal elements + double off_diag = 0; + for (int i = 0; i < n; i++) { + for (int j = i+1; j < n; j++) { + off_diag += mat[i*n + j] * mat[i*n + j]; + } + } + if (off_diag < tol * tol) break; + + // sweep over all off-diagonal pairs + for (int p = 0; p < n; p++) { + for (int q = p+1; q < n; q++) { + double apq = mat[p*n + q]; + if (std::abs(apq) < tol * 1e-3) continue; + + // compute rotation angle + double app = mat[p*n + p]; + double aqq = mat[q*n + q]; + double tau = (aqq - app) / (2.0 * apq); + double t = (tau >= 0 ? 1.0 : -1.0) / + (std::abs(tau) + std::sqrt(1.0 + tau*tau)); + double c = 1.0 / std::sqrt(1.0 + t*t); + double s = t * c; + + // update matrix (Jacobi rotation) + mat[p*n + p] -= t * apq; + mat[q*n + q] += t * apq; + mat[p*n + q] = 0; + mat[q*n + p] = 0; + + for (int r = 0; r < n; r++) { + if (r == p || r == q) continue; + double mrp = mat[r*n + p]; + double mrq = mat[r*n + q]; + mat[r*n + p] = mat[p*n + r] = c*mrp - s*mrq; + mat[r*n + q] = mat[q*n + r] = s*mrp + c*mrq; + } + + // accumulate eigenvectors + for (int r = 0; r < n; r++) { + double vrp = eigvec[r*n + p]; + double vrq = eigvec[r*n + q]; + eigvec[r*n + p] = c*vrp - s*vrq; + eigvec[r*n + q] = s*vrp + c*vrq; + } + } + } + } + + // extract eigenvalues from diagonal + for (int i = 0; i < n; i++) { + eigval[i] = mat[i*n + i]; + } + + return sweep; +} + + // transform vector by pose void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4], const double vec[3]) { @@ -1189,10 +1264,10 @@ template std::string VectorToString(const std::vector& v) { return s; } -template std::string VectorToString(const std::vector& v); -template std::string VectorToString(const std::vector& v); -template std::string VectorToString(const std::vector& v); -template std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); namespace { @@ -1258,7 +1333,7 @@ template std::vector StringToVector(char* cs) { return v; } -template<> std::vector StringToVector(const std::string& s) { +template<> MJAPI std::vector StringToVector(const std::string& s) { std::vector v; std::stringstream ss(s); std::string word; @@ -1268,17 +1343,17 @@ template<> std::vector StringToVector(const std::string& s) { return v; } -template std::vector StringToVector(char* cs); -template std::vector StringToVector(char* cs); -template std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); template std::vector StringToVector(const std::string& s) { return StringToVector(const_cast(s.c_str())); } -template std::vector StringToVector(const std::string& s); -template std::vector StringToVector(const std::string& s); -template std::vector StringToVector(const std::string& s); -template std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(const std::string& s); } // namespace mujoco::user diff --git a/src/user/user_util.h b/src/user/user_util.h index 2eeb3a0f..6d17fb43 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -26,6 +26,8 @@ #include #include +#include + const double mjEPS = 1E-14; // minimum value in various calculations const double mjMINMASS = 1E-6; // minimum mass allowed @@ -157,6 +159,12 @@ double mjuu_updateFrame(double quat[4], double normal[3], const double edge[3], // eigenvalue decomposition of symmetric 3x3 matrix int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], const double mat[9]); +// Jacobi eigenvalue decomposition of symmetric n×n matrix +// eigval[n]: output eigenvalues, eigvec[n*n]: output eigenvectors (columns) +// mat[n*n]: input matrix (destroyed on output) +// returns number of sweeps used +MJAPI int mjuu_eigendecompose(double* mat, double* eigval, double* eigvec, int n); + // transform vector by pose void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4], const double vec[3]); @@ -166,7 +174,7 @@ const char* mjuu_fullInertia(double quat[4], double inertia[3], const double ful namespace mujoco::user { // utility class for handling file paths -class FilePath { +class MJAPI FilePath { public: FilePath() = default; explicit FilePath(const std::string& str) : path_(PathReduce(str)) {} @@ -251,11 +259,11 @@ struct Cleanup { std::vector FileToMemory(const char* filename); // convert vector to string separating elements by whitespace -template std::string VectorToString(const std::vector& v); +template MJAPI std::string VectorToString(const std::vector& v); // convert string to vector -template std::vector StringToVector(char *cs); -template std::vector StringToVector(const std::string& s); +template MJAPI std::vector StringToVector(char *cs); +template MJAPI std::vector StringToVector(const std::string& s); } // namespace mujoco::user diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index d795a404..93e3b947 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -622,6 +622,149 @@ TEST_F(CoreConstraintTest, StrainConstraintNoPinning) { mj_deleteModel(m); } +// Test flex strain constraint with quadratic interpolation +TEST_F(CoreConstraintTest, StrainConstraintQuadratic) { + static constexpr char xml[] = R"( + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + mjData* d = mj_makeData(m); + + mj_resetData(m, d); + mj_forward(m, d); + + // Check constraints generated + EXPECT_GT(d->ne, 0) << "Expected strain constraints"; + + // Check that initial strain is ~0 + mjtNum max_pos = 0; + for (int i = 0; i < d->ne; i++) { + if (mju_abs(d->efc_pos[i]) > max_pos) { + max_pos = mju_abs(d->efc_pos[i]); + } + } + EXPECT_LT(max_pos, 1e-6) << "Initial strain should be ~0"; + + // Check Jacobian for NaN + int nv = m->nv; + bool has_bad_jacobian = false; + for (int i = 0; i < d->ne; i++) { + for (int j = 0; j < nv; j++) { + if (mju_isBad(d->efc_J[i*nv + j])) { + has_bad_jacobian = true; + } + } + } + EXPECT_FALSE(has_bad_jacobian) << "Jacobian has NaN"; + + // Run simulation for a few steps + for (int i = 0; i < 100; i++) { + mj_step(m, d); + ASSERT_FALSE(mju_isBad(d->qpos[0])) + << "Simulation unstable at step " << i; + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +// Test quadratic passive forces (no constraints) for stability +TEST_F(CoreConstraintTest, QuadraticPassiveForceStability) { + static constexpr char xml[] = R"( + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + mjData* d = mj_makeData(m); + + // Run for 500 steps — should stay stable + for (int i = 0; i < 500; i++) { + mj_step(m, d); + ASSERT_FALSE(mju_isBad(d->qpos[0])) + << "Passive quadratic unstable at step " << i; + for (int j = 0; j < m->nv; j++) { + ASSERT_LT(mju_abs(d->qvel[j]), 1000.0) + << "Velocity exploded at step " << i; + } + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +// Test quadratic with anisotropic cells (like what mesh bounding box creates) +TEST_F(CoreConstraintTest, QuadraticAnisotropicStrain) { + static constexpr char xml[] = R"( + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + mjData* d = mj_makeData(m); + + mj_forward(m, d); + EXPECT_GT(d->ne, 0) << "Expected strain constraints"; + + // Run for 200 steps with gravity + contact + for (int i = 0; i < 200; i++) { + mj_step(m, d); + ASSERT_FALSE(mju_isBad(d->qpos[0])) + << "Anisotropic quadratic unstable at step " << i; + for (int j = 0; j < m->nv; j++) { + ASSERT_LT(mju_abs(d->qvel[j]), 1000.0) + << "Velocity exploded at step " << i + << ", qvel[" << j << "]=" << d->qvel[j]; + } + } + + mj_deleteData(d); + mj_deleteModel(m); +} + TEST_F(CoreConstraintTest, ContactSharedDofJacobian) { constexpr char xml[] = R"( diff --git a/test/user/CMakeLists.txt b/test/user/CMakeLists.txt index 912a26c0..c2d3b69d 100644 --- a/test/user/CMakeLists.txt +++ b/test/user/CMakeLists.txt @@ -40,3 +40,5 @@ mujoco_test(user_composite_test) mujoco_test(user_resource_test) mujoco_test(user_vfs_test) + +mujoco_test(user_util_test) diff --git a/test/user/user_util_test.cc b/test/user/user_util_test.cc index 9873c74b..67c91e2b 100644 --- a/test/user/user_util_test.cc +++ b/test/user/user_util_test.cc @@ -17,6 +17,8 @@ #include "src/user/user_util.h" #include +#include +#include #include #include @@ -180,5 +182,142 @@ TEST_F(UserUtilTest, VectorToStringEmpty) { EXPECT_EQ(VectorToString(v), ""); } +// utility: modified Gram-Schmidt to orthogonalize columns of Q (n x n) +static void gramSchmidt(double* Q, int n) { + for (int j = 0; j < n; j++) { + // subtract projections onto previous columns + for (int k = 0; k < j; k++) { + double dot = 0; + for (int i = 0; i < n; i++) { + dot += Q[i * n + j] * Q[i * n + k]; + } + for (int i = 0; i < n; i++) { + Q[i * n + j] -= dot * Q[i * n + k]; + } + } + // normalize + double norm = 0; + for (int i = 0; i < n; i++) { + norm += Q[i * n + j] * Q[i * n + j]; + } + norm = std::sqrt(norm); + for (int i = 0; i < n; i++) { + Q[i * n + j] /= norm; + } + } +} + +// utility: compose SPD matrix A = Q * diag(eigvals) * Q^T +static void composeMatrix(double* A, const double* Q, + const double* eigvals, int n) { + for (int i = 0; i < n; i++) { + for (int j = 0; j <= i; j++) { + double sum = 0; + for (int k = 0; k < n; k++) { + sum += Q[i * n + k] * eigvals[k] * Q[j * n + k]; + } + A[i * n + j] = sum; + A[j * n + i] = sum; + } + } +} + +TEST_F(UserUtilTest, EigendecomposeConvergence) { + // seeded RNG for reproducibility + std::mt19937_64 rng; + rng.seed(42); + std::normal_distribution dist(0, 1); + + // sweep over matrix sizes used by flex stiffness + // order=1: 8 nodes * 3 dof = 24 + // order=2: 27 nodes * 3 dof = 81 + for (int n : {24, 81}) { + int total_sweeps = 0; + int max_sweeps = 0; + int count = 0; + + // generate random orthogonal matrix Q via Gram-Schmidt + std::vector Q(n * n); + for (int i = 0; i < n * n; i++) { + Q[i] = dist(rng); + } + gramSchmidt(Q.data(), n); + + // sweep eigenvalue spectra of varying difficulty + // well-separated, clustered, wide condition number + for (double condition : {1e1, 1e3, 1e6}) { + for (double cluster : {0.0, 0.5, 0.9}) { + // construct eigenvalues + std::vector eigvals(n); + for (int i = 0; i < n; i++) { + // base: logarithmically spaced from 1 to condition + double t = (double)i / (n - 1); + double base = std::exp(t * std::log(condition)); + + // cluster: push eigenvalues toward geometric mean + double mean = std::sqrt(condition); + eigvals[i] = (1 - cluster) * base + cluster * mean; + } + + // compose A = Q * diag(eigvals) * Q^T + std::vector A(n * n); + composeMatrix(A.data(), Q.data(), eigvals.data(), n); + + // save copy for verification + std::vector A_copy(A); + + // decompose + std::vector found_eigval(n); + std::vector found_eigvec(n * n); + int sweeps = mjuu_eigendecompose( + A.data(), found_eigval.data(), + found_eigvec.data(), n); + + total_sweeps += sweeps; + if (sweeps > max_sweeps) max_sweeps = sweeps; + count++; + + // verify convergence + EXPECT_LT(sweeps, 200) + << "n=" << n + << " condition=" << condition + << " cluster=" << cluster; + + // verify A*v = lambda*v for each eigenpair + for (int i = 0; i < n; i++) { + for (int r = 0; r < n; r++) { + double Av = 0; + for (int c = 0; c < n; c++) { + Av += A_copy[r * n + c] * found_eigvec[c * n + i]; + } + double lv = found_eigval[i] * found_eigvec[r * n + i]; + EXPECT_NEAR(Av, lv, + 1e-6 * std::abs(found_eigval[i])) + << "n=" << n << " condition=" << condition + << " cluster=" << cluster + << " eigpair=" << i << " row=" << r; + } + } + + // verify all eigenvalues are positive + for (int i = 0; i < n; i++) { + EXPECT_GT(found_eigval[i], 0) + << "n=" << n << " eigenvalue " << i; + } + } + } + + double mean_sweeps = (double)total_sweeps / count; + + // assert reasonable average convergence + EXPECT_LE(mean_sweeps, 20.0) + << "n=" << n << ": mean sweeps too high"; + + // assert max sweeps within budget + EXPECT_LT(max_sweeps, 200) + << "n=" << n << ": max sweeps exceeded 200"; + } +} + } // namespace } // namespace mujoco From e6d77650f727f026da5922b3ca1857b4b9ad5b40 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 20 Apr 2026 03:09:31 -0700 Subject: [PATCH 094/251] Refactor `mju_combineSparse` to eliminate temporary buffers. Combine sparse vectors in-place by first counting total `nnz` and then working backwards from the end. This removes the need for temporary buffers in `mju_combineSparse` and its callers and speeds up the function by ~10%. PiperOrigin-RevId: 902530210 Change-Id: I4f48c327103552ab968d3915399c6067367bec9f --- src/engine/engine_core_constraint.c | 9 +-- src/engine/engine_solver.c | 19 ++---- src/engine/engine_support.c | 12 +--- src/engine/engine_util_solve.c | 9 +-- src/engine/engine_util_sparse.c | 11 ++- src/engine/engine_util_sparse.h | 67 ++++++++++--------- test/benchmark/chol_benchmark_test.cc | 8 +-- .../engine_util_sparse_benchmark_test.cc | 8 +-- 8 files changed, 57 insertions(+), 86 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 5eab4da6..7849836e 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -496,12 +496,12 @@ static void mj_equalityAnchors(const mjModel* m, const mjData* d, int eq_id, // equality constraints void mj_instantiateEquality(const mjModel* m, mjData* d) { int issparse = mj_isSparse(m), nv = m->nv; - int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL, *buf_ind = NULL; + int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL; int flex_edgeadr, flex_edgenum; int flex_vertadr, flex_vertnum; mjtNum cpos[6], pos[2][3], ref[2], dif, deriv; mjtNum quat[4], quat1[4], quat2[4], quat3[4], axis[3]; - mjtNum *jac[2], *jacdif, *data, *sparse_buf = NULL; + mjtNum *jac[2], *jacdif, *data; // disabled or no equality constraints: return if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax == 0) { @@ -520,8 +520,6 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { if (issparse) { chain = mjSTACKALLOC(d, nv, int); chain2 = mjSTACKALLOC(d, nv, int); - buf_ind = mjSTACKALLOC(d, nv, int); - sparse_buf = mjSTACKALLOC(d, nv, mjtNum); } // find active equality constraints @@ -689,8 +687,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { // compute Jacobian: sparse or dense if (issparse) { - NV = mju_combineSparse(jac[0], jac[1], 1, -deriv, NV, NV2, chain, - chain2, sparse_buf, buf_ind); + NV = mju_combineSparse(jac[0], jac[1], 1, -deriv, NV, NV2, chain, chain2); } else { mju_addToScl(jac[0], jac[1], -deriv, nv); } diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index cc9e01ce..1644f841 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -829,8 +829,6 @@ typedef struct { int* L_rowadr; // Hessian factor row addresses (nv x 1) int* LT_rownnz; // Hessian factor transpose row nonzeros (nv x 1) int* LT_rowadr; // Hessian factor transpose row addresses (nv x 1) - int* buf_ind; // index buffer for sparse addition (nv x 1) - mjtNum* buf_val; // value buffer for sparse addition (nv x 1) // Newton arrays, computed-size (MakeHessian) int nH; // number of nonzeros in Hessian H @@ -979,9 +977,7 @@ static void PrimalAllocate(mjData* d, mjPrimalContext* ctx, int flg_Newton) { if (flg_Newton) { nNum += nefc + nv; // D, cholupd if (is_elliptic) nNum += 6*nv; // LTJ - if (is_sparse) { - nNum += nv; // buf_val - } else { + if (!is_sparse) { nNum += nv*nv; // L (dense) if (is_elliptic) nNum += nv*nv; // Lcone (dense) } @@ -993,7 +989,7 @@ static void PrimalAllocate(mjData* d, mjPrimalContext* ctx, int flg_Newton) { size_t nInt = nefc; // oldstate if (is_sparse) { nInt += 3*nv + nJ; // JT sparse - if (flg_Newton) nInt += 9*nv; // Newton sparse + if (flg_Newton) nInt += 8*nv; // Newton sparse } // allocate mjtNum and int blocks @@ -1018,9 +1014,7 @@ static void PrimalAllocate(mjData* d, mjPrimalContext* ctx, int flg_Newton) { if (is_elliptic) { ctx->LTJ = numblock; numblock += 6*nv; } - if (is_sparse) { - ctx->buf_val = numblock; numblock += nv; - } else { + if (!is_sparse) { ctx->nL = nv*nv; ctx->L = numblock; numblock += ctx->nL; ctx->Lcone = is_elliptic ? numblock : NULL; @@ -1049,7 +1043,6 @@ static void PrimalAllocate(mjData* d, mjPrimalContext* ctx, int flg_Newton) { ctx->L_rowadr = intblock; intblock += nv; ctx->LT_rownnz = intblock; intblock += nv; ctx->LT_rowadr = intblock; intblock += nv; - ctx->buf_ind = intblock; intblock += nv; } // sparse: compute Jacobian transpose @@ -1616,8 +1609,7 @@ static void MakeHessian(mjData* d, mjPrimalContext* ctx) { // add mass matrix: H = J'*D*J + M mju_addToMatSparse(ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, nv, - ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind, - ctx->buf_val, ctx->buf_ind); + ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind); // compute H' sparse structure (upper triangle, required for symbolic Cholesky) mju_transposeSparse(NULL, NULL, nv, nv, ctx->HT_rownnz, ctx->HT_rowadr, ctx->HT_colind, NULL, @@ -1691,8 +1683,7 @@ static void FactorizeHessian(mjData* d, mjPrimalContext* ctx, int flg_recompute) // add mass matrix: H = J'*D*J + C mju_addToMatSparse(ctx->H, ctx->H_rownnz, ctx->H_rowadr, ctx->H_colind, nv, - ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind, - ctx->buf_val, ctx->buf_ind); + ctx->M, ctx->M_rownnz, ctx->M_rowadr, ctx->M_colind); } // numeric sparse factorization: L = chol(H) using pre-computed sparsity pattern diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 27537d39..a8725bf7 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -420,17 +420,11 @@ void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec) void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, int* rownnz, int* rowadr, int* colind) { int nv = m->nv; + // sparse if (rownnz && rowadr && colind) { - mj_markStack(d); - mjtNum* buf_val = mjSTACKALLOC(d, nv, mjtNum); - int* buf_ind = mjSTACKALLOC(d, nv, int); - - mju_addToMatSparse(dst, rownnz, rowadr, colind, nv, - d->M, m->M_rownnz, m->M_rowadr, m->M_colind, - buf_val, buf_ind); - - mj_freeStack(d); + mju_addToMatSparse(dst, rownnz, rowadr, colind, nv, d->M, + m->M_rownnz, m->M_rowadr, m->M_colind); } // dense diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 259f7c59..1f8b839b 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -144,10 +144,7 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag, int* rownnz, const int* rowadr, int* colind, mjData* d) { int rank = n; - - mj_markStack(d); - mjtNum* buf = mjSTACKALLOC(d, n, mjtNum); - int* buf_ind = mjSTACKALLOC(d, n, int); + (void) d; // backpass over rows for (int r=n-1; r >= 0; r--) { @@ -175,15 +172,13 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag, // mat(c,0:c) = mat(c,0:c) - mat(r,c) * mat(r,0:c) int nnz_c = mju_combineSparse(mat + rowadr[c], mat+rowadr[r], 1, -mat[adr+i], - rownnz[c], i+1, colind+rowadr[c], colind+rowadr[r], - buf, buf_ind); + rownnz[c], i+1, colind+rowadr[c], colind+rowadr[r]); // assign new nnz to row c rownnz[c] = nnz_c; } } - mj_freeStack(d); return rank; } diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 05960567..cfed023b 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -198,15 +198,14 @@ void mju_mulMatTVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int } -// add sparse matrix M to sparse destination matrix, requires pre-allocated buffers +// add sparse matrix M to sparse destination matrix void mju_addToMatSparse(mjtNum* dst, int* rownnz, int* rowadr, int* colind, int nr, const mjtNum* M, const int* M_rownnz, const int* M_rowadr, - const int* M_colind, - mjtNum* buf_val, int* buf_ind) { + const int* M_colind) { for (int i=0; i < nr; i++) { rownnz[i] = mju_combineSparse(dst + rowadr[i], M + M_rowadr[i], 1, 1, rownnz[i], M_rownnz[i], colind + rowadr[i], - M_colind + M_rowadr[i], buf_val, buf_ind); + M_colind + M_rowadr[i]); } } @@ -256,8 +255,8 @@ void mju_mulSymVecSparse(mjtNum* restrict res, const mjtNum* restrict mat, for (int k=diag-1; k >= 0; k--) { int j = ind[k]; mjtNum val = row[k]; - res[i] += val * vec[j]; // strict lower - res[j] += val * vec[i]; // strict upper + res[i] += val * vec[j]; // strict lower + res[j] += val * vec[i]; // strict upper } } } diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index 1350ccff..68bda070 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -62,11 +62,10 @@ MJAPI void mju_mulMatVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec MJAPI void mju_mulMatTVecSparse(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc, const int* rownnz, const int* rowadr, const int* colind); -// add sparse matrix M to sparse destination matrix, requires pre-allocated buffers +// add sparse matrix M to sparse destination matrix MJAPI void mju_addToMatSparse(mjtNum* dst, int* rownnz, int* rowadr, int* colind, int nr, const mjtNum* M, const int* M_rownnz, const int* M_rowadr, - const int* M_colind, - mjtNum* buf_val, int* buf_ind); + const int* M_colind); // add symmetric matrix (only lower triangle represented) to dense matrix MJAPI void mju_addToSymSparse(mjtNum* res, const mjtNum* mat, int n, @@ -294,8 +293,7 @@ void mju_addToSclScl(mjtNum* res, const mjtNum* vec, mjtNum scl1, mjtNum scl2, i // combine two sparse vectors: dst = a*dst + b*src, return nnz of result static inline int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b, - int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind, - mjtNum* buf, int* buf_ind) { + int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind) { // check for identical pattern if (dst_nnz == src_nnz) { if (mju_compare(dst_ind, src_ind, dst_nnz)) { @@ -305,49 +303,54 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b, } } - // copy dst into buf - if (dst_nnz) { - memcpy(buf, dst, dst_nnz * sizeof(mjtNum)); - memcpy(buf_ind, dst_ind, dst_nnz * sizeof(int)); - } + // compute total nnz of result + int nnz = mju_combineSparseCount(dst_nnz, src_nnz, dst_ind, src_ind); - // prepare to merge buf and src into dst - int bi = 0, si = 0, nnz = 0; - int buf_nnz = dst_nnz; + // set up read/write pointers at end of arrays + int bi = dst_nnz - 1, si = src_nnz - 1, w = nnz - 1; - // merge vectors - while (bi < buf_nnz && si < src_nnz) { - int badr = buf_ind[bi]; + // merge backwards + while (bi >= 0 && si >= 0) { + int badr = dst_ind[bi]; int sadr = src_ind[si]; if (badr == sadr) { - dst[nnz] = a*buf[bi++] + b*src[si++]; - dst_ind[nnz++] = badr; + dst[w] = a*dst[bi] + b*src[si]; + dst_ind[w] = badr; + bi--; + si--; } - // buf only - else if (badr < sadr) { - dst[nnz] = a*buf[bi++]; - dst_ind[nnz++] = badr; + // dst only + else if (badr > sadr) { + dst[w] = a*dst[bi]; + dst_ind[w] = badr; + bi--; } // src only else { - dst[nnz] = b*src[si++]; - dst_ind[nnz++] = sadr; + dst[w] = b*src[si]; + dst_ind[w] = sadr; + si--; } + w--; } - // the rest of src only - while (si < src_nnz) { - dst[nnz] = b*src[si]; - dst_ind[nnz++] = src_ind[si++]; + // remaining src elements + while (si >= 0) { + dst[w] = b*src[si]; + dst_ind[w] = src_ind[si]; + si--; + w--; } - // the rest of buf only - while (bi < buf_nnz) { - dst[nnz] = a*buf[bi]; - dst_ind[nnz++] = buf_ind[bi++]; + // remaining dst elements: already in place, scale by a + if (a != 1) { + while (bi >= 0) { + dst[bi] *= a; + bi--; + } } return nnz; diff --git a/test/benchmark/chol_benchmark_test.cc b/test/benchmark/chol_benchmark_test.cc index 004e0c2a..ba8d94bf 100644 --- a/test/benchmark/chol_benchmark_test.cc +++ b/test/benchmark/chol_benchmark_test.cc @@ -22,7 +22,6 @@ #include #include #include -#include "src/engine/engine_memory.h" #include "src/engine/engine_support.h" #include "src/engine/engine_util_solve.h" #include "src/engine/engine_util_sparse.h" @@ -352,10 +351,6 @@ constexpr int kNumUpdateVectors = 25; int ABSL_ATTRIBUTE_NOINLINE mju_cholUpdateSparse_old( mjtNum* mat, mjtNum* x, int n, int flg_plus, const int* rownnz, const int* rowadr, const int* colind, int x_nnz, int* x_ind, mjData* d) { - mj_markStack(d); - int* buf_ind = mjSTACKALLOC(d, n, int); - mjtNum* sparse_buf = mjSTACKALLOC(d, n, mjtNum); - int rank = n, i = x_nnz - 1; while (i >= 0) { int nnz = rownnz[x_ind[i]], adr = rowadr[x_ind[i]]; @@ -372,10 +367,9 @@ int ABSL_ATTRIBUTE_NOINLINE mju_cholUpdateSparse_old( mju_combineSparseInc(mat + adr, x, n, 1 / c, (flg_plus ? s / c : -s / c), nnz - 1, i, colind + adr, x_ind); int new_x_nnz = mju_combineSparse(x, mat + adr, c, -s, i, nnz - 1, x_ind, - colind + adr, sparse_buf, buf_ind); + colind + adr); i = i - 1 + (new_x_nnz - i); } - mj_freeStack(d); return rank; } diff --git a/test/benchmark/engine_util_sparse_benchmark_test.cc b/test/benchmark/engine_util_sparse_benchmark_test.cc index a82be98e..3a28a291 100644 --- a/test/benchmark/engine_util_sparse_benchmark_test.cc +++ b/test/benchmark/engine_util_sparse_benchmark_test.cc @@ -99,8 +99,7 @@ int ABSL_ATTRIBUTE_NOINLINE combineSparse_baseline(mjtNum* dst, mjtNum a, mjtNum b, int dst_nnz, int src_nnz, int* dst_ind, - const int* src_ind, - mjtNum* buf, int* buf_ind) { + const int* src_ind) { // check for identical pattern if (compare_baseline(dst_ind, src_ind, dst_nnz)) { // combine mjtNum data directly @@ -116,8 +115,7 @@ int ABSL_ATTRIBUTE_NOINLINE combineSparse_new(mjtNum* dst, mjtNum a, mjtNum b, int dst_nnz, int src_nnz, int* dst_ind, - const int* src_ind, - mjtNum* buf, int* buf_ind) { + const int* src_ind) { // check for identical pattern if (compare_memcmp(dst_ind, src_ind, dst_nnz)) { // combine mjtNum data directly @@ -372,7 +370,7 @@ static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) { // in order to trigger all if's in combineSparse func(H+rowadr[c], H+rowadr[r], 1, -H[adr+i], rownnz[c], rownnz[c], - colind+rowadr[c], colind+rowadr[c], NULL, NULL); + colind+rowadr[c], colind+rowadr[c]); } } } From eb31027a7ca2f5587abc14d09b032ccc68d4dea7 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Mon, 20 Apr 2026 03:27:10 -0700 Subject: [PATCH 095/251] Simplify RenderTarget creation. Remove the RenderTargetTextureType enum and, instead, create RenderTargets directly from pixel formats. PiperOrigin-RevId: 902536708 Change-Id: I2683aa9db23c011212087d93df0260ad27621170 --- .../filament/filament/filament_context.cc | 15 +++-- .../filament/filament/render_target.cc | 45 +++++++++++---- .../filament/filament/render_target.h | 15 +++-- .../filament/filament/scene_view.cc | 9 ++- src/experimental/filament/filament/texture.cc | 55 ++++++------------- src/experimental/filament/filament/texture.h | 32 ++++------- 6 files changed, 87 insertions(+), 84 deletions(-) diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index 4da0d63b..09f26cfa 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -191,14 +191,17 @@ void FilamentContext::SetFrameBuffer(int framebuffer) { } void FilamentContext::PrepareRenderTargets(int width, int height) { - color_target_ = std::make_unique( - engine_, RenderTargetTextureType::kColor, - RenderTargetTextureType::kDepth); + RenderTargetConfig config; + DefaultRenderTargetConfig(&config); + + config.color_format = mjPIXEL_FORMAT_RGB8; + config.depth_format = mjPIXEL_FORMAT_DEPTH32F; + color_target_ = std::make_unique(engine_, config); color_target_->Prepare(width, height); - depth_target_ = std::make_unique( - engine_, RenderTargetTextureType::kDepthColor, - RenderTargetTextureType::kDepth); + config.color_format = mjPIXEL_FORMAT_R32F; + config.depth_format = mjPIXEL_FORMAT_DEPTH32F; + depth_target_ = std::make_unique(engine_, config); depth_target_->Prepare(width, height); } diff --git a/src/experimental/filament/filament/render_target.cc b/src/experimental/filament/filament/render_target.cc index eb2d3490..b01fb94c 100644 --- a/src/experimental/filament/filament/render_target.cc +++ b/src/experimental/filament/filament/render_target.cc @@ -30,10 +30,14 @@ namespace mujoco { +void DefaultRenderTargetConfig(RenderTargetConfig* config) { + config->color_format = mjPIXEL_FORMAT_RGBA8; + config->depth_format = mjPIXEL_FORMAT_DEPTH32F; +} + RenderTarget::RenderTarget(filament::Engine* engine, - RenderTargetTextureType color, - RenderTargetTextureType depth) - : engine_(engine), color_type_(color), depth_type_(depth) {} + const RenderTargetConfig& config) + : engine_(engine), config_(config) {} RenderTarget::~RenderTarget() noexcept { Destroy(); @@ -47,10 +51,29 @@ void RenderTarget::Prepare(int width, int height) { width_ = width; height_ = height; - color_texture_ = - std::make_unique(engine_, color_type_, width, height); - depth_texture_ = - std::make_unique(engine_, depth_type_, width, height); + TextureConfig color_config; + DefaultTextureConfig(&color_config); + Texture::InternalFlags color_flags; + color_config.width = width; + color_config.height = height; + color_config.target = mjTEXTURE_2D; + color_config.format = config_.color_format; + color_config.color_space = mjCOLORSPACE_LINEAR; + color_config.format = mjPIXEL_FORMAT_RGB8; + color_flags.color_attachment = true; + color_texture_ = std::make_unique(engine_, color_config, color_flags); + + TextureConfig depth_config; + DefaultTextureConfig(&depth_config); + Texture::InternalFlags depth_flags; + depth_config.width = width; + depth_config.height = height; + depth_config.target = mjTEXTURE_2D; + depth_config.format = config_.depth_format; + depth_config.color_space = mjCOLORSPACE_LINEAR; + depth_config.format = mjPIXEL_FORMAT_DEPTH32F; + depth_flags.depth_attachment = true; + depth_texture_ = std::make_unique(engine_, depth_config, depth_flags); filament::RenderTarget::Builder builder; builder.texture(filament::RenderTarget::AttachmentPoint::COLOR, @@ -65,19 +88,19 @@ void RenderTarget::ReadColorPixels(filament::Renderer* renderer, uint8_t* bytes, filament::backend::PixelDataFormat format; filament::backend::PixelDataType type; size_t expected_num_bytes = 0; - switch (color_type_) { - case RenderTargetTextureType::kColor: + switch (config_.color_format) { + case mjPIXEL_FORMAT_RGB8: format = filament::backend::PixelDataFormat::RGB; type = filament::backend::PixelDataType::UBYTE; expected_num_bytes = width_ * height_ * 3; break; - case RenderTargetTextureType::kDepthColor: + case mjPIXEL_FORMAT_R32F: format = filament::backend::PixelDataFormat::R; type = filament::backend::PixelDataType::FLOAT; expected_num_bytes = width_ * height_ * sizeof(float); break; default: - mju_error("Unsupported pixel format: %d", color_type_); + mju_error("Unsupported pixel format: %d", config_.color_format); return; } if (num_bytes != expected_num_bytes) { diff --git a/src/experimental/filament/filament/render_target.h b/src/experimental/filament/filament/render_target.h index ee033de9..22a02143 100644 --- a/src/experimental/filament/filament/render_target.h +++ b/src/experimental/filament/filament/render_target.h @@ -25,13 +25,21 @@ namespace mujoco { +// Defines the basic properties of a render target. +struct RenderTargetConfig { + mjtPixelFormat color_format; + mjtPixelFormat depth_format; +}; + +// Initializes the RenderTargetConfig to default values. +void DefaultRenderTargetConfig(RenderTargetConfig* config); + // Manages a filament RenderTarget and the textures which are bound to it. class RenderTarget { public: // Defines the types of textures to create for the color and depth // attachments. - RenderTarget(filament::Engine* engine, RenderTargetTextureType color, - RenderTargetTextureType depth); + RenderTarget(filament::Engine* engine, const RenderTargetConfig& config); ~RenderTarget() noexcept; RenderTarget(const RenderTarget&) = delete; @@ -58,11 +66,10 @@ class RenderTarget { void Destroy(); filament::Engine* engine_ = nullptr; + RenderTargetConfig config_; filament::RenderTarget* render_target_ = nullptr; std::unique_ptr color_texture_ = nullptr; std::unique_ptr depth_texture_ = nullptr; - RenderTargetTextureType color_type_; - RenderTargetTextureType depth_type_; int width_ = 0; int height_ = 0; }; diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index d6832f6f..18d950d7 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -325,9 +325,12 @@ void SceneView::AddReflectiveRenderable(Renderable* renderable) { // Ensure we have the same number of render targets as we do reflective // renderables. while (reflect_targets_.size() < reflectives_.size()) { - reflect_targets_.push_back(std::make_unique( - engine_, RenderTargetTextureType::kReflectionColor, - RenderTargetTextureType::kDepth)); + RenderTargetConfig config; + DefaultRenderTargetConfig(&config); + + config.color_format = mjPIXEL_FORMAT_RGBA8; + config.depth_format = mjPIXEL_FORMAT_DEPTH32F; + reflect_targets_.push_back(std::make_unique(engine_, config)); } // Prepare a render target for the reflective renderable. diff --git a/src/experimental/filament/filament/texture.cc b/src/experimental/filament/filament/texture.cc index 1ce62774..1277ec44 100644 --- a/src/experimental/filament/filament/texture.cc +++ b/src/experimental/filament/filament/texture.cc @@ -99,6 +99,10 @@ static filament::Texture::InternalFormat GetTextureInternalFormat( return filament::Texture::InternalFormat::RGB8; case mjPIXEL_FORMAT_RGBA8: return filament::Texture::InternalFormat::RGBA8; + case mjPIXEL_FORMAT_R32F: + return filament::Texture::InternalFormat::R32F; + case mjPIXEL_FORMAT_DEPTH32F: + return filament::Texture::InternalFormat::DEPTH32F; default: mju_error("Unsupported format: %d", (int)config.format); return filament::Texture::InternalFormat::UNUSED; @@ -114,7 +118,8 @@ void DefaultTextureConfig(TextureConfig* config) { std::memset(config, 0, sizeof(TextureConfig)); } -Texture::Texture(filament::Engine* engine, const TextureConfig& config) +Texture::Texture(filament::Engine* engine, const TextureConfig& config, + InternalFlags flags) : engine_(engine), config_(config) { if (IsCompressed(config_)) { // We defer creation of compressed textures until Upload() is called. In @@ -139,45 +144,19 @@ Texture::Texture(filament::Engine* engine, const TextureConfig& config) builder.sampler(filament::Texture::Sampler::SAMPLER_2D); } - if (config_.color_space != mjCOLORSPACE_SRGB) { - builder.usage(filament::Texture::Usage::GEN_MIPMAPPABLE | - filament::Texture::Usage::SAMPLEABLE | - filament::Texture::Usage::UPLOADABLE); + filament::Texture::Usage usage = filament::Texture::Usage::DEFAULT; + if (flags.color_attachment) { + usage |= filament::Texture::Usage::COLOR_ATTACHMENT; + usage |= filament::Texture::Usage::BLIT_SRC; + } else if (flags.depth_attachment) { + usage |= filament::Texture::Usage::DEPTH_ATTACHMENT; + usage |= filament::Texture::Usage::BLIT_SRC; + } else if (config_.color_space != mjCOLORSPACE_SRGB) { + usage |= filament::Texture::Usage::GEN_MIPMAPPABLE; } - texture_ = builder.build(*engine_); -} + builder.usage(usage); -Texture::Texture(filament::Engine* engine, RenderTargetTextureType type, - int width, int height) : engine_(engine) { - filament::Texture::Builder builder; - builder.width(width); - builder.height(height); - switch (type) { - case RenderTargetTextureType::kColor: - builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT | - filament::Texture::Usage::BLIT_SRC); - builder.format(filament::Texture::InternalFormat::RGB8); - break; - case RenderTargetTextureType::kDepth: - builder.usage(filament::Texture::Usage::DEPTH_ATTACHMENT | - filament::Texture::Usage::SAMPLEABLE); - builder.format(filament::Texture::InternalFormat::DEPTH32F); - break; - case RenderTargetTextureType::kDepthColor: - builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT | - filament::Texture::Usage::BLIT_SRC); - builder.format(filament::Texture::InternalFormat::R32F); - break; - case RenderTargetTextureType::kReflectionColor: - builder.usage(filament::Texture::Usage::COLOR_ATTACHMENT | - filament::Texture::Usage::BLIT_SRC | - filament::Texture::Usage::SAMPLEABLE); - builder.format(filament::Texture::InternalFormat::RGBA8); - break; - default: - mju_error("Unknown type: %d", static_cast(type)); - } - texture_ = builder.build(*engine); + texture_ = builder.build(*engine_); } Texture::~Texture() { diff --git a/src/experimental/filament/filament/texture.h b/src/experimental/filament/filament/texture.h index 44b0ea31..b1493470 100644 --- a/src/experimental/filament/filament/texture.h +++ b/src/experimental/filament/filament/texture.h @@ -25,29 +25,13 @@ // Functions for creating filament textures. namespace mujoco { -// The types of textures we can create. For internal use only. -enum class TextureTarget { - // A standard 2D image with a width and a height. - kNormal2d, - // A 2D texture split up into the 6 faces of a cube. - kCube, -}; - -// The different types of textures we can create for a render target. -// For internal use only. -enum class RenderTargetTextureType { - kColor, - kDepth, - kDepthColor, - kReflectionColor, -}; - // Pixel formats for textures. typedef enum mjtPixelFormat_ { mjPIXEL_FORMAT_UNKNOWN = 0, mjPIXEL_FORMAT_R8, mjPIXEL_FORMAT_RGB8, mjPIXEL_FORMAT_RGBA8, + mjPIXEL_FORMAT_R32F, mjPIXEL_FORMAT_DEPTH32F, mjPIXEL_FORMAT_KTX, } mjtPixelFormat; @@ -98,12 +82,16 @@ void DefaultTextureConfig(TextureConfig* config); // Wrapper around a filament::Texture. class Texture { public: - // Creates a texture with the given data. - Texture(filament::Engine* engine, const TextureConfig& config); + // Flags for internal use. + struct InternalFlags { + InternalFlags() : color_attachment(false), depth_attachment(false) {} + bool color_attachment; + bool depth_attachment; + }; - // Creates a texture for use with a render target, for internal use. - Texture(filament::Engine* engine, RenderTargetTextureType type, int width, - int height); + // Creates a texture with the given data. + Texture(filament::Engine* engine, const TextureConfig& config, + InternalFlags flags = InternalFlags()); ~Texture(); From fa7b36d1111f1504cf215cdb1b1c61cc47bd8a6b Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 20 Apr 2026 04:16:27 -0700 Subject: [PATCH 096/251] Fix undefined reference errors for StringToVector with GCC and LTO. This CL addresses link failures when building MuJoCo with GCC and Link Time Optimization (LTO) enabled: - Declared the `StringToVector(const std::string&)` specialization in `user_util.h` to prevent the compiler from incorrectly trying to instantiate the general template. - Added an explicit instantiation for `StringToVector(char*)` in `user_util.cc` to provide the definition needed by its `const std::string&` counterpart. PiperOrigin-RevId: 902557141 Change-Id: I27b585cf1deb276d4150a5cbe6cf7e59c323a20a --- src/user/user_util.cc | 1 + src/user/user_util.h | 1 + 2 files changed, 2 insertions(+) diff --git a/src/user/user_util.cc b/src/user/user_util.cc index b7bbc00e..b98516fc 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -1346,6 +1346,7 @@ template<> MJAPI std::vector StringToVector(const std::string& s) { template MJAPI std::vector StringToVector(char* cs); template MJAPI std::vector StringToVector(char* cs); template MJAPI std::vector StringToVector(char* cs); +template MJAPI std::vector StringToVector(char* cs); template std::vector StringToVector(const std::string& s) { diff --git a/src/user/user_util.h b/src/user/user_util.h index 6d17fb43..b2842d85 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -264,6 +264,7 @@ template MJAPI std::string VectorToString(const std::vector& v); // convert string to vector template MJAPI std::vector StringToVector(char *cs); template MJAPI std::vector StringToVector(const std::string& s); +template<> MJAPI std::vector StringToVector(const std::string& s); } // namespace mujoco::user From 508e581ba9232930dc116cf3d3781808d70fcfed Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 20 Apr 2026 04:40:34 -0700 Subject: [PATCH 097/251] Optimize flex by pinning nodes in empty cells. This change introduces an optimization for flexcomp objects defined by a mesh. It identifies grid cells that do not contain any mesh vertices and marks them as empty. Nodes that are exclusively part of empty cells are pinned, preventing them from moving. Stiffness computations are skipped for empty cells, reducing computational cost. The total mass is now distributed only among the non-pinned nodes. PiperOrigin-RevId: 902565735 Change-Id: Id0a9a685536d5e18a3e42124a25ab08ff3a918f2 --- src/engine/engine_core_constraint.c | 5 +- src/engine/engine_core_smooth.c | 30 ++--- src/engine/engine_core_util.c | 42 +++--- src/engine/engine_core_util.h | 2 +- src/engine/engine_derivative.c | 98 +++++++++----- src/engine/engine_passive.c | 30 +++-- src/engine/engine_vis_visualize.c | 15 ++- src/user/user_flexcomp.cc | 115 +++++++++++++++- src/user/user_flexcomp.h | 5 + src/user/user_mesh.cc | 5 + src/user/user_objects.h | 1 + test/user/user_flex_test.cc | 197 ++++++++++++++++++++++++++++ 12 files changed, 455 insertions(+), 90 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 7849836e..0f8f4a00 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -743,7 +743,10 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mjtNum* refpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); for (int n = 0; n < npc; n++) { int gn = gindices[n]; - if (m->flex_centered[f]) { + if (m->flex_centered[f] || + (m->flex_node[3*(gn + nstart)+0] == 0 && + m->flex_node[3*(gn + nstart)+1] == 0 && + m->flex_node[3*(gn + nstart)+2] == 0)) { mju_copy3(xpos_c + 3*n, d->xpos + 3*bodyid[gn]); } else { mju_mulMatVec3(xpos_c + 3*n, d->xmat + 9*bodyid[gn], m->flex_node + 3*(gn + nstart)); diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 8f3412f1..6b052147 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -564,16 +564,13 @@ void mj_flex(const mjModel* m, mjData* d) { // 0: vertices are the mesh vertices, 1: vertices are interpolated from nodal dofs if (m->flex_interp[f] == 0) { - // centered: copy body position - if (m->flex_centered[f]) { - for (int i=vstart; i < vend; i++) { + for (int i=vstart; i < vend; i++) { + if (m->flex_centered[f] || + (m->flex_vert[3*i+0] == 0 && + m->flex_vert[3*i+1] == 0 && + m->flex_vert[3*i+2] == 0)) { mji_copy3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]); - } - } - - // non-centered: map from local to global - else { - for (int i=vstart; i < vend; i++) { + } else { mji_mulMatVec3(d->flexvert_xpos+3*i, d->xmat+9*m->flex_vertbodyid[i], m->flex_vert+3*i); mji_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]); } @@ -585,13 +582,14 @@ void mj_flex(const mjModel* m, mjData* d) { int nodenum = nend - nstart; mj_markStack(d); mjtNum* nodexpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); - if (m->flex_centered[f]) { - for (int i=nstart; i < nend; i++) { - mji_copy3(nodexpos + 3*(i-nstart), d->xpos + 3*m->flex_nodebodyid[i]); - } - } else { - for (int i=nstart; i < nend; i++) { - int j = i - nstart; + for (int i=nstart; i < nend; i++) { + int j = i - nstart; + if (m->flex_centered[f] || + (m->flex_node[3*i+0] == 0 && + m->flex_node[3*i+1] == 0 && + m->flex_node[3*i+2] == 0)) { + mji_copy3(nodexpos + 3*j, d->xpos + 3*m->flex_nodebodyid[i]); + } else { mji_mulMatVec3(nodexpos + 3*j, d->xmat + 9*m->flex_nodebodyid[i], m->flex_node + 3*i); mji_addTo3(nodexpos + 3*j, d->xpos + 3*m->flex_nodebodyid[i]); } diff --git a/src/engine/engine_core_util.c b/src/engine/engine_core_util.c index bf2b6994..52c5d46f 100644 --- a/src/engine/engine_core_util.c +++ b/src/engine/engine_core_util.c @@ -988,28 +988,36 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], //-------------------------- miscellaneous utilities ----------------------------------------------- // gather global node positions and velocities -void mju_flexGatherState(const mjModel* m, mjData* d, int f, mjtNum* xpos, mjtNum* vel) { +void mju_flexGatherState(const mjModel* m, const mjData* d, int f, mjtNum* xpos, mjtNum* vel) { int nodenum = m->flex_nodenum[f]; int nstart = m->flex_nodeadr[f]; int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; - // compute positions - if (m->flex_centered[f]) { - for (int i=0; i < nodenum; i++) { - mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]); - if (vel) { - mju_copy3(vel + 3*i, d->qvel + m->body_dofadr[bodyid[i]]); - } + // compute positions and velocities + for (int i=0; i < nodenum; i++) { + int bid = bodyid[i]; + if (m->flex_centered[f] || + (m->flex_node[3*(i+nstart)+0] == 0 && + m->flex_node[3*(i+nstart)+1] == 0 && + m->flex_node[3*(i+nstart)+2] == 0)) { + mju_copy3(xpos + 3*i, d->xpos + 3*bid); + } else { + mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bid, m->flex_node + 3*(i+nstart)); + mju_addTo3(xpos + 3*i, d->xpos + 3*bid); } - } else { - mjtNum screw[6]; - for (int i=0; i < nodenum; i++) { - mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart)); - mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]); - if (vel) { - mj_objectVelocity(m, d, mjOBJ_BODY, bodyid[i], screw, 0); - mju_copy3(vel + 3*i, screw + 3); - } + + if (vel) { + mjtNum body_vel[6]; + mj_objectVelocity(m, d, mjOBJ_BODY, bid, body_vel, 0); // returns [omega, v_CoM] in world frame + + // linear velocity at CoM + mju_copy3(vel + 3*i, body_vel + 3); + + // add omega x (xpos - xipos) + mjtNum r[3], cross[3]; + mju_sub3(r, xpos + 3*i, d->xipos + 3*bid); + mju_cross(cross, body_vel, r); + mju_addTo3(vel + 3*i, cross); } } } diff --git a/src/engine/engine_core_util.h b/src/engine/engine_core_util.h index 949aa257..36ec5fd0 100644 --- a/src/engine/engine_core_util.h +++ b/src/engine/engine_core_util.h @@ -130,7 +130,7 @@ MJAPI void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], //-------------------------- miscellaneous --------------------------------------------------------- // gather global node positions and velocities -MJAPI void mju_flexGatherState(const mjModel* m, mjData* d, int f, mjtNum* xpos, mjtNum* vel); +MJAPI void mju_flexGatherState(const mjModel* m, const mjData* d, int f, mjtNum* xpos, mjtNum* vel); // extract 6D force:torque for one contact, in contact frame MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]); diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index f5db7108..875fb70c 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -880,17 +880,62 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, const int* dof_indices, int ndof, int nband) { int nv = m->nv; - // build global2local map for ADDH - int* global2local = NULL; + // compute upper bounds across all interpolated flexes + int max_nodenum = 0; + int max_npc = 0; + for (int f = 0; f < m->nflex; f++) { + if (!m->flex_interp[f]) continue; + if (m->flex_rigid[f]) continue; + int order = m->flex_interp[f]; + int npc = (order+1)*(order+1)*(order+1); + if (npc > max_npc) max_npc = npc; + if (m->flex_nodenum[f] > max_nodenum) max_nodenum = m->flex_nodenum[f]; + } + + // nothing to do + if (max_npc == 0) { + return; + } + + int max_dim_c = 3 * max_npc; + + // single unconditional markStack + mj_markStack(d); + + // global2local map for ADDH + int* global2local = mjSTACKALLOC(d, nv, int); if (op == mjFLEXOP_ADDH) { - mj_markStack(d); - global2local = mjSTACKALLOC(d, nv, int); mju_fillInt(global2local, -1, nv); for (int i=0; inflex; f++) { // only process flex_interp @@ -899,10 +944,10 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, } // get stiffness and damping - mjtNum* k = m->flex_stiffness + m->flex_stiffnessadr[f]; + mjtNum* K = m->flex_stiffness + m->flex_stiffnessadr[f]; // skip if rigid or no stiffness - if (m->flex_rigid[f] || k[0] == 0) { + if (m->flex_rigid[f] || K[0] == 0) { continue; } @@ -926,27 +971,9 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; - int nodenum = m->flex_nodenum[f]; int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; - // standard stack allocation - mj_markStack(d); - mjtNum* xpos = mjSTACKALLOC(d, 3*nodenum, mjtNum); - - // per-cell arrays int dim_c = 3 * npc; - mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* K_rot_cell = mjSTACKALLOC(d, dim_c*dim_c, mjtNum); - - // sparse Jacobian for one cell - int* J_rownnz = mjSTACKALLOC(d, dim_c, int); - int* J_rowadr = mjSTACKALLOC(d, dim_c, int); - mjtNum* J_val = mjSTACKALLOC(d, dim_c*nv, mjtNum); - int* J_colind = mjSTACKALLOC(d, dim_c*nv, int); - - // temp allocations for chain - int* chain_colind = mjSTACKALLOC(d, nv, int); - mjtNum* blk_jac = mjSTACKALLOC(d, 3*nv, mjtNum); // gather raw node positions (unrotated) mju_flexGatherState(m, d, f, xpos, NULL); @@ -956,6 +983,16 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, for (int ci = 0; ci < cx; ci++) { for (int cj = 0; cj < cy; cj++) { for (int ck = 0; ck < cz; ck++) { + // get cell stiffness + mjtNum* k_cell = K + cell_idx * 3*npc * 3*npc; + + // skip empty cells: stiffness buffer is zero-initialized at compile time + // (user_model.cc), and non-empty cells have strictly positive diagonal + if (k_cell[0] == 0) { + cell_idx++; + continue; + } + // gather cell-local node positions int gindices[125]; // max npc = 125 for quadratic mjtNum quat[4]; @@ -967,9 +1004,6 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, mju_quat2Mat(R, quat); mju_transpose(RT, R, 3, 3); - // get cell stiffness - mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc; - // compute K_rot_cell = RT * K_cell * R (block-wise) mju_zero(K_rot_cell, dim_c*dim_c); for (int a = 0; a < npc; a++) { @@ -1025,9 +1059,7 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, addJTBJ_mulSparse(m, d, res, vec, J_rownnz, J_rowadr, J_colind, J_val, K_rot_cell, dim_c); } else if (op == mjFLEXOP_ADDH) { - mj_markStack(d); // H -= J_cell^T * K_rot_cell * J_cell (banded format) - mjtNum* J_reduced = mjSTACKALLOC(d, dim_c*ndof, mjtNum); mju_zero(J_reduced, dim_c*ndof); for (int i = 0; i < dim_c; i++) { @@ -1043,7 +1075,6 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, } // KJ = K_rot_cell * J_reduced (dim_c x ndof) - mjtNum* KJ = mjSTACKALLOC(d, dim_c*ndof, mjtNum); mju_mulMatMat(KJ, K_rot_cell, J_reduced, dim_c, dim_c, ndof); // H[i,j] -= J_reduced[k,i] * KJ[k,j], store lower triangle in banded format @@ -1056,20 +1087,15 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, res[i*nband + nband-1-(i-j)] -= val; } } - mj_freeStack(d); } cell_idx++; } } } - - mj_freeStack(d); } - if (op == mjFLEXOP_ADDH) { - mj_freeStack(d); // free global2local - } + mj_freeStack(d); } diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index bf59900b..8c76a573 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -273,6 +273,15 @@ static void mj_springdamper(const mjModel* m, mjData* d) { for (int ci = 0; ci < cx; ci++) { for (int cj = 0; cj < cy; cj++) { for (int ck = 0; ck < cz; ck++) { + // get cell stiffness matrix + mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc; + + // skip empty cells (zero stiffness) + if (k_cell[0] == 0) { + cell_idx++; + continue; + } + // gather cell-local node data mjtNum quat[4]; mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, xpos0, @@ -289,9 +298,6 @@ static void mj_springdamper(const mjModel* m, mjData* d) { mji_addScl3(displ_c+3*n, xpos_c+3*n, xpos0_c+3*n, -1); } - // get cell stiffness matrix - mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc; - // compute force in corotational frame if (enbl_spring) { mju_mulMatVec(frc_c, k_cell, displ_c, 3*npc, 3*npc); @@ -332,12 +338,20 @@ static void mj_springdamper(const mjModel* m, mjData* d) { // apply accumulated forces to bodies for (int i = 0; i < nodenum; i++) { mju_scl3(dmp_g+3*i, dmp_g+3*i, m->flex_damping[f]); - if (m->flex_centered[f]) { - if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bodyid[i]], frc_g+3*i); - if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bodyid[i]], dmp_g+3*i); + int bid = bodyid[i]; + int nidx = i + m->flex_nodeadr[f]; + + // fast path: node at body origin (not pinned), direct DOF write + if (m->body_dofnum[bid] > 0 && + (m->flex_centered[f] || + (m->flex_node[3*nidx+0] == 0 && + m->flex_node[3*nidx+1] == 0 && + m->flex_node[3*nidx+2] == 0))) { + if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bid], frc_g+3*i); + if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bid], dmp_g+3*i); } else { - if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bodyid[i], d->qfrc_spring); - if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bodyid[i], d->qfrc_damper); + if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_spring); + if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_damper); } } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index c0072dbb..327f0108 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1459,11 +1459,18 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, for (int i=0; i < NX; i++) { for (int j=0; j < NY; j++) { for (int k=0; k < NZ; k++) { - int offset = 3*(i*NY*NZ + j*NZ + k); + int n0 = i*NY*NZ + j*NZ + k; + + // skip if this node is pinned (no joints on its body) + if (m->body_jntnum[bodyid[n0]] == 0) { + continue; + } + + int offset = 3*n0; int offset1 = 3*((i+1)*NY*NZ + j*NZ + k); int offset2 = 3*(i*NY*NZ + (j+1)*NZ + k); int offset3 = 3*(i*NY*NZ + j*NZ + (k+1)); - if (i < NX-1) { + if (i < NX-1 && m->body_jntnum[bodyid[(i+1)*NY*NZ + j*NZ + k]] > 0) { mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN); if (!thisgeom) { return; @@ -1472,7 +1479,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset1); releaseGeom(&thisgeom, scn); } - if (j < NY-1) { + if (j < NY-1 && m->body_jntnum[bodyid[i*NY*NZ + (j+1)*NZ + k]] > 0) { mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN); if (!thisgeom) { return; @@ -1481,7 +1488,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+offset, xpos+offset2); releaseGeom(&thisgeom, scn); } - if (k < NZ-1) { + if (k < NZ-1 && m->body_jntnum[bodyid[i*NY*NZ + j*NZ + (k+1)]] > 0) { mjvGeom* thisgeom = acquireGeom(scn, i, mjCAT_DECOR, mjOBJ_UNKNOWN); if (!thisgeom) { return; diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index d4c567d3..6bd14bb0 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -98,6 +98,71 @@ mjCFlexcomp::mjCFlexcomp(void) { } +// identify empty cells and pin nodes exclusively in empty cells +void mjCFlexcomp::MarkEmptyCells(mjCFlex* flex, const double* points, + int npnt, const double minmax[6], + int nx, int ny, int nz) { + int cx = flex->spec.cellcount[0]; + int cy = flex->spec.cellcount[1]; + int cz = flex->spec.cellcount[2]; + int ncells = cx * cy * cz; + int order = flex->spec.order; + + // determine which cells contain mesh vertices + flex->cell_empty.assign(ncells, true); + for (int i = 0; i < npnt; i++) { + // compute parametric coordinates of mesh vertex in [0, 1]^3 + // for flat meshes (zero extent along an axis), default to 0.5 + double dx = minmax[3] - minmax[0]; + double dy = minmax[4] - minmax[1]; + double dz = minmax[5] - minmax[2]; + double sx = dx > 0 ? (points[3*i+0] - minmax[0]) / dx : 0.5; + double sy = dy > 0 ? (points[3*i+1] - minmax[1]) / dy : 0.5; + double sz = dz > 0 ? (points[3*i+2] - minmax[2]) / dz : 0.5; + + // find containing cell + int ci = std::min((int)(sx * cx), cx - 1); + int cj = std::min((int)(sy * cy), cy - 1); + int ck = std::min((int)(sz * cz), cz - 1); + ci = std::max(ci, 0); + cj = std::max(cj, 0); + ck = std::max(ck, 0); + + flex->cell_empty[ci * cy * cz + cj * cz + ck] = false; + } + + // pin nodes that belong exclusively to empty cells + for (int gi = 0; gi < nx; gi++) { + for (int gj = 0; gj < ny; gj++) { + for (int gk = 0; gk < nz; gk++) { + // find all cells that reference this node + bool all_empty = true; + int ci_min = std::max(0, gi == 0 ? 0 : (gi - 1) / order); + int ci_max = std::min(cx - 1, gi / order); + int cj_min = std::max(0, gj == 0 ? 0 : (gj - 1) / order); + int cj_max = std::min(cy - 1, gj / order); + int ck_min = std::max(0, gk == 0 ? 0 : (gk - 1) / order); + int ck_max = std::min(cz - 1, gk / order); + + for (int ci = ci_min; ci <= ci_max && all_empty; ci++) { + for (int cj = cj_min; cj <= cj_max && all_empty; cj++) { + for (int ck = ck_min; ck <= ck_max && all_empty; ck++) { + if (!flex->cell_empty[ci * cy * cz + cj * cz + ck]) { + all_empty = false; + } + } + } + } + + if (all_empty) { + int idx = gi * ny * nz + gj * nz + gk; + pinned[idx] = true; + } + } + } + } +} + // make flexcomp object bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vfs) { @@ -588,15 +653,30 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf int nz = flex->spec.cellcount[2] * flex->spec.order + 1; int nnode = nx * ny * nz; + // mark empty cells and pin nodes exclusively in empty cells + MarkEmptyCells(flex, point.data(), npnt, minmax, nx, ny, nz); + + // if MarkEmptyCells pinned any nodes, force centered=false + // so that pf->node (local positions) is saved to the model + if (centered) { + for (int i = 0; i < nnode; i++) { + if (pinned[i]) { + centered = false; + break; + } + } + } + std::vector node(3 * nnode, 0); int idx = 0; // Simpson's rule weights for quadratic mass distribution double massP2[3] = {1. / 6., 2. / 3., 1. / 6.}; - // compute per-node mass for trilinear: - // mass / nnode (uniform), or use Simpson for quadratic - double node_mass_uniform = mass / nnode; + + + // collect created bodies for mass normalization + std::vector node_bodies; for (int gi = 0; gi < nx; gi++) { for (int gj = 0; gj < ny; gj++) { @@ -629,7 +709,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf // mass distribution if (doftype == mjFCOMPDOF_TRILINEAR) { - pb->mass = node_mass_uniform; + pb->mass = 1.0; } else { // local index within the cell for mass computation int li = gi % flex->spec.order; @@ -639,14 +719,15 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf int ncells_i = (gi > 0 && gi < nx-1 && li == 0) ? 2 : 1; int ncells_j = (gj > 0 && gj < ny-1 && lj == 0) ? 2 : 1; int ncells_k = (gk > 0 && gk < nz-1 && lk == 0) ? 2 : 1; - // use Simpson weights scaled by cell count + // use Simpson weights double wi = massP2[li == 0 ? 0 : li]; double wj = massP2[lj == 0 ? 0 : lj]; double wk = massP2[lk == 0 ? 0 : lk]; - pb->mass = mass * wi * wj * wk * ncells_i * ncells_j * ncells_k - / (flex->spec.cellcount[0] * flex->spec.cellcount[1] * flex->spec.cellcount[2]); + pb->mass = wi * wj * wk * ncells_i * ncells_j * ncells_k; } + node_bodies.push_back(pb); + pb->inertia[0] = pb->mass*(2.0*inertiabox*inertiabox)/3.0; pb->inertia[1] = pb->mass*(2.0*inertiabox*inertiabox)/3.0; pb->inertia[2] = pb->mass*(2.0*inertiabox*inertiabox)/3.0; @@ -671,6 +752,21 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } } + // normalize masses so total equals prescribed mass + double total_mass = 0; + for (mjsBody* pb : node_bodies) { + total_mass += pb->mass; + } + if (total_mass > 0) { + double scale = mass / total_mass; + for (mjsBody* pb : node_bodies) { + pb->mass *= scale; + pb->inertia[0] *= scale; + pb->inertia[1] *= scale; + pb->inertia[2] *= scale; + } + } + if (!centered) { mjs_setDouble(pf->node, node.data(), node.size()); } @@ -698,6 +794,11 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf for (int ci = 0; ci < cell_cx; ci++) { for (int cj = 0; cj < cell_cy; cj++) { for (int ck = 0; ck < cell_cz; ck++) { + // skip empty cells + if (!flex->cell_empty.empty() && + flex->cell_empty[ci * cell_cy * cell_cz + cj * cell_cz + ck]) { + continue; + } mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); mjs_setDefault(pe->element, &model->Default()->spec); pe->type = mjEQ_FLEXSTRAIN; diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index 8624be7b..07977a39 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -116,6 +116,11 @@ class mjCFlexcomp { std::string plugin_name; std::string plugin_instance_name; mjsPlugin plugin; + + private: + // identify empty cells and pin nodes exclusively in empty cells + void MarkEmptyCells(mjCFlex* flex, const double* points, int npnt, + const double minmax[6], int nx, int ny, int nz); }; #endif // MUJOCO_SRC_USER_USER_FLEXCOMP_H_ diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 3e448fb5..ae54b634 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4407,6 +4407,11 @@ void mjCFlex::Compile(const mjVFS* vfs) { for (int ck = 0; ck < cz; ck++) { int cell_idx = ci * cy * cz + cj * cz + ck; + // skip stiffness computation for empty cells (no mesh content) + if (!cell_empty.empty() && cell_empty[cell_idx]) { + continue; + } + // gather cell's local node positions std::vector cell_pos(3 * npc); int local = 0; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 1f80949d..5c81e40c 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -984,6 +984,7 @@ class mjCFlex_ : public mjCBase { std::vector stiffness; // elasticity stiffness matrix std::vector bending; // bending stiffness matrix bool has_strain_eq = false; // true if strain constraints reference this flex + std::vector cell_empty; // true if cell contains no mesh geometry // variable-size data std::vector vertbody_; // vertex body names diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index 5482568d..52e0bb30 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -1031,5 +1031,202 @@ TEST_F(UserFlexTest, FlexNoConstraintsWarning) { mj_deleteModel(m); } +TEST_F(UserFlexTest, EmptyCellNodePinning) { + // A 2x2x2 grid with a box mesh that fills all cells. + // No nodes should be pinned. + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // A 2x2x2 grid with trilinear order has (2+1)^3 = 27 node positions. + int nadr = m->flex_nodeadr[0]; + int nnode = m->flex_nodenum[0]; + EXPECT_EQ(nnode, 27); + + // All cells are occupied by the box, so no node should be pinned. + int pinned = 0; + for (int n = nadr; n < nadr + nnode; n++) { + int bid = m->flex_nodebodyid[n]; + if (m->body_jntnum[bid] == 0) { + pinned++; + } + } + EXPECT_EQ(pinned, 0); + + // Verify simulation works + mjData* d = mj_makeData(m); + for (int i = 0; i < 10; i++) { + mj_step(m, d); + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(UserFlexTest, EmptyCellNodePinningMesh) { + // Load bunny_multicell.xml which has a 3x3x3 grid. + // The bunny mesh only occupies some cells, so many nodes should be pinned. + const std::string xml_path = + GetModelPath("flex/bunny_multicell.xml"); + std::array error; + mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // 3x3x3 grid, order=1: (3+1)^3 = 64 node positions + int nadr = m->flex_nodeadr[0]; + int nnode = m->flex_nodenum[0]; + EXPECT_EQ(nnode, 64); + + // Count pinned nodes (no joints) + int pinned = 0; + int free_nodes = 0; + for (int n = nadr; n < nadr + nnode; n++) { + int bid = m->flex_nodebodyid[n]; + if (m->body_jntnum[bid] == 0) { + pinned++; + } else { + free_nodes++; + } + } + + // At least some nodes should be pinned since the bunny doesn't fill all cells + EXPECT_GT(pinned, 0) << "Expected some nodes to be pinned from empty cells"; + EXPECT_GT(free_nodes, 0) << "Expected some nodes to remain free"; + EXPECT_EQ(pinned + free_nodes, nnode); + + // Verify the model can simulate + mjData* d = mj_makeData(m); + mj_forward(m, d); + for (int i = 0; i < 10; i++) { + mj_step(m, d); + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) { + // Regression test for ci_min calculation with order=2. + // A 2x1x1 quadratic grid has nodes at gi=0..4 (5 nodes per axis). + // We place mesh vertices only in cell 0 (x in [0, 0.5]), so cell 1 is empty. + // + // Node gi=3 belongs only to cell 1 (1*2 <= 3 <= 2*2). + // With the old formula (gi-order)/order = (3-2)/2 = 0, it would also check + // cell 0 (non-empty), incorrectly marking gi=3 as non-pinned. + // Single hex element at x=[0,0.3], well inside cell 0 of a 3x1x1 grid. + // Anchor vertex at x=1.0 extends the bounding box to [0,1]^3. + // The 3x1x1 quadratic grid splits at x=0.33, 0.67. + // Cell 0 has vertices, cells 1 and 2 are empty. + // Interior nodes for cells 1,2 should be pinned to the parent body. + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // 3x1x1 quadratic grid: (3*2+1) * (1*2+1) * (1*2+1) = 7*3*3 = 63 nodes + int nadr = m->flex_nodeadr[0]; + int nnode = m->flex_nodenum[0]; + EXPECT_EQ(nnode, 63); + + // Count pinned nodes: pinned nodes are assigned to the parent body. + int parent_bid = mj_name2id(m, mjOBJ_BODY, "parent"); + ASSERT_GT(parent_bid, 0); + int pinned = 0; + for (int n = nadr; n < nadr + nnode; n++) { + if (m->flex_nodebodyid[n] == parent_bid) { + pinned++; + } + } + + // Cells 1 and 2 are empty, so nodes exclusively in those cells are pinned. + // Nodes at gi=3..6 (with any gj, gk) are only in cells 1 and/or 2. + // That's 4 * 3 * 3 = 36 nodes. + EXPECT_EQ(pinned, 36); + + mj_deleteData(mj_makeData(m)); + mj_deleteModel(m); +} + +TEST_F(UserFlexTest, TotalMassTrilinear) { + static constexpr char xml[] = R"( + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + double total_mass = 0; + for (int i = 1; i < m->nbody; ++i) { + total_mass += m->body_mass[i]; + } + + EXPECT_NEAR(total_mass, 1.5, 1e-5); + mj_deleteModel(m); +} + +TEST_F(UserFlexTest, TotalMassQuadratic) { + static constexpr char xml[] = R"( + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + double total_mass = 0; + for (int i = 1; i < m->nbody; ++i) { + total_mass += m->body_mass[i]; + } + + EXPECT_NEAR(total_mass, 2.0, 1e-5); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco + From 86ad41b9c2ab9de5fd95c60e6da842bf2b804be4 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Mon, 20 Apr 2026 04:50:38 -0700 Subject: [PATCH 098/251] Use RenderableParams for creating Renderables. PiperOrigin-RevId: 902569516 Change-Id: Id233a5ef7fe86ce5902ce2552d8d0fec6ce860b7 --- .../filament/filament/imgui_bridge.cc | 5 +- .../filament/filament/renderable.cc | 62 ++++++++++--------- .../filament/filament/renderable.h | 31 ++++++---- .../filament/filament/scene_geom_util.cc | 11 ++-- 4 files changed, 63 insertions(+), 46 deletions(-) diff --git a/src/experimental/filament/filament/imgui_bridge.cc b/src/experimental/filament/filament/imgui_bridge.cc index f5553ea8..5ee492eb 100644 --- a/src/experimental/filament/filament/imgui_bridge.cc +++ b/src/experimental/filament/filament/imgui_bridge.cc @@ -268,8 +268,11 @@ void ImguiBridge::Update() { void ImguiBridge::PrepareRenderables(int count) { while (renderables_.size() < count) { + RenderableParams config; + DefaultRenderableParams(&config); + config.shading_model = ShadingModel::Ux; auto& r = renderables_.emplace_back( - std::make_unique(Renderable::Usage::Ux, object_mgr_)); + std::make_unique(object_mgr_, config)); r->SetCastShadows(false); r->SetReceiveShadows(false); r->SetBlendOrder(static_cast(renderables_.size())); diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 4fc9aaf3..3e518938 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -16,7 +16,6 @@ #include #include -#include #include #include @@ -31,8 +30,12 @@ namespace mujoco { -Renderable::Renderable(Usage usage, ObjectManager* object_mgr) - : usage_(usage), object_mgr_(object_mgr) {} +void DefaultRenderableParams(RenderableParams* params) { + params->shading_model = ShadingModel::SceneObject; +} + +Renderable::Renderable(ObjectManager* object_mgr, const RenderableParams& params) + : object_mgr_(object_mgr), params_(params) {} Renderable::~Renderable() noexcept { while (!entities_.empty()) { @@ -188,18 +191,19 @@ void Renderable::RemoveFromScene(filament::Scene* scene) { void Renderable::UpdateMaterial(const MaterialParams& params, const MaterialTextures& textures) { - params_ = params; - textures_ = textures; + material_params_ = params; + material_textures_ = textures; AssignMaterial(DrawMode::Color, GetColorMaterialType()); - if (usage_ == Usage::SceneObject) { + if (params_.shading_model == ShadingModel::SceneObject) { AssignMaterial(DrawMode::Depth, ObjectManager::kUnlitDepth); AssignMaterial(DrawMode::Segmentation, ObjectManager::kUnlitSegmentation); } for (int i = 0; i < kNumDrawModes; ++i) { if (instances_[i]) { - UpdateMaterialInstance(instances_[i], params_, textures_, object_mgr_); + UpdateMaterialInstance(instances_[i], material_params_, + material_textures_, object_mgr_); } } SetDrawMode(draw_mode_); @@ -225,16 +229,16 @@ void Renderable::AssignMaterial(DrawMode mode, } const MaterialParams& Renderable::GetMaterialParams() const { - return params_; + return material_params_; } const MaterialTextures& Renderable::GetMaterialTextures() const { - return textures_; + return material_textures_; } void Renderable::SetDrawMode(DrawMode mode) { // Only SceneObjects support non-color draw modes. - if (usage_ != Usage::SceneObject) { + if (params_.shading_model != ShadingModel::SceneObject) { mode = DrawMode::Color; } @@ -333,21 +337,21 @@ void Renderable::SetWireframe(bool wireframe) { ObjectManager::MaterialType Renderable::GetColorMaterialType() const { - if (usage_ == Usage::DecorLines) { + if (params_.shading_model == ShadingModel::DecorLines) { return ObjectManager::kUnlitLine; - } else if (usage_ == Usage::Decor) { - return ObjectManager::kUnlitSegmentation; - } else if (usage_ == Usage::Ux) { + } else if (params_.shading_model == ShadingModel::Decor) { + return ObjectManager::kUnlitDecor; + } else if (params_.shading_model == ShadingModel::Ux) { return ObjectManager::kUnlitUi; - } else if (textures_.orm) { + } else if (material_textures_.orm) { return ObjectManager::kPbrPacked; - } else if (textures_.metallic) { + } else if (material_textures_.metallic) { return ObjectManager::kPbr; - } else if (textures_.roughness) { + } else if (material_textures_.roughness) { return ObjectManager::kPbr; - } else if (params_.metallic >= 0) { + } else if (material_params_.metallic >= 0) { return ObjectManager::kPbr; - } else if (params_.roughness >= 0) { + } else if (material_params_.roughness >= 0) { return ObjectManager::kPbr; } @@ -363,35 +367,35 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { has_texcoords = (it != attribs.end()); } - if (textures_.color == nullptr) { - if (params_.color.a < 1.0f) { + if (material_textures_.color == nullptr) { + if (material_params_.color.a < 1.0f) { return ObjectManager::kPhongColorFade; - } else if (params_.reflective) { + } else if (material_params_.reflective) { return ObjectManager::kPhongColorReflect; } else { return ObjectManager::kPhongColor; } - } else if (textures_.color->GetFilamentTexture()->getTarget() == + } else if (material_textures_.color->GetFilamentTexture()->getTarget() == filament::Texture::Sampler::SAMPLER_CUBEMAP) { - if (params_.color.a < 1.0f) { + if (material_params_.color.a < 1.0f) { return ObjectManager::kPhongCubeFade; - } else if (params_.reflective) { + } else if (material_params_.reflective) { return ObjectManager::kPhongCubeReflect; } else { return ObjectManager::kPhongCube; } } else if (has_texcoords) { - if (params_.color.a < 1.0f) { + if (material_params_.color.a < 1.0f) { return ObjectManager::kPhong2dUvFade; - } else if (params_.reflective) { + } else if (material_params_.reflective) { return ObjectManager::kPhong2dUvReflect; } else { return ObjectManager::kPhong2dUv; } } else { - if (params_.color.a < 1.0f) { + if (material_params_.color.a < 1.0f) { return ObjectManager::kPhong2dFade; - } else if (params_.reflective) { + } else if (material_params_.reflective) { return ObjectManager::kPhong2dReflect; } else { return ObjectManager::kPhong2d; diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 016824b8..50674c82 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -28,6 +28,21 @@ namespace mujoco { +// The shading model (material) for a Renderable. +enum class ShadingModel { + SceneObject, + Decor, + DecorLines, + Ux, +}; + +// Configuration parameters for a Renderable. +struct RenderableParams { + ShadingModel shading_model; +}; + +void DefaultRenderableParams(RenderableParams* params); + // A collection of meshes and a material that, together, define an object that // can be rendered in a scene. // @@ -38,19 +53,11 @@ namespace mujoco { // assigns the same material instance to all of them. class Renderable { public: - // How the material is to be used for rendering. - enum class Usage { - SceneObject, - Decor, - DecorLines, - Ux, - }; - // Default filament values for priority and layer mask. static constexpr std::uint8_t kDefaultPriority = 4; static constexpr std::uint8_t kDefaultLayerMask = 0x01; - Renderable(Usage usage, ObjectManager* object_mgr); + Renderable(ObjectManager* object_mgr, const RenderableParams& params); ~Renderable() noexcept; Renderable(const Renderable&) = delete; @@ -145,11 +152,11 @@ class Renderable { ObjectManager::MaterialType GetColorMaterialType() const; - Usage usage_; ObjectManager* object_mgr_; + RenderableParams params_; filament::MaterialInstance* instances_[kNumDrawModes] = {nullptr}; - MaterialParams params_; - MaterialTextures textures_; + MaterialParams material_params_; + MaterialTextures material_textures_; DrawMode draw_mode_ = DrawMode::Color; filament::Scene* assigned_scene_ = nullptr; std::vector entities_; diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index 1b20fd18..f68f620e 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -459,14 +459,17 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, std::unique_ptr CreateGeomRenderable( const mjvGeom& geom, const mjvScene* scene, ObjectManager* object_mgr, ModelObjects* model_objs, const float headpos[3]) { - Renderable::Usage usage = Renderable::Usage::SceneObject; + ShadingModel shading_model = ShadingModel::SceneObject; if (geom.type == mjGEOM_LINE || geom.type == mjGEOM_LINEBOX) { - usage = Renderable::Usage::DecorLines; + shading_model = ShadingModel::DecorLines; } else if (geom.category == mjCAT_DECOR) { - usage = Renderable::Usage::Decor; + shading_model = ShadingModel::Decor; } - auto renderable = std::make_unique(usage, object_mgr); + RenderableParams config; + DefaultRenderableParams(&config); + config.shading_model = shading_model; + auto renderable = std::make_unique(object_mgr, config); // The order of these calls is important. e.g. We need to create the filament // renderable entities before we can set their transform. From 35f7db8e91a58dfda279f599bb347a8221ac34f2 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 20 Apr 2026 06:48:16 -0700 Subject: [PATCH 099/251] Add `StringToVector` overload for `char*`. This overload allows `StringToVector` to accept a `char*` argument when parsing into a `std::vector`, by converting the `char*` to `std::string` before processing. PiperOrigin-RevId: 902611443 Change-Id: I2048918f915c1bac74522f368ea28f23ca9a6885 --- src/user/user_util.cc | 4 ++++ src/user/user_util.h | 1 + 2 files changed, 5 insertions(+) diff --git a/src/user/user_util.cc b/src/user/user_util.cc index b98516fc..6f5bc1a4 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -1333,6 +1333,10 @@ template std::vector StringToVector(char* cs) { return v; } +template<> MJAPI std::vector StringToVector(char* cs) { + return StringToVector(std::string(cs)); +} + template<> MJAPI std::vector StringToVector(const std::string& s) { std::vector v; std::stringstream ss(s); diff --git a/src/user/user_util.h b/src/user/user_util.h index b2842d85..65a21a7b 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -264,6 +264,7 @@ template MJAPI std::string VectorToString(const std::vector& v); // convert string to vector template MJAPI std::vector StringToVector(char *cs); template MJAPI std::vector StringToVector(const std::string& s); +template<> MJAPI std::vector StringToVector(char* cs); template<> MJAPI std::vector StringToVector(const std::string& s); } // namespace mujoco::user From cad734ae7048e300672247253f09f6a14c8655fd Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Mon, 20 Apr 2026 07:13:05 -0700 Subject: [PATCH 100/251] Introduce Trs type. Combines a translation, rotation, and size into a single struct. PiperOrigin-RevId: 902620586 Change-Id: I628bfdedebde8c83239215efe0bee4a338eabd06 --- .../filament/filament/math_util.h | 24 +++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/src/experimental/filament/filament/math_util.h b/src/experimental/filament/filament/math_util.h index ee3b2741..5438b5e4 100644 --- a/src/experimental/filament/filament/math_util.h +++ b/src/experimental/filament/filament/math_util.h @@ -46,15 +46,31 @@ inline filament::math::float4 ReadFloat4(const T* arr, int index = 0) { // Reads a mat3 from an array buffer in the model/scene. template -inline filament::math::mat3 ReadMat3(const T* arr, int index = 0) { +inline filament::math::mat3f ReadMat3(const T* arr, int index = 0) { // clang-format off const T* ptr = arr + (9 * index); - return filament::math::mat3(ptr[0], ptr[3], ptr[6], - ptr[1], ptr[4], ptr[7], - ptr[2], ptr[5], ptr[8]); + return filament::math::mat3f(ptr[0], ptr[3], ptr[6], + ptr[1], ptr[4], ptr[7], + ptr[2], ptr[5], ptr[8]); // clang-format on } +// A tuple of translation, rotation, and size. +struct Trs { + filament::math::float3 translation{0.0f, 0.0f, 0.0f}; + filament::math::mat3f rotation; + // Note: this is _slightly_ different than scale. For example, for capsules, + // the size determines the length of the tube and the radius of the domes, + // but the shape remains a capsule. + filament::math::float3 size{1.0f, 1.0f, 1.0f}; + + // Converts the TRS to a transform matrix. + filament::math::mat4f ToTransform() const { + return filament::math::mat4f(rotation, translation) * + filament::math::mat4f::scaling(size); + } +}; + // Calculates a reflection matrix for a plane defined by its transform. filament::math::mat4 ToReflectionMatrix(const filament::math::mat4& xform); From 2d12dee025b58c9f535b9adc9311006f9d9f4e4a Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 20 Apr 2026 07:13:17 -0700 Subject: [PATCH 101/251] Add dof="2d" option to flexcomp for in-plane deformations. PiperOrigin-RevId: 902620719 Change-Id: Ib06d3f7b9439e1d90a8373a4289ade0c327e72e4 --- doc/XMLreference.rst | 6 +- model/flex/gripper_2d.xml | 429 +++++++++++++++++++++++++++++++++++ src/user/user_flexcomp.cc | 11 + src/user/user_flexcomp.h | 1 + src/xml/xml_native_reader.cc | 3 +- test/user/user_flex_test.cc | 65 ++++++ 6 files changed, 513 insertions(+), 2 deletions(-) create mode 100644 model/flex/gripper_2d.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 7e5cfe5a..be74eede 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -3593,7 +3593,7 @@ saving the XML: .. _body-flexcomp-dof: -:at:`dof`: :at-val:`[full, radial, trilinear, quadratic], "full"` +:at:`dof`: :at-val:`[full, radial, trilinear, quadratic, 2d], "full"` The parametrization of the flex's degrees of freedom (dofs). See the video on the right illustrating the different parametrizations with deformable spheres. The three models in the video are respectively `sphere_full `__, @@ -3608,6 +3608,10 @@ saving the XML: requires a free joint at the flex's parent in order for free body motion to be possible. This type of parametrization is appropriate for shapes that are relatively spherical. + **2d** + Two orthogonal translational dofs (X and Y) per vertex. This restricts the motion of the vertices to planes + parallel to the parent body's X-Y plane. + **trilinear** Three translational dofs at each corner of the bounding box of the flex, for a total of 24 dofs for the entire flex, independent of the number of vertices. The positions of the vertices are updated using trilinear diff --git a/model/flex/gripper_2d.xml b/model/flex/gripper_2d.xml new file mode 100644 index 00000000..b6c6c611 --- /dev/null +++ b/model/flex/gripper_2d.xml @@ -0,0 +1,429 @@ + + + + + + diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 6bd14bb0..c62b5d6f 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -613,6 +613,17 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf } } + // add two orthogonal sliders (x and y only) + else if (doftype == mjFCOMPDOF_2D) { + for (int j=0; j < 2; j++) { + mjsJoint* jnt = mjs_addJoint(pb, 0); + jnt->type = mjJNT_SLIDE; + mjuu_setvec(jnt->pos, 0, 0, 0); + mjuu_setvec(jnt->axis, 0, 0, 0); + jnt->axis[j] = 1; + } + } + // construct body name, add to vertbody char txt[100]; mju::sprintf_arr(txt, "%s_%d", name.c_str(), i); diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index 07977a39..782a7915 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -45,6 +45,7 @@ typedef enum _mjtDof { mjFCOMPDOF_RADIAL, mjFCOMPDOF_TRILINEAR, mjFCOMPDOF_QUADRATIC, + mjFCOMPDOF_2D, mjNFCOMPDOFS } mjtDof; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 360c46be..5e17aa1d 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -932,7 +932,8 @@ const mjMap fdof_map[mjNFCOMPDOFS] = { {"full", mjFCOMPDOF_FULL}, {"radial", mjFCOMPDOF_RADIAL}, {"trilinear", mjFCOMPDOF_TRILINEAR}, - {"quadratic", mjFCOMPDOF_QUADRATIC} + {"quadratic", mjFCOMPDOF_QUADRATIC}, + {"2d", mjFCOMPDOF_2D} }; diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index 52e0bb30..f3df253c 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -1227,6 +1227,71 @@ TEST_F(UserFlexTest, TotalMassQuadratic) { mj_deleteModel(m); } +TEST_F(UserFlexTest, Dof2d) { + // 3x3 grid with dof="2d": 9 vertices, 2 DOFs each -> nv = 18 + static constexpr char xml_2d[] = R"( + + + + + + + + )"; + + // same model with dof="full" for comparison: 9 vertices, 3 DOFs each -> nv = 27 + static constexpr char xml_full[] = R"( + + + + + + + + )"; + + std::array error; + + // load 2d model + mjModel* m_2d = LoadModelFromString(xml_2d, error.data(), error.size()); + ASSERT_THAT(m_2d, NotNull()) << error.data(); + mjData* d_2d = mj_makeData(m_2d); + + // load full model + mjModel* m_full = LoadModelFromString(xml_full, error.data(), error.size()); + ASSERT_THAT(m_full, NotNull()) << error.data(); + mjData* d_full = mj_makeData(m_full); + + // verify DOF counts + EXPECT_EQ(m_2d->nv, 18); // 9 vertices * 2 DOFs + EXPECT_EQ(m_full->nv, 27); // 9 vertices * 3 DOFs + + // same number of vertices and elements + EXPECT_EQ(m_2d->nflexvert, m_full->nflexvert); + EXPECT_EQ(m_2d->nflexelem, m_full->nflexelem); + + // each body has 2 DOFs in 2d mode, 3 in full mode + for (int i = 1; i < m_2d->nbody; i++) { + EXPECT_EQ(m_2d->body_dofnum[i], 2) << "body " << i; + } + for (int i = 1; i < m_full->nbody; i++) { + EXPECT_EQ(m_full->body_dofnum[i], 3) << "body " << i; + } + + // simulate a few steps to make sure nothing crashes + for (int i = 0; i < 10; i++) { + mj_step(m_2d, d_2d); + mj_step(m_full, d_full); + } + + mj_deleteModel(m_2d); + mj_deleteModel(m_full); + mj_deleteData(d_2d); + mj_deleteData(d_full); +} + } // namespace } // namespace mujoco From a8a5afc8dcbf905b71d9a9c3e900dc1815d5851b Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 20 Apr 2026 08:29:16 -0700 Subject: [PATCH 102/251] Cache benchmark data in `engine_util_sparse_benchmark_test` PiperOrigin-RevId: 902651349 Change-Id: I760e3f696b37699366c40c3c93a74e2b1b35678e --- .../engine_util_sparse_benchmark_test.cc | 351 +++++++++++------- 1 file changed, 209 insertions(+), 142 deletions(-) diff --git a/test/benchmark/engine_util_sparse_benchmark_test.cc b/test/benchmark/engine_util_sparse_benchmark_test.cc index 3a28a291..81609beb 100644 --- a/test/benchmark/engine_util_sparse_benchmark_test.cc +++ b/test/benchmark/engine_util_sparse_benchmark_test.cc @@ -14,7 +14,6 @@ // A benchmark for comparing different implementations of mj_solveLD. -#include #include #include @@ -31,14 +30,179 @@ namespace { using CombineFuncPtr = decltype(&mju_combineSparse); using TransposeFuncPtr = decltype(&mju_transposeSparse); -using SqrMatTDFuncPtr = decltype(&mju_sqrMatTDSparse); -// number of steps to roll out before benchmarking -static const int kNumWarmupSteps = 500; +// ================================ Cached Data ================================ -// ----------------------------- old functions -------------------------------- +// ---- MatVecSparse data ---- +struct MatVecData { + int nv; + int nefc; + int nJ; + std::vector efc_J; + std::vector efc_J_rownnz, efc_J_rowadr, efc_J_colind, efc_J_rowsuper; + std::vector vec; +}; +MatVecData& GetMatVecData() { + static MatVecData data = [] { + MatVecData d; + mjModel* m = LoadModelFromPath("flex/flag.xml"); + mjData* dat = mj_makeData(m); + for (int i = 0; i < 500; i++) { + mj_step(m, dat); + } + + d.nv = m->nv; + d.nefc = dat->nefc; + d.nJ = dat->nJ; + d.efc_J.assign(dat->efc_J, dat->efc_J + d.nJ); + d.efc_J_rownnz.assign(dat->efc_J_rownnz, dat->efc_J_rownnz + d.nefc); + d.efc_J_rowadr.assign(dat->efc_J_rowadr, dat->efc_J_rowadr + d.nefc); + d.efc_J_colind.assign(dat->efc_J_colind, dat->efc_J_colind + d.nJ); + d.efc_J_rowsuper.assign(dat->efc_J_rowsuper, dat->efc_J_rowsuper + d.nefc); + + // compute direction: vec = -M^{-1} * (Ma - qfrc_smooth - qfrc_constraint) + mj_markStack(dat); + mjtNum* Ma = mj_stackAllocNum(dat, m->nv); + mjtNum* grad = mj_stackAllocNum(dat, m->nv); + mjtNum* Mgrad = mj_stackAllocNum(dat, m->nv); + mj_mulM(m, dat, Ma, dat->qacc); + for (int i = 0; i < m->nv; i++) { + grad[i] = Ma[i] - dat->qfrc_smooth[i] - dat->qfrc_constraint[i]; + } + mj_solveM(m, dat, Mgrad, grad, 1); + d.vec.resize(m->nv); + mju_scl(d.vec.data(), Mgrad, -1, m->nv); + mj_freeStack(dat); + + mj_deleteData(dat); + mj_deleteModel(m); + return d; + }(); + return data; +} + +// ---- CombineSparse data ---- +struct CombineData { + int nv; + std::vector H; + std::vector rownnz, rowadr, colind; +}; + +CombineData& GetCombineData() { + static CombineData data = [] { + CombineData cd; + mjModel* m = LoadModelFromPath("humanoid/humanoid.xml"); + m->opt.jacobian = mjJAC_SPARSE; + mjData* d = mj_makeData(m); + + for (int i = 0; i < 500; i++) { + mj_step(m, d); + } + + cd.nv = m->nv; + mj_markStack(d); + mjtNum* H = mj_stackAllocNum(d, m->nv*m->nv); + int* rownnz = mj_stackAllocInt(d, m->nv); + int* rowadr = mj_stackAllocInt(d, m->nv); + int* colind = mj_stackAllocInt(d, m->nv*m->nv); + int* diagind = mj_stackAllocInt(d, m->nv); + + mjtNum* D = mj_stackAllocNum(d, d->nefc); + for (int i = 0; i < d->nefc; i++) { + if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) { + D[i] = d->efc_D[i]; + } else { + D[i] = 0; + } + } + + int* JT_rownnz = mj_stackAllocInt(d, m->nv); + int* JT_rowadr = mj_stackAllocInt(d, m->nv); + int* JT_rowsuper = mj_stackAllocInt(d, m->nv); + int* JT_colind = mj_stackAllocInt(d, d->nJ); + mjtNum* JT = mj_stackAllocNum(d, d->nJ); + mju_transposeSparse(JT, d->efc_J, d->nefc, m->nv, + JT_rownnz, JT_rowadr, JT_colind, JT_rowsuper, + d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); + + // compute H = J'*D*J, uncompressed layout + mju_sqrMatTDUncompressedInit(rowadr, m->nv); + mju_sqrMatTDSparse(H, d->efc_J, JT, D, d->nefc, m->nv, + rownnz, rowadr, colind, + d->efc_J_rownnz, d->efc_J_rowadr, + d->efc_J_colind, d->efc_J_rowsuper, + JT_rownnz, JT_rowadr, + JT_colind, JT_rowsuper, d, + diagind); + + // compute H = M + J'*D*J + mj_addM(m, d, H, rownnz, rowadr, colind); + + // copy to persistent storage + int nH = rowadr[m->nv-1] + m->nv; // uncompressed: rowadr[r] = r*nv + cd.H.assign(H, H + nH); + cd.rownnz.assign(rownnz, rownnz + m->nv); + cd.rowadr.assign(rowadr, rowadr + m->nv); + cd.colind.assign(colind, colind + nH); + + mj_freeStack(d); + mj_deleteData(d); + mj_deleteModel(m); + return cd; + }(); + return data; +} + +// ---- TransposeSparse data ---- +struct TransposeData { + int nv; + int nefc; + int nJ; + std::vector efc_J; + std::vector efc_J_rownnz, efc_J_rowadr, efc_J_colind; +}; + +enum class Size { H2_100, H100 }; + +template +const char* ModelPath() { + if constexpr (S == Size::H2_100) { + return "../test/benchmark/testdata/2humanoid100_chol.xml"; + } else { + return "../test/benchmark/testdata/100_humanoids_chol.xml"; + } +} + +template +TransposeData& GetTransposeData() { + static TransposeData data = [] { + TransposeData td; + mjModel* m = LoadModelFromPath(ModelPath()); + m->opt.jacobian = mjJAC_SPARSE; + mjData* d = mj_makeData(m); + + while (d->time < 2) { + mj_step(m, d); + } + + td.nv = m->nv; + td.nefc = d->nefc; + td.nJ = d->nJ; + td.efc_J.assign(d->efc_J, d->efc_J + d->nJ); + td.efc_J_rownnz.assign(d->efc_J_rownnz, d->efc_J_rownnz + d->nefc); + td.efc_J_rowadr.assign(d->efc_J_rowadr, d->efc_J_rowadr + d->nefc); + td.efc_J_colind.assign(d->efc_J_colind, d->efc_J_colind + d->nJ); + + mj_deleteData(d); + mj_deleteModel(m); + return td; + }(); + return data; +} + +// ================================ old functions ============================== // transpose sparse matrix (uncompressed) void ABSL_ATTRIBUTE_NOINLINE transposeSparse_baseline( @@ -229,61 +393,31 @@ void ABSL_ATTRIBUTE_NOINLINE mulMatVecSparse_8(mjtNum* res, } } -// ----------------------------- benchmark ------------------------------------ +// ----------------------------- benchmark ------------------------------------- static void BM_MatVecSparse(benchmark::State& state, int unroll) { - static mjModel* m = LoadModelFromPath("flex/flag.xml"); - mjData* d = mj_makeData(m); + MatVecData& data = GetMatVecData(); + std::vector res(data.nefc); - // warm-up rollout to get a typical state - for (int i=0; i < kNumWarmupSteps; i++) { - mj_step(m, d); - } - - // allocate gradient - mj_markStack(d); - mjtNum *Ma = mj_stackAllocNum(d, m->nv); - mjtNum *vec = mj_stackAllocNum(d, m->nv); - mjtNum *res = mj_stackAllocNum(d, d->nefc); - mjtNum *grad = mj_stackAllocNum(d, m->nv); - mjtNum *Mgrad = mj_stackAllocNum(d, m->nv); - - // compute gradient - mj_mulM(m, d, Ma, d->qacc); - for (int i=0; i < m->nv; i++) { - grad[i] = Ma[i] - d->qfrc_smooth[i] - d->qfrc_constraint[i]; - } - - // compute search direction - mj_solveM(m, d, Mgrad, grad, 1); - mju_scl(vec, Mgrad, -1, m->nv); - - // save state - std::vector qpos = AsVector(d->qpos, m->nq); - std::vector qvel = AsVector(d->qvel, m->nv); - std::vector act = AsVector(d->act, m->na); - std::vector warmstart = AsVector(d->qacc_warmstart, m->nv); - - // time benchmark for (auto s : state) { if (unroll == 4) { - mju_mulMatVecSparse(res, d->efc_J, vec, d->nefc, - d->efc_J_rownnz, d->efc_J_rowadr, - d->efc_J_colind, d->efc_J_rowsuper); + mju_mulMatVecSparse(res.data(), data.efc_J.data(), data.vec.data(), + data.nefc, data.efc_J_rownnz.data(), + data.efc_J_rowadr.data(), data.efc_J_colind.data(), + data.efc_J_rowsuper.data()); } else if (unroll == 1) { - mulMatVecSparse_1(res, d->efc_J, vec, d->nefc, - d->efc_J_rownnz, d->efc_J_rowadr, - d->efc_J_colind, d->efc_J_rowsuper); + mulMatVecSparse_1(res.data(), data.efc_J.data(), data.vec.data(), + data.nefc, data.efc_J_rownnz.data(), + data.efc_J_rowadr.data(), data.efc_J_colind.data(), + data.efc_J_rowsuper.data()); } else if (unroll == 8) { - mulMatVecSparse_8(res, d->efc_J, vec, d->nefc, - d->efc_J_rownnz, d->efc_J_rowadr, - d->efc_J_colind, d->efc_J_rowsuper); + mulMatVecSparse_8(res.data(), data.efc_J.data(), data.vec.data(), + data.nefc, data.efc_J_rownnz.data(), + data.efc_J_rowadr.data(), data.efc_J_colind.data(), + data.efc_J_rowsuper.data()); } } - // finalize - mj_freeStack(d); - mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } @@ -309,75 +443,30 @@ void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_MatVecSparse_1( BENCHMARK(BM_MatVecSparse_1); static void BM_combineSparse(benchmark::State& state, CombineFuncPtr func) { - static mjModel* m = LoadModelFromPath("humanoid/humanoid.xml"); - m->opt.jacobian = mjJAC_SPARSE; + CombineData& data = GetCombineData(); - mjData* d = mj_makeData(m); - - // warm-up rollout to get a typical state - for (int i=0; i < kNumWarmupSteps; i++) { - mj_step(m, d); - } - - // allocate - mj_markStack(d); - mjtNum* H = mj_stackAllocNum(d, m->nv*m->nv); - int* rownnz = mj_stackAllocInt(d, m->nv); - int* rowadr = mj_stackAllocInt(d, m->nv); - int* colind = mj_stackAllocInt(d, m->nv*m->nv); - int* diagind = mj_stackAllocInt(d, m->nv); - - // compute D corresponding to quad states - mjtNum* D = mj_stackAllocNum(d, d->nefc); - for (int i = 0; i < d->nefc; i++) { - if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) { - D[i] = d->efc_D[i]; - } else { - D[i] = 0; - } - } - - int* JT_rownnz = mj_stackAllocInt(d, m->nv); - int* JT_rowadr = mj_stackAllocInt(d, m->nv); - int* JT_rowsuper = mj_stackAllocInt(d, m->nv); - int* JT_colind = mj_stackAllocInt(d, d->nJ); - mjtNum* JT = mj_stackAllocNum(d, d->nJ); - mju_transposeSparse(JT, d->efc_J, d->nefc, m->nv, - JT_rownnz, JT_rowadr, JT_colind, JT_rowsuper, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); - - // compute H = J'*D*J, uncompressed layout - mju_sqrMatTDUncompressedInit(rowadr, m->nv); - mju_sqrMatTDSparse(H, d->efc_J, JT, D, d->nefc, m->nv, - rownnz, rowadr, colind, - d->efc_J_rownnz, d->efc_J_rowadr, - d->efc_J_colind, d->efc_J_rowsuper, - JT_rownnz, JT_rowadr, - JT_colind, JT_rowsuper, d, - diagind); - - // compute H = M + J'*D*J - mj_addM(m, d, H, rownnz, rowadr, colind); + // make working copies that get modified each iteration + std::vector H = data.H; + std::vector rownnz = data.rownnz; + std::vector rowadr = data.rowadr; + std::vector colind = data.colind; // time benchmark for (auto s : state) { - for (int r = m->nv-1; r >= 0; r--) { + for (int r = data.nv-1; r >= 0; r--) { for (int i = 0; i < rownnz[r]-1; i++) { int adr = rowadr[r]; int c = colind[adr+i]; // true arguments should be i+1 and colind+rowadr[r] // but instead we repeat rownnz[c] and colind+rowadr[c] // in order to trigger all if's in combineSparse - func(H+rowadr[c], H+rowadr[r], 1, -H[adr+i], + func(H.data()+rowadr[c], H.data()+rowadr[r], 1, -H[adr+i], rownnz[c], rownnz[c], - colind+rowadr[c], colind+rowadr[c]); + colind.data()+rowadr[c], colind.data()+rowadr[c]); } } } - // finalize - mj_freeStack(d); - mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } @@ -395,17 +484,6 @@ void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_combineSparse_old( } BENCHMARK(BM_combineSparse_old); -enum class Size { H2_100, H100 }; - -template -const char* ModelPath() { - if constexpr (S == Size::H2_100) { - return "../test/benchmark/testdata/2humanoid100_chol.xml"; - } else { - return "../test/benchmark/testdata/100_humanoids_chol.xml"; - } -} - enum class Supernode { None, PostProcess, @@ -415,44 +493,33 @@ enum class Supernode { template static void BM_transposeSparse(benchmark::State& state, TransposeFuncPtr func, Supernode super) { - static mjModel* m = LoadModelFromPath(ModelPath()); + TransposeData& data = GetTransposeData(); - // force use of sparse matrices - m->opt.jacobian = mjJAC_SPARSE; - - mjData* d = mj_makeData(m); - - // warm-up rollout to get a typical state - while (d->time < 2) { - mj_step(m, d); - } - - mj_markStack(d); - - // need uncompressed layout - mjtNum* res = mj_stackAllocNum(d, m->nv * d->nefc); - int* res_rownnz = mj_stackAllocInt(d, m->nv); - int* res_rowadr = mj_stackAllocInt(d, m->nv); - int* res_rowsuper = mj_stackAllocInt(d, m->nv); - int* res_colind = mj_stackAllocInt(d, m->nv * d->nefc); + // allocate output buffers (uncompressed layout) + std::vector res(data.nv * data.nefc); + std::vector res_rownnz(data.nv); + std::vector res_rowadr(data.nv); + std::vector res_rowsuper(data.nv); + std::vector res_colind(data.nv * data.nefc); // time benchmark for (auto s : state) { - int* rowsuper = (super == Supernode::Inline) ? res_rowsuper : nullptr; - func(res, d->efc_J, d->nefc, m->nv, - res_rownnz, res_rowadr, res_colind, rowsuper, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); + int* rowsuper = + (super == Supernode::Inline) ? res_rowsuper.data() : nullptr; + func(res.data(), data.efc_J.data(), data.nefc, data.nv, + res_rownnz.data(), res_rowadr.data(), res_colind.data(), rowsuper, + data.efc_J_rownnz.data(), data.efc_J_rowadr.data(), + data.efc_J_colind.data()); if (super == Supernode::PostProcess) { - mju_superSparse(m->nv, res_rowsuper, - res_rownnz, res_rowadr, res_colind); + mju_superSparse(data.nv, res_rowsuper.data(), + res_rownnz.data(), res_rowadr.data(), res_colind.data()); } } - mj_freeStack(d); - mj_deleteData(d); state.SetItemsProcessed(state.iterations()); } + void ABSL_ATTRIBUTE_NO_TAIL_CALL BM_transposeSparse_2H100_old(benchmark::State& state) { MujocoErrorTestGuard guard; From b2281883dd8f772d8937868853eb650a7df28390 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 20 Apr 2026 08:53:28 -0700 Subject: [PATCH 103/251] Improve slider precision and range clamping, fixes #3206 Add clamping to slider values in both ImGui widgets and mjUI to ensure they remain within the defined min/max ranges, preventing potential floating point inaccuracies from causing values to slightly exceed the bounds. PiperOrigin-RevId: 902660458 Change-Id: Ia153ae7907f07b890f2b5ac2e3a4be93ecae7bd3 --- simulate/simulate.cc | 4 ++-- src/experimental/platform/ux/imgui_widgets.cc | 2 +- src/ui/ui_main.c | 5 +++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 522404b9..b4e29e35 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -1192,7 +1192,7 @@ void MakeJointSection(mj::Simulate* sim) { // set range if (sim->jnt_range_[i].has_value()) - mju::sprintf_arr(defSlider[0].other, "%.4g %.4g", + mju::sprintf_arr(defSlider[0].other, "%.17g %.17g", sim->jnt_range_[i]->first, sim->jnt_range_[i]->second); else if (sim->jnt_type_[i]==mjJNT_SLIDE) { mju::strcpy_arr(defSlider[0].other, "-1 1"); @@ -1251,7 +1251,7 @@ void MakeControlSection(mj::Simulate* sim) { // set range if (sim->actuator_ctrlrange_[i].has_value()) - mju::sprintf_arr(defSlider[0].other, "%.4g %.4g", + mju::sprintf_arr(defSlider[0].other, "%.17g %.17g", sim->actuator_ctrlrange_[i]->first, sim->actuator_ctrlrange_[i]->second); else { mju::strcpy_arr(defSlider[0].other, "-1 1"); diff --git a/src/experimental/platform/ux/imgui_widgets.cc b/src/experimental/platform/ux/imgui_widgets.cc index 9ffbbe3e..61d2989a 100644 --- a/src/experimental/platform/ux/imgui_widgets.cc +++ b/src/experimental/platform/ux/imgui_widgets.cc @@ -321,7 +321,7 @@ bool ImGui_Slider(const char* name, mjtNum* value, mjtNum min, mjtNum max) { float f = *value; const bool res = ImGui::SliderFloat(name, &f, min, max); if (res) { - *value = f; + *value = mju_clip(f, min, max); } return res; } diff --git a/src/ui/ui_main.c b/src/ui/ui_main.c index ffc2fca2..14f588f1 100644 --- a/src/ui/ui_main.c +++ b/src/ui/ui_main.c @@ -849,8 +849,9 @@ static void setslider(mjuiItem* it, mjUI* ui, rx = mju_round(rx * it->slider.divisions) / mjMAX(1, it->slider.divisions); rx = mjMAX(0, mjMIN(1, rx)); - // compute value - mjtNum val = (mjtNum)(it->slider.range[0]*(1-rx) + it->slider.range[1]*rx); + // compute value, clamp to range + mjtNum val = mju_clip(it->slider.range[0]*(1-rx) + it->slider.range[1]*rx, + it->slider.range[0], it->slider.range[1]); // set slider position if (it->type == mjITEM_SLIDERINT) { From 476e2e909e0b12f7044fdb604dc22a460386991b Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Mon, 20 Apr 2026 09:05:43 -0700 Subject: [PATCH 104/251] Allow Transforms to be set directly on Renderable. Also allow multiple Meshes to be assigned at once rather than one at a time. This simplifies both the usage and implementation of Renderable. PiperOrigin-RevId: 902665683 Change-Id: I23ff365a54fd6a3814ed3452b1e094ae9f698a1b --- .../filament/filament/filament_context.cc | 2 - .../filament/filament/imgui_bridge.cc | 17 +- .../filament/filament/imgui_bridge.h | 3 - .../filament/filament/renderable.cc | 213 +++++----- .../filament/filament/renderable.h | 74 ++-- .../filament/filament/scene_geom_util.cc | 378 +++++++++--------- .../filament/filament/scene_view.cc | 7 +- .../filament/filament/scene_view.h | 2 - 8 files changed, 343 insertions(+), 353 deletions(-) diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index 09f26cfa..427306b8 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -156,7 +156,6 @@ void FilamentContext::Render(const mjrRect& viewport, const mjvScene* scene) { request.viewport = viewport; request.camera = last_camera_; request.enable_ux = (gui_swap_chain_target_ == kWindowSwapChain); - request.gui_scale = imgui_bridge_ ? imgui_bridge_->GetScale() : 1.0f; scene_view_->Render(renderer_, request); renderer_->endFrame(); } @@ -233,7 +232,6 @@ void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb, request.target = color_target_.get(); request.camera = last_camera_; request.enable_ux = (gui_swap_chain_target_ == kOffscreenSwapChain); - request.gui_scale = imgui_bridge_ ? imgui_bridge_->GetScale() : 1.0f; scene_view_->Render(renderer_, request); const size_t num_bytes = viewport.width * viewport.height * 3; diff --git a/src/experimental/filament/filament/imgui_bridge.cc b/src/experimental/filament/filament/imgui_bridge.cc index 5ee492eb..97dd852a 100644 --- a/src/experimental/filament/filament/imgui_bridge.cc +++ b/src/experimental/filament/filament/imgui_bridge.cc @@ -21,6 +21,8 @@ #include #include +#include +#include #include #include #include "experimental/filament/filament/material.h" @@ -32,6 +34,9 @@ namespace mujoco { +using filament::math::float3; +using filament::math::mat3f; + ImguiBridge::ImguiBridge(ObjectManager* object_mgr, SceneView* scene_view) : object_mgr_(object_mgr), scene_view_(scene_view) {} @@ -235,11 +240,7 @@ void ImguiBridge::Update() { const int height = size.y * scale.y; auto& renderable = renderables_[renderable_index]; - if (renderable->GetNumMeshes() == 0) { - renderable->AppendMesh(mesh, index_offset, command.ElemCount); - } else { - renderable->UpdateMesh(0, mesh, index_offset, command.ElemCount); - } + renderable->SetMesh(mesh, index_offset, command.ElemCount); MaterialTextures textures; textures.color = textures_[command.GetTexID()].get(); @@ -259,6 +260,8 @@ void ImguiBridge::Update() { properties.scissor[3] = height; } renderable->UpdateMaterial(properties, textures); + renderable->SetTransform( + {float3{0, 0, 0}, mat3f(), float3(scale.x, scale.y, 1.0f)}); index_offset += command.ElemCount; ++renderable_index; @@ -284,10 +287,6 @@ void ImguiBridge::PrepareRenderables(int count) { } } -float ImguiBridge::GetScale() const { - return ImGui::GetIO().DisplayFramebufferScale.x; -} - static ImVec2 ClipSpaceToWindowCoordinates(float x, float y) { const ImVec2& display_size = ImGui::GetIO().DisplaySize; const float pos_x = display_size.x * ((x + 1) * 0.5f); diff --git a/src/experimental/filament/filament/imgui_bridge.h b/src/experimental/filament/filament/imgui_bridge.h index 4e36b333..54b5f494 100644 --- a/src/experimental/filament/filament/imgui_bridge.h +++ b/src/experimental/filament/filament/imgui_bridge.h @@ -40,9 +40,6 @@ class ImguiBridge { // synced. void Update(); - // Returns the current ImGui scale factor. - float GetScale() const; - // Uploads texture to be used with ImGui's Image and ImageButton functions. uintptr_t UploadImage(uintptr_t tex_id, const uint8_t* pixels, int width, int height, int bpp); diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 3e518938..2a9659ce 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -16,20 +16,26 @@ #include #include +#include #include #include #include #include +#include +#include #include #include #include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/material.h" +#include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/object_manager.h" namespace mujoco { +using filament::math::mat4f; + void DefaultRenderableParams(RenderableParams* params) { params->shading_model = ShadingModel::SceneObject; } @@ -38,68 +44,77 @@ Renderable::Renderable(ObjectManager* object_mgr, const RenderableParams& params : object_mgr_(object_mgr), params_(params) {} Renderable::~Renderable() noexcept { - while (!entities_.empty()) { - RemoveLastEntity(); + filament::Engine* engine = GetEngine(); + utils::EntityManager& em = utils::EntityManager::get(); + + for (Part& part : parts_) { + if (assigned_scene_) { + assigned_scene_->remove(part.entity); + } + engine->destroy(part.entity); + em.destroy(part.entity); } for (int i = 0; i < kNumDrawModes; ++i) { if (instances_[i] != nullptr) { - GetEngine()->destroy(instances_[i]); + engine->destroy(instances_[i]); instances_[i] = nullptr; } } } -void Renderable::RemoveLastEntity() { - if (entities_.empty()) { - return; +void Renderable::SetMesh(const Mesh* mesh, int elem_offset, int elem_count) { + if (mesh == nullptr) { + mju_error("Cannot set mesh to nullptr."); } - - utils::EntityManager& em = utils::EntityManager::get(); - utils::Entity entity = entities_.back(); - - if (assigned_scene_) { - assigned_scene_->remove(entity); - } - - GetEngine()->destroy(entity); - em.destroy(entity); - entities_.pop_back(); - meshes_.pop_back(); -} - -void Renderable::UpdateMesh(int index, const Mesh* mesh, int elem_offset, - int elem_count) { - MeshInfo& mesh_info = SetMesh(index, mesh, elem_offset, elem_count); - UpdateEntity(index, mesh_info); -} - -void Renderable::AppendMesh(const Mesh* mesh, int elem_offset, int elem_count) { - MeshInfo& mesh_info = SetMesh(-1, mesh, elem_offset, elem_count); - AppendEntity(mesh_info); -} - -void Renderable::AppendEntity(const MeshInfo& mesh_info) { - const Mesh* mesh = mesh_info.mesh; filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer(); if (vertex_buffer == nullptr) { mju_error("Invalid (null) vertex buffer."); } - filament::IndexBuffer* index_buffer = mesh->GetFilamentIndexBuffer(); if (index_buffer == nullptr) { mju_error("Invalid (null) index buffer."); } - utils::Entity entity = utils::EntityManager::get().create(); - if (entity.isNull()) { + if (elem_count == 0) { + elem_count = index_buffer->getIndexCount() - elem_offset; + } + + if (parts_.empty()) { + Part& part = parts_.emplace_back(); + part.mesh = mesh; + part.elem_offset = elem_offset; + part.elem_count = elem_count; + InitPartEntity(part); + } else if (parts_.size() == 1) { + Part& part = parts_[0]; + part.mesh = mesh; + part.elem_offset = elem_offset; + part.elem_count = elem_count; + + filament::RenderableManager& rm = GetEngine()->getRenderableManager(); + rm.setGeometryAt(rm.getInstance(part.entity), 0, + part.mesh->GetPrimitiveType(), vertex_buffer, index_buffer, + part.elem_offset, part.elem_count); + + } else { + mju_error("Cannot set mesh for renderable with multiple parts."); + } +} + +void Renderable::InitPartEntity(Part& part) { + part.entity = utils::EntityManager::get().create(); + if (part.entity.isNull()) { mju_error("Failed to create entity."); } + filament::VertexBuffer* vertex_buffer = part.mesh->GetFilamentVertexBuffer(); + filament::IndexBuffer* index_buffer = part.mesh->GetFilamentIndexBuffer(); + filament::RenderableManager::Builder builder(1); - builder.geometry(0, mesh->GetPrimitiveType(), vertex_buffer, index_buffer, - mesh_info.elem_offset, mesh_info.elem_count); - if (mesh->HasBounds()) { - builder.boundingBox(mesh->GetBounds()); + builder.geometry(0, part.mesh->GetPrimitiveType(), vertex_buffer, index_buffer, + part.elem_offset, part.elem_count); + if (part.mesh->HasBounds()) { + builder.boundingBox(part.mesh->GetBounds()); } else { builder.culling(false); } @@ -113,56 +128,43 @@ void Renderable::AppendEntity(const MeshInfo& mesh_info) { builder.blendOrder(0, blend_order_); builder.screenSpaceContactShadows(true); - builder.build(*GetEngine(), entity); + builder.build(*GetEngine(), part.entity); if (assigned_scene_) { - assigned_scene_->addEntity(entity); + assigned_scene_->addEntity(part.entity); } - entities_.push_back(entity); } -void Renderable::UpdateEntity(int index, const MeshInfo& mesh_info) { - if (index < 0 || index >= entities_.size()) { - mju_error("Invalid index %d for renderable.", index); +void Renderable::SetTransform(const Trs& trs) { + transform_ = trs.ToTransform(); + filament::TransformManager& tm = GetEngine()->getTransformManager(); + for (Part& part : parts_) { + tm.setTransform(tm.getInstance(part.entity), transform_); } - utils::Entity entity = entities_[index]; - - const Mesh* mesh = mesh_info.mesh; - filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer(); - if (vertex_buffer == nullptr) { - mju_error("Invalid (null) vertex buffer."); - } - - filament::IndexBuffer* index_buffer = mesh->GetFilamentIndexBuffer(); - if (index_buffer == nullptr) { - mju_error("Invalid (null) index buffer."); - } - - filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - rm.setGeometryAt(rm.getInstance(entity), 0, mesh->GetPrimitiveType(), - vertex_buffer, index_buffer, mesh_info.elem_offset, - mesh_info.elem_count); } -Renderable::MeshInfo& Renderable::SetMesh(int index, const Mesh* mesh, - int elem_offset, int elem_count) { - if (index == -1) { - index = meshes_.size(); - meshes_.emplace_back(); +const mat4f& Renderable::GetTransform() const { + return transform_; +} + +void Renderable::SetMeshes(std::span meshes, + std::span transforms) { + if (meshes.size() != transforms.size()) { + mju_error("Number of meshes does not match number of transforms."); } - if (index < 0 || index >= static_cast(meshes_.size())) { - mju_error("Invalid index %d for renderable.", index); + if (!parts_.empty()) { + mju_error("Cannot set meshes for renderable with multiple parts."); } - MeshInfo* mesh_info = &meshes_[index]; - mesh_info->mesh = mesh; - mesh_info->elem_offset = elem_offset; - mesh_info->elem_count = elem_count; - if (mesh_info->elem_count == 0) { - const int total = - mesh_info->mesh->GetFilamentIndexBuffer()->getIndexCount(); - mesh_info->elem_count = total - mesh_info->elem_offset; + filament::TransformManager& tm = GetEngine()->getTransformManager(); + for (int i = 0; i < meshes.size(); ++i) { + Part& part = parts_.emplace_back(); + part.mesh = meshes[i]; + part.elem_offset = 0; + part.elem_count = part.mesh->GetFilamentIndexBuffer()->getIndexCount(); + InitPartEntity(part); + + tm.setTransform(tm.getInstance(part.entity), transforms[i]); } - return *mesh_info; } void Renderable::AddToScene(filament::Scene* scene) { @@ -173,8 +175,8 @@ void Renderable::AddToScene(filament::Scene* scene) { // Entities are already added to the scene. return; } - for (utils::Entity& entity : entities_) { - scene->addEntity(entity); + for (Part& part : parts_) { + scene->addEntity(part.entity); } assigned_scene_ = scene; } @@ -183,8 +185,8 @@ void Renderable::RemoveFromScene(filament::Scene* scene) { if (assigned_scene_ != scene) { mju_error("Attempting to remove renderable from wrong scene."); } - for (utils::Entity& entity : entities_) { - scene->remove(entity); + for (Part& part : parts_) { + scene->remove(part.entity); } assigned_scene_ = nullptr; } @@ -245,8 +247,8 @@ void Renderable::SetDrawMode(DrawMode mode) { filament::MaterialInstance* instance = instances_[static_cast(mode)]; if (instance) { filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - filament::RenderableManager::Instance ri = rm.getInstance(entity); + for (Part& part : parts_) { + filament::RenderableManager::Instance ri = rm.getInstance(part.entity); rm.setMaterialInstanceAt(ri, 0, instance); } } @@ -259,8 +261,8 @@ std::uint8_t Renderable::SetLayerMask(std::uint8_t mask) { layer_mask_ = mask; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - rm.setLayerMask(rm.getInstance(entity), 0xff, layer_mask_); + for (Part& part : parts_) { + rm.setLayerMask(rm.getInstance(part.entity), 0xff, layer_mask_); } } return prev; @@ -272,8 +274,8 @@ std::uint8_t Renderable::SetPriority(std::uint8_t priority) { priority_ = priority; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - rm.setPriority(rm.getInstance(entity), priority_); + for (Part& part : parts_) { + rm.setPriority(rm.getInstance(part.entity), priority_); } } return prev; @@ -285,8 +287,8 @@ std::uint16_t Renderable::SetBlendOrder(std::uint16_t blend_order) { blend_order_ = blend_order; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - rm.setBlendOrderAt(rm.getInstance(entity), 0, blend_order_); + for (Part& part : parts_) { + rm.setBlendOrderAt(rm.getInstance(part.entity), 0, blend_order_); } } return prev; @@ -297,8 +299,8 @@ void Renderable::SetCastShadows(bool cast_shadows) { cast_shadows_ = cast_shadows; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - rm.setCastShadows(rm.getInstance(entity), cast_shadows_); + for (Part& part : parts_) { + rm.setCastShadows(rm.getInstance(part.entity), cast_shadows_); } } } @@ -308,8 +310,8 @@ void Renderable::SetReceiveShadows(bool receive_shadows) { receive_shadows_ = receive_shadows; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (utils::Entity& entity : entities_) { - rm.setReceiveShadows(rm.getInstance(entity), receive_shadows_); + for (Part& part : parts_) { + rm.setReceiveShadows(rm.getInstance(part.entity), receive_shadows_); } } } @@ -322,20 +324,17 @@ void Renderable::SetWireframe(bool wireframe) { wireframe_ = wireframe; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); - for (int i = 0; i < entities_.size(); ++i) { - utils::Entity& entity = entities_[i]; - const Mesh* mesh = meshes_[i].mesh; - filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer(); - filament::IndexBuffer* index_buffer = mesh->GetFilamentIndexBuffer(); - rm.setGeometryAt(rm.getInstance(entity), 0, - wireframe_ ? kWireframeType : mesh->GetPrimitiveType(), - vertex_buffer, index_buffer, meshes_[i].elem_offset, - meshes_[i].elem_count); + for (Part& part : parts_) { + filament::VertexBuffer* vertex_buffer = part.mesh->GetFilamentVertexBuffer(); + filament::IndexBuffer* index_buffer = part.mesh->GetFilamentIndexBuffer(); + rm.setGeometryAt(rm.getInstance(part.entity), 0, + wireframe_ ? kWireframeType : part.mesh->GetPrimitiveType(), + vertex_buffer, index_buffer, part.elem_offset, + part.elem_count); } } } - ObjectManager::MaterialType Renderable::GetColorMaterialType() const { if (params_.shading_model == ShadingModel::DecorLines) { return ObjectManager::kUnlitLine; @@ -360,8 +359,8 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { // geometry) and `mesh_texcoordadr` stores the address of the mesh uvs if // it has them. bool has_texcoords = false; - if (!meshes_.empty()) { - const auto attribs = meshes_[0].mesh->GetVertexAttributes(); + if (!parts_.empty()) { + const auto attribs = parts_[0].mesh->GetVertexAttributes(); auto it = std::find(attribs.begin(), attribs.end(), filament::VertexAttribute::UV0); has_texcoords = (it != attribs.end()); diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 50674c82..380eea3b 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -16,13 +16,16 @@ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDERABLE_H_ #include +#include #include #include #include +#include #include #include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/material.h" +#include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/object_manager.h" @@ -43,14 +46,20 @@ struct RenderableParams { void DefaultRenderableParams(RenderableParams* params); -// A collection of meshes and a material that, together, define an object that -// can be rendered in a scene. +// A Renderable is effectively two things: a mesh and a material. // -// Meshes can be added to the Renderable either by unique_ptr or raw pointer. -// This determines whether or not the Renderable takes ownership of the mesh. +// The mesh describes the surface geometry of the object and the material +// describes how that surface interacts with light (i.e. the color of each point +// on the surface). // -// Internally, the Renderable creates a filament::Entity for each mesh and -// assigns the same material instance to all of them. +// Defining the mesh is easy; just call SetMesh. +// +// Defining a Material happens in two stages. First, the user specifies the +// ShadingModel to use for Rendering. This describes the overall intent of +// how the Renderable will appear (e.g. lit, unlit, wireframe, etc.). Next, +// the user specifies the MaterialParams and MaterialTextures to use with the +// ShadingModel. Its these properties that ultimately define the actual material +// of the Renderable. class Renderable { public: // Default filament values for priority and layer mask. @@ -63,19 +72,21 @@ class Renderable { Renderable(const Renderable&) = delete; Renderable& operator=(const Renderable&) = delete; - // Appends a mesh to the renderable. The elem_offset and elem_count parameters - // can be used to specify a submesh to append. If elem_count is 0, assumes - // the entire mesh should be appended. - void AppendMesh(const Mesh* mesh, int elem_offset = 0, int elem_count = 0); + // Sets the mesh of the renderable. The elem_offset and elem_count parameters + // can be used to specify a submesh within the mesh. If elem_count is 0, + // assumes the entire mesh should be appended. + void SetMesh(const Mesh* mesh, int elem_offset = 0, int elem_count = 0); - // Replaces the mesh at the index with a new mesh. The elem_offset and - // elem_count parameters can be used to specify a submesh to append. If - // elem_count is 0, assumes the entire mesh should be appended. - void UpdateMesh(int index, const Mesh* mesh, int elem_offset = 0, - int elem_count = 0); + // Sets the transform of the renderable. + void SetTransform(const Trs& trs); - // Returns the number of meshes that define the renderable. - int GetNumMeshes() const { return meshes_.size(); } + // Returns the current transform of the renderable. + const filament::math::mat4f& GetTransform() const; + + // Sets multiple meshes for a renderable. Each mesh is assigned a specific + // transform to allow for assembly of compound shapes. + void SetMeshes(std::span meshes, + std::span transforms); // Sets the layer mask for the managed filament Entities. Layer masks can be // used to show/hide the renderable in different views. Returns the previous @@ -107,7 +118,8 @@ class Renderable { // Removes the renderable from the given filament Scene. void RemoveFromScene(filament::Scene* scene); - // Sets the material instance for all managed entities. + // Further defines the material of the renderable. Only applies to renderables + // with a SceneObject shading model. void SetDrawMode(DrawMode mode); // Updates the parameters for the material. @@ -123,30 +135,15 @@ class Renderable { // Returns the filament Engine managing the renderables. filament::Engine* GetEngine(); - // Returns the underlying filament::entity for the given mesh. - utils::Entity operator[](int index) { return entities_[index]; } - private: - struct MeshInfo { + struct Part { + utils::Entity entity; const Mesh* mesh = nullptr; int elem_offset = 0; int elem_count = 0; }; - // Sets the mesh information for the mesh at the given index. If index is -1, - // a new mesh will be appended to the renderable. - MeshInfo& SetMesh(int index, const Mesh* mesh, int elem_offset, - int elem_count); - - // Appends a new filament::Entity to the renderable, configured to use the - // given mesh. - void AppendEntity(const MeshInfo& mesh_info); - - // Updates the filament::Entity at the given index to use the given mesh. - void UpdateEntity(int index, const MeshInfo& mesh_info); - - // Removes the last filament::Entity from the renderable. - void RemoveLastEntity(); + void InitPartEntity(Part& part); void AssignMaterial(DrawMode mode, ObjectManager::MaterialType material_type); @@ -159,8 +156,9 @@ class Renderable { MaterialTextures material_textures_; DrawMode draw_mode_ = DrawMode::Color; filament::Scene* assigned_scene_ = nullptr; - std::vector entities_; - std::vector meshes_; + std::vector parts_; + filament::math::mat4f transform_; + std::uint8_t priority_ = kDefaultPriority; std::uint8_t layer_mask_ = kDefaultLayerMask; std::uint16_t blend_order_ = 0; diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index f68f620e..3b15b32d 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -44,30 +45,12 @@ namespace mujoco { using filament::math::float2; using filament::math::float3; using filament::math::float4; -using filament::math::mat4; +using filament::math::mat4f; // An arbitrary scale factor for arrows. static constexpr float kArrowScale = 1.f / 6.f; static constexpr float kArrowHeadSize = 1.75f; -// Some built-in geometries are actually composed of multiple simple shapes. A -// capsule, for example, is a open-ended tube with two dome ends. We use these -// constants to help identify which entity (by index) represents which part of -// the overall shape. -static constexpr int kCapsuleTopDome = 1; -static constexpr int kCapsuleBottomDome = 2; -static constexpr int kCylinderTopDisk = 1; -static constexpr int kCylinderBottomDisk = 2; -static constexpr int kArrow0Cone = 1; -static constexpr int kArrow0ConeDisk = 2; -static constexpr int kArrow0BottomDisk = 3; -static constexpr int kArrow1Cone = 1; -static constexpr int kArrow1BottomDisk = 2; -static constexpr int kArrow2TopCone = 1; -static constexpr int kArrow2BottomCone = 2; -static constexpr int kArrow2TopConeDisk = 3; -static constexpr int kArrow2BottomConeDisk = 4; - // Returns the tile size for infinite plane texture alignment. // This is duplicated from engine_vis_visualize.c (re-center infinite plane) // to ensure UV scaling matches the re-centering increments. @@ -87,102 +70,240 @@ static bool IsBehind(const float* headpos, const float* pos, const float* mat) { 0.0f); } -static void AddMesh(Renderable& renderable, ModelObjects* model_objs, - int data_id) { +static const Mesh* GetMesh(ModelObjects* model_objs, int data_id) { const Mesh* mesh = model_objs->GetMeshBuffer(data_id); if (mesh == nullptr) { mju_error("Unknown mesh %d", data_id); } - renderable.AppendMesh(mesh); + return mesh; } -static void AddSkinFlexMesh(Renderable& renderable, ModelObjects* model_objs, - int objid) { - renderable.AppendMesh(model_objs->GetFlexSkinGeomMesh(objid)); +static const Mesh* GetSkinFlexMesh(ModelObjects* model_objs, int objid) { + return model_objs->GetFlexSkinGeomMesh(objid); } -static void AddHeightField(Renderable& renderable, ModelObjects* model_objs, - int hfield_id) { +static const Mesh* GetHeightField(ModelObjects* model_objs, int hfield_id) { const Mesh* mesh = model_objs->GetHeightFieldBuffer(hfield_id); if (mesh == nullptr) { mju_error("Unknown height field %d", hfield_id); } - renderable.AppendMesh(mesh); + return mesh; } -static void AddShape(Renderable& renderable, ModelObjects* model_objs, - ModelObjects::ShapeType shape_type) { +static const Mesh* GetShape(ModelObjects* model_objs, + ModelObjects::ShapeType shape_type) { const Mesh* mesh = model_objs->GetShapeBuffer(shape_type); if (mesh == nullptr) { mju_error("Unknown shape %d", shape_type); } - renderable.AppendMesh(mesh); + return mesh; } static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, const mjvScene* scene, ModelObjects* model_objects) { + std::vector meshes; + std::vector transforms; + + Trs trs = { + .translation = ReadFloat3(geom.pos), + .rotation = ReadMat3(geom.mat), + .size = ReadFloat3(geom.size), + }; + switch ((mjtGeom)geom.type) { case mjGEOM_MESH: - AddMesh(renderable, model_objects, geom.dataid); + meshes.push_back(GetMesh(model_objects, geom.dataid)); + // Ignore size for meshes. + transforms.push_back(mat4f(trs.rotation, trs.translation)); break; case mjGEOM_HFIELD: - AddHeightField(renderable, model_objects, geom.dataid); + meshes.push_back(GetHeightField(model_objects, geom.dataid)); + // Ignore size for height fields. + transforms.push_back(mat4f(trs.rotation, trs.translation)); break; - case mjGEOM_PLANE: - AddShape(renderable, model_objects, ModelObjects::kPlane); + case mjGEOM_PLANE: { + meshes.push_back(GetShape(model_objects, ModelObjects::kPlane)); + const bool is_infinite = !(trs.size.x > 0 && trs.size.y > 0); + if (is_infinite) { + // Infinite planes are scaled to match the tile size used by + // re-centering in engine_vis_visualize.c. + const float plane_scale = static_cast(mjMAXPLANEGRID) / 2.0f; + trs.size.x = plane_scale; + trs.size.y = plane_scale; + } + // Planes only define an xy size, so set the z-dimension to 1.0f. + trs.size.z = 1.0f; + transforms.push_back(trs.ToTransform()); break; + } case mjGEOM_SPHERE: - AddShape(renderable, model_objects, ModelObjects::kSphere); + meshes.push_back(GetShape(model_objects, ModelObjects::kSphere)); + transforms.push_back(trs.ToTransform()); break; case mjGEOM_ELLIPSOID: - AddShape(renderable, model_objects, ModelObjects::kSphere); + meshes.push_back(GetShape(model_objects, ModelObjects::kSphere)); + transforms.push_back(trs.ToTransform()); break; case mjGEOM_BOX: - AddShape(renderable, model_objects, ModelObjects::kBox); + meshes.push_back(GetShape(model_objects, ModelObjects::kBox)); + transforms.push_back(trs.ToTransform()); break; - case mjGEOM_CAPSULE: - AddShape(renderable, model_objects, ModelObjects::kTube); - AddShape(renderable, model_objects, ModelObjects::kDome); - AddShape(renderable, model_objects, ModelObjects::kDome); + case mjGEOM_CAPSULE: { + // Capsules are a tube with two domes at the ends. + meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDome)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDome)); + + transforms.push_back(trs.ToTransform()); + + // We apply an inverse scale to the domes to counteract the capsule's + // overall scale so that the domes remain spherical in shape. + const float xz_size = 0.5f * (trs.size.x + trs.size.y); + + // Move the first dome to the top of the capsule. + mat4f top = mat4f(trs.rotation, trs.translation); + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); + transforms.push_back(top); + + // Move the second dome to the bottom of the capsule and rotate it 180 + // degrees so that it's facing the right way. + mat4f bottom = mat4f(trs.rotation, trs.translation); + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); + transforms.push_back(bottom); break; - case mjGEOM_CYLINDER: - AddShape(renderable, model_objects, ModelObjects::kTube); - AddShape(renderable, model_objects, ModelObjects::kDisk); - AddShape(renderable, model_objects, ModelObjects::kDisk); + } + case mjGEOM_CYLINDER: { + // Cylinders are a tube with two disks at the ends. + meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + + transforms.push_back(trs.ToTransform()); + + // Move the first disk to the top of the cylinder. + mat4f top = mat4f(trs.rotation, trs.translation); + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(trs.size); + transforms.push_back(top); + + // Move the second disk to the bottom of the cylinder. Rotate the disk + // 180 degrees so that the normals point outwards. + mat4f bottom = mat4f(trs.rotation, trs.translation); + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(trs.size); + transforms.push_back(bottom); break; - case mjGEOM_ARROW: - AddShape(renderable, model_objects, ModelObjects::kTube); - AddShape(renderable, model_objects, ModelObjects::kCone); - AddShape(renderable, model_objects, ModelObjects::kDisk); + } + case mjGEOM_ARROW: { + meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); + meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + + mat4f base = mat4f(trs.rotation, trs.translation); + base *= mat4f::scaling(float3{1, 1, kArrowScale}); + base *= mat4f::translation(float3{0, 0, trs.size.z}); + transforms.push_back(base * mat4f::scaling(trs.size)); + + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + transforms.push_back(top * mat4f::scaling(trs.size)); + + mat4f top_disk = base; + top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); + top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + transforms.push_back(top_disk * mat4f::scaling(trs.size)); + + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + transforms.push_back(bottom * mat4f::scaling(trs.size)); + break; - case mjGEOM_ARROW1: - AddShape(renderable, model_objects, ModelObjects::kTube); - AddShape(renderable, model_objects, ModelObjects::kCone); - AddShape(renderable, model_objects, ModelObjects::kDisk); - AddShape(renderable, model_objects, ModelObjects::kDisk); + } + case mjGEOM_ARROW1: { + meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); + meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + + mat4f base = mat4f(trs.rotation, trs.translation); + base *= mat4f::scaling(float3{1, 1, kArrowScale}); + base *= mat4f::translation(float3{0, 0, trs.size.z}); + transforms.push_back(base * mat4f::scaling(trs.size)); + + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + transforms.push_back(top * mat4f::scaling(trs.size)); + + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + transforms.push_back(bottom * mat4f::scaling(trs.size)); break; - case mjGEOM_ARROW2: - AddShape(renderable, model_objects, ModelObjects::kTube); - AddShape(renderable, model_objects, ModelObjects::kCone); - AddShape(renderable, model_objects, ModelObjects::kCone); - AddShape(renderable, model_objects, ModelObjects::kDisk); - AddShape(renderable, model_objects, ModelObjects::kDisk); + } + case mjGEOM_ARROW2: { + meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); + meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); + meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + + mat4f base = mat4f(trs.rotation, trs.translation); + base *= mat4f::scaling(float3{1, 1, kArrowScale}); + base *= mat4f::translation(float3{0, 0, trs.size.z}); + transforms.push_back(base * mat4f::scaling(trs.size)); + + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + transforms.push_back(top * mat4f::scaling(trs.size)); + + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + transforms.push_back(bottom * mat4f::scaling(trs.size)); + + mat4f top_disk = base; + top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); + top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + transforms.push_back(top_disk * mat4f::scaling(trs.size)); + + mat4f bottom_disk = base; + bottom_disk *= mat4f::translation(float3{0, 0, -trs.size.z}); + transforms.push_back(bottom_disk * mat4f::scaling(trs.size)); + break; + } case mjGEOM_LINE: - AddShape(renderable, model_objects, ModelObjects::kLine); + meshes.push_back(GetShape(model_objects, ModelObjects::kLine)); + transforms.push_back(trs.ToTransform()); break; case mjGEOM_LINEBOX: - AddShape(renderable, model_objects, ModelObjects::kLineBox); + meshes.push_back(GetShape(model_objects, ModelObjects::kLineBox)); + transforms.push_back(trs.ToTransform()); break; case mjGEOM_TRIANGLE: - AddShape(renderable, model_objects, ModelObjects::kTriangle); + meshes.push_back(GetShape(model_objects, ModelObjects::kTriangle)); + transforms.push_back(trs.ToTransform()); break; case mjGEOM_FLEX: - AddSkinFlexMesh(renderable, model_objects, geom.objid); + meshes.push_back(GetSkinFlexMesh(model_objects, geom.objid)); + // Flexes are defined in global space. + transforms.push_back(mat4f()); break; case mjGEOM_SKIN: - AddSkinFlexMesh(renderable, model_objects, geom.objid); + meshes.push_back(GetSkinFlexMesh(model_objects, geom.objid)); + // Skins are defined in global space. + transforms.push_back(mat4f()); break; case mjGEOM_NONE: case mjGEOM_LABEL: @@ -193,124 +314,8 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, mju_warning("Unsupported geom type: %d", geom.type); break; } -} -static void SetGeomTransform(Renderable& renderable, const mjvGeom& geom) { - // Flex and skin geometries are in global space. - if (geom.type == mjGEOM_FLEX || geom.type == mjGEOM_SKIN) { - return; - } - - mat4 transform = mat4(ReadMat3(geom.mat), ReadFloat3(geom.pos)); - renderable.SetLayerMask(geom.category); - - float3 size = ReadFloat3(geom.size); - filament::TransformManager& tm = - renderable.GetEngine()->getTransformManager(); - for (int j = 0; j < renderable.GetNumMeshes(); ++j) { - const utils::Entity& entity = renderable[j]; - - // Update object transform. - mat4 entity_transform = transform; - - // Some built-in drawables are composed of multiple entities. For example, - // capsules are a combination of a open tube and two dome end caps. - - if (geom.type == mjGEOM_CYLINDER) { - // Cylinders are a tube with two disks at the ends. The "bottom" disk is - // rotated so that the normals point outwards. - if (j == kCylinderTopDisk) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - } else if (j == kCylinderBottomDisk) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - } - } else if (geom.type == mjGEOM_CAPSULE) { - // Capsules are a tube with two domes at the ends. We apply an inverse - // scale to the domes to "counteract" the capsule's overall scale so that - // the domes remain spherical in shape. - const float xz_size = 0.5f * (size.x + size.y); - if (j == kCapsuleTopDome) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - entity_transform *= mat4::scaling(float3{1, 1, xz_size / size.z}); - } else if (j == kCapsuleBottomDome) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - entity_transform *= mat4::scaling(float3{1, 1, xz_size / size.z}); - } - } else if (geom.type == mjGEOM_ARROW) { - // An arrow is a tube with a cone at the end and a disk cap at the other - // end. Because the cone head's base is larger than the tube, an extra - // disk is added to the base of the cone. This disk is rotated such that - // its normal points outwards. - entity_transform *= mat4::scaling(float3{1, 1, kArrowScale}); - entity_transform *= mat4::translation(float3{0, 0, size.z}); - if (j == kArrow0Cone) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } else if (j == kArrow0ConeDisk) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } else if (j == kArrow0BottomDisk) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - } - } else if (geom.type == mjGEOM_ARROW1) { - // An arrow1 is a tube with a cone at the end and a disk cap at the other - // end. - entity_transform *= mat4::scaling(float3{1, 1, kArrowScale}); - entity_transform *= mat4::translation(float3{0, 0, size.z}); - if (j == kArrow1Cone) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - } else if (j == kArrow1BottomDisk) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - } - } else if (geom.type == mjGEOM_ARROW2) { - // An arrow2 is a tube with a cone at both ends. Like the standard arrow, - // an extra disk is added to the base of each cone. - entity_transform *= mat4::scaling(float3{1, 1, kArrowScale}); - entity_transform *= mat4::translation(float3{0, 0, size.z}); - if (j == kArrow2TopCone) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } else if (j == kArrow2BottomCone) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } else if (j == kArrow2TopConeDisk) { - entity_transform *= mat4::translation(float3{0, 0, size.z}); - entity_transform *= mat4::rotation(std::numbers::pi, float3{1, 0, 0}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } else if (j == kArrow2BottomConeDisk) { - entity_transform *= mat4::translation(float3{0, 0, -size.z}); - entity_transform *= - mat4::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - } - } - if (geom.type == mjGEOM_PLANE) { - const bool is_infinite = !(size.x > 0 && size.y > 0); - if (is_infinite) { - // Infinite planes are scaled to match the tile size used by - // re-centering in engine_vis_visualize.c. - const float plane_scale = static_cast(mjMAXPLANEGRID) / 2.0f; - entity_transform *= - mat4::scaling(float3{plane_scale, plane_scale, 1.0f}); - } else { - // Regular planes are scaled by geom.size. - entity_transform *= mat4::scaling(float3{size.x, size.y, 1.0f}); - } - } else if (geom.type != mjGEOM_MESH && geom.type != mjGEOM_HFIELD) { - entity_transform *= mat4::scaling(size); - } - tm.setTransform(tm.getInstance(entity), entity_transform); - } + renderable.SetMeshes(meshes, transforms); } static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, @@ -334,10 +339,12 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, enable_reflection && geom.reflectance > 0 && params.color.a == 1.0f; } } - renderable.SetWireframe(scene->flags[mjRND_WIREFRAME]); + renderable.SetLayerMask(geom.category); if (geom.category == mjCAT_DECOR) { renderable.SetCastShadows(false); renderable.SetReceiveShadows(false); + } else { + renderable.SetWireframe(scene->flags[mjRND_WIREFRAME]); } MaterialTextures textures; @@ -471,10 +478,7 @@ std::unique_ptr CreateGeomRenderable( config.shading_model = shading_model; auto renderable = std::make_unique(object_mgr, config); - // The order of these calls is important. e.g. We need to create the filament - // renderable entities before we can set their transform. PrepareGeomMeshes(*renderable, geom, scene, model_objs); - SetGeomTransform(*renderable, geom); UpdateGeomMaterial(*renderable, geom, scene, model_objs, object_mgr, headpos); return renderable; diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 18d950d7..efcbd76f 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -277,13 +277,11 @@ void SceneView::Render(filament::Renderer* renderer, // Render reflection passes. if (request.draw_mode == DrawMode::Color) { - filament::TransformManager& tm = engine_->getTransformManager(); for (size_t i = 0; i < reflectives_.size(); ++i) { Renderable* renderable = reflectives_[i]; // We assume the 0th entity is the reflective entity. - const utils::Entity entity = (*renderable)[0]; - const mat4 transform(tm.getTransform(tm.getInstance(entity))); + mat4 transform(renderable->GetTransform()); SetupReflectionCamera(transform, camera_, reflect_camera_); // Hide reflective surface from its own reflection pass. @@ -305,8 +303,7 @@ void SceneView::Render(filament::Renderer* renderer, if (request.enable_ux) { ux_camera_->setProjection(filament::Camera::Projection::ORTHO, 0.0f, - viewport.width / request.gui_scale, - viewport.height / request.gui_scale, 0.0f, 0.0f, + viewport.width, viewport.height, 0.0f, 0.0f, 1.0f); ux_view_->setRenderTarget(render_target); renderer->render(ux_view_); diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index f50a86fa..a9a2874b 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -69,8 +69,6 @@ class SceneView { RenderTarget* target = nullptr; // Whether or not to render the UX as a separate pass. bool enable_ux = false; - // The scale factor to use for UX rendering. - float gui_scale = 1.0f; }; // Renders the scene. From 188196603d5c1bdfa36f4f6d9ba5b7395324a174 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 20 Apr 2026 09:27:03 -0700 Subject: [PATCH 105/251] Fix flexcomp empty cell detection that was causing missing cells. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The previous implementation only checked whether grid cells contained mesh vertices to determine occupancy. For coarse meshes with large faces, most cells were incorrectly marked empty and pruned—even cells fully inside the object volume. This change improves the algorithm by: - Marking cells that overlap with any mesh element's AABB as non-empty. - For surface meshes, running a flood-fill from the grid boundary through non-overlapping cells to identify truly exterior cells. This preserves empty interior cells, preventing incorrect pruning of the object's core. - For volumetric meshes, defaulting to element-AABB overlap detection directly. Limitations for non-watertight meshes: If the mesh contains holes larger than the grid cell size, the flood-fill will leak into the interior. In this case, all non-element cells (including interior ones) will be marked as empty. PiperOrigin-RevId: 902675331 Change-Id: I5a84303a33d5ca7436213e7ce9aca3806c9f5a0f --- src/user/user_flexcomp.cc | 125 ++++++++++++++++++++++++++++++------ test/user/user_flex_test.cc | 58 +++++++++++++++++ 2 files changed, 164 insertions(+), 19 deletions(-) diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index c62b5d6f..081781ac 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -13,12 +13,14 @@ // limitations under the License. #include +#include #include #include #include #include #include #include +#include #include #include #include @@ -108,27 +110,112 @@ void mjCFlexcomp::MarkEmptyCells(mjCFlex* flex, const double* points, int ncells = cx * cy * cz; int order = flex->spec.order; - // determine which cells contain mesh vertices - flex->cell_empty.assign(ncells, true); - for (int i = 0; i < npnt; i++) { - // compute parametric coordinates of mesh vertex in [0, 1]^3 - // for flat meshes (zero extent along an axis), default to 0.5 - double dx = minmax[3] - minmax[0]; - double dy = minmax[4] - minmax[1]; - double dz = minmax[5] - minmax[2]; - double sx = dx > 0 ? (points[3*i+0] - minmax[0]) / dx : 0.5; - double sy = dy > 0 ? (points[3*i+1] - minmax[1]) / dy : 0.5; - double sz = dz > 0 ? (points[3*i+2] - minmax[2]) / dz : 0.5; + // determine which cells contain mesh elements (not just vertices) + // for each element, compute its AABB and mark all overlapping cells + std::vector has_element(ncells, false); - // find containing cell - int ci = std::min((int)(sx * cx), cx - 1); - int cj = std::min((int)(sy * cy), cy - 1); - int ck = std::min((int)(sz * cz), cz - 1); - ci = std::max(ci, 0); - cj = std::max(cj, 0); - ck = std::max(ck, 0); + double dx = minmax[3] - minmax[0]; + double dy = minmax[4] - minmax[1]; + double dz = minmax[5] - minmax[2]; - flex->cell_empty[ci * cy * cz + cj * cz + ck] = false; + // vertices per element: dim+1 (edges=2, triangles=3, tets=4) + int nvpe = flex->spec.dim + 1; + + if (nvpe > 0 && !element.empty()) { + int nelem = element.size() / nvpe; + for (int e = 0; e < nelem; e++) { + // compute element AABB + double elo[3] = {1e30, 1e30, 1e30}; + double ehi[3] = {-1e30, -1e30, -1e30}; + for (int v = 0; v < nvpe; v++) { + int vid = element[nvpe * e + v]; + for (int j = 0; j < 3; j++) { + elo[j] = std::min(elo[j], points[3 * vid + j]); + ehi[j] = std::max(ehi[j], points[3 * vid + j]); + } + } + + // map element AABB to cell range + auto cellIdx = [](double coord, double lo, double d, int nc) { + if (d <= 0) return 0; + int c = (int)((coord - lo) / d * nc); + return std::max(0, std::min(nc - 1, c)); + }; + + int ci0 = cellIdx(elo[0], minmax[0], dx, cx); + int ci1 = cellIdx(ehi[0], minmax[0], dx, cx); + int cj0 = cellIdx(elo[1], minmax[1], dy, cy); + int cj1 = cellIdx(ehi[1], minmax[1], dy, cy); + int ck0 = cellIdx(elo[2], minmax[2], dz, cz); + int ck1 = cellIdx(ehi[2], minmax[2], dz, cz); + + // mark all overlapping cells as containing elements + for (int ci = ci0; ci <= ci1; ci++) { + for (int cj = cj0; cj <= cj1; cj++) { + for (int ck = ck0; ck <= ck1; ck++) { + has_element[ci * cy * cz + cj * cz + ck] = true; + } + } + } + } + } + + // default: all cells non-empty (only exterior cells will be empty) + flex->cell_empty.assign(ncells, false); + + // for dim=2 (surface mesh): check watertightness and flood-fill + if (flex->spec.dim == 2 && nvpe == 3 && !element.empty()) { + // flood-fill from grid boundary to find exterior cells + // cells reachable from the boundary through non-element cells + // are outside the mesh volume; cells NOT reachable are interior + std::vector visited(ncells, false); + std::queue> bfs; + + // seed BFS from boundary cells that have no elements + for (int ci = 0; ci < cx; ci++) { + for (int cj = 0; cj < cy; cj++) { + for (int ck = 0; ck < cz; ck++) { + if (ci == 0 || ci == cx - 1 || + cj == 0 || cj == cy - 1 || + ck == 0 || ck == cz - 1) { + int idx = ci * cy * cz + cj * cz + ck; + if (!has_element[idx] && !visited[idx]) { + visited[idx] = true; + flex->cell_empty[idx] = true; + bfs.push({ci, cj, ck}); + } + } + } + } + } + + // BFS: spread through non-element cells + const int dirs[6][3] = { + {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, + {0, 1, 0}, {0, 0, -1}, {0, 0, 1}}; + while (!bfs.empty()) { + auto [ci, cj, ck] = bfs.front(); + bfs.pop(); + for (auto& d : dirs) { + int ni = ci + d[0], nj = cj + d[1], nk = ck + d[2]; + if (ni < 0 || ni >= cx || + nj < 0 || nj >= cy || + nk < 0 || nk >= cz) { + continue; + } + int nidx = ni * cy * cz + nj * cz + nk; + if (!visited[nidx] && !has_element[nidx]) { + visited[nidx] = true; + flex->cell_empty[nidx] = true; + bfs.push({ni, nj, nk}); + } + } + } + } else { + // dim!=2 (e.g., tet mesh): cells without element overlap are empty + for (int c = 0; c < ncells; c++) { + flex->cell_empty[c] = !has_element[c]; + } } // pin nodes that belong exclusively to empty cells diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index f3df253c..c4d10e10 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -1179,6 +1179,64 @@ TEST_F(UserFlexTest, EmptyCellNodePinningQuadratic) { mj_deleteModel(m); } +TEST_F(UserFlexTest, EmptyCellDetectsElements) { + // A cube surface mesh (dim=2, 12 triangles) spanning [0,1]^3. + // With cellcount="6 6 6" (216 cells), only 8 corner cells contain + // mesh vertices. + // + // Bug: MarkEmptyCells only checked vertices, so 208/216 cells are + // marked empty, causing most interior nodes to be incorrectly pinned. + // Fix: check element AABBs to correctly identify occupied cells. + static constexpr char xml[] = R"( + + + + + + + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + // 6x6x6 trilinear grid: (6+1)^3 = 343 nodes + int nadr = m->flex_nodeadr[0]; + int nnode = m->flex_nodenum[0]; + ASSERT_EQ(nnode, 343); + + // Count pinned nodes: those assigned to the parent body. + int parent_bid = mj_name2id(m, mjOBJ_BODY, "parent"); + ASSERT_GT(parent_bid, 0); + int pinned = 0; + for (int n = nadr; n < nadr + nnode; n++) { + if (m->flex_nodebodyid[n] == parent_bid) { + pinned++; + } + } + + // The cube surface fills the entire bounding box. The element-AABB + // marks all boundary cells as surface cells (152/216). The interior + // flood-fill finds no exterior seeds (all boundary cells are surface), + // so the remaining 64 cells are classified as interior (non-empty). + // No cells are empty → 0 nodes pinned. + EXPECT_EQ(pinned, 0); + + mj_deleteData(mj_makeData(m)); + mj_deleteModel(m); +} + TEST_F(UserFlexTest, TotalMassTrilinear) { static constexpr char xml[] = R"( From 8e7787ad09007e0e5fd6311a00eb4d343ecfb2a4 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Mon, 20 Apr 2026 10:38:58 -0700 Subject: [PATCH 106/251] Use function for applying transforms to multimesh Renderables. PiperOrigin-RevId: 902712241 Change-Id: I5bfdab8bce12ffb1a1f030ae0a32a61e9b8157e1 --- .../filament/filament/renderable.cc | 29 ++- .../filament/filament/renderable.h | 12 +- .../filament/filament/scene_geom_util.cc | 234 ++++++++++-------- 3 files changed, 156 insertions(+), 119 deletions(-) diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 2a9659ce..f1a4e2bc 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -135,11 +135,23 @@ void Renderable::InitPartEntity(Part& part) { } void Renderable::SetTransform(const Trs& trs) { - transform_ = trs.ToTransform(); - filament::TransformManager& tm = GetEngine()->getTransformManager(); - for (Part& part : parts_) { - tm.setTransform(tm.getInstance(part.entity), transform_); + if (parts_.empty()) { + transform_ = trs.ToTransform(); + return; } + + filament::TransformManager& tm = GetEngine()->getTransformManager(); + if (get_transform_fn_) { + for (int i = 0; i < parts_.size(); ++i) { + const mat4f& transform = get_transform_fn_(i, trs); + tm.setTransform(tm.getInstance(parts_[i].entity), transform); + } + } else { + for (Part& part : parts_) { + tm.setTransform(tm.getInstance(part.entity), trs.ToTransform()); + } + } + transform_ = tm.getTransform(tm.getInstance(parts_[0].entity)); } const mat4f& Renderable::GetTransform() const { @@ -147,23 +159,18 @@ const mat4f& Renderable::GetTransform() const { } void Renderable::SetMeshes(std::span meshes, - std::span transforms) { - if (meshes.size() != transforms.size()) { - mju_error("Number of meshes does not match number of transforms."); - } + GetTransformFn get_transform_fn) { if (!parts_.empty()) { mju_error("Cannot set meshes for renderable with multiple parts."); } - filament::TransformManager& tm = GetEngine()->getTransformManager(); + get_transform_fn_ = get_transform_fn; for (int i = 0; i < meshes.size(); ++i) { Part& part = parts_.emplace_back(); part.mesh = meshes[i]; part.elem_offset = 0; part.elem_count = part.mesh->GetFilamentIndexBuffer()->getIndexCount(); InitPartEntity(part); - - tm.setTransform(tm.getInstance(part.entity), transforms[i]); } } diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 380eea3b..9227e634 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -16,6 +16,7 @@ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_RENDERABLE_H_ #include +#include #include #include @@ -83,10 +84,13 @@ class Renderable { // Returns the current transform of the renderable. const filament::math::mat4f& GetTransform() const; - // Sets multiple meshes for a renderable. Each mesh is assigned a specific - // transform to allow for assembly of compound shapes. + // Sets multiple meshes for a renderable. Users can optionally provide a + // function that will be used to compute the transform for each (sub)mesh + // relative to the transform of the renderable itself. This allows users to + // construct compound (but rigid) objects from multiple meshes. + using GetTransformFn = std::function; void SetMeshes(std::span meshes, - std::span transforms); + GetTransformFn get_transform = nullptr); // Sets the layer mask for the managed filament Entities. Layer masks can be // used to show/hide the renderable in different views. Returns the previous @@ -158,7 +162,7 @@ class Renderable { filament::Scene* assigned_scene_ = nullptr; std::vector parts_; filament::math::mat4f transform_; - + GetTransformFn get_transform_fn_; std::uint8_t priority_ = kDefaultPriority; std::uint8_t layer_mask_ = kDefaultLayerMask; std::uint16_t blend_order_ = 0; diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index 3b15b32d..80c653a7 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -103,7 +103,7 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, const mjvScene* scene, ModelObjects* model_objects) { std::vector meshes; - std::vector transforms; + Renderable::GetTransformFn get_transforms; Trs trs = { .translation = ReadFloat3(geom.pos), @@ -115,12 +115,12 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, case mjGEOM_MESH: meshes.push_back(GetMesh(model_objects, geom.dataid)); // Ignore size for meshes. - transforms.push_back(mat4f(trs.rotation, trs.translation)); + trs.size = float3{1.0f, 1.0f, 1.0f}; break; case mjGEOM_HFIELD: meshes.push_back(GetHeightField(model_objects, geom.dataid)); // Ignore size for height fields. - transforms.push_back(mat4f(trs.rotation, trs.translation)); + trs.size = float3{1.0f, 1.0f, 1.0f}; break; case mjGEOM_PLANE: { meshes.push_back(GetShape(model_objects, ModelObjects::kPlane)); @@ -134,20 +134,16 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, } // Planes only define an xy size, so set the z-dimension to 1.0f. trs.size.z = 1.0f; - transforms.push_back(trs.ToTransform()); break; } case mjGEOM_SPHERE: meshes.push_back(GetShape(model_objects, ModelObjects::kSphere)); - transforms.push_back(trs.ToTransform()); break; case mjGEOM_ELLIPSOID: meshes.push_back(GetShape(model_objects, ModelObjects::kSphere)); - transforms.push_back(trs.ToTransform()); break; case mjGEOM_BOX: meshes.push_back(GetShape(model_objects, ModelObjects::kBox)); - transforms.push_back(trs.ToTransform()); break; case mjGEOM_CAPSULE: { // Capsules are a tube with two domes at the ends. @@ -155,25 +151,31 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, meshes.push_back(GetShape(model_objects, ModelObjects::kDome)); meshes.push_back(GetShape(model_objects, ModelObjects::kDome)); - transforms.push_back(trs.ToTransform()); - - // We apply an inverse scale to the domes to counteract the capsule's - // overall scale so that the domes remain spherical in shape. - const float xz_size = 0.5f * (trs.size.x + trs.size.y); - - // Move the first dome to the top of the capsule. - mat4f top = mat4f(trs.rotation, trs.translation); - top *= mat4f::translation(float3{0, 0, trs.size.z}); - top *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); - transforms.push_back(top); - - // Move the second dome to the bottom of the capsule and rotate it 180 - // degrees so that it's facing the right way. - mat4f bottom = mat4f(trs.rotation, trs.translation); - bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); - bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - bottom *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); - transforms.push_back(bottom); + get_transforms = [](int index, const Trs& trs) { + // We apply an inverse scale to the domes to counteract the capsule's + // overall scale so that the domes remain spherical in shape. + const float xz_size = 0.5f * (trs.size.x + trs.size.y); + if (index == 0) { + return trs.ToTransform(); + } else if (index == 1) { + // Move the first dome to the top of the capsule. + mat4f top = mat4f(trs.rotation, trs.translation); + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); + return top; + } else if (index == 2) { + // Move the second dome to the bottom of the capsule and rotate it 180 + // degrees so that it's facing the right way. + mat4f bottom = mat4f(trs.rotation, trs.translation); + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); + return bottom; + } else { + mju_error("Invalid index for capsule geom: %d (expected [0,2])", index); + return trs.ToTransform(); + } + }; break; } case mjGEOM_CYLINDER: { @@ -182,21 +184,28 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); - transforms.push_back(trs.ToTransform()); - - // Move the first disk to the top of the cylinder. - mat4f top = mat4f(trs.rotation, trs.translation); - top *= mat4f::translation(float3{0, 0, trs.size.z}); - top *= mat4f::scaling(trs.size); - transforms.push_back(top); - - // Move the second disk to the bottom of the cylinder. Rotate the disk - // 180 degrees so that the normals point outwards. - mat4f bottom = mat4f(trs.rotation, trs.translation); - bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); - bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - bottom *= mat4f::scaling(trs.size); - transforms.push_back(bottom); + get_transforms = [](int index, const Trs& trs) { + if (index == 0) { + return trs.ToTransform(); + } else if (index == 1) { + // Move the first disk to the top of the cylinder. + mat4f top = mat4f(trs.rotation, trs.translation); + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(trs.size); + return top; + } else if (index == 2) { + // Move the second disk to the bottom of the cylinder. Rotate the disk + // 180 degrees so that the normals point outwards. + mat4f bottom = mat4f(trs.rotation, trs.translation); + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(trs.size); + return bottom; + } else { + mju_error("Invalid index for cylinder geom: %d (expected [0,2])", index); + return trs.ToTransform(); + } + }; break; } case mjGEOM_ARROW: { @@ -205,27 +214,33 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); - mat4f base = mat4f(trs.rotation, trs.translation); - base *= mat4f::scaling(float3{1, 1, kArrowScale}); - base *= mat4f::translation(float3{0, 0, trs.size.z}); - transforms.push_back(base * mat4f::scaling(trs.size)); - - mat4f top = base; - top *= mat4f::translation(float3{0, 0, trs.size.z}); - top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - transforms.push_back(top * mat4f::scaling(trs.size)); - - mat4f top_disk = base; - top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); - top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - transforms.push_back(top_disk * mat4f::scaling(trs.size)); - - mat4f bottom = base; - bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); - bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - transforms.push_back(bottom * mat4f::scaling(trs.size)); - + get_transforms = [](int index, const Trs& trs) { + mat4f base = mat4f(trs.rotation, trs.translation); + base *= mat4f::scaling(float3{1, 1, kArrowScale}); + base *= mat4f::translation(float3{0, 0, trs.size.z}); + if (index == 0) { + return base * mat4f::scaling(trs.size); + } else if (index == 1) { + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return top * mat4f::scaling(trs.size); + } else if (index == 2) { + mat4f top_disk = base; + top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); + top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return top_disk * mat4f::scaling(trs.size); + } else if (index == 3) { + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + return bottom * mat4f::scaling(trs.size); + } else { + mju_error("Invalid index for arrow geom: %d (expected [0,3])", index); + return trs.ToTransform(); + } + }; break; } case mjGEOM_ARROW1: { @@ -233,19 +248,26 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); + get_transforms = [](int index, const Trs& trs) { mat4f base = mat4f(trs.rotation, trs.translation); base *= mat4f::scaling(float3{1, 1, kArrowScale}); base *= mat4f::translation(float3{0, 0, trs.size.z}); - transforms.push_back(base * mat4f::scaling(trs.size)); - - mat4f top = base; - top *= mat4f::translation(float3{0, 0, trs.size.z}); - transforms.push_back(top * mat4f::scaling(trs.size)); - - mat4f bottom = base; - bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); - bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - transforms.push_back(bottom * mat4f::scaling(trs.size)); + if (index == 0) { + return base * mat4f::scaling(trs.size); + } else if (index == 1) { + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + return top * mat4f::scaling(trs.size); + } else if (index == 2) { + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + return bottom * mat4f::scaling(trs.size); + } else { + mju_error("Invalid index for arrow1 geom: %d (expected [0,2])", index); + return trs.ToTransform(); + } + }; break; } case mjGEOM_ARROW2: { @@ -255,55 +277,58 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); - mat4f base = mat4f(trs.rotation, trs.translation); - base *= mat4f::scaling(float3{1, 1, kArrowScale}); - base *= mat4f::translation(float3{0, 0, trs.size.z}); - transforms.push_back(base * mat4f::scaling(trs.size)); - - mat4f top = base; - top *= mat4f::translation(float3{0, 0, trs.size.z}); - top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - transforms.push_back(top * mat4f::scaling(trs.size)); - - mat4f bottom = base; - bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); - bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - bottom *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - transforms.push_back(bottom * mat4f::scaling(trs.size)); - - mat4f top_disk = base; - top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); - top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - transforms.push_back(top_disk * mat4f::scaling(trs.size)); - - mat4f bottom_disk = base; - bottom_disk *= mat4f::translation(float3{0, 0, -trs.size.z}); - transforms.push_back(bottom_disk * mat4f::scaling(trs.size)); - + get_transforms = [](int index, const Trs& trs) { + mat4f base = mat4f(trs.rotation, trs.translation); + base *= mat4f::scaling(float3{1, 1, kArrowScale}); + base *= mat4f::translation(float3{0, 0, trs.size.z}); + if (index == 0) { + return base * mat4f::scaling(trs.size); + } else if (index == 1) { + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return top * mat4f::scaling(trs.size); + } else if (index == 2) { + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return bottom * mat4f::scaling(trs.size); + } else if (index == 3) { + mat4f top_disk = base; + top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); + top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return top_disk * mat4f::scaling(trs.size); + } else if (index == 4) { + mat4f bottom_disk = base; + bottom_disk *= mat4f::translation(float3{0, 0, -trs.size.z}); + return bottom_disk * mat4f::scaling(trs.size); + } else { + mju_error("Invalid index for arrow2 geom: %d (expected [0,4])", index); + return trs.ToTransform(); + } + }; break; } case mjGEOM_LINE: meshes.push_back(GetShape(model_objects, ModelObjects::kLine)); - transforms.push_back(trs.ToTransform()); break; case mjGEOM_LINEBOX: meshes.push_back(GetShape(model_objects, ModelObjects::kLineBox)); - transforms.push_back(trs.ToTransform()); break; case mjGEOM_TRIANGLE: meshes.push_back(GetShape(model_objects, ModelObjects::kTriangle)); - transforms.push_back(trs.ToTransform()); break; case mjGEOM_FLEX: meshes.push_back(GetSkinFlexMesh(model_objects, geom.objid)); // Flexes are defined in global space. - transforms.push_back(mat4f()); + trs = Trs(); break; case mjGEOM_SKIN: meshes.push_back(GetSkinFlexMesh(model_objects, geom.objid)); // Skins are defined in global space. - transforms.push_back(mat4f()); + trs = Trs(); break; case mjGEOM_NONE: case mjGEOM_LABEL: @@ -315,7 +340,8 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, break; } - renderable.SetMeshes(meshes, transforms); + renderable.SetMeshes(meshes, get_transforms); + renderable.SetTransform(trs); } static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, From 6cb6e5a93f62235df6f00a38fe7f023ae99802ae Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Mon, 20 Apr 2026 11:59:24 -0700 Subject: [PATCH 107/251] Enable multiccd by default. PiperOrigin-RevId: 902752921 Change-Id: I8e2085ff17db0ac0db1641b8837415c458e5eca4 --- doc/includes/references.h | 8 ++++---- include/mujoco/mjmodel.h | 8 ++++---- mjx/mujoco/mjx/_src/types.py | 2 -- .../mjx/third_party/mujoco_warp/_src/collision_convex.py | 4 ++-- mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py | 2 +- mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py | 4 ++-- plugin/usd_decoder/usd_decoder.cc | 2 +- python/mujoco/bindings_test.py | 2 +- python/mujoco/introspect/enums.py | 8 ++++---- python/mujoco/introspect/enums_test.py | 5 ++--- src/engine/engine_collision_convex.c | 4 ++-- src/engine/engine_support.c | 4 ++-- src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc | 4 ++-- src/xml/xml_native_reader.cc | 2 +- src/xml/xml_native_writer.cc | 6 +++--- test/engine/engine_collision_convex_test.cc | 2 +- test/engine/engine_collision_gjk_test.cc | 2 +- test/engine/engine_solver_test.cc | 1 + .../experimental/usd/mjcPhysics/mjc_physics_scene_test.cc | 2 -- unity/Runtime/Bindings/MjBindings.cs | 8 ++++---- wasm/codegen/generated/bindings.cc | 2 +- wasm/tests/bindings_test.ts | 8 ++++---- 22 files changed, 43 insertions(+), 47 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 0ee5a22b..1e1db78d 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -494,8 +494,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags mjDSBL_AUTORESET = 1<<16, // automatic reset when numerical issues are detected mjDSBL_NATIVECCD = 1<<17, // native convex collision detection mjDSBL_ISLAND = 1<<18, // constraint island discovery + mjDSBL_MULTICCD = 1<<19, // multiple CCD contact points - mjNDISABLE = 19 // number of disable flags + mjNDISABLE = 20 // number of disable flags } mjtDisableBit; typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_OVERRIDE = 1<<0, // override contact parameters @@ -503,10 +504,9 @@ typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_FWDINV = 1<<2, // record solver statistics mjENBL_INVDISCRETE = 1<<3, // discrete-time inverse dynamics // experimental features: - mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection - mjENBL_SLEEP = 1<<5, // sleeping + mjENBL_SLEEP = 1<<4, // sleeping - mjNENABLE = 6 // number of enable flags + mjNENABLE = 5 // number of enable flags } mjtEnableBit; typedef enum mjtJoint_ { // type of degree of freedom mjJNT_FREE = 0, // global position and orientation (quat) (7) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index fba680f9..1c9214ae 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -70,8 +70,9 @@ typedef enum mjtDisableBit_ { // disable default feature bitflags mjDSBL_AUTORESET = 1<<16, // automatic reset when numerical issues are detected mjDSBL_NATIVECCD = 1<<17, // native convex collision detection mjDSBL_ISLAND = 1<<18, // constraint island discovery + mjDSBL_MULTICCD = 1<<19, // multiple CCD contact points - mjNDISABLE = 19 // number of disable flags + mjNDISABLE = 20 // number of disable flags } mjtDisableBit; @@ -81,10 +82,9 @@ typedef enum mjtEnableBit_ { // enable optional feature bitflags mjENBL_FWDINV = 1<<2, // record solver statistics mjENBL_INVDISCRETE = 1<<3, // discrete-time inverse dynamics // experimental features: - mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection - mjENBL_SLEEP = 1<<5, // sleeping + mjENBL_SLEEP = 1<<4, // sleeping - mjNENABLE = 6 // number of enable flags + mjNENABLE = 5 // number of enable flags } mjtEnableBit; diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 8bc60bd2..ff08579c 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -92,8 +92,6 @@ class EnableBit(enum.IntFlag): INVDISCRETE = mujoco.mjtEnableBit.mjENBL_INVDISCRETE # unsupported: OVERRIDE, ENERGY, FWDINV, ISLAND - # required by the C implementation only, ignored otherwise: MULTICCD - MULTICCD = mujoco.mjtEnableBit.mjENBL_MULTICCD SLEEP = mujoco.mjtEnableBit.mjENBL_SLEEP diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py index 4f6b21c5..88917ea7 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_convex.py @@ -36,7 +36,7 @@ from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAX_EPAHORIZON from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXCONPAIR from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXVAL from mujoco.mjx.third_party.mujoco_warp._src.types import Data -from mujoco.mjx.third_party.mujoco_warp._src.types import EnableBit +from mujoco.mjx.third_party.mujoco_warp._src.types import DisableBit from mujoco.mjx.third_party.mujoco_warp._src.types import GeomType from mujoco.mjx.third_party.mujoco_warp._src.types import Model from mujoco.mjx.third_party.mujoco_warp._src.types import mat43 @@ -1127,7 +1127,7 @@ def convex_narrowphase(m: Model, d: Data, ctx: CollisionContext, collision_table epa_iterations = 16 if nboxbox == ncollision else m.opt.ccd_iterations # set to true to enable multiccd - use_multiccd = m.opt.enableflags & EnableBit.MULTICCD + use_multiccd = m.opt.disableflags & DisableBit.MULTICCD == 0 # need at least 4 (square sides) if there's a box collision needing multiccd nmaxpolygon = 4 if nboxbox > 0 else 0 diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py index 4a743681..56bbb18c 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py @@ -370,7 +370,7 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: ) # check for unsupported margin + multicontact / box-box CCD combinations - use_multiccd = mjm.opt.enableflags & types.EnableBit.MULTICCD + use_multiccd = (mjm.opt.disableflags & types.DisableBit.MULTICCD) == 0 nativeccd_disabled = mjm.opt.disableflags & types.DisableBit.NATIVECCD BOX = int(mujoco.mjtGeom.mjGEOM_BOX) MESH = int(mujoco.mjtGeom.mjGEOM_MESH) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py index 7a9dc2a5..bf4f1b6b 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py @@ -184,6 +184,7 @@ class DisableBit(enum.IntFlag): EULERDAMP: implicit damping for Euler integration NATIVECCD: native convex collision detection (ignored in MJWarp) ISLAND: constraint islands + MULTICCD: multiple CCD contact points """ CONSTRAINT = mujoco.mjtDisableBit.mjDSBL_CONSTRAINT @@ -203,6 +204,7 @@ class DisableBit(enum.IntFlag): EULERDAMP = mujoco.mjtDisableBit.mjDSBL_EULERDAMP NATIVECCD = mujoco.mjtDisableBit.mjDSBL_NATIVECCD ISLAND = mujoco.mjtDisableBit.mjDSBL_ISLAND + MULTICCD = mujoco.mjtDisableBit.mjDSBL_MULTICCD # unsupported: MIDPHASE, AUTORESET @@ -212,12 +214,10 @@ class EnableBit(enum.IntFlag): Attributes: ENERGY: energy computation INVDISCRETE: discrete-time inverse dynamics - MULTICCD: multiple contacts with CCD """ ENERGY = mujoco.mjtEnableBit.mjENBL_ENERGY INVDISCRETE = mujoco.mjtEnableBit.mjENBL_INVDISCRETE - MULTICCD = mujoco.mjtEnableBit.mjENBL_MULTICCD # unsupported: OVERRIDE, FWDINV, ISLAND diff --git a/plugin/usd_decoder/usd_decoder.cc b/plugin/usd_decoder/usd_decoder.cc index f1ab1c05..04fe6976 100644 --- a/plugin/usd_decoder/usd_decoder.cc +++ b/plugin/usd_decoder/usd_decoder.cc @@ -704,7 +704,7 @@ void ParseUsdPhysicsScene(mjSpec* spec, bool multiccd_flag; mjc_physics_scene.GetMultiCCDFlagAttr().Get(&multiccd_flag); - spec->option.enableflags |= (multiccd_flag ? mjENBL_MULTICCD : 0); + spec->option.disableflags |= (!multiccd_flag ? mjDSBL_MULTICCD : 0); // Compiler attributes auto auto_limits_attr = mjc_physics_scene.GetAutoLimitsAttr(); diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 8dd91c60..c4b550d1 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -966,7 +966,7 @@ Euler integrator, semi-implicit in velocity. self.assertEqual(mujoco.mjtEnableBit.mjENBL_OVERRIDE, 1 << 0) self.assertEqual(mujoco.mjtEnableBit.mjENBL_ENERGY, 1 << 1) self.assertEqual(mujoco.mjtEnableBit.mjENBL_FWDINV, 1 << 2) - self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 6) + self.assertEqual(mujoco.mjtEnableBit.mjNENABLE, 5) self.assertEqual(mujoco.mjtGeom.mjGEOM_PLANE, 0) self.assertEqual(mujoco.mjtGeom.mjGEOM_HFIELD, 1) self.assertEqual(mujoco.mjtGeom.mjGEOM_SPHERE, 2) diff --git a/python/mujoco/introspect/enums.py b/python/mujoco/introspect/enums.py index d686e861..1ec77cb1 100644 --- a/python/mujoco/introspect/enums.py +++ b/python/mujoco/introspect/enums.py @@ -46,7 +46,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjDSBL_AUTORESET', 65536), ('mjDSBL_NATIVECCD', 131072), ('mjDSBL_ISLAND', 262144), - ('mjNDISABLE', 19), + ('mjDSBL_MULTICCD', 524288), + ('mjNDISABLE', 20), ]), )), ('mjtEnableBit', @@ -58,9 +59,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjENBL_ENERGY', 2), ('mjENBL_FWDINV', 4), ('mjENBL_INVDISCRETE', 8), - ('mjENBL_MULTICCD', 16), - ('mjENBL_SLEEP', 32), - ('mjNENABLE', 6), + ('mjENBL_SLEEP', 16), + ('mjNENABLE', 5), ]), )), ('mjtJoint', diff --git a/python/mujoco/introspect/enums_test.py b/python/mujoco/introspect/enums_test.py index d023fbcb..e373034b 100644 --- a/python/mujoco/introspect/enums_test.py +++ b/python/mujoco/introspect/enums_test.py @@ -42,9 +42,8 @@ class EnumsTest(absltest.TestCase): ('mjENBL_ENERGY', 1<<1), ('mjENBL_FWDINV', 1<<2), ('mjENBL_INVDISCRETE', 1<<3), - ('mjENBL_MULTICCD', 1<<4), - ('mjENBL_SLEEP', 1<<5), - ('mjNENABLE', 6))) + ('mjENBL_SLEEP', 1<<4), + ('mjNENABLE', 5))) # values mostly increment by one with occasional overrides def test_mjtGeom(self): # pylint: disable=invalid-name diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index a847a099..46f4cf09 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -828,7 +828,7 @@ static int maxContacts(const mjModel* m, const mjCCDObj* obj1, const mjCCDObj* o // reduce mesh collisions to 4 contacts max if (type1 == mjGEOM_BOX || type1 == mjGEOM_MESH) { if (type2 == mjGEOM_BOX || type2 == mjGEOM_MESH) { - return mjENABLED(mjENBL_MULTICCD) ? 4 : 1; + return mjDISABLED(mjDSBL_MULTICCD) ? 1 : 4; } } @@ -857,7 +857,7 @@ int mjc_Convex(const mjModel* m, mjData* d, mjContact* con, int g1, int g2, mjtN } // look for additional contacts - if (ncon == 1 && mjENABLED(mjENBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?) + if (ncon == 1 && !mjDISABLED(mjDSBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?) && m->geom_type[g1] != mjGEOM_ELLIPSOID && m->geom_type[g1] != mjGEOM_SPHERE && m->geom_type[g2] != mjGEOM_ELLIPSOID && m->geom_type[g2] != mjGEOM_SPHERE) { // multiCCD parameters diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index a8725bf7..2cff8056 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -66,7 +66,8 @@ const char* mjDISABLESTRING[mjNDISABLE] = { "Eulerdamp", "AutoReset", "NativeCCD", - "Island" + "Island", + "MultiCCD" }; @@ -76,7 +77,6 @@ const char* mjENABLESTRING[mjNENABLE] = { "Energy", "Fwdinv", "InvDiscrete", - "MultiCCD", "Sleep" }; diff --git a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc index 2820a3aa..ccc4158c 100644 --- a/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc +++ b/src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc @@ -649,7 +649,6 @@ class ModelWriter { }; const std::vector> enable_flags = { - {MjcPhysicsTokens->mjcFlagMulticcd, mjENBL_MULTICCD}, {MjcPhysicsTokens->mjcFlagFwdinv, mjENBL_FWDINV}, {MjcPhysicsTokens->mjcFlagEnergy, mjENBL_ENERGY}, {MjcPhysicsTokens->mjcFlagOverride, mjENBL_OVERRIDE}, @@ -677,7 +676,8 @@ class ModelWriter { {MjcPhysicsTokens->mjcFlagEulerdamp, mjDSBL_EULERDAMP}, {MjcPhysicsTokens->mjcFlagAutoreset, mjDSBL_AUTORESET}, {MjcPhysicsTokens->mjcFlagNativeccd, mjDSBL_NATIVECCD}, - {MjcPhysicsTokens->mjcFlagIsland, mjDSBL_ISLAND}}; + {MjcPhysicsTokens->mjcFlagIsland, mjDSBL_ISLAND}, + {MjcPhysicsTokens->mjcFlagMulticcd, mjDSBL_MULTICCD}}; for (const auto &[token, flag] : disable_flags) { create_flag_attr(token, flag, false); } diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 5e17aa1d..af291787 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -1284,6 +1284,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { READDSBL("autoreset", mjDSBL_AUTORESET) READDSBL("nativeccd", mjDSBL_NATIVECCD) READDSBL("island", mjDSBL_ISLAND) + READDSBL("multiccd", mjDSBL_MULTICCD) #undef READDSBL #define READENBL(NAME, MASK) \ @@ -1295,7 +1296,6 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) { READENBL("energy", mjENBL_ENERGY) READENBL("fwdinv", mjENBL_FWDINV) READENBL("invdiscrete", mjENBL_INVDISCRETE) - READENBL("multiccd", mjENBL_MULTICCD) READENBL("sleep", mjENBL_SLEEP) #undef READENBL } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index ebe95bab..0bc4c07a 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -1093,7 +1093,7 @@ void mjXWriter::Option(XMLElement* root) { XMLElement* sub = InsertEnd(section, "flag"); #define WRITEDSBL(NAME, MASK) \ - if( model->option.disableflags & MASK ) \ + if (model->option.disableflags & MASK) \ WriteAttrKey(sub, NAME, enable_map, 2, 0); WRITEDSBL("constraint", mjDSBL_CONSTRAINT) WRITEDSBL("equality", mjDSBL_EQUALITY) @@ -1114,16 +1114,16 @@ void mjXWriter::Option(XMLElement* root) { WRITEDSBL("autoreset", mjDSBL_AUTORESET) WRITEDSBL("nativeccd", mjDSBL_NATIVECCD) WRITEDSBL("island", mjDSBL_ISLAND) + WRITEDSBL("multiccd", mjDSBL_MULTICCD) #undef WRITEDSBL #define WRITEENBL(NAME, MASK) \ - if( model->option.enableflags & MASK ) \ + if (model->option.enableflags & MASK) \ WriteAttrKey(sub, NAME, enable_map, 2, 1); WRITEENBL("override", mjENBL_OVERRIDE) WRITEENBL("energy", mjENBL_ENERGY) WRITEENBL("fwdinv", mjENBL_FWDINV) WRITEENBL("invdiscrete", mjENBL_INVDISCRETE) - WRITEENBL("multiccd", mjENBL_MULTICCD) WRITEENBL("sleep", mjENBL_SLEEP) #undef WRITEENBL } diff --git a/test/engine/engine_collision_convex_test.cc b/test/engine/engine_collision_convex_test.cc index 9615b542..7d4346e5 100644 --- a/test/engine/engine_collision_convex_test.cc +++ b/test/engine/engine_collision_convex_test.cc @@ -68,7 +68,7 @@ TEST_F(MjcConvexTest, CylinderBox) { EXPECT_EQ(data->ncon, 5); // with multiCCD disabled, should find 1 contact - model->opt.enableflags &= ~mjENBL_MULTICCD; + model->opt.disableflags |= mjDSBL_MULTICCD; mj_forward(model, data); EXPECT_EQ(data->ncon, 1); diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 653abbb4..4e4f9f10 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -1998,7 +1998,7 @@ TEST_F(MjGjkTest, CylinderBoxMargin) { diff --git a/test/engine/engine_solver_test.cc b/test/engine/engine_solver_test.cc index 523dfd33..8aaf1d9d 100644 --- a/test/engine/engine_solver_test.cc +++ b/test/engine/engine_solver_test.cc @@ -46,6 +46,7 @@ TEST_F(SolverTest, IslandsEquivalent) { model->opt.tolerance = 0; // set tolerance to 0 model->opt.ls_tolerance = 0; // set ls_tolerance to 0 model->opt.ccd_tolerance = 0; // set ccd_tolerance to 0 + model->opt.disableflags |= mjDSBL_MULTICCD; // disable multiccd int nv = model->nv; diff --git a/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc b/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc index 30cb4d70..d7f106d4 100644 --- a/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc +++ b/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc @@ -179,8 +179,6 @@ TEST_F(MjcPhysicsSceneTest, TestDefaults) { EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(FwdinvFlag, mjENBL_FWDINV); EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(InvDiscreteFlag, mjENBL_INVDISCRETE); - EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(MultiCCDFlag, - mjENBL_MULTICCD); mj_deleteModel(default_model); mj_deleteSpec(empty_spec); diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 1673c4d0..d5ea2b37 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -137,16 +137,16 @@ public enum mjtDisableBit : int{ mjDSBL_AUTORESET = 65536, mjDSBL_NATIVECCD = 131072, mjDSBL_ISLAND = 262144, - mjNDISABLE = 19, + mjDSBL_MULTICCD = 524288, + mjNDISABLE = 20, } public enum mjtEnableBit : int{ mjENBL_OVERRIDE = 1, mjENBL_ENERGY = 2, mjENBL_FWDINV = 4, mjENBL_INVDISCRETE = 8, - mjENBL_MULTICCD = 16, - mjENBL_SLEEP = 32, - mjNENABLE = 6, + mjENBL_SLEEP = 16, + mjNENABLE = 5, } public enum mjtJoint : int{ mjJNT_FREE = 0, diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 185f5bb7..dfe18aea 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -10942,6 +10942,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjDSBL_AUTORESET", mjDSBL_AUTORESET) .value("mjDSBL_NATIVECCD", mjDSBL_NATIVECCD) .value("mjDSBL_ISLAND", mjDSBL_ISLAND) + .value("mjDSBL_MULTICCD", mjDSBL_MULTICCD) .value("mjNDISABLE", mjNDISABLE); enum_("mjtDyn") .value("mjDYN_NONE", mjDYN_NONE) @@ -10956,7 +10957,6 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .value("mjENBL_ENERGY", mjENBL_ENERGY) .value("mjENBL_FWDINV", mjENBL_FWDINV) .value("mjENBL_INVDISCRETE", mjENBL_INVDISCRETE) - .value("mjENBL_MULTICCD", mjENBL_MULTICCD) .value("mjENBL_SLEEP", mjENBL_SLEEP) .value("mjNENABLE", mjNENABLE); enum_("mjtEq") diff --git a/wasm/tests/bindings_test.ts b/wasm/tests/bindings_test.ts index 516cf507..a58fd087 100644 --- a/wasm/tests/bindings_test.ts +++ b/wasm/tests/bindings_test.ts @@ -684,10 +684,10 @@ describe('MuJoCo WASM Bindings', () => { it('should check constants values', () => { expect(mujoco.mjNEQDATA).toBe(11); expect(mujoco.mjDISABLESTRING).toEqual([ - 'Constraint', 'Equality', 'Frictionloss', 'Limit', 'Contact', 'Spring', - 'Damper', 'Gravity', 'Clampctrl', 'Warmstart', 'Filterparent', - 'Actuation', 'Refsafe', 'Sensor', 'Midphase', 'Eulerdamp', 'AutoReset', - 'NativeCCD', 'Island' + 'Constraint', 'Equality', 'Frictionloss', 'Limit', 'Contact', + 'Spring', 'Damper', 'Gravity', 'Clampctrl', 'Warmstart', + 'Filterparent', 'Actuation', 'Refsafe', 'Sensor', 'Midphase', + 'Eulerdamp', 'AutoReset', 'NativeCCD', 'Island', 'MultiCCD', ]); expect(mujoco.mjRNDSTRING).toEqual([ ['Shadow', '1', 'S'], ['Wireframe', '0', 'W'], ['Reflection', '1', 'R'], From bc5883e82f20d2596816944c3c9c64de021927e7 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Mon, 20 Apr 2026 12:52:30 -0700 Subject: [PATCH 108/251] Update multiccd documentation. PiperOrigin-RevId: 902779253 Change-Id: I767cd0a230b78efe2a71f83e5f2f134268997dcb --- doc/XMLreference.rst | 2 +- doc/changelog.rst | 8 ++++++++ doc/computation/index.rst | 20 +++++++++++--------- doc/mjwarp/index.rst | 6 ++---- doc/modeling.rst | 9 +++++---- 5 files changed, 27 insertions(+), 18 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index be74eede..f01541e4 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -677,7 +677,7 @@ from its default. .. _option-flag-multiccd: -:at:`multiccd`: :at-val:`[disable, enable], "disable"` +:at:`multiccd`: :at-val:`[disable, enable], "enable"` This flag enables multiple-contact collision detection for geom pairs that use a general-purpose convex-convex collider e.g., mesh-mesh collisions. This can be useful when the contacting geoms have a flat surface and the single contact point generated by the convex-convex collider cannot accurately capture the surface contact, leading diff --git a/doc/changelog.rst b/doc/changelog.rst index 218bec89..451b8d0c 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -14,6 +14,14 @@ General number of degrees of freedom per constraint row. The equality can be associated with a specific cell with the new attribute ":ref:`cell ` + .. admonition:: Breaking API changes + :class: attention + + - The feature :ref:`multiccd` is now enabled by default. This feature has little performance overhead + and gives better contact behavior for stability. + + **Migration:** The flag :ref:`multiccd` must be explicitly disabled. + Version 3.7.0 (April 14, 2026) ------------------------------ diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 01b06886..08eea043 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -1656,23 +1656,25 @@ Both pipelines are controlled by a tolerance (in units of distance) and maximum Multiple contacts ^^^^^^^^^^^^^^^^^ -Some colliders can return more than one contact per colliding pair to model line or surface contacts, as when two flat +Some colliders can return more than one contact per colliding pair to model edge or surface contacts, as when two flat objects touch. For example the capsule-plane and box-plane colliders can return up to two or four contacts, -respectively. Standard general-purpose convex collision algorithms like MPR and GJK always return a single contact +respectively. Standard general-purpose convex collision algorithms like MPR and GJK/EPA always return a single contact point, which is problematic for surface contact scenarios (e.g., box-stacking). Both of MuJoCo's CCD pipelines can return multiple points per contacting pair ("multiccd"). This behavior is controlled by the :ref:`multiccd` flag, but is implemented in different ways with different trade-offs: -libccd pipeline (legacy) +multi-run pipeline (legacy) Multiple contact points are found by rotating the two geoms by ±1e-3 radians around the tangential axes and re-running the collision routine. If a new contact is detected it is added, allowing for up to 4 additional contact - points. This method is effective, but increases the cost of each collision call by a factor of 5. + points. This method is effective, but increases the cost of each collision call by a factor of 5. This method is + used when the :ref:`nativeccd` flag is disabled, and for geoms collisions involving cylinders + and capsules or with :ref:`positive contact margins`. -native pipeline - Native multiccd discovers multiple contacts using a novel analysis of the contacting surfaces at the solution, - avoiding full re-runs of the collision routine, and is thus effectively "free". Note that native multiccd currently - does not support positive contact margins. If one of the two geoms has a positive margin, native multiccd will fall - back to legacy algorithm. +single-shot pipeline + The single-shot pipeline is used in conjunction with the native CCD pipeline, i.e., when the + :ref:`nativeccd` flag is enabled. As this pipeline is one-shot and most of the geom analysis + is done at compilation time, there is very little performance overhead. Supported geoms are boxes and meshes without + :ref:`positive contact margins`. .. _coDistance: diff --git a/doc/mjwarp/index.rst b/doc/mjwarp/index.rst index bd65306b..f5d10431 100644 --- a/doc/mjwarp/index.rst +++ b/doc/mjwarp/index.rst @@ -1265,8 +1265,6 @@ is available by setting the ``NATIVECCD`` disable flag: The specialized collider generates up to 8 contact points, compared to up to 4 for the convex pipeline, and may improve contact stability for tasks involving box stacking or manipulation. -.. TODO(taylorhowell): update this section once multiccd is on by default. - CCD margin ---------- @@ -1283,8 +1281,8 @@ CCD colliders and will raise a ``NotImplementedError`` when calling :func:`mjw.p - Scenario - Workaround * - box-box, box-mesh, mesh-mesh - - :ref:`MULTICCD ` enabled - - Set margin to ``0`` or do not enable ``MULTICCD`` + - :ref:`MULTICCD ` enabled (on by default) + - Set margin to ``0`` or disable ``MULTICCD`` * - box-box - :ref:`NATIVECCD ` enabled (on by default) - Set margin to ``0`` or disable ``NATIVECCD`` diff --git a/doc/modeling.rst b/doc/modeling.rst index 017e68fc..c5ed084a 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -1767,10 +1767,11 @@ better visualize and understand the contact configuration and resulting forces. a. Improve the geometry of the contacting geoms in order to add more contact points, possibly with non-flat geometry (e.g., bumps), so slippage is prevented by the normal force and not only frictional components. - b. If contacts are between flat surfaces, try enabling the :ref:`multiccd` flag, which allows - the detector to find more contacts than the single contact returned by the convex-convex collider. - c. Try enabling the native collision detection pipeline by setting the :ref:`nativeccd` flag, - which uses a more accurate and efficient convex collision detection algorithm. + b. If contacts are between flat surfaces, make sure that the flag :ref:`multiccd` is not + disabled (enabled by default), as it allows the detector to find more contacts than the single contact + returned by the convex-convex collider. + c. Make sure that the flag :ref:`nativeccd` is not disabled (enabled by default), + as NativeCCD is a more accurate and efficient convex collision detection algorithm. **High-frequency vibration** High-frequency, low-amplitude vibrations are also a real-world problem in many industrial settings, but unlike in From 3325971840e92177ca3a38a539f446cf739b36cc Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Tue, 21 Apr 2026 04:02:34 -0700 Subject: [PATCH 109/251] Add mj_maxContact API function. PiperOrigin-RevId: 903135055 Change-Id: I5f103c7d51f97e327c923bc567002ace835f5517 --- doc/APIreference/functions.rst | 12 ++ doc/changelog.rst | 3 +- doc/computation/index.rst | 145 +++++++++++++++++--- doc/css/theme_overrides.css | 92 +++++++++++++ doc/includes/references.h | 1 + include/mujoco/mujoco.h | 5 + python/mujoco/functions.cc | 1 + python/mujoco/introspect/functions.py | 26 ++++ src/engine/engine_collision_driver.c | 100 +++++++++++++- src/engine/engine_collision_driver.h | 8 +- test/engine/engine_collision_driver_test.cc | 68 +++++++++ unity/Runtime/Bindings/MjBindings.cs | 3 + wasm/codegen/generated/bindings.cc | 5 + 13 files changed, 448 insertions(+), 21 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index ef49bf39..2e83a385 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -1179,6 +1179,18 @@ It is also triggered for :ref:`user sensors` of :ref:`stage`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_maxContact + +Return the maximum number of contacts that can be generated between two geoms. + +If has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 +indicates that the geoms have a positive margin. + .. _mj_collision: `mj_collision <#mj_collision>`__ diff --git a/doc/changelog.rst b/doc/changelog.rst index 451b8d0c..03b768f7 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,7 +7,8 @@ Upcoming version (not yet released) General ^^^^^^^ - +- Added new :ref:`mj_maxContact` function to get the maximum number of possible contacts returned by + two geoms. - Added :ref:`multi-cell support` for trilinear and quadratic flexes. Note that the implicit integrator uses a dense solver for the flex degrees of freedom, which can be slow for multi-cell flexes. - Refactored ``flexstrain`` equality constraints to be instantiated per cell instead of per flex object, reducing the diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 08eea043..07f9adb5 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -1718,9 +1718,36 @@ work, but it pays off at runtime and yields both faster and more stable simulati Pair-wise colliders ^^^^^^^^^^^^^^^^^^^ -The table below provides information about the colliders used for different geom pairs. The second row in each cell -lists the maximum number of contacts generated, possibly with ``multiccd`` enabled. For example, ``Mesh`` / ``Mesh`` -will generate up to 1 contact or with ``multiccd`` up to 4 contacts. +The table below provides information about the colliders used for different geom pairs. These values can be computed +dynamically by the :ref:`mj_maxContact` function. Use the toggles to see the max number of contacts returned with the +parameters :ref:`nativeccd`, :ref:`multiccd`, and +:ref:`margin`. + +.. raw:: html + +

+
+ + nativeccd +
+
+ + multiccd +
+
+ + with margin +
+
.. list-table:: :header-rows: 1 @@ -1744,7 +1771,7 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - | primitive | **1** - | primitive - | **2** + | **4** - | primitive | **4** - | primitive @@ -1787,12 +1814,28 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. | **2** - | CCD | **1** - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ - | primitive | **2** - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ - | SDF | :ref:`sdf_initpoints ` * - Ellipsoid @@ -1812,12 +1855,36 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - - - - - | CCD - | **1**, **4** - - | CCD - | **1**, **4** - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ + - + .. raw:: html + +
CCD
+
+
1
+
5
+
5
+
+ - | SDF | :ref:`sdf_initpoints ` * - Box @@ -1827,8 +1894,16 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - - | primitive | **8** - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
4
+
5
+
+ - | SDF | :ref:`sdf_initpoints ` * - Mesh @@ -1837,8 +1912,16 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - - - - - | CCD - | **1**, **4** + - + .. raw:: html + +
CCD
+
+
1
+
4
+
5
+
+ - | MeshSDF | :ref:`sdf_initpoints ` * - SDF @@ -1851,6 +1934,32 @@ will generate up to 1 contact or with ``multiccd`` up to 4 contacts. - | SDF | :ref:`sdf_initpoints ` +.. raw:: html + + + + + .. _Sleeping: Sleeping islands diff --git a/doc/css/theme_overrides.css b/doc/css/theme_overrides.css index 45f94131..a7318056 100644 --- a/doc/css/theme_overrides.css +++ b/doc/css/theme_overrides.css @@ -54,6 +54,98 @@ body[data-theme="dark"] table.docutils:not(.mjcf-attributes) { font-size: 85%; } +.pairwise-toggles { + display: flex; + align-items: center; + gap: 1.5em; + margin-bottom: 0.75em; +} + +.pairwise-toggle-item { + display: flex; + align-items: center; + gap: 0.5em; +} + +.pairwise-switch { + position: relative; + display: inline-block; + width: 36px; + height: 20px; +} + +.pairwise-switch input { + opacity: 0; + width: 0; + height: 0; +} + +.pairwise-slider { + position: absolute; + cursor: pointer; + inset: 0; + background-color: #ccc; + transition: 0.3s; + border-radius: 20px; +} + +.pairwise-slider:before { + content: ""; + position: absolute; + height: 14px; + width: 14px; + left: 3px; + bottom: 3px; + background-color: white; + transition: 0.3s; + border-radius: 50%; +} + +.pairwise-switch input:checked + .pairwise-slider { + background-color: var(--secondary-header-color, #123693); +} + +.pairwise-switch input:checked + .pairwise-slider:before { + transform: translateX(16px); +} + +.multiccd-off, +.multiccd-native, +.multiccd-legacy { + display: none; + margin: 0; +} + +.multiccd-off { + display: inline; +} + +.multiccd-enabled .multiccd-off { + display: none; +} + +.multiccd-enabled.nativeccd-enabled:not(.margin-enabled) .multiccd-native { + display: inline; +} + +.multiccd-enabled:not(.nativeccd-enabled) .multiccd-legacy, +.multiccd-enabled.nativeccd-enabled.margin-enabled .multiccd-legacy { + display: inline; +} + +.margin-show { + display: none; +} + +.margin-enabled .margin-hide { + display: none; +} + +.margin-enabled .margin-show { + display: inline; +} + + .small-centered td, .small-centered th, .table-pairwise td, .table-pairwise th { text-align: center !important; diff --git a/doc/includes/references.h b/doc/includes/references.h index 1e1db78d..4d55e8ba 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3272,6 +3272,7 @@ void mj_passive(const mjModel* m, mjData* d); void mj_subtreeVel(const mjModel* m, mjData* d); void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result); void mj_rnePostConstraint(const mjModel* m, mjData* d); +int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin); void mj_collision(const mjModel* m, mjData* d); void mj_makeConstraint(const mjModel* m, mjData* d); void mj_island(const mjModel* m, mjData* d); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index fa67e0bf..31954cfb 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -460,6 +460,11 @@ MJAPI void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result); // RNE with complete data: compute cacc, cfrc_ext, cfrc_int. MJAPI void mj_rnePostConstraint(const mjModel* m, mjData* d); +// Return the maximum number of contacts that can be generated between two geoms. +// If has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 +// indicates that the geoms have a positive margin. +MJAPI int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin); + // Run collision detection. MJAPI void mj_collision(const mjModel* m, mjData* d); diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index d6e0c724..03a1be1b 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -304,6 +304,7 @@ PYBIND11_MODULE(_functions, pymodule) { m, d, flg_acc, result.data()); }); Def(pymodule); + Def(pymodule); Def(pymodule); Def(pymodule); Def(pymodule); diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index f05bd8a5..ee5795f4 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -2407,6 +2407,32 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='RNE with complete data: compute cacc, cfrc_ext, cfrc_int.', )), + ('mj_maxContact', + FunctionDecl( + name='mj_maxContact', + return_type=ValueType(name='int'), + parameters=( + FunctionParameterDecl( + name='m', + type=PointerType( + inner_type=ValueType(name='mjModel', is_const=True), + ), + ), + FunctionParameterDecl( + name='g1', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='g2', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='has_margin', + type=ValueType(name='int'), + ), + ), + doc='Return the maximum number of contacts that can be generated between two geoms. If has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 indicates that the geoms have a positive margin.', # pylint: disable=line-too-long + )), ('mj_collision', FunctionDecl( name='mj_collision', diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index b77164f6..ca271e5a 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -38,7 +38,7 @@ #include "engine/engine_util_spatial.h" -// table of pair-wise collision functions +// table of pairwise collision functions mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = { /* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH SDF */ /*PLANE */ {0, 0, mjc_PlaneSphere, mjc_PlaneCapsule, mjc_PlaneConvex, mjc_PlaneCylinder, mjc_PlaneBox, mjc_PlaneConvex, mjc_PlaneConvex}, @@ -56,6 +56,104 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = { //------------------------------------ utility functions ------------------------------------------ + +// return the maximum number of contacts that can be generated between two geoms +// if has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 +// indicates that the geoms have a positive margin +int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin) { + int type1 = m->geom_type[g1]; + int type2 = m->geom_type[g2]; + + if (type1 == mjGEOM_SDF || type2 == mjGEOM_SDF) { + return m->opt.sdf_initpoints; + } + + if (type1 == mjGEOM_HFIELD || type2 == mjGEOM_HFIELD) { + int type = (type1 == mjGEOM_HFIELD) ? type2 : type1; + return (type != mjGEOM_PLANE && type != mjGEOM_HFIELD) ? mjMAXCONPAIR : 0; + } + + // spheres and ellipsoids always generate a single contact + if (type1 == mjGEOM_SPHERE || type1 == mjGEOM_ELLIPSOID || + type2 == mjGEOM_SPHERE || type2 == mjGEOM_ELLIPSOID) { + return 1; + } + + // box-box primitive collider + if (type1 == mjGEOM_BOX && type2 == mjGEOM_BOX) { + return 8; + } + + // capsule-capsule primitive collider + if (type1 == mjGEOM_CAPSULE && type2 == mjGEOM_CAPSULE) { + return 2; + } + + // capsule-box primitive collider + if ((type1 == mjGEOM_CAPSULE && type2 == mjGEOM_BOX) || + (type1 == mjGEOM_BOX && type2 == mjGEOM_CAPSULE)) { + return 4; + } + + // the remaining plane cases + if (type1 == mjGEOM_PLANE || type2 == mjGEOM_PLANE) { + int type = (type1 == mjGEOM_PLANE) ? type2 : type1; + switch (type) { + case mjGEOM_CAPSULE: + return 2; + case mjGEOM_CYLINDER: + case mjGEOM_BOX: + return 4; + case mjGEOM_MESH: + return 3; + default: + return 0; + } + } + + int is_multiccd = !mjDISABLED(mjDSBL_MULTICCD); + if (!is_multiccd) { + return 1; + } + + if (type1 == mjGEOM_CAPSULE || type2 == mjGEOM_CAPSULE || + type1 == mjGEOM_CYLINDER || type2 == mjGEOM_CYLINDER) { + return 5; + } + + if (mjDISABLED(mjDSBL_NATIVECCD)) { + return is_multiccd ? 5 : 1; // mesh-mesh or mesh-box with libccd + } + + // check margin from model + if (has_margin < 0) { + has_margin = 0; + if (mjENABLED(mjENBL_OVERRIDE)) { + has_margin = m->opt.o_margin > 0.0; + } else { + int npair = m->npair; + int ipair = -1; + for (int k=0; k < npair; k++) { + if ((m->pair_geom1[k] == g1 && m->pair_geom2[k] == g2) || + (m->pair_geom1[k] == g2 && m->pair_geom2[k] == g1)) { + ipair = k; + break; + } + } + + if (ipair > -1) { + has_margin = m->pair_margin[ipair] > 0.0; + } else { + has_margin = m->geom_margin[g1] > 0.0 || m->geom_margin[g2] > 0.0; + } + } + } + + // 4 contacts for mesh-mesh or mesh-box without margins, 5 with margins + return has_margin ? 5 : 4; +} + + // move arena pointer back to the end of the contact array static inline void resetArena(mjData* d) { d->parena = d->ncon * sizeof(mjContact); diff --git a/src/engine/engine_collision_driver.h b/src/engine/engine_collision_driver.h index 9583cf5f..e231eaaf 100644 --- a/src/engine/engine_collision_driver.h +++ b/src/engine/engine_collision_driver.h @@ -18,6 +18,7 @@ #include #include #include +#include #ifdef __cplusplus extern "C" { @@ -26,10 +27,15 @@ extern "C" { // collision function pointers and max contact pairs MJAPI extern mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES]; +// return the maximum number of contacts that can be generated between two geoms +// if has_margin is -1, then the margin is pulled from the model, otherwise if has_margin > 0 +// indicates that the geoms have a positive margin +MJAPI int mj_maxContact(const mjModel* m, int g1, int g2, int has_margin); + // collision detection entry point MJAPI void mj_collision(const mjModel* m, mjData* d); -// applies Separating Axis Theorem for rotated AABBs +// apply the Separating Axis Theorem for rotated AABBs MJAPI int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], const mjtNum xpos1[3], const mjtNum xmat1[9], const mjtNum xpos2[3], const mjtNum xmat2[9], mjtNum margin, diff --git a/test/engine/engine_collision_driver_test.cc b/test/engine/engine_collision_driver_test.cc index 0db2556d..aef1d9fe 100644 --- a/test/engine/engine_collision_driver_test.cc +++ b/test/engine/engine_collision_driver_test.cc @@ -390,5 +390,73 @@ TEST_F(MjCollisionTest, MarginSumming) { mj_deleteModel(m); } +TEST_F(MjCollisionTest, MaxContact) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + ASSERT_THAT(d, NotNull()); + + int mesh = mj_name2id(m, mjOBJ_GEOM, "mesh"); + int box = mj_name2id(m, mjOBJ_GEOM, "box"); + int plane = mj_name2id(m, mjOBJ_GEOM, "plane"); + int sphere = mj_name2id(m, mjOBJ_GEOM, "sphere"); + int capsule = mj_name2id(m, mjOBJ_GEOM, "capsule"); + int ellipsoid = mj_name2id(m, mjOBJ_GEOM, "ellipsoid"); + int cylinder = mj_name2id(m, mjOBJ_GEOM, "cylinder"); + + EXPECT_EQ(mj_maxContact(m, mesh, box, -1), 4); + EXPECT_EQ(mj_maxContact(m, mesh, plane, -1), 3); + EXPECT_EQ(mj_maxContact(m, box, plane, -1), 4); + EXPECT_EQ(mj_maxContact(m, mesh, mesh, -1), 4); + EXPECT_EQ(mj_maxContact(m, box, box, -1), 8); + EXPECT_EQ(mj_maxContact(m, capsule, capsule, -1), 2); + EXPECT_EQ(mj_maxContact(m, capsule, box, -1), 4); + EXPECT_EQ(mj_maxContact(m, capsule, plane, -1), 2); + EXPECT_EQ(mj_maxContact(m, cylinder, plane, -1), 4); + EXPECT_EQ(mj_maxContact(m, sphere, sphere, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, capsule, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, box, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, mesh, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, plane, -1), 1); + EXPECT_EQ(mj_maxContact(m, sphere, cylinder, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, ellipsoid, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, box, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, mesh, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, plane, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, cylinder, -1), 1); + EXPECT_EQ(mj_maxContact(m, ellipsoid, capsule, -1), 1); + EXPECT_EQ(mj_maxContact(m, capsule, cylinder, -1), 5); + EXPECT_EQ(mj_maxContact(m, capsule, mesh, -1), 5); + EXPECT_EQ(mj_maxContact(m, cylinder, cylinder, -1), 5); + EXPECT_EQ(mj_maxContact(m, cylinder, box, -1), 5); + EXPECT_EQ(mj_maxContact(m, cylinder, mesh, -1), 5); + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index d5ea2b37..cb4c402f 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -6951,6 +6951,9 @@ public static unsafe extern void mj_rne(mjModel_* m, mjData_* d, int flg_acc, do [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_rnePostConstraint(mjModel_* m, mjData_* d); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern int mj_maxContact(mjModel_* m, int g1, int g2, int has_margin); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_collision(mjModel_* m, mjData_* d); diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index dfe18aea..ae618fd8 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -8845,6 +8845,10 @@ void mj_makeM_wrapper(const MjModel& m, MjData& d) { mj_makeM(m.get(), d.get()); } +int mj_maxContact_wrapper(const MjModel& m, int g1, int g2, int has_margin) { + return mj_maxContact(m.get(), g1, g2, has_margin); +} + void mj_mulJacTVec_wrapper(const MjModel& m, const MjData& d, const val& res, const NumberArray& vec) { UNPACK_VALUE(mjtNum, res); UNPACK_ARRAY(mjtNum, vec); @@ -13173,6 +13177,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { function("mj_local2Global", &mj_local2Global_wrapper); function("mj_makeConstraint", &mj_makeConstraint_wrapper); function("mj_makeM", &mj_makeM_wrapper); + function("mj_maxContact", &mj_maxContact_wrapper); function("mj_mulJacTVec", &mj_mulJacTVec_wrapper); function("mj_mulJacVec", &mj_mulJacVec_wrapper); function("mj_mulM", &mj_mulM_wrapper); From da01bd37a2666630629abf14806d2dd177ea9164 Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Tue, 21 Apr 2026 06:59:17 -0700 Subject: [PATCH 110/251] Remove modelfiledir_ from compiled assets, use owning spec modelfiledir instead. Previously specs that were attached to some parent spec would resolve its asset paths relative to the modelfiledir of the parent spec. This means path resolution would change depending on the source of the parent spec. Instead, this change makes asset file path resolution relative to the "owning spec" i.e. the spec where the asset was created. This enables workflows such as loading a parent spec via resource provider, then loading a child spec via `from_zip` or in memory providing `spec.assets` and resolution will work as intended. PiperOrigin-RevId: 903207910 Change-Id: Ia58020ab372a3ceadf31e804d145e2ae53d8e5f9 --- doc/changelog.rst | 7 +++ src/user/user_mesh.cc | 20 ++------ src/user/user_objects.cc | 20 ++------ src/user/user_objects.h | 12 ----- test/user/user_api_test.cc | 99 ++++++++++++++++++++++++++++++++++++++ 5 files changed, 114 insertions(+), 44 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 03b768f7..ff3091c8 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -23,6 +23,13 @@ General **Migration:** The flag :ref:`multiccd` must be explicitly disabled. +Bug fixes +^^^^^^^^^ + +- Asset paths in attached child specs are now resolved relative to the model file directory of the child spec, rather + than the parent spec. This prevents the origin of the parent spec to affect the resolution of asset paths in the child + spec. + Version 3.7.0 (April 14, 2026) ------------------------------ diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index ae54b634..a7b7dc04 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -300,9 +300,6 @@ void mjCMesh::NameSpace(const mjCModel* m) { name = mjuu_stripext(stripped); } mjCBase::NameSpace(m); - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(m->spec_modelfiledir_); - } if (!plugin_instance_name.empty()) { plugin_instance_name = m->prefix + plugin_instance_name + m->suffix; } @@ -712,17 +709,14 @@ void mjCMesh::TryCompile(const mjVFS* vfs) { mujoco::user::FilePath meshdir_; meshdir_ = FilePath(mjs_getString(compiler->meshdir)); - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(model->modelfiledir_); - } - // remove path from file if necessary if (model->strippath) { file_ = mjuu_strippath(file_); } + mjSpec* owning_spec = model->FindSpec(compiler); FilePath filename = meshdir_ + FilePath(file_); - resource_ = LoadResource(modelfiledir_.Str(), filename.Str(), vfs); + resource_ = LoadResource(owning_spec->modelfiledir->c_str(), filename.Str(), vfs); // try loading from cache if (cache != nullptr && LoadCachedMesh(cache, resource_)) { @@ -2957,9 +2951,6 @@ void mjCSkin::NameSpace(const mjCModel* m) { for (auto& name : spec_bodyname_) { name = m->prefix + name + m->suffix; } - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(m->spec_modelfiledir_); - } } @@ -3046,15 +3037,12 @@ void mjCSkin::Compile(const mjVFS* vfs) { throw mjCError(this, "Unknown skin file type: %s", file_.c_str()); } - // copy paths from model if not already defined - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(model->modelfiledir_); - } mujoco::user::FilePath meshdir_; meshdir_ = FilePath(mjs_getString(compiler->meshdir)); FilePath filename = meshdir_ + FilePath(file_); - mjResource* resource = LoadResource(modelfiledir_.Str(), filename.Str(), vfs); + mjSpec* owning_spec = model->FindSpec(compiler); + mjResource* resource = LoadResource(owning_spec->modelfiledir->c_str(), filename.Str(), vfs); try { LoadSKN(resource); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index f83bd387..75a7e679 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -4668,9 +4668,6 @@ void mjCHField::NameSpace(const mjCModel* m) { name = mjuu_stripext(stripped); } mjCBase::NameSpace(m); - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(m->spec_modelfiledir_); - } } @@ -4798,15 +4795,12 @@ void mjCHField::Compile(const mjVFS* vfs) { throw mjCError(this, "unsupported content type: '%s'", asset_type.c_str()); } - // copy paths from model if not already defined - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(model->modelfiledir_); - } mujoco::user::FilePath meshdir_; meshdir_ = FilePath(mjs_getString(compiler->meshdir)); FilePath filename = meshdir_ + FilePath(file_); - mjResource* resource = LoadResource(modelfiledir_.Str(), filename.Str(), vfs); + mjSpec* owning_spec = model->FindSpec(compiler); + mjResource* resource = LoadResource(owning_spec->modelfiledir->c_str(), filename.Str(), vfs); struct CachedHField { int nrow, ncol; @@ -4965,9 +4959,6 @@ void mjCTexture::NameSpace(const mjCModel* m) { name = mjuu_stripext(stripped); } mjCBase::NameSpace(m); - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(m->spec_modelfiledir_); - } } @@ -5388,7 +5379,8 @@ void mjCTexture::LoadFlip(std::string filename, const mjVFS* vfs, } // try loading from cache - mjResource* resource = LoadResource(modelfiledir_.Str(), filename, vfs); + mjSpec* owning_spec = model->FindSpec(compiler); + mjResource* resource = LoadResource(owning_spec->modelfiledir->c_str(), filename, vfs); if (cache && cache->PopulateData(GetCacheId(resource, asset_type), resource, callback)) { mju_closeResource(resource); return; @@ -5640,10 +5632,6 @@ void mjCTexture::LoadCubeSeparate(const mjVFS* vfs) { void mjCTexture::Compile(const mjVFS* vfs) { CopyFromSpec(); - // copy paths from model if not already defined - if (modelfiledir_.empty()) { - modelfiledir_ = FilePath(model->modelfiledir_); - } mujoco::user::FilePath texturedir_; texturedir_ = FilePath(mjs_getString(compiler->texturedir)); diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 5c81e40c..4f4da58d 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1122,9 +1122,6 @@ class mjCMesh_ : public mjCBase { // octree mjCOctree octree_; // octree of the mesh - - // paths stored during model attachment - mujoco::user::FilePath modelfiledir_; }; class mjCMesh: public mjCMesh_, private mjsMesh { @@ -1336,9 +1333,6 @@ class mjCSkin_ : public mjCBase { int matid; // material id std::vector bodyid; // body ids - - // paths stored during model attachment - mujoco::user::FilePath modelfiledir_; }; class mjCSkin: public mjCSkin_, private mjsSkin { @@ -1391,9 +1385,6 @@ class mjCHField_ : public mjCBase { std::string spec_file_; std::string spec_content_type_; std::vector spec_userdata_; - - // paths stored during model attachment - mujoco::user::FilePath modelfiledir_; }; class mjCHField : public mjCHField_, private mjsHField { @@ -1442,9 +1433,6 @@ class mjCTexture_ : public mjCBase { std::string spec_file_; std::string spec_content_type_; std::vector spec_cubefiles_; - - // paths stored during model attachment - mujoco::user::FilePath modelfiledir_; }; class mjCTexture : public mjCTexture_, private mjsTexture { diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 3c7af426..57e889bb 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include // NOLINT #include #include @@ -32,6 +33,7 @@ #include "src/cc/array_safety.h" #include #include +#include #include "src/xml/xml_api.h" #include "src/xml/xml_numeric_format.h" #include "test/fixture.h" @@ -203,6 +205,103 @@ TEST_F(MujocoTest, AttachAndChildDeletion) { mj_deleteSpec(parent_spec); } +int open_mock(mjResource* resource) { + static const char parent_xml[] = R"( + + + + + + )"; + resource->data = mju_malloc(sizeof(parent_xml)); + std::strcpy((char*)resource->data, parent_xml); + return 1; +} + +int read_mock(mjResource* resource, const void** buffer) { + *buffer = resource->data; + return std::strlen((const char*)resource->data); +} + +void close_mock(mjResource* resource) { + mju_free(resource->data); + resource->data = nullptr; +} + +TEST_F(MujocoTest, AttachedSpecDoesNotInheritURI) { + // This test checks that when we attach a child spec to a parent spec that was + // loaded from a resource provider, the child spec does not inherit the + // resource URI from the parent. This allows the child spec to specify assets + // relative to its model file or in the VFS. + mjpResourceProvider provider = { + .prefix = "fakeprovider", + .open = open_mock, + .read = read_mock, + .close = close_mock, + }; + + mjp_registerResourceProvider(&provider); + + std::array err; + mjSpec* parent_spec = + mj_parseXML("fakeprovider:parent.xml", nullptr, err.data(), err.size()); + mjs_setString(parent_spec->modelname, "parent"); + ASSERT_THAT(parent_spec, NotNull()) << err.data(); + + // Create child spec + static constexpr char child_xml[] = R"( + + + + + + + + + + + )"; + + // Setup VFS with asset + mjVFS vfs; + mj_defaultVFS(&vfs); + static constexpr char asset_data[] = R"( + v 0 0 0 + v 1 0 0 + v 0 1 0 + v 0 0 1 + f 1 2 3 + f 1 2 4 + f 2 3 4 + f 3 1 4 + )"; + mj_addBufferVFS(&vfs, "asset.obj", asset_data, sizeof(asset_data)); + + mjSpec* child_spec = + mj_parseXMLString(child_xml, &vfs, err.data(), err.size()); + mjs_setString(child_spec->modelname, "child"); + ASSERT_THAT(child_spec, NotNull()) << err.data(); + + // Attach child spec to parent spec's world body + mjsBody* world = mjs_findBody(parent_spec, "world"); + ASSERT_THAT(world, NotNull()); + + mjsElement* attached = + mjs_attach(world->element, child_spec->element, "", ""); + ASSERT_THAT(attached, NotNull()); + + mjModel* model = mj_compile(parent_spec, &vfs); + mj_deleteVFS(&vfs); + + EXPECT_THAT(model, NotNull()) << mjs_getError(parent_spec); + + if (model) { + mj_deleteModel(model); + } + mj_deleteSpec(parent_spec); + mj_deleteSpec(child_spec); +} + TEST_F(MujocoTest, ActivatePlugin) { mjSpec* spec = mj_makeSpec(); mjs_activatePlugin(spec, "mujoco.elasticity.cable"); From ba149aa043718f6b2018804e27e4f96674b25fc3 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 21 Apr 2026 08:01:32 -0700 Subject: [PATCH 111/251] Fix flexcomp strain constraints with rotated grids. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The reference node positions and the positions used for computing stiffness eigenvectors were previously stored in world frame. However, the runtime expects these quantities in the unrotated local frame. This caused non-zero constraint residuals and simulation instability when the grid was rotated — either by the parent body's initial orientation, or by the flexcomp's own frame attributes. Rather than tracking each rotation source individually, this change extracts the total grid rotation directly from the cell geometry. All node positions are then un-rotated before computing the stiffness matrix. PiperOrigin-RevId: 903232388 Change-Id: If877af89025ce1e61a76b38c29403d593d892749 --- src/user/user_mesh.cc | 83 +++++++++++- src/user/user_objects.h | 3 +- test/engine/engine_core_constraint_test.cc | 140 ++++++++++++++++++++- 3 files changed, 220 insertions(+), 6 deletions(-) diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index a7b7dc04..212381c8 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4251,6 +4251,9 @@ void mjCFlex::Compile(const mjVFS* vfs) { } } + // compute unrotated node positions for stiffness computation + std::vector nodexpos_local = ComputeUnrotatedNodePositions(nodexpos); + // reorder tetrahedra so right-handed face orientation is outside // faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2) if (dim == 3) { @@ -4410,7 +4413,7 @@ void mjCFlex::Compile(const mjVFS* vfs) { int gj = cj * spec.order + lj; int gk = ck * spec.order + lk; int global = gi * ny_global * nz_global + gj * nz_global + gk; - mjuu_copyvec(cell_pos.data() + 3*local, nodexpos.data() + 3*global, 3); + mjuu_copyvec(cell_pos.data() + 3*local, nodexpos_local.data() + 3*global, 3); local++; } } @@ -4453,14 +4456,88 @@ void mjCFlex::Compile(const mjVFS* vfs) { } } - // store node cartesian positions + // store node positions in unrotated (body-local) frame + // this ensures the runtime displacement refpos - R^{-1}*x is zero at rest node0_.assign(3*nnode, 0); for (int i=0; i < nnode; i++) { - mjuu_copyvec(node0_.data()+3*i, nodexpos.data()+3*i, 3); + mjuu_copyvec(node0_.data()+3*i, nodexpos_local.data()+3*i, 3); } } +// compute unrotated node positions for stiffness computation and node0_ +// +// the runtime corotational code extracts rotation R from the deformation +// gradient and computes displacement as R^{-1}*x - refpos; at rest R = R0 +// (the total grid rotation), so refpos must equal R0^{-1}*nodexpos to get +// zero displacement at rest; additionally, the stiffness eigenvectors must +// be computed from axis-aligned positions to preserve the diagonal Jacobian +// assumption in ComputeLinearStiffness. +std::vector mjCFlex::ComputeUnrotatedNodePositions( + const std::vector& nodexpos) const { + std::vector nodexpos_local(3*nnode); + if (interpolated && nnode > 0) { + int ny_global = spec.cellcount[1] * spec.order + 1; + int nz_global = spec.cellcount[2] * spec.order + 1; + + // find first non-empty cell + int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2]; + int ref_ci = 0, ref_cj = 0, ref_ck = 0; + bool found = false; + for (int ci = 0; ci < cx && !found; ci++) { + for (int cj = 0; cj < cy && !found; cj++) { + for (int ck = 0; ck < cz && !found; ck++) { + int cell_idx = ci * cy * cz + cj * cz + ck; + if (cell_empty.empty() || !cell_empty[cell_idx]) { + ref_ci = ci; ref_cj = cj; ref_ck = ck; + found = true; + } + } + } + } + + // corner indices of the reference cell (order=1 corners at local 0,0,0 + // and at offsets along each parametric axis) + int g000 = (ref_ci * spec.order) * ny_global * nz_global + + (ref_cj * spec.order) * nz_global + + (ref_ck * spec.order); + int g100 = ((ref_ci * spec.order) + spec.order) * ny_global * nz_global + + (ref_cj * spec.order) * nz_global + + (ref_ck * spec.order); + int g010 = (ref_ci * spec.order) * ny_global * nz_global + + ((ref_cj * spec.order) + spec.order) * nz_global + + (ref_ck * spec.order); + int g001 = (ref_ci * spec.order) * ny_global * nz_global + + (ref_cj * spec.order) * nz_global + + ((ref_ck * spec.order) + spec.order); + + // edge vectors (columns of the deformation gradient F = R * S) + // we store them as rows in R0 to use mjuu_mulvecmat for applying R0^{-1} + double R0[9]; + for (int d = 0; d < 3; d++) { + R0[0+d] = nodexpos[3*g100 + d] - nodexpos[3*g000 + d]; + R0[3+d] = nodexpos[3*g010 + d] - nodexpos[3*g000 + d]; + R0[6+d] = nodexpos[3*g001 + d] - nodexpos[3*g000 + d]; + } + + // normalize to get rotation matrix columns (valid for regular grids) + double li = mjuu_normvec(R0+0, 3); + double lj = mjuu_normvec(R0+3, 3); + double lk = mjuu_normvec(R0+6, 3); + (void)li; (void)lj; (void)lk; + + // apply inverse rotation to each nodexpos to get local-frame positions + for (int i = 0; i < nnode; i++) { + const double* p = nodexpos.data() + 3*i; + double* q = nodexpos_local.data() + 3*i; + mjuu_mulvecmat(q, p, R0); + } + } else { + nodexpos_local = nodexpos; + } + return nodexpos_local; +} + // create flex BVH void mjCFlex::CreateBVH() { diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 4f4da58d..bb3bbed5 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1054,7 +1054,8 @@ class mjCFlex: public mjCFlex_, private mjsFlex { std::vector vert0_; // vertex positions in [0, 1]^d in the bounding box std::vector node0_; // node Cartesian positions - + // compute unrotated node positions for stiffness computation + std::vector ComputeUnrotatedNodePositions(const std::vector& nodexpos) const; // stiffness caching std::string ComputeStiffnessCacheKey() const; diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index 93e3b947..07204df8 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -15,8 +15,6 @@ // Tests for engine/engine_core_constraint.c. #include -#include -#include #include #include @@ -914,5 +912,143 @@ TEST_F(CoreConstraintTest, JdotvFwdInvIdentity) { } } +// --------------------------- strain constraint rotated parent ---------------- + +struct StrainConstraintTestCase { + std::string test_name; + std::string body_pos; + std::string body_quat; + std::string flex_spacing; + std::string flex_xyaxes; +}; + +class StrainConstraintRotatedTest : public CoreConstraintTest, + public ::testing::WithParamInterface< + StrainConstraintTestCase> { +}; + +TEST_P(StrainConstraintRotatedTest, ResidualIsZero) { + auto param = GetParam(); + std::string xml = R"( + + + )"; + + std::array error; + mjModel* m = LoadModelFromString(xml.c_str(), error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + mjData* d = mj_makeData(m); + + mj_forward(m, d); + + // Check we have strain constraints + EXPECT_GT(d->ne, 0) << "Expected strain constraints"; + + // The critical check: constraint residuals must be ~0 at the initial + // (undeformed) configuration, even though the body is rotated. + mjtNum max_pos = 0; + for (int i = 0; i < d->ne; i++) { + max_pos = mju_max(max_pos, mju_abs(d->efc_pos[i])); + } + EXPECT_LT(max_pos, 1e-6) + << "Strain constraint residual should be ~0" + << " (max_pos=" << max_pos << ")"; + + // Verify stability + for (int i = 0; i < 200; i++) { + mj_step(m, d); + ASSERT_FALSE(mju_isBad(d->qpos[0])) + << "Simulation unstable at step " << i; + for (int j = 0; j < m->nv; j++) { + ASSERT_LT(mju_abs(d->qvel[j]), 1000.0) + << "Velocity exploded at step " << i + << ", qvel[" << j << "]=" << d->qvel[j]; + } + } + + mj_deleteData(d); + mj_deleteModel(m); +} + +INSTANTIATE_TEST_SUITE_P( + StrainConstraintRotatedTests, StrainConstraintRotatedTest, + testing::ValuesIn({ + // Test strain constraint with a rotated parent body. + // The flexcomp is placed inside a parent body that has a non-identity + // initial rotation. This reproduces the "grocery scene" bug where the + // stiffness matrix eigenvectors and reference positions were computed + // in world frame instead of the unrotated local frame, causing + // spurious constraint forces. + { + "RotatedParent", + "1 2 3", + "0.707107 0 0.707107 0", + ".1 .1 .1", + "" + }, + // Same test with an anisotropic box (different spacing per axis) and + // arbitrary rotation (combined 45-deg Y + 30-deg X). + { + "RotatedParentAnisotropic", + "0.5 -1 2", + "0.8924 0.2392 0.3696 -0.0990", + ".15 .08 .05", + "" + }, + // Test strain constraint with flexcomp-level xyaxes rotation. + // This is the "grocery scene" pattern where the flexcomp grid itself is + // rotated via xyaxes="0 1 0 0 0 1" (X->Y, Y->Z). + { + "FlexcompXyaxes", + "", + "", + ".1 .02 .1", + "0 1 0 0 0 1" + }, + // Test combining parent body rotation with flexcomp xyaxes rotation. + // The total rotation is the composition of both. + { + "RotatedParentPlusXyaxes", + "1 2 3", + "0.707107 0 0.707107 0", + ".15 .08 .05", + "0 1 0 0 0 1" + } + }), + [](const testing::TestParamInfo< + StrainConstraintRotatedTest::ParamType>& info) { + return info.param.test_name; + } +); + } // namespace } // namespace mujoco From c3fddf5dc150745a43cede9a3646568fed2c14e6 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Tue, 21 Apr 2026 10:02:57 -0700 Subject: [PATCH 112/251] Update doc/mjwarp/update_types.py PiperOrigin-RevId: 903289569 Change-Id: I7e05736c54fa72dc0c89152f49cf3d9ee6e5492d --- doc/mjwarp/update_types.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/doc/mjwarp/update_types.py b/doc/mjwarp/update_types.py index a19a0b61..316b2d2e 100644 --- a/doc/mjwarp/update_types.py +++ b/doc/mjwarp/update_types.py @@ -29,7 +29,7 @@ This script updates such instances with valid types @dataclasses.dataclass class Option: ... - timestep: wp.array(dtype=float) + timestep: wp.array[float] ... """ @@ -48,13 +48,13 @@ def replace_array_calls(match): dtype = args[-1] if n_args == 2: - return f'wp.array(dtype={dtype})' + return f'wp.array[{dtype}]' elif n_args == 3: - return f'wp.array2d(dtype={dtype})' + return f'wp.array2d[{dtype}]' elif n_args == 4: - return f'wp.array3d(dtype={dtype})' + return f'wp.array3d[{dtype}]' elif n_args == 5: - return f'wp.array4d(dtype={dtype})' + return f'wp.array4d[{dtype}]' else: return match.group(0) From 4cfebcc32bfa4cf6b7577a4a0fe14c1dbc06d817 Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Wed, 22 Apr 2026 05:26:33 -0700 Subject: [PATCH 113/251] Add mj_containsFileVFS and mj_containsBufferVFS functions. PiperOrigin-RevId: 903785790 Change-Id: I013b37a177284f8440179c4ae5c6221e0f572b49 --- doc/APIreference/functions.rst | 18 +++++++++++ doc/changelog.rst | 1 + doc/includes/references.h | 2 ++ include/mujoco/mujoco.h | 6 ++++ python/mujoco/introspect/functions.py | 46 +++++++++++++++++++++++++++ src/user/user_vfs.cc | 37 +++++++++++++++++++++ src/user/user_vfs.h | 6 ++++ test/user/user_vfs_test.cc | 32 +++++++++++++++++++ unity/Runtime/Bindings/MjBindings.cs | 6 ++++ wasm/codegen/generators/constants.py | 2 ++ 10 files changed, 156 insertions(+) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 2e83a385..fc513e20 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -1519,6 +1519,24 @@ Add file to VFS from buffer; return 0: success, 2: repeated name, -1: failed to Delete file from VFS; return 0: success, -1: not found in VFS. +.. _mj_containsBufferVFS: + +`mj_containsBufferVFS <#mj_containsBufferVFS>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_containsBufferVFS + +Check if buffer exists in VFS; return 1: exists, 0: not found. + +.. _mj_containsFileVFS: + +`mj_containsFileVFS <#mj_containsFileVFS>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_containsFileVFS + +Check if file exists in VFS; return 1: exists, 0: not found. + .. _mj_deleteVFS: `mj_deleteVFS <#mj_deleteVFS>`__ diff --git a/doc/changelog.rst b/doc/changelog.rst index ff3091c8..e3f311b2 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -9,6 +9,7 @@ General ^^^^^^^ - Added new :ref:`mj_maxContact` function to get the maximum number of possible contacts returned by two geoms. +- Added ``mj_containsBufferVFS`` and ``mj_containsFileVFS`` to check for existence of buffers and files in VFS. - Added :ref:`multi-cell support` for trilinear and quadratic flexes. Note that the implicit integrator uses a dense solver for the flex degrees of freedom, which can be slow for multi-cell flexes. - Refactored ``flexstrain`` equality constraints to be instantiated per cell instead of per flex object, reducing the diff --git a/doc/includes/references.h b/doc/includes/references.h index 4d55e8ba..4d767933 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3161,6 +3161,8 @@ int mj_unmountVFS(mjVFS* vfs, const char* filename); int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename); int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int nbuffer); int mj_deleteFileVFS(mjVFS* vfs, const char* filename); +int mj_containsBufferVFS(mjVFS* vfs, const char* name); +int mj_containsFileVFS(mjVFS* vfs, const char* directory, const char* filename); void mj_deleteVFS(mjVFS* vfs); size_t mj_getCacheSize(const mjCache* cache); size_t mj_getCacheCapacity(const mjCache* cache); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 31954cfb..2bd0cae8 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -94,6 +94,12 @@ MJAPI int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int // Delete file from VFS; return 0: success, -1: not found in VFS. MJAPI int mj_deleteFileVFS(mjVFS* vfs, const char* filename); +// Check if buffer exists in VFS; return 1: exists, 0: not found. +MJAPI int mj_containsBufferVFS(mjVFS* vfs, const char* name); + +// Check if file exists in VFS; return 1: exists, 0: not found. +MJAPI int mj_containsFileVFS(mjVFS* vfs, const char* directory, const char* filename); + // Delete all files from VFS and deallocates VFS internal memory. MJAPI void mj_deleteVFS(mjVFS* vfs); diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index ee5795f4..e16f434f 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -162,6 +162,52 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Delete file from VFS; return 0: success, -1: not found in VFS.', )), + ('mj_containsBufferVFS', + FunctionDecl( + name='mj_containsBufferVFS', + return_type=ValueType(name='int'), + parameters=( + FunctionParameterDecl( + name='vfs', + type=PointerType( + inner_type=ValueType(name='mjVFS'), + ), + ), + FunctionParameterDecl( + name='name', + type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + ), + ), + doc='Check if buffer exists in VFS; return 1: exists, 0: not found.', + )), + ('mj_containsFileVFS', + FunctionDecl( + name='mj_containsFileVFS', + return_type=ValueType(name='int'), + parameters=( + FunctionParameterDecl( + name='vfs', + type=PointerType( + inner_type=ValueType(name='mjVFS'), + ), + ), + FunctionParameterDecl( + name='directory', + type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + ), + FunctionParameterDecl( + name='filename', + type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + ), + ), + doc='Check if file exists in VFS; return 1: exists, 0: not found.', + )), ('mj_deleteVFS', FunctionDecl( name='mj_deleteVFS', diff --git a/src/user/user_vfs.cc b/src/user/user_vfs.cc index 11d35c71..60c5c945 100644 --- a/src/user/user_vfs.cc +++ b/src/user/user_vfs.cc @@ -236,6 +236,24 @@ VFS::Status VFS::Unmount(const FilePath& path) { return kInvalidResourceProvider; } +bool VFS::ContainsBuffer(const char* name) { + if (name == nullptr) { + return false; + } + std::lock_guard lock(mutex_); + return mounts_.contains(name); +} + +bool VFS::ContainsFile(const char* directory, const char* filename) { + if (filename == nullptr) { + return false; + } + mujoco::user::FilePath path(directory ? directory : "", filename); + std::string key = path.StripPath().Lower().Str(); + std::lock_guard lock(mutex_); + return mounts_.contains(key); +} + int VFS::Read(mjResource* resource, const void** buffer) { if (resource && resource->provider && resource->provider->read) { return resource->provider->read(resource, buffer); @@ -498,3 +516,22 @@ int mj_deleteFileVFS(mjVFS* vfs, const char* filename) { } return mujoco::user::VFS::kSuccess; } + +int mj_containsBufferVFS(mjVFS* vfs, const char* name) { + mujoco::user::VFS* impl = mujoco::user::VFS::Upcast(vfs); + if (impl == nullptr) { + mju_error("mjVFS is null."); + return -1; + } + return impl->ContainsBuffer(name); +} + +int mj_containsFileVFS(mjVFS* vfs, const char* directory, const char* filename) { + mujoco::user::VFS* impl = mujoco::user::VFS::Upcast(vfs); + if (impl == nullptr) { + mju_error("mjVFS is null."); + return -1; + } + return impl->ContainsFile(directory, filename); +} + diff --git a/src/user/user_vfs.h b/src/user/user_vfs.h index cfa2694c..6bef85c2 100644 --- a/src/user/user_vfs.h +++ b/src/user/user_vfs.h @@ -90,6 +90,12 @@ class VFS { // Unmounts the ResourceProvider from the given path. Status Unmount(const FilePath& path); + // Returns true if the VFS contains a buffer with the given name. + bool ContainsBuffer(const char* name); + + // Returns true if the VFS contains a file with the given name. + bool ContainsFile(const char* directory, const char* filename); + // Sets a destructor to be called when the VFS has no more open resources. // Assumes that `destructor` will delete `this`. // diff --git a/test/user/user_vfs_test.cc b/test/user/user_vfs_test.cc index db5a22b5..16a62da2 100644 --- a/test/user/user_vfs_test.cc +++ b/test/user/user_vfs_test.cc @@ -224,6 +224,38 @@ TEST_F(UserVfsTest, DeleteFileRepeat) { mj_deleteVFS(&vfs); } +TEST_F(UserVfsTest, ContainsBuffer) { + mjVFS vfs; + mj_defaultVFS(&vfs); + std::string buffer = ""; + const void* ptr = static_cast(buffer.c_str()); + mj_addBufferVFS(&vfs, "model", ptr, buffer.size()); + + EXPECT_TRUE(mj_containsBufferVFS(&vfs, "model")); + EXPECT_FALSE(mj_containsBufferVFS(&vfs, "nonexistent")); + EXPECT_FALSE(mj_containsBufferVFS(&vfs, "Model")); + + mj_deleteVFS(&vfs); +} + +TEST_F(UserVfsTest, ContainsFile) { + mjVFS vfs; + mj_defaultVFS(&vfs); + + constexpr char path[] = "engine/testdata/actuation/"; + const std::string dir = GetTestDataFilePath(path); + std::string file = "activation.xml"; + mj_addFileVFS(&vfs, dir.c_str(), file.c_str()); + + EXPECT_TRUE(mj_containsFileVFS(&vfs, dir.c_str(), file.c_str())); + EXPECT_TRUE(mj_containsFileVFS(&vfs, nullptr, (dir + file).c_str())); + EXPECT_TRUE(mj_containsFileVFS(&vfs, nullptr, "Activation.xml")); + EXPECT_TRUE(mj_containsFileVFS(&vfs, "some/dir/", "activation.xml")); + EXPECT_FALSE(mj_containsFileVFS(&vfs, nullptr, "nonexistent.xml")); + + mj_deleteVFS(&vfs); +} + TEST_F(UserVfsTest, AddBuffer) { mjVFS vfs; diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index cb4c402f..e3bd9fe9 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -6684,6 +6684,12 @@ public static unsafe extern int mj_addBufferVFS(void* vfs, [MarshalAs(UnmanagedT [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern int mj_deleteFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string filename); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern int mj_containsBufferVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string name); + +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern int mj_containsFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string directory, [MarshalAs(UnmanagedType.LPStr)]string filename); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_deleteVFS(void* vfs); diff --git a/wasm/codegen/generators/constants.py b/wasm/codegen/generators/constants.py index 1a8aaeee..111c9997 100644 --- a/wasm/codegen/generators/constants.py +++ b/wasm/codegen/generators/constants.py @@ -124,6 +124,8 @@ _SKIPPED_ASSET_CACHE_FUNCTIONS: tuple[str, ...] = ( _SKIPPED_VFS_FUNCTIONS: tuple[str, ...] = ( # go/keep-sorted start "mj_addFileVFS", + "mj_containsBufferVFS", + "mj_containsFileVFS", "mj_mountVFS", "mj_unmountVFS", # go/keep-sorted end From 863a084d7f18867c674338ff795ace1db210592d Mon Sep 17 00:00:00 2001 From: Matej Aleksandrov Date: Wed, 22 Apr 2026 06:33:10 -0700 Subject: [PATCH 114/251] Adjust refcount expectations for Python 3.14 PiperOrigin-RevId: 903813225 Change-Id: Ieb529f2a850e697b643e5023554341c0718ac0e1 --- python/mujoco/bindings_test.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index c4b550d1..8df15728 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -1216,13 +1216,15 @@ Euler integrator, semi-implicit in velocity. mujoco.set_mjcb_control(lambda m, d: None) mujoco.mj_step(model_instances[-1], data_instances[-1]) mujoco.set_mjcb_control(None) + # Reference counting changed in Python 3.14. + expected_refcount = 2 if sys.version_info < (3, 14) else 1 while data_instances: d = data_instances.pop() - self.assertEqual(sys.getrefcount(d), 2) + self.assertEqual(sys.getrefcount(d), expected_refcount) del d while model_instances: m = model_instances.pop() - self.assertEqual(sys.getrefcount(m), 2) + self.assertEqual(sys.getrefcount(m), expected_refcount) # This test is disabled on PyPy as it uses sys.getrefcount # However PyPy is not officially supported by MuJoCo @@ -1236,7 +1238,9 @@ Euler integrator, semi-implicit in velocity. # passed to getrefcount. self.assertEqual(sys.getrefcount(data.model), 3) del data - self.assertEqual(sys.getrefcount(model), 2) + # Reference counting changed in Python 3.14. + expected_refcount = 2 if sys.version_info < (3, 14) else 1 + self.assertEqual(sys.getrefcount(model), expected_refcount) def test_can_initialize_mjv_structs(self): self.assertIsInstance(mujoco.MjvScene(), mujoco.MjvScene) From a891782553abf2d433a91415b53b97a5456490be Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 22 Apr 2026 08:00:42 -0700 Subject: [PATCH 115/251] Clean-up flex assumptions. Do not allow a mix of `elastic2d != none` with `dof = trilinear` since the latter assumes 3d elasticity. Also, do not assume that `flex_interp > 0` in the engine. This will enable to use, e.g., `flex_interp = -1` to mean a linear surface finite element instead of a 3d finite element which is currently identified with `flex_interp = 1`. PiperOrigin-RevId: 903852035 Change-Id: Ia6290b4a05e9e510ffb7f36d141cd525b40d3110 --- doc/XMLreference.rst | 2 +- model/flex/bunny.xml | 2 +- model/flex/bunny_multicell.xml | 2 +- model/flex/bunny_quadratic.xml | 2 +- model/flex/bunny_with_uv.xml | 2 +- src/engine/engine_core_constraint.c | 4 +++ src/engine/engine_core_smooth.c | 6 ++-- src/engine/engine_derivative.c | 2 ++ src/engine/engine_passive.c | 1 + src/engine/engine_setconst.c | 1 + src/engine/engine_vis_interact.c | 1 + src/engine/engine_vis_visualize.c | 1 + src/user/user_mesh.cc | 16 +++++++-- test/user/user_mesh_test.cc | 52 +++++++++++++++++++++++++++++ 14 files changed, 84 insertions(+), 10 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index f01541e4..1bc3c846 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4328,7 +4328,7 @@ stress-strain relationship. See also :ref:`deformable ` objects and :at:`elastic2d`: :at-val:`[none, bend, stretch, both], "none"` Elastic contribution to passive forces of 2D flexes. "none": none, "bend": bending only, "stretch": stretching only, - "both": bending and stretching. + "both": bending and stretching. Not yet supported by :ref:`dof` **trilinear** and **quadratic**. .. _flex-contact: diff --git a/model/flex/bunny.xml b/model/flex/bunny.xml index ec31684e..0d10cad9 100644 --- a/model/flex/bunny.xml +++ b/model/flex/bunny.xml @@ -31,7 +31,7 @@ - + diff --git a/model/flex/bunny_multicell.xml b/model/flex/bunny_multicell.xml index a77f5376..730cbc5d 100644 --- a/model/flex/bunny_multicell.xml +++ b/model/flex/bunny_multicell.xml @@ -31,7 +31,7 @@ - + diff --git a/model/flex/bunny_quadratic.xml b/model/flex/bunny_quadratic.xml index 5b98759f..59e255aa 100644 --- a/model/flex/bunny_quadratic.xml +++ b/model/flex/bunny_quadratic.xml @@ -31,7 +31,7 @@ - + diff --git a/model/flex/bunny_with_uv.xml b/model/flex/bunny_with_uv.xml index c3cde418..9de1d8c2 100644 --- a/model/flex/bunny_with_uv.xml +++ b/model/flex/bunny_with_uv.xml @@ -37,7 +37,7 @@ - + diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 0f8f4a00..39996c33 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -275,6 +275,7 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v, } int order = m->flex_interp[f]; + order = order < 0 ? -order : order; int npc = (order+1)*(order+1)*(order+1); // number of nodes per cell // cell lookup: get local coords and node indices @@ -709,6 +710,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { int f = id[0]; int nodenum = m->flex_nodenum[f]; int order = m->flex_interp[f]; + order = order < 0 ? -order : order; // skip if not interpolated (order == 0 or no nodes) if (!order || !nodenum) { @@ -1676,6 +1678,7 @@ void mj_diagApprox(const mjModel* m, mjData* d) { int flex_id = m->eq_obj1id[id]; int nstart = m->flex_nodeadr[flex_id]; int order = m->flex_interp[flex_id]; + order = order < 0 ? -order : order; int npc = (order+1)*(order+1)*(order+1); // per-cell constraint count @@ -2296,6 +2299,7 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { // per-cell strain constraints: each equality is one cell int f = id[0]; int order = m->flex_interp[f]; + order = order < 0 ? -order : order; if (!order || !m->flex_nodenum[f]) { break; } diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 6b052147..164a6527 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -595,7 +595,8 @@ void mj_flex(const mjModel* m, mjData* d) { } } - int order = m->flex_interp[f]; + int interp = m->flex_interp[f]; + int order = interp < 0 ? -interp : interp; int cx = m->flex_cellnum[3*f+0]; int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; @@ -2624,7 +2625,8 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) { case mjEQ_FLEXSTRAIN: { // increment: trilinear uses 2 center (I1,J-1) + 3*ngauss shear, quadratic uses 6*ngauss k = m->eq_obj1id[id]; - int order = m->flex_interp[k]; + int interp_k = m->flex_interp[k]; + int order = interp_k < 0 ? -interp_k : interp_k; int nodenum = m->flex_nodenum[k]; if (order && nodenum) { int nquad = order + 1; diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 875fb70c..60c80073 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -887,6 +887,7 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, if (!m->flex_interp[f]) continue; if (m->flex_rigid[f]) continue; int order = m->flex_interp[f]; + order = order < 0 ? -order : order; int npc = (order+1)*(order+1)*(order+1); if (npc > max_npc) max_npc = npc; if (m->flex_nodenum[f] > max_nodenum) max_nodenum = m->flex_nodenum[f]; @@ -966,6 +967,7 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, } int order = m->flex_interp[f]; + order = order < 0 ? -order : order; int npc = (order+1)*(order+1)*(order+1); int cx = m->flex_cellnum[3*f+0]; int cy = m->flex_cellnum[3*f+1]; diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 8c76a573..295ec4a7 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -236,6 +236,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) { if (m->flex_interp[f]) { int order = m->flex_interp[f]; + order = order < 0 ? -order : order; int npc = (order+1)*(order+1)*(order+1); // nodes per cell int cx = m->flex_cellnum[3*f+0]; int cy = m->flex_cellnum[3*f+1]; diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 24ef9955..754aa29b 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -714,6 +714,7 @@ static void makeFlexBandwidth(mjModel* m, mjData* d) { for (int f = 0; f < m->nflex; f++) { if (!m->flex_interp[f]) continue; int order = m->flex_interp[f]; + order = order < 0 ? -order : order; int nodeadr = m->flex_nodeadr[f]; int nodenum = m->flex_nodenum[f]; int cx = m->flex_cellnum[3*f+0]; diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index d1ce7ffe..60533e27 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -864,6 +864,7 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt, if (m->flex_interp[i]) { mjtNum* coord = m->flex_vert0 + 3*(m->flex_vertadr[i] + vertid); int order = m->flex_interp[i]; + order = order < 0 ? -order : order; int npc = (order+1)*(order+1)*(order+1); // cell lookup: get local coords and node indices diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 327f0108..0e058bd1 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -1452,6 +1452,7 @@ static void addFlexBvhGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; int order = m->flex_interp[f]; + order = order < 0 ? -order : order; int NX = cx * order + 1; int NY = cy * order + 1; int NZ = cz * order + 1; diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 212381c8..3389a71e 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4100,6 +4100,19 @@ void mjCFlex::Compile(const mjVFS* vfs) { } nelem = (int)elem_.size()/(dim+1); + // elastic2d checks + if (elastic2d) { + if (thickness <= 0) { + throw mjCError(this, "2d elasticity requires positive thickness"); + } + if (interpolated) { + throw mjCError(this, "interpolated flex does not yet support 2d elasticity"); + } + if (dim != 2 && !interpolated) { + throw mjCError(this, "2d elasticity requires 2d flex"); + } + } + // set nvert, rigid, centered; check size if (vert_.empty()) { centered = true; @@ -4342,9 +4355,6 @@ void mjCFlex::Compile(const mjVFS* vfs) { // bending stiffness (2D only) if (dim == 2 && (elastic2d == 1 || elastic2d == 3)) { - if (thickness < 0) { - throw mjCError(this, "thickness must be positive for bending stiffness"); - } bending.assign(nedge*17, 0); for (unsigned int e = 0; e < nedge; e++) { diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index 98944622..59833aef 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -819,6 +819,58 @@ TEST_F(MjCMeshTest, VolumeSmallAllowedShell) { mj_deleteModel(model); } +TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) { + static constexpr char xml[] = R"( + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::IsNull()); + EXPECT_THAT(error.data(), + HasSubstr("2d elasticity requires positive thickness")); +} + +TEST_F(MjCMeshTest, InterpolatedFlexDoesNotSupport2DElasticity) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::IsNull()); + EXPECT_THAT( + error.data(), + HasSubstr("interpolated flex does not yet support 2d elasticity")); +} + +TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) { + static constexpr char xml[] = R"( + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::IsNull()); + EXPECT_THAT(error.data(), HasSubstr("2d elasticity requires 2d flex")); +} + TEST_F(MjCMeshTest, VolumeNegativeThrowsError) { static constexpr char xml[] = R"( From 4b3f3aee6b1f7911b3e6485d1bc6297549f7a133 Mon Sep 17 00:00:00 2001 From: Michael Moss Date: Thu, 23 Apr 2026 01:21:51 -0700 Subject: [PATCH 116/251] Add hashes for 3.14 wheels to Python build requirements. PiperOrigin-RevId: 904299450 Change-Id: I6ac356d1fa9ae4965bd57d24ae11adac8d875a6c --- mjx/cuda_requirements.txt | 34 ++-- mjx/requirements.txt | 255 +++++++++++++++++++++--------- python/build_requirements.txt | 37 ++++- python/build_requirements_usd.txt | 128 ++++++++++----- 4 files changed, 324 insertions(+), 130 deletions(-) diff --git a/mjx/cuda_requirements.txt b/mjx/cuda_requirements.txt index c8de6c4a..1db7abfb 100644 --- a/mjx/cuda_requirements.txt +++ b/mjx/cuda_requirements.txt @@ -1,21 +1,25 @@ -jax-cuda12-plugin==0.5.3; python_version >= '3.10' \ - --hash=sha256:1862595b2b6d815679d11e0e889e523185ee54a46d46e022689f70fc4554dd91 \ - --hash=sha256:21fec1b56c98783ea0569b747a56751f1f9ff2187b48acc11c700d3bfc5e1a31 \ +jax-cuda12-plugin==0.8.3; python_version >= '3.13' \ + --hash=sha256:11d7cb222cfd4d5f6b7691df61516a5edc9298b7879dd6caaccc6dfc64678c70 \ + --hash=sha256:7bb68f693e16038cad77197aba914375795e5bb7e99f4a4f023b67bb50cad8c2 \ + --hash=sha256:9e3f1335a6812f5b28ace211a17d9ede46b8b46ceb4b8d9c8c697d3b4ada513f \ + --hash=sha256:b38c4bd34062c01d22a8c6a2225254dcd429771e97e024f3a2ccc1d6d24fd856 \ + --hash=sha256:b3f64b5059a39c18ec006c136475f46d088a11164e7e6ee2444e76305da87a1b \ + --hash=sha256:bb20d8b794ce52d644967c7229c5e0a01d3b1fac7efc60fbbfcd046a745a66c6 \ + --hash=sha256:f9a722128e2b423469a5dab8f9f96d77257d153ca7d96850b25a475191efaacc \ + --hash=sha256:fb9d49d43c4447793630079632e6fbb4a5ce30c388955131fe3ba96efec91817 +jax-cuda12-plugin==0.5.3; python_version < '3.13' \ --hash=sha256:2030cf1208ce4ea70ee56cac61ddd239f9798695fc39bb7739c50a25d6e9da44 \ - --hash=sha256:c2517a7c2186f8708894696e26cf96ebd60b7879ceca398b2c46abb28d2c96c8 \ - --hash=sha256:aaa704a5ef547595d022db1c1e4878a0677116412a9360c115d67ff4b64e1596 \ --hash=sha256:298d2d768f1029b74a0b1d01270e549349d2c37dc07658796542cda967eb7bd3 \ + --hash=sha256:6171aed2f4b3bdd5fc13782de1072c6a634fce13731b75d0cb0a6ab8f4e6e650 \ + --hash=sha256:aaa704a5ef547595d022db1c1e4878a0677116412a9360c115d67ff4b64e1596 \ --hash=sha256:ba2555967f9b6c381c8b4ef9fb03d05bc55ec25ecfee5cfe45c5ace34f7d4152 \ - --hash=sha256:6171aed2f4b3bdd5fc13782de1072c6a634fce13731b75d0cb0a6ab8f4e6e650 -jax-cuda12-plugin==0.4.30; python_version == '3.9' \ - --hash=sha256:d8d196241b9253ecb1144a4409b5deacbb9771624f097b2bbf025da3c7d8f4f8 \ - --hash=sha256:cb8edccdce358451205f689e3536272200761c625c8e8059ab10523984cf8b61 -jax-cuda12-pjrt==0.5.3; python_version >= '3.10' \ - --hash=sha256:c5378306568ba0c81b230a779dd3194c9dd10339ab6360ae80928108d37e7f75 \ - --hash=sha256:04ee111eaf5fc2692978ad4a5c84d5925e42eb05c1701849ba3a53f6515400cc -jax-cuda12-pjrt==0.4.30; python_version == '3.9' \ - --hash=sha256:895d0198ad99638fcaf976c47592e2a543eef79ea15fabd24a402d055390c328 \ - --hash=sha256:c36fb1e0c236563bf3a87e70f4d1ab28a31d7cf5d722c9ede30c4172116e8bcb + --hash=sha256:c2517a7c2186f8708894696e26cf96ebd60b7879ceca398b2c46abb28d2c96c8 +jax-cuda12-pjrt==0.8.3; python_version >= '3.13' \ + --hash=sha256:f6d085fa7b2836cd79b14cabf1058ddb50c5161bfca9ede407993fa4f7547b7b \ + --hash=sha256:f740c661dd4064ff45dedf170fd0c4ff1a25d077636ad293307e8d28c78e65d7 +jax-cuda12-pjrt==0.5.3; python_version < '3.13' \ + --hash=sha256:04ee111eaf5fc2692978ad4a5c84d5925e42eb05c1701849ba3a53f6515400cc \ + --hash=sha256:c5378306568ba0c81b230a779dd3194c9dd10339ab6360ae80928108d37e7f75 warp-lang==1.12.1 \ --hash=sha256:98df3533a6c40a33cce961f8efa991006b30c9d286356e4cd77ea8ce86928f1d \ --hash=sha256:6bf01f10509488ba8eacaf4ec7fcf7cfbd503118b22e002ecba407b40a17424e \ diff --git a/mjx/requirements.txt b/mjx/requirements.txt index 4e5b56f0..835dbfa8 100644 --- a/mjx/requirements.txt +++ b/mjx/requirements.txt @@ -2,29 +2,48 @@ absl-py==2.1.0 \ --hash=sha256:526a04eadab8b4ee719ce68f204172ead1027549089702d99b9059f129ff1308 etils[epath]==1.10.0; python_version >= '3.10' \ --hash=sha256:0777fe60a234b4c65ca53470fc64f2dd2d0c6bca7fcc623fdaa8d7fa5a317098 -jax==0.5.3; python_version >= '3.10' and (sys_platform != 'darwin' or platform_machine != 'x86_64') \ +jax==0.8.3; python_version >= '3.13' and (sys_platform != 'darwin' or platform_machine != 'x86_64') \ + --hash=sha256:fb75f4cfee64e990ce6d7a0424cb11eb0520e4de19d7a52d0ca3498bff78261a +jax==0.5.3; python_version >= '3.10' and python_version < '3.13' and (sys_platform != 'darwin' or platform_machine != 'x86_64') \ --hash=sha256:1483dc237b4f47e41755d69429e8c3c138736716147cd43bb2b99b259d4e3c41 \ --hash=sha256:f17fcb0fd61dc289394af6ce4de2dada2312f2689bb0d73642c6f026a95fbb2c -jax==0.4.38; python_version >= '3.10' and sys_platform == 'darwin' and platform_machine == 'x86_64' \ +# 0.4.38 is the last Jax release that provides macOS x86-64 wheels, and only up +# to Python 3.13. Unfortunately, later releases can't readily be built from +# sources because some dependencies, like jaxlib, don't provide tarballs. +# Thus, building with Python 3.14+ on macOS x86-64 is disabled in the build +# scripts; if it's still needed, we'll have to come up with another method. +jax==0.4.38; python_version >= '3.10' and python_version <= '3.13' and sys_platform == 'darwin' and platform_machine == 'x86_64' \ --hash=sha256:78987306f7041ea8500d99df1a17c33ed92620c2268c4c3677fb24e06712be64 -jaxlib==0.5.3; python_version >= '3.10' and (sys_platform != 'darwin' or platform_machine != 'x86_64') \ +jaxlib==0.8.3; python_version >= '3.13' and (sys_platform != 'darwin' or platform_machine != 'x86_64') \ + --hash=sha256:1b3acadba65863254cc5482455d31a41f1fc8d38a449701f13cdbbc45beb240d \ + --hash=sha256:1e5481f9d7df9bfc0ae053d9f9d4f97ee77639a0169b45beb1cced070dc26da6 \ + --hash=sha256:3233198422c7ef49e5340785fc23f61c25ee61a5d767aa6f9183b8015e6ac6e2 \ + --hash=sha256:41067d2ec14a140e1d692fc84bc714f3372e208966b5f29cc309f4fca63c081b \ + --hash=sha256:4be4273edeb2cc75c409e446fd4f59a63d2e14f1311e714a978b5fa67ac16b70 \ + --hash=sha256:4d80575513f351eef4582908039c6e456d4e2ca028a043d2cdcbd059fa55e56c \ + --hash=sha256:56adca7fc1e24972633e2f33c758a28d924e116ba54b2a4e73f8ae4647657cf1 \ + --hash=sha256:831d817fa04218cc91b813920229c8ee0bec076c03ea744bec398e570b699d1c \ + --hash=sha256:92e755030e7862d3ba15929f25c0d7647baa95b083c87ed0c2da297aeb50c48c \ + --hash=sha256:9e76868758330eb63c5e4dd31e4d7c500a7e5523c7f09b34f64780c4a0503c7c \ + --hash=sha256:be237754eead89788264e112b2b5c722ab2a941406d19f1a8da6bf48bfde0bf2 \ + --hash=sha256:c96e562ad771fc81dfbb9a696519caa7e214f9b6dab1ce1df8bcd4759597d4a3 \ + --hash=sha256:ef3376145cc6c768f7847b688b004f37382e952bdc34b9a9404eb28b15ab50f8 \ + --hash=sha256:fe490fe3d81b02d21aaf936475fc4f47171473599e9d3907f6be0823ec4321bb +jaxlib==0.5.3; python_version >= '3.10' and python_version < '3.13' and (sys_platform != 'darwin' or platform_machine != 'x86_64') \ + --hash=sha256:29e1530fc81833216f1e28b578d0c59697654f72ee31c7a44ed7753baf5ac466 \ --hash=sha256:48ff5c89fb8a0fe04d475e9ddc074b4879a91d7ab68a51cec5cd1e87f81e6c47 \ - --hash=sha256:972400db4af6e85270d81db5e6e620d31395f0472e510c50dfcd4cb3f72b7220 \ + --hash=sha256:520665929649f29f7d948d4070dbaf3e032a4c1f7c11f2863eac73320fcee784 \ --hash=sha256:52be6c9775aff738a61170d8c047505c75bb799a45518e66a7a0908127b11785 \ + --hash=sha256:5a5e88ab1cd6fdf78d69abe3544e8f09cce200dd339bb85fbe3c2ea67f2a5e68 \ + --hash=sha256:8eb54e38d789557579f900ea3d70f104a440f8555a9681ed45f4a122dcbfd92e \ + --hash=sha256:972400db4af6e85270d81db5e6e620d31395f0472e510c50dfcd4cb3f72b7220 \ + --hash=sha256:a4666f81d72c060ed3e581ded116a9caa9b0a70a148a54cb12a1d3afca3624b5 \ --hash=sha256:b41a6fcaeb374fabc4ee7e74cfed60843bdab607cd54f60a68b7f7655cde2b66 \ --hash=sha256:b62bd8b29e5a4f9bfaa57c8daf6e04820b2c994f448f3dec602d64255545e9f2 \ - --hash=sha256:a4666f81d72c060ed3e581ded116a9caa9b0a70a148a54cb12a1d3afca3624b5 \ - --hash=sha256:29e1530fc81833216f1e28b578d0c59697654f72ee31c7a44ed7753baf5ac466 \ - --hash=sha256:8eb54e38d789557579f900ea3d70f104a440f8555a9681ed45f4a122dcbfd92e \ - --hash=sha256:d394dbde4a1c6bd67501cfb29d3819a10b900cb534cc0fc603319f7092f24cfa \ --hash=sha256:bddf6360377aa1c792e47fd87f307c342e331e5ff3582f940b1bca00f6b4bc73 \ - --hash=sha256:5a5e88ab1cd6fdf78d69abe3544e8f09cce200dd339bb85fbe3c2ea67f2a5e68 \ - --hash=sha256:520665929649f29f7d948d4070dbaf3e032a4c1f7c11f2863eac73320fcee784 \ - --hash=sha256:31321c25282a06a6dfc940507bc14d0a0ac838d8ced6c07aa00a7fae34ce7b3f \ - --hash=sha256:e904b92dedfbc7e545725a8d7676987030ae9c069001d94701bc109c6dab4100 \ - --hash=sha256:bb7593cb7fffcb13963f22fa5229ed960b8fb4ae5ec3b0820048cbd67f1e8e31 \ - --hash=sha256:8019f73a10b1290f988dd3768c684f3a8a147239091c3b790ce7e47e3bbc00bd -jaxlib==0.4.38; python_version >= '3.10' and sys_platform == 'darwin' and platform_machine == 'x86_64' \ + --hash=sha256:d394dbde4a1c6bd67501cfb29d3819a10b900cb534cc0fc603319f7092f24cfa +# See note above about macOS x86-64 support. +jaxlib==0.4.38; python_version >= '3.10' and python_version <= '3.13' and sys_platform == 'darwin' and platform_machine == 'x86_64' \ --hash=sha256:55c19b9d3f33a6fc59f644aa5a21fba02639ccdd776cb4a9b5526625f57839ff \ --hash=sha256:30b2f52cb50d74734af2f477c2533a7a583e3bb7b2c8acdeb361ee77d940577a \ --hash=sha256:ee19c163a8fdf0839d4c18b88a5fbfb4e731ba7c437416d3e5483e570bb764e4 \ @@ -52,35 +71,68 @@ pytest==8.3.3 \ --hash=sha256:a6853c7375b2663155079443d2e45de913a911a11d669df02a50814944db57b2 pytest-xdist==3.6.1 \ --hash=sha256:9ed4adfb68a016610848639bb7e02c9352d5d9f03d04809919e2dafc3be4cca7 -scipy==1.14.1; python_version >= '3.10' \ - --hash=sha256:baff393942b550823bfce952bb62270ee17504d02a1801d7fd0719534dfb9c84 \ - --hash=sha256:5149e3fd2d686e42144a093b206aef01932a0059c2a33ddfa67f5f035bdfe13e \ - --hash=sha256:b99722ea48b7ea25e8e015e8341ae74624f72e5f21fc2abd45f3a93266de4c5d \ - --hash=sha256:0c2f95de3b04e26f5f3ad5bb05e74ba7f68b837133a4492414b3afd79dfe540e \ - --hash=sha256:e0cf28db0f24a38b2a0ca33a85a54852586e43cf6fd876365c86e0657cfe7d73 \ - --hash=sha256:4079b90df244709e675cdc8b93bfd8a395d59af40b72e339c2287c91860deb8e \ - --hash=sha256:1729560c906963fc8389f6aac023739ff3983e727b1a4d87696b7bf108316a79 \ - --hash=sha256:2ff38e22128e6c03ff73b6bb0f85f897d2362f8c052e3b8ad00532198fbdae3f \ - --hash=sha256:8f9ea80f2e65bdaa0b7627fb00cbeb2daf163caa015e59b7516395fe3bd1e066 \ - --hash=sha256:30ac8812c1d2aab7131a79ba62933a2a76f582d5dbbc695192453dae67ad6310 \ - --hash=sha256:eb58ca0abd96911932f688528977858681a59d61a7ce908ffd355957f7025cfc \ +scipy==1.17.0; python_version >= '3.13' \ + --hash=sha256:00fb5f8ec8398ad90215008d8b6009c9db9fa924fd4c7d6be307c6f945f9cd73 \ + --hash=sha256:130d12926ae34399d157de777472bf82e9061c60cc081372b3118edacafe1d00 \ + --hash=sha256:13c4096ac6bc31d706018f06a49abe0485f96499deb82066b94d19b02f664209 \ + --hash=sha256:13e861634a2c480bd237deb69333ac79ea1941b94568d4b0efa5db5e263d4fd1 \ + --hash=sha256:1f9586a58039d7229ce77b52f8472c972448cded5736eaf102d5658bbac4c269 \ + --hash=sha256:1ff269abf702f6c7e67a4b7aad981d42871a11b9dd83c58d2d2ea624efbd1088 \ + --hash=sha256:2b531f57e09c946f56ad0b4a3b2abee778789097871fc541e267d2eca081cff1 \ + --hash=sha256:33af70d040e8af9d5e7a38b5ed3b772adddd281e3062ff23fec49e49681c38cf \ + --hash=sha256:3625c631a7acd7cfd929e4e31d2582cf00f42fcf06011f59281271746d77e061 \ + --hash=sha256:363ad4ae2853d88ebcde3ae6ec46ccca903ea9835ee8ba543f12f575e7b07e4e \ + --hash=sha256:423ca1f6584fc03936972b5f7c06961670dbba9f234e71676a7c7ccf938a0d61 \ + --hash=sha256:4e00562e519c09da34c31685f6acc3aa384d4d50604db0f245c14e1b4488bfa2 \ + --hash=sha256:5fb10d17e649e1446410895639f3385fd2bf4c3c7dfc9bea937bddcbc3d7b9ba \ + --hash=sha256:65ec32f3d32dfc48c72df4291345dae4f048749bc8d5203ee0a3f347f96c5ce6 \ + --hash=sha256:6680f2dfd4f6182e7d6db161344537da644d1cf85cf293f015c60a17ecf08752 \ + --hash=sha256:6e886000eb4919eae3a44f035e63f0fd8b651234117e8f6f29bad1cd26e7bc45 \ + --hash=sha256:819fc26862b4b3c73a60d486dbb919202f3d6d98c87cf20c223511429f2d1a97 \ + --hash=sha256:8547e7c57f932e7354a2319fab613981cde910631979f74c9b542bb167a8b9db \ + --hash=sha256:87b411e42b425b84777718cc41516b8a7e0795abfa8e8e1d573bf0ef014f0812 \ + --hash=sha256:979c3a0ff8e5ba254d45d59ebd38cde48fce4f10b5125c680c7a4bfe177aab07 \ + --hash=sha256:9fad7d3578c877d606b1150135c2639e9de9cecd3705caa37b66862977cc3e72 \ + --hash=sha256:a38c3337e00be6fd8a95b4ed66b5d988bac4ec888fd922c2ea9fe5fb1603dd67 \ + --hash=sha256:aabf057c632798832f071a8dde013c2e26284043934f53b00489f1773b33527e \ + --hash=sha256:c17514d11b78be8f7e6331b983a65a7f5ca1fd037b95e27b280921fe5606286a \ + --hash=sha256:c5e8647f60679790c2f5c76be17e2e9247dc6b98ad0d3b065861e082c56e078d \ + --hash=sha256:cacbaddd91fcffde703934897c5cd2c7cb0371fac195d383f4e1f1c5d3f3bd04 \ + --hash=sha256:d7425fcafbc09a03731e1bc05581f5fad988e48c6a861f441b7ab729a49a55ea \ + --hash=sha256:dbf133ced83889583156566d2bdf7a07ff89228fe0c0cb727f777de92092ec6b \ + --hash=sha256:eb2651271135154aa24f6481cbae5cc8af1f0dd46e6533fb7b56aa9727b6a232 \ + --hash=sha256:ec0827aa4d36cb79ff1b81de898e948a51ac0b9b1c43e4a372c0508c38c0f9a3 \ + --hash=sha256:edce1a1cf66298cccdc48a1bdf8fb10a3bf58e8b58d6c3883dd1530e103f87c0 \ + --hash=sha256:eec3842ec9ac9de5917899b277428886042a93db0b227ebbe3a333b64ec7643d \ + --hash=sha256:f2a4942b0f5f7c23c7cd641a0ca1955e2ae83dedcff537e3a0259096635e186b \ + --hash=sha256:f7df7941d71314e60a481e02d5ebcb3f0185b8d799c70d03d8258f6c80f3d467 \ + --hash=sha256:f9eb55bb97d00f8b7ab95cb64f873eb0bf54d9446264d9f3609130381233483f \ + --hash=sha256:fe508b5690e9eaaa9467fc047f833af58f1152ae51a0d0aed67aa5801f4dd7d6 +scipy==1.14.1; python_version >= '3.10' and python_version < '3.13' \ + --hash=sha256:278266012eb69f4a720827bdd2dc54b2271c97d84255b2faaa8f161a158c3b37 \ --hash=sha256:2843f2d527d9eebec9a43e6b406fb7266f3af25a751aa91d62ff416f54170bc5 \ - --hash=sha256:af29a935803cc707ab2ed7791c44288a682f9c8107bc00f0eccc4f92c08d6e07 \ + --hash=sha256:2da0469a4ef0ecd3693761acbdc20f2fdeafb69e6819cc081308cc978153c675 \ + --hash=sha256:2ff0a7e01e422c15739ecd64432743cf7aae2b03f3084288f399affcefe5222d \ + --hash=sha256:2ff38e22128e6c03ff73b6bb0f85f897d2362f8c052e3b8ad00532198fbdae3f \ + --hash=sha256:30ac8812c1d2aab7131a79ba62933a2a76f582d5dbbc695192453dae67ad6310 \ + --hash=sha256:3a1b111fac6baec1c1d92f27e76511c9e7218f1695d61b59e05e0fe04dc59617 \ --hash=sha256:631f07b3734d34aced009aaf6fedfd0eb3498a97e581c3b1e5f14a04164a456d \ --hash=sha256:716e389b694c4bb564b4fc0c51bc84d381735e0d39d3f26ec1af2556ec6aad94 \ - --hash=sha256:fef8c87f8abfb884dac04e97824b61299880c43f4ce675dd2cbeadd3c9b466d2 \ - --hash=sha256:278266012eb69f4a720827bdd2dc54b2271c97d84255b2faaa8f161a158c3b37 \ + --hash=sha256:8426251ad1e4ad903a4514712d2fa8fdd5382c978010d1c6f5f37ef286a713ad \ --hash=sha256:8475230e55549ab3f207bff11ebfc91c805dc3463ef62eda3ccf593254524ce8 \ - --hash=sha256:3a1b111fac6baec1c1d92f27e76511c9e7218f1695d61b59e05e0fe04dc59617 \ - --hash=sha256:c0ee987efa6737242745f347835da2cc5bb9f1b42996a4d97d5c7ff7928cb6f2 \ - --hash=sha256:2da0469a4ef0ecd3693761acbdc20f2fdeafb69e6819cc081308cc978153c675 \ - --hash=sha256:a49f6ed96f83966f576b33a44257d869756df6cf1ef4934f59dd58b25e0327e5 \ - --hash=sha256:8e32dced201274bf96899e6491d9ba3e9a5f6b336708656466ad0522d8528f69 \ - --hash=sha256:2ff0a7e01e422c15739ecd64432743cf7aae2b03f3084288f399affcefe5222d \ - --hash=sha256:97c5dddd5932bd2a1a31c927ba5e1463a53b87ca96b5c9bdf5dfd6096e27efc3 \ --hash=sha256:8bddf15838ba768bb5f5083c1ea012d64c9a444e16192762bd858f1e126196d0 \ + --hash=sha256:8e32dced201274bf96899e6491d9ba3e9a5f6b336708656466ad0522d8528f69 \ + --hash=sha256:8f9ea80f2e65bdaa0b7627fb00cbeb2daf163caa015e59b7516395fe3bd1e066 \ + --hash=sha256:97c5dddd5932bd2a1a31c927ba5e1463a53b87ca96b5c9bdf5dfd6096e27efc3 \ + --hash=sha256:a49f6ed96f83966f576b33a44257d869756df6cf1ef4934f59dd58b25e0327e5 \ + --hash=sha256:af29a935803cc707ab2ed7791c44288a682f9c8107bc00f0eccc4f92c08d6e07 \ + --hash=sha256:b05d43735bb2f07d689f56f7b474788a13ed8adc484a85aa65c0fd931cf9ccd2 \ + --hash=sha256:b28d2ca4add7ac16ae8bb6632a3c86e4b9e4d52d3e34267f6e1b0c1f8d87e389 \ + --hash=sha256:c0ee987efa6737242745f347835da2cc5bb9f1b42996a4d97d5c7ff7928cb6f2 \ --hash=sha256:d0d2821003174de06b69e58cef2316a6622b60ee613121199cb2852a873f8cf3 \ - --hash=sha256:b28d2ca4add7ac16ae8bb6632a3c86e4b9e4d52d3e34267f6e1b0c1f8d87e389 + --hash=sha256:eb58ca0abd96911932f688528977858681a59d61a7ce908ffd355957f7025cfc \ + --hash=sha256:edaf02b82cd7639db00dbff629995ef185c8df4c3ffa71a5562a595765a06ce1 \ + --hash=sha256:fef8c87f8abfb884dac04e97824b61299880c43f4ce675dd2cbeadd3c9b466d2 setuptools==78.1.1 \ --hash=sha256:c3a9c4211ff4c309edb8b8c4f1cbfa7ae324c4ba9f91ff254e3d305b9fd54561 \ --hash=sha256:fcc17fd9cd898242f6b4adfaca46137a9edef687f43e6f78469692a5e70d851d @@ -100,56 +152,103 @@ zipp==3.21.0 \ --hash=sha256:ac1bbe05fd2991f160ebce24ffbac5f6d11d83dc90891255885223d42b3cd931 # Transitive dependencies of jax and jaxlib -ml-dtypes==0.5.0 \ - --hash=sha256:cb5cc7b25acabd384f75bbd78892d0c724943f3e2e1986254665a1aa10982e07 \ - --hash=sha256:54415257f00eb44fbcc807454efac3356f75644f1cbfc2d4e5522a72ae1dacab \ - --hash=sha256:e04fde367b2fe901b1d47234426fe8819909bd1dd862a5adb630f27789c20599 \ - --hash=sha256:d3b3db9990c3840986a0e70524e122cfa32b91139c3653df76121ba7776e015f \ - --hash=sha256:afa08343069874a30812871d639f9c02b4158ace065601406a493a8511180c02 \ +ml-dtypes==0.5.4; python_version >= '3.13' \ + --hash=sha256:0d2ffd05a2575b1519dc928c0b93c06339eb67173ff53acb00724502cda231cf \ + --hash=sha256:14a4fd3228af936461db66faccef6e4f41c1d82fcc30e9f8d58a08916b1d811f \ + --hash=sha256:2314892cdc3fcf05e373d76d72aaa15fda9fb98625effa73c1d646f331fcecb7 \ + --hash=sha256:2b857d3af6ac0d39db1de7c706e69c7f9791627209c3d6dedbfca8c7e5faec22 \ + --hash=sha256:304ad47faa395415b9ccbcc06a0350800bc50eda70f0e45326796e27c62f18b6 \ + --hash=sha256:4381fe2f2452a2d7589689693d3162e876b3ddb0a832cde7a414f8e1adf7eab1 \ + --hash=sha256:531eff30e4d368cb6255bc2328d070e35836aa4f282a0fb5f3a0cd7260257298 \ + --hash=sha256:533ce891ba774eabf607172254f2e7260ba5f57bdd64030c9a4fcfbd99815d0d \ + --hash=sha256:6a0df4223b514d799b8a1629c65ddc351b3efa833ccf7f8ea0cf654a61d1e35d \ + --hash=sha256:805cef3a38f4eafae3a5bf9ebdcdb741d0bcfd9e1bd90eb54abd24f928cd2465 \ + --hash=sha256:8c6a2dcebd6f3903e05d51960a8058d6e131fe69f952a5397e5dbabc841b6d56 \ + --hash=sha256:8c760d85a2f82e2bed75867079188c9d18dae2ee77c25a54d60e9cc79be1bc48 \ + --hash=sha256:bfc534409c5d4b0bf945af29e5d0ab075eae9eecbb549ff8a29280db822f34f9 \ + --hash=sha256:cb73dccfc991691c444acc8c0012bee8f2470da826a92e3a20bb333b1a7894e6 \ + --hash=sha256:ce756d3a10d0c4067172804c9cc276ba9cc0ff47af9078ad439b075d1abdc29b \ + --hash=sha256:f21c9219ef48ca5ee78402d5cc831bd58ea27ce89beda894428bc67a52da5328 +ml-dtypes==0.5.0; python_version < '3.13' \ + --hash=sha256:2e7534392682c3098bc7341648c650864207169c654aed83143d7a19c67ae06f \ + --hash=sha256:60275f2b51b56834e840c4809fca840565f9bf8e9a73f6d8c94f5b5935701215 \ + --hash=sha256:76942f6aeb5c40766d5ea62386daa4148e6a54322aaf5b53eae9e7553240222f \ + --hash=sha256:8c32138975797e681eb175996d64356bcfa124bdbb6a70460b9768c2b35a6fa4 \ + --hash=sha256:968fede07d1f9b926a63df97d25ac656cac1a57ebd33701734eaf704bc55d8d8 \ --hash=sha256:a38df8df61194aeaae1ab7579075779b4ad32cd1cffd012c28be227fa7f2a70a \ --hash=sha256:a988bac6572630e1e9c2edd9b1277b4eefd1c86209e52b0d061b775ac33902ff \ - --hash=sha256:d4b1a70a3e5219790d6b55b9507606fc4e02911d1497d16c18dd721eb7efe7d0 \ - --hash=sha256:dc74fd9995513d33eac63d64e436240f5494ec74d522a9f0920194942fc3d2d7 \ - --hash=sha256:2e7534392682c3098bc7341648c650864207169c654aed83143d7a19c67ae06f \ - --hash=sha256:76942f6aeb5c40766d5ea62386daa4148e6a54322aaf5b53eae9e7553240222f \ - --hash=sha256:60275f2b51b56834e840c4809fca840565f9bf8e9a73f6d8c94f5b5935701215 \ - --hash=sha256:968fede07d1f9b926a63df97d25ac656cac1a57ebd33701734eaf704bc55d8d8 \ - --hash=sha256:c7a9152f5876fef565516aa5dd1dccd6fc298a5891b2467973905103eb5c7856 \ --hash=sha256:ab046f2ff789b1f11b2491909682c5d089934835f9a760fafc180e47dcb676b8 \ - --hash=sha256:8c32138975797e681eb175996d64356bcfa124bdbb6a70460b9768c2b35a6fa4 \ - --hash=sha256:7ee9c320bb0f9ffdf9f6fa6a696ef2e005d1f66438d6f1c1457338e00a02e8cf \ - --hash=sha256:a03fc861b86cc586728e3d093ba37f0cc05e65330c3ebd7688e7bae8290f8859 \ - --hash=sha256:099e09edd54e676903b4538f3815b5ab96f5b119690514602d96bfdb67172cbe \ - --hash=sha256:5f2b59233a0dbb6a560b3137ed6125433289ccba2f8d9c3695a52423a369ed15 -numpy==2.1.3; python_version >= '3.10' \ - --hash=sha256:747641635d3d44bcb380d950679462fae44f54b131be347d5ec2bce47d3df9ed \ - --hash=sha256:5641516794ca9e5f8a4d17bb45446998c6554704d888f86df9b200e66bdcce56 \ - --hash=sha256:c181ba05ce8299c7aa3125c27b9c2167bca4a4445b7ce73d5febc411ca692e43 \ - --hash=sha256:016d0f6f5e77b0f0d45d77387ffa4bb89816b57c835580c3ce8e099ef830befe \ - --hash=sha256:dc258a761a16daa791081d026f0ed4399b582712e6fc887a95af09df10c5ca57 \ - --hash=sha256:f653490b33e9c3a4c1c01d41bc2aef08f9475af51146e4a7710c450cf9761598 \ - --hash=sha256:96fe52fcdb9345b7cd82ecd34547fca4321f7656d500eca497eb7ea5a926692f \ + --hash=sha256:afa08343069874a30812871d639f9c02b4158ace065601406a493a8511180c02 \ + --hash=sha256:c7a9152f5876fef565516aa5dd1dccd6fc298a5891b2467973905103eb5c7856 \ + --hash=sha256:d4b1a70a3e5219790d6b55b9507606fc4e02911d1497d16c18dd721eb7efe7d0 \ + --hash=sha256:dc74fd9995513d33eac63d64e436240f5494ec74d522a9f0920194942fc3d2d7 +numpy==2.4.4; python_version >= '3.13' \ + --hash=sha256:08f2e31ed5e6f04b118e49821397f12767934cfdd12a1ce86a058f91e004ee50 \ + --hash=sha256:0aec54fd785890ecca25a6003fd9a5aed47ad607bbac5cd64f836ad8666f4959 \ + --hash=sha256:2483e4584a1cb3092da4470b38866634bafb223cbcd551ee047633fd2584599a \ + --hash=sha256:27a8d92cd10f1382a67d7cf4db7ce18341b66438bdd9f691d7b0e48d104c2a9d \ + --hash=sha256:2aa0613a5177c264ff5921051a5719d20095ea586ca88cc802c5c218d1c67d3e \ + --hash=sha256:2d19e6e2095506d1736b7d80595e0f252d76b89f5e715c35e06e937679ea7d7a \ + --hash=sha256:42c16925aa5a02362f986765f9ebabf20de75cdefdca827d14315c568dcab113 \ + --hash=sha256:45dbed2ab436a9e826e302fcdcbe9133f9b0006e5af7168afb8963a6520da103 \ + --hash=sha256:4636de7fd195197b7535f231b5de9e4b36d2c440b6e566d2e4e4746e6af0ca93 \ + --hash=sha256:4a19d9dba1a76618dd86b164d608566f393f8ec6ac7c44f0cc879011c45e65af \ + --hash=sha256:4bbc7f303d125971f60ec0aaad5e12c62d0d2c925f0ab1273debd0e4ba37aba5 \ + --hash=sha256:4d6d57903571f86180eb98f8f0c839fa9ebbfb031356d87f1361be91e433f5b7 \ + --hash=sha256:4e874c976154687c1f71715b034739b45c7711bec81db01914770373d125e392 \ + --hash=sha256:5a285b3b96f951841799528cd1f4f01cd70e7e0204b4abebac9463eecfcf2a40 \ + --hash=sha256:5c70f1cc1c4efbe316a572e2d8b9b9cc44e89b95f79ca3331553fbb63716e2bf \ + --hash=sha256:6a246d5914aa1c820c9443ddcee9c02bec3e203b0c080349533fae17727dfd1b \ + --hash=sha256:715d1c092715954784bc79e1174fc2a90093dc4dc84ea15eb14dad8abdcdeb74 \ + --hash=sha256:874f200b2a981c647340f841730fc3a2b54c9d940566a3c4149099591e2c4c3d \ + --hash=sha256:8e3ed142f2728df44263aaf5fb1f5b0b99f4070c553a0d7f033be65338329150 \ + --hash=sha256:93e15038125dc1e5345d9b5b68aa7f996ec33b98118d18c6ca0d0b7d6198b7e8 \ + --hash=sha256:989824e9faf85f96ec9c7761cd8d29c531ad857bfa1daa930cba85baaecf1a9a \ + --hash=sha256:99d838547ace2c4aace6c4f76e879ddfe02bb58a80c1549928477862b7a6d6ed \ + --hash=sha256:a7164afb23be6e37ad90b2f10426149fd75aee07ca55653d2aa41e66c4ef697e \ + --hash=sha256:ac6b31e35612a26483e20750126d30d0941f949426974cace8e6b5c58a3657b0 \ + --hash=sha256:ad2e2ef14e0b04e544ea2fa0a36463f847f113d314aa02e5b402fdf910ef309e \ + --hash=sha256:b268594bccac7d7cf5844c7732e3f20c50921d94e36d7ec9b79e9857694b1b2f \ + --hash=sha256:c901b15172510173f5cb310eae652908340f8dede90fff9e3bf6c0d8dfd92f83 \ + --hash=sha256:c9b39d38a9bd2ae1becd7eac1303d031c5c110ad31f2b319c6e7d98b135c934d \ + --hash=sha256:d2a8490669bfe99a233298348acc2d824d496dee0e66e31b66a6022c2ad74a5c \ + --hash=sha256:e44319a2953c738205bf3354537979eaa3998ed673395b964c1176083dd46252 \ + --hash=sha256:e823b8b6edc81e747526f70f71a9c0a07ac4e7ad13020aa736bb7c9d67196115 \ + --hash=sha256:e892aff75639bbef0d2a2cfd55535510df26ff92f63c92cd84ef8d4ba5a5557f \ + --hash=sha256:f169b9a863d34f5d11b8698ead99febeaa17a13ca044961aa8e2662a6c7766a0 \ + --hash=sha256:f8474c4241bc18b750be2abea9d7a9ec84f46ef861dbacf86a4f6e043401f79e +numpy==2.1.3; python_version >= '3.10' and python_version < '3.13' \ + --hash=sha256:02135ade8b8a84011cbb67dc44e07c58f28575cf9ecf8ab304e51c05528c19f0 \ --hash=sha256:0d30c543f02e84e92c4b1f415b7c6b5326cbe45ee7882b6b77db7195fb971e3a \ - --hash=sha256:2312b2aa89e1f43ecea6da6ea9a810d06aae08321609d8dc0d0eda6d946a541b \ - --hash=sha256:8637dcd2caa676e475503d1f8fdb327bc495554e10838019651b76d17b98e512 \ --hash=sha256:0fa14563cc46422e99daef53d725d0c326e99e468a9320a240affffe87852564 \ - --hash=sha256:a6b46587b14b888e95e4a24d7b13ae91fa22386c199ee7b418f449032b2fa3b8 \ --hash=sha256:13138eadd4f4da03074851a698ffa7e405f41a0845a6b1ad135b81596e4e9958 \ - --hash=sha256:f55ba01150f52b1027829b50d70ef1dafd9821ea82905b63936668403c3b471e \ - --hash=sha256:d89dd2b6da69c4fff5e39c28a382199ddedc3a5be5390115608345dec660b9e2 \ - --hash=sha256:bc6f24b3d1ecc1eebfbf5d6051faa49af40b03be1aaa781ebdadcbc090b4539b \ - --hash=sha256:762479be47a4863e261a840e8e01608d124ee1361e48b96916f38b119cfda04a \ - --hash=sha256:973faafebaae4c0aaa1a1ca1ce02434554d67e628b8d805e61f874b84e136b09 \ - --hash=sha256:576a1c1d25e9e02ed7fa5477f30a127fe56debd53b8d2c89d5578f9857d03ca9 \ - --hash=sha256:c80e4a09b3d95b4e1cac08643f1152fa71a0a821a2d4277334c88d54b2219a41 \ + --hash=sha256:15cb89f39fa6d0bdfb600ea24b250e5f1a3df23f901f51c8debaa6a5d122b2f0 \ + --hash=sha256:17ee83a1f4fef3c94d16dc1802b998668b5419362c8a4f4e8a491de1b41cc3ee \ + --hash=sha256:2312b2aa89e1f43ecea6da6ea9a810d06aae08321609d8dc0d0eda6d946a541b \ + --hash=sha256:3522b0dfe983a575e6a9ab3a4a4dfe156c3e428468ff08ce582b9bb6bd1d71d4 \ --hash=sha256:4d1167c53b93f1f5d8a139a742b3c6f4d429b54e74e6b57d0eff40045187b15d \ - --hash=sha256:ecc76a9ba2911d8d37ac01de72834d8849e55473457558e12995f4cd53e778e0 \ - --hash=sha256:78574ac2d1a4a02421f25da9559850d59457bac82f2b8d7a44fe83a64f770098 \ - --hash=sha256:e711e02f49e176a01d0349d82cb5f05ba4db7d5e7e0defd026328e5cfb3226d3 \ + --hash=sha256:4f2015dfe437dfebbfce7c85c7b53d81ba49e71ba7eadbf1df40c915af75979f \ + --hash=sha256:576a1c1d25e9e02ed7fa5477f30a127fe56debd53b8d2c89d5578f9857d03ca9 \ --hash=sha256:6a4825252fcc430a182ac4dee5a505053d262c807f8a924603d411f6718b88fd \ + --hash=sha256:762479be47a4863e261a840e8e01608d124ee1361e48b96916f38b119cfda04a \ + --hash=sha256:78574ac2d1a4a02421f25da9559850d59457bac82f2b8d7a44fe83a64f770098 \ --hash=sha256:825656d0743699c529c5943554d223c021ff0494ff1442152ce887ef4f7561a1 \ + --hash=sha256:8637dcd2caa676e475503d1f8fdb327bc495554e10838019651b76d17b98e512 \ + --hash=sha256:973faafebaae4c0aaa1a1ca1ce02434554d67e628b8d805e61f874b84e136b09 \ + --hash=sha256:a38c19106902bb19351b83802531fea19dee18e5b37b36454f27f11ff956f7fc \ + --hash=sha256:a6b46587b14b888e95e4a24d7b13ae91fa22386c199ee7b418f449032b2fa3b8 \ --hash=sha256:b47fbb433d3260adcd51eb54f92a2ffbc90a4595f8970ee00e064c644ac788f5 \ - --hash=sha256:c894b4305373b9c5576d7a12b473702afdf48ce5369c074ba304cc5ad8730dff + --hash=sha256:bc6f24b3d1ecc1eebfbf5d6051faa49af40b03be1aaa781ebdadcbc090b4539b \ + --hash=sha256:c006b607a865b07cd981ccb218a04fc86b600411d83d6fc261357f1c0966755d \ + --hash=sha256:c7662f0e3673fe4e832fe07b65c50342ea27d989f92c80355658c7f888fcc83c \ + --hash=sha256:c80e4a09b3d95b4e1cac08643f1152fa71a0a821a2d4277334c88d54b2219a41 \ + --hash=sha256:c894b4305373b9c5576d7a12b473702afdf48ce5369c074ba304cc5ad8730dff \ + --hash=sha256:d89dd2b6da69c4fff5e39c28a382199ddedc3a5be5390115608345dec660b9e2 \ + --hash=sha256:e14e26956e6f1696070788252dcdff11b4aca4c3e8bd166e0df1bb8f315a67cb \ + --hash=sha256:e711e02f49e176a01d0349d82cb5f05ba4db7d5e7e0defd026328e5cfb3226d3 \ + --hash=sha256:ecc76a9ba2911d8d37ac01de72834d8849e55473457558e12995f4cd53e778e0 \ + --hash=sha256:f55ba01150f52b1027829b50d70ef1dafd9821ea82905b63936668403c3b471e \ + --hash=sha256:fa2d1337dc61c8dc417fbccf20f6d1e139896a30721b7f1e832b2bb6ef4eb6c4 opt-einsum==3.4.0 \ --hash=sha256:69bb92469f86a1565195ece4ac0323943e83477171b91d24c35afe028a90d7cd diff --git a/python/build_requirements.txt b/python/build_requirements.txt index 9be02aab..d3d97cc2 100644 --- a/python/build_requirements.txt +++ b/python/build_requirements.txt @@ -15,7 +15,42 @@ glfw==2.9.0 \ --hash=sha256:fcc430cb21984afba74945b7df38a5e1a02b36c0b4a2a2bab42b4a26d7cc51d6 \ --hash=sha256:aef5b555673b9555216e4cd7bc0bdbbb9983f66c620a85ba7310cfcfda5cd38c \ --hash=sha256:183da99152f63469e9263146db2eb1b6cc4ee0c4082b280743e57bd1b0a3bd70 -numpy==2.1.3; python_version >= '3.10' \ +numpy==2.4.4; python_version >= '3.13' \ + --hash=sha256:08f2e31ed5e6f04b118e49821397f12767934cfdd12a1ce86a058f91e004ee50 \ + --hash=sha256:0aec54fd785890ecca25a6003fd9a5aed47ad607bbac5cd64f836ad8666f4959 \ + --hash=sha256:2483e4584a1cb3092da4470b38866634bafb223cbcd551ee047633fd2584599a \ + --hash=sha256:27a8d92cd10f1382a67d7cf4db7ce18341b66438bdd9f691d7b0e48d104c2a9d \ + --hash=sha256:2aa0613a5177c264ff5921051a5719d20095ea586ca88cc802c5c218d1c67d3e \ + --hash=sha256:2d19e6e2095506d1736b7d80595e0f252d76b89f5e715c35e06e937679ea7d7a \ + --hash=sha256:42c16925aa5a02362f986765f9ebabf20de75cdefdca827d14315c568dcab113 \ + --hash=sha256:45dbed2ab436a9e826e302fcdcbe9133f9b0006e5af7168afb8963a6520da103 \ + --hash=sha256:4636de7fd195197b7535f231b5de9e4b36d2c440b6e566d2e4e4746e6af0ca93 \ + --hash=sha256:4a19d9dba1a76618dd86b164d608566f393f8ec6ac7c44f0cc879011c45e65af \ + --hash=sha256:4bbc7f303d125971f60ec0aaad5e12c62d0d2c925f0ab1273debd0e4ba37aba5 \ + --hash=sha256:4d6d57903571f86180eb98f8f0c839fa9ebbfb031356d87f1361be91e433f5b7 \ + --hash=sha256:4e874c976154687c1f71715b034739b45c7711bec81db01914770373d125e392 \ + --hash=sha256:5a285b3b96f951841799528cd1f4f01cd70e7e0204b4abebac9463eecfcf2a40 \ + --hash=sha256:5c70f1cc1c4efbe316a572e2d8b9b9cc44e89b95f79ca3331553fbb63716e2bf \ + --hash=sha256:6a246d5914aa1c820c9443ddcee9c02bec3e203b0c080349533fae17727dfd1b \ + --hash=sha256:715d1c092715954784bc79e1174fc2a90093dc4dc84ea15eb14dad8abdcdeb74 \ + --hash=sha256:874f200b2a981c647340f841730fc3a2b54c9d940566a3c4149099591e2c4c3d \ + --hash=sha256:8e3ed142f2728df44263aaf5fb1f5b0b99f4070c553a0d7f033be65338329150 \ + --hash=sha256:93e15038125dc1e5345d9b5b68aa7f996ec33b98118d18c6ca0d0b7d6198b7e8 \ + --hash=sha256:989824e9faf85f96ec9c7761cd8d29c531ad857bfa1daa930cba85baaecf1a9a \ + --hash=sha256:99d838547ace2c4aace6c4f76e879ddfe02bb58a80c1549928477862b7a6d6ed \ + --hash=sha256:a7164afb23be6e37ad90b2f10426149fd75aee07ca55653d2aa41e66c4ef697e \ + --hash=sha256:ac6b31e35612a26483e20750126d30d0941f949426974cace8e6b5c58a3657b0 \ + --hash=sha256:ad2e2ef14e0b04e544ea2fa0a36463f847f113d314aa02e5b402fdf910ef309e \ + --hash=sha256:b268594bccac7d7cf5844c7732e3f20c50921d94e36d7ec9b79e9857694b1b2f \ + --hash=sha256:c901b15172510173f5cb310eae652908340f8dede90fff9e3bf6c0d8dfd92f83 \ + --hash=sha256:c9b39d38a9bd2ae1becd7eac1303d031c5c110ad31f2b319c6e7d98b135c934d \ + --hash=sha256:d2a8490669bfe99a233298348acc2d824d496dee0e66e31b66a6022c2ad74a5c \ + --hash=sha256:e44319a2953c738205bf3354537979eaa3998ed673395b964c1176083dd46252 \ + --hash=sha256:e823b8b6edc81e747526f70f71a9c0a07ac4e7ad13020aa736bb7c9d67196115 \ + --hash=sha256:e892aff75639bbef0d2a2cfd55535510df26ff92f63c92cd84ef8d4ba5a5557f \ + --hash=sha256:f169b9a863d34f5d11b8698ead99febeaa17a13ca044961aa8e2662a6c7766a0 \ + --hash=sha256:f8474c4241bc18b750be2abea9d7a9ec84f46ef861dbacf86a4f6e043401f79e +numpy==2.1.3; python_version >= '3.10' and python_version < '3.13' \ --hash=sha256:747641635d3d44bcb380d950679462fae44f54b131be347d5ec2bce47d3df9ed \ --hash=sha256:5641516794ca9e5f8a4d17bb45446998c6554704d888f86df9b200e66bdcce56 \ --hash=sha256:c181ba05ce8299c7aa3125c27b9c2167bca4a4445b7ce73d5febc411ca692e43 \ diff --git a/python/build_requirements_usd.txt b/python/build_requirements_usd.txt index e6d6323c..99329f6c 100644 --- a/python/build_requirements_usd.txt +++ b/python/build_requirements_usd.txt @@ -1,39 +1,95 @@ -pillow==10.4.0 \ - --hash=sha256:030abdbe43ee02e0de642aee345efa443740aa4d828bfe8e2eb11922ea6a21ea \ - --hash=sha256:e4db64794ccdf6cb83a59d73405f63adbe2a1887012e308828596100a0b2f6cc \ - --hash=sha256:297e388da6e248c98bc4a02e018966af0c5f92dfacf5a5ca22fa01cb3179bca0 \ - --hash=sha256:1ef61f5dd14c300786318482456481463b9d6b91ebe5ef12f405afbba77ed0be \ - --hash=sha256:bee197b30783295d2eb680b311af15a20a8b24024a19c3a26431ff83eb8d1f70 \ - --hash=sha256:6209bb41dc692ddfee4942517c19ee81b86c864b626dbfca272ec0f7cff5d9fb \ - --hash=sha256:8bc1a764ed8c957a2e9cacf97c8b2b053b70307cf2996aafd70e91a082e70df3 \ - --hash=sha256:1d846aea995ad352d4bdcc847535bd56e0fd88d36829d2c90be880ef1ee4668a \ - --hash=sha256:86dcb5a1eb778d8b25659d5e4341269e8590ad6b4e8b44d9f4b07f8d136c414a \ - --hash=sha256:f5b92f4d70791b4a67157321c4e8225d60b119c5cc9aee8ecf153aace4aad4ef \ - --hash=sha256:bf2342ac639c4cf38799a44950bbc2dfcb685f052b9e262f446482afaf4bffca \ - --hash=sha256:29dbdc4207642ea6aad70fbde1a9338753d33fb23ed6956e706936706f52dd80 \ - --hash=sha256:866b6942a92f56300012f5fbac71f2d610312ee65e22f1aa2609e491284e5597 \ - --hash=sha256:673655af3eadf4df6b5457033f086e90299fdd7a47983a13827acf7459c15d94 \ - --hash=sha256:cbed61494057c0f83b83eb3a310f0bf774b09513307c434d4366ed64f4128a91 \ - --hash=sha256:76a911dfe51a36041f2e756b00f96ed84677cdeb75d25c767f296c1c1eda1319 \ - --hash=sha256:bbc527b519bd3aa9d7f429d152fea69f9ad37c95f0b02aebddff592688998abe \ - --hash=sha256:5e84b6cc6a4a3d76c153a6b19270b3526a5a8ed6b09501d3af891daa2a9de7d6 \ - --hash=sha256:5dc6761a6efc781e6a1544206f22c80c3af4c8cf461206d46a1e6006e4429ff3 \ - --hash=sha256:dfe91cb65544a1321e631e696759491ae04a2ea11d36715eca01ce07284738be \ - --hash=sha256:0a9ec697746f268507404647e531e92889890a087e03681a3606d9b920fbee3c \ - --hash=sha256:ecd85a8d3e79cd7158dec1c9e5808e821feea088e2f69a974db5edf84dc53141 \ - --hash=sha256:a985e028fc183bf12a77a8bbf36318db4238a3ded7fa9df1b9a133f1cb79f8fc \ - --hash=sha256:6c762a5b0997f5659a5ef2266abc1d8851ad7749ad9a6a5506eb23d314e4f46b \ - --hash=sha256:e4d49b85c4348ea0b31ea63bc75a9f3857869174e2bf17e7aba02945cd218e6f \ - --hash=sha256:7928ecbf1ece13956b95d9cbcfc77137652b02763ba384d9ab508099a2eca856 \ - --hash=sha256:543f3dc61c18dafb755773efc89aae60d06b6596a63914107f75459cf984164d \ - --hash=sha256:4d9667937cfa347525b319ae34375c37b9ee6b525440f3ef48542fcf66f2731e \ - --hash=sha256:961a7293b2457b405967af9c77dcaa43cc1a8cd50d23c532e62d48ab6cdd56f5 \ - --hash=sha256:b2724fdb354a868ddf9a880cb84d102da914e99119211ef7ecbdc613b8c96b3c \ - --hash=sha256:c76e5786951e72ed3686e122d14c5d7012f16c8303a674d18cdcd6d89557fc5b \ - --hash=sha256:930044bb7679ab003b14023138b50181899da3f25de50e9dbee23b61b4de2126 \ - --hash=sha256:134ace6dc392116566980ee7436477d844520a26a4b1bd4053f6f47d096997fd \ - --hash=sha256:298478fe4f77a4408895605f3482b6cc6222c018b2ce565c2b6b9c354ac3229b \ - --hash=sha256:0ae24a547e8b711ccaaf99c9ae3cd975470e1a30caa80a6aaee9a2f19c05701d +pillow==12.1.0 \ + --hash=sha256:00162e9ca6d22b7c3ee8e61faa3c3253cd19b6a37f126cad04f2f88b306f557d \ + --hash=sha256:079af2fb0c599c2ec144ba2c02766d1b55498e373b3ac64687e43849fbbef5bc \ + --hash=sha256:0b022eaaf709541b391ee069f0022ee5b36c709df71986e3f7be312e46f42c84 \ + --hash=sha256:0c27407a2d1b96774cbc4a7594129cc027339fd800cd081e44497722ea1179de \ + --hash=sha256:0ddedfaa8b5f0b4ffbc2fa87b556dc59f6bb4ecb14a53b33f9189713ae8053c0 \ + --hash=sha256:0deedf2ea233722476b3a81e8cdfbad786f7adbed5d848469fa59fe52396e4ef \ + --hash=sha256:0ed07dca4a8464bada6139ab38f5382f83e5f111698caf3191cb8dbf27d908b4 \ + --hash=sha256:0fde7ec5538ab5095cc02df38ee99b0443ff0e1c847a045554cf5f9af1f4aa82 \ + --hash=sha256:15c794d74303828eaa957ff8070846d0efe8c630901a1c753fdc63850e19ecd9 \ + --hash=sha256:1a949604f73eb07a8adab38c4fe50791f9919344398bdc8ac6b307f755fc7030 \ + --hash=sha256:1f345e7bc9d7f368887c712aa5054558bad44d2a301ddf9248599f4161abc7c0 \ + --hash=sha256:1fcc52d86ce7a34fd17cb04e87cfdb164648a3662a6f20565910a99653d66c18 \ + --hash=sha256:21e686a21078b0f9cb8c8a961d99e6a4ddb88e0fc5ea6e130172ddddc2e5221a \ + --hash=sha256:2415373395a831f53933c23ce051021e79c8cd7979822d8cc478547a3f4da8ef \ + --hash=sha256:277518bf4fe74aa91489e1b20577473b19ee70fb97c374aa50830b279f25841b \ + --hash=sha256:27b9baecb428899db6c0de572d6d305cfaf38ca1596b5c0542a5182e3e74e8c6 \ + --hash=sha256:29a4cef9cb672363926f0470afc516dbf7305a14d8c54f7abbb5c199cd8f8179 \ + --hash=sha256:3413c2ae377550f5487991d444428f1a8ae92784aac79caa8b1e3b89b175f77e \ + --hash=sha256:351889afef0f485b84078ea40fe33727a0492b9af3904661b0abbafee0355b72 \ + --hash=sha256:3ffaa2f0659e2f740473bcf03c702c39a8d4b2b7ffc629052028764324842c64 \ + --hash=sha256:40a8e3b9e8773876d6e30daed22f016509e3987bab61b3b7fe309d7019a87451 \ + --hash=sha256:414b9a78e14ffeb98128863314e62c3f24b8a86081066625700b7985b3f529bd \ + --hash=sha256:43aca0a55ce1eefc0aefa6253661cb54571857b1a7b2964bd8a1e3ef4b729924 \ + --hash=sha256:43b4899cfd091a9693a1278c4982f3e50f7fb7cff5153b05174b4afc9593b616 \ + --hash=sha256:461f9dfdafa394c59cd6d818bdfdbab4028b83b02caadaff0ffd433faf4c9a7a \ + --hash=sha256:4f9f6a650743f0ddee5593ac9e954ba1bdbc5e150bc066586d4f26127853ab94 \ + --hash=sha256:53d8b764726d3af1a138dd353116f774e3862ec7e3794e0c8781e30db0f35dfc \ + --hash=sha256:565c986f4b45c020f5421a4cea13ef294dde9509a8577f29b2fc5edc7587fff8 \ + --hash=sha256:5c5ae0a06e9ea030ab786b0251b32c7e4ce10e58d983c0d5c56029455180b5b9 \ + --hash=sha256:5cb7bc1966d031aec37ddb9dcf15c2da5b2e9f7cc3ca7c54473a20a927e1eb91 \ + --hash=sha256:5da841d81b1a05ef940a8567da92decaa15bc4d7dedb540a8c219ad83d91808a \ + --hash=sha256:5fee4c04aad8932da9f8f710af2c1a15a83582cfb884152a9caa79d4efcdbf9c \ + --hash=sha256:609e89d9f90b581c8d16358c9087df76024cf058fa693dd3e1e1620823f39670 \ + --hash=sha256:6258f3260986990ba2fa8a874f8b6e808cf5abb51a94015ca3dc3c68aa4f30ea \ + --hash=sha256:64efdf00c09e31efd754448a383ea241f55a994fd079866b92d2bbff598aad91 \ + --hash=sha256:65b80c1ee7e14a87d6a068dd3b0aea268ffcabfe0498d38661b00c5b4b22e74c \ + --hash=sha256:6741e6f3074a35e47c77b23a4e4f2d90db3ed905cb1c5e6e0d49bff2045632bc \ + --hash=sha256:681088909d7e8fa9e31b9799aaa59ba5234c58e5e4f1951b4c4d1082a2e980e0 \ + --hash=sha256:6b7a9d1db5dad90e2991645874f708e87d9a3c370c243c2d7684d28f7e133e6b \ + --hash=sha256:7315f9137087c4e0ee73a761b163fc9aa3b19f5f606a7fc08d83fd3e4379af65 \ + --hash=sha256:742aea052cf5ab5034a53c3846165bc3ce88d7c38e954120db0ab867ca242661 \ + --hash=sha256:75af0b4c229ac519b155028fa1be632d812a519abba9b46b20e50c6caa184f19 \ + --hash=sha256:7b5dd7cbae20285cdb597b10eb5a2c13aa9de6cde9bb64a3c1317427b1db1ae1 \ + --hash=sha256:7d6daa89a00b58c37cb1747ec9fb7ac3bc5ffd5949f5888657dfddde6d1312e0 \ + --hash=sha256:800429ac32c9b72909c671aaf17ecd13110f823ddb7db4dfef412a5587c2c24e \ + --hash=sha256:806f3987ffe10e867bab0ddad45df1148a2b98221798457fa097ad85d6e8bc75 \ + --hash=sha256:808b99604f7873c800c4840f55ff389936ef1948e4e87645eaf3fccbc8477ac4 \ + --hash=sha256:80941e6d573197a0c28f394753de529bb436b1ca990ed6e765cf42426abc39f8 \ + --hash=sha256:84cabc7095dd535ca934d57e9ce2a72ffd216e435a84acb06b2277b1de2689bd \ + --hash=sha256:8637e29d13f478bc4f153d8daa9ffb16455f0a6cb287da1b432fdad2bfbd66c7 \ + --hash=sha256:896866d2d436563fa2a43a9d72f417874f16b5545955c54a64941e87c1376c61 \ + --hash=sha256:8e178e3e99d3c0ea8fc64b88447f7cac8ccf058af422a6cedc690d0eadd98c51 \ + --hash=sha256:907bfa8a9cb790748a9aa4513e37c88c59660da3bcfffbd24a7d9e6abf224551 \ + --hash=sha256:9212d6b86917a2300669511ed094a9406888362e085f2431a7da985a6b124f45 \ + --hash=sha256:92a7fe4225365c5e3a8e598982269c6d6698d3e783b3b1ae979e7819f9cd55c1 \ + --hash=sha256:935b9d1aed48fcfb3f838caac506f38e29621b44ccc4f8a64d575cb1b2a88644 \ + --hash=sha256:97e9993d5ed946aba26baf9c1e8cf18adbab584b99f452ee72f7ee8acb882796 \ + --hash=sha256:983976c2ab753166dc66d36af6e8ec15bb511e4a25856e2227e5f7e00a160587 \ + --hash=sha256:9f5fefaca968e700ad1a4a9de98bf0869a94e397fe3524c4c9450c1445252304 \ + --hash=sha256:a332ac4ccb84b6dde65dbace8431f3af08874bf9770719d32a635c4ef411b18b \ + --hash=sha256:a40905599d8079e09f25027423aed94f2823adaf2868940de991e53a449e14a8 \ + --hash=sha256:a6dfc2af5b082b635af6e08e0d1f9f1c4e04d17d4e2ca0ef96131e85eda6eb17 \ + --hash=sha256:a786bf667724d84aa29b5db1c61b7bfdde380202aaca12c3461afd6b71743171 \ + --hash=sha256:a83e0850cb8f5ac975291ebfc4170ba481f41a28065277f7f735c202cd8e0af3 \ + --hash=sha256:aa0c9cc0b82b14766a99fbe6084409972266e82f459821cd26997a488a7261a7 \ + --hash=sha256:b17fbdbe01c196e7e159aacb889e091f28e61020a8abeac07b68079b6e626988 \ + --hash=sha256:b63e13dd27da389ed9475b3d28510f0f954bca0041e8e551b2a4eb1eab56a39a \ + --hash=sha256:b6e53e82ec2db0717eabb276aa56cf4e500c9a7cec2c2e189b55c24f65a3e8c0 \ + --hash=sha256:bb0984b30e973f7e2884362b7d23d0a348c7143ee559f38ef3eaab640144204c \ + --hash=sha256:bc11908616c8a283cf7d664f77411a5ed2a02009b0097ff8abbba5e79128ccf2 \ + --hash=sha256:bdec5e43377761c5dbca620efb69a77f6855c5a379e32ac5b158f54c84212b14 \ + --hash=sha256:bef9768cab184e7ae6e559c032e95ba8d07b3023c289f79a2bd36e8bf85605a5 \ + --hash=sha256:c990547452ee2800d8506c4150280757f88532f3de2a58e3022e9b179107862a \ + --hash=sha256:ca94b6aac0d7af2a10ba08c0f888b3d5114439b6b3ef39968378723622fed377 \ + --hash=sha256:cad302dc10fac357d3467a74a9561c90609768a6f73a1923b0fd851b6486f8b0 \ + --hash=sha256:d0a7735df32ccbcc98b98a1ac785cc4b19b580be1bdf0aeb5c03223220ea09d5 \ + --hash=sha256:d70347c8a5b7ccd803ec0c85c8709f036e6348f1e6a5bf048ecd9c64d3550b8b \ + --hash=sha256:d70534cea9e7966169ad29a903b99fc507e932069a881d0965a1a84bb57f6c6d \ + --hash=sha256:db44d5c160a90df2d24a24760bbd37607d53da0b34fb546c4c232af7192298ac \ + --hash=sha256:e115c15e3bc727b1ca3e641a909f77f8ca72a64fff150f666fcc85e57701c26c \ + --hash=sha256:e2479c7f02f9d505682dc47df8c0ea1fc5e264c4d1629a5d63fe3e2334b89554 \ + --hash=sha256:e5dcbe95016e88437ecf33544ba5db21ef1b8dd6e1b434a2cb2a3d605299e643 \ + --hash=sha256:e6bdb408f7c9dd2a5ff2b14a3b0bb6d4deb29fb9961e6eb3ae2031ae9a5cec13 \ + --hash=sha256:e75d3dba8fc1ddfec0cd752108f93b83b4f8d6ab40e524a95d35f016b9683b09 \ + --hash=sha256:efdc140e7b63b8f739d09a99033aa430accce485ff78e6d311973a67b6bf3208 \ + --hash=sha256:f10c98f49227ed8383d28174ee95155a675c4ed7f85e2e573b04414f7e371bda \ + --hash=sha256:f188028b5af6b8fb2e9a76ac0f841a575bd1bd396e46ef0840d9b88a48fdbcea \ + --hash=sha256:f188d580bd870cda1e15183790d1cc2fa78f666e76077d103edf048eed9c356e \ + --hash=sha256:f45bd71d1fa5e5749587613037b172e0b3b23159d1c00ef2fc920da6f470e6f0 \ + --hash=sha256:f61333d817698bdcdd0f9d7793e365ac3d2a21c1f1eb02b32ad6aefb8d8ea831 \ + --hash=sha256:fb125d860738a09d363a88daa0f59c4533529a90e564785e20fe875b200b6dbd usd-core==24.11; python_version<='3.11' and (platform_machine=='x86_64' or platform_system=='Darwin') \ --hash=sha256:b25bde521bb65497b8bb882e4dd0de03d111dab4937c941ff4ceea6238933d5b \ --hash=sha256:a0416e3f5bc120977028d82dda38bd652478042c228d9a7d053f736bb79cde96 \ From e8ebc994d2ad9f1625faa500cb0079c53e33e965 Mon Sep 17 00:00:00 2001 From: Michael Moss Date: Thu, 23 Apr 2026 01:27:01 -0700 Subject: [PATCH 117/251] Add 3.14 to the supported Python versions. PiperOrigin-RevId: 904301647 Change-Id: I30a75bec09068494be060e6ad046badf968d4241 --- mjx/pyproject.toml | 1 + python/pyproject.toml | 1 + 2 files changed, 2 insertions(+) diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index e6a48dec..c602c023 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -21,6 +21,7 @@ classifiers = [ "Programming Language :: Python :: 3.11", "Programming Language :: Python :: 3.12", "Programming Language :: Python :: 3.13", + "Programming Language :: Python :: 3.14", "Topic :: Scientific/Engineering", ] requires-python = ">=3.10" diff --git a/python/pyproject.toml b/python/pyproject.toml index 136e7822..45a5180b 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -21,6 +21,7 @@ classifiers = [ "Programming Language :: Python :: 3.11", "Programming Language :: Python :: 3.12", "Programming Language :: Python :: 3.13", + "Programming Language :: Python :: 3.14", "Topic :: Scientific/Engineering", ] dependencies = [ From c41ed42407172bbfe68cf77b1e9422558d63007e Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 23 Apr 2026 01:37:01 -0700 Subject: [PATCH 118/251] Split the core filament context from the mjr_ API compatibility layer. PiperOrigin-RevId: 904305762 Change-Id: I462068a847a21be8ad7f3f1a8889f6b5b2aa2d5f --- .../filament/filament/filament_context.cc | 255 +++++------------- .../filament/filament/filament_context.h | 101 ++++--- .../filament/mjr_filament_renderer.cc | 186 +++++++++++++ .../filament/filament/mjr_filament_renderer.h | 90 +++++++ .../filament/render_context_filament.cc | 10 +- 5 files changed, 411 insertions(+), 231 deletions(-) create mode 100644 src/experimental/filament/filament/mjr_filament_renderer.cc create mode 100644 src/experimental/filament/filament/mjr_filament_renderer.h diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index 427306b8..39e3dc0a 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -14,13 +14,11 @@ #include "experimental/filament/filament/filament_context.h" -#include #include -#include #include +#include #include -#include #include #include #include @@ -35,19 +33,10 @@ #include #include #include -#include -#include #include -#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/filament_platform_factory.h" -#include "experimental/filament/filament/imgui_bridge.h" -#include "experimental/filament/filament/imgui_editor.h" -#include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/object_manager.h" -#include "experimental/filament/filament/render_target.h" -#include "experimental/filament/filament/scene_bridge.h" #include "experimental/filament/filament/scene_view.h" -#include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -82,10 +71,6 @@ FilamentContext::FilamentContext(const mjrFilamentConfig* config) } FilamentContext::~FilamentContext() { - DestroyRenderTargets(); - imgui_bridge_.reset(); - scene_bridge_.reset(); - scene_view_.reset(); object_manager_.reset(); engine_->destroy(renderer_); engine_->destroy(window_swap_chain_); @@ -93,207 +78,95 @@ FilamentContext::~FilamentContext() { filament::Engine::destroy(engine_); } -void FilamentContext::Init(const mjModel* model) { - scene_view_ = std::make_unique(engine_); - scene_bridge_ = std::make_unique(object_manager_.get(), - scene_view_.get(), model); - imgui_bridge_ = - std::make_unique(object_manager_.get(), scene_view_.get()); +FilamentContext::FrameHandle FilamentContext::Render( + std::span requests, + std::span read_requests) { + if (requests.size() != 1) { + mju_error("Only one render request is supported for now."); + } + if (read_requests.size() > 1) { + mju_error("Only one read request is supported for now."); + } - // Set clear options. - filament::Renderer::ClearOptions opts; - opts.clear = true; - opts.discard = true; - opts.clearColor = ReadElement(model, "filament.clearColor", - filament::math::float4(0, 0, 0, 1)); - renderer_->setClearOptions(opts); -} + const RenderRequest& request = requests[0]; -void FilamentContext::Render(const mjrRect& viewport, const mjvScene* scene) { - // If we're rendering to the window, and the window size has changed, we need - // to reacquire the swap chain. - if (scene_swap_chain_target_ == kWindowSwapChain && - (viewport.width != window_width_ || viewport.height != window_height_)) { - if (window_width_ != 0 && window_height_ != 0) { - if constexpr (UTILS_HAS_THREADING) { - engine_->flushAndWait(); - } - engine_->destroy(window_swap_chain_); - window_swap_chain_ = engine_->createSwapChain(config_.native_window); + if (request.target == nullptr) { + if (!read_requests.empty()) { + mju_error("Cannot read pixels from the window."); } - window_width_ = viewport.width; - window_height_ = viewport.height; - } - scene_bridge_->Update(viewport, scene); - // Update the UX renderable entity after processing the scene in case there - // are any elements in the scene which generate UX draw calls (e.g. labels). - if (imgui_bridge_ && gui_swap_chain_target_ == scene_swap_chain_target_) { - // Prepare the filament Renderable that contains the GUI draw commands. We - // must call this function even if we do not plan on rendering the GUI to - // ensure the ImGui state is updated. - imgui_bridge_->Update(); - } - - last_render_mode_ = DrawMode::Color; - if (scene->flags[mjRND_SEGMENT]) { - last_render_mode_ = DrawMode::Segmentation; - } else if (scene->flags[mjRND_DEPTH]) { - last_render_mode_ = DrawMode::Depth; - } - last_camera_ = mjv_averageCamera(scene->camera, scene->camera + 1); - - // Render the frame if we're not rendering to a texture. - if (scene_swap_chain_target_ == kWindowSwapChain) { if constexpr (UTILS_HAS_THREADING) { // Wait until previous frame is completed before requesting a new frame. engine_->flushAndWait(); } + // If the window size has changed, we need to reacquire the swap chain. + if (request.width != window_width_ || request.height != window_height_) { + if (window_width_ != 0 && window_height_ != 0) { + engine_->destroy(window_swap_chain_); + window_swap_chain_ = engine_->createSwapChain(config_.native_window); + } + window_width_ = request.width; + window_height_ = request.height; + } + + SceneView::RenderRequest scene_view_request; + scene_view_request.draw_mode = request.draw_mode; + scene_view_request.viewport = {0, 0, request.width, request.height}; + scene_view_request.camera = request.camera; + scene_view_request.enable_ux = request.draw_ux; if (renderer_->beginFrame(window_swap_chain_)) { - SceneView::RenderRequest request; - request.draw_mode = last_render_mode_; - request.viewport = viewport; - request.camera = last_camera_; - request.enable_ux = (gui_swap_chain_target_ == kWindowSwapChain); - scene_view_->Render(renderer_, request); + request.scene->Render(renderer_, scene_view_request); renderer_->endFrame(); } if constexpr (!UTILS_HAS_THREADING) { engine_->execute(); } - } -} - -void FilamentContext::SetFrameBuffer(int framebuffer) { - switch (framebuffer) { - case mjFB_WINDOW: - scene_swap_chain_target_ = kWindowSwapChain; - gui_swap_chain_target_ = kWindowSwapChain; - break; - case mjFB_OFFSCREEN: - scene_swap_chain_target_ = kOffscreenSwapChain; - gui_swap_chain_target_ = kWindowSwapChain; - break; - case 2: // No official constant fo this. - scene_swap_chain_target_ = kOffscreenSwapChain; - gui_swap_chain_target_ = kOffscreenSwapChain; - break; - default: - mju_error("Invalid framebuffer mode: %d", framebuffer); - } - - if (framebuffer == 0) { - DestroyRenderTargets(); - } -} - -void FilamentContext::PrepareRenderTargets(int width, int height) { - RenderTargetConfig config; - DefaultRenderTargetConfig(&config); - - config.color_format = mjPIXEL_FORMAT_RGB8; - config.depth_format = mjPIXEL_FORMAT_DEPTH32F; - color_target_ = std::make_unique(engine_, config); - color_target_->Prepare(width, height); - - config.color_format = mjPIXEL_FORMAT_R32F; - config.depth_format = mjPIXEL_FORMAT_DEPTH32F; - depth_target_ = std::make_unique(engine_, config); - depth_target_->Prepare(width, height); -} - -void FilamentContext::DestroyRenderTargets() { - depth_target_.reset(); - color_target_.reset(); -} - -void FilamentContext::ReadPixels(mjrRect viewport, unsigned char* rgb, - float* depth) { - if (scene_swap_chain_target_ != kOffscreenSwapChain) { - mju_error("Cannot read pixels unless framebuffer is set."); - } - if (color_target_ == nullptr || depth_target_ == nullptr) { - if (viewport.left != 0) { - mju_error("Reading subpixels not supported."); + } else { + if (read_requests.empty()) { + mju_error( + "Rendering to a render target without a read request is pointless."); + } + + const ReadPixelsRequest& read_request = read_requests[0]; + if (read_request.num_bytes == 0) { + mju_error("Output buffer size is zero."); + } - if (viewport.bottom != 0) { - mju_error("Reading subpixels not supported."); - } - PrepareRenderTargets(viewport.width, viewport.height); - } - if (rgb) { if (renderer_->beginFrame(offscreen_swap_chain_)) { - SceneView::RenderRequest request; - request.draw_mode = last_render_mode_; - request.viewport = viewport; - request.target = color_target_.get(); - request.camera = last_camera_; - request.enable_ux = (gui_swap_chain_target_ == kOffscreenSwapChain); - scene_view_->Render(renderer_, request); - - const size_t num_bytes = viewport.width * viewport.height * 3; - color_target_->ReadColorPixels(renderer_, rgb, num_bytes); - + SceneView::RenderRequest scene_view_request; + scene_view_request.draw_mode = request.draw_mode; + scene_view_request.viewport = {0, 0, request.width, request.height}; + scene_view_request.camera = request.camera; + scene_view_request.enable_ux = request.draw_ux; + scene_view_request.target = request.target; + request.scene->Render(renderer_, scene_view_request); + request.target->ReadColorPixels(renderer_, read_request.output, + read_request.num_bytes); renderer_->endFrame(); } - } - - if (depth) { - if (renderer_->beginFrame(offscreen_swap_chain_)) { - SceneView::RenderRequest request; - request.draw_mode = DrawMode::Depth; - request.viewport = viewport; - request.target = depth_target_.get(); - request.camera = last_camera_; - scene_view_->Render(renderer_, request); - - const size_t num_bytes = viewport.width * viewport.height * sizeof(float); - depth_target_->ReadColorPixels( - renderer_, reinterpret_cast(depth), num_bytes); - - renderer_->endFrame(); + engine_->flushAndWait(); + if (read_request.read_completed_callback) { + read_request.read_completed_callback(read_request.user_data); } } + return ++frame_counter_; +} - if (rgb || depth) { - if constexpr (UTILS_HAS_THREADING) { - // Wait for rendering to copy back to buffer to complete. - engine_->flushAndWait(); - } +void FilamentContext::WaitForFrame(FrameHandle frame_handle) { + if (frame_counter_ < frame_handle) { + engine_->flushAndWait(); } } -void FilamentContext::UploadMesh(const mjModel* model, int id) { - if (!scene_bridge_) { - mju_error("SceneBridge is not initialized."); - } - scene_bridge_->UploadMesh(model, id); -} - -void FilamentContext::UploadTexture(const mjModel* model, int id) { - if (!scene_bridge_) { - mju_error("SceneBridge is not initialized."); - } - scene_bridge_->UploadTexture(model, id); -} - -void FilamentContext::UploadHeightField(const mjModel* model, int id) { - if (!scene_bridge_) { - mju_error("SceneBridge is not initialized."); - } - scene_bridge_->UploadHeightField(model, id); -} - -uintptr_t FilamentContext::UploadGuiImage(uintptr_t tex_id, - const uint8_t* pixels, int width, - int height, int bpp) { - if (imgui_bridge_) { - return imgui_bridge_->UploadImage(tex_id, pixels, width, height, bpp); - } - return 0; +void FilamentContext::SetClearColor(const filament::math::float4& color) { + filament::Renderer::ClearOptions opts; + opts.clear = true; + opts.discard = true; + opts.clearColor = color; + renderer_->setClearOptions(opts); } double FilamentContext::GetFrameRate() const { @@ -306,6 +179,4 @@ double FilamentContext::GetFrameRate() const { return 1.0e9 / static_cast(ns); } -void FilamentContext::UpdateGui() { DrawGui(scene_bridge_.get()); } - } // namespace mujoco diff --git a/src/experimental/filament/filament/filament_context.h b/src/experimental/filament/filament/filament_context.h index 3f4eeb7f..7bb0dd62 100644 --- a/src/experimental/filament/filament/filament_context.h +++ b/src/experimental/filament/filament/filament_context.h @@ -15,64 +15,106 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_FILAMENT_CONTEXT_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_FILAMENT_CONTEXT_H_ +#include #include +#include #include #include #include #include -#include -#include +#include +#include #include #include "experimental/filament/filament/draw_mode.h" -#include "experimental/filament/filament/imgui_bridge.h" +#include "experimental/filament/filament/scene_view.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/render_target.h" -#include "experimental/filament/filament/scene_bridge.h" -#include "experimental/filament/filament/scene_view.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { -// Manages the filament renderer that is exposed via the mjr functions. +// Manages the filament renderer and provides APIs for rendering scenes. class FilamentContext { public: explicit FilamentContext(const mjrFilamentConfig* config); ~FilamentContext(); - void Init(const mjModel* model); + // Information needed to render a single image of a scene. + struct RenderRequest { + // The scene to render. + SceneView* scene = nullptr; - void Render(const mjrRect& viewport, const mjvScene* scene); + // The method (e.g. Color, Depth, Segmentation, etc.) to use for rendering. + DrawMode draw_mode = DrawMode::Color; - void SetFrameBuffer(int framebuffer); + // The camera from which to render the scene. + mjvGLCamera camera; - void ReadPixels(mjrRect viewport, unsigned char* rgb, float* depth); + // The dimensions of the output image. + int width = 0; + int height = 0; - void UploadMesh(const mjModel* model, int id); + // The render target into which to render the image. If nullptr, the image + // will be rendered to the window (as previously configured in + // mjrFilamentConfig::native_window). + RenderTarget* target = nullptr; - void UploadTexture(const mjModel* model, int id); + // Whether or not to include the UX scene in the render. (The SceneView + // stores both a main simulation scene and the UX scene.) + bool draw_ux = true; + }; - void UploadHeightField(const mjModel* model, int id); + // Information needed to read pixels from a render target. + struct ReadPixelsRequest { + RenderTarget* target = nullptr; - uintptr_t UploadGuiImage(uintptr_t tex_id, const uint8_t* pixels, int width, - int height, int bpp); + // The buffer into which the read pixels will be written. + uint8_t* output = nullptr; + // The number of bytes in the output buffer. This should match the size of + // the render target texture. + std::size_t num_bytes = 0; + + // Callback when the read pixels operation is complete. This will be called + // during WaitForFrame() or in a subsequent call to Render(). This function + // can optionally be used to free the output buffer if needed. + void (*read_completed_callback)(void* user_data) = nullptr; + + // User data to pass to the completion callback. + void* user_data = nullptr; + }; + + // Rendering is asynchronous by nature. Each render request is assigned a + // unique Handle which can be used to query the status of the request. The + // Handle can also be used to block until the request is completed. + using FrameHandle = std::uint64_t; + + // Queues the given render requests for rendering. This function copies the + // necessary data from the requests into the renderer thread and returns + // immediately afterwards. The renderer thread will then perform the actual + // rendering on the GPU. Callers can use WaitForFrame to block until the + // rendering is complete. + FrameHandle Render(std::span render_requests, + std::span read_requests = {}); + + // Blocks until the given frame has completed rendering. + void WaitForFrame(FrameHandle frame_handle); + + // Sets the clear color for the renderer. + void SetClearColor(const filament::math::float4& color); + + // Returns the current frame rate of the renderer. double GetFrameRate() const; - void UpdateGui(); + filament::Engine* GetEngine() const { return engine_; } + + ObjectManager* GetObjectManager() const { return object_manager_.get(); } FilamentContext(const FilamentContext&) = delete; FilamentContext& operator=(const FilamentContext&) = delete; private: - enum SwapChainType { - kWindowSwapChain, - kOffscreenSwapChain, - }; - - void PrepareRenderTargets(int width, int height); - void DestroyRenderTargets(); - mjrFilamentConfig config_; filament::Engine* engine_ = nullptr; @@ -80,19 +122,10 @@ class FilamentContext { filament::SwapChain* window_swap_chain_ = nullptr; filament::SwapChain* offscreen_swap_chain_ = nullptr; std::unique_ptr platform_; - - DrawMode last_render_mode_ = DrawMode::Color; - mjvGLCamera last_camera_; - SwapChainType scene_swap_chain_target_ = kWindowSwapChain; - SwapChainType gui_swap_chain_target_ = kWindowSwapChain; - std::unique_ptr color_target_; - std::unique_ptr depth_target_; std::unique_ptr object_manager_; - std::unique_ptr scene_view_; - std::unique_ptr scene_bridge_; - std::unique_ptr imgui_bridge_; int window_width_ = 0; int window_height_ = 0; + std::uint64_t frame_counter_ = 0; }; } // namespace mujoco diff --git a/src/experimental/filament/filament/mjr_filament_renderer.cc b/src/experimental/filament/filament/mjr_filament_renderer.cc new file mode 100644 index 00000000..36908518 --- /dev/null +++ b/src/experimental/filament/filament/mjr_filament_renderer.cc @@ -0,0 +1,186 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "experimental/filament/filament/mjr_filament_renderer.h" + +#include +#include + +#include +#include +#include +#include +#include "experimental/filament/filament/draw_mode.h" +#include "experimental/filament/filament/filament_context.h" +#include "experimental/filament/filament/imgui_bridge.h" +#include "experimental/filament/filament/imgui_editor.h" +#include "experimental/filament/filament/model_util.h" +#include "experimental/filament/filament/render_target.h" +#include "experimental/filament/filament/scene_bridge.h" +#include "experimental/filament/filament/scene_view.h" +#include "experimental/filament/filament/texture.h" +#include "experimental/filament/render_context_filament.h" + +namespace mujoco { + +MjrFilamentRenderer::MjrFilamentRenderer(const mjrFilamentConfig* config) + : FilamentContext(config) { +} + +void MjrFilamentRenderer::Init(const mjModel* model) { + scene_view_ = std::make_unique(GetEngine()); + scene_bridge_ = std::make_unique(GetObjectManager(), + scene_view_.get(), model); + imgui_bridge_ = + std::make_unique(GetObjectManager(), scene_view_.get()); + + SetClearColor(ReadElement(model, "filament.clearColor", + filament::math::float4(0, 0, 0, 1))); +} + +void MjrFilamentRenderer::Render(const mjrRect& viewport, const mjvScene* scene) { + scene_bridge_->Update(viewport, scene); + // Update the UX renderable entity after processing the scene in case there + // are any elements in the scene which generate UX draw calls (e.g. labels). + if (imgui_bridge_ && gui_swap_chain_target_ == scene_swap_chain_target_) { + // Prepare the filament Renderable that contains the GUI draw commands. We + // must call this function even if we do not plan on rendering the GUI to + // ensure the ImGui state is updated. + imgui_bridge_->Update(); + } + + last_render_mode_ = DrawMode::Color; + if (scene->flags[mjRND_SEGMENT]) { + last_render_mode_ = DrawMode::Segmentation; + } else if (scene->flags[mjRND_DEPTH]) { + last_render_mode_ = DrawMode::Depth; + } + last_camera_ = mjv_averageCamera(scene->camera, scene->camera + 1); + + if (scene_swap_chain_target_ == kWindowSwapChain) { + RenderRequest request; + request.scene = scene_view_.get(); + request.draw_mode = last_render_mode_; + request.camera = last_camera_; + request.draw_ux = (gui_swap_chain_target_ == kWindowSwapChain); + request.width = viewport.width; + request.height = viewport.height; + FilamentContext::Render({&request, 1}); + } +} + +void MjrFilamentRenderer::SetFrameBuffer(int framebuffer) { + switch (framebuffer) { + case mjFB_WINDOW: + scene_swap_chain_target_ = kWindowSwapChain; + gui_swap_chain_target_ = kWindowSwapChain; + break; + case mjFB_OFFSCREEN: + scene_swap_chain_target_ = kOffscreenSwapChain; + gui_swap_chain_target_ = kWindowSwapChain; + break; + case 2: // No official constant fo this. + scene_swap_chain_target_ = kOffscreenSwapChain; + gui_swap_chain_target_ = kOffscreenSwapChain; + break; + default: + mju_error("Invalid framebuffer mode: %d", framebuffer); + } +} + +void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, + float* depth) { + if (scene_swap_chain_target_ != kOffscreenSwapChain) { + mju_error("ReadPixels is only supported for offscreen rendering."); + } + + RenderRequest request; + request.scene = scene_view_.get(); + request.camera = last_camera_; + request.draw_ux = (gui_swap_chain_target_ == kOffscreenSwapChain); + request.width = viewport.width; + request.height = viewport.height; + + if (rgb) { + request.draw_mode = last_render_mode_; + + RenderTargetConfig config; + DefaultRenderTargetConfig(&config); + config.color_format = mjPIXEL_FORMAT_RGB8; + config.depth_format = mjPIXEL_FORMAT_DEPTH32F; + auto target = std::make_unique(GetEngine(), config); + target->Prepare(request.width, request.height); + request.target = target.get(); + + ReadPixelsRequest read_request; + read_request.output = rgb; + read_request.num_bytes = viewport.width * viewport.height * 3; + const FrameHandle frame = + FilamentContext::Render({&request, 1}, {&read_request, 1}); + FilamentContext::WaitForFrame(frame); + } + + if (depth) { + request.draw_mode = DrawMode::Depth; + + RenderTargetConfig config; + DefaultRenderTargetConfig(&config); + config.color_format = mjPIXEL_FORMAT_R32F; + config.depth_format = mjPIXEL_FORMAT_DEPTH32F; + auto target = std::make_unique(GetEngine(), config); + target->Prepare(request.width, request.height); + request.target = target.get(); + + ReadPixelsRequest read_request; + read_request.output = reinterpret_cast(depth); + read_request.num_bytes = viewport.width * viewport.height * sizeof(float); + const FrameHandle frame = + FilamentContext::Render({&request, 1}, {&read_request, 1}); + FilamentContext::WaitForFrame(frame); + } +} + +void MjrFilamentRenderer::UploadMesh(const mjModel* model, int id) { + if (!scene_bridge_) { + mju_error("SceneBridge is not initialized."); + } + scene_bridge_->UploadMesh(model, id); +} + +void MjrFilamentRenderer::UploadTexture(const mjModel* model, int id) { + if (!scene_bridge_) { + mju_error("SceneBridge is not initialized."); + } + scene_bridge_->UploadTexture(model, id); +} + +void MjrFilamentRenderer::UploadHeightField(const mjModel* model, int id) { + if (!scene_bridge_) { + mju_error("SceneBridge is not initialized."); + } + scene_bridge_->UploadHeightField(model, id); +} + +uintptr_t MjrFilamentRenderer::UploadGuiImage(uintptr_t tex_id, + const uint8_t* pixels, int width, + int height, int bpp) { + if (imgui_bridge_) { + return imgui_bridge_->UploadImage(tex_id, pixels, width, height, bpp); + } + return 0; +} + +void MjrFilamentRenderer::UpdateGui() { DrawGui(scene_bridge_.get()); } + +} // namespace mujoco diff --git a/src/experimental/filament/filament/mjr_filament_renderer.h b/src/experimental/filament/filament/mjr_filament_renderer.h new file mode 100644 index 00000000..929e3378 --- /dev/null +++ b/src/experimental/filament/filament/mjr_filament_renderer.h @@ -0,0 +1,90 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MJR_FILAMENT_RENDERER_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MJR_FILAMENT_RENDERER_H_ + +#include +#include + +#include +#include +#include +#include "experimental/filament/filament/draw_mode.h" +#include "experimental/filament/filament/filament_context.h" +#include "experimental/filament/filament/imgui_bridge.h" +#include "experimental/filament/filament/scene_bridge.h" +#include "experimental/filament/filament/scene_view.h" +#include "experimental/filament/render_context_filament.h" + +namespace mujoco { + +// Subclass of the FilamentContext that implements the legacy mjr API. +class MjrFilamentRenderer : public FilamentContext { + public: + explicit MjrFilamentRenderer(const mjrFilamentConfig* config); + ~MjrFilamentRenderer() = default; + + // Initializes the renderer with the given model. + void Init(const mjModel* model); + + // Renders the given mjvScene to the viewport. + void Render(const mjrRect& viewport, const mjvScene* scene); + + // Configures the renderer to render to the window (0) or an offscreen + // texture (1 or 2). Rendering to the window always includes UX data from + // ImGui. A value of 1 indicates the UX should not be included in the + // offscreen render, whereas 2 indicates that it should. + void SetFrameBuffer(int framebuffer); + + // Renders the scene to a texture if the framebuffer is not 0. + void ReadPixels(mjrRect viewport, unsigned char* rgb, float* depth); + + // Uploads the mesh data from the model to the GPU. + void UploadMesh(const mjModel* model, int id); + + // Uploads the texture data from the model to the GPU. + void UploadTexture(const mjModel* model, int id); + + // Uploads the height field data from the model to the GPU. + void UploadHeightField(const mjModel* model, int id); + + // Uploads a texture that can be used with ImGui to the GPU. + uintptr_t UploadGuiImage(uintptr_t tex_id, const uint8_t* pixels, int width, + int height, int bpp); + + // Renders an ImGui window containing Filament-specific editor UI. + void UpdateGui(); + + MjrFilamentRenderer(const MjrFilamentRenderer&) = delete; + MjrFilamentRenderer& operator=(const MjrFilamentRenderer&) = delete; + + private: + enum SwapChainType { + kWindowSwapChain, + kOffscreenSwapChain, + }; + + DrawMode last_render_mode_ = DrawMode::Color; + mjvGLCamera last_camera_; + SwapChainType scene_swap_chain_target_ = kWindowSwapChain; + SwapChainType gui_swap_chain_target_ = kWindowSwapChain; + std::unique_ptr scene_view_; + std::unique_ptr scene_bridge_; + std::unique_ptr imgui_bridge_; +}; + +} // namespace mujoco + +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MJR_FILAMENT_RENDERER_H_ diff --git a/src/experimental/filament/render_context_filament.cc b/src/experimental/filament/render_context_filament.cc index d49069f5..10f0aa2b 100644 --- a/src/experimental/filament/render_context_filament.cc +++ b/src/experimental/filament/render_context_filament.cc @@ -21,13 +21,13 @@ #include #include #include -#include "experimental/filament/filament/filament_context.h" +#include "experimental/filament/filament/mjr_filament_renderer.h" #if defined(TLS_FILAMENT_CONTEXT) -static thread_local mujoco::FilamentContext* g_filament_context = nullptr; +static thread_local mujoco::MjrFilamentRenderer* g_filament_context = nullptr; #else -static mujoco::FilamentContext* g_filament_context = nullptr; +static mujoco::MjrFilamentRenderer* g_filament_context = nullptr; #endif static void CheckFilamentContext() { @@ -43,13 +43,13 @@ void mjrf_defaultFilamentConfig(mjrFilamentConfig* config) { } void mjrf_makeFilamentContext(const mjModel* m, mjrContext* con, - const mjrFilamentConfig* config) { + const mjrFilamentConfig* config) { // TODO: Support multiple contexts and multiple threads. For now, we'll just // assume a single, global context. if (g_filament_context != nullptr) { mju_error("Context already exists!"); } - g_filament_context = new mujoco::FilamentContext(config); + g_filament_context = new mujoco::MjrFilamentRenderer(config); g_filament_context->Init(m); } From 57ad7463b11da49bc33cb48884b47c8a1f8c3d76 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 23 Apr 2026 02:22:53 -0700 Subject: [PATCH 119/251] Use separate SceneView for main scene and UX. Refactor FilamentContext to allow multiple render requests to be submitted onto a single RenderTarget. The main scene and the ux scene are then submitted as two separate requests that share the same target, resulting in a single, composited render. PiperOrigin-RevId: 904324801 Change-Id: I7c47ec4f8d159320612fa33384c01e8db3414daa --- .../filament/filament/filament_context.cc | 129 ++++++++++-------- .../filament/filament/filament_context.h | 4 - .../filament/filament/imgui_bridge.cc | 13 +- .../filament/filament/imgui_bridge.h | 9 +- .../filament/mjr_filament_renderer.cc | 127 ++++++++++------- .../filament/filament/mjr_filament_renderer.h | 15 +- .../filament/filament/scene_bridge.cc | 18 ++- .../filament/filament/scene_bridge.h | 9 +- .../filament/filament/scene_geom_util.cc | 4 +- .../filament/filament/scene_view.cc | 83 +++++------ .../filament/filament/scene_view.h | 28 ++-- 11 files changed, 244 insertions(+), 195 deletions(-) diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index 39e3dc0a..8c159ce2 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -36,6 +36,7 @@ #include #include "experimental/filament/filament/filament_platform_factory.h" #include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/filament/render_target.h" #include "experimental/filament/filament/scene_view.h" #include "experimental/filament/render_context_filament.h" @@ -81,77 +82,91 @@ FilamentContext::~FilamentContext() { FilamentContext::FrameHandle FilamentContext::Render( std::span requests, std::span read_requests) { - if (requests.size() != 1) { - mju_error("Only one render request is supported for now."); - } if (read_requests.size() > 1) { mju_error("Only one read request is supported for now."); } - const RenderRequest& request = requests[0]; - - if (request.target == nullptr) { - if (!read_requests.empty()) { - mju_error("Cannot read pixels from the window."); - } - - if constexpr (UTILS_HAS_THREADING) { - // Wait until previous frame is completed before requesting a new frame. - engine_->flushAndWait(); - } - - // If the window size has changed, we need to reacquire the swap chain. - if (request.width != window_width_ || request.height != window_height_) { - if (window_width_ != 0 && window_height_ != 0) { - engine_->destroy(window_swap_chain_); - window_swap_chain_ = engine_->createSwapChain(config_.native_window); - } - window_width_ = request.width; - window_height_ = request.height; - } - - SceneView::RenderRequest scene_view_request; - scene_view_request.draw_mode = request.draw_mode; - scene_view_request.viewport = {0, 0, request.width, request.height}; - scene_view_request.camera = request.camera; - scene_view_request.enable_ux = request.draw_ux; - if (renderer_->beginFrame(window_swap_chain_)) { - request.scene->Render(renderer_, scene_view_request); + bool render_began = false; + RenderTarget* current_target = nullptr; + for (const RenderRequest& request : requests) { + if (request.target != current_target && render_began) { renderer_->endFrame(); + render_began = false; } + current_target = request.target; + if (current_target == nullptr) { + if (!read_requests.empty()) { + mju_error("Cannot read pixels from the window."); + } + + if constexpr (UTILS_HAS_THREADING) { + // Wait until previous frame is completed before requesting a new frame. + engine_->flushAndWait(); + } + + // If the window size has changed, we need to reacquire the swap chain. + if (request.width != window_width_ || request.height != window_height_) { + if (window_width_ != 0 && window_height_ != 0) { + engine_->destroy(window_swap_chain_); + window_swap_chain_ = engine_->createSwapChain(config_.native_window); + } + window_width_ = request.width; + window_height_ = request.height; + } + + if (!render_began) { + render_began = renderer_->beginFrame(window_swap_chain_); + } + if (render_began) { + SceneView::RenderRequest scene_view_request; + scene_view_request.draw_mode = request.draw_mode; + scene_view_request.viewport = {0, 0, request.width, request.height}; + scene_view_request.camera = request.camera; + request.scene->Render(renderer_, scene_view_request); + } + } else { + if (read_requests.empty()) { + mju_error( + "Rendering to a render target without a read request is pointless."); + } + + const ReadPixelsRequest& read_request = read_requests[0]; + if (read_request.num_bytes == 0) { + mju_error("Output buffer size is zero."); + } + + if (!render_began) { + render_began = renderer_->beginFrame(offscreen_swap_chain_); + } + if (render_began) { + SceneView::RenderRequest scene_view_request; + scene_view_request.draw_mode = request.draw_mode; + scene_view_request.viewport = {0, 0, request.width, request.height}; + scene_view_request.camera = request.camera; + scene_view_request.target = request.target; + request.scene->Render(renderer_, scene_view_request); + request.target->ReadColorPixels(renderer_, read_request.output, + read_request.num_bytes); + } + } + } + + if (render_began) { + renderer_->endFrame(); + render_began = false; if constexpr (!UTILS_HAS_THREADING) { engine_->execute(); } - } else { - if (read_requests.empty()) { - mju_error( - "Rendering to a render target without a read request is pointless."); - } + } - const ReadPixelsRequest& read_request = read_requests[0]; - if (read_request.num_bytes == 0) { - mju_error("Output buffer size is zero."); - - } - - if (renderer_->beginFrame(offscreen_swap_chain_)) { - SceneView::RenderRequest scene_view_request; - scene_view_request.draw_mode = request.draw_mode; - scene_view_request.viewport = {0, 0, request.width, request.height}; - scene_view_request.camera = request.camera; - scene_view_request.enable_ux = request.draw_ux; - scene_view_request.target = request.target; - request.scene->Render(renderer_, scene_view_request); - request.target->ReadColorPixels(renderer_, read_request.output, - read_request.num_bytes); - renderer_->endFrame(); - } + if (!read_requests.empty()) { engine_->flushAndWait(); - if (read_request.read_completed_callback) { - read_request.read_completed_callback(read_request.user_data); + if (read_requests[0].read_completed_callback) { + read_requests[0].read_completed_callback(read_requests[0].user_data); } } + return ++frame_counter_; } diff --git a/src/experimental/filament/filament/filament_context.h b/src/experimental/filament/filament/filament_context.h index 7bb0dd62..391e9818 100644 --- a/src/experimental/filament/filament/filament_context.h +++ b/src/experimental/filament/filament/filament_context.h @@ -59,10 +59,6 @@ class FilamentContext { // will be rendered to the window (as previously configured in // mjrFilamentConfig::native_window). RenderTarget* target = nullptr; - - // Whether or not to include the UX scene in the render. (The SceneView - // stores both a main simulation scene and the UX scene.) - bool draw_ux = true; }; // Information needed to read pixels from a render target. diff --git a/src/experimental/filament/filament/imgui_bridge.cc b/src/experimental/filament/filament/imgui_bridge.cc index 97dd852a..85da0d00 100644 --- a/src/experimental/filament/filament/imgui_bridge.cc +++ b/src/experimental/filament/filament/imgui_bridge.cc @@ -37,8 +37,13 @@ namespace mujoco { using filament::math::float3; using filament::math::mat3f; -ImguiBridge::ImguiBridge(ObjectManager* object_mgr, SceneView* scene_view) - : object_mgr_(object_mgr), scene_view_(scene_view) {} +ImguiBridge::ImguiBridge(ObjectManager* object_mgr) + : object_mgr_(object_mgr) { + scene_view_ = std::make_unique(object_mgr_->GetEngine()); + scene_view_->DisableShadows(); + scene_view_->DisableReflections(); + scene_view_->DisablePostProcessing(); +} ImguiBridge::~ImguiBridge() { PrepareRenderables(0); } @@ -279,10 +284,10 @@ void ImguiBridge::PrepareRenderables(int count) { r->SetCastShadows(false); r->SetReceiveShadows(false); r->SetBlendOrder(static_cast(renderables_.size())); - scene_view_->AddToUxScene(r.get()); + scene_view_->AddToScene(r.get()); } while (renderables_.size() > count) { - scene_view_->RemoveFromUxScene(renderables_.back().get()); + scene_view_->RemoveFromScene(renderables_.back().get()); renderables_.pop_back(); } } diff --git a/src/experimental/filament/filament/imgui_bridge.h b/src/experimental/filament/filament/imgui_bridge.h index 54b5f494..d205db5c 100644 --- a/src/experimental/filament/filament/imgui_bridge.h +++ b/src/experimental/filament/filament/imgui_bridge.h @@ -29,10 +29,10 @@ namespace mujoco { -// Manages Renderables that will be added a SceneView's UX scene. +// Creates and manages a SceneView using data read from ImGui. class ImguiBridge { public: - ImguiBridge(ObjectManager* object_mgr, SceneView* scene_view); + explicit ImguiBridge(ObjectManager* object_mgr); ~ImguiBridge(); // Prepares the Renderables using data from the current ImGui state. This @@ -40,6 +40,9 @@ class ImguiBridge { // synced. void Update(); + // Returns the managed UX scene. + SceneView* GetSceneView() const { return scene_view_.get(); } + // Uploads texture to be used with ImGui's Image and ImageButton functions. uintptr_t UploadImage(uintptr_t tex_id, const uint8_t* pixels, int width, int height, int bpp); @@ -57,7 +60,7 @@ class ImguiBridge { void DestroyTexture(ImTextureData* data); ObjectManager* object_mgr_ = nullptr; - SceneView* scene_view_ = nullptr; + std::unique_ptr scene_view_; std::vector> renderables_; std::vector> meshes_; std::unordered_map> textures_; diff --git a/src/experimental/filament/filament/mjr_filament_renderer.cc b/src/experimental/filament/filament/mjr_filament_renderer.cc index 36908518..5e1112c8 100644 --- a/src/experimental/filament/filament/mjr_filament_renderer.cc +++ b/src/experimental/filament/filament/mjr_filament_renderer.cc @@ -14,6 +14,7 @@ #include "experimental/filament/filament/mjr_filament_renderer.h" +#include #include #include @@ -28,7 +29,6 @@ #include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/render_target.h" #include "experimental/filament/filament/scene_bridge.h" -#include "experimental/filament/filament/scene_view.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" @@ -39,11 +39,31 @@ MjrFilamentRenderer::MjrFilamentRenderer(const mjrFilamentConfig* config) } void MjrFilamentRenderer::Init(const mjModel* model) { - scene_view_ = std::make_unique(GetEngine()); - scene_bridge_ = std::make_unique(GetObjectManager(), - scene_view_.get(), model); - imgui_bridge_ = - std::make_unique(GetObjectManager(), scene_view_.get()); + scene_bridge_ = std::make_unique(GetObjectManager(), model); + imgui_bridge_ = std::make_unique(GetObjectManager()); + + render_requests_[0].scene = scene_bridge_->GetSceneView(); + render_requests_[0].draw_mode = DrawMode::Color; + + render_requests_[1].scene = imgui_bridge_->GetSceneView(); + render_requests_[1].draw_mode = DrawMode::Color; + + // The UX camera is a fixed orthographic camera. We only need to change the + // width/height based on the viewport per frame. + render_requests_[1].camera.orthographic = true; + render_requests_[1].camera.pos[0] = 0.0f; + render_requests_[1].camera.pos[1] = 0.0f; + render_requests_[1].camera.pos[2] = 1.0f; + render_requests_[1].camera.forward[0] = 0.0f; + render_requests_[1].camera.forward[1] = 0.0f; + render_requests_[1].camera.forward[2] = -1.0f; + render_requests_[1].camera.up[0] = 0.0f; + render_requests_[1].camera.up[1] = 1.0f; + render_requests_[1].camera.up[2] = 0.0f; + render_requests_[1].camera.frustum_top = 0.0f; + render_requests_[1].camera.frustum_near = 0.0f; + render_requests_[1].camera.frustum_far = 1.0f; + SetClearColor(ReadElement(model, "filament.clearColor", filament::math::float4(0, 0, 0, 1))); @@ -53,46 +73,45 @@ void MjrFilamentRenderer::Render(const mjrRect& viewport, const mjvScene* scene) scene_bridge_->Update(viewport, scene); // Update the UX renderable entity after processing the scene in case there // are any elements in the scene which generate UX draw calls (e.g. labels). - if (imgui_bridge_ && gui_swap_chain_target_ == scene_swap_chain_target_) { - // Prepare the filament Renderable that contains the GUI draw commands. We - // must call this function even if we do not plan on rendering the GUI to - // ensure the ImGui state is updated. + if (mode_ != FrameBufferMode::OffScreen) { imgui_bridge_->Update(); } - last_render_mode_ = DrawMode::Color; if (scene->flags[mjRND_SEGMENT]) { - last_render_mode_ = DrawMode::Segmentation; + render_requests_[0].draw_mode = DrawMode::Segmentation; } else if (scene->flags[mjRND_DEPTH]) { - last_render_mode_ = DrawMode::Depth; + render_requests_[0].draw_mode = DrawMode::Depth; + } else { + render_requests_[0].draw_mode = DrawMode::Color; } - last_camera_ = mjv_averageCamera(scene->camera, scene->camera + 1); - if (scene_swap_chain_target_ == kWindowSwapChain) { - RenderRequest request; - request.scene = scene_view_.get(); - request.draw_mode = last_render_mode_; - request.camera = last_camera_; - request.draw_ux = (gui_swap_chain_target_ == kWindowSwapChain); - request.width = viewport.width; - request.height = viewport.height; - FilamentContext::Render({&request, 1}); + render_requests_[0].width = viewport.width; + render_requests_[0].height = viewport.height; + render_requests_[1].width = viewport.width; + render_requests_[1].height = viewport.height; + + render_requests_[0].camera = mjv_averageCamera(scene->camera, scene->camera + 1); + render_requests_[1].camera.frustum_center = viewport.width / 2.0f; + render_requests_[1].camera.frustum_width = viewport.width / 2.0f; + render_requests_[1].camera.frustum_bottom = viewport.height; + + if (mode_ == FrameBufferMode::Window) { + render_requests_[0].target = nullptr; + render_requests_[1].target = nullptr; + FilamentContext::Render(render_requests_); } } void MjrFilamentRenderer::SetFrameBuffer(int framebuffer) { switch (framebuffer) { case mjFB_WINDOW: - scene_swap_chain_target_ = kWindowSwapChain; - gui_swap_chain_target_ = kWindowSwapChain; + mode_ = FrameBufferMode::Window; break; case mjFB_OFFSCREEN: - scene_swap_chain_target_ = kOffscreenSwapChain; - gui_swap_chain_target_ = kWindowSwapChain; + mode_ = FrameBufferMode::OffScreen; break; case 2: // No official constant fo this. - scene_swap_chain_target_ = kOffscreenSwapChain; - gui_swap_chain_target_ = kOffscreenSwapChain; + mode_ = FrameBufferMode::OffScreenWithGui; break; default: mju_error("Invalid framebuffer mode: %d", framebuffer); @@ -101,53 +120,62 @@ void MjrFilamentRenderer::SetFrameBuffer(int framebuffer) { void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, float* depth) { - if (scene_swap_chain_target_ != kOffscreenSwapChain) { + if (mode_ == FrameBufferMode::Window) { mju_error("ReadPixels is only supported for offscreen rendering."); } - RenderRequest request; - request.scene = scene_view_.get(); - request.camera = last_camera_; - request.draw_ux = (gui_swap_chain_target_ == kOffscreenSwapChain); - request.width = viewport.width; - request.height = viewport.height; + render_requests_[0].width = viewport.width; + render_requests_[0].height = viewport.height; + render_requests_[1].width = viewport.width; + render_requests_[1].height = viewport.height; if (rgb) { - request.draw_mode = last_render_mode_; - RenderTargetConfig config; DefaultRenderTargetConfig(&config); config.color_format = mjPIXEL_FORMAT_RGB8; config.depth_format = mjPIXEL_FORMAT_DEPTH32F; auto target = std::make_unique(GetEngine(), config); - target->Prepare(request.width, request.height); - request.target = target.get(); + target->Prepare(viewport.width, viewport.height); + render_requests_[0].target = target.get(); + render_requests_[1].target = target.get(); + + const size_t num_requests = + (mode_ == FrameBufferMode::OffScreenWithGui) ? 2 : 1; ReadPixelsRequest read_request; read_request.output = rgb; read_request.num_bytes = viewport.width * viewport.height * 3; - const FrameHandle frame = - FilamentContext::Render({&request, 1}, {&read_request, 1}); + const FrameHandle frame = FilamentContext::Render( + {&render_requests_[0], num_requests}, {&read_request, 1}); FilamentContext::WaitForFrame(frame); + + render_requests_[0].target = nullptr; + render_requests_[1].target = nullptr; } if (depth) { - request.draw_mode = DrawMode::Depth; - RenderTargetConfig config; DefaultRenderTargetConfig(&config); config.color_format = mjPIXEL_FORMAT_R32F; config.depth_format = mjPIXEL_FORMAT_DEPTH32F; auto target = std::make_unique(GetEngine(), config); - target->Prepare(request.width, request.height); - request.target = target.get(); + target->Prepare(viewport.width, viewport.height); + render_requests_[0].target = target.get(); + render_requests_[1].target = target.get(); + + DrawMode last_draw_mode = render_requests_[0].draw_mode; + render_requests_[0].draw_mode = DrawMode::Depth; ReadPixelsRequest read_request; read_request.output = reinterpret_cast(depth); read_request.num_bytes = viewport.width * viewport.height * sizeof(float); const FrameHandle frame = - FilamentContext::Render({&request, 1}, {&read_request, 1}); + FilamentContext::Render({&render_requests_[0], 1}, {&read_request, 1}); FilamentContext::WaitForFrame(frame); + + render_requests_[0].target = nullptr; + render_requests_[1].target = nullptr; + render_requests_[0].draw_mode = last_draw_mode; } } @@ -175,10 +203,7 @@ void MjrFilamentRenderer::UploadHeightField(const mjModel* model, int id) { uintptr_t MjrFilamentRenderer::UploadGuiImage(uintptr_t tex_id, const uint8_t* pixels, int width, int height, int bpp) { - if (imgui_bridge_) { - return imgui_bridge_->UploadImage(tex_id, pixels, width, height, bpp); - } - return 0; + return imgui_bridge_->UploadImage(tex_id, pixels, width, height, bpp); } void MjrFilamentRenderer::UpdateGui() { DrawGui(scene_bridge_.get()); } diff --git a/src/experimental/filament/filament/mjr_filament_renderer.h b/src/experimental/filament/filament/mjr_filament_renderer.h index 929e3378..205b8beb 100644 --- a/src/experimental/filament/filament/mjr_filament_renderer.h +++ b/src/experimental/filament/filament/mjr_filament_renderer.h @@ -25,7 +25,6 @@ #include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/imgui_bridge.h" #include "experimental/filament/filament/scene_bridge.h" -#include "experimental/filament/filament/scene_view.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -71,16 +70,14 @@ class MjrFilamentRenderer : public FilamentContext { MjrFilamentRenderer& operator=(const MjrFilamentRenderer&) = delete; private: - enum SwapChainType { - kWindowSwapChain, - kOffscreenSwapChain, + enum class FrameBufferMode { + Window, + OffScreen, + OffScreenWithGui, }; - DrawMode last_render_mode_ = DrawMode::Color; - mjvGLCamera last_camera_; - SwapChainType scene_swap_chain_target_ = kWindowSwapChain; - SwapChainType gui_swap_chain_target_ = kWindowSwapChain; - std::unique_ptr scene_view_; + FrameBufferMode mode_ = FrameBufferMode::Window; + RenderRequest render_requests_[2]; std::unique_ptr scene_bridge_; std::unique_ptr imgui_bridge_; }; diff --git a/src/experimental/filament/filament/scene_bridge.cc b/src/experimental/filament/filament/scene_bridge.cc index 3438b8c9..4cd23fa9 100644 --- a/src/experimental/filament/filament/scene_bridge.cc +++ b/src/experimental/filament/filament/scene_bridge.cc @@ -87,9 +87,9 @@ static std::unique_ptr CreateFallbackIndirectLightTexture( return texture; } -SceneBridge::SceneBridge(ObjectManager* object_mgr, SceneView* scene_view, - const mjModel* model) - : scene_view_(scene_view), object_mgr_(object_mgr) { +SceneBridge::SceneBridge(ObjectManager* object_mgr, const mjModel* model) + : object_mgr_(object_mgr) { + scene_view_ = std::make_unique(object_mgr_->GetEngine()); model_objects_ = std::make_unique(model, object_mgr_->GetEngine()); @@ -354,8 +354,16 @@ filament::math::mat4 CalculateClipFromWorld(const mjrRect& viewport, } void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { - filament::View* view = scene_view_->GetDefaultRenderView(); - view->setShadowingEnabled(scene->flags[mjRND_SHADOW] ? true : false); + if (scene->flags[mjRND_SHADOW]) { + scene_view_->EnableShadows(); + } else { + scene_view_->DisableShadows(); + } + if (scene->flags[mjRND_REFLECTION]) { + scene_view_->EnableReflections(); + } else { + scene_view_->DisableReflections(); + } mjtNum hpos[3], hfwd[3]; float headpos[3], gazedir[3]; diff --git a/src/experimental/filament/filament/scene_bridge.h b/src/experimental/filament/filament/scene_bridge.h index e707d6ed..8c31de05 100644 --- a/src/experimental/filament/filament/scene_bridge.h +++ b/src/experimental/filament/filament/scene_bridge.h @@ -25,7 +25,6 @@ #include #include #include "experimental/filament/filament/light.h" -#include "experimental/filament/filament/material.h" #include "experimental/filament/filament/model_objects.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/renderable.h" @@ -37,8 +36,7 @@ namespace mujoco { // Manages all mjModel data and updates a SceneView using an mjvScene. class SceneBridge { public: - SceneBridge(ObjectManager* object_mgr, SceneView* scene_view, - const mjModel* model); + SceneBridge(ObjectManager* object_mgr, const mjModel* model); ~SceneBridge(); // Updates the environment light using the KTX image at the given path. @@ -56,7 +54,8 @@ class SceneBridge { void UploadTexture(const mjModel* model, int id); void UploadHeightField(const mjModel* model, int id); - SceneView* GetSceneView() const { return scene_view_; } + // Returns the managed scene. + SceneView* GetSceneView() const { return scene_view_.get(); } SceneBridge(const SceneBridge&) = delete; SceneBridge& operator=(const SceneBridge&) = delete; @@ -69,7 +68,7 @@ class SceneBridge { std::optional ClipFromWorld( const filament::math::float3& pos) const; - SceneView* scene_view_ = nullptr; + std::unique_ptr scene_view_; ObjectManager* object_mgr_ = nullptr; std::unique_ptr model_objects_; std::unique_ptr fallback_ibl_; diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index 80c653a7..8db3d76e 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -351,7 +351,6 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, const mjModel* model = model_objs->GetModel(); const bool use_segid_color = scene->flags[mjRND_IDCOLOR]; - const bool enable_reflection = scene->flags[mjRND_REFLECTION]; MaterialParams params; params.color = ReadFloat4(geom.rgba); if (geom.type == mjGEOM_PLANE) { @@ -361,8 +360,7 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, params.reflective = false; } else { renderable.SetReceiveShadows(true); - params.reflective = - enable_reflection && geom.reflectance > 0 && params.color.a == 1.0f; + params.reflective = geom.reflectance > 0 && params.color.a == 1.0f; } } renderable.SetLayerMask(geom.category); diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index efcbd76f..eb2bb47c 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -127,9 +127,7 @@ static void SetupReflectionCamera(const mat4& surface_xform, SceneView::SceneView(filament::Engine* engine) : engine_(engine) { scene_ = engine->createScene(); - ux_scene_ = engine->createScene(); camera_ = engine->createCamera(utils::EntityManager::get().create()); - ux_camera_ = engine->createCamera(utils::EntityManager::get().create()); reflect_camera_ = engine->createCamera(utils::EntityManager::get().create()); for (auto& view : views_) { @@ -139,12 +137,6 @@ SceneView::SceneView(filament::Engine* engine) : engine_(engine) { view->setVisibleLayers(0xff, mjCAT_ALL); } - ux_view_ = engine->createView(); - ux_view_->setScene(ux_scene_); - ux_view_->setCamera(ux_camera_); - ux_view_->setPostProcessingEnabled(false); - ux_view_->setShadowingEnabled(false); - reflect_view_ = engine->createView(); reflect_view_->setScene(scene_); reflect_view_->setCamera(reflect_camera_); @@ -172,22 +164,16 @@ SceneView::~SceneView() { for (auto& renderable : renderables_) { renderable->RemoveFromScene(scene_); } - for (auto& renderable : ux_renderables_) { - renderable->RemoveFromScene(ux_scene_); - } lights_.clear(); renderables_.clear(); reflect_targets_.clear(); engine_->destroyCameraComponent(reflect_camera_->getEntity()); engine_->destroy(reflect_view_); - engine_->destroyCameraComponent(ux_camera_->getEntity()); - engine_->destroy(ux_view_); engine_->destroyCameraComponent(camera_->getEntity()); if (color_grading_) { engine_->destroy(color_grading_); } engine_->destroy(scene_); - engine_->destroy(ux_scene_); for (auto& view : views_) { engine_->destroy(view); } @@ -224,18 +210,6 @@ void SceneView::RemoveFromScene(Renderable* renderable) { } } -void SceneView::AddToUxScene(Renderable* renderable) { - if (ux_renderables_.insert(renderable).second) { - renderable->AddToScene(ux_scene_); - } -} - -void SceneView::RemoveFromUxScene(Renderable* renderable) { - if (ux_renderables_.erase(renderable)) { - renderable->RemoveFromScene(ux_scene_); - } -} - void SceneView::AddToScene(filament::Skybox* skybox) { skybox_ = skybox; scene_->setSkybox(skybox); @@ -255,7 +229,6 @@ void SceneView::Render(filament::Renderer* renderer, for (auto& view : views_) { view->setViewport(viewport); } - ux_view_->setViewport(viewport); reflect_view_->setViewport(viewport); SetupCamera(request.camera, viewport, camera_); @@ -276,7 +249,7 @@ void SceneView::Render(filament::Renderer* renderer, } // Render reflection passes. - if (request.draw_mode == DrawMode::Color) { + if (request.draw_mode == DrawMode::Color && reflections_enabled_) { for (size_t i = 0; i < reflectives_.size(); ++i) { Renderable* renderable = reflectives_[i]; @@ -301,15 +274,6 @@ void SceneView::Render(filament::Renderer* renderer, renderer->render(view); view->setRenderTarget(nullptr); - if (request.enable_ux) { - ux_camera_->setProjection(filament::Camera::Projection::ORTHO, 0.0f, - viewport.width, viewport.height, 0.0f, 0.0f, - 1.0f); - ux_view_->setRenderTarget(render_target); - renderer->render(ux_view_); - ux_view_->setRenderTarget(nullptr); - } - if (request.target) { view->setMultiSampleAntiAliasingOptions(options); } @@ -335,9 +299,11 @@ void SceneView::AddReflectiveRenderable(Renderable* renderable) { auto& target = reflect_targets_[index]; target->Prepare(viewport.width, viewport.height); - MaterialTextures textures = renderable->GetMaterialTextures(); - textures.reflection = target->GetColorTexture(); - renderable->UpdateMaterial(renderable->GetMaterialParams(), textures); + if (reflections_enabled_) { + MaterialTextures textures = renderable->GetMaterialTextures(); + textures.reflection = target->GetColorTexture(); + renderable->UpdateMaterial(renderable->GetMaterialParams(), textures); + } } void SceneView::SetColorGradingOptions(const ColorGradingOptions& opts) { @@ -353,6 +319,43 @@ void SceneView::SetColorGradingOptions(const ColorGradingOptions& opts) { color_grading_options_ = opts; } +void SceneView::EnableShadows() { + views_[kNormalIndex]->setShadowingEnabled(true); +} + +void SceneView::DisableShadows() { + views_[kNormalIndex]->setShadowingEnabled(false); +} + +void SceneView::EnableReflections() { + reflections_enabled_ = true; + + for (int i = 0; i < reflectives_.size(); ++i) { + Renderable* renderable = reflectives_[i]; + MaterialTextures textures = renderable->GetMaterialTextures(); + textures.reflection = reflect_targets_[i]->GetColorTexture(); + renderable->UpdateMaterial(renderable->GetMaterialParams(), textures); + } +} + +void SceneView::DisableReflections() { + reflections_enabled_ = false; + for (Renderable* renderable : reflectives_) { + MaterialTextures textures = renderable->GetMaterialTextures(); + textures.reflection = nullptr; + renderable->UpdateMaterial(renderable->GetMaterialParams(), textures); + } + +} + +void SceneView::EnablePostProcessing() { + views_[kNormalIndex]->setPostProcessingEnabled(true); +} + +void SceneView::DisablePostProcessing() { + views_[kNormalIndex]->setPostProcessingEnabled(false); +} + filament::View* SceneView::GetDefaultRenderView() { return views_[kNormalIndex]; } diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index a9a2874b..b6d5dfa6 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -38,8 +38,7 @@ namespace mujoco { // // The filament Scene is populated with the objects (e.g. lights, renderables, // skybox, etc.). It manages multiple views to support a variety of draw modes -// (e.g. normal, depth, segmentation, etc.) as well as reflective surfaces. It -// also manages a separate scene and view for UX rendering. +// (e.g. normal, depth, segmentation, etc.) as well as reflective surfaces. class SceneView { public: SceneView(filament::Engine* engine); @@ -53,10 +52,6 @@ class SceneView { void AddToScene(filament::Skybox* skybox); void RemoveFromScene(filament::Skybox* skybox); - // Adds/removes entities from the UX scene, which is rendered separately. - void AddToUxScene(Renderable* renderable); - void RemoveFromUxScene(Renderable* renderable); - // Parameters for rendering the scene. struct RenderRequest { // The draw mode (e.g. normal, depth, segmentation) to render. @@ -67,8 +62,6 @@ class SceneView { mjvGLCamera camera; // An optional render target into which the scene will be rendered. RenderTarget* target = nullptr; - // Whether or not to render the UX as a separate pass. - bool enable_ux = false; }; // Renders the scene. @@ -77,6 +70,18 @@ class SceneView { // Returns the filament Engine managing the scene. filament::Engine* GetEngine() const { return engine_; } + // Enables/disables shadows for the default render view. + void EnableShadows(); + void DisableShadows(); + + // Enables/disables reflections for the default render view. + void EnableReflections(); + void DisableReflections(); + + // Enables/disables post processing for the default render view. + void EnablePostProcessing(); + void DisablePostProcessing(); + // Returns the underlying filament View that is used for normal rendering. // Callers can update rendering settings (e.g. post processing) directly. filament::View* GetDefaultRenderView(); @@ -95,7 +100,6 @@ class SceneView { filament::Engine* engine_ = nullptr; filament::Scene* scene_ = nullptr; - filament::Scene* ux_scene_ = nullptr; filament::Camera* camera_ = nullptr; filament::ColorGrading* color_grading_ = nullptr; ColorGradingOptions color_grading_options_; @@ -106,16 +110,12 @@ class SceneView { std::unordered_set renderables_; filament::Skybox* skybox_ = nullptr; - // Custom view for UX. - filament::View* ux_view_ = nullptr; - filament::Camera* ux_camera_ = nullptr; - std::unordered_set ux_renderables_; - // Custom view and camera for reflective surfaces. filament::View* reflect_view_ = nullptr; filament::Camera* reflect_camera_ = nullptr; // The list of reflective renderables and their corresponding render targets. + bool reflections_enabled_ = true; std::vector reflectives_; std::vector> reflect_targets_; }; From e1794b3d1c840d9c841288f12a78c42e9ac49345 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 23 Apr 2026 03:10:05 -0700 Subject: [PATCH 120/251] Add missing files. PiperOrigin-RevId: 904346200 Change-Id: I63f4e8741098d5da9403c05ca13419c670743848 --- src/experimental/filament/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index fa2081b5..8fe1ecac 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -44,6 +44,8 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/math_util.h filament/mesh.cc filament/mesh.h + filament/mjr_filament_renderer.cc + filament/mjr_filament_renderer.h filament/model_objects.cc filament/model_objects.h filament/model_util.h From fa33310750f07ea2a08629547f0960392ce17737 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 23 Apr 2026 03:23:12 -0700 Subject: [PATCH 121/251] Fix use-after-release issue. PiperOrigin-RevId: 904351356 Change-Id: Ie6a574763b2ee0ffc0cf5537d8ccdfe87ab1b477 --- src/experimental/filament/filament/scene_bridge.cc | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/experimental/filament/filament/scene_bridge.cc b/src/experimental/filament/filament/scene_bridge.cc index 4cd23fa9..23e76cbe 100644 --- a/src/experimental/filament/filament/scene_bridge.cc +++ b/src/experimental/filament/filament/scene_bridge.cc @@ -180,7 +180,10 @@ SceneBridge::~SceneBridge() { scene_view_->RemoveFromScene(iter.get()); } lights_.clear(); - + if (fallback_ibl_) { + scene_view_->RemoveFromScene(fallback_ibl_.get()); + } + fallback_ibl_.reset(); for (auto& iter : renderables_) { scene_view_->RemoveFromScene(iter.get()); } From c1c7643fad6e958c6d98dfe20e2b58415dd18763 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 23 Apr 2026 07:09:44 -0700 Subject: [PATCH 122/251] Rename types to conform to mjr naming conventions. PiperOrigin-RevId: 904438811 Change-Id: I77fe14592a4460234e0aff473ee3f530e652bca9 --- .../filament/filament/imgui_bridge.cc | 16 +++++----- .../filament/filament/model_objects.cc | 8 ++--- .../filament/filament/render_target.cc | 8 ++--- .../filament/filament/render_target.h | 4 +-- .../filament/filament/scene_bridge.cc | 10 +++--- src/experimental/filament/filament/texture.cc | 24 +++++++------- src/experimental/filament/filament/texture.h | 32 +++++++++++-------- 7 files changed, 53 insertions(+), 49 deletions(-) diff --git a/src/experimental/filament/filament/imgui_bridge.cc b/src/experimental/filament/filament/imgui_bridge.cc index 85da0d00..68519b87 100644 --- a/src/experimental/filament/filament/imgui_bridge.cc +++ b/src/experimental/filament/filament/imgui_bridge.cc @@ -72,8 +72,8 @@ uintptr_t ImguiBridge::UploadImage(uintptr_t tex_id, const uint8_t* pixels, // new texture. if (texture == nullptr || texture->GetWidth() != width || texture->GetHeight() != height) { - TextureConfig config; - DefaultTextureConfig(&config); + mjrTextureConfig config; + mjr_defaultTextureConfig(&config); config.width = width; config.height = height; config.target = mjTEXTURE_2D; @@ -89,8 +89,8 @@ uintptr_t ImguiBridge::UploadImage(uintptr_t tex_id, const uint8_t* pixels, const auto callback = +[](void* user) { delete[] reinterpret_cast(user); }; - TextureData texture_data; - DefaultTextureData(&texture_data); + mjrTextureData texture_data; + mjr_defaultTextureData(&texture_data); texture_data.bytes = bytes; texture_data.nbytes = num_bytes; texture_data.user_data = bytes; @@ -106,8 +106,8 @@ void ImguiBridge::CreateTexture(ImTextureData* data) { mju_error("Unsupported texture format."); } - TextureConfig config; - DefaultTextureConfig(&config); + mjrTextureConfig config; + mjr_defaultTextureConfig(&config); config.width = data->Width; config.height = data->Height; config.target = mjTEXTURE_2D; @@ -127,8 +127,8 @@ void ImguiBridge::UpdateTexture(ImTextureData* data) { mju_error("Texture not found: %llu", data->TexID); } - TextureData texture_data; - DefaultTextureData(&texture_data); + mjrTextureData texture_data; + mjr_defaultTextureData(&texture_data); texture_data.bytes = data->GetPixels(); texture_data.nbytes = data->Width * data->Height * 4; texture_data.user_data = nullptr; diff --git a/src/experimental/filament/filament/model_objects.cc b/src/experimental/filament/filament/model_objects.cc index 15121743..931670e0 100644 --- a/src/experimental/filament/filament/model_objects.cc +++ b/src/experimental/filament/filament/model_objects.cc @@ -575,8 +575,8 @@ void ModelObjects::UploadTexture(const mjModel* model, int id) { mju_error("Invalid texture index: %d", id); } - TextureConfig config; - DefaultTextureConfig(&config); + mjrTextureConfig config; + mjr_defaultTextureConfig(&config); config.width = model->tex_width[id]; config.height = model->tex_height[id]; config.target = (mjtTexture)model->tex_type[id]; @@ -600,8 +600,8 @@ void ModelObjects::UploadTexture(const mjModel* model, int id) { } - TextureData payload; - DefaultTextureData(&payload); + mjrTextureData payload; + mjr_defaultTextureData(&payload); payload.bytes = model->tex_data + model->tex_adr[id]; payload.nbytes = model->tex_width[id] * model->tex_height[id] * model->tex_nchannel[id]; diff --git a/src/experimental/filament/filament/render_target.cc b/src/experimental/filament/filament/render_target.cc index b01fb94c..4ebd7adf 100644 --- a/src/experimental/filament/filament/render_target.cc +++ b/src/experimental/filament/filament/render_target.cc @@ -51,8 +51,8 @@ void RenderTarget::Prepare(int width, int height) { width_ = width; height_ = height; - TextureConfig color_config; - DefaultTextureConfig(&color_config); + mjrTextureConfig color_config; + mjr_defaultTextureConfig(&color_config); Texture::InternalFlags color_flags; color_config.width = width; color_config.height = height; @@ -63,8 +63,8 @@ void RenderTarget::Prepare(int width, int height) { color_flags.color_attachment = true; color_texture_ = std::make_unique(engine_, color_config, color_flags); - TextureConfig depth_config; - DefaultTextureConfig(&depth_config); + mjrTextureConfig depth_config; + mjr_defaultTextureConfig(&depth_config); Texture::InternalFlags depth_flags; depth_config.width = width; depth_config.height = height; diff --git a/src/experimental/filament/filament/render_target.h b/src/experimental/filament/filament/render_target.h index 22a02143..1ab77c0d 100644 --- a/src/experimental/filament/filament/render_target.h +++ b/src/experimental/filament/filament/render_target.h @@ -27,8 +27,8 @@ namespace mujoco { // Defines the basic properties of a render target. struct RenderTargetConfig { - mjtPixelFormat color_format; - mjtPixelFormat depth_format; + mjrPixelFormat color_format; + mjrPixelFormat depth_format; }; // Initializes the RenderTargetConfig to default values. diff --git a/src/experimental/filament/filament/scene_bridge.cc b/src/experimental/filament/filament/scene_bridge.cc index 23e76cbe..ad970247 100644 --- a/src/experimental/filament/filament/scene_bridge.cc +++ b/src/experimental/filament/filament/scene_bridge.cc @@ -64,8 +64,8 @@ static std::unique_ptr CreateFallbackIndirectLightTexture( std::unique_ptr asset = object_mgr->LoadAsset(filename); - TextureConfig config; - DefaultTextureConfig(&config); + mjrTextureConfig config; + mjr_defaultTextureConfig(&config); config.width = 1; config.height = 1; config.target = mjTEXTURE_CUBE; @@ -74,9 +74,9 @@ static std::unique_ptr CreateFallbackIndirectLightTexture( auto texture = std::make_unique(object_mgr->GetEngine(), config); - TextureData payload; - DefaultTextureData(&payload); - payload.bytes = (void*)asset->GetBytes().data(); + mjrTextureData payload; + mjr_defaultTextureData(&payload); + payload.bytes = asset->GetBytes().data(); payload.nbytes = asset->GetBytes().size(); payload.release_callback = +[](void* user_data) { delete static_cast(user_data); diff --git a/src/experimental/filament/filament/texture.cc b/src/experimental/filament/filament/texture.cc index 1277ec44..81b37448 100644 --- a/src/experimental/filament/filament/texture.cc +++ b/src/experimental/filament/filament/texture.cc @@ -29,15 +29,15 @@ namespace mujoco { static constexpr int kNumFacesPerCube = 6; -static bool IsCompressed(const TextureConfig& config) { +static bool IsCompressed(const mjrTextureConfig& config) { return config.format == mjPIXEL_FORMAT_KTX; } -static bool IsCubeMap(const TextureConfig& config) { +static bool IsCubeMap(const mjrTextureConfig& config) { return config.target == mjTEXTURE_CUBE || config.target == mjTEXTURE_SKYBOX; } -static int GetFaceHeight(const TextureConfig& config) { +static int GetFaceHeight(const mjrTextureConfig& config) { int face_height = config.height; if (config.width != config.height) { if (config.width * kNumFacesPerCube != config.height) { @@ -51,7 +51,7 @@ static int GetFaceHeight(const TextureConfig& config) { return face_height; } -static int GetNumChannels(const TextureConfig& config) { +static int GetNumChannels(const mjrTextureConfig& config) { switch (config.format) { case mjPIXEL_FORMAT_R8: return 1; @@ -65,7 +65,7 @@ static int GetNumChannels(const TextureConfig& config) { } } -static filament::Texture::Format GetTextureFormat(const TextureConfig& config) { +static filament::Texture::Format GetTextureFormat(const mjrTextureConfig& config) { switch (config.format) { case mjPIXEL_FORMAT_R8: return filament::Texture::Format::R; @@ -80,7 +80,7 @@ static filament::Texture::Format GetTextureFormat(const TextureConfig& config) { } static filament::Texture::InternalFormat GetTextureInternalFormat( - const TextureConfig& config) { + const mjrTextureConfig& config) { if (config.color_space == mjCOLORSPACE_SRGB) { switch (config.format) { case mjPIXEL_FORMAT_RGB8: @@ -110,15 +110,15 @@ static filament::Texture::InternalFormat GetTextureInternalFormat( } } -void DefaultTextureData(TextureData* data) { - std::memset(data, 0, sizeof(TextureData)); +void mjr_defaultTextureData(mjrTextureData* data) { + std::memset(data, 0, sizeof(mjrTextureData)); } -void DefaultTextureConfig(TextureConfig* config) { - std::memset(config, 0, sizeof(TextureConfig)); +void mjr_defaultTextureConfig(mjrTextureConfig* config) { + std::memset(config, 0, sizeof(mjrTextureConfig)); } -Texture::Texture(filament::Engine* engine, const TextureConfig& config, +Texture::Texture(filament::Engine* engine, const mjrTextureConfig& config, InternalFlags flags) : engine_(engine), config_(config) { if (IsCompressed(config_)) { @@ -166,7 +166,7 @@ Texture::~Texture() { } } -void Texture::Upload(const TextureData& data) { +void Texture::Upload(const mjrTextureData& data) { user_data_ = data.user_data; release_callback_ = data.release_callback; diff --git a/src/experimental/filament/filament/texture.h b/src/experimental/filament/filament/texture.h index b1493470..e51bbfff 100644 --- a/src/experimental/filament/filament/texture.h +++ b/src/experimental/filament/filament/texture.h @@ -21,12 +21,13 @@ #include #include #include +#include // Functions for creating filament textures. namespace mujoco { // Pixel formats for textures. -typedef enum mjtPixelFormat_ { +typedef enum mjrPixelFormat_ { mjPIXEL_FORMAT_UNKNOWN = 0, mjPIXEL_FORMAT_R8, mjPIXEL_FORMAT_RGB8, @@ -34,15 +35,18 @@ typedef enum mjtPixelFormat_ { mjPIXEL_FORMAT_R32F, mjPIXEL_FORMAT_DEPTH32F, mjPIXEL_FORMAT_KTX, -} mjtPixelFormat; +} mjrPixelFormat; + +typedef mjtTexture mjrTextureTarget; +typedef mjtColorSpace mjrColorSpace; // The binary contents of a texture. -struct TextureData { +struct mjrTextureData { // Pointer to the image data. If null, an empty texture will be created. - void* bytes; + const void* bytes; // The number of bytes in the image data. - size_t nbytes; + mjtSize nbytes; // Because rendering may be multithreaded, we cannot make assumptions about // when the image data will finish uploading to the GPU. As such, we will use @@ -54,10 +58,10 @@ struct TextureData { }; // Initializes the TextureData to default values. -void DefaultTextureData(TextureData* data); +void mjr_defaultTextureData(mjrTextureData* data); // Defines the basic properties of a texture. -struct TextureConfig { +struct mjrTextureConfig { // The width of the texture. For compressed textures (e.g. KTX), this is the // number of bytes in the compressed data. int width; @@ -67,17 +71,17 @@ struct TextureConfig { int height; // The target of the texture (e.g. 2D, cube, etc.) - mjtTexture target; + mjrTextureTarget target; // The format of the pixels in the texture (e.g. RGB8, RGBA8, KTX, etc.) - mjtPixelFormat format; + mjrPixelFormat format; // The color space of the texture (e.g. LINEAR, sRGB, etc.) - mjtColorSpace color_space; + mjrColorSpace color_space; }; // Initializes the TextureConfig to default values. -void DefaultTextureConfig(TextureConfig* config); +void mjr_defaultTextureConfig(mjrTextureConfig* config); // Wrapper around a filament::Texture. class Texture { @@ -90,13 +94,13 @@ class Texture { }; // Creates a texture with the given data. - Texture(filament::Engine* engine, const TextureConfig& config, + Texture(filament::Engine* engine, const mjrTextureConfig& config, InternalFlags flags = InternalFlags()); ~Texture(); // Uploads the given data to the texture. - void Upload(const TextureData& data); + void Upload(const mjrTextureData& data); // Returns the width of the texture. int GetWidth() const { return config_.width; } @@ -121,7 +125,7 @@ class Texture { filament::Engine* engine_ = nullptr; filament::Texture* texture_ = nullptr; - TextureConfig config_; + mjrTextureConfig config_; SphericalHarmonics spherical_harmonics_; bool has_spherical_harmonics_ = false; From 4345a2a09ff350e6a7e9a8f90fb939136522fc17 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 23 Apr 2026 07:14:13 -0700 Subject: [PATCH 123/251] Rename types to conform to mjr naming conventions. PiperOrigin-RevId: 904440645 Change-Id: I0b9a0c6bd61170ace1a3ea7fb54fb70b615c346b --- .../filament/filament/imgui_bridge.cc | 8 +++---- .../filament/filament/renderable.cc | 16 +++++++------- .../filament/filament/renderable.h | 22 +++++++++---------- .../filament/filament/scene_geom_util.cc | 14 ++++++------ 4 files changed, 30 insertions(+), 30 deletions(-) diff --git a/src/experimental/filament/filament/imgui_bridge.cc b/src/experimental/filament/filament/imgui_bridge.cc index 68519b87..2e533fad 100644 --- a/src/experimental/filament/filament/imgui_bridge.cc +++ b/src/experimental/filament/filament/imgui_bridge.cc @@ -276,11 +276,11 @@ void ImguiBridge::Update() { void ImguiBridge::PrepareRenderables(int count) { while (renderables_.size() < count) { - RenderableParams config; - DefaultRenderableParams(&config); - config.shading_model = ShadingModel::Ux; + mjrRenderableParams params; + mjr_defaultRenderableParams(¶ms); + params.shading_model = mjSHADING_MODEL_UX; auto& r = renderables_.emplace_back( - std::make_unique(object_mgr_, config)); + std::make_unique(object_mgr_, params)); r->SetCastShadows(false); r->SetReceiveShadows(false); r->SetBlendOrder(static_cast(renderables_.size())); diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index f1a4e2bc..93d66fe4 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -36,11 +36,11 @@ namespace mujoco { using filament::math::mat4f; -void DefaultRenderableParams(RenderableParams* params) { - params->shading_model = ShadingModel::SceneObject; +void mjr_defaultRenderableParams(mjrRenderableParams* params) { + params->shading_model = mjSHADING_MODEL_SCENE_OBJECT; } -Renderable::Renderable(ObjectManager* object_mgr, const RenderableParams& params) +Renderable::Renderable(ObjectManager* object_mgr, const mjrRenderableParams& params) : object_mgr_(object_mgr), params_(params) {} Renderable::~Renderable() noexcept { @@ -204,7 +204,7 @@ void Renderable::UpdateMaterial(const MaterialParams& params, material_textures_ = textures; AssignMaterial(DrawMode::Color, GetColorMaterialType()); - if (params_.shading_model == ShadingModel::SceneObject) { + if (params_.shading_model == mjSHADING_MODEL_SCENE_OBJECT) { AssignMaterial(DrawMode::Depth, ObjectManager::kUnlitDepth); AssignMaterial(DrawMode::Segmentation, ObjectManager::kUnlitSegmentation); } @@ -247,7 +247,7 @@ const MaterialTextures& Renderable::GetMaterialTextures() const { void Renderable::SetDrawMode(DrawMode mode) { // Only SceneObjects support non-color draw modes. - if (params_.shading_model != ShadingModel::SceneObject) { + if (params_.shading_model != mjSHADING_MODEL_SCENE_OBJECT) { mode = DrawMode::Color; } @@ -343,11 +343,11 @@ void Renderable::SetWireframe(bool wireframe) { } ObjectManager::MaterialType Renderable::GetColorMaterialType() const { - if (params_.shading_model == ShadingModel::DecorLines) { + if (params_.shading_model == mjSHADING_MODEL_DECOR_LINES) { return ObjectManager::kUnlitLine; - } else if (params_.shading_model == ShadingModel::Decor) { + } else if (params_.shading_model == mjSHADING_MODEL_DECOR) { return ObjectManager::kUnlitDecor; - } else if (params_.shading_model == ShadingModel::Ux) { + } else if (params_.shading_model == mjSHADING_MODEL_UX) { return ObjectManager::kUnlitUi; } else if (material_textures_.orm) { return ObjectManager::kPbrPacked; diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 9227e634..7e3fd732 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -33,19 +33,19 @@ namespace mujoco { // The shading model (material) for a Renderable. -enum class ShadingModel { - SceneObject, - Decor, - DecorLines, - Ux, -}; +typedef enum mjrShadingModel_ { + mjSHADING_MODEL_SCENE_OBJECT, + mjSHADING_MODEL_DECOR, + mjSHADING_MODEL_DECOR_LINES, + mjSHADING_MODEL_UX, +} mjrShadingModel; // Configuration parameters for a Renderable. -struct RenderableParams { - ShadingModel shading_model; +struct mjrRenderableParams { + mjrShadingModel shading_model; }; -void DefaultRenderableParams(RenderableParams* params); +void mjr_defaultRenderableParams(mjrRenderableParams* params); // A Renderable is effectively two things: a mesh and a material. // @@ -67,7 +67,7 @@ class Renderable { static constexpr std::uint8_t kDefaultPriority = 4; static constexpr std::uint8_t kDefaultLayerMask = 0x01; - Renderable(ObjectManager* object_mgr, const RenderableParams& params); + Renderable(ObjectManager* object_mgr, const mjrRenderableParams& params); ~Renderable() noexcept; Renderable(const Renderable&) = delete; @@ -154,7 +154,7 @@ class Renderable { ObjectManager::MaterialType GetColorMaterialType() const; ObjectManager* object_mgr_; - RenderableParams params_; + mjrRenderableParams params_; filament::MaterialInstance* instances_[kNumDrawModes] = {nullptr}; MaterialParams material_params_; MaterialTextures material_textures_; diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index 8db3d76e..c14987f3 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -490,17 +490,17 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, std::unique_ptr CreateGeomRenderable( const mjvGeom& geom, const mjvScene* scene, ObjectManager* object_mgr, ModelObjects* model_objs, const float headpos[3]) { - ShadingModel shading_model = ShadingModel::SceneObject; + mjrShadingModel shading_model = mjSHADING_MODEL_SCENE_OBJECT; if (geom.type == mjGEOM_LINE || geom.type == mjGEOM_LINEBOX) { - shading_model = ShadingModel::DecorLines; + shading_model = mjSHADING_MODEL_DECOR_LINES; } else if (geom.category == mjCAT_DECOR) { - shading_model = ShadingModel::Decor; + shading_model = mjSHADING_MODEL_DECOR; } - RenderableParams config; - DefaultRenderableParams(&config); - config.shading_model = shading_model; - auto renderable = std::make_unique(object_mgr, config); + mjrRenderableParams params; + mjr_defaultRenderableParams(¶ms); + params.shading_model = shading_model; + auto renderable = std::make_unique(object_mgr, params); PrepareGeomMeshes(*renderable, geom, scene, model_objs); UpdateGeomMaterial(*renderable, geom, scene, model_objs, object_mgr, headpos); From 3d13d43c9c707461e45fc4aedda27b98c937c70f Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 23 Apr 2026 07:53:08 -0700 Subject: [PATCH 124/251] Rename types to conform to mjr naming conventions. PiperOrigin-RevId: 904456797 Change-Id: I759b1430a43c7072ed9750d084679d02f0c9cb6e --- .../filament/filament/mjr_filament_renderer.cc | 8 ++++---- src/experimental/filament/filament/render_target.cc | 4 ++-- src/experimental/filament/filament/render_target.h | 8 ++++---- src/experimental/filament/filament/scene_view.cc | 4 ++-- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/experimental/filament/filament/mjr_filament_renderer.cc b/src/experimental/filament/filament/mjr_filament_renderer.cc index 5e1112c8..8ca1ebe6 100644 --- a/src/experimental/filament/filament/mjr_filament_renderer.cc +++ b/src/experimental/filament/filament/mjr_filament_renderer.cc @@ -130,8 +130,8 @@ void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, render_requests_[1].height = viewport.height; if (rgb) { - RenderTargetConfig config; - DefaultRenderTargetConfig(&config); + mjrRenderTargetConfig config; + mjr_defaultRenderTargetConfig(&config); config.color_format = mjPIXEL_FORMAT_RGB8; config.depth_format = mjPIXEL_FORMAT_DEPTH32F; auto target = std::make_unique(GetEngine(), config); @@ -154,8 +154,8 @@ void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, } if (depth) { - RenderTargetConfig config; - DefaultRenderTargetConfig(&config); + mjrRenderTargetConfig config; + mjr_defaultRenderTargetConfig(&config); config.color_format = mjPIXEL_FORMAT_R32F; config.depth_format = mjPIXEL_FORMAT_DEPTH32F; auto target = std::make_unique(GetEngine(), config); diff --git a/src/experimental/filament/filament/render_target.cc b/src/experimental/filament/filament/render_target.cc index 4ebd7adf..3e53ac51 100644 --- a/src/experimental/filament/filament/render_target.cc +++ b/src/experimental/filament/filament/render_target.cc @@ -30,13 +30,13 @@ namespace mujoco { -void DefaultRenderTargetConfig(RenderTargetConfig* config) { +void mjr_defaultRenderTargetConfig(mjrRenderTargetConfig* config) { config->color_format = mjPIXEL_FORMAT_RGBA8; config->depth_format = mjPIXEL_FORMAT_DEPTH32F; } RenderTarget::RenderTarget(filament::Engine* engine, - const RenderTargetConfig& config) + const mjrRenderTargetConfig& config) : engine_(engine), config_(config) {} RenderTarget::~RenderTarget() noexcept { diff --git a/src/experimental/filament/filament/render_target.h b/src/experimental/filament/filament/render_target.h index 1ab77c0d..b731a567 100644 --- a/src/experimental/filament/filament/render_target.h +++ b/src/experimental/filament/filament/render_target.h @@ -26,20 +26,20 @@ namespace mujoco { // Defines the basic properties of a render target. -struct RenderTargetConfig { +struct mjrRenderTargetConfig { mjrPixelFormat color_format; mjrPixelFormat depth_format; }; // Initializes the RenderTargetConfig to default values. -void DefaultRenderTargetConfig(RenderTargetConfig* config); +void mjr_defaultRenderTargetConfig(mjrRenderTargetConfig* config); // Manages a filament RenderTarget and the textures which are bound to it. class RenderTarget { public: // Defines the types of textures to create for the color and depth // attachments. - RenderTarget(filament::Engine* engine, const RenderTargetConfig& config); + RenderTarget(filament::Engine* engine, const mjrRenderTargetConfig& config); ~RenderTarget() noexcept; RenderTarget(const RenderTarget&) = delete; @@ -66,7 +66,7 @@ class RenderTarget { void Destroy(); filament::Engine* engine_ = nullptr; - RenderTargetConfig config_; + mjrRenderTargetConfig config_; filament::RenderTarget* render_target_ = nullptr; std::unique_ptr color_texture_ = nullptr; std::unique_ptr depth_texture_ = nullptr; diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index eb2bb47c..ee06e464 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -286,8 +286,8 @@ void SceneView::AddReflectiveRenderable(Renderable* renderable) { // Ensure we have the same number of render targets as we do reflective // renderables. while (reflect_targets_.size() < reflectives_.size()) { - RenderTargetConfig config; - DefaultRenderTargetConfig(&config); + mjrRenderTargetConfig config; + mjr_defaultRenderTargetConfig(&config); config.color_format = mjPIXEL_FORMAT_RGBA8; config.depth_format = mjPIXEL_FORMAT_DEPTH32F; From ead7b1f65ed90cca61669a76ff5e994389c5d269 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 23 Apr 2026 07:55:42 -0700 Subject: [PATCH 125/251] Rename types to conform to mjr naming conventions. PiperOrigin-RevId: 904457841 Change-Id: I023ffe87eee0a190006dea72ced8748725490098 --- src/experimental/filament/filament/light.cc | 29 ++++++++-- src/experimental/filament/filament/light.h | 56 ++++++++++--------- .../filament/filament/scene_bridge.cc | 29 ++++++---- 3 files changed, 72 insertions(+), 42 deletions(-) diff --git a/src/experimental/filament/filament/light.cc b/src/experimental/filament/filament/light.cc index d00f6097..bdc1f2cd 100644 --- a/src/experimental/filament/filament/light.cc +++ b/src/experimental/filament/filament/light.cc @@ -25,6 +25,7 @@ #include #include #include +#include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/texture.h" namespace mujoco { @@ -32,7 +33,22 @@ namespace mujoco { using filament::math::float3; using filament::math::mat3f; -Light::Light(filament::Engine* engine, const Params& params) +void mjr_defaultLightParams(mjrLightParams* params) { + params->type = mjLIGHT_POINT; + params->texture = nullptr; + params->color[0] = 0; + params->color[1] = 0; + params->color[2] = 0; + params->intensity = 0.0f; + params->cast_shadows = true; + params->range = 10.0f; + params->spot_cone_angle = 180.f; + params->bulb_radius = 0.0f; + params->shadow_map_size = 2048; + params->vsm_blur_width = 0.0f; +} + +Light::Light(filament::Engine* engine, const mjrLightParams& params) : engine_(engine), params_(params) { // Filament treats image-based lights (IBLs) as separate objects (i.e. // filament::IndirectLight) and so we need to handle IBLs specially. @@ -71,9 +87,9 @@ Light::Light(filament::Engine* engine, const Params& params) } filament::LightManager::Builder builder(type); - builder.color(params.color); + builder.color(ReadFloat3(params.color)); builder.intensityCandela(params.intensity); - builder.castShadows(params.castshadow); + builder.castShadows(params.cast_shadows); if (type == filament::LightManager::Type::FOCUSED_SPOT) { builder.spotLightCone(0, params.spot_cone_angle * std::numbers::pi / 180.0f); @@ -85,7 +101,7 @@ Light::Light(filament::Engine* engine, const Params& params) opts.mapSize = 4096; opts.shadowCascades = type == filament::LightManager::Type::DIRECTIONAL ? 4 : 1; - opts.shadowBulbRadius = params.bulbradius; + opts.shadowBulbRadius = params.bulb_radius; opts.mapSize = params.shadow_map_size; if (params.vsm_blur_width > 0.0f) { opts.vsm.elvsm = true; @@ -141,7 +157,10 @@ void Light::SetTransform(filament::math::float3 position, void Light::SetColor(const filament::math::float3& color) { if (!ibl_) { - params_.color = color; + params_.color[0] = color.r; + params_.color[1] = color.g; + params_.color[2] = color.b; + filament::LightManager& lm = engine_->getLightManager(); const filament::LightManager::Instance li = lm.getInstance(entity_); lm.setColor(li, color); diff --git a/src/experimental/filament/filament/light.h b/src/experimental/filament/filament/light.h index 93974972..93ad0bb8 100644 --- a/src/experimental/filament/filament/light.h +++ b/src/experimental/filament/filament/light.h @@ -24,34 +24,38 @@ namespace mujoco { +typedef mjtLightType mjrLightType; + +// Configuration parameters for a light. +struct mjrLightParams { + // The type of light (e.g. spot, point, directional, etc.) + mjrLightType type; + // The texture to use for image lights. + const Texture* texture; + // The color of the light. + float color[3]; + // The intensity of the light, in candela. + float intensity; + // Whether or not the light casts shadows. + mjtByte cast_shadows; + // The range/distance in which the light is effective, in meters. + float range; + // The angle of the spot light cone, in degrees. + float spot_cone_angle; + // The radius of the bulb used for soft shadows. + float bulb_radius; + // The size of the shadow map. + int shadow_map_size; + // Blur width for EL VSM. + float vsm_blur_width; +}; + +void mjr_defaultLightParams(mjrLightParams* params); + // Manages the filament Entities for a single mjvLight. class Light { public: - // Configuration parameters for a light. - struct Params { - // The type of light (e.g. spot, point, directional, etc.) - mjtLightType type; - // The texture to use for image lights. - const Texture* texture = nullptr; - // The color of the light. - filament::math::float3 color = {0, 0, 0}; - // The intensity of the light, in candela. - float intensity = 0.0f; - // Whether or not the light casts shadows. - bool castshadow = true; - // The range/distance in which the light is effective, in meters. - float range = 10.0f; - // The angle of the spot light cone, in degrees. - float spot_cone_angle = 180.f; - // The radius of the bulb used for soft shadows. - float bulbradius = 0.0f; - // The size of the shadow map. - int shadow_map_size = 2048; - // Blur width for EL VSM. - float vsm_blur_width = 0.0f; - }; - - Light(filament::Engine* engine, const Params& params); + Light(filament::Engine* engine, const mjrLightParams& params); ~Light() noexcept; Light(const Light&) = delete; @@ -85,7 +89,7 @@ class Light { filament::IndirectLight* ibl_ = nullptr; utils::Entity entity_; bool enabled_ = true; - Params params_; + mjrLightParams params_; }; } // namespace mujoco diff --git a/src/experimental/filament/filament/scene_bridge.cc b/src/experimental/filament/filament/scene_bridge.cc index ad970247..fd62b392 100644 --- a/src/experimental/filament/filament/scene_bridge.cc +++ b/src/experimental/filament/filament/scene_bridge.cc @@ -207,7 +207,8 @@ void SceneBridge::SetEnvironmentLight(std::string_view filename, fallback_ibl_texture_ = CreateFallbackIndirectLightTexture(object_mgr_, filename); - Light::Params params; + mjrLightParams params; + mjr_defaultLightParams(¶ms); params.type = mjLIGHT_IMAGE; params.texture = fallback_ibl_texture_.get(); params.intensity = intensity; @@ -233,7 +234,8 @@ void SceneBridge::PrepareLights() { total_light_intensity += model->light_intensity[i]; if (model->light_type[i] == mjLIGHT_IMAGE) { - Light::Params params; + mjrLightParams params; + mjr_defaultLightParams(¶ms); params.type = mjLIGHT_IMAGE; params.texture = model_objects_->GetTexture(model->light_texid[i]); params.intensity = model->light_intensity[i]; @@ -242,11 +244,14 @@ void SceneBridge::PrepareLights() { lights_.emplace_back(std::move(light_obj)); has_image_based_light = true; } else { - Light::Params params; - params.color = ReadFloat3(model->light_diffuse); + mjrLightParams params; + mjr_defaultLightParams(¶ms); + params.color[0] = model->light_diffuse[0]; + params.color[1] = model->light_diffuse[1]; + params.color[2] = model->light_diffuse[2]; params.type = (mjtLightType)model->light_type[i]; - params.castshadow = model->light_castshadow[i]; - params.bulbradius = model->light_bulbradius[i]; + params.cast_shadows = model->light_castshadow[i]; + params.bulb_radius = model->light_bulbradius[i]; params.range = model->light_range[i]; params.intensity = model->light_intensity[i]; params.shadow_map_size = default_shadow_map_size_; @@ -267,15 +272,15 @@ void SceneBridge::PrepareLights() { // Add a placeholder (black) headlight as our last light. Going forward, we'll // assume lights_.back() is always the headlight. { - Light::Params params; - params.color = float3(0, 0, 0); + mjrLightParams params; + mjr_defaultLightParams(¶ms); // We break with the spec here slightly and use a spot light for the head // light instead of a directional params. This is because filament only // supports a single directional light, and we'd rather allow a scene // light to be that directional params. It's also a bit odd for a // directional light to move with the camera. params.type = mjLIGHT_SPOT; - params.castshadow = 0; + params.cast_shadows = 0; params.intensity = 0.0f; params.spot_cone_angle = 90.0f; auto light_obj = std::make_unique(engine, params); @@ -290,7 +295,8 @@ void SceneBridge::PrepareLights() { // Create a black indirect light to ensure that the skybox is // oriented to respect mujoco's Z-up convention. filament::Engine* engine = object_mgr_->GetEngine(); - Light::Params params; + mjrLightParams params; + mjr_defaultLightParams(¶ms); params.type = mjLIGHT_IMAGE; params.intensity = 10.0f; fallback_ibl_ = std::make_unique(engine, params); @@ -304,7 +310,8 @@ void SceneBridge::PrepareLights() { // Create a fallback environment light. fallback_ibl_texture_ = CreateFallbackIndirectLightTexture(object_mgr_); - Light::Params params; + mjrLightParams params; + mjr_defaultLightParams(¶ms); params.type = mjLIGHT_IMAGE; params.texture = fallback_ibl_texture_.get(); params.intensity = fallback_environment_light_intensity_; From 2bc97d840876cd3d955d705ea4718861ce365917 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 23 Apr 2026 07:58:32 -0700 Subject: [PATCH 126/251] Rename types to conform to mjr naming conventions. PiperOrigin-RevId: 904458866 Change-Id: Id9ca27b698e0e40469524ba4454ab9721a65e785 --- .../filament/filament/builtins.cc | 18 ++--- .../filament/filament/imgui_bridge.cc | 12 +-- src/experimental/filament/filament/mesh.cc | 69 ++++++++--------- src/experimental/filament/filament/mesh.h | 74 +++++++++---------- .../filament/filament/model_objects.cc | 44 +++++------ 5 files changed, 109 insertions(+), 108 deletions(-) diff --git a/src/experimental/filament/filament/builtins.cc b/src/experimental/filament/filament/builtins.cc index 86a6a5c5..fc7b8c23 100644 --- a/src/experimental/filament/filament/builtins.cc +++ b/src/experimental/filament/filament/builtins.cc @@ -59,16 +59,16 @@ static std::size_t NumIndicesPerSide(int num_quads_per_axis) { return kNumIndicesPerQuad * num_quads_per_axis * num_quads_per_axis; } -class BuiltinBuilder : MeshData { +class BuiltinBuilder : mjrMeshData { public: - BuiltinBuilder() { DefaultMeshData(this); } + BuiltinBuilder() { mjr_defaultMeshData(this); } virtual ~BuiltinBuilder() = default; template static std::unique_ptr Create(filament::Engine* engine, Args&&... args) { auto builder = new T(std::forward(args)...); - MeshData* mesh_data = builder->PrepareMeshData(); + mjrMeshData* mesh_data = builder->PrepareMeshData(); mesh_data->release_callback = +[](void* user_data) { delete static_cast(user_data); }; @@ -76,13 +76,13 @@ class BuiltinBuilder : MeshData { return std::make_unique(engine, *mesh_data); } - MeshData* PrepareMeshData() { - // Update the `MeshData` fields. + mjrMeshData* PrepareMeshData() { + // Update the `mjrMeshData` fields. nattributes = 2; - attributes[0].usage = mjVERTEX_ATTRIBUTE_POSITION; + attributes[0].usage = mjVERTEX_ATTRIBUTE_USAGE_POSITION; attributes[0].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT3; attributes[0].bytes = reinterpret_cast(positions_.data()); - attributes[1].usage = mjVERTEX_ATTRIBUTE_TANGENTS; + attributes[1].usage = mjVERTEX_ATTRIBUTE_USAGE_TANGENTS; attributes[1].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT4; attributes[1].bytes = reinterpret_cast(orientations_.data()); nvertices = positions_.size(); @@ -91,8 +91,8 @@ class BuiltinBuilder : MeshData { nindices = indices_.size(); primitive_type = primitive_type_ == filament::backend::PrimitiveType::TRIANGLES - ? mjPRIM_TYPE_TRIANGLES - : mjPRIM_TYPE_LINES; + ? mjMESH_PRIMITIVE_TYPE_TRIANGLES + : mjMESH_PRIMITIVE_TYPE_LINES; index_type = mjINDEX_TYPE_USHORT; bounds_min[0] = bounds_.getMin().x; bounds_min[1] = bounds_.getMin().y; diff --git a/src/experimental/filament/filament/imgui_bridge.cc b/src/experimental/filament/filament/imgui_bridge.cc index 2e533fad..86b440f5 100644 --- a/src/experimental/filament/filament/imgui_bridge.cc +++ b/src/experimental/filament/filament/imgui_bridge.cc @@ -217,16 +217,16 @@ void ImguiBridge::Update() { for (int n = 0; n < commands->CmdListsCount; ++n) { const ImDrawList* cmds = commands->CmdLists[n]; - MeshData data; - DefaultMeshData(&data); + mjrMeshData data; + mjr_defaultMeshData(&data); data.nattributes = 3; - data.attributes[0].usage = mjVERTEX_ATTRIBUTE_POSITION; + data.attributes[0].usage = mjVERTEX_ATTRIBUTE_USAGE_POSITION; data.attributes[0].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT2; data.attributes[0].bytes = cmds->VtxBuffer.Data; - data.attributes[1].usage = mjVERTEX_ATTRIBUTE_UV; + data.attributes[1].usage = mjVERTEX_ATTRIBUTE_USAGE_UV; data.attributes[1].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT2; data.attributes[1].bytes = cmds->VtxBuffer.Data + sizeof(float) * 2; - data.attributes[2].usage = mjVERTEX_ATTRIBUTE_COLOR; + data.attributes[2].usage = mjVERTEX_ATTRIBUTE_USAGE_COLOR; data.attributes[2].type = mjVERTEX_ATTRIBUTE_TYPE_UBYTE4; data.attributes[2].bytes = cmds->VtxBuffer.Data + sizeof(float) * 4; data.interleaved = true; @@ -234,7 +234,7 @@ void ImguiBridge::Update() { data.nindices = cmds->IdxBuffer.Size; data.indices = cmds->IdxBuffer.Data; data.index_type = mjINDEX_TYPE_USHORT; - data.primitive_type = mjPRIM_TYPE_TRIANGLES; + data.primitive_type = mjMESH_PRIMITIVE_TYPE_TRIANGLES; meshes_.push_back(std::make_unique(scene_view_->GetEngine(), data)); const Mesh* mesh = meshes_.back().get(); diff --git a/src/experimental/filament/filament/mesh.cc b/src/experimental/filament/filament/mesh.cc index 0736cbd9..1e958658 100644 --- a/src/experimental/filament/filament/mesh.cc +++ b/src/experimental/filament/filament/mesh.cc @@ -37,17 +37,17 @@ namespace mujoco { using filament::math::float3; using filament::math::float4; -static filament::VertexAttribute GetUsage(const VertexAttribute& attrib) { +static filament::VertexAttribute GetUsage(const mjrVertexAttribute& attrib) { switch (attrib.usage) { - case mjVERTEX_ATTRIBUTE_POSITION: + case mjVERTEX_ATTRIBUTE_USAGE_POSITION: return filament::VertexAttribute::POSITION; - case mjVERTEX_ATTRIBUTE_NORMAL: + case mjVERTEX_ATTRIBUTE_USAGE_NORMAL: return filament::VertexAttribute::TANGENTS; - case mjVERTEX_ATTRIBUTE_TANGENTS: + case mjVERTEX_ATTRIBUTE_USAGE_TANGENTS: return filament::VertexAttribute::TANGENTS; - case mjVERTEX_ATTRIBUTE_UV: + case mjVERTEX_ATTRIBUTE_USAGE_UV: return filament::VertexAttribute::UV0; - case mjVERTEX_ATTRIBUTE_COLOR: + case mjVERTEX_ATTRIBUTE_USAGE_COLOR: return filament::VertexAttribute::COLOR; default: mju_error("Unsupported vertex attribute usage: %d", attrib.usage); @@ -56,7 +56,7 @@ static filament::VertexAttribute GetUsage(const VertexAttribute& attrib) { } static filament::VertexBuffer::AttributeType GetType( - const VertexAttribute& attrib) { + const mjrVertexAttribute& attrib) { switch (attrib.type) { case mjVERTEX_ATTRIBUTE_TYPE_FLOAT2: return filament::VertexBuffer::AttributeType::FLOAT2; @@ -72,7 +72,7 @@ static filament::VertexBuffer::AttributeType GetType( } } -int VertexAttributeTypeSize(const VertexAttribute& attrib) { +int VertexAttributeTypeSize(const mjrVertexAttribute& attrib) { switch (attrib.type) { case mjVERTEX_ATTRIBUTE_TYPE_FLOAT2: return sizeof(float) * 2; @@ -99,14 +99,14 @@ int FillSequence(std::byte* buffer, std::size_t num_bytes) { return num; } -// Initializes the MeshData to default values. -void DefaultMeshData(MeshData* data) { - std::memset(data, 0, sizeof(MeshData)); +// Initializes the mjrMeshData to default values. +void mjr_defaultMeshData(mjrMeshData* data) { + std::memset(data, 0, sizeof(mjrMeshData)); } -Mesh::Mesh(filament::Engine* engine, const MeshData& data) +Mesh::Mesh(filament::Engine* engine, const mjrMeshData& data) : engine_(engine) { - type_ = data.primitive_type == mjPRIM_TYPE_TRIANGLES + type_ = data.primitive_type == mjMESH_PRIMITIVE_TYPE_TRIANGLES ? filament::RenderableManager::PrimitiveType::TRIANGLES : filament::RenderableManager::PrimitiveType::LINES; @@ -133,9 +133,9 @@ Mesh::~Mesh() { } } -void Mesh::BuildVertexBuffer(const MeshData& data) { +void Mesh::BuildVertexBuffer(const mjrMeshData& data) { if (data.nvertices == 0) { - mju_error("MeshData has no vertices."); + mju_error("mjrMeshData has no vertices."); } // The filament BufferDescriptor callback for releasing the memory. We assume @@ -147,26 +147,26 @@ void Mesh::BuildVertexBuffer(const MeshData& data) { // Pointers to specific attributes in the mesh data, used for additional // validation and processing. - const VertexAttribute* positions = nullptr; - const VertexAttribute* normals = nullptr; - const VertexAttribute* tangents = nullptr; + const mjrVertexAttribute* positions = nullptr; + const mjrVertexAttribute* normals = nullptr; + const mjrVertexAttribute* tangents = nullptr; for (int i = 0; i < data.nattributes; ++i) { - if (data.attributes[i].usage == mjVERTEX_ATTRIBUTE_POSITION) { + if (data.attributes[i].usage == mjVERTEX_ATTRIBUTE_USAGE_POSITION) { positions = &data.attributes[i]; - } else if (data.attributes[i].usage == mjVERTEX_ATTRIBUTE_NORMAL) { + } else if (data.attributes[i].usage == mjVERTEX_ATTRIBUTE_USAGE_NORMAL) { normals = &data.attributes[i]; - } else if (data.attributes[i].usage == mjVERTEX_ATTRIBUTE_TANGENTS) { + } else if (data.attributes[i].usage == mjVERTEX_ATTRIBUTE_USAGE_TANGENTS) { tangents = &data.attributes[i]; } } if (!positions) { - mju_error("MeshData has no positions."); + mju_error("mjrMeshData has no positions."); } - if (data.attributes[0].usage != mjVERTEX_ATTRIBUTE_POSITION) { + if (data.attributes[0].usage != mjVERTEX_ATTRIBUTE_USAGE_POSITION) { mju_error("Positions must be the first attribute."); } if (normals && tangents) { - mju_error("MeshData has both normals and tangents."); + mju_error("mjrMeshData has both normals and tangents."); } if (normals && data.interleaved) { // We need to build orientations from normals and so we require each @@ -195,7 +195,7 @@ void Mesh::BuildVertexBuffer(const MeshData& data) { // each offset is the sum of the sizes of the preceding attributes. int offset = 0; for (int i = 0; i < data.nattributes; ++i) { - const VertexAttribute& attrib = data.attributes[i]; + const mjrVertexAttribute& attrib = data.attributes[i]; const filament::VertexAttribute usage = GetUsage(attrib); filament::VertexBuffer::AttributeType type = GetType(attrib); vb_builder.attribute(usage, 0, type, offset, total_vertex_size); @@ -212,10 +212,10 @@ void Mesh::BuildVertexBuffer(const MeshData& data) { // attribute. vb_builder.bufferCount(data.nattributes); for (int i = 0; i < data.nattributes; ++i) { - const VertexAttribute& attrib = data.attributes[i]; + const mjrVertexAttribute& attrib = data.attributes[i]; const filament::VertexAttribute usage = GetUsage(attrib); filament::VertexBuffer::AttributeType type = GetType(attrib); - if (attrib.usage == mjVERTEX_ATTRIBUTE_NORMAL) { + if (attrib.usage == mjVERTEX_ATTRIBUTE_USAGE_NORMAL) { // We will replace normals with orientations. type = filament::VertexBuffer::AttributeType::FLOAT4; } @@ -230,10 +230,10 @@ void Mesh::BuildVertexBuffer(const MeshData& data) { // Assign the individual data buffers. for (int i = 0; i < data.nattributes; ++i) { - const VertexAttribute& attrib = data.attributes[i]; + const mjrVertexAttribute& attrib = data.attributes[i]; const void* bytes = attrib.bytes; size_t nbytes = data.nvertices * VertexAttributeTypeSize(attrib); - if (attrib.usage == mjVERTEX_ATTRIBUTE_NORMAL) { + if (attrib.usage == mjVERTEX_ATTRIBUTE_USAGE_NORMAL) { // Replace normals with orientations. nbytes = data.nvertices * sizeof(float4); bytes = BuildOrientationsFromNormals(data.nvertices, attrib); @@ -243,7 +243,7 @@ void Mesh::BuildVertexBuffer(const MeshData& data) { } } -void Mesh::BuildIndexBuffer(const MeshData& data) { +void Mesh::BuildIndexBuffer(const mjrMeshData& data) { if (data.nindices == 0) { return; } @@ -283,7 +283,8 @@ void Mesh::BuildIndexBuffer(const MeshData& data) { index_buffer_->setBuffer(*engine_, std::move(desc)); } -float4* Mesh::BuildOrientationsFromNormals(int nvertices, const VertexAttribute& normals) { +float4* Mesh::BuildOrientationsFromNormals(int nvertices, + const mjrVertexAttribute& normals) { float4* orientations = new float4[nvertices]; release_callbacks_.push_back([=]() { delete[] orientations; @@ -295,7 +296,7 @@ float4* Mesh::BuildOrientationsFromNormals(int nvertices, const VertexAttribute& return orientations; } -void Mesh::UpdateBounds(const MeshData& data) { +void Mesh::UpdateBounds(const mjrMeshData& data) { float3 bounds_min = ReadFloat3(data.bounds_min); float3 bounds_max = ReadFloat3(data.bounds_max); if (bounds_min != bounds_max) { @@ -304,8 +305,8 @@ void Mesh::UpdateBounds(const MeshData& data) { bounds_min = float3(FLT_MAX, FLT_MAX, FLT_MAX); bounds_max = float3(-FLT_MAX, -FLT_MAX, -FLT_MAX); - if (data.attributes[0].usage != mjVERTEX_ATTRIBUTE_POSITION) { - mju_error("MeshData has no positions."); + if (data.attributes[0].usage != mjVERTEX_ATTRIBUTE_USAGE_POSITION) { + mju_error("mjrMeshData has no positions."); } const float* positions = reinterpret_cast(data.attributes[0].bytes); diff --git a/src/experimental/filament/filament/mesh.h b/src/experimental/filament/filament/mesh.h index b75ad6de..ab259bc4 100644 --- a/src/experimental/filament/filament/mesh.h +++ b/src/experimental/filament/filament/mesh.h @@ -18,7 +18,6 @@ #include #include #include -#include #include #include #include @@ -29,66 +28,67 @@ #include #include #include +#include // Functions for creating filament vertex and index buffers. namespace mujoco { -// Maximum number of vertex attributes that can be used by a mesh. -static constexpr int kMaxVertexAttributes = 16; - // The type of data stored in an index buffer. -typedef enum mjtIndexType_ { +typedef enum mjrIndexType_ { mjINDEX_TYPE_USHORT = 0, mjINDEX_TYPE_UINT = 1, -} mjtIndexType; +} mjrIndexType; // The type of primitive to be drawn by vertex data. -typedef enum mjtMeshPrimitiveType_ { - mjPRIM_TYPE_TRIANGLES = 0, - mjPRIM_TYPE_LINES = 1, -} mjtMeshPrimitiveType; +typedef enum mjrMeshPrimitiveType_ { + mjMESH_PRIMITIVE_TYPE_TRIANGLES = 0, + mjMESH_PRIMITIVE_TYPE_LINES = 1, +} mjrMeshPrimitiveType; // The usage/purpose of an attribute of a vertex. -typedef enum mjtVertexAttributeUsage_ { - mjVERTEX_ATTRIBUTE_POSITION = 0, - mjVERTEX_ATTRIBUTE_NORMAL = 1, - mjVERTEX_ATTRIBUTE_TANGENTS = 2, - mjVERTEX_ATTRIBUTE_UV = 3, - mjVERTEX_ATTRIBUTE_COLOR = 4, -} mjtVertexAttributeUsage; +typedef enum mjrVertexAttributeUsage_ { + mjVERTEX_ATTRIBUTE_USAGE_POSITION = 0, + mjVERTEX_ATTRIBUTE_USAGE_NORMAL = 1, + mjVERTEX_ATTRIBUTE_USAGE_TANGENTS = 2, + mjVERTEX_ATTRIBUTE_USAGE_UV = 3, + mjVERTEX_ATTRIBUTE_USAGE_COLOR = 4, +} mjrVertexAttributeUsage; // The data format of an attribute of a vertex. -typedef enum mjtVertexAttributeType_ { +typedef enum mjrVertexAttributeType_ { mjVERTEX_ATTRIBUTE_TYPE_FLOAT2 = 0, mjVERTEX_ATTRIBUTE_TYPE_FLOAT3 = 1, mjVERTEX_ATTRIBUTE_TYPE_FLOAT4 = 2, mjVERTEX_ATTRIBUTE_TYPE_UBYTE4 = 3, -} mjtVertexAttributeType; +} mjrVertexAttributeType; + +// Maximum number of vertex attributes that can be used by a mesh. +enum { mjMAX_VERTEX_ATTRIBUTES = 16 }; // Information about a single attribute of a vertex. -struct VertexAttribute { +struct mjrVertexAttribute { // The data for the attribute. const void* bytes; // The usage/purpose of the attribute. - mjtVertexAttributeUsage usage; + mjrVertexAttributeUsage usage; // The data format of the attribute. - mjtVertexAttributeType type; + mjrVertexAttributeType type; }; // The binary contents of a mesh. -struct MeshData { +struct mjrMeshData { // The number of vertices in the mesh. Each of the vertex arrays below is // assumed to have this number of elements. - size_t nvertices; + mjtSize nvertices; // The number of attributes for each vertex in the mesh. int nattributes; // Information about each attribute of a vertex in the mesh. See `interleaved` // for more details. - VertexAttribute attributes[kMaxVertexAttributes]; + mjrVertexAttribute attributes[mjMAX_VERTEX_ATTRIBUTES]; // Whether the vertex attributes are interleaved or not. // @@ -99,24 +99,24 @@ struct MeshData { // // If false, assume each attribute is stored in a separate array as defined // by the `data` field of the attribute. - bool interleaved; + mjtByte interleaved; // The number of indices in the mesh. The indices array is assumed to have // this number of elements. - size_t nindices; + mjtSize nindices; // The indices of the mesh, stored as either ushort or uint depending on the // index type. const void* indices; // The type of data stored in the indices array. - mjtIndexType index_type; + mjrIndexType index_type; // The type of primitive to be drawn by vertex data. - mjtMeshPrimitiveType primitive_type; + mjrMeshPrimitiveType primitive_type; // Whether to compute the bounds of the mesh using the vertex positions. - bool compute_bounds; + mjtByte compute_bounds; // The bounds of the mesh. If bounds_min == bounds_max, then we assume that // that the bounds are not set (i.e. the bounds is empty). @@ -133,13 +133,13 @@ struct MeshData { }; // Initializes the MeshData to default values. -void DefaultMeshData(MeshData* data); +void mjr_defaultMeshData(mjrMeshData* data); // Owns a Vertex and Index buffer representing a geometry mesh. class Mesh { public: // Creates a Mesh from the given MeshData. - Mesh(filament::Engine* engine, const MeshData& data); + Mesh(filament::Engine* engine, const mjrMeshData& data); ~Mesh(); @@ -165,12 +165,12 @@ class Mesh { Mesh& operator=(const Mesh&) = delete; private: - void BuildVertexBuffer(const MeshData& data); - void BuildIndexBuffer(const MeshData& data); - void UpdateBounds(const MeshData& data); + void BuildVertexBuffer(const mjrMeshData& data); + void BuildIndexBuffer(const mjrMeshData& data); + void UpdateBounds(const mjrMeshData& data); filament::math::float4* BuildOrientationsFromNormals( - int nvertices, const VertexAttribute& normals); + int nvertices, const mjrVertexAttribute& normals); void ReleaseResources(); @@ -181,7 +181,7 @@ class Mesh { filament::RenderableManager::PrimitiveType::TRIANGLES; std::optional bounds_; std::vector> release_callbacks_; - std::array attributes_; + std::array attributes_; int num_attributes_ = 0; }; diff --git a/src/experimental/filament/filament/model_objects.cc b/src/experimental/filament/filament/model_objects.cc index 931670e0..742c3609 100644 --- a/src/experimental/filament/filament/model_objects.cc +++ b/src/experimental/filament/filament/model_objects.cc @@ -412,7 +412,7 @@ static std::span GetIndices(const mjModel* model, } } -static void UpdateMeshData(MeshData* data, const mjModel* model, int id, +static void UpdatemjrMeshData(mjrMeshData* data, const mjModel* model, int id, MeshType mesh_type) { if (!IsValidIndex(model, id, mesh_type)) { mju_error("Invalid index %d for type %d", id, mesh_type); @@ -440,7 +440,7 @@ static void UpdateMeshData(MeshData* data, const mjModel* model, int id, break; } - data->primitive_type = mjPRIM_TYPE_TRIANGLES; + data->primitive_type = mjMESH_PRIMITIVE_TYPE_TRIANGLES; data->nvertices = num_vertices; data->nindices = data->nvertices; data->indices = nullptr; @@ -448,14 +448,14 @@ static void UpdateMeshData(MeshData* data, const mjModel* model, int id, ? mjINDEX_TYPE_UINT : mjINDEX_TYPE_USHORT; data->nattributes = has_uvs ? 3 : 2; - data->attributes[0].usage = mjVERTEX_ATTRIBUTE_POSITION; + data->attributes[0].usage = mjVERTEX_ATTRIBUTE_USAGE_POSITION; data->attributes[0].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT3; data->attributes[0].bytes = builder->positions.data(); - data->attributes[1].usage = mjVERTEX_ATTRIBUTE_TANGENTS; + data->attributes[1].usage = mjVERTEX_ATTRIBUTE_USAGE_TANGENTS; data->attributes[1].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT4; data->attributes[1].bytes = builder->orientations.data(); if (has_uvs) { - data->attributes[2].usage = mjVERTEX_ATTRIBUTE_UV; + data->attributes[2].usage = mjVERTEX_ATTRIBUTE_USAGE_UV; data->attributes[2].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT2; data->attributes[2].bytes = builder->uvs.data(); } @@ -467,7 +467,7 @@ static void UpdateMeshData(MeshData* data, const mjModel* model, int id, data->bounds_max[2] = builder->bounds_max.z; } -void UpdateSkinFlexMeshData(MeshData* data, const mjModel* model, +void UpdateSkinFlexmjrMeshData(mjrMeshData* data, const mjModel* model, const mjvScene* scene, const mjvGeom& geom) { auto positions = GetPositions(model, scene, geom); auto normals = GetNormals(model, scene, geom); @@ -480,20 +480,20 @@ void UpdateSkinFlexMeshData(MeshData* data, const mjModel* model, } data->nattributes = uvs.data() ? 3 : 2; - data->attributes[0].usage = mjVERTEX_ATTRIBUTE_POSITION; + data->attributes[0].usage = mjVERTEX_ATTRIBUTE_USAGE_POSITION; data->attributes[0].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT3; data->attributes[0].bytes = positions.data(); - data->attributes[1].usage = mjVERTEX_ATTRIBUTE_NORMAL; + data->attributes[1].usage = mjVERTEX_ATTRIBUTE_USAGE_NORMAL; data->attributes[1].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT3; data->attributes[1].bytes = normals.data(); - data->attributes[2].usage = mjVERTEX_ATTRIBUTE_UV; + data->attributes[2].usage = mjVERTEX_ATTRIBUTE_USAGE_UV; data->attributes[2].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT2; data->attributes[2].bytes = uvs.data(); data->nvertices = positions.size() / 3; data->nindices = num_indices; data->indices = indices.data(); data->index_type = mjINDEX_TYPE_UINT; - data->primitive_type = mjPRIM_TYPE_TRIANGLES; + data->primitive_type = mjMESH_PRIMITIVE_TYPE_TRIANGLES; data->compute_bounds = true; data->release_callback = nullptr; data->user_data = nullptr; @@ -554,15 +554,15 @@ void ModelObjects::UploadMesh(const mjModel* model, int id) { meshes_.erase(id); convex_hulls_.erase(id); - MeshData data; - DefaultMeshData(&data); - UpdateMeshData(&data, model, id, MeshType::kNormal); + mjrMeshData data; + mjr_defaultMeshData(&data); + UpdatemjrMeshData(&data, model, id, MeshType::kNormal); meshes_[id] = std::make_unique(engine_, data); if (model->mesh_graphadr[id] >= 0) { - MeshData convex_hull_data; - DefaultMeshData(&convex_hull_data); - UpdateMeshData(&convex_hull_data, model, id, MeshType::kConvexHull); + mjrMeshData convex_hull_data; + mjr_defaultMeshData(&convex_hull_data); + UpdatemjrMeshData(&convex_hull_data, model, id, MeshType::kConvexHull); convex_hulls_[id] = std::make_unique(engine_, convex_hull_data); } } @@ -624,16 +624,16 @@ void ModelObjects::UploadHeightField(const mjModel* model, int id) { height_fields_.erase(id); - MeshData data; - DefaultMeshData(&data); - UpdateMeshData(&data, model, id, MeshType::kHeightField); + mjrMeshData data; + mjr_defaultMeshData(&data); + UpdatemjrMeshData(&data, model, id, MeshType::kHeightField); height_fields_[id] = std::make_unique(engine_, data); } void ModelObjects::CreateSkinFlexMesh(const mjvScene* scene, const mjvGeom& geom) { - MeshData data; - DefaultMeshData(&data); - UpdateSkinFlexMeshData(&data, model_, scene, geom); + mjrMeshData data; + mjr_defaultMeshData(&data); + UpdateSkinFlexmjrMeshData(&data, model_, scene, geom); dynamic_meshes_[geom.objid] = std::make_unique(engine_, data); } From 8a87a1efbfbdd43fd500805ace46f8eed9603270 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 23 Apr 2026 08:01:03 -0700 Subject: [PATCH 127/251] Rename types to conform to mjr naming conventions. PiperOrigin-RevId: 904459745 Change-Id: I9369bec34e72dc973df45f844fcf1a6cb08b26b1 --- .../filament/filament/imgui_bridge.cc | 6 +- .../filament/filament/material.cc | 51 ++++++++++++++-- src/experimental/filament/filament/material.h | 60 ++++++++++--------- .../filament/filament/renderable.cc | 21 ++++--- .../filament/filament/renderable.h | 12 ++-- .../filament/filament/scene_geom_util.cc | 56 +++++++++-------- .../filament/filament/scene_view.cc | 6 +- 7 files changed, 132 insertions(+), 80 deletions(-) diff --git a/src/experimental/filament/filament/imgui_bridge.cc b/src/experimental/filament/filament/imgui_bridge.cc index 86b440f5..7a645be6 100644 --- a/src/experimental/filament/filament/imgui_bridge.cc +++ b/src/experimental/filament/filament/imgui_bridge.cc @@ -247,10 +247,12 @@ void ImguiBridge::Update() { auto& renderable = renderables_[renderable_index]; renderable->SetMesh(mesh, index_offset, command.ElemCount); - MaterialTextures textures; + mjrMaterialTextures textures; + mjr_defaultMaterialTextures(&textures); textures.color = textures_[command.GetTexID()].get(); - MaterialParams properties; + mjrMaterialParams properties; + mjr_defaultMaterialParams(&properties); properties.scissor[0] = command.ClipRect.x; properties.scissor[1] = height - command.ClipRect.w; properties.scissor[2] = command.ClipRect.z - command.ClipRect.x; diff --git a/src/experimental/filament/filament/material.cc b/src/experimental/filament/filament/material.cc index 6522bd31..a5402dde 100644 --- a/src/experimental/filament/filament/material.cc +++ b/src/experimental/filament/filament/material.cc @@ -14,20 +14,59 @@ #include "experimental/filament/filament/material.h" +#include + #include #include #include #include #include #include +#include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/filament/object_manager.h" namespace mujoco { +template +static void setf(float (&arr)[N], const std::array& values) { + for (int i = 0; i < N; ++i) { + arr[i] = values[i]; + } +} + +void mjr_defaultMaterialTextures(mjrMaterialTextures* textures) { + textures->color = nullptr; + textures->normal = nullptr; + textures->metallic = nullptr; + textures->roughness = nullptr; + textures->occlusion = nullptr; + textures->orm = nullptr; + textures->emissive = nullptr; + textures->reflection = nullptr; +} + +void mjr_defaultMaterialParams(mjrMaterialParams* params) { + setf(params->color, {1.f, 1.f, 1.f, 1.f}); + setf(params->segmentation_color, {1, 1, 1, 1}); + setf(params->uv_scale, {1, 1}); + setf(params->uv_offset, {0, 0}); + setf(params->scissor, {0, 0, 0, 0}); + + params->emissive = -1.0f; + params->specular = -1.0f; + params->glossiness = -1.0f; + params->metallic = -1.0f; + params->roughness = -1.0f; + params->reflectance = 0.0f; + params->tex_uniform = false; + params->reflective = false; +} + + void UpdateMaterialInstance(filament::MaterialInstance* instance, - const MaterialParams& params, - const MaterialTextures& textures, + const mjrMaterialParams& params, + const mjrMaterialTextures& textures, ObjectManager* object_mgr) { if (params.scissor[2] != 0 && params.scissor[3] != 0) { instance->setScissor(params.scissor[0], params.scissor[1], @@ -37,11 +76,11 @@ void UpdateMaterialInstance(filament::MaterialInstance* instance, const filament::Material* material = instance->getMaterial(); if (material->hasParameter("BaseColorFactor")) { instance->setParameter("BaseColorFactor", filament::RgbaType::sRGB, - params.color); + ReadFloat4(params.color)); } if (material->hasParameter("SegmentationColor")) { instance->setParameter("SegmentationColor", filament::RgbaType::LINEAR, - params.segmentation_color); + ReadFloat4(params.segmentation_color)); } if (material->hasParameter("EmissiveFactor")) { instance->setParameter("EmissiveFactor", params.emissive); @@ -61,10 +100,10 @@ void UpdateMaterialInstance(filament::MaterialInstance* instance, params.roughness >= 0 ? params.roughness : 1.0f); } if (material->hasParameter("UvScale")) { - instance->setParameter("UvScale", params.uv_scale); + instance->setParameter("UvScale", ReadFloat3(params.uv_scale)); } if (material->hasParameter("UvOffset")) { - instance->setParameter("UvOffset", params.uv_offset); + instance->setParameter("UvOffset", ReadFloat3(params.uv_offset)); } if (material->hasParameter("Reflectance")) { instance->setParameter("Reflectance", params.reflectance); diff --git a/src/experimental/filament/filament/material.h b/src/experimental/filament/filament/material.h index 339b5b5b..630f4844 100644 --- a/src/experimental/filament/filament/material.h +++ b/src/experimental/filament/filament/material.h @@ -17,49 +17,51 @@ #include #include -#include -#include -#include +#include #include "experimental/filament/filament/texture.h" #include "experimental/filament/filament/object_manager.h" namespace mujoco { // The textures that can be assigned to the drawable's material. -struct MaterialTextures { - const Texture* color = nullptr; - const Texture* normal = nullptr; - const Texture* metallic = nullptr; - const Texture* roughness = nullptr; - const Texture* occlusion = nullptr; - const Texture* orm = nullptr; - const Texture* emissive = nullptr; - const Texture* reflection = nullptr; +struct mjrMaterialTextures { + const Texture* color; + const Texture* normal; + const Texture* metallic; + const Texture* roughness; + const Texture* occlusion; + const Texture* orm; + const Texture* emissive; + const Texture* reflection; }; +void mjr_defaultMaterialTextures(mjrMaterialTextures* textures); + // The parameters that can be applied to the drawable's material. -struct MaterialParams { - filament::math::float4 color = {1, 1, 1, 1}; - filament::math::float4 segmentation_color = {1, 1, 1, 1}; - filament::math::float2 tex_repeat = {1, 1}; - filament::math::float3 uv_scale = {1, 1, 1}; - filament::math::float3 uv_offset = {0, 0, 0}; - filament::math::float4 scissor = {0, 0, 0, 0}; - float specular = -1.0f; - float glossiness = -1.0f; - float metallic = -1.0f; - float roughness = -1.0f; - float emissive = -1.0f; - float reflectance = 0.0f; - bool tex_uniform = false; - bool reflective = false; +struct mjrMaterialParams { + float color[4]; + float segmentation_color[4]; + float tex_repeat[2]; + float uv_scale[3]; + float uv_offset[3]; + float scissor[4]; + float specular; + float glossiness; + float metallic; + float roughness; + float emissive; + float reflectance; + mjtByte tex_uniform; + mjtByte reflective; }; +void mjr_defaultMaterialParams(mjrMaterialParams* params); + // Updates the material instances based on the currently set parameters and // textures. void UpdateMaterialInstance(filament::MaterialInstance* instance, - const MaterialParams& params, - const MaterialTextures& textures, + const mjrMaterialParams& params, + const mjrMaterialTextures& textures, ObjectManager* object_mgr); } // namespace mujoco diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 93d66fe4..4381cbd4 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -41,7 +41,10 @@ void mjr_defaultRenderableParams(mjrRenderableParams* params) { } Renderable::Renderable(ObjectManager* object_mgr, const mjrRenderableParams& params) - : object_mgr_(object_mgr), params_(params) {} + : object_mgr_(object_mgr), params_(params) { + mjr_defaultMaterialParams(&material_params_); + mjr_defaultMaterialTextures(&material_textures_); +} Renderable::~Renderable() noexcept { filament::Engine* engine = GetEngine(); @@ -198,8 +201,8 @@ void Renderable::RemoveFromScene(filament::Scene* scene) { assigned_scene_ = nullptr; } -void Renderable::UpdateMaterial(const MaterialParams& params, - const MaterialTextures& textures) { +void Renderable::UpdateMaterial(const mjrMaterialParams& params, + const mjrMaterialTextures& textures) { material_params_ = params; material_textures_ = textures; @@ -237,11 +240,11 @@ void Renderable::AssignMaterial(DrawMode mode, } } -const MaterialParams& Renderable::GetMaterialParams() const { +const mjrMaterialParams& Renderable::GetMaterialParams() const { return material_params_; } -const MaterialTextures& Renderable::GetMaterialTextures() const { +const mjrMaterialTextures& Renderable::GetMaterialTextures() const { return material_textures_; } @@ -374,7 +377,7 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { } if (material_textures_.color == nullptr) { - if (material_params_.color.a < 1.0f) { + if (material_params_.color[3] < 1.0f) { return ObjectManager::kPhongColorFade; } else if (material_params_.reflective) { return ObjectManager::kPhongColorReflect; @@ -383,7 +386,7 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { } } else if (material_textures_.color->GetFilamentTexture()->getTarget() == filament::Texture::Sampler::SAMPLER_CUBEMAP) { - if (material_params_.color.a < 1.0f) { + if (material_params_.color[3] < 1.0f) { return ObjectManager::kPhongCubeFade; } else if (material_params_.reflective) { return ObjectManager::kPhongCubeReflect; @@ -391,7 +394,7 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { return ObjectManager::kPhongCube; } } else if (has_texcoords) { - if (material_params_.color.a < 1.0f) { + if (material_params_.color[3] < 1.0f) { return ObjectManager::kPhong2dUvFade; } else if (material_params_.reflective) { return ObjectManager::kPhong2dUvReflect; @@ -399,7 +402,7 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { return ObjectManager::kPhong2dUv; } } else { - if (material_params_.color.a < 1.0f) { + if (material_params_.color[3] < 1.0f) { return ObjectManager::kPhong2dFade; } else if (material_params_.reflective) { return ObjectManager::kPhong2dReflect; diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 7e3fd732..a0da8166 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -127,14 +127,14 @@ class Renderable { void SetDrawMode(DrawMode mode); // Updates the parameters for the material. - void UpdateMaterial(const MaterialParams& params, - const MaterialTextures& textures); + void UpdateMaterial(const mjrMaterialParams& params, + const mjrMaterialTextures& textures); // Returns the current material parameters. - const MaterialParams& GetMaterialParams() const; + const mjrMaterialParams& GetMaterialParams() const; // Returns the current material textures. - const MaterialTextures& GetMaterialTextures() const; + const mjrMaterialTextures& GetMaterialTextures() const; // Returns the filament Engine managing the renderables. filament::Engine* GetEngine(); @@ -156,8 +156,8 @@ class Renderable { ObjectManager* object_mgr_; mjrRenderableParams params_; filament::MaterialInstance* instances_[kNumDrawModes] = {nullptr}; - MaterialParams material_params_; - MaterialTextures material_textures_; + mjrMaterialParams material_params_; + mjrMaterialTextures material_textures_; DrawMode draw_mode_ = DrawMode::Color; filament::Scene* assigned_scene_ = nullptr; std::vector parts_; diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/filament/scene_geom_util.cc index c14987f3..49f612af 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/filament/scene_geom_util.cc @@ -351,8 +351,12 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, const mjModel* model = model_objs->GetModel(); const bool use_segid_color = scene->flags[mjRND_IDCOLOR]; - MaterialParams params; - params.color = ReadFloat4(geom.rgba); + mjrMaterialParams params; + mjr_defaultMaterialParams(¶ms); + params.color[0] = geom.rgba[0]; + params.color[1] = geom.rgba[1]; + params.color[2] = geom.rgba[2]; + params.color[3] = geom.rgba[3]; if (geom.type == mjGEOM_PLANE) { if (IsBehind(headpos, geom.pos, geom.mat)) { params.color[3] *= 0.3; @@ -360,7 +364,7 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, params.reflective = false; } else { renderable.SetReceiveShadows(true); - params.reflective = geom.reflectance > 0 && params.color.a == 1.0f; + params.reflective = geom.reflectance > 0 && params.color[3] == 1.0f; } } renderable.SetLayerMask(geom.category); @@ -371,7 +375,8 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, renderable.SetWireframe(scene->flags[mjRND_WIREFRAME]); } - MaterialTextures textures; + mjrMaterialTextures textures; + mjr_defaultMaterialTextures(&textures); if (geom.matid >= 0) { textures.color = model_objs->GetTexture(geom.matid, mjTEXROLE_RGB); textures.normal = model_objs->GetTexture(geom.matid, mjTEXROLE_NORMAL); @@ -392,7 +397,8 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, params.metallic = model->mat_metallic[geom.matid]; params.roughness = model->mat_roughness[geom.matid]; params.tex_uniform = model->mat_texuniform[geom.matid]; - params.tex_repeat = ReadFloat2(model->mat_texrepeat, geom.matid); + params.tex_repeat[0] = model->mat_texrepeat[(geom.matid * 2) + 0]; + params.tex_repeat[1] = model->mat_texrepeat[(geom.matid * 2) + 1]; } if (geom.segid >= 0) { @@ -408,9 +414,9 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, const uint8_t red = (segmentation_color >> 0) & 0xff; const uint8_t green = (segmentation_color >> 8) & 0xff; const uint8_t blue = (segmentation_color >> 16) & 0xff; - params.segmentation_color.x = static_cast(red) / 255.0f; - params.segmentation_color.y = static_cast(green) / 255.0f; - params.segmentation_color.z = static_cast(blue) / 255.0f; + params.segmentation_color[0] = static_cast(red) / 255.0f; + params.segmentation_color[1] = static_cast(green) / 255.0f; + params.segmentation_color[2] = static_cast(blue) / 255.0f; } // UvScale only applies to objects that don't have explicit UV coordinates @@ -426,23 +432,23 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, // For 2D textures, `tex_repeat` specifies how many times the texture // image is repeated. The `tex_uniform` flag determines if the repetition // is applied at in object space (false) or in world space (true). - params.uv_scale.x = params.tex_repeat.x; - params.uv_scale.y = params.tex_repeat.y; + params.uv_scale[0] = params.tex_repeat[0]; + params.uv_scale[1] = params.tex_repeat[1]; if (geom.dataid >= 0 && geom.type != mjGEOM_PLANE) { if (geom.size[0] > mjMINVAL) { - params.uv_scale.x /= geom.size[0]; + params.uv_scale[0] /= geom.size[0]; } if (geom.size[1] > mjMINVAL) { - params.uv_scale.y /= geom.size[1]; + params.uv_scale[1] /= geom.size[1]; } } if (params.tex_uniform) { if (geom.size[0] > 0) { - params.uv_scale.x *= geom.size[0]; + params.uv_scale[0] *= geom.size[0]; } if (geom.size[1] > 0) { - params.uv_scale.y *= geom.size[1]; + params.uv_scale[1] *= geom.size[1]; } } const bool is_infinite_plane = @@ -452,11 +458,11 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, // re-centering in engine_vis_visualize.c. const float plane_scale = static_cast(mjMAXPLANEGRID) / 2.0f; const float tile_size_x = - GetPlaneTileSize(model, geom.matid, params.tex_repeat.x); + GetPlaneTileSize(model, geom.matid, params.tex_repeat[0]); const float tile_size_y = - GetPlaneTileSize(model, geom.matid, params.tex_repeat.y); - params.uv_scale.x = 2.0f * plane_scale / tile_size_x; - params.uv_scale.y = 2.0f * plane_scale / tile_size_y; + GetPlaneTileSize(model, geom.matid, params.tex_repeat[1]); + params.uv_scale[0] = 2.0f * plane_scale / tile_size_x; + params.uv_scale[1] = 2.0f * plane_scale / tile_size_y; } // We want to do the equivalent of: @@ -464,17 +470,17 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, // mjr_setf4(tplane, 0, -0.5 * scl.y, 0, -0.5); // glTexGenfv(GL_S, GL_OBJECT_PLANE, splane); // glTexGenfv(GL_T, GL_OBJECT_PLANE, tplane); - params.uv_scale.x = 0.5f * params.uv_scale.x; - params.uv_scale.y = -0.5f * params.uv_scale.y; - params.uv_offset.x = -0.5f; - params.uv_offset.y = -0.5f; + params.uv_scale[0] = 0.5f * params.uv_scale[0]; + params.uv_scale[1] = -0.5f * params.uv_scale[1]; + params.uv_offset[0] = -0.5f; + params.uv_offset[1] = -0.5f; } else { // For cube maps, if `tex_uniform` is true, then scale the texture so that // it covers a 1x1 area of world space rather than the area of the object. if (params.tex_uniform) { - params.uv_scale.x = 1.0f / (geom.size[0] ? geom.size[0] : 1.0f); - params.uv_scale.y = 1.0f / (geom.size[1] ? geom.size[1] : 1.0f); - params.uv_scale.z = 1.0f / (geom.size[2] ? geom.size[2] : 1.0f); + params.uv_scale[0] = 1.0f / (geom.size[0] ? geom.size[0] : 1.0f); + params.uv_scale[1] = 1.0f / (geom.size[1] ? geom.size[1] : 1.0f); + params.uv_scale[2] = 1.0f / (geom.size[2] ? geom.size[2] : 1.0f); } } } diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index ee06e464..997b048f 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -300,7 +300,7 @@ void SceneView::AddReflectiveRenderable(Renderable* renderable) { target->Prepare(viewport.width, viewport.height); if (reflections_enabled_) { - MaterialTextures textures = renderable->GetMaterialTextures(); + mjrMaterialTextures textures = renderable->GetMaterialTextures(); textures.reflection = target->GetColorTexture(); renderable->UpdateMaterial(renderable->GetMaterialParams(), textures); } @@ -332,7 +332,7 @@ void SceneView::EnableReflections() { for (int i = 0; i < reflectives_.size(); ++i) { Renderable* renderable = reflectives_[i]; - MaterialTextures textures = renderable->GetMaterialTextures(); + mjrMaterialTextures textures = renderable->GetMaterialTextures(); textures.reflection = reflect_targets_[i]->GetColorTexture(); renderable->UpdateMaterial(renderable->GetMaterialParams(), textures); } @@ -341,7 +341,7 @@ void SceneView::EnableReflections() { void SceneView::DisableReflections() { reflections_enabled_ = false; for (Renderable* renderable : reflectives_) { - MaterialTextures textures = renderable->GetMaterialTextures(); + mjrMaterialTextures textures = renderable->GetMaterialTextures(); textures.reflection = nullptr; renderable->UpdateMaterial(renderable->GetMaterialParams(), textures); } From 2b211e1ce6f449b770082765dfbd3d7b9ffd2bd9 Mon Sep 17 00:00:00 2001 From: Matija Kecman Date: Thu, 23 Apr 2026 08:02:05 -0700 Subject: [PATCH 128/251] Detect wayland sessions and report a clear error prompting users to switch to X11 or choose a different graphics mode PiperOrigin-RevId: 904460146 Change-Id: I9324f403cb09787833c06f863ac157059eea60e7 --- src/experimental/platform/hal/window.cc | 1 + src/experimental/studio/main.cc | 16 +++++++++++++++- 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/src/experimental/platform/hal/window.cc b/src/experimental/platform/hal/window.cc index faf9a6d1..84941444 100644 --- a/src/experimental/platform/hal/window.cc +++ b/src/experimental/platform/hal/window.cc @@ -16,6 +16,7 @@ #include #include +#include #include #include #include diff --git a/src/experimental/studio/main.cc b/src/experimental/studio/main.cc index 9d48834f..ca84aa4f 100644 --- a/src/experimental/studio/main.cc +++ b/src/experimental/studio/main.cc @@ -75,7 +75,7 @@ class FileResource { int main(int argc, char** argv, char** envp) { absl::ParseCommandLine(argc, argv); - const char* home = getenv("HOME"); + const char* home = std::getenv("HOME"); const std::string ini_path = std::string(home ? home : ".") + "/.mujoco.ini"; mjpResourceProvider resource_provider; @@ -103,6 +103,20 @@ int main(int argc, char** argv, char** envp) { std::string gfx = absl::GetFlag(FLAGS_gfx); + const char* session_type = std::getenv("XDG_SESSION_TYPE"); + const char* wayland_display = std::getenv("WAYLAND_DISPLAY"); + if ((session_type && std::string_view(session_type) == "wayland") || + wayland_display) { + if (gfx.empty()) { + gfx = "opengl_headless"; + } else if (gfx == "classic" || gfx == "opengl") { + mju_error( + "Wayland does not support '%s' graphics mode. " + "Restart with a different graphics mode, or login using X11.", + gfx.c_str()); + } + } + mujoco::platform::GraphicsMode gfx_mode = mujoco::platform::GraphicsModeFromString( gfx, mujoco::platform::GraphicsMode::FilamentOpenGl); From e5a38431af64790dffce48e39429153951fe2018 Mon Sep 17 00:00:00 2001 From: Michael Moss Date: Thu, 23 Apr 2026 08:45:34 -0700 Subject: [PATCH 129/251] Update dependencies ahead of the 3.8.0 release. PiperOrigin-RevId: 904477783 Change-Id: I58a9749d16241fb37c0fbb848fcaf383b28a658d --- python/mujoco/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index 6a6e100f..a0fb042b 100644 --- a/python/mujoco/CMakeLists.txt +++ b/python/mujoco/CMakeLists.txt @@ -193,7 +193,7 @@ findorfetch( GIT_REPO https://github.com/pybind/pybind11 GIT_TAG - d0f1a2168f3335426f544171a3463c36edbd5cc3 # v3.0.3 + c7fb32eea8c92bebeea9f0735041a72aa20c75f5 # v3.0.4 TARGETS pybind11::pybind11_headers EXCLUDE_FROM_ALL From 15f61679bf90f70be5ce392b39c1f4039b041b61 Mon Sep 17 00:00:00 2001 From: Michael Moss Date: Thu, 23 Apr 2026 09:32:20 -0700 Subject: [PATCH 130/251] Update next MuJoCo version to 3.8.0 for pending breaking changes. PiperOrigin-RevId: 904496070 Change-Id: I0b8b01cbcf27f28ac80dbc283bc63090f745f403 --- CMakeLists.txt | 2 +- dist/mujoco.rc | 8 ++++---- dist/simulate.rc | 8 ++++---- doc/APIreference/APIglobals.rst | 2 +- doc/unity.rst | 4 ++-- include/mujoco/mujoco.h | 2 +- mjx/pyproject.toml | 8 ++++---- python/mujoco/CMakeLists.txt | 4 ++-- python/mujoco/mjpython/Info.plist | 8 ++++---- python/pyproject.toml | 6 +++--- sample/CMakeLists.txt | 2 +- simulate/CMakeLists.txt | 2 +- src/engine/engine_support.c | 4 ++-- unity/Editor/Bindings/MujocoBinaryRetriever.cs | 4 ++-- unity/Runtime/Bindings/MjBindings.cs | 2 +- unity/package.json | 2 +- 16 files changed, 34 insertions(+), 34 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 36a64004..a2184a1c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco - VERSION 3.7.1 + VERSION 3.8.0 DESCRIPTION "MuJoCo Physics Simulator" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/dist/mujoco.rc b/dist/mujoco.rc index 2f55a201..7411149b 100644 --- a/dist/mujoco.rc +++ b/dist/mujoco.rc @@ -1,6 +1,6 @@ 1 VERSIONINFO -FILEVERSION 3,7,1,0 -PRODUCTVERSION 3,7,1,0 +FILEVERSION 3,8,0,0 +PRODUCTVERSION 3,8,0,0 FILEOS 0x4 FILETYPE 0x1 { @@ -9,9 +9,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.7.1" + VALUE "ProductVersion", "3.8.0" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.7.1" + VALUE "FileVersion", "3.8.0" VALUE "InternalName", "mujoco.dll" VALUE "OriginalFilename", "mujoco.dll" VALUE "CompanyName", "Google DeepMind" diff --git a/dist/simulate.rc b/dist/simulate.rc index a1517c13..87fdacb9 100644 --- a/dist/simulate.rc +++ b/dist/simulate.rc @@ -1,8 +1,8 @@ MUJOCO ICON "mujoco.ico" 1 VERSIONINFO -FILEVERSION 3,7,1,0 -PRODUCTVERSION 3,7,1,0 +FILEVERSION 3,8,0,0 +PRODUCTVERSION 3,8,0,0 FILEOS 0x4 FILETYPE 0x1 { @@ -11,9 +11,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.7.1" + VALUE "ProductVersion", "3.8.0" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.7.1" + VALUE "FileVersion", "3.8.0" VALUE "InternalName", "simulate.exe" VALUE "OriginalFilename", "simulate.exe" VALUE "CompanyName", "Google DeepMind" diff --git a/doc/APIreference/APIglobals.rst b/doc/APIreference/APIglobals.rst index 72f8e741..b5da5d55 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -388,7 +388,7 @@ Defined in `mujoco.h diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index c602c023..dc0583df 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name="mujoco-mjx" -version = "3.7.1" +version = "3.8.0" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -30,7 +30,7 @@ dependencies = [ "etils[epath]", "jax", "jaxlib", - "mujoco>=3.7.1.dev0", + "mujoco>=3.8.0.dev0", "scipy", "trimesh", ] @@ -46,9 +46,9 @@ mjx-viewer = "mujoco.mjx.viewer:main" [project.urls] Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Documentation = "https://mujoco.readthedocs.io/en/3.7.1" +Documentation = "https://mujoco.readthedocs.io/en/3.8.0" Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Changelog = "https://mujoco.readthedocs.io/en/3.7.1/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.8.0/changelog.html" [tool.isort] force_single_line = true diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index a0fb042b..546ba27d 100644 --- a/python/mujoco/CMakeLists.txt +++ b/python/mujoco/CMakeLists.txt @@ -86,7 +86,7 @@ if(NOT TARGET mujoco) if(MUJOCO_FRAMEWORK) message("MuJoCo framework is at ${MUJOCO_FRAMEWORK}/mujoco.framework") set(MUJOCO_LIBRARY - ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.7.1.dylib + ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.8.0.dylib ) target_compile_options(mujoco INTERFACE -F${MUJOCO_FRAMEWORK}) endif() @@ -94,7 +94,7 @@ if(NOT TARGET mujoco) if(NOT MUJOCO_FRAMEWORK) find_library( - MUJOCO_LIBRARY mujoco mujoco.3.7.1 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED + MUJOCO_LIBRARY mujoco mujoco.3.8.0 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED ) find_path(MUJOCO_INCLUDE mujoco/mujoco.h HINTS ${MUJOCO_INCLUDE_DIR} REQUIRED) message("MuJoCo is at ${MUJOCO_LIBRARY}") diff --git a/python/mujoco/mjpython/Info.plist b/python/mujoco/mjpython/Info.plist index 1c3b5e7c..8f2939fa 100644 --- a/python/mujoco/mjpython/Info.plist +++ b/python/mujoco/mjpython/Info.plist @@ -7,13 +7,13 @@ CFBundleIdentifier org.mujoco.mjpython CFBundleVersion - 3.7.1 + 3.8.0 CFBundleGetInfoString - 3.7.1 + 3.8.0 CFBundleLongVersionString - 3.7.1 + 3.8.0 CFBundleShortVersionString - 3.7.1 + 3.8.0 CFBundleExecutable mjpython CFBundleIconFile diff --git a/python/pyproject.toml b/python/pyproject.toml index 45a5180b..b323d778 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "mujoco" -version = "3.7.1" +version = "3.8.0" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -35,9 +35,9 @@ dynamic = ["readme", "scripts"] [project.urls] Homepage = "https://github.com/google-deepmind/mujoco" -Documentation = "https://mujoco.readthedocs.io/en/3.7.1" +Documentation = "https://mujoco.readthedocs.io/en/3.8.0" Repository = "https://github.com/google-deepmind/mujoco" -Changelog = "https://mujoco.readthedocs.io/en/3.7.1/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.8.0/changelog.html" [tool.setuptools] include-package-data = false diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt index 41902f1e..b83e8e63 100644 --- a/sample/CMakeLists.txt +++ b/sample/CMakeLists.txt @@ -24,7 +24,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco_samples - VERSION 3.7.1 + VERSION 3.8.0 DESCRIPTION "MuJoCo samples binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/simulate/CMakeLists.txt b/simulate/CMakeLists.txt index cbb2cab8..a6a136fe 100644 --- a/simulate/CMakeLists.txt +++ b/simulate/CMakeLists.txt @@ -29,7 +29,7 @@ set(MUJOCO_DEP_VERSION_lodepng project( mujoco_simulate - VERSION 3.7.1 + VERSION 3.8.0 DESCRIPTION "MuJoCo simulate binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 2cff8056..d00cebf8 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -43,8 +43,8 @@ //-------------------------- Constants ------------------------------------------------------------- - #define mjVERSION 3007001 -#define mjVERSIONSTRING "3.7.1" + #define mjVERSION 3008000 +#define mjVERSIONSTRING "3.8.0" // names of disable flags const char* mjDISABLESTRING[mjNDISABLE] = { diff --git a/unity/Editor/Bindings/MujocoBinaryRetriever.cs b/unity/Editor/Bindings/MujocoBinaryRetriever.cs index b3ee6cee..321bd610 100644 --- a/unity/Editor/Bindings/MujocoBinaryRetriever.cs +++ b/unity/Editor/Bindings/MujocoBinaryRetriever.cs @@ -37,7 +37,7 @@ public class MujocoBinaryRetriever { if (AssetDatabase.LoadMainAssetAtPath(mujocoPath + "/mujoco.dylib") == null) { File.Copy( "/Applications/MuJoCo.app/Contents/Frameworks" + - "/mujoco.framework/Versions/Current/libmujoco.3.7.1.dylib", + "/mujoco.framework/Versions/Current/libmujoco.3.8.0.dylib", mujocoPath + "/mujoco.dylib"); AssetDatabase.Refresh(); } @@ -45,7 +45,7 @@ public class MujocoBinaryRetriever { if (AssetDatabase.LoadMainAssetAtPath(mujocoPath + "/libmujoco.so") == null) { File.Copy( Environment.GetFolderPath(Environment.SpecialFolder.UserProfile) + - "/.mujoco/mujoco-3.7.1/lib/libmujoco.so.3.7.1", + "/.mujoco/mujoco-3.8.0/lib/libmujoco.so.3.8.0", mujocoPath + "/libmujoco.so"); AssetDatabase.Refresh(); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index e3bd9fe9..a25bef87 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -113,7 +113,7 @@ public const int mjMAXLINEPNT = 1001; public const int mjMAXPLANEGRID = 200; public const bool THIRD_PARTY_MUJOCO_MJXMACRO_H_ = true; public const bool THIRD_PARTY_MUJOCO_MUJOCO_H_ = true; -public const int mjVERSION_HEADER = 3007001; +public const int mjVERSION_HEADER = 3008000; // ------------------------------------Enums------------------------------------ diff --git a/unity/package.json b/unity/package.json index 3f32a5c5..ef30e334 100644 --- a/unity/package.json +++ b/unity/package.json @@ -1,7 +1,7 @@ { "name": "org.mujoco", "displayName": "MuJoCo", - "version": "3.7.1", + "version": "3.8.0", "description": "MuJoCo importer and runtime plug-in", "dependencies": {}, "author": { From c52d3d07895f9bd0b6261525e4a12d73ca4cc6d9 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 23 Apr 2026 09:51:56 -0700 Subject: [PATCH 131/251] Split out the compatibility classes from the filament renderer. The filament library provides APIs for creating the low-level objects such as meshes and textures. The compat library provides the high-level compatibility layer between mjvScene and the core library. PiperOrigin-RevId: 904504900 Change-Id: Ida1aecab4b9a8aefbbf6b0a0c4ba7ff164864b8e --- src/experimental/filament/CMakeLists.txt | 24 +++++++++---------- .../{filament => compat}/imgui_bridge.cc | 3 +-- .../{filament => compat}/imgui_bridge.h | 6 ++--- .../{filament => compat}/imgui_editor.cc | 4 ++-- .../{filament => compat}/imgui_editor.h | 8 +++---- .../mjr_filament_renderer.cc | 8 +++---- .../mjr_filament_renderer.h | 11 ++++----- .../{filament => compat}/model_objects.cc | 2 +- .../{filament => compat}/model_objects.h | 6 ++--- .../{filament => compat}/scene_bridge.cc | 9 ++++--- .../{filament => compat}/scene_bridge.h | 8 +++---- .../{filament => compat}/scene_geom_util.cc | 7 ++---- .../{filament => compat}/scene_geom_util.h | 9 ++++--- .../filament/render_context_filament.cc | 2 +- 14 files changed, 50 insertions(+), 57 deletions(-) rename src/experimental/filament/{filament => compat}/imgui_bridge.cc (99%) rename src/experimental/filament/{filament => compat}/imgui_bridge.h (92%) rename src/experimental/filament/{filament => compat}/imgui_editor.cc (99%) rename src/experimental/filament/{filament => compat}/imgui_editor.h (74%) rename src/experimental/filament/{filament => compat}/mjr_filament_renderer.cc (96%) rename src/experimental/filament/{filament => compat}/mjr_filament_renderer.h (87%) rename src/experimental/filament/{filament => compat}/model_objects.cc (99%) rename src/experimental/filament/{filament => compat}/model_objects.h (94%) rename src/experimental/filament/{filament => compat}/scene_bridge.cc (98%) rename src/experimental/filament/{filament => compat}/scene_bridge.h (92%) rename src/experimental/filament/{filament => compat}/scene_geom_util.cc (98%) rename src/experimental/filament/{filament => compat}/scene_geom_util.h (76%) diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index 8fe1ecac..59e8432f 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -32,10 +32,6 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/filament_context.h filament/filament_platform_factory.cc filament/filament_platform_factory.h - filament/imgui_bridge.cc - filament/imgui_bridge.h - filament/imgui_editor.cc - filament/imgui_editor.h filament/light.cc filament/light.h filament/material.cc @@ -44,10 +40,6 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/math_util.h filament/mesh.cc filament/mesh.h - filament/mjr_filament_renderer.cc - filament/mjr_filament_renderer.h - filament/model_objects.cc - filament/model_objects.h filament/model_util.h filament/object_manager.cc filament/object_manager.h @@ -55,14 +47,22 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/render_target.h filament/renderable.cc filament/renderable.h - filament/scene_bridge.cc - filament/scene_bridge.h - filament/scene_geom_util.cc - filament/scene_geom_util.h filament/scene_view.cc filament/scene_view.h filament/texture.cc filament/texture.h + compat/imgui_bridge.cc + compat/imgui_bridge.h + compat/imgui_editor.cc + compat/imgui_editor.h + compat/mjr_filament_renderer.cc + compat/mjr_filament_renderer.h + compat/model_objects.cc + compat/model_objects.h + compat/scene_bridge.cc + compat/scene_bridge.h + compat/scene_geom_util.cc + compat/scene_geom_util.h ) if(MUJOCO_USE_FILAMENT_MJR_COMPAT) target_sources(${MUJOCO_FILAMENT_TARGET_NAME} diff --git a/src/experimental/filament/filament/imgui_bridge.cc b/src/experimental/filament/compat/imgui_bridge.cc similarity index 99% rename from src/experimental/filament/filament/imgui_bridge.cc rename to src/experimental/filament/compat/imgui_bridge.cc index 7a645be6..8b43a3a4 100644 --- a/src/experimental/filament/filament/imgui_bridge.cc +++ b/src/experimental/filament/compat/imgui_bridge.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/filament/filament/imgui_bridge.h" +#include "experimental/filament/compat/imgui_bridge.h" #include #include @@ -23,7 +23,6 @@ #include #include #include -#include #include #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/mesh.h" diff --git a/src/experimental/filament/filament/imgui_bridge.h b/src/experimental/filament/compat/imgui_bridge.h similarity index 92% rename from src/experimental/filament/filament/imgui_bridge.h rename to src/experimental/filament/compat/imgui_bridge.h index d205db5c..06807f68 100644 --- a/src/experimental/filament/filament/imgui_bridge.h +++ b/src/experimental/filament/compat/imgui_bridge.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_IMGUI_BRIDGE_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_IMGUI_BRIDGE_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_IMGUI_BRIDGE_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_IMGUI_BRIDGE_H_ #include #include @@ -72,4 +72,4 @@ void DrawTextAt(const char* text, float x, float y, float z); } // namespace mujoco -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_IMGUI_BRIDGE_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_IMGUI_BRIDGE_H_ diff --git a/src/experimental/filament/filament/imgui_editor.cc b/src/experimental/filament/compat/imgui_editor.cc similarity index 99% rename from src/experimental/filament/filament/imgui_editor.cc rename to src/experimental/filament/compat/imgui_editor.cc index bfc773f4..dd707914 100644 --- a/src/experimental/filament/filament/imgui_editor.cc +++ b/src/experimental/filament/compat/imgui_editor.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/filament/filament/imgui_editor.h" +#include "experimental/filament/compat/imgui_editor.h" #include #include @@ -34,8 +34,8 @@ #include #include #include +#include "experimental/filament/compat/scene_bridge.h" #include "experimental/filament/filament/color_grading_options.h" -#include "experimental/filament/filament/scene_bridge.h" #include "experimental/filament/filament/scene_view.h" namespace mujoco { diff --git a/src/experimental/filament/filament/imgui_editor.h b/src/experimental/filament/compat/imgui_editor.h similarity index 74% rename from src/experimental/filament/filament/imgui_editor.h rename to src/experimental/filament/compat/imgui_editor.h index f073fe45..3457fd26 100644 --- a/src/experimental/filament/filament/imgui_editor.h +++ b/src/experimental/filament/compat/imgui_editor.h @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_IMGUI_EDITOR_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_IMGUI_EDITOR_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_IMGUI_EDITOR_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_IMGUI_EDITOR_H_ -#include "experimental/filament/filament/scene_bridge.h" +#include "experimental/filament/compat/scene_bridge.h" namespace mujoco { @@ -24,4 +24,4 @@ void DrawGui(SceneBridge* scene_bridge); } // namespace mujoco -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_IMGUI_EDITOR_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_IMGUI_EDITOR_H_ diff --git a/src/experimental/filament/filament/mjr_filament_renderer.cc b/src/experimental/filament/compat/mjr_filament_renderer.cc similarity index 96% rename from src/experimental/filament/filament/mjr_filament_renderer.cc rename to src/experimental/filament/compat/mjr_filament_renderer.cc index 8ca1ebe6..48242258 100644 --- a/src/experimental/filament/filament/mjr_filament_renderer.cc +++ b/src/experimental/filament/compat/mjr_filament_renderer.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/filament/filament/mjr_filament_renderer.h" +#include "experimental/filament/compat/mjr_filament_renderer.h" #include #include @@ -22,13 +22,13 @@ #include #include #include +#include "experimental/filament/compat/imgui_bridge.h" +#include "experimental/filament/compat/imgui_editor.h" +#include "experimental/filament/compat/scene_bridge.h" #include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/filament_context.h" -#include "experimental/filament/filament/imgui_bridge.h" -#include "experimental/filament/filament/imgui_editor.h" #include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/render_target.h" -#include "experimental/filament/filament/scene_bridge.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" diff --git a/src/experimental/filament/filament/mjr_filament_renderer.h b/src/experimental/filament/compat/mjr_filament_renderer.h similarity index 87% rename from src/experimental/filament/filament/mjr_filament_renderer.h rename to src/experimental/filament/compat/mjr_filament_renderer.h index 205b8beb..a1c97edc 100644 --- a/src/experimental/filament/filament/mjr_filament_renderer.h +++ b/src/experimental/filament/compat/mjr_filament_renderer.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MJR_FILAMENT_RENDERER_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MJR_FILAMENT_RENDERER_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_MJR_FILAMENT_RENDERER_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_MJR_FILAMENT_RENDERER_H_ #include #include @@ -21,10 +21,9 @@ #include #include #include -#include "experimental/filament/filament/draw_mode.h" +#include "experimental/filament/compat/imgui_bridge.h" +#include "experimental/filament/compat/scene_bridge.h" #include "experimental/filament/filament/filament_context.h" -#include "experimental/filament/filament/imgui_bridge.h" -#include "experimental/filament/filament/scene_bridge.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -84,4 +83,4 @@ class MjrFilamentRenderer : public FilamentContext { } // namespace mujoco -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MJR_FILAMENT_RENDERER_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_MJR_FILAMENT_RENDERER_H_ diff --git a/src/experimental/filament/filament/model_objects.cc b/src/experimental/filament/compat/model_objects.cc similarity index 99% rename from src/experimental/filament/filament/model_objects.cc rename to src/experimental/filament/compat/model_objects.cc index 742c3609..fb796882 100644 --- a/src/experimental/filament/filament/model_objects.cc +++ b/src/experimental/filament/compat/model_objects.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/filament/filament/model_objects.h" +#include "experimental/filament/compat/model_objects.h" #include #include diff --git a/src/experimental/filament/filament/model_objects.h b/src/experimental/filament/compat/model_objects.h similarity index 94% rename from src/experimental/filament/filament/model_objects.h rename to src/experimental/filament/compat/model_objects.h index d85693e3..db528d36 100644 --- a/src/experimental/filament/filament/model_objects.h +++ b/src/experimental/filament/compat/model_objects.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_OBJECTS_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_OBJECTS_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_MODEL_OBJECTS_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_MODEL_OBJECTS_H_ #include #include @@ -100,4 +100,4 @@ class ModelObjects { } // namespace mujoco -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_OBJECTS_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_MODEL_OBJECTS_H_ diff --git a/src/experimental/filament/filament/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc similarity index 98% rename from src/experimental/filament/filament/scene_bridge.cc rename to src/experimental/filament/compat/scene_bridge.cc index fd62b392..4f00828d 100644 --- a/src/experimental/filament/filament/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/filament/filament/scene_bridge.h" +#include "experimental/filament/compat/scene_bridge.h" #include #include @@ -36,16 +36,15 @@ #include #include #include +#include "experimental/filament/compat/imgui_bridge.h" +#include "experimental/filament/compat/model_objects.h" +#include "experimental/filament/compat/scene_geom_util.h" #include "experimental/filament/filament/color_grading_options.h" -#include "experimental/filament/filament/imgui_bridge.h" #include "experimental/filament/filament/light.h" -#include "experimental/filament/filament/material.h" #include "experimental/filament/filament/math_util.h" -#include "experimental/filament/filament/model_objects.h" #include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/renderable.h" -#include "experimental/filament/filament/scene_geom_util.h" #include "experimental/filament/filament/scene_view.h" #include "experimental/filament/filament/texture.h" diff --git a/src/experimental/filament/filament/scene_bridge.h b/src/experimental/filament/compat/scene_bridge.h similarity index 92% rename from src/experimental/filament/filament/scene_bridge.h rename to src/experimental/filament/compat/scene_bridge.h index 8c31de05..96e8ab31 100644 --- a/src/experimental/filament/filament/scene_bridge.h +++ b/src/experimental/filament/compat/scene_bridge.h @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_SCENE_BRIDGE_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_SCENE_BRIDGE_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_SCENE_BRIDGE_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_SCENE_BRIDGE_H_ #include #include @@ -24,8 +24,8 @@ #include #include #include +#include "experimental/filament/compat/model_objects.h" #include "experimental/filament/filament/light.h" -#include "experimental/filament/filament/model_objects.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/scene_view.h" @@ -85,4 +85,4 @@ class SceneBridge { } // namespace mujoco -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_SCENE_BRIDGE_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_SCENE_BRIDGE_H_ diff --git a/src/experimental/filament/filament/scene_geom_util.cc b/src/experimental/filament/compat/scene_geom_util.cc similarity index 98% rename from src/experimental/filament/filament/scene_geom_util.cc rename to src/experimental/filament/compat/scene_geom_util.cc index 49f612af..527fe56f 100644 --- a/src/experimental/filament/filament/scene_geom_util.cc +++ b/src/experimental/filament/compat/scene_geom_util.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/filament/filament/scene_geom_util.h" +#include "experimental/filament/compat/scene_geom_util.h" #include #include @@ -22,20 +22,17 @@ #include #include -#include #include -#include #include #include #include #include -#include #include #include +#include "experimental/filament/compat/model_objects.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" -#include "experimental/filament/filament/model_objects.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/texture.h" diff --git a/src/experimental/filament/filament/scene_geom_util.h b/src/experimental/filament/compat/scene_geom_util.h similarity index 76% rename from src/experimental/filament/filament/scene_geom_util.h rename to src/experimental/filament/compat/scene_geom_util.h index deef9c58..c702f687 100644 --- a/src/experimental/filament/filament/scene_geom_util.h +++ b/src/experimental/filament/compat/scene_geom_util.h @@ -12,14 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_SCENE_GEOM_UTIL_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_SCENE_GEOM_UTIL_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_SCENE_GEOM_UTIL_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_SCENE_GEOM_UTIL_H_ #include #include -#include "experimental/filament/filament/material.h" -#include "experimental/filament/filament/model_objects.h" +#include "experimental/filament/compat/model_objects.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/renderable.h" @@ -32,4 +31,4 @@ std::unique_ptr CreateGeomRenderable( } // namespace mujoco -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_SCENE_GEOM_UTIL_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_SCENE_GEOM_UTIL_H_ diff --git a/src/experimental/filament/render_context_filament.cc b/src/experimental/filament/render_context_filament.cc index 10f0aa2b..65b4086a 100644 --- a/src/experimental/filament/render_context_filament.cc +++ b/src/experimental/filament/render_context_filament.cc @@ -21,7 +21,7 @@ #include #include #include -#include "experimental/filament/filament/mjr_filament_renderer.h" +#include "experimental/filament/compat/mjr_filament_renderer.h" #if defined(TLS_FILAMENT_CONTEXT) From f0b61207717dca0b872f2a726daca17856474827 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Thu, 23 Apr 2026 18:12:46 +0100 Subject: [PATCH 132/251] update .readthedocs.yml --- .readthedocs.yml | 56 ++++++++++++++++++++++++++++++++++++++++++------ 1 file changed, 50 insertions(+), 6 deletions(-) diff --git a/.readthedocs.yml b/.readthedocs.yml index 00c093bb..5f9792b3 100644 --- a/.readthedocs.yml +++ b/.readthedocs.yml @@ -11,21 +11,65 @@ build: os: ubuntu-24.04 tools: python: "3.12" + apt_packages: + - libgl-dev jobs: create_environment: + # install uv - asdf plugin add uv - asdf install uv latest - asdf global uv latest - uv venv $READTHEDOCS_VIRTUALENV_PATH - - UV_PROJECT_ENVIRONMENT=$READTHEDOCS_VIRTUALENV_PATH uv pip install -r doc/requirements.txt - - UV_PROJECT_ENVIRONMENT=$READTHEDOCS_VIRTUALENV_PATH uv pip install mujoco mujoco-mjx - # replace mujoco.mjx.third_party.mujoco_warp import paths with mujoco_warp + # install doc requirements and build tools - | - find mjx/mujoco/mjx/third_party/mujoco_warp -type f -exec sed -i 's/mujoco\.mjx\.third_party\.mujoco_warp/mujoco_warp/g' {} \; + UV_PROJECT_ENVIRONMENT=$READTHEDOCS_VIRTUALENV_PATH \ + uv pip install \ + -r doc/requirements.txt \ + cmake pip build setuptools absl-py + # build and install MuJoCo C library + - | + VENV=$READTHEDOCS_VIRTUALENV_PATH && \ + $VENV/bin/cmake -B build \ + -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX=$VENV \ + -DMUJOCO_BUILD_EXAMPLES=OFF \ + -DMUJOCO_BUILD_SIMULATE=OFF \ + -DMUJOCO_BUILD_TESTS=OFF \ + -DMUJOCO_TEST_PYTHON_UTIL=OFF && \ + $VENV/bin/cmake --build build --parallel && \ + $VENV/bin/cmake --install build + # copy plugins + - | + mkdir -p $READTHEDOCS_VIRTUALENV_PATH/mujoco_plugin && \ + cp build/lib/libactuator.* \ + build/lib/libelasticity.* \ + build/lib/libsensor.* \ + $READTHEDOCS_VIRTUALENV_PATH/mujoco_plugin/ && \ + cp build/lib/libsdf_plugin.* \ + $READTHEDOCS_VIRTUALENV_PATH/mujoco_plugin/ || true + # build and install Python bindings from source + - | + export VIRTUAL_ENV=$READTHEDOCS_VIRTUALENV_PATH \ + PATH=$READTHEDOCS_VIRTUALENV_PATH/bin:$PATH && \ + cd python && bash make_sdist.sh && cd dist && \ + MUJOCO_PATH=$READTHEDOCS_VIRTUALENV_PATH \ + MUJOCO_PLUGIN_PATH=$READTHEDOCS_VIRTUALENV_PATH/mujoco_plugin \ + MUJOCO_CMAKE_ARGS="-DCMAKE_INTERPROCEDURAL_OPTIMIZATION:BOOL=OFF \ + -DGLFW_BUILD_WAYLAND=OFF -DGLFW_BUILD_X11=OFF" \ + UV_PROJECT_ENVIRONMENT=$READTHEDOCS_VIRTUALENV_PATH \ + uv pip install mujoco-*.tar.gz && \ + cd ../.. + # install mjx and mujoco_warp + - UV_PROJECT_ENVIRONMENT=$READTHEDOCS_VIRTUALENV_PATH uv pip install -e mjx + - | + find mjx/mujoco/mjx/third_party/mujoco_warp -type f -exec \ + sed -i 's/mujoco\.mjx\.third_party\.mujoco_warp/mujoco_warp/g' {} \; - python doc/mjwarp/update_types.py mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py - - UV_PROJECT_ENVIRONMENT=$READTHEDOCS_VIRTUALENV_PATH uv pip install mjx/mujoco/mjx/third_party/mujoco_warp + - | + UV_PROJECT_ENVIRONMENT=$READTHEDOCS_VIRTUALENV_PATH \ + uv pip install mjx/mujoco/mjx/third_party/mujoco_warp install: - - "true" # skip + - "true" # skip default install sphinx: builder: html From 4eba85093a4b8061ae92275d2e11c7b0ef8802aa Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 23 Apr 2026 12:15:25 -0700 Subject: [PATCH 133/251] Add new flex_node fields to MJX. PiperOrigin-RevId: 904576383 Change-Id: Ie0cb8e0f25bdeadbecc2ea229835b0f2a2957254 --- mjx/mujoco/mjx/_src/types.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index ff08579c..faf47012 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -776,6 +776,10 @@ class Model(PyTreeNode): flex_vertadr: np.ndarray flex_vertnum: np.ndarray flex_vert0: np.ndarray + flex_nodeadr: np.ndarray + flex_nodenum: np.ndarray + flex_nodebodyid: np.ndarray + flex_node0: np.ndarray hfield_size: np.ndarray hfield_nrow: np.ndarray hfield_ncol: np.ndarray From 1465d8b6cec816c525b9342f2c986203ea1454ba Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Thu, 23 Apr 2026 12:55:24 -0700 Subject: [PATCH 134/251] Initial implementation of MuJoCo Live. This is a combination of minor tweaks to the existing Studio WASM application, CMake cleanup to make it buildable, and integration with GitHub Actions for deployment. The new features added are drag-and-drop, model specification through `?model=` URL parameter, HTTP and HTTPS resource provider, and a user-visible loading message while the model is loading. PiperOrigin-RevId: 904594610 Change-Id: I3a73b1ca0fcd6fc9ba192469942b2dab010a9308 --- .github/workflows/build_steps.sh | 26 ++++ .github/workflows/live.yml | 51 ++++++ CMakeLists.txt | 40 ++++- cmake/MujocoOptions.cmake | 6 +- cmake/third_party_deps/filament.cmake | 12 +- sample/cmake/SampleOptions.cmake | 6 +- simulate/cmake/SimulateOptions.cmake | 6 +- src/experimental/filament/CMakeLists.txt | 40 +++-- src/experimental/filament/filament/mesh.cc | 44 ++++-- src/experimental/filament/filament/mesh.h | 9 +- src/experimental/studio/CMakeLists.txt | 173 +++++++++++++-------- src/experimental/studio/index.html | 161 ++++++++++++++++--- src/experimental/studio/wasm.cc | 115 ++++++++++++++ src/render/noop/CMakeLists.txt | 4 +- 14 files changed, 573 insertions(+), 120 deletions(-) create mode 100644 .github/workflows/live.yml diff --git a/.github/workflows/build_steps.sh b/.github/workflows/build_steps.sh index c173333f..9e5440f9 100755 --- a/.github/workflows/build_steps.sh +++ b/.github/workflows/build_steps.sh @@ -295,6 +295,32 @@ EOF } +build_mujoco_live() { + echo "Setting up Emscripten SDK..." + source emsdk/emsdk_env.sh + + echo "Building Filament tools, targeting host platform..." + cmake -S . -B build_host -G Ninja \ + -DCMAKE_BUILD_TYPE=Release \ + -DUSE_STATIC_LIBCXX=OFF \ + -DMUJOCO_BUILD_STUDIO=ON \ + -DMUJOCO_USE_FILAMENT=ON \ + -DMUJOCO_BUILD_TESTS=OFF \ + -DMUJOCO_BUILD_EXAMPLES=OFF \ + -DMUJOCO_BUILD_SIMULATE=OFF + cmake --build build_host --target matc resgen cmgen mujoco_filament_assets -j$(nproc) + + echo "Building WASM app..." + emcmake cmake -S . -B build_wasm -G Ninja \ + -DCMAKE_BUILD_TYPE=Release \ + -DMUJOCO_BUILD_STUDIO=ON \ + -DMUJOCO_USE_FILAMENT=ON \ + -DMUJOCO_BUILD_TESTS_WASM=OFF \ + -DMUJOCO_NATIVE_BUILD_DIR=$(pwd)/build_host + cmake --build build_wasm --target mujoco_live -j$(nproc) +} + + # Discover functions defined in this script by finding identifiers followed by # "()" and capturing the identifier as a valid function name. VALID_FUNCTIONS=() diff --git a/.github/workflows/live.yml b/.github/workflows/live.yml new file mode 100644 index 00000000..49cd5359 --- /dev/null +++ b/.github/workflows/live.yml @@ -0,0 +1,51 @@ +name: live + +on: + push: + branches: + - live + +permissions: + contents: read + pages: write + id-token: write + +jobs: + build-and-upload-artifacts: + runs-on: ubuntu-24.04 + steps: + - uses: actions/checkout@v4 + + - name: Prepare Linux + run: bash ./.github/workflows/build_steps.sh prepare_linux + + - name: Setup Emscripten + run: bash ./.github/workflows/build_steps.sh setup_emsdk + + - name: Build MuJoCo Live + env: + CC: clang-18 + CXX: clang++-18 + run: bash ./.github/workflows/build_steps.sh build_mujoco_live + + - name: Prepare files for GitHub Pages + run: | + mkdir -p dist/bin + cp -r build_wasm/bin/* dist/bin/ + cp src/experimental/studio/index.html dist/index.html + + - name: Upload GitHub Pages artifacts + uses: actions/upload-pages-artifact@v3 + with: + path: dist + + deploy-pages: + needs: build-and-upload-artifacts + runs-on: ubuntu-latest + environment: + name: github-pages + url: ${{ steps.deployment.outputs.page_url }} + steps: + - name: Deploy to GitHub Pages + id: deployment + uses: actions/deploy-pages@v4 diff --git a/CMakeLists.txt b/CMakeLists.txt index a2184a1c..c6a8f634 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,12 +53,49 @@ endif() if(EMSCRIPTEN) option(MUJOCO_BUILD_TESTS_WASM "Build tests for WASM bindings" ON) + option(MUJOCO_BUILD_STUDIO "Build studio for MuJoCo (WASM)" OFF) + option(MUJOCO_USE_FILAMENT "Use filament rendering" OFF) option(MUJOCO_WASM_THREADS "Build with multi-threading support" ON) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++20 -O3 -fexceptions") if(MUJOCO_WASM_THREADS) set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -pthread") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") endif() + + # Filament uses the WEBGL variable (not EMSCRIPTEN) to identify web builds. + # Without this, it falls into the LINUX path and tries to compile with + # futex, X11, Vulkan, etc. + if(MUJOCO_USE_FILAMENT) + set(WEBGL ON CACHE BOOL "Filament WebGL mode" FORCE) + endif() + + # Automatically generate host tool imports for Filament cross-compilation. + # When WEBGL is true, Filament hardcodes the path to: + # ${FILAMENT}/${IMPORT_EXECUTABLES_DIR}/ImportExecutables-Release.cmake + # So we write our file with that exact name and set IMPORT_EXECUTABLES_DIR + # to point at CMAKE_BINARY_DIR (which is ../../ relative to filament-src). + set(MUJOCO_NATIVE_BUILD_DIR "${PROJECT_SOURCE_DIR}/build-host" CACHE PATH "Path to native build directory containing host tools") + find_program(MATC_EXE matc PATHS "${MUJOCO_NATIVE_BUILD_DIR}/bin" NO_DEFAULT_PATH) + find_program(RESGEN_EXE resgen PATHS "${MUJOCO_NATIVE_BUILD_DIR}/bin" NO_DEFAULT_PATH) + find_program(CMGEN_EXE cmgen PATHS "${MUJOCO_NATIVE_BUILD_DIR}/bin" NO_DEFAULT_PATH) + + if(MATC_EXE AND RESGEN_EXE AND CMGEN_EXE) + message(STATUS "Found host tools in ${MUJOCO_NATIVE_BUILD_DIR}/bin") + set(IMPORT_EXECUTABLES_FILE "${CMAKE_BINARY_DIR}/ImportExecutables-Release.cmake") + file(WRITE "${IMPORT_EXECUTABLES_FILE}" + "add_executable(matc IMPORTED)\n" + "set_property(TARGET matc PROPERTY IMPORTED_LOCATION \"${MATC_EXE}\")\n" + "add_executable(resgen IMPORTED)\n" + "set_property(TARGET resgen PROPERTY IMPORTED_LOCATION \"${RESGEN_EXE}\")\n" + "add_executable(cmgen IMPORTED)\n" + "set_property(TARGET cmgen PROPERTY IMPORTED_LOCATION \"${CMGEN_EXE}\")\n" + ) + # Filament's WEBGL path resolves: ${FILAMENT}/${IMPORT_EXECUTABLES_DIR}/ImportExecutables-Release.cmake + # FILAMENT = _deps/filament-src, so ../../ resolves to CMAKE_BINARY_DIR. + set(IMPORT_EXECUTABLES_DIR "../../" CACHE PATH "" FORCE) + else() + message(WARNING "Host tools (matc, resgen, cmgen) not found in ${MUJOCO_NATIVE_BUILD_DIR}/bin. WASM build of Studio might fail.") + endif() endif() if(APPLE AND (MUJOCO_BUILD_EXAMPLES OR MUJOCO_BUILD_SIMULATE)) @@ -125,8 +162,9 @@ if(NOT EMSCRIPTEN AND NOT MUJOCO_USE_FILAMENT_MJR_COMPAT) add_subdirectory(src/render/classic) add_subdirectory(src/ui) endif() +add_subdirectory(src/render/noop) -if(MUJOCO_USE_FILAMENT AND NOT EMSCRIPTEN) +if(MUJOCO_USE_FILAMENT) add_subdirectory(src/experimental/filament) endif() diff --git a/cmake/MujocoOptions.cmake b/cmake/MujocoOptions.cmake index a606220c..74dc340f 100644 --- a/cmake/MujocoOptions.cmake +++ b/cmake/MujocoOptions.cmake @@ -18,7 +18,11 @@ set(CMAKE_C_STANDARD_REQUIRED ON) set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) -set(CMAKE_C_EXTENSIONS OFF) +if(EMSCRIPTEN) + set(CMAKE_C_EXTENSIONS ON) +else() + set(CMAKE_C_EXTENSIONS OFF) +endif() set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # For LLVM tooling if(NOT CMAKE_CONFIGURATION_TYPES) diff --git a/cmake/third_party_deps/filament.cmake b/cmake/third_party_deps/filament.cmake index 007bc23b..0c94d53f 100644 --- a/cmake/third_party_deps/filament.cmake +++ b/cmake/third_party_deps/filament.cmake @@ -13,7 +13,7 @@ # limitations under the License. set(MUJOCO_DEP_VERSION_filament - a4945939de514d049baeed654efbbdd06bc5bdbf + 06793c4a80dd467025b2db1b3b7ea63bf1a865bb CACHE STRING "Tag/version of `filament` to be fetched." ) mark_as_advanced(MUJOCO_DEP_VERSION_filament) @@ -23,6 +23,15 @@ include(FindOrFetch) set(BUILD_SHARED_LIBS_OLD ${BUILD_SHARED_LIBS}) set(BUILD_SHARED_LIBS OFF) +# Filament's ShaderMinifier.cpp uses strlen without including , and +# PostProcessManager.h uses std::optional without including . +set(CMAKE_CXX_FLAGS_OLD "${CMAKE_CXX_FLAGS}") +if(MSVC) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /FI cstring /FI optional") +else() + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -include cstring -include optional") +endif() + set(FILAMENT_ENABLE_EXPERIMENTAL_GCC_SUPPORT ON) set(FILAMENT_SKIP_SDL2 ON) set(FILAMENT_USE_EXTERNAL_ABSL ON) @@ -39,3 +48,4 @@ fetchpackage( ) set(BUILD_SHARED_LIBS ${BUILD_SHARED_LIBS_OLD}) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_OLD}") diff --git a/sample/cmake/SampleOptions.cmake b/sample/cmake/SampleOptions.cmake index a606220c..74dc340f 100644 --- a/sample/cmake/SampleOptions.cmake +++ b/sample/cmake/SampleOptions.cmake @@ -18,7 +18,11 @@ set(CMAKE_C_STANDARD_REQUIRED ON) set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) -set(CMAKE_C_EXTENSIONS OFF) +if(EMSCRIPTEN) + set(CMAKE_C_EXTENSIONS ON) +else() + set(CMAKE_C_EXTENSIONS OFF) +endif() set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # For LLVM tooling if(NOT CMAKE_CONFIGURATION_TYPES) diff --git a/simulate/cmake/SimulateOptions.cmake b/simulate/cmake/SimulateOptions.cmake index a606220c..74dc340f 100644 --- a/simulate/cmake/SimulateOptions.cmake +++ b/simulate/cmake/SimulateOptions.cmake @@ -18,7 +18,11 @@ set(CMAKE_C_STANDARD_REQUIRED ON) set(CMAKE_CXX_STANDARD 20) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) -set(CMAKE_C_EXTENSIONS OFF) +if(EMSCRIPTEN) + set(CMAKE_C_EXTENSIONS ON) +else() + set(CMAKE_C_EXTENSIONS OFF) +endif() set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # For LLVM tooling if(NOT CMAKE_CONFIGURATION_TYPES) diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index 59e8432f..9e69f162 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -129,20 +129,32 @@ foreach(MATERIAL_FILE ${MATERIAL_FILES}) set(INPUT_FILE "${ASSETS_DIR}/${MATERIAL_FILE}") set(OUTPUT_FILE "${OUTPUT_ASSETS_DIR}/${MATERIAL_NAME}.filamat") - add_custom_command( - OUTPUT ${OUTPUT_FILE} - COMMAND ${MATC_EXECUTABLE} - --platform=all - --api=vulkan - --api=opengl - --variant-filter skinning - --optimize-size - --output ${OUTPUT_FILE} - ${INPUT_FILE} - DEPENDS ${INPUT_FILE} - DEPENDS matc - COMMENT "Compiling ${MATERIAL_FILE}" - ) + if(CMAKE_SYSTEM_NAME STREQUAL "Emscripten") + set(PRECOMPILED_FILE "${MUJOCO_NATIVE_BUILD_DIR}/src/experimental/filament/assets/${MATERIAL_NAME}.filamat") + add_custom_command( + OUTPUT ${OUTPUT_FILE} + COMMAND ${CMAKE_COMMAND} -E copy + ${PRECOMPILED_FILE} + ${OUTPUT_FILE} + DEPENDS ${PRECOMPILED_FILE} + COMMENT "Copying precompiled material ${MATERIAL_NAME}.filamat" + ) + else() + add_custom_command( + OUTPUT ${OUTPUT_FILE} + COMMAND ${MATC_EXECUTABLE} + --platform=all + --api=vulkan + --api=opengl + --variant-filter skinning + --optimize-size + --output ${OUTPUT_FILE} + ${INPUT_FILE} + DEPENDS ${INPUT_FILE} + DEPENDS matc + COMMENT "Compiling ${MATERIAL_FILE}" + ) + endif() list(APPEND MUJOCO_FILAMENT_ASSET_FILES ${OUTPUT_FILE}) endforeach() diff --git a/src/experimental/filament/filament/mesh.cc b/src/experimental/filament/filament/mesh.cc index 1e958658..e38c4399 100644 --- a/src/experimental/filament/filament/mesh.cc +++ b/src/experimental/filament/filament/mesh.cc @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include #include @@ -105,7 +107,7 @@ void mjr_defaultMeshData(mjrMeshData* data) { } Mesh::Mesh(filament::Engine* engine, const mjrMeshData& data) - : engine_(engine) { + : engine_(engine), shared_state_(std::make_shared()) { type_ = data.primitive_type == mjMESH_PRIMITIVE_TYPE_TRIANGLES ? filament::RenderableManager::PrimitiveType::TRIANGLES : filament::RenderableManager::PrimitiveType::LINES; @@ -113,7 +115,7 @@ Mesh::Mesh(filament::Engine* engine, const mjrMeshData& data) // If the user has provided a release callback, then we need to ensure we // call is when filament is done with the mesh data. if (data.release_callback) { - release_callbacks_.push_back([=]() { + shared_state_->callbacks.push_back([=]() { data.release_callback(data.user_data); }); } @@ -138,11 +140,21 @@ void Mesh::BuildVertexBuffer(const mjrMeshData& data) { mju_error("mjrMeshData has no vertices."); } - // The filament BufferDescriptor callback for releasing the memory. We assume - // that ReleaseResources() can be called multiple times, so we assign this - // callback to each buffer descriptor. + // The filament BufferDescriptor callback for releasing the memory. + // We pass a heap-allocated shared_ptr to the shared state as the user data. auto callback = +[](void* buffer, size_t size, void* user) { - static_cast(user)->ReleaseResources(); + auto* state_ptr = static_cast*>(user); + auto state = *state_ptr; + delete state_ptr; + + std::lock_guard lock(state->mutex); + if (!state->called) { + for (const auto& cb : state->callbacks) { + cb(); + } + state->callbacks.clear(); + state->called = true; + } }; // Pointers to specific attributes in the mesh data, used for additional @@ -206,7 +218,8 @@ void Mesh::BuildVertexBuffer(const mjrMeshData& data) { attributes_[i] = usage; } vertex_buffer_ = vb_builder.build(*engine_); - vertex_buffer_->setBufferAt(*engine_, 0, {bytes, nbytes, callback, this}); + auto* user_data = new std::shared_ptr(shared_state_); + vertex_buffer_->setBufferAt(*engine_, 0, {bytes, nbytes, callback, user_data}); } else { // For a non-interleaved vertex buffer, we assign a separate buffer to each // attribute. @@ -238,7 +251,8 @@ void Mesh::BuildVertexBuffer(const mjrMeshData& data) { nbytes = data.nvertices * sizeof(float4); bytes = BuildOrientationsFromNormals(data.nvertices, attrib); } - vertex_buffer_->setBufferAt(*engine_, i, {bytes, nbytes, callback, this}); + auto* user_data = new std::shared_ptr(shared_state_); + vertex_buffer_->setBufferAt(*engine_, i, {bytes, nbytes, callback, user_data}); } } } @@ -259,7 +273,7 @@ void Mesh::BuildIndexBuffer(const mjrMeshData& data) { const void* indices = data.indices; if (indices == nullptr) { std::byte* sequence = new std::byte[num_bytes]; - release_callbacks_.push_back([=]() { + shared_state_->callbacks.push_back([=]() { delete[] sequence; }); @@ -286,7 +300,7 @@ void Mesh::BuildIndexBuffer(const mjrMeshData& data) { float4* Mesh::BuildOrientationsFromNormals(int nvertices, const mjrVertexAttribute& normals) { float4* orientations = new float4[nvertices]; - release_callbacks_.push_back([=]() { + shared_state_->callbacks.push_back([=]() { delete[] orientations; }); const float* normals_ptr = reinterpret_cast(normals.bytes); @@ -321,10 +335,14 @@ void Mesh::UpdateBounds(const mjrMeshData& data) { } void Mesh::ReleaseResources() { - for (const auto& callback : release_callbacks_) { - callback(); + std::lock_guard lock(shared_state_->mutex); + if (!shared_state_->called) { + for (const auto& callback : shared_state_->callbacks) { + callback(); + } + shared_state_->callbacks.clear(); + shared_state_->called = true; } - release_callbacks_.clear(); } filament::IndexBuffer* Mesh::GetFilamentIndexBuffer() const { diff --git a/src/experimental/filament/filament/mesh.h b/src/experimental/filament/filament/mesh.h index ab259bc4..ba15d8ca 100644 --- a/src/experimental/filament/filament/mesh.h +++ b/src/experimental/filament/filament/mesh.h @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include #include #include @@ -180,7 +182,12 @@ class Mesh { filament::RenderableManager::PrimitiveType type_ = filament::RenderableManager::PrimitiveType::TRIANGLES; std::optional bounds_; - std::vector> release_callbacks_; + struct SharedState { + std::vector> callbacks; + std::mutex mutex; + bool called = false; + }; + std::shared_ptr shared_state_; std::array attributes_; int num_attributes_ = 0; }; diff --git a/src/experimental/studio/CMakeLists.txt b/src/experimental/studio/CMakeLists.txt index cafa6a66..6dfd90f5 100644 --- a/src/experimental/studio/CMakeLists.txt +++ b/src/experimental/studio/CMakeLists.txt @@ -14,87 +14,124 @@ cmake_minimum_required(VERSION 3.16) -set(MUJOCO_STUDIO_TARGET_NAME mujoco_studio) - -add_executable(${MUJOCO_STUDIO_TARGET_NAME}) - -target_sources(${MUJOCO_STUDIO_TARGET_NAME} - PRIVATE - app.cc - app.h - main.cc -) - -target_include_directories(${MUJOCO_STUDIO_TARGET_NAME} - PUBLIC - ${PROJECT_SOURCE_DIR}/include - ${PROJECT_SOURCE_DIR}/src -) - -if (WIN32) - target_compile_definitions(${MUJOCO_STUDIO_TARGET_NAME} - PRIVATE - -D_USE_MATH_DEFINES - ) - set_target_properties(${MUJOCO_STUDIO_TARGET_NAME} - PROPERTIES - VS_DEBUGGER_WORKING_DIRECTORY "${CMAKE_RUNTIME_OUTPUT_DIRECTORY}" - ) -endif() - include(third_party_deps/dear_imgui) include(third_party_deps/implot) include(third_party_deps/opensans) include(third_party_deps/font_awesome) -target_link_libraries(${MUJOCO_STUDIO_TARGET_NAME} - PRIVATE - absl::flags - absl::flags_parse - dear_imgui - implot - mujoco::mujoco - mujoco::platform -) - -# TODO: re-enable mjz support on Windows builds once DllMain issue is resolved. -if (NOT WIN32) - target_link_libraries(${MUJOCO_STUDIO_TARGET_NAME} +# Common configuration shared between mujoco_studio and mujoco_live. +function(configure_studio_target TARGET_NAME) + target_sources(${TARGET_NAME} PRIVATE - mujoco::mjz + app.cc + app.h ) -endif() -file(MAKE_DIRECTORY ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/assets) + target_include_directories(${TARGET_NAME} + PUBLIC + ${PROJECT_SOURCE_DIR}/include + ${PROJECT_SOURCE_DIR}/src + ) -add_custom_command( - TARGET ${MUJOCO_STUDIO_TARGET_NAME} - POST_BUILD - COMMAND ${CMAKE_COMMAND} - -E copy - ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/../_deps/opensans-src/fonts/ttf/OpenSans-Regular.ttf - ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/assets - COMMENT "Copying OpenSans-Regular.ttf assets to build directory" -) -add_custom_command( - TARGET ${MUJOCO_STUDIO_TARGET_NAME} - POST_BUILD - COMMAND ${CMAKE_COMMAND} - -E copy - ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/../_deps/font_awesome-src/fonts/fontawesome-webfont.ttf - ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/assets - COMMENT "Copying fontawesome-webfont.ttf to build directory" -) + if (WIN32) + target_compile_definitions(${TARGET_NAME} + PRIVATE + -D_USE_MATH_DEFINES + ) + set_target_properties(${TARGET_NAME} + PROPERTIES + VS_DEBUGGER_WORKING_DIRECTORY "${CMAKE_RUNTIME_OUTPUT_DIRECTORY}" + ) + endif() + + target_link_libraries(${TARGET_NAME} + PRIVATE + absl::flags + absl::flags_parse + dear_imgui + implot + mujoco::mujoco + mujoco::platform + ) + + # TODO: re-enable mjz support on Windows builds once DllMain issue is resolved. + if (NOT WIN32) + target_link_libraries(${TARGET_NAME} + PRIVATE + mujoco::mjz + ) + endif() + + file(MAKE_DIRECTORY ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/assets) -# Filament backend requires additional files to be copied into an "assets" folder. -if(MUJOCO_USE_FILAMENT) add_custom_command( - TARGET ${MUJOCO_STUDIO_TARGET_NAME} + TARGET ${TARGET_NAME} POST_BUILD COMMAND ${CMAKE_COMMAND} - -E copy_directory - ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/../src/experimental/filament/assets + -E copy + ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/../_deps/opensans-src/fonts/ttf/OpenSans-Regular.ttf ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/assets - COMMENT "Copying Filament assets to build directory" + COMMENT "Copying OpenSans-Regular.ttf assets to build directory" + ) + add_custom_command( + TARGET ${TARGET_NAME} + POST_BUILD + COMMAND ${CMAKE_COMMAND} + -E copy + ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/../_deps/font_awesome-src/fonts/fontawesome-webfont.ttf + ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/assets + COMMENT "Copying fontawesome-webfont.ttf to build directory" + ) + + # Filament backend requires additional files to be copied into an "assets" folder. + if(MUJOCO_USE_FILAMENT) + add_custom_command( + TARGET ${TARGET_NAME} + POST_BUILD + COMMAND ${CMAKE_COMMAND} + -E copy_directory + ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/../src/experimental/filament/assets + ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/assets + COMMENT "Copying Filament assets to build directory" + ) + endif() +endfunction() + +# Desktop Studio target. +if(NOT EMSCRIPTEN) + add_executable(mujoco_studio) + target_sources(mujoco_studio PRIVATE main.cc) + configure_studio_target(mujoco_studio) +endif() + +# WASM Live target. +if(EMSCRIPTEN) + add_executable(mujoco_live) + target_sources(mujoco_live PRIVATE wasm.cc) + target_compile_options(mujoco_live PRIVATE -g) + configure_studio_target(mujoco_live) + target_link_libraries(mujoco_live PRIVATE mujoco::render_noop) + + # Ensure resource decoder plugins are linked into the WASM binary. + target_link_options(mujoco_live PRIVATE + -Wl,--whole-archive $ -Wl,--no-whole-archive + ) + + # Filament's OpenGL backend requires WebGL2 / full ES3 bindings. + target_link_options(mujoco_live PRIVATE + --bind + -sUSE_WEBGL2=1 + -sFULL_ES3 + -sMIN_WEBGL_VERSION=2 + -sMAX_WEBGL_VERSION=2 + -sALLOW_MEMORY_GROWTH=1 + -sASYNCIFY=1 + -sFETCH=1 + -sGL_PREINITIALIZED_CONTEXT=1 + -sSTACK_SIZE=512mb + -sINITIAL_MEMORY=1024mb + -sASSERTIONS=1 + -fexceptions + -g ) endif() diff --git a/src/experimental/studio/index.html b/src/experimental/studio/index.html index 8eb3190f..ec40e14e 100644 --- a/src/experimental/studio/index.html +++ b/src/experimental/studio/index.html @@ -1,8 +1,8 @@ - + - MuJoCo Studio! + MuJoCo Live
@@ -16,6 +16,28 @@ style="width: 100vw; height: 100vh; display: block" > - + diff --git a/src/experimental/studio/wasm.cc b/src/experimental/studio/wasm.cc index 8e5e8768..29da5294 100644 --- a/src/experimental/studio/wasm.cc +++ b/src/experimental/studio/wasm.cc @@ -18,7 +18,11 @@ #include #include +#include +#include +#include #include +#include #include #include #include @@ -59,6 +63,84 @@ class AssetRegistry { std::unordered_map assets_; }; +// --------------------------------------------------------------------------- +// HTTP/HTTPS resource fetching via the JS fetch API (uses ASYNCIFY to yield). +// --------------------------------------------------------------------------- + +// Fetches a URL using the JS fetch API. Returns a malloc'd buffer and its size. +// The caller is responsible for freeing the buffer. Returns 0 on failure. +EM_ASYNC_JS(int, FetchUrl, + (const char* url, char** out_data, std::int32_t* out_size), { + try { + const urlStr = UTF8ToString(url); + const response = await fetch(urlStr); + if (!response.ok) { + console.error('Fetch failed: ' + response.status + ' ' + + urlStr); + return 0; + } + const buffer = await response.arrayBuffer(); + const bytes = new Uint8Array(buffer); + const ptr = _malloc(bytes.length); + HEAPU8.set(bytes, ptr); + setValue(out_data, ptr, '*'); + setValue(out_size, bytes.length, 'i32'); + return 1; + } catch (e) { + console.error('Fetch error:', e); + return 0; + } + }); + +// Cache for data fetched via HTTP/HTTPS. Stores the downloaded bytes keyed by +// the resource name (URL) so that read() can return a pointer to the data. +class FetchCache { + public: + static FetchCache& Instance() { + static FetchCache instance; + return instance; + } + + // Fetches the URL and stores the result. Returns the size (>0) on success. + int Fetch(const char* url) { + char* data = nullptr; + std::int32_t size = 0; + if (!FetchUrl(url, &data, &size)) { + return 0; + } + entries_[url] = Entry{UniquePtrWasm(data), size}; + return size; + } + + // Returns pointer and size for a previously fetched URL. + int Read(const char* url, const void** buffer) { + auto it = entries_.find(url); + if (it == entries_.end()) { + return -1; + } + *buffer = it->second.data.get(); + return it->second.size; + } + + // Frees the data for a URL. + void Close(const char* url) { entries_.erase(url); } + + private: + struct FreeDeleter { + void operator()(void* p) const { std::free(p); } + }; + template + using UniquePtrWasm = std::unique_ptr; + + struct Entry { + UniquePtrWasm data; + int size; + }; + std::unordered_map entries_; +}; + +// --------------------------------------------------------------------------- + // Javascript-facing function to register an asset. void RegisterAsset(std::string filename, std::string contents) { AssetRegistry::Instance().RegisterAsset(std::move(filename), @@ -92,6 +174,27 @@ void Init() { resource_provider.prefix = "filament"; mjp_registerResourceProvider(&resource_provider); + // Register HTTP/HTTPS resource providers so that models loaded from URLs + // can automatically fetch referenced assets (meshes, textures, etc.) over + // the network. + mjpResourceProvider http_provider; + mjp_defaultResourceProvider(&http_provider); + + http_provider.open = [](mjResource* resource) { + return FetchCache::Instance().Fetch(resource->name); + }; + http_provider.read = [](mjResource* resource, const void** buffer) { + return FetchCache::Instance().Read(resource->name, buffer); + }; + http_provider.close = [](mjResource* resource) { + FetchCache::Instance().Close(resource->name); + }; + + http_provider.prefix = "http"; + mjp_registerResourceProvider(&http_provider); + http_provider.prefix = "https"; + mjp_registerResourceProvider(&http_provider); + g_app = new mujoco::studio::App({ .width = width, .height = height, @@ -124,6 +227,17 @@ void LoadFile(const std::string& filename, const std::string& data) { g_app->LoadModelFromBuffer({ptr, ptr + data.size()}, content_type, filename); } +// Javascript-facing function to load a model from a URL. +// The URL is passed directly to LoadModelFromFile, which will use the +// registered HTTP/HTTPS resource providers to fetch the model and any +// referenced assets. +void LoadUrl(const std::string& url) { + if (!g_app) { + return; + } + g_app->LoadModelFromFile(url); +} + // Javascript-facing function to render a single frame. void RenderFrame() { if (g_app) { @@ -144,6 +258,7 @@ EMSCRIPTEN_BINDINGS(studio_bindings) { emscripten::function("registerAsset", &RegisterAsset); emscripten::function("init", &Init); emscripten::function("loadFile", &LoadFile); + emscripten::function("loadUrl", &LoadUrl); emscripten::function("renderFrame", &RenderFrame); emscripten::function("deinit", &Deinit); } diff --git a/src/render/noop/CMakeLists.txt b/src/render/noop/CMakeLists.txt index 782c4858..313ca063 100644 --- a/src/render/noop/CMakeLists.txt +++ b/src/render/noop/CMakeLists.txt @@ -12,4 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -target_sources(mujoco PRIVATE render_noop.c) +add_library(render_noop STATIC render_noop.c) +target_link_libraries(render_noop PUBLIC mujoco::mujoco) +add_library(mujoco::render_noop ALIAS render_noop) From a04cf1b2b35723e234278f8bc83c79ac7b8d6b33 Mon Sep 17 00:00:00 2001 From: Michael Moss Date: Fri, 24 Apr 2026 02:25:58 -0700 Subject: [PATCH 135/251] Mention Python 3.14 in the changelog. PiperOrigin-RevId: 904898900 Change-Id: I5ba3d619b496cd038f8614df924fe9cec38b56f9 --- doc/changelog.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/changelog.rst b/doc/changelog.rst index e3f311b2..93a4312d 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,6 +7,7 @@ Upcoming version (not yet released) General ^^^^^^^ +- Added support for Python 3.14. - Added new :ref:`mj_maxContact` function to get the maximum number of possible contacts returned by two geoms. - Added ``mj_containsBufferVFS`` and ``mj_containsFileVFS`` to check for existence of buffers and files in VFS. From a2ee85f47799b625039602f9ef5948d24b46cc90 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Fri, 24 Apr 2026 06:41:48 -0700 Subject: [PATCH 136/251] Fix wasm flickering. Always execute the engine in non-threading (wasm) builds. And stop rendering if any beginFrame call returns false. PiperOrigin-RevId: 905000139 Change-Id: I0a6f06a07a8fc0ab0251a67be13a9d170d4a905d --- .../filament/filament/filament_context.cc | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index 8c159ce2..e367b44f 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -118,6 +118,9 @@ FilamentContext::FrameHandle FilamentContext::Render( if (!render_began) { render_began = renderer_->beginFrame(window_swap_chain_); } + if (!render_began) { + break; + } if (render_began) { SceneView::RenderRequest scene_view_request; scene_view_request.draw_mode = request.draw_mode; @@ -139,6 +142,9 @@ FilamentContext::FrameHandle FilamentContext::Render( if (!render_began) { render_began = renderer_->beginFrame(offscreen_swap_chain_); } + if (!render_began) { + break; + } if (render_began) { SceneView::RenderRequest scene_view_request; scene_view_request.draw_mode = request.draw_mode; @@ -154,10 +160,9 @@ FilamentContext::FrameHandle FilamentContext::Render( if (render_began) { renderer_->endFrame(); - render_began = false; - if constexpr (!UTILS_HAS_THREADING) { - engine_->execute(); - } + } + if constexpr (!UTILS_HAS_THREADING) { + engine_->execute(); } if (!read_requests.empty()) { From 2f5e5d3da120f80152522df945cb908f7d1e39ae Mon Sep 17 00:00:00 2001 From: Michael Moss Date: Fri, 24 Apr 2026 08:37:43 -0700 Subject: [PATCH 137/251] Add documentation for mjpDecoder. PiperOrigin-RevId: 905048609 Change-Id: I6342673cdc53b80406ad77fc95cb8bb5790a5df6 --- doc/changelog.rst | 5 ++ doc/programming/extension.rst | 137 +++++++++++++++++++++++++++++++++- 2 files changed, 140 insertions(+), 2 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 93a4312d..af200ca1 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -25,6 +25,11 @@ General **Migration:** The flag :ref:`multiccd` must be explicitly disabled. +Documentation +^^^^^^^^^^^^^ + +* Added :ref:`documentation` for :ref:`mjpDecoder` plugins. + Bug fixes ^^^^^^^^^ diff --git a/doc/programming/extension.rst b/doc/programming/extension.rst index 720f5829..bd2df7e9 100644 --- a/doc/programming/extension.rst +++ b/doc/programming/extension.rst @@ -3,8 +3,8 @@ Extensions ---------- -This section describes MuJoCo's mechanisms for user-authored extensions. At present, extensibility is provided -via :ref:`engine plugins` and :ref:`resource providers`. +This section describes MuJoCo's mechanisms for user-authored extensions. At present, extensibility is provided by +via :ref:`engine plugins`, :ref:`decoders`, and :ref:`resource providers`. .. _exPlugin: @@ -337,6 +337,139 @@ For the sdf plugin, the following methods need to be specified Computes the axis-aligned bounding box in local coordinates. This volume is voxelized uniformly before the call to the marching cubes algorithm. +.. _exDecoder: + +Decoders +~~~~~~~~ + +Decoder plugins extend asset loading capabilities beyond MJCF and URDF. They are :ref:`registered` +similarly to other MuJoCo plugins. + +MuJoCo ships with two built-in decoders for common mesh formats: + +- **OBJ decoder** (``plugin/obj_decoder``) -- `Wavefront OBJ `_. +- **STL decoder** (``plugin/stl_decoder``) -- `STL `_. + +Additionally, we provide the following optional decoder plugins: + +- **USD decoder** (``plugin/usd_decoder``) -- `Universal Scene Description `_. + +These plugins also serve as examples for how to write custom decoders. The obj decoder is perhaps the simplest to +understand, while the USD decoder is more complex due to its support for entire scenes. + +.. _exDecoderInterface: + +Decoder interface +^^^^^^^^^^^^^^^^^ + +A decoder is described by the :ref:`mjpDecoder` struct, which has the following fields: + +``content_type`` + A MIME-like content type string identifying the format. For example, ``"model/obj"``, or ``"model/stl"``. + When a mesh asset specifies a ``content-type`` attribute in MJCF, this string is used + to find the appropriate decoder. + +``extension`` + A file extension string (including the dot) used for matching when no content type is specified. Multiple + extensions can be separated by pipes (`|`) for formats with multiple extensions such as ``.usd|.usda|.usdc|.usdz``. + +``can_decode`` + A callback of type :ref:`mjfCanDecode` that determines whether the decoder can handle a given resource. This is + typically implemented by checking the file extension but may also check the file contents to differentiate between + formats. For example, URDF and MJCF files both have a ``.xml`` extension. Returns nonzero if the decoder can handle + the resource. + +``decode`` + A callback of type :ref:`mjfDecode` that performs the actual decoding. It receives an :ref:`mjResource` and + returns a newly allocated :ref:`mjSpec` containing the decoded asset data. The caller takes + ownership of the returned spec and is responsible for freeing it with :ref:`mj_deleteSpec`. Returns ``NULL`` on + failure. + +When a decoder is invoked for a mesh asset, the compiler will reference the first mesh element in the spec returned +by the ``decode`` callback. + +When a decoder is invoked for a model asset, the spec returned by the ``decode`` callback may contain any number of +elements of any type. + +.. _exDecoderRegistration: + +Registration +^^^^^^^^^^^^ + +Decoders must be registered before they can be used. Registration is performed via +:ref:`mjp_registerDecoder`. The :ref:`mjp_defaultDecoder` function initializes an :ref:`mjpDecoder` struct with +default values. The :ref:`mjPLUGIN_LIB_INIT` macro is used to define the initialization function that registers the +decoder when the library is loaded. + +.. code-block:: C + + mjPLUGIN_LIB_INIT(my_format_decoder) { + mjpDecoder decoder; + mjp_defaultDecoder(&decoder); + decoder.content_type = "model/my-format"; + decoder.extension = ".myf|.myfa|.myfc"; + decoder.decode = MyDecode; + decoder.can_decode = MyCanDecode; + mjp_registerDecoder(&decoder); + } + + +.. _exDecoderExample: + +Example +^^^^^^^ + +Below is a minimal decoder that reads a hypothetical binary mesh format: + +.. code-block:: C + + #include + + static mjSpec* MyDecode(mjResource* resource, const mjVFS* vfs) { + const void* bytes = NULL; + int nbytes = mju_readResource(resource, &bytes); + if (nbytes < 0) { + mju_warning("failed to read resource '%s'", resource->name); + return NULL; + } + + /* ... parse bytes into vertex/face arrays ... */ + + mjSpec* spec = mj_makeSpec(); + mjsMesh* mesh = mjs_addMesh(spec, NULL); + mjs_setString(mesh->file, resource->name); + mjs_setFloat(mesh->uservert, vertices, nvert * 3); + mjs_setInt(mesh->userface, faces, nface * 3); + return spec; + } + + static int MyCanDecode(const mjResource* resource) { + /* check file extension */ + const char* name = resource->name; + int len = strlen(name); + return len > 4 && strcmp(name + len - 4, ".myf") == 0; + } + + mjPLUGIN_LIB_INIT(my_format_decoder) { + mjpDecoder decoder; + mjp_defaultDecoder(&decoder); + decoder.content_type = "model/my-format"; + decoder.extension = ".myf"; + decoder.decode = MyDecode; + decoder.can_decode = MyCanDecode; + mjp_registerDecoder(&decoder); + } + +Once registered, the decoder is used automatically when MuJoCo encounters an asset with a matching file extension +or content type: + +.. code-block:: xml + + + + + + .. _exProvider: Resource providers From 34d69ad4cb1a21846b8297e2bc5e68a4938276c1 Mon Sep 17 00:00:00 2001 From: Michael Moss Date: Fri, 24 Apr 2026 09:55:48 -0700 Subject: [PATCH 138/251] Update changelog for the 3.8.0 release. PiperOrigin-RevId: 905086535 Change-Id: Id5b32348465b9fa756a8d60234f5c300ded7a27b --- doc/changelog.rst | 37 +++++++++++++++++++------------------ 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index af200ca1..b6ff32ef 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,40 +2,41 @@ Changelog ========= -Upcoming version (not yet released) ------------------------------------ +Version 3.8.0 (April 24, 2026) +------------------------------ General ^^^^^^^ -- Added support for Python 3.14. -- Added new :ref:`mj_maxContact` function to get the maximum number of possible contacts returned by - two geoms. -- Added ``mj_containsBufferVFS`` and ``mj_containsFileVFS`` to check for existence of buffers and files in VFS. -- Added :ref:`multi-cell support` for trilinear and quadratic flexes. Note that the implicit - integrator uses a dense solver for the flex degrees of freedom, which can be slow for multi-cell flexes. -- Refactored ``flexstrain`` equality constraints to be instantiated per cell instead of per flex object, reducing the - number of degrees of freedom per constraint row. The equality can be associated with a specific cell with the new - attribute ":ref:`cell ` +1. Added support for Python 3.14. +2. Added :ref:`multi-cell support` for trilinear and quadratic flexes. Note that the implicit + integrator uses a dense solver for the flex degrees of freedom, which can be slow for multi-cell flexes. +3. Refactored ``strain`` flex :ref:`equality constraints` to be instantiated per cell instead of + per flex object, reducing the number of degrees of freedom per constraint row. The equality can be associated with a + specific cell with the new attribute :ref:`cell ` +4. Added new :ref:`mj_maxContact` function to get the maximum number of possible contacts returned by + colliding two geoms. +5. Added ``mj_containsBufferVFS`` and ``mj_containsFileVFS`` to check for existence of buffers and files in VFS. .. admonition:: Breaking API changes :class: attention - - The feature :ref:`multiccd` is now enabled by default. This feature has little performance overhead - and gives better contact behavior for stability. + 6. The :ref:`multiccd` option (multiple contacts returned from the convex collision detection pipeline) + is now enabled by default. The new implementation (as opposed to the legacy pipeline) has little performance + overhead and improves stability. - **Migration:** The flag :ref:`multiccd` must be explicitly disabled. + **Migration:** Disable :ref:`multiccd` to recover the previous behavior. Documentation ^^^^^^^^^^^^^ -* Added :ref:`documentation` for :ref:`mjpDecoder` plugins. +7. Added :ref:`documentation` for :ref:`mjpDecoder` plugins. Bug fixes ^^^^^^^^^ -- Asset paths in attached child specs are now resolved relative to the model file directory of the child spec, rather - than the parent spec. This prevents the origin of the parent spec to affect the resolution of asset paths in the child - spec. +8. Asset paths in attached child specs are now resolved relative to the model file directory of the child spec, rather + than the parent spec. This prevents the origin of the parent spec to affect the resolution of asset paths in the + child spec. Version 3.7.0 (April 14, 2026) ------------------------------ From 2b7cc28afac4f28ea42df5c8caacb242875ee6f3 Mon Sep 17 00:00:00 2001 From: Michael Moss Date: Fri, 24 Apr 2026 10:37:13 -0700 Subject: [PATCH 139/251] Update MuJoCo version to 3.8.1 following the 3.8.0 release PiperOrigin-RevId: 905108575 Change-Id: Ief1f9c6f34559e7a5ccc60dcceec589316bcdf9e --- CMakeLists.txt | 2 +- dist/mujoco.rc | 8 ++++---- dist/simulate.rc | 8 ++++---- doc/APIreference/APIglobals.rst | 2 +- doc/unity.rst | 4 ++-- include/mujoco/mujoco.h | 2 +- mjx/pyproject.toml | 8 ++++---- python/mujoco/CMakeLists.txt | 4 ++-- python/mujoco/mjpython/Info.plist | 8 ++++---- python/pyproject.toml | 6 +++--- sample/CMakeLists.txt | 2 +- simulate/CMakeLists.txt | 2 +- src/engine/engine_support.c | 4 ++-- unity/Editor/Bindings/MujocoBinaryRetriever.cs | 4 ++-- unity/Runtime/Bindings/MjBindings.cs | 2 +- unity/package.json | 2 +- 16 files changed, 34 insertions(+), 34 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c6a8f634..14fb6eec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco - VERSION 3.8.0 + VERSION 3.8.1 DESCRIPTION "MuJoCo Physics Simulator" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/dist/mujoco.rc b/dist/mujoco.rc index 7411149b..6400ab0d 100644 --- a/dist/mujoco.rc +++ b/dist/mujoco.rc @@ -1,6 +1,6 @@ 1 VERSIONINFO -FILEVERSION 3,8,0,0 -PRODUCTVERSION 3,8,0,0 +FILEVERSION 3,8,1,0 +PRODUCTVERSION 3,8,1,0 FILEOS 0x4 FILETYPE 0x1 { @@ -9,9 +9,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.8.0" + VALUE "ProductVersion", "3.8.1" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.8.0" + VALUE "FileVersion", "3.8.1" VALUE "InternalName", "mujoco.dll" VALUE "OriginalFilename", "mujoco.dll" VALUE "CompanyName", "Google DeepMind" diff --git a/dist/simulate.rc b/dist/simulate.rc index 87fdacb9..a7696780 100644 --- a/dist/simulate.rc +++ b/dist/simulate.rc @@ -1,8 +1,8 @@ MUJOCO ICON "mujoco.ico" 1 VERSIONINFO -FILEVERSION 3,8,0,0 -PRODUCTVERSION 3,8,0,0 +FILEVERSION 3,8,1,0 +PRODUCTVERSION 3,8,1,0 FILEOS 0x4 FILETYPE 0x1 { @@ -11,9 +11,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.8.0" + VALUE "ProductVersion", "3.8.1" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.8.0" + VALUE "FileVersion", "3.8.1" VALUE "InternalName", "simulate.exe" VALUE "OriginalFilename", "simulate.exe" VALUE "CompanyName", "Google DeepMind" diff --git a/doc/APIreference/APIglobals.rst b/doc/APIreference/APIglobals.rst index b5da5d55..dae8fccd 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -388,7 +388,7 @@ Defined in `mujoco.h diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index dc0583df..65b614b9 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name="mujoco-mjx" -version = "3.8.0" +version = "3.8.1" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -30,7 +30,7 @@ dependencies = [ "etils[epath]", "jax", "jaxlib", - "mujoco>=3.8.0.dev0", + "mujoco>=3.8.1.dev0", "scipy", "trimesh", ] @@ -46,9 +46,9 @@ mjx-viewer = "mujoco.mjx.viewer:main" [project.urls] Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Documentation = "https://mujoco.readthedocs.io/en/3.8.0" +Documentation = "https://mujoco.readthedocs.io/en/3.8.1" Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Changelog = "https://mujoco.readthedocs.io/en/3.8.0/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.8.1/changelog.html" [tool.isort] force_single_line = true diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index 546ba27d..35eeb831 100644 --- a/python/mujoco/CMakeLists.txt +++ b/python/mujoco/CMakeLists.txt @@ -86,7 +86,7 @@ if(NOT TARGET mujoco) if(MUJOCO_FRAMEWORK) message("MuJoCo framework is at ${MUJOCO_FRAMEWORK}/mujoco.framework") set(MUJOCO_LIBRARY - ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.8.0.dylib + ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.8.1.dylib ) target_compile_options(mujoco INTERFACE -F${MUJOCO_FRAMEWORK}) endif() @@ -94,7 +94,7 @@ if(NOT TARGET mujoco) if(NOT MUJOCO_FRAMEWORK) find_library( - MUJOCO_LIBRARY mujoco mujoco.3.8.0 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED + MUJOCO_LIBRARY mujoco mujoco.3.8.1 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED ) find_path(MUJOCO_INCLUDE mujoco/mujoco.h HINTS ${MUJOCO_INCLUDE_DIR} REQUIRED) message("MuJoCo is at ${MUJOCO_LIBRARY}") diff --git a/python/mujoco/mjpython/Info.plist b/python/mujoco/mjpython/Info.plist index 8f2939fa..f18c2a35 100644 --- a/python/mujoco/mjpython/Info.plist +++ b/python/mujoco/mjpython/Info.plist @@ -7,13 +7,13 @@ CFBundleIdentifier org.mujoco.mjpython CFBundleVersion - 3.8.0 + 3.8.1 CFBundleGetInfoString - 3.8.0 + 3.8.1 CFBundleLongVersionString - 3.8.0 + 3.8.1 CFBundleShortVersionString - 3.8.0 + 3.8.1 CFBundleExecutable mjpython CFBundleIconFile diff --git a/python/pyproject.toml b/python/pyproject.toml index b323d778..8621f35c 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "mujoco" -version = "3.8.0" +version = "3.8.1" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -35,9 +35,9 @@ dynamic = ["readme", "scripts"] [project.urls] Homepage = "https://github.com/google-deepmind/mujoco" -Documentation = "https://mujoco.readthedocs.io/en/3.8.0" +Documentation = "https://mujoco.readthedocs.io/en/3.8.1" Repository = "https://github.com/google-deepmind/mujoco" -Changelog = "https://mujoco.readthedocs.io/en/3.8.0/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.8.1/changelog.html" [tool.setuptools] include-package-data = false diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt index b83e8e63..deb9cf63 100644 --- a/sample/CMakeLists.txt +++ b/sample/CMakeLists.txt @@ -24,7 +24,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco_samples - VERSION 3.8.0 + VERSION 3.8.1 DESCRIPTION "MuJoCo samples binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/simulate/CMakeLists.txt b/simulate/CMakeLists.txt index a6a136fe..2e8cee6a 100644 --- a/simulate/CMakeLists.txt +++ b/simulate/CMakeLists.txt @@ -29,7 +29,7 @@ set(MUJOCO_DEP_VERSION_lodepng project( mujoco_simulate - VERSION 3.8.0 + VERSION 3.8.1 DESCRIPTION "MuJoCo simulate binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index d00cebf8..c68a5d5c 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -43,8 +43,8 @@ //-------------------------- Constants ------------------------------------------------------------- - #define mjVERSION 3008000 -#define mjVERSIONSTRING "3.8.0" + #define mjVERSION 3008001 +#define mjVERSIONSTRING "3.8.1" // names of disable flags const char* mjDISABLESTRING[mjNDISABLE] = { diff --git a/unity/Editor/Bindings/MujocoBinaryRetriever.cs b/unity/Editor/Bindings/MujocoBinaryRetriever.cs index 321bd610..be5112ab 100644 --- a/unity/Editor/Bindings/MujocoBinaryRetriever.cs +++ b/unity/Editor/Bindings/MujocoBinaryRetriever.cs @@ -37,7 +37,7 @@ public class MujocoBinaryRetriever { if (AssetDatabase.LoadMainAssetAtPath(mujocoPath + "/mujoco.dylib") == null) { File.Copy( "/Applications/MuJoCo.app/Contents/Frameworks" + - "/mujoco.framework/Versions/Current/libmujoco.3.8.0.dylib", + "/mujoco.framework/Versions/Current/libmujoco.3.8.1.dylib", mujocoPath + "/mujoco.dylib"); AssetDatabase.Refresh(); } @@ -45,7 +45,7 @@ public class MujocoBinaryRetriever { if (AssetDatabase.LoadMainAssetAtPath(mujocoPath + "/libmujoco.so") == null) { File.Copy( Environment.GetFolderPath(Environment.SpecialFolder.UserProfile) + - "/.mujoco/mujoco-3.8.0/lib/libmujoco.so.3.8.0", + "/.mujoco/mujoco-3.8.1/lib/libmujoco.so.3.8.1", mujocoPath + "/libmujoco.so"); AssetDatabase.Refresh(); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index a25bef87..7f8d625d 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -113,7 +113,7 @@ public const int mjMAXLINEPNT = 1001; public const int mjMAXPLANEGRID = 200; public const bool THIRD_PARTY_MUJOCO_MJXMACRO_H_ = true; public const bool THIRD_PARTY_MUJOCO_MUJOCO_H_ = true; -public const int mjVERSION_HEADER = 3008000; +public const int mjVERSION_HEADER = 3008001; // ------------------------------------Enums------------------------------------ diff --git a/unity/package.json b/unity/package.json index ef30e334..4e9bf364 100644 --- a/unity/package.json +++ b/unity/package.json @@ -1,7 +1,7 @@ { "name": "org.mujoco", "displayName": "MuJoCo", - "version": "3.8.0", + "version": "3.8.1", "description": "MuJoCo importer and runtime plug-in", "dependencies": {}, "author": { From 4eb987ad2557cf448fc2b61473bb6409b68e50eb Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Fri, 24 Apr 2026 12:37:01 -0700 Subject: [PATCH 140/251] Fix uninitialized value. PiperOrigin-RevId: 905169930 Change-Id: I8df07e8731e8eb1847bc43388f26654d28bcef4c --- src/experimental/filament/filament/material.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/experimental/filament/filament/material.cc b/src/experimental/filament/filament/material.cc index a5402dde..fcdc54fa 100644 --- a/src/experimental/filament/filament/material.cc +++ b/src/experimental/filament/filament/material.cc @@ -49,8 +49,8 @@ void mjr_defaultMaterialTextures(mjrMaterialTextures* textures) { void mjr_defaultMaterialParams(mjrMaterialParams* params) { setf(params->color, {1.f, 1.f, 1.f, 1.f}); setf(params->segmentation_color, {1, 1, 1, 1}); - setf(params->uv_scale, {1, 1}); - setf(params->uv_offset, {0, 0}); + setf(params->uv_scale, {1, 1, 1}); + setf(params->uv_offset, {0, 0, 0}); setf(params->scissor, {0, 0, 0, 0}); params->emissive = -1.0f; From 2f2d00daed03c4413cf04401b23b78d865d51358 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sat, 25 Apr 2026 14:59:53 -0700 Subject: [PATCH 141/251] Fix issues in changelog. PiperOrigin-RevId: 905663581 Change-Id: Ic5146db3282e303b2a5676e77e27dea3100bf072 --- doc/changelog.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index b6ff32ef..79c01e44 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -8,7 +8,7 @@ Version 3.8.0 (April 24, 2026) General ^^^^^^^ 1. Added support for Python 3.14. -2. Added :ref:`multi-cell support` for trilinear and quadratic flexes. Note that the implicit +2. Added :ref:`multi-cell support` for trilinear and quadratic flexes. Note that the implicit integrator uses a dense solver for the flex degrees of freedom, which can be slow for multi-cell flexes. 3. Refactored ``strain`` flex :ref:`equality constraints` to be instantiated per cell instead of per flex object, reducing the number of degrees of freedom per constraint row. The equality can be associated with a @@ -17,7 +17,7 @@ General colliding two geoms. 5. Added ``mj_containsBufferVFS`` and ``mj_containsFileVFS`` to check for existence of buffers and files in VFS. - .. admonition:: Breaking API changes +.. admonition:: Breaking API changes :class: attention 6. The :ref:`multiccd` option (multiple contacts returned from the convex collision detection pipeline) From 82eab11a88ce0cc150ea3f98221c3027a0800a71 Mon Sep 17 00:00:00 2001 From: Matija Kecman Date: Sun, 26 Apr 2026 05:18:54 -0700 Subject: [PATCH 142/251] Remove deprecated Fonts->Build() and clean up ImGui texture lifecycle * Remove the explicit io.Fonts->Build() call from InitImGui, which was deprecated in Dear ImGui v1.92.0. With ImGuiBackendFlags_RendererHasTextures already set, the font atlas is now built lazily by ImGui and communicated to backends via the ImTextureStatus_WantCreate protocol. * Add proper texture shutdown in ~ImguiBridge by iterating ImGui::GetPlatformIO().Textures and destroying all tracked textures, per the //third_party/dear_imgui/docs/BACKENDS.md instructions. * Remove the "OK but missing" and "WantUpdates but missing" texture recovery hacks from Update(). These handled a state where ImGui thought textures were alive but the Filament-side storage had been torn down. With proper destruction acknowledgement (SetStatus(Destroyed) + SetTexID(Invalid)), ImGui will now transition destroyed textures to WantCreate on the next frame, and the normal creation path handles it cleanly. PiperOrigin-RevId: 905886622 Change-Id: Ic449eeaf083740a67eae9c637fb53546a6192b28 --- .../filament/compat/imgui_bridge.cc | 34 +++++++++---------- src/experimental/platform/hal/window.cc | 12 ++----- 2 files changed, 19 insertions(+), 27 deletions(-) diff --git a/src/experimental/filament/compat/imgui_bridge.cc b/src/experimental/filament/compat/imgui_bridge.cc index 8b43a3a4..de21a303 100644 --- a/src/experimental/filament/compat/imgui_bridge.cc +++ b/src/experimental/filament/compat/imgui_bridge.cc @@ -26,8 +26,8 @@ #include #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/mesh.h" -#include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/scene_view.h" #include "experimental/filament/filament/texture.h" @@ -36,15 +36,25 @@ namespace mujoco { using filament::math::float3; using filament::math::mat3f; -ImguiBridge::ImguiBridge(ObjectManager* object_mgr) - : object_mgr_(object_mgr) { +ImguiBridge::ImguiBridge(ObjectManager* object_mgr) : object_mgr_(object_mgr) { scene_view_ = std::make_unique(object_mgr_->GetEngine()); scene_view_->DisableShadows(); scene_view_->DisableReflections(); scene_view_->DisablePostProcessing(); } -ImguiBridge::~ImguiBridge() { PrepareRenderables(0); } +ImguiBridge::~ImguiBridge() { + PrepareRenderables(0); + + // Destroy all textures tracked by ImGui. + if (ImGui::GetCurrentContext()) { + for (ImTextureData* tex : ImGui::GetPlatformIO().Textures) { + if (tex->Status != ImTextureStatus_Destroyed) { + DestroyTexture(tex); + } + } + } +} uintptr_t ImguiBridge::UploadImage(uintptr_t tex_id, const uint8_t* pixels, int width, int height, int bpp) { @@ -183,22 +193,10 @@ void ImguiBridge::Update() { if (commands->Textures != nullptr) { for (ImTextureData* tex : *commands->Textures) { - if (tex->Status == ImTextureStatus_OK) { - // ImGui's lifecycle is independent of the filament context lifecycle. - // As such, it is possible to destroy and create a new filament context - // while ImGui is still expecting the "OK" textures to work. In this - // case, we simply recreate the texture. - if (textures_.find(tex->TexID) == textures_.end()) { - CreateTexture(tex); - } - } else if (tex->Status == ImTextureStatus_WantCreate) { + if (tex->Status == ImTextureStatus_WantCreate) { CreateTexture(tex); } else if (tex->Status == ImTextureStatus_WantUpdates) { - if (textures_.find(tex->TexID) == textures_.end()) { - CreateTexture(tex); - } else { - UpdateTexture(tex); - } + UpdateTexture(tex); } else if (tex->Status == ImTextureStatus_WantDestroy && tex->UnusedFrames > 0) { DestroyTexture(tex); diff --git a/src/experimental/platform/hal/window.cc b/src/experimental/platform/hal/window.cc index 84941444..961e9ab9 100644 --- a/src/experimental/platform/hal/window.cc +++ b/src/experimental/platform/hal/window.cc @@ -47,8 +47,8 @@ extern void* GetNativeWindowOsx(void* window); namespace mujoco::platform { -static void InitImGui(SDL_Window* window, float content_scale, bool load_fonts, - bool build_fonts) { +static void InitImGui(SDL_Window* window, float content_scale, + bool load_fonts) { ImGui::CreateContext(); ImGuiIO& io = ImGui::GetIO(); @@ -85,10 +85,6 @@ static void InitImGui(SDL_Window* window, float content_scale, bool load_fonts, constexpr ImWchar icon_ranges[] = {0xf000, 0xf3ff, 0x000}; io.Fonts->AddFontFromMemoryTTF(data, size, 14.f, &icon_cfg, icon_ranges); - if (build_fonts) { - io.Fonts->Build(); - } - // Note: we purposefully do not "close" the font resources as ImGui may // need them again to resize fonts. } @@ -133,9 +129,7 @@ Window::Window(std::string_view title, int width, int height, Config config) mju_error("Error creating window: %s", SDL_GetError()); } - InitImGui(sdl_window_, content_scale, config.load_fonts, - (config_.gfx_mode != GraphicsMode::ClassicOpenGl && - config_.gfx_mode != GraphicsMode::ClassicOpenGlHeadless)); + InitImGui(sdl_window_, content_scale, config.load_fonts); // Filament (except WebGL) manages its own swap chain including when to swap. // In all other cases, we'll use SDL to manage the swap chain. From 2fa2db7ae0ebc0e55e93e233f668fd6048dc681c Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Sun, 26 Apr 2026 07:33:50 -0700 Subject: [PATCH 143/251] Re-enable spotlights in wasm builds. PiperOrigin-RevId: 905917482 Change-Id: Iaadd2b2ccf3be91ec2edfb8ecfd40db32909dd4e --- src/experimental/filament/compat/scene_bridge.cc | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/experimental/filament/compat/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc index 4f00828d..6ab29a36 100644 --- a/src/experimental/filament/compat/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -260,10 +260,7 @@ void SceneBridge::PrepareLights() { } auto light_obj = std::make_unique(engine, params); -#ifndef __EMSCRIPTEN__ - // TODO(b/458045799): Re-enable when lights work on glinux and chromebook. scene_view_->AddToScene(light_obj.get()); -#endif lights_.emplace_back(std::move(light_obj)); } } @@ -283,10 +280,7 @@ void SceneBridge::PrepareLights() { params.intensity = 0.0f; params.spot_cone_angle = 90.0f; auto light_obj = std::make_unique(engine, params); -#ifndef __EMSCRIPTEN__ - // TODO(b/458045799): Re-enable when lights work on glinux and chromebook. scene_view_->AddToScene(light_obj.get()); -#endif lights_.emplace_back(std::move(light_obj)); } From 8f32f6e72ef95e192e15659691890975e01450cc Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Sun, 26 Apr 2026 14:03:15 -0700 Subject: [PATCH 144/251] Rename enum values to match type name. PiperOrigin-RevId: 906014884 Change-Id: I0f37ab0a0a910724723fb9f7e477b64643ca05fc --- .../filament/filament/filament_platform_factory.cc | 6 +++--- src/experimental/filament/render_context_filament.h | 10 +++++----- src/experimental/platform/hal/renderer.cc | 5 +++-- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/experimental/filament/filament/filament_platform_factory.cc b/src/experimental/filament/filament/filament_platform_factory.cc index 4b5483fc..a7dc9709 100644 --- a/src/experimental/filament/filament/filament_platform_factory.cc +++ b/src/experimental/filament/filament/filament_platform_factory.cc @@ -31,13 +31,13 @@ static filament::Engine::Backend ResolveBackend(int graphics_api) { #endif switch (graphics_api) { - case mjGFX_DEFAULT: + case mjGRAPHICS_API_DEFAULT: // Use the default based on the platform above. break; - case mjGFX_OPENGL: + case mjGRAPHICS_API_OPENGL: backend = filament::Engine::Backend::OPENGL; break; - case mjGFX_VULKAN: + case mjGRAPHICS_API_VULKAN: backend = filament::Engine::Backend::VULKAN; break; default: diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index 9db6554f..28183fec 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -29,11 +29,11 @@ extern "C" { // IMPORTANT: This API should still be considered experimental and is likely // change frequently. -typedef enum mjtGraphicsApi_ { // backend graphics API to use - mjGFX_DEFAULT = 0, // default based on platform - mjGFX_OPENGL, // OpenGL (desktop) - mjGFX_VULKAN // Vulkan -} mjtGraphicsApi; +typedef enum mjrGraphicsApi_ { // backend graphics API to use + mjGRAPHICS_API_DEFAULT = 0, // default based on platform + mjGRAPHICS_API_OPENGL, // OpenGL (desktop) / WebGL + mjGRAPHICS_API_VULKAN // Vulkan +} mjrGraphicsApi; struct mjrFilamentConfig { // The native window handle into which we can render directly. diff --git a/src/experimental/platform/hal/renderer.cc b/src/experimental/platform/hal/renderer.cc index 2dc60e18..a023c54e 100644 --- a/src/experimental/platform/hal/renderer.cc +++ b/src/experimental/platform/hal/renderer.cc @@ -90,8 +90,9 @@ void Renderer::Init(const mjModel* model) { render_config.width = model->vis.global.offwidth; render_config.height = model->vis.global.offheight; render_config.force_software_rendering = IsSoftware(gfx_); - render_config.graphics_api = - IsOpenGl(gfx_) || IsWebGl(gfx_) ? mjGFX_OPENGL : mjGFX_VULKAN; + render_config.graphics_api = IsOpenGl(gfx_) || IsWebGl(gfx_) + ? mjGRAPHICS_API_OPENGL + : mjGRAPHICS_API_VULKAN; mjrf_makeFilamentContext(model, &render_context_, &render_config); render_ = [&](mjrRect rect, mjvScene* scene) { mjrf_render(rect, scene, &render_context_); From 521d152ec544d51205b372192ecb9416ef746689 Mon Sep 17 00:00:00 2001 From: Google DeepMind Date: Sun, 26 Apr 2026 14:18:49 -0700 Subject: [PATCH 145/251] Enable Warp execution on CPU devices in MJX. This change modifies the Warp JAX FFI to register callbacks for both CUDA and CPU platforms, allowing Warp kernels to be launched on CPU devices. MJX's io.py is updated to allow Warp to use CPU devices if no CUDA GPU is available. Tests are adjusted to no longer skip when CUDA GPUs are absent. A minor fix for sorting precision in test_util.py is also included. Kristian Hartikainen - https://github.com/google-deepmind/mujoco/pull/2948 Github issue - https://github.com/google-deepmind/mujoco/issues/2947 PiperOrigin-RevId: 906018844 Change-Id: I9c026b4a4e2a0276d5d3af3954fd027cf75b6d7a --- mjx/mujoco/mjx/_src/io.py | 30 ++-- mjx/mujoco/mjx/_src/io_test.py | 19 +-- .../warp/_src/jax_experimental/ffi.py | 159 ++++++++++++------ mjx/mujoco/mjx/warp/forward_test.py | 51 ++++++ mjx/mujoco/mjx/warp/smooth_test.py | 9 +- mjx/mujoco/mjx/warp/test_util.py | 8 +- 6 files changed, 191 insertions(+), 85 deletions(-) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 74937bc0..918b0fa4 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -96,16 +96,15 @@ def _resolve_device( return cpu_0 if impl == types.Impl.WARP: - # WARP implementation requires a CUDA GPU. - cuda_gpus = [d for d in jax.devices('cuda')] - if not cuda_gpus: - raise AssertionError( - 'No CUDA GPU devices found in' - f' jax.devices("cuda")={jax.devices("cuda")}.' - ) + # WARP implementation requires a CUDA GPU or CPU. + if has_cuda_gpu_device(): + cuda_gpus = [d for d in jax.devices('cuda')] + if cuda_gpus: + logging.debug('Picking default device: %s', cuda_gpus[0]) + return cuda_gpus[0] - logging.debug('Picking default device: %s', cuda_gpus[0]) - return cuda_gpus[0] + logging.debug('Picking default device for Warp: CPU') + return jax.devices('cpu')[0] raise ValueError(f'Unsupported implementation: {impl}') @@ -121,9 +120,12 @@ def _check_impl_device_compatibility( impl = types.Impl(impl) if impl == types.Impl.WARP: - if not _is_cuda_gpu_device(device): + is_cuda_device = _is_cuda_gpu_device(device) + is_cpu_device = device.platform == 'cpu' + if not (is_cuda_device or is_cpu_device): raise AssertionError( - f'Warp implementation requires a CUDA GPU device, got {device}.' + 'Warp implementation requires a CUDA GPU or CPU device, got ' + f'{device}.' ) _check_warp_installed() @@ -425,8 +427,6 @@ def _put_model_jax( return _strip_weak_type(model) - - def _put_model_warp( m: mujoco.MjModel, graph_mode: mjxw.types.GraphMode, @@ -719,8 +719,6 @@ def _make_data_jax( return d - - def _get_nested_attr(obj: Any, attr_name: str, split: str) -> Any: """Returns the nested attribute from an object.""" for part in attr_name.split(split): @@ -1084,8 +1082,6 @@ def _put_data_jax( return _strip_weak_type(data) - - # TODO(josechenf): Iterate on the keepalive implementation to make it easier to # use before OSS. def _put_data_cpp( diff --git a/mjx/mujoco/mjx/_src/io_test.py b/mjx/mujoco/mjx/_src/io_test.py index dfcb541d..542f0718 100644 --- a/mjx/mujoco/mjx/_src/io_test.py +++ b/mjx/mujoco/mjx/_src/io_test.py @@ -934,8 +934,8 @@ _DEVICE_TEST_CASES = [ ('gpu-nvidia', 'jax', ('gpu', Impl.JAX)), ('tpu', 'jax', ('tpu', Impl.JAX)), # WARP backend specified. - ('cpu', 'warp', ('cpu', 'error')), - ('gpu-notnvidia', 'warp', ('cpu', 'error')), + ('cpu', 'warp', ('cpu', Impl.WARP)), + ('gpu-notnvidia', 'warp', ('gpu', 'error')), ('gpu-nvidia', 'warp', ('gpu', Impl.WARP)), ('tpu', 'warp', ('tpu', 'error')), # CPP backend specified. @@ -962,10 +962,10 @@ _DEFAULT_DEVICE_TEST_CASES = [ ('gpu-nvidia', 'jax', ('gpu', Impl.JAX)), ('tpu', 'jax', ('tpu', Impl.JAX)), # WARP backend impl specified. - ('cpu', 'warp', ('cpu', 'error')), - ('gpu-notnvidia', 'warp', ('cpu', 'error')), + ('cpu', 'warp', ('cpu', Impl.WARP)), + ('gpu-notnvidia', 'warp', ('cpu', Impl.WARP)), ('gpu-nvidia', 'warp', ('gpu', Impl.WARP)), - ('tpu', 'warp', ('tpu', 'error')), + ('tpu', 'warp', ('cpu', Impl.WARP)), # CPP backend impl specified, CPU should always be available. ('cpu', 'cpp', ('cpu', Impl.CPP)), ('gpu-notnvidia', 'cpp', ('cpu', Impl.CPP)), @@ -1140,15 +1140,6 @@ class ResolveImplAndDeviceTest(parameterized.TestCase): self.mock_jax_backends.side_effect = backends_side_effect expected_device, expected_impl = expected - if ( - expected_impl == 'error' - and default_device_str != 'gpu-nvidia' - and impl_str == 'warp' - ): - with self.assertRaisesRegex(RuntimeError, 'cuda backend not supported'): - mjx_io._resolve_impl_and_device(impl=impl_str, device=None) - return - if expected_impl == 'error': with self.assertRaises(AssertionError): mjx_io._resolve_impl_and_device(impl=impl_str, device=None) diff --git a/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/ffi.py b/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/ffi.py index c3f9e6e9..d931dded 100644 --- a/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/ffi.py +++ b/mjx/mujoco/mjx/third_party/warp/_src/jax_experimental/ffi.py @@ -210,11 +210,19 @@ class FfiKernel: self.input_output_aliases = input_output_aliases # register the callback - FFI_CCALLFUNC = ctypes.CFUNCTYPE(ctypes.c_void_p, ctypes.POINTER(XLA_FFI_CallFrame)) - self.callback_func = FFI_CCALLFUNC(self.ffi_callback) - ffi_ccall_address = ctypes.cast(self.callback_func, ctypes.c_void_p) - ffi_capsule = jax.ffi.pycapsule(ffi_ccall_address.value) - jax.ffi.register_ffi_target(self.name, ffi_capsule, platform="CUDA") + FFI_CCALLFUNC = ctypes.CFUNCTYPE( + ctypes.c_void_p, ctypes.POINTER(XLA_FFI_CallFrame) + ) + + self.callback_func_cuda = FFI_CCALLFUNC(lambda call_frame: self.ffi_callback(call_frame, platform="CUDA")) + ffi_ccall_address_cuda = ctypes.cast(self.callback_func_cuda, ctypes.c_void_p) + ffi_capsule_cuda = jax.ffi.pycapsule(ffi_ccall_address_cuda.value) + jax.ffi.register_ffi_target(self.name, ffi_capsule_cuda, platform="CUDA") + + self.callback_func_host = FFI_CCALLFUNC(lambda call_frame: self.ffi_callback(call_frame, platform="Host")) + ffi_ccall_address_host = ctypes.cast(self.callback_func_host, ctypes.c_void_p) + ffi_capsule_host = jax.ffi.pycapsule(ffi_ccall_address_host.value) + jax.ffi.register_ffi_target(self.name, ffi_capsule_host, platform="Host") def __call__(self, *args, output_dims=None, launch_dims=None, vmap_method=None): num_inputs = len(args) @@ -241,18 +249,19 @@ class FfiKernel: # check dtype if input_value.dtype != input_arg.jax_scalar_type: raise TypeError( - f"Invalid data type for array argument '{input_arg.name}', expected {input_arg.jax_scalar_type}, got {input_value.dtype}" + f"Invalid data type for array argument '{input_arg.name}'," + f" expected {input_arg.jax_scalar_type}, got {input_value.dtype}" ) # check ndim if input_value.ndim != input_arg.jax_ndim: raise TypeError( - f"Invalid dimensionality for array argument '{input_arg.name}', expected {input_arg.jax_ndim} dimensions, got {input_value.ndim}" + f"Invalid dimensionality for array argument '{input_arg.name}', expected {input_arg.jax_ndim} dimensions, got {input_value.ndim}" ) # check inner dims for d in range(input_arg.dtype_ndim): if input_value.shape[input_arg.type.ndim + d] != input_arg.dtype_shape[d]: raise TypeError( - f"Invalid inner dimensions for array argument '{input_arg.name}', expected {input_arg.dtype_shape}, got {input_value.shape[-input_arg.dtype_ndim :]}" + f"Invalid inner dimensions for array argument '{input_arg.name}', expected {input_arg.dtype_shape}, got {input_value.shape[-input_arg.dtype_ndim :]}" ) else: # make sure scalar is not a traced variable, should be static @@ -328,7 +337,7 @@ class FfiKernel: return call(*args, launch_id=launch_id) - def ffi_callback(self, call_frame): + def ffi_callback(self, call_frame, platform="CUDA"): try: # On the first call, XLA runtime will query the API version and traits # metadata using the |extension| field. Let us respond to that query @@ -340,10 +349,11 @@ class FfiKernel: metadata_ext = ctypes.cast(extension, ctypes.POINTER(XLA_FFI_Metadata_Extension)) metadata_ext.contents.metadata.contents.api_version.major_version = 0 metadata_ext.contents.metadata.contents.api_version.minor_version = 1 - # Turn on CUDA graphs for this handler. - metadata_ext.contents.metadata.contents.traits = ( - XLA_FFI_Handler_TraitsBits.COMMAND_BUFFER_COMPATIBLE - ) + # Turn on CUDA graphs for this handler if on CUDA platform. + if platform == "CUDA": + metadata_ext.contents.metadata.contents.traits = ( + XLA_FFI_Handler_TraitsBits.COMMAND_BUFFER_COMPATIBLE + ) return None # Lock is required to prevent race conditions when callback is invoked @@ -423,29 +433,43 @@ class FfiKernel: kernel_params[0] = ctypes.addressof(launch_bounds) # get device and stream - device = wp.get_cuda_device(get_device_ordinal_from_callframe(call_frame.contents)) - stream = get_stream_from_callframe(call_frame.contents) + if platform == "CUDA": + device = wp.get_cuda_device(get_device_ordinal_from_callframe(call_frame.contents)) + stream = get_stream_from_callframe(call_frame.contents) + else: + device = wp.get_device("cpu") + stream = None # get kernel hooks hooks = self.kernel.module.get_kernel_hooks(self.kernel, device) assert hooks.forward, "Failed to find kernel entry point" # launch the kernel - wp._src.context.runtime.core.wp_cuda_launch_kernel( - device.context, - hooks.forward, - launch_bounds.size, - 0, - 256, - hooks.forward_smem_bytes, - kernel_params, - stream, - ) + if device.is_cuda: + wp._src.context.runtime.core.wp_cuda_launch_kernel( + device.context, + hooks.forward, + launch_bounds.size, + 0, + 256, + hooks.forward_smem_bytes, + kernel_params, + stream, + ) + else: + wp._src.context.runtime.core.wp_cpu_launch_kernel( + device.context, + hooks.forward, + launch_bounds.size, + kernel_params, + ) except Exception as e: print(traceback.format_exc()) return create_ffi_error( - call_frame.contents.api, XLA_FFI_Error_Code.UNKNOWN, f"FFI callback error: {type(e).__name__}: {e}" + call_frame.contents.api, + XLA_FFI_Error_Code.UNKNOWN, + f"FFI callback error: {type(e).__name__}: {e}", ) @@ -594,10 +618,16 @@ class FfiCallable: # register the callback FFI_CCALLFUNC = ctypes.CFUNCTYPE(ctypes.c_void_p, ctypes.POINTER(XLA_FFI_CallFrame)) - self.callback_func = FFI_CCALLFUNC(self.ffi_callback) - ffi_ccall_address = ctypes.cast(self.callback_func, ctypes.c_void_p) - ffi_capsule = jax.ffi.pycapsule(ffi_ccall_address.value) - jax.ffi.register_ffi_target(self.name, ffi_capsule, platform="CUDA") + + self.callback_func_cuda = FFI_CCALLFUNC(lambda call_frame: self.ffi_callback(call_frame, platform="CUDA")) + ffi_ccall_address_cuda = ctypes.cast(self.callback_func_cuda, ctypes.c_void_p) + ffi_capsule_cuda = jax.ffi.pycapsule(ffi_ccall_address_cuda.value) + jax.ffi.register_ffi_target(self.name, ffi_capsule_cuda, platform="CUDA") + + self.callback_func_host = FFI_CCALLFUNC(lambda call_frame: self.ffi_callback(call_frame, platform="Host")) + ffi_ccall_address_host = ctypes.cast(self.callback_func_host, ctypes.c_void_p) + ffi_capsule_host = jax.ffi.pycapsule(ffi_ccall_address_host.value) + jax.ffi.register_ffi_target(self.name, ffi_capsule_host, platform="Host") def __call__(self, *args, output_dims=None, vmap_method=None): num_inputs = len(args) @@ -688,8 +718,7 @@ class FfiCallable: except Exception: # ignore unsupported devices like TPUs pass - # we only support CUDA devices for now - if dev.is_cuda: + if dev.is_cuda or dev.is_cpu: module.load(dev) # save call data to be retrieved by callback @@ -698,7 +727,7 @@ class FfiCallable: self.call_id += 1 return call(*args, call_id=call_id) - def ffi_callback(self, call_frame): + def ffi_callback(self, call_frame, platform="CUDA"): try: # On the first call, XLA runtime will query the API version and traits # metadata using the |extension| field. Let us respond to that query @@ -710,8 +739,8 @@ class FfiCallable: metadata_ext = ctypes.cast(extension, ctypes.POINTER(XLA_FFI_Metadata_Extension)) metadata_ext.contents.metadata.contents.api_version.major_version = 0 metadata_ext.contents.metadata.contents.api_version.minor_version = 1 - # Turn on CUDA graphs for this handler. - if self.graph_mode is GraphMode.JAX: + # Turn on CUDA graphs for this handler if on CUDA platform. + if self.graph_mode is GraphMode.JAX and platform == "CUDA": metadata_ext.contents.metadata.contents.traits = ( XLA_FFI_Handler_TraitsBits.COMMAND_BUFFER_COMPATIBLE ) @@ -738,6 +767,35 @@ class FfiCallable: assert num_inputs == self.num_inputs assert num_outputs == self.num_outputs + if platform == "Host": + device = wp.get_device("cpu") + # reconstruct the argument list + arg_list = [] + + # input and in-out args + for i, arg in enumerate(self.input_args): + if arg.is_array: + buffer = inputs[i].contents + shape = collapse_batch_dims(buffer.dims[: buffer.rank - arg.dtype_ndim], arg.type.ndim) + arr = wp.array(ptr=buffer.data, dtype=arg.type.dtype, shape=shape, device=device) + arg_list.append(arr) + else: + # scalar argument, get stashed value + value = call_desc.static_inputs[arg.name] + arg_list.append(value) + + # pure output args (skip in-out FFI buffers) + for i, arg in enumerate(self.output_args): + buffer = outputs[i + self.num_in_out].contents + shape = collapse_batch_dims(buffer.dims[: buffer.rank - arg.dtype_ndim], arg.type.ndim) + arr = wp.array(ptr=buffer.data, dtype=arg.type.dtype, shape=shape, device=device) + arg_list.append(arr) + + # call the Python function with reconstructed arguments + with wp.ScopedDevice(device): + self.func(*arg_list) + return + cuda_stream = get_stream_from_callframe(call_frame.contents) device_ordinal = get_device_ordinal_from_callframe(call_frame.contents) @@ -870,8 +928,8 @@ class FfiCallable: arg_list.append(arr) # call the Python function with reconstructed arguments - with wp.ScopedStream(stream, sync_enter=False): - if stream.is_capturing: + with wp.ScopedStream(stream, sync_enter=False) if stream else wp.ScopedDevice(device): + if stream and stream.is_capturing: # capturing with JAX with wp.ScopedCapture(external=True) as capture: self.func(*arg_list) @@ -879,7 +937,7 @@ class FfiCallable: # keep a reference to the capture object to prevent required modules getting unloaded call_desc.capture = capture - elif self.graph_mode == GraphMode.WARP: + elif self.graph_mode == GraphMode.WARP and device.is_cuda: # capturing with WARP with wp.ScopedCapture() as capture: self.func(*arg_list) @@ -892,7 +950,7 @@ class FfiCallable: if self._graph_cache_max is not None and len(self.captures) > self._graph_cache_max: self.captures.popitem(last=False) - elif self.graph_mode == GraphMode.WARP_STAGED_EX: + elif self.graph_mode == GraphMode.WARP_STAGED_EX and device.is_cuda: # capturing with WARP using staging buffers and memcopies done outside of the graph wp_memcpy_batch = wp._src.context.runtime.core.wp_memcpy_batch @@ -935,7 +993,7 @@ class FfiCallable: # TODO: we should have a way of freeing this call_desc.capture = capture - elif self.graph_mode == GraphMode.WARP_STAGED: + elif self.graph_mode == GraphMode.WARP_STAGED and device.is_cuda: # capturing with WARP using staging buffers and memcopies done inside of the graph wp_cuda_graph_insert_memcpy_batch = ( wp._src.context.runtime.core.wp_cuda_graph_insert_memcpy_batch @@ -1013,7 +1071,7 @@ class FfiCallable: call_desc.capture = capture else: - # not capturing + # not capturing or on CPU self.func(*arg_list) except Exception as e: @@ -1621,7 +1679,7 @@ def register_ffi_callback(name: str, func: Callable, graph_compatible: bool = Tr # TODO check that the name is not already registered - def ffi_callback(call_frame): + def ffi_callback(call_frame, platform="CUDA"): try: extension = call_frame.contents.extension_start # On the first call, XLA runtime will query the API version and traits @@ -1633,7 +1691,7 @@ def register_ffi_callback(name: str, func: Callable, graph_compatible: bool = Tr metadata_ext = ctypes.cast(extension, ctypes.POINTER(XLA_FFI_Metadata_Extension)) metadata_ext.contents.metadata.contents.api_version.major_version = 0 metadata_ext.contents.metadata.contents.api_version.minor_version = 1 - if graph_compatible: + if graph_compatible and platform == "CUDA": # Turn on CUDA graphs for this handler. metadata_ext.contents.metadata.contents.traits = ( XLA_FFI_Handler_TraitsBits.COMMAND_BUFFER_COMPATIBLE @@ -1666,12 +1724,17 @@ def register_ffi_callback(name: str, func: Callable, graph_compatible: bool = Tr return None FFI_CCALLFUNC = ctypes.CFUNCTYPE(ctypes.c_void_p, ctypes.POINTER(XLA_FFI_CallFrame)) - callback_func = FFI_CCALLFUNC(ffi_callback) + callback_func_cuda = FFI_CCALLFUNC(lambda call_frame: ffi_callback(call_frame, platform="CUDA")) + callback_func_host = FFI_CCALLFUNC(lambda call_frame: ffi_callback(call_frame, platform="Host")) with _FFI_REGISTRY_LOCK: - _FFI_CALLBACK_REGISTRY[name] = callback_func - ffi_ccall_address = ctypes.cast(callback_func, ctypes.c_void_p) - ffi_capsule = jax.ffi.pycapsule(ffi_ccall_address.value) - jax.ffi.register_ffi_target(name, ffi_capsule, platform="CUDA") + _FFI_CALLBACK_REGISTRY[f"{name}_cuda"] = callback_func_cuda + _FFI_CALLBACK_REGISTRY[f"{name}_host"] = callback_func_host + ffi_ccall_address_cuda = ctypes.cast(callback_func_cuda, ctypes.c_void_p) + ffi_capsule_cuda = jax.ffi.pycapsule(ffi_ccall_address_cuda.value) + jax.ffi.register_ffi_target(name, ffi_capsule_cuda, platform="CUDA") + ffi_ccall_address_host = ctypes.cast(callback_func_host, ctypes.c_void_p) + ffi_capsule_host = jax.ffi.pycapsule(ffi_ccall_address_host.value) + jax.ffi.register_ffi_target(name, ffi_capsule_host, platform="Host") ############################################################################### diff --git a/mjx/mujoco/mjx/warp/forward_test.py b/mjx/mujoco/mjx/warp/forward_test.py index b80c921d..056a0c51 100644 --- a/mjx/mujoco/mjx/warp/forward_test.py +++ b/mjx/mujoco/mjx/warp/forward_test.py @@ -31,6 +31,7 @@ from mujoco.mjx.warp import test_util as tu from mujoco.mjx.warp import warp as wp # pylint: disable=g-importing-member import numpy as np + try: from mujoco.mjx.warp import forward # pylint: disable=g-import-not-at-top except ImportError: @@ -300,6 +301,56 @@ class StepTest(parameterized.TestCase): tu.assert_attr_eq(dx, d, 'mocap_quat') tu.assert_attr_eq(dx, d, 'sensordata') + @parameterized.parameters( + 'humanoid/humanoid.xml', + 'pendula.xml', + ) + def test_step_cpu(self, xml: str): + """Tests step on the CPU device.""" + if not _FORCE_TEST: + if not mjxw.WARP_INSTALLED: + self.skipTest('Warp not installed.') + + batch_size = 1 + m = test_util.load_test_file(xml) + m.opt.iterations = 10 + m.opt.ls_iterations = 10 + + cpu_device = jax.devices('cpu')[0] + mx = mjx.put_model(m, impl='warp', device=cpu_device) + + d = mujoco.MjData(m) + worldids = jp.arange(batch_size) + dx_batch = jax.vmap(functools.partial(tu.make_data, m))(worldids) + dx_batch = jax.device_put(dx_batch, cpu_device) + dx_batch_orig = dx_batch + + for _ in range(10): + dx_batch = jax.vmap(forward.step, in_axes=(None, 0))( + mx, dx_batch + ) + + for i in range(batch_size): + dx = dx_batch[i] + dx_orig = dx_batch_orig[i] + + d.qpos[:] = dx_orig.qpos + d.qvel[:] = dx_orig.qvel + d.ctrl[:] = dx_orig.ctrl + d.mocap_pos[:] = dx_orig.mocap_pos + d.mocap_quat[:] = dx_orig.mocap_quat + d.time = dx_orig.time + mujoco.mj_step(m, d, 10) + + tu.assert_attr_eq(dx, d, 'qpos') + tu.assert_attr_eq(dx, d, 'qvel') + tu.assert_attr_eq(dx, d, 'time') + tu.assert_attr_eq(dx, d, 'ctrl') + tu.assert_attr_eq(dx, d, 'act') + tu.assert_attr_eq(dx, d, 'mocap_pos') + tu.assert_attr_eq(dx, d, 'mocap_quat') + tu.assert_attr_eq(dx, d, 'sensordata') + def test_step_leading_dim_mismatch(self): if not _FORCE_TEST: if not mjxw.WARP_INSTALLED: diff --git a/mjx/mujoco/mjx/warp/smooth_test.py b/mjx/mujoco/mjx/warp/smooth_test.py index e7ffb92b..7c8bb368 100644 --- a/mjx/mujoco/mjx/warp/smooth_test.py +++ b/mjx/mujoco/mjx/warp/smooth_test.py @@ -133,10 +133,11 @@ class SmoothTest(parameterized.TestCase): def test_kinematics_vmap(self): """Tests kinematics with batched data.""" - if not mjxw.WARP_INSTALLED: - self.skipTest('Warp not installed.') - if not io.has_cuda_gpu_device(): - self.skipTest('No CUDA GPU device available.') + if not _FORCE_TEST: + if not mjxw.WARP_INSTALLED: + self.skipTest('Warp not installed.') + if not io.has_cuda_gpu_device(): + self.skipTest('No CUDA GPU device available.') m = tu.load_test_file('pendula.xml') diff --git a/mjx/mujoco/mjx/warp/test_util.py b/mjx/mujoco/mjx/warp/test_util.py index 1752f3c9..78a28671 100644 --- a/mjx/mujoco/mjx/warp/test_util.py +++ b/mjx/mujoco/mjx/warp/test_util.py @@ -153,7 +153,9 @@ def _mjx_efc(dx, worldid: int): efc_pos = select(dx._impl.efc__pos)[:nefc] efc_type = select(dx._impl.efc__type)[:nefc] efc_d = select(dx._impl.efc__D)[:nefc] - keys_sorted = np.lexsort((-efc_pos, efc_type, efc_d)) + keys_sorted = np.lexsort( + (-np.round(efc_pos, 12), efc_type, np.round(efc_d, 12)) + ) keys = keys[keys_sorted] nefc = len(keys) @@ -180,7 +182,9 @@ def _mj_efc(d): else: efc_j = d.efc_J.reshape((-1, d.qvel.shape[0])) - keys = np.lexsort((-d.efc_pos, d.efc_type, d.efc_D)) + keys = np.lexsort( + (-np.round(d.efc_pos, 12), d.efc_type, np.round(d.efc_D, 12)) + ) type_ = d.efc_type[keys] pos = d.efc_pos[keys] efc_j = efc_j[keys] From b643f50b2a2db2e3e4ca67c9ed94b2b821f1c81f Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Sun, 26 Apr 2026 14:55:26 -0700 Subject: [PATCH 146/251] Rename IndexType USHORT/UINT to U16/U32. PiperOrigin-RevId: 906026433 Change-Id: I0016c1923560fc8517e5388ed6068d54b84d5923 --- src/experimental/filament/compat/imgui_bridge.cc | 2 +- src/experimental/filament/compat/model_objects.cc | 6 +++--- src/experimental/filament/filament/builtins.cc | 2 +- src/experimental/filament/filament/mesh.cc | 6 +++--- src/experimental/filament/filament/mesh.h | 4 ++-- 5 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/experimental/filament/compat/imgui_bridge.cc b/src/experimental/filament/compat/imgui_bridge.cc index de21a303..71c9da01 100644 --- a/src/experimental/filament/compat/imgui_bridge.cc +++ b/src/experimental/filament/compat/imgui_bridge.cc @@ -230,7 +230,7 @@ void ImguiBridge::Update() { data.nvertices = cmds->VtxBuffer.Size; data.nindices = cmds->IdxBuffer.Size; data.indices = cmds->IdxBuffer.Data; - data.index_type = mjINDEX_TYPE_USHORT; + data.index_type = mjINDEX_TYPE_U16; data.primitive_type = mjMESH_PRIMITIVE_TYPE_TRIANGLES; meshes_.push_back(std::make_unique(scene_view_->GetEngine(), data)); diff --git a/src/experimental/filament/compat/model_objects.cc b/src/experimental/filament/compat/model_objects.cc index fb796882..1a52b6f1 100644 --- a/src/experimental/filament/compat/model_objects.cc +++ b/src/experimental/filament/compat/model_objects.cc @@ -445,8 +445,8 @@ static void UpdatemjrMeshData(mjrMeshData* data, const mjModel* model, int id, data->nindices = data->nvertices; data->indices = nullptr; data->index_type = data->nvertices >= std::numeric_limits::max() - ? mjINDEX_TYPE_UINT - : mjINDEX_TYPE_USHORT; + ? mjINDEX_TYPE_U32 + : mjINDEX_TYPE_U16; data->nattributes = has_uvs ? 3 : 2; data->attributes[0].usage = mjVERTEX_ATTRIBUTE_USAGE_POSITION; data->attributes[0].type = mjVERTEX_ATTRIBUTE_TYPE_FLOAT3; @@ -492,7 +492,7 @@ void UpdateSkinFlexmjrMeshData(mjrMeshData* data, const mjModel* model, data->nvertices = positions.size() / 3; data->nindices = num_indices; data->indices = indices.data(); - data->index_type = mjINDEX_TYPE_UINT; + data->index_type = mjINDEX_TYPE_U32; data->primitive_type = mjMESH_PRIMITIVE_TYPE_TRIANGLES; data->compute_bounds = true; data->release_callback = nullptr; diff --git a/src/experimental/filament/filament/builtins.cc b/src/experimental/filament/filament/builtins.cc index fc7b8c23..f438fa39 100644 --- a/src/experimental/filament/filament/builtins.cc +++ b/src/experimental/filament/filament/builtins.cc @@ -93,7 +93,7 @@ class BuiltinBuilder : mjrMeshData { primitive_type_ == filament::backend::PrimitiveType::TRIANGLES ? mjMESH_PRIMITIVE_TYPE_TRIANGLES : mjMESH_PRIMITIVE_TYPE_LINES; - index_type = mjINDEX_TYPE_USHORT; + index_type = mjINDEX_TYPE_U16; bounds_min[0] = bounds_.getMin().x; bounds_min[1] = bounds_.getMin().y; bounds_min[2] = bounds_.getMin().z; diff --git a/src/experimental/filament/filament/mesh.cc b/src/experimental/filament/filament/mesh.cc index e38c4399..2d7a74b7 100644 --- a/src/experimental/filament/filament/mesh.cc +++ b/src/experimental/filament/filament/mesh.cc @@ -262,7 +262,7 @@ void Mesh::BuildIndexBuffer(const mjrMeshData& data) { return; } - const int element_size = data.index_type == mjINDEX_TYPE_USHORT + const int element_size = data.index_type == mjINDEX_TYPE_U16 ? sizeof(uint16_t) : sizeof(uint32_t); const int num_bytes = data.nindices * element_size; @@ -277,7 +277,7 @@ void Mesh::BuildIndexBuffer(const mjrMeshData& data) { delete[] sequence; }); - if (data.index_type == mjINDEX_TYPE_USHORT) { + if (data.index_type == mjINDEX_TYPE_U16) { FillSequence(sequence, num_bytes); } else { FillSequence(sequence, num_bytes); @@ -287,7 +287,7 @@ void Mesh::BuildIndexBuffer(const mjrMeshData& data) { filament::IndexBuffer::Builder ib_builder; ib_builder.indexCount(data.nindices); - ib_builder.bufferType(data.index_type == mjINDEX_TYPE_USHORT + ib_builder.bufferType(data.index_type == mjINDEX_TYPE_U16 ? filament::IndexBuffer::IndexType::USHORT : filament::IndexBuffer::IndexType::UINT); index_buffer_ = ib_builder.build(*engine_); diff --git a/src/experimental/filament/filament/mesh.h b/src/experimental/filament/filament/mesh.h index ba15d8ca..3b7fc5ab 100644 --- a/src/experimental/filament/filament/mesh.h +++ b/src/experimental/filament/filament/mesh.h @@ -37,8 +37,8 @@ namespace mujoco { // The type of data stored in an index buffer. typedef enum mjrIndexType_ { - mjINDEX_TYPE_USHORT = 0, - mjINDEX_TYPE_UINT = 1, + mjINDEX_TYPE_U16 = 0, + mjINDEX_TYPE_U32 = 1, } mjrIndexType; // The type of primitive to be drawn by vertex data. From 3a5a48b9f9450683f6574abe49e3f63c7f8fe61f Mon Sep 17 00:00:00 2001 From: Matija Kecman Date: Mon, 27 Apr 2026 01:58:43 -0700 Subject: [PATCH 147/251] Fix texture ID collisions in ImguiBridge Texture IDs were assigned using textures_.size() + 1, which is not monotonic: after erasures the map shrinks and previously used IDs can be reassigned to new textures. This causes multiple ImGui images to reference the same underlying Filament texture, rendering them all with the same pixel data. This fix replaces the size-based scheme with a monotonically increasing counter (next_tex_id_) that never reuses IDs. PiperOrigin-RevId: 906214496 Change-Id: I5a388861ad9565c8e00e27b39cc9c76c9750ca1e --- src/experimental/filament/compat/imgui_bridge.cc | 4 ++-- src/experimental/filament/compat/imgui_bridge.h | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/experimental/filament/compat/imgui_bridge.cc b/src/experimental/filament/compat/imgui_bridge.cc index 71c9da01..c7b83514 100644 --- a/src/experimental/filament/compat/imgui_bridge.cc +++ b/src/experimental/filament/compat/imgui_bridge.cc @@ -72,7 +72,7 @@ uintptr_t ImguiBridge::UploadImage(uintptr_t tex_id, const uint8_t* pixels, // Assign a new texture ID. if (tex_id == 0) { - tex_id = textures_.size() + 1; + tex_id = next_tex_id_++; } std::unique_ptr& texture = textures_[tex_id]; @@ -123,7 +123,7 @@ void ImguiBridge::CreateTexture(ImTextureData* data) { config.format = mjPIXEL_FORMAT_RGBA8; config.color_space = mjCOLORSPACE_LINEAR; - const uintptr_t tex_id = textures_.size() + 1; + const uintptr_t tex_id = next_tex_id_++; textures_[tex_id] = std::make_unique(scene_view_->GetEngine(), config); data->SetTexID((ImTextureID)tex_id); diff --git a/src/experimental/filament/compat/imgui_bridge.h b/src/experimental/filament/compat/imgui_bridge.h index 06807f68..953cbd1e 100644 --- a/src/experimental/filament/compat/imgui_bridge.h +++ b/src/experimental/filament/compat/imgui_bridge.h @@ -64,6 +64,7 @@ class ImguiBridge { std::vector> renderables_; std::vector> meshes_; std::unordered_map> textures_; + uintptr_t next_tex_id_ = 1; }; // Draws text at the given screen coordinates in clip space (i.e. [-1,-1,-1] to From 2e3205229c6339f3fbdb031e0b038e9bd7685426 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 27 Apr 2026 03:11:07 -0700 Subject: [PATCH 148/251] Studio: various panel usability improvements MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Reorder Physics section, move Actuator Groups to end. - Rename "Physics Settings" → "Physics", "Rendering Settings" → "Rendering". - Reduce IndentSpacing from 20 to 6. - Reduce FramePadding Y from 6 to 2 for narrower buttons and popups. - Remove explicit Unindent/Indent overrides in Rendering and Visibility Groups sections so buttons are naturally indented under their tree nodes. - Dynamically adjust convergence and counts plot X-axis limits based on actual solver iterations instead of hardcoded 20. PiperOrigin-RevId: 906244477 Change-Id: Ic3e6e6e793395158c8cb6692af885884ca3f7f50 --- src/experimental/platform/ux/gui.cc | 66 +++++++++++++++-------------- src/experimental/studio/app.cc | 4 +- 2 files changed, 36 insertions(+), 34 deletions(-) diff --git a/src/experimental/platform/ux/gui.cc b/src/experimental/platform/ux/gui.cc index b20993ee..e70bf13c 100644 --- a/src/experimental/platform/ux/gui.cc +++ b/src/experimental/platform/ux/gui.cc @@ -192,7 +192,7 @@ void SetupTheme(GuiTheme theme) { float rounding = 4.0f; s.DisplaySafeAreaPadding = ImVec2(0, 0); s.WindowPadding = ImVec2(hspacing, vspacing); - s.FramePadding = ImVec2(hspacing, vspacing); + s.FramePadding = ImVec2(hspacing, 2); s.ItemSpacing = ImVec2(hspacing, vspacing); s.ItemInnerSpacing = ImVec2(hspacing, vspacing); s.WindowRounding = rounding; @@ -205,7 +205,7 @@ void SetupTheme(GuiTheme theme) { s.WindowBorderSize = 0.0f; s.FrameBorderSize = 1.0f; s.PopupBorderSize = 1.0f; - s.IndentSpacing = 20.0f; + s.IndentSpacing = 6.0f; s.ScrollbarSize = 12.0f; s.GrabMinSize = 5.0f; s.WindowMenuButtonPosition = ImGuiDir_None; @@ -817,22 +817,6 @@ void PhysicsGui(mjModel* model, float min_width) { ImGui::TreePop(); } - if (ImGui::TreeNodeEx("Actuator Groups")) { - if (ImGui::BeginTable("##ActuatorGroupsTable", num_cols)) { - const ImVec2 size = GetFlexElementSize(num_cols); - for (int i = 0; i < 6; ++i) { - char label[64]; - std::snprintf(label, sizeof(label), "Act Group %d", i); - ImGui::TableNextColumn(); - int flipped = ~opt.disableactuator; - ImGui_BitToggle(label, &flipped, 1 << i, size); - opt.disableactuator = ~flipped; - } - ImGui::EndTable(); - } - ImGui::TreePop(); - }; - if (ImGui::TreeNodeEx("Algorithmic Parameters")) { ImGui_Input("Timestep", &opt.timestep, {0, 1, 0.01, 0.1}); ImGui_Input("Iterations", &opt.iterations, {0, 1000, 1, 10}); @@ -867,6 +851,22 @@ void PhysicsGui(mjModel* model, float min_width) { ImGui::TreePop(); } + if (ImGui::TreeNodeEx("Actuator Groups")) { + if (ImGui::BeginTable("##ActuatorGroupsTable", num_cols)) { + const ImVec2 size = GetFlexElementSize(num_cols); + for (int i = 0; i < 6; ++i) { + char label[64]; + std::snprintf(label, sizeof(label), "Act Group %d", i); + ImGui::TableNextColumn(); + int flipped = ~opt.disableactuator; + ImGui_BitToggle(label, &flipped, 1 << i, size); + opt.disableactuator = ~flipped; + } + ImGui::EndTable(); + } + ImGui::TreePop(); + } + ImGui::PopItemWidth(); } @@ -986,8 +986,6 @@ void RenderingGui(const mjModel* model, mjvOption* vis_options, static_cast(std::floor(available_width / min_width)), 1, 6); if (ImGui::TreeNodeEx("Model Elements", ImGuiTreeNodeFlags_DefaultOpen)) { - ImGui::Unindent(ImGui::GetTreeNodeToLabelSpacing() / 2); - if (ImGui::BeginTable("##ModelElementsTable", num_cols)) { const ImVec2 size = GetFlexElementSize(num_cols); for (int i = 0; i < mjNVISFLAG; ++i) { @@ -996,14 +994,10 @@ void RenderingGui(const mjModel* model, mjvOption* vis_options, } ImGui::EndTable(); } - - ImGui::Indent(ImGui::GetTreeNodeToLabelSpacing() / 2); ImGui::TreePop(); } if (ImGui::TreeNodeEx("Render Flags", ImGuiTreeNodeFlags_DefaultOpen)) { - ImGui::Unindent(ImGui::GetTreeNodeToLabelSpacing() / 2); - if (ImGui::BeginTable("##RenderFlagsTable", num_cols)) { const ImVec2 size = GetFlexElementSize(num_cols); for (int i = 0; i < mjNRNDFLAG; ++i) { @@ -1012,8 +1006,6 @@ void RenderingGui(const mjModel* model, mjvOption* vis_options, } ImGui::EndTable(); } - - ImGui::Indent(ImGui::GetTreeNodeToLabelSpacing() / 2); ImGui::TreePop(); } } @@ -1030,8 +1022,6 @@ void GroupsGui(const mjModel* model, mjvOption* vis_options, float min_width) { auto GroupGui = [&](const char* name, mjtByte* group) { if (ImGui::TreeNodeEx(name, ImGuiTreeNodeFlags_DefaultOpen)) { - ImGui::Unindent(ImGui::GetTreeNodeToLabelSpacing() / 2); - char label[64]; std::snprintf(label, sizeof(label), "##%s", name); if (ImGui::BeginTable(label, num_cols)) { @@ -1044,8 +1034,6 @@ void GroupsGui(const mjModel* model, mjvOption* vis_options, float min_width) { ImGui::EndTable(); } - - ImGui::Indent(ImGui::GetTreeNodeToLabelSpacing() / 2); ImGui::TreePop(); } }; @@ -1150,12 +1138,24 @@ void ControlsGui(const mjModel* model, const mjData* data, ImGui::PopItemWidth(); } +static int GetPlotXLimit(const mjData* data) { + int max_niter = 0; + const int nisland0 = + data->nefc ? mjMAX(1, mjMIN(data->nisland, mjNISLAND)) : 0; + for (int k = 0; k < nisland0; k++) { + max_niter = mjMAX(max_niter, data->solver_niter[k]); + } + return mjMAX(10, ((max_niter + 9) / 10) * 10); +} + void ConvergenceGui(const mjModel* model, mjData* data) { + int xlim = GetPlotXLimit(data); + if (ImPlot::BeginPlot("Convergence (log 10)", ImVec2(-1, 0), ImPlotFlags_NoMouseText)) { ImPlot::PushStyleVar(ImPlotStyleVar_LineWeight, 2.0f); ImPlot::SetupAxis(ImAxis_X1, "iteration", ImPlotAxisFlags_AutoFit); - ImPlot::SetupAxisLimits(ImAxis_X1, 0, 20, ImPlotCond_Always); + ImPlot::SetupAxisLimits(ImAxis_X1, 0, xlim, ImPlotCond_Always); ImPlot::SetupAxisFormat(ImAxis_Y1, "%.1f"); ImPlot::SetupAxisLimits(ImAxis_Y1, -20, 5, ImPlotCond_Always); ImPlot::SetupLegend(ImPlotLocation_NorthEast); @@ -1212,10 +1212,12 @@ void ConvergenceGui(const mjModel* model, mjData* data) { } void CountsGui(const mjModel* model, mjData* data) { + int xlim = GetPlotXLimit(data); + if (ImPlot::BeginPlot("Counts", ImVec2(-1, 0), ImPlotFlags_NoMouseText)) { ImPlot::PushStyleVar(ImPlotStyleVar_LineWeight, 2.0f); ImPlot::SetupAxis(ImAxis_X1, "iteration", ImPlotAxisFlags_AutoFit); - ImPlot::SetupAxisLimits(ImAxis_X1, 0, 20, ImPlotCond_Always); + ImPlot::SetupAxisLimits(ImAxis_X1, 0, xlim, ImPlotCond_Always); ImPlot::SetupAxisFormat(ImAxis_Y1, "%.0f"); ImPlot::SetupAxisLimits(ImAxis_Y1, 0, 80, ImPlotCond_Always); ImPlot::SetupLegend(ImPlotLocation_NorthEast); diff --git a/src/experimental/studio/app.cc b/src/experimental/studio/app.cc index e7ad6c89..24a43649 100644 --- a/src/experimental/studio/app.cc +++ b/src/experimental/studio/app.cc @@ -1011,14 +1011,14 @@ void App::ModelOptionsGui() { ImGuiTreeNodeFlags_SpanAvailWidth | ImGuiTreeNodeFlags_Framed; ImGui::BeginChild("PhysicsGui", {0, 0}, child_flags); - if (ImGui::TreeNodeEx("Physics Settings", node_flags)) { + if (ImGui::TreeNodeEx("Physics", node_flags)) { platform::PhysicsGui(model(), min_width); ImGui::TreePop(); } ImGui::EndChild(); ImGui::BeginChild("RenderingGui", {0, 0}, child_flags); - if (ImGui::TreeNodeEx("Rendering Settings", node_flags)) { + if (ImGui::TreeNodeEx("Rendering", node_flags)) { platform::RenderingGui(model(), &vis_options_, renderer_->GetRenderFlags(), min_width); ImGui::TreePop(); From 9cd89eec2fd2ffce76206945736457deaa180fea Mon Sep 17 00:00:00 2001 From: Matija Kecman Date: Mon, 27 Apr 2026 07:42:16 -0700 Subject: [PATCH 149/251] Make Studio's monitoring charts responsive and full-window * **Dynamic Full-Window Scaling**: Charts in the Performance and Solver panels now expand to fill the entire pane, automatically arranging side-by-side or stacked based on window orientation to eliminate scrollbars. * **Responsive Decorators**: Added reusable C++ ImPlot helper functions that auto-hide titles, legends, and tick labels when the plot surface is small. * **Cleaner Visualization**: Refined chart titles (e.g., "CPU msec vs frame", "Convergence vs iter") and removed redundant axis labels for improved scannability. These changes were based on the plot behavior of //third_party/mujoco/src/experimental/py/sample/implot.py This is an incremental improvement, we can make the plots even nicer in future. PiperOrigin-RevId: 906351914 Change-Id: I8cb19c19bf807f0107c717066250a71bcf7e325f --- src/experimental/platform/sim/sim_profiler.cc | 41 +++++------ src/experimental/platform/sim/sim_profiler.h | 5 +- src/experimental/platform/ux/gui.cc | 21 +++--- src/experimental/platform/ux/gui.h | 6 +- src/experimental/platform/ux/imgui_widgets.cc | 70 +++++++++++++++++++ src/experimental/platform/ux/imgui_widgets.h | 41 +++++++++++ src/experimental/studio/app.cc | 17 +++-- 7 files changed, 162 insertions(+), 39 deletions(-) diff --git a/src/experimental/platform/sim/sim_profiler.cc b/src/experimental/platform/sim/sim_profiler.cc index 7aca0c4c..e3b794b8 100644 --- a/src/experimental/platform/sim/sim_profiler.cc +++ b/src/experimental/platform/sim/sim_profiler.cc @@ -17,12 +17,11 @@ #include #include #include +#include "experimental/platform/ux/imgui_widgets.h" namespace mujoco::platform { -SimProfiler::SimProfiler() { - Clear(); -} +SimProfiler::SimProfiler() { Clear(); } void SimProfiler::Clear() { constexpr int kProfilerMaxFrames = 200; @@ -88,21 +87,21 @@ void SimProfiler::Update(const mjModel* model, const mjData* data) { // Solver diagnostics. mjtNum sqrt_nnz = 0; int solver_niter = 0; - const int nisland = data->nefc ? mjMAX(1, mjMIN(data->nisland, mjNISLAND)) : 0; - for (int island=0; island < nisland; island++) { + const int nisland = + data->nefc ? mjMAX(1, mjMIN(data->nisland, mjNISLAND)) : 0; + for (int island = 0; island < nisland; island++) { sqrt_nnz += data->solver_nnz[island]; solver_niter += data->solver_niter[island]; } sqrt_nnz = mju_sqrt(sqrt_nnz); dim_dof_.erase(dim_dof_.begin()); - int nv = (model->opt.enableflags & mjENBL_SLEEP) ? data->nv_awake - : model->nv; + int nv = (model->opt.enableflags & mjENBL_SLEEP) ? data->nv_awake : model->nv; dim_dof_.push_back(nv); dim_body_.erase(dim_body_.begin()); int nbody = (model->opt.enableflags & mjENBL_SLEEP) ? data->nbody_awake - : model->nbody; + : model->nbody; dim_body_.push_back(nbody); dim_constraint_.erase(dim_constraint_.begin()); @@ -115,16 +114,17 @@ void SimProfiler::Update(const mjModel* model, const mjData* data) { dim_contact_.push_back(data->ncon); dim_iteration_.erase(dim_iteration_.begin()); - dim_iteration_.push_back(static_cast(solver_niter) / nisland); + dim_iteration_.push_back(static_cast(solver_niter) / + mjMAX(1, nisland)); } - -void SimProfiler::CpuTimeGraph() { - if (ImPlot::BeginPlot("CPU Time", ImVec2(-1, 0), ImPlotFlags_NoMouseText)) { +void SimProfiler::CpuTimeGraph(ImVec2 plot_size) { + ImPlotFlags flags = + ImPlot_SetupPlotFlags(plot_size) | ImPlotFlags_NoMouseText; + if (ImPlot::BeginPlot("CPU msec vs frame", plot_size, flags)) { ImPlot::PushStyleVar(ImPlotStyleVar_LineWeight, 2.0f); - ImPlot::SetupAxis(ImAxis_X1, "frame", ImPlotAxisFlags_AutoFit); - ImPlot::SetupAxis(ImAxis_Y1, "msec", ImPlotAxisFlags_AutoFit); - ImPlot::SetupAxisFormat(ImAxis_Y1, "%.2f"); + ImPlot_SetupTimeAxis(plot_size, ""); + ImPlot_SetupValueAxis(plot_size, "", "%.2f"); ImPlot::SetupLegend(ImPlotLocation_NorthEast); ImPlot::SetupFinish(); @@ -143,12 +143,13 @@ void SimProfiler::CpuTimeGraph() { } } -void SimProfiler::DimensionsGraph() { - if (ImPlot::BeginPlot("Dimensions", ImVec2(-1, 0), ImPlotFlags_NoMouseText)) { +void SimProfiler::DimensionsGraph(ImVec2 plot_size) { + ImPlotFlags flags = + ImPlot_SetupPlotFlags(plot_size) | ImPlotFlags_NoMouseText; + if (ImPlot::BeginPlot("Dimensions vs frame", plot_size, flags)) { ImPlot::PushStyleVar(ImPlotStyleVar_LineWeight, 2.0f); - ImPlot::SetupAxis(ImAxis_X1, "frame", ImPlotAxisFlags_AutoFit); - ImPlot::SetupAxis(ImAxis_Y1, "count", ImPlotAxisFlags_AutoFit); - ImPlot::SetupAxisFormat(ImAxis_Y1, "%.0f"); + ImPlot_SetupTimeAxis(plot_size, ""); + ImPlot_SetupValueAxis(plot_size, "", "%.0f"); ImPlot::SetupLegend(ImPlotLocation_NorthEast); ImPlot::SetupFinish(); diff --git a/src/experimental/platform/sim/sim_profiler.h b/src/experimental/platform/sim/sim_profiler.h index 4e9cce46..c1f1ef8e 100644 --- a/src/experimental/platform/sim/sim_profiler.h +++ b/src/experimental/platform/sim/sim_profiler.h @@ -17,6 +17,7 @@ #include +#include #include namespace mujoco::platform { @@ -33,8 +34,8 @@ class SimProfiler { void Update(const mjModel* model, const mjData* data); // Displays the profiling data using ImPlot. - void CpuTimeGraph(); - void DimensionsGraph(); + void CpuTimeGraph(ImVec2 plot_size = ImVec2(-1, 0)); + void DimensionsGraph(ImVec2 plot_size = ImVec2(-1, 0)); private: std::vector cpu_total_; diff --git a/src/experimental/platform/ux/gui.cc b/src/experimental/platform/ux/gui.cc index e70bf13c..9098092c 100644 --- a/src/experimental/platform/ux/gui.cc +++ b/src/experimental/platform/ux/gui.cc @@ -506,7 +506,7 @@ bool CameraSelectionGui(const mjModel* model, mjData* data, mjvCamera& camera, auto select = [&](int type, int idx) { if (ImGui::Selectable(GetCameraName(model, camera, type).c_str(), - (type == idx))) { + (type == idx))) { return true; } return false; @@ -1148,13 +1148,13 @@ static int GetPlotXLimit(const mjData* data) { return mjMAX(10, ((max_niter + 9) / 10) * 10); } -void ConvergenceGui(const mjModel* model, mjData* data) { +void ConvergenceGui(const mjModel* model, mjData* data, ImVec2 plot_size) { int xlim = GetPlotXLimit(data); - - if (ImPlot::BeginPlot("Convergence (log 10)", ImVec2(-1, 0), - ImPlotFlags_NoMouseText)) { + ImPlotFlags flags = + ImPlot_SetupPlotFlags(plot_size) | ImPlotFlags_NoMouseText; + if (ImPlot::BeginPlot("Convergence (log 10) vs iter", plot_size, flags)) { ImPlot::PushStyleVar(ImPlotStyleVar_LineWeight, 2.0f); - ImPlot::SetupAxis(ImAxis_X1, "iteration", ImPlotAxisFlags_AutoFit); + ImPlot::SetupAxis(ImAxis_X1, "", ImPlotAxisFlags_AutoFit); ImPlot::SetupAxisLimits(ImAxis_X1, 0, xlim, ImPlotCond_Always); ImPlot::SetupAxisFormat(ImAxis_Y1, "%.1f"); ImPlot::SetupAxisLimits(ImAxis_Y1, -20, 5, ImPlotCond_Always); @@ -1211,12 +1211,13 @@ void ConvergenceGui(const mjModel* model, mjData* data) { } } -void CountsGui(const mjModel* model, mjData* data) { +void CountsGui(const mjModel* model, mjData* data, ImVec2 plot_size) { int xlim = GetPlotXLimit(data); - - if (ImPlot::BeginPlot("Counts", ImVec2(-1, 0), ImPlotFlags_NoMouseText)) { + ImPlotFlags flags = + ImPlot_SetupPlotFlags(plot_size) | ImPlotFlags_NoMouseText; + if (ImPlot::BeginPlot("Counts vs iter", plot_size, flags)) { ImPlot::PushStyleVar(ImPlotStyleVar_LineWeight, 2.0f); - ImPlot::SetupAxis(ImAxis_X1, "iteration", ImPlotAxisFlags_AutoFit); + ImPlot::SetupAxis(ImAxis_X1, "", ImPlotAxisFlags_AutoFit); ImPlot::SetupAxisLimits(ImAxis_X1, 0, xlim, ImPlotCond_Always); ImPlot::SetupAxisFormat(ImAxis_Y1, "%.0f"); ImPlot::SetupAxisLimits(ImAxis_Y1, 0, 80, ImPlotCond_Always); diff --git a/src/experimental/platform/ux/gui.h b/src/experimental/platform/ux/gui.h index 938466bb..4dc8b69c 100644 --- a/src/experimental/platform/ux/gui.h +++ b/src/experimental/platform/ux/gui.h @@ -138,10 +138,12 @@ void NoiseGui(const mjModel* model, const mjData* data, float& noise_scale, float& noise_rate); // UX for the solver convergence chart. -void ConvergenceGui(const mjModel* model, mjData* data); +void ConvergenceGui(const mjModel* model, mjData* data, + ImVec2 plot_size = ImVec2(-1, 0)); // UX for the solver counts chart. -void CountsGui(const mjModel* model, mjData* data); +void CountsGui(const mjModel* model, mjData* data, + ImVec2 plot_size = ImVec2(-1, 0)); // UX for displaying basic simulation information. Note that the pause state and // FPS needs to be tracked by the caller and passed here to be displayed. diff --git a/src/experimental/platform/ux/imgui_widgets.cc b/src/experimental/platform/ux/imgui_widgets.cc index 61d2989a..7d0f1cba 100644 --- a/src/experimental/platform/ux/imgui_widgets.cc +++ b/src/experimental/platform/ux/imgui_widgets.cc @@ -14,6 +14,7 @@ #include "experimental/platform/ux/imgui_widgets.h" +#include #include #include #include @@ -23,6 +24,7 @@ #include #include +#include #include namespace mujoco::platform { @@ -378,4 +380,72 @@ void MaybeSaveToClipboard(const std::string& contents) { } } +ImPlotFlags ImPlot_SetupPlotFlags(ImVec2 plot_size) { + ImPlotFlags flags = ImPlotFlags_None; + if (plot_size.x > 0 && plot_size.y > 0) { + const float min_dim = std::min(plot_size.x, plot_size.y); + if (min_dim < 300) { + flags |= ImPlotFlags_NoTitle; + } + if (min_dim < 200) { + flags |= ImPlotFlags_NoLegend; + } + } + return flags; +} + +void ImPlot_SetupTimeAxis(ImVec2 plot_size, const char* label, + ImPlotAxisFlags extra_flags) { + ImPlotAxisFlags flags = extra_flags; + if (plot_size.x > 0 && plot_size.x < 300) { + flags |= ImPlotAxisFlags_NoTickLabels; + } + ImPlot::SetupAxis(ImAxis_X1, label, flags); +} + +void ImPlot_SetupValueAxis(ImVec2 plot_size, const char* label, + const char* format, ImPlotAxisFlags extra_flags) { + ImPlotAxisFlags flags = extra_flags; + if (plot_size.y > 0 && plot_size.y < 150) { + flags |= ImPlotAxisFlags_NoTickLabels; + } + ImPlot::SetupAxis(ImAxis_Y1, label, flags); + if (format) { + ImPlot::SetupAxisFormat(ImAxis_Y1, format); + } +} + +void ImPlot_SetupFixedAxis(ImVec2 plot_size, double y_min, double y_max, + const char* label, const char* format, + const double* tick_values, + const char* const* tick_labels, int n_ticks) { + ImPlotAxisFlags flags = ImPlotAxisFlags_None; + if (plot_size.y > 0 && plot_size.y < 150) { + flags |= ImPlotAxisFlags_NoTickLabels; + } + ImPlot::SetupAxis(ImAxis_Y1, label, flags); + ImPlot::SetupAxisLimits(ImAxis_Y1, y_min, y_max, ImPlotCond_Always); + if (format) { + ImPlot::SetupAxisFormat(ImAxis_Y1, format); + } + if (tick_values && n_ticks > 0) { + ImPlot::SetupAxisTicks(ImAxis_Y1, tick_values, n_ticks, tick_labels); + } +} + +ImPlotPairLayout ImPlot_ComputePairLayout() { + ImVec2 avail = ImGui::GetContentRegionAvail(); + bool is_wide = avail.x > avail.y; + + const float item_spacing = ImGui::GetStyle().ItemSpacing.y; + ImVec2 plot_size(is_wide ? (avail.x - item_spacing) * 0.5f : avail.x, + is_wide ? avail.y : (avail.y - item_spacing) * 0.5f); + + return { + plot_size, + is_wide ? ImPlotLayoutDirection::kHorizontal + : ImPlotLayoutDirection::kVertical, + }; +} + } // namespace mujoco::platform diff --git a/src/experimental/platform/ux/imgui_widgets.h b/src/experimental/platform/ux/imgui_widgets.h index 619e1055..187dabba 100644 --- a/src/experimental/platform/ux/imgui_widgets.h +++ b/src/experimental/platform/ux/imgui_widgets.h @@ -27,6 +27,7 @@ #include #include #include +#include #include #include "experimental/platform/ux/enum_utils.h" @@ -566,6 +567,46 @@ inline void EndBoxSection() { ImGui::EndTable(); } // Saves the given contents to the clipboard if the clipboard is available. void MaybeSaveToClipboard(const std::string& contents); +// Returns plot flags with title/legend conditionally hidden when the plot +// area is too small. `plot_size` is the final rendered size of the plot. +ImPlotFlags ImPlot_SetupPlotFlags(ImVec2 plot_size); + +// Sets up the X axis as a "time/frame" axis. +// Hides tick labels when the plot is narrow. +// Uses `label` as the axis label (empty string to hide) and auto-fit limits. +void ImPlot_SetupTimeAxis( + ImVec2 plot_size, const char* label = "", + ImPlotAxisFlags extra_flags = ImPlotAxisFlags_AutoFit); + +// Sets up a Y axis with auto-fit limits. +// Hides tick labels when the plot is short. +void ImPlot_SetupValueAxis( + ImVec2 plot_size, const char* label = "", const char* format = nullptr, + ImPlotAxisFlags extra_flags = ImPlotAxisFlags_AutoFit); + +// Sets up a Y axis with fixed limits and optional explicit ticks. +// Hides tick labels when the plot is short. +void ImPlot_SetupFixedAxis(ImVec2 plot_size, double y_min, double y_max, + const char* label = "", const char* format = nullptr, + const double* tick_values = nullptr, + const char* const* tick_labels = nullptr, + int n_ticks = 0); + +enum class ImPlotLayoutDirection { + kHorizontal, + kVertical, +}; + +struct ImPlotPairLayout { + ImVec2 plot_size; // Size for each individual plot. + ImPlotLayoutDirection direction; +}; + +// Computes a responsive layout for two plots that share the available +// content region. When the region is wider than tall, the plots are placed +// side-by-side; otherwise they are stacked vertically. +ImPlotPairLayout ImPlot_ComputePairLayout(); + } // namespace mujoco::platform #endif // MUJOCO_SRC_EXPERIMENTAL_PLATFORM_UX_IMGUI_WIDGETS_H_ diff --git a/src/experimental/studio/app.cc b/src/experimental/studio/app.cc index 24a43649..dec270ae 100644 --- a/src/experimental/studio/app.cc +++ b/src/experimental/studio/app.cc @@ -884,8 +884,12 @@ void App::BuildGui() { ImGui::SetNextWindowPos(chart_pos, ImGuiCond_FirstUseEver); ImGui::SetNextWindowSize(chart_size, ImGuiCond_FirstUseEver); if (ImGui::Begin("Performance", &tmp_.chart_performance)) { - profiler_.CpuTimeGraph(); - profiler_.DimensionsGraph(); + auto layout = platform::ImPlot_ComputePairLayout(); + profiler_.CpuTimeGraph(layout.plot_size); + if (layout.direction == platform::ImPlotLayoutDirection::kHorizontal) { + ImGui::SameLine(); + } + profiler_.DimensionsGraph(layout.plot_size); } ImGui::End(); } @@ -894,8 +898,12 @@ void App::BuildGui() { ImGui::SetNextWindowPos(chart_pos, ImGuiCond_FirstUseEver); ImGui::SetNextWindowSize(chart_size, ImGuiCond_FirstUseEver); if (ImGui::Begin("Solver", &tmp_.chart_solver)) { - platform::CountsGui(model(), data()); - platform::ConvergenceGui(model(), data()); + auto layout = platform::ImPlot_ComputePairLayout(); + platform::CountsGui(model(), data(), layout.plot_size); + if (layout.direction == platform::ImPlotLayoutDirection::kHorizontal) { + ImGui::SameLine(); + } + platform::ConvergenceGui(model(), data(), layout.plot_size); } ImGui::End(); } @@ -1452,7 +1460,6 @@ void App::ToolBarGui() { } ImGui::SetItemTooltip("%s", "Reset"); - // Combined (Normal Pause, Viscous Pause, Play) widget and Speed selection. ImGui::SameLine(0, separator_width); platform::StepControlGui(model(), &step_control_, tmp_.speed_index); From e6354b43687cd2b72827dd731d9a9d11078255bf Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 27 Apr 2026 13:32:20 -0700 Subject: [PATCH 150/251] Add flex_interp to the MJX Model. PiperOrigin-RevId: 906518134 Change-Id: If716e7e67827a6659fb72a6064809910fe99944f --- mjx/mujoco/mjx/_src/types.py | 1 + 1 file changed, 1 insertion(+) diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index faf47012..e29d4aca 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -775,6 +775,7 @@ class Model(PyTreeNode): mesh_texcoord: np.ndarray flex_vertadr: np.ndarray flex_vertnum: np.ndarray + flex_interp: np.ndarray flex_vert0: np.ndarray flex_nodeadr: np.ndarray flex_nodenum: np.ndarray From 25a9114705bf93013c83398b26391a7ef612d607 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 28 Apr 2026 02:10:44 -0700 Subject: [PATCH 151/251] No-op refactor of PGS and NoSlip solvers in preparation for island support. PiperOrigin-RevId: 906826882 Change-Id: I2003097e1bb81ebabda3a7075f1f8200d4d5ff95 --- src/engine/engine_solver.c | 281 +++++++++++++++++++++---------------- 1 file changed, 157 insertions(+), 124 deletions(-) diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 1644f841..5700a3e5 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -78,9 +78,11 @@ static void dualFinish(const mjModel* m, mjData* d) { // compute 1/diag(AR) +// res[c] = 1 / AR[efclist[c], efclist[c]] for c = 0..nefc-1 +// efclist is NULL for monolithic (sequential) iteration // TODO: b/295296178 - add island support to Dual solvers -static void ARdiaginv(const mjModel* m, const mjData* d, mjtNum* res, int flg_subR) { - int nefc = d->nefc; +static void ARdiaginv(const mjModel* m, const mjData* d, mjtNum* res, + int nefc, const int* efclist, int flg_subR) { const mjtNum *AR = d->efc_AR; const mjtNum *R = d->efc_R; @@ -90,12 +92,13 @@ static void ARdiaginv(const mjModel* m, const mjData* d, mjtNum* res, int flg_su const int *rownnz = d->efc_AR_rownnz; const int *colind = d->efc_AR_colind; - for (int i=0; i < nefc; i++) { + for (int c=0; c < nefc; c++) { + int i = efclist ? efclist[c] : c; int nnz = rownnz[i]; for (int j=0; j < nnz; j++) { int adr = rowadr[i] + j; if (i == colind[adr]) { - res[i] = 1 / (flg_subR ? mju_max(mjMINVAL, AR[adr] - R[i]) : AR[adr]); + res[c] = 1 / (flg_subR ? mju_max(mjMINVAL, AR[adr] - R[i]) : AR[adr]); break; } } @@ -104,9 +107,11 @@ static void ARdiaginv(const mjModel* m, const mjData* d, mjtNum* res, int flg_su // dense else { - for (int i=0; i < nefc; i++) { - int adr = i * (nefc + 1); - res[i] = 1 / (flg_subR ? mju_max(mjMINVAL, AR[adr] - R[i]) : AR[adr]); + int d_nefc = d->nefc; // global nefc + for (int c=0; c < nefc; c++) { + int i = efclist ? efclist[c] : c; + int adr = i * (d_nefc + 1); + res[c] = 1 / (flg_subR ? mju_max(mjMINVAL, AR[adr] - R[i]) : AR[adr]); } } } @@ -229,9 +234,10 @@ static mjtNum costChange(const mjtNum* A, mjtNum* force, const mjtNum* oldforce, // set efc_state to dual constraint state; return nactive +// iterates over efclist (or sequentially if NULL), classifies by ne/nf ranges // TODO: b/295296178 - add island support to Dual solvers -static int dualState(const mjModel* m, const mjData* d, int* state) { - int ne = d->ne, nf = d->nf, nefc = d->nefc; +static int dualState(const mjData* d, int* state, + int ne, int nf, int nefc, const int* efclist) { const mjtNum* force = d->efc_force; const mjtNum* floss = d->efc_frictionloss; @@ -239,10 +245,14 @@ static int dualState(const mjModel* m, const mjData* d, int* state) { int nactive = ne + nf; // equality - mju_fillInt(state, mjCNSTRSTATE_QUADRATIC, ne); + for (int c=0; c < ne; c++) { + int i = efclist ? efclist[c] : c; + state[i] = mjCNSTRSTATE_QUADRATIC; + } // friction - for (int i=ne; i < ne+nf; i++) { + for (int c=ne; c < ne+nf; c++) { + int i = efclist ? efclist[c] : c; if (force[i] <= -floss[i]) { state[i] = mjCNSTRSTATE_LINEARPOS; // opposite of primal } else if (force[i] >= floss[i]) { @@ -253,7 +263,9 @@ static int dualState(const mjModel* m, const mjData* d, int* state) { } // limit and contact - for (int i=ne+nf; i < nefc; i++) { + for (int c=ne+nf; c < nefc; c++) { + int i = efclist ? efclist[c] : c; + // non-negative if (d->efc_type[i] != mjCNSTR_CONTACT_ELLIPTIC) { if (force[i] <= 0) { @@ -302,7 +314,7 @@ static int dualState(const mjModel* m, const mjData* d, int* state) { mju_fillInt(state+i, result, dim); // advance - i += (dim-1); + c += (dim-1); } } @@ -310,26 +322,86 @@ static int dualState(const mjModel* m, const mjData* d, int* state) { } +// update constraint state, return nactive and nchange +static int dualStateChange(const mjData* d, int* state, int* oldstate, + int ne, int nf, int nefc, + const int* efclist, int* nchange) { + // save old state + for (int c=0; c < nefc; c++) { + int i = efclist ? efclist[c] : c; + oldstate[c] = state[i]; + } + + // update state + int nactive = dualState(d, state, ne, nf, nefc, efclist); + + // count state changes + *nchange = 0; + for (int c=0; c < nefc; c++) { + int i = efclist ? efclist[c] : c; + *nchange += (oldstate[c] != state[i]); + } + + return nactive; +} + + +// solve QCQP and project onto friction ellipsoid, write to force[i+1..i+dim-1] +static void solveQCQP(mjtNum* force, int i, int dim, + mjtNum* Ac, mjtNum* bc, const mjtNum* mu) { + int flg_active; + mjtNum v[6]; + + // solve + if (dim == 3) { + flg_active = mju_QCQP2(v, Ac, bc, mu, force[i]); + } else if (dim == 4) { + flg_active = mju_QCQP3(v, Ac, bc, mu, force[i]); + } else { // dim == 5 + flg_active = mju_QCQP(v, Ac, bc, mu, force[i], dim-1); + } + + // on constraint: put v on ellipsoid, in case QCQP is approximate + if (flg_active) { + mjtNum s = 0; + for (int j=0; j < dim-1; j++) { + s += v[j]*v[j] / (mu[j]*mu[j]); + } + s = mju_sqrt(force[i]*force[i] / mju_max(mjMINVAL, s)); + for (int j=0; j < dim-1; j++) { + v[j] *= s; + } + } + + // assign + mju_copy(force+i+1, v, dim-1); +} + + //---------------------------- PGS solver ---------------------------------------------------------- +// core PGS solver: iterates over constraints specified by efclist +// island: island index for stats (use -1 for monolithic, mapped to 0) +// ne, nf, nefc: constraint type counts +// efclist: maps list position c to monolithic efc index (NULL for sequential) // TODO: b/295296178 - add island support to Dual solvers -void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { - int ne = d->ne, nf = d->nf, nefc = d->nefc; +static void solPGS(const mjModel* m, mjData* d, int island, + int ne, int nf, int nefc, + const int* efclist, int maxiter) { const mjtNum *floss = d->efc_frictionloss; mjtNum *force = d->efc_force; mj_markStack(d); mjtNum* ARinv = mjSTACKALLOC(d, nefc, mjtNum); int* oldstate = mjSTACKALLOC(d, nefc, int); - // TODO: b/295296178 - Use island index (currently hardcoded to 0) - int island = 0; + int island_stat = mjMAX(0, island); // island index for diagnostic stats mjtNum scale = 1 / (m->stat.meaninertia * mjMAX(1, m->nv)); // precompute inverse diagonal of AR - ARdiaginv(m, d, ARinv, 0); + ARdiaginv(m, d, ARinv, nefc, efclist, 0); // initial constraint state - dualState(m, d, d->efc_state); + dualState(d, d->efc_state, ne, nf, nefc, efclist); // main iteration int iter = 0; @@ -338,7 +410,9 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { mjtNum improvement = 0; // perform one sweep - for (int i=0; i < nefc; i++) { + for (int c=0; c < nefc; c++) { + int i = efclist ? efclist[c] : c; + // get constraint dimensionality int dim; if (d->efc_type[i] == mjCNSTR_CONTACT_ELLIPTIC) { @@ -361,16 +435,16 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { // simple constraint if (d->efc_type[i] != mjCNSTR_CONTACT_ELLIPTIC) { // unconstrained minimum - force[i] -= res[0]*ARinv[i]; + force[i] -= res[0]*ARinv[c]; // impose interval and inequality constraints - if (i >= ne && i < ne+nf) { + if (c >= ne && c < ne+nf) { if (force[i] < -floss[i]) { force[i] = -floss[i]; } else if (force[i] > floss[i]) { force[i] = floss[i]; } - } else if (i >= ne+nf) { + } else if (c >= ne+nf) { if (force[i] < 0) { force[i] = 0; } @@ -380,7 +454,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { // elliptic cone constraint else { // get friction - mjtNum *mu = d->contact[d->efc_id[i]].friction; + mjtNum *mu = d->contact[d->efc_id[i]].friction; //-------------------- perform normal or ray update @@ -390,7 +464,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { // normal force too small: normal update if (force[i] < mjMINVAL) { // unconstrained minimum - force[i] -= res[0]*ARinv[i]; + force[i] -= res[0]*ARinv[c]; // clamp if (force[i] < 0) { @@ -447,61 +521,31 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { // QCQP else { - int flg_active; - mjtNum v[6]; - - // solve - if (dim == 3) { - flg_active = mju_QCQP2(v, Ac, bc, mu, force[i]); - } else if (dim == 4) { - flg_active = mju_QCQP3(v, Ac, bc, mu, force[i]); - } else { - flg_active = mju_QCQP(v, Ac, bc, mu, force[i], dim-1); - } - - // on constraint: put v on ellipsoid, in case QCQP is approximate - if (flg_active) { - mjtNum s = 0; - for (int j=0; j < dim-1; j++) { - s += v[j]*v[j] / (mu[j]*mu[j]); - } - s = mju_sqrt(force[i]*force[i] / mju_max(mjMINVAL, s)); - for (int j=0; j < dim-1; j++) { - v[j] *= s; - } - } - - // assign - mju_copy(force+i+1, v, dim-1); + solveQCQP(force, i, dim, Ac, bc, mu); } } // accumulate improvement if (dim == 1) { - Athis[0] = 1/ARinv[i]; + Athis[0] = 1/ARinv[c]; } improvement -= costChange(Athis, force+i, oldforce, res, dim); // skip the rest of this constraint - i += (dim-1); + c += (dim-1); } - // process state - mju_copyInt(oldstate, d->efc_state, nefc); - int nactive = dualState(m, d, d->efc_state); - int nchange = 0; - for (int i=0; i < nefc; i++) { - nchange += (oldstate[i] != d->efc_state[i]); - } + // update constraint state + int nchange; + int nactive = dualStateChange(d, d->efc_state, oldstate, ne, nf, nefc, efclist, &nchange); // scale improvement, save stats improvement *= scale; - saveStats(m, d, island, iter, improvement, 0, 0, nactive, nchange, 0, 0); + saveStats(m, d, island_stat, iter, improvement, 0, 0, nactive, nchange, 0, 0); // increment iteration count iter++; - // terminate if (improvement < m->opt.tolerance) { break; @@ -509,51 +553,59 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { } // finalize statistics - if (island < mjNISLAND) { + if (island_stat < mjNISLAND) { // update solver iterations - d->solver_niter[island] += iter; + d->solver_niter[island_stat] += iter; // set nnz if (mj_isSparse(m)) { - d->solver_nnz[island] = 0; - for (int i=0; i < nefc; i++) { - d->solver_nnz[island] += d->efc_AR_rownnz[i]; + d->solver_nnz[island_stat] = 0; + for (int c=0; c < nefc; c++) { + d->solver_nnz[island_stat] += d->efc_AR_rownnz[efclist ? efclist[c] : c]; } } else { - d->solver_nnz[island] = nefc*nefc; + d->solver_nnz[island_stat] = nefc*nefc; } } - // map to joint space - dualFinish(m, d); - mj_freeStack(d); } +// PGS entry point (monolithic) +void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { + solPGS(m, d, /*island=*/-1, d->ne, d->nf, d->nefc, /*efclist=*/NULL, maxiter); + dualFinish(m, d); +} + + //---------------------------- NoSlip solver ------------------------------------------------------- -// TODO: b/295296178 - add island support to Dual solvers -void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { - int dim, iter = 0, ne = d->ne, nf = d->nf, nefc = d->nefc; +// core NoSlip solver: iterates over constraints specified by efclist +// island: island index for stats (use -1 for monolithic, mapped to 0) +// ne, nf, nefc: constraint type counts +// efclist: maps list position c to monolithic efc index (NULL for sequential) +static void solNoSlip(const mjModel* m, mjData* d, int island, + int ne, int nf, int nefc, + const int* efclist, int maxiter) { + int dim, iter = 0; const mjtNum *floss = d->efc_frictionloss; mjtNum *force = d->efc_force; mjtNum *mu, improvement; - mjtNum v[5], Ac[25], bc[5], res[5], oldforce[5], delta[5], mid, y, K0, K1; + mjtNum Ac[25], bc[5], res[5], oldforce[5], delta[5], mid, y, K0, K1; mjContact* con; mj_markStack(d); mjtNum* ARinv = mjSTACKALLOC(d, nefc, mjtNum); int* oldstate = mjSTACKALLOC(d, nefc, int); - // TODO: b/295296178 - Use island index (currently hardcoded to 0) - int island = 0; + int island_stat = mjMAX(0, island); mjtNum scale = 1 / (m->stat.meaninertia * mjMAX(1, m->nv)); // precompute inverse diagonal of A - ARdiaginv(m, d, ARinv, 1); + ARdiaginv(m, d, ARinv, nefc, efclist, 1); // initial constraint state - dualState(m, d, d->efc_state); + dualState(d, d->efc_state, ne, nf, nefc, efclist); // main iteration while (iter < maxiter) { @@ -562,19 +614,22 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { // correct for cost change at iter 0 if (iter == 0) { - for (int i=0; i < nefc; i++) { + for (int c=0; c < nefc; c++) { + int i = efclist ? efclist[c] : c; improvement += 0.5*force[i]*force[i]*d->efc_R[i]; } } // perform one sweep: dry friction - for (int i=ne; i < ne+nf; i++) { + for (int c=ne; c < ne+nf; c++) { + int i = efclist ? efclist[c] : c; + // compute residual, save old residual(m, d, res, i, 1, 1); oldforce[0] = force[i]; // unconstrained minimum - force[i] -= res[0]*ARinv[i]; + force[i] -= res[0]*ARinv[c]; // impose interval constraints if (force[i] < -floss[i]) { @@ -585,11 +640,13 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { // add to improvement delta[0] = force[i] - oldforce[0]; - improvement -= 0.5*delta[0]*delta[0]/ARinv[i] + delta[0]*res[0]; + improvement -= 0.5*delta[0]*delta[0]/ARinv[c] + delta[0]*res[0]; } // perform one sweep: contact friction - for (int i=ne+nf; i < nefc; i++) { + for (int c=ne+nf; c < nefc; c++) { + int i = efclist ? efclist[c] : c; + // pyramidal contact if (d->efc_type[i] == mjCNSTR_CONTACT_PYRAMIDAL) { // get contact info @@ -648,7 +705,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { } // skip the rest of this contact - i += 2*(dim-1)-1; + c += 2*(dim-1)-1; } // elliptic contact @@ -678,55 +735,27 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { // QCQP else { - int flg_active = 0; - - // solve - if (dim == 3) { - flg_active = mju_QCQP2(v, Ac, bc, mu, force[i]); - } else if (dim == 4) { - flg_active = mju_QCQP3(v, Ac, bc, mu, force[i]); - } else { - flg_active = mju_QCQP(v, Ac, bc, mu, force[i], dim-1); - } - - // on constraint: put v on ellipsoid, in case QCQP is approximate - if (flg_active) { - mjtNum s = 0; - for (int j=0; j < dim-1; j++) { - s += v[j]*v[j]/(mu[j]*mu[j]); - } - s = mju_sqrt(force[i]*force[i] / mju_max(mjMINVAL, s)); - for (int j=0; j < dim-1; j++) { - v[j] *= s; - } - } - - // assign - mju_copy(force+i+1, v, dim-1); + solveQCQP(force, i, dim, Ac, bc, mu); } // accumulate improvement improvement -= costChange(Ac, force+i+1, oldforce, res, dim-1); // skip the rest of this contact - i += (dim-1); + c += (dim-1); } } - // process state - mju_copyInt(oldstate, d->efc_state, nefc); - int nactive = dualState(m, d, d->efc_state); - int nchange = 0; - for (int i=0; i < nefc; i++) { - nchange += (oldstate[i] != d->efc_state[i]); - } + // update constraint state + int nchange; + int nactive = dualStateChange(d, d->efc_state, oldstate, ne, nf, nefc, efclist, &nchange); // scale improvement, save stats improvement *= scale; // save noslip stats after all the entries from regular solver - int stats_iter = iter + d->solver_niter[island]; - saveStats(m, d, island, stats_iter, improvement, 0, 0, nactive, nchange, 0, 0); + int stats_iter = iter + d->solver_niter[island_stat]; + saveStats(m, d, island_stat, stats_iter, improvement, 0, 0, nactive, nchange, 0, 0); // increment iteration count iter++; @@ -738,15 +767,19 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { } // update solver iterations - d->solver_niter[island] += iter; - - // map to joint space - dualFinish(m, d); + d->solver_niter[island_stat] += iter; mj_freeStack(d); } +// NoSlip entry point (monolithic) +void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { + solNoSlip(m, d, /*island=*/-1, d->ne, d->nf, d->nefc, /*efclist=*/NULL, maxiter); + dualFinish(m, d); +} + + //------------------------- Primal solvers --------------------------------------------------------- // Primal context From 6f275b79c6d8e3f0c09118f57167890d6d75e6df Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 28 Apr 2026 03:12:12 -0700 Subject: [PATCH 152/251] Studio: toolbar cosmetic improvements MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Remove hard-coded button sizes (48×32) from toolbar icons; buttons now use default ImGui sizing, matching the existing copy-camera button. - Remove the Unload button (available via menu). - Add ImGui_ColorButtonEx widget with per-corner rounding via ImDrawFlags, enabling segmented button-group visuals. - Use segmented rounding for the pause/viscous/play triplet: rounded-left, square, rounded-right. - Make pause and play buttons 40% wider than the viscous button for emphasis. - Replace the theme combo dropdown with a cycling button. - Reduce toolbar height from 48px to 34px and vertically center content via WindowPadding. - Zero out table CellPadding in the toolbar for precise vertical centering. - Remove the speed combo's inflated vertical padding. PiperOrigin-RevId: 906853958 Change-Id: I6e4ec7243adb9e0f1f06f8779fa896a6865c46c7 --- src/experimental/platform/ux/gui.cc | 79 +++++++++---------- src/experimental/platform/ux/gui.h | 2 +- src/experimental/platform/ux/imgui_widgets.cc | 4 +- src/experimental/platform/ux/imgui_widgets.h | 49 ++++++++++++ src/experimental/studio/app.cc | 49 ++++-------- 5 files changed, 103 insertions(+), 80 deletions(-) diff --git a/src/experimental/platform/ux/gui.cc b/src/experimental/platform/ux/gui.cc index 9098092c..ccf1ed91 100644 --- a/src/experimental/platform/ux/gui.cc +++ b/src/experimental/platform/ux/gui.cc @@ -97,15 +97,15 @@ void SetupTheme(GuiTheme theme) { c[ImGuiCol_TextSelectedBg] = ImVec4(0.73, 0.73, 0.73, 0.35); c[ImGuiCol_ModalWindowDimBg] = ImVec4(0.80, 0.80, 0.80, 0.35); c[ImGuiCol_DragDropTarget] = ImVec4(1.00, 1.00, 0.00, 0.90); - c[ImGuiCol_NavHighlight] = ImVec4(0.26, 0.59, 0.98, 1.00); + c[ImGuiCol_NavCursor] = ImVec4(0.26, 0.59, 0.98, 1.00); c[ImGuiCol_NavWindowingHighlight] = ImVec4(1.00, 1.00, 1.00, 0.70); c[ImGuiCol_NavWindowingDimBg] = ImVec4(0.80, 0.80, 0.80, 0.20); c[ImGuiCol_DockingEmptyBg] = ImVec4(0.38, 0.38, 0.38, 1.00); c[ImGuiCol_Tab] = ImVec4(0.25, 0.25, 0.25, 1.00); c[ImGuiCol_TabHovered] = ImVec4(0.40, 0.40, 0.40, 1.00); - c[ImGuiCol_TabActive] = ImVec4(0.33, 0.33, 0.33, 1.00); - c[ImGuiCol_TabUnfocused] = ImVec4(0.25, 0.25, 0.25, 1.00); - c[ImGuiCol_TabUnfocusedActive] = ImVec4(0.33, 0.33, 0.33, 1.00); + c[ImGuiCol_TabSelected] = ImVec4(0.33, 0.33, 0.33, 1.00); + c[ImGuiCol_TabDimmed] = ImVec4(0.25, 0.25, 0.25, 1.00); + c[ImGuiCol_TabDimmedSelected] = ImVec4(0.33, 0.33, 0.33, 1.00); c[ImGuiCol_DockingPreview] = ImVec4(0.85, 0.85, 0.85, 0.28); c[ImGuiCol_WindowBg].w = 1.0f; } else if (theme == GuiTheme::kLight) { @@ -220,7 +220,7 @@ ImVec4 ConfigureDockingLayout() { const float kOptionsRelWidth = 0.22f; const float kInspectorRelWidth = 0.22f; const float kStatsRelHeight = 0.3f; - const float kToolsBarHeight = 48.f * scale; + const float kToolsBarHeight = 36.f * scale; const float kStatusBarHeight = 32.f * scale; const ImVec2 dockspace_pos{viewport->WorkPos.x, @@ -303,6 +303,9 @@ ImVec4 ConfigureDockingLayout() { platform::ScopedStyle style; style.Var(ImGuiStyleVar_WindowBorderSize, 1.0f); style.Var(ImGuiStyleVar_WindowRounding, 0.0f); + const float toolbar_vpad = + std::max(0.f, (kToolsBarHeight - ImGui::GetFrameHeight()) * 0.5f); + style.Var(ImGuiStyleVar_WindowPadding, ImVec2(4, toolbar_vpad)); ImGui::SetNextWindowPos(viewport->WorkPos, ImGuiCond_Always); ImGui::SetNextWindowSize(ImVec2(viewport->Size.x, kToolsBarHeight), ImGuiCond_Always); @@ -335,18 +338,23 @@ ImVec4 ConfigureDockingLayout() { void StepControlGui(const mjModel* model, StepControl* step_control, int& speed_index) { platform::ScopedStyle style; - style.Var(ImGuiStyleVar_FrameRounding, 2.f); + style.Var(ImGuiStyleVar_FrameRounding, 8.f); const ImColor yellow(255, 215, 0, 255); const ImColor green(40, 180, 40, 255); - const float scale = ImGui::GetWindowDpiScale(); - ImVec2 button_size(48.f * scale, 32.f * scale); auto make_button = [&](const char* icon, StepControl::PauseState target_state, - ImColor color, const char* tooltip = "", - float hover_alpha = 1.f) { + ImColor color, ImDrawFlags corners, + const char* tooltip = "", + float hover_alpha = 1.f, float width_scale = 1.f) { + ImVec2 size(0, 0); + if (width_scale != 1.f) { + const ImGuiStyle& s = ImGui::GetStyle(); + const float w = ImGui::CalcTextSize(icon).x + s.FramePadding.x * 2; + size.x = w * width_scale; + } bool active = step_control->GetPauseState() == target_state; - if (ImGui_ColorButton(icon, active, color, button_size, hover_alpha)) { + if (ImGui_ColorButtonEx(icon, active, color, corners, size, hover_alpha)) { step_control->SetPauseState(target_state); } if (!std::string_view(tooltip).empty()) { @@ -355,26 +363,28 @@ void StepControlGui(const mjModel* model, StepControl* step_control, }; make_button(ICON_FA_PAUSE, StepControl::PauseState::kNormalPaused, yellow, - "Pause"); + ImDrawFlags_RoundCornersLeft, "Pause", .3f, 1.6f); ImGui::SameLine(0.f, 0.f); make_button(ICON_FA_MAGIC, StepControl::PauseState::kViscousPaused, yellow, - "Viscous Pause"); + ImDrawFlags_RoundCornersNone, "Viscous Pause", .3f, 1.3f); ImGui::SameLine(0.f, 0.f); - make_button(ICON_FA_PLAY, StepControl::PauseState::kUnpaused, green, "", .6f); + make_button(ICON_FA_PLAY, StepControl::PauseState::kUnpaused, green, + ImDrawFlags_RoundCornersRight, "", .3f, 1.6f); // Speed selection. - ImGui::SameLine(); - const float pad_y = (button_size.y - ImGui::GetFontSize()) * .5f; + style.Reset(); + ImGui::SameLine(0, ImGui::GetFrameHeight() * .6f); ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, - ImVec2(ImGui::GetStyle().FramePadding.x + 5.f, pad_y)); + ImVec2(ImGui::GetStyle().FramePadding.x + 5.f, + ImGui::GetStyle().FramePadding.y)); const auto [misaligned, measured] = IsSpeedMisaligned(*step_control); char speed_preview[64]; if (misaligned) { - snprintf(speed_preview, sizeof(speed_preview), "%s%s (%-4.1f%%)", + snprintf(speed_preview, sizeof(speed_preview), "%s %s (%-4.1f%%)", ICON_FA_TACHOMETER, kPercentRealTime[speed_index], measured); } else { - snprintf(speed_preview, sizeof(speed_preview), "%s%s", ICON_FA_TACHOMETER, + snprintf(speed_preview, sizeof(speed_preview), "%s %s", ICON_FA_TACHOMETER, kPercentRealTime[speed_index]); } @@ -400,39 +410,22 @@ void StepControlGui(const mjModel* model, StepControl* step_control, } } -bool ThemeSelectGui(GuiTheme* theme) { +bool ThemeSelectGui(GuiTheme* theme, const ImVec2& size) { static constexpr const char* ICON_DARKMODE = ICON_FA_CIRCLE; static constexpr const char* ICON_LIGHTMODE = ICON_FA_CIRCLE_O; static constexpr const char* ICON_CLASSICMODE = ICON_FA_ADJUST; const char* theme_icons[] = {ICON_LIGHTMODE, ICON_DARKMODE, ICON_CLASSICMODE}; const char* theme_tooltips[] = {"Light Mode", "Dark Mode", "Classic Mode"}; - const GuiTheme theme_values[] = { - GuiTheme::kLight, - GuiTheme::kDark, - GuiTheme::kClassic, - }; - - bool changed = false; int theme_idx = static_cast(*theme); - ImGui::SetNextItemWidth(ImGui::CalcTextSize(theme_icons[0]).x + - ImGui::GetStyle().FramePadding.x * 2); - if (ImGui::BeginCombo("##Theme", theme_icons[theme_idx], - ImGuiComboFlags_NoArrowButton)) { - for (int n = 0; n < IM_ARRAYSIZE(theme_icons); n++) { - if (ImGui::Selectable(theme_icons[n], (theme_idx == n))) { - *theme = theme_values[n]; - changed = true; - } - if (ImGui::IsItemHovered()) { - ImGui::SetTooltip("%s", theme_tooltips[n]); - } - } - ImGui::EndCombo(); + if (ImGui::Button(theme_icons[theme_idx], size)) { + theme_idx = (theme_idx + 1) % IM_ARRAYSIZE(theme_icons); + *theme = static_cast(theme_idx); + return true; } - ImGui::SetItemTooltip("%s", "Theme"); + ImGui::SetItemTooltip("%s", theme_tooltips[theme_idx]); - return changed; + return false; } bool LabelSelectionGui(mjvOption* opts) { diff --git a/src/experimental/platform/ux/gui.h b/src/experimental/platform/ux/gui.h index 4dc8b69c..e9b23dfc 100644 --- a/src/experimental/platform/ux/gui.h +++ b/src/experimental/platform/ux/gui.h @@ -81,7 +81,7 @@ void StepControlGui(const mjModel* model, StepControl* step_control, int& speed_index); // UX for selecting the GUI theme. -bool ThemeSelectGui(GuiTheme* theme); +bool ThemeSelectGui(GuiTheme* theme, const ImVec2& size = ImVec2(0, 0)); // UX for selecting the visualization label option. bool LabelSelectionGui(mjvOption* opts); diff --git a/src/experimental/platform/ux/imgui_widgets.cc b/src/experimental/platform/ux/imgui_widgets.cc index 7d0f1cba..305f292c 100644 --- a/src/experimental/platform/ux/imgui_widgets.cc +++ b/src/experimental/platform/ux/imgui_widgets.cc @@ -375,9 +375,7 @@ void ImGui_EndHSplit(bool open) { } void MaybeSaveToClipboard(const std::string& contents) { - if (ImGui::GetIO().SetClipboardTextFn) { - ImGui::GetIO().SetClipboardTextFn(nullptr, contents.c_str()); - } + ImGui::SetClipboardText(contents.c_str()); } ImPlotFlags ImPlot_SetupPlotFlags(ImVec2 plot_size) { diff --git a/src/experimental/platform/ux/imgui_widgets.h b/src/experimental/platform/ux/imgui_widgets.h index 187dabba..3ee85efa 100644 --- a/src/experimental/platform/ux/imgui_widgets.h +++ b/src/experimental/platform/ux/imgui_widgets.h @@ -551,6 +551,55 @@ inline bool ImGui_ColorButton(const char* label, bool active, ImColor color, return ImGui::Button(label, size); } +// Like ImGui_ColorButton, but with per-corner rounding control via ImDrawFlags. +// Use ImDrawFlags_RoundCornersLeft, ImDrawFlags_RoundCornersRight, +// ImDrawFlags_RoundCornersNone, ImDrawFlags_RoundCornersAll, etc. +inline bool ImGui_ColorButtonEx(const char* label, bool active, ImColor color, + ImDrawFlags corners, + const ImVec2& size = ImVec2(0, 0), + float hover_alpha = 0.5f) { + const ImGuiStyle& s = ImGui::GetStyle(); + const ImVec2 label_size = ImGui::CalcTextSize(label, nullptr, true); + const ImVec2 btn_size( + size.x > 0 ? size.x : label_size.x + s.FramePadding.x * 2, + size.y > 0 ? size.y : label_size.y + s.FramePadding.y * 2); + + const ImVec2 pos = ImGui::GetCursorScreenPos(); + ImGui::InvisibleButton(label, btn_size); + const bool clicked = ImGui::IsItemClicked(); + const bool hovered = ImGui::IsItemHovered(); + + // Determine background color. + const ImColor hover_color(color.Value.x, color.Value.y, color.Value.z, + color.Value.w * hover_alpha); + ImColor bg; + if (active) { + bg = color; + } else if (hovered) { + bg = hover_color; + } else { + bg = ImGui::GetColorU32(ImGuiCol_Button); + } + + // Draw background with per-corner rounding. + ImDrawList* dl = ImGui::GetWindowDrawList(); + const ImVec2 max(pos.x + btn_size.x, pos.y + btn_size.y); + dl->AddRectFilled(pos, max, bg, s.FrameRounding, corners); + + // Draw border. + if (s.FrameBorderSize > 0) { + dl->AddRect(pos, max, ImGui::GetColorU32(ImGuiCol_Border), + s.FrameRounding, corners, s.FrameBorderSize); + } + + // Draw label centered. + const ImVec2 text_pos(pos.x + (btn_size.x - label_size.x) * 0.5f, + pos.y + (btn_size.y - label_size.y) * 0.5f); + dl->AddText(text_pos, ImGui::GetColorU32(ImGuiCol_Text), label); + + return clicked; +} + // Begin a boxed section with outer borders - use EndBoxSection to close. inline bool BeginBoxSection(const char* id, ImGuiTableFlags extra_flags = 0) { ImGuiTableFlags flags = ImGuiTableFlags_BordersOuter | extra_flags; diff --git a/src/experimental/studio/app.cc b/src/experimental/studio/app.cc index dec270ae..25dd1676 100644 --- a/src/experimental/studio/app.cc +++ b/src/experimental/studio/app.cc @@ -72,13 +72,11 @@ static void SelectParentPerturb(const mjModel* model, mjvPerturb& perturb) { } static constexpr const char* ICON_COPY_CAMERA = platform::ICON_FA_COPY; -static constexpr const char* ICON_UNLOAD_MODEL = platform::ICON_FA_EJECT; static constexpr const char* ICON_RELOAD_MODEL = platform::ICON_FA_REFRESH; static constexpr const char* ICON_RESET_MODEL = platform::ICON_FA_UNDO; static constexpr const char* ICON_PREV_FRAME = platform::ICON_FA_CARET_LEFT; static constexpr const char* ICON_NEXT_FRAME = platform::ICON_FA_CARET_RIGHT; static constexpr const char* ICON_CURR_FRAME = platform::ICON_FA_FAST_FORWARD; -static constexpr const char* ICON_RELOAD_SPEC = platform::ICON_FA_REFRESH; static constexpr const char* ICON_UNDO_SPEC = platform::ICON_FA_UNDO; static constexpr const char* ICON_REDO_SPEC = platform::ICON_FA_REPEAT; @@ -1402,13 +1400,12 @@ void App::HelpGui() { } void App::ToolBarGui() { + ImGui::PushStyleVar(ImGuiStyleVar_CellPadding, ImVec2(0, 0)); if (ImGui::BeginTable("##ToolBarTable", 2)) { platform::ScopedStyle style; - const ImColor red(220, 40, 40, 255); - - const float scale = ImGui::GetWindowDpiScale(); - const ImVec2 button_size(48.f * scale, 32.f * scale); - const ImVec2 play_button_size(80.f * scale, 32.f * scale); + style.Var(ImGuiStyleVar_ItemSpacing, + ImVec2(ImGui::GetStyle().ItemSpacing.x * 2.0f, + ImGui::GetStyle().ItemSpacing.y)); const float label_width = GetExpectedLabelWidth(); const float copy_btn_width = ImGui::CalcTextSize(ICON_COPY_CAMERA).x + @@ -1420,42 +1417,29 @@ void App::ToolBarGui() { const float right_width = label_width + sp + label_width + sp + label_width + sp + copy_btn_width + sp + theme_width; - const float separator_width = .2f * button_size.x; + const float separator_width = ImGui::GetFrameHeight() * .6f; ImGui::TableSetupColumn("", ImGuiTableColumnFlags_WidthStretch); ImGui::TableSetupColumn("", ImGuiTableColumnFlags_WidthFixed, right_width); ImGui::TableNextColumn(); + ImGui::SetCursorPosX(ImGui::GetCursorPosX() + ImGui::GetStyle().WindowPadding.x); - // Combined (Unload, Reload) widget + const float btn_size = ImGui::GetFrameHeight(); + const ImVec2 square_size(btn_size, btn_size); + + // Reload button. { style.Var(ImGuiStyleVar_FrameRounding, 2.0f); - - // Unload button. - { - const ImColor a = red; - const ImColor h(a.Value.x, a.Value.y, a.Value.z, a.Value.w * 0.6f); - style.Color(ImGuiCol_ButtonHovered, h); - style.Color(ImGuiCol_ButtonActive, a); - - if (ImGui::Button(ICON_UNLOAD_MODEL, button_size)) { - InitEmptyModel(); - } - ImGui::SetItemTooltip("%s", "Unload"); - style.Reset(); - } - - // Reload button. - ImGui::SameLine(0, 0); - if (ImGui::Button(ICON_RELOAD_MODEL, button_size)) { + if (ImGui::Button(ICON_RELOAD_MODEL, square_size)) { RequestModelReload(); } ImGui::SetItemTooltip("%s", "Reload"); } // Reset button. - ImGui::SameLine(0, separator_width); - if (ImGui::Button(ICON_RESET_MODEL, button_size)) { + ImGui::SameLine(0, 0.5 * separator_width); + if (ImGui::Button(ICON_RESET_MODEL, square_size)) { ResetPhysics(); } ImGui::SetItemTooltip("%s", "Reset"); @@ -1465,10 +1449,8 @@ void App::ToolBarGui() { platform::StepControlGui(model(), &step_control_, tmp_.speed_index); ImGui::TableNextColumn(); - ImGui::SetCursorPosY(ImGui::GetCursorPosY() + - (button_size.y - ImGui::GetFrameHeight()) * 0.5f); - if (ImGui::Button(ICON_COPY_CAMERA)) { + if (ImGui::Button(ICON_COPY_CAMERA, square_size)) { std::string camera_string = platform::CameraToString(data(), &camera_); platform::MaybeSaveToClipboard(camera_string); } @@ -1488,13 +1470,14 @@ void App::ToolBarGui() { ImGui::SameLine(); ImGui::SetNextItemWidth(GetExpectedLabelWidth()); - if (platform::ThemeSelectGui(&ui_.theme)) { + if (platform::ThemeSelectGui(&ui_.theme, square_size)) { platform::SetupTheme(ui_.theme); ImGui::GetIO().WantSaveIniSettings = true; } ImGui::EndTable(); } + ImGui::PopStyleVar(); } void App::StatusBarGui() { From 32aeb377b5defe490b24c1d88c954835f6e670c4 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 28 Apr 2026 03:55:28 -0700 Subject: [PATCH 153/251] Move filament::Skybox object management into SceneView. ModelObjects no longer directly manages any filament objects. PiperOrigin-RevId: 906873925 Change-Id: Iaa317cbeb9c5236745a5f0d04a740985bec54a5c --- .../filament/compat/model_objects.cc | 55 ++----------------- .../filament/compat/model_objects.h | 15 +---- .../filament/compat/scene_bridge.cc | 5 +- .../filament/filament/scene_view.cc | 22 +++++--- .../filament/filament/scene_view.h | 4 +- 5 files changed, 23 insertions(+), 78 deletions(-) diff --git a/src/experimental/filament/compat/model_objects.cc b/src/experimental/filament/compat/model_objects.cc index 1a52b6f1..67e07a08 100644 --- a/src/experimental/filament/compat/model_objects.cc +++ b/src/experimental/filament/compat/model_objects.cc @@ -26,12 +26,7 @@ #include #include -#include -#include -#include #include -#include -#include #include #include #include @@ -47,7 +42,6 @@ namespace mujoco { using filament::math::float2; using filament::math::float3; using filament::math::float4; -using filament::math::mat3f; enum class MeshType { kNormal, @@ -534,12 +528,6 @@ ModelObjects::ModelObjects(const mjModel* model, filament::Engine* engine) } ModelObjects::~ModelObjects() { - for (auto& iter : skyboxes_) { - engine_->destroy(iter); - } - for (auto& iter : indirect_lights_) { - engine_->destroy(iter); - } meshes_.clear(); textures_.clear(); } @@ -681,48 +669,13 @@ const Texture* ModelObjects::GetTexture(int mat_id, int role) const { return GetTexture(tex_id); } -filament::IndirectLight* ModelObjects::CreateIndirectLight(int tex_id, - float intensity) { - filament::Texture* texture = nullptr; - const Texture::SphericalHarmonics* spherical_harmonics = nullptr; - auto texture_iter = textures_.find(tex_id); - if (texture_iter != textures_.end()) { - texture = texture_iter->second->GetFilamentTexture(); - spherical_harmonics = texture_iter->second->GetSphericalHarmonics(); - } - - filament::IndirectLight::Builder builder; - builder.reflections(texture); - if (spherical_harmonics != nullptr) { - builder.irradiance(3, *spherical_harmonics); - } - builder.intensity(intensity); - // Rotate the light to match mujoco's Z-up convention. - builder.rotation(mat3f::rotation(filament::math::f::PI / 2, float3{1, 0, 0})); - filament::IndirectLight* indirect_light = builder.build(*engine_); - indirect_lights_.push_back(indirect_light); - return indirect_light; -} - -filament::Skybox* ModelObjects::CreateSkybox() { - filament::Texture* skybox_texture = nullptr; +const Texture* ModelObjects::GetSkyboxTexture() const { for (auto& iter : textures_) { - const int texture_type = model_->tex_type[iter.first]; - if (texture_type == mjTEXTURE_SKYBOX) { - skybox_texture = iter.second->GetFilamentTexture(); - break; + if (model_->tex_type[iter.first] == mjTEXTURE_SKYBOX) { + return iter.second.get(); } } - - if (skybox_texture == nullptr) { - return nullptr; - } - - filament::Skybox::Builder builder; - builder.environment(skybox_texture); - filament::Skybox* skybox = builder.build(*engine_); - skyboxes_.push_back(skybox); - return skybox; + return nullptr; } } // namespace mujoco diff --git a/src/experimental/filament/compat/model_objects.h b/src/experimental/filament/compat/model_objects.h index db528d36..4b7b0afd 100644 --- a/src/experimental/filament/compat/model_objects.h +++ b/src/experimental/filament/compat/model_objects.h @@ -18,11 +18,8 @@ #include #include #include -#include #include -#include -#include #include #include #include "experimental/filament/filament/mesh.h" @@ -30,7 +27,7 @@ namespace mujoco { -// Creates and owns various filament objects based on the data in a mjrContext. +// Creates and owns various filament objects based on the mjModel. class ModelObjects { public: ModelObjects(const mjModel* model, filament::Engine* engine); @@ -58,10 +55,6 @@ class ModelObjects { void CreateSkinFlexMesh(const mjvScene* scene, const mjvGeom& geom); - // Returns the filament engine used by the ModelObjects to create filament - // objects. - filament::Engine* GetEngine() const { return engine_; } - // Returns the cached instance of a filament object created from the mjModel. const Mesh* GetShapeBuffer(ShapeType shape) const; const Mesh* GetMeshBuffer(int data_id) const; @@ -69,9 +62,7 @@ class ModelObjects { const Mesh* GetFlexSkinGeomMesh(int geom_id) const; const Texture* GetTexture(int tex_id) const; const Texture* GetTexture(int mat_id, int role) const; - - filament::Skybox* CreateSkybox(); - filament::IndirectLight* CreateIndirectLight(int tex_id, float intensity); + const Texture* GetSkyboxTexture() const; float GetSpecularMultiplier() const { return specular_multiplier_; } float GetShininessMultiplier() const { return shininess_multiplier_; } @@ -85,8 +76,6 @@ class ModelObjects { private: const mjModel* model_ = nullptr; filament::Engine* engine_ = nullptr; - std::vector skyboxes_; - std::vector indirect_lights_; std::array, kNumShapes> shapes_; std::unordered_map> meshes_; std::unordered_map> convex_hulls_; diff --git a/src/experimental/filament/compat/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc index 6ab29a36..aebb85ef 100644 --- a/src/experimental/filament/compat/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -322,10 +322,7 @@ void SceneBridge::PrepareLights() { } } - filament::Skybox* skybox = model_objects_->CreateSkybox(); - if (skybox) { - scene_view_->AddToScene(skybox); - } + scene_view_->SetSkybox(model_objects_->GetSkyboxTexture()); } filament::math::mat4 CalculateClipFromWorld(const mjrRect& viewport, diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 997b048f..d11edc13 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -158,6 +158,10 @@ SceneView::SceneView(filament::Engine* engine) : engine_(engine) { } SceneView::~SceneView() { + if (skybox_) { + scene_->setSkybox(nullptr); + engine_->destroy(skybox_); + } for (auto& light : lights_) { light->RemoveFromScene(scene_); } @@ -210,15 +214,17 @@ void SceneView::RemoveFromScene(Renderable* renderable) { } } -void SceneView::AddToScene(filament::Skybox* skybox) { - skybox_ = skybox; - scene_->setSkybox(skybox); -} - -void SceneView::RemoveFromScene(filament::Skybox* skybox) { - if (skybox_ == skybox) { - skybox_ = nullptr; +void SceneView::SetSkybox(const Texture* skybox_texture) { + if (skybox_) { scene_->setSkybox(nullptr); + engine_->destroy(skybox_); + skybox_ = nullptr; + } + if (skybox_texture) { + filament::Skybox::Builder builder; + builder.environment(skybox_texture->GetFilamentTexture()); + skybox_ = builder.build(*engine_); + scene_->setSkybox(skybox_); } } diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index b6d5dfa6..59a3e9b3 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -31,6 +31,7 @@ #include "experimental/filament/filament/light.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/render_target.h" +#include "experimental/filament/filament/texture.h" namespace mujoco { @@ -49,8 +50,7 @@ class SceneView { void RemoveFromScene(Light* light); void AddToScene(Renderable* renderable); void RemoveFromScene(Renderable* renderable); - void AddToScene(filament::Skybox* skybox); - void RemoveFromScene(filament::Skybox* skybox); + void SetSkybox(const Texture* skybox_texture); // Parameters for rendering the scene. struct RenderRequest { From 00fff8c780ede515c375599f4bff81f835cb12cb Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 28 Apr 2026 04:40:05 -0700 Subject: [PATCH 154/251] Update APIs to use opaque handle types instead of concrete types. PiperOrigin-RevId: 906895053 Change-Id: Id61d437ea814e038f25d92375eb747a6c6775c32 --- .../filament/filament/filament_context.cc | 14 +++++++++----- .../filament/filament/filament_context.h | 19 +++++++++++++------ src/experimental/filament/filament/light.cc | 5 +++-- src/experimental/filament/filament/light.h | 12 ++++++++++-- src/experimental/filament/filament/mesh.h | 14 +++++++++++--- .../filament/filament/render_target.h | 10 +++++++++- .../filament/filament/renderable.h | 10 +++++++++- .../filament/filament/scene_view.h | 14 +++++++++++--- src/experimental/filament/filament/texture.h | 16 +++++++++++----- .../filament/render_context_filament.h | 11 +++++++++++ 10 files changed, 97 insertions(+), 28 deletions(-) diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index e367b44f..906c32e3 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -87,7 +87,7 @@ FilamentContext::FrameHandle FilamentContext::Render( } bool render_began = false; - RenderTarget* current_target = nullptr; + mjrRenderTarget* current_target = nullptr; for (const RenderRequest& request : requests) { if (request.target != current_target && render_began) { renderer_->endFrame(); @@ -126,7 +126,8 @@ FilamentContext::FrameHandle FilamentContext::Render( scene_view_request.draw_mode = request.draw_mode; scene_view_request.viewport = {0, 0, request.width, request.height}; scene_view_request.camera = request.camera; - request.scene->Render(renderer_, scene_view_request); + SceneView* scene_view = SceneView::downcast(request.scene); + scene_view->Render(renderer_, scene_view_request); } } else { if (read_requests.empty()) { @@ -146,13 +147,16 @@ FilamentContext::FrameHandle FilamentContext::Render( break; } if (render_began) { + RenderTarget* render_target = RenderTarget::downcast(request.target); + SceneView::RenderRequest scene_view_request; scene_view_request.draw_mode = request.draw_mode; scene_view_request.viewport = {0, 0, request.width, request.height}; scene_view_request.camera = request.camera; - scene_view_request.target = request.target; - request.scene->Render(renderer_, scene_view_request); - request.target->ReadColorPixels(renderer_, read_request.output, + scene_view_request.target = render_target; + SceneView* scene_view = SceneView::downcast(request.scene); + scene_view->Render(renderer_, scene_view_request); + render_target->ReadColorPixels(renderer_, read_request.output, read_request.num_bytes); } } diff --git a/src/experimental/filament/filament/filament_context.h b/src/experimental/filament/filament/filament_context.h index 391e9818..2e98acaf 100644 --- a/src/experimental/filament/filament/filament_context.h +++ b/src/experimental/filament/filament/filament_context.h @@ -35,15 +35,18 @@ namespace mujoco { // Manages the filament renderer and provides APIs for rendering scenes. -class FilamentContext { +class FilamentContext : public mjrfContext { public: explicit FilamentContext(const mjrFilamentConfig* config); ~FilamentContext(); + FilamentContext(const FilamentContext&) = delete; + FilamentContext& operator=(const FilamentContext&) = delete; + // Information needed to render a single image of a scene. struct RenderRequest { // The scene to render. - SceneView* scene = nullptr; + mjrScene* scene = nullptr; // The method (e.g. Color, Depth, Segmentation, etc.) to use for rendering. DrawMode draw_mode = DrawMode::Color; @@ -58,12 +61,12 @@ class FilamentContext { // The render target into which to render the image. If nullptr, the image // will be rendered to the window (as previously configured in // mjrFilamentConfig::native_window). - RenderTarget* target = nullptr; + mjrRenderTarget* target = nullptr; }; // Information needed to read pixels from a render target. struct ReadPixelsRequest { - RenderTarget* target = nullptr; + mjrRenderTarget* target = nullptr; // The buffer into which the read pixels will be written. uint8_t* output = nullptr; @@ -107,8 +110,12 @@ class FilamentContext { ObjectManager* GetObjectManager() const { return object_manager_.get(); } - FilamentContext(const FilamentContext&) = delete; - FilamentContext& operator=(const FilamentContext&) = delete; + static FilamentContext* downcast(mjrfContext* context) { + return static_cast(context); + } + static const FilamentContext* downcast(const mjrfContext* context) { + return static_cast(context); + } private: mjrFilamentConfig config_; diff --git a/src/experimental/filament/filament/light.cc b/src/experimental/filament/filament/light.cc index bdc1f2cd..d79a1551 100644 --- a/src/experimental/filament/filament/light.cc +++ b/src/experimental/filament/filament/light.cc @@ -56,9 +56,10 @@ Light::Light(filament::Engine* engine, const mjrLightParams& params) filament::IndirectLight::Builder builder; if (params.texture) { // Allow null textures for fallback lights. - builder.reflections(params.texture->GetFilamentTexture()); + const Texture* texture = Texture::downcast(params.texture); + builder.reflections(texture->GetFilamentTexture()); const Texture::SphericalHarmonics* spherical_harmonics = - params.texture->GetSphericalHarmonics(); + texture->GetSphericalHarmonics(); if (spherical_harmonics != nullptr) { builder.irradiance(3, *spherical_harmonics); } diff --git a/src/experimental/filament/filament/light.h b/src/experimental/filament/filament/light.h index 93ad0bb8..d58dbdd0 100644 --- a/src/experimental/filament/filament/light.h +++ b/src/experimental/filament/filament/light.h @@ -21,6 +21,7 @@ #include #include #include "experimental/filament/filament/texture.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -31,7 +32,7 @@ struct mjrLightParams { // The type of light (e.g. spot, point, directional, etc.) mjrLightType type; // The texture to use for image lights. - const Texture* texture; + const mjrTexture* texture; // The color of the light. float color[3]; // The intensity of the light, in candela. @@ -53,7 +54,7 @@ struct mjrLightParams { void mjr_defaultLightParams(mjrLightParams* params); // Manages the filament Entities for a single mjvLight. -class Light { +class Light : public mjrLight { public: Light(filament::Engine* engine, const mjrLightParams& params); ~Light() noexcept; @@ -84,6 +85,13 @@ class Light { void Enable(); void Disable(); + static Light* downcast(mjrLight* light) { + return static_cast(light); + } + static const Light* downcast(const mjrLight* light) { + return static_cast(light); + } + private: filament::Engine* engine_ = nullptr; filament::IndirectLight* ibl_ = nullptr; diff --git a/src/experimental/filament/filament/mesh.h b/src/experimental/filament/filament/mesh.h index 3b7fc5ab..9746e4ab 100644 --- a/src/experimental/filament/filament/mesh.h +++ b/src/experimental/filament/filament/mesh.h @@ -31,6 +31,7 @@ #include #include #include +#include "experimental/filament/render_context_filament.h" // Functions for creating filament vertex and index buffers. namespace mujoco { @@ -138,13 +139,16 @@ struct mjrMeshData { void mjr_defaultMeshData(mjrMeshData* data); // Owns a Vertex and Index buffer representing a geometry mesh. -class Mesh { +class Mesh : public mjrMesh { public: // Creates a Mesh from the given MeshData. Mesh(filament::Engine* engine, const mjrMeshData& data); ~Mesh(); + Mesh(const Mesh&) = delete; + Mesh& operator=(const Mesh&) = delete; + // Returns the filament IndexBuffer for the mesh. filament::IndexBuffer* GetFilamentIndexBuffer() const; @@ -163,8 +167,12 @@ class Mesh { // Returns the bounds of the mesh. filament::Box GetBounds() const; - Mesh(const Mesh&) = delete; - Mesh& operator=(const Mesh&) = delete; + static Mesh* downcast(mjrMesh* mesh) { + return static_cast(mesh); + } + static const Mesh* downcast(const mjrMesh* mesh) { + return static_cast(mesh); + } private: void BuildVertexBuffer(const mjrMeshData& data); diff --git a/src/experimental/filament/filament/render_target.h b/src/experimental/filament/filament/render_target.h index b731a567..9e5aed68 100644 --- a/src/experimental/filament/filament/render_target.h +++ b/src/experimental/filament/filament/render_target.h @@ -22,6 +22,7 @@ #include #include #include "experimental/filament/filament/texture.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -35,7 +36,7 @@ struct mjrRenderTargetConfig { void mjr_defaultRenderTargetConfig(mjrRenderTargetConfig* config); // Manages a filament RenderTarget and the textures which are bound to it. -class RenderTarget { +class RenderTarget : public mjrRenderTarget { public: // Defines the types of textures to create for the color and depth // attachments. @@ -62,6 +63,13 @@ class RenderTarget { // Returns the underlying filament render target. filament::RenderTarget* GetFilamentRenderTarget() const; + static RenderTarget* downcast(mjrRenderTarget* render_target) { + return static_cast(render_target); + } + static const RenderTarget* downcast(const mjrRenderTarget* render_target) { + return static_cast(render_target); + } + private: void Destroy(); diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index a0da8166..35f9028a 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -29,6 +29,7 @@ #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -61,7 +62,7 @@ void mjr_defaultRenderableParams(mjrRenderableParams* params); // the user specifies the MaterialParams and MaterialTextures to use with the // ShadingModel. Its these properties that ultimately define the actual material // of the Renderable. -class Renderable { +class Renderable : public mjrRenderable { public: // Default filament values for priority and layer mask. static constexpr std::uint8_t kDefaultPriority = 4; @@ -139,6 +140,13 @@ class Renderable { // Returns the filament Engine managing the renderables. filament::Engine* GetEngine(); + static Renderable* downcast(mjrRenderable* renderable) { + return static_cast(renderable); + } + static const Renderable* downcast(const mjrRenderable* renderable) { + return static_cast(renderable); + } + private: struct Part { utils::Entity entity; diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index 59a3e9b3..f35110f8 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -32,6 +32,7 @@ #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/render_target.h" #include "experimental/filament/filament/texture.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -40,11 +41,14 @@ namespace mujoco { // The filament Scene is populated with the objects (e.g. lights, renderables, // skybox, etc.). It manages multiple views to support a variety of draw modes // (e.g. normal, depth, segmentation, etc.) as well as reflective surfaces. -class SceneView { +class SceneView : public mjrScene { public: SceneView(filament::Engine* engine); ~SceneView(); + SceneView(const SceneView&) = delete; + SceneView& operator=(const SceneView&) = delete; + // Adds/removes entities from the scene. void AddToScene(Light* light); void RemoveFromScene(Light* light); @@ -90,8 +94,12 @@ class SceneView { ColorGradingOptions GetColorGradingOptions() const; void SetColorGradingOptions(const ColorGradingOptions& opts); - SceneView(const SceneView&) = delete; - SceneView& operator=(const SceneView&) = delete; + static SceneView* downcast(mjrScene* scene) { + return static_cast(scene); + } + static const SceneView* downcast(const mjrScene* scene) { + return static_cast(scene); + } private: // Marks a renderable as reflective. Reflective renderables have to be diff --git a/src/experimental/filament/filament/texture.h b/src/experimental/filament/filament/texture.h index e51bbfff..764ae911 100644 --- a/src/experimental/filament/filament/texture.h +++ b/src/experimental/filament/filament/texture.h @@ -15,13 +15,12 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_TEXTURE_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_TEXTURE_H_ -#include - #include #include #include #include #include +#include "experimental/filament/render_context_filament.h" // Functions for creating filament textures. namespace mujoco { @@ -84,7 +83,7 @@ struct mjrTextureConfig { void mjr_defaultTextureConfig(mjrTextureConfig* config); // Wrapper around a filament::Texture. -class Texture { +class Texture : public mjrTexture { public: // Flags for internal use. struct InternalFlags { @@ -99,6 +98,9 @@ class Texture { ~Texture(); + Texture(const Texture&) = delete; + Texture& operator=(const Texture&) = delete; + // Uploads the given data to the texture. void Upload(const mjrTextureData& data); @@ -117,8 +119,12 @@ class Texture { return has_spherical_harmonics_ ? &spherical_harmonics_ : nullptr; } - Texture(const Texture&) = delete; - Texture& operator=(const Texture&) = delete; + static Texture* downcast(mjrTexture* texture) { + return static_cast(texture); + } + static const Texture* downcast(const mjrTexture* texture) { + return static_cast(texture); + } private: void ReleaseData(); diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index 28183fec..ef6f88ea 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -29,6 +29,17 @@ extern "C" { // IMPORTANT: This API should still be considered experimental and is likely // change frequently. +// Opaque types. +struct mjrTexture {}; +struct mjrMesh {}; +struct mjrScene {}; +struct mjrLight {}; +struct mjrRenderable {}; +struct mjrRenderTarget {}; + +// Opaque type for the filament rendering context. +struct mjrfContext {}; + typedef enum mjrGraphicsApi_ { // backend graphics API to use mjGRAPHICS_API_DEFAULT = 0, // default based on platform mjGRAPHICS_API_OPENGL, // OpenGL (desktop) / WebGL From 647af382c11224d8a64ba124fc4bfa949c268d97 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 28 Apr 2026 05:13:22 -0700 Subject: [PATCH 155/251] Add per-island PGS solver dispatch. Total testspeed runtime for `2humanoids100.xml` reduced by 27.6% (63.4 -> 49.5s) due to early termination on small islands PiperOrigin-RevId: 906910915 Change-Id: If55ad468c3680ef44eda7000455a77f8003b3122 --- doc/changelog.rst | 5 ++ doc/computation/index.rst | 6 -- simulate/simulate.cc | 2 +- src/engine/engine_forward.c | 93 +++++++++++++++++++------------ src/engine/engine_solver.c | 51 ++++++++++++----- src/engine/engine_solver.h | 9 +++ test/engine/engine_island_test.cc | 37 ++++++++++++ 7 files changed, 146 insertions(+), 57 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 79c01e44..4a8f22d4 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,6 +2,11 @@ Changelog ========= +Upcoming version (not yet released) +----------------------------------- + +- Added island support for the :ref:`PGS solver`. + Version 3.8.0 (April 24, 2026) ------------------------------ diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 07f9adb5..000abcc4 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -1424,12 +1424,6 @@ While islanding is not free (see implementation in `engine_island.c - Unconstrained DOFs are completely untouched by the solver, which otherwise needs to discover that they are unaffected. - Solving separate islands can be multi-threaded. -.. admonition:: Known issues - :class: note - - Islanding is not yet supported by the PGS solver. - - .. _soParameters: Parameters diff --git a/simulate/simulate.cc b/simulate/simulate.cc index b4e29e35..76e2b788 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -349,7 +349,7 @@ void UpdateProfiler(mj::Simulate* sim, const mjModel* m, const mjData* d) { sim->figcost.linepnt[start + 2] = 0; } - for (int i=0; ifigcost.linepnt[0]; i++) { + for (int i=0; ifigcost.linedata[start + 0][2*i] = i; sim->figcost.linedata[start + 1][2*i] = i; diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index dbd3a74e..0df4c0f1 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -953,7 +953,7 @@ static void solve_threaded(const mjModel* m, mjData* d, int flg_Newton) { // compute efc_b, efc_force, qfrc_constraint; update qacc void mj_fwdConstraint(const mjModel* m, mjData* d) { TM_START; - int nv = m->nv, nefc = d->nefc, nisland = d->nisland; + int nv = m->nv, nefc = d->nefc, nisland = d->nisland, nidof; // always clear qfrc_constraint mju_zero(d->qfrc_constraint, nv); @@ -970,50 +970,69 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) { mj_mulJacVec(m, d, d->efc_b, d->qacc_smooth); mju_subFrom(d->efc_b, d->efc_aref, nefc); + // check for invalid solver type + if (m->opt.solver != mjSOL_PGS && m->opt.solver != mjSOL_CG && m->opt.solver != mjSOL_NEWTON) { + mjERROR("unknown solver type %d", m->opt.solver); + } + // warmstart solver warmstart(m, d); mju_zeroInt(d->solver_niter, mjNISLAND); // check if islands are supported - int islands_supported = !mjDISABLED(mjDSBL_ISLAND) && - nisland > 0 && - m->opt.noslip_iterations == 0 && - (m->opt.solver == mjSOL_CG || m->opt.solver == mjSOL_NEWTON); + int islands_supported = !mjDISABLED(mjDSBL_ISLAND) && nisland > 0; // run solver over constraint islands if (islands_supported) { - int nidof = d->nidof; - - // copy inputs to islands (vel+acc deps, pos-dependent already copied in mj_island) - mju_gather(d->ifrc_smooth, d->qfrc_smooth, d->map_idof2dof, nidof); - mju_gather(d->ifrc_constraint, d->qfrc_constraint, d->map_idof2dof, nidof); - mju_gather(d->iacc_smooth, d->qacc_smooth, d->map_idof2dof, nidof); - mju_gather(d->iacc, d->qacc, d->map_idof2dof, nidof); - mju_gather(d->iefc_force, d->efc_force, d->map_iefc2efc, nefc); - mju_gather(d->iefc_aref, d->efc_aref, d->map_iefc2efc, nefc); - - // solve per island, with or without threads - if (!d->threadpool) { - // no threadpool, loop over islands + switch ((mjtSolver) m->opt.solver) { + case mjSOL_PGS: for (int island=0; island < nisland; island++) { - if (m->opt.solver == mjSOL_NEWTON) { - mj_solNewton_island(m, d, island, m->opt.iterations); - } else { - mj_solCG_island(m, d, island, m->opt.iterations); - } + mj_solPGS_island(m, d, island, m->opt.iterations); } - } else { - // have threadpool, solve using threads - solve_threaded(m, d, m->opt.solver == mjSOL_NEWTON); + break; + + case mjSOL_CG: + case mjSOL_NEWTON: + // copy inputs to islands (vel+acc deps, pos-dependent already copied in mj_island) + nidof = d->nidof; + mju_gather(d->ifrc_smooth, d->qfrc_smooth, d->map_idof2dof, nidof); + mju_gather(d->ifrc_constraint, d->qfrc_constraint, d->map_idof2dof, nidof); + mju_gather(d->iacc_smooth, d->qacc_smooth, d->map_idof2dof, nidof); + mju_gather(d->iacc, d->qacc, d->map_idof2dof, nidof); + mju_gather(d->iefc_force, d->efc_force, d->map_iefc2efc, nefc); + mju_gather(d->iefc_aref, d->efc_aref, d->map_iefc2efc, nefc); + + // solve per island, with or without threads + if (!d->threadpool) { + // no threadpool, loop over islands + for (int island=0; island < nisland; island++) { + if (m->opt.solver == mjSOL_NEWTON) { + mj_solNewton_island(m, d, island, m->opt.iterations); + } else { + mj_solCG_island(m, d, island, m->opt.iterations); + } + } + } else { + // have threadpool, solve using threads + solve_threaded(m, d, m->opt.solver == mjSOL_NEWTON); + } + + // copy back solver outputs (scatter dofs since ni <= nv) + mju_scatter(d->qacc, d->iacc, d->map_idof2dof, nidof); + mju_scatter(d->qfrc_constraint, d->ifrc_constraint, d->map_idof2dof, nidof); + mju_gather(d->efc_force, d->iefc_force, d->map_efc2iefc, nefc); + break; } - // copy back solver outputs (scatter dofs since ni <= nv) - mju_scatter(d->qacc, d->iacc, d->map_idof2dof, nidof); - mju_scatter(d->qfrc_constraint, d->ifrc_constraint, d->map_idof2dof, nidof); - mju_gather(d->efc_force, d->iefc_force, d->map_efc2iefc, nefc); + // run noslip solver per island if enabled + if (m->opt.noslip_iterations > 0) { + for (int island=0; island < nisland; island++) { + mj_solNoSlip_island(m, d, island, m->opt.noslip_iterations); + } + } } - // run solver over all constraints + // run solver over all constraints (monolithic) else { switch ((mjtSolver) m->opt.solver) { case mjSOL_PGS: // PGS @@ -1027,15 +1046,17 @@ void mj_fwdConstraint(const mjModel* m, mjData* d) { case mjSOL_NEWTON: // Newton mj_solNewton(m, d, m->opt.iterations); break; + } - default: - mjERROR("unknown solver type %d", m->opt.solver); + // run noslip solver if enabled + if (m->opt.noslip_iterations > 0) { + mj_solNoSlip(m, d, m->opt.noslip_iterations); } } - // run noslip solver if enabled - if (m->opt.noslip_iterations > 0) { - mj_solNoSlip(m, d, m->opt.noslip_iterations); + // dual solvers: map efc_force to joint space (always monolithic) + if (m->opt.solver == mjSOL_PGS || m->opt.noslip_iterations > 0) { + mj_dualFinish(m, d); } TM_END(mjTIMER_CONSTRAINT); diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 5700a3e5..8a7b1d00 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -66,7 +66,6 @@ static void saveStats(const mjModel* m, mjData* d, int island, int iter, // finalize dual solver: map to joint space -// TODO: b/295296178 - add island support to Dual solvers static void dualFinish(const mjModel* m, mjData* d) { // map constraint force to joint space mj_mulJacTVec(m, d, d->qfrc_constraint, d->efc_force); @@ -77,10 +76,15 @@ static void dualFinish(const mjModel* m, mjData* d) { } +// PGS: map efc_force to joint space +void mj_dualFinish(const mjModel* m, mjData* d) { + dualFinish(m, d); +} + + // compute 1/diag(AR) // res[c] = 1 / AR[efclist[c], efclist[c]] for c = 0..nefc-1 // efclist is NULL for monolithic (sequential) iteration -// TODO: b/295296178 - add island support to Dual solvers static void ARdiaginv(const mjModel* m, const mjData* d, mjtNum* res, int nefc, const int* efclist, int flg_subR) { const mjtNum *AR = d->efc_AR; @@ -118,7 +122,6 @@ static void ARdiaginv(const mjModel* m, const mjData* d, mjtNum* res, // extract diagonal block from AR, clamp diag to 1e-10 if flg_subR -// TODO: b/295296178 - add island support to Dual solvers static void extractBlock(const mjModel* m, const mjData* d, mjtNum* Ac, int start, int n, int flg_subR) { int nefc = d->nefc; @@ -178,7 +181,6 @@ static void extractBlock(const mjModel* m, const mjData* d, mjtNum* Ac, // compute residual for one block -// TODO: b/295296178 - add island support to Dual solvers static void residual(const mjModel* m, const mjData* d, mjtNum* res, int i, int dim, int flg_subR) { int nefc = d->nefc; @@ -208,7 +210,6 @@ static void residual(const mjModel* m, const mjData* d, mjtNum* res, int i, int // compute cost change -// TODO: b/295296178 - add island support to Dual solvers static mjtNum costChange(const mjtNum* A, mjtNum* force, const mjtNum* oldforce, const mjtNum* res, int dim) { mjtNum change; @@ -235,7 +236,6 @@ static mjtNum costChange(const mjtNum* A, mjtNum* force, const mjtNum* oldforce, // set efc_state to dual constraint state; return nactive // iterates over efclist (or sequentially if NULL), classifies by ne/nf ranges -// TODO: b/295296178 - add island support to Dual solvers static int dualState(const mjData* d, int* state, int ne, int nf, int nefc, const int* efclist) { const mjtNum* force = d->efc_force; @@ -384,7 +384,6 @@ static void solveQCQP(mjtNum* force, int i, int dim, // island: island index for stats (use -1 for monolithic, mapped to 0) // ne, nf, nefc: constraint type counts // efclist: maps list position c to monolithic efc index (NULL for sequential) -// TODO: b/295296178 - add island support to Dual solvers static void solPGS(const mjModel* m, mjData* d, int island, int ne, int nf, int nefc, const int* efclist, int maxiter) { @@ -572,10 +571,20 @@ static void solPGS(const mjModel* m, mjData* d, int island, } -// PGS entry point (monolithic) +// PGS entry point (monolithic, no dualFinish — caller handles it) void mj_solPGS(const mjModel* m, mjData* d, int maxiter) { solPGS(m, d, /*island=*/-1, d->ne, d->nf, d->nefc, /*efclist=*/NULL, maxiter); - dualFinish(m, d); +} + + +// PGS entry point (one island) +void mj_solPGS_island(const mjModel* m, mjData* d, int island, int maxiter) { + int ne = d->island_ne[island]; + int nf = d->island_nf[island]; + int nefc = d->island_nefc[island]; + int iefcadr = d->island_iefcadr[island]; + + solPGS(m, d, island, ne, nf, nefc, d->map_iefc2efc + iefcadr, maxiter); } @@ -754,8 +763,10 @@ static void solNoSlip(const mjModel* m, mjData* d, int island, improvement *= scale; // save noslip stats after all the entries from regular solver - int stats_iter = iter + d->solver_niter[island_stat]; - saveStats(m, d, island_stat, stats_iter, improvement, 0, 0, nactive, nchange, 0, 0); + if (island_stat < mjNISLAND) { + int stats_iter = iter + d->solver_niter[island_stat]; + saveStats(m, d, island_stat, stats_iter, improvement, 0, 0, nactive, nchange, 0, 0); + } // increment iteration count iter++; @@ -767,16 +778,28 @@ static void solNoSlip(const mjModel* m, mjData* d, int island, } // update solver iterations - d->solver_niter[island_stat] += iter; + if (island_stat < mjNISLAND) { + d->solver_niter[island_stat] += iter; + } mj_freeStack(d); } -// NoSlip entry point (monolithic) +// NoSlip entry point (monolithic, no dualFinish — caller handles it) void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) { solNoSlip(m, d, /*island=*/-1, d->ne, d->nf, d->nefc, /*efclist=*/NULL, maxiter); - dualFinish(m, d); +} + + +// NoSlip entry point (one island) +void mj_solNoSlip_island(const mjModel* m, mjData* d, int island, int maxiter) { + int ne = d->island_ne[island]; + int nf = d->island_nf[island]; + int nefc = d->island_nefc[island]; + int iefcadr = d->island_iefcadr[island]; + + solNoSlip(m, d, island, ne, nf, nefc, d->map_iefc2efc + iefcadr, maxiter); } diff --git a/src/engine/engine_solver.h b/src/engine/engine_solver.h index 7ee007de..46657619 100644 --- a/src/engine/engine_solver.h +++ b/src/engine/engine_solver.h @@ -35,10 +35,19 @@ void mj_solNewton(const mjModel* m, mjData* d, int maxiter); //------------------------------ per-island solvers ------------------------------------------------ +// PGS solver (one island, no dualFinish — caller handles it) +void mj_solPGS_island(const mjModel* m, mjData* d, int island, int maxiter); + +// NoSlip solver (one island, no dualFinish — caller handles it) +void mj_solNoSlip_island(const mjModel* m, mjData* d, int island, int maxiter); + // CG solver void mj_solCG_island(const mjModel* m, mjData* d, int island, int maxiter); // Newton entry point void mj_solNewton_island(const mjModel* m, mjData* d, int island, int maxiter); +// map efc_force to joint space (used after dual island dispatch) +void mj_dualFinish(const mjModel* m, mjData* d); + #endif // MUJOCO_SRC_ENGINE_ENGINE_SOLVER_H_ diff --git a/test/engine/engine_island_test.cc b/test/engine/engine_island_test.cc index d934f852..c8055293 100644 --- a/test/engine/engine_island_test.cc +++ b/test/engine/engine_island_test.cc @@ -15,6 +15,7 @@ // Tests for engine/engine_island.c. #include +#include #include #include @@ -602,5 +603,41 @@ TEST_F(IslandTest, EqualityConstraintOfTendons) { mj_deleteModel(model); } +TEST_F(IslandTest, PGSIslandExact) { + const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath); + char error[1024]; + mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + + // simulate to get a non-trivial state + while (d->time < 0.5) { + mj_step(m, d); + } + + // switch to PGS, disable early termination + m->opt.solver = mjSOL_PGS; + m->opt.tolerance = 0; + + // solve with islands + m->opt.disableflags &= ~mjDSBL_ISLAND; + mj_forward(m, d); + ASSERT_GT(d->nisland, 1); + std::vector qfrc_island(d->qfrc_constraint, + d->qfrc_constraint + m->nv); + + // solve without islands + m->opt.disableflags |= mjDSBL_ISLAND; + mj_forward(m, d); + std::vector qfrc_mono(d->qfrc_constraint, + d->qfrc_constraint + m->nv); + + // expect exact match + EXPECT_EQ(qfrc_island, qfrc_mono); + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco From 5751b09ff63f8e026eb5fb36c0a1c753349d19d4 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 28 Apr 2026 05:59:15 -0700 Subject: [PATCH 156/251] Initial pass at exposing the new mjr APIs. Moves all internal types and structs to the "public" render_context_filament.h header file. PiperOrigin-RevId: 906929736 Change-Id: I7e7c315fb99a3601b09841073eaeec795718b999 --- .../filament/compat/mjr_filament_renderer.cc | 28 +- .../filament/compat/mjr_filament_renderer.h | 2 +- .../filament/compat/scene_geom_util.cc | 4 +- .../filament/filament/draw_mode.h | 36 -- .../filament/filament/filament_context.cc | 14 +- .../filament/filament/filament_context.h | 57 +--- src/experimental/filament/filament/light.cc | 16 +- src/experimental/filament/filament/light.h | 29 -- .../filament/filament/material.cc | 44 +-- src/experimental/filament/filament/material.h | 37 +- src/experimental/filament/filament/mesh.cc | 6 +- src/experimental/filament/filament/mesh.h | 104 ------ .../filament/filament/render_target.cc | 5 - .../filament/filament/render_target.h | 9 - .../filament/filament/renderable.cc | 28 +- .../filament/filament/renderable.h | 24 +- .../filament/filament/scene_view.cc | 28 +- .../filament/filament/scene_view.h | 5 +- src/experimental/filament/filament/texture.cc | 8 - src/experimental/filament/filament/texture.h | 57 ---- .../filament/render_context_filament.cc | 80 +++++ .../filament/render_context_filament.h | 322 ++++++++++++++++++ 22 files changed, 468 insertions(+), 475 deletions(-) delete mode 100644 src/experimental/filament/filament/draw_mode.h diff --git a/src/experimental/filament/compat/mjr_filament_renderer.cc b/src/experimental/filament/compat/mjr_filament_renderer.cc index 48242258..8953e2a2 100644 --- a/src/experimental/filament/compat/mjr_filament_renderer.cc +++ b/src/experimental/filament/compat/mjr_filament_renderer.cc @@ -25,7 +25,6 @@ #include "experimental/filament/compat/imgui_bridge.h" #include "experimental/filament/compat/imgui_editor.h" #include "experimental/filament/compat/scene_bridge.h" -#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/render_target.h" @@ -42,11 +41,14 @@ void MjrFilamentRenderer::Init(const mjModel* model) { scene_bridge_ = std::make_unique(GetObjectManager(), model); imgui_bridge_ = std::make_unique(GetObjectManager()); + mjr_defaultRenderRequest(&render_requests_[0]); + mjr_defaultRenderRequest(&render_requests_[1]); + render_requests_[0].scene = scene_bridge_->GetSceneView(); - render_requests_[0].draw_mode = DrawMode::Color; + render_requests_[0].draw_mode = mjDRAW_MODE_COLOR; render_requests_[1].scene = imgui_bridge_->GetSceneView(); - render_requests_[1].draw_mode = DrawMode::Color; + render_requests_[1].draw_mode = mjDRAW_MODE_COLOR; // The UX camera is a fixed orthographic camera. We only need to change the // width/height based on the viewport per frame. @@ -78,11 +80,11 @@ void MjrFilamentRenderer::Render(const mjrRect& viewport, const mjvScene* scene) } if (scene->flags[mjRND_SEGMENT]) { - render_requests_[0].draw_mode = DrawMode::Segmentation; + render_requests_[0].draw_mode = mjDRAW_MODE_SEGMENTATION; } else if (scene->flags[mjRND_DEPTH]) { - render_requests_[0].draw_mode = DrawMode::Depth; + render_requests_[0].draw_mode = mjDRAW_MODE_DEPTH; } else { - render_requests_[0].draw_mode = DrawMode::Color; + render_requests_[0].draw_mode = mjDRAW_MODE_COLOR; } render_requests_[0].width = viewport.width; @@ -142,10 +144,11 @@ void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, const size_t num_requests = (mode_ == FrameBufferMode::OffScreenWithGui) ? 2 : 1; - ReadPixelsRequest read_request; + mjrReadPixelsRequest read_request; + mjr_defaultReadPixelsRequest(&read_request); read_request.output = rgb; read_request.num_bytes = viewport.width * viewport.height * 3; - const FrameHandle frame = FilamentContext::Render( + const mjrFrameHandle frame = FilamentContext::Render( {&render_requests_[0], num_requests}, {&read_request, 1}); FilamentContext::WaitForFrame(frame); @@ -163,13 +166,14 @@ void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, render_requests_[0].target = target.get(); render_requests_[1].target = target.get(); - DrawMode last_draw_mode = render_requests_[0].draw_mode; - render_requests_[0].draw_mode = DrawMode::Depth; + mjrDrawMode last_draw_mode = render_requests_[0].draw_mode; + render_requests_[0].draw_mode = mjDRAW_MODE_DEPTH; - ReadPixelsRequest read_request; + mjrReadPixelsRequest read_request; + mjr_defaultReadPixelsRequest(&read_request); read_request.output = reinterpret_cast(depth); read_request.num_bytes = viewport.width * viewport.height * sizeof(float); - const FrameHandle frame = + const mjrFrameHandle frame = FilamentContext::Render({&render_requests_[0], 1}, {&read_request, 1}); FilamentContext::WaitForFrame(frame); diff --git a/src/experimental/filament/compat/mjr_filament_renderer.h b/src/experimental/filament/compat/mjr_filament_renderer.h index a1c97edc..3d87e624 100644 --- a/src/experimental/filament/compat/mjr_filament_renderer.h +++ b/src/experimental/filament/compat/mjr_filament_renderer.h @@ -76,7 +76,7 @@ class MjrFilamentRenderer : public FilamentContext { }; FrameBufferMode mode_ = FrameBufferMode::Window; - RenderRequest render_requests_[2]; + mjrRenderRequest render_requests_[2]; std::unique_ptr scene_bridge_; std::unique_ptr imgui_bridge_; }; diff --git a/src/experimental/filament/compat/scene_geom_util.cc b/src/experimental/filament/compat/scene_geom_util.cc index 527fe56f..ddc85bee 100644 --- a/src/experimental/filament/compat/scene_geom_util.cc +++ b/src/experimental/filament/compat/scene_geom_util.cc @@ -30,12 +30,12 @@ #include #include #include "experimental/filament/compat/model_objects.h" -#include "experimental/filament/filament/material.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/texture.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -424,7 +424,7 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, // the programmatic UVs. if (textures.color) { - if (textures.color->GetFilamentTexture()->getTarget() == + if (Texture::downcast(textures.color)->GetFilamentTexture()->getTarget() == filament::Texture::Sampler::SAMPLER_2D) { // For 2D textures, `tex_repeat` specifies how many times the texture // image is repeated. The `tex_uniform` flag determines if the repetition diff --git a/src/experimental/filament/filament/draw_mode.h b/src/experimental/filament/filament/draw_mode.h deleted file mode 100644 index 15bb2bca..00000000 --- a/src/experimental/filament/filament/draw_mode.h +++ /dev/null @@ -1,36 +0,0 @@ -// Copyright 2026 DeepMind Technologies Limited -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_DRAW_MODE_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_DRAW_MODE_H_ - -namespace mujoco { - -// The different modes that can be used to render the scene. -enum class DrawMode { - // Render the scene with "normal" colors and lighting. - Color, - // Render the scene as a grayscale depth map. - Depth, - // Render each object with a unique, uniform (flat) color regardless of - // lighting and texture. - Segmentation, -}; - -static constexpr int kNumDrawModes = 3; - -} // namespace mujoco - - -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_DRAW_MODE_H_ diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index 906c32e3..4149559c 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -79,16 +79,16 @@ FilamentContext::~FilamentContext() { filament::Engine::destroy(engine_); } -FilamentContext::FrameHandle FilamentContext::Render( - std::span requests, - std::span read_requests) { +mjrFrameHandle FilamentContext::Render( + std::span requests, + std::span read_requests) { if (read_requests.size() > 1) { mju_error("Only one read request is supported for now."); } bool render_began = false; mjrRenderTarget* current_target = nullptr; - for (const RenderRequest& request : requests) { + for (const mjrRenderRequest& request : requests) { if (request.target != current_target && render_began) { renderer_->endFrame(); render_began = false; @@ -135,7 +135,7 @@ FilamentContext::FrameHandle FilamentContext::Render( "Rendering to a render target without a read request is pointless."); } - const ReadPixelsRequest& read_request = read_requests[0]; + const mjrReadPixelsRequest& read_request = read_requests[0]; if (read_request.num_bytes == 0) { mju_error("Output buffer size is zero."); } @@ -156,7 +156,7 @@ FilamentContext::FrameHandle FilamentContext::Render( scene_view_request.target = render_target; SceneView* scene_view = SceneView::downcast(request.scene); scene_view->Render(renderer_, scene_view_request); - render_target->ReadColorPixels(renderer_, read_request.output, + render_target->ReadColorPixels(renderer_, (uint8_t*)read_request.output, read_request.num_bytes); } } @@ -179,7 +179,7 @@ FilamentContext::FrameHandle FilamentContext::Render( return ++frame_counter_; } -void FilamentContext::WaitForFrame(FrameHandle frame_handle) { +void FilamentContext::WaitForFrame(mjrFrameHandle frame_handle) { if (frame_counter_ < frame_handle) { engine_->flushAndWait(); } diff --git a/src/experimental/filament/filament/filament_context.h b/src/experimental/filament/filament/filament_context.h index 2e98acaf..732229e0 100644 --- a/src/experimental/filament/filament/filament_context.h +++ b/src/experimental/filament/filament/filament_context.h @@ -15,7 +15,6 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_FILAMENT_CONTEXT_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_FILAMENT_CONTEXT_H_ -#include #include #include #include @@ -25,11 +24,7 @@ #include #include #include -#include -#include "experimental/filament/filament/draw_mode.h" -#include "experimental/filament/filament/scene_view.h" #include "experimental/filament/filament/object_manager.h" -#include "experimental/filament/filament/render_target.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -43,62 +38,16 @@ class FilamentContext : public mjrfContext { FilamentContext(const FilamentContext&) = delete; FilamentContext& operator=(const FilamentContext&) = delete; - // Information needed to render a single image of a scene. - struct RenderRequest { - // The scene to render. - mjrScene* scene = nullptr; - - // The method (e.g. Color, Depth, Segmentation, etc.) to use for rendering. - DrawMode draw_mode = DrawMode::Color; - - // The camera from which to render the scene. - mjvGLCamera camera; - - // The dimensions of the output image. - int width = 0; - int height = 0; - - // The render target into which to render the image. If nullptr, the image - // will be rendered to the window (as previously configured in - // mjrFilamentConfig::native_window). - mjrRenderTarget* target = nullptr; - }; - - // Information needed to read pixels from a render target. - struct ReadPixelsRequest { - mjrRenderTarget* target = nullptr; - - // The buffer into which the read pixels will be written. - uint8_t* output = nullptr; - - // The number of bytes in the output buffer. This should match the size of - // the render target texture. - std::size_t num_bytes = 0; - - // Callback when the read pixels operation is complete. This will be called - // during WaitForFrame() or in a subsequent call to Render(). This function - // can optionally be used to free the output buffer if needed. - void (*read_completed_callback)(void* user_data) = nullptr; - - // User data to pass to the completion callback. - void* user_data = nullptr; - }; - - // Rendering is asynchronous by nature. Each render request is assigned a - // unique Handle which can be used to query the status of the request. The - // Handle can also be used to block until the request is completed. - using FrameHandle = std::uint64_t; - // Queues the given render requests for rendering. This function copies the // necessary data from the requests into the renderer thread and returns // immediately afterwards. The renderer thread will then perform the actual // rendering on the GPU. Callers can use WaitForFrame to block until the // rendering is complete. - FrameHandle Render(std::span render_requests, - std::span read_requests = {}); + mjrFrameHandle Render(std::span render_requests, + std::span read_requests = {}); // Blocks until the given frame has completed rendering. - void WaitForFrame(FrameHandle frame_handle); + void WaitForFrame(mjrFrameHandle frame_handle); // Sets the clear color for the renderer. void SetClearColor(const filament::math::float4& color); diff --git a/src/experimental/filament/filament/light.cc b/src/experimental/filament/filament/light.cc index d79a1551..2918f0a0 100644 --- a/src/experimental/filament/filament/light.cc +++ b/src/experimental/filament/filament/light.cc @@ -27,27 +27,13 @@ #include #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/texture.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { using filament::math::float3; using filament::math::mat3f; -void mjr_defaultLightParams(mjrLightParams* params) { - params->type = mjLIGHT_POINT; - params->texture = nullptr; - params->color[0] = 0; - params->color[1] = 0; - params->color[2] = 0; - params->intensity = 0.0f; - params->cast_shadows = true; - params->range = 10.0f; - params->spot_cone_angle = 180.f; - params->bulb_radius = 0.0f; - params->shadow_map_size = 2048; - params->vsm_blur_width = 0.0f; -} - Light::Light(filament::Engine* engine, const mjrLightParams& params) : engine_(engine), params_(params) { // Filament treats image-based lights (IBLs) as separate objects (i.e. diff --git a/src/experimental/filament/filament/light.h b/src/experimental/filament/filament/light.h index d58dbdd0..d0b6a884 100644 --- a/src/experimental/filament/filament/light.h +++ b/src/experimental/filament/filament/light.h @@ -20,39 +20,10 @@ #include #include #include -#include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { -typedef mjtLightType mjrLightType; - -// Configuration parameters for a light. -struct mjrLightParams { - // The type of light (e.g. spot, point, directional, etc.) - mjrLightType type; - // The texture to use for image lights. - const mjrTexture* texture; - // The color of the light. - float color[3]; - // The intensity of the light, in candela. - float intensity; - // Whether or not the light casts shadows. - mjtByte cast_shadows; - // The range/distance in which the light is effective, in meters. - float range; - // The angle of the spot light cone, in degrees. - float spot_cone_angle; - // The radius of the bulb used for soft shadows. - float bulb_radius; - // The size of the shadow map. - int shadow_map_size; - // Blur width for EL VSM. - float vsm_blur_width; -}; - -void mjr_defaultLightParams(mjrLightParams* params); - // Manages the filament Entities for a single mjvLight. class Light : public mjrLight { public: diff --git a/src/experimental/filament/filament/material.cc b/src/experimental/filament/filament/material.cc index fcdc54fa..925e88fb 100644 --- a/src/experimental/filament/filament/material.cc +++ b/src/experimental/filament/filament/material.cc @@ -14,8 +14,6 @@ #include "experimental/filament/filament/material.h" -#include - #include #include #include @@ -25,45 +23,10 @@ #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { -template -static void setf(float (&arr)[N], const std::array& values) { - for (int i = 0; i < N; ++i) { - arr[i] = values[i]; - } -} - -void mjr_defaultMaterialTextures(mjrMaterialTextures* textures) { - textures->color = nullptr; - textures->normal = nullptr; - textures->metallic = nullptr; - textures->roughness = nullptr; - textures->occlusion = nullptr; - textures->orm = nullptr; - textures->emissive = nullptr; - textures->reflection = nullptr; -} - -void mjr_defaultMaterialParams(mjrMaterialParams* params) { - setf(params->color, {1.f, 1.f, 1.f, 1.f}); - setf(params->segmentation_color, {1, 1, 1, 1}); - setf(params->uv_scale, {1, 1, 1}); - setf(params->uv_offset, {0, 0, 0}); - setf(params->scissor, {0, 0, 0, 0}); - - params->emissive = -1.0f; - params->specular = -1.0f; - params->glossiness = -1.0f; - params->metallic = -1.0f; - params->roughness = -1.0f; - params->reflectance = 0.0f; - params->tex_uniform = false; - params->reflective = false; -} - - void UpdateMaterialInstance(filament::MaterialInstance* instance, const mjrMaterialParams& params, const mjrMaterialTextures& textures, @@ -118,11 +81,12 @@ void UpdateMaterialInstance(filament::MaterialInstance* instance, sampler.setMinFilter( filament::TextureSampler::MinFilter::LINEAR_MIPMAP_LINEAR); - auto TrySetTexture = [&](const char* name, const Texture* texture, + auto TrySetTexture = [&](const char* name, const mjrTexture* texture, mjtTextureRole role) { if (material->hasParameter(name)) { if (texture != nullptr) { - instance->setParameter(name, texture->GetFilamentTexture(), sampler); + instance->setParameter( + name, Texture::downcast(texture)->GetFilamentTexture(), sampler); } else { instance->setParameter(name, object_mgr->GetFallbackTexture(role), sampler); diff --git a/src/experimental/filament/filament/material.h b/src/experimental/filament/filament/material.h index 630f4844..abc0f9a1 100644 --- a/src/experimental/filament/filament/material.h +++ b/src/experimental/filament/filament/material.h @@ -17,46 +17,11 @@ #include #include -#include -#include "experimental/filament/filament/texture.h" #include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { -// The textures that can be assigned to the drawable's material. -struct mjrMaterialTextures { - const Texture* color; - const Texture* normal; - const Texture* metallic; - const Texture* roughness; - const Texture* occlusion; - const Texture* orm; - const Texture* emissive; - const Texture* reflection; -}; - -void mjr_defaultMaterialTextures(mjrMaterialTextures* textures); - -// The parameters that can be applied to the drawable's material. -struct mjrMaterialParams { - float color[4]; - float segmentation_color[4]; - float tex_repeat[2]; - float uv_scale[3]; - float uv_offset[3]; - float scissor[4]; - float specular; - float glossiness; - float metallic; - float roughness; - float emissive; - float reflectance; - mjtByte tex_uniform; - mjtByte reflective; -}; - -void mjr_defaultMaterialParams(mjrMaterialParams* params); - // Updates the material instances based on the currently set parameters and // textures. void UpdateMaterialInstance(filament::MaterialInstance* instance, diff --git a/src/experimental/filament/filament/mesh.cc b/src/experimental/filament/filament/mesh.cc index 2d7a74b7..9c06cfc7 100644 --- a/src/experimental/filament/filament/mesh.cc +++ b/src/experimental/filament/filament/mesh.cc @@ -33,6 +33,7 @@ #include #include #include "experimental/filament/filament/math_util.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -101,11 +102,6 @@ int FillSequence(std::byte* buffer, std::size_t num_bytes) { return num; } -// Initializes the mjrMeshData to default values. -void mjr_defaultMeshData(mjrMeshData* data) { - std::memset(data, 0, sizeof(mjrMeshData)); -} - Mesh::Mesh(filament::Engine* engine, const mjrMeshData& data) : engine_(engine), shared_state_(std::make_shared()) { type_ = data.primitive_type == mjMESH_PRIMITIVE_TYPE_TRIANGLES diff --git a/src/experimental/filament/filament/mesh.h b/src/experimental/filament/filament/mesh.h index 9746e4ab..206aa27d 100644 --- a/src/experimental/filament/filament/mesh.h +++ b/src/experimental/filament/filament/mesh.h @@ -16,7 +16,6 @@ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MESH_H_ #include -#include #include #include #include @@ -30,114 +29,11 @@ #include #include #include -#include #include "experimental/filament/render_context_filament.h" // Functions for creating filament vertex and index buffers. namespace mujoco { -// The type of data stored in an index buffer. -typedef enum mjrIndexType_ { - mjINDEX_TYPE_U16 = 0, - mjINDEX_TYPE_U32 = 1, -} mjrIndexType; - -// The type of primitive to be drawn by vertex data. -typedef enum mjrMeshPrimitiveType_ { - mjMESH_PRIMITIVE_TYPE_TRIANGLES = 0, - mjMESH_PRIMITIVE_TYPE_LINES = 1, -} mjrMeshPrimitiveType; - -// The usage/purpose of an attribute of a vertex. -typedef enum mjrVertexAttributeUsage_ { - mjVERTEX_ATTRIBUTE_USAGE_POSITION = 0, - mjVERTEX_ATTRIBUTE_USAGE_NORMAL = 1, - mjVERTEX_ATTRIBUTE_USAGE_TANGENTS = 2, - mjVERTEX_ATTRIBUTE_USAGE_UV = 3, - mjVERTEX_ATTRIBUTE_USAGE_COLOR = 4, -} mjrVertexAttributeUsage; - -// The data format of an attribute of a vertex. -typedef enum mjrVertexAttributeType_ { - mjVERTEX_ATTRIBUTE_TYPE_FLOAT2 = 0, - mjVERTEX_ATTRIBUTE_TYPE_FLOAT3 = 1, - mjVERTEX_ATTRIBUTE_TYPE_FLOAT4 = 2, - mjVERTEX_ATTRIBUTE_TYPE_UBYTE4 = 3, -} mjrVertexAttributeType; - -// Maximum number of vertex attributes that can be used by a mesh. -enum { mjMAX_VERTEX_ATTRIBUTES = 16 }; - -// Information about a single attribute of a vertex. -struct mjrVertexAttribute { - // The data for the attribute. - const void* bytes; - - // The usage/purpose of the attribute. - mjrVertexAttributeUsage usage; - - // The data format of the attribute. - mjrVertexAttributeType type; -}; - -// The binary contents of a mesh. -struct mjrMeshData { - // The number of vertices in the mesh. Each of the vertex arrays below is - // assumed to have this number of elements. - mjtSize nvertices; - - // The number of attributes for each vertex in the mesh. - int nattributes; - - // Information about each attribute of a vertex in the mesh. See `interleaved` - // for more details. - mjrVertexAttribute attributes[mjMAX_VERTEX_ATTRIBUTES]; - - // Whether the vertex attributes are interleaved or not. - // - // If true, assumes that the attributes are packed in the order specified in - // the attributes array, with no padding in-between. Additionally, the - // `data` pointer for each attribute is assumed to point to the first element - // of that type. - // - // If false, assume each attribute is stored in a separate array as defined - // by the `data` field of the attribute. - mjtByte interleaved; - - // The number of indices in the mesh. The indices array is assumed to have - // this number of elements. - mjtSize nindices; - - // The indices of the mesh, stored as either ushort or uint depending on the - // index type. - const void* indices; - - // The type of data stored in the indices array. - mjrIndexType index_type; - - // The type of primitive to be drawn by vertex data. - mjrMeshPrimitiveType primitive_type; - - // Whether to compute the bounds of the mesh using the vertex positions. - mjtByte compute_bounds; - - // The bounds of the mesh. If bounds_min == bounds_max, then we assume that - // that the bounds are not set (i.e. the bounds is empty). - float bounds_min[3]; - float bounds_max[3]; - - // Because rendering may be multithreaded, we cannot make assumptions about - // when the mesh data will finish uploading to the GPU. As such, we will use - // this callback to notify callers when it is safe to free the mesh data. - void (*release_callback)(void* user_data); - - // User data to pass to the release callback. - void* user_data; -}; - -// Initializes the MeshData to default values. -void mjr_defaultMeshData(mjrMeshData* data); - // Owns a Vertex and Index buffer representing a geometry mesh. class Mesh : public mjrMesh { public: diff --git a/src/experimental/filament/filament/render_target.cc b/src/experimental/filament/filament/render_target.cc index 3e53ac51..4c083136 100644 --- a/src/experimental/filament/filament/render_target.cc +++ b/src/experimental/filament/filament/render_target.cc @@ -30,11 +30,6 @@ namespace mujoco { -void mjr_defaultRenderTargetConfig(mjrRenderTargetConfig* config) { - config->color_format = mjPIXEL_FORMAT_RGBA8; - config->depth_format = mjPIXEL_FORMAT_DEPTH32F; -} - RenderTarget::RenderTarget(filament::Engine* engine, const mjrRenderTargetConfig& config) : engine_(engine), config_(config) {} diff --git a/src/experimental/filament/filament/render_target.h b/src/experimental/filament/filament/render_target.h index 9e5aed68..92e221b3 100644 --- a/src/experimental/filament/filament/render_target.h +++ b/src/experimental/filament/filament/render_target.h @@ -26,15 +26,6 @@ namespace mujoco { -// Defines the basic properties of a render target. -struct mjrRenderTargetConfig { - mjrPixelFormat color_format; - mjrPixelFormat depth_format; -}; - -// Initializes the RenderTargetConfig to default values. -void mjr_defaultRenderTargetConfig(mjrRenderTargetConfig* config); - // Manages a filament RenderTarget and the textures which are bound to it. class RenderTarget : public mjrRenderTarget { public: diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 4381cbd4..47c48849 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -26,20 +26,17 @@ #include #include #include -#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/filament/texture.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { using filament::math::mat4f; -void mjr_defaultRenderableParams(mjrRenderableParams* params) { - params->shading_model = mjSHADING_MODEL_SCENE_OBJECT; -} - Renderable::Renderable(ObjectManager* object_mgr, const mjrRenderableParams& params) : object_mgr_(object_mgr), params_(params) { mjr_defaultMaterialParams(&material_params_); @@ -57,7 +54,7 @@ Renderable::~Renderable() noexcept { engine->destroy(part.entity); em.destroy(part.entity); } - for (int i = 0; i < kNumDrawModes; ++i) { + for (int i = 0; i < mjNUM_DRAW_MODES; ++i) { if (instances_[i] != nullptr) { engine->destroy(instances_[i]); instances_[i] = nullptr; @@ -206,13 +203,13 @@ void Renderable::UpdateMaterial(const mjrMaterialParams& params, material_params_ = params; material_textures_ = textures; - AssignMaterial(DrawMode::Color, GetColorMaterialType()); + AssignMaterial(mjDRAW_MODE_COLOR, GetColorMaterialType()); if (params_.shading_model == mjSHADING_MODEL_SCENE_OBJECT) { - AssignMaterial(DrawMode::Depth, ObjectManager::kUnlitDepth); - AssignMaterial(DrawMode::Segmentation, ObjectManager::kUnlitSegmentation); + AssignMaterial(mjDRAW_MODE_DEPTH, ObjectManager::kUnlitDepth); + AssignMaterial(mjDRAW_MODE_SEGMENTATION, ObjectManager::kUnlitSegmentation); } - for (int i = 0; i < kNumDrawModes; ++i) { + for (int i = 0; i < mjNUM_DRAW_MODES; ++i) { if (instances_[i]) { UpdateMaterialInstance(instances_[i], material_params_, material_textures_, object_mgr_); @@ -221,7 +218,7 @@ void Renderable::UpdateMaterial(const mjrMaterialParams& params, SetDrawMode(draw_mode_); } -void Renderable::AssignMaterial(DrawMode mode, +void Renderable::AssignMaterial(mjrDrawMode mode, ObjectManager::MaterialType material_type) { const int index = static_cast(mode); @@ -248,10 +245,10 @@ const mjrMaterialTextures& Renderable::GetMaterialTextures() const { return material_textures_; } -void Renderable::SetDrawMode(DrawMode mode) { +void Renderable::SetDrawMode(mjrDrawMode mode) { // Only SceneObjects support non-color draw modes. if (params_.shading_model != mjSHADING_MODEL_SCENE_OBJECT) { - mode = DrawMode::Color; + mode = mjDRAW_MODE_COLOR; } filament::MaterialInstance* instance = instances_[static_cast(mode)]; @@ -369,6 +366,7 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { // geometry) and `mesh_texcoordadr` stores the address of the mesh uvs if // it has them. bool has_texcoords = false; + const Texture* color_texture = Texture::downcast(material_textures_.color); if (!parts_.empty()) { const auto attribs = parts_[0].mesh->GetVertexAttributes(); auto it = std::find(attribs.begin(), attribs.end(), @@ -376,7 +374,7 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { has_texcoords = (it != attribs.end()); } - if (material_textures_.color == nullptr) { + if (color_texture == nullptr) { if (material_params_.color[3] < 1.0f) { return ObjectManager::kPhongColorFade; } else if (material_params_.reflective) { @@ -384,7 +382,7 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { } else { return ObjectManager::kPhongColor; } - } else if (material_textures_.color->GetFilamentTexture()->getTarget() == + } else if (color_texture->GetFilamentTexture()->getTarget() == filament::Texture::Sampler::SAMPLER_CUBEMAP) { if (material_params_.color[3] < 1.0f) { return ObjectManager::kPhongCubeFade; diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 35f9028a..9a740a19 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -24,7 +24,6 @@ #include #include #include -#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" @@ -33,21 +32,6 @@ namespace mujoco { -// The shading model (material) for a Renderable. -typedef enum mjrShadingModel_ { - mjSHADING_MODEL_SCENE_OBJECT, - mjSHADING_MODEL_DECOR, - mjSHADING_MODEL_DECOR_LINES, - mjSHADING_MODEL_UX, -} mjrShadingModel; - -// Configuration parameters for a Renderable. -struct mjrRenderableParams { - mjrShadingModel shading_model; -}; - -void mjr_defaultRenderableParams(mjrRenderableParams* params); - // A Renderable is effectively two things: a mesh and a material. // // The mesh describes the surface geometry of the object and the material @@ -125,7 +109,7 @@ class Renderable : public mjrRenderable { // Further defines the material of the renderable. Only applies to renderables // with a SceneObject shading model. - void SetDrawMode(DrawMode mode); + void SetDrawMode(mjrDrawMode mode); // Updates the parameters for the material. void UpdateMaterial(const mjrMaterialParams& params, @@ -157,16 +141,16 @@ class Renderable : public mjrRenderable { void InitPartEntity(Part& part); - void AssignMaterial(DrawMode mode, ObjectManager::MaterialType material_type); + void AssignMaterial(mjrDrawMode mode, ObjectManager::MaterialType material_type); ObjectManager::MaterialType GetColorMaterialType() const; ObjectManager* object_mgr_; mjrRenderableParams params_; - filament::MaterialInstance* instances_[kNumDrawModes] = {nullptr}; + filament::MaterialInstance* instances_[mjNUM_DRAW_MODES] = {nullptr}; mjrMaterialParams material_params_; mjrMaterialTextures material_textures_; - DrawMode draw_mode_ = DrawMode::Color; + mjrDrawMode draw_mode_ = mjDRAW_MODE_COLOR; filament::Scene* assigned_scene_ = nullptr; std::vector parts_; filament::math::mat4f transform_; diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index d11edc13..d361fc53 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -40,13 +40,12 @@ #include #include #include "experimental/filament/filament/color_grading_options.h" -#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/light.h" -#include "experimental/filament/filament/material.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/render_target.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/texture.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -54,10 +53,6 @@ using filament::math::float3; using filament::math::float4; using filament::math::mat4; -static constexpr int kNormalIndex = static_cast(DrawMode::Color); -static constexpr int kDepthIndex = static_cast(DrawMode::Depth); -static constexpr int kSegmentIndex = static_cast(DrawMode::Segmentation); - static filament::ColorGrading::Builder ToBuilder( const ColorGradingOptions& opts) { return filament::ColorGrading::Builder() @@ -146,11 +141,11 @@ SceneView::SceneView(filament::Engine* engine) : engine_(engine) { // Disable post processing for the depth and segmentation views to preserve // the values. - views_[kDepthIndex]->setPostProcessingEnabled(false); - views_[kSegmentIndex]->setPostProcessingEnabled(false); + views_[mjDRAW_MODE_DEPTH]->setPostProcessingEnabled(false); + views_[mjDRAW_MODE_SEGMENTATION]->setPostProcessingEnabled(false); // Rotate the fog to align with mujoco's +Z up space. - auto fog = views_[kNormalIndex]->getFogEntity(); + auto fog = views_[mjDRAW_MODE_COLOR]->getFogEntity(); auto& tm = engine->getTransformManager(); tm.create(fog); tm.setTransform(tm.getInstance(fog), @@ -255,7 +250,7 @@ void SceneView::Render(filament::Renderer* renderer, } // Render reflection passes. - if (request.draw_mode == DrawMode::Color && reflections_enabled_) { + if (request.draw_mode == mjDRAW_MODE_COLOR && reflections_enabled_) { for (size_t i = 0; i < reflectives_.size(); ++i) { Renderable* renderable = reflectives_[i]; @@ -317,7 +312,7 @@ void SceneView::SetColorGradingOptions(const ColorGradingOptions& opts) { auto color_grading = ToBuilder(color_grading_options_) .toneMapper(tone_mapper.get()) .build(*engine_); - views_[kNormalIndex]->setColorGrading(color_grading); + views_[mjDRAW_MODE_COLOR]->setColorGrading(color_grading); if (color_grading_) { engine_->destroy(color_grading_); } @@ -326,11 +321,11 @@ void SceneView::SetColorGradingOptions(const ColorGradingOptions& opts) { } void SceneView::EnableShadows() { - views_[kNormalIndex]->setShadowingEnabled(true); + views_[mjDRAW_MODE_COLOR]->setShadowingEnabled(true); } void SceneView::DisableShadows() { - views_[kNormalIndex]->setShadowingEnabled(false); + views_[mjDRAW_MODE_COLOR]->setShadowingEnabled(false); } void SceneView::EnableReflections() { @@ -351,19 +346,18 @@ void SceneView::DisableReflections() { textures.reflection = nullptr; renderable->UpdateMaterial(renderable->GetMaterialParams(), textures); } - } void SceneView::EnablePostProcessing() { - views_[kNormalIndex]->setPostProcessingEnabled(true); + views_[mjDRAW_MODE_COLOR]->setPostProcessingEnabled(true); } void SceneView::DisablePostProcessing() { - views_[kNormalIndex]->setPostProcessingEnabled(false); + views_[mjDRAW_MODE_COLOR]->setPostProcessingEnabled(false); } filament::View* SceneView::GetDefaultRenderView() { - return views_[kNormalIndex]; + return views_[mjDRAW_MODE_COLOR]; } ColorGradingOptions SceneView::GetColorGradingOptions() const { diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index f35110f8..d23a6ab2 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -27,7 +27,6 @@ #include #include #include "experimental/filament/filament/color_grading_options.h" -#include "experimental/filament/filament/draw_mode.h" #include "experimental/filament/filament/light.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/render_target.h" @@ -59,7 +58,7 @@ class SceneView : public mjrScene { // Parameters for rendering the scene. struct RenderRequest { // The draw mode (e.g. normal, depth, segmentation) to render. - DrawMode draw_mode = DrawMode::Color; + mjrDrawMode draw_mode = mjDRAW_MODE_COLOR; // The target viewport for the rendered image. mjrRect viewport; // The camera from which to render the scene. @@ -111,7 +110,7 @@ class SceneView : public mjrScene { filament::Camera* camera_ = nullptr; filament::ColorGrading* color_grading_ = nullptr; ColorGradingOptions color_grading_options_; - std::array views_; + std::array views_; // Scene objects. std::unordered_set lights_; diff --git a/src/experimental/filament/filament/texture.cc b/src/experimental/filament/filament/texture.cc index 81b37448..f27b89a6 100644 --- a/src/experimental/filament/filament/texture.cc +++ b/src/experimental/filament/filament/texture.cc @@ -110,14 +110,6 @@ static filament::Texture::InternalFormat GetTextureInternalFormat( } } -void mjr_defaultTextureData(mjrTextureData* data) { - std::memset(data, 0, sizeof(mjrTextureData)); -} - -void mjr_defaultTextureConfig(mjrTextureConfig* config) { - std::memset(config, 0, sizeof(mjrTextureConfig)); -} - Texture::Texture(filament::Engine* engine, const mjrTextureConfig& config, InternalFlags flags) : engine_(engine), config_(config) { diff --git a/src/experimental/filament/filament/texture.h b/src/experimental/filament/filament/texture.h index 764ae911..e5b7f305 100644 --- a/src/experimental/filament/filament/texture.h +++ b/src/experimental/filament/filament/texture.h @@ -25,63 +25,6 @@ // Functions for creating filament textures. namespace mujoco { -// Pixel formats for textures. -typedef enum mjrPixelFormat_ { - mjPIXEL_FORMAT_UNKNOWN = 0, - mjPIXEL_FORMAT_R8, - mjPIXEL_FORMAT_RGB8, - mjPIXEL_FORMAT_RGBA8, - mjPIXEL_FORMAT_R32F, - mjPIXEL_FORMAT_DEPTH32F, - mjPIXEL_FORMAT_KTX, -} mjrPixelFormat; - -typedef mjtTexture mjrTextureTarget; -typedef mjtColorSpace mjrColorSpace; - -// The binary contents of a texture. -struct mjrTextureData { - // Pointer to the image data. If null, an empty texture will be created. - const void* bytes; - - // The number of bytes in the image data. - mjtSize nbytes; - - // Because rendering may be multithreaded, we cannot make assumptions about - // when the image data will finish uploading to the GPU. As such, we will use - // this callback to notify callers when it is safe to free the image data. - void (*release_callback)(void* user_data); - - // User data to pass to the release callback. - void* user_data; -}; - -// Initializes the TextureData to default values. -void mjr_defaultTextureData(mjrTextureData* data); - -// Defines the basic properties of a texture. -struct mjrTextureConfig { - // The width of the texture. For compressed textures (e.g. KTX), this is the - // number of bytes in the compressed data. - int width; - - // The height of the texture. For compressed textures (e.g. KTX), this should - // be 0. - int height; - - // The target of the texture (e.g. 2D, cube, etc.) - mjrTextureTarget target; - - // The format of the pixels in the texture (e.g. RGB8, RGBA8, KTX, etc.) - mjrPixelFormat format; - - // The color space of the texture (e.g. LINEAR, sRGB, etc.) - mjrColorSpace color_space; -}; - -// Initializes the TextureConfig to default values. -void mjr_defaultTextureConfig(mjrTextureConfig* config); - // Wrapper around a filament::Texture. class Texture : public mjrTexture { public: diff --git a/src/experimental/filament/render_context_filament.cc b/src/experimental/filament/render_context_filament.cc index 65b4086a..30891f5a 100644 --- a/src/experimental/filament/render_context_filament.cc +++ b/src/experimental/filament/render_context_filament.cc @@ -14,6 +14,7 @@ #include "experimental/filament/render_context_filament.h" +#include #include #include @@ -36,12 +37,91 @@ static void CheckFilamentContext() { } } +template +static void setf(float (&arr)[N], const std::array& values) { + for (int i = 0; i < N; ++i) { + arr[i] = values[i]; + } +} + + extern "C" { void mjrf_defaultFilamentConfig(mjrFilamentConfig* config) { memset(config, 0, sizeof(mjrFilamentConfig)); } +void mjr_defaultTextureData(mjrTextureData* data) { + memset(data, 0, sizeof(mjrTextureData)); +} + +void mjr_defaultTextureConfig(mjrTextureConfig* config) { + memset(config, 0, sizeof(mjrTextureConfig)); +} + +void mjr_defaultMeshData(mjrMeshData* data) { + std::memset(data, 0, sizeof(mjrMeshData)); +} + +void mjr_defaultLightParams(mjrLightParams* params) { + params->type = mjLIGHT_POINT; + params->texture = nullptr; + params->color[0] = 0; + params->color[1] = 0; + params->color[2] = 0; + params->intensity = 0.0f; + params->cast_shadows = true; + params->range = 10.0f; + params->spot_cone_angle = 180.f; + params->bulb_radius = 0.0f; + params->shadow_map_size = 2048; + params->vsm_blur_width = 0.0f; +} + +void mjr_defaultMaterialTextures(mjrMaterialTextures* textures) { + textures->color = nullptr; + textures->normal = nullptr; + textures->metallic = nullptr; + textures->roughness = nullptr; + textures->occlusion = nullptr; + textures->orm = nullptr; + textures->emissive = nullptr; + textures->reflection = nullptr; +} + +void mjr_defaultMaterialParams(mjrMaterialParams* params) { + setf(params->color, {1.f, 1.f, 1.f, 1.f}); + setf(params->segmentation_color, {1, 1, 1, 1}); + setf(params->uv_scale, {1, 1, 1}); + setf(params->uv_offset, {0, 0, 0}); + setf(params->scissor, {0, 0, 0, 0}); + params->emissive = -1.0f; + params->specular = -1.0f; + params->glossiness = -1.0f; + params->metallic = -1.0f; + params->roughness = -1.0f; + params->reflectance = 0.0f; + params->tex_uniform = false; + params->reflective = false; +} + +void mjr_defaultRenderableParams(mjrRenderableParams* params) { + params->shading_model = mjSHADING_MODEL_SCENE_OBJECT; +} + +void mjr_defaultRenderTargetConfig(mjrRenderTargetConfig* config) { + config->color_format = mjPIXEL_FORMAT_RGBA8; + config->depth_format = mjPIXEL_FORMAT_DEPTH32F; +} + +void mjr_defaultRenderRequest(mjrRenderRequest* request) { + memset(request, 0, sizeof(mjrRenderRequest)); +} + +void mjr_defaultReadPixelsRequest(mjrReadPixelsRequest* request) { + memset(request, 0, sizeof(mjrReadPixelsRequest)); +} + void mjrf_makeFilamentContext(const mjModel* m, mjrContext* con, const mjrFilamentConfig* config) { // TODO: Support multiple contexts and multiple threads. For now, we'll just diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index ef6f88ea..e338a55c 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -40,12 +40,334 @@ struct mjrRenderTarget {}; // Opaque type for the filament rendering context. struct mjrfContext {}; +// The different modes that can be used to render a scene. +typedef enum mjrDrawMode_ { + // Render the scene with "normal" colors and lighting. + mjDRAW_MODE_COLOR, + // Render the scene as a grayscale depth map. + mjDRAW_MODE_DEPTH, + // Render each object with a unique, uniform (flat) color regardless of + // lighting and texture. + mjDRAW_MODE_SEGMENTATION, +} mjrDrawMode; + +enum { mjNUM_DRAW_MODES = 3 }; + +// The shading model (material) for a Renderable. +typedef enum mjrShadingModel_ { + // For renderables in the main 3D scene. + mjSHADING_MODEL_SCENE_OBJECT = 0, + // For UX renderables. + mjSHADING_MODEL_UX, + // For decorative elements in a Scene (e.g. contact points, force vectors, + // etc.). These objects will not be affected by lighting. + mjSHADING_MODEL_DECOR, + // As above, but uses a line primitives for drawing. + mjSHADING_MODEL_DECOR_LINES, +} mjrShadingModel; + +// The type of data stored in an index buffer. +typedef enum mjrIndexType_ { + mjINDEX_TYPE_U16 = 0, + mjINDEX_TYPE_U32, +} mjrIndexType; + +// The type of primitive to be drawn by vertex data. +typedef enum mjrMeshPrimitiveType_ { + mjMESH_PRIMITIVE_TYPE_TRIANGLES = 0, + mjMESH_PRIMITIVE_TYPE_LINES, +} mjrMeshPrimitiveType; + +// The usage/purpose of an attribute of a vertex. +typedef enum mjrVertexAttributeUsage_ { + mjVERTEX_ATTRIBUTE_USAGE_POSITION = 0, + mjVERTEX_ATTRIBUTE_USAGE_NORMAL, + mjVERTEX_ATTRIBUTE_USAGE_TANGENTS, + mjVERTEX_ATTRIBUTE_USAGE_UV, + mjVERTEX_ATTRIBUTE_USAGE_COLOR, +} mjrVertexAttributeUsage; + +// The data format of an attribute of a vertex. +typedef enum mjrVertexAttributeType_ { + mjVERTEX_ATTRIBUTE_TYPE_FLOAT2 = 0, + mjVERTEX_ATTRIBUTE_TYPE_FLOAT3, + mjVERTEX_ATTRIBUTE_TYPE_FLOAT4, + mjVERTEX_ATTRIBUTE_TYPE_UBYTE4, +} mjrVertexAttributeType; + +// Pixel formats for textures. +typedef enum mjrPixelFormat_ { + mjPIXEL_FORMAT_UNKNOWN = 0, + mjPIXEL_FORMAT_R8, + mjPIXEL_FORMAT_RGB8, + mjPIXEL_FORMAT_RGBA8, + mjPIXEL_FORMAT_R32F, + mjPIXEL_FORMAT_DEPTH32F, + mjPIXEL_FORMAT_KTX, +} mjrPixelFormat; + typedef enum mjrGraphicsApi_ { // backend graphics API to use mjGRAPHICS_API_DEFAULT = 0, // default based on platform mjGRAPHICS_API_OPENGL, // OpenGL (desktop) / WebGL mjGRAPHICS_API_VULKAN // Vulkan } mjrGraphicsApi; + +// Rendering is asynchronous by nature. Each render request is assigned a +// unique Handle which can be used to query the status of the request. The +// Handle can also be used to block until the request is completed. +typedef std::uint64_t mjrFrameHandle; + +// Bring some legacy mjt types into the mjr namespace. +typedef mjtTexture mjrTextureTarget; +typedef mjtColorSpace mjrColorSpace; +typedef mjtLightType mjrLightType; + +// The textures that can be assigned to the drawable's material. +struct mjrMaterialTextures { + const mjrTexture* color; + const mjrTexture* normal; + const mjrTexture* metallic; + const mjrTexture* roughness; + const mjrTexture* occlusion; + const mjrTexture* orm; + const mjrTexture* emissive; + const mjrTexture* reflection; +}; + +// Initializes the mjrMaterialTextures to default values. +void mjr_defaultMaterialTextures(mjrMaterialTextures* textures); + +// The parameters that can be applied to the drawable's material. +struct mjrMaterialParams { + float color[4]; + float segmentation_color[4]; + float tex_repeat[2]; + float uv_scale[3]; + float uv_offset[3]; + float scissor[4]; + float specular; + float glossiness; + float metallic; + float roughness; + float emissive; + float reflectance; + mjtByte tex_uniform; + mjtByte reflective; +}; + +// Initializes the mjrMaterialParams to default values. +void mjr_defaultMaterialParams(mjrMaterialParams* params); + +// The binary contents of a texture. +struct mjrTextureData { + // Pointer to the image data. If null, an empty texture will be created. + const void* bytes; + + // The number of bytes in the image data. + mjtSize nbytes; + + // Because rendering may be multithreaded, we cannot make assumptions about + // when the image data will finish uploading to the GPU. As such, we will use + // this callback to notify callers when it is safe to free the image data. + void (*release_callback)(void* user_data); + + // User data to pass to the release callback. + void* user_data; +}; + +// Initializes the mjrTextureData to default values. +void mjr_defaultTextureData(mjrTextureData* data); + +// Defines the basic properties of a texture. +struct mjrTextureConfig { + // The width of the texture. For compressed textures (e.g. KTX), this is the + // number of bytes in the compressed data. + int width; + + // The height of the texture. For compressed textures (e.g. KTX), this should + // be 0. + int height; + + // The target of the texture (e.g. 2D, cube, etc.) + mjrTextureTarget target; + + // The format of the pixels in the texture (e.g. RGB8, RGBA8, KTX, etc.) + mjrPixelFormat format; + + // The color space of the texture (e.g. LINEAR, sRGB, etc.) + mjrColorSpace color_space; +}; + +// Initializes the mjrTextureConfig to default values. +void mjr_defaultTextureConfig(mjrTextureConfig* config); + +// Configuration parameters for a Renderable. +struct mjrRenderableParams { + // The shading model to use for the Renderable. + mjrShadingModel shading_model; +}; + +// Initializes the mjrRenderableParams to default values. +void mjr_defaultRenderableParams(mjrRenderableParams* params); + +// Information about a single attribute of a vertex. +struct mjrVertexAttribute { + // The data for the attribute. + const void* bytes; + + // The usage/purpose of the attribute. + mjrVertexAttributeUsage usage; + + // The data format of the attribute. + mjrVertexAttributeType type; +}; + +// Maximum number of vertex attributes in a mesh. +enum { mjMAX_VERTEX_ATTRIBUTES = 16 }; + +// The binary contents of a mesh. +struct mjrMeshData { + // The number of vertices in the mesh. Each of the vertex arrays below is + // assumed to have this number of elements. + mjtSize nvertices; + + // The number of attributes for each vertex in the mesh. + int nattributes; + + // Information about each attribute of a vertex in the mesh. See `interleaved` + // for more details. + mjrVertexAttribute attributes[mjMAX_VERTEX_ATTRIBUTES]; + + // Whether the vertex attributes are interleaved or not. + // + // If true, assumes that the attributes are packed in the order specified in + // the attributes array, with no padding in-between. Additionally, the + // `data` pointer for each attribute is assumed to point to the first element + // of that type. + // + // If false, assume each attribute is stored in a separate array as defined + // by the `data` field of the attribute. + mjtByte interleaved; + + // The number of indices in the mesh. The indices array is assumed to have + // this number of elements. + mjtSize nindices; + + // The indices of the mesh, stored as either ushort or uint depending on the + // index type. + const void* indices; + + // The type of data stored in the indices array. + mjrIndexType index_type; + + // The type of primitive to be drawn by vertex data. + mjrMeshPrimitiveType primitive_type; + + // Whether to compute the bounds of the mesh using the vertex positions. + mjtByte compute_bounds; + + // The bounds of the mesh. If bounds_min == bounds_max, then we assume that + // that the bounds are not set (i.e. the bounds is empty). + float bounds_min[3]; + float bounds_max[3]; + + // Because rendering may be multithreaded, we cannot make assumptions about + // when the mesh data will finish uploading to the GPU. As such, we will use + // this callback to notify callers when it is safe to free the mesh data. + void (*release_callback)(void* user_data); + + // User data to pass to the release callback. + void* user_data; +}; + +// Initializes the mjrMeshData to default values. +void mjr_defaultMeshData(mjrMeshData* data); + +// Configuration parameters for a light. +struct mjrLightParams { + // The type of light (e.g. spot, point, directional, etc.) + mjrLightType type; + // The texture to use for image lights. + const mjrTexture* texture; + // The color of the light. + float color[3]; + // The intensity of the light, in candela. + float intensity; + // Whether or not the light casts shadows. + mjtByte cast_shadows; + // The range/distance in which the light is effective, in meters. + float range; + // The angle of the spot light cone, in degrees. + float spot_cone_angle; + // The radius of the bulb used for soft shadows. + float bulb_radius; + // The size of the shadow map. + int shadow_map_size; + // Blur width for EL VSM. + float vsm_blur_width; +}; + +// Initializes the mjrLightParams to default values. +void mjr_defaultLightParams(mjrLightParams* params); + +// Defines the basic properties of a render target. +struct mjrRenderTargetConfig { + mjrPixelFormat color_format; + mjrPixelFormat depth_format; +}; + +// Initializes the RenderTargetConfig to default values. +void mjr_defaultRenderTargetConfig(mjrRenderTargetConfig* config); + +// Information needed to render a single image of a scene. +struct mjrRenderRequest { + // The scene to render. + mjrScene* scene; + + // The method (e.g. Color, Depth, Segmentation, etc.) to use for rendering. + mjrDrawMode draw_mode; + + // The camera from which to render the scene. + mjvGLCamera camera; + + // The dimensions of the output image. + int width; + int height; + + // The render target into which to render the image. If nullptr, the image + // will be rendered to the window (as previously configured in + // mjrFilamentConfig::native_window). + mjrRenderTarget* target; +}; + +// Initializes the mjrRenderRequest to default values. +void mjr_defaultRenderRequest(mjrRenderRequest* request); + +// Information needed to read pixels from a render target. +struct mjrReadPixelsRequest { + mjrRenderTarget* target; + + // The buffer into which the read pixels will be written. + void* output; + + // The number of bytes in the output buffer. This should match the size of + // the render target texture. + mjtSize num_bytes; + + // Callback when the read pixels operation is complete. This will be called + // during WaitForFrame() or in a subsequent call to Render(). This function + // can optionally be used to free the output buffer if needed. + void (*read_completed_callback)(void* user_data); + + // User data to pass to the completion callback. + void* user_data; +}; + +// Initializes the mjrReadPixelsRequest to default values. +void mjr_defaultReadPixelsRequest(mjrReadPixelsRequest* request); + +// Configuration parameters for the filament rendering context. struct mjrFilamentConfig { // The native window handle into which we can render directly. void* native_window; From ace90c00b90f45b48546f368ca73a109aceaacd1 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 28 Apr 2026 06:34:01 -0700 Subject: [PATCH 157/251] Remove deleted file. PiperOrigin-RevId: 906944517 Change-Id: I8471d1014888cc03632df631b47080618694f4a4 --- src/experimental/filament/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index 9e69f162..8b655e9a 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -27,7 +27,6 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/builtins.h filament/color_grading_options.cc filament/color_grading_options.h - filament/draw_mode.h filament/filament_context.cc filament/filament_context.h filament/filament_platform_factory.cc From d92fe0810c3283e0250345c9bae7838668c0730e Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Tue, 28 Apr 2026 14:22:11 -0700 Subject: [PATCH 158/251] Add MjSpec.encode method. PiperOrigin-RevId: 907177387 Change-Id: I65800298798037c4a0c0cd79ec86706be67db51e --- doc/changelog.rst | 5 ++++ doc/python.rst | 6 +++++ python/mujoco/specs.cc | 45 +++++++++++++++++++++++++++++++++ python/mujoco/specs_test.py | 50 +++++++++++++++++++++++++++++++++++++ src/user/user_api.cc | 13 ++++++++++ 5 files changed, 119 insertions(+) diff --git a/doc/changelog.rst b/doc/changelog.rst index 4a8f22d4..bb1c7a20 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,6 +7,11 @@ Upcoming version (not yet released) - Added island support for the :ref:`PGS solver`. +Python +^^^^^^ + +- Added ``MjSpec.encode`` method, wrapping :ref:`mj_encode`. + Version 3.8.0 (April 24, 2026) ------------------------------ diff --git a/doc/python.rst b/doc/python.rst index 61761b0b..95beb3ff 100644 --- a/doc/python.rst +++ b/doc/python.rst @@ -547,6 +547,12 @@ Compiled ``MjSpec`` objects can be saved to XML string with the ``to_xml()`` met +Alternatively, the spec can be saved directly to a file using ``encode()``: + +.. code-block:: python + + spec.encode('model.xml', model) + Attachment ---------- diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index e58d66cc..2ba20431 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -431,6 +431,51 @@ PYBIND11_MODULE(_specs, m) { throw FatalError(std::string(err.data())); } }); + mjSpec.def( + "encode", + [](MjSpec& self, std::string filename, + std::optional model, + std::optional content_type) -> int { + raw::MjModel* m = nullptr; + if (model.has_value() && !model->is_none()) { + auto& wrapper = + py::cast<_impl::MjModelWrapper&>(*model); + m = wrapper.get(); + } + + mjVFS vfs; + mjVFS* vfs_ptr = nullptr; + if (!self.assets.empty()) { + mj_defaultVFS(&vfs); + vfs_ptr = &vfs; + for (const auto& asset : self.assets) { + std::string buffer_name = + py::cast(asset.first); + std::string buffer = + py::cast(asset.second); + mj_addBufferVFS(vfs_ptr, buffer_name.c_str(), + buffer.c_str(), buffer.size()); + } + } + + std::array err; + err[0] = '\0'; + const char* ct = + content_type.has_value() ? content_type->c_str() : nullptr; + int nbytes = mj_encode(self.ptr, m, filename.c_str(), ct, + vfs_ptr, err.data(), err.size()); + + if (vfs_ptr) { + mj_deleteVFS(vfs_ptr); + } + + if (nbytes < 0) { + throw FatalError(std::string(err.data())); + } + return nbytes; + }, + py::arg("filename"), py::arg("model") = py::none(), + py::arg("content_type") = py::none()); mjSpec.def( "add_default", [](MjSpec* spec, std::string& classname, diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index 8bb9360e..fe3f45b1 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -22,6 +22,7 @@ import textwrap import typing import zipfile # pylint: disable=unused-import +from absl import flags from absl.testing import absltest from etils import epath import mujoco @@ -34,6 +35,11 @@ def get_linenumber(): class SpecsTest(absltest.TestCase): + def setUp(self): + super().setUp() + # Mark flags as parsed to avoid pytest errors about unparsed flags. + # This is needed for `create_tempdir()` calls below. + flags.FLAGS.mark_as_parsed() def test_typing(self): spec = mujoco.MjSpec() @@ -1964,5 +1970,49 @@ class SpecsTest(absltest.TestCase): self.assertGreater(cam_sd[0], cam_sd[1]) # dist > depth self.assertAlmostEqual(cam_sd[1], 2.0, places=6) # depth is still 2.0 + def test_encode_xml(self): + # Create a simple spec and compile. + spec = mujoco.MjSpec() + body = spec.worldbody.add_body() + geom = body.add_geom() + geom.size[0] = 1 + model = spec.compile() + + # Encode to XML. + filename = os.path.join(self.create_tempdir().full_path, 'output.xml') + nbytes = spec.encode(filename, model) + self.assertGreater(nbytes, 0) + + # Verify the output is valid XML that can be loaded. + reloaded = mujoco.MjSpec.from_file(filename) + reloaded_model = reloaded.compile() + self.assertEqual(reloaded_model.ngeom, model.ngeom) + + def test_encode_xml_without_model(self): + # Create a simple spec and compile so XML can be written. + spec = mujoco.MjSpec() + body = spec.worldbody.add_body() + geom = body.add_geom() + geom.size[0] = 1 + spec.compile() + + # Encode to XML without passing a model explicitly. + filename = os.path.join(self.create_tempdir().full_path, 'output.xml') + nbytes = spec.encode(filename) + self.assertGreater(nbytes, 0) + + def test_encode_no_encoder_raises(self): + # Create a simple spec and compile. + spec = mujoco.MjSpec() + body = spec.worldbody.add_body() + geom = body.add_geom() + geom.size[0] = 1 + model = spec.compile() + + # Encode with an unknown extension should fail. + filename = os.path.join(self.create_tempdir().full_path, 'output.unknown') + with self.assertRaises(mujoco.FatalError): + spec.encode(filename, model) + if __name__ == '__main__': absltest.main() diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 4b5effaf..f03b5e85 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -143,6 +144,18 @@ mjSpec* mj_parse(const char* filename, const char* content_type, int mj_encode(const mjSpec* s, const mjModel* m, const char* filename, const char* content_type, const mjVFS* vfs, char* error, int error_sz) { + // TODO(shaves) Move MJCF and URDF to encoders/decoders. + auto filepath = mujoco::user::FilePath(filename); + if (filepath.Ext() == ".xml" || + (content_type && std::strcmp(content_type, "text/xml") == 0)) { + int result = mj_saveXML(s, filename, error, error_sz); + if (result < 0) { + return -1; + } + + return std::filesystem::file_size(filename); + } + const mjpEncoder* encoder = mjp_findEncoder(filename, content_type); if (!encoder) { if (error) { From 9796345888533747d755a39b0def8face2a0b958 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 29 Apr 2026 03:41:21 -0700 Subject: [PATCH 159/251] Add font scaling to studio, bound to CTRL+MINUS/PLUS PiperOrigin-RevId: 907491215 Change-Id: I144467c3ff0e3d11f4a5bfb6ed2b1e4e88b8693d --- src/experimental/platform/ux/gui.cc | 23 +++++++++++++++++++++-- src/experimental/platform/ux/gui.h | 3 +++ src/experimental/studio/app.cc | 12 ++++++++++++ src/experimental/studio/app.h | 1 + 4 files changed, 37 insertions(+), 2 deletions(-) diff --git a/src/experimental/platform/ux/gui.cc b/src/experimental/platform/ux/gui.cc index ccf1ed91..d7a3946c 100644 --- a/src/experimental/platform/ux/gui.cc +++ b/src/experimental/platform/ux/gui.cc @@ -213,15 +213,32 @@ void SetupTheme(GuiTheme theme) { s.DockingNodeHasCloseButton = false; } +void RescaleDock(float ratio) { + if (ratio == 1) return; + ImGuiID root = ImGui::GetID("Root"); + ImGuiDockNode* root_node = ImGui::DockBuilderGetNode(root); + if (root_node) { + struct ScaleNodes { + static void Apply(ImGuiDockNode* node, float r) { + node->SizeRef.x *= r; + if (node->ChildNodes[0]) Apply(node->ChildNodes[0], r); + if (node->ChildNodes[1]) Apply(node->ChildNodes[1], r); + } + }; + ScaleNodes::Apply(root_node, ratio); + } +} + ImVec4 ConfigureDockingLayout() { ImGuiViewport* viewport = ImGui::GetMainViewport(); const float scale = ImGui::GetWindowDpiScale(); + const float font_scale = ImGui::GetIO().FontGlobalScale; const float kOptionsRelWidth = 0.22f; const float kInspectorRelWidth = 0.22f; const float kStatsRelHeight = 0.3f; - const float kToolsBarHeight = 36.f * scale; - const float kStatusBarHeight = 32.f * scale; + const float kToolsBarHeight = 36.f * scale * font_scale; + const float kStatusBarHeight = 32.f * scale * font_scale; const ImVec2 dockspace_pos{viewport->WorkPos.x, viewport->WorkPos.y + kToolsBarHeight}; @@ -303,6 +320,7 @@ ImVec4 ConfigureDockingLayout() { platform::ScopedStyle style; style.Var(ImGuiStyleVar_WindowBorderSize, 1.0f); style.Var(ImGuiStyleVar_WindowRounding, 0.0f); + style.Var(ImGuiStyleVar_WindowMinSize, ImVec2(1, 1)); const float toolbar_vpad = std::max(0.f, (kToolsBarHeight - ImGui::GetFrameHeight()) * 0.5f); style.Var(ImGuiStyleVar_WindowPadding, ImVec2(4, toolbar_vpad)); @@ -318,6 +336,7 @@ ImVec4 ConfigureDockingLayout() { platform::ScopedStyle style; style.Var(ImGuiStyleVar_WindowBorderSize, 1.0f); style.Var(ImGuiStyleVar_WindowRounding, 0.0f); + style.Var(ImGuiStyleVar_WindowMinSize, ImVec2(1, 1)); ImGui::SetNextWindowPos(ImVec2(0, viewport->Size.y - kStatusBarHeight), ImGuiCond_Always); ImGui::SetNextWindowSize(ImVec2(viewport->Size.x, kStatusBarHeight), diff --git a/src/experimental/platform/ux/gui.h b/src/experimental/platform/ux/gui.h index e9b23dfc..cc2ae08f 100644 --- a/src/experimental/platform/ux/gui.h +++ b/src/experimental/platform/ux/gui.h @@ -42,6 +42,9 @@ enum class GuiTheme { // Updates the ImGui internal style state to match the requested theme. void SetupTheme(GuiTheme theme); +// Rescales all dock node widths by the given ratio. +void RescaleDock(float ratio); + // Configures the ImGui docking module to the standard layout used by Studio. // This includes the following named sections: // "ToolBar": fixed size bar spanning the top of the window; for placing diff --git a/src/experimental/studio/app.cc b/src/experimental/studio/app.cc index 25dd1676..576462a5 100644 --- a/src/experimental/studio/app.cc +++ b/src/experimental/studio/app.cc @@ -578,6 +578,14 @@ void App::HandleKeyboardEvents() { tmp_.inspector_panel = !tmp_.inspector_panel; } else if (ImGui_IsChordJustPressed(ImGuiKey_Tab)) { tmp_.options_panel = !tmp_.options_panel; + } else if (ImGui_IsChordJustPressed(ImGuiKey_Minus | ImGuiMod_Ctrl)) { + float old_scale = ui_.font_scale; + ui_.font_scale = std::clamp(ui_.font_scale - 0.1f, 0.5f, 3.0f); + platform::RescaleDock(ui_.font_scale / old_scale); + } else if (ImGui_IsChordJustPressed(ImGuiKey_Equal | ImGuiMod_Ctrl)) { + float old_scale = ui_.font_scale; + ui_.font_scale = std::clamp(ui_.font_scale + 0.1f, 0.5f, 3.0f); + platform::RescaleDock(ui_.font_scale / old_scale); } else if (ImGui_IsChordJustPressed(ImGuiKey_Minus)) { SetSpeedIndex(tmp_.speed_index + 1); } else if (ImGui_IsChordJustPressed(ImGuiKey_Equal)) { @@ -823,6 +831,8 @@ void App::BuildGui() { platform::SetupTheme(ui_.theme); } + ImGui::GetIO().FontGlobalScale = ui_.font_scale; + const ImVec4 workspace_rect = platform::ConfigureDockingLayout(); // Place charts in bottom right corner of the workspace. @@ -1841,11 +1851,13 @@ float App::GetExpectedLabelWidth() { App::UiState::Dict App::UiState::ToDict() const { return { {"theme", std::to_string(static_cast(theme))}, + {"font_scale", std::to_string(font_scale)}, }; } void App::UiState::FromDict(const Dict& dict) { *this = UiState(); theme = ReadIniValue(dict, "theme", theme); + font_scale = platform::ReadIniValue(dict, "font_scale", font_scale); } } // namespace mujoco::studio diff --git a/src/experimental/studio/app.h b/src/experimental/studio/app.h index 2a1c7326..bef5f6ed 100644 --- a/src/experimental/studio/app.h +++ b/src/experimental/studio/app.h @@ -105,6 +105,7 @@ class App { int camera_idx = platform::kTumbleCameraIdx; int key_idx = 0; platform::GuiTheme theme = platform::GuiTheme::kLight; + float font_scale = 1.0f; using Dict = std::unordered_map; Dict ToDict() const; From e745538ad6e4956388e037c2a6f573ea135d040d Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 29 Apr 2026 07:04:00 -0700 Subject: [PATCH 160/251] Use FilamentContext for creating objects (instead of filament::Engine). PiperOrigin-RevId: 907568857 Change-Id: If7b1b55bac9ef56dde43dc56b45c2c1034149dac --- .../filament/compat/imgui_bridge.cc | 19 ++-- .../filament/compat/imgui_bridge.h | 6 +- .../filament/compat/mjr_filament_renderer.cc | 42 ++++---- .../filament/compat/mjr_filament_renderer.h | 7 +- .../filament/compat/model_objects.cc | 36 +++---- .../filament/compat/model_objects.h | 6 +- .../filament/compat/scene_bridge.cc | 50 ++++------ .../filament/compat/scene_bridge.h | 6 +- .../filament/compat/scene_geom_util.cc | 14 +-- .../filament/compat/scene_geom_util.h | 4 +- .../filament/filament/builtins.cc | 97 +++++++++---------- src/experimental/filament/filament/builtins.h | 22 ++--- .../filament/filament/filament_context.cc | 28 ++---- src/experimental/filament/filament/light.cc | 5 +- src/experimental/filament/filament/light.h | 3 +- src/experimental/filament/filament/mesh.cc | 5 +- src/experimental/filament/filament/mesh.h | 3 +- .../filament/filament/object_manager.cc | 1 - .../filament/filament/render_target.cc | 14 +-- .../filament/filament/render_target.h | 5 +- .../filament/filament/renderable.cc | 5 +- .../filament/filament/renderable.h | 4 +- .../filament/filament/scene_view.cc | 54 ++++++++--- .../filament/filament/scene_view.h | 7 +- src/experimental/filament/filament/texture.cc | 6 +- src/experimental/filament/filament/texture.h | 5 +- 26 files changed, 233 insertions(+), 221 deletions(-) diff --git a/src/experimental/filament/compat/imgui_bridge.cc b/src/experimental/filament/compat/imgui_bridge.cc index c7b83514..6b303f05 100644 --- a/src/experimental/filament/compat/imgui_bridge.cc +++ b/src/experimental/filament/compat/imgui_bridge.cc @@ -24,20 +24,20 @@ #include #include #include -#include "experimental/filament/filament/material.h" +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/mesh.h" -#include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/scene_view.h" #include "experimental/filament/filament/texture.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { using filament::math::float3; using filament::math::mat3f; -ImguiBridge::ImguiBridge(ObjectManager* object_mgr) : object_mgr_(object_mgr) { - scene_view_ = std::make_unique(object_mgr_->GetEngine()); +ImguiBridge::ImguiBridge(FilamentContext* ctx) : ctx_(ctx) { + scene_view_ = std::make_unique(ctx_); scene_view_->DisableShadows(); scene_view_->DisableReflections(); scene_view_->DisablePostProcessing(); @@ -88,7 +88,7 @@ uintptr_t ImguiBridge::UploadImage(uintptr_t tex_id, const uint8_t* pixels, config.target = mjTEXTURE_2D; config.format = bpp == 4 ? mjPIXEL_FORMAT_RGBA8 : mjPIXEL_FORMAT_RGB8; config.color_space = mjCOLORSPACE_LINEAR; - texture = std::make_unique(scene_view_->GetEngine(), config); + texture = std::make_unique(ctx_, config); } // Create a copy of the image to pass it to filament as we don't know the @@ -124,8 +124,7 @@ void ImguiBridge::CreateTexture(ImTextureData* data) { config.color_space = mjCOLORSPACE_LINEAR; const uintptr_t tex_id = next_tex_id_++; - textures_[tex_id] = - std::make_unique(scene_view_->GetEngine(), config); + textures_[tex_id] = std::make_unique(ctx_, config); data->SetTexID((ImTextureID)tex_id); UpdateTexture(data); } @@ -232,7 +231,7 @@ void ImguiBridge::Update() { data.indices = cmds->IdxBuffer.Data; data.index_type = mjINDEX_TYPE_U16; data.primitive_type = mjMESH_PRIMITIVE_TYPE_TRIANGLES; - meshes_.push_back(std::make_unique(scene_view_->GetEngine(), data)); + meshes_.push_back(std::make_unique(ctx_, data)); const Mesh* mesh = meshes_.back().get(); @@ -278,8 +277,8 @@ void ImguiBridge::PrepareRenderables(int count) { mjrRenderableParams params; mjr_defaultRenderableParams(¶ms); params.shading_model = mjSHADING_MODEL_UX; - auto& r = renderables_.emplace_back( - std::make_unique(object_mgr_, params)); + auto& r = + renderables_.emplace_back(std::make_unique(ctx_, params)); r->SetCastShadows(false); r->SetReceiveShadows(false); r->SetBlendOrder(static_cast(renderables_.size())); diff --git a/src/experimental/filament/compat/imgui_bridge.h b/src/experimental/filament/compat/imgui_bridge.h index 953cbd1e..756456fc 100644 --- a/src/experimental/filament/compat/imgui_bridge.h +++ b/src/experimental/filament/compat/imgui_bridge.h @@ -21,18 +21,18 @@ #include #include +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/scene_view.h" #include "experimental/filament/filament/texture.h" -#include "experimental/filament/filament/object_manager.h" namespace mujoco { // Creates and manages a SceneView using data read from ImGui. class ImguiBridge { public: - explicit ImguiBridge(ObjectManager* object_mgr); + explicit ImguiBridge(FilamentContext* ctx); ~ImguiBridge(); // Prepares the Renderables using data from the current ImGui state. This @@ -59,7 +59,7 @@ class ImguiBridge { void UpdateTexture(ImTextureData* data); void DestroyTexture(ImTextureData* data); - ObjectManager* object_mgr_ = nullptr; + FilamentContext* ctx_ = nullptr; std::unique_ptr scene_view_; std::vector> renderables_; std::vector> meshes_; diff --git a/src/experimental/filament/compat/mjr_filament_renderer.cc b/src/experimental/filament/compat/mjr_filament_renderer.cc index 8953e2a2..c7f74ecf 100644 --- a/src/experimental/filament/compat/mjr_filament_renderer.cc +++ b/src/experimental/filament/compat/mjr_filament_renderer.cc @@ -28,18 +28,17 @@ #include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/render_target.h" -#include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { -MjrFilamentRenderer::MjrFilamentRenderer(const mjrFilamentConfig* config) - : FilamentContext(config) { +MjrFilamentRenderer::MjrFilamentRenderer(const mjrFilamentConfig* config) { + filament_context_ = std::make_unique(config); } void MjrFilamentRenderer::Init(const mjModel* model) { - scene_bridge_ = std::make_unique(GetObjectManager(), model); - imgui_bridge_ = std::make_unique(GetObjectManager()); + scene_bridge_ = std::make_unique(filament_context_.get(), model); + imgui_bridge_ = std::make_unique(filament_context_.get()); mjr_defaultRenderRequest(&render_requests_[0]); mjr_defaultRenderRequest(&render_requests_[1]); @@ -66,12 +65,12 @@ void MjrFilamentRenderer::Init(const mjModel* model) { render_requests_[1].camera.frustum_near = 0.0f; render_requests_[1].camera.frustum_far = 1.0f; - - SetClearColor(ReadElement(model, "filament.clearColor", - filament::math::float4(0, 0, 0, 1))); + filament_context_->SetClearColor(ReadElement( + model, "filament.clearColor", filament::math::float4(0, 0, 0, 1))); } -void MjrFilamentRenderer::Render(const mjrRect& viewport, const mjvScene* scene) { +void MjrFilamentRenderer::Render(const mjrRect& viewport, + const mjvScene* scene) { scene_bridge_->Update(viewport, scene); // Update the UX renderable entity after processing the scene in case there // are any elements in the scene which generate UX draw calls (e.g. labels). @@ -92,7 +91,8 @@ void MjrFilamentRenderer::Render(const mjrRect& viewport, const mjvScene* scene) render_requests_[1].width = viewport.width; render_requests_[1].height = viewport.height; - render_requests_[0].camera = mjv_averageCamera(scene->camera, scene->camera + 1); + render_requests_[0].camera = + mjv_averageCamera(scene->camera, scene->camera + 1); render_requests_[1].camera.frustum_center = viewport.width / 2.0f; render_requests_[1].camera.frustum_width = viewport.width / 2.0f; render_requests_[1].camera.frustum_bottom = viewport.height; @@ -100,7 +100,7 @@ void MjrFilamentRenderer::Render(const mjrRect& viewport, const mjvScene* scene) if (mode_ == FrameBufferMode::Window) { render_requests_[0].target = nullptr; render_requests_[1].target = nullptr; - FilamentContext::Render(render_requests_); + filament_context_->Render(render_requests_); } } @@ -136,7 +136,8 @@ void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, mjr_defaultRenderTargetConfig(&config); config.color_format = mjPIXEL_FORMAT_RGB8; config.depth_format = mjPIXEL_FORMAT_DEPTH32F; - auto target = std::make_unique(GetEngine(), config); + auto target = + std::make_unique(filament_context_.get(), config); target->Prepare(viewport.width, viewport.height); render_requests_[0].target = target.get(); render_requests_[1].target = target.get(); @@ -148,9 +149,9 @@ void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, mjr_defaultReadPixelsRequest(&read_request); read_request.output = rgb; read_request.num_bytes = viewport.width * viewport.height * 3; - const mjrFrameHandle frame = FilamentContext::Render( + const mjrFrameHandle frame = filament_context_->Render( {&render_requests_[0], num_requests}, {&read_request, 1}); - FilamentContext::WaitForFrame(frame); + filament_context_->WaitForFrame(frame); render_requests_[0].target = nullptr; render_requests_[1].target = nullptr; @@ -161,7 +162,8 @@ void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, mjr_defaultRenderTargetConfig(&config); config.color_format = mjPIXEL_FORMAT_R32F; config.depth_format = mjPIXEL_FORMAT_DEPTH32F; - auto target = std::make_unique(GetEngine(), config); + auto target = + std::make_unique(filament_context_.get(), config); target->Prepare(viewport.width, viewport.height); render_requests_[0].target = target.get(); render_requests_[1].target = target.get(); @@ -173,9 +175,9 @@ void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, mjr_defaultReadPixelsRequest(&read_request); read_request.output = reinterpret_cast(depth); read_request.num_bytes = viewport.width * viewport.height * sizeof(float); - const mjrFrameHandle frame = - FilamentContext::Render({&render_requests_[0], 1}, {&read_request, 1}); - FilamentContext::WaitForFrame(frame); + const mjrFrameHandle frame = filament_context_->Render( + {&render_requests_[0], 1}, {&read_request, 1}); + filament_context_->WaitForFrame(frame); render_requests_[0].target = nullptr; render_requests_[1].target = nullptr; @@ -205,8 +207,8 @@ void MjrFilamentRenderer::UploadHeightField(const mjModel* model, int id) { } uintptr_t MjrFilamentRenderer::UploadGuiImage(uintptr_t tex_id, - const uint8_t* pixels, int width, - int height, int bpp) { + const uint8_t* pixels, int width, + int height, int bpp) { return imgui_bridge_->UploadImage(tex_id, pixels, width, height, bpp); } diff --git a/src/experimental/filament/compat/mjr_filament_renderer.h b/src/experimental/filament/compat/mjr_filament_renderer.h index 3d87e624..ab77f2c8 100644 --- a/src/experimental/filament/compat/mjr_filament_renderer.h +++ b/src/experimental/filament/compat/mjr_filament_renderer.h @@ -29,7 +29,7 @@ namespace mujoco { // Subclass of the FilamentContext that implements the legacy mjr API. -class MjrFilamentRenderer : public FilamentContext { +class MjrFilamentRenderer { public: explicit MjrFilamentRenderer(const mjrFilamentConfig* config); ~MjrFilamentRenderer() = default; @@ -65,6 +65,10 @@ class MjrFilamentRenderer : public FilamentContext { // Renders an ImGui window containing Filament-specific editor UI. void UpdateGui(); + double GetFrameRate() const { + return filament_context_->GetFrameRate(); + } + MjrFilamentRenderer(const MjrFilamentRenderer&) = delete; MjrFilamentRenderer& operator=(const MjrFilamentRenderer&) = delete; @@ -75,6 +79,7 @@ class MjrFilamentRenderer : public FilamentContext { OffScreenWithGui, }; + std::unique_ptr filament_context_; FrameBufferMode mode_ = FrameBufferMode::Window; mjrRenderRequest render_requests_[2]; std::unique_ptr scene_bridge_; diff --git a/src/experimental/filament/compat/model_objects.cc b/src/experimental/filament/compat/model_objects.cc index 67e07a08..d87dd4c0 100644 --- a/src/experimental/filament/compat/model_objects.cc +++ b/src/experimental/filament/compat/model_objects.cc @@ -32,6 +32,7 @@ #include #include #include "experimental/filament/filament/builtins.h" +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/model_util.h" @@ -493,21 +494,21 @@ void UpdateSkinFlexmjrMeshData(mjrMeshData* data, const mjModel* model, data->user_data = nullptr; } -ModelObjects::ModelObjects(const mjModel* model, filament::Engine* engine) - : model_(model), engine_(engine) { +ModelObjects::ModelObjects(const mjModel* model, FilamentContext* ctx) + : model_(model), ctx_(ctx) { const int nstack = model->vis.quality.numstacks; const int nslice = model->vis.quality.numslices; const int nquad = model->vis.quality.numquads; - shapes_[kLine] = CreateLine(engine_); - shapes_[kBox] = CreateBox(engine_, nquad); - shapes_[kLineBox] = CreateLineBox(engine_); - shapes_[kCone] = CreateCone(engine_, nstack, nslice); - shapes_[kDisk] = CreateDisk(engine_, nslice); - shapes_[kDome] = CreateDome(engine_, nstack / 2, nslice); - shapes_[kTube] = CreateTube(engine_, nstack, nslice); - shapes_[kPlane] = CreatePlane(engine_, nquad); - shapes_[kSphere] = CreateSphere(engine_, nstack, nslice); - shapes_[kTriangle] = CreateTriangle(engine_); + shapes_[kLine] = CreateLine(ctx_); + shapes_[kBox] = CreateBox(ctx_, nquad); + shapes_[kLineBox] = CreateLineBox(ctx_); + shapes_[kCone] = CreateCone(ctx_, nstack, nslice); + shapes_[kDisk] = CreateDisk(ctx_, nslice); + shapes_[kDome] = CreateDome(ctx_, nstack / 2, nslice); + shapes_[kTube] = CreateTube(ctx_, nstack, nslice); + shapes_[kPlane] = CreatePlane(ctx_, nquad); + shapes_[kSphere] = CreateSphere(ctx_, nstack, nslice); + shapes_[kTriangle] = CreateTriangle(ctx_); for (int i = 0; i < model_->ntex; ++i) { UploadTexture(model_, i); @@ -545,13 +546,13 @@ void ModelObjects::UploadMesh(const mjModel* model, int id) { mjrMeshData data; mjr_defaultMeshData(&data); UpdatemjrMeshData(&data, model, id, MeshType::kNormal); - meshes_[id] = std::make_unique(engine_, data); + meshes_[id] = std::make_unique(ctx_, data); if (model->mesh_graphadr[id] >= 0) { mjrMeshData convex_hull_data; mjr_defaultMeshData(&convex_hull_data); UpdatemjrMeshData(&convex_hull_data, model, id, MeshType::kConvexHull); - convex_hulls_[id] = std::make_unique(engine_, convex_hull_data); + convex_hulls_[id] = std::make_unique(ctx_, convex_hull_data); } } @@ -587,7 +588,6 @@ void ModelObjects::UploadTexture(const mjModel* model, int id) { config.format = mjPIXEL_FORMAT_KTX; } - mjrTextureData payload; mjr_defaultTextureData(&payload); payload.bytes = model->tex_data + model->tex_adr[id]; @@ -597,7 +597,7 @@ void ModelObjects::UploadTexture(const mjModel* model, int id) { payload.user_data = nullptr; payload.release_callback = nullptr; - auto texture = std::make_unique(engine_, config); + auto texture = std::make_unique(ctx_, config); texture->Upload(payload); textures_[id] = std::move(texture); } @@ -615,14 +615,14 @@ void ModelObjects::UploadHeightField(const mjModel* model, int id) { mjrMeshData data; mjr_defaultMeshData(&data); UpdatemjrMeshData(&data, model, id, MeshType::kHeightField); - height_fields_[id] = std::make_unique(engine_, data); + height_fields_[id] = std::make_unique(ctx_, data); } void ModelObjects::CreateSkinFlexMesh(const mjvScene* scene, const mjvGeom& geom) { mjrMeshData data; mjr_defaultMeshData(&data); UpdateSkinFlexmjrMeshData(&data, model_, scene, geom); - dynamic_meshes_[geom.objid] = std::make_unique(engine_, data); + dynamic_meshes_[geom.objid] = std::make_unique(ctx_, data); } const Mesh* ModelObjects::GetMeshBuffer(int data_id) const { diff --git a/src/experimental/filament/compat/model_objects.h b/src/experimental/filament/compat/model_objects.h index 4b7b0afd..2db285f7 100644 --- a/src/experimental/filament/compat/model_objects.h +++ b/src/experimental/filament/compat/model_objects.h @@ -19,9 +19,9 @@ #include #include -#include #include #include +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/texture.h" @@ -30,7 +30,7 @@ namespace mujoco { // Creates and owns various filament objects based on the mjModel. class ModelObjects { public: - ModelObjects(const mjModel* model, filament::Engine* engine); + ModelObjects(const mjModel* model, FilamentContext* ctx); ~ModelObjects(); enum ShapeType { @@ -75,7 +75,7 @@ class ModelObjects { private: const mjModel* model_ = nullptr; - filament::Engine* engine_ = nullptr; + FilamentContext* ctx_ = nullptr; std::array, kNumShapes> shapes_; std::unordered_map> meshes_; std::unordered_map> convex_hulls_; diff --git a/src/experimental/filament/compat/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc index aebb85ef..85d2b1e0 100644 --- a/src/experimental/filament/compat/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -19,15 +19,7 @@ #include #include -#include -#include -#include -#include #include -#include -#include -#include -#include #include #include #include @@ -40,6 +32,7 @@ #include "experimental/filament/compat/model_objects.h" #include "experimental/filament/compat/scene_geom_util.h" #include "experimental/filament/filament/color_grading_options.h" +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/light.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/model_util.h" @@ -47,6 +40,7 @@ #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/scene_view.h" #include "experimental/filament/filament/texture.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -56,12 +50,13 @@ using filament::math::mat3; using filament::math::mat4; static std::unique_ptr CreateFallbackIndirectLightTexture( - ObjectManager* object_mgr, std::string_view filename = "") { + FilamentContext* ctx, std::string_view filename = "") { if (filename.empty()) { filename = ObjectManager::kDefaultEnvironmentLight; } - std::unique_ptr asset = object_mgr->LoadAsset(filename); + std::unique_ptr asset = + ctx->GetObjectManager()->LoadAsset(filename); mjrTextureConfig config; mjr_defaultTextureConfig(&config); @@ -71,7 +66,7 @@ static std::unique_ptr CreateFallbackIndirectLightTexture( config.format = mjPIXEL_FORMAT_KTX; config.color_space = mjCOLORSPACE_AUTO; - auto texture = std::make_unique(object_mgr->GetEngine(), config); + auto texture = std::make_unique(ctx, config); mjrTextureData payload; mjr_defaultTextureData(&payload); @@ -86,11 +81,10 @@ static std::unique_ptr CreateFallbackIndirectLightTexture( return texture; } -SceneBridge::SceneBridge(ObjectManager* object_mgr, const mjModel* model) - : object_mgr_(object_mgr) { - scene_view_ = std::make_unique(object_mgr_->GetEngine()); - model_objects_ = - std::make_unique(model, object_mgr_->GetEngine()); +SceneBridge::SceneBridge(FilamentContext* ctx, const mjModel* model) + : ctx_(ctx) { + scene_view_ = std::make_unique(ctx_); + model_objects_ = std::make_unique(model, ctx_); // Configure options for the normal view. auto cg = scene_view_->GetColorGradingOptions(); @@ -203,15 +197,14 @@ void SceneBridge::SetEnvironmentLight(std::string_view filename, fallback_ibl_.reset(); } - fallback_ibl_texture_ = - CreateFallbackIndirectLightTexture(object_mgr_, filename); + fallback_ibl_texture_ = CreateFallbackIndirectLightTexture(ctx_, filename); mjrLightParams params; mjr_defaultLightParams(¶ms); params.type = mjLIGHT_IMAGE; params.texture = fallback_ibl_texture_.get(); params.intensity = intensity; - fallback_ibl_ = std::make_unique(object_mgr_->GetEngine(), params); + fallback_ibl_ = std::make_unique(ctx_, params); scene_view_->AddToScene(fallback_ibl_.get()); } @@ -224,7 +217,6 @@ std::optional SceneBridge::ClipFromWorld(const float3& pos) const{ } void SceneBridge::PrepareLights() { - filament::Engine* engine = object_mgr_->GetEngine(); const mjModel* model = model_objects_->GetModel(); bool has_image_based_light = false; @@ -238,7 +230,7 @@ void SceneBridge::PrepareLights() { params.type = mjLIGHT_IMAGE; params.texture = model_objects_->GetTexture(model->light_texid[i]); params.intensity = model->light_intensity[i]; - auto light_obj = std::make_unique(engine, params); + auto light_obj = std::make_unique(ctx_, params); scene_view_->AddToScene(light_obj.get()); lights_.emplace_back(std::move(light_obj)); has_image_based_light = true; @@ -259,7 +251,7 @@ void SceneBridge::PrepareLights() { params.spot_cone_angle = model->light_cutoff[i]; } - auto light_obj = std::make_unique(engine, params); + auto light_obj = std::make_unique(ctx_, params); scene_view_->AddToScene(light_obj.get()); lights_.emplace_back(std::move(light_obj)); } @@ -279,7 +271,7 @@ void SceneBridge::PrepareLights() { params.cast_shadows = 0; params.intensity = 0.0f; params.spot_cone_angle = 90.0f; - auto light_obj = std::make_unique(engine, params); + auto light_obj = std::make_unique(ctx_, params); scene_view_->AddToScene(light_obj.get()); lights_.emplace_back(std::move(light_obj)); } @@ -287,12 +279,11 @@ void SceneBridge::PrepareLights() { if (!has_image_based_light && total_light_intensity > 0.0f) { // Create a black indirect light to ensure that the skybox is // oriented to respect mujoco's Z-up convention. - filament::Engine* engine = object_mgr_->GetEngine(); mjrLightParams params; mjr_defaultLightParams(¶ms); params.type = mjLIGHT_IMAGE; params.intensity = 10.0f; - fallback_ibl_ = std::make_unique(engine, params); + fallback_ibl_ = std::make_unique(ctx_, params); scene_view_->AddToScene(fallback_ibl_.get()); } @@ -301,14 +292,14 @@ void SceneBridge::PrepareLights() { // default environment light and set the light intensity ourselves. if (total_light_intensity == 0.0f) { // Create a fallback environment light. - fallback_ibl_texture_ = CreateFallbackIndirectLightTexture(object_mgr_); + fallback_ibl_texture_ = CreateFallbackIndirectLightTexture(ctx_); mjrLightParams params; mjr_defaultLightParams(¶ms); params.type = mjLIGHT_IMAGE; params.texture = fallback_ibl_texture_.get(); params.intensity = fallback_environment_light_intensity_; - fallback_ibl_ = std::make_unique(engine, params); + fallback_ibl_ = std::make_unique(ctx_, params); scene_view_->AddToScene(fallback_ibl_.get()); // Distribute the fallback scene light intensity among the lights. @@ -325,8 +316,7 @@ void SceneBridge::PrepareLights() { scene_view_->SetSkybox(model_objects_->GetSkyboxTexture()); } -filament::math::mat4 CalculateClipFromWorld(const mjrRect& viewport, - const mjvGLCamera& cam) { +mat4 CalculateClipFromWorld(const mjrRect& viewport, const mjvGLCamera& cam) { const float3 cam_pos(cam.pos[0], cam.pos[1], cam.pos[2]); const float3 cam_fwd(cam.forward[0], cam.forward[1], cam.forward[2]); const float3 cam_up(cam.up[0], cam.up[1], cam.up[2]); @@ -394,7 +384,7 @@ void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { } std::unique_ptr renderable = CreateGeomRenderable( - *geom, scene, object_mgr_, model_objects_.get(), headpos); + *geom, scene, ctx_, model_objects_.get(), headpos); scene_view_->AddToScene(renderable.get()); renderables_.push_back(std::move(renderable)); diff --git a/src/experimental/filament/compat/scene_bridge.h b/src/experimental/filament/compat/scene_bridge.h index 96e8ab31..d2d1dfdb 100644 --- a/src/experimental/filament/compat/scene_bridge.h +++ b/src/experimental/filament/compat/scene_bridge.h @@ -25,8 +25,8 @@ #include #include #include "experimental/filament/compat/model_objects.h" +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/light.h" -#include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/scene_view.h" #include "experimental/filament/filament/texture.h" @@ -36,7 +36,7 @@ namespace mujoco { // Manages all mjModel data and updates a SceneView using an mjvScene. class SceneBridge { public: - SceneBridge(ObjectManager* object_mgr, const mjModel* model); + SceneBridge(FilamentContext* ctx, const mjModel* model); ~SceneBridge(); // Updates the environment light using the KTX image at the given path. @@ -68,8 +68,8 @@ class SceneBridge { std::optional ClipFromWorld( const filament::math::float3& pos) const; + FilamentContext* ctx_ = nullptr; std::unique_ptr scene_view_; - ObjectManager* object_mgr_ = nullptr; std::unique_ptr model_objects_; std::unique_ptr fallback_ibl_; std::unique_ptr fallback_ibl_texture_; diff --git a/src/experimental/filament/compat/scene_geom_util.cc b/src/experimental/filament/compat/scene_geom_util.cc index ddc85bee..382e6c88 100644 --- a/src/experimental/filament/compat/scene_geom_util.cc +++ b/src/experimental/filament/compat/scene_geom_util.cc @@ -20,9 +20,6 @@ #include #include -#include -#include -#include #include #include #include @@ -30,9 +27,9 @@ #include #include #include "experimental/filament/compat/model_objects.h" +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" -#include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" @@ -343,7 +340,6 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, const mjvScene* scene, ModelObjects* model_objs, - ObjectManager* object_mgr, const float headpos[3]) { const mjModel* model = model_objs->GetModel(); @@ -491,7 +487,7 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, } std::unique_ptr CreateGeomRenderable( - const mjvGeom& geom, const mjvScene* scene, ObjectManager* object_mgr, + const mjvGeom& geom, const mjvScene* scene, FilamentContext* ctx, ModelObjects* model_objs, const float headpos[3]) { mjrShadingModel shading_model = mjSHADING_MODEL_SCENE_OBJECT; if (geom.type == mjGEOM_LINE || geom.type == mjGEOM_LINEBOX) { @@ -503,11 +499,9 @@ std::unique_ptr CreateGeomRenderable( mjrRenderableParams params; mjr_defaultRenderableParams(¶ms); params.shading_model = shading_model; - auto renderable = std::make_unique(object_mgr, params); - + auto renderable = std::make_unique(ctx, params); PrepareGeomMeshes(*renderable, geom, scene, model_objs); - UpdateGeomMaterial(*renderable, geom, scene, model_objs, object_mgr, headpos); - + UpdateGeomMaterial(*renderable, geom, scene, model_objs, headpos); return renderable; } } // namespace mujoco diff --git a/src/experimental/filament/compat/scene_geom_util.h b/src/experimental/filament/compat/scene_geom_util.h index c702f687..2a0ee2f5 100644 --- a/src/experimental/filament/compat/scene_geom_util.h +++ b/src/experimental/filament/compat/scene_geom_util.h @@ -19,14 +19,14 @@ #include #include "experimental/filament/compat/model_objects.h" -#include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/renderable.h" namespace mujoco { // Creates a Renderable from the given mjvGeom. std::unique_ptr CreateGeomRenderable( - const mjvGeom& geom, const mjvScene* scene, ObjectManager* object_mgr, + const mjvGeom& geom, const mjvScene* scene, FilamentContext* ctx, ModelObjects* model_objs, const float headpos[3]); } // namespace mujoco diff --git a/src/experimental/filament/filament/builtins.cc b/src/experimental/filament/filament/builtins.cc index f438fa39..abde82df 100644 --- a/src/experimental/filament/filament/builtins.cc +++ b/src/experimental/filament/filament/builtins.cc @@ -21,14 +21,13 @@ #include #include -#include -#include -#include #include #include #include +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -59,13 +58,13 @@ static std::size_t NumIndicesPerSide(int num_quads_per_axis) { return kNumIndicesPerQuad * num_quads_per_axis * num_quads_per_axis; } -class BuiltinBuilder : mjrMeshData { +class BuiltinBuilder : public mjrMeshData { public: BuiltinBuilder() { mjr_defaultMeshData(this); } virtual ~BuiltinBuilder() = default; template - static std::unique_ptr Create(filament::Engine* engine, + static std::unique_ptr Create(FilamentContext* ctx, Args&&... args) { auto builder = new T(std::forward(args)...); mjrMeshData* mesh_data = builder->PrepareMeshData(); @@ -73,7 +72,7 @@ class BuiltinBuilder : mjrMeshData { delete static_cast(user_data); }; mesh_data->user_data = builder; - return std::make_unique(engine, *mesh_data); + return std::make_unique(ctx, *mesh_data); } mjrMeshData* PrepareMeshData() { @@ -89,33 +88,29 @@ class BuiltinBuilder : mjrMeshData { indices = indices_.data(); nindices = indices_.size(); - primitive_type = - primitive_type_ == filament::backend::PrimitiveType::TRIANGLES - ? mjMESH_PRIMITIVE_TYPE_TRIANGLES - : mjMESH_PRIMITIVE_TYPE_LINES; index_type = mjINDEX_TYPE_U16; - bounds_min[0] = bounds_.getMin().x; - bounds_min[1] = bounds_.getMin().y; - bounds_min[2] = bounds_.getMin().z; - bounds_max[0] = bounds_.getMax().x; - bounds_max[1] = bounds_.getMax().y; - bounds_max[2] = bounds_.getMax().z; return this; } protected: + void SetBounds(const float3& min, const float3& max) { + bounds_min[0] = min.x; + bounds_min[1] = min.y; + bounds_min[2] = min.z; + bounds_max[0] = max.x; + bounds_max[1] = max.y; + bounds_max[2] = max.z; + } + std::vector positions_; std::vector orientations_; std::vector indices_; - filament::Box bounds_; - filament::RenderableManager::PrimitiveType primitive_type_ = - filament::RenderableManager::PrimitiveType::TRIANGLES; }; class LineBuilder : public BuiltinBuilder { public: LineBuilder() { - primitive_type_ = filament::RenderableManager::PrimitiveType::LINES; + primitive_type = mjMESH_PRIMITIVE_TYPE_LINES; positions_.reserve(2); positions_.emplace_back(0, 0, 0); @@ -127,7 +122,7 @@ class LineBuilder : public BuiltinBuilder { indices_.push_back(0); indices_.push_back(1); - bounds_.set({0, 0, 0}, {0, 0, 1}); + SetBounds({0, 0, 0}, {0, 0, 1}); } }; @@ -161,7 +156,7 @@ class PlaneBuilder : public BuiltinBuilder { } } - bounds_.set({-1, -1, -0.001}, {1, 1, 0.001}); + SetBounds({-1, -1, -0.001}, {1, 1, 0.001}); } }; @@ -180,14 +175,14 @@ class TriangleBuilder : public BuiltinBuilder { indices_.emplace_back(1); indices_.emplace_back(2); - bounds_.set({-1, -1, -0.001}, {1, 1, 0.001}); + SetBounds({-1, -1, -0.001}, {1, 1, 0.001}); } }; class LineBoxBuilder : public BuiltinBuilder { public: explicit LineBoxBuilder() { - primitive_type_ = filament::RenderableManager::PrimitiveType::LINES; + primitive_type = mjMESH_PRIMITIVE_TYPE_LINES; positions_.reserve(8); positions_.emplace_back(-1.0f, -1.0f, -1.0f); @@ -229,7 +224,7 @@ class LineBoxBuilder : public BuiltinBuilder { indices_.push_back(1); indices_.push_back(5); - bounds_.set({-1, -1, -1}, {1, 1, 1}); + SetBounds({-1, -1, -1}, {1, 1, 1}); } }; @@ -277,7 +272,7 @@ class BoxBuilder : public BuiltinBuilder { } } - bounds_.set({-1, -1, -1}, {1, 1, 1}); + SetBounds({-1, -1, -1}, {1, 1, 1}); } private: @@ -334,7 +329,7 @@ class TubeBuilder : public BuiltinBuilder { } } - bounds_.set({-1, -1, -1}, {1, 1, 1}); + SetBounds({-1, -1, -1}, {1, 1, 1}); } }; @@ -398,7 +393,7 @@ class ConeBuilder : public BuiltinBuilder { } } - bounds_.set({-1, -1, 0}, {1, 1, 1}); + SetBounds({-1, -1, 0}, {1, 1, 1}); } private: @@ -440,7 +435,7 @@ class DiskBuilder : public BuiltinBuilder { indices_.push_back(1 + next); } - bounds_.set({-1, -1, -0.001}, {1, 1, 0.001}); + SetBounds({-1, -1, -0.001}, {1, 1, 0.001}); } }; @@ -527,7 +522,7 @@ class SphereBuilder : public BuiltinBuilder { indices_.push_back(row_start + adjacent); } - bounds_.set({-1, -1, -1}, {1, 1, 1}); + SetBounds({-1, -1, -1}, {1, 1, 1}); } private: @@ -611,7 +606,7 @@ class DomeBuilder : public BuiltinBuilder { row_start += num_slices; } - bounds_.set({-1, -1, 0}, {1, 1, 1}); + SetBounds({-1, -1, 0}, {1, 1, 1}); } private: @@ -622,44 +617,44 @@ class DomeBuilder : public BuiltinBuilder { } }; -std::unique_ptr CreateLine(filament::Engine* engine) { - return BuiltinBuilder::Create(engine); +std::unique_ptr CreateLine(FilamentContext* ctx) { + return BuiltinBuilder::Create(ctx); } -std::unique_ptr CreatePlane(filament::Engine* engine, int nquad) { - return BuiltinBuilder::Create(engine, nquad); +std::unique_ptr CreatePlane(FilamentContext* ctx, int nquad) { + return BuiltinBuilder::Create(ctx, nquad); } -std::unique_ptr CreateTriangle(filament::Engine* engine) { - return BuiltinBuilder::Create(engine); +std::unique_ptr CreateTriangle(FilamentContext* ctx) { + return BuiltinBuilder::Create(ctx); } -std::unique_ptr CreateBox(filament::Engine* engine, int nquad) { - return BuiltinBuilder::Create(engine, nquad); +std::unique_ptr CreateBox(FilamentContext* ctx, int nquad) { + return BuiltinBuilder::Create(ctx, nquad); } -std::unique_ptr CreateLineBox(filament::Engine* engine) { - return BuiltinBuilder::Create(engine); +std::unique_ptr CreateLineBox(FilamentContext* ctx) { + return BuiltinBuilder::Create(ctx); } -std::unique_ptr CreateSphere(filament::Engine* engine, int nstack, int nslice) { - return BuiltinBuilder::Create(engine, nstack, nslice); +std::unique_ptr CreateSphere(FilamentContext* ctx, int nstack, int nslice) { + return BuiltinBuilder::Create(ctx, nstack, nslice); } -std::unique_ptr CreateTube(filament::Engine* engine, int nstack, int nslice) { - return BuiltinBuilder::Create(engine, nstack, nslice); +std::unique_ptr CreateTube(FilamentContext* ctx, int nstack, int nslice) { + return BuiltinBuilder::Create(ctx, nstack, nslice); } -std::unique_ptr CreateDisk(filament::Engine* engine, int nslice) { - return BuiltinBuilder::Create(engine, nslice); +std::unique_ptr CreateDisk(FilamentContext* ctx, int nslice) { + return BuiltinBuilder::Create(ctx, nslice); } -std::unique_ptr CreateDome(filament::Engine* engine, int nstack, int nslice) { - return BuiltinBuilder::Create(engine, nstack, nslice); +std::unique_ptr CreateDome(FilamentContext* ctx, int nstack, int nslice) { + return BuiltinBuilder::Create(ctx, nstack, nslice); } -std::unique_ptr CreateCone(filament::Engine* engine, int nstack, int nslice) { - return BuiltinBuilder::Create(engine, nstack, nslice); +std::unique_ptr CreateCone(FilamentContext* ctx, int nstack, int nslice) { + return BuiltinBuilder::Create(ctx, nstack, nslice); } } // namespace mujoco diff --git a/src/experimental/filament/filament/builtins.h b/src/experimental/filament/filament/builtins.h index 5fd5c8a5..1727f378 100644 --- a/src/experimental/filament/filament/builtins.h +++ b/src/experimental/filament/filament/builtins.h @@ -17,22 +17,22 @@ #include -#include +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/mesh.h" // Generates buffers for built-in shapes. namespace mujoco { -std::unique_ptr CreateLine(filament::Engine* engine); -std::unique_ptr CreatePlane(filament::Engine* engine, int nquad); -std::unique_ptr CreateTriangle(filament::Engine* engine); -std::unique_ptr CreateBox(filament::Engine* engine, int nquad); -std::unique_ptr CreateLineBox(filament::Engine* engine); -std::unique_ptr CreateSphere(filament::Engine* engine, int nstack, int nslice); -std::unique_ptr CreateTube(filament::Engine* engine, int nstack, int nslice); -std::unique_ptr CreateDisk(filament::Engine* engine, int nslice); -std::unique_ptr CreateDome(filament::Engine* engine, int nstack, int nslice); -std::unique_ptr CreateCone(filament::Engine* engine, int nstack, int nslice); +std::unique_ptr CreateLine(FilamentContext* ctx); +std::unique_ptr CreatePlane(FilamentContext* ctx, int nquad); +std::unique_ptr CreateTriangle(FilamentContext* ctx); +std::unique_ptr CreateBox(FilamentContext* ctx, int nquad); +std::unique_ptr CreateLineBox(FilamentContext* ctx); +std::unique_ptr CreateSphere(FilamentContext* ctx, int nstack, int nslice); +std::unique_ptr CreateTube(FilamentContext* ctx, int nstack, int nslice); +std::unique_ptr CreateDisk(FilamentContext* ctx, int nslice); +std::unique_ptr CreateDome(FilamentContext* ctx, int nstack, int nslice); +std::unique_ptr CreateCone(FilamentContext* ctx, int nstack, int nslice); } // namespace mujoco diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index 4149559c..3166bea8 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -36,12 +36,16 @@ #include #include "experimental/filament/filament/filament_platform_factory.h" #include "experimental/filament/filament/object_manager.h" -#include "experimental/filament/filament/render_target.h" -#include "experimental/filament/filament/scene_view.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { +// Forward declarations of functions defined in scene_view.cc to prevent +// circular dependencies. +void DoRender(filament::Renderer* renderer, const mjrRenderRequest& request); +void DoReadPixels(filament::Renderer* renderer, const mjrRenderRequest& request, + const mjrReadPixelsRequest& read_request); + FilamentContext::FilamentContext(const mjrFilamentConfig* config) : config_(*config) { FilamentPlatformSetup setup = CreateFilamentPlatform(config_); @@ -122,12 +126,7 @@ mjrFrameHandle FilamentContext::Render( break; } if (render_began) { - SceneView::RenderRequest scene_view_request; - scene_view_request.draw_mode = request.draw_mode; - scene_view_request.viewport = {0, 0, request.width, request.height}; - scene_view_request.camera = request.camera; - SceneView* scene_view = SceneView::downcast(request.scene); - scene_view->Render(renderer_, scene_view_request); + DoRender(renderer_, request); } } else { if (read_requests.empty()) { @@ -147,17 +146,8 @@ mjrFrameHandle FilamentContext::Render( break; } if (render_began) { - RenderTarget* render_target = RenderTarget::downcast(request.target); - - SceneView::RenderRequest scene_view_request; - scene_view_request.draw_mode = request.draw_mode; - scene_view_request.viewport = {0, 0, request.width, request.height}; - scene_view_request.camera = request.camera; - scene_view_request.target = render_target; - SceneView* scene_view = SceneView::downcast(request.scene); - scene_view->Render(renderer_, scene_view_request); - render_target->ReadColorPixels(renderer_, (uint8_t*)read_request.output, - read_request.num_bytes); + DoRender(renderer_, request); + DoReadPixels(renderer_, request, read_request); } } } diff --git a/src/experimental/filament/filament/light.cc b/src/experimental/filament/filament/light.cc index 2918f0a0..a4cc0ed1 100644 --- a/src/experimental/filament/filament/light.cc +++ b/src/experimental/filament/filament/light.cc @@ -25,6 +25,7 @@ #include #include #include +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" @@ -34,8 +35,8 @@ namespace mujoco { using filament::math::float3; using filament::math::mat3f; -Light::Light(filament::Engine* engine, const mjrLightParams& params) - : engine_(engine), params_(params) { +Light::Light(FilamentContext* ctx, const mjrLightParams& params) + : engine_(ctx->GetEngine()), params_(params) { // Filament treats image-based lights (IBLs) as separate objects (i.e. // filament::IndirectLight) and so we need to handle IBLs specially. if (params.type == mjLIGHT_IMAGE) { diff --git a/src/experimental/filament/filament/light.h b/src/experimental/filament/filament/light.h index d0b6a884..cc2a16a6 100644 --- a/src/experimental/filament/filament/light.h +++ b/src/experimental/filament/filament/light.h @@ -20,6 +20,7 @@ #include #include #include +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -27,7 +28,7 @@ namespace mujoco { // Manages the filament Entities for a single mjvLight. class Light : public mjrLight { public: - Light(filament::Engine* engine, const mjrLightParams& params); + Light(FilamentContext* ctx, const mjrLightParams& params); ~Light() noexcept; Light(const Light&) = delete; diff --git a/src/experimental/filament/filament/mesh.cc b/src/experimental/filament/filament/mesh.cc index 9c06cfc7..c2024f3b 100644 --- a/src/experimental/filament/filament/mesh.cc +++ b/src/experimental/filament/filament/mesh.cc @@ -32,6 +32,7 @@ #include #include #include +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/render_context_filament.h" @@ -102,8 +103,8 @@ int FillSequence(std::byte* buffer, std::size_t num_bytes) { return num; } -Mesh::Mesh(filament::Engine* engine, const mjrMeshData& data) - : engine_(engine), shared_state_(std::make_shared()) { +Mesh::Mesh(FilamentContext* ctx, const mjrMeshData& data) + : engine_(ctx->GetEngine()), shared_state_(std::make_shared()) { type_ = data.primitive_type == mjMESH_PRIMITIVE_TYPE_TRIANGLES ? filament::RenderableManager::PrimitiveType::TRIANGLES : filament::RenderableManager::PrimitiveType::LINES; diff --git a/src/experimental/filament/filament/mesh.h b/src/experimental/filament/filament/mesh.h index 206aa27d..ba9945da 100644 --- a/src/experimental/filament/filament/mesh.h +++ b/src/experimental/filament/filament/mesh.h @@ -29,6 +29,7 @@ #include #include #include +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/render_context_filament.h" // Functions for creating filament vertex and index buffers. @@ -38,7 +39,7 @@ namespace mujoco { class Mesh : public mjrMesh { public: // Creates a Mesh from the given MeshData. - Mesh(filament::Engine* engine, const mjrMeshData& data); + Mesh(FilamentContext* ctx, const mjrMeshData& data); ~Mesh(); diff --git a/src/experimental/filament/filament/object_manager.cc b/src/experimental/filament/filament/object_manager.cc index 9beb1490..2db47f0d 100644 --- a/src/experimental/filament/filament/object_manager.cc +++ b/src/experimental/filament/filament/object_manager.cc @@ -27,7 +27,6 @@ #include #include #include -#include "experimental/filament/filament/texture.h" #include "user/user_resource.h" namespace mujoco { diff --git a/src/experimental/filament/filament/render_target.cc b/src/experimental/filament/filament/render_target.cc index 4c083136..8b6afb3e 100644 --- a/src/experimental/filament/filament/render_target.cc +++ b/src/experimental/filament/filament/render_target.cc @@ -26,13 +26,15 @@ #include #include #include +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/texture.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { -RenderTarget::RenderTarget(filament::Engine* engine, +RenderTarget::RenderTarget(FilamentContext* ctx, const mjrRenderTargetConfig& config) - : engine_(engine), config_(config) {} + : ctx_(ctx), config_(config) {} RenderTarget::~RenderTarget() noexcept { Destroy(); @@ -56,7 +58,7 @@ void RenderTarget::Prepare(int width, int height) { color_config.color_space = mjCOLORSPACE_LINEAR; color_config.format = mjPIXEL_FORMAT_RGB8; color_flags.color_attachment = true; - color_texture_ = std::make_unique(engine_, color_config, color_flags); + color_texture_ = std::make_unique(ctx_, color_config, color_flags); mjrTextureConfig depth_config; mjr_defaultTextureConfig(&depth_config); @@ -68,14 +70,14 @@ void RenderTarget::Prepare(int width, int height) { depth_config.color_space = mjCOLORSPACE_LINEAR; depth_config.format = mjPIXEL_FORMAT_DEPTH32F; depth_flags.depth_attachment = true; - depth_texture_ = std::make_unique(engine_, depth_config, depth_flags); + depth_texture_ = std::make_unique(ctx_, depth_config, depth_flags); filament::RenderTarget::Builder builder; builder.texture(filament::RenderTarget::AttachmentPoint::COLOR, color_texture_->GetFilamentTexture()); builder.texture(filament::RenderTarget::AttachmentPoint::DEPTH, depth_texture_->GetFilamentTexture()); - render_target_ = builder.build(*engine_); + render_target_ = builder.build(*ctx_->GetEngine()); } void RenderTarget::ReadColorPixels(filament::Renderer* renderer, uint8_t* bytes, @@ -109,7 +111,7 @@ void RenderTarget::ReadColorPixels(filament::Renderer* renderer, uint8_t* bytes, void RenderTarget::Destroy() { if (render_target_) { - engine_->destroy(render_target_); + ctx_->GetEngine()->destroy(render_target_); render_target_ = nullptr; } color_texture_.reset(); diff --git a/src/experimental/filament/filament/render_target.h b/src/experimental/filament/filament/render_target.h index 92e221b3..8e3fa83c 100644 --- a/src/experimental/filament/filament/render_target.h +++ b/src/experimental/filament/filament/render_target.h @@ -21,6 +21,7 @@ #include #include +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" @@ -31,7 +32,7 @@ class RenderTarget : public mjrRenderTarget { public: // Defines the types of textures to create for the color and depth // attachments. - RenderTarget(filament::Engine* engine, const mjrRenderTargetConfig& config); + RenderTarget(FilamentContext* ctx, const mjrRenderTargetConfig& config); ~RenderTarget() noexcept; RenderTarget(const RenderTarget&) = delete; @@ -64,7 +65,7 @@ class RenderTarget : public mjrRenderTarget { private: void Destroy(); - filament::Engine* engine_ = nullptr; + FilamentContext* ctx_ = nullptr; mjrRenderTargetConfig config_; filament::RenderTarget* render_target_ = nullptr; std::unique_ptr color_texture_ = nullptr; diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 47c48849..7d26c12c 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -26,6 +26,7 @@ #include #include #include +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" @@ -37,8 +38,8 @@ namespace mujoco { using filament::math::mat4f; -Renderable::Renderable(ObjectManager* object_mgr, const mjrRenderableParams& params) - : object_mgr_(object_mgr), params_(params) { +Renderable::Renderable(FilamentContext* ctx, const mjrRenderableParams& params) + : object_mgr_(ctx->GetObjectManager()), params_(params) { mjr_defaultMaterialParams(&material_params_); mjr_defaultMaterialTextures(&material_textures_); } diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 9a740a19..2f6f4d48 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -24,7 +24,7 @@ #include #include #include -#include "experimental/filament/filament/material.h" +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/object_manager.h" @@ -52,7 +52,7 @@ class Renderable : public mjrRenderable { static constexpr std::uint8_t kDefaultPriority = 4; static constexpr std::uint8_t kDefaultLayerMask = 0x01; - Renderable(ObjectManager* object_mgr, const mjrRenderableParams& params); + Renderable(FilamentContext* ctx, const mjrRenderableParams& params); ~Renderable() noexcept; Renderable(const Renderable&) = delete; diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index d361fc53..588e98ec 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -40,6 +40,7 @@ #include #include #include "experimental/filament/filament/color_grading_options.h" +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/light.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/render_target.h" @@ -120,7 +121,8 @@ static void SetupReflectionCamera(const mat4& surface_xform, reflection_camera->setCustomProjection(oblique, near, far); } -SceneView::SceneView(filament::Engine* engine) : engine_(engine) { +SceneView::SceneView(FilamentContext* ctx) : ctx_(ctx) { + filament::Engine* engine = ctx_->GetEngine(); scene_ = engine->createScene(); camera_ = engine->createCamera(utils::EntityManager::get().create()); reflect_camera_ = engine->createCamera(utils::EntityManager::get().create()); @@ -153,9 +155,10 @@ SceneView::SceneView(filament::Engine* engine) : engine_(engine) { } SceneView::~SceneView() { + filament::Engine* engine = ctx_->GetEngine(); if (skybox_) { scene_->setSkybox(nullptr); - engine_->destroy(skybox_); + engine->destroy(skybox_); } for (auto& light : lights_) { light->RemoveFromScene(scene_); @@ -166,15 +169,15 @@ SceneView::~SceneView() { lights_.clear(); renderables_.clear(); reflect_targets_.clear(); - engine_->destroyCameraComponent(reflect_camera_->getEntity()); - engine_->destroy(reflect_view_); - engine_->destroyCameraComponent(camera_->getEntity()); + engine->destroyCameraComponent(reflect_camera_->getEntity()); + engine->destroy(reflect_view_); + engine->destroyCameraComponent(camera_->getEntity()); if (color_grading_) { - engine_->destroy(color_grading_); + engine->destroy(color_grading_); } - engine_->destroy(scene_); + engine->destroy(scene_); for (auto& view : views_) { - engine_->destroy(view); + engine->destroy(view); } } @@ -212,13 +215,13 @@ void SceneView::RemoveFromScene(Renderable* renderable) { void SceneView::SetSkybox(const Texture* skybox_texture) { if (skybox_) { scene_->setSkybox(nullptr); - engine_->destroy(skybox_); + GetEngine()->destroy(skybox_); skybox_ = nullptr; } if (skybox_texture) { filament::Skybox::Builder builder; builder.environment(skybox_texture->GetFilamentTexture()); - skybox_ = builder.build(*engine_); + skybox_ = builder.build(*GetEngine()); scene_->setSkybox(skybox_); } } @@ -292,7 +295,7 @@ void SceneView::AddReflectiveRenderable(Renderable* renderable) { config.color_format = mjPIXEL_FORMAT_RGBA8; config.depth_format = mjPIXEL_FORMAT_DEPTH32F; - reflect_targets_.push_back(std::make_unique(engine_, config)); + reflect_targets_.push_back(std::make_unique(ctx_, config)); } // Prepare a render target for the reflective renderable. @@ -311,10 +314,10 @@ void SceneView::SetColorGradingOptions(const ColorGradingOptions& opts) { auto tone_mapper = CreateToneMapper(opts.tone_mapper); auto color_grading = ToBuilder(color_grading_options_) .toneMapper(tone_mapper.get()) - .build(*engine_); + .build(*GetEngine()); views_[mjDRAW_MODE_COLOR]->setColorGrading(color_grading); if (color_grading_) { - engine_->destroy(color_grading_); + GetEngine()->destroy(color_grading_); } color_grading_ = color_grading; color_grading_options_ = opts; @@ -364,4 +367,29 @@ ColorGradingOptions SceneView::GetColorGradingOptions() const { return color_grading_options_; } +void DoRender(filament::Renderer* renderer, const mjrRenderRequest& request) { + SceneView::RenderRequest scene_view_request; + scene_view_request.draw_mode = request.draw_mode; + scene_view_request.viewport = {0, 0, request.width, request.height}; + scene_view_request.camera = request.camera; + SceneView* scene_view = SceneView::downcast(request.scene); + scene_view->Render(renderer, scene_view_request); +} + +void DoReadPixels(filament::Renderer* renderer, + const mjrRenderRequest& request, + const mjrReadPixelsRequest& read_request) { + RenderTarget* render_target = RenderTarget::downcast(request.target); + + SceneView::RenderRequest scene_view_request; + scene_view_request.draw_mode = request.draw_mode; + scene_view_request.viewport = {0, 0, request.width, request.height}; + scene_view_request.camera = request.camera; + scene_view_request.target = render_target; + SceneView* scene_view = SceneView::downcast(request.scene); + scene_view->Render(renderer, scene_view_request); + render_target->ReadColorPixels(renderer, (uint8_t*)read_request.output, + read_request.num_bytes); +} + } // namespace mujoco diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index d23a6ab2..26f41161 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -27,6 +27,7 @@ #include #include #include "experimental/filament/filament/color_grading_options.h" +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/light.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/render_target.h" @@ -42,7 +43,7 @@ namespace mujoco { // (e.g. normal, depth, segmentation, etc.) as well as reflective surfaces. class SceneView : public mjrScene { public: - SceneView(filament::Engine* engine); + explicit SceneView(FilamentContext* ctx); ~SceneView(); SceneView(const SceneView&) = delete; @@ -71,7 +72,7 @@ class SceneView : public mjrScene { void Render(filament::Renderer* renderer, const RenderRequest& request); // Returns the filament Engine managing the scene. - filament::Engine* GetEngine() const { return engine_; } + filament::Engine* GetEngine() const { return ctx_->GetEngine(); } // Enables/disables shadows for the default render view. void EnableShadows(); @@ -105,7 +106,7 @@ class SceneView : public mjrScene { // rendered in their own passes to create the reflective texture. void AddReflectiveRenderable(Renderable* renderable); - filament::Engine* engine_ = nullptr; + FilamentContext* ctx_ = nullptr; filament::Scene* scene_ = nullptr; filament::Camera* camera_ = nullptr; filament::ColorGrading* color_grading_ = nullptr; diff --git a/src/experimental/filament/filament/texture.cc b/src/experimental/filament/filament/texture.cc index f27b89a6..af174a6e 100644 --- a/src/experimental/filament/filament/texture.cc +++ b/src/experimental/filament/filament/texture.cc @@ -24,6 +24,8 @@ #include #include #include +#include "experimental/filament/filament/filament_context.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -110,9 +112,9 @@ static filament::Texture::InternalFormat GetTextureInternalFormat( } } -Texture::Texture(filament::Engine* engine, const mjrTextureConfig& config, +Texture::Texture(FilamentContext* ctx, const mjrTextureConfig& config, InternalFlags flags) - : engine_(engine), config_(config) { + : engine_(ctx->GetEngine()), config_(config) { if (IsCompressed(config_)) { // We defer creation of compressed textures until Upload() is called. In // the meantime, we don't really know anything about the texture (e.g. diff --git a/src/experimental/filament/filament/texture.h b/src/experimental/filament/filament/texture.h index e5b7f305..c0ec1e0b 100644 --- a/src/experimental/filament/filament/texture.h +++ b/src/experimental/filament/filament/texture.h @@ -18,8 +18,7 @@ #include #include #include -#include -#include +#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/render_context_filament.h" // Functions for creating filament textures. @@ -36,7 +35,7 @@ class Texture : public mjrTexture { }; // Creates a texture with the given data. - Texture(filament::Engine* engine, const mjrTextureConfig& config, + Texture(FilamentContext* ctx, const mjrTextureConfig& config, InternalFlags flags = InternalFlags()); ~Texture(); From a2cd7aa52fad288e677ea1a06ebc82d90e1f2f74 Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Wed, 29 Apr 2026 07:18:34 -0700 Subject: [PATCH 161/251] Make mjz_decoder threadsafe. PiperOrigin-RevId: 907574530 Change-Id: Ie292ff10dfa515e4ddc9dad1459e44c3ba9eeabe --- src/experimental/mjz/mjz_decoder.cc | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/experimental/mjz/mjz_decoder.cc b/src/experimental/mjz/mjz_decoder.cc index ee29a80f..12fc0ba5 100644 --- a/src/experimental/mjz/mjz_decoder.cc +++ b/src/experimental/mjz/mjz_decoder.cc @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -141,6 +142,9 @@ class ZipArchiveProvider : public mjpResourceProvider { FileInfo& info = it->second; // Lazily read and store the file contents from the archive. + // The mutex is needed because mz_zip_archive is not thread-safe, and + // multiple threads may call Read concurrently during parallel compilation. + std::lock_guard lock(mutex_); if (info.contents.empty()) { info.contents.resize(info.size); if (!mz_zip_reader_extract_to_mem(&archive_, info.index, @@ -168,6 +172,7 @@ class ZipArchiveProvider : public mjpResourceProvider { mz_zip_archive archive_; std::vector buffer_; std::unordered_map files_; + mutable std::mutex mutex_; }; static mjSpec* ParseZipBuffer(const void* buffer, int nbuffer, const char* name, From 52e90bf55da282f5da201b210a0fb33c96a71fc3 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 29 Apr 2026 07:47:29 -0700 Subject: [PATCH 162/251] Fix high-DPI window size in Studio. PiperOrigin-RevId: 907585586 Change-Id: Ie7db20f5242d89926a4a34ffed3a6e39698defd3 --- src/experimental/platform/hal/window.cc | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/experimental/platform/hal/window.cc b/src/experimental/platform/hal/window.cc index 961e9ab9..054fcb88 100644 --- a/src/experimental/platform/hal/window.cc +++ b/src/experimental/platform/hal/window.cc @@ -16,8 +16,6 @@ #include #include -#include -#include #include #include @@ -121,10 +119,13 @@ Window::Window(std::string_view title, int width, int height, Config config) mju_error("Unsupported window config: %d", config_.gfx_mode); } - const float content_scale = ImGui_ImplSDL2_GetContentScaleForDisplay(0); + const float content_scale = + std::max(1.0f, ImGui_ImplSDL2_GetContentScaleForDisplay(0)); + width_ = width * content_scale; + height_ = height * content_scale; sdl_window_ = SDL_CreateWindow(title.data(), SDL_WINDOWPOS_UNDEFINED, - SDL_WINDOWPOS_UNDEFINED, width, height, window_flags); + SDL_WINDOWPOS_UNDEFINED, width_, height_, window_flags); if (!sdl_window_) { mju_error("Error creating window: %s", SDL_GetError()); } @@ -160,8 +161,8 @@ Window::Window(std::string_view title, int width, int height, Config config) #endif } - int drawable_width = width; - int drawable_height = height; + int drawable_width = width_; + int drawable_height = height_; SDL_GL_GetDrawableSize(sdl_window_, &drawable_width, &drawable_height); scale_ = (float)drawable_width / (float)width_; } From 7e848e9406e69b582198e3680ffc9341a1ee85a7 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 29 Apr 2026 07:57:55 -0700 Subject: [PATCH 163/251] Add mjr_sceneParams for creating Scene objects. PiperOrigin-RevId: 907589621 Change-Id: If7537cf6fbac6d6418ac2bf8315714ada969511d --- src/experimental/filament/compat/imgui_bridge.cc | 10 ++++++---- src/experimental/filament/compat/scene_bridge.cc | 4 +++- src/experimental/filament/filament/scene_view.cc | 13 ++++++++++++- src/experimental/filament/filament/scene_view.h | 2 +- .../filament/render_context_filament.cc | 6 ++++++ src/experimental/filament/render_context_filament.h | 13 +++++++++++++ 6 files changed, 41 insertions(+), 7 deletions(-) diff --git a/src/experimental/filament/compat/imgui_bridge.cc b/src/experimental/filament/compat/imgui_bridge.cc index 6b303f05..26ca06c2 100644 --- a/src/experimental/filament/compat/imgui_bridge.cc +++ b/src/experimental/filament/compat/imgui_bridge.cc @@ -37,10 +37,12 @@ using filament::math::float3; using filament::math::mat3f; ImguiBridge::ImguiBridge(FilamentContext* ctx) : ctx_(ctx) { - scene_view_ = std::make_unique(ctx_); - scene_view_->DisableShadows(); - scene_view_->DisableReflections(); - scene_view_->DisablePostProcessing(); + mjrSceneParams params; + mjr_defaultSceneParams(¶ms); + params.enable_post_processing = false; + params.enable_reflections = false; + params.enable_shadows = false; + scene_view_ = std::make_unique(ctx_, params); } ImguiBridge::~ImguiBridge() { diff --git a/src/experimental/filament/compat/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc index 85d2b1e0..ed35625d 100644 --- a/src/experimental/filament/compat/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -83,7 +83,9 @@ static std::unique_ptr CreateFallbackIndirectLightTexture( SceneBridge::SceneBridge(FilamentContext* ctx, const mjModel* model) : ctx_(ctx) { - scene_view_ = std::make_unique(ctx_); + mjrSceneParams params; + mjr_defaultSceneParams(¶ms); + scene_view_ = std::make_unique(ctx_, params); model_objects_ = std::make_unique(model, ctx_); // Configure options for the normal view. diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 588e98ec..c8fe57fa 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -121,7 +121,8 @@ static void SetupReflectionCamera(const mat4& surface_xform, reflection_camera->setCustomProjection(oblique, near, far); } -SceneView::SceneView(FilamentContext* ctx) : ctx_(ctx) { +SceneView::SceneView(FilamentContext* ctx, const mjrSceneParams& params) + : ctx_(ctx) { filament::Engine* engine = ctx_->GetEngine(); scene_ = engine->createScene(); camera_ = engine->createCamera(utils::EntityManager::get().create()); @@ -152,6 +153,16 @@ SceneView::SceneView(FilamentContext* ctx) : ctx_(ctx) { tm.create(fog); tm.setTransform(tm.getInstance(fog), mat4::rotation(filament::math::f::PI / 2, float3{-1, 0, 0})); + + if (!params.enable_post_processing) { + DisablePostProcessing(); + } + if (!params.enable_reflections) { + DisableReflections(); + } + if (!params.enable_shadows) { + DisableShadows(); + } } SceneView::~SceneView() { diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index 26f41161..e64be22e 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -43,7 +43,7 @@ namespace mujoco { // (e.g. normal, depth, segmentation, etc.) as well as reflective surfaces. class SceneView : public mjrScene { public: - explicit SceneView(FilamentContext* ctx); + SceneView(FilamentContext* ctx, const mjrSceneParams& params); ~SceneView(); SceneView(const SceneView&) = delete; diff --git a/src/experimental/filament/render_context_filament.cc b/src/experimental/filament/render_context_filament.cc index 30891f5a..74c1d3e1 100644 --- a/src/experimental/filament/render_context_filament.cc +++ b/src/experimental/filament/render_context_filament.cc @@ -63,6 +63,12 @@ void mjr_defaultMeshData(mjrMeshData* data) { std::memset(data, 0, sizeof(mjrMeshData)); } +void mjr_defaultSceneParams(mjrSceneParams* params) { + params->enable_post_processing = true; + params->enable_reflections = true; + params->enable_shadows = true; +} + void mjr_defaultLightParams(mjrLightParams* params) { params->type = mjLIGHT_POINT; params->texture = nullptr; diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index e338a55c..577b11a6 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -284,6 +284,19 @@ struct mjrMeshData { // Initializes the mjrMeshData to default values. void mjr_defaultMeshData(mjrMeshData* data); +// Configuration parameters for a Scene. +struct mjrSceneParams { + // Whether or not to enable post processing; enabled by default. + mjtByte enable_post_processing; + // Whether or not to enable reflections; enabled by default. + mjtByte enable_reflections; + // Whether or not to enable shadows; enabled by default. + mjtByte enable_shadows; +}; + +// Initializes the mjrSceneParams to default values. +void mjr_defaultSceneParams(mjrSceneParams* params); + // Configuration parameters for a light. struct mjrLightParams { // The type of light (e.g. spot, point, directional, etc.) From 4e7b2f21249086e5d5d5f7ead7c36a7ccd093688 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 29 Apr 2026 09:12:58 -0700 Subject: [PATCH 164/251] Allow dimensions to be specified when creating a render target. PiperOrigin-RevId: 907622395 Change-Id: I0f806f4d77afa6a178a20d36dae3e96b81359494 --- .../filament/compat/mjr_filament_renderer.cc | 3 ++- src/experimental/filament/filament/render_target.cc | 11 ++++++++++- src/experimental/filament/render_context_filament.cc | 1 + src/experimental/filament/render_context_filament.h | 6 ++++++ 4 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/experimental/filament/compat/mjr_filament_renderer.cc b/src/experimental/filament/compat/mjr_filament_renderer.cc index c7f74ecf..fcd8f31e 100644 --- a/src/experimental/filament/compat/mjr_filament_renderer.cc +++ b/src/experimental/filament/compat/mjr_filament_renderer.cc @@ -134,11 +134,12 @@ void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, if (rgb) { mjrRenderTargetConfig config; mjr_defaultRenderTargetConfig(&config); + config.width = viewport.width; + config.height = viewport.height; config.color_format = mjPIXEL_FORMAT_RGB8; config.depth_format = mjPIXEL_FORMAT_DEPTH32F; auto target = std::make_unique(filament_context_.get(), config); - target->Prepare(viewport.width, viewport.height); render_requests_[0].target = target.get(); render_requests_[1].target = target.get(); diff --git a/src/experimental/filament/filament/render_target.cc b/src/experimental/filament/filament/render_target.cc index 8b6afb3e..d8919a96 100644 --- a/src/experimental/filament/filament/render_target.cc +++ b/src/experimental/filament/filament/render_target.cc @@ -34,7 +34,11 @@ namespace mujoco { RenderTarget::RenderTarget(FilamentContext* ctx, const mjrRenderTargetConfig& config) - : ctx_(ctx), config_(config) {} + : ctx_(ctx), config_(config) { + if (config_.width > 0 && config_.height > 0) { + Prepare(config_.width, config_.height); + } +} RenderTarget::~RenderTarget() noexcept { Destroy(); @@ -47,6 +51,11 @@ void RenderTarget::Prepare(int width, int height) { Destroy(); width_ = width; height_ = height; + if (width_ <= 0 || height_ <= 0) { + width_ = 0; + height_ = 0; + return; + } mjrTextureConfig color_config; mjr_defaultTextureConfig(&color_config); diff --git a/src/experimental/filament/render_context_filament.cc b/src/experimental/filament/render_context_filament.cc index 74c1d3e1..322118a1 100644 --- a/src/experimental/filament/render_context_filament.cc +++ b/src/experimental/filament/render_context_filament.cc @@ -116,6 +116,7 @@ void mjr_defaultRenderableParams(mjrRenderableParams* params) { } void mjr_defaultRenderTargetConfig(mjrRenderTargetConfig* config) { + memset(config, 0, sizeof(mjrRenderTargetConfig)); config->color_format = mjPIXEL_FORMAT_RGBA8; config->depth_format = mjPIXEL_FORMAT_DEPTH32F; } diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index 577b11a6..0f5ee16a 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -326,7 +326,13 @@ void mjr_defaultLightParams(mjrLightParams* params); // Defines the basic properties of a render target. struct mjrRenderTargetConfig { + // The width of the render target. + int width; + // The height of the render target. + int height; + // The format of the color buffer in the render target. mjrPixelFormat color_format; + // The format of the depth buffer in the render target. mjrPixelFormat depth_format; }; From cae70086dad9f2a67149b0381d603474dd26ca0e Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 29 Apr 2026 09:30:57 -0700 Subject: [PATCH 165/251] Scale UI style sizes by DPI. PiperOrigin-RevId: 907630627 Change-Id: Ia6dc35ce00e9764a454f4021250089b0d9a94073 --- src/experimental/platform/ux/gui.cc | 37 ++++++++++++++++------------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/src/experimental/platform/ux/gui.cc b/src/experimental/platform/ux/gui.cc index d7a3946c..b9fa91d2 100644 --- a/src/experimental/platform/ux/gui.cc +++ b/src/experimental/platform/ux/gui.cc @@ -187,27 +187,29 @@ void SetupTheme(GuiTheme theme) { c[ImGuiCol_TabDimmedSelected] = check; } + float scale = s.FontScaleDpi; + int hspacing = 4; int vspacing = 6; float rounding = 4.0f; s.DisplaySafeAreaPadding = ImVec2(0, 0); - s.WindowPadding = ImVec2(hspacing, vspacing); - s.FramePadding = ImVec2(hspacing, 2); - s.ItemSpacing = ImVec2(hspacing, vspacing); - s.ItemInnerSpacing = ImVec2(hspacing, vspacing); - s.WindowRounding = rounding; - s.FrameRounding = rounding; - s.TabRounding = rounding; - s.ScrollbarRounding = rounding; - s.ChildRounding = rounding; - s.GrabRounding = rounding; - s.PopupRounding = rounding; + s.WindowPadding = ImTrunc(ImVec2(hspacing * scale, vspacing * scale)); + s.FramePadding = ImTrunc(ImVec2(hspacing * scale, 2.0f * scale)); + s.ItemSpacing = ImTrunc(ImVec2(hspacing * scale, vspacing * scale)); + s.ItemInnerSpacing = ImTrunc(ImVec2(hspacing * scale, vspacing * scale)); + s.WindowRounding = ImTrunc(rounding * scale); + s.FrameRounding = ImTrunc(rounding * scale); + s.TabRounding = ImTrunc(rounding * scale); + s.ScrollbarRounding = ImTrunc(rounding * scale); + s.ChildRounding = ImTrunc(rounding * scale); + s.GrabRounding = ImTrunc(rounding * scale); + s.PopupRounding = ImTrunc(rounding * scale); s.WindowBorderSize = 0.0f; s.FrameBorderSize = 1.0f; s.PopupBorderSize = 1.0f; - s.IndentSpacing = 6.0f; - s.ScrollbarSize = 12.0f; - s.GrabMinSize = 5.0f; + s.IndentSpacing = ImTrunc(6.0f * scale); + s.ScrollbarSize = ImTrunc(12.0f * scale); + s.GrabMinSize = ImTrunc(5.0f * scale); s.WindowMenuButtonPosition = ImGuiDir_None; s.TabCloseButtonMinWidthSelected = 0.0f; s.DockingNodeHasCloseButton = false; @@ -323,7 +325,7 @@ ImVec4 ConfigureDockingLayout() { style.Var(ImGuiStyleVar_WindowMinSize, ImVec2(1, 1)); const float toolbar_vpad = std::max(0.f, (kToolsBarHeight - ImGui::GetFrameHeight()) * 0.5f); - style.Var(ImGuiStyleVar_WindowPadding, ImVec2(4, toolbar_vpad)); + style.Var(ImGuiStyleVar_WindowPadding, ImVec2(4 * scale, toolbar_vpad)); ImGui::SetNextWindowPos(viewport->WorkPos, ImGuiCond_Always); ImGui::SetNextWindowSize(ImVec2(viewport->Size.x, kToolsBarHeight), ImGuiCond_Always); @@ -357,7 +359,7 @@ ImVec4 ConfigureDockingLayout() { void StepControlGui(const mjModel* model, StepControl* step_control, int& speed_index) { platform::ScopedStyle style; - style.Var(ImGuiStyleVar_FrameRounding, 8.f); + style.Var(ImGuiStyleVar_FrameRounding, 8.f * ImGui::GetStyle().FontScaleDpi); const ImColor yellow(255, 215, 0, 255); const ImColor green(40, 180, 40, 255); @@ -394,7 +396,8 @@ void StepControlGui(const mjModel* model, StepControl* step_control, style.Reset(); ImGui::SameLine(0, ImGui::GetFrameHeight() * .6f); ImGui::PushStyleVar(ImGuiStyleVar_FramePadding, - ImVec2(ImGui::GetStyle().FramePadding.x + 5.f, + ImVec2(ImGui::GetStyle().FramePadding.x + + 5.f * ImGui::GetStyle().FontScaleDpi, ImGui::GetStyle().FramePadding.y)); const auto [misaligned, measured] = IsSpeedMisaligned(*step_control); From 517c11365608ae7683c12674f26f97ed265c0ed3 Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Wed, 29 Apr 2026 09:57:41 -0700 Subject: [PATCH 166/251] Add Rendering differences section to MjWarp documentation. PiperOrigin-RevId: 907644012 Change-Id: I6d79792e4cb4088a89cb3f995f3bddf9b0aada15 --- doc/mjwarp/index.rst | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/doc/mjwarp/index.rst b/doc/mjwarp/index.rst index f5d10431..2d4c0f83 100644 --- a/doc/mjwarp/index.rst +++ b/doc/mjwarp/index.rst @@ -1286,3 +1286,19 @@ CCD colliders and will raise a ``NotImplementedError`` when calling :func:`mjw.p * - box-box - :ref:`NATIVECCD ` enabled (on by default) - Set margin to ``0`` or disable ``NATIVECCD`` + +Rendering +--------- + +The batch renderer included in MJWarp serves a different purpose than MuJoCo's renderer. The MJWarp batch +renderer is a single hit raycaster optimized for high throughput and low fidelity. + +It supports: + * Simple lambertian diffuse shading + * Basic point lights and directional lights + * Textures + * Shadows + +It does not support: + * Advanced lighting effects such as global illumination + * Physically based material properties From 9c6a4f76eb84bedd3f32d765001c40d1764cfd72 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 29 Apr 2026 10:16:38 -0700 Subject: [PATCH 167/251] Add 2D membrane elasticity for interpolated flex shell mode When elastic2d="stretch" is set on an interpolated flexcomp, treat the bounding box boundary as membrane elements rather than volumetric cells. This computes plane-stress stiffness over the boundary faces and updates the runtime force/derivative kernels accordingly. Interior vertex tracking (moving vertices that follow the deforming shell) is not yet implemented so all mesh vertices need to be on the bounding box surface or the background grid should have no interior nodes (i.e. cellcount should be 1 on at least one axis). PiperOrigin-RevId: 907654080 Change-Id: I51b90e2f6a1d1b036f9604e42de20e377dc5d3f9 --- doc/XMLreference.rst | 3 +- doc/changelog.rst | 2 + model/flex/hollow_vs_solid.xml | 102 ++++++ src/engine/engine_core_constraint.c | 262 +++++++++----- src/engine/engine_derivative.c | 222 ++++++------ src/engine/engine_passive.c | 137 ++++---- src/engine/engine_util_misc.c | 127 +++++++ src/engine/engine_util_misc.h | 13 + src/user/user_flexcomp.cc | 52 ++- src/user/user_mesh.cc | 259 +++++++++++--- src/user/user_model.cc | 60 +++- test/engine/engine_core_constraint_test.cc | 40 +++ test/engine/engine_passive_test.cc | 36 ++ test/engine/engine_util_misc_test.cc | 385 +++++++++++++++++++++ test/user/user_mesh_test.cc | 27 +- 15 files changed, 1378 insertions(+), 349 deletions(-) create mode 100644 model/flex/hollow_vs_solid.xml diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 1bc3c846..55c220b7 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -4328,7 +4328,8 @@ stress-strain relationship. See also :ref:`deformable ` objects and :at:`elastic2d`: :at-val:`[none, bend, stretch, both], "none"` Elastic contribution to passive forces of 2D flexes. "none": none, "bend": bending only, "stretch": stretching only, - "both": bending and stretching. Not yet supported by :ref:`dof` **trilinear** and **quadratic**. + "both": bending and stretching. Bending is not yet supported by :ref:`dof` **trilinear** and + **quadratic**. .. _flex-contact: diff --git a/doc/changelog.rst b/doc/changelog.rst index bb1c7a20..1f5e1fd1 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -6,6 +6,8 @@ Upcoming version (not yet released) ----------------------------------- - Added island support for the :ref:`PGS solver`. +- Added support for :ref:`elastic2d` for trilinear and quadratic flex + :ref:`dofs`. Python ^^^^^^ diff --git a/model/flex/hollow_vs_solid.xml b/model/flex/hollow_vs_solid.xml new file mode 100644 index 00000000..709fc114 --- /dev/null +++ b/model/flex/hollow_vs_solid.xml @@ -0,0 +1,102 @@ + + + + + + diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 39996c33..b0285a33 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -706,11 +706,12 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { break; case mjEQ_FLEXSTRAIN: { - // each constraint represents a single cell; cell index in eq_data + // each constraint represents a single element (3D cell or 2D face) int f = id[0]; int nodenum = m->flex_nodenum[f]; - int order = m->flex_interp[f]; - order = order < 0 ? -order : order; + int interp = m->flex_interp[f]; + int order = interp < 0 ? -interp : interp; + int shell_mode = (interp < 0); // skip if not interpolated (order == 0 or no nodes) if (!order || !nodenum) { @@ -722,60 +723,93 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { mjERROR("flex strain constraints only support order 1 and 2, got %d", order); } - int npc = (order+1)*(order+1)*(order+1); + int cx = m->flex_cellnum[3*f+0]; int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; int nstart = m->flex_nodeadr[f]; int* bodyid = m->flex_nodebodyid + nstart; - // read cell index from eq_data - int ci = (int)data[0]; - int cj = (int)data[1]; - int ck = (int)data[2]; + // nodes per element and element index + int npe; + int elem_idx; + if (shell_mode) { + npe = (order+1) * (order+1); + elem_idx = (int)data[0]; // face element index + } else { + npe = (order+1) * (order+1) * (order+1); + int ci = (int)data[0]; + int cj = (int)data[1]; + int ck = (int)data[2]; + elem_idx = ci * cy * cz + cj * cz + ck; + } mj_markStack(d); - // get cell node indices + // get element node indices int gindices[125]; // max npc = 125 for quadratic - mju_flexGatherCellState(order, cy, cz, ci, cj, ck, - NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + if (shell_mode) { + mju_flexGatherFaceState(order, cx, cy, cz, elem_idx, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + } else { + int ci = (int)data[0], cj = (int)data[1], ck = (int)data[2]; + mju_flexGatherCellState(order, cy, cz, ci, cj, ck, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + } - // compute positions only for cell nodes (npc << nodenum) - mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* refpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); - for (int n = 0; n < npc; n++) { + // compute positions only for element nodes (npe << nodenum) + mjtNum* xpos_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* refpos_e = mjSTACKALLOC(d, 3*npe, mjtNum); + for (int n = 0; n < npe; n++) { int gn = gindices[n]; if (m->flex_centered[f] || (m->flex_node[3*(gn + nstart)+0] == 0 && m->flex_node[3*(gn + nstart)+1] == 0 && m->flex_node[3*(gn + nstart)+2] == 0)) { - mju_copy3(xpos_c + 3*n, d->xpos + 3*bodyid[gn]); + mju_copy3(xpos_e + 3*n, d->xpos + 3*bodyid[gn]); } else { - mju_mulMatVec3(xpos_c + 3*n, d->xmat + 9*bodyid[gn], m->flex_node + 3*(gn + nstart)); - mju_addTo3(xpos_c + 3*n, d->xpos + 3*bodyid[gn]); + mju_mulMatVec3(xpos_e + 3*n, d->xmat + 9*bodyid[gn], m->flex_node + 3*(gn + nstart)); + mju_addTo3(xpos_e + 3*n, d->xpos + 3*bodyid[gn]); } - mju_copy3(refpos_c + 3*n, m->flex_node0 + 3*(gn + nstart)); + mju_copy3(refpos_e + 3*n, m->flex_node0 + 3*(gn + nstart)); } - // compute corotational quaternion from cell-local positions - mjtNum cell_quat[4] = {1, 0, 0, 0}; - { + // compute corotational quaternion + mjtNum elem_quat[4] = {1, 0, 0, 0}; + if (shell_mode) { + // determine face normal axis from elem_idx + int face_sizes[6] = {cy*cz, cy*cz, cx*cz, cx*cz, cx*cy, cx*cy}; + int face_normals[6] = {0, 0, 1, 1, 2, 2}; + int cumul = 0, normal_axis = 0; + for (int ff = 0; ff < 6; ff++) { + if (elem_idx < cumul + face_sizes[ff]) { + normal_axis = face_normals[ff]; + break; + } + cumul += face_sizes[ff]; + } + int na0 = (normal_axis + 1) % 3; + int na1 = (normal_axis + 2) % 3; + + // compute corotational rotation from 2D deformation gradient at face center + mjtNum p[2] = {.5, .5}; + mju_flexInterpRotation2D(order, xpos_e, npe, na0, na1, normal_axis, p, elem_quat); + } else { mjtNum center[3] = {0.5, 0.5, 0.5}; mjtNum mat[9]; - mju_defGradient(mat, center, xpos_c, order); - mju_mat2Rot(cell_quat, mat); - mju_negQuat(cell_quat, cell_quat); + mju_defGradient(mat, center, xpos_e, order); + mju_mat2Rot(elem_quat, mat); + mju_negQuat(elem_quat, elem_quat); } - // build per-cell sparse chain and node Jacobians - int* cell_chain = mjSTACKALLOC(d, nv, int); - int cell_nnz = 0; - mjtNum* cell_node_jac = cell_pos_and_jac(m, d, f, npc, gindices, nv, xpos_c, cell_chain, - &cell_nnz); + // build per-element sparse chain and node Jacobians + int* elem_chain = mjSTACKALLOC(d, nv, int); + int elem_nnz = 0; + mjtNum* elem_node_jac = cell_pos_and_jac(m, d, f, npe, gindices, nv, xpos_e, elem_chain, + &elem_nnz); - mjtNum* strain_jac = mjSTACKALLOC(d, cell_nnz, mjtNum); - mjtNum* dSdx_local = mjSTACKALLOC(d, 3*npc, mjtNum); + mjtNum* strain_jac = mjSTACKALLOC(d, elem_nnz, mjtNum); + mjtNum* dSdx_local = mjSTACKALLOC(d, 3*npe, mjtNum); // for dense mode: allocate and zero a dense Jacobian buffer once mjtNum* dense_jac = NULL; @@ -785,58 +819,55 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { } // read eigenmode data from flex_stiffness - int ndof_cell = 3 * npc; - int cell_idx = ci * m->flex_cellnum[3*f+1] * m->flex_cellnum[3*f+2] - + cj * m->flex_cellnum[3*f+2] + ck; - const mjtNum* k_cell = m->flex_stiffness + m->flex_stiffnessadr[f] - + cell_idx * ndof_cell * ndof_cell; - int neig = (int)k_cell[0]; + int ndof_elem = 3 * npe; + const mjtNum* k_elem = m->flex_stiffness + m->flex_stiffnessadr[f] + + elem_idx * ndof_elem * ndof_elem; + int neig = (int)k_elem[0]; // compute displacement in corotational frame - mjtNum* displ_c = mjSTACKALLOC(d, ndof_cell, mjtNum); - for (int n = 0; n < npc; n++) { - // rotate xpos_c to corotational frame + mjtNum* displ_e = mjSTACKALLOC(d, ndof_elem, mjtNum); + for (int n = 0; n < npe; n++) { + // rotate xpos_e to corotational frame mjtNum xrot[3]; - mju_rotVecQuat(xrot, xpos_c + 3*n, cell_quat); - displ_c[3*n + 0] = xrot[0] - refpos_c[3*n + 0]; - displ_c[3*n + 1] = xrot[1] - refpos_c[3*n + 1]; - displ_c[3*n + 2] = xrot[2] - refpos_c[3*n + 2]; + mju_rotVecQuat(xrot, xpos_e + 3*n, elem_quat); + displ_e[3*n + 0] = xrot[0] - refpos_e[3*n + 0]; + displ_e[3*n + 1] = xrot[1] - refpos_e[3*n + 1]; + displ_e[3*n + 2] = xrot[2] - refpos_e[3*n + 2]; } // compute inverse quaternion for rotating eigenvectors to world frame - mjtNum cell_quat_inv[4]; - mju_negQuat(cell_quat_inv, cell_quat); + mjtNum elem_quat_inv[4]; + mju_negQuat(elem_quat_inv, elem_quat); // loop over eigenmodes for (int eig = 0; eig < neig; eig++) { - const mjtNum* eigvec = k_cell + 1 + eig * ndof_cell; + const mjtNum* eigvec = k_elem + 1 + eig * ndof_elem; // constraint residual: dot product of scaled eigenvector with displacement mjtNum residual = 0; - for (int j = 0; j < ndof_cell; j++) { - residual += eigvec[j] * displ_c[j]; + for (int j = 0; j < ndof_elem; j++) { + residual += eigvec[j] * displ_e[j]; } cpos[0] = residual; // rotate eigenvector to world frame for Jacobian - // dSdx_local[3*n+c] = Σ_d R_inv[c][d] * eigvec[3*n+d] - for (int n = 0; n < npc; n++) { - mju_rotVecQuat(dSdx_local + 3*n, eigvec + 3*n, cell_quat_inv); + for (int n = 0; n < npe; n++) { + mju_rotVecQuat(dSdx_local + 3*n, eigvec + 3*n, elem_quat_inv); } - // contract with cell_node_jac to get sparse Jacobian - cell_strain_jacobian(npc, cell_nnz, dSdx_local, cell_node_jac, strain_jac); + // contract with elem_node_jac to get sparse Jacobian + cell_strain_jacobian(npe, elem_nnz, dSdx_local, elem_node_jac, strain_jac); if (issparse) { mj_addConstraint(m, d, strain_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, - cell_nnz, cell_chain); + elem_nnz, elem_chain); } else { - for (int k = 0; k < cell_nnz; k++) { - dense_jac[cell_chain[k]] = strain_jac[k]; + for (int k = 0; k < elem_nnz; k++) { + dense_jac[elem_chain[k]] = strain_jac[k]; } mj_addConstraint(m, d, dense_jac, cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL); - for (int k = 0; k < cell_nnz; k++) { - dense_jac[cell_chain[k]] = 0; + for (int k = 0; k < elem_nnz; k++) { + dense_jac[elem_chain[k]] = 0; } } } @@ -1674,36 +1705,56 @@ void mj_diagApprox(const mjModel* m, mjData* d) { break; case mjEQ_FLEXSTRAIN: { - // strain constraints: per-cell, use avg inv weight of cell's npc nodes + // strain constraints: use avg inv weight of element's nodes int flex_id = m->eq_obj1id[id]; int nstart = m->flex_nodeadr[flex_id]; - int order = m->flex_interp[flex_id]; - order = order < 0 ? -order : order; - int npc = (order+1)*(order+1)*(order+1); + int interp = m->flex_interp[flex_id]; + int order = interp < 0 ? -interp : interp; + int is_shell = (interp < 0); - // per-cell constraint count - int nquad = order + 1; - int ngauss = nquad * nquad * nquad; - int nconstraint = (order == 1) ? (2 + 3 * ngauss) : (6 * ngauss); - - // get cell index from eq_data - int eq_id = d->efc_id[i]; - int ci_cell = (int)m->eq_data[mjNEQDATA*eq_id + 0]; - int cj_cell = (int)m->eq_data[mjNEQDATA*eq_id + 1]; - int ck_cell = (int)m->eq_data[mjNEQDATA*eq_id + 2]; + int cx = m->flex_cellnum[3*flex_id+0]; int cy = m->flex_cellnum[3*flex_id+1]; int cz = m->flex_cellnum[3*flex_id+2]; + // nodes per element + int npe; + int elem_idx; + if (is_shell) { + npe = (order+1) * (order+1); + elem_idx = (int)m->eq_data[mjNEQDATA*id + 0]; + } else { + npe = (order+1) * (order+1) * (order+1); + int ci_cell = (int)m->eq_data[mjNEQDATA*id + 0]; + int cj_cell = (int)m->eq_data[mjNEQDATA*id + 1]; + int ck_cell = (int)m->eq_data[mjNEQDATA*id + 2]; + elem_idx = ci_cell * cy * cz + cj_cell * cz + ck_cell; + } + + // read neig from flex_stiffness + int ndof_elem = 3 * npe; + const mjtNum* k_elem = m->flex_stiffness + m->flex_stiffnessadr[flex_id] + + elem_idx * ndof_elem * ndof_elem; + int nconstraint = (int)k_elem[0]; + + // get element node indices int gindices[125]; - mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell, - NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + if (is_shell) { + mju_flexGatherFaceState(order, cx, cy, cz, elem_idx, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + } else { + int ci_cell = (int)m->eq_data[mjNEQDATA*id + 0]; + int cj_cell = (int)m->eq_data[mjNEQDATA*id + 1]; + int ck_cell = (int)m->eq_data[mjNEQDATA*id + 2]; + mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + } mjtNum avg_invweight = 0; - for (int n = 0; n < npc; n++) { + for (int n = 0; n < npe; n++) { int bodyid = m->flex_nodebodyid[nstart + gindices[n]]; avg_invweight += m->body_invweight0[2*bodyid]; } - avg_invweight /= npc; + avg_invweight /= npe; for (int c = 0; c < nconstraint; c++) { dA[i++] = avg_invweight; } @@ -2296,37 +2347,56 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { break; case mjEQ_FLEXSTRAIN: { - // per-cell strain constraints: each equality is one cell + // per-element strain constraints: each equality is one cell or face int f = id[0]; - int order = m->flex_interp[f]; - order = order < 0 ? -order : order; + int interp = m->flex_interp[f]; + int order = interp < 0 ? -interp : interp; + int is_shell = (interp < 0); if (!order || !m->flex_nodenum[f]) { break; } - int npc = (order+1)*(order+1)*(order+1); - // read eigenmode count from flex_stiffness - int ndof_cell = 3 * npc; - int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0]; - int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1]; - int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2]; + int cx = m->flex_cellnum[3*f+0]; int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; - int cell_idx = ci_cell * cy * cz + cj_cell * cz + ck_cell; - const mjtNum* k_cell = m->flex_stiffness + m->flex_stiffnessadr[f] - + cell_idx * ndof_cell * ndof_cell; - size = (int)k_cell[0]; // neig stored as first element + + int npe; + int elem_idx; + if (is_shell) { + npe = (order+1) * (order+1); + elem_idx = (int)m->eq_data[mjNEQDATA*i + 0]; + } else { + npe = (order+1) * (order+1) * (order+1); + int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0]; + int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1]; + int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2]; + elem_idx = ci_cell * cy * cz + cj_cell * cz + ck_cell; + } + + // read eigenmode count from flex_stiffness + int ndof_elem = 3 * npe; + const mjtNum* k_elem = m->flex_stiffness + m->flex_stiffnessadr[f] + + elem_idx * ndof_elem * ndof_elem; + size = (int)k_elem[0]; // neig stored as first element if (nnz) { - // get the npc node body IDs for this cell + // get element node body IDs int gindices[125]; - mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell, - NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + if (is_shell) { + mju_flexGatherFaceState(order, cx, cy, cz, elem_idx, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + } else { + int ci_cell = (int)m->eq_data[mjNEQDATA*i + 0]; + int cj_cell = (int)m->eq_data[mjNEQDATA*i + 1]; + int ck_cell = (int)m->eq_data[mjNEQDATA*i + 2]; + mju_flexGatherCellState(order, cy, cz, ci_cell, cj_cell, ck_cell, + NULL, NULL, NULL, NULL, NULL, NULL, gindices, NULL); + } int nstart = m->flex_nodeadr[f]; - for (int n = 0; n < npc; n++) { + for (int n = 0; n < npe; n++) { cell_bodies[n] = m->flex_nodebodyid[nstart + gindices[n]]; } - NV = mj_jacSumCount(m, d, chain, npc, cell_bodies); // npc nodes only + NV = mj_jacSumCount(m, d, chain, npe, cell_bodies); NV = size * NV; } break; diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 60c80073..3728a239 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -882,23 +882,29 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, // compute upper bounds across all interpolated flexes int max_nodenum = 0; - int max_npc = 0; + int max_npe = 0; // max nodes per element (3D cell or 2D face) for (int f = 0; f < m->nflex; f++) { if (!m->flex_interp[f]) continue; if (m->flex_rigid[f]) continue; int order = m->flex_interp[f]; + int shell_mode = order < 0; order = order < 0 ? -order : order; - int npc = (order+1)*(order+1)*(order+1); - if (npc > max_npc) max_npc = npc; + int npe; + if (shell_mode) { + npe = (order+1)*(order+1); + } else { + npe = (order+1)*(order+1)*(order+1); + } + if (npe > max_npe) max_npe = npe; if (m->flex_nodenum[f] > max_nodenum) max_nodenum = m->flex_nodenum[f]; } // nothing to do - if (max_npc == 0) { + if (max_npe == 0) { return; } - int max_dim_c = 3 * max_npc; + int max_dim_c = 3 * max_npe; // single unconditional markStack mj_markStack(d); @@ -915,8 +921,8 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, // per-flex node positions (upper bound) mjtNum* xpos = mjSTACKALLOC(d, 3*max_nodenum, mjtNum); - // per-cell arrays (upper bound) - mjtNum* xpos_c = mjSTACKALLOC(d, 3*max_npc, mjtNum); + // per-element arrays (upper bound) + mjtNum* xpos_c = mjSTACKALLOC(d, 3*max_npe, mjtNum); mjtNum* K_rot_cell = mjSTACKALLOC(d, max_dim_c*max_dim_c, mjtNum); // sparse Jacobian for one cell (upper bound) @@ -967,131 +973,141 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, } int order = m->flex_interp[f]; + int shell_mode = order < 0; order = order < 0 ? -order : order; - int npc = (order+1)*(order+1)*(order+1); int cx = m->flex_cellnum[3*f+0]; int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; - int dim_c = 3 * npc; + // determine element type: 2D boundary quads (shell) or 3D cells (volume) + int npe; + int nelem_fe; + if (shell_mode) { + npe = (order+1)*(order+1); + nelem_fe = 2*(cy*cz + cx*cz + cx*cy); + } else { + npe = (order+1)*(order+1)*(order+1); + nelem_fe = cx * cy * cz; + } + int dim_e = 3 * npe; // gather raw node positions (unrotated) mju_flexGatherState(m, d, f, xpos, NULL); - // loop over cells - int cell_idx = 0; - for (int ci = 0; ci < cx; ci++) { - for (int cj = 0; cj < cy; cj++) { - for (int ck = 0; ck < cz; ck++) { - // get cell stiffness - mjtNum* k_cell = K + cell_idx * 3*npc * 3*npc; + // loop over finite elements + for (int fe = 0; fe < nelem_fe; fe++) { + // get element stiffness + mjtNum* k_elem = K + fe * 3*npe * 3*npe; - // skip empty cells: stiffness buffer is zero-initialized at compile time - // (user_model.cc), and non-empty cells have strictly positive diagonal - if (k_cell[0] == 0) { - cell_idx++; - continue; - } + // skip empty elements: stiffness buffer is zero-initialized at compile time + // (user_model.cc), and non-empty elements have strictly positive diagonal + if (k_elem[0] == 0) { + continue; + } - // gather cell-local node positions - int gindices[125]; // max npc = 125 for quadratic - mjtNum quat[4]; - mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos, NULL, NULL, - xpos_c, NULL, NULL, gindices, quat); + // gather element-local node positions + int gindices[125]; // max npe = 125 for quadratic 3D + mjtNum quat[4]; + if (shell_mode) { + mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos, NULL, NULL, + xpos_c, NULL, NULL, gindices, quat); + } else { + int ci = fe / (cy * cz); + int cj = (fe / cz) % cy; + int ck = fe % cz; + mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos, NULL, NULL, + xpos_c, NULL, NULL, gindices, quat); + } - // R = R_global2local, RT = R_local2global - mjtNum R[9], RT[9]; - mju_quat2Mat(R, quat); - mju_transpose(RT, R, 3, 3); + // R = R_global2local, RT = R_local2global + mjtNum R[9], RT[9]; + mju_quat2Mat(R, quat); + mju_transpose(RT, R, 3, 3); - // compute K_rot_cell = RT * K_cell * R (block-wise) - mju_zero(K_rot_cell, dim_c*dim_c); - for (int a = 0; a < npc; a++) { - for (int b = 0; b < npc; b++) { - mjtNum blk[9], tmp[9]; + // compute K_rot = RT * K_elem * R (block-wise) + mju_zero(K_rot_cell, dim_e*dim_e); + for (int a = 0; a < npe; a++) { + for (int b = 0; b < npe; b++) { + mjtNum blk[9], tmp[9]; - // get K_cell(a,b) 3x3 block - int adr_cell = (3*a)*(3*npc) + 3*b; - for (int r = 0; r < 3; r++) { - for (int c = 0; c < 3; c++) { - blk[3*r+c] = k_cell[adr_cell + r*(3*npc) + c]; - } - } - - // tmp = K * R - mju_mulMatMat3(tmp, blk, R); - // blk = RT * tmp = RT * K * R - mju_mulMatMat3(blk, RT, tmp); - - // store in K_rot_cell at (a, b) - int adr_out = (3*a)*dim_c + 3*b; - for (int r = 0; r < 3; r++) { - for (int c = 0; c < 3; c++) { - K_rot_cell[adr_out + r*dim_c + c] = scale * blk[3*r+c]; - } - } + // get K_elem(a,b) 3x3 block + int adr_cell = (3*a)*(3*npe) + 3*b; + for (int r = 0; r < 3; r++) { + for (int c = 0; c < 3; c++) { + blk[3*r+c] = k_elem[adr_cell + r*(3*npe) + c]; } } - // construct sparse Jacobian for this cell's nodes - int current_adr = 0; - for (int n = 0; n < npc; n++) { - int bid = bodyid[gindices[n]]; - int chain_nnz = mj_bodyChain(m, bid, chain_colind); - mj_jacSparse(m, d, blk_jac, NULL, xpos+3*gindices[n], bid, - chain_nnz, chain_colind, /*flg_skipcommon=*/0); + // tmp = K * R + mju_mulMatMat3(tmp, blk, R); + // blk = RT * tmp = RT * K * R + mju_mulMatMat3(blk, RT, tmp); - for (int r = 0; r < 3; r++) { - int row_idx = 3*n + r; - J_rownnz[row_idx] = chain_nnz; - J_rowadr[row_idx] = current_adr; - - for (int idx = 0; idx < chain_nnz; idx++) { - J_colind[current_adr] = chain_colind[idx]; - J_val[current_adr] = blk_jac[r*chain_nnz + idx]; - current_adr++; - } + // store in K_rot_cell at (a, b) + int adr_out = (3*a)*dim_e + 3*b; + for (int r = 0; r < 3; r++) { + for (int c = 0; c < 3; c++) { + K_rot_cell[adr_out + r*dim_e + c] = scale * blk[3*r+c]; } } + } + } - // apply operation with cell's K_rot and J - if (op == mjFLEXOP_VEC) { - addJTBJ_mulSparse(m, d, res, vec, J_rownnz, J_rowadr, J_colind, - J_val, K_rot_cell, dim_c); - } else if (op == mjFLEXOP_ADDH) { - // H -= J_cell^T * K_rot_cell * J_cell (banded format) - mju_zero(J_reduced, dim_c*ndof); + // construct sparse Jacobian for this element's nodes + int current_adr = 0; + for (int n = 0; n < npe; n++) { + int bid = bodyid[gindices[n]]; + int chain_nnz = mj_bodyChain(m, bid, chain_colind); + mj_jacSparse(m, d, blk_jac, NULL, xpos+3*gindices[n], bid, + chain_nnz, chain_colind, /*flg_skipcommon=*/0); - for (int i = 0; i < dim_c; i++) { - int nnz = J_rownnz[i]; - int adr = J_rowadr[i]; - for (int idx = 0; idx < nnz; idx++) { - int global_col = J_colind[adr + idx]; - int local_idx = global2local[global_col]; - if (local_idx >= 0) { - J_reduced[i*ndof + local_idx] = J_val[adr + idx]; - } - } - } + for (int r = 0; r < 3; r++) { + int row_idx = 3*n + r; + J_rownnz[row_idx] = chain_nnz; + J_rowadr[row_idx] = current_adr; - // KJ = K_rot_cell * J_reduced (dim_c x ndof) - mju_mulMatMat(KJ, K_rot_cell, J_reduced, dim_c, dim_c, ndof); + for (int idx = 0; idx < chain_nnz; idx++) { + J_colind[current_adr] = chain_colind[idx]; + J_val[current_adr] = blk_jac[r*chain_nnz + idx]; + current_adr++; + } + } + } - // H[i,j] -= J_reduced[k,i] * KJ[k,j], store lower triangle in banded format - for (int i = 0; i < ndof; i++) { - for (int j = mjMAX(0, i-nband+1); j <= i; j++) { - mjtNum val = 0; - for (int dim_idx = 0; dim_idx < dim_c; dim_idx++) { - val += J_reduced[dim_idx*ndof + i] * KJ[dim_idx*ndof + j]; - } - res[i*nband + nband-1-(i-j)] -= val; - } + // apply operation with element's K_rot and J + if (op == mjFLEXOP_VEC) { + addJTBJ_mulSparse(m, d, res, vec, J_rownnz, J_rowadr, J_colind, + J_val, K_rot_cell, dim_e); + } else if (op == mjFLEXOP_ADDH) { + // H -= J_elem^T * K_rot * J_elem (banded format) + mju_zero(J_reduced, dim_e*ndof); + + for (int i = 0; i < dim_e; i++) { + int nnz = J_rownnz[i]; + int adr = J_rowadr[i]; + for (int idx = 0; idx < nnz; idx++) { + int global_col = J_colind[adr + idx]; + int local_idx = global2local[global_col]; + if (local_idx >= 0) { + J_reduced[i*ndof + local_idx] = J_val[adr + idx]; } } + } - cell_idx++; + // KJ = K_rot * J_reduced (dim_e x ndof) + mju_mulMatMat(KJ, K_rot_cell, J_reduced, dim_e, dim_e, ndof); + + // H[i,j] -= J_reduced[k,i] * KJ[k,j], store lower triangle in banded format + for (int i = 0; i < ndof; i++) { + for (int j = mjMAX(0, i-nband+1); j <= i; j++) { + mjtNum val = 0; + for (int dim_idx = 0; dim_idx < dim_e; dim_idx++) { + val += J_reduced[dim_idx*ndof + i] * KJ[dim_idx*ndof + j]; + } + res[i*nband + nband-1-(i-j)] -= val; + } } } } diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 295ec4a7..f8d2ba42 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -236,13 +236,23 @@ static void mj_springdamper(const mjModel* m, mjData* d) { if (m->flex_interp[f]) { int order = m->flex_interp[f]; + int shell_mode = order < 0; order = order < 0 ? -order : order; - int npc = (order+1)*(order+1)*(order+1); // nodes per cell int cx = m->flex_cellnum[3*f+0]; int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; - int ny_g = cy * order + 1; - int nz_g = cz * order + 1; + + // determine element type: 2D boundary quads (shell) or 3D cells (volume) + int npe; // nodes per element + int nelem_fe; // total finite elements + + if (shell_mode) { + npe = (order+1)*(order+1); + nelem_fe = 2*(cy*cz + cx*cz + cx*cy); + } else { + npe = (order+1)*(order+1)*(order+1); + nelem_fe = cx * cy * cz; + } mj_markStack(d); @@ -261,77 +271,70 @@ static void mj_springdamper(const mjModel* m, mjData* d) { mju_zero(frc_g, 3*nodenum); mju_zero(dmp_g, 3*nodenum); - // per-cell arrays - mjtNum* xpos_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* vel_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* xpos0_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* displ_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* frc_c = mjSTACKALLOC(d, 3*npc, mjtNum); - mjtNum* dmp_c = mjSTACKALLOC(d, 3*npc, mjtNum); + // per-element arrays (sized for npe) + mjtNum* xpos_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* vel_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* xpos0_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* displ_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* frc_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* dmp_e = mjSTACKALLOC(d, 3*npe, mjtNum); + int* gindices = mjSTACKALLOC(d, npe, int); - // loop over cells - int cell_idx = 0; - for (int ci = 0; ci < cx; ci++) { - for (int cj = 0; cj < cy; cj++) { - for (int ck = 0; ck < cz; ck++) { - // get cell stiffness matrix - mjtNum* k_cell = k + cell_idx * 3*npc * 3*npc; + // loop over finite elements + for (int fe = 0; fe < nelem_fe; fe++) { + // get element stiffness matrix + mjtNum* k_elem = k + fe * 3*npe * 3*npe; - // skip empty cells (zero stiffness) - if (k_cell[0] == 0) { - cell_idx++; - continue; - } + // skip empty elements (zero stiffness) + if (k_elem[0] == 0) { + continue; + } - // gather cell-local node data - mjtNum quat[4]; - mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, xpos0, - xpos_c, vel_c, xpos0_c, NULL, quat); + // gather element-local node data and compute corotational rotation + mjtNum quat[4]; + if (shell_mode) { + mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos_g, vel_g, xpos0, + xpos_e, vel_e, xpos0_e, gindices, quat); + } else { + int ci = fe / (cy * cz); + int cj = (fe / cz) % cy; + int ck = fe % cz; + mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, + xpos0, xpos_e, vel_e, xpos0_e, gindices, + quat); + } - // rotate to corotational frame - for (int n = 0; n < npc; n++) { - mju_rotVecQuat(xpos_c+3*n, xpos_c+3*n, quat); - mju_rotVecQuat(vel_c+3*n, vel_c+3*n, quat); - } + // rotate to corotational frame + for (int n = 0; n < npe; n++) { + mju_rotVecQuat(xpos_e+3*n, xpos_e+3*n, quat); + mju_rotVecQuat(vel_e+3*n, vel_e+3*n, quat); + } - // compute displacement - for (int n = 0; n < npc; n++) { - mji_addScl3(displ_c+3*n, xpos_c+3*n, xpos0_c+3*n, -1); - } + // compute displacement + for (int n = 0; n < npe; n++) { + mji_addScl3(displ_e+3*n, xpos_e+3*n, xpos0_e+3*n, -1); + } - // compute force in corotational frame - if (enbl_spring) { - mju_mulMatVec(frc_c, k_cell, displ_c, 3*npc, 3*npc); - } - if (enbl_damper) { - mju_mulMatVec(dmp_c, k_cell, vel_c, 3*npc, 3*npc); - } + // compute force in corotational frame + if (enbl_spring) { + mju_mulMatVec(frc_e, k_elem, displ_e, 3*npe, 3*npe); + } + if (enbl_damper) { + mju_mulMatVec(dmp_e, k_elem, vel_e, 3*npe, 3*npe); + } - // rotate back to global frame and scatter - mju_negQuat(quat, quat); - int local = 0; - for (int li = 0; li <= order; li++) { - for (int lj = 0; lj <= order; lj++) { - for (int lk = 0; lk <= order; lk++) { - int gi = ci*order + li; - int gj = cj*order + lj; - int gk = ck*order + lk; - int gidx = gi*ny_g*nz_g + gj*nz_g + gk; - mjtNum qfrc[3], qdmp[3]; - mji_rotVecQuat(qfrc, frc_c+3*local, quat); - mji_rotVecQuat(qdmp, dmp_c+3*local, quat); - if (enbl_spring) { - mji_addTo3(frc_g + 3*gidx, qfrc); - } - if (enbl_damper) { - mji_addTo3(dmp_g + 3*gidx, qdmp); - } - local++; - } - } - } - - cell_idx++; + // rotate back to global frame and scatter using node indices + mju_negQuat(quat, quat); + for (int n = 0; n < npe; n++) { + mjtNum qfrc[3], qdmp[3]; + mji_rotVecQuat(qfrc, frc_e+3*n, quat); + mji_rotVecQuat(qdmp, dmp_e+3*n, quat); + int gidx = gindices[n]; + if (enbl_spring) { + mji_addTo3(frc_g + 3*gidx, qfrc); + } + if (enbl_damper) { + mji_addTo3(dmp_g + 3*gidx, qdmp); } } } diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 6ec01f72..ef9916a0 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -726,6 +726,133 @@ void mju_flexGatherCellState(int order, int cy, int cz, int ci, int cj, int ck, } +// compute corotational rotation from 2D deformation gradient on a flat face +void mju_flexInterpRotation2D(int order, const mjtNum* xpos_f, int npe, + int axis0, int axis1, int normal_axis, + const mjtNum local[2], mjtNum* quat) { + // compute 3x2 deformation gradient F at parametric point local + mjtNum t1[3] = {0, 0, 0}; // tangent along axis0 + mjtNum t2[3] = {0, 0, 0}; // tangent along axis1 + int idx = 0; + for (int l0 = 0; l0 <= order; l0++) { + for (int l1 = 0; l1 <= order; l1++) { + mjtNum grad0 = dphi(local[0], l0, order) * phi(local[1], l1, order); + mjtNum grad1 = phi(local[0], l0, order) * dphi(local[1], l1, order); + for (int d = 0; d < 3; d++) { + t1[d] += xpos_f[3*idx + d] * grad0; + t2[d] += xpos_f[3*idx + d] * grad1; + } + idx++; + } + } + + // normal = t1 x t2 + mjtNum normal[3]; + mju_cross(normal, t1, t2); + + // build 3x3 matrix with columns assigned to canonical axes (row-major) + // axis0 → t1, axis1 → t2, normal_axis → normal + // this ensures identity rotation for axis-aligned grids + mjtNum mat[9] = {0}; + mjtNum* vecs[3]; + vecs[axis0] = t1; + vecs[axis1] = t2; + vecs[normal_axis] = normal; + + for (int col = 0; col < 3; col++) { + mat[0*3 + col] = vecs[col][0]; + mat[1*3 + col] = vecs[col][1]; + mat[2*3 + col] = vecs[col][2]; + } + + // extract rotation via polar decomposition + quat[0] = 1; + quat[1] = 0; + quat[2] = 0; + quat[3] = 0; + mju_mat2Rot(quat, mat); + mju_negQuat(quat, quat); +} + + +// gather face-element-local quantities and optionally compute rotation (shell mode) +// +// face element enumeration for a grid with cell counts (cx, cy, cz): +// face 0: x=0 cy*cz quads (normal=0) +// face 1: x=max cy*cz quads (normal=0) +// face 2: y=0 cx*cz quads (normal=1) +// face 3: y=max cx*cz quads (normal=1) +// face 4: z=0 cx*cy quads (normal=2) +// face 5: z=max cx*cy quads (normal=2) +void mju_flexGatherFaceState(int order, int cx, int cy, int cz, + int face_elem_idx, + const mjtNum* xpos_g, const mjtNum* vel_g, + const mjtNum* xpos0_g, + mjtNum* xpos_f, mjtNum* vel_f, mjtNum* xpos0_f, + int* nodeindices, mjtNum* quat) { + int ny_g = cy * order + 1; + int nz_g = cz * order + 1; + int npe = (order + 1) * (order + 1); + + // face sizes and properties + int face_sizes[6] = {cy*cz, cy*cz, cx*cz, cx*cz, cx*cy, cx*cy}; + int face_normal[6] = {0, 0, 1, 1, 2, 2}; + int face_count1[6] = {cz, cz, cx, cx, cy, cy}; + int face_fixed_vals[6]; + face_fixed_vals[0] = 0; + face_fixed_vals[1] = cx * order; + face_fixed_vals[2] = 0; + face_fixed_vals[3] = cy * order; + face_fixed_vals[4] = 0; + face_fixed_vals[5] = cz * order; + + // determine which face and quad within face + int face_id = 0; + int within_face = face_elem_idx; + int cumul = 0; + for (int f = 0; f < 6; f++) { + if (face_elem_idx < cumul + face_sizes[f]) { + face_id = f; + within_face = face_elem_idx - cumul; + break; + } + cumul += face_sizes[f]; + } + + int normal_axis = face_normal[face_id]; + int na0 = (normal_axis + 1) % 3; // slow in-plane axis + int na1 = (normal_axis + 2) % 3; // fast in-plane axis + int c1 = face_count1[face_id]; + int g_fixed = face_fixed_vals[face_id]; + int q0 = within_face / c1; + int q1 = within_face % c1; + + // gather nodes + int local = 0; + for (int l0 = 0; l0 <= order; l0++) { + for (int l1 = 0; l1 <= order; l1++) { + int g[3]; + g[normal_axis] = g_fixed; + g[na0] = q0 * order + l0; + g[na1] = q1 * order + l1; + int gidx = g[0] * ny_g * nz_g + g[1] * nz_g + g[2]; + + if (xpos_f && xpos_g) mju_copy3(xpos_f + 3*local, xpos_g + 3*gidx); + if (vel_f && vel_g) mju_copy3(vel_f + 3*local, vel_g + 3*gidx); + if (xpos0_f && xpos0_g) mju_copy3(xpos0_f + 3*local, xpos0_g + 3*gidx); + if (nodeindices) nodeindices[local] = gidx; + + local++; + } + } + + if (quat && xpos_f) { + mjtNum p[2] = {.5, .5}; + mju_flexInterpRotation2D(order, xpos_f, npe, na0, na1, normal_axis, p, quat); + } +} + + //------------------------------ actuator models --------------------------------------------------- // normalized muscle length-gain curve diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index b5574670..374f54c2 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -103,6 +103,19 @@ MJAPI void mju_flexGatherCellState(int order, int cy, int cz, int ci, int cj, in const mjtNum* xpos0_g, mjtNum* xpos_c, mjtNum* vel_c, mjtNum* xpos0_c, int* nodeindices, mjtNum* quat); +// gather face-element-local quantities and optionally compute rotation (shell mode) +MJAPI void mju_flexGatherFaceState(int order, int cx, int cy, int cz, + int face_elem_idx, + const mjtNum* xpos_g, const mjtNum* vel_g, + const mjtNum* xpos0_g, + mjtNum* xpos_f, mjtNum* vel_f, mjtNum* xpos0_f, + int* nodeindices, mjtNum* quat); + +// compute corotational rotation from 2D deformation gradient on a flat face +MJAPI void mju_flexInterpRotation2D(int order, const mjtNum* xpos_f, int npe, + int axis0, int axis1, int normal_axis, + const mjtNum local[2], mjtNum* quat); + // ----------------------------- Base64 ------------------------------------------------------------ diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 081781ac..85b900ae 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -884,27 +884,47 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vf pe->active = true; mjs_setString(pe->name1, name.c_str()); } else if (equality == 3) { - // create one strain constraint per cell, storing cell index in eq_data + // create one strain constraint per finite element, storing element index flex->has_strain_eq = true; int cell_cx = flex->spec.cellcount[0]; int cell_cy = flex->spec.cellcount[1]; int cell_cz = flex->spec.cellcount[2]; - for (int ci = 0; ci < cell_cx; ci++) { - for (int cj = 0; cj < cell_cy; cj++) { - for (int ck = 0; ck < cell_cz; ck++) { - // skip empty cells - if (!flex->cell_empty.empty() && - flex->cell_empty[ci * cell_cy * cell_cz + cj * cell_cz + ck]) { - continue; + bool shell = (doftype == mjFCOMPDOF_TRILINEAR || + doftype == mjFCOMPDOF_QUADRATIC) && + flex->spec.elastic2d; + + if (shell) { + // shell mode: one constraint per boundary face element + int nelem_fe = 2*(cell_cy*cell_cz + cell_cx*cell_cz + cell_cx*cell_cy); + for (int fe = 0; fe < nelem_fe; fe++) { + mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); + mjs_setDefault(pe->element, &model->Default()->spec); + pe->type = mjEQ_FLEXSTRAIN; + pe->active = true; + mjs_setString(pe->name1, name.c_str()); + pe->data[0] = fe; + pe->data[1] = -1; // sentinel: shell mode + pe->data[2] = -1; + } + } else { + // volume mode: one constraint per 3D cell + for (int ci = 0; ci < cell_cx; ci++) { + for (int cj = 0; cj < cell_cy; cj++) { + for (int ck = 0; ck < cell_cz; ck++) { + // skip empty cells + if (!flex->cell_empty.empty() && + flex->cell_empty[ci * cell_cy * cell_cz + cj * cell_cz + ck]) { + continue; + } + mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); + mjs_setDefault(pe->element, &model->Default()->spec); + pe->type = mjEQ_FLEXSTRAIN; + pe->active = true; + mjs_setString(pe->name1, name.c_str()); + pe->data[0] = ci; + pe->data[1] = cj; + pe->data[2] = ck; } - mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec); - mjs_setDefault(pe->element, &model->Default()->spec); - pe->type = mjEQ_FLEXSTRAIN; - pe->active = true; - mjs_setString(pe->name1, name.c_str()); - pe->data[0] = ci; - pe->data[1] = cj; - pe->data[2] = ck; } } } diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 3389a71e..c4012201 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -3807,6 +3807,98 @@ void inline ComputeLinearStiffness(std::vector& K, } +// compute the linear stiffness matrix for a flat 2D quad face element (membrane) +// K: output stiffness matrix, size 3*npe x 3*npe, npe = (order+1)^2 +// pos: node positions (3*npe doubles), ordered row-major in 2D parametric domain +// E, nu: Young's modulus and Poisson's ratio +// order: interpolation order (1 or 2) +// thickness: shell thickness +// normal_axis: axis perpendicular to the face (0=x, 1=y, 2=z) +void inline ComputeLinearStiffness2D(std::vector& K, + const double* pos, + double E, double nu, int order, + double thickness, int normal_axis) { + int nbasis = order + 1; + int npe = nbasis * nbasis; // nodes per face element + int ndof = 3 * npe; + + // in-plane axes + int axis0 = (normal_axis + 1) % 3; // slow-varying + int axis1 = (normal_axis + 2) % 3; // fast-varying + + // compute quadrature points + std::vector points(nbasis); + std::vector weight(nbasis); + quadratureGaussLegendre(points.data(), weight.data(), nbasis, 0, 1); + + // compute element transformation (diagonal Jacobian on flat face) + double d0 = (pos + 3*(npe-1))[axis0] - pos[axis0]; // extent along axis0 + double d1 = (pos + 3*(npe-1))[axis1] - pos[axis1]; // extent along axis1 + if (d0 == 0 || d1 == 0) { + throw mjCError(nullptr, "degenerate 2D element with zero extent"); + } + double detJ = d0 * d1; + double invJ0 = 1.0 / d0; + double invJ1 = 1.0 / d1; + + // plane-stress Lamé parameter: lambda* = E*nu/(1 - nu^2) + double la = E * nu / (1.0 - nu * nu); + double mu = E / (2.0 * (1.0 + nu)); + + // basis function gradients (2-component) + std::vector> F(npe); + + // loop over quadrature points (2D) + for (int ps = 0; ps < nbasis; ps++) { + for (int pt = 0; pt < nbasis; pt++) { + double s = points[ps]; + double t = points[pt]; + double dvol = weight[ps] * weight[pt] * detJ * thickness; + int dof = 0; + + // cartesian product of 2D basis functions + for (int b0 = 0; b0 < nbasis; b0++) { + for (int b1 = 0; b1 < nbasis; b1++) { + F[dof][0] = dphi(s, b0, order) * phi(t, b1, order); + F[dof][1] = phi(s, b0, order) * dphi(t, b1, order); + dof++; + } + } + + if (dof != npe) { + throw mjCError(nullptr, "incorrect number of 2D basis functions"); + } + + // tensor contraction (same structure as 3D but with zero normal column) + for (int i = 0; i < npe; i++) { + for (int j = 0; j < npe; j++) { + Matrix du; + Matrix dv; + du.fill({0, 0, 0}); + dv.fill({0, 0, 0}); + for (int k = 0; k < 3; k++) { + for (int l = 0; l < 3; l++) { + // du[k] has non-zero entries only at in-plane axes + du[k][axis0] = invJ0 * F[i][0]; + du[k][axis1] = invJ1 * F[i][1]; + // du[k][normal_axis] = 0 (already zero) + dv[l][axis0] = invJ0 * F[j][0]; + dv[l][axis1] = invJ1 * F[j][1]; + // dv[l][normal_axis] = 0 (already zero) + K[ndof*(3*i+k) + 3*j+l] -= la * trace(du) * trace(dv) * dvol; + // mu (not 2*mu): same convention as 3D ComputeLinearStiffness + K[ndof*(3*i+k) + 3*j+l] -= mu * trace(inner(sym(du), sym(dv))) * dvol; + mjuu_zerovec(du[k].data(), 3); + mjuu_zerovec(dv[l].data(), 3); + } + } + } + } + } + } +} + + // Eigendecompose cell stiffness matrix and store scaled eigenvectors. // K_cell is n×n stored (negative convention: K_stored = -K_physical). // Output layout in `out`: @@ -3964,7 +4056,8 @@ void mjCFlex::ResolveReferences(const mjCModel* m) { mjCBody* pbody = static_cast(m->FindObject(mjOBJ_BODY, vertbody)); if (pbody) { vertbodyid.push_back(pbody->id); - if (pbody->joints.size() != 3 && dim == 2 && (elastic2d == 1 || elastic2d == 3)) { + if (pbody->joints.size() != 3 && dim == 2 && + (elastic2d == 1 || elastic2d == 3) && !interpolated) { // TODO(quaglino): add support for pins throw mjCError(this, "pins are not supported for bending"); } @@ -4105,8 +4198,8 @@ void mjCFlex::Compile(const mjVFS* vfs) { if (thickness <= 0) { throw mjCError(this, "2d elasticity requires positive thickness"); } - if (interpolated) { - throw mjCError(this, "interpolated flex does not yet support 2d elasticity"); + if (interpolated && elastic2d != 2) { + mju_warning("bending passive force is not implemented for interpolated flex"); } if (dim != 2 && !interpolated) { throw mjCError(this, "2d elasticity requires 2d flex"); @@ -4141,6 +4234,11 @@ void mjCFlex::Compile(const mjVFS* vfs) { if (spec.cellcount[0] == 0 || spec.cellcount[1] == 0 || spec.cellcount[2] == 0) { throw mjCError(this, "cellcount cannot be 0 in any dimension when interpolation order > 0"); } + if (elastic2d && !(spec.cellcount[0] == 1 || spec.cellcount[1] == 1 || spec.cellcount[2] == 1)) { + throw mjCError(this, + "shell trilinear flex requires at least one dimension " + "with cell count equal to one (no interior nodes)"); + } int expected_nodes = (spec.cellcount[0] * spec.order + 1) * (spec.cellcount[1] * spec.order + 1) * (spec.cellcount[2] * spec.order + 1); @@ -4354,7 +4452,7 @@ void mjCFlex::Compile(const mjVFS* vfs) { } // bending stiffness (2D only) - if (dim == 2 && (elastic2d == 1 || elastic2d == 3)) { + if (dim == 2 && (elastic2d == 1 || elastic2d == 3) && !interpolated) { bending.assign(nedge*17, 0); for (unsigned int e = 0; e < nedge; e++) { @@ -4392,57 +4490,130 @@ void mjCFlex::Compile(const mjVFS* vfs) { double K_young = has_strain_eq ? 1e1 : young; double K_poisson = has_strain_eq ? 0.3 : poisson; - int npc = pow(spec.order + 1, 3); // nodes per cell - int ndof_cell = 3 * npc; int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2]; - int ncells = cx * cy * cz; int ny_global = cy * spec.order + 1; int nz_global = cz * spec.order + 1; - // total stiffness = ncells * ndof_cell^2 - stiffness.resize(ncells * ndof_cell * ndof_cell, 0); + // determine element type: 2D boundary quads (shell) or 3D cells (volume) + bool shell_mode = elastic2d != 0; + int npe; // nodes per element + int nelem_fe; // total finite elements - // compute stiffness per cell - for (int ci = 0; ci < cx; ci++) { - for (int cj = 0; cj < cy; cj++) { - for (int ck = 0; ck < cz; ck++) { - int cell_idx = ci * cy * cz + cj * cz + ck; + if (shell_mode) { + npe = pow(spec.order + 1, 2); // (order+1)^2 for 2D quads + nelem_fe = 2*(cy*cz + cx*cz + cx*cy); + } else { + npe = pow(spec.order + 1, 3); // (order+1)^3 for 3D cells + nelem_fe = cx * cy * cz; + } + int ndof_elem = 3 * npe; - // skip stiffness computation for empty cells (no mesh content) - if (!cell_empty.empty() && cell_empty[cell_idx]) { - continue; + // total stiffness = nelem_fe * ndof_elem^2 + stiffness.resize(nelem_fe * ndof_elem * ndof_elem, 0); + + // face layout for shell mode: + // face 0: x=0 (cy*cz quads, normal=0, in-plane=(1,2)) + // face 1: x=max (cy*cz quads, normal=0, in-plane=(1,2)) + // face 2: y=0 (cx*cz quads, normal=1, in-plane=(0,2)) + // face 3: y=max (cx*cz quads, normal=1, in-plane=(0,2)) + // face 4: z=0 (cx*cy quads, normal=2, in-plane=(0,1)) + // face 5: z=max (cx*cy quads, normal=2, in-plane=(0,1)) + // face_sizes = {cy*cz, cy*cz, cx*cz, cx*cz, cx*cy, cx*cy} + int face_sizes[6] = {cy*cz, cy*cz, cx*cz, cx*cz, cx*cy, cx*cy}; + int face_normal[6] = {0, 0, 1, 1, 2, 2}; + // cell counts along each in-plane axis for each face + int face_count1[6] = {cz, cz, cx, cx, cy, cy}; // fast axis count + // fixed axis value (in grid node units, 0 or max) + int face_fixed[6] = {0, cx*spec.order, 0, cy*spec.order, 0, cz*spec.order}; + + // compute stiffness per element + for (int fe = 0; fe < nelem_fe; fe++) { + // gather element node positions + std::vector elem_pos(3 * npe); + int normal_axis = -1; + + if (shell_mode) { + // determine which face and quad within face + int face_id = 0, within_face = fe; + int cumul = 0; + for (int f = 0; f < 6; f++) { + if (fe < cumul + face_sizes[f]) { + face_id = f; + within_face = fe - cumul; + break; } + cumul += face_sizes[f]; + } - // gather cell's local node positions - std::vector cell_pos(3 * npc); - int local = 0; - for (int li = 0; li <= spec.order; li++) { - for (int lj = 0; lj <= spec.order; lj++) { - for (int lk = 0; lk <= spec.order; lk++) { - int gi = ci * spec.order + li; - int gj = cj * spec.order + lj; - int gk = ck * spec.order + lk; - int global = gi * ny_global * nz_global + gj * nz_global + gk; - mjuu_copyvec(cell_pos.data() + 3*local, nodexpos_local.data() + 3*global, 3); - local++; - } - } - } + normal_axis = face_normal[face_id]; + int na0 = (normal_axis + 1) % 3; // slow in-plane axis + int na1 = (normal_axis + 2) % 3; // fast in-plane axis + int c1 = face_count1[face_id]; // cell count along fast axis + int g_fixed = face_fixed[face_id]; // grid index along normal axis + int q0 = within_face / c1; // quad index along slow in-plane axis + int q1 = within_face % c1; // quad index along fast in-plane axis - // compute per-cell stiffness - std::vector K_cell(ndof_cell * ndof_cell, 0); - ComputeLinearStiffness(K_cell, cell_pos.data(), K_young, K_poisson, spec.order); - double* out = stiffness.data() + cell_idx * ndof_cell * ndof_cell; - - if (has_strain_eq) { - // eigendecompose: store [neig, sqrt(λ)*v_1, sqrt(λ)*v_2, ...] - std::fill(out, out + ndof_cell * ndof_cell, 0.0); - EigendecomposeStiffness(K_cell.data(), out, ndof_cell); - } else { - // store raw K for passive forces - std::copy(K_cell.begin(), K_cell.end(), out); + // gather 2D face element nodes + int local = 0; + for (int l0 = 0; l0 <= spec.order; l0++) { + for (int l1 = 0; l1 <= spec.order; l1++) { + // build global node index from 3 axis values + int g[3]; + g[normal_axis] = g_fixed; + g[na0] = q0 * spec.order + l0; + g[na1] = q1 * spec.order + l1; + int global = g[0] * ny_global * nz_global + g[1] * nz_global + g[2]; + mjuu_copyvec(elem_pos.data() + 3*local, + nodexpos_local.data() + 3*global, 3); + local++; } } + } else { + // 3D cell: convert flat index to (ci, cj, ck) + int ci = fe / (cy * cz); + int cj = (fe / cz) % cy; + int ck = fe % cz; + + // skip stiffness computation for empty cells (no mesh content) + if (!cell_empty.empty() && cell_empty[fe]) { + continue; + } + + // gather cell's local node positions + int local = 0; + for (int li = 0; li <= spec.order; li++) { + for (int lj = 0; lj <= spec.order; lj++) { + for (int lk = 0; lk <= spec.order; lk++) { + int gi = ci * spec.order + li; + int gj = cj * spec.order + lj; + int gk = ck * spec.order + lk; + int global = gi * ny_global * nz_global + gj * nz_global + gk; + mjuu_copyvec(elem_pos.data() + 3*local, + nodexpos_local.data() + 3*global, 3); + local++; + } + } + } + } + + // compute per-element stiffness + std::vector K_elem(ndof_elem * ndof_elem, 0); + if (shell_mode) { + ComputeLinearStiffness2D(K_elem, elem_pos.data(), K_young, K_poisson, + spec.order, thickness, normal_axis); + } else { + ComputeLinearStiffness(K_elem, elem_pos.data(), K_young, K_poisson, + spec.order); + } + double* out = stiffness.data() + fe * ndof_elem * ndof_elem; + + if (has_strain_eq) { + // eigendecompose: store [neig, sqrt(λ)*v_1, sqrt(λ)*v_2, ...] + std::fill(out, out + ndof_elem * ndof_elem, 0.0); + EigendecomposeStiffness(K_elem.data(), out, ndof_elem); + } else { + // store raw K for passive forces + std::copy(K_elem.begin(), K_elem.end(), out); } } } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 19672803..6c33cdec 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2191,12 +2191,20 @@ void mjCModel::SetSizes() { nflexevpair += (int)flexes_[i]->evpair.size()/2; nflextexcoord += (flexes_[i]->HasTexcoord() ? flexes_[i]->get_texcoord().size()/2 : 0); if (flexes_[i]->spec.order != 0) { - int npc = (int)pow(flexes_[i]->spec.order + 1, 3); - int ndof_cell = 3 * npc; - int ncells = flexes_[i]->spec.cellcount[0] * - flexes_[i]->spec.cellcount[1] * - flexes_[i]->spec.cellcount[2]; - extra_stiffness_size += ncells * ndof_cell * ndof_cell; + int cx = flexes_[i]->spec.cellcount[0]; + int cy = flexes_[i]->spec.cellcount[1]; + int cz = flexes_[i]->spec.cellcount[2]; + bool shell = (flexes_[i]->elastic2d != 0); + int npe, nelem; + if (shell) { + npe = (int)pow(flexes_[i]->spec.order + 1, 2); + nelem = 2*(cy*cz + cx*cz + cx*cy); + } else { + npe = (int)pow(flexes_[i]->spec.order + 1, 3); + nelem = cx * cy * cz; + } + int ndof_elem = 3 * npe; + extra_stiffness_size += nelem * ndof_elem * ndof_elem; } if (flexes_[i]->interpolated || flexes_[i]->rigid) { continue; @@ -3476,10 +3484,20 @@ void mjCModel::CopyObjects(mjModel* m) { m->flex_stiffnessadr[i] = 21 * elem_adr; } else { m->flex_stiffnessadr[i] = current_extra_stiffness_adr; - int npc = (int)pow(pfl->spec.order + 1, 3); - int ndof_cell = 3 * npc; - int ncells = pfl->spec.cellcount[0] * pfl->spec.cellcount[1] * pfl->spec.cellcount[2]; - current_extra_stiffness_adr += ncells * ndof_cell * ndof_cell; + int pcx = pfl->spec.cellcount[0]; + int pcy = pfl->spec.cellcount[1]; + int pcz = pfl->spec.cellcount[2]; + bool shell = (pfl->elastic2d != 0); + int npe, nelem; + if (shell) { + npe = (int)pow(pfl->spec.order + 1, 2); + nelem = 2*(pcy*pcz + pcx*pcz + pcx*pcy); + } else { + npe = (int)pow(pfl->spec.order + 1, 3); + nelem = pcx * pcy * pcz; + } + int ndof_elem = 3 * npe; + current_extra_stiffness_adr += nelem * ndof_elem * ndof_elem; } if (!pfl->stiffness.empty()) { @@ -3490,10 +3508,20 @@ void mjCModel::CopyObjects(mjModel* m) { if (pfl->spec.order == 0) { stiff_size = 21 * pfl->nelem; } else { - int npc = (int)pow(pfl->spec.order + 1, 3); - int ndof_cell = 3 * npc; - int ncells = pfl->spec.cellcount[0] * pfl->spec.cellcount[1] * pfl->spec.cellcount[2]; - stiff_size = ncells * ndof_cell * ndof_cell; + int scx = pfl->spec.cellcount[0]; + int scy = pfl->spec.cellcount[1]; + int scz = pfl->spec.cellcount[2]; + bool shell = (pfl->elastic2d != 0); + int npe, sncells; + if (shell) { + npe = (int)pow(pfl->spec.order + 1, 2); + sncells = 2*(scy*scz + scx*scz + scx*scy); + } else { + npe = (int)pow(pfl->spec.order + 1, 3); + sncells = scx * scy * scz; + } + int ndof_elem = 3 * npe; + stiff_size = sncells * ndof_elem * ndof_elem; } mjuu_zerovec(m->flex_stiffness + m->flex_stiffnessadr[i], stiff_size); } @@ -3629,8 +3657,8 @@ void mjCModel::CopyObjects(mjModel* m) { memcpy(m->flex_nodebodyid + node_adr, pfl->nodebodyid.data(), pfl->nnode*sizeof(int)); } - // set interpolation type, only two types for now - m->flex_interp[i] = pfl->spec.order; + // set interpolation type: positive = volumetric, negative = shell mode + m->flex_interp[i] = pfl->spec.elastic2d ? -pfl->spec.order : pfl->spec.order; // set cell count for multi-cell finite cell method m->flex_cellnum[3*i+0] = pfl->spec.cellcount[0]; diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index 07204df8..69b0a173 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -682,6 +682,46 @@ TEST_F(CoreConstraintTest, StrainConstraintQuadratic) { mj_deleteModel(m); } +TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) { + static constexpr char xml[] = R"( + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, testing::NotNull()) << error; + mjData* d = mj_makeData(m); + + mj_forward(m, d); + + // Check number of equalities + EXPECT_EQ(m->neq, 6); + + // Check total number of scalar equality constraints + EXPECT_EQ(d->ne, 48); // 6 faces * 8 modes per face + + // all constraint residuals should be zero at rest + for (int i = 0; i < d->ne; i++) { + EXPECT_NEAR(d->efc_pos[i], 0, 1e-10) + << "nonzero constraint residual at " << i; + } + + mj_deleteData(d); + mj_deleteModel(m); +} + // Test quadratic passive forces (no constraints) for stability TEST_F(CoreConstraintTest, QuadraticPassiveForceStability) { static constexpr char xml[] = R"( diff --git a/test/engine/engine_passive_test.cc b/test/engine/engine_passive_test.cc index dc4f4bcf..f87dc0a5 100644 --- a/test/engine/engine_passive_test.cc +++ b/test/engine/engine_passive_test.cc @@ -847,5 +847,41 @@ TEST_F(PassiveTest, PolynomialDampingTendon) { mj_deleteModel(m); } +// shell-mode (elastic2d=stretch) flexcomp must have zero passive spring forces +// at rest (initial configuration); any nonzero force indicates a rotation +// mismatch between compile-time reference positions and runtime corotation. +TEST_F(ElasticityTest, ShellModeZeroForceAtRest) { + static constexpr char xml[] = R"( + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, testing::NotNull()) << error; + mjData* d = mj_makeData(m); + + mj_forward(m, d); + + // all spring forces should be zero at rest + for (int i = 0; i < m->nv; i++) { + EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-10) + << "nonzero spring force at DOF " << i; + } + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco diff --git a/test/engine/engine_util_misc_test.cc b/test/engine/engine_util_misc_test.cc index d1eb8314..053a1e09 100644 --- a/test/engine/engine_util_misc_test.cc +++ b/test/engine/engine_util_misc_test.cc @@ -1165,5 +1165,390 @@ TEST_F(HistoryTest, CubicInterpolation) { EXPECT_NEAR(res[1], 1.0 - expected_0_8, MjTol(1e-9, 1e-9)); } +// -------------------------------- Face State --------------------------------- + +using FaceStateTest = MujocoTest; + +// verify mju_flexGatherFaceState returns correct node indices for all 6 faces +// of a 1x1x1 trilinear grid (2x2x2 = 8 nodes, 4 nodes per face) +TEST_F(FaceStateTest, NodeIndicesSingleCell) { + int order = 1; + int cx = 1, cy = 1, cz = 1; + int ny_g = cy * order + 1; // 2 + int nz_g = cz * order + 1; // 2 + + // nelem_fe = 2*(1*1 + 1*1 + 1*1) = 6 face elements + // face 0: x=0, face 1: x=max, face 2: y=0, face 3: y=max, + // face 4: z=0, face 5: z=max + + // create dummy positions for 8 nodes + std::vector xpos(3 * 8, 0); + for (int i = 0; i < 8; i++) { + xpos[3*i + 0] = (i / 4) * 1.0; + xpos[3*i + 1] = ((i / 2) % 2) * 1.0; + xpos[3*i + 2] = (i % 2) * 1.0; + } + + // helper: compute expected global node index from (gx, gy, gz) + auto gidx = [&](int gx, int gy, int gz) { + return gx * ny_g * nz_g + gy * nz_g + gz; + }; + + // face 0: x=0 (fixed g[0]=0, varying g[1], g[2]) + // normal_axis=0, na0=1, na1=2 + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(0, 0, 0)); + EXPECT_EQ(indices[1], gidx(0, 0, 1)); + EXPECT_EQ(indices[2], gidx(0, 1, 0)); + EXPECT_EQ(indices[3], gidx(0, 1, 1)); + } + + // face 1: x=max (fixed g[0]=1, varying g[1], g[2]) + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 1, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(1, 0, 0)); + EXPECT_EQ(indices[1], gidx(1, 0, 1)); + EXPECT_EQ(indices[2], gidx(1, 1, 0)); + EXPECT_EQ(indices[3], gidx(1, 1, 1)); + } + + // face 2: y=0 (fixed g[1]=0) + // normal_axis=1, na0=2(z slow), na1=0(x fast) + // loop order: l0→z, l1→x + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 2, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(0, 0, 0)); // l0=0(z=0), l1=0(x=0) + EXPECT_EQ(indices[1], gidx(1, 0, 0)); // l0=0(z=0), l1=1(x=1) + EXPECT_EQ(indices[2], gidx(0, 0, 1)); // l0=1(z=1), l1=0(x=0) + EXPECT_EQ(indices[3], gidx(1, 0, 1)); // l0=1(z=1), l1=1(x=1) + } + + // face 3: y=max (fixed g[1]=1) + // normal_axis=1, na0=2(z slow), na1=0(x fast) + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(0, 1, 0)); // l0=0(z=0), l1=0(x=0) + EXPECT_EQ(indices[1], gidx(1, 1, 0)); // l0=0(z=0), l1=1(x=1) + EXPECT_EQ(indices[2], gidx(0, 1, 1)); // l0=1(z=1), l1=0(x=0) + EXPECT_EQ(indices[3], gidx(1, 1, 1)); // l0=1(z=1), l1=1(x=1) + } + + // face 4: z=0 (fixed g[2]=0, varying g[0], g[1]) + // normal_axis=2, na0=0, na1=1 + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(0, 0, 0)); + EXPECT_EQ(indices[1], gidx(0, 1, 0)); + EXPECT_EQ(indices[2], gidx(1, 0, 0)); + EXPECT_EQ(indices[3], gidx(1, 1, 0)); + } + + // face 5: z=max (fixed g[2]=1) + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(0, 0, 1)); + EXPECT_EQ(indices[1], gidx(0, 1, 1)); + EXPECT_EQ(indices[2], gidx(1, 0, 1)); + EXPECT_EQ(indices[3], gidx(1, 1, 1)); + } +} + +// verify node indices for a multi-cell grid (2x2x2 cells → 3x3x3 = 27 nodes) +TEST_F(FaceStateTest, NodeIndicesMultiCell) { + int order = 1; + int cx = 2, cy = 2, cz = 2; + int ny_g = 3, nz_g = 3; // (2*1+1) = 3 + + // nelem_fe = 2*(2*2 + 2*2 + 2*2) = 24 face elements + // face 0: x=0, cy*cz = 4 quads (indices 0-3) + // face 1: x=max, 4 quads (indices 4-7) + // face 2: y=0, cx*cz = 4 quads (indices 8-11) + // face 3: y=max, 4 quads (indices 12-15) + // face 4: z=0, cx*cy = 4 quads (indices 16-19) + // face 5: z=max, 4 quads (indices 20-23) + + std::vector xpos(3 * 27, 0); + for (int i = 0; i < 27; i++) { + int gi = i / 9; + int gj = (i / 3) % 3; + int gk = i % 3; + xpos[3*i + 0] = gi * 0.1; + xpos[3*i + 1] = gj * 0.1; + xpos[3*i + 2] = gk * 0.1; + } + + auto gidx = [&](int gx, int gy, int gz) { + return gx * ny_g * nz_g + gy * nz_g + gz; + }; + + // face 0 (x=0), quad 0: (q0=0, q1=0) within cy*cz face + // c1 = face_count1[0] = cz = 2, so quad (0,0) → within_face = 0 + // na0=1, na1=2: g[0]=0, g[1]=0..1, g[2]=0..1 + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 0, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(0, 0, 0)); + EXPECT_EQ(indices[1], gidx(0, 0, 1)); + EXPECT_EQ(indices[2], gidx(0, 1, 0)); + EXPECT_EQ(indices[3], gidx(0, 1, 1)); + } + + // face 0 (x=0), quad 3: (q0=1, q1=1) → within_face = 1*2+1 = 3 + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 3, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(0, 1, 1)); + EXPECT_EQ(indices[1], gidx(0, 1, 2)); + EXPECT_EQ(indices[2], gidx(0, 2, 1)); + EXPECT_EQ(indices[3], gidx(0, 2, 2)); + } + + // face 1 (x=max), quad 0: fe_idx = 4 (after face 0's 4 quads) + // g[0] = cx*order = 2 + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(2, 0, 0)); + EXPECT_EQ(indices[1], gidx(2, 0, 1)); + EXPECT_EQ(indices[2], gidx(2, 1, 0)); + EXPECT_EQ(indices[3], gidx(2, 1, 1)); + } +} + +// verify node indices for a non-cubic grid (cx != cz) +TEST_F(FaceStateTest, NodeIndicesNonCubicGrid) { + int order = 1; + int cx = 2, cy = 1, cz = 3; + int ny_g = cy * order + 1; // 2 + int nz_g = cz * order + 1; // 4 + + // create dummy positions for (2*1+1)*(1*1+1)*(3*1+1) = 3*2*4 = 24 nodes + std::vector xpos(3 * 24, 0); + for (int i = 0; i < 24; i++) { + int gi = i / 8; + int gj = (i / 4) % 2; + int gk = i % 4; + xpos[3*i + 0] = gi * 0.1; + xpos[3*i + 1] = gj * 0.1; + xpos[3*i + 2] = gk * 0.1; + } + + auto gidx = [&](int gx, int gy, int gz) { + return gx * ny_g * nz_g + gy * nz_g + gz; + }; + + // face 2 (y=0): normal_axis=1, na0=2(z slow), na1=0(x fast) + // counts: na0 -> cz = 3, na1 -> cx = 2 + // total quads on face 2 = 6 + // we test within_face = 2 (third quad) + // correct: c1 = cx = 2. q0 = 2/2 = 1, q1 = 2%2 = 0 + // + // face element index calculation: + // face 0: cy*cz = 1*3 = 3 quads (indices 0-2) + // face 1: cy*cz = 1*3 = 3 quads (indices 3-5) + // face 2: cx*cz = 2*3 = 6 quads. Quad 2 is index 2 within this face. + // Total flat index = 3 + 3 + 2 = 8 + { + int indices[4]; + mju_flexGatherFaceState(order, cx, cy, cz, 8, xpos.data(), NULL, NULL, + NULL, NULL, NULL, indices, NULL); + EXPECT_EQ(indices[0], gidx(0, 0, 1)); + EXPECT_EQ(indices[1], gidx(1, 0, 1)); + EXPECT_EQ(indices[2], gidx(0, 0, 2)); + EXPECT_EQ(indices[3], gidx(1, 0, 2)); + } +} + +// verify data gathering: positions, velocities, and reference positions +TEST_F(FaceStateTest, DataGathering) { + int order = 1; + int cx = 1, cy = 1, cz = 1; + int npe = 4; + int nnodes = 8; + + // create positions and velocities for 8 nodes + std::vector xpos(3 * nnodes); + std::vector vel(3 * nnodes); + std::vector xpos0(3 * nnodes); + for (int i = 0; i < nnodes; i++) { + for (int d = 0; d < 3; d++) { + xpos[3*i + d] = 10 * i + d; + vel[3*i + d] = 100 * i + d; + xpos0[3*i + d] = 1000 * i + d; + } + } + + // gather face 4 (z=0): nodes at (0,0,0), (0,1,0), (1,0,0), (1,1,0) + // = global indices 0, 2, 4, 6 + std::vector xpos_f(3 * npe); + std::vector vel_f(3 * npe); + std::vector xpos0_f(3 * npe); + int indices[4]; + + mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), vel.data(), + xpos0.data(), xpos_f.data(), vel_f.data(), + xpos0_f.data(), indices, NULL); + + for (int n = 0; n < npe; n++) { + int gi = indices[n]; + for (int d = 0; d < 3; d++) { + EXPECT_EQ(xpos_f[3*n + d], xpos[3*gi + d]); + EXPECT_EQ(vel_f[3*n + d], vel[3*gi + d]); + EXPECT_EQ(xpos0_f[3*n + d], xpos0[3*gi + d]); + } + } +} + +// verify that flexInterpRotation2D produces identity for axis-aligned faces +// (tested via mju_flexGatherFaceState with quat output) +TEST_F(FaceStateTest, IdentityRotationAxisAligned) { + int order = 1; + int cx = 1, cy = 1, cz = 1; + int npe = 4; + + // create an axis-aligned unit cube: 8 nodes at {0,1}^3 + std::vector xpos(3 * 8); + int idx = 0; + for (int i = 0; i <= 1; i++) { + for (int j = 0; j <= 1; j++) { + for (int k = 0; k <= 1; k++) { + xpos[3*idx + 0] = i; + xpos[3*idx + 1] = j; + xpos[3*idx + 2] = k; + idx++; + } + } + } + + std::vector xpos_f(3 * npe); + mjtNum quat[4]; + + // test all 6 faces: each should give identity rotation (quat = [1,0,0,0]) + int nelem_fe = 6; + for (int fe = 0; fe < nelem_fe; fe++) { + mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos.data(), NULL, NULL, + xpos_f.data(), NULL, NULL, NULL, quat); + EXPECT_NEAR(mju_abs(quat[0]), 1.0, 1e-10) << "face " << fe; + EXPECT_NEAR(quat[1], 0.0, 1e-10) << "face " << fe; + EXPECT_NEAR(quat[2], 0.0, 1e-10) << "face " << fe; + EXPECT_NEAR(quat[3], 0.0, 1e-10) << "face " << fe; + } +} + +// verify that flexInterpRotation2D extracts the correct rotation for a +// globally rotated cube (90° around z-axis) +TEST_F(FaceStateTest, RotatedCubeRotation) { + int order = 1; + int cx = 1, cy = 1, cz = 1; + int npe = 4; + + // create an axis-aligned unit cube, then rotate 90° around z + // rotation: (x,y,z) → (-y, x, z) + std::vector xpos(3 * 8); + int idx = 0; + for (int i = 0; i <= 1; i++) { + for (int j = 0; j <= 1; j++) { + for (int k = 0; k <= 1; k++) { + mjtNum orig[3] = {(mjtNum)i, (mjtNum)j, (mjtNum)k}; + mjtNum axis[3] = {0, 0, 1}; + mjtNum rot_quat[4]; + mju_axisAngle2Quat(rot_quat, axis, mjPI / 2); + mju_rotVecQuat(xpos.data() + 3*idx, orig, rot_quat); + idx++; + } + } + } + + std::vector xpos_f(3 * npe); + mjtNum quat[4]; + + // expected rotation: global→local is inverse of the 90° z rotation + // 90° around z: quat = [cos(45°), 0, 0, sin(45°)] + // inverse (global→local): [cos(45°), 0, 0, -sin(45°)] + mjtNum sq2 = mju_sqrt(0.5); + + // test face 4 (z=0): normal_axis=2, in-plane axes are (0,1) + // tangent vectors should reflect the 90° z rotation + mju_flexGatherFaceState(order, cx, cy, cz, 4, xpos.data(), NULL, NULL, + xpos_f.data(), NULL, NULL, NULL, quat); + + EXPECT_NEAR(quat[0], sq2, 1e-5); + EXPECT_NEAR(quat[1], 0.0, 1e-5); + EXPECT_NEAR(quat[2], 0.0, 1e-5); + EXPECT_NEAR(quat[3], -sq2, 1e-5); + + // test face 5 (z=max): should give same rotation + mju_flexGatherFaceState(order, cx, cy, cz, 5, xpos.data(), NULL, NULL, + xpos_f.data(), NULL, NULL, NULL, quat); + + EXPECT_NEAR(quat[0], sq2, 1e-5); + EXPECT_NEAR(quat[1], 0.0, 1e-5); + EXPECT_NEAR(quat[2], 0.0, 1e-5); + EXPECT_NEAR(quat[3], -sq2, 1e-5); +} + +// verify that flexInterpRotation2D matches the 3D cell rotation for +// the same globally-rotated cube +TEST_F(FaceStateTest, RotationConsistencyWith3D) { + int order = 1; + int cx = 1, cy = 1, cz = 1; + + // create 90° z-rotated unit cube + std::vector xpos(3 * 8); + int idx = 0; + for (int i = 0; i <= 1; i++) { + for (int j = 0; j <= 1; j++) { + for (int k = 0; k <= 1; k++) { + mjtNum orig[3] = {(mjtNum)i, (mjtNum)j, (mjtNum)k}; + mjtNum axis[3] = {0, 0, 1}; + mjtNum rot_quat[4]; + mju_axisAngle2Quat(rot_quat, axis, mjPI / 6); + mju_rotVecQuat(xpos.data() + 3*idx, orig, rot_quat); + idx++; + } + } + } + + // get 3D cell rotation + int npc = 8; + std::vector xpos_c(3 * npc); + mjtNum quat_3d[4]; + mju_flexGatherCellState(order, cy, cz, 0, 0, 0, xpos.data(), NULL, NULL, + xpos_c.data(), NULL, NULL, NULL, quat_3d); + + // get 2D face rotation for each face and verify it matches the 3D rotation + int npe = 4; + std::vector xpos_f(3 * npe); + + int nelem_fe = 6; + for (int fe = 0; fe < nelem_fe; fe++) { + mjtNum quat_2d[4]; + mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos.data(), NULL, NULL, + xpos_f.data(), NULL, NULL, NULL, quat_2d); + + // quaternions may differ by sign; compare unsigned + mjtNum dot = quat_3d[0]*quat_2d[0] + quat_3d[1]*quat_2d[1] + + quat_3d[2]*quat_2d[2] + quat_3d[3]*quat_2d[3]; + EXPECT_NEAR(mju_abs(dot), 1.0, 1e-5) + << "face " << fe << ": 2D rotation differs from 3D cell rotation"; + } +} + } // namespace } // namespace mujoco diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index 59833aef..67d06da7 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -836,11 +836,11 @@ TEST_F(MjCMeshTest, Flex2DElasticityRequiresPositiveThickness) { HasSubstr("2d elasticity requires positive thickness")); } -TEST_F(MjCMeshTest, InterpolatedFlexDoesNotSupport2DElasticity) { +TEST_F(MjCMeshTest, InterpolatedFlexSupportsBendElasticityWithWarning) { static constexpr char xml[] = R"( - + @@ -849,10 +849,25 @@ TEST_F(MjCMeshTest, InterpolatedFlexDoesNotSupport2DElasticity) { )"; std::array error; mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, testing::IsNull()); - EXPECT_THAT( - error.data(), - HasSubstr("interpolated flex does not yet support 2d elasticity")); + EXPECT_THAT(model, testing::NotNull()) << error.data(); + mj_deleteModel(model); +} + +TEST_F(MjCMeshTest, InterpolatedFlexSupportsBothElasticityWithWarning) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, testing::NotNull()) << error.data(); + mj_deleteModel(model); } TEST_F(MjCMeshTest, Flex2DElasticityRequires2DFlex) { From e552a5f80db8db25dd13712ea02d5db0633777f7 Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Wed, 29 Apr 2026 10:26:13 -0700 Subject: [PATCH 168/251] Expose more filament render settings via XML custom fields PiperOrigin-RevId: 907659170 Change-Id: I0d8ca1ec058d40ed072db09c370f48b1e79ca6b3 --- .../filament/compat/scene_bridge.cc | 56 +++++++++++++++---- 1 file changed, 45 insertions(+), 11 deletions(-) diff --git a/src/experimental/filament/compat/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc index ed35625d..b8606f88 100644 --- a/src/experimental/filament/compat/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -90,15 +90,33 @@ SceneBridge::SceneBridge(FilamentContext* ctx, const mjModel* model) // Configure options for the normal view. auto cg = scene_view_->GetColorGradingOptions(); - cg.exposure = ReadElement(model, "filament.out.exposure", cg.exposure); - cg.contrast = ReadElement(model, "filament.out.contrast", cg.contrast); - cg.vibrance = ReadElement(model, "filament.out.vibrance", cg.vibrance); - cg.saturation = ReadElement(model, "filament.out.saturation", cg.saturation); - cg.temperature = ReadElement(model, "filament.out.temperature", cg.temperature); - cg.tint = ReadElement(model, "filament.out.tint", cg.tint); + cg.exposure = ReadElement(model, "filament.cg.exposure", cg.exposure); + cg.contrast = ReadElement(model, "filament.cg.contrast", cg.contrast); + cg.vibrance = ReadElement(model, "filament.cg.vibrance", cg.vibrance); + cg.saturation = ReadElement(model, "filament.cg.saturation", cg.saturation); + cg.temperature = + ReadElement(model, "filament.cg.temperature", cg.temperature); + cg.tint = ReadElement(model, "filament.cg.tint", cg.tint); + cg.gamut_mapping = + ReadElement(model, "filament.cg.gamut_mapping", cg.gamut_mapping); + cg.luminance_scaling = + ReadElement(model, "filament.cg.luminance_scaling", cg.luminance_scaling); + cg.slope = ReadElement(model, "filament.cg.slope", cg.slope); + cg.offset = ReadElement(model, "filament.cg.offset", cg.offset); + cg.power = ReadElement(model, "filament.cg.power", cg.power); + cg.shadow_gamma = + ReadElement(model, "filament.cg.shadow_gamma", cg.shadow_gamma); + cg.mid_point = ReadElement(model, "filament.cg.mid_point", cg.mid_point); + cg.highlight_scale = + ReadElement(model, "filament.cg.highlight_scale", cg.highlight_scale); + cg.shadows = ReadElement(model, "filament.cg.shadows", cg.shadows); + cg.midtones = ReadElement(model, "filament.cg.midtones", cg.midtones); + cg.highlights = ReadElement(model, "filament.cg.highlights", cg.highlights); + cg.tonal_ranges = + ReadElement(model, "filament.cg.tonal_ranges", cg.tonal_ranges); auto tone_mapping = - ReadElement(model, "filament.out.tone_mapping"); + ReadElement(model, "filament.cg.tone_mapping"); if (tone_mapping == "aces") { cg.tone_mapper = ToneMapperType::kACES; } else if (tone_mapping == "aces_legacy") { @@ -117,12 +135,28 @@ SceneBridge::SceneBridge(FilamentContext* ctx, const mjModel* model) ao.enabled = ReadElement(model, "filament.ao.enabled", true); ao.bentNormals = ReadElement(model, "filament.ao.bent_normals", false); ao.ssct.enabled = ReadElement(model, "filament.ao.ssct", ao.ssct.enabled); - ao.quality = filament::QualityLevel::ULTRA; - ao.lowPassFilter = filament::QualityLevel::ULTRA; - ao.upsampling = filament::QualityLevel::ULTRA; - ao.bilateralThreshold = 0.5f; + ao.quality = + ReadElement(model, "filament.ao.quality", filament::QualityLevel::ULTRA); + ao.lowPassFilter = ReadElement(model, "filament.ao.low_pass_filter", + filament::QualityLevel::ULTRA); + ao.upsampling = ReadElement(model, "filament.ao.upsampling", + filament::QualityLevel::ULTRA); + ao.bilateralThreshold = + ReadElement(model, "filament.ao.bilateral_threshold", 0.5f); fview->setAmbientOcclusionOptions(ao); + auto bloom = fview->getBloomOptions(); + bloom.enabled = ReadElement(model, "filament.bloom.enabled", bloom.enabled); + bloom.strength = + ReadElement(model, "filament.bloom.strength", bloom.strength); + bloom.dirtStrength = + ReadElement(model, "filament.bloom.dirt_strength", bloom.dirtStrength); + bloom.quality = ReadElement(model, "filament.bloom.quality", bloom.quality); + bloom.resolution = + ReadElement(model, "filament.bloom.resolution", bloom.resolution); + bloom.levels = ReadElement(model, "filament.bloom.levels", bloom.levels); + fview->setBloomOptions(bloom); + auto msaa = fview->getMultiSampleAntiAliasingOptions(); msaa.enabled = ReadElement(model, "filament.msaa.enabled", true); fview->setMultiSampleAntiAliasingOptions(msaa); From e71bd3db8e401d0cef0e3cea4bf72ea864b32b9c Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 29 Apr 2026 10:50:07 -0700 Subject: [PATCH 169/251] Refactor flex passive forces into dedicated functions. The code for computing passive forces for flex elements is moved into new static functions `mj_flexPassiveInterp`, `mj_flexPassiveBend`, and `mj_flexPassiveStretch`. This improves the structure of `mj_springdamper`. PiperOrigin-RevId: 907671516 Change-Id: I6d1e781bbd370331ae645a954b78092ecb1925b9 --- src/engine/engine_passive.c | 624 +++++++++++++++++++----------------- 1 file changed, 322 insertions(+), 302 deletions(-) diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index f8d2ba42..7438f4c7 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -59,6 +59,320 @@ static void inline GradSquaredLengths(mjtNum gradient[6][2][3], } +// passive forces for interpolated flex (stretch + bending) +static void mj_flexPassiveInterp(const mjModel* m, mjData* d, int f, + int enbl_spring, int enbl_damper) { + mjtNum* k = m->flex_stiffness + m->flex_stiffnessadr[f]; + int nodenum = m->flex_nodenum[f]; + + int order = m->flex_interp[f]; + int shell_mode = order < 0; + order = order < 0 ? -order : order; + int cx = m->flex_cellnum[3*f+0]; + int cy = m->flex_cellnum[3*f+1]; + int cz = m->flex_cellnum[3*f+2]; + + // determine element type: 2D boundary quads (shell) or 3D cells (volume) + int npe; // nodes per element + int nelem_fe; // total finite elements + + if (shell_mode) { + npe = (order+1)*(order+1); + nelem_fe = 2*(cy*cz + cx*cz + cx*cy); + } else { + npe = (order+1)*(order+1)*(order+1); + nelem_fe = cx * cy * cz; + } + + // check if we have any work to do + int has_stretch = k[0] != 0 && m->flex_edgeequality[f] != 3; + if (!has_stretch) { + return; + } + + mj_markStack(d); + + // allocate global arrays + mjtNum* xpos_g = mjSTACKALLOC(d, 3*nodenum, mjtNum); + mjtNum* vel_g = mjSTACKALLOC(d, 3*nodenum, mjtNum); + mjtNum* frc_g = mjSTACKALLOC(d, 3*nodenum, mjtNum); + mjtNum* dmp_g = mjSTACKALLOC(d, 3*nodenum, mjtNum); + mjtNum* xpos0 = m->flex_node0 + 3*m->flex_nodeadr[f]; + int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; + + // gather global node positions and velocities (unrotated) + mju_flexGatherState(m, d, f, xpos_g, vel_g); + + // zero global force accumulators + mju_zero(frc_g, 3*nodenum); + mju_zero(dmp_g, 3*nodenum); + + // per-element arrays (sized for npe) + mjtNum* xpos_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* vel_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* xpos0_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* displ_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* frc_e = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* dmp_e = mjSTACKALLOC(d, 3*npe, mjtNum); + int* gindices = mjSTACKALLOC(d, npe, int); + + // -------------------- stretch forces -------------------- + if (has_stretch) { + for (int fe = 0; fe < nelem_fe; fe++) { + // get element stiffness matrix + mjtNum* k_elem = k + fe * 3*npe * 3*npe; + + // skip empty elements (zero stiffness) + if (k_elem[0] == 0) { + continue; + } + + // gather element-local node data and compute corotational rotation + mjtNum quat[4]; + if (shell_mode) { + mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos_g, vel_g, xpos0, + xpos_e, vel_e, xpos0_e, gindices, quat); + } else { + int ci = fe / (cy * cz); + int cj = (fe / cz) % cy; + int ck = fe % cz; + mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, + xpos0, xpos_e, vel_e, xpos0_e, gindices, + quat); + } + + // rotate to corotational frame + for (int n = 0; n < npe; n++) { + mju_rotVecQuat(xpos_e+3*n, xpos_e+3*n, quat); + mju_rotVecQuat(vel_e+3*n, vel_e+3*n, quat); + } + + // compute displacement + for (int n = 0; n < npe; n++) { + mji_addScl3(displ_e+3*n, xpos_e+3*n, xpos0_e+3*n, -1); + } + + // compute force in corotational frame + if (enbl_spring) { + mju_mulMatVec(frc_e, k_elem, displ_e, 3*npe, 3*npe); + } + if (enbl_damper) { + mju_mulMatVec(dmp_e, k_elem, vel_e, 3*npe, 3*npe); + } + + // rotate back to global frame and scatter using node indices + mju_negQuat(quat, quat); + for (int n = 0; n < npe; n++) { + mjtNum qfrc[3], qdmp[3]; + mji_rotVecQuat(qfrc, frc_e+3*n, quat); + mji_rotVecQuat(qdmp, dmp_e+3*n, quat); + int gidx = gindices[n]; + if (enbl_spring) { + mji_addTo3(frc_g + 3*gidx, qfrc); + } + if (enbl_damper) { + mji_addTo3(dmp_g + 3*gidx, qdmp); + } + } + } + } + + // apply accumulated forces to bodies + for (int i = 0; i < nodenum; i++) { + mju_scl3(dmp_g+3*i, dmp_g+3*i, m->flex_damping[f]); + int bid = bodyid[i]; + int nidx = i + m->flex_nodeadr[f]; + + // fast path: node at body origin (not pinned), direct DOF write + if (m->body_dofnum[bid] > 0 && + (m->flex_centered[f] || + (m->flex_node[3*nidx+0] == 0 && + m->flex_node[3*nidx+1] == 0 && + m->flex_node[3*nidx+2] == 0))) { + if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bid], frc_g+3*i); + if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bid], dmp_g+3*i); + } else { + if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_spring); + if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_damper); + } + } + + mj_freeStack(d); +} + + +// passive forces for flex bending +static void mj_flexPassiveBend(const mjModel* m, mjData* d, int f, + int enbl_spring, int enbl_damper) { + if (m->flex_dim[f] != 2) { + return; + } + + int edgenum = m->flex_edgenum[f]; + mjtNum* xpos = d->flexvert_xpos + 3*m->flex_vertadr[f]; + int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f]; + mjtNum* b = m->flex_bending + 17*m->flex_edgeadr[f]; + + for (int e = 0; e < edgenum; e++) { + const int* edge = m->flex_edge + 2*(e+m->flex_edgeadr[f]); + const int* flap = m->flex_edgeflap + 2*(e+m->flex_edgeadr[f]); + int v[4] = {edge[0], edge[1], flap[0], flap[1]}; + if (v[3] == -1) { + // skip boundary edges + continue; + } + + // flap edges + mjtNum ed[3][3]; + mji_sub3(ed[0], xpos + 3*v[1], xpos + 3*v[0]); + mji_sub3(ed[1], xpos + 3*v[2], xpos + 3*v[0]); + mji_sub3(ed[2], xpos + 3*v[3], xpos + 3*v[0]); + + // forces at the vertices due to curved reference + mjtNum frc[4][3]; + mji_cross(frc[1], ed[1], ed[2]); + mji_cross(frc[2], ed[2], ed[0]); + mji_cross(frc[3], ed[0], ed[1]); + frc[0][0] = -(frc[1][0] + frc[2][0] + frc[3][0]); + frc[0][1] = -(frc[1][1] + frc[2][1] + frc[3][1]); + frc[0][2] = -(frc[1][2] + frc[2][2] + frc[3][2]); + + // velocities + mjtNum* vel[4]; + for (int i = 0; i < 4; i++) { + vel[i] = d->qvel + m->body_dofadr[bodyid[v[i]]]; + } + + // force + mjtNum spring[12] = {0}; + mjtNum damper[12] = {0}; + for (int i = 0; i < 4; i++) { + for (int x = 0; x < 3; x++) { + for (int j = 0; j < 4; j++) { + // thin plate bending force + if (enbl_spring) spring[3*i+x] += b[17*e+4*i+j] * xpos[3*v[j]+x]; + + // thin plate damping force + // TODO: do not assume DOFs are in the world frame + if (enbl_damper) damper[3*i+x] += b[17*e+4*i+j] * vel[j][x]; + } + + // curved reference contribution + if (enbl_spring) spring[3*i+x] += b[17*e+16] * frc[i][x]; + } + } + + // insert into global force + for (int i = 0; i < 4; i++) { + int bid = bodyid[v[i]]; + int body_dofnum = m->body_dofnum[bid]; + int body_dofadr = m->body_dofadr[bid]; + for (int x = 0; x < body_dofnum; x++) { + if (enbl_spring) d->qfrc_spring[body_dofadr+x] -= spring[3*i+x]; + if (enbl_damper) d->qfrc_damper[body_dofadr+x] -= damper[3*i+x] * m->flex_damping[f]; + } + } + } +} + + +// passive forces for flex stretch +static void mj_flexPassiveStretch(const mjModel* m, mjData* d, int f, + int enbl_spring, int enbl_damper) { + mjtNum* k = m->flex_stiffness + m->flex_stiffnessadr[f]; + if (k[0] == 0) { + return; + } + + int dim = m->flex_dim[f]; + int nedge = (dim == 2) ? 3 : 6; + int nvert = (dim == 2) ? 3 : 4; + const int* elem = m->flex_elem + m->flex_elemdataadr[f]; + const int* edgeelem = m->flex_elemedge + m->flex_elemedgeadr[f]; + mjtNum* xpos = d->flexvert_xpos + 3*m->flex_vertadr[f]; + mjtNum* vel = d->flexedge_velocity + m->flex_edgeadr[f]; + mjtNum* deformed = d->flexedge_length + m->flex_edgeadr[f]; + mjtNum* reference = m->flexedge_length0 + m->flex_edgeadr[f]; + int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f]; + mjtNum kD = m->opt.timestep > 0 ? m->flex_damping[f] / m->opt.timestep : 0; + + mj_markStack(d); + mjtNum* qfrc = mjSTACKALLOC(d, 3*m->flex_vertnum[f], mjtNum); + mju_zero(qfrc, 3*m->flex_vertnum[f]); + + // compute force element-by-element + int elemnum = m->flex_elemnum[f]; + for (int t = 0; t < elemnum; t++) { + const int* vert = elem + (dim+1) * t; + + // compute length gradient with respect to dofs + mjtNum gradient[6][2][3]; + GradSquaredLengths(gradient, xpos, vert, edges[dim-2], nedge); + + // we add generalized Rayleigh damping as described in Section 5.2 of + // Kharevych et al., "Geometric, Variational Integrators for Computer + // Animation" http://multires.caltech.edu/pubs/DiscreteLagrangian.pdf + + // extract elongation of edges belonging to this element + mjtNum elongation[6]; + for (int e = 0; e < nedge; e++) { + int idx = edgeelem[t * nedge + e]; + mjtNum previous = deformed[idx] - vel[idx] * m->opt.timestep; + elongation[e] = deformed[idx]*deformed[idx] - reference[idx]*reference[idx] + + (deformed[idx]*deformed[idx] - previous*previous) * kD; + } + + // unpack triangular representation + mjtNum metric[36]; + int id = 0; + for (int ed1 = 0; ed1 < nedge; ed1++) { + for (int ed2 = ed1; ed2 < nedge; ed2++) { + metric[nedge*ed1 + ed2] = k[21*t + id]; + metric[nedge*ed2 + ed1] = k[21*t + id++]; + } + } + + // compute local force + mjtNum force[12] = {0}; + for (int ed1 = 0; ed1 < nedge; ed1++) { + for (int ed2 = 0; ed2 < nedge; ed2++) { + for (int i = 0; i < 2; i++) { + for (int x = 0; x < 3; x++) { + force[3 * edges[dim-2][ed2][i] + x] -= + elongation[ed1] * gradient[ed2][i][x] * + metric[nedge * ed1 + ed2]; + } + } + } + } + + // insert into global force + for (int i = 0; i < nvert; i++) { + for (int x = 0; x < 3; x++) { + qfrc[3*vert[i]+x] += force[3*i+x]; + } + } + } + + // insert force into qfrc_passive, straightforward for simple bodies, + // need to distribute the force in case of pinned vertices + for (int v = 0; v < m->flex_vertnum[f]; v++) { + int bid = bodyid[v]; + if (m->body_simple[bid] != 2) { + // this should only occur for pinned flex vertices + mj_applyFT(m, d, qfrc + 3*v, 0, xpos + 3*v, bid, d->qfrc_spring); + } else { + int body_dofnum = m->body_dofnum[bid]; + int body_dofadr = m->body_dofadr[bid]; + for (int x = 0; x < body_dofnum; x++) { + d->qfrc_spring[body_dofadr+x] += qfrc[3*v+x]; + } + } + } + + mj_freeStack(d); +} + // spring and damper forces static void mj_springdamper(const mjModel* m, mjData* d) { @@ -147,314 +461,20 @@ static void mj_springdamper(const mjModel* m, mjData* d) { // flex elasticity for (int f=0; f < m->nflex; f++) { - mjtNum* k = m->flex_stiffness + m->flex_stiffnessadr[f]; - mjtNum* b = m->flex_bending + 17*m->flex_edgeadr[f]; - int dim = m->flex_dim[f]; - int nodenum = m->flex_nodenum[f]; - int edgenum = m->flex_edgenum[f]; - int vertnum = m->flex_vertnum[f]; - - if (dim == 1 || m->flex_rigid[f]) { - continue; - } - - // add bending forces to qfrc_spring - if (dim == 2) { - mjtNum* xpos = d->flexvert_xpos + 3*m->flex_vertadr[f]; - int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f]; - - for (int e = 0; e < edgenum; e++) { - const int* edge = m->flex_edge + 2*(e+m->flex_edgeadr[f]); - const int* flap = m->flex_edgeflap + 2*(e+m->flex_edgeadr[f]); - int v[4] = {edge[0], edge[1], flap[0], flap[1]}; - if (v[3] == -1) { - // skip boundary edges - continue; - } - - // flap edges - mjtNum ed[3][3]; - mji_sub3(ed[0], xpos + 3*v[1], xpos + 3*v[0]); - mji_sub3(ed[1], xpos + 3*v[2], xpos + 3*v[0]); - mji_sub3(ed[2], xpos + 3*v[3], xpos + 3*v[0]); - - // forces at the vertices due to curved reference - mjtNum frc[4][3]; - mji_cross(frc[1], ed[1], ed[2]); - mji_cross(frc[2], ed[2], ed[0]); - mji_cross(frc[3], ed[0], ed[1]); - frc[0][0] = -(frc[1][0] + frc[2][0] + frc[3][0]); - frc[0][1] = -(frc[1][1] + frc[2][1] + frc[3][1]); - frc[0][2] = -(frc[1][2] + frc[2][2] + frc[3][2]); - - // velocities - mjtNum* vel[4]; - for (int i = 0; i < 4; i++) { - vel[i] = d->qvel + m->body_dofadr[bodyid[v[i]]]; - } - - // force - mjtNum spring[12] = {0}; - mjtNum damper[12] = {0}; - for (int i = 0; i < 4; i++) { - for (int x = 0; x < 3; x++) { - for (int j = 0; j < 4; j++) { - // thin plate bending force - if (enbl_spring) spring[3*i+x] += b[17*e+4*i+j] * xpos[3*v[j]+x]; - - // thin plate damping force - // TODO: do not assume DOFs are in the world frame - if (enbl_damper) damper[3*i+x] += b[17*e+4*i+j] * vel[j][x]; - } - - // curved reference contribution - if (enbl_spring) spring[3*i+x] += b[17*e+16] * frc[i][x]; - } - } - - // insert into global force - for (int i = 0; i < 4; i++) { - int bid = bodyid[v[i]]; - int body_dofnum = m->body_dofnum[bid]; - int body_dofadr = m->body_dofadr[bid]; - for (int x = 0; x < body_dofnum; x++) { - if (enbl_spring) d->qfrc_spring[body_dofadr+x] -= spring[3*i+x]; - if (enbl_damper) d->qfrc_damper[body_dofadr+x] -= damper[3*i+x] * m->flex_damping[f]; - } - } - } - } - - if (k[0] == 0) { - continue; - } - - // skip interpolated flex with strain constraints (stiffness in constraint solver) - if (m->flex_edgeequality[f] == 3) { + if (m->flex_dim[f] == 1 || m->flex_rigid[f]) { continue; } if (m->flex_interp[f]) { - int order = m->flex_interp[f]; - int shell_mode = order < 0; - order = order < 0 ? -order : order; - int cx = m->flex_cellnum[3*f+0]; - int cy = m->flex_cellnum[3*f+1]; - int cz = m->flex_cellnum[3*f+2]; + // interpolated flex + mj_flexPassiveInterp(m, d, f, enbl_spring, enbl_damper); + } else { + // add bending forces + mj_flexPassiveBend(m, d, f, enbl_spring, enbl_damper); - // determine element type: 2D boundary quads (shell) or 3D cells (volume) - int npe; // nodes per element - int nelem_fe; // total finite elements - - if (shell_mode) { - npe = (order+1)*(order+1); - nelem_fe = 2*(cy*cz + cx*cz + cx*cy); - } else { - npe = (order+1)*(order+1)*(order+1); - nelem_fe = cx * cy * cz; - } - - mj_markStack(d); - - // allocate global arrays - mjtNum* xpos_g = mjSTACKALLOC(d, 3*nodenum, mjtNum); - mjtNum* vel_g = mjSTACKALLOC(d, 3*nodenum, mjtNum); - mjtNum* frc_g = mjSTACKALLOC(d, 3*nodenum, mjtNum); - mjtNum* dmp_g = mjSTACKALLOC(d, 3*nodenum, mjtNum); - mjtNum* xpos0 = m->flex_node0 + 3*m->flex_nodeadr[f]; - int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; - - // gather global node positions and velocities (unrotated) - mju_flexGatherState(m, d, f, xpos_g, vel_g); - - // zero global force accumulators - mju_zero(frc_g, 3*nodenum); - mju_zero(dmp_g, 3*nodenum); - - // per-element arrays (sized for npe) - mjtNum* xpos_e = mjSTACKALLOC(d, 3*npe, mjtNum); - mjtNum* vel_e = mjSTACKALLOC(d, 3*npe, mjtNum); - mjtNum* xpos0_e = mjSTACKALLOC(d, 3*npe, mjtNum); - mjtNum* displ_e = mjSTACKALLOC(d, 3*npe, mjtNum); - mjtNum* frc_e = mjSTACKALLOC(d, 3*npe, mjtNum); - mjtNum* dmp_e = mjSTACKALLOC(d, 3*npe, mjtNum); - int* gindices = mjSTACKALLOC(d, npe, int); - - // loop over finite elements - for (int fe = 0; fe < nelem_fe; fe++) { - // get element stiffness matrix - mjtNum* k_elem = k + fe * 3*npe * 3*npe; - - // skip empty elements (zero stiffness) - if (k_elem[0] == 0) { - continue; - } - - // gather element-local node data and compute corotational rotation - mjtNum quat[4]; - if (shell_mode) { - mju_flexGatherFaceState(order, cx, cy, cz, fe, xpos_g, vel_g, xpos0, - xpos_e, vel_e, xpos0_e, gindices, quat); - } else { - int ci = fe / (cy * cz); - int cj = (fe / cz) % cy; - int ck = fe % cz; - mju_flexGatherCellState(order, cy, cz, ci, cj, ck, xpos_g, vel_g, - xpos0, xpos_e, vel_e, xpos0_e, gindices, - quat); - } - - // rotate to corotational frame - for (int n = 0; n < npe; n++) { - mju_rotVecQuat(xpos_e+3*n, xpos_e+3*n, quat); - mju_rotVecQuat(vel_e+3*n, vel_e+3*n, quat); - } - - // compute displacement - for (int n = 0; n < npe; n++) { - mji_addScl3(displ_e+3*n, xpos_e+3*n, xpos0_e+3*n, -1); - } - - // compute force in corotational frame - if (enbl_spring) { - mju_mulMatVec(frc_e, k_elem, displ_e, 3*npe, 3*npe); - } - if (enbl_damper) { - mju_mulMatVec(dmp_e, k_elem, vel_e, 3*npe, 3*npe); - } - - // rotate back to global frame and scatter using node indices - mju_negQuat(quat, quat); - for (int n = 0; n < npe; n++) { - mjtNum qfrc[3], qdmp[3]; - mji_rotVecQuat(qfrc, frc_e+3*n, quat); - mji_rotVecQuat(qdmp, dmp_e+3*n, quat); - int gidx = gindices[n]; - if (enbl_spring) { - mji_addTo3(frc_g + 3*gidx, qfrc); - } - if (enbl_damper) { - mji_addTo3(dmp_g + 3*gidx, qdmp); - } - } - } - - // apply accumulated forces to bodies - for (int i = 0; i < nodenum; i++) { - mju_scl3(dmp_g+3*i, dmp_g+3*i, m->flex_damping[f]); - int bid = bodyid[i]; - int nidx = i + m->flex_nodeadr[f]; - - // fast path: node at body origin (not pinned), direct DOF write - if (m->body_dofnum[bid] > 0 && - (m->flex_centered[f] || - (m->flex_node[3*nidx+0] == 0 && - m->flex_node[3*nidx+1] == 0 && - m->flex_node[3*nidx+2] == 0))) { - if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bid], frc_g+3*i); - if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bid], dmp_g+3*i); - } else { - if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_spring); - if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_damper); - } - } - - mj_freeStack(d); - - // do not continue with the rest of the flex passive forces - continue; + // stretch forces + mj_flexPassiveStretch(m, d, f, enbl_spring, enbl_damper); } - - int nedge = (dim == 2) ? 3 : 6; - int nvert = (dim == 2) ? 3 : 4; - const int* elem = m->flex_elem + m->flex_elemdataadr[f]; - const int* edgeelem = m->flex_elemedge + m->flex_elemedgeadr[f]; - mjtNum* xpos = d->flexvert_xpos + 3*m->flex_vertadr[f]; - mjtNum* vel = d->flexedge_velocity + m->flex_edgeadr[f]; - mjtNum* deformed = d->flexedge_length + m->flex_edgeadr[f]; - mjtNum* reference = m->flexedge_length0 + m->flex_edgeadr[f]; - int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f]; - mjtNum kD = m->opt.timestep > 0 ? m->flex_damping[f] / m->opt.timestep : 0; - - mj_markStack(d); - mjtNum* qfrc = mjSTACKALLOC(d, 3*m->flex_vertnum[f], mjtNum); - mju_zero(qfrc, 3*m->flex_vertnum[f]); - - // compute force element-by-element - int elemnum = m->flex_elemnum[f]; - for (int t = 0; t < elemnum; t++) { - const int* vert = elem + (dim+1) * t; - - // compute length gradient with respect to dofs - mjtNum gradient[6][2][3]; - GradSquaredLengths(gradient, xpos, vert, edges[dim-2], nedge); - - // we add generalized Rayleigh damping as described in Section 5.2 of - // Kharevych et al., "Geometric, Variational Integrators for Computer - // Animation" http://multires.caltech.edu/pubs/DiscreteLagrangian.pdf - - // extract elongation of edges belonging to this element - mjtNum elongation[6]; - for (int e = 0; e < nedge; e++) { - int idx = edgeelem[t * nedge + e]; - mjtNum previous = deformed[idx] - vel[idx] * m->opt.timestep; - elongation[e] = deformed[idx]*deformed[idx] - reference[idx]*reference[idx] + - (deformed[idx]*deformed[idx] - previous*previous) * kD; - } - - // unpack triangular representation - mjtNum metric[36]; - int id = 0; - for (int ed1 = 0; ed1 < nedge; ed1++) { - for (int ed2 = ed1; ed2 < nedge; ed2++) { - metric[nedge*ed1 + ed2] = k[21*t + id]; - metric[nedge*ed2 + ed1] = k[21*t + id++]; - } - } - - // we now multiply the elongations by the precomputed metric tensor, - // notice that if metric=diag(1/reference) then this would yield a - // mass-spring model - - // compute local force - mjtNum force[12] = {0}; - for (int ed1 = 0; ed1 < nedge; ed1++) { - for (int ed2 = 0; ed2 < nedge; ed2++) { - for (int i = 0; i < 2; i++) { - for (int x = 0; x < 3; x++) { - force[3 * edges[dim-2][ed2][i] + x] -= - elongation[ed1] * gradient[ed2][i][x] * - metric[nedge * ed1 + ed2]; - } - } - } - } - - // insert into global force - for (int i = 0; i < nvert; i++) { - for (int x = 0; x < 3; x++) { - qfrc[3*vert[i]+x] += force[3*i+x]; - } - } - } - - // insert force into qfrc_passive, straightforward for simple bodies, - // need to distribute the force in case of pinned vertices - for (int v = 0; v < vertnum; v++) { - int bid = bodyid[v]; - if (m->body_simple[bid] != 2) { - // this should only occur for pinned flex vertices - mj_applyFT(m, d, qfrc + 3*v, 0, xpos + 3*v, bid, d->qfrc_spring); - } else { - int body_dofnum = m->body_dofnum[bid]; - int body_dofadr = m->body_dofadr[bid]; - for (int x = 0; x < body_dofnum; x++) { - d->qfrc_spring[body_dofadr+x] += qfrc[3*v+x]; - } - } - } - - mj_freeStack(d); } // flexedge-level spring-dampers From 22ca5fe0dfe4246c01584143819c26afa07310ad Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 30 Apr 2026 06:20:38 -0700 Subject: [PATCH 170/251] studio: improve behavior of help window PiperOrigin-RevId: 908132295 Change-Id: Iac0aead82a575dffbe35e3be8ba06af13986d1a2 --- src/experimental/platform/ux/gui.cc | 5 +++++ src/experimental/studio/app.cc | 31 ++++++++++++++++++++--------- 2 files changed, 27 insertions(+), 9 deletions(-) diff --git a/src/experimental/platform/ux/gui.cc b/src/experimental/platform/ux/gui.cc index b9fa91d2..b952688e 100644 --- a/src/experimental/platform/ux/gui.cc +++ b/src/experimental/platform/ux/gui.cc @@ -347,6 +347,11 @@ ImVec4 ConfigureDockingLayout() { ImGui::End(); } + ImGuiDockNode* central = ImGui::DockBuilderGetCentralNode(root); + if (central) { + return ImVec4(central->Pos.x, central->Pos.y, + central->Size.x, central->Size.y); + } const int settings_width = dockspace_size.x * kOptionsRelWidth; const int inspector_width = dockspace_size.x * kInspectorRelWidth; const float workspace_x = dockspace_pos.x + settings_width; diff --git a/src/experimental/studio/app.cc b/src/experimental/studio/app.cc index 576462a5..f3cf2f62 100644 --- a/src/experimental/studio/app.cc +++ b/src/experimental/studio/app.cc @@ -926,12 +926,15 @@ void App::BuildGui() { if (tmp_.help) { platform::ScopedStyle style; - style.Var(ImGuiStyleVar_Alpha, 0.6f); + style.Var(ImGuiStyleVar_Alpha, 0.8f); ImGui::SetNextWindowPos(ImVec2(workspace_rect.x, workspace_rect.y), - ImGuiCond_FirstUseEver); - ImGui::SetNextWindowSize(ImVec2(400, 0), ImGuiCond_FirstUseEver); - if (ImGui::Begin("Help", &tmp_.help)) { + ImGuiCond_Appearing); + ImGui::SetNextWindowSize(ImVec2(0, 0), ImGuiCond_Appearing); + if (ImGui::Begin("Help", &tmp_.help, + ImGuiWindowFlags_AlwaysAutoResize)) { + ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(2.0f, 5.0f)); HelpGui(); + ImGui::PopStyleVar(); } ImGui::End(); } @@ -1297,11 +1300,18 @@ void App::SpecEditorGui() { } void App::HelpGui() { + const float pad = ImGui::GetStyle().ItemSpacing.x; + const float indent = pad * 2; + const float col0 = ImGui::CalcTextSize("Toggle Visc Pause").x + pad; + const float col1 = ImGui::CalcTextSize("Ctrl+Spc").x + pad + indent; + const float col2 = ImGui::CalcTextSize("Center of Mass").x + pad + indent; + const float col3 = ImGui::CalcTextSize("M").x + pad + indent; + ImGui::Dummy(ImVec2(col0 + col1 + col2 + col3, 0)); ImGui::Columns(4); - ImGui::SetColumnWidth(0, ImGui::GetWindowWidth() * 0.35f); - ImGui::SetColumnWidth(1, ImGui::GetWindowWidth() * 0.15f); - ImGui::SetColumnWidth(2, ImGui::GetWindowWidth() * 0.4f); - ImGui::SetColumnWidth(3, ImGui::GetWindowWidth() * 0.1f); + ImGui::SetColumnWidth(0, col0); + ImGui::SetColumnWidth(1, col1); + ImGui::SetColumnWidth(2, col2); + ImGui::SetColumnWidth(3, col3); ImGui::Text("Help"); ImGui::Text("Stats"); @@ -1329,6 +1339,7 @@ void App::HelpGui() { ImGui::Text("Site Group"); ImGui::NextColumn(); + ImGui::Indent(indent); ImGui::Text("F1"); ImGui::Text("F2"); ImGui::Text("F6"); @@ -1342,7 +1353,7 @@ void App::HelpGui() { ImGui::Text("Bksp"); ImGui::Text("Tab"); ImGui::Text("Sh+Tab"); - ImGui::Text("="); + ImGui::Text("+"); ImGui::Text("-"); ImGui::Text("["); ImGui::Text("]"); @@ -1355,6 +1366,7 @@ void App::HelpGui() { ImGui::Text("Sh+0-5"); ImGui::NextColumn(); + ImGui::Indent(indent); ImGui::Text("Activation"); ImGui::Text("Auto Connect"); ImGui::Text("Body Tree"); @@ -1381,6 +1393,7 @@ void App::HelpGui() { ImGui::Text("Transparent"); ImGui::NextColumn(); + ImGui::Indent(indent); ImGui::Text(","); ImGui::Text("K"); ImGui::Text("`"); From 7d541233033c3afe99b813bbc7555ec86dc9d14e Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 30 Apr 2026 08:40:20 -0700 Subject: [PATCH 171/251] Make flex vert0 rotation-invariant for interpolated flexes. The `vert0` array, used for parametric vertex coordinates in flexes, is now computed in the flex's local, unrotated frame when using interpolation. This ensures that the parametric coordinates are independent of the flex's initial orientation. A new test confirms that `flex_vert0` is identical for an unrotated and a rotated flex grid. PiperOrigin-RevId: 908196464 Change-Id: I47d6bcc2bc5df581479d485480e0e947ec6d3ffd --- src/user/user_mesh.cc | 79 +++++++++++++++++++++++++----- src/user/user_objects.h | 4 +- test/user/user_flex_test.cc | 55 +++++++++++++++++++-- test/xml/xml_native_writer_test.cc | 5 +- 4 files changed, 126 insertions(+), 17 deletions(-) diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index c4012201..a1e6b3d2 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4363,7 +4363,8 @@ void mjCFlex::Compile(const mjVFS* vfs) { } // compute unrotated node positions for stiffness computation - std::vector nodexpos_local = ComputeUnrotatedNodePositions(nodexpos); + double R0[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; // identity by default + std::vector nodexpos_local = ComputeUnrotatedNodePositions(nodexpos, R0); // reorder tetrahedra so right-handed face orientation is outside // faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2) @@ -4623,16 +4624,54 @@ void mjCFlex::Compile(const mjVFS* vfs) { // compute bounding box coordinates vert0_.assign(3*nvert, 0); - const mjtNum* bvh = tree.Bvh().data(); - size[0] = bvh[3] - radius; - size[1] = bvh[4] - radius; - size[2] = bvh[5] - radius; - for (int j=0; j < nvert; j++) { - for (int k=0; k < 3; k++) { - if (size[k] > mjMINVAL) { - vert0_[3*j+k] = (vertxpos[3*j+k] - bvh[k]) / (2*size[k]) + 0.5; - } else { - vert0_[3*j+k] = 0.5; + + if (interpolated && nnode > 0) { + // for interpolated flex, compute vert0_ in the unrotated local frame + // to make parametric coordinates rotation-invariant + std::vector vertxpos_local(3*nvert); + for (int j = 0; j < nvert; j++) { + mjuu_mulvecmat(vertxpos_local.data()+3*j, vertxpos.data()+3*j, R0); + } + + // compute local-frame bounding box from unrotated node positions + double lo[3] = {1e30, 1e30, 1e30}; + double hi[3] = {-1e30, -1e30, -1e30}; + for (int i = 0; i < nnode; i++) { + for (int k = 0; k < 3; k++) { + lo[k] = std::min(lo[k], nodexpos_local[3*i+k]); + hi[k] = std::max(hi[k], nodexpos_local[3*i+k]); + } + } + + // set size from local bounding box + for (int k = 0; k < 3; k++) { + size[k] = (hi[k] - lo[k]) / 2; + } + + // normalize vertex positions within local bounding box + for (int j = 0; j < nvert; j++) { + for (int k = 0; k < 3; k++) { + double extent = hi[k] - lo[k]; + if (extent > mjMINVAL) { + vert0_[3*j+k] = (vertxpos_local[3*j+k] - lo[k]) / extent; + } else { + vert0_[3*j+k] = 0.5; + } + } + } + } else { + // non-interpolated: use BVH bounding box (original behavior) + const mjtNum* bvh = tree.Bvh().data(); + size[0] = bvh[3] - radius; + size[1] = bvh[4] - radius; + size[2] = bvh[5] - radius; + for (int j=0; j < nvert; j++) { + for (int k=0; k < 3; k++) { + if (size[k] > mjMINVAL) { + vert0_[3*j+k] = (vertxpos[3*j+k] - bvh[k]) / (2*size[k]) + 0.5; + } else { + vert0_[3*j+k] = 0.5; + } } } } @@ -4655,7 +4694,7 @@ void mjCFlex::Compile(const mjVFS* vfs) { // be computed from axis-aligned positions to preserve the diagonal Jacobian // assumption in ComputeLinearStiffness. std::vector mjCFlex::ComputeUnrotatedNodePositions( - const std::vector& nodexpos) const { + const std::vector& nodexpos, double* R0_out) const { std::vector nodexpos_local(3*nnode); if (interpolated && nnode > 0) { int ny_global = spec.cellcount[1] * spec.order + 1; @@ -4707,6 +4746,22 @@ std::vector mjCFlex::ComputeUnrotatedNodePositions( double lk = mjuu_normvec(R0+6, 3); (void)li; (void)lj; (void)lk; + // assert R0 is orthonormal (rows are the normalized edge vectors) + for (int a = 0; a < 3; a++) { + for (int b = a; b < 3; b++) { + double dot = mjuu_dot3(R0 + 3*a, R0 + 3*b); + double expected = (a == b) ? 1.0 : 0.0; + if (std::abs(dot - expected) > 1e-8) { + throw mjCError(this, "flex grid rotation R0 is not orthonormal"); + } + } + } + + // output R0 if requested + if (R0_out) { + mjuu_copyvec(R0_out, R0, 9); + } + // apply inverse rotation to each nodexpos to get local-frame positions for (int i = 0; i < nnode; i++) { const double* p = nodexpos.data() + 3*i; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index bb3bbed5..c8e63938 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1055,7 +1055,9 @@ class mjCFlex: public mjCFlex_, private mjsFlex { std::vector node0_; // node Cartesian positions // compute unrotated node positions for stiffness computation - std::vector ComputeUnrotatedNodePositions(const std::vector& nodexpos) const; + // optionally outputs the grid rotation matrix R0 (stored as rows) + std::vector ComputeUnrotatedNodePositions( + const std::vector& nodexpos, double* R0_out = nullptr) const; // stiffness caching std::string ComputeStiffnessCacheKey() const; diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index c4d10e10..47bae7c8 100644 --- a/test/user/user_flex_test.cc +++ b/test/user/user_flex_test.cc @@ -392,9 +392,9 @@ TEST_F(UserFlexTest, TrilinearInterpolation) { EXPECT_EQ(m1->nflexvert, m2->nflexvert); for (int i = 0; i < 3*m1->nflexvert; ++i) { - EXPECT_EQ(m1->flex_vert[i], d2->flexvert_xpos[i]); - EXPECT_EQ(m1->flex_vert0[i], m2->flex_vert0[i]); - EXPECT_EQ(d1->flexvert_xpos[i], d2->flexvert_xpos[i]); + EXPECT_NEAR(m1->flex_vert[i], d2->flexvert_xpos[i], 1e-7); + EXPECT_NEAR(m1->flex_vert0[i], m2->flex_vert0[i], 1e-7); + EXPECT_NEAR(d1->flexvert_xpos[i], d2->flexvert_xpos[i], 1e-7); } EXPECT_EQ(m1->nM, m2->nM); @@ -1350,6 +1350,55 @@ TEST_F(UserFlexTest, Dof2d) { mj_deleteData(d_full); } +TEST_F(UserFlexTest, Vert0RotationInvariant) { + // unrotated trilinear grid + static constexpr char xml_unrotated[] = R"( + + + + + + + + + + )"; + + // same grid rotated 45 degrees around Z via parent body quaternion + static constexpr char xml_rotated[] = R"( + + + + + + + + + + )"; + + std::array error; + mjModel* m1 = LoadModelFromString(xml_unrotated, error.data(), error.size()); + ASSERT_THAT(m1, NotNull()) << error.data(); + + mjModel* m2 = LoadModelFromString(xml_rotated, error.data(), error.size()); + ASSERT_THAT(m2, NotNull()) << error.data(); + + // same number of vertices + ASSERT_EQ(m1->nflexvert, m2->nflexvert); + + // vert0 must be identical regardless of rotation + for (int i = 0; i < 3 * m1->nflexvert; ++i) { + EXPECT_NEAR(m1->flex_vert0[i], m2->flex_vert0[i], 1e-10) + << "vert0 mismatch at index " << i; + } + + mj_deleteModel(m1); + mj_deleteModel(m2); +} + } // namespace } // namespace mujoco diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index baa613fa..9a70372f 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -1404,7 +1404,10 @@ std::vector GetWriteReadTestModels() { absl::StrContains(xml, "hfield_xml") || absl::StrContains(xml, "fromto_convex") || absl::StrContains(xml, "cube_skin") || - absl::StrContains(xml, "cube_3x3x3")) { + absl::StrContains(xml, "cube_3x3x3") || + // exclude files that fail since we do not save pinned flex nodes + absl::StrContains(xml, "gripper_trilinear") || + absl::StrContains(xml, "strain")) { continue; } models.push_back(xml); From 0399bfd058316a84177662276c0b0a424ff6e557 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Fri, 1 May 2026 00:17:41 -0700 Subject: [PATCH 172/251] Allow custom getProcAddress for glad resolution. PiperOrigin-RevId: 908587023 Change-Id: Idc4dcc5ee8d2d67f83486ca2ff4667f2b4b283eb --- src/render/classic/glad/glad.c | 15 +++++++++------ src/render/classic/glad/glad.h | 2 +- src/render/classic/glad/loader.cc | 2 +- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/render/classic/glad/glad.c b/src/render/classic/glad/glad.c index e570906d..41fe00ae 100644 --- a/src/render/classic/glad/glad.c +++ b/src/render/classic/glad/glad.c @@ -86,7 +86,7 @@ static PFNWGLGETPROCADDRESSPROC_PRIVATE mjGladGetProcAddressPtr; #endif static -int mjGlad_open_gl(void) { +int mjGlad_open_gl(void* get_proc_address) { #ifndef IS_UWP mjGlad_libGL = LoadLibraryW(L"opengl32.dll"); if(mjGlad_libGL != NULL) { @@ -164,8 +164,11 @@ static int mjGlad_dl_iterate_callback(struct dl_phdr_info* info, size_t size, vo return result; } -static int mjGlad_open_gl(void) { - mjGladGetProcAddressPtr = NULL; +static int mjGlad_open_gl(void* get_proc_address) { + mjGladGetProcAddressPtr = (PFNGLXGETPROCADDRESSPROC_PRIVATE)get_proc_address; + if (mjGladGetProcAddressPtr != NULL) { + return 1; + } // We try to load a GetProcAddress symbol that's already been loaded into the process first. // Since MuJoCo relies on user code to set up a working OpenGL environment, one of these symbols @@ -224,7 +227,7 @@ static int mjGlad_open_gl(void) { return mjGladGetProcAddressPtr ? 1 : 0; } #else -static int mjGlad_open_gl(void) { +static int mjGlad_open_gl(void* get_proc_address) { static const char *NAMES[] = { "../Frameworks/OpenGL.framework/OpenGL", "/Library/Frameworks/OpenGL.framework/OpenGL", @@ -1490,8 +1493,8 @@ static void mjGlad_find_coreGL(void) { } } -int mjGladLoadGLUnsafe(void) { - if (mjGlad_open_gl()) { +int mjGladLoadGLUnsafe(void* get_proc_address) { + if (mjGlad_open_gl(get_proc_address)) { mjGLVersion.major = 0; mjGLVersion.minor = 0; glGetString = (PFNGLGETSTRINGPROC)mjGlad_get_proc("glGetString"); if (glGetString == NULL) return 0; diff --git a/src/render/classic/glad/glad.h b/src/render/classic/glad/glad.h index 419968bc..44c700cb 100644 --- a/src/render/classic/glad/glad.h +++ b/src/render/classic/glad/glad.h @@ -114,7 +114,7 @@ GLAPI struct gladGLversionStruct mjGLVersion; GLAPI int mjGladLoadGL(void); -GLAPI int mjGladLoadGLUnsafe(void) MJGLAD_NOEXCEPT; +GLAPI int mjGladLoadGLUnsafe(void*) MJGLAD_NOEXCEPT; typedef unsigned int GLenum; typedef unsigned char GLboolean; diff --git a/src/render/classic/glad/loader.cc b/src/render/classic/glad/loader.cc index 5eb1638e..b43357c3 100644 --- a/src/render/classic/glad/loader.cc +++ b/src/render/classic/glad/loader.cc @@ -17,7 +17,7 @@ extern "C" { GLAPI int mjGladLoadGL() { - static const int glad_initialized = mjGladLoadGLUnsafe(); + static const int glad_initialized = mjGladLoadGLUnsafe(nullptr); return glad_initialized; } From ee3ac5b349295714e7c01a0b704de07de8aeb192 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Fri, 1 May 2026 00:17:56 -0700 Subject: [PATCH 173/251] Expose Renderable settings in mjrRenderableParams. PiperOrigin-RevId: 908587103 Change-Id: Ic0e19a7fb3535bf32782b85d2da0049ec4ac8ec7 --- .../filament/compat/scene_bridge.cc | 2 + .../filament/filament/renderable.cc | 46 +++++++++---------- .../filament/filament/renderable.h | 9 ---- .../filament/filament/scene_view.cc | 4 +- .../filament/render_context_filament.cc | 7 +++ .../filament/render_context_filament.h | 19 ++++++++ 6 files changed, 53 insertions(+), 34 deletions(-) diff --git a/src/experimental/filament/compat/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc index b8606f88..bc247649 100644 --- a/src/experimental/filament/compat/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -85,6 +85,8 @@ SceneBridge::SceneBridge(FilamentContext* ctx, const mjModel* model) : ctx_(ctx) { mjrSceneParams params; mjr_defaultSceneParams(¶ms); + params.layer_mask = mjCAT_ALL; + params.reflection_layer_mask = mjCAT_DYNAMIC | mjCAT_STATIC; scene_view_ = std::make_unique(ctx_, params); model_objects_ = std::make_unique(model, ctx_); diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 7d26c12c..279d718c 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -122,11 +122,11 @@ void Renderable::InitPartEntity(Part& part) { if (instances_[static_cast(draw_mode_)] != nullptr) { builder.material(0, instances_[static_cast(draw_mode_)]); } - builder.castShadows(cast_shadows_); - builder.receiveShadows(receive_shadows_); - builder.layerMask(0xff, layer_mask_); - builder.priority(priority_); - builder.blendOrder(0, blend_order_); + builder.castShadows(params_.cast_shadows); + builder.receiveShadows(params_.receive_shadows); + builder.layerMask(0xff, params_.layer_mask); + builder.priority(params_.priority); + builder.blendOrder(0, params_.blend_order); builder.screenSpaceContactShadows(true); builder.build(*GetEngine(), part.entity); @@ -264,62 +264,62 @@ void Renderable::SetDrawMode(mjrDrawMode mode) { } std::uint8_t Renderable::SetLayerMask(std::uint8_t mask) { - std::uint8_t prev = layer_mask_; - if (mask != layer_mask_) { - layer_mask_ = mask; + std::uint8_t prev = params_.layer_mask; + if (mask != params_.layer_mask) { + params_.layer_mask = mask; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); for (Part& part : parts_) { - rm.setLayerMask(rm.getInstance(part.entity), 0xff, layer_mask_); + rm.setLayerMask(rm.getInstance(part.entity), 0xff, params_.layer_mask); } } return prev; } std::uint8_t Renderable::SetPriority(std::uint8_t priority) { - std::uint8_t prev = priority_; - if (priority != priority_) { - priority_ = priority; + std::uint8_t prev = params_.priority; + if (priority != params_.priority) { + params_.priority = priority; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); for (Part& part : parts_) { - rm.setPriority(rm.getInstance(part.entity), priority_); + rm.setPriority(rm.getInstance(part.entity), params_.priority); } } return prev; } std::uint16_t Renderable::SetBlendOrder(std::uint16_t blend_order) { - std::uint16_t prev = blend_order_; - if (blend_order != blend_order_) { - blend_order_ = blend_order; + std::uint16_t prev = params_.blend_order; + if (blend_order != params_.blend_order) { + params_.blend_order = blend_order; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); for (Part& part : parts_) { - rm.setBlendOrderAt(rm.getInstance(part.entity), 0, blend_order_); + rm.setBlendOrderAt(rm.getInstance(part.entity), 0, params_.blend_order); } } return prev; } void Renderable::SetCastShadows(bool cast_shadows) { - if (cast_shadows_ != cast_shadows) { - cast_shadows_ = cast_shadows; + if (params_.cast_shadows != cast_shadows) { + params_.cast_shadows = cast_shadows; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); for (Part& part : parts_) { - rm.setCastShadows(rm.getInstance(part.entity), cast_shadows_); + rm.setCastShadows(rm.getInstance(part.entity), params_.cast_shadows); } } } void Renderable::SetReceiveShadows(bool receive_shadows) { - if (receive_shadows_ != receive_shadows) { - receive_shadows_ = receive_shadows; + if (params_.receive_shadows != receive_shadows) { + params_.receive_shadows = receive_shadows; filament::RenderableManager& rm = GetEngine()->getRenderableManager(); for (Part& part : parts_) { - rm.setReceiveShadows(rm.getInstance(part.entity), receive_shadows_); + rm.setReceiveShadows(rm.getInstance(part.entity), params_.receive_shadows); } } } diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 2f6f4d48..6d284e92 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -48,10 +48,6 @@ namespace mujoco { // of the Renderable. class Renderable : public mjrRenderable { public: - // Default filament values for priority and layer mask. - static constexpr std::uint8_t kDefaultPriority = 4; - static constexpr std::uint8_t kDefaultLayerMask = 0x01; - Renderable(FilamentContext* ctx, const mjrRenderableParams& params); ~Renderable() noexcept; @@ -155,12 +151,7 @@ class Renderable : public mjrRenderable { std::vector parts_; filament::math::mat4f transform_; GetTransformFn get_transform_fn_; - std::uint8_t priority_ = kDefaultPriority; - std::uint8_t layer_mask_ = kDefaultLayerMask; - std::uint16_t blend_order_ = 0; bool wireframe_ = false; - bool cast_shadows_ = true; - bool receive_shadows_ = true; }; } // namespace mujoco diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index c8fe57fa..594b842a 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -132,7 +132,7 @@ SceneView::SceneView(FilamentContext* ctx, const mjrSceneParams& params) view = engine->createView(); view->setScene(scene_); view->setCamera(camera_); - view->setVisibleLayers(0xff, mjCAT_ALL); + view->setVisibleLayers(0xff, params.layer_mask); } reflect_view_ = engine->createView(); @@ -140,7 +140,7 @@ SceneView::SceneView(FilamentContext* ctx, const mjrSceneParams& params) reflect_view_->setCamera(reflect_camera_); reflect_view_->setShadowingEnabled(false); reflect_view_->setPostProcessingEnabled(false); - reflect_view_->setVisibleLayers(0xff, mjCAT_DYNAMIC | mjCAT_STATIC); + reflect_view_->setVisibleLayers(0xff, params.reflection_layer_mask); // Disable post processing for the depth and segmentation views to preserve // the values. diff --git a/src/experimental/filament/render_context_filament.cc b/src/experimental/filament/render_context_filament.cc index 322118a1..dbe80ee1 100644 --- a/src/experimental/filament/render_context_filament.cc +++ b/src/experimental/filament/render_context_filament.cc @@ -67,6 +67,8 @@ void mjr_defaultSceneParams(mjrSceneParams* params) { params->enable_post_processing = true; params->enable_reflections = true; params->enable_shadows = true; + params->layer_mask = 0xff; + params->reflection_layer_mask = 0xff; } void mjr_defaultLightParams(mjrLightParams* params) { @@ -113,6 +115,11 @@ void mjr_defaultMaterialParams(mjrMaterialParams* params) { void mjr_defaultRenderableParams(mjrRenderableParams* params) { params->shading_model = mjSHADING_MODEL_SCENE_OBJECT; + params->cast_shadows = true; + params->receive_shadows = true; + params->layer_mask = 0x01; + params->priority = 4; + params->blend_order = 0; } void mjr_defaultRenderTargetConfig(mjrRenderTargetConfig* config) { diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index 0f5ee16a..0edf75c2 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -206,6 +206,20 @@ void mjr_defaultTextureConfig(mjrTextureConfig* config); struct mjrRenderableParams { // The shading model to use for the Renderable. mjrShadingModel shading_model; + // Whether or not the Renderable casts shadows. + mjtByte cast_shadows; + // Whether or not the Renderable receives shadows. + mjtByte receive_shadows; + // The layers to which the Renderable belongs. This mask is used in + // conjunction with the layer mask in the Scene to determine which + // Renderables to render. Defaults to 0xff. + uint8_t layer_mask; + // Controls the order in which the Renderable is drawn relative to other + // Renderables; defaults to 4. + uint8_t priority; + // Similar to priority, but provides finer-grained control for Renderables + // with transparency; defaults to 0. + uint16_t blend_order; }; // Initializes the mjrRenderableParams to default values. @@ -292,6 +306,11 @@ struct mjrSceneParams { mjtByte enable_reflections; // Whether or not to enable shadows; enabled by default. mjtByte enable_shadows; + // This mask, in conjunction with the layer mask in the Renderable, determines + // which Renderables to render within the Scene. + uint8_t layer_mask; + // The layer mask to use for reflections. + uint8_t reflection_layer_mask; }; // Initializes the mjrSceneParams to default values. From f7d31e06f0fb10766fb07f1c71c610442f75c6ba Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 1 May 2026 08:24:06 -0700 Subject: [PATCH 174/251] Refactor utility functions for block extraction. PiperOrigin-RevId: 908744392 Change-Id: I8d1323946870c90d5e37b1853697fb35d5291e11 --- src/engine/engine_util_sparse.c | 122 ++++++++++++++++++++------------ src/engine/engine_util_sparse.h | 16 ++++- 2 files changed, 89 insertions(+), 49 deletions(-) diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index cfed023b..b4a74b1a 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -1376,11 +1376,29 @@ void mju_sqrMatTDSparse_row(mjtNum* res, const mjtNum* mat, const mjtNum* matT, } -// block-diagonalize a dense matrix +// extract a single block of a dense matrix // res output matrix // mat input matrix // nc_mat number of columns in mat // nc_res number of columns in res +// nr number of rows in res +// perm_r reverse permutation of rows (res -> mat) +// perm_c reverse permutation of columns (res -> mat) +void mju_block(mjtNum* restrict res, const mjtNum* restrict mat, + int nc_mat, int nc_res, int nr, + const int* restrict perm_r, const int* restrict perm_c) { + for (int r = 0; r < nr; r++) { + mjtNum* res_r = res + r * nc_res; + const mjtNum* mat_r = mat + perm_r[r] * nc_mat; + mju_gather(res_r, mat_r, perm_c, nc_res); + } +} + +// block-diagonalize a dense matrix +// res target matrix +// mat source full matrix +// nc_mat number of columns in source matrix +// nc_res number of columns in the block-diagonal target matrix // nb number of blocks // perm_r reverse permutation of rows (res -> mat) // perm_c reverse permutation of columns (res -> mat) @@ -1393,16 +1411,54 @@ void mju_blockDiag(mjtNum* restrict res, const mjtNum* restrict mat, const int* restrict perm_r, const int* restrict perm_c, const int* restrict block_nr, const int* restrict block_nc, const int* restrict block_r, const int* restrict block_c) { - for (int b=0; b < nb; b++) { - int bnr = block_nr[b]; - int bnc = block_nc[b]; - const int* adr_r = perm_r + block_r[b]; - const int* adr_c = perm_c + block_c[b]; + for (int b = 0; b < nb; b++) { int adr = nc_res * block_r[b]; - for (int r = 0; r < bnr; r++) { - for (int c = 0; c < bnc; c++) { - res[adr++] = mat[nc_mat * adr_r[r] + adr_c[c]]; - } + mju_block(res + adr, mat, nc_mat, block_nc[b], block_nr[b], + perm_r + block_r[b], perm_c + block_c[b]); + } +} + + +// extract a single block of a sparse matrix +// res, res2 target matrix values +// res_rownnz non-zeros in each row of target matrix +// res_rowadr row address of each initial row in target matrix +// res_colind column indices for each extracted value (relative) +// mat, mat2 source matrix values +// rownnz non-zeros in each row of source matrix +// rowadr addresses within the source matrix values +// colind source matrix column indices +// nr number of rows to extract +// perm_r row permutation (maps local row to source row) +// perm_c column permutation (maps source col to local col) +// col_offset subtrahend to shift mapped absolute columns into relative block space +// res_offset rowadr starting offset for the extracted submatrix +void mju_blockSparse(mjtNum* restrict res, int* restrict res_rownnz, + int* restrict res_rowadr, int* restrict res_colind, + const mjtNum* restrict mat, const int* restrict rownnz, + const int* restrict rowadr, const int* restrict colind, + int nr, + const int* restrict perm_r, const int* restrict perm_c, + int col_offset, int res_offset, + mjtNum* restrict res2, const mjtNum* restrict mat2) { + for (int r = 0; r < nr; r++) { + int k = perm_r[r]; + int nnz = rownnz[k]; + res_rownnz[r] = nnz; + + int res_adr = (r == 0) ? res_offset : (res_rowadr[r-1] + res_rownnz[r-1]); + res_rowadr[r] = res_adr; + + int* res_colind_r = res_colind + (res_adr - res_offset); + int mat_adr = rowadr[k]; + const int* colind_k = colind + mat_adr; + for (int j = 0; j < nnz; j++) { + res_colind_r[j] = perm_c[colind_k[j]] - col_offset; + } + + mju_copy(res + (res_adr - res_offset), mat + mat_adr, nnz); + if (mat2 && res2) { + mju_copy(res2 + (res_adr - res_offset), mat2 + mat_adr, nnz); } } } @@ -1433,43 +1489,15 @@ void mju_blockDiagSparse(mjtNum* restrict res, int* restrict res_rownnz, const int* restrict perm_r, const int* restrict perm_c, const int* restrict block_r, const int* restrict block_c, mjtNum* restrict res2, const mjtNum* restrict mat2) { - int block = 0; - int col_offset = block_c[block]; - int row_next = block + 1 < nb ? block_r[block + 1] : nr; - for (int r=0; r < nr; r++) { - // row k in mat goes to row r in res - int k = perm_r[r]; + for (int b = 0; b < nb; b++) { + int nr_block = (b + 1 < nb ? block_r[b + 1] : nr) - block_r[b]; + int res_adr = (block_r[b] == 0) ? 0 : (res_rowadr[block_r[b] - 1] + res_rownnz[block_r[b] - 1]); - // rownnz - int nnz = rownnz[k]; - res_rownnz[r] = nnz; - - // rowadr - int res_adr = (r == 0) ? 0 : res_rowadr[r-1] + res_rownnz[r-1]; - res_rowadr[r] = res_adr; - - // colind - int* res_colind_r = res_colind + res_adr; - mjtNum* res_r = res + res_adr; - int mat_adr = rowadr[k]; - const int* colind_k = colind + mat_adr; - const mjtNum* mat_k = mat + mat_adr; - for (int j=0; j < nnz; j++) { - res_colind_r[j] = perm_c[colind_k[j]] - col_offset; - } - - // values (dense copy: partial order within block is guaranteed) - mju_copy(res_r, mat_k, nnz); - if (mat2 && res2) { - mju_copy(res2 + res_adr, mat2 + mat_adr, nnz); - } - - // end of block reached: update block counter, column offset, next row - if (r + 1 >= row_next && block + 1 < nb) { - block++; - col_offset = block_c[block]; - row_next = block + 1 < nb ? block_r[block + 1] : nr; - } + mju_blockSparse(res + res_adr, res_rownnz + block_r[b], + res_rowadr + block_r[b], res_colind + res_adr, + mat, rownnz, rowadr, colind, + nr_block, perm_r + block_r[b], perm_c, + block_c[b], res_adr, + res2 ? res2 + res_adr : NULL, mat2); } } - diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index 68bda070..3d5badaa 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -158,13 +158,25 @@ MJAPI void mju_sqrMatTDSparseNumeric( // precompute res_rowadr for mju_sqrMatTDSparse using uncompressed memory MJAPI void mju_sqrMatTDUncompressedInit(int* res_rowadr, int nc); +// extract a single block of a dense matrix +void mju_block(mjtNum* res, const mjtNum* mat, int nc_mat, int nc_res, int nr, + const int* perm_r, const int* perm_c); + // block-diagonalize a dense matrix -MJAPI void mju_blockDiag(mjtNum* res, const mjtNum* mat, - int nc_mat, int nc_res, int nb, +MJAPI void mju_blockDiag(mjtNum* res, const mjtNum* mat, int nc_mat, int nc_res, int nb, const int* perm_r, const int* perm_c, const int* block_nr, const int* block_nc, const int* block_r, const int* block_c); +// extract a single block of a sparse matrix +void mju_blockSparse( + mjtNum* res, int* res_rownnz, int* res_rowadr, int* res_colind, + const mjtNum* mat, const int* rownnz, const int* rowadr, const int* colind, + int nr, + const int* perm_r, const int* perm_c, + int col_offset, int res_offset, + mjtNum* res2, const mjtNum* mat2); + // block-diagonalize a sparse matrix MJAPI void mju_blockDiagSparse( mjtNum* res, int* res_rownnz, int* res_rowadr, int* res_colind, From 8287d9d1526ca475cfa51990362e39a0092c42e4 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 1 May 2026 08:26:21 -0700 Subject: [PATCH 175/251] Fix insidesite sensor for massless flex parent bodies. mjSENS_INSIDESITE uses xipos to test containment, but for massless flex parent bodies xipos equals the static body frame origin and does not track the actual flex position. Fix: in the INSIDESITE sensor case, when the object is a massless body with positive subtree mass (i.e., a flex parent), use subtree_com instead of xipos. This correctly reflects the mass-weighted centroid of the flex child bodies without changing the global semantics of xipos. PiperOrigin-RevId: 908745144 Change-Id: I527f0efb7b419345188411c9da883505156ebeea --- src/engine/engine_sensor.c | 9 +++++ test/engine/engine_sensor_test.cc | 57 +++++++++++++++++++++++++++++++ 2 files changed, 66 insertions(+) diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 11a07786..d6a2eb13 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -795,6 +795,15 @@ static void mj_computeSensorPos(const mjModel* m, mjData* d, int i, mjtNum* sens case mjSENS_INSIDESITE: // 1 if object is inside site get_xpos_xmat(d, objtype, objid, i, &xpos, &xmat); + + // for massless bodies with positive subtree mass (e.g., flex parents), + // xipos is the static body frame origin; use subtree_com instead + if (objtype == mjOBJ_BODY && objid > 0 && + m->body_mass[objid] < mjMINVAL && + m->body_subtreemass[objid] >= mjMINVAL) { + xpos = d->subtree_com + 3*objid; + } + sensordata[0] = mju_insideGeom(d->site_xpos + 3*refid, d->site_xmat + 9*refid, m->site_size + 3*refid, diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index 8b477f2c..01aced2e 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -1727,5 +1727,62 @@ TEST_F(SensorTest, TactileSkipTangents) { mj_deleteModel(model); } + +// insidesite uses subtree_com for massless flex parent bodies +TEST_F(SensorTest, InsideSiteFlexBody) { + static constexpr char xml[] = R"( + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d = mj_makeData(m); + + // flex is at origin, site is a large box at origin — should be inside + mj_forward(m, d); + EXPECT_EQ(d->sensordata[0], 1) + << "flex body should be inside the container site"; + + // shift all vertex/node bodies far outside the site via qpos + // each body has 3 slide joints (x, y, z); shift z by +10 + int parent_id = mj_name2id(m, mjOBJ_BODY, "parent"); + for (int b = parent_id + 1; b < m->nbody; b++) { + if (m->body_parentid[b] == parent_id) { + int jadr = m->body_jntadr[b]; + if (jadr >= 0 && m->body_jntnum[b] == 3) { + // z-slide is the 3rd joint + d->qpos[m->jnt_qposadr[jadr + 2]] = 10.0; + } + } + } + mj_forward(m, d); + + // subtree_com should now be far outside; sensor should read 0 + EXPECT_EQ(d->sensordata[0], 0) + << "flex body should be outside the container site after displacement"; + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco From 910b3336edc67cecfd256905690604ea47f15b75 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 1 May 2026 08:40:53 -0700 Subject: [PATCH 176/251] Restrict midpoint integration to unconstrained free bodies in implicitfast PiperOrigin-RevId: 908750768 Change-Id: I9a45a160ac757cc82bfe54871609956769988369 --- doc/changelog.rst | 5 + doc/computation/index.rst | 20 ++-- src/engine/engine_forward.c | 82 +++++++++++++--- test/engine/engine_derivative_test.cc | 74 -------------- test/engine/engine_forward_test.cc | 134 ++++++++++++++++++++++++++ 5 files changed, 221 insertions(+), 94 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 1f5e1fd1..eb2b05f2 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -8,6 +8,11 @@ Upcoming version (not yet released) - Added island support for the :ref:`PGS solver`. - Added support for :ref:`elastic2d` for trilinear and quadratic flex :ref:`dofs`. +- :ref:`Midpoint integration` is now restricted to the ``implicitfast`` + :ref:`integrator` and is disabled when fluid forces are active + (nonzero :ref:`density` or :ref:`viscosity`). + Midpoint integration treats external forces as zero-order-hold constants, which causes + energy gain in the presence of contacts and in fluid media. Python ^^^^^^ diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 000abcc4..8ef76cb8 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -575,7 +575,7 @@ Solving for :math:`v_{t+h}`, we obtain the implicit-in-velocity update .. _geMidpoint: -Midpoint integration for free bodies +Midpoint integration for free bodies in vacuum The implicit-in-velocity update :eq:`eq_implicit_update` treats the acceleration as a function of velocity and linearizes. While effective for damping-like forces, it is sub-optimal for rotational dynamics, where Coriolis and gyroscopic forces are *quadratic* in angular velocity. For this case, a better approach is to directly @@ -606,7 +606,9 @@ Midpoint integration for free bodies Casimir function of the `Lie-Poisson `__ structure, the midpoint method is a symmetric (time-reversible) and second-order accurate *Poisson integrator*. - **Eligibility.** Midpoint integration is only applied to free bodies with no child bodies. + **Eligibility.** Midpoint integration is only applied when using the ``implicitfast`` integrator, to + free bodies with no child bodies, and only when the medium has zero :ref:`density` and + :ref:`viscosity`. **Performance.** While the midpoint method carries computational overhead, we've found it to be negligible compared to the rest of the pipeline, on the order of 1% in the worst case. @@ -652,9 +654,8 @@ Fast implicit-in-velocity (``implicitfast``) scenarios which are not common and already well-handled by the Runge-Kutta integrator (see below). Because the RNE derivatives are also the main source of asymmetry of :math:`D`, by dropping them and symmetrizing, we can use the faster :math:`L^TL` rather than :math:`LU` decomposition. - -Both ``implicit`` and ``implicitfast`` apply :ref:`midpoint integration` to eligible free bodies, -providing exact energy conservation for spinning objects at negligible additional cost. + The ``implicitfast`` integrator applies :ref:`midpoint integration` to eligible free bodies in vacuum, + providing exact energy conservation for spinning objects at negligible additional cost. 4th-order Runge-Kutta (``RK4``) One advantage of our continuous-time formulation is that we can use higher order integrators such as Runge-Kutta or @@ -688,10 +689,11 @@ providing exact energy conservation for spinning objects at negligible additiona increased stability, and is therefore a strict improvement. It is the recommended integrator for most models. **implicit**: The benefit over ``implicitfast`` is the implicit integration of Coriolis and centripetal forces for *coupled* - rotational systems such as multi-link pendula. Both ``implicitfast`` and ``implicit`` apply :ref:`midpoint - integration` to eligible free bodies with no children, for example - `gyroscopic.xml <../_static/gyroscopic.xml>`__ shows an ellipsoid rolling on an - inclined plane; both ``implicitfast`` and ``implicit`` handle this case well, while ``Euler`` quickly diverges. + rotational systems such as multi-link pendula. Note that ``implicit`` does not apply :ref:`midpoint + integration` (only ``implicitfast`` does), but its RNE derivatives provide comparable stability + for free-body rotation. For example, `gyroscopic.xml <../_static/gyroscopic.xml>`__ shows an ellipsoid rolling + on an inclined plane; both ``implicitfast`` and ``implicit`` handle this case well, while ``Euler`` quickly + diverges. **RK4**: This integrator is best for systems which are energy conserving, or almost energy-conserving. `pendulum.xml <../_static/pendulum.xml>`__ shows a complicated pendulum mechanism which diverges quickly using ``Euler`` or diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 0df4c0f1..cae49ba9 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -1597,16 +1597,75 @@ static void flexInterp_solve(const mjModel* m, mjData* d, const FlexInterpContex // return 1 if free joint is eligible for midpoint quaternion integration: -// standalone 6-DOF tree with no children -static int midpoint_eligible(const mjModel* m, int jnt) { - if (m->jnt_type[jnt] == mjJNT_FREE) { - int body = m->jnt_bodyid[jnt]; - int treeid = m->dof_treeid[m->jnt_dofadr[jnt]]; - return m->tree_dofnum[treeid] == 6 && - m->body_subtreemass[body] == m->body_mass[body]; +// standalone 6-DOF tree with no children, awake, and unconstrained +static int midpoint_eligible(const mjModel* m, const mjData* d, int jnt) { + if (m->jnt_type[jnt] != mjJNT_FREE) { + return 0; } - return 0; + int body = m->jnt_bodyid[jnt]; + int adr = m->jnt_dofadr[jnt]; + int tree = m->dof_treeid[adr]; + + // must be standalone 6-DOF tree with no children + if (m->tree_dofnum[tree] != 6 || + m->body_subtreemass[body] != m->body_mass[body]) { + return 0; + } + + // must be awake + if (!d->tree_awake[tree]) { + return 0; + } + + // must be unconstrained + if (d->nefc) { + // islands enabled: O(1) lookup + if (!mjDISABLED(mjDSBL_ISLAND)) { + if (d->dof_island[adr] >= 0) { + return 0; + } + } + + // islands disabled: check if any constraint involves this tree + else { + for (int c=0; c < d->nefc; c++) { + int type = d->efc_type[c]; + int id = d->efc_id[c]; + + // contact: check if either geom belongs to this body + if (type == mjCNSTR_CONTACT_FRICTIONLESS || + type == mjCNSTR_CONTACT_PYRAMIDAL || + type == mjCNSTR_CONTACT_ELLIPTIC) { + int g1 = d->contact[id].geom[0]; + int g2 = d->contact[id].geom[1]; + if (g1 >= 0 && m->geom_bodyid[g1] == body) return 0; + if (g2 >= 0 && m->geom_bodyid[g2] == body) return 0; + } + + // connect or weld: check if either body is this body + else if (type == mjCNSTR_EQUALITY && + (m->eq_type[id] == mjEQ_CONNECT || m->eq_type[id] == mjEQ_WELD)) { + int b1 = m->eq_obj1id[id]; + int b2 = m->eq_obj2id[id]; + if (m->eq_objtype[id] == mjOBJ_SITE) { + b1 = m->site_bodyid[b1]; + b2 = m->site_bodyid[b2]; + } + if (b1 == body || b2 == body) return 0; + } + + // tendon limit or friction: check first two trees + else if (type == mjCNSTR_LIMIT_TENDON || type == mjCNSTR_FRICTION_TENDON) { + if (m->tendon_treeid[2*id] == tree || + m->tendon_treeid[2*id+1] == tree) return 0; + } + } + } + } + + // otherwise eligible + return 1; } @@ -1980,11 +2039,12 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { // count and list joints of free bodies eligible for midpoint integration int nfree = 0; int* free_jntid = NULL; - if (!mjENABLED(mjENBL_INVDISCRETE)) { + if (!mjENABLED(mjENBL_INVDISCRETE) && + m->opt.integrator == mjINT_IMPLICITFAST && + m->opt.density == 0 && m->opt.viscosity == 0) { free_jntid = mjSTACKALLOC(d, m->njnt, int); for (int j=0; j < m->njnt; j++) { - // add to list if eligible and awake - if (midpoint_eligible(m, j) && d->tree_awake[m->dof_treeid[m->jnt_dofadr[j]]]) { + if (midpoint_eligible(m, d, j)) { free_jntid[nfree++] = j; } } diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index e0b693d3..0e5c1849 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -1745,79 +1745,5 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) { mj_deleteModel(model); } -TEST_F(DerivativeTest, MidpointFluidAccuracy) { - const std::string xml_path = - GetTestDataFilePath(kTumblingThinObjectEllipsoidPath); - char error[1024]; - mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); - ASSERT_THAT(m, NotNull()) << error; - - mjtNum dt_small = 1e-4; - mjtNum dt_large = m->opt.timestep; // 2e-3, the default - mjtNum duration = 0.5; - - mjData* d_ref = mj_makeData(m); - mjData* d_midpoint = mj_makeData(m); - mjData* d_nomidpoint = mj_makeData(m); - - // give initial angular velocity for tumbling - mj_resetData(m, d_ref); - mj_resetData(m, d_midpoint); - mj_resetData(m, d_nomidpoint); - d_ref->qvel[3] = 5; - d_ref->qvel[4] = 3; - d_ref->qvel[5] = 1; - d_midpoint->qvel[3] = 5; - d_midpoint->qvel[4] = 3; - d_midpoint->qvel[5] = 1; - d_nomidpoint->qvel[3] = 5; - d_nomidpoint->qvel[4] = 3; - d_nomidpoint->qvel[5] = 1; - - int nsteps_large = static_cast(duration / dt_large); - int substeps = static_cast(dt_large / dt_small); - - mjtNum error_midpoint = 0; - mjtNum error_nomidpoint = 0; - - for (int i = 0; i < nsteps_large; i++) { - // reference: RK4 at small timestep - m->opt.integrator = mjINT_RK4; - m->opt.timestep = dt_small; - m->opt.enableflags &= ~mjENBL_INVDISCRETE; - for (int j = 0; j < substeps; j++) { - mj_step(m, d_ref); - } - - // implicit with midpoint (default) - m->opt.integrator = mjINT_IMPLICIT; - m->opt.timestep = dt_large; - m->opt.enableflags &= ~mjENBL_INVDISCRETE; - mj_step(m, d_midpoint); - - // implicit without midpoint - m->opt.enableflags |= mjENBL_INVDISCRETE; - mj_step(m, d_nomidpoint); - - // accumulate position errors - for (int k = 0; k < 7; k++) { - mjtNum diff_mid = d_ref->qpos[k] - d_midpoint->qpos[k]; - mjtNum diff_nomid = d_ref->qpos[k] - d_nomidpoint->qpos[k]; - error_midpoint += diff_mid * diff_mid; - error_nomidpoint += diff_nomid * diff_nomid; - } - } - - // expect midpoint to be more accurate - EXPECT_LT(error_midpoint, error_nomidpoint) - << "implicit midpoint should be more accurate than implicit without " - << "midpoint for a free body with fluid forces"; - - mj_deleteData(d_nomidpoint); - mj_deleteData(d_midpoint); - mj_deleteData(d_ref); - mj_deleteModel(m); -} - } // namespace } // namespace mujoco diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 3e36934f..dddf612b 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -783,6 +783,140 @@ TEST_F(ImplicitIntegratorTest, MidpointFullNewtonConvergence) { EXPECT_LT((mjtNum)total_iter / ncases, 3.0); } +// verify midpoint eligibility: compare with/without invdiscrete +// if trajectories differ, midpoint was applied +// if trajectories match, midpoint was skipped +TEST_F(ImplicitIntegratorTest, MidpointEligibility) { + // free body with asymmetric inertia, optionally near a plane + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + + char error[1024]; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + mjData* d1 = mj_makeData(m); + mjData* d2 = mj_makeData(m); + int nsteps = 50; + + auto spin_and_compare = [&](const char* label, + bool expect_midpoint) { + mj_resetData(m, d1); + mj_resetData(m, d2); + d1->qvel[3] = d2->qvel[3] = 5; + d1->qvel[4] = d2->qvel[4] = 3; + d1->qvel[5] = d2->qvel[5] = 1; + + // d1: midpoint enabled (default) + m->opt.enableflags &= ~mjENBL_INVDISCRETE; + for (int i = 0; i < nsteps; i++) mj_step(m, d1); + + // d2: midpoint disabled + m->opt.enableflags |= mjENBL_INVDISCRETE; + mj_resetData(m, d2); + d2->qvel[3] = 5; d2->qvel[4] = 3; d2->qvel[5] = 1; + for (int i = 0; i < nsteps; i++) mj_step(m, d2); + m->opt.enableflags &= ~mjENBL_INVDISCRETE; + + // compare angular velocities + mjtNum diff = 0; + for (int k = 3; k < 6; k++) { + mjtNum d = d1->qvel[k] - d2->qvel[k]; + diff += d * d; + } + if (expect_midpoint) { + EXPECT_GT(diff, 1e-6) + << label << ": expected midpoint to be applied"; + } else { + EXPECT_LT(diff, 1e-20) + << label << ": expected midpoint to be skipped"; + } + }; + + // case 1: free body in vacuum, implicitfast -> midpoint applied + m->opt.integrator = mjINT_IMPLICITFAST; + m->opt.density = 0; + m->opt.viscosity = 0; + spin_and_compare("vacuum+implicitfast", true); + + // case 2: implicit integrator -> midpoint NOT applied + m->opt.integrator = mjINT_IMPLICIT; + spin_and_compare("vacuum+implicit", false); + + // case 3: fluid (nonzero density) -> midpoint NOT applied + m->opt.integrator = mjINT_IMPLICITFAST; + m->opt.density = 1.2; + spin_and_compare("fluid+implicitfast", false); + m->opt.density = 0; + + // case 4: fluid (nonzero viscosity) -> midpoint NOT applied + m->opt.viscosity = 0.001; + spin_and_compare("viscosity+implicitfast", false); + m->opt.viscosity = 0; + + // case 5: body with active contacts -> midpoint NOT applied + // test both island-enabled and island-disabled branches + for (int disable_island = 0; disable_island < 2; disable_island++) { + m->opt.integrator = mjINT_IMPLICITFAST; + if (disable_island) { + m->opt.disableflags |= mjDSBL_ISLAND; + } else { + m->opt.disableflags &= ~mjDSBL_ISLAND; + } + + mj_resetData(m, d1); + mj_resetData(m, d2); + d1->qpos[2] = d2->qpos[2] = 0.05; + d1->qvel[3] = d2->qvel[3] = 5; + d1->qvel[4] = d2->qvel[4] = 3; + d1->qvel[5] = d2->qvel[5] = 1; + + // verify contacts are active + mj_forward(m, d1); + ASSERT_GT(d1->ncon, 0) << "body should be in contact with the plane"; + + // single step with midpoint enabled + mj_resetData(m, d1); + d1->qpos[2] = 0.05; + d1->qvel[3] = 5; d1->qvel[4] = 3; d1->qvel[5] = 1; + m->opt.enableflags &= ~mjENBL_INVDISCRETE; + mj_step(m, d1); + + // single step with midpoint disabled + mj_resetData(m, d2); + d2->qpos[2] = 0.05; + d2->qvel[3] = 5; d2->qvel[4] = 3; d2->qvel[5] = 1; + m->opt.enableflags |= mjENBL_INVDISCRETE; + mj_step(m, d2); + m->opt.enableflags &= ~mjENBL_INVDISCRETE; + + mjtNum diff = 0; + for (int k = 0; k < m->nv; k++) { + mjtNum d = d1->qvel[k] - d2->qvel[k]; + diff += d * d; + } + EXPECT_LT(diff, 1e-20) + << "contact (island " << (disable_island ? "disabled" : "enabled") + << "): expected midpoint to be skipped"; + } + m->opt.disableflags &= ~mjDSBL_ISLAND; + + mj_deleteData(d2); + mj_deleteData(d1); + mj_deleteModel(m); +} + TEST_F(ForwardTest, ControlClamping) { static constexpr char xml[] = R"( From 723b8b1ea60237c6ce2ef4a47f19050288bdcd7d Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Fri, 1 May 2026 09:33:43 -0700 Subject: [PATCH 177/251] Introduce explicit VFS in Python MjVfs This change introduces MjVFS, an explicit object to mirror the C mjVFS. This is meant to replace the MjSpec.assets dict. This latter while convenient guides users towards harmful authoring patterns with respect to data duplication and spec attachment workflows. Making VFS management explicit should encourage better memory usage and allow us to make better compile time optimizations. From this change, `spec.assets` is deprecated. However we will temporarily support backwards compatibility due to the wide spread usage. An error will be thrown if calling code tries to use a spec that uses both the assets dict and the new MjVfs. PiperOrigin-RevId: 908772050 Change-Id: I77c6d0369307fc300c954768fed17401261e18da --- doc/changelog.rst | 13 ++ doc/python.rst | 58 ++++++- python/mujoco/specs.cc | 109 ++++++++++++- python/mujoco/specs_wrapper.cc | 60 +++---- python/mujoco/specs_wrapper.h | 2 +- python/mujoco/structs.cc | 18 +-- python/mujoco/structs.h | 12 +- python/mujoco/structs_wrappers.cc | 108 ++++++++++--- python/mujoco/vfs.h | 46 ++++++ python/mujoco/vfs_test.py | 251 ++++++++++++++++++++++++++++++ 10 files changed, 599 insertions(+), 78 deletions(-) create mode 100644 python/mujoco/vfs.h create mode 100644 python/mujoco/vfs_test.py diff --git a/doc/changelog.rst b/doc/changelog.rst index eb2b05f2..26f1f591 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -55,6 +55,19 @@ Bug fixes than the parent spec. This prevents the origin of the parent spec to affect the resolution of asset paths in the child spec. +Python +^^^^^^ + +- Added ``mujoco.MjVfs`` Python binding to interact with the Virtual File System directly from Python. + See :ref:`Virtual File System ` for usage details. + + .. warning:: + The previous way of passing assets via a dictionary mapping asset names to bytes is **deprecated** and will be + removed in an upcoming release. You cannot specify both the ``assets`` dictionary and the ``vfs`` argument at the same + time. ``MjVfs`` should be used as a drop-in replacement. + + + Version 3.7.0 (April 14, 2026) ------------------------------ diff --git a/doc/python.rst b/doc/python.rst index 95beb3ff..963d366d 100644 --- a/doc/python.rst +++ b/doc/python.rst @@ -275,6 +275,8 @@ that create a new :ref:`mjModel` instance: ``mujoco.MjModel.from_xml_string``, ` functions accept the path to either an XML or MJB model file. All three functions optionally accept a Python dictionary which is converted into a MuJoCo :ref:`Virtualfilesystem` for use during model compilation. + + .. _PyFunctions: Functions @@ -514,11 +516,53 @@ The ``MjSpec`` object wraps the :ref:`mjSpec` struct and can be constructed in t Note the ``from_string()`` and ``from_file()`` methods can only be called at construction time. +.. _PyVFS: + Assets ^^^^^^ -All three methods take in an optional argument called ``assets`` which is used to resolve asset references in the XML. -This argument is a dictionary that maps asset name (string) to asset data (bytes), as demonstrated below: +MuJoCo optionally uses a :ref:`Virtual File System ` (VFS) to load assets (like meshes and textures) +from memory. Some :ref:`decoders` may also choose to leverage the VFS as a way to load assets on +demand, such as when addressing files in an archive format. This requires the same VFS to be used when parsing and +compiling a spec (and all attached specs) into a model. + +The Python bindings provide the ``mujoco.MjVfs`` as a wrapper around the :ref:`mjVFS` C struct. + +``MjVfs`` supports the context manager protocol, which ensures that resources are properly freed when leaving the block: + +.. code-block:: python + + with mujoco.MjVfs() as vfs: + vfs["model.xml"] = b"" + spec = mujoco.MjSpec.from_string("model.xml", vfs=vfs) + spec.compile(vfs=vfs) + +You can also create an instance directly and call ``close()`` when done: + +.. code-block:: python + + vfs = mujoco.MjVfs() + vfs["model.xml"] = some_xml_string.encode("utf-8") + spec = mujoco.MjSpec.from_file("model.xml", vfs=vfs) + spec.compile(vfs=vfs) + vfs.close() + +The ``MjVfs`` object supports dictionary-like operations to manage buffers: + +- ``vfs["name"] = data``: Adds a buffer to the VFS. ``data`` must be of type ``bytes``. +- ``del vfs["name"]``: Deletes a file from the VFS. +- ``"name" in vfs``: Checks if a file exists in the VFS. + +The static factory functions ``mujoco.MjModel.from_xml_string``, ``mujoco.MjModel.from_xml_path``, +``mujoco.MjSpec.from_string`` and ``mujoco.MjSpec.from_file`` accept an optional ``vfs`` argument. Additionally, the +``spec.compile()`` function also accepts an optional ``vfs`` argument. + +.. warning:: + The previous way of passing assets via a dictionary mapping asset names to bytes is **deprecated** and will be + removed in the next release. You cannot specify both the ``assets`` dictionary and the ``vfs`` argument at the same + time. ``MjVfs`` should be used as a drop-in replacement. + +For reference, the deprecated ``assets`` dictionary approach looked like this: .. code-block:: python @@ -526,6 +570,12 @@ This argument is a dictionary that maps asset name (string) to asset data (bytes spec = mujoco.MjSpec.from_string(xml_referencing_image_png, assets=assets) model = spec.compile() + # Or + + spec = mujoco.MjSpec.from_string(xml_referencing_image_png) + spec.assets = {'image.png': b'image_data'} + model = spec.compile() + Save to XML ----------- @@ -566,8 +616,8 @@ It is possible to combine multiple specs by using attachments. The following opt the reference to a frame, which is the attached worldbody transformed into a frame. The site must belong to the child spec. Prefix and suffix can also be specified as keyword arguments. - Attach a child spec to a frame in the parent spec: ``parent_spec.attach(child_spec, frame=frame_name_or_obj)``, - returns the reference to a frame, which is the attached worldbody transformed into a frame. The frame must belong to - the child spec. Prefix and suffix can also be specified as keyword arguments. + returns the reference to a frame, which is the attached worldbody transformed into a frame. The frame must + belong to the child spec. Prefix and suffix can also be specified as keyword arguments. The default behavior of attaching is to not copy, so all the child references (except for the worldbody) are still valid in the parent and therefore modifying the child will modify the parent. This is not true for the attach diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index 2ba20431..c5a60b52 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -18,6 +18,7 @@ #include #include #include +#include #include #include // IWYU pragma: keep #include @@ -33,6 +34,7 @@ #include "specs_wrapper.h" // IWYU pragma: keep #include "raw.h" #include "structs.h" // IWYU pragma: keep +#include "vfs.h" #include #include #include @@ -264,6 +266,49 @@ PYBIND11_MODULE(_specs, m) { DefineArray(m, "MjFloatVec"); DefineArray(m, "MjIntVec"); + // ============================= MJVFS ===================================== + py::class_(m, "MjVfs") + .def(py::init<>()) + .def("close", &MjVfs::Close) + .def("__enter__", [](MjVfs& self) -> MjVfs& { return self; }) + .def("__exit__", + [](MjVfs& self, py::object, py::object, py::object) { + self.Close(); + }) + .def("__setitem__", + [](MjVfs& self, const std::string& name, py::bytes data) { + if (!self.is_open()) { + throw std::runtime_error("VFS is closed"); + } + std::string_view buffer = data; + const int err = mj_addBufferVFS( + self.get(), name.c_str(), buffer.data(), buffer.size()); + if (err == 2) { + throw py::value_error( + "Repeated file name in VFS: " + name); + } else if (err) { + throw py::value_error( + "Failed to add buffer to VFS: " + name); + } + }) + .def("__delitem__", + [](MjVfs& self, const std::string& name) { + if (!self.is_open()) { + throw std::runtime_error("VFS is closed"); + } + if (mj_deleteFileVFS(self.get(), name.c_str())) { + throw py::key_error(name); + } + }) + .def("__contains__", + [](MjVfs& self, const std::string& name) { + if (!self.is_open()) { + throw std::runtime_error("VFS is closed"); + } + return mj_containsBufferVFS(self.get(), name.c_str()) == 1; + }); + + // ============================= MJSPEC ===================================== mjSpec.def(py::init<>()); mjSpec.def_property_readonly( @@ -272,7 +317,26 @@ PYBIND11_MODULE(_specs, m) { "from_file", [](std::string& filename, std::optional>& include, - std::optional& assets) -> MjSpec { + std::optional& assets, + MjVfs* vfs) -> MjSpec { + if (vfs && (include.has_value() || assets.has_value())) { + throw py::value_error( + "Cannot specify both 'vfs' and 'include'/'assets'."); + } + if (vfs) { + raw::MjSpec* spec; + { + py::gil_scoped_release no_gil; + char error[1024]; + spec = InterceptMjErrors(mj_parse)( + filename.c_str(), nullptr, vfs->get(), + error, sizeof(error)); + if (!spec) { + throw py::value_error(error); + } + } + return MjSpec(spec); + } const auto files = _impl::ConvertAssetsDict(include); raw::MjSpec* spec; { @@ -294,7 +358,8 @@ PYBIND11_MODULE(_specs, m) { return MjSpec(spec); }, py::arg("filename"), py::arg("include") = py::none(), - py::arg("assets") = py::none(), R"mydelimiter( + py::arg("assets") = py::none(), py::arg("vfs") = py::none(), + R"mydelimiter( Creates a spec from an XML file. Parameters @@ -307,13 +372,34 @@ PYBIND11_MODULE(_specs, m) { assets : dict, optional A dictionary of assets to be used by the spec. The keys are asset names and the values are asset contents. + vfs : MjVfs, optional + A VFS to use for resolving includes and assets. Cannot be used with + include or assets. )mydelimiter", py::return_value_policy::move); mjSpec.def_static( "from_string", [](std::string& xml, std::optional>& include, - std::optional& assets) -> MjSpec { + std::optional& assets, + MjVfs* vfs) -> MjSpec { + if (vfs && (include.has_value() || assets.has_value())) { + throw py::value_error( + "Cannot specify both 'vfs' and 'include'/'assets'."); + } + if (vfs) { + raw::MjSpec* spec; + { + py::gil_scoped_release no_gil; + char error[1024]; + spec = InterceptMjErrors(mj_parseXMLString)( + xml.c_str(), vfs->get(), error, sizeof(error)); + if (!spec) { + throw py::value_error(error); + } + } + return MjSpec(spec); + } auto files = _impl::ConvertAssetsDict(include); raw::MjSpec* spec; { @@ -345,7 +431,8 @@ PYBIND11_MODULE(_specs, m) { return MjSpec(spec); }, py::arg("xml"), py::arg("include") = py::none(), - py::arg("assets") = py::none(), R"mydelimiter( + py::arg("assets") = py::none(), py::arg("vfs") = py::none(), + R"mydelimiter( Creates a spec from an XML string. Parameters @@ -358,6 +445,9 @@ PYBIND11_MODULE(_specs, m) { assets : dict, optional A dictionary of assets to be used by the spec. The keys are asset names and the values are asset contents. + vfs : MjVfs, optional + A VFS to use for resolving includes and assets. Cannot be used with + include or assets. )mydelimiter", py::return_value_policy::move); mjSpec.def("recompile", [mjmodel_mjdata_from_spec_ptr]( @@ -391,9 +481,14 @@ PYBIND11_MODULE(_specs, m) { return mjs_findDefault(self.ptr, classname.c_str()); }, py::return_value_policy::reference_internal); - mjSpec.def("compile", [mjmodel_from_raw_ptr](MjSpec& self) -> py::object { - return mjmodel_from_raw_ptr(reinterpret_cast(self.Compile())); - }); + mjSpec.def("compile", + [mjmodel_from_raw_ptr](MjSpec& self, + std::optional vfs) -> py::object { + mjVFS* vfs_ptr = vfs.has_value() ? (*vfs)->get() : nullptr; + return mjmodel_from_raw_ptr( + reinterpret_cast(self.Compile(vfs_ptr))); + }, + py::arg("vfs") = py::none()); mjSpec.def_property( "assets", [](MjSpec& self) -> py::dict { diff --git a/python/mujoco/specs_wrapper.cc b/python/mujoco/specs_wrapper.cc index f1ba1188..31d52e41 100644 --- a/python/mujoco/specs_wrapper.cc +++ b/python/mujoco/specs_wrapper.cc @@ -15,6 +15,7 @@ #include "specs_wrapper.h" #include // IWYU pragma: keep +#include #include #include // IWYU pragma: keep #include // IWYU pragma: keep @@ -87,44 +88,45 @@ MjSpec& MjSpec::operator=(MjSpec&& other) { MjSpec::~MjSpec() { mj_deleteSpec(ptr); } -raw::MjModel* MjSpec::Compile() { - if (assets.empty()) { - raw::MjModel* m; - { - // Release GIL before calling mj_compile which may spawn threads - py::gil_scoped_release no_gil; - m = mj_compile(ptr, 0); - } - if (!m || mjs_isWarning(ptr)) { - throw py::value_error(mjs_getError(ptr)); - } - return m; +raw::MjModel* MjSpec::Compile(mjVFS* vfs) { + if (vfs != nullptr && !assets.empty()) { + throw py::value_error("Cannot specify both 'vfs' and 'assets'."); } - mjVFS vfs; - mj_defaultVFS(&vfs); - for (const auto& asset : assets) { - std::string buffer_name = py::cast(asset.first).c_str(); - std::string buffer = py::cast(asset.second); - const int vfs_error = InterceptMjErrors(mj_addBufferVFS)( - &vfs, buffer_name.c_str(), buffer.c_str(), buffer.size()); - if (vfs_error) { - mj_deleteVFS(&vfs); - if (vfs_error == 2) { - throw py::value_error("Repeated file name in assets dict: " + - buffer_name); - } else { - throw py::value_error("Asset failed to load: " + buffer_name); + + std::optional local_vfs; + if (vfs == nullptr) { + vfs = &local_vfs.emplace(); + mj_defaultVFS(vfs); + + for (const auto& asset : assets) { + std::string buffer_name = py::cast(asset.first).c_str(); + std::string buffer = py::cast(asset.second); + const int vfs_error = InterceptMjErrors(mj_addBufferVFS)( + vfs, buffer_name.c_str(), buffer.c_str(), buffer.size()); + if (vfs_error) { + mj_deleteVFS(vfs); + if (vfs_error == 2) { + throw py::value_error("Repeated file name in assets dict: " + + buffer_name); + } else { + throw py::value_error("Asset failed to load: " + buffer_name); + } } } } raw::MjModel* m; { - // Release GIL before calling mj_compile which may spawn threads py::gil_scoped_release no_gil; - m = mj_compile(ptr, &vfs); + m = mj_compile(ptr, vfs); } - mj_deleteVFS(&vfs); + + if (local_vfs.has_value()) { + // vfs points at local_vfs value. + mj_deleteVFS(vfs); + local_vfs = std::nullopt; + } + if (!m || mjs_isWarning(ptr)) { throw py::value_error(mjs_getError(ptr)); } diff --git a/python/mujoco/specs_wrapper.h b/python/mujoco/specs_wrapper.h index 2ebaac60..66c1adab 100644 --- a/python/mujoco/specs_wrapper.h +++ b/python/mujoco/specs_wrapper.h @@ -35,7 +35,7 @@ struct MjSpec { MjSpec& operator=(MjSpec&& other); ~MjSpec(); - raw::MjModel* Compile(); + raw::MjModel* Compile(mjVFS* vfs = nullptr); raw::MjSpec* ptr; py::dict assets; diff --git a/python/mujoco/structs.cc b/python/mujoco/structs.cc index 1c9e04d9..79840742 100644 --- a/python/mujoco/structs.cc +++ b/python/mujoco/structs.cc @@ -41,6 +41,7 @@ #include #include #include +#include "vfs.h" namespace mujoco::python::_impl { @@ -301,24 +302,21 @@ PYBIND11_MODULE(_structs, m) { // ==================== MJMODEL ============================================== py::class_ mjModel(m, "MjModel"); mjModel.def_static( - "from_xml_string", &MjModelWrapper::LoadXML, py::arg("xml"), - py::arg_v("assets", py::none()), + "from_xml_string", MjModelWrapper::LoadXML, py::arg("xml"), + py::arg_v("assets", py::none()), py::arg("vfs") = py::none(), py::doc( - R"(Loads an MjModel from an XML string and an optional assets dictionary.)")); + R"(Loads an MjModel from an XML string and optional assets dict or VFS.)")); mjModel.def_static("_from_model_ptr", [](uintptr_t addr) { return MjModelWrapper::WrapRawModel(reinterpret_cast(addr)); }); mjModel.def_static( - "from_xml_path", &MjModelWrapper::LoadXMLFile, py::arg("filename"), - py::arg_v("assets", py::none()), + "from_xml_path", MjModelWrapper::LoadXMLFile, py::arg("filename"), + py::arg_v("assets", py::none()), py::arg("vfs") = py::none(), py::doc( - R"(Loads an MjModel from an XML file and an optional assets dictionary. - -The filename for the XML can also refer to a key in the assets dictionary. -This is useful for example when the XML is not available as a file on disk.)")); + R"(Loads an MjModel from an XML file and optional assets dict or VFS.)")); mjModel.def_static( "from_binary_path", &MjModelWrapper::LoadBinaryFile, py::arg("filename"), - py::arg_v("assets", py::none()), + py::arg_v("assets", py::none()), py::arg("vfs") = py::none(), py::doc( R"(Loads an MjModel from an MJB file and an optional assets dictionary. diff --git a/python/mujoco/structs.h b/python/mujoco/structs.h index 5c1e70cb..03080618 100644 --- a/python/mujoco/structs.h +++ b/python/mujoco/structs.h @@ -44,6 +44,9 @@ namespace py = ::pybind11; namespace mujoco::python { + +class MjVfs; + namespace _impl { struct VfsAsset { @@ -501,17 +504,20 @@ class MjWrapper : public WrapperBase { static MjWrapper LoadXMLFile( const std::string& filename, const std::optional< - std::unordered_map>& assets); + std::unordered_map>& assets, + MjVfs* vfs = nullptr); static MjWrapper LoadBinaryFile( const std::string& filename, const std::optional< - std::unordered_map>& assets); + std::unordered_map>& assets, + MjVfs* vfs = nullptr); static MjWrapper LoadXML( const std::string& xml, const std::optional< - std::unordered_map>& assets); + std::unordered_map>& assets, + MjVfs* vfs = nullptr); static MjWrapper WrapRawModel(raw::MjModel* m); diff --git a/python/mujoco/structs_wrappers.cc b/python/mujoco/structs_wrappers.cc index ebfa907b..0f46d811 100644 --- a/python/mujoco/structs_wrappers.cc +++ b/python/mujoco/structs_wrappers.cc @@ -19,6 +19,7 @@ #include #include // NOLINT(build/c++11) #include +#include #include #include #include @@ -38,6 +39,7 @@ #include "raw.h" #include "serialization.h" #include "structs.h" +#include "vfs.h" #include #include #include @@ -79,6 +81,18 @@ constexpr auto XArrayShapeImpl(const std::string_view dim1_str) { inline std::size_t NConMax(const mjData* d) { return d->narena / sizeof(mjContact); } +template +struct Cleanup final { + Callback clean_func; + Cleanup(Callback callback) : clean_func(std::move(callback)) {} + ~Cleanup() { clean_func(); } +}; + +// `Cleanup c = /* callback */;` +// +// C++17 type deduction API for creating an instance of `Cleanup` +template +Cleanup(Callback callback) -> Cleanup; } // namespace @@ -295,18 +309,29 @@ MjModelWrapper::~MjWrapper() { template static raw::MjModel* LoadModelFileImpl(const std::string& filename, const std::vector& assets, + mjVFS* vfs, LoadFunc&& loadfunc) { - mjVFS vfs; - mjVFS* vfs_ptr = nullptr; + if (!assets.empty() && vfs != nullptr) { + throw py::value_error("Cannot specify both 'assets' and 'vfs'."); + } + + std::optional local_vfs; + Cleanup vfs_cleanup = [&]() { + if (local_vfs.has_value()) { + mj_deleteVFS(vfs); + }; + }; + if (!assets.empty()) { - mj_defaultVFS(&vfs); - vfs_ptr = &vfs; + vfs = &local_vfs.emplace(); + mj_defaultVFS(vfs); + for (const auto& asset : assets) { std::string buffer_name = StripPath(asset.name); const int vfs_error = InterceptMjErrors(mj_addBufferVFS)( - vfs_ptr, buffer_name.c_str(), asset.content, asset.content_size); + vfs, buffer_name.c_str(), asset.content, + asset.content_size); if (vfs_error) { - mj_deleteVFS(vfs_ptr); if (vfs_error == 2) { throw py::value_error("Repeated file name in assets dict: " + buffer_name); @@ -317,28 +342,34 @@ static raw::MjModel* LoadModelFileImpl(const std::string& filename, } } - raw::MjModel* model = loadfunc(filename.c_str(), vfs_ptr); - mj_deleteVFS(vfs_ptr); + raw::MjModel* model = loadfunc(filename.c_str(), vfs); if (model && !model->buffer) { mj_deleteModel(model); model = nullptr; } + return model; } MjModelWrapper MjModelWrapper::LoadXMLFile( const std::string& filename, - const std::optional>& assets) { + const std::optional>& assets, + MjVfs* vfs) { + if (assets.has_value() && vfs != nullptr) { + throw py::value_error("Cannot specify both 'assets' and 'vfs'."); + } + const auto converted_assets = ConvertAssetsDict(assets); raw::MjModel* model; { py::gil_scoped_release no_gil; char error[1024]; - model = LoadModelFileImpl(filename, converted_assets, - [&error](const char* filename, const mjVFS* vfs) { - return InterceptMjErrors(mj_loadXML)( - filename, vfs, error, sizeof(error)); - }); + model = LoadModelFileImpl( + filename, converted_assets, vfs ? vfs->get() : nullptr, + [&error](const char* filename, const mjVFS* vfs) { + return InterceptMjErrors(mj_loadXML)( + filename, vfs, error, sizeof(error)); + }); if (!model) { throw py::value_error(error); } @@ -348,12 +379,18 @@ MjModelWrapper MjModelWrapper::LoadXMLFile( MjModelWrapper MjModelWrapper::LoadBinaryFile( const std::string& filename, - const std::optional>& assets) { + const std::optional>& assets, + MjVfs* vfs) { + if (assets.has_value() && vfs != nullptr) { + throw py::value_error("Cannot specify both 'assets' and 'vfs'."); + } + const auto converted_assets = ConvertAssetsDict(assets); raw::MjModel* model; { py::gil_scoped_release no_gil; model = LoadModelFileImpl(filename, converted_assets, + vfs ? vfs->get() : nullptr, InterceptMjErrors(mj_loadModel)); if (!model) { throw py::value_error("mj_loadModel: failed to load from mjb"); @@ -364,22 +401,44 @@ MjModelWrapper MjModelWrapper::LoadBinaryFile( MjModelWrapper MjModelWrapper::LoadXML( const std::string& xml, - const std::optional>& assets) { + const std::optional>& assets, + MjVfs* vfs) { + if (assets.has_value() && vfs != nullptr) { + throw py::value_error("Cannot specify both 'assets' and 'vfs'."); + } + auto converted_assets = ConvertAssetsDict(assets); raw::MjModel* model; { py::gil_scoped_release no_gil; - std::string model_filename = "model_.xml"; - if (assets.has_value()) { - while (assets->find(model_filename) != assets->end()) { - model_filename = - model_filename.substr(0, model_filename.size() - 4) + "_.xml"; + std::string model_identifier = "model_.xml"; + bool file_added = false; + Cleanup file_cleanup = [&]() { + if (file_added) { + mj_deleteFileVFS(vfs->get(), model_identifier.c_str()); } + }; + if (vfs != nullptr) { + while (mj_containsBufferVFS(vfs->get(), model_identifier.c_str())) { + model_identifier = + model_identifier.substr(0, model_identifier.size() - 4) + "_.xml"; + } + + mj_addBufferVFS(vfs->get(), model_identifier.c_str(), xml.c_str(), + xml.length()); + file_added = true; + } else { + while (assets.has_value() && + assets->find(model_identifier) != assets->end()) { + model_identifier = + model_identifier.substr(0, model_identifier.size() - 4) + "_.xml"; + } + converted_assets.emplace_back(model_identifier.c_str(), xml.c_str(), + xml.length()); } - converted_assets.emplace_back(model_filename.c_str(), xml.c_str(), - xml.length()); char error[1024]; - model = LoadModelFileImpl(model_filename, converted_assets, + model = LoadModelFileImpl(model_identifier, converted_assets, + vfs ? vfs->get() : nullptr, [&error](const char* filename, const mjVFS* vfs) { return InterceptMjErrors(mj_loadXML)( filename, vfs, error, sizeof(error)); @@ -468,6 +527,7 @@ std::unique_ptr MjModelWrapper::Deserialize( raw::MjModel* model = LoadModelFileImpl( "model.mjb", {{"model.mjb", model_bytes.data(), static_cast(model_size)}}, + nullptr, InterceptMjErrors(mj_loadModel)); if (!model) { throw py::value_error("Invalid serialized mjModel."); diff --git a/python/mujoco/vfs.h b/python/mujoco/vfs.h new file mode 100644 index 00000000..f14ec198 --- /dev/null +++ b/python/mujoco/vfs.h @@ -0,0 +1,46 @@ +// Copyright 2026 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_PYTHON_VFS_H_ +#define MUJOCO_PYTHON_VFS_H_ + +#include + +#include + +namespace mujoco::python { + +class MjVfs { + public: + MjVfs() : vfs_(new mjVFS) { mj_defaultVFS(vfs_.get()); } + + void Close() { vfs_.reset(); } + + mjVFS* get() const { return vfs_.get(); } + + bool is_open() const { return vfs_ != nullptr; } + + private: + struct VfsDeleter { + void operator()(mjVFS* vfs) const { + mj_deleteVFS(vfs); + delete vfs; + } + }; + std::unique_ptr vfs_; +}; + +} // namespace mujoco::python + +#endif // MUJOCO_PYTHON_VFS_H_ diff --git a/python/mujoco/vfs_test.py b/python/mujoco/vfs_test.py new file mode 100644 index 00000000..dff3cfd1 --- /dev/null +++ b/python/mujoco/vfs_test.py @@ -0,0 +1,251 @@ +# Copyright 2026 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +import textwrap + +from absl.testing import absltest +import mujoco + + +SIMPLE_XML = b"" + +XML_WITH_MESH = textwrap.dedent("""\ + + + + + + + + + + +""").encode() + +BOX_OBJ = textwrap.dedent("""\ + v -1 -1 -1 + v 1 -1 -1 + v 1 1 -1 + v -1 1 -1 + v -1 -1 1 + v 1 -1 1 + v 1 1 1 + v -1 1 1 + f 1 2 3 4 + f 5 8 7 6 + f 1 5 6 2 + f 2 6 7 3 + f 3 7 8 4 + f 4 8 5 1 +""").encode() + + +class VfsLifecycleTest(absltest.TestCase): + + def test_create_and_close(self): + vfs = mujoco.MjVfs() + vfs.close() + + def test_context_manager(self): + with mujoco.MjVfs() as vfs: + self.assertIsNotNone(vfs) + + def test_double_close_is_safe(self): + vfs = mujoco.MjVfs() + vfs.close() + vfs.close() + + def test_operations_after_close_raise(self): + vfs = mujoco.MjVfs() + vfs.close() + with self.assertRaises(RuntimeError): + vfs["model.xml"] = SIMPLE_XML + + +class VfsBufferTest(absltest.TestCase): + + def test_add_buffer(self): + with mujoco.MjVfs() as vfs: + vfs["model.xml"] = SIMPLE_XML + self.assertIn("model.xml", vfs) + + def test_add_duplicate_raises(self): + with mujoco.MjVfs() as vfs: + vfs["model.xml"] = SIMPLE_XML + with self.assertRaises(ValueError): + vfs["model.xml"] = SIMPLE_XML + + def test_delete_buffer(self): + with mujoco.MjVfs() as vfs: + vfs["model.xml"] = SIMPLE_XML + self.assertIn("model.xml", vfs) + del vfs["model.xml"] + self.assertNotIn("model.xml", vfs) + + def test_delete_missing_raises(self): + with mujoco.MjVfs() as vfs: + with self.assertRaises(KeyError): + del vfs["nonexistent"] + + def test_contains_missing(self): + with mujoco.MjVfs() as vfs: + self.assertNotIn("nonexistent", vfs) + + +class VfsCompileTest(absltest.TestCase): + + def test_compile_simple_model(self): + with mujoco.MjVfs() as vfs: + vfs["model.xml"] = SIMPLE_XML + model = mujoco.MjModel.from_xml_path("model.xml", vfs=vfs) + self.assertEqual(model.nq, 0) + + def test_compile_model_with_mesh(self): + with mujoco.MjVfs() as vfs: + vfs["model.xml"] = XML_WITH_MESH + vfs["box.obj"] = BOX_OBJ + model = mujoco.MjModel.from_xml_path("model.xml", vfs=vfs) + self.assertEqual(model.nmesh, 1) + + def test_spec_from_string_with_vfs(self): + with mujoco.MjVfs() as vfs: + vfs["box.obj"] = BOX_OBJ + spec = mujoco.MjSpec.from_string(XML_WITH_MESH.decode(), vfs=vfs) + model = spec.compile(vfs=vfs) + self.assertEqual(model.nmesh, 1) + + def test_from_xml_string_with_vfs(self): + with mujoco.MjVfs() as vfs: + vfs["box.obj"] = BOX_OBJ + model = mujoco.MjModel.from_xml_string( + XML_WITH_MESH.decode(), vfs=vfs + ) + self.assertEqual(model.nmesh, 1) + + def test_spec_compile_with_vfs(self): + with mujoco.MjVfs() as vfs: + vfs["box.obj"] = BOX_OBJ + spec = mujoco.MjSpec.from_string(XML_WITH_MESH.decode()) + model = spec.compile(vfs=vfs) + self.assertEqual(model.nmesh, 1) + + def test_spec_recompile_with_vfs(self): + with mujoco.MjVfs() as vfs: + vfs["box.obj"] = BOX_OBJ + spec = mujoco.MjSpec.from_string(XML_WITH_MESH.decode()) + model1 = spec.compile(vfs=vfs) + self.assertEqual(model1.ngeom, 1) + + body = spec.worldbody.add_body() + body.add_geom(size=[1, 0, 0]) + model2 = spec.compile(vfs=vfs) + self.assertEqual(model2.ngeom, 2) + + def test_long_lived_vfs_without_context(self): + vfs = mujoco.MjVfs() + vfs["box.obj"] = BOX_OBJ + + spec = mujoco.MjSpec.from_string(XML_WITH_MESH.decode()) + model1 = spec.compile(vfs=vfs) + self.assertEqual(model1.nmesh, 1) + + spec.worldbody.add_body().add_geom(size=[1, 0, 0]) + model2 = spec.compile(vfs=vfs) + self.assertEqual(model2.nmesh, 1) + self.assertEqual(model2.ngeom, 2) + + vfs.close() + + def test_vfs_and_assets_raises(self): + with mujoco.MjVfs() as vfs: + vfs["model.xml"] = SIMPLE_XML + with self.assertRaises(ValueError): + mujoco.MjModel.from_xml_string( + SIMPLE_XML.decode(), assets={"a": b"b"}, vfs=vfs + ) + + def test_backward_compat_assets_dict(self): + model = mujoco.MjModel.from_xml_string( + XML_WITH_MESH.decode(), assets={"box.obj": BOX_OBJ} + ) + self.assertEqual(model.nmesh, 1) + + +class VfsAttachTest(absltest.TestCase): + + def test_attach_shared_vfs(self): + child_xml = textwrap.dedent("""\ + + + + + + + + + + + + """) + parent_xml = textwrap.dedent("""\ + + + + + + + + """) + + with mujoco.MjVfs() as vfs: + vfs["box.obj"] = BOX_OBJ + + parent = mujoco.MjSpec.from_string(parent_xml) + child = mujoco.MjSpec.from_string(child_xml) + parent.attach(child, site="mount") + model = parent.compile(vfs=vfs) + self.assertEqual(model.nmesh, 1) + self.assertEmpty(parent.assets) + + def test_attach_no_asset_dict_needed(self): + child_xml = textwrap.dedent("""\ + + + + + + + + + """) + parent_xml = textwrap.dedent("""\ + + + + + + + + """) + + with mujoco.MjVfs() as vfs: + parent = mujoco.MjSpec.from_string(parent_xml) + child = mujoco.MjSpec.from_string(child_xml) + parent.attach(child, site="mount") + model = parent.compile(vfs=vfs) + self.assertGreater(model.nbody, 1) + + +if __name__ == "__main__": + absltest.main() From b2feed63e492b189c87068b028ba72d5d0bf1e44 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 1 May 2026 14:08:33 -0700 Subject: [PATCH 178/251] Skip implicit solver if there are strain constraints. PiperOrigin-RevId: 908890994 Change-Id: Ieb05a162142edac14269ec6a509fc507c9146ac5 --- src/engine/engine_forward.c | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index cae49ba9..ef15c37a 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -1371,6 +1371,14 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { } +// return 1 if flex f needs implicit interp treatment +static int flexInterp_active(const mjModel* m, int f) { + return m->flex_interp[f] && !m->flex_rigid[f] && + m->flex_edgeequality[f] != 3 && + m->flex_stiffness[m->flex_stiffnessadr[f]] != 0; +} + + // context for flex interp reduced banded factorization/solve typedef struct { mjtNum* H; // banded Cholesky-factored matrix (ndof x nband) @@ -1425,7 +1433,7 @@ static FlexInterpContext flexInterp_factor(const mjModel* m, mjData* d, int nv) // count flex DOFs int ndof = 0; for (int f=0; f < m->nflex; f++) { - if (m->flex_interp[f]) { + if (flexInterp_active(m, f)) { flexInterp_collect(m, f, chain_dofs, seen_dof, &ndof); } } @@ -1442,7 +1450,7 @@ static FlexInterpContext flexInterp_factor(const mjModel* m, mjData* d, int nv) int cnt = 0; mju_fillInt(seen_dof, 0, nv); for (int f=0; f < m->nflex; f++) { - if (m->flex_interp[f]) { + if (flexInterp_active(m, f)) { int nodenum = m->flex_nodenum[f]; int nodeadr = m->flex_nodeadr[f]; for (int n=0; n < nodenum; n++) { @@ -1481,7 +1489,7 @@ static FlexInterpContext flexInterp_factor(const mjModel* m, mjData* d, int nv) // get precomputed bandwidth int bandwidth = 0; for (int f=0; f < m->nflex; f++) { - if (m->flex_interp[f]) { + if (flexInterp_active(m, f)) { if (m->flex_bandwidth[f] > bandwidth) { bandwidth = m->flex_bandwidth[f]; } @@ -1963,10 +1971,10 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv); } - // check for flex_interp + // check for flex_interp that needs implicit treatment int has_flex_interp = 0; for (int f=0; f < m->nflex; f++) { - if (m->flex_interp[f]) { + if (flexInterp_active(m, f)) { has_flex_interp = 1; break; } From dbd451138c673aba6cc38efecda5e37a510e5e1b Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Sat, 2 May 2026 00:13:09 -0700 Subject: [PATCH 179/251] Add flex_bendingadr to mjModel. PiperOrigin-RevId: 909088123 Change-Id: If062335d0aa3702c1e0b082dd9e29c3be0e76ea6 --- doc/includes/references.h | 4 +- include/mujoco/mjmodel.h | 4 +- include/mujoco/mjxmacro.h | 4 +- python/mujoco/introspect/structs.py | 15 +++- src/engine/engine_io.c | 29 +++---- src/engine/engine_io.h | 10 +-- src/user/user_model.cc | 111 ++++++++++++++------------- src/user/user_model.h | 1 + unity/Runtime/Bindings/MjBindings.cs | 2 + wasm/codegen/generated/bindings.cc | 13 +++- 10 files changed, 115 insertions(+), 78 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 4d767933..c594a32d 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1041,6 +1041,7 @@ struct mjModel_ { mjtSize nflexelem; // number of elements in all flexes mjtSize nflexelemdata; // number of element vertex ids in all flexes mjtSize nflexstiffness; // number of stiffness parameters in all flexes + mjtSize nflexbending; // number of bending parameters in all flexes mjtSize nflexelemedge; // number of element edge ids in all flexes mjtSize nflexshelldata; // number of shell fragment vertex ids in all flexes mjtSize nflexevpair; // number of element-vertex pairs in all flexes @@ -1328,6 +1329,7 @@ struct mjModel_ { int* flex_elemdataadr; // first element vertex id address (nflex x 1) int* flex_stiffnessadr; // stiffness matrix address (nflex x 1) int* flex_elemedgeadr; // first element edge id address (nflex x 1) + int* flex_bendingadr; // first bending data address (nflex x 1) int* flex_shellnum; // number of shells (nflex x 1) int* flex_shelldataadr; // first shell data address (nflex x 1) int* flex_evpairadr; // first evpair address (nflex x 1) @@ -1356,7 +1358,7 @@ struct mjModel_ { mjtNum* flex_radius; // radius around primitive element (nflex x 1) mjtNum* flex_size; // vertex bounding box half sizes in qpos0 (nflex x 3) mjtNum* flex_stiffness; // finite element stiffness matrix (nflexstiffness x 1) - mjtNum* flex_bending; // bending stiffness (nflexedge x 17) + mjtNum* flex_bending; // bending stiffness (nflexbending x 1) mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1) mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1) mjtNum* flex_edgedamping; // edge damping (nflex x 1) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 1c9214ae..ec2c8250 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -704,6 +704,7 @@ struct mjModel_ { mjtSize nflexelem; // number of elements in all flexes mjtSize nflexelemdata; // number of element vertex ids in all flexes mjtSize nflexstiffness; // number of stiffness parameters in all flexes + mjtSize nflexbending; // number of bending parameters in all flexes mjtSize nflexelemedge; // number of element edge ids in all flexes mjtSize nflexshelldata; // number of shell fragment vertex ids in all flexes mjtSize nflexevpair; // number of element-vertex pairs in all flexes @@ -991,6 +992,7 @@ struct mjModel_ { int* flex_elemdataadr; // first element vertex id address (nflex x 1) int* flex_stiffnessadr; // stiffness matrix address (nflex x 1) int* flex_elemedgeadr; // first element edge id address (nflex x 1) + int* flex_bendingadr; // first bending data address (nflex x 1) int* flex_shellnum; // number of shells (nflex x 1) int* flex_shelldataadr; // first shell data address (nflex x 1) int* flex_evpairadr; // first evpair address (nflex x 1) @@ -1019,7 +1021,7 @@ struct mjModel_ { mjtNum* flex_radius; // radius around primitive element (nflex x 1) mjtNum* flex_size; // vertex bounding box half sizes in qpos0 (nflex x 3) mjtNum* flex_stiffness; // finite element stiffness matrix (nflexstiffness x 1) - mjtNum* flex_bending; // bending stiffness (nflexedge x 17) + mjtNum* flex_bending; // bending stiffness (nflexbending x 1) mjtNum* flex_damping; // Rayleigh's damping coefficient (nflex x 1) mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1) mjtNum* flex_edgedamping; // edge damping (nflex x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 4efaf94b..080cbc07 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -186,6 +186,7 @@ X( nflexelem ) \ X( nflexelemdata ) \ X( nflexstiffness ) \ + X( nflexbending ) \ X( nflexelemedge ) \ X( nflexshelldata ) \ X( nflexevpair ) \ @@ -467,6 +468,7 @@ X ( int, flex_elemdataadr, nflex, 1 ) \ X ( int, flex_stiffnessadr, nflex, 1 ) \ X ( int, flex_elemedgeadr, nflex, 1 ) \ + X ( int, flex_bendingadr, nflex, 1 ) \ X ( int, flex_shellnum, nflex, 1 ) \ X ( int, flex_shelldataadr, nflex, 1 ) \ X ( int, flex_evpairadr, nflex, 1 ) \ @@ -495,7 +497,7 @@ X ( mjtNum, flex_radius, nflex, 1 ) \ X ( mjtNum, flex_size, nflex, 3 ) \ X ( mjtNum, flex_stiffness, nflexstiffness, 1 ) \ - X ( mjtNum, flex_bending, nflexedge, 17 ) \ + X ( mjtNum, flex_bending, nflexbending, 1 ) \ X ( mjtNum, flex_damping, nflex, 1 ) \ X ( mjtNum, flex_edgestiffness, nflex, 1 ) \ X ( mjtNum, flex_edgedamping, nflex, 1 ) \ diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 9bd45295..6ce4cb62 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -977,6 +977,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='mjtSize'), doc='number of stiffness parameters in all flexes', ), + StructFieldDecl( + name='nflexbending', + type=ValueType(name='mjtSize'), + doc='number of bending parameters in all flexes', + ), StructFieldDecl( name='nflexelemedge', type=ValueType(name='mjtSize'), @@ -2772,6 +2777,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='first element edge id address', array_extent=('nflex',), ), + StructFieldDecl( + name='flex_bendingadr', + type=PointerType( + inner_type=ValueType(name='int'), + ), + doc='first bending data address', + array_extent=('nflex',), + ), StructFieldDecl( name='flex_shellnum', type=PointerType( @@ -3002,7 +3015,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='mjtNum'), ), doc='bending stiffness', - array_extent=('nflexedge', 17), + array_extent=('nflexbending',), ), StructFieldDecl( name='flex_damping', diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 33fafe6c..77dd0063 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -205,9 +205,9 @@ void mj_makeModel(mjModel** dest, mjtSize nbvhdynamic, mjtSize noct, mjtSize njnt, mjtSize ntree, mjtSize nM, mjtSize nB, mjtSize nC, mjtSize nD, mjtSize ngeom, mjtSize nsite, mjtSize ncam, mjtSize nlight, mjtSize nflex, mjtSize nflexnode, mjtSize nflexvert, mjtSize nflexedge, mjtSize nflexelem, - mjtSize nflexelemdata, mjtSize nflexstiffness, mjtSize nflexelemedge, mjtSize nflexshelldata, - mjtSize nflexevpair, mjtSize nflextexcoord, mjtSize nJfe, mjtSize nJfv, mjtSize nmesh, - mjtSize nmeshvert, mjtSize nmeshnormal, mjtSize nmeshtexcoord, mjtSize nmeshface, + mjtSize nflexelemdata, mjtSize nflexstiffness, mjtSize nflexbending, mjtSize nflexelemedge, + mjtSize nflexshelldata, mjtSize nflexevpair, mjtSize nflextexcoord, mjtSize nJfe, mjtSize nJfv, + mjtSize nmesh, mjtSize nmeshvert, mjtSize nmeshnormal, mjtSize nmeshtexcoord, mjtSize nmeshface, mjtSize nmeshgraph, mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap, mjtSize nskin, mjtSize nskinvert, mjtSize nskintexvert, mjtSize nskinface, mjtSize nskinbone, mjtSize nskinbonevert, mjtSize nhfield, mjtSize nhfielddata, mjtSize ntex, mjtSize ntexdata, @@ -294,6 +294,7 @@ void mj_makeModel(mjModel** dest, m->nflexelem = nflexelem; m->nflexelemdata = nflexelemdata; m->nflexstiffness = nflexstiffness; + m->nflexbending = nflexbending; m->nflexelemedge = nflexelemedge; m->nflexshelldata = nflexshelldata; m->nflexevpair = nflexevpair; @@ -405,16 +406,16 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) { src->nbvhdynamic, src->noct, src->njnt, src->ntree, src->nM, src->nB, src->nC, src->nD, src->ngeom, src->nsite, src->ncam, src->nlight, src->nflex, src->nflexnode, src->nflexvert, src->nflexedge, src->nflexelem, src->nflexelemdata, src->nflexstiffness, - src->nflexelemedge, src->nflexshelldata, src->nflexevpair, src->nflextexcoord, src->nJfe, - src->nJfv, src->nmesh, src->nmeshvert, src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, - src->nmeshgraph, src->nmeshpoly, src->nmeshpolyvert, src->nmeshpolymap, src->nskin, - src->nskinvert, src->nskintexvert, src->nskinface, src->nskinbone, src->nskinbonevert, - src->nhfield, src->nhfielddata, src->ntex, src->ntexdata, src->nmat, src->npair, - src->nexclude, src->neq, src->ntendon, src->nJten, src->nwrap, src->nsensor, src->nnumeric, - src->nnumericdata, src->ntext, src->ntextdata, src->ntuple, src->ntupledata, src->nkey, - src->nmocap, src->nplugin, src->npluginattr, src->nuser_body, src->nuser_jnt, - src->nuser_geom, src->nuser_site, src->nuser_cam, src->nuser_tendon, src->nuser_actuator, - src->nuser_sensor, src->nnames, src->npaths); + src->nflexbending, src->nflexelemedge, src->nflexshelldata, src->nflexevpair, + src->nflextexcoord, src->nJfe, src->nJfv, src->nmesh, src->nmeshvert, src->nmeshnormal, + src->nmeshtexcoord, src->nmeshface, src->nmeshgraph, src->nmeshpoly, src->nmeshpolyvert, + src->nmeshpolymap, src->nskin, src->nskinvert, src->nskintexvert, src->nskinface, + src->nskinbone, src->nskinbonevert, src->nhfield, src->nhfielddata, src->ntex, + src->ntexdata, src->nmat, src->npair, src->nexclude, src->neq, src->ntendon, src->nJten, + src->nwrap, src->nsensor, src->nnumeric, src->nnumericdata, src->ntext, src->ntextdata, + src->ntuple, src->ntupledata, src->nkey, src->nmocap, src->nplugin, src->npluginattr, + src->nuser_body, src->nuser_jnt, src->nuser_geom, src->nuser_site, src->nuser_cam, + src->nuser_tendon, src->nuser_actuator, src->nuser_sensor, src->nnames, src->npaths); } if (!dest) { mjERROR("failed to make mjModel. Invalid sizes."); @@ -597,7 +598,7 @@ mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) { sizes[56], sizes[57], sizes[58], sizes[59], sizes[60], sizes[61], sizes[62], sizes[63], sizes[64], sizes[65], sizes[66], sizes[67], sizes[68], sizes[69], sizes[70], sizes[71], sizes[72], sizes[73], sizes[74], sizes[75], sizes[76], - sizes[77], sizes[78]); + sizes[77], sizes[78], sizes[79]); // mj_makeModel may fail if the input buffer has invalid sizes if (!m) { diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index 1aedaf86..dccd55a4 100644 --- a/src/engine/engine_io.h +++ b/src/engine/engine_io.h @@ -52,11 +52,11 @@ void mj_makeModel(mjModel** dest, mjtSize nbvhdynamic, mjtSize noct, mjtSize njnt, mjtSize ntree, mjtSize nM, mjtSize nB, mjtSize nC, mjtSize nD, mjtSize ngeom, mjtSize nsite, mjtSize ncam, mjtSize nlight, mjtSize nflex, mjtSize nflexnode, mjtSize nflexvert, mjtSize nflexedge, mjtSize nflexelem, - mjtSize nflexelemdata, mjtSize nflexstiffness, mjtSize nflexelemedge, mjtSize nflexshelldata, mjtSize nflexevpair, - mjtSize nflextexcoord, mjtSize nJfe, mjtSize nJfv, mjtSize nmesh, mjtSize nmeshvert, - mjtSize nmeshnormal, mjtSize nmeshtexcoord, mjtSize nmeshface, mjtSize nmeshgraph, - mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap, mjtSize nskin, - mjtSize nskinvert, mjtSize nskintexvert, mjtSize nskinface, mjtSize nskinbone, + mjtSize nflexelemdata, mjtSize nflexstiffness, mjtSize nflexbending, mjtSize nflexelemedge, + mjtSize nflexshelldata, mjtSize nflexevpair, mjtSize nflextexcoord, mjtSize nJfe, mjtSize nJfv, + mjtSize nmesh, mjtSize nmeshvert, mjtSize nmeshnormal, mjtSize nmeshtexcoord, mjtSize nmeshface, + mjtSize nmeshgraph, mjtSize nmeshpoly, mjtSize nmeshpolyvert, mjtSize nmeshpolymap, + mjtSize nskin, mjtSize nskinvert, mjtSize nskintexvert, mjtSize nskinface, mjtSize nskinbone, mjtSize nskinbonevert, mjtSize nhfield, mjtSize nhfielddata, mjtSize ntex, mjtSize ntexdata, mjtSize nmat, mjtSize npair, mjtSize nexclude, mjtSize neq, mjtSize ntendon, mjtSize nJten, mjtSize nwrap, mjtSize nsensor, mjtSize nnumeric, mjtSize nnumericdata, mjtSize ntext, diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 6c33cdec..e85cbe64 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1195,6 +1195,7 @@ void mjCModel::Clear() { nflexelem = 0; nflexelemdata = 0; nflexstiffness = 0; + nflexbending = 0; nflexelemedge = 0; nflexshelldata = 0; nflexevpair = 0; @@ -2105,6 +2106,21 @@ static size_t getpathslength(std::vector list) { return result; } +// compute extra stiffness/bending array size for an interpolated flex +static int flexInterpExtraSize(int order, const int cellcount[3], bool shell) { + int npe, nelem; + int cx = cellcount[0], cy = cellcount[1], cz = cellcount[2]; + if (shell) { + npe = (int)pow(order + 1, 2); + nelem = 2*(cy*cz + cx*cz + cx*cy); + } else { + npe = (int)pow(order + 1, 3); + nelem = cx * cy * cz; + } + int ndof_elem = 3 * npe; + return nelem * ndof_elem * ndof_elem; +} + // set array sizes void mjCModel::SetSizes() { // set from object list sizes @@ -2179,6 +2195,7 @@ void mjCModel::SetSizes() { nbvh = nbvhstatic + nbvhdynamic; int extra_stiffness_size = 0; + int extra_bending_size = 0; // flex counts for (int i=0; i < nflex; i++) { nflexnode += flexes_[i]->nnode; @@ -2191,20 +2208,11 @@ void mjCModel::SetSizes() { nflexevpair += (int)flexes_[i]->evpair.size()/2; nflextexcoord += (flexes_[i]->HasTexcoord() ? flexes_[i]->get_texcoord().size()/2 : 0); if (flexes_[i]->spec.order != 0) { - int cx = flexes_[i]->spec.cellcount[0]; - int cy = flexes_[i]->spec.cellcount[1]; - int cz = flexes_[i]->spec.cellcount[2]; - bool shell = (flexes_[i]->elastic2d != 0); - int npe, nelem; - if (shell) { - npe = (int)pow(flexes_[i]->spec.order + 1, 2); - nelem = 2*(cy*cz + cx*cz + cx*cy); - } else { - npe = (int)pow(flexes_[i]->spec.order + 1, 3); - nelem = cx * cy * cz; - } - int ndof_elem = 3 * npe; - extra_stiffness_size += nelem * ndof_elem * ndof_elem; + int extra_size = flexInterpExtraSize( + flexes_[i]->spec.order, flexes_[i]->spec.cellcount, + flexes_[i]->elastic2d != 0); + extra_stiffness_size += extra_size; + extra_bending_size += extra_size; } if (flexes_[i]->interpolated || flexes_[i]->rigid) { continue; @@ -2256,8 +2264,9 @@ void mjCModel::SetSizes() { } } // TODO: This can be compacted further when we update mjwarp to not rely on - // 21*elem_adr for non-interpolated flexes. + // 21*elem_adr for non-interpolated flexes and 17*edge_adr for bending. nflexstiffness = nflexelem * 21 + extra_stiffness_size; + nflexbending = nflexedge * 17 + extra_bending_size; // mesh counts for (int i=0; i < nmesh; i++) { @@ -3458,6 +3467,8 @@ void mjCModel::CopyObjects(mjModel* m) { texcoord_adr = 0; int standard_stiffness_size = 21 * m->nflexelem; int current_extra_stiffness_adr = standard_stiffness_size; + int standard_bending_size = 17 * m->nflexedge; + int current_extra_bending_adr = standard_bending_size; for (int i=0; i < nflex; i++) { // get pointer mjCFlex* pfl = flexes_[i]; @@ -3484,20 +3495,8 @@ void mjCModel::CopyObjects(mjModel* m) { m->flex_stiffnessadr[i] = 21 * elem_adr; } else { m->flex_stiffnessadr[i] = current_extra_stiffness_adr; - int pcx = pfl->spec.cellcount[0]; - int pcy = pfl->spec.cellcount[1]; - int pcz = pfl->spec.cellcount[2]; - bool shell = (pfl->elastic2d != 0); - int npe, nelem; - if (shell) { - npe = (int)pow(pfl->spec.order + 1, 2); - nelem = 2*(pcy*pcz + pcx*pcz + pcx*pcy); - } else { - npe = (int)pow(pfl->spec.order + 1, 3); - nelem = pcx * pcy * pcz; - } - int ndof_elem = 3 * npe; - current_extra_stiffness_adr += nelem * ndof_elem * ndof_elem; + current_extra_stiffness_adr += flexInterpExtraSize( + pfl->spec.order, pfl->spec.cellcount, pfl->elastic2d != 0); } if (!pfl->stiffness.empty()) { @@ -3508,27 +3507,30 @@ void mjCModel::CopyObjects(mjModel* m) { if (pfl->spec.order == 0) { stiff_size = 21 * pfl->nelem; } else { - int scx = pfl->spec.cellcount[0]; - int scy = pfl->spec.cellcount[1]; - int scz = pfl->spec.cellcount[2]; - bool shell = (pfl->elastic2d != 0); - int npe, sncells; - if (shell) { - npe = (int)pow(pfl->spec.order + 1, 2); - sncells = 2*(scy*scz + scx*scz + scx*scy); - } else { - npe = (int)pow(pfl->spec.order + 1, 3); - sncells = scx * scy * scz; - } - int ndof_elem = 3 * npe; - stiff_size = sncells * ndof_elem * ndof_elem; + stiff_size = flexInterpExtraSize( + pfl->spec.order, pfl->spec.cellcount, pfl->elastic2d != 0); } mjuu_zerovec(m->flex_stiffness + m->flex_stiffnessadr[i], stiff_size); } - if (!pfl->bending.empty()) { - mjuu_copyvec(m->flex_bending + 17 * edge_adr, pfl->bending.data(), pfl->bending.size()); + if (pfl->spec.order == 0) { + m->flex_bendingadr[i] = 17 * edge_adr; } else { - mjuu_zerovec(m->flex_bending + 17 * edge_adr, 17 * pfl->nedge); + m->flex_bendingadr[i] = current_extra_bending_adr; + current_extra_bending_adr += flexInterpExtraSize( + pfl->spec.order, pfl->spec.cellcount, pfl->elastic2d != 0); + } + + if (!pfl->bending.empty()) { + mjuu_copyvec(m->flex_bending + m->flex_bendingadr[i], pfl->bending.data(), pfl->bending.size()); + } else { + int bending_size; + if (pfl->spec.order == 0) { + bending_size = 17 * pfl->nedge; + } else { + bending_size = flexInterpExtraSize( + pfl->spec.order, pfl->spec.cellcount, pfl->elastic2d != 0); + } + mjuu_zerovec(m->flex_bending + m->flex_bendingadr[i], bending_size); } m->flex_damping[i] = (mjtNum)pfl->damping; @@ -3602,7 +3604,8 @@ void mjCModel::CopyObjects(mjModel* m) { } if (!pfl->rigid && m->flex_edgeequality[i] == 0 && - !pfl->edgestiffness && !pfl->edgedamping && !pfl->damping) { + !pfl->edgestiffness && !pfl->edgedamping && !pfl->damping && + pfl->bending.empty()) { mju_warning("flex '%s' is not rigid and has no equality constraints " "or passive forces", pfl->name.c_str()); } @@ -5209,13 +5212,13 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { mj_makeModel(&m, nq, nv, nu, na, nbody, nbvh, nbvhstatic, nbvhdynamic, noct, njnt, ntree, nM, nB, nC, nD, ngeom, nsite, ncam, nlight, nflex, nflexnode, nflexvert, nflexedge, nflexelem, - nflexelemdata, nflexstiffness, nflexelemedge, nflexshelldata, nflexevpair, - nflextexcoord, nJfe, nJfv, nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, - nmeshgraph, nmeshpoly, nmeshpolyvert, nmeshpolymap, nskin, nskinvert, nskintexvert, - nskinface, nskinbone, nskinbonevert, nhfield, nhfielddata, ntex, ntexdata, nmat, - npair, nexclude, neq, ntendon, nJten, nwrap, nsensor, nnumeric, nnumericdata, ntext, - ntextdata, ntuple, ntupledata, nkey, nmocap, nplugin, npluginattr, - nuser_body, nuser_jnt, nuser_geom, nuser_site, nuser_cam, + nflexelemdata, nflexstiffness, nflexbending, nflexelemedge, nflexshelldata, + nflexevpair, nflextexcoord, nJfe, nJfv, nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, + nmeshface, nmeshgraph, nmeshpoly, nmeshpolyvert, nmeshpolymap, nskin, nskinvert, + nskintexvert, nskinface, nskinbone, nskinbonevert, nhfield, nhfielddata, ntex, + ntexdata, nmat, npair, nexclude, neq, ntendon, nJten, nwrap, nsensor, nnumeric, + nnumericdata, ntext, ntextdata, ntuple, ntupledata, nkey, nmocap, nplugin, + npluginattr, nuser_body, nuser_jnt, nuser_geom, nuser_site, nuser_cam, nuser_tendon, nuser_actuator, nuser_sensor, nnames, npaths); if (!m) { throw mjCError(0, "could not create mjModel"); diff --git a/src/user/user_model.h b/src/user/user_model.h index 9513b27b..b22e0dcb 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -97,6 +97,7 @@ class mjCModel_ : public mjsElement { mjtSize nflexelem; // number of elements in all flexes mjtSize nflexelemdata; // number of element vertex ids in all flexes mjtSize nflexstiffness; // number of stiffness parameters in all flexes + mjtSize nflexbending; // number of bending parameters in all flexes mjtSize nflexelemedge; // number of element edges in all flexes mjtSize nflexshelldata; // number of shell fragment vertex ids in all flexes mjtSize nflexevpair; // number of element-vertex pairs in all flexes diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 7f8d625d..211665e7 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -963,6 +963,7 @@ public unsafe struct mjModel_ { public UInt64 nflexelem; public UInt64 nflexelemdata; public UInt64 nflexstiffness; + public UInt64 nflexbending; public UInt64 nflexelemedge; public UInt64 nflexshelldata; public UInt64 nflexevpair; @@ -1213,6 +1214,7 @@ public unsafe struct mjModel_ { public int* flex_elemdataadr; public int* flex_stiffnessadr; public int* flex_elemedgeadr; + public int* flex_bendingadr; public int* flex_shellnum; public int* flex_shelldataadr; public int* flex_evpairadr; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index ae618fd8..2dd89505 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -3752,6 +3752,12 @@ struct MjModel { void set_nflexstiffness(int value) { ptr_->nflexstiffness = static_cast(value); } + int nflexbending() const { + return static_cast(ptr_->nflexbending); + } + void set_nflexbending(int value) { + ptr_->nflexbending = static_cast(value); + } int nflexelemedge() const { return static_cast(ptr_->nflexelemedge); } @@ -4688,6 +4694,9 @@ struct MjModel { emscripten::val flex_elemedgeadr() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nflex, ptr_->flex_elemedgeadr)); } + emscripten::val flex_bendingadr() const { + return emscripten::val(emscripten::typed_memory_view(ptr_->nflex, ptr_->flex_bendingadr)); + } emscripten::val flex_shellnum() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nflex, ptr_->flex_shellnum)); } @@ -4773,7 +4782,7 @@ struct MjModel { return emscripten::val(emscripten::typed_memory_view(ptr_->nflexstiffness, ptr_->flex_stiffness)); } emscripten::val flex_bending() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nflexedge * 17, ptr_->flex_bending)); + return emscripten::val(emscripten::typed_memory_view(ptr_->nflexbending, ptr_->flex_bending)); } emscripten::val flex_damping() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nflex, ptr_->flex_damping)); @@ -11824,6 +11833,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("flex_activelayers", &MjModel::flex_activelayers) .property("flex_bandwidth", &MjModel::flex_bandwidth) .property("flex_bending", &MjModel::flex_bending) + .property("flex_bendingadr", &MjModel::flex_bendingadr) .property("flex_bvhadr", &MjModel::flex_bvhadr) .property("flex_bvhnum", &MjModel::flex_bvhnum) .property("flex_cellnum", &MjModel::flex_cellnum) @@ -12069,6 +12079,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("neq", &MjModel::neq, &MjModel::set_neq, reference()) .property("nexclude", &MjModel::nexclude, &MjModel::set_nexclude, reference()) .property("nflex", &MjModel::nflex, &MjModel::set_nflex, reference()) + .property("nflexbending", &MjModel::nflexbending, &MjModel::set_nflexbending, reference()) .property("nflexedge", &MjModel::nflexedge, &MjModel::set_nflexedge, reference()) .property("nflexelem", &MjModel::nflexelem, &MjModel::set_nflexelem, reference()) .property("nflexelemdata", &MjModel::nflexelemdata, &MjModel::set_nflexelemdata, reference()) From 25751a7b98099b1b9841c4ee3acb5d60b6d4d315 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Sat, 2 May 2026 13:26:14 -0700 Subject: [PATCH 180/251] Add dense LU factorization and solve functions. PiperOrigin-RevId: 909290647 Change-Id: I77ae2352b20abf96ef7b48ae32b03a1f4098604e --- src/engine/engine_util_solve.c | 85 ++++++++++++++- src/engine/engine_util_solve.h | 10 ++ test/engine/engine_util_solve_test.cc | 142 ++++++++++++++++++++++++++ 3 files changed, 236 insertions(+), 1 deletion(-) diff --git a/src/engine/engine_util_solve.c b/src/engine/engine_util_solve.c index 1f8b839b..a6e5541c 100644 --- a/src/engine/engine_util_solve.c +++ b/src/engine/engine_util_solve.c @@ -751,7 +751,90 @@ void mju_bandMulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, } -//------------------------------ LU factorization -------------------------------------------------- +//------------------------------ dense LU factorization -------------------------------------------- + +// dense LU factorization with partial pivoting +// factorizes n x n row-major matrix A in-place into L and U +// L has unit diagonal (not stored), U has explicit diagonal +// pivot stores row permutation: row i of original = row pivot[i] of result +// return 1 if successful, 0 if singular (diagonal element < mjMINVAL) +int mju_factorLU(mjtNum* restrict A, int n, int* pivot) { + for (int k=0; k < n; k++) { + // initialize pivot + pivot[k] = k; + + // find pivot: max absolute value in column k, rows k..n-1 + mjtNum maxval = mju_abs(A[k*n+k]); + int maxrow = k; + for (int i=k+1; i < n; i++) { + mjtNum val = mju_abs(A[i*n+k]); + if (val > maxval) { + maxval = val; + maxrow = i; + } + } + + // check singularity + if (maxval < mjMINVAL) { + return 0; + } + + // swap rows k and maxrow + if (maxrow != k) { + pivot[k] = maxrow; + for (int j=0; j < n; j++) { + mjtNum tmp = A[k*n+j]; + A[k*n+j] = A[maxrow*n+j]; + A[maxrow*n+j] = tmp; + } + } + + // compute multipliers and update trailing submatrix + mjtNum diaginv = 1.0 / A[k*n+k]; + for (int i=k+1; i < n; i++) { + A[i*n+k] *= diaginv; + mjtNum Aik = A[i*n+k]; + for (int j=k+1; j < n; j++) { + A[i*n+j] -= Aik * A[k*n+j]; + } + } + } + + return 1; +} + + +// solve A*x = b given LU factorization of A, LU and pivot are output of mju_factorLU +void mju_solveLU(mjtNum* restrict x, const mjtNum* LU, const mjtNum* b, const int* pivot, int n) { + // copy b into x + mju_copy(x, b, n); + + // apply row permutation and forward substitution: solve L*y = P*b + for (int i=0; i < n; i++) { + // apply pivot swap + if (pivot[i] != i) { + mjtNum tmp = x[i]; + x[i] = x[pivot[i]]; + x[pivot[i]] = tmp; + } + + // subtract known terms + for (int j=0; j < i; j++) { + x[i] -= LU[i*n+j] * x[j]; + } + } + + // back substitution: solve U*x = y + for (int i=n-1; i >= 0; i--) { + for (int j=i+1; j < n; j++) { + x[i] -= LU[i*n+j] * x[j]; + } + x[i] /= LU[i*n+i]; + } +} + + +//------------------------------ sparse LU factorization ------------------------------------------- // sparse reverse-order LU factorization, no fill-in (assuming tree topology) // result: LU = L + U; original = (U+I) * L; scratch size is n diff --git a/src/engine/engine_util_solve.h b/src/engine/engine_util_solve.h index a50ab4ee..8cdb8ea9 100644 --- a/src/engine/engine_util_solve.h +++ b/src/engine/engine_util_solve.h @@ -94,6 +94,16 @@ MJAPI void mju_bandMulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, // address of diagonal element i in band-dense matrix representation MJAPI int mju_bandDiag(int i, int ntotal, int nband, int ndense); +// dense LU factorization with partial pivoting +// factorizes n x n row-major matrix A in-place into L and U +// L has unit diagonal (not stored), U has explicit diagonal +// pivot stores row permutation: row i of original = row pivot[i] of result +// return 1 if successful, 0 if singular (diagonal element < mjMINVAL) +MJAPI int mju_factorLU(mjtNum* A, int n, int* pivot); + +// solve A*x = b given LU factorization of A, LU and pivot are output of mju_factorLU +MJAPI void mju_solveLU(mjtNum* x, const mjtNum* LU, const mjtNum* b, const int* pivot, int n); + // sparse reverse-order LU factorization, assume tree topology (only dofs in index, if given) // LU = L + U; original = (U+I) * L; scratch is size n void mju_factorLUSparse(mjtNum *LU, int n, int* scratch, diff --git a/test/engine/engine_util_solve_test.cc b/test/engine/engine_util_solve_test.cc index 232297a3..d80abde1 100644 --- a/test/engine/engine_util_solve_test.cc +++ b/test/engine/engine_util_solve_test.cc @@ -1041,5 +1041,147 @@ TEST_F(EngineUtilSolveTest, CholFactorSymbolicNumeric) { mj_deleteModel(model); } +// ----------------------------- dense LU -------------------------------------- + +using DenseLUTest = MujocoTest; + +// factor identity, solve recovers b exactly +TEST_F(DenseLUTest, Identity) { + constexpr int n = 4; + mjtNum A[n*n] = { + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1, + }; + int pivot[n]; + mjtNum b[n] = {1, 2, 3, 4}; + mjtNum x[n]; + + EXPECT_EQ(mju_factorLU(A, n, pivot), 1); + mju_solveLU(x, A, b, pivot, n); + + for (int i = 0; i < n; i++) { + EXPECT_MJTNUM_EQ(x[i], b[i]); + } +} + +// 3x3 with known solution +TEST_F(DenseLUTest, SmallKnown) { + constexpr int n = 3; + // A = [2 1 1; 4 3 3; 8 7 9], b = [1; 1; 1] + // solution: x = [1; -1; 0] (verified: A*x = [2-1; 4-3; 8-7] = [1;1;1]) + mjtNum A[n*n] = { + 2, 1, 1, + 4, 3, 3, + 8, 7, 9, + }; + int pivot[n]; + mjtNum b[n] = {1, 1, 1}; + mjtNum x[n]; + + EXPECT_EQ(mju_factorLU(A, n, pivot), 1); + mju_solveLU(x, A, b, pivot, n); + + mjtNum eps = MjTol(1e-14, 1e-6); + EXPECT_NEAR(x[0], 1, eps); + EXPECT_NEAR(x[1], -1, eps); + EXPECT_NEAR(x[2], 0, eps); +} + +// random SPD matrices: compare LU solve against Cholesky solve +TEST_F(DenseLUTest, RandomSPD) { + std::mt19937_64 rng; + rng.seed(7); + std::normal_distribution dist(0, 1); + + for (int n : {4, 8, 16}) { + vector sqrtH(n * n); + vector A(n * n); + vector A_chol(n * n); + vector b(n); + vector x_lu(n); + vector x_chol(n); + vector pivot(n); + + // generate random SPD matrix + for (int i = 0; i < n * n; i++) sqrtH[i] = dist(rng); + mju_mulMatTMat(A.data(), sqrtH.data(), sqrtH.data(), n, n, n); + + // add diagonal regularizer + for (int i = 0; i < n; i++) A[i*n+i] += n; + + // generate random rhs + for (int i = 0; i < n; i++) b[i] = dist(rng); + + // solve with Cholesky + mju_copy(A_chol.data(), A.data(), n * n); + int rank = mju_cholFactor(A_chol.data(), n, 0); + EXPECT_EQ(rank, n); + mju_cholSolve(x_chol.data(), A_chol.data(), b.data(), n); + + // solve with LU + int ok = mju_factorLU(A.data(), n, pivot.data()); + EXPECT_EQ(ok, 1); + mju_solveLU(x_lu.data(), A.data(), b.data(), pivot.data(), n); + + // compare + mjtNum eps = MjTol(1e-15, 1e-7); + EXPECT_THAT(AsVector(x_lu.data(), n), + Pointwise(MjNear(eps, eps), + AsVector(x_chol.data(), n))); + } +} + +// random non-symmetric matrices: verify A*x == b +TEST_F(DenseLUTest, RandomGeneral) { + std::mt19937_64 rng; + rng.seed(42); + std::normal_distribution dist(0, 1); + + for (int n : {3, 5, 10, 20}) { + vector A(n * n); + vector A_orig(n * n); + vector b(n); + vector x(n); + vector Ax(n); + vector pivot(n); + + // random non-symmetric matrix with diagonal dominance + for (int i = 0; i < n; i++) { + for (int j = 0; j < n; j++) { + A[i*n+j] = dist(rng); + } + A[i*n+i] += 2 * n; + } + mju_copy(A_orig.data(), A.data(), n * n); + + // random rhs + for (int i = 0; i < n; i++) b[i] = dist(rng); + + // factor and solve + int ok = mju_factorLU(A.data(), n, pivot.data()); + EXPECT_EQ(ok, 1); + mju_solveLU(x.data(), A.data(), b.data(), pivot.data(), n); + + // verify: A_orig * x == b + mju_mulMatVec(Ax.data(), A_orig.data(), x.data(), n, n); + + mjtNum eps = MjTol(1e-14, 1e-5); + EXPECT_THAT(AsVector(Ax.data(), n), + Pointwise(MjNear(eps, eps), AsVector(b.data(), n))); + } +} + +// near-singular matrix returns 0 +TEST_F(DenseLUTest, Singular) { + constexpr int n = 3; + // all zeros: maximally singular + mjtNum A[n*n] = {0}; + int pivot[n]; + + EXPECT_EQ(mju_factorLU(A, n, pivot), 0); +} + } // namespace } // namespace mujoco From 0aeea2e4f673b08e217e6ed64c6bcbaadcf90788 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Mon, 4 May 2026 10:32:36 -0700 Subject: [PATCH 181/251] Import google-deepmind/mujoco_warp from GitHub. PiperOrigin-RevId: 910110405 Change-Id: Ia20c749e07b0d92613948313d38161e9f9a074b9 --- .../mjx/third_party/mujoco_warp/__init__.py | 2 + .../third_party/mujoco_warp/_src/benchmark.py | 171 --- .../mujoco_warp/_src/block_cholesky.py | 11 +- .../mjx/third_party/mujoco_warp/_src/bvh.py | 15 +- .../mjx/third_party/mujoco_warp/_src/cli.py | 226 ++++ .../mujoco_warp/_src/collision_flex.py | 227 +++- .../mujoco_warp/_src/constraint.py | 1071 +++++++++-------- .../mujoco_warp/_src/derivative.py | 44 +- .../third_party/mujoco_warp/_src/forward.py | 321 ++++- .../third_party/mujoco_warp/_src/inverse.py | 15 +- .../mjx/third_party/mujoco_warp/_src/io.py | 64 +- .../third_party/mujoco_warp/_src/passive.py | 113 +- .../third_party/mujoco_warp/_src/render.py | 88 +- .../mujoco_warp/_src/render_util.py | 17 +- .../third_party/mujoco_warp/_src/sensor.py | 29 +- .../third_party/mujoco_warp/_src/smooth.py | 64 +- .../third_party/mujoco_warp/_src/solver.py | 63 +- .../third_party/mujoco_warp/_src/support.py | 54 +- .../mjx/third_party/mujoco_warp/_src/types.py | 49 +- .../third_party/mujoco_warp/_src/util_misc.py | 104 ++ .../third_party/mujoco_warp/pyproject.toml | 9 +- .../mjx/third_party/mujoco_warp/viewer.py | 74 +- mjx/mujoco/mjx/warp/bvh.py | 1 - mjx/mujoco/mjx/warp/collision_driver.py | 18 +- mjx/mujoco/mjx/warp/forward.py | 124 +- mjx/mujoco/mjx/warp/render.py | 1 - mjx/mujoco/mjx/warp/smooth.py | 1 - mjx/mujoco/mjx/warp/types.py | 32 + 28 files changed, 2067 insertions(+), 941 deletions(-) delete mode 100644 mjx/mujoco/mjx/third_party/mujoco_warp/_src/benchmark.py create mode 100644 mjx/mujoco/mjx/third_party/mujoco_warp/_src/cli.py diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/__init__.py b/mjx/mujoco/mjx/third_party/mujoco_warp/__init__.py index 40653b62..44224545 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/__init__.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/__init__.py @@ -28,6 +28,7 @@ from mujoco.mjx.third_party.mujoco_warp._src.types import Model as Model from mujoco.mjx.third_party.mujoco_warp._src.types import Data as Data # isort: on + from mujoco.mjx.third_party.mujoco_warp._src.bvh import refit_bvh as refit_bvh from mujoco.mjx.third_party.mujoco_warp._src.collision_driver import collision as collision from mujoco.mjx.third_party.mujoco_warp._src.collision_driver import nxn_broadphase as nxn_broadphase @@ -104,6 +105,7 @@ from mujoco.mjx.third_party.mujoco_warp._src.types import GainType as GainType from mujoco.mjx.third_party.mujoco_warp._src.types import GeomType as GeomType from mujoco.mjx.third_party.mujoco_warp._src.types import IntegratorType as IntegratorType from mujoco.mjx.third_party.mujoco_warp._src.types import JointType as JointType +from mujoco.mjx.third_party.mujoco_warp._src.types import ObjType as ObjType from mujoco.mjx.third_party.mujoco_warp._src.types import Option as Option from mujoco.mjx.third_party.mujoco_warp._src.types import RenderContext as RenderContext from mujoco.mjx.third_party.mujoco_warp._src.types import SolverType as SolverType diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/benchmark.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/benchmark.py deleted file mode 100644 index 293aca63..00000000 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/benchmark.py +++ /dev/null @@ -1,171 +0,0 @@ -# Copyright 2025 The Newton Developers -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# ============================================================================== - -"""Utilities for benchmarking MuJoCo Warp.""" - -import time -from typing import Callable, Tuple - -import numpy as np -import warp as wp - -from mujoco.mjx.third_party.mujoco_warp._src import warp_util -from mujoco.mjx.third_party.mujoco_warp._src.types import Data -from mujoco.mjx.third_party.mujoco_warp._src.types import Model -from mujoco.mjx.third_party.mujoco_warp._src.types import RenderContext -from mujoco.mjx.third_party.mujoco_warp._src.util_misc import halton - - -def _sum(stack1, stack2): - ret = {} - for k in stack1: - times1, sub_stack1 = stack1[k] - times2, sub_stack2 = stack2[k] - times = [t1 + t2 for t1, t2 in zip(times1, times2)] - ret[k] = (times, _sum(sub_stack1, sub_stack2)) - return ret - - -@wp.kernel -def ctrl_noise( - # Model: - opt_timestep: wp.array[float], - actuator_ctrllimited: wp.array[bool], - actuator_ctrlrange: wp.array2d[wp.vec2], - # Data in: - ctrl_in: wp.array2d[float], - # In: - ctrl_center: wp.array[float], - step: int, - ctrlnoisestd: float, - ctrlnoiserate: float, - # Data out: - ctrl_out: wp.array2d[float], -): - worldid, actid = wp.tid() - - # convert rate and scale to discrete time (Ornstein-Uhlenbeck) - rate = wp.exp(-opt_timestep[worldid % opt_timestep.shape[0]] / ctrlnoiserate) - scale = ctrlnoisestd * wp.sqrt(1.0 - rate * rate) - - midpoint = 0.0 - halfrange = 1.0 - ctrlrange = actuator_ctrlrange[worldid % actuator_ctrlrange.shape[0], actid] - is_limited = actuator_ctrllimited[actid] - if is_limited: - midpoint = 0.5 * (ctrlrange[1] + ctrlrange[0]) - halfrange = 0.5 * (ctrlrange[1] - ctrlrange[0]) - if ctrl_center.shape[0] > 0: - midpoint = ctrl_center[actid] - - # exponential convergence to midpoint at ctrlnoiserate - ctrl = rate * ctrl_in[worldid, actid] + (1.0 - rate) * midpoint - - # add noise - ctrl += scale * halfrange * (2.0 * halton((step + 1) * (worldid + 1), actid + 2) - 1.0) - - # clip to range if limited - if is_limited: - ctrl = wp.clamp(ctrl, ctrlrange[0], ctrlrange[1]) - - ctrl_out[worldid, actid] = ctrl - - -def benchmark( - fn: Callable[[Model, Data], None], - m: Model, - d: Data, - nstep: int, - ctrls: np.ndarray | None = None, - event_trace: bool = False, - measure_alloc: bool = False, - measure_solver_niter: bool = False, - render_context: RenderContext | None = None, -) -> Tuple[float, float, dict, list, list, list, int]: - """Benchmark a function of Model and Data. - - Args: - fn: Function to benchmark. - m: The model containing kinematic and dynamic information (device). - d: The data object containing the current state and output information (device). - nstep: Number of timesteps. - ctrls: Control sequence to apply during benchmarking. - event_trace: If True, time routines decorated with @event_scope. - measure_alloc: If True, record number of contacts and constraints. - measure_solver_niter: If True, record the number of solver iterations. - render_context: The render context to use for rendering. - - Returns: - - Time to JIT fn. - - Total time to run the benchmark. - - Trace. - - Number of contacts. - - Number of constraints. - - Number of solver iterations. - - Number of converged worlds. - """ - trace = {} - nacon, nefc, solver_niter = [], [], [] - center = wp.array([], dtype=wp.float32) - - with warp_util.EventTracer(enabled=event_trace) as tracer: - # capture the whole function as a CUDA graph - jit_beg = time.perf_counter() - - if render_context is not None: - with wp.ScopedCapture() as capture: - fn(m, d, render_context) - else: - with wp.ScopedCapture() as capture: - fn(m, d) - - jit_end = time.perf_counter() - jit_duration = jit_end - jit_beg - - graph = capture.graph - - time_vec = np.zeros(nstep) - for i in range(nstep): - with wp.ScopedStream(wp.get_stream()): - if ctrls is not None: - center = wp.array(ctrls[i], dtype=wp.float32) - wp.launch( - ctrl_noise, - dim=(d.nworld, m.nu), - inputs=[m.opt.timestep, m.actuator_ctrllimited, m.actuator_ctrlrange, d.ctrl, center, i, 0.01, 0.1], - outputs=[d.ctrl], - ) - wp.synchronize() - - run_beg = time.perf_counter() - wp.capture_launch(graph) - wp.synchronize() - run_end = time.perf_counter() - - time_vec[i] = run_end - run_beg - if trace: - trace = _sum(trace, tracer.trace()) - else: - trace = tracer.trace() - if measure_alloc: - nacon.append(np.max([d.nacon.numpy()[0], d.ncollision.numpy()[0]])) - nefc.append(np.max(d.nefc.numpy())) - if measure_solver_niter: - solver_niter.append(d.solver_niter.numpy()) - - nsuccess = np.sum(~np.any(np.isnan(d.qpos.numpy()), axis=1)) - run_duration = np.sum(time_vec) - - return jit_duration, run_duration, trace, nacon, nefc, solver_niter, nsuccess diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/block_cholesky.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/block_cholesky.py index 628ad5a0..855f9686 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/block_cholesky.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/block_cholesky.py @@ -45,8 +45,8 @@ def create_blocked_cholesky_func(block_size: int): wp.tile_matmul(L_block, wp.tile_transpose(L_block), A_kk_tile, alpha=-1.0) # Compute the Cholesky factorization for the block - L_kk_tile = wp.tile_cholesky(A_kk_tile) - wp.tile_store(L, L_kk_tile, offset=(k, k)) + wp.tile_cholesky_inplace(A_kk_tile) + wp.tile_store(L, A_kk_tile, offset=(k, k)) # Process the blocks below the current block for i in range(end, matrix_size, block_size): @@ -57,7 +57,7 @@ def create_blocked_cholesky_func(block_size: int): L_2_tile = wp.tile_load(L, shape=(block_size, block_size), offset=(k, j), storage="shared") wp.tile_matmul(L_tile, wp.tile_transpose(L_2_tile), A_ik_tile, alpha=-1.0) - wp.tile_lower_solve_inplace(L_kk_tile, wp.tile_transpose(A_ik_tile)) + wp.tile_lower_solve_inplace(A_kk_tile, wp.tile_transpose(A_ik_tile)) wp.tile_store(L, A_ik_tile, offset=(i, k)) return blocked_cholesky_func @@ -98,11 +98,12 @@ def create_blocked_cholesky_solve_func(block_size: int, matrix_size_static: int) tmp_tile = wp.tile_view(rhs_tile, shape=(block_size, 1), offset=(i, 0)) for j in range(i_end, matrix_size, block_size): L_tile = wp.tile_load(L, shape=(block_size, block_size), offset=(j, i), storage="shared") - x_tile = wp.tile_load(x, shape=(block_size, 1), offset=(j, 0), storage="shared", bounds_check=False) + x_tile = wp.tile_view(rhs_tile, shape=(block_size, 1), offset=(j, 0)) wp.tile_matmul(wp.tile_transpose(L_tile), x_tile, tmp_tile, alpha=-1.0) L_tile = wp.tile_load(L, shape=(block_size, block_size), offset=(i, i), storage="shared") wp.tile_upper_solve_inplace(wp.tile_transpose(L_tile), tmp_tile) - wp.tile_store(x, tmp_tile, offset=(i, 0), bounds_check=False) + + wp.tile_store(x, rhs_tile, offset=(0, 0), bounds_check=False) return blocked_cholesky_solve_func diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/bvh.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/bvh.py index 32899e07..37823711 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/bvh.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/bvh.py @@ -245,6 +245,19 @@ def compute_bvh_group_roots( group_root_out[tid] = root +# Warp exposes mesh group-root lookup as a kernel builtin in this version. +@wp.kernel +def compute_mesh_group_roots( + # In: + mesh_id: wp.uint64, + # Out: + group_root_out: wp.array[int], +): + tid = wp.tid() + root = wp.mesh_get_group_root(mesh_id, tid) + group_root_out[tid] = root + + @wp.kernel def _compute_flex_bvh_bounds( # Model: @@ -1083,7 +1096,7 @@ def build_flex_bvh( group_root = wp.empty(nworld, dtype=int) wp.launch( - kernel=compute_bvh_group_roots, + kernel=compute_mesh_group_roots, dim=nworld, inputs=[flex_mesh.id], outputs=[group_root], diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/cli.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/cli.py new file mode 100644 index 00000000..912df58f --- /dev/null +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/cli.py @@ -0,0 +1,226 @@ +# Copyright 2026 The Newton Developers +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== + +"""Shared utilities and flags for MJWarp CLI tools.""" + +import time +from typing import Callable, Tuple, get_type_hints + +import mujoco +import numpy as np +import warp as wp +from absl import app +from absl import flags +from etils import epath + +import mujoco.mjx.third_party.mujoco_warp as mjw +from mujoco.mjx.third_party.mujoco_warp._src import warp_util +from mujoco.mjx.third_party.mujoco_warp._src.io import find_keys +from mujoco.mjx.third_party.mujoco_warp._src.io import make_trajectory +from mujoco.mjx.third_party.mujoco_warp._src.io import override_model +from mujoco.mjx.third_party.mujoco_warp._src.util_misc import halton + +# shared flags for cli tool +NWORLD = flags.DEFINE_integer("nworld", 8192, "number of parallel rollouts") +NSTEP = flags.DEFINE_integer("nstep", 1000, "number of steps per rollout") +NCONMAX = flags.DEFINE_integer("nconmax", None, "override maximum number of contacts per world") +NJMAX = flags.DEFINE_integer("njmax", None, "override maximum number of constraints per world") +NJMAX_NNZ = flags.DEFINE_integer("njmax_nnz", None, "override maximum number of non-zeros in constraint Jacobian") +NCCDMAX = flags.DEFINE_integer("nccdmax", None, "override maximum number of CCD contacts per world") +OVERRIDE = flags.DEFINE_multi_string("override", [], "Model overrides (notation: foo.bar = baz)", short_name="o") +KEYFRAME = flags.DEFINE_integer("keyframe", 0, "keyframe to initialize simulation.") +EVENT_TRACE = flags.DEFINE_bool("event_trace", False, "print an event trace report") +NOISE_STD = flags.DEFINE_float("noise_std", 0.01, "add noise to ctrl signal (standard deviation)") +NOISE_RATE = flags.DEFINE_float("noise_rate", 0.1, "add noise to ctrl signal (noise rate)") + +DEVICE = flags.DEFINE_string("device", None, "override the default Warp device") +REPLAY = flags.DEFINE_string("replay", None, "keyframe sequence to replay, keyframe name must prefix match") + +RENDER_WIDTH = flags.DEFINE_integer("render_width", 64, "render width (pixels)") +RENDER_HEIGHT = flags.DEFINE_integer("render_height", 64, "render height (pixels)") +RENDER_RGB = flags.DEFINE_bool("render_rgb", True, "render RGB image") +RENDER_DEPTH = flags.DEFINE_bool("render_depth", True, "render depth image") +RENDER_TEXTURES = flags.DEFINE_bool("render_textures", True, "use textures") +RENDER_SHADOWS = flags.DEFINE_bool("render_shadows", False, "use shadows") + + +def load_model(path: epath.Path) -> mujoco.MjModel: + """Load a MuJoCo model from a path, handling resources and plugins.""" + if not path.exists(): + resource_path = epath.resource_path("mjx") / "third_party/mujoco_warp" / path + if not resource_path.exists(): + raise FileNotFoundError(f"file not found: {path}\nalso tried: {resource_path}") + path = resource_path + + if path.suffix == ".mjb": + return mujoco.MjModel.from_binary_path(path.as_posix()) + + spec = mujoco.MjSpec.from_file(path.as_posix()) + if any(p.plugin_name.startswith("mujoco.sdf") for p in spec.plugins): + from mujoco.mjx.third_party.mujoco_warp.test_data.collision_sdf.utils import register_sdf_plugins as register_sdf_plugins + + register_sdf_plugins(mjw) + + mjm = spec.compile() + + if OVERRIDE.value: + override_model(mjm, OVERRIDE.value) + + return mjm + + +@wp.kernel +def _ctrl_noise( + # Model: + opt_timestep: wp.array[float], + actuator_ctrllimited: wp.array[bool], + actuator_ctrlrange: wp.array2d[wp.vec2], + # Data in: + ctrl_in: wp.array2d[float], + # In: + ctrl_center: wp.array[float], + step: int, + ctrlnoisestd: float, + ctrlnoiserate: float, + # Data out: + ctrl_out: wp.array2d[float], +): + worldid, actid = wp.tid() + + # convert rate and scale to discrete time (Ornstein-Uhlenbeck) + rate = wp.exp(-opt_timestep[worldid % opt_timestep.shape[0]] / ctrlnoiserate) + scale = ctrlnoisestd * wp.sqrt(1.0 - rate * rate) + + midpoint = 0.0 + halfrange = 1.0 + ctrlrange = actuator_ctrlrange[worldid % actuator_ctrlrange.shape[0], actid] + is_limited = actuator_ctrllimited[actid] + if is_limited: + midpoint = 0.5 * (ctrlrange[1] + ctrlrange[0]) + halfrange = 0.5 * (ctrlrange[1] - ctrlrange[0]) + if ctrl_center.shape[0] > 0: + midpoint = ctrl_center[actid] + + # exponential convergence to midpoint at ctrlnoiserate + ctrl = rate * ctrl_in[worldid, actid] + (1.0 - rate) * midpoint + + # add noise + ctrl += scale * halfrange * (2.0 * halton((step + 1) * (worldid + 1), actid + 2) - 1.0) + + # clip to range if limited + if is_limited: + ctrl = wp.clamp(ctrl, ctrlrange[0], ctrlrange[1]) + + ctrl_out[worldid, actid] = ctrl + + +def init_structs( + fn: Callable[..., None], mjm: mujoco.MjModel +) -> Tuple[mjw.Model, mjw.Data, mjw.RenderContext | None, list[np.ndarray] | None]: + """Initialize device structs.""" + mjd = mujoco.MjData(mjm) + ctrls = None + if REPLAY.value: + keys = find_keys(mjm, REPLAY.value) + if not keys: + raise app.UsageError(f"Key prefix not found: {REPLAY.value}") + ctrls = make_trajectory(mjm, keys) + mujoco.mj_resetDataKeyframe(mjm, mjd, keys[0]) + elif mjm.nkey > 0 and KEYFRAME.value > -1: + mujoco.mj_resetDataKeyframe(mjm, mjd, KEYFRAME.value) + ctrls = [mjd.ctrl.copy() for _ in range(NSTEP.value)] + + with wp.ScopedDevice(wp.get_device(DEVICE.value)): + m = mjw.put_model(mjm) + if OVERRIDE.value: + override_model(m, OVERRIDE.value) + d = mjw.put_data( + mjm, mjd, nworld=NWORLD.value, nconmax=NCONMAX.value, njmax=NJMAX.value, njmax_nnz=NJMAX_NNZ.value, nccdmax=NCCDMAX.value + ) + + if mjw.RenderContext not in get_type_hints(fn).values(): + return m, d, None, ctrls + + rc = mjw.create_render_context( + mjm, + NWORLD.value, + (RENDER_WIDTH.value, RENDER_HEIGHT.value), + RENDER_RGB.value, + RENDER_DEPTH.value, + RENDER_TEXTURES.value, + RENDER_SHADOWS.value, + ) + + return m, d, rc, ctrls + + +def unroll( + fn: Callable[..., None], + m: mjw.Model, + d: mjw.Data, + rc: mjw.RenderContext | None, + callback: Callable[[int, dict, float], None] | None = None, + ctrls: list[np.ndarray] | None = None, +) -> dict: + """Unroll a function on batched Data and return some statistics. + + Args: + fn: Function to unroll (e.g. mjw.step). + m: Model. + d: Data. + rc: Render context (optional). + callback: Optional callback called after each step with (step count, trace, latency). + ctrls: Optional control trajectory. + + Returns: + jit_duration: Time to JIT capture the function. + """ + with wp.ScopedDevice(wp.get_device(DEVICE.value)): + with warp_util.EventTracer(enabled=EVENT_TRACE.value) as tracer: + jit_beg = time.perf_counter() + with wp.ScopedCapture() as capture: + fn(*(m, d) if rc is None else (m, d, rc)) + jit_end = time.perf_counter() + + for i in range(NSTEP.value): + with wp.ScopedStream(wp.get_stream()): + if ctrls is not None: + center = wp.array(ctrls[i], dtype=wp.float32) + wp.launch( + _ctrl_noise, + dim=(d.nworld, m.nu), + inputs=[ + m.opt.timestep, + m.actuator_ctrllimited, + m.actuator_ctrlrange, + d.ctrl, + center, + i, + NOISE_STD.value, + NOISE_RATE.value, + ], + outputs=[d.ctrl], + ) + wp.synchronize() + + run_beg = time.perf_counter() + wp.capture_launch(capture.graph) + wp.synchronize() + run_end = time.perf_counter() + + if callback: + callback(i, tracer.trace(), run_end - run_beg) + + return jit_end - jit_beg diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_flex.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_flex.py index cd5c9087..af166dd8 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_flex.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_flex.py @@ -19,6 +19,7 @@ from mujoco.mjx.third_party.mujoco_warp._src import collision_primitive_core from mujoco.mjx.third_party.mujoco_warp._src.math import make_frame from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXVAL from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINMU +from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINVAL from mujoco.mjx.third_party.mujoco_warp._src.types import Data from mujoco.mjx.third_party.mujoco_warp._src.types import GeomType from mujoco.mjx.third_party.mujoco_warp._src.types import Model @@ -28,6 +29,93 @@ from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope wp.set_module_options({"enable_backward": False}) +# TODO(team): generalize into a shared contact parameter mixing function +# (mj_contactParam) that works for both geom-geom and geom-flex contacts. +@wp.func +def _mix_flex_contact_params( + # In: + a_condim: int, + a_priority: int, + a_solmix: float, + a_solref: wp.vec2, + a_solimp: vec5, + a_friction: wp.vec3, + a_gap: float, + b_condim: int, + b_priority: int, + b_solmix: float, + b_solref: wp.vec2, + b_solimp: vec5, + b_friction: wp.vec3, + b_gap: float, +): + """Mix contact parameters between geom and flex, matching mj_contactParam.""" + gap = a_gap + b_gap + + if a_priority > b_priority: + condim = a_condim + solref = a_solref + solimp = a_solimp + fri = a_friction + elif a_priority < b_priority: + condim = b_condim + solref = b_solref + solimp = b_solimp + fri = b_friction + else: + # same priority + condim = wp.max(a_condim, b_condim) + + # compute solver mix factor + if a_solmix >= MJ_MINVAL and b_solmix >= MJ_MINVAL: + mix = a_solmix / (a_solmix + b_solmix) + elif a_solmix < MJ_MINVAL and b_solmix < MJ_MINVAL: + mix = 0.5 + elif a_solmix < MJ_MINVAL: + mix = 0.0 + else: + mix = 1.0 + + # solref: mix if both standard, min if either direct + if a_solref[0] > 0.0 and b_solref[0] > 0.0: + solref = wp.vec2( + mix * a_solref[0] + (1.0 - mix) * b_solref[0], + mix * a_solref[1] + (1.0 - mix) * b_solref[1], + ) + else: + solref = wp.vec2( + wp.min(a_solref[0], b_solref[0]), + wp.min(a_solref[1], b_solref[1]), + ) + + # solimp: mix + solimp = vec5( + mix * a_solimp[0] + (1.0 - mix) * b_solimp[0], + mix * a_solimp[1] + (1.0 - mix) * b_solimp[1], + mix * a_solimp[2] + (1.0 - mix) * b_solimp[2], + mix * a_solimp[3] + (1.0 - mix) * b_solimp[3], + mix * a_solimp[4] + (1.0 - mix) * b_solimp[4], + ) + + # friction: max + fri = wp.vec3( + wp.max(a_friction[0], b_friction[0]), + wp.max(a_friction[1], b_friction[1]), + wp.max(a_friction[2], b_friction[2]), + ) + + # unpack 5D friction with MJ_MINMU floor + friction = vec5( + wp.max(MJ_MINMU, fri[0]), + wp.max(MJ_MINMU, fri[0]), + wp.max(MJ_MINMU, fri[1]), + wp.max(MJ_MINMU, fri[2]), + wp.max(MJ_MINMU, fri[2]), + ) + + return condim, gap, solref, solimp, friction + + @wp.func def _write_flex_contact( # Data in: @@ -264,13 +352,21 @@ def _flex_plane_narrowphase( nflexvert: int, geom_type: wp.array[int], geom_condim: wp.array[int], + geom_priority: wp.array[int], + geom_solmix: wp.array2d[float], geom_solref: wp.array2d[wp.vec2], geom_solimp: wp.array2d[vec5], geom_friction: wp.array2d[wp.vec3], geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], flex_condim: wp.array[int], + flex_priority: wp.array[int], + flex_solmix: wp.array[float], + flex_solref: wp.array[wp.vec2], + flex_solimp: wp.array[vec5], flex_friction: wp.array[wp.vec3], flex_margin: wp.array[float], + flex_gap: wp.array[float], flex_vertadr: wp.array[int], flex_radius: wp.array[float], flex_vertflexid: wp.array[int], @@ -303,8 +399,6 @@ def _flex_plane_narrowphase( flexid = flex_vertflexid[vertid] radius = flex_radius[flexid] flex_margin_val = flex_margin[flexid] - flex_condim_val = flex_condim[flexid] - flex_fric = flex_friction[flexid] # Convert global vertid to local vertex index within this flex local_vertid = vertid - flex_vertadr[flexid] @@ -327,20 +421,21 @@ def _flex_plane_narrowphase( dist = signed_dist - radius if dist < margin: - geom_condim_val = geom_condim[geomid] - condim = wp.max(geom_condim_val, flex_condim_val) - solref = geom_solref[worldid % geom_solref.shape[0], geomid] - solimp = geom_solimp[worldid % geom_solimp.shape[0], geomid] - geom_fric = geom_friction[worldid % geom_friction.shape[0], geomid] - fric0 = wp.max(geom_fric[0], flex_fric[0]) - fric1 = wp.max(geom_fric[1], flex_fric[1]) - fric2 = wp.max(geom_fric[2], flex_fric[2]) - friction = vec5( - wp.max(MJ_MINMU, fric0), - wp.max(MJ_MINMU, fric0), - wp.max(MJ_MINMU, fric1), - wp.max(MJ_MINMU, fric2), - wp.max(MJ_MINMU, fric2), + condim, gap, solref, solimp, friction = _mix_flex_contact_params( + geom_condim[geomid], + geom_priority[geomid], + geom_solmix[worldid % geom_solmix.shape[0], geomid], + geom_solref[worldid % geom_solref.shape[0], geomid], + geom_solimp[worldid % geom_solimp.shape[0], geomid], + geom_friction[worldid % geom_friction.shape[0], geomid], + geom_gap[worldid % geom_gap.shape[0], geomid], + flex_condim[flexid], + flex_priority[flexid], + flex_solmix[flexid], + flex_solref[flexid], + flex_solimp[flexid], + flex_friction[flexid], + flex_gap[flexid], ) contact_pos = vert - plane_normal * (dist * 0.5 + radius) @@ -349,7 +444,7 @@ def _flex_plane_narrowphase( dist, contact_pos, make_frame(plane_normal), - margin, + margin - gap, condim, friction, solref, @@ -386,14 +481,24 @@ def _flex_narrowphase_dim2( geom_contype: wp.array[int], geom_conaffinity: wp.array[int], geom_condim: wp.array[int], + geom_priority: wp.array[int], + geom_solmix: wp.array2d[float], geom_solref: wp.array2d[wp.vec2], geom_solimp: wp.array2d[vec5], geom_size: wp.array2d[wp.vec3], geom_friction: wp.array2d[wp.vec3], geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], flex_contype: wp.array[int], flex_conaffinity: wp.array[int], + flex_condim: wp.array[int], + flex_priority: wp.array[int], + flex_solmix: wp.array[float], + flex_solref: wp.array[wp.vec2], + flex_solimp: wp.array[vec5], + flex_friction: wp.array[wp.vec3], flex_margin: wp.array[float], + flex_gap: wp.array[float], flex_dim: wp.array[int], flex_vertadr: wp.array[int], flex_elemadr: wp.array[int], @@ -478,17 +583,22 @@ def _flex_narrowphase_dim2( geom_rot = geom_xmat_in[worldid, geomid] geom_size_val = geom_size[worldid % geom_size.shape[0], geomid] - condim = geom_condim[geomid] - gf = geom_friction[worldid % geom_friction.shape[0], geomid] - friction = vec5( - wp.max(MJ_MINMU, gf[0]), - wp.max(MJ_MINMU, gf[0]), - wp.max(MJ_MINMU, gf[1]), - wp.max(MJ_MINMU, gf[2]), - wp.max(MJ_MINMU, gf[2]), + condim, gap, solref, solimp, friction = _mix_flex_contact_params( + geom_condim[geomid], + geom_priority[geomid], + geom_solmix[worldid % geom_solmix.shape[0], geomid], + geom_solref[worldid % geom_solref.shape[0], geomid], + geom_solimp[worldid % geom_solimp.shape[0], geomid], + geom_friction[worldid % geom_friction.shape[0], geomid], + geom_gap[worldid % geom_gap.shape[0], geomid], + flex_condim[flexid], + flex_priority[flexid], + flex_solmix[flexid], + flex_solref[flexid], + flex_solimp[flexid], + flex_friction[flexid], + flex_gap[flexid], ) - solref = geom_solref[worldid % geom_solref.shape[0], geomid] - solimp = geom_solimp[worldid % geom_solimp.shape[0], geomid] _collide_geom_triangle( naconmax_in, @@ -537,14 +647,24 @@ def _flex_narrowphase_dim3( geom_contype: wp.array[int], geom_conaffinity: wp.array[int], geom_condim: wp.array[int], + geom_priority: wp.array[int], + geom_solmix: wp.array2d[float], geom_solref: wp.array2d[wp.vec2], geom_solimp: wp.array2d[vec5], geom_size: wp.array2d[wp.vec3], geom_friction: wp.array2d[wp.vec3], geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], flex_contype: wp.array[int], flex_conaffinity: wp.array[int], + flex_condim: wp.array[int], + flex_priority: wp.array[int], + flex_solmix: wp.array[float], + flex_solref: wp.array[wp.vec2], + flex_solimp: wp.array[vec5], + flex_friction: wp.array[wp.vec3], flex_margin: wp.array[float], + flex_gap: wp.array[float], flex_dim: wp.array[int], flex_vertadr: wp.array[int], flex_shellnum: wp.array[int], @@ -632,17 +752,22 @@ def _flex_narrowphase_dim3( geom_rot = geom_xmat_in[worldid, geomid] geom_size_val = geom_size[worldid % geom_size.shape[0], geomid] - condim = geom_condim[geomid] - gf = geom_friction[worldid % geom_friction.shape[0], geomid] - friction = vec5( - wp.max(MJ_MINMU, gf[0]), - wp.max(MJ_MINMU, gf[0]), - wp.max(MJ_MINMU, gf[1]), - wp.max(MJ_MINMU, gf[2]), - wp.max(MJ_MINMU, gf[2]), + condim, gap, solref, solimp, friction = _mix_flex_contact_params( + geom_condim[geomid], + geom_priority[geomid], + geom_solmix[worldid % geom_solmix.shape[0], geomid], + geom_solref[worldid % geom_solref.shape[0], geomid], + geom_solimp[worldid % geom_solimp.shape[0], geomid], + geom_friction[worldid % geom_friction.shape[0], geomid], + geom_gap[worldid % geom_gap.shape[0], geomid], + flex_condim[flexid], + flex_priority[flexid], + flex_solmix[flexid], + flex_solref[flexid], + flex_solimp[flexid], + flex_friction[flexid], + flex_gap[flexid], ) - solref = geom_solref[worldid % geom_solref.shape[0], geomid] - solimp = geom_solimp[worldid % geom_solimp.shape[0], geomid] _collide_geom_triangle( naconmax_in, @@ -698,14 +823,24 @@ def flex_narrowphase(m: Model, d: Data): m.geom_contype, m.geom_conaffinity, m.geom_condim, + m.geom_priority, + m.geom_solmix, m.geom_solref, m.geom_solimp, m.geom_size, m.geom_friction, m.geom_margin, + m.geom_gap, m.flex_contype, m.flex_conaffinity, + m.flex_condim, + m.flex_priority, + m.flex_solmix, + m.flex_solref, + m.flex_solimp, + m.flex_friction, m.flex_margin, + m.flex_gap, m.flex_dim, m.flex_vertadr, m.flex_elemadr, @@ -749,14 +884,24 @@ def flex_narrowphase(m: Model, d: Data): m.geom_contype, m.geom_conaffinity, m.geom_condim, + m.geom_priority, + m.geom_solmix, m.geom_solref, m.geom_solimp, m.geom_size, m.geom_friction, m.geom_margin, + m.geom_gap, m.flex_contype, m.flex_conaffinity, + m.flex_condim, + m.flex_priority, + m.flex_solmix, + m.flex_solref, + m.flex_solimp, + m.flex_friction, m.flex_margin, + m.flex_gap, m.flex_dim, m.flex_vertadr, m.flex_shellnum, @@ -797,13 +942,21 @@ def flex_narrowphase(m: Model, d: Data): m.nflexvert, m.geom_type, m.geom_condim, + m.geom_priority, + m.geom_solmix, m.geom_solref, m.geom_solimp, m.geom_friction, m.geom_margin, + m.geom_gap, m.flex_condim, + m.flex_priority, + m.flex_solmix, + m.flex_solref, + m.flex_solimp, m.flex_friction, m.flex_margin, + m.flex_gap, m.flex_vertadr, m.flex_radius, m.flex_vertflexid, diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/constraint.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/constraint.py index 788f1af5..59a7ad72 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/constraint.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/constraint.py @@ -144,6 +144,7 @@ def _equality_connect( eq_solimp: wp.array2d[vec5], eq_data: wp.array2d[vec11], is_sparse: bool, + body_isdofancestor: wp.array2d[int], eq_connect_adr: wp.array[int], # Data in: qvel_in: wp.array2d[float], @@ -259,6 +260,7 @@ def _equality_connect( body_parentid, body_rootid, dof_bodyid, + body_isdofancestor, subtree_com_in, cdof_in, pos1, @@ -270,6 +272,7 @@ def _equality_connect( body_parentid, body_rootid, dof_bodyid, + body_isdofancestor, subtree_com_in, cdof_in, pos2, @@ -301,6 +304,7 @@ def _equality_connect( body_parentid, body_rootid, dof_bodyid, + body_isdofancestor, subtree_com_in, cdof_in, pos1, @@ -312,6 +316,7 @@ def _equality_connect( body_parentid, body_rootid, dof_bodyid, + body_isdofancestor, subtree_com_in, cdof_in, pos2, @@ -694,6 +699,7 @@ def _equality_flex(is_sparse: bool): eq_flex_adr: wp.array[int], # Data in: qvel_in: wp.array2d[float], + eq_active_in: wp.array2d[bool], flexedge_J_in: wp.array2d[float], flexedge_length_in: wp.array2d[float], njmax_in: int, @@ -718,6 +724,10 @@ def _equality_flex(is_sparse: bool): ): worldid, eqflexid, edgeid = wp.tid() eqid = eq_flex_adr[eqflexid] + + if not eq_active_in[worldid, eqid]: + return + flexid = eq_obj1id[eqid] if edgeid < flex_edgeadr[flexid] or edgeid >= flex_edgeadr[flexid] + flex_edgenum[flexid]: return @@ -813,6 +823,7 @@ def _equality_weld( eq_solimp: wp.array2d[vec5], eq_data: wp.array2d[vec11], is_sparse: bool, + body_isdofancestor: wp.array2d[int], eq_wld_adr: wp.array[int], # Data in: qvel_in: wp.array2d[float], @@ -947,6 +958,7 @@ def _equality_weld( body_parentid, body_rootid, dof_bodyid, + body_isdofancestor, subtree_com_in, cdof_in, pos1, @@ -958,6 +970,7 @@ def _equality_weld( body_parentid, body_rootid, dof_bodyid, + body_isdofancestor, subtree_com_in, cdof_in, pos2, @@ -1003,6 +1016,7 @@ def _equality_weld( body_parentid, body_rootid, dof_bodyid, + body_isdofancestor, subtree_com_in, cdof_in, pos1, @@ -1014,6 +1028,7 @@ def _equality_weld( body_parentid, body_rootid, dof_bodyid, + body_isdofancestor, subtree_com_in, cdof_in, pos2, @@ -1665,97 +1680,181 @@ def _limit_tendon( ) -@wp.kernel -def _contact_pyramidal( - # Model: - nv: int, - opt_timestep: wp.array[float], - opt_disableflags: int, - opt_impratio_invsqrt: wp.array[float], - body_parentid: wp.array[int], - body_rootid: wp.array[int], - body_weldid: wp.array[int], - body_dofnum: wp.array[int], - body_dofadr: wp.array[int], - body_invweight0: wp.array2d[wp.vec2], - dof_bodyid: wp.array[int], - dof_parentid: wp.array[int], - geom_bodyid: wp.array[int], - flex_vertadr: wp.array[int], - flex_vertbodyid: wp.array[int], - is_sparse: bool, - # Data in: - qvel_in: wp.array2d[float], - subtree_com_in: wp.array2d[wp.vec3], - cdof_in: wp.array2d[wp.spatial_vector], - njmax_in: int, - njmax_nnz_in: int, - nacon_in: wp.array[int], - # In: - dist_in: wp.array[float], - condim_in: wp.array[int], - includemargin_in: wp.array[float], - worldid_in: wp.array[int], - geom_in: wp.array[wp.vec2i], - flex_in: wp.array[wp.vec2i], - vert_in: wp.array[wp.vec2i], - pos_in: wp.array[wp.vec3], - frame_in: wp.array[wp.mat33], - friction_in: wp.array[vec5], - solref_in: wp.array[wp.vec2], - solimp_in: wp.array[vec5], - type_in: wp.array[int], - # Data out: - nefc_out: wp.array[int], - contact_efc_address_out: wp.array2d[int], - efc_type_out: wp.array2d[int], - efc_id_out: wp.array2d[int], - efc_J_rownnz_out: wp.array2d[int], - efc_J_rowadr_out: wp.array2d[int], - efc_J_colind_out: wp.array3d[int], - efc_J_out: wp.array3d[float], - efc_pos_out: wp.array2d[float], - efc_margin_out: wp.array2d[float], - efc_D_out: wp.array2d[float], - efc_vel_out: wp.array2d[float], - efc_aref_out: wp.array2d[float], - efc_frictionloss_out: wp.array2d[float], - # Out: - efc_nnz_out: wp.array[int], -): - conid, dimid = wp.tid() +def _efc_contact_init(cone_type: types.ConeType, is_sparse: bool): + IS_ELLIPTIC = cone_type == types.ConeType.ELLIPTIC + IS_SPARSE = is_sparse - if conid >= nacon_in[0]: - return + @wp.kernel(module="unique", enable_backward=False) + def kernel( + # Model: + body_weldid: wp.array[int], + body_dofnum: wp.array[int], + body_dofadr: wp.array[int], + dof_parentid: wp.array[int], + geom_bodyid: wp.array[int], + flex_vertadr: wp.array[int], + flex_vertbodyid: wp.array[int], + # Data in: + njmax_in: int, + njmax_nnz_in: int, + nacon_in: wp.array[int], + # In: + dist_in: wp.array[float], + condim_in: wp.array[int], + includemargin_in: wp.array[float], + worldid_in: wp.array[int], + geom_in: wp.array[wp.vec2i], + flex_in: wp.array[wp.vec2i], + vert_in: wp.array[wp.vec2i], + type_in: wp.array[int], + # Data out: + nefc_out: wp.array[int], + contact_efc_address_out: wp.array2d[int], + efc_id_out: wp.array2d[int], + efc_J_rownnz_out: wp.array2d[int], + efc_J_rowadr_out: wp.array2d[int], + # Out: + efc_nnz_out: wp.array[int], + ): + conid = wp.tid() - if not type_in[conid] & ContactType.CONSTRAINT: - return - - condim = condim_in[conid] - - if condim == 1 and dimid > 0: - return - elif condim > 1 and dimid >= 2 * (condim - 1): - return - - includemargin = includemargin_in[conid] - pos = dist_in[conid] - includemargin - active = pos < 0 - - if active: - worldid = worldid_in[conid] - - efcid = wp.atomic_add(nefc_out, worldid, 1) - if efcid >= njmax_in: - contact_efc_address_out[conid, dimid] = -1 + if conid >= nacon_in[0]: return - timestep = opt_timestep[worldid % opt_timestep.shape[0]] - impratio_invsqrt = opt_impratio_invsqrt[worldid % opt_impratio_invsqrt.shape[0]] - contact_efc_address_out[conid, dimid] = efcid + if not type_in[conid] & ContactType.CONSTRAINT: + return + + condim = condim_in[conid] + + includemargin = includemargin_in[conid] + pos = dist_in[conid] - includemargin + active = pos < 0 + + if not active: + return + + if wp.static(IS_ELLIPTIC): + ndim = condim + else: + if condim == 1: + ndim = 1 + else: + ndim = 2 * (condim - 1) + + worldid = worldid_in[conid] + + # Allocate contiguous block of efcids for all dimids + base_efcid = wp.atomic_add(nefc_out, worldid, ndim) + for dim in range(ndim): + efcid = base_efcid + dim + if efcid >= njmax_in: + contact_efc_address_out[conid, dim] = -1 + else: + contact_efc_address_out[conid, dim] = efcid + # This is redundant with the _efc_row call later but needed for the jac calculation + efc_id_out[worldid, efcid] = conid + + if wp.static(IS_SPARSE): + geom = geom_in[conid] + + if geom[0] >= 0: + body1 = geom_bodyid[geom[0]] + else: + flex = flex_in[conid] + vert = vert_in[conid] + body1 = flex_vertbodyid[flex_vertadr[flex[0]] + vert[0]] + + if geom[1] >= 0: + body2 = geom_bodyid[geom[1]] + else: + flex = flex_in[conid] + vert = vert_in[conid] + body2 = flex_vertbodyid[flex_vertadr[flex[1]] + vert[1]] + + # skip fixed bodies + body1 = body_weldid[body1] + body2 = body_weldid[body2] + + da1 = int(body_dofadr[body1] + body_dofnum[body1] - 1) + da2 = int(body_dofadr[body2] + body_dofnum[body2] - 1) + + # count non-zeros + rownnz = int(0) + while da1 >= 0 or da2 >= 0: + da = wp.max(da1, da2) + # skip common dofs + if da1 == da and da2 == da: + break + if da1 == da: + da1 = dof_parentid[da1] + if da2 == da: + da2 = dof_parentid[da2] + rownnz += 1 + + rowadr = wp.atomic_add(efc_nnz_out, worldid, rownnz * ndim) + if rowadr + rownnz * ndim > njmax_nnz_in: + return + for dim in range(ndim): + efcid = base_efcid + dim + if efcid < njmax_in: + efc_J_rowadr_out[worldid, efcid] = rowadr + dim * rownnz + efc_J_rownnz_out[worldid, efcid] = rownnz + + return kernel + + +def _efc_contact_jac_sparse(cone_type: types.ConeType): + IS_ELLIPTIC = cone_type == types.ConeType.ELLIPTIC + + @wp.kernel(module="unique", enable_backward=False) + def kernel( + # Model: + body_parentid: wp.array[int], + body_rootid: wp.array[int], + body_weldid: wp.array[int], + body_dofnum: wp.array[int], + body_dofadr: wp.array[int], + dof_bodyid: wp.array[int], + dof_parentid: wp.array[int], + geom_bodyid: wp.array[int], + flex_vertadr: wp.array[int], + flex_vertbodyid: wp.array[int], + body_isdofancestor: wp.array2d[int], + # Data in: + qvel_in: wp.array2d[float], + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], + contact_efc_address_in: wp.array2d[int], + efc_J_rownnz_in: wp.array2d[int], + efc_J_rowadr_in: wp.array2d[int], + nacon_in: wp.array[int], + # In: + condim_in: wp.array[int], + geom_in: wp.array[wp.vec2i], + flex_in: wp.array[wp.vec2i], + vert_in: wp.array[wp.vec2i], + pos_in: wp.array[wp.vec3], + frame_in: wp.array2d[wp.vec3], + friction_in: wp.array2d[float], + worldid_in: wp.array[int], + # Data out: + efc_J_colind_out: wp.array3d[int], + efc_J_out: wp.array3d[float], + efc_Jqvel_out: wp.array2d[float], + ): + conid, dimid = wp.tid() + + if conid >= nacon_in[0]: + return + + efcid = contact_efc_address_in[conid, dimid] + if efcid < 0: + return + + worldid = worldid_in[conid] + condim = condim_in[conid] geom = geom_in[conid] - if geom[0] >= 0: body1 = geom_bodyid[geom[0]] else: @@ -1770,75 +1869,39 @@ def _contact_pyramidal( vert = vert_in[conid] body2 = flex_vertbodyid[flex_vertadr[flex[1]] + vert[1]] - con_pos = pos_in[conid] - frame = frame_in[conid] - - # pyramidal has common invweight across all edges - body_invweight0_id = worldid % body_invweight0.shape[0] - invweight = body_invweight0[body_invweight0_id, body1][0] + body_invweight0[body_invweight0_id, body2][0] - - if condim > 1: - dimid2 = dimid / 2 + 1 - - friction = friction_in[conid] - fri0 = friction[0] - frii = friction[dimid2 - 1] - invweight = invweight + fri0 * fri0 * invweight - invweight = invweight * 2.0 * fri0 * fri0 * impratio_invsqrt * impratio_invsqrt - - Jqvel = float(0.0) - # skip fixed bodies body1 = body_weldid[body1] body2 = body_weldid[body2] + con_pos = pos_in[conid] + + if not wp.static(IS_ELLIPTIC): + frame_0 = frame_in[conid, 0] + if condim > 1: + dimid2 = dimid / 2 + 1 + frii = friction_in[conid, dimid2 - 1] + da1 = int(body_dofadr[body1] + body_dofnum[body1] - 1) da2 = int(body_dofadr[body2] + body_dofnum[body2] - 1) - - if is_sparse: - pda1 = da1 - pda2 = da2 - rownnz = int(0) - while pda1 >= 0 or pda2 >= 0: - da = wp.max(pda1, pda2) - # skip common dofs - if pda1 == da and pda2 == da: - break - if pda1 == da: - pda1 = dof_parentid[pda1] - if pda2 == da: - pda2 = dof_parentid[pda2] - rownnz += 1 - - # get rowadr - rowadr = wp.atomic_add(efc_nnz_out, worldid, rownnz) - if rowadr + rownnz > njmax_nnz_in: - return - efc_J_rowadr_out[worldid, efcid] = rowadr - efc_J_rownnz_out[worldid, efcid] = rownnz - da = wp.max(da1, da2) - if is_sparse: - nnz = int(0) - dofid = int(da) - else: - dofid = int(nv - 1) + rowadr = efc_J_rowadr_in[worldid, efcid] + rownnz = efc_J_rownnz_in[worldid, efcid] + + Jqvel = float(0.0) + nnz = int(0) + dofid = int(da) while True: - if is_sparse: - if nnz >= rownnz: - break - else: - if dofid < 0: - break + if nnz >= rownnz: + break if dofid == da: - # TODO(team): contact_jacobian jac1p, jac1r = support.jac_dof( body_parentid, body_rootid, dof_bodyid, + body_isdofancestor, subtree_com_in, cdof_in, con_pos, @@ -1850,6 +1913,7 @@ def _contact_pyramidal( body_parentid, body_rootid, dof_bodyid, + body_isdofancestor, subtree_com_in, cdof_in, con_pos, @@ -1858,37 +1922,43 @@ def _contact_pyramidal( worldid, ) - J = float(0.0) - Ji = float(0.0) - if condim > 1: - dimid2 = dimid / 2 + 1 + jacp_dif = jac2p - jac1p + jacr_dif = jac2r - jac1r - for xyz in range(3): - jacp_dif = jac2p[xyz] - jac1p[xyz] - J += frame[0, xyz] * jacp_dif + if wp.static(IS_ELLIPTIC): + J = float(0.0) + if dimid < 3: + frame_row = frame_in[conid, dimid] + for xyz in range(3): + J += frame_row[xyz] * jacp_dif[xyz] + else: + frame_row = frame_in[conid, dimid - 3] + for xyz in range(3): + J += frame_row[xyz] * jacr_dif[xyz] + else: + J = float(0.0) + Ji = float(0.0) + + for xyz in range(3): + J += frame_0[xyz] * jacp_dif[xyz] + + if condim > 1: + if dimid2 < 3: + Ji += frame_in[conid, dimid2][xyz] * jacp_dif[xyz] + else: + Ji += frame_in[conid, dimid2 - 3][xyz] * jacr_dif[xyz] if condim > 1: - if dimid2 < 3: - Ji += frame[dimid2, xyz] * jacp_dif + if dimid % 2 == 0: + J += Ji * frii else: - Ji += frame[dimid2 - 3, xyz] * (jac2r[xyz] - jac1r[xyz]) + J -= Ji * frii - if condim > 1: - if dimid % 2 == 0: - J += Ji * frii - else: - J -= Ji * frii - - if is_sparse: - sparseid = rowadr + nnz - efc_J_colind_out[worldid, 0, sparseid] = dofid - efc_J_out[worldid, 0, sparseid] = J - nnz += 1 - else: - efc_J_out[worldid, efcid, dofid] = J + sparseid = rowadr + nnz + efc_J_colind_out[worldid, 0, sparseid] = dofid + efc_J_out[worldid, 0, sparseid] = J + nnz += 1 Jqvel += J * qvel_in[worldid, dofid] - if is_sparse and nnz >= rownnz: - break # Advance tree pointers and recompute da for next iteration if da1 == da: @@ -1896,135 +1966,229 @@ def _contact_pyramidal( if da2 == da: da2 = dof_parentid[da2] da = wp.max(da1, da2) - if is_sparse: - dofid = da - else: - dofid -= 1 - else: - if not is_sparse: - efc_J_out[worldid, efcid, dofid] = 0.0 - dofid -= 1 + dofid = da - if condim == 1: - efc_type = ConstraintType.CONTACT_FRICTIONLESS - else: - efc_type = ConstraintType.CONTACT_PYRAMIDAL + efc_Jqvel_out[worldid, efcid] = Jqvel - _efc_row( - opt_disableflags, - worldid, - timestep, - efcid, - pos, - pos, - invweight, - solref_in[conid], - solimp_in[conid], - includemargin, - Jqvel, - 0.0, - efc_type, - conid, - efc_type_out, - efc_id_out, - efc_pos_out, - efc_margin_out, - efc_D_out, - efc_vel_out, - efc_aref_out, - efc_frictionloss_out, - ) + return kernel -@wp.kernel -def _contact_elliptic( - # Model: - nv: int, - opt_timestep: wp.array[float], - opt_disableflags: int, - opt_impratio_invsqrt: wp.array[float], - body_parentid: wp.array[int], - body_rootid: wp.array[int], - body_weldid: wp.array[int], - body_dofnum: wp.array[int], - body_dofadr: wp.array[int], - body_invweight0: wp.array2d[wp.vec2], - dof_bodyid: wp.array[int], - dof_parentid: wp.array[int], - geom_bodyid: wp.array[int], - flex_vertadr: wp.array[int], - flex_vertbodyid: wp.array[int], - is_sparse: bool, - # Data in: - qvel_in: wp.array2d[float], - subtree_com_in: wp.array2d[wp.vec3], - cdof_in: wp.array2d[wp.spatial_vector], - njmax_in: int, - njmax_nnz_in: int, - nacon_in: wp.array[int], - # In: - dist_in: wp.array[float], - condim_in: wp.array[int], - includemargin_in: wp.array[float], - worldid_in: wp.array[int], - geom_in: wp.array[wp.vec2i], - flex_in: wp.array[wp.vec2i], - vert_in: wp.array[wp.vec2i], - pos_in: wp.array[wp.vec3], - frame_in: wp.array[wp.mat33], - friction_in: wp.array[vec5], - solref_in: wp.array[wp.vec2], - solreffriction_in: wp.array[wp.vec2], - solimp_in: wp.array[vec5], - type_in: wp.array[int], - # Data out: - nefc_out: wp.array[int], - contact_efc_address_out: wp.array2d[int], - efc_type_out: wp.array2d[int], - efc_id_out: wp.array2d[int], - efc_J_rownnz_out: wp.array2d[int], - efc_J_rowadr_out: wp.array2d[int], - efc_J_colind_out: wp.array3d[int], - efc_J_out: wp.array3d[float], - efc_pos_out: wp.array2d[float], - efc_margin_out: wp.array2d[float], - efc_D_out: wp.array2d[float], - efc_vel_out: wp.array2d[float], - efc_aref_out: wp.array2d[float], - efc_frictionloss_out: wp.array2d[float], - # Out: - efc_nnz_out: wp.array[int], -): - conid, dimid = wp.tid() +def _efc_contact_jac_dense(tile_size: int, cone_type: types.ConeType): + TILE_SIZE = tile_size + IS_ELLIPTIC = cone_type == types.ConeType.ELLIPTIC - if conid >= nacon_in[0]: - return + @wp.kernel(module="unique", enable_backward=False) + def kernel( + # Model: + body_rootid: wp.array[int], + geom_bodyid: wp.array[int], + flex_vertadr: wp.array[int], + flex_vertbodyid: wp.array[int], + body_isdofancestor: wp.array2d[int], + # Data in: + ne_in: wp.array[int], + nf_in: wp.array[int], + nl_in: wp.array[int], + nefc_in: wp.array[int], + qvel_in: wp.array2d[float], + subtree_com_in: wp.array2d[wp.vec3], + cdof_in: wp.array2d[wp.spatial_vector], + contact_efc_address_in: wp.array2d[int], + efc_id_in: wp.array2d[int], + njmax_in: int, + # In: + nv_padded: int, + condim_in: wp.array[int], + geom_in: wp.array[wp.vec2i], + flex_in: wp.array[wp.vec2i], + vert_in: wp.array[wp.vec2i], + pos_in: wp.array[wp.vec3], + frame_in: wp.array2d[wp.vec3], + friction_in: wp.array2d[float], + # Data out: + efc_J_out: wp.array3d[float], + efc_Jqvel_out: wp.array2d[float], + ): + worldid, dof_block_id, tid = wp.tid() - if not type_in[conid] & ContactType.CONSTRAINT: - return - - condim = condim_in[conid] - - if dimid > condim - 1: - return - - includemargin = includemargin_in[conid] - pos = dist_in[conid] - includemargin - active = pos < 0.0 - - if active: - worldid = worldid_in[conid] - - efcid = wp.atomic_add(nefc_out, worldid, 1) - if efcid >= njmax_in: - contact_efc_address_out[conid, dimid] = -1 + dof_start = dof_block_id * wp.static(TILE_SIZE) + if dof_start >= nv_padded: return + cdof_tile = wp.tile_load(cdof_in[worldid], shape=TILE_SIZE, offset=dof_start, bounds_check=True) + qvel_tile = wp.tile_load(qvel_in[worldid], shape=TILE_SIZE, offset=dof_start, bounds_check=True) + + efcid_start = ne_in[worldid] + nf_in[worldid] + nl_in[worldid] + efcid_end = wp.min(nefc_in[worldid], njmax_in) + + prev_conid = int(-1) + condim = int(0) + + for efcid in range(efcid_start, efcid_end): + conid = efc_id_in[worldid, efcid] + + # Recompute per-contact data only when contact changes + if conid != prev_conid: + prev_conid = conid + condim = condim_in[conid] + + geom = geom_in[conid] + if geom[0] >= 0: + body1 = geom_bodyid[geom[0]] + else: + flex = flex_in[conid] + vert = vert_in[conid] + body1 = flex_vertbodyid[flex_vertadr[flex[0]] + vert[0]] + + if geom[1] >= 0: + body2 = geom_bodyid[geom[1]] + else: + flex = flex_in[conid] + vert = vert_in[conid] + body2 = flex_vertbodyid[flex_vertadr[flex[1]] + vert[1]] + + con_pos = pos_in[conid] + offset1 = con_pos - subtree_com_in[worldid, body_rootid[body1]] + offset2 = con_pos - subtree_com_in[worldid, body_rootid[body2]] + + affects1_tile = wp.tile_load(body_isdofancestor[body1], shape=TILE_SIZE, offset=dof_start, bounds_check=True) + affects2_tile = wp.tile_load(body_isdofancestor[body2], shape=TILE_SIZE, offset=dof_start, bounds_check=True) + + jacp1_tile = wp.tile_map(support._compute_jacp, cdof_tile, offset1, affects1_tile) + jacp2_tile = wp.tile_map(support._compute_jacp, cdof_tile, offset2, affects2_tile) + jacp_dif_tile = wp.tile_map(wp.sub, jacp2_tile, jacp1_tile) + + jacr1_tile = wp.tile_map(support._compute_jacr, cdof_tile, affects1_tile) + jacr2_tile = wp.tile_map(support._compute_jacr, cdof_tile, affects2_tile) + jacr_dif_tile = wp.tile_map(wp.sub, jacr2_tile, jacr1_tile) + + if not wp.static(IS_ELLIPTIC): + frame_0 = frame_in[conid, 0] + Ji_0p_tile = wp.tile_map(wp.dot, jacp_dif_tile, frame_0) + + if condim > 1: + Ji_0r_tile = wp.tile_map(wp.dot, jacr_dif_tile, frame_0) + frame_1 = frame_in[conid, 1] + Ji_1p_tile = wp.tile_map(wp.dot, jacp_dif_tile, frame_1) + Ji_1r_tile = wp.tile_map(wp.dot, jacr_dif_tile, frame_1) + frame_2 = frame_in[conid, 2] + Ji_2p_tile = wp.tile_map(wp.dot, jacp_dif_tile, frame_2) + Ji_2r_tile = wp.tile_map(wp.dot, jacr_dif_tile, frame_2) + + if wp.static(IS_ELLIPTIC): + dimid = efcid - contact_efc_address_in[conid, 0] + if dimid < 3: + frame_idx = dimid + else: + frame_idx = dimid - 3 + + frame_row = frame_in[conid, frame_idx] + + if dimid < 3: + J_tile = wp.tile_map(wp.dot, jacp_dif_tile, frame_row) + else: + J_tile = wp.tile_map(wp.dot, jacr_dif_tile, frame_row) + else: + J_tile = Ji_0p_tile + if condim > 1: + dimid = efcid - contact_efc_address_in[conid, 0] + dimid2 = dimid / 2 + 1 + frii = friction_in[conid, dimid2 - 1] + frii_sign = frii * (1.0 - 2.0 * float(dimid & 1)) + + if dimid2 == 1: + J_tile = wp.tile_map(wp.add, J_tile, wp.tile_map(wp.mul, Ji_1p_tile, frii_sign)) + elif dimid2 == 2: + J_tile = wp.tile_map(wp.add, J_tile, wp.tile_map(wp.mul, Ji_2p_tile, frii_sign)) + elif dimid2 == 3: + J_tile = wp.tile_map(wp.add, J_tile, wp.tile_map(wp.mul, Ji_0r_tile, frii_sign)) + elif dimid2 == 4: + J_tile = wp.tile_map(wp.add, J_tile, wp.tile_map(wp.mul, Ji_1r_tile, frii_sign)) + else: + J_tile = wp.tile_map(wp.add, J_tile, wp.tile_map(wp.mul, Ji_2r_tile, frii_sign)) + + wp.tile_store(efc_J_out[worldid, efcid], J_tile, offset=dof_start, bounds_check=True) + + Jqvel_tile = wp.tile_map(wp.mul, J_tile, qvel_tile) + Jqvel_sum = wp.tile_reduce(wp.add, Jqvel_tile) + if tid == 0: + wp.atomic_add(efc_Jqvel_out[worldid], efcid, wp.tile_extract(Jqvel_sum, 0)) + + return kernel + + +def _efc_contact_update(cone_type: types.ConeType): + IS_ELLIPTIC = cone_type == types.ConeType.ELLIPTIC + + @wp.kernel(module="unique", enable_backward=False) + def kernel( + # Model: + opt_timestep: wp.array[float], + opt_disableflags: int, + opt_impratio_invsqrt: wp.array[float], + body_invweight0: wp.array2d[wp.vec2], + geom_bodyid: wp.array[int], + flex_vertadr: wp.array[int], + flex_vertbodyid: wp.array[int], + # Data in: + contact_efc_address_in: wp.array2d[int], + efc_Jqvel_in: wp.array2d[float], + nacon_in: wp.array[int], + # In: + dist_in: wp.array[float], + condim_in: wp.array[int], + includemargin_in: wp.array[float], + worldid_in: wp.array[int], + geom_in: wp.array[wp.vec2i], + flex_in: wp.array[wp.vec2i], + vert_in: wp.array[wp.vec2i], + friction_in: wp.array[vec5], + solref_in: wp.array[wp.vec2], + solreffriction_in: wp.array[wp.vec2], + solimp_in: wp.array[vec5], + type_in: wp.array[int], + # Data out: + efc_type_out: wp.array2d[int], + efc_id_out: wp.array2d[int], + efc_pos_out: wp.array2d[float], + efc_margin_out: wp.array2d[float], + efc_D_out: wp.array2d[float], + efc_vel_out: wp.array2d[float], + efc_aref_out: wp.array2d[float], + efc_frictionloss_out: wp.array2d[float], + ): + conid, dimid = wp.tid() + + if conid >= nacon_in[0]: + return + + if not type_in[conid] & ContactType.CONSTRAINT: + return + + condim = condim_in[conid] + + if wp.static(IS_ELLIPTIC): + if dimid > condim - 1: + return + else: + if condim == 1 and dimid > 0: + return + elif condim > 1 and dimid >= 2 * (condim - 1): + return + + efcid = contact_efc_address_in[conid, dimid] + if efcid < 0: + return + + worldid = worldid_in[conid] timestep = opt_timestep[worldid % opt_timestep.shape[0]] impratio_invsqrt = opt_impratio_invsqrt[worldid % opt_impratio_invsqrt.shape[0]] - contact_efc_address_out[conid, dimid] = efcid + + includemargin = includemargin_in[conid] + pos = dist_in[conid] - includemargin geom = geom_in[conid] + Jqvel = efc_Jqvel_in[worldid, efcid] if geom[0] >= 0: body1 = geom_bodyid[geom[0]] @@ -2040,145 +2204,43 @@ def _contact_elliptic( vert = vert_in[conid] body2 = flex_vertbodyid[flex_vertadr[flex[1]] + vert[1]] - con_pos = pos_in[conid] - frame = frame_in[conid] - - Jqvel = float(0.0) - - # skip fixed bodies - body1 = body_weldid[body1] - body2 = body_weldid[body2] - - da1 = int(body_dofadr[body1] + body_dofnum[body1] - 1) - da2 = int(body_dofadr[body2] + body_dofnum[body2] - 1) - - if is_sparse: - # count non-zeros - pda1 = da1 - pda2 = da2 - rownnz = int(0) - while pda1 >= 0 or pda2 >= 0: - da = wp.max(pda1, pda2) - # skip common dofs - if pda1 == da and pda2 == da: - break - if pda1 == da: - pda1 = dof_parentid[pda1] - if pda2 == da: - pda2 = dof_parentid[pda2] - rownnz += 1 - - # get rowadr - rowadr = wp.atomic_add(efc_nnz_out, worldid, rownnz) - if rowadr + rownnz > njmax_nnz_in: - return - efc_J_rowadr_out[worldid, efcid] = rowadr - efc_J_rownnz_out[worldid, efcid] = rownnz - - da = wp.max(da1, da2) - - if is_sparse: - nnz = int(0) - dofid = int(da) - else: - dofid = int(nv - 1) - - while True: - if is_sparse: - if nnz >= rownnz: - break - else: - if dofid < 0: - break - - if dofid == da: - # TODO(team): contact jacobian - jac1p, jac1r = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - con_pos, - body1, - dofid, - worldid, - ) - jac2p, jac2r = support.jac_dof( - body_parentid, - body_rootid, - dof_bodyid, - subtree_com_in, - cdof_in, - con_pos, - body2, - dofid, - worldid, - ) - - J = float(0.0) - for xyz in range(3): - if dimid < 3: - jac_dif = jac2p[xyz] - jac1p[xyz] - J += frame[dimid, xyz] * jac_dif - else: - jac_dif = jac2r[xyz] - jac1r[xyz] - J += frame[dimid - 3, xyz] * jac_dif - - if is_sparse: - sparseid = rowadr + nnz - efc_J_colind_out[worldid, 0, sparseid] = dofid - efc_J_out[worldid, 0, sparseid] = J - nnz += 1 - else: - efc_J_out[worldid, efcid, dofid] = J - Jqvel += J * qvel_in[worldid, dofid] - if is_sparse and nnz >= rownnz: - break - - # Advance tree pointers and recompute da for next iteration - if da1 == da: - da1 = dof_parentid[da1] - if da2 == da: - da2 = dof_parentid[da2] - da = wp.max(da1, da2) - if is_sparse: - dofid = da - else: - dofid -= 1 - else: - if not is_sparse: - efc_J_out[worldid, efcid, dofid] = 0.0 - dofid -= 1 - body_invweight0_id = worldid % body_invweight0.shape[0] invweight = body_invweight0[body_invweight0_id, body1][0] + body_invweight0[body_invweight0_id, body2][0] ref = solref_in[conid] pos_aref = pos - if dimid > 0: - solreffriction = solreffriction_in[conid] + if wp.static(IS_ELLIPTIC): + if dimid > 0: + solreffriction = solreffriction_in[conid] - # non-normal directions use solreffriction (if non-zero) - if solreffriction[0] or solreffriction[1]: - ref = solreffriction + # non-normal directions use solreffriction (if non-zero) + if solreffriction[0] or solreffriction[1]: + ref = solreffriction - invweight = invweight * impratio_invsqrt * impratio_invsqrt - friction = friction_in[conid] + invweight = invweight * impratio_invsqrt * impratio_invsqrt + friction = friction_in[conid] - if dimid > 1: + if dimid > 1: + fri0 = friction[0] + frii = friction[dimid - 1] + fri = fri0 * fri0 / (frii * frii) + invweight *= fri + + pos_aref = 0.0 + else: + if condim > 1: + friction = friction_in[conid] fri0 = friction[0] - frii = friction[dimid - 1] - fri = fri0 * fri0 / (frii * frii) - invweight *= fri - - pos_aref = 0.0 + invweight = invweight + fri0 * fri0 * invweight + invweight = invweight * 2.0 * fri0 * fri0 * impratio_invsqrt * impratio_invsqrt if condim == 1: efc_type = ConstraintType.CONTACT_FRICTIONLESS - else: + elif wp.static(IS_ELLIPTIC): efc_type = ConstraintType.CONTACT_ELLIPTIC + else: + efc_type = ConstraintType.CONTACT_PYRAMIDAL _efc_row( opt_disableflags, @@ -2205,6 +2267,8 @@ def _contact_elliptic( efc_frictionloss_out, ) + return kernel + @event_scope def make_constraint(m: types.Model, d: types.Data): @@ -2243,6 +2307,7 @@ def make_constraint(m: types.Model, d: types.Data): m.eq_solimp, m.eq_data, m.is_sparse, + m.body_isdofancestor, m.eq_connect_adr, d.qvel, d.eq_active, @@ -2297,6 +2362,7 @@ def make_constraint(m: types.Model, d: types.Data): m.eq_solimp, m.eq_data, m.is_sparse, + m.body_isdofancestor, m.eq_wld_adr, d.qvel, d.eq_active, @@ -2433,6 +2499,7 @@ def make_constraint(m: types.Model, d: types.Data): m.eq_solimp, m.eq_flex_adr, d.qvel, + d.eq_active, d.flexedge_J, d.flexedge_length, d.njmax, @@ -2658,122 +2725,170 @@ def make_constraint(m: types.Model, d: types.Data): # contact if not (m.opt.disableflags & types.DisableBit.CONTACT): - if m.opt.cone == types.ConeType.PYRAMIDAL: + nmaxdim = int(m.nmaxpyramid) if m.opt.cone == types.ConeType.PYRAMIDAL else int(m.nmaxcondim) + + # Reinterpret to avoid unnecessary loads + contact_frame_2d = wp.array( + ptr=d.contact.frame.ptr, + dtype=wp.vec3, + shape=(d.naconmax, 3), + device=d.contact.frame.device, + copy=False, + ) + contact_friction_2d = wp.array( + ptr=d.contact.friction.ptr, + dtype=float, + shape=(d.naconmax, 5), + device=d.contact.friction.device, + copy=False, + ) + + wp.launch( + _efc_contact_init(m.opt.cone, m.is_sparse), + dim=d.naconmax, + inputs=[ + m.body_weldid, + m.body_dofnum, + m.body_dofadr, + m.dof_parentid, + m.geom_bodyid, + m.flex_vertadr, + m.flex_vertbodyid, + d.njmax, + d.njmax_nnz, + d.nacon, + d.contact.dist, + d.contact.dim, + d.contact.includemargin, + d.contact.worldid, + d.contact.geom, + d.contact.flex, + d.contact.vert, + d.contact.type, + ], + outputs=[ + d.nefc, + d.contact.efc_address, + d.efc.id, + d.efc.J_rownnz, + d.efc.J_rowadr, + efc_nnz, + ], + ) + + if m.is_sparse: wp.launch( - _contact_pyramidal, - dim=(d.naconmax, m.nmaxpyramid), + _efc_contact_jac_sparse(m.opt.cone), + dim=(d.naconmax, nmaxdim), inputs=[ - m.nv, - m.opt.timestep, - m.opt.disableflags, - m.opt.impratio_invsqrt, m.body_parentid, m.body_rootid, m.body_weldid, m.body_dofnum, m.body_dofadr, - m.body_invweight0, m.dof_bodyid, m.dof_parentid, m.geom_bodyid, m.flex_vertadr, m.flex_vertbodyid, - m.is_sparse, + m.body_isdofancestor, d.qvel, d.subtree_com, d.cdof, - d.njmax, - d.njmax_nnz, + d.contact.efc_address, + d.efc.J_rownnz, + d.efc.J_rowadr, d.nacon, - d.contact.dist, d.contact.dim, - d.contact.includemargin, - d.contact.worldid, d.contact.geom, d.contact.flex, d.contact.vert, d.contact.pos, - d.contact.frame, - d.contact.friction, - d.contact.solref, - d.contact.solimp, - d.contact.type, + contact_frame_2d, + contact_friction_2d, + d.contact.worldid, ], outputs=[ - d.nefc, - d.contact.efc_address, - d.efc.type, - d.efc.id, - d.efc.J_rownnz, - d.efc.J_rowadr, d.efc.J_colind, d.efc.J, - d.efc.pos, - d.efc.margin, - d.efc.D, - d.efc.vel, - d.efc.aref, - d.efc.frictionloss, - efc_nnz, + d.efc.Jqvel, ], ) - elif m.opt.cone == types.ConeType.ELLIPTIC: - wp.launch( - _contact_elliptic, - dim=(d.naconmax, m.nmaxcondim), + else: + d.efc.Jqvel.zero_() + tile_size = m.block_dim.contact_jac_tiled + n_dof_blocks = (m.nv_pad + tile_size - 1) // tile_size + + wp.launch_tiled( + _efc_contact_jac_dense(tile_size, m.opt.cone), + dim=(d.nworld, n_dof_blocks), inputs=[ - m.nv, - m.opt.timestep, - m.opt.disableflags, - m.opt.impratio_invsqrt, - m.body_parentid, m.body_rootid, - m.body_weldid, - m.body_dofnum, - m.body_dofadr, - m.body_invweight0, - m.dof_bodyid, - m.dof_parentid, m.geom_bodyid, m.flex_vertadr, m.flex_vertbodyid, - m.is_sparse, + m.body_isdofancestor, + d.ne, + d.nf, + d.nl, + d.nefc, d.qvel, d.subtree_com, d.cdof, + d.contact.efc_address, + d.efc.id, d.njmax, - d.njmax_nnz, - d.nacon, - d.contact.dist, + m.nv_pad, d.contact.dim, - d.contact.includemargin, - d.contact.worldid, d.contact.geom, d.contact.flex, d.contact.vert, d.contact.pos, - d.contact.frame, - d.contact.friction, - d.contact.solref, - d.contact.solreffriction, - d.contact.solimp, - d.contact.type, + contact_frame_2d, + contact_friction_2d, ], outputs=[ - d.nefc, - d.contact.efc_address, - d.efc.type, - d.efc.id, - d.efc.J_rownnz, - d.efc.J_rowadr, - d.efc.J_colind, d.efc.J, - d.efc.pos, - d.efc.margin, - d.efc.D, - d.efc.vel, - d.efc.aref, - d.efc.frictionloss, - efc_nnz, + d.efc.Jqvel, ], + block_dim=tile_size, ) + + wp.launch( + _efc_contact_update(m.opt.cone), + dim=(d.naconmax, nmaxdim), + inputs=[ + m.opt.timestep, + m.opt.disableflags, + m.opt.impratio_invsqrt, + m.body_invweight0, + m.geom_bodyid, + m.flex_vertadr, + m.flex_vertbodyid, + d.contact.efc_address, + d.efc.Jqvel, + d.nacon, + d.contact.dist, + d.contact.dim, + d.contact.includemargin, + d.contact.worldid, + d.contact.geom, + d.contact.flex, + d.contact.vert, + d.contact.friction, + d.contact.solref, + d.contact.solreffriction, + d.contact.solimp, + d.contact.type, + ], + outputs=[ + d.efc.type, + d.efc.id, + d.efc.pos, + d.efc.margin, + d.efc.D, + d.efc.vel, + d.efc.aref, + d.efc.frictionloss, + ], + ) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/derivative.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/derivative.py index bdcd81cb..4a241f3b 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/derivative.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/derivative.py @@ -15,7 +15,9 @@ import warp as wp +from mujoco.mjx.third_party.mujoco_warp._src import util_misc from mujoco.mjx.third_party.mujoco_warp._src.support import next_act +from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINVAL from mujoco.mjx.third_party.mujoco_warp._src.types import BiasType from mujoco.mjx.third_party.mujoco_warp._src.types import Data from mujoco.mjx.third_party.mujoco_warp._src.types import DisableBit @@ -65,6 +67,28 @@ def _qderiv_actuator_passive_vel( if actuator_biastype[actid] == BiasType.AFFINE: bias = actuator_biasprm[actuator_biasprm_id, actid][2] + elif actuator_biastype[actid] == BiasType.DCMOTOR: + dynprm = actuator_dynprm[worldid % actuator_dynprm.shape[0], actid] + te = dynprm[0] + if te <= 0.0: + gainprm = actuator_gainprm[actuator_gainprm_id, actid] + R = gainprm[0] + K = gainprm[1] + + slots = util_misc.dcmotor_slots(dynprm, gainprm) + slot_Ta = slots[2] + + if slot_Ta >= 0: + adr = actuator_actadr[actid] + slot_Ta + T = act_in[worldid, adr] + alpha = gainprm[2] + T0 = gainprm[3] + Ta = dynprm[4] + R *= 1.0 + alpha * (T + Ta - T0) + + bias = -K * K / wp.max(MJ_MINVAL, R) + else: + bias = 0.0 else: bias = 0.0 @@ -228,8 +252,10 @@ def _qderiv_actuator_passive( opt_timestep: wp.array[float], opt_disableflags: int, dof_damping: wp.array2d[float], + dof_dampingpoly: wp.array2d[wp.vec2], is_sparse: bool, # Data in: + qvel_in: wp.array2d[float], qM_in: wp.array3d[float], # In: qMi: wp.array[int], @@ -249,7 +275,10 @@ def _qderiv_actuator_passive( qderiv = qDeriv_in[worldid, dofiid, dofjid] if not (opt_disableflags & DisableBit.DAMPER) and dofiid == dofjid: - qderiv -= dof_damping[worldid % dof_damping.shape[0], dofiid] + damping = dof_damping[worldid % dof_damping.shape[0], dofiid] + dpoly = dof_dampingpoly[worldid % dof_dampingpoly.shape[0], dofiid] + v = qvel_in[worldid, dofiid] + qderiv -= util_misc._poly_force_deriv(damping, dpoly, v, 1) qderiv *= opt_timestep[worldid % opt_timestep.shape[0]] @@ -272,9 +301,11 @@ def _qderiv_tendon_damping( ten_J_rowadr: wp.array[int], ten_J_colind: wp.array[int], tendon_damping: wp.array2d[float], + tendon_dampingpoly: wp.array2d[wp.vec2], is_sparse: bool, # Data in: ten_J_in: wp.array2d[float], + ten_velocity_in: wp.array2d[float], # In: qMi: wp.array[int], qMj: wp.array[int], @@ -289,7 +320,8 @@ def _qderiv_tendon_damping( tendon_damping_id = worldid % tendon_damping.shape[0] for tenid in range(ntendon): damping = tendon_damping[tendon_damping_id, tenid] - if damping == 0.0: + dpoly = tendon_dampingpoly[worldid % tendon_dampingpoly.shape[0], tenid] + if damping == 0.0 and dpoly[0] == 0.0 and dpoly[1] == 0.0: continue rownnz = ten_J_rownnz[tenid] @@ -305,7 +337,9 @@ def _qderiv_tendon_damping( Ji = ten_J_in[worldid, sparseid] if colind == dofjid: Jj = ten_J_in[worldid, sparseid] - qderiv -= Ji * Jj * damping + + v = ten_velocity_in[worldid, tenid] + qderiv -= Ji * Jj * util_misc._poly_force_deriv(damping, dpoly, v, 1) qderiv *= opt_timestep[worldid % opt_timestep.shape[0]] @@ -382,7 +416,9 @@ def deriv_smooth_vel(m: Model, d: Data, out: wp.array2d[float]): m.opt.timestep, m.opt.disableflags, m.dof_damping, + m.dof_dampingpoly, m.is_sparse, + d.qvel, d.qM, qMi, qMj, @@ -405,8 +441,10 @@ def deriv_smooth_vel(m: Model, d: Data, out: wp.array2d[float]): m.ten_J_rowadr, m.ten_J_colind, m.tendon_damping, + m.tendon_dampingpoly, m.is_sparse, d.ten_J, + d.ten_velocity, qMi, qMj, ], diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/forward.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/forward.py index 55c7f357..c9b3c153 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/forward.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/forward.py @@ -138,10 +138,13 @@ def _next_activation( actuator_actnum: wp.array[int], actuator_actlimited: wp.array[bool], actuator_dynprm: wp.array2d[vec10f], + actuator_gainprm: wp.array2d[vec10f], + actuator_biasprm: wp.array2d[vec10f], actuator_actrange: wp.array2d[wp.vec2], # Data in: act_in: wp.array2d[float], act_dot_in: wp.array2d[float], + actuator_velocity_in: wp.array2d[float], # In: act_dot_scale: float, limit: bool, @@ -152,20 +155,65 @@ def _next_activation( opt_timestep_id = worldid % opt_timestep.shape[0] actuator_dynprm_id = worldid % actuator_dynprm.shape[0] actuator_actrange_id = worldid % actuator_actrange.shape[0] + actuator_gainprm_id = worldid % actuator_gainprm.shape[0] + actuator_biasprm_id = worldid % actuator_biasprm.shape[0] + actadr = actuator_actadr[uid] actnum = actuator_actnum[uid] - for j in range(actadr, actadr + actnum): - act = next_act( - opt_timestep[opt_timestep_id], - actuator_dyntype[uid], - actuator_dynprm[actuator_dynprm_id, uid], - actuator_actrange[actuator_actrange_id, uid], - act_in[worldid, j], - act_dot_in[worldid, j], - act_dot_scale, - limit and actuator_actlimited[uid], - ) - act_out[worldid, j] = act + dyntype = actuator_dyntype[uid] + + if dyntype == DynType.DCMOTOR: + dynprm = actuator_dynprm[actuator_dynprm_id, uid] + gainprm = actuator_gainprm[actuator_gainprm_id, uid] + biasprm = actuator_biasprm[actuator_biasprm_id, uid] + slots = util_misc.dcmotor_slots(dynprm, gainprm) + + for j in range(actadr, actadr + actnum): + offset = j - actadr + act = act_in[worldid, j] + act_dot = act_dot_in[worldid, j] + + if offset == slots[4]: # current + R = gainprm[0] + te = wp.max(MJ_MINVAL, dynprm[0]) + act = act + act_dot * te * (1.0 - wp.exp(-opt_timestep[opt_timestep_id] / te)) + elif offset == slots[3]: # bristle + F_C = biasprm[3] + F_S = biasprm[4] + v_S = biasprm[5] + sigma0 = dynprm[5] + velocity = actuator_velocity_in[worldid, uid] + g = util_misc.lugre_stribeck(velocity, F_C, F_S, v_S) + + a = -sigma0 * wp.abs(velocity) / wp.max(MJ_MINVAL, g) + h = opt_timestep[opt_timestep_id] + exp_ah = wp.exp(a * h) + int_h = h + if wp.abs(a) > MJ_MINVAL: + int_h = (exp_ah - 1.0) / a + act = exp_ah * act + int_h * velocity + elif offset == slots[1]: # integral + act = act + act_dot * opt_timestep[opt_timestep_id] + Imax = dynprm[8] + if Imax > 0.0: + act = wp.clamp(act, -Imax, Imax) + else: # temperature and slew + act = act + act_dot * opt_timestep[opt_timestep_id] + + act_out[worldid, j] = act + else: + for j in range(actadr, actadr + actnum): + act = next_act( + opt_timestep[opt_timestep_id], + dyntype, + actuator_dynprm[actuator_dynprm_id, uid], + actuator_actrange[actuator_actrange_id, uid], + act_in[worldid, j], + act_dot_in[worldid, j], + act_dot_scale, + limit and actuator_actlimited[uid], + ) + act_out[worldid, j] = act @wp.kernel @@ -225,9 +273,12 @@ def _advance(m: Model, d: Data, qacc: wp.array, qvel: Optional[wp.array] = None) m.actuator_actnum, m.actuator_actlimited, m.actuator_dynprm, + m.actuator_gainprm, + m.actuator_biasprm, m.actuator_actrange, d.act, d.act_dot, + d.actuator_velocity, 1.0, True, ], @@ -274,12 +325,30 @@ def _advance(m: Model, d: Data, qacc: wp.array, qvel: Optional[wp.array] = None) wp.copy(d.qacc_warmstart, d.qacc) +@wp.kernel +def _compute_damping_deriv( + # Model: + dof_damping: wp.array2d[float], + dof_dampingpoly: wp.array2d[wp.vec2], + # Data in: + qvel_in: wp.array2d[float], + # Out: + deriv_out: wp.array2d[float], +): + worldid, tid = wp.tid() + damping = dof_damping[worldid % dof_damping.shape[0], tid] + dpoly = dof_dampingpoly[worldid % dof_dampingpoly.shape[0], tid] + v = qvel_in[worldid, tid] + deriv_out[worldid, tid] = util_misc._poly_force_deriv(damping, dpoly, v, 1) + + @wp.kernel def _euler_damp_qfrc_sparse( # Model: opt_timestep: wp.array[float], dof_Madr: wp.array[int], - dof_damping: wp.array2d[float], + # In: + damp_deriv: wp.array2d[float], # Out: qM_integration_out: wp.array3d[float], ): @@ -287,7 +356,7 @@ def _euler_damp_qfrc_sparse( timestep = opt_timestep[worldid % opt_timestep.shape[0]] adr = dof_Madr[tid] - qM_integration_out[worldid, 0, adr] += timestep * dof_damping[worldid % dof_damping.shape[0], tid] + qM_integration_out[worldid, 0, adr] += timestep * damp_deriv[worldid, tid] @cache_kernel @@ -296,11 +365,11 @@ def _tile_euler_dense(tile: TileSet): def euler_dense( # Model: opt_timestep: wp.array[float], - dof_damping: wp.array2d[float], # Data in: qM_in: wp.array3d[float], efc_Ma_in: wp.array2d[float], # In: + damp_deriv: wp.array2d[float], adr_in: wp.array[int], # Data out: qacc_out: wp.array2d[float], @@ -311,7 +380,7 @@ def _tile_euler_dense(tile: TileSet): dofid = adr_in[nodeid] M_tile = wp.tile_load(qM_in[worldid], shape=(TILE_SIZE, TILE_SIZE), offset=(dofid, dofid)) - damping_tile = wp.tile_load(dof_damping[worldid % dof_damping.shape[0]], shape=(TILE_SIZE,), offset=(dofid,)) + damping_tile = wp.tile_load(damp_deriv[worldid], shape=(TILE_SIZE,), offset=(dofid,)) damping_scaled = damping_tile * timestep qm_integration_tile = wp.tile_diag_add(M_tile, damping_scaled) @@ -329,6 +398,16 @@ def euler(m: Model, d: Data): # integrate damping implicitly if not (m.opt.disableflags & (DisableBit.EULERDAMP | DisableBit.DAMPER)): qacc = wp.empty((d.nworld, m.nv), dtype=float) + + # Compute damping derivative + damp_deriv = wp.empty((d.nworld, m.nv), dtype=float) + wp.launch( + _compute_damping_deriv, + dim=(d.nworld, m.nv), + inputs=[m.dof_damping, m.dof_dampingpoly, d.qvel], + outputs=[damp_deriv], + ) + if m.is_sparse: qM = wp.clone(d.qM) qLD = wp.empty((d.nworld, 1, m.nC), dtype=float) @@ -336,7 +415,7 @@ def euler(m: Model, d: Data): wp.launch( _euler_damp_qfrc_sparse, dim=(d.nworld, m.nv), - inputs=[m.opt.timestep, m.dof_Madr, m.dof_damping], + inputs=[m.opt.timestep, m.dof_Madr, damp_deriv], outputs=[qM], ) smooth.factor_solve_i(m, d, qM, qLD, qLDiagInv, qacc, d.efc.Ma) @@ -345,7 +424,7 @@ def euler(m: Model, d: Data): wp.launch_tiled( _tile_euler_dense(tile), dim=(d.nworld, tile.adr.size), - inputs=[m.opt.timestep, m.dof_damping, d.qM, d.efc.Ma, tile.adr], + inputs=[m.opt.timestep, d.qM, d.efc.Ma, damp_deriv, tile.adr], outputs=[qacc], block_dim=m.block_dim.euler_dense, ) @@ -390,9 +469,12 @@ def _rk_perturb_state( m.actuator_actnum, m.actuator_actlimited, m.actuator_dynprm, + m.actuator_gainprm, + m.actuator_biasprm, m.actuator_actrange, act_t0, d.act_dot, + d.actuator_velocity, scale, False, ], @@ -672,6 +754,96 @@ def _actuator_force( dynprm = actuator_dynprm[worldid % actuator_dynprm.shape[0], uid] act = act_in[worldid, act_last] act_dot = util_misc.muscle_dynamics(ctrl, act, dynprm) + elif dyntype == DynType.DCMOTOR: + gainprm = actuator_gainprm[worldid % actuator_gainprm.shape[0], uid] + slots = util_misc.dcmotor_slots(dynprm, gainprm) + adr = act_first + + act_dot = 0.0 + + # slew rate + if slots[0] >= 0: + u_prev = act_in[worldid, adr] + slew_s = dynprm[7] + slew = slew_s * opt_timestep[worldid % opt_timestep.shape[0]] + u_eff = wp.clamp(ctrl, u_prev - slew, u_prev + slew) + act_dot = (u_eff - u_prev) / opt_timestep[worldid % opt_timestep.shape[0]] + act_dot_out[worldid, adr] = act_dot + ctrl = u_eff + adr += 1 + + # integral + if slots[1] >= 0: + x_I = act_in[worldid, adr] + input_mode = int(gainprm[8]) + Imax = dynprm[8] + act_dot = ctrl + if input_mode == 1: + act_dot = ctrl - actuator_length_in[worldid, uid] + + if Imax > 0.0: + if x_I >= Imax: + act_dot = wp.min(act_dot, 0.0) + elif x_I <= -Imax: + act_dot = wp.max(act_dot, 0.0) + + act_dot_out[worldid, adr] = act_dot + adr += 1 + + # voltage + V = util_misc.dcmotor_voltage( + ctrl, + actuator_length_in[worldid, uid], + actuator_velocity_in[worldid, uid], + x_I, + gainprm, + ) + + # temperature + R = gainprm[0] + K = gainprm[1] + te = wp.max(MJ_MINVAL, dynprm[0]) + + if slots[2] >= 0: + RT = dynprm[2] + C = dynprm[3] + Ta = dynprm[4] + alpha = gainprm[2] + T0 = gainprm[3] + T = act_in[worldid, adr] + R_eff = R * (1.0 + alpha * (T + Ta - T0)) + + current = (V - K * actuator_velocity_in[worldid, uid]) / R_eff + if slots[4] >= 0: + current = act_in[worldid, act_last] + + act_dot = (R_eff * current * current - T / RT) / C + act_dot_out[worldid, adr] = act_dot + adr += 1 + R = R_eff + + # bristle + if slots[3] >= 0: + sigma0 = dynprm[5] + biasprm = actuator_biasprm[worldid % actuator_biasprm.shape[0], uid] + F_C = biasprm[3] + F_S = biasprm[4] + v_S = biasprm[5] + z = act_in[worldid, adr] + g = util_misc.lugre_stribeck(actuator_velocity_in[worldid, uid], F_C, F_S, v_S) + a = -sigma0 * wp.abs(actuator_velocity_in[worldid, uid]) / wp.max(MJ_MINVAL, g) + act_dot = a * z + actuator_velocity_in[worldid, uid] + act_dot_out[worldid, adr] = act_dot + adr += 1 + + # current + if slots[4] >= 0: + dimax = dynprm[1] + act_dot = (V / R - K / R * actuator_velocity_in[worldid, uid] - act_in[worldid, act_last]) / te + if dimax > 0.0: + act_dot = wp.clamp(act_dot, -dimax, dimax) + act_dot_out[worldid, act_last] = act_dot + elif dyntype == DynType.USER: act_dot = 0.0 # set by act_dyn_callback else: # DynType.NONE @@ -680,19 +852,54 @@ def _actuator_force( act_dot_out[worldid, act_last] = act_dot if actuator_actearly[uid]: - if dyntype == DynType.INTEGRATOR or dyntype == DynType.NONE: + if dyntype == DynType.INTEGRATOR or dyntype == DynType.NONE or dyntype == DynType.DCMOTOR: act = act_in[worldid, act_last] - ctrl_act = next_act( - opt_timestep[worldid % opt_timestep.shape[0]], - dyntype, - dynprm, - actuator_actrange[worldid % actuator_actrange.shape[0], uid], - act, - act_dot, - 1.0, - actuator_actlimited[uid], - ) + if dyntype == DynType.DCMOTOR: + gainprm = actuator_gainprm[worldid % actuator_gainprm.shape[0], uid] + slots = util_misc.dcmotor_slots(dynprm, gainprm) + offset = actuator_actnum[uid] - 1 + + if offset == slots[4]: # current + te = wp.max(MJ_MINVAL, dynprm[0]) + ctrl_act = act + act_dot * te * (1.0 - wp.exp(-opt_timestep[worldid % opt_timestep.shape[0]] / te)) + elif offset == slots[3]: # bristle + sigma0 = dynprm[5] + biasprm = actuator_biasprm[worldid % actuator_biasprm.shape[0], uid] + F_C = biasprm[3] + F_S = biasprm[4] + v_S = biasprm[5] + velocity = actuator_velocity_in[worldid, uid] + g = util_misc.lugre_stribeck(velocity, F_C, F_S, v_S) + a = -sigma0 * wp.abs(velocity) / wp.max(MJ_MINVAL, g) + h = opt_timestep[worldid % opt_timestep.shape[0]] + exp_ah = wp.exp(a * h) + int_h = h + if wp.abs(a) > MJ_MINVAL: + int_h = (exp_ah - 1.0) / a + ctrl_act = exp_ah * act + int_h * velocity + elif offset == slots[1]: # integral + ctrl_act = act + act_dot * opt_timestep[worldid % opt_timestep.shape[0]] + Imax = dynprm[8] + if Imax > 0.0: + ctrl_act = wp.clamp(ctrl_act, -Imax, Imax) + else: # temperature or slew or default + ctrl_act = act + act_dot * opt_timestep[worldid % opt_timestep.shape[0]] + + if actuator_actlimited[uid]: + actrange = actuator_actrange[worldid % actuator_actrange.shape[0], uid] + ctrl_act = wp.clamp(ctrl_act, actrange[0], actrange[1]) + else: + ctrl_act = next_act( + opt_timestep[worldid % opt_timestep.shape[0]], + dyntype, + dynprm, + actuator_actrange[worldid % actuator_actrange.shape[0], uid], + act, + act_dot, + 1.0, + actuator_actlimited[uid], + ) else: ctrl_act = act_in[worldid, act_last] @@ -712,6 +919,32 @@ def _actuator_force( acc0 = actuator_acc0[worldid % actuator_acc0.shape[0], uid] lengthrange = actuator_lengthrange[worldid % actuator_lengthrange.shape[0], uid] gain = util_misc.muscle_gain(length, velocity, lengthrange, acc0, gainprm) + elif gaintype == GainType.DCMOTOR: + R = gainprm[0] + K = gainprm[1] + te = dynprm[0] + + slots = util_misc.dcmotor_slots(dynprm, gainprm) + adr = act_first + + if slots[2] >= 0: + T = act_in[worldid, adr + slots[2]] + alpha = gainprm[2] + T0 = gainprm[3] + Ta = dynprm[4] + R *= 1.0 + alpha * (T + Ta - T0) + + gain = K if te > 0.0 else K / wp.max(MJ_MINVAL, R) + + if te <= 0.0: + input_mode = int(gainprm[8]) + if input_mode > 0: + x_I = 0.0 + if slots[1] >= 0: + x_I = act_in[worldid, adr + slots[1]] + ctrl_act = util_misc.dcmotor_voltage(ctrl, length, velocity, x_I, gainprm) + else: + ctrl_act = ctrl # GainType.USER: gain stays 0, modified by act_gain_callback # bias @@ -725,6 +958,10 @@ def _actuator_force( acc0 = actuator_acc0[worldid % actuator_acc0.shape[0], uid] lengthrange = actuator_lengthrange[worldid % actuator_lengthrange.shape[0], uid] bias = util_misc.muscle_bias(length, lengthrange, acc0, biasprm) + elif biastype == BiasType.DCMOTOR: + if dynprm[0] <= 0.0: + K = gainprm[1] + bias -= gain * K * velocity force = gain * ctrl_act + bias @@ -732,6 +969,25 @@ def _actuator_force( forcerange = actuator_forcerange[worldid % actuator_forcerange.shape[0], uid] force = wp.clamp(force, forcerange[0], forcerange[1]) + # add DC motor mechanical forces (not subject to current limits) + if biastype == BiasType.DCMOTOR: + # cogging torque + A = biasprm[0] + if A != 0.0: + Np = biasprm[1] + phi = biasprm[2] + force += A * wp.sin(Np * length + phi) + + # LuGre friction + sigma0 = dynprm[5] + if sigma0 > 0.0: + sigma1 = dynprm[6] + slots = util_misc.dcmotor_slots(dynprm, gainprm) + adr = act_first + slots[3] # slots[3] is bristle + z = act_in[worldid, adr] + z_dot = act_dot_out[worldid, adr] + force -= sigma0 * z + sigma1 * z_dot + actuator_force_out[worldid, uid] = force @@ -839,6 +1095,7 @@ def fwd_actuation(m: Model, d: Data): if not m.nu or (m.opt.disableflags & DisableBit.ACTUATION): d.act_dot.zero_() d.qfrc_actuator.zero_() + d.actuator_force.zero_() return wp.launch( @@ -1003,10 +1260,7 @@ def forward(m: Model, d: Data): @event_scope def step(m: Model, d: Data): """Advance simulation.""" - # TODO(team): mj_checkPos - # TODO(team): mj_checkVel forward(m, d) - # TODO(team): mj_checkAcc if m.opt.integrator == IntegratorType.EULER: euler(m, d) @@ -1022,8 +1276,6 @@ def step(m: Model, d: Data): def step1(m: Model, d: Data): """Advance simulation in two phases: before input is set by user.""" energy = m.opt.enableflags & EnableBit.ENERGY - # TODO(team): mj_checkPos - # TODO(team): mj_checkVel fwd_position(m, d) d.sensordata.zero_() sensor.sensor_pos(m, d) @@ -1053,7 +1305,6 @@ def step2(m: Model, d: Data): fwd_acceleration(m, d) solver.solve(m, d) sensor.sensor_acc(m, d) - # TODO(team): mj_checkAcc # integrate with Euler or implicitfast # TODO(team): implicit diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/inverse.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/inverse.py index 065afde0..82b7cf4a 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/inverse.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/inverse.py @@ -21,6 +21,7 @@ from mujoco.mjx.third_party.mujoco_warp._src import sensor from mujoco.mjx.third_party.mujoco_warp._src import smooth from mujoco.mjx.third_party.mujoco_warp._src import solver from mujoco.mjx.third_party.mujoco_warp._src import support +from mujoco.mjx.third_party.mujoco_warp._src import util_misc from mujoco.mjx.third_party.mujoco_warp._src.support import mul_m from mujoco.mjx.third_party.mujoco_warp._src.types import Data from mujoco.mjx.third_party.mujoco_warp._src.types import DisableBit @@ -36,14 +37,22 @@ def _qfrc_eulerdamp( # Model: opt_timestep: wp.array[float], dof_damping: wp.array2d[float], + dof_dampingpoly: wp.array2d[wp.vec2], # Data in: + qvel_in: wp.array2d[float], qacc_in: wp.array2d[float], # Out: qfrc_out: wp.array2d[float], ): worldid, dofid = wp.tid() timestep = opt_timestep[worldid % opt_timestep.shape[0]] - qfrc_out[worldid, dofid] += timestep * dof_damping[worldid % dof_damping.shape[0], dofid] * qacc_in[worldid, dofid] + + damping = dof_damping[worldid % dof_damping.shape[0], dofid] + dpoly = dof_dampingpoly[worldid % dof_dampingpoly.shape[0], dofid] + v = qvel_in[worldid, dofid] + + damp_deriv = util_misc._poly_force_deriv(damping, dpoly, v, 1) + qfrc_out[worldid, dofid] += timestep * damp_deriv * qacc_in[worldid, dofid] @wp.kernel @@ -91,11 +100,11 @@ def discrete_acc(m: Model, d: Data, qacc: wp.array2d[float]): # d.qM @ d.qacc support.mul_m(m, d, qfrc, d.qacc) - # qfrc += m.opt.timestep * m.dof_damping * d.qacc + # qfrc += m.opt.timestep * damp_deriv * d.qacc wp.launch( _qfrc_eulerdamp, dim=(d.nworld, m.nv), - inputs=[m.opt.timestep, m.dof_damping, d.qacc], + inputs=[m.opt.timestep, m.dof_damping, m.dof_dampingpoly, d.qvel, d.qacc], outputs=[qfrc], ) elif m.opt.integrator == IntegratorType.IMPLICITFAST: diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py index 56bbb18c..dc834e06 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py @@ -138,6 +138,10 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: if (mjm.sensor_plugin != -1).any(): raise NotImplementedError("Sensor plugins not supported.") + # array sizes may change in the future + if mujoco.mjNPOLY != 2: + warnings.warn(f"mujoco.mjNPOLY is {mujoco.mjNPOLY}, expected 2. Higher order polynomials may not be supported correctly.") + # TODO(team): remove after _update_gradient for Newton uses tile operations for islands nv_max = 60 if mjm.nv > nv_max and mjm.opt.jacobian == mujoco.mjtJacobian.mjJAC_DENSE: @@ -222,7 +226,10 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: m.nsensortaxel = mjm.mesh_vertnum[mjm.sensor_objid[mjm.sensor_type == mujoco.mjtSensor.mjSENS_TACTILE]].sum() m.nsensorcontact = (mjm.sensor_type == mujoco.mjtSensor.mjSENS_CONTACT).sum() m.nrangefinder = (mjm.sensor_type == mujoco.mjtSensor.mjSENS_RANGEFINDER).sum() - m.nmaxcondim = np.concatenate(([0], mjm.geom_condim, mjm.pair_dim)).max() + condim_arrays = [np.array([0]), mjm.geom_condim, mjm.pair_dim] + if mjm.nflex > 0: + condim_arrays.append(mjm.flex_condim) + m.nmaxcondim = np.concatenate(condim_arrays).max() m.nmaxpyramid = np.maximum(1, 2 * (m.nmaxcondim - 1)) m.has_sdf_geom = (mjm.geom_type == mujoco.mjtGeom.mjGEOM_SDF).any() m.block_dim = types.BlockDim() @@ -266,6 +273,21 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: m.jnt_limited_ball_adr = np.nonzero(mjm.jnt_limited & (mjm.jnt_type == mujoco.mjtJoint.mjJNT_BALL))[0] m.dof_tri_row, m.dof_tri_col = np.tril_indices(mjm.nv) + # precompute body_isdofancestor: which DOFs affect each body + # TODO: Investigate alternative approach such as bitmap + body_isdofancestor = np.zeros((mjm.nbody, m.nv_pad), dtype=np.int32) + for bodyid in range(mjm.nbody): + b = bodyid + while b > 0 and mjm.body_dofnum[b] == 0: + b = mjm.body_parentid[b] + if mjm.body_dofnum[b] == 0: + continue + dofid = mjm.body_dofadr[b] + mjm.body_dofnum[b] - 1 + while dofid >= 0: + body_isdofancestor[bodyid, dofid] = 1 + dofid = mjm.dof_parentid[dofid] + m.body_isdofancestor = body_isdofancestor + # precalculated geom pairs filterparent = not (mjm.opt.disableflags & types.DisableBit.FILTERPARENT) @@ -918,7 +940,10 @@ def make_data( raise ValueError(f"nccdmax ({nccdmax}) must be <= nconmax ({nconmax})") sizes = dict({"*": 1}, **{f.name: getattr(mjm, f.name, None) for f in dataclasses.fields(types.Model) if f.type is int}) - sizes["nmaxcondim"] = np.concatenate(([0], mjm.geom_condim, mjm.pair_dim)).max() + condim_arrays = [np.array([0]), mjm.geom_condim, mjm.pair_dim] + if mjm.nflex > 0: + condim_arrays.append(mjm.flex_condim) + sizes["nmaxcondim"] = np.concatenate(condim_arrays).max() sizes["nmaxpyramid"] = np.maximum(1, 2 * (sizes["nmaxcondim"] - 1)) tile_size = types.TILE_SIZE_JTDAJ_SPARSE if is_sparse(mjm) else types.TILE_SIZE_JTDAJ_DENSE sizes["njmax_pad"], sizes["nv_pad"] = _get_padded_sizes(mjm.nv, njmax, is_sparse(mjm), tile_size) @@ -1090,7 +1115,10 @@ def put_data( raise ValueError(f"njmax overflow (njmax must be >= {mjd.nefc})") sizes = dict({"*": 1}, **{f.name: getattr(mjm, f.name, None) for f in dataclasses.fields(types.Model) if f.type is int}) - sizes["nmaxcondim"] = np.concatenate(([0], mjm.geom_condim, mjm.pair_dim)).max() + condim_arrays = [np.array([0]), mjm.geom_condim, mjm.pair_dim] + if mjm.nflex > 0: + condim_arrays.append(mjm.flex_condim) + sizes["nmaxcondim"] = np.concatenate(condim_arrays).max() sizes["nmaxpyramid"] = np.maximum(1, 2 * (sizes["nmaxcondim"] - 1)) tile_size = types.TILE_SIZE_JTDAJ_SPARSE if is_sparse(mjm) else types.TILE_SIZE_JTDAJ_DENSE sizes["njmax_pad"], sizes["nv_pad"] = _get_padded_sizes(mjm.nv, njmax, is_sparse(mjm), tile_size) @@ -1183,7 +1211,7 @@ def put_data( if mujoco.mj_isSparse(mjm): mujoco.mju_sparse2dense(mj_efc_J, mjd.efc_J, mjd.efc_J_rownnz, mjd.efc_J_rowadr, mjd.efc_J_colind) else: - mj_efc_J = mjd.efc_J.reshape((mjd.nefc, mjm.nv)) + mj_efc_J = mjd.efc_J.reshape((-1, mjm.nv))[: mjd.nefc] efc_J = np.zeros((nworld, sizes["njmax_pad"], sizes["nv_pad"]), dtype=float) efc_J[:, : mjd.nefc, : mjm.nv] = np.tile(mj_efc_J, (nworld, 1, 1)) efc.J = wp.array(efc_J, dtype=float) @@ -2659,6 +2687,7 @@ def create_render_context( cam_active: list[bool] | None = None, flex_render_smooth: bool = True, use_precomputed_rays: bool = True, + render_skybox: bool = False, ) -> types.RenderContext: """Creates a render context on device. @@ -2669,8 +2698,8 @@ def create_render_context( MuJoCo model values. render_rgb: Whether to render RGB images. If None, uses the MuJoCo model values. render_depth: Whether to render depth images. If None, uses the MuJoCo model values. - render_seg: Whether to render segmentation (per-pixel geom IDs). If None, - uses the MuJoCo model values. + render_seg: Whether to render segmentation (per-pixel object ID/type pairs). + If None, uses the MuJoCo model values. use_textures: Whether to use textures. use_shadows: Whether to use shadows. enabled_geom_groups: The geom groups to render. @@ -2679,6 +2708,8 @@ def create_render_context( flex_render_smooth: Whether to render flex meshes smoothly. use_precomputed_rays: Use precomputed rays instead of computing during rendering. When using domain randomization for camera intrinsics, set to False. + render_skybox: Whether to shade missed rays with the MuJoCo skybox texture. + Requires the model to contain a texture with type `mjTEXTURE_SKYBOX`. Returns: The render context containing rendering fields and output arrays on device. @@ -2737,27 +2768,37 @@ def create_render_context( flex_geom_flexid = [] flex_geom_edgeid = [] flex_bvh_id = np.full(nflex, 0, dtype=wp.uint64) - flex_group_root = np.zeros((nflex, nworld), dtype=int) + # Indexed later as [worldid, flexid]. + flex_group_root = np.full((nworld, nflex), -1, dtype=int) for f in range(nflex): if mjm.flex_dim[f] == 1: edge_adr = mjm.flex_edgeadr[f] flex_geom_flexid.extend([f] * mjm.flex_edgenum[f]) flex_geom_edgeid.extend([edge_adr + e for e in range(mjm.flex_edgenum[f])]) - flex_group_root[f] = np.zeros(nworld, dtype=int) else: flex_geom_flexid.append(f) flex_geom_edgeid.append(-1) fmesh, group_root = bvh.build_flex_bvh(mjm, mjd, nworld, f) flex_registry[f] = fmesh flex_bvh_id[f] = fmesh.id - flex_group_root[f] = group_root.numpy() + flex_group_root[:, f] = group_root.numpy() textures_registry = [] for i in range(mjm.ntex): textures_registry.append(render_util.create_warp_texture(mjm, i)) textures = wp.array(textures_registry, dtype=wp.Texture2D) + # Locate skybox texture + skybox_tex_ids = np.nonzero(mjm.tex_type == mujoco.mjtTexture.mjTEXTURE_SKYBOX)[0] if mjm.ntex else np.array([], dtype=int) + if render_skybox: + assert skybox_tex_ids.size > 0, "render_skybox=True but the model has no texture with type mjTEXTURE_SKYBOX" + skybox_tex_id = int(skybox_tex_ids[0]) + skybox_face_width = int(mjm.tex_width[skybox_tex_id]) + else: + skybox_tex_id = -1 + skybox_face_width = 1 + # Filter active cameras if cam_active is not None: assert len(cam_active) == mjm.ncam, f"cam_active must have length {mjm.ncam} (got {len(cam_active)})" @@ -2857,6 +2898,9 @@ def create_render_context( use_shadows=use_shadows, background_color=render_util.pack_rgba_to_uint32(0.1 * 255.0, 0.1 * 255.0, 0.2 * 255.0, 1.0 * 255.0), use_precomputed_rays=use_precomputed_rays, + render_skybox=render_skybox, + skybox_tex_id=skybox_tex_id, + skybox_face_width=skybox_face_width, bvh_ngeom=bvh_ngeom, enabled_geom_ids=wp.array(geom_enabled_idx, dtype=int), mesh_registry=mesh_registry, @@ -2892,7 +2936,7 @@ def create_render_context( depth_adr=wp.array(depth_adr, dtype=int), render_rgb=wp.array(render_rgb, dtype=bool), render_depth=wp.array(render_depth, dtype=bool), - seg_data=wp.zeros((nworld, max(si, 1)), dtype=int), + seg_data=wp.zeros((nworld, max(si, 1)), dtype=wp.vec2i), seg_adr=wp.array(seg_adr, dtype=int), render_seg=wp.array(render_seg, dtype=bool), znear=znear, diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py index 4c76d1f8..31469590 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py @@ -17,6 +17,7 @@ import warp as wp from mujoco.mjx.third_party.mujoco_warp._src import math from mujoco.mjx.third_party.mujoco_warp._src import support +from mujoco.mjx.third_party.mujoco_warp._src import util_misc from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINVAL from mujoco.mjx.third_party.mujoco_warp._src.types import Data from mujoco.mjx.third_party.mujoco_warp._src.types import DisableBit @@ -76,7 +77,9 @@ def _spring_damper_dof_passive( jnt_qposadr: wp.array[int], jnt_dofadr: wp.array[int], jnt_stiffness: wp.array2d[float], + jnt_stiffnesspoly: wp.array2d[wp.vec2], dof_damping: wp.array2d[float], + dof_dampingpoly: wp.array2d[wp.vec2], # Data in: qpos_in: wp.array2d[float], qvel_in: wp.array2d[float], @@ -86,22 +89,37 @@ def _spring_damper_dof_passive( ): worldid, jntid = wp.tid() dofid = jnt_dofadr[jntid] + jnttype = jnt_type[jntid] stiffness = jnt_stiffness[worldid % jnt_stiffness.shape[0], jntid] + spoly = jnt_stiffnesspoly[worldid % jnt_stiffnesspoly.shape[0], jntid] damping = dof_damping[worldid % dof_damping.shape[0], dofid] + dpoly = dof_dampingpoly[worldid % dof_dampingpoly.shape[0], dofid] - has_stiffness = stiffness != 0.0 and not (opt_disableflags & DisableBit.SPRING) - has_damping = damping != 0.0 and not (opt_disableflags & DisableBit.DAMPER) + has_stiffness = (stiffness != 0.0 or spoly[0] != 0.0 or spoly[1] != 0.0) and not (opt_disableflags & DisableBit.SPRING) + has_damping = (damping != 0.0 or dpoly[0] != 0.0 or dpoly[1] != 0.0) and not (opt_disableflags & DisableBit.DAMPER) if not has_stiffness: - qfrc_spring_out[worldid, dofid] = 0.0 + if jnttype == JointType.FREE: + for i in range(6): + qfrc_spring_out[worldid, dofid + i] = 0.0 + elif jnttype == JointType.BALL: + for i in range(3): + qfrc_spring_out[worldid, dofid + i] = 0.0 + else: + qfrc_spring_out[worldid, dofid] = 0.0 if not has_damping: - qfrc_damper_out[worldid, dofid] = 0.0 + if jnttype == JointType.FREE: + for i in range(6): + qfrc_damper_out[worldid, dofid + i] = 0.0 + elif jnttype == JointType.BALL: + for i in range(3): + qfrc_damper_out[worldid, dofid + i] = 0.0 + else: + qfrc_damper_out[worldid, dofid] = 0.0 if not (has_stiffness or has_damping): return - - jnttype = jnt_type[jntid] qposid = jnt_qposadr[jntid] qpos_spring_id = worldid % qpos_spring.shape[0] @@ -113,9 +131,12 @@ def _spring_damper_dof_passive( qpos_in[worldid, qposid + 1] - qpos_spring[qpos_spring_id, qposid + 1], qpos_in[worldid, qposid + 2] - qpos_spring[qpos_spring_id, qposid + 2], ) - qfrc_spring_out[worldid, dofid + 0] = -stiffness * dif[0] - qfrc_spring_out[worldid, dofid + 1] = -stiffness * dif[1] - qfrc_spring_out[worldid, dofid + 2] = -stiffness * dif[2] + r = wp.length(dif) + k = util_misc._poly_force(stiffness, spoly, r, 0) + qfrc_spring_out[worldid, dofid + 0] = -k * dif[0] + qfrc_spring_out[worldid, dofid + 1] = -k * dif[1] + qfrc_spring_out[worldid, dofid + 2] = -k * dif[2] + rot = wp.quat( qpos_in[worldid, qposid + 3], qpos_in[worldid, qposid + 4], @@ -130,18 +151,18 @@ def _spring_damper_dof_passive( qpos_spring[qpos_spring_id, qposid + 6], ) dif = math.quat_sub(rot, ref) - qfrc_spring_out[worldid, dofid + 3] = -stiffness * dif[0] - qfrc_spring_out[worldid, dofid + 4] = -stiffness * dif[1] - qfrc_spring_out[worldid, dofid + 5] = -stiffness * dif[2] + r_rot = wp.length(dif) + k_rot = util_misc._poly_force(stiffness, spoly, r_rot, 0) + qfrc_spring_out[worldid, dofid + 3] = -k_rot * dif[0] + qfrc_spring_out[worldid, dofid + 4] = -k_rot * dif[1] + qfrc_spring_out[worldid, dofid + 5] = -k_rot * dif[2] # damper if has_damping: - qfrc_damper_out[worldid, dofid + 0] = -damping * qvel_in[worldid, dofid + 0] - qfrc_damper_out[worldid, dofid + 1] = -damping * qvel_in[worldid, dofid + 1] - qfrc_damper_out[worldid, dofid + 2] = -damping * qvel_in[worldid, dofid + 2] - qfrc_damper_out[worldid, dofid + 3] = -damping * qvel_in[worldid, dofid + 3] - qfrc_damper_out[worldid, dofid + 4] = -damping * qvel_in[worldid, dofid + 4] - qfrc_damper_out[worldid, dofid + 5] = -damping * qvel_in[worldid, dofid + 5] + for i in range(6): + v = qvel_in[worldid, dofid + i] + qfrc_damper_out[worldid, dofid + i] = -v * util_misc._poly_force(damping, dpoly, v, 1) + elif jnttype == JointType.BALL: # spring if has_stiffness: @@ -159,24 +180,28 @@ def _spring_damper_dof_passive( qpos_spring[qpos_spring_id, qposid + 3], ) dif = math.quat_sub(rot, ref) - qfrc_spring_out[worldid, dofid + 0] = -stiffness * dif[0] - qfrc_spring_out[worldid, dofid + 1] = -stiffness * dif[1] - qfrc_spring_out[worldid, dofid + 2] = -stiffness * dif[2] + r = wp.length(dif) + k = util_misc._poly_force(stiffness, spoly, r, 0) + qfrc_spring_out[worldid, dofid + 0] = -k * dif[0] + qfrc_spring_out[worldid, dofid + 1] = -k * dif[1] + qfrc_spring_out[worldid, dofid + 2] = -k * dif[2] # damper if has_damping: - qfrc_damper_out[worldid, dofid + 0] = -damping * qvel_in[worldid, dofid + 0] - qfrc_damper_out[worldid, dofid + 1] = -damping * qvel_in[worldid, dofid + 1] - qfrc_damper_out[worldid, dofid + 2] = -damping * qvel_in[worldid, dofid + 2] + for i in range(3): + v = qvel_in[worldid, dofid + i] + qfrc_damper_out[worldid, dofid + i] = -v * util_misc._poly_force(damping, dpoly, v, 1) + else: # mjJNT_SLIDE, mjJNT_HINGE # spring if has_stiffness: fdif = qpos_in[worldid, qposid] - qpos_spring[qpos_spring_id, qposid] - qfrc_spring_out[worldid, dofid] = -stiffness * fdif + qfrc_spring_out[worldid, dofid] = -fdif * util_misc._poly_force(stiffness, spoly, fdif, 0) # damper if has_damping: - qfrc_damper_out[worldid, dofid] = -damping * qvel_in[worldid, dofid] + v = qvel_in[worldid, dofid] + qfrc_damper_out[worldid, dofid] = -v * util_misc._poly_force(damping, dpoly, v, 1) @wp.kernel @@ -186,7 +211,9 @@ def _spring_damper_tendon_passive( ten_J_rowadr: wp.array[int], ten_J_colind: wp.array[int], tendon_stiffness: wp.array2d[float], + tendon_stiffnesspoly: wp.array2d[wp.vec2], tendon_damping: wp.array2d[float], + tendon_dampingpoly: wp.array2d[wp.vec2], tendon_lengthspring: wp.array2d[wp.vec2], # Data in: ten_J_in: wp.array2d[float], @@ -202,10 +229,12 @@ def _spring_damper_tendon_passive( worldid, tenid, dofid_sparse = wp.tid() stiffness = tendon_stiffness[worldid % tendon_stiffness.shape[0], tenid] + spoly = tendon_stiffnesspoly[worldid % tendon_stiffnesspoly.shape[0], tenid] damping = tendon_damping[worldid % tendon_damping.shape[0], tenid] + dpoly = tendon_dampingpoly[worldid % tendon_dampingpoly.shape[0], tenid] - has_stiffness = stiffness != 0.0 and not dsbl_spring - has_damping = damping != 0.0 and not dsbl_damper + has_stiffness = (stiffness != 0.0 or spoly[0] != 0.0 or spoly[1] != 0.0) and not dsbl_spring + has_damping = (damping != 0.0 or dpoly[0] != 0.0 or dpoly[1] != 0.0) and not dsbl_damper if not has_stiffness and not has_damping: return @@ -225,19 +254,16 @@ def _spring_damper_tendon_passive( lower = lengthspring[0] upper = lengthspring[1] - if length > upper: - frc_spring = stiffness * (upper - length) - elif length < lower: - frc_spring = stiffness * (lower - length) - else: - frc_spring = 0.0 + x = wp.where(length > upper, length - upper, wp.where(length < lower, length - lower, 0.0)) + frc_spring = -x * util_misc._poly_force(stiffness, spoly, x, 0) # transform to joint torque wp.atomic_add(qfrc_spring_out[worldid], dofid, J * frc_spring) if has_damping: - # compute damper linear force along tendon - frc_damper = -damping * ten_velocity_in[worldid, tenid] + # compute damper force along tendon + v = ten_velocity_in[worldid, tenid] + frc_damper = -v * util_misc._poly_force(damping, dpoly, v, 1) # transform to joint torque wp.atomic_add(qfrc_damper_out[worldid], dofid, J * frc_damper) @@ -252,6 +278,7 @@ def _gravity_force( body_mass: wp.array2d[float], body_gravcomp: wp.array2d[float], dof_bodyid: wp.array[int], + body_isdofancestor: wp.array2d[int], # Data in: xipos_in: wp.array2d[wp.vec3], subtree_com_in: wp.array2d[wp.vec3], @@ -267,7 +294,9 @@ def _gravity_force( if gravcomp: force = -gravity * body_mass[worldid % body_mass.shape[0], bodyid] * gravcomp pos = xipos_in[worldid, bodyid] - jac, _ = support.jac_dof(body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, pos, bodyid, dofid, worldid) + jac, _ = support.jac_dof( + body_parentid, body_rootid, dof_bodyid, body_isdofancestor, subtree_com_in, cdof_in, pos, bodyid, dofid, worldid + ) wp.atomic_add(qfrc_gravcomp_out[worldid], dofid, wp.dot(jac, force)) @@ -715,9 +744,10 @@ def _flex_bending( force = wp.matrix(0.0, shape=(nvert, 3)) for i in range(nvert): for x in range(3): + acc = float(0.0) for j in range(nvert): - force[i, x] -= flex_bending[edgeid, 4 * i + j] * flexvert_xpos_in[worldid, v[j]][x] - force[i, x] -= flex_bending[edgeid, 16] * frc[i, x] + acc += flex_bending[edgeid, 4 * i + j] * flexvert_xpos_in[worldid, v[j]][x] + force[i, x] = -(acc + flex_bending[edgeid, 16] * frc[i, x]) for i in range(nvert): bodyid = flex_vertbodyid[v[i]] @@ -749,7 +779,9 @@ def passive(m: Model, d: Data): m.jnt_qposadr, m.jnt_dofadr, m.jnt_stiffness, + m.jnt_stiffnesspoly, m.dof_damping, + m.dof_dampingpoly, d.qpos, d.qvel, ], @@ -765,7 +797,9 @@ def passive(m: Model, d: Data): m.ten_J_rowadr, m.ten_J_colind, m.tendon_stiffness, + m.tendon_stiffnesspoly, m.tendon_damping, + m.tendon_dampingpoly, m.tendon_lengthspring, d.ten_J, d.ten_length, @@ -840,6 +874,7 @@ def passive(m: Model, d: Data): m.body_mass, m.body_gravcomp, m.dof_bodyid, + m.body_isdofancestor, d.xipos, d.subtree_com, d.cdof, diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render.py index 371032e1..ba3b4cfe 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render.py @@ -34,6 +34,7 @@ from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MAXVAL from mujoco.mjx.third_party.mujoco_warp._src.types import Data from mujoco.mjx.third_party.mujoco_warp._src.types import GeomType from mujoco.mjx.third_party.mujoco_warp._src.types import Model +from mujoco.mjx.third_party.mujoco_warp._src.types import ObjType from mujoco.mjx.third_party.mujoco_warp._src.types import RenderContext from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope @@ -84,6 +85,72 @@ def sample_texture( return wp.vec3(tex_color[0], tex_color[1], tex_color[2]) +@wp.func +def sample_skybox( + # In: + skybox_tex: wp.Texture2D, + face_width_inv: float, + ray_dir_world: wp.vec3, +) -> wp.vec3: + # MuJoCo maps a world-space direction to cube-map space by rotating 90° about X + # (see render_gl3.c: S=x, T=z, R=-y). Faces in tex_data are stacked vertically + # in OpenGL cube-face order: +X, -X, +Y, -Y, +Z, -Z. + rx = ray_dir_world[0] + ry = ray_dir_world[2] + rz = -ray_dir_world[1] + + arx = wp.abs(rx) + ary = wp.abs(ry) + arz = wp.abs(rz) + + face = int(0) + sc = float(0.0) + tc = float(0.0) + ma = float(1.0) + + if arx >= ary and arx >= arz: + ma = arx + if rx > 0.0: + face = 0 + sc = -rz + tc = -ry + else: + face = 1 + sc = rz + tc = -ry + elif ary >= arz: + ma = ary + if ry > 0.0: + face = 2 + sc = rx + tc = rz + else: + face = 3 + sc = rx + tc = -rz + else: + ma = arz + if rz > 0.0: + face = 4 + sc = rx + tc = -ry + else: + face = 5 + sc = -rx + tc = -ry + + s = (math.safe_div(sc, ma) + 1.0) * 0.5 + t = (math.safe_div(tc, ma) + 1.0) * 0.5 + + # Keep the linear filter from bleeding between adjacent faces in the vertical strip. + t_min = 0.5 * face_width_inv + t = wp.clamp(t, t_min, 1.0 - t_min) + + v = (float(face) + t) * wp.static(1.0 / 6.0) + color = wp.texture_sample(skybox_tex, wp.vec2(s, v), dtype=wp.vec4) + return wp.vec3(color[0], color[1], color[2]) + + # TODO: Investigate combining cast_ray and cast_ray_first_hit @wp.func def cast_ray( @@ -525,7 +592,7 @@ def render(m: Model, d: Data, rc: RenderContext): """ rc.rgb_data.fill_(rc.background_color) rc.depth_data.fill_(0.0) - rc.seg_data.fill_(-1) + rc.seg_data.fill_(wp.vec2i(-1, -1)) @wp.kernel(module="unique", enable_backward=False) def _render_megakernel( @@ -588,7 +655,7 @@ def render(m: Model, d: Data, rc: RenderContext): # Out: rgb_out: wp.array2d[wp.uint32], depth_out: wp.array2d[float], - seg_out: wp.array2d[int], + seg_out: wp.array2d[wp.vec2i], ): worldid, rayid = wp.tid() @@ -661,10 +728,25 @@ def render(m: Model, d: Data, rc: RenderContext): ) if render_seg[cam_idx] and geom_id != -1: - seg_out[worldid, seg_adr[cam_idx] + rayid_local] = geom_id + if geom_id == -2: + seg_out[worldid, seg_adr[cam_idx] + rayid_local] = wp.vec2i(mesh_id, int(ObjType.FLEX)) + else: + seg_out[worldid, seg_adr[cam_idx] + rayid_local] = wp.vec2i(geom_id, int(ObjType.GEOM)) # Early Out if geom_id == -1: + if wp.static(rc.render_skybox) and render_rgb[cam_idx]: + skybox_color = sample_skybox( + textures[wp.static(rc.skybox_tex_id)], + wp.static(1.0 / float(rc.skybox_face_width)), + ray_dir_world, + ) + rgb_out[worldid, rgb_adr[cam_idx] + rayid_local] = pack_rgba_to_uint32( + skybox_color[0] * 255.0, + skybox_color[1] * 255.0, + skybox_color[2] * 255.0, + 255.0, + ) return if render_depth[cam_idx]: diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render_util.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render_util.py index c59da005..b87c8b2e 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render_util.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/render_util.py @@ -211,13 +211,13 @@ def get_depth(rc: RenderContext, camera_index: int, depth_scale: float, depth_ou @wp.kernel def _extract_seg_kernel( # In: - seg_data: wp.array2d[int], + seg_data: wp.array2d[wp.vec2i], seg_adr: wp.array[int], camera_index: int, # Out: - seg_out: wp.array3d[int], + seg_out: wp.array3d[wp.vec2i], ): - """Extract per-pixel geom IDs from the render context buffers for a given camera index.""" + """Extract per-pixel `(object_id, object_type)` pairs for a camera.""" worldid, pixelid = wp.tid() xid = pixelid % seg_out.shape[2] yid = pixelid // seg_out.shape[2] @@ -226,17 +226,18 @@ def _extract_seg_kernel( seg_out[worldid, yid, xid] = seg_data[worldid, seg_adr_offset + pixelid] -def get_segmentation(rc: RenderContext, camera_index: int, seg_out: wp.array3d[int]): +def get_segmentation(rc: RenderContext, camera_index: int, seg_out: wp.array3d[wp.vec2i]): """Get the segmentation data from the render context buffers for a given camera index. - Each pixel contains the MuJoCo geom ID of the geometry hit by the ray, -1 for - background, or -2 for flex bodies. + Each pixel stores MuJoCo-style `(object_id, object_type)` data. Background + pixels are `(-1, -1)`. Regular geometry hits are `(geom_id, mjOBJ_GEOM)`. + Flex hits are `(flex_id, mjOBJ_FLEX)`. Args: rc: The render context on device. camera_index: The index of the camera to get the segmentation data for. - seg_out: The output array to store the geom IDs in, with shape - (nworld, height, width). + seg_out: The output array to store segmentation data in, with shape + `(nworld, height, width)` and dtype `wp.vec2i`. """ wp.launch( _extract_seg_kernel, diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py index b615d20c..381dfd66 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py @@ -44,6 +44,7 @@ from mujoco.mjx.third_party.mujoco_warp._src.types import vec8 from mujoco.mjx.third_party.mujoco_warp._src.types import vec8i from mujoco.mjx.third_party.mujoco_warp._src.types import vec_pluginattr from mujoco.mjx.third_party.mujoco_warp._src.util_misc import inside_geom +from mujoco.mjx.third_party.mujoco_warp._src.util_misc import poly_potential from mujoco.mjx.third_party.mujoco_warp._src.warp_util import cache_kernel from mujoco.mjx.third_party.mujoco_warp._src.warp_util import event_scope @@ -2735,6 +2736,7 @@ def _energy_pos_passive_joint( jnt_type: wp.array[int], jnt_qposadr: wp.array[int], jnt_stiffness: wp.array2d[float], + jnt_stiffnesspoly: wp.array2d[wp.vec2], # Data in: qpos_in: wp.array2d[float], # Data out: @@ -2743,8 +2745,9 @@ def _energy_pos_passive_joint( worldid, jntid = wp.tid() jnt_stiffness_id = worldid % jnt_stiffness.shape[0] stiffness = jnt_stiffness[jnt_stiffness_id, jntid] + spoly = jnt_stiffnesspoly[worldid % jnt_stiffnesspoly.shape[0], jntid] - if stiffness == 0.0: + if stiffness == 0.0 and spoly[0] == 0.0 and spoly[1] == 0.0: return padr = jnt_qposadr[jntid] @@ -2776,8 +2779,11 @@ def _energy_pos_passive_joint( dif1 = math.quat_sub(quat1, quat_spring) + r0 = wp.length(dif0) + r1 = wp.length(dif1) + energy = wp.vec2( - 0.5 * stiffness * (wp.dot(dif0, dif0) + wp.dot(dif1, dif1)), + poly_potential(stiffness, spoly, r0, 0) + poly_potential(stiffness, spoly, r1, 0), 0.0, ) @@ -2800,15 +2806,16 @@ def _energy_pos_passive_joint( ) dif = math.quat_sub(quat, quat_spring) + r = wp.length(dif) energy = wp.vec2( - 0.5 * stiffness * wp.dot(dif, dif), + poly_potential(stiffness, spoly, r, 0), 0.0, ) wp.atomic_add(energy_out, worldid, energy) elif jnttype == JointType.SLIDE or jnttype == JointType.HINGE: dif_ = qpos_in[worldid, padr] - qpos_spring[qpos_spring_id, padr] energy = wp.vec2( - 0.5 * stiffness * dif_ * dif_, + poly_potential(stiffness, spoly, dif_, 0), 0.0, ) wp.atomic_add(energy_out, worldid, energy) @@ -2818,6 +2825,7 @@ def _energy_pos_passive_joint( def _energy_pos_passive_tendon( # Model: tendon_stiffness: wp.array2d[float], + tendon_stiffnesspoly: wp.array2d[wp.vec2], tendon_lengthspring: wp.array2d[wp.vec2], # Data in: ten_length_in: wp.array2d[float], @@ -2828,8 +2836,9 @@ def _energy_pos_passive_tendon( tendon_stiffness_id = worldid % tendon_stiffness.shape[0] stiffness = tendon_stiffness[tendon_stiffness_id, tenid] + spoly = tendon_stiffnesspoly[worldid % tendon_stiffnesspoly.shape[0], tenid] - if stiffness == 0.0: + if stiffness == 0.0 and spoly[0] == 0.0 and spoly[1] == 0.0: return length = ten_length_in[worldid, tenid] @@ -2841,13 +2850,13 @@ def _energy_pos_passive_tendon( upper = lengthspring[1] if length > upper: - displacement = upper - length + x = length - upper elif length < lower: - displacement = lower - length + x = length - lower else: - displacement = 0.0 + x = 0.0 - energy = wp.vec2(0.5 * stiffness * displacement * displacement, 0.0) + energy = wp.vec2(poly_potential(stiffness, spoly, x, 0), 0.0) wp.atomic_add(energy_out, worldid, energy) @@ -2871,6 +2880,7 @@ def energy_pos(m: Model, d: Data): m.jnt_type, m.jnt_qposadr, m.jnt_stiffness, + m.jnt_stiffnesspoly, d.qpos, ], outputs=[d.energy], @@ -2883,6 +2893,7 @@ def energy_pos(m: Model, d: Data): dim=(d.nworld, m.ntendon), inputs=[ m.tendon_stiffness, + m.tendon_stiffnesspoly, m.tendon_lengthspring, d.ten_length, ], diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/smooth.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/smooth.py index bdb90ecd..bda36c7c 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/smooth.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/smooth.py @@ -1197,7 +1197,9 @@ def _cfrc( cfrc_int_out: wp.array2d[wp.spatial_vector], ): worldid, bodyid = wp.tid() - bodyid += 1 # skip world body + if bodyid == 0: + cfrc_int_out[worldid, 0] = wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + return cacc = cacc_in[worldid, bodyid] cinert = cinert_in[worldid, bodyid] cvel = cvel_in[worldid, bodyid] @@ -1210,9 +1212,7 @@ def _cfrc( def _rne_cfrc(m: Model, d: Data, flg_cfrc_ext: bool = False): - wp.launch( - _cfrc, dim=[d.nworld, m.nbody - 1], inputs=[d.cinert, d.cvel, d.cacc, d.cfrc_ext, flg_cfrc_ext], outputs=[d.cfrc_int] - ) + wp.launch(_cfrc, dim=[d.nworld, m.nbody], inputs=[d.cinert, d.cvel, d.cacc, d.cfrc_ext, flg_cfrc_ext], outputs=[d.cfrc_int]) @wp.kernel @@ -1983,6 +1983,9 @@ def _comvel_branch( cvel += cdof[dofid + 1] * qvel[dofid + 1] cvel += cdof[dofid + 2] * qvel[dofid + 2] + cdof_dot_out[worldid, dofid + 0] = wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + cdof_dot_out[worldid, dofid + 1] = wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + cdof_dot_out[worldid, dofid + 2] = wp.spatial_vector(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) cdof_dot_out[worldid, dofid + 3] = math.motion_cross(cvel, cdof[dofid + 3]) cdof_dot_out[worldid, dofid + 4] = math.motion_cross(cvel, cdof[dofid + 4]) cdof_dot_out[worldid, dofid + 5] = math.motion_cross(cvel, cdof[dofid + 5]) @@ -2061,6 +2064,7 @@ def _transmission( actuator_trnid: wp.array[wp.vec2i], actuator_gear: wp.array2d[wp.spatial_vector], actuator_cranklength: wp.array2d[float], + body_isdofancestor: wp.array2d[int], # Data in: qpos_in: wp.array2d[float], xquat_in: wp.array2d[wp.quat], @@ -2219,12 +2223,30 @@ def _transmission( # get Jacobians of axis(jacA) and vec(jac) jacp, jacr = support.jac_dof( - body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, site_xpos_idslider, site_bodyid[idslider], da, worldid + body_parentid, + body_rootid, + dof_bodyid, + body_isdofancestor, + subtree_com_in, + cdof_in, + site_xpos_idslider, + site_bodyid[idslider], + da, + worldid, ) jacS = jacp jacA = wp.cross(jacr, axis) jac, _ = support.jac_dof( - body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, site_xpos_id, site_bodyid[id], da, worldid + body_parentid, + body_rootid, + dof_bodyid, + body_isdofancestor, + subtree_com_in, + cdof_in, + site_xpos_id, + site_bodyid[id], + da, + worldid, ) jac -= jacS @@ -2313,6 +2335,7 @@ def _transmission( body_parentid, body_rootid, dof_bodyid, + body_isdofancestor, subtree_com_in, cdof_in, site_xpos_in[worldid, siteid], @@ -2419,10 +2442,28 @@ def _transmission( break jacp, jacr = support.jac_dof( - body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, site_xpos, site_bodyid[siteid], da, worldid + body_parentid, + body_rootid, + dof_bodyid, + body_isdofancestor, + subtree_com_in, + cdof_in, + site_xpos, + site_bodyid[siteid], + da, + worldid, ) jacpref, jacrref = support.jac_dof( - body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, ref_xpos, site_bodyid[refid], da, worldid + body_parentid, + body_rootid, + dof_bodyid, + body_isdofancestor, + subtree_com_in, + cdof_in, + ref_xpos, + site_bodyid[refid], + da, + worldid, ) moment = float(0.0) @@ -2453,6 +2494,7 @@ def _transmission_body_moment( dof_bodyid: wp.array[int], geom_bodyid: wp.array[int], actuator_trnid: wp.array[wp.vec2i], + body_isdofancestor: wp.array2d[int], actuator_trntype_body_adr: wp.array[int], # Data in: subtree_com_in: wp.array2d[wp.vec3], @@ -2568,10 +2610,10 @@ def _transmission_body_moment( colind = dofid jacp1, _ = support.jac_dof( - body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, contact_pos, b1, colind, worldid + body_parentid, body_rootid, dof_bodyid, body_isdofancestor, subtree_com_in, cdof_in, contact_pos, b1, colind, worldid ) jacp2, _ = support.jac_dof( - body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, contact_pos, b2, colind, worldid + body_parentid, body_rootid, dof_bodyid, body_isdofancestor, subtree_com_in, cdof_in, contact_pos, b2, colind, worldid ) jacdif = jacp2 - jacp1 @@ -2635,6 +2677,7 @@ def transmission(m: Model, d: Data): m.actuator_trnid, m.actuator_gear, m.actuator_cranklength, + m.body_isdofancestor, d.qpos, d.xquat, d.site_xpos, @@ -2662,6 +2705,7 @@ def transmission(m: Model, d: Data): m.dof_bodyid, m.geom_bodyid, m.actuator_trnid, + m.body_isdofancestor, m.actuator_trntype_body_adr, d.subtree_com, d.cdof, diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/solver.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/solver.py index 3964107d..0c804716 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/solver.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/solver.py @@ -2785,6 +2785,36 @@ def update_gradient_cholesky_blocked(tile_size: int, matrix_size: int): return kernel +def update_gradient_cholesky_blocked_skip_unchanged(tile_size: int, matrix_size: int): + """Blocked Cholesky that skips factorization when no constraints changed.""" + + @wp.kernel(module="unique", enable_backward=False) + def kernel( + # In: + ctx_done_in: wp.array[bool], + ctx_grad_in: wp.array3d[float], + ctx_h_in: wp.array3d[float], + changed_count_in: wp.array[int], + ctx_hfactor: wp.array3d[float], + # Out: + ctx_Mgrad_out: wp.array3d[float], + ): + worldid = wp.tid() + TILE_SIZE = wp.static(tile_size) + + if ctx_done_in[worldid]: + return + + if changed_count_in[worldid] > 0: + wp.static(create_blocked_cholesky_func(TILE_SIZE))(ctx_h_in[worldid], matrix_size, ctx_hfactor[worldid]) + + wp.static(create_blocked_cholesky_solve_func(TILE_SIZE, matrix_size))( + ctx_hfactor[worldid], ctx_grad_in[worldid], matrix_size, ctx_Mgrad_out[worldid] + ) + + return kernel + + @wp.kernel def padding_h(nv: int, ctx_done_in: wp.array[bool], ctx_h_out: wp.array3d[float]): worldid, elementid = wp.tid() @@ -2796,8 +2826,12 @@ def padding_h(nv: int, ctx_done_in: wp.array[bool], ctx_h_out: wp.array3d[float] ctx_h_out[worldid, dofid, dofid] = 1.0 -def _cholesky_factorize_solve(m: types.Model, d: types.Data, ctx: SolverContext): - """Cholesky factorize ctx.h and solve for Mgrad.""" +def _cholesky_factorize_solve(m: types.Model, d: types.Data, ctx: SolverContext, skip_unchanged: bool = False): + """Cholesky factorize ctx.h and solve for Mgrad. + + If skip_unchanged is True (blocked path only), worlds where no constraints + changed reuse the cached factorization in hfactor instead of refactorizing. + """ if m.nv <= _BLOCK_CHOLESKY_DIM: wp.launch_tiled( update_gradient_cholesky(m.nv), @@ -2814,13 +2848,22 @@ def _cholesky_factorize_solve(m: types.Model, d: types.Data, ctx: SolverContext) outputs=[ctx.h], ) - wp.launch_tiled( - update_gradient_cholesky_blocked(types.TILE_SIZE_JTDAJ_DENSE, m.nv_pad), - dim=d.nworld, - inputs=[ctx.done, ctx.grad.reshape(shape=(d.nworld, ctx.grad.shape[1], 1)), ctx.h, ctx.hfactor], - outputs=[ctx.Mgrad.reshape(shape=(d.nworld, ctx.Mgrad.shape[1], 1))], - block_dim=m.block_dim.update_gradient_cholesky_blocked, - ) + if skip_unchanged: + wp.launch_tiled( + update_gradient_cholesky_blocked_skip_unchanged(types.TILE_SIZE_JTDAJ_DENSE, m.nv_pad), + dim=d.nworld, + inputs=[ctx.done, ctx.grad.reshape(shape=(d.nworld, ctx.grad.shape[1], 1)), ctx.h, ctx.changed_efc_count, ctx.hfactor], + outputs=[ctx.Mgrad.reshape(shape=(d.nworld, ctx.Mgrad.shape[1], 1))], + block_dim=m.block_dim.update_gradient_cholesky_blocked, + ) + else: + wp.launch_tiled( + update_gradient_cholesky_blocked(types.TILE_SIZE_JTDAJ_DENSE, m.nv_pad), + dim=d.nworld, + inputs=[ctx.done, ctx.grad.reshape(shape=(d.nworld, ctx.grad.shape[1], 1)), ctx.h, ctx.hfactor], + outputs=[ctx.Mgrad.reshape(shape=(d.nworld, ctx.Mgrad.shape[1], 1))], + block_dim=m.block_dim.update_gradient_cholesky_blocked, + ) @wp.kernel @@ -3055,7 +3098,7 @@ def _update_gradient_incremental(m: types.Model, d: types.Data, ctx: SolverConte outputs=[ctx.h], ) - _cholesky_factorize_solve(m, d, ctx) + _cholesky_factorize_solve(m, d, ctx, skip_unchanged=True) @wp.kernel diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/support.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/support.py index f6b3fe9d..b42e9808 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/support.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/support.py @@ -393,12 +393,29 @@ def transform_force(frc: wp.spatial_vector, offset: wp.vec3) -> wp.spatial_vecto return transform_force(force, torque, offset) +@wp.func +def _compute_jacp(cdof_clip: wp.spatial_vector, offset: wp.vec3, affect: int) -> wp.vec3: + if affect == 0: + return wp.vec3(0.0) + cdof_lin = wp.spatial_bottom(cdof_clip) + cdof_ang = wp.spatial_top(cdof_clip) + return cdof_lin + wp.cross(cdof_ang, offset) + + +@wp.func +def _compute_jacr(cdof_clip: wp.spatial_vector, affect: int) -> wp.vec3: + if affect == 0: + return wp.vec3(0.0) + return wp.spatial_top(cdof_clip) + + @wp.func def jac_dof( # Model: body_parentid: wp.array[int], body_rootid: wp.array[int], dof_bodyid: wp.array[int], + body_isdofancestor: wp.array2d[int], # Data in: subtree_com_in: wp.array2d[wp.vec3], cdof_in: wp.array2d[wp.spatial_vector], @@ -408,16 +425,7 @@ def jac_dof( dofid: int, worldid: int, ) -> Tuple[wp.vec3, wp.vec3]: - dof_bodyid_ = dof_bodyid[dofid] - in_tree = int(dof_bodyid_ == 0) - parentid = bodyid - while parentid != 0: - if parentid == dof_bodyid_: - in_tree = 1 - break - parentid = body_parentid[parentid] - - if not in_tree: + if body_isdofancestor[bodyid, dofid] == 0: return wp.vec3(0.0), wp.vec3(0.0) offset = point - wp.vec3(subtree_com_in[worldid, body_rootid[bodyid]]) @@ -440,6 +448,7 @@ def _make_jac_kernel(has_jacp: bool, has_jacr: bool): body_parentid: wp.array[int], body_rootid: wp.array[int], dof_bodyid: wp.array[int], + body_isdofancestor: wp.array2d[int], # Data in: subtree_com_in: wp.array2d[wp.vec3], cdof_in: wp.array2d[wp.spatial_vector], @@ -453,7 +462,16 @@ def _make_jac_kernel(has_jacp: bool, has_jacr: bool): worldid, dofid = wp.tid() jacp_val, jacr_val = jac_dof( - body_parentid, body_rootid, dof_bodyid, subtree_com_in, cdof_in, point_in[worldid], bodyid_in[worldid], dofid, worldid + body_parentid, + body_rootid, + dof_bodyid, + body_isdofancestor, + subtree_com_in, + cdof_in, + point_in[worldid], + bodyid_in[worldid], + dofid, + worldid, ) if wp.static(has_jacp): @@ -496,7 +514,7 @@ def jac( wp.launch( kernel, dim=(d.nworld, m.nv), - inputs=[m.body_parentid, m.body_rootid, m.dof_bodyid, d.subtree_com, d.cdof, point, body], + inputs=[m.body_parentid, m.body_rootid, m.dof_bodyid, m.body_isdofancestor, d.subtree_com, d.cdof, point, body], outputs=[jacp_arr, jacr_arr], ) @@ -510,6 +528,7 @@ def jac_dot_dof( jnt_dofadr: wp.array[int], dof_bodyid: wp.array[int], dof_jntid: wp.array[int], + body_isdofancestor: wp.array2d[int], # Data in: subtree_com_in: wp.array2d[wp.vec3], cdof_in: wp.array2d[wp.spatial_vector], @@ -521,16 +540,7 @@ def jac_dot_dof( dofid: int, worldid: int, ) -> Tuple[wp.vec3, wp.vec3]: - dof_bodyid_ = dof_bodyid[dofid] - in_tree = int(dof_bodyid_ == 0) - parentid = bodyid - while parentid != 0: - if parentid == dof_bodyid_: - in_tree = 1 - break - parentid = body_parentid[parentid] - - if not in_tree: + if body_isdofancestor[bodyid, dofid] == 0: return wp.vec3(0.0), wp.vec3(0.0) com = subtree_com_in[worldid, body_rootid[bodyid]] diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py index bf4f1b6b..2395a711 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py @@ -67,6 +67,7 @@ class BlockDim: update_gradient_JTDAJ_sparse: int = 64 update_gradient_JTDAJ_dense: int = 96 linesearch_iterative: int = 32 + contact_jac_tiled: int = 32 # derivative qderiv_actuator_dense: int = 32 @@ -184,7 +185,6 @@ class DisableBit(enum.IntFlag): EULERDAMP: implicit damping for Euler integration NATIVECCD: native convex collision detection (ignored in MJWarp) ISLAND: constraint islands - MULTICCD: multiple CCD contact points """ CONSTRAINT = mujoco.mjtDisableBit.mjDSBL_CONSTRAINT @@ -214,6 +214,7 @@ class EnableBit(enum.IntFlag): Attributes: ENERGY: energy computation INVDISCRETE: discrete-time inverse dynamics + MULTICCD: multiple contacts with CCD """ ENERGY = mujoco.mjtEnableBit.mjENBL_ENERGY @@ -251,6 +252,7 @@ class DynType(enum.IntEnum): FILTEREXACT: linear filter: da/dt = (u-a) / tau, with exact integration MUSCLE: piece-wise linear filter with two time constants USER: user-defined dynamics via act_dyn_callback + DCMOTOR: DC motor dynamics """ NONE = mujoco.mjtDyn.mjDYN_NONE @@ -259,6 +261,7 @@ class DynType(enum.IntEnum): FILTEREXACT = mujoco.mjtDyn.mjDYN_FILTEREXACT MUSCLE = mujoco.mjtDyn.mjDYN_MUSCLE USER = mujoco.mjtDyn.mjDYN_USER + DCMOTOR = mujoco.mjtDyn.mjDYN_DCMOTOR class GainType(enum.IntEnum): @@ -269,12 +272,14 @@ class GainType(enum.IntEnum): AFFINE: const + kp*length + kv*velocity MUSCLE: muscle FLV curve computed by muscle_gain USER: user-defined gain via act_gain_callback + DCMOTOR: DC motor gain """ FIXED = mujoco.mjtGain.mjGAIN_FIXED AFFINE = mujoco.mjtGain.mjGAIN_AFFINE MUSCLE = mujoco.mjtGain.mjGAIN_MUSCLE USER = mujoco.mjtGain.mjGAIN_USER + DCMOTOR = mujoco.mjtGain.mjGAIN_DCMOTOR class BiasType(enum.IntEnum): @@ -285,12 +290,14 @@ class BiasType(enum.IntEnum): AFFINE: const + kp*length + kv*velocity MUSCLE: muscle passive force computed by muscle_bias USER: user-defined bias via act_bias_callback + DCMOTOR: DC motor back-EMF bias """ NONE = mujoco.mjtBias.mjBIAS_NONE AFFINE = mujoco.mjtBias.mjBIAS_AFFINE MUSCLE = mujoco.mjtBias.mjBIAS_MUSCLE USER = mujoco.mjtBias.mjBIAS_USER + DCMOTOR = mujoco.mjtBias.mjBIAS_DCMOTOR class JointType(enum.IntEnum): @@ -546,6 +553,7 @@ class ObjType(enum.IntEnum): BODY: body XBODY: body, used to access regular frame instead of i-frame GEOM: geom + FLEX: flex SITE: site CAMERA: camera """ @@ -554,6 +562,7 @@ class ObjType(enum.IntEnum): BODY = mujoco.mjtObj.mjOBJ_BODY XBODY = mujoco.mjtObj.mjOBJ_XBODY GEOM = mujoco.mjtObj.mjOBJ_GEOM + FLEX = mujoco.mjtObj.mjOBJ_FLEX SITE = mujoco.mjtObj.mjOBJ_SITE CAMERA = mujoco.mjtObj.mjOBJ_CAMERA @@ -646,6 +655,10 @@ class vec6f(wp.types.vector(length=6, dtype=float)): pass +class vec6i(wp.types.vector(length=6, dtype=int)): + pass + + class vec8f(wp.types.vector(length=8, dtype=float)): pass @@ -921,6 +934,7 @@ class Model: jnt_pos: local anchor position (*, njnt, 3) jnt_axis: local joint axis (*, njnt, 3) jnt_stiffness: stiffness coefficient (*, njnt) + jnt_stiffnesspoly: high-order stiffness coefficients (*, njnt, 2) jnt_range: joint limits (*, njnt, 2) jnt_actfrcrange: range of total actuator force (*, njnt, 2) jnt_margin: min distance for limit detection (*, njnt) @@ -934,6 +948,7 @@ class Model: dof_frictionloss: dof friction loss (*, nv) dof_armature: dof armature inertia/mass (*, nv) dof_damping: damping coefficient (*, nv) + dof_dampingpoly: high-order damping coefficients (*, nv, 2) dof_invweight0: diag. inverse inertia in qpos0 (*, nv) tree_bodynum: number of bodies in tree (incl. root) (ntree,) tree_dofadr: start address of tree's dofs (ntree,) @@ -992,8 +1007,13 @@ class Model: flex_contype: flex contact type (nflex,) flex_conaffinity: flex contact affinity (nflex,) flex_condim: contact dimensionality (1, 3, 4, 6) (nflex,) + flex_priority: geom contact priority (nflex,) + flex_solmix: mixing coef for solref/imp in geom pair (nflex,) + flex_solref: constraint solver reference: contact (nflex, mjNREF) + flex_solimp: constraint solver impedance: contact (nflex, mjNIMP) flex_friction: friction for (slide, spin, roll) (nflex, 3) flex_margin: detect contact if dist float: return dctrl / wp.max(MJ_MINVAL, tau) +@wp.func +def dcmotor_slots(dynprm: types.vec10, gainprm: types.vec10) -> types.vec6i: + """Compute activation slot layout for a DC motor actuator. + + Each DC motor can have up to 5 optional activation states. This function + determines which states are enabled (based on nonzero parameters) and + assigns each a contiguous slot offset in the activation array. + + Returns a vec6i where: + s[0]: slew rate — enabled when dynprm[7] > 0 (slew rate limit) + s[1]: integral — enabled when gainprm[5] > 0 (integral gain ki) + s[2]: temperature — enabled when dynprm[2] > 0 (thermal resistance RT) + s[3]: bristle — enabled when dynprm[5] > 0 (LuGre stiffness sigma0) + s[4]: current — enabled when dynprm[0] > 0 (electrical time const te) + s[5]: total number of active slots (num_slots) + + Enabled slots hold a contiguous offset (0, 1, 2, ...); disabled slots + are set to -1. + """ + s = types.vec6i(-1, -1, -1, -1, -1, 0) + num_slots = 0 + if dynprm[7] > 0.0: + s[0] = num_slots + num_slots += 1 + if gainprm[5] > 0.0: + s[1] = num_slots + num_slots += 1 + if dynprm[2] > 0.0: + s[2] = num_slots + num_slots += 1 + if dynprm[5] > 0.0: + s[3] = num_slots + num_slots += 1 + if dynprm[0] > 0.0: + s[4] = num_slots + num_slots += 1 + s[5] = num_slots + return s + + +@wp.func +def lugre_stribeck(velocity: float, F_C: float, F_S: float, v_S: float) -> float: + ratio = velocity / wp.max(MJ_MINVAL, v_S) + return F_C + (F_S - F_C) * wp.exp(-ratio * ratio) + + +@wp.func +def dcmotor_voltage(u: float, length: float, velocity: float, x_I: float, gainprm: types.vec10) -> float: + input_mode = int(gainprm[8]) + Vmax = gainprm[7] + voltage = 0.0 + + if input_mode > 0: + kp = gainprm[4] + ki = gainprm[5] + kd = gainprm[6] + + if input_mode == 1: + # position mode + voltage = kp * (u - length) + ki * x_I - kd * velocity + else: + # velocity mode + voltage = kp * (u - velocity) + ki * (x_I - length) + else: + voltage = u + + if Vmax > 0.0: + voltage = wp.clamp(voltage, -Vmax, Vmax) + + return voltage + + @wp.func def inside_geom(pos: wp.vec3, mat: wp.mat33, size: wp.vec3, geomtype: int, point: wp.vec3) -> bool: """Return True if point is inside primitive geom, False otherwise.""" @@ -630,3 +703,34 @@ def inside_geom(pos: wp.vec3, mat: wp.mat33, size: wp.vec3, geomtype: int, point return plocal[2] < 0.0 return False + + +@wp.func +def _poly_force(linear: float, poly: wp.vec2, x: float, flg_odd: int) -> float: + x_val = wp.where(flg_odd == 1, wp.abs(x), x) + res = linear + res += poly[0] * x_val + res += poly[1] * x_val * x_val + return res + + +@wp.func +def _poly_force_deriv(linear: float, poly: wp.vec2, x: float, flg_odd: int) -> float: + x_val = wp.where(flg_odd == 1, wp.abs(x), x) + res = linear + res += 2.0 * poly[0] * x_val + res += 3.0 * poly[1] * x_val * x_val + return res + + +@wp.func +def poly_potential(linear: float, poly: wp.vec2, x: float, flg_odd: int) -> float: + x_val = wp.where(flg_odd == 1, wp.abs(x), x) + x_val2 = x_val * x_val + x_val3 = x_val2 * x_val + x_val4 = x_val3 * x_val + + res = 0.5 * linear * x_val2 + res += poly[0] * wp.static(1.0 / 3.0) * x_val3 + res += poly[1] * 0.25 * x_val4 + return res diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/pyproject.toml b/mjx/mujoco/mjx/third_party/mujoco_warp/pyproject.toml index f1c4bc4a..a5caa4b4 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/pyproject.toml +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name="mujoco-warp" -version = "3.6.0" +version = "3.8.0.1" # TODO(team): create a distribution list authors = [ {name = "Newton Developers", email = "mujoco@deepmind.com"}, @@ -28,7 +28,7 @@ requires-python = ">=3.10" dependencies = [ "absl-py", "etils[epath]", - "mujoco>=3.6.0", + "mujoco>=3.8.0", "numpy", "warp-lang>=1.12", ] @@ -55,9 +55,10 @@ dev = [ "ruff", "pygls>=1.0.0,<2.0.0", "lsprotocol>=2023.0.1,<2024.0.0", - "mujoco>=3.6.0.dev0", + "mujoco>=3.8.0.dev0", "warp-lang>=1.11.0.dev0", "mjviser>=0.0.10", + "pillow", ] # TODO(team): cpu and cuda JAX optional dependencies are temporary, remove after we land MJX:Warp cpu = [ @@ -70,6 +71,8 @@ cuda = [ [project.scripts] mjwarp-testspeed = "mujoco_warp.testspeed:main" mjwarp-viewer = "mujoco_warp.viewer:main" +mjwarp-record = "mujoco_warp.record:main" + [project.urls] Homepage = "https://github.com/google-deepmind/mujoco_warp" diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/viewer.py b/mjx/mujoco/mjx/third_party/mujoco_warp/viewer.py index 8a825da2..bac30dd3 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/viewer.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/viewer.py @@ -24,7 +24,6 @@ Example: import copy import enum import logging -import shutil import sys import time from typing import Sequence @@ -39,11 +38,6 @@ from etils import epath import mujoco.mjx.third_party.mujoco_warp as mjw -# mjwarp-viewer has priviledged access to a few internal methods -from mujoco.mjx.third_party.mujoco_warp._src.io import find_keys -from mujoco.mjx.third_party.mujoco_warp._src.io import make_trajectory -from mujoco.mjx.third_party.mujoco_warp._src.io import override_model - class EngineOptions(enum.IntEnum): """Engine option.""" @@ -52,16 +46,9 @@ class EngineOptions(enum.IntEnum): C = 1 -_CLEAR_WARP_CACHE = flags.DEFINE_bool("clear_warp_cache", False, "Clear warp caches (kernel, LTO, CUDA compute)") _ENGINE = flags.DEFINE_enum_class("engine", EngineOptions.WARP, EngineOptions, "Simulation engine") -_NCONMAX = flags.DEFINE_integer("nconmax", None, "Maximum number of contacts.") -_NJMAX = flags.DEFINE_integer("njmax", None, "Maximum number of constraints per world.") -_NJMAX_NNZ = flags.DEFINE_integer("njmax_nnz", None, "Maximum number of non-zeros in constraint Jacobian.") -_NCCDMAX = flags.DEFINE_integer("nccdmax", None, "Maximum number of CCD contacts per world.") -_OVERRIDE = flags.DEFINE_multi_string("override", [], "Model overrides (notation: foo.bar = baz)", short_name="o") -_KEYFRAME = flags.DEFINE_integer("keyframe", 0, "keyframe to initialize simulation.") -_DEVICE = flags.DEFINE_string("device", None, "override the default Warp device") -_REPLAY = flags.DEFINE_string("replay", None, "keyframe sequence to replay, keyframe name must prefix match") +from mujoco.mjx.third_party.mujoco_warp._src import cli + _VIEWER = flags.DEFINE_enum("viewer", "mujoco", ["mujoco", "viser"], "Viewer backend (mujoco native or mjviser web)") _VIEWER_GLOBAL_STATE = {"running": True, "step_once": False} @@ -75,26 +62,6 @@ def key_callback(key: int) -> None: _VIEWER_GLOBAL_STATE["step_once"] = True -def _load_model(path: epath.Path) -> mujoco.MjModel: - if not path.exists(): - resource_path = epath.resource_path("mjx") / "third_party/mujoco_warp" / path - if not resource_path.exists(): - raise FileNotFoundError(f"file not found: {path}\nalso tried: {resource_path}") - path = resource_path - - print(f"Loading model from: {path}...") - if path.suffix == ".mjb": - return mujoco.MjModel.from_binary_path(path.as_posix()) - - spec = mujoco.MjSpec.from_file(path.as_posix()) - # check if the file has any mujoco.sdf test plugins - if any(p.plugin_name.startswith("mujoco.sdf") for p in spec.plugins): - from mujoco.mjx.third_party.mujoco_warp.test_data.collision_sdf.utils import register_sdf_plugins as register_sdf_plugins - - register_sdf_plugins(mjw) - return spec.compile() - - def _compile_step(m, d): print("Compiling physics step...", end="", flush=True) start = time.time() @@ -177,20 +144,13 @@ def _main(argv: Sequence[str]) -> None: elif len(argv) > 2: raise app.UsageError("Too many command-line arguments.") - mjm = _load_model(epath.Path(argv[1])) - mjd = mujoco.MjData(mjm) - ctrls = None - if _REPLAY.value: - keys = find_keys(mjm, _REPLAY.value) - if not keys: - raise app.UsageError(f"Key prefix not find: {_REPLAY.value}") - ctrls = make_trajectory(mjm, keys) - mujoco.mj_resetDataKeyframe(mjm, mjd, keys[0]) - elif mjm.nkey > 0 and _KEYFRAME.value > -1: - mujoco.mj_resetDataKeyframe(mjm, mjd, _KEYFRAME.value) + wp.config.quiet = flags.FLAGS["verbosity"].value < 1 + wp.init() + + mjm = cli.load_model(epath.Path(argv[1])) + m, d, rc, ctrls = cli.init_structs(mjw.step, mjm) if _ENGINE.value == EngineOptions.C: - override_model(mjm, _OVERRIDE.value) print( f" nbody: {mjm.nbody} nv: {mjm.nv} ngeom: {mjm.ngeom} nu: {mjm.nu}\n" f" solver: {mujoco.mjtSolver(mjm.opt.solver).name} cone: {mujoco.mjtCone(mjm.opt.cone).name}" @@ -199,22 +159,8 @@ def _main(argv: Sequence[str]) -> None: ) print(f"MuJoCo C simulating with dt = {mjm.opt.timestep:.3f}...") else: - wp.config.quiet = flags.FLAGS["verbosity"].value < 1 - wp.init() - wp.set_device(_DEVICE.value) - if _CLEAR_WARP_CACHE.value: - wp.clear_kernel_cache() - wp.clear_lto_cache() - # Clear CUDA compute cache for truly cold start JIT - compute_cache = epath.Path("~/.nv/ComputeCache").expanduser() - if compute_cache.exists(): - shutil.rmtree(compute_cache) - compute_cache.mkdir() + wp.set_device(cli.DEVICE.value) - override_model(mjm, _OVERRIDE.value) - m = mjw.put_model(mjm) - override_model(m, _OVERRIDE.value) - d = mjw.put_data(mjm, mjd, nconmax=_NCONMAX.value, njmax=_NJMAX.value, njmax_nnz=_NJMAX_NNZ.value, nccdmax=_NCCDMAX.value) graph = _compile_step(m, d) if wp.get_device().is_cuda else None if graph is None: mjw.step(m, d) # warmup step @@ -238,6 +184,8 @@ def _main(argv: Sequence[str]) -> None: else: step_fn = _make_c_step_fn(ctrls) + mjd = mujoco.MjData(mjm) + mjw.get_data_into(mjd, mjm, d) if _VIEWER.value == "viser": _run_viser_viewer(mjm, mjd, step_fn) else: @@ -249,6 +197,8 @@ def main(): # pyproject bin scripts break this assumption, so manually set argv and docstring sys.argv[0] = "mujoco_warp.viewer" sys.modules["__main__"].__doc__ = __doc__ + # default to single world with no noise + flags.FLAGS.set_default("nworld", 1) app.run(_main) diff --git a/mjx/mujoco/mjx/warp/bvh.py b/mjx/mujoco/mjx/warp/bvh.py index edde9732..d80b12fd 100644 --- a/mjx/mujoco/mjx/warp/bvh.py +++ b/mjx/mujoco/mjx/warp/bvh.py @@ -48,7 +48,6 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) - @ffi.format_args_for_warp def _refit_bvh_shim( # Model diff --git a/mjx/mujoco/mjx/warp/collision_driver.py b/mjx/mujoco/mjx/warp/collision_driver.py index 530de314..dbecb9c1 100644 --- a/mjx/mujoco/mjx/warp/collision_driver.py +++ b/mjx/mujoco/mjx/warp/collision_driver.py @@ -61,11 +61,16 @@ def _collision_shim( flex_elemdataadr: wp.array[int], flex_elemnum: wp.array[int], flex_friction: wp.array[wp.vec3], + flex_gap: wp.array[float], flex_margin: wp.array[float], + flex_priority: wp.array[int], flex_radius: wp.array[float], flex_shell: wp.array[int], flex_shelldataadr: wp.array[int], flex_shellnum: wp.array[int], + flex_solimp: wp.array[mjwp_types.vec5], + flex_solmix: wp.array[float], + flex_solref: wp.array[wp.vec2], flex_vertadr: wp.array[int], flex_vertflexid: wp.array[int], geom_aabb: wp.array3d[wp.vec3], @@ -136,7 +141,6 @@ def _collision_shim( opt__ccd_iterations: int, opt__ccd_tolerance: wp.array[float], opt__disableflags: int, - opt__enableflags: int, opt__sdf_initpoints: int, opt__sdf_iterations: int, # Data @@ -179,11 +183,16 @@ def _collision_shim( _m.flex_elemdataadr = flex_elemdataadr _m.flex_elemnum = flex_elemnum _m.flex_friction = flex_friction + _m.flex_gap = flex_gap _m.flex_margin = flex_margin + _m.flex_priority = flex_priority _m.flex_radius = flex_radius _m.flex_shell = flex_shell _m.flex_shelldataadr = flex_shelldataadr _m.flex_shellnum = flex_shellnum + _m.flex_solimp = flex_solimp + _m.flex_solmix = flex_solmix + _m.flex_solref = flex_solref _m.flex_vertadr = flex_vertadr _m.flex_vertflexid = flex_vertflexid _m.geom_aabb = geom_aabb @@ -245,7 +254,6 @@ def _collision_shim( _m.opt.ccd_iterations = opt__ccd_iterations _m.opt.ccd_tolerance = opt__ccd_tolerance _m.opt.disableflags = opt__disableflags - _m.opt.enableflags = opt__enableflags _m.opt.sdf_initpoints = opt__sdf_initpoints _m.opt.sdf_iterations = opt__sdf_iterations _m.pair_dim = pair_dim @@ -366,11 +374,16 @@ def _collision_jax_impl(m: types.Model, d: types.Data): m._impl.flex_elemdataadr, m._impl.flex_elemnum, m._impl.flex_friction, + m._impl.flex_gap, m._impl.flex_margin, + m._impl.flex_priority, m._impl.flex_radius, m._impl.flex_shell, m._impl.flex_shelldataadr, m._impl.flex_shellnum, + m._impl.flex_solimp, + m._impl.flex_solmix, + m._impl.flex_solref, m.flex_vertadr, m._impl.flex_vertflexid, m.geom_aabb, @@ -441,7 +454,6 @@ def _collision_jax_impl(m: types.Model, d: types.Data): m.opt._impl.ccd_iterations, m.opt._impl.ccd_tolerance, m.opt.disableflags, - m.opt.enableflags, m.opt._impl.sdf_initpoints, m.opt._impl.sdf_iterations, d._impl.naccdmax, diff --git a/mjx/mujoco/mjx/warp/forward.py b/mjx/mujoco/mjx/warp/forward.py index 98459fbb..2f46f1f3 100644 --- a/mjx/mujoco/mjx/warp/forward.py +++ b/mjx/mujoco/mjx/warp/forward.py @@ -88,6 +88,7 @@ def _forward_shim( body_invweight0: wp.array2d[wp.vec2], body_ipos: wp.array2d[wp.vec3], body_iquat: wp.array2d[wp.quat], + body_isdofancestor: wp.array2d[int], body_jntadr: wp.array[int], body_jntnum: wp.array[int], body_mass: wp.array2d[float], @@ -116,6 +117,7 @@ def _forward_shim( dof_armature: wp.array2d[float], dof_bodyid: wp.array[int], dof_damping: wp.array2d[float], + dof_dampingpoly: wp.array2d[wp.vec2], dof_frictionloss: wp.array2d[float], dof_invweight0: wp.array2d[float], dof_jntid: wp.array[int], @@ -155,11 +157,16 @@ def _forward_shim( flex_elemedgeadr: wp.array[int], flex_elemnum: wp.array[int], flex_friction: wp.array[wp.vec3], + flex_gap: wp.array[float], flex_margin: wp.array[float], + flex_priority: wp.array[int], flex_radius: wp.array[float], flex_shell: wp.array[int], flex_shelldataadr: wp.array[int], flex_shellnum: wp.array[int], + flex_solimp: wp.array[mjwp_types.vec5], + flex_solmix: wp.array[float], + flex_solref: wp.array[wp.vec2], flex_stiffness: wp.array2d[float], flex_vert: wp.array[wp.vec3], flex_vertadr: wp.array[int], @@ -218,6 +225,7 @@ def _forward_shim( jnt_solimp: wp.array2d[mjwp_types.vec5], jnt_solref: wp.array2d[wp.vec2], jnt_stiffness: wp.array2d[float], + jnt_stiffnesspoly: wp.array2d[wp.vec2], jnt_type: wp.array[int], light_bodyid: wp.array[int], light_dir: wp.array2d[wp.vec3], @@ -352,6 +360,7 @@ def _forward_shim( tendon_adr: wp.array[int], tendon_armature: wp.array2d[float], tendon_damping: wp.array2d[float], + tendon_dampingpoly: wp.array2d[wp.vec2], tendon_frictionloss: wp.array2d[float], tendon_geom_adr: wp.array[int], tendon_invweight0: wp.array2d[float], @@ -368,6 +377,7 @@ def _forward_shim( tendon_solref_fri: wp.array2d[wp.vec2], tendon_solref_lim: wp.array2d[wp.vec2], tendon_stiffness: wp.array2d[float], + tendon_stiffnesspoly: wp.array2d[wp.vec2], wrap_geom_adr: wp.array[int], wrap_jnt_adr: wp.array[int], wrap_objid: wp.array[int], @@ -509,6 +519,7 @@ def _forward_shim( efc__J_colind: wp.array3d[int], efc__J_rowadr: wp.array2d[int], efc__J_rownnz: wp.array2d[int], + efc__Jqvel: wp.array2d[float], efc__Ma: wp.array2d[float], efc__aref: wp.array2d[float], efc__force: wp.array2d[float], @@ -562,6 +573,7 @@ def _forward_shim( _m.body_invweight0 = body_invweight0 _m.body_ipos = body_ipos _m.body_iquat = body_iquat + _m.body_isdofancestor = body_isdofancestor _m.body_jntadr = body_jntadr _m.body_jntnum = body_jntnum _m.body_mass = body_mass @@ -590,6 +602,7 @@ def _forward_shim( _m.dof_armature = dof_armature _m.dof_bodyid = dof_bodyid _m.dof_damping = dof_damping + _m.dof_dampingpoly = dof_dampingpoly _m.dof_frictionloss = dof_frictionloss _m.dof_invweight0 = dof_invweight0 _m.dof_jntid = dof_jntid @@ -629,11 +642,16 @@ def _forward_shim( _m.flex_elemedgeadr = flex_elemedgeadr _m.flex_elemnum = flex_elemnum _m.flex_friction = flex_friction + _m.flex_gap = flex_gap _m.flex_margin = flex_margin + _m.flex_priority = flex_priority _m.flex_radius = flex_radius _m.flex_shell = flex_shell _m.flex_shelldataadr = flex_shelldataadr _m.flex_shellnum = flex_shellnum + _m.flex_solimp = flex_solimp + _m.flex_solmix = flex_solmix + _m.flex_solref = flex_solref _m.flex_stiffness = flex_stiffness _m.flex_vert = flex_vert _m.flex_vertadr = flex_vertadr @@ -692,6 +710,7 @@ def _forward_shim( _m.jnt_solimp = jnt_solimp _m.jnt_solref = jnt_solref _m.jnt_stiffness = jnt_stiffness + _m.jnt_stiffnesspoly = jnt_stiffnesspoly _m.jnt_type = jnt_type _m.light_bodyid = light_bodyid _m.light_dir = light_dir @@ -853,6 +872,7 @@ def _forward_shim( _m.tendon_adr = tendon_adr _m.tendon_armature = tendon_armature _m.tendon_damping = tendon_damping + _m.tendon_dampingpoly = tendon_dampingpoly _m.tendon_frictionloss = tendon_frictionloss _m.tendon_geom_adr = tendon_geom_adr _m.tendon_invweight0 = tendon_invweight0 @@ -869,6 +889,7 @@ def _forward_shim( _m.tendon_solref_fri = tendon_solref_fri _m.tendon_solref_lim = tendon_solref_lim _m.tendon_stiffness = tendon_stiffness + _m.tendon_stiffnesspoly = tendon_stiffnesspoly _m.wrap_geom_adr = wrap_geom_adr _m.wrap_jnt_adr = wrap_jnt_adr _m.wrap_objid = wrap_objid @@ -914,6 +935,7 @@ def _forward_shim( _d.efc.J_colind = efc__J_colind _d.efc.J_rowadr = efc__J_rowadr _d.efc.J_rownnz = efc__J_rownnz + _d.efc.Jqvel = efc__Jqvel _d.efc.Ma = efc__Ma _d.efc.aref = efc__aref _d.efc.force = efc__force @@ -1090,6 +1112,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): 'efc__J_colind': d._impl.efc__J_colind.shape, 'efc__J_rowadr': d._impl.efc__J_rowadr.shape, 'efc__J_rownnz': d._impl.efc__J_rownnz.shape, + 'efc__Jqvel': d._impl.efc__Jqvel.shape, 'efc__Ma': d._impl.efc__Ma.shape, 'efc__aref': d._impl.efc__aref.shape, 'efc__force': d._impl.efc__force.shape, @@ -1103,7 +1126,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): } jf = ffi.jax_callable_variadic_tuple( _forward_shim, - num_outputs=102, + num_outputs=103, output_dims=output_dims, vmap_method=None, in_out_argnames=set([ @@ -1199,6 +1222,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): 'efc__J_colind', 'efc__J_rowadr', 'efc__J_rownnz', + 'efc__Jqvel', 'efc__Ma', 'efc__aref', 'efc__force', @@ -1249,6 +1273,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): 'cvel', 'dof_armature', 'dof_damping', + 'dof_dampingpoly', 'dof_frictionloss', 'dof_invweight0', 'dof_solimp', @@ -1281,6 +1306,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): 'jnt_solimp', 'jnt_solref', 'jnt_stiffness', + 'jnt_stiffnesspoly', 'light_dir', 'light_dir0', 'light_pos', @@ -1328,6 +1354,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): 'tendon_actfrcrange', 'tendon_armature', 'tendon_damping', + 'tendon_dampingpoly', 'tendon_frictionloss', 'tendon_invweight0', 'tendon_length0', @@ -1339,6 +1366,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): 'tendon_solref_fri', 'tendon_solref_lim', 'tendon_stiffness', + 'tendon_stiffnesspoly', 'time', 'xanchor', 'xaxis', @@ -1425,6 +1453,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m.body_invweight0, m.body_ipos, m.body_iquat, + m._impl.body_isdofancestor, m.body_jntadr, m.body_jntnum, m.body_mass, @@ -1453,6 +1482,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m.dof_armature, m.dof_bodyid, m.dof_damping, + m.dof_dampingpoly, m.dof_frictionloss, m.dof_invweight0, m.dof_jntid, @@ -1492,11 +1522,16 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m._impl.flex_elemedgeadr, m._impl.flex_elemnum, m._impl.flex_friction, + m._impl.flex_gap, m._impl.flex_margin, + m._impl.flex_priority, m._impl.flex_radius, m._impl.flex_shell, m._impl.flex_shelldataadr, m._impl.flex_shellnum, + m._impl.flex_solimp, + m._impl.flex_solmix, + m._impl.flex_solref, m._impl.flex_stiffness, m._impl.flex_vert, m.flex_vertadr, @@ -1555,6 +1590,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m.jnt_solimp, m.jnt_solref, m.jnt_stiffness, + m.jnt_stiffnesspoly, m.jnt_type, m._impl.light_bodyid, m.light_dir, @@ -1689,6 +1725,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m.tendon_adr, m.tendon_armature, m.tendon_damping, + m.tendon_dampingpoly, m.tendon_frictionloss, m._impl.tendon_geom_adr, m.tendon_invweight0, @@ -1705,6 +1742,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m.tendon_solref_fri, m.tendon_solref_lim, m.tendon_stiffness, + m.tendon_stiffnesspoly, m._impl.wrap_geom_adr, m._impl.wrap_jnt_adr, m.wrap_objid, @@ -1845,6 +1883,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): d._impl.efc__J_colind, d._impl.efc__J_rowadr, d._impl.efc__J_rownnz, + d._impl.efc__Jqvel, d._impl.efc__Ma, d._impl.efc__aref, d._impl.efc__force, @@ -1949,16 +1988,17 @@ def _forward_jax_impl(m: types.Model, d: types.Data): '_impl.efc__J_colind': out[89], '_impl.efc__J_rowadr': out[90], '_impl.efc__J_rownnz': out[91], - '_impl.efc__Ma': out[92], - '_impl.efc__aref': out[93], - '_impl.efc__force': out[94], - '_impl.efc__frictionloss': out[95], - '_impl.efc__id': out[96], - '_impl.efc__margin': out[97], - '_impl.efc__pos': out[98], - '_impl.efc__state': out[99], - '_impl.efc__type': out[100], - '_impl.efc__vel': out[101], + '_impl.efc__Jqvel': out[92], + '_impl.efc__Ma': out[93], + '_impl.efc__aref': out[94], + '_impl.efc__force': out[95], + '_impl.efc__frictionloss': out[96], + '_impl.efc__id': out[97], + '_impl.efc__margin': out[98], + '_impl.efc__pos': out[99], + '_impl.efc__state': out[100], + '_impl.efc__type': out[101], + '_impl.efc__vel': out[102], }) return d @@ -2017,6 +2057,7 @@ def _step_shim( body_invweight0: wp.array2d[wp.vec2], body_ipos: wp.array2d[wp.vec3], body_iquat: wp.array2d[wp.quat], + body_isdofancestor: wp.array2d[int], body_jntadr: wp.array[int], body_jntnum: wp.array[int], body_mass: wp.array2d[float], @@ -2045,6 +2086,7 @@ def _step_shim( dof_armature: wp.array2d[float], dof_bodyid: wp.array[int], dof_damping: wp.array2d[float], + dof_dampingpoly: wp.array2d[wp.vec2], dof_frictionloss: wp.array2d[float], dof_invweight0: wp.array2d[float], dof_jntid: wp.array[int], @@ -2084,11 +2126,16 @@ def _step_shim( flex_elemedgeadr: wp.array[int], flex_elemnum: wp.array[int], flex_friction: wp.array[wp.vec3], + flex_gap: wp.array[float], flex_margin: wp.array[float], + flex_priority: wp.array[int], flex_radius: wp.array[float], flex_shell: wp.array[int], flex_shelldataadr: wp.array[int], flex_shellnum: wp.array[int], + flex_solimp: wp.array[mjwp_types.vec5], + flex_solmix: wp.array[float], + flex_solref: wp.array[wp.vec2], flex_stiffness: wp.array2d[float], flex_vert: wp.array[wp.vec3], flex_vertadr: wp.array[int], @@ -2147,6 +2194,7 @@ def _step_shim( jnt_solimp: wp.array2d[mjwp_types.vec5], jnt_solref: wp.array2d[wp.vec2], jnt_stiffness: wp.array2d[float], + jnt_stiffnesspoly: wp.array2d[wp.vec2], jnt_type: wp.array[int], light_bodyid: wp.array[int], light_dir: wp.array2d[wp.vec3], @@ -2282,6 +2330,7 @@ def _step_shim( tendon_adr: wp.array[int], tendon_armature: wp.array2d[float], tendon_damping: wp.array2d[float], + tendon_dampingpoly: wp.array2d[wp.vec2], tendon_frictionloss: wp.array2d[float], tendon_geom_adr: wp.array[int], tendon_invweight0: wp.array2d[float], @@ -2298,6 +2347,7 @@ def _step_shim( tendon_solref_fri: wp.array2d[wp.vec2], tendon_solref_lim: wp.array2d[wp.vec2], tendon_stiffness: wp.array2d[float], + tendon_stiffnesspoly: wp.array2d[wp.vec2], wrap_geom_adr: wp.array[int], wrap_jnt_adr: wp.array[int], wrap_objid: wp.array[int], @@ -2440,6 +2490,7 @@ def _step_shim( efc__J_colind: wp.array3d[int], efc__J_rowadr: wp.array2d[int], efc__J_rownnz: wp.array2d[int], + efc__Jqvel: wp.array2d[float], efc__Ma: wp.array2d[float], efc__aref: wp.array2d[float], efc__force: wp.array2d[float], @@ -2493,6 +2544,7 @@ def _step_shim( _m.body_invweight0 = body_invweight0 _m.body_ipos = body_ipos _m.body_iquat = body_iquat + _m.body_isdofancestor = body_isdofancestor _m.body_jntadr = body_jntadr _m.body_jntnum = body_jntnum _m.body_mass = body_mass @@ -2521,6 +2573,7 @@ def _step_shim( _m.dof_armature = dof_armature _m.dof_bodyid = dof_bodyid _m.dof_damping = dof_damping + _m.dof_dampingpoly = dof_dampingpoly _m.dof_frictionloss = dof_frictionloss _m.dof_invweight0 = dof_invweight0 _m.dof_jntid = dof_jntid @@ -2560,11 +2613,16 @@ def _step_shim( _m.flex_elemedgeadr = flex_elemedgeadr _m.flex_elemnum = flex_elemnum _m.flex_friction = flex_friction + _m.flex_gap = flex_gap _m.flex_margin = flex_margin + _m.flex_priority = flex_priority _m.flex_radius = flex_radius _m.flex_shell = flex_shell _m.flex_shelldataadr = flex_shelldataadr _m.flex_shellnum = flex_shellnum + _m.flex_solimp = flex_solimp + _m.flex_solmix = flex_solmix + _m.flex_solref = flex_solref _m.flex_stiffness = flex_stiffness _m.flex_vert = flex_vert _m.flex_vertadr = flex_vertadr @@ -2623,6 +2681,7 @@ def _step_shim( _m.jnt_solimp = jnt_solimp _m.jnt_solref = jnt_solref _m.jnt_stiffness = jnt_stiffness + _m.jnt_stiffnesspoly = jnt_stiffnesspoly _m.jnt_type = jnt_type _m.light_bodyid = light_bodyid _m.light_dir = light_dir @@ -2786,6 +2845,7 @@ def _step_shim( _m.tendon_adr = tendon_adr _m.tendon_armature = tendon_armature _m.tendon_damping = tendon_damping + _m.tendon_dampingpoly = tendon_dampingpoly _m.tendon_frictionloss = tendon_frictionloss _m.tendon_geom_adr = tendon_geom_adr _m.tendon_invweight0 = tendon_invweight0 @@ -2802,6 +2862,7 @@ def _step_shim( _m.tendon_solref_fri = tendon_solref_fri _m.tendon_solref_lim = tendon_solref_lim _m.tendon_stiffness = tendon_stiffness + _m.tendon_stiffnesspoly = tendon_stiffnesspoly _m.wrap_geom_adr = wrap_geom_adr _m.wrap_jnt_adr = wrap_jnt_adr _m.wrap_objid = wrap_objid @@ -2847,6 +2908,7 @@ def _step_shim( _d.efc.J_colind = efc__J_colind _d.efc.J_rowadr = efc__J_rowadr _d.efc.J_rownnz = efc__J_rownnz + _d.efc.Jqvel = efc__Jqvel _d.efc.Ma = efc__Ma _d.efc.aref = efc__aref _d.efc.force = efc__force @@ -3027,6 +3089,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): 'efc__J_colind': d._impl.efc__J_colind.shape, 'efc__J_rowadr': d._impl.efc__J_rowadr.shape, 'efc__J_rownnz': d._impl.efc__J_rownnz.shape, + 'efc__Jqvel': d._impl.efc__Jqvel.shape, 'efc__Ma': d._impl.efc__Ma.shape, 'efc__aref': d._impl.efc__aref.shape, 'efc__force': d._impl.efc__force.shape, @@ -3040,7 +3103,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): } jf = ffi.jax_callable_variadic_tuple( _step_shim, - num_outputs=106, + num_outputs=107, output_dims=output_dims, vmap_method=None, in_out_argnames=set([ @@ -3140,6 +3203,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): 'efc__J_colind', 'efc__J_rowadr', 'efc__J_rownnz', + 'efc__Jqvel', 'efc__Ma', 'efc__aref', 'efc__force', @@ -3190,6 +3254,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): 'cvel', 'dof_armature', 'dof_damping', + 'dof_dampingpoly', 'dof_frictionloss', 'dof_invweight0', 'dof_solimp', @@ -3222,6 +3287,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): 'jnt_solimp', 'jnt_solref', 'jnt_stiffness', + 'jnt_stiffnesspoly', 'light_dir', 'light_dir0', 'light_pos', @@ -3269,6 +3335,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): 'tendon_actfrcrange', 'tendon_armature', 'tendon_damping', + 'tendon_dampingpoly', 'tendon_frictionloss', 'tendon_invweight0', 'tendon_length0', @@ -3280,6 +3347,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): 'tendon_solref_fri', 'tendon_solref_lim', 'tendon_stiffness', + 'tendon_stiffnesspoly', 'time', 'xanchor', 'xaxis', @@ -3370,6 +3438,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): m.body_invweight0, m.body_ipos, m.body_iquat, + m._impl.body_isdofancestor, m.body_jntadr, m.body_jntnum, m.body_mass, @@ -3398,6 +3467,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): m.dof_armature, m.dof_bodyid, m.dof_damping, + m.dof_dampingpoly, m.dof_frictionloss, m.dof_invweight0, m.dof_jntid, @@ -3437,11 +3507,16 @@ def _step_jax_impl(m: types.Model, d: types.Data): m._impl.flex_elemedgeadr, m._impl.flex_elemnum, m._impl.flex_friction, + m._impl.flex_gap, m._impl.flex_margin, + m._impl.flex_priority, m._impl.flex_radius, m._impl.flex_shell, m._impl.flex_shelldataadr, m._impl.flex_shellnum, + m._impl.flex_solimp, + m._impl.flex_solmix, + m._impl.flex_solref, m._impl.flex_stiffness, m._impl.flex_vert, m.flex_vertadr, @@ -3500,6 +3575,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): m.jnt_solimp, m.jnt_solref, m.jnt_stiffness, + m.jnt_stiffnesspoly, m.jnt_type, m._impl.light_bodyid, m.light_dir, @@ -3635,6 +3711,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): m.tendon_adr, m.tendon_armature, m.tendon_damping, + m.tendon_dampingpoly, m.tendon_frictionloss, m._impl.tendon_geom_adr, m.tendon_invweight0, @@ -3651,6 +3728,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): m.tendon_solref_fri, m.tendon_solref_lim, m.tendon_stiffness, + m.tendon_stiffnesspoly, m._impl.wrap_geom_adr, m._impl.wrap_jnt_adr, m.wrap_objid, @@ -3792,6 +3870,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): d._impl.efc__J_colind, d._impl.efc__J_rowadr, d._impl.efc__J_rownnz, + d._impl.efc__Jqvel, d._impl.efc__Ma, d._impl.efc__aref, d._impl.efc__force, @@ -3900,16 +3979,17 @@ def _step_jax_impl(m: types.Model, d: types.Data): '_impl.efc__J_colind': out[93], '_impl.efc__J_rowadr': out[94], '_impl.efc__J_rownnz': out[95], - '_impl.efc__Ma': out[96], - '_impl.efc__aref': out[97], - '_impl.efc__force': out[98], - '_impl.efc__frictionloss': out[99], - '_impl.efc__id': out[100], - '_impl.efc__margin': out[101], - '_impl.efc__pos': out[102], - '_impl.efc__state': out[103], - '_impl.efc__type': out[104], - '_impl.efc__vel': out[105], + '_impl.efc__Jqvel': out[96], + '_impl.efc__Ma': out[97], + '_impl.efc__aref': out[98], + '_impl.efc__force': out[99], + '_impl.efc__frictionloss': out[100], + '_impl.efc__id': out[101], + '_impl.efc__margin': out[102], + '_impl.efc__pos': out[103], + '_impl.efc__state': out[104], + '_impl.efc__type': out[105], + '_impl.efc__vel': out[106], }) return d diff --git a/mjx/mujoco/mjx/warp/render.py b/mjx/mujoco/mjx/warp/render.py index c97003f0..57de37f9 100644 --- a/mjx/mujoco/mjx/warp/render.py +++ b/mjx/mujoco/mjx/warp/render.py @@ -48,7 +48,6 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) - @ffi.format_args_for_warp def _render_shim( # Model diff --git a/mjx/mujoco/mjx/warp/smooth.py b/mjx/mujoco/mjx/warp/smooth.py index 020b9392..ce765d8b 100644 --- a/mjx/mujoco/mjx/warp/smooth.py +++ b/mjx/mujoco/mjx/warp/smooth.py @@ -46,7 +46,6 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) - @ffi.format_args_for_warp def _kinematics_shim( # Model diff --git a/mjx/mujoco/mjx/warp/types.py b/mjx/mujoco/mjx/warp/types.py index 14e416a8..81e77ec8 100644 --- a/mjx/mujoco/mjx/warp/types.py +++ b/mjx/mujoco/mjx/warp/types.py @@ -76,6 +76,7 @@ class BlockDim: cholesky_factorize: int cholesky_factorize_solve: int cholesky_solve: int + contact_jac_tiled: int contact_sort: int energy_vel_kinetic: int euler_dense: int @@ -129,6 +130,7 @@ class ModelWarp(PyTreeNode): body_branch_start: np.ndarray body_branches: np.ndarray body_fluid_ellipsoid: np.ndarray + body_isdofancestor: np.ndarray body_tree: Tuple[np.ndarray, ...] callback: Callback cam_projection: np.ndarray @@ -158,11 +160,16 @@ class ModelWarp(PyTreeNode): flex_elemedgeadr: np.ndarray flex_elemnum: np.ndarray flex_friction: np.ndarray + flex_gap: np.ndarray flex_margin: np.ndarray + flex_priority: np.ndarray flex_radius: np.ndarray flex_shell: np.ndarray flex_shelldataadr: np.ndarray flex_shellnum: np.ndarray + flex_solimp: np.ndarray + flex_solmix: np.ndarray + flex_solref: np.ndarray flex_stiffness: np.ndarray flex_vert: np.ndarray flex_vertbodyid: np.ndarray @@ -302,6 +309,7 @@ class DataWarp(PyTreeNode): efc__J_colind: jax.Array efc__J_rowadr: jax.Array efc__J_rownnz: jax.Array + efc__Jqvel: jax.Array efc__Ma: jax.Array efc__aref: jax.Array efc__force: jax.Array @@ -442,6 +450,7 @@ _NDIM = { 'efc__J_colind': 3, 'efc__J_rowadr': 2, 'efc__J_rownnz': 2, + 'efc__Jqvel': 2, 'efc__Ma': 2, 'efc__aref': 2, 'efc__force': 2, @@ -554,6 +563,7 @@ _NDIM = { 'block_dim__cholesky_factorize': 0, 'block_dim__cholesky_factorize_solve': 0, 'block_dim__cholesky_solve': 0, + 'block_dim__contact_jac_tiled': 0, 'block_dim__contact_sort': 0, 'block_dim__energy_vel_kinetic': 0, 'block_dim__euler_dense': 0, @@ -580,6 +590,7 @@ _NDIM = { 'body_invweight0': 3, 'body_ipos': 3, 'body_iquat': 3, + 'body_isdofancestor': 2, 'body_jntadr': 1, 'body_jntnum': 1, 'body_mass': 2, @@ -610,6 +621,7 @@ _NDIM = { 'dof_armature': 2, 'dof_bodyid': 1, 'dof_damping': 2, + 'dof_dampingpoly': 3, 'dof_frictionloss': 2, 'dof_invweight0': 2, 'dof_jntid': 1, @@ -651,11 +663,16 @@ _NDIM = { 'flex_elemedgeadr': 1, 'flex_elemnum': 1, 'flex_friction': 2, + 'flex_gap': 1, 'flex_margin': 1, + 'flex_priority': 1, 'flex_radius': 1, 'flex_shell': 1, 'flex_shelldataadr': 1, 'flex_shellnum': 1, + 'flex_solimp': 2, + 'flex_solmix': 1, + 'flex_solref': 2, 'flex_stiffness': 2, 'flex_vert': 2, 'flex_vertadr': 1, @@ -715,6 +732,7 @@ _NDIM = { 'jnt_solimp': 3, 'jnt_solref': 3, 'jnt_stiffness': 2, + 'jnt_stiffnesspoly': 3, 'jnt_type': 1, 'light_active': 2, 'light_bodyid': 1, @@ -910,6 +928,7 @@ _NDIM = { 'tendon_adr': 1, 'tendon_armature': 2, 'tendon_damping': 2, + 'tendon_dampingpoly': 3, 'tendon_frictionloss': 2, 'tendon_geom_adr': 1, 'tendon_invweight0': 2, @@ -927,6 +946,7 @@ _NDIM = { 'tendon_solref_fri': 3, 'tendon_solref_lim': 3, 'tendon_stiffness': 2, + 'tendon_stiffnesspoly': 3, 'tree_bodynum': 1, 'tree_dofadr': 1, 'tree_dofnum': 1, @@ -1008,6 +1028,7 @@ _BATCH_DIM = { 'efc__J_colind': True, 'efc__J_rowadr': True, 'efc__J_rownnz': True, + 'efc__Jqvel': True, 'efc__Ma': True, 'efc__aref': True, 'efc__force': True, @@ -1120,6 +1141,7 @@ _BATCH_DIM = { 'block_dim__cholesky_factorize': False, 'block_dim__cholesky_factorize_solve': False, 'block_dim__cholesky_solve': False, + 'block_dim__contact_jac_tiled': False, 'block_dim__contact_sort': False, 'block_dim__energy_vel_kinetic': False, 'block_dim__euler_dense': False, @@ -1146,6 +1168,7 @@ _BATCH_DIM = { 'body_invweight0': True, 'body_ipos': True, 'body_iquat': True, + 'body_isdofancestor': False, 'body_jntadr': False, 'body_jntnum': False, 'body_mass': True, @@ -1176,6 +1199,7 @@ _BATCH_DIM = { 'dof_armature': True, 'dof_bodyid': False, 'dof_damping': True, + 'dof_dampingpoly': True, 'dof_frictionloss': True, 'dof_invweight0': True, 'dof_jntid': False, @@ -1217,11 +1241,16 @@ _BATCH_DIM = { 'flex_elemedgeadr': False, 'flex_elemnum': False, 'flex_friction': False, + 'flex_gap': False, 'flex_margin': False, + 'flex_priority': False, 'flex_radius': False, 'flex_shell': False, 'flex_shelldataadr': False, 'flex_shellnum': False, + 'flex_solimp': False, + 'flex_solmix': False, + 'flex_solref': False, 'flex_stiffness': False, 'flex_vert': False, 'flex_vertadr': False, @@ -1281,6 +1310,7 @@ _BATCH_DIM = { 'jnt_solimp': True, 'jnt_solref': True, 'jnt_stiffness': True, + 'jnt_stiffnesspoly': True, 'jnt_type': False, 'light_active': True, 'light_bodyid': False, @@ -1476,6 +1506,7 @@ _BATCH_DIM = { 'tendon_adr': False, 'tendon_armature': True, 'tendon_damping': True, + 'tendon_dampingpoly': True, 'tendon_frictionloss': True, 'tendon_geom_adr': False, 'tendon_invweight0': True, @@ -1493,6 +1524,7 @@ _BATCH_DIM = { 'tendon_solref_fri': True, 'tendon_solref_lim': True, 'tendon_stiffness': True, + 'tendon_stiffnesspoly': True, 'tree_bodynum': False, 'tree_dofadr': False, 'tree_dofnum': False, From a51a7bf062a5db5bef84e1a4f78272752197d5f3 Mon Sep 17 00:00:00 2001 From: Baruch Tabanpour Date: Mon, 4 May 2026 13:12:54 -0700 Subject: [PATCH 182/251] Open-source MJX-Warp codegen to make external contribution easier. PiperOrigin-RevId: 910200635 Change-Id: I0218f8e281413803500c98fc29d0589c4e90d6b1 --- mjx/mujoco/mjx/codegen/README.md | 23 + mjx/mujoco/mjx/codegen/__init__.py | 14 + mjx/mujoco/mjx/codegen/file.py | 98 +++ mjx/mujoco/mjx/codegen/generate_warp_shim.py | 536 ++++++++++++++++ mjx/mujoco/mjx/codegen/generate_warp_types.py | 570 ++++++++++++++++++ mjx/mujoco/mjx/codegen/trace.py | 318 ++++++++++ .../mjx/codegen/update_for_mujoco_warp.sh | 119 ++++ mjx/mujoco/mjx/warp/collision_driver.py | 1 - mjx/mujoco/mjx/warp/forward.py | 1 - mjx/pyproject.toml | 4 + 10 files changed, 1682 insertions(+), 2 deletions(-) create mode 100644 mjx/mujoco/mjx/codegen/README.md create mode 100644 mjx/mujoco/mjx/codegen/__init__.py create mode 100644 mjx/mujoco/mjx/codegen/file.py create mode 100644 mjx/mujoco/mjx/codegen/generate_warp_shim.py create mode 100644 mjx/mujoco/mjx/codegen/generate_warp_types.py create mode 100644 mjx/mujoco/mjx/codegen/trace.py create mode 100755 mjx/mujoco/mjx/codegen/update_for_mujoco_warp.sh diff --git a/mjx/mujoco/mjx/codegen/README.md b/mjx/mujoco/mjx/codegen/README.md new file mode 100644 index 00000000..3161b534 --- /dev/null +++ b/mjx/mujoco/mjx/codegen/README.md @@ -0,0 +1,23 @@ +# MJX Warp Codegen + +Generates the MJX-Warp shim layer in `mujoco/mjx/warp/` by reading the vendored +`mujoco_warp` source in `mujoco/mjx/third_party/mujoco_warp/`. + +## Setup + +From the root `mjx/` directory, once you install [`uv`](https://docs.astral.sh/uv/getting-started/installation/), install the latest MuJoCo and local MJX: + +```bash +uv venv .venv --default-index https://pypi.org/simple +source .venv/bin/activate +uv pip install --upgrade --force-reinstall mujoco --default-index https://pypi.org/simple --extra-index-url https://py.mujoco.org/ +uv pip install -e ".[warp,dev]" --default-index https://pypi.org/simple +``` + +## Run codegen + +From the root `mjx/` directory: + +```bash +bash mujoco/mjx/codegen/update_for_mujoco_warp.sh +``` diff --git a/mjx/mujoco/mjx/codegen/__init__.py b/mjx/mujoco/mjx/codegen/__init__.py new file mode 100644 index 00000000..100b5347 --- /dev/null +++ b/mjx/mujoco/mjx/codegen/__init__.py @@ -0,0 +1,14 @@ +# Copyright 2026 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== diff --git a/mjx/mujoco/mjx/codegen/file.py b/mjx/mujoco/mjx/codegen/file.py new file mode 100644 index 00000000..06d6e6d1 --- /dev/null +++ b/mjx/mujoco/mjx/codegen/file.py @@ -0,0 +1,98 @@ +# Copyright 2026 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +"""File tools.""" + +import ast +import os +import subprocess +from typing import Dict +from absl import logging +from etils import epath + +LICENSE_TEXT = """ +# Copyright 2026 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +""" + + +def get_base_path() -> epath.Path: + """Resolves the base workspace path.""" + base_path = os.environ.get('BUILD_WORKSPACE_DIRECTORY') + if base_path: + return epath.Path(base_path) + # Assume this file is at /mujoco/mjx/codegen/file.py. + root = epath.Path(os.path.abspath(__file__)).parents[3] + if not (root / 'mujoco' / 'mjx' / 'codegen').is_dir(): + raise RuntimeError( + f'Unexpected codegen layout, resolved root: {root}' + ) + return root + + +def format_file(target_fpath: epath.Path): + """Formats a Python file.""" + logging.info('Running pyink on: %s', target_fpath) + subprocess.run( + ['pyink', str(target_fpath)], + check=True, + text=True, + capture_output=True, + ) + logging.info('Running isort on: %s', target_fpath) + subprocess.run( + ['isort', str(target_fpath)], + check=True, + text=True, + capture_output=True, + ) + + +def write_license(target_fpath: epath.Path): + """Writes license to the target file.""" + src = target_fpath.read_text() + target_fpath.write_text(LICENSE_TEXT + src) + + +def get_cls_type_annotations(src: str) -> Dict[str, Dict[str, str]]: + """Return classes with their field annotation strings from source code.""" + ret = {} + tree = ast.parse(src) + + class Visitor(ast.NodeVisitor): + + def visit_ClassDef(self, node: ast.ClassDef): # pylint: disable=invalid-name + class_name = node.name + ret[class_name] = {} + for item in node.body: + if not isinstance(item, ast.AnnAssign): + continue + field_name = item.target.id # pytype: disable=attribute-error + annotation_str = ast.unparse(item.annotation).strip() + ret[class_name][field_name] = annotation_str + + Visitor().visit(tree) + return ret diff --git a/mjx/mujoco/mjx/codegen/generate_warp_shim.py b/mjx/mujoco/mjx/codegen/generate_warp_shim.py new file mode 100644 index 00000000..84c63925 --- /dev/null +++ b/mjx/mujoco/mjx/codegen/generate_warp_shim.py @@ -0,0 +1,536 @@ +# Copyright 2026 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +"""Creates a shim between JAX and Warp for a given function.""" + +import enum +import inspect +import re +from typing import Dict, List, Sequence + +from absl import app +from absl import flags +from absl import logging +from etils import epath +from mujoco.mjx._src import types as mjx_types +from mujoco.mjx.codegen import file +from mujoco.mjx.codegen import trace +import jax +from mujoco.mjx.third_party import mujoco_warp # pylint: disable=unused-import + + +_MJWARP_FUNCTION = flags.DEFINE_string( + 'mjwarp_function', + 'third_party/py/mujoco_warp/_src/smooth.py:kinematics', + 'Function to create the shim for.', +) +_MJWARP_TYPES = flags.DEFINE_string( + 'mjwarp_types', + 'third_party/py/mujoco_warp/_src/types.py', + 'Path to the mjwarp types file.', +) +_MJX_WARP_OUTPUT_PATH = flags.DEFINE_string( + 'mjx_warp_output_path', + 'third_party/py/mujoco/mjx/warp', + 'Path to the output file.', +) +_ONLY_PUBLIC_OUTPUT_FIELDS = flags.DEFINE_bool( + 'only_public_output_fields', + False, + 'Whether to keep only public fields in the output.', +) +_APPEND_TO_OUTPUT_FILE = flags.DEFINE_bool( + 'append_to_output_file', + False, + 'Whether to append to the output file.', +) + +_RENDER_CONTEXT_BUFFER_NAME = '_MJX_RENDER_CONTEXT_BUFFERS' + + +def _clean_type(type_: str): + # check for enums + if type_ in [ + name + for name, obj in inspect.getmembers(mujoco_warp, inspect.isclass) + if issubclass(obj, (enum.IntEnum, enum.IntFlag)) + ]: + return 'int' + + types_to_prefix = ( + 'vec5', + 'vec8', + 'vec8i', + 'vec10', + 'vec10f', + 'vec11', + 'TileSet', + 'BlockDim', + 'vec_pluginattr', + ) + m = re.match(r'array\((.*)\)', type_) + if m: # match custom mujoco_warp array annotation types + args_str = m.group(1) + args = [a.strip() for a in args_str.split(',')] + ndim, dtype = len(args) - 1, args[-1] + + dims = {1: '', 2: '2d', 3: '3d', 4: '4d'} + if ndim not in dims: + raise ValueError(f'Unsupported array ndim: {ndim} for type: {dtype}') + + type_ = f'wp.array{dims[ndim]}[{dtype}]' + + for t in types_to_prefix: + type_ = re.sub(rf'\b{t}\b', f'mjwp_types.{t}', type_) + return type_ + + +def _get_stage_fields( + field_usage: trace.FieldUsage, +) -> tuple[list[str], list[str]]: + """Returns stage_in and stage_out fields after tracing. + + stage_in: + * Model/ModelWarp jax.Array input fields + * Data jax.Array input fields + * Option/OptionWarp jax.Array input fields + + stage_out: + * Data jax.Array output fields + + Args: + field_usage: FieldUsage object + + Returns: + A tuple of (stage_in, stage_out) field name lists. + """ + stage_in = [] + stage_out = [] + + def is_jax_array(cls, field): + if cls is None: + return False + return cls.__annotations__.get(field) is jax.Array + + ModelWarp = getattr(mjx_types, 'ModelWarp', None) + OptionWarp = getattr(mjx_types, 'OptionWarp', None) + + # stage_in: Model/ModelWarp jax.Array input fields + for field in field_usage.model_fields: + if is_jax_array(mjx_types.Model, field): + stage_in.append(field) + elif is_jax_array(ModelWarp, field): + stage_in.append(field) + # stage_in: Option/OptionWarp jax.Array input fields + elif field.startswith('opt__'): + sub_field = field.split('opt__')[-1] + if is_jax_array(mjx_types.Option, sub_field): + stage_in.append(field) + elif is_jax_array(OptionWarp, sub_field): + stage_in.append(field) + + # stage_in: Data jax.Array input fields + for field in field_usage.data_fields: + if is_jax_array(mjx_types.Data, field): + stage_in.append(field) + + # stage_out: Data jax.Array output fields + for field in field_usage.data_out_fields: + if is_jax_array(mjx_types.Data, field): + stage_out.append(field) + + return sorted(stage_in), sorted(stage_out) + + +def _top_level_imports(field_usage: trace.FieldUsage): + """Returns top-level imports.""" + imports = ''' +"""DO NOT EDIT. This file is auto-generated.""" +import dataclasses +import functools +from mujoco.mjx._src import types +from mujoco.mjx.warp import ffi +import mujoco.mjx.third_party.mujoco_warp as mjwarp +import warp as wp +import jax +from mujoco.mjx.third_party.mujoco_warp._src import types as mjwp_types +''' + + if field_usage.render_context_in_caller: + imports += ( + """ +from mujoco.mjx.warp.render_context import """ + + _RENDER_CONTEXT_BUFFER_NAME + + """ +from mujoco.mjx.warp.render_context import RenderContextPytree +""" + ) + + return imports + + +def _global_assignments(): + """Returns global assignments.""" + assignments = '' + for attr, cls in ( + ('_m', 'mjwarp.Model'), + ('_d', 'mjwarp.Data'), + ('_o', 'mjwarp.Option'), + ('_s', 'mjwarp.Statistic'), + ('_c', 'mjwarp.Contact'), + ('_e', 'mjwarp.Constraint'), + ('_cb', 'mjwp_types.Callback'), + ): + assignments += ( + f'{attr} = {cls}(**{{f.name: None for f in dataclasses.fields({cls}) if' + ' f.init})\n' + ) + return assignments + + +def _warp_function( + fn_name: str, + field_usage: trace.FieldUsage, + mjwarp_field_info: Dict[str, trace.FieldInfo], + mjx_warp_field_info: Dict[str, trace.FieldInfo], +): + """Returns warp function arguments, assignments, and call.""" + # create warp function. + fn_args_model, fn_assignments = [('nworld: int,', (-1, ''))], [] + if field_usage.model_fields: + for f in field_usage.model_fields: + if f not in mjwarp_field_info: + raise AssertionError(f'Field {f} not found in mjwarp_field_info.') + info = mjwarp_field_info[f] + expected_type = _clean_type(info.expected_type) + fn_args_model.append((f'{f}: {expected_type},', info.param_order)) + fn_assignments.append(f' _m.{f.replace('__', '.')} = {f}') + fn_args_model = sorted(fn_args_model, key=lambda x: x[1]) + fn_args_model = ['# Model'] + [f[0] for f in fn_args_model] + + fn_args_data = [] + if field_usage.data_fields: + for f in field_usage.data_fields: + if f not in mjwarp_field_info: + raise AssertionError(f'Field {f} not found in mjwarp_field_info.') + if f == 'nworld': + continue # this gets set manually below + j = trace.FieldInfo(f, 'jax.Array', (0, '')) + is_jax_arr = mjx_warp_field_info.get(f, j).expected_type == 'jax.Array' + is_out = is_jax_arr + info = mjwarp_field_info[f] + param_order = info.param_order + expected_type = _clean_type(info.expected_type) + fn_args_data.append((f'{f}: {expected_type},', (is_out, param_order))) + fn_assignments.append(f' _d.{f.replace('__', '.')} = {f}') + fn_args_data = sorted(fn_args_data, key=lambda x: x[1]) + fn_args_data = ['# Data'] + [f[0] for f in fn_args_data] + + fn_assignments.append(' _d.nworld = nworld') + + render_context_args = [] + render_context_call_arg = '' + if field_usage.render_context_in_caller: + render_context_args = ['# Registry', 'rc_id: int,'] + render_context_call_arg = ', render_context' + fn_assignments.append( + f' render_context = {_RENDER_CONTEXT_BUFFER_NAME}[(rc_id, wp.get_device().ordinal)]' + ) + + if fn_name == 'render': + render_context_args.append('rgb: wp.array2d[wp.uint32],') + render_context_args.append('depth: wp.array2d[wp.float32],') + fn_assignments.append(' render_context.rgb_data = rgb') + fn_assignments.append(' render_context.depth_data = depth') + else: + fn_assignments.append(' dummy.zero_()') + + fn_call = f'mjwarp.{fn_name}(_m, _d{render_context_call_arg})' + fn_args_raw = fn_args_model + fn_args_data + render_context_args + + # create a dummy output if there are no output fields + needs_dummy_output = not field_usage.data_out_fields + if needs_dummy_output and fn_name != 'render': + fn_args_raw.append('# Dummy output') + fn_args_raw.append('dummy: wp.array[int],') + + return fn_args_raw, fn_assignments, fn_call + + +def _jax_shim_fn( + fn_name: str, + field_usage: trace.FieldUsage, + warp_fn_args: List[str], + mjwarp_field_info: Dict[str, trace.FieldInfo], +): + """Generates a JAX shim for the Warp function.""" + num_outputs = 0 + output_dims = [] + jax_args = [] + tree_replace = [] + in_out_argnames = [] + has_side_effect = False + stage_in_fields, stage_out_fields = _get_stage_fields(field_usage) + stage_in_argnames = [f"'{f}'" for f in stage_in_fields] + stage_out_argnames = [f"'{f}'" for f in stage_out_fields] + + for arg in warp_fn_args: + if 'nworld' in arg: + jax_args.append('d.qpos.shape[0]') + continue + + if arg in ('rc_id', 'dummy'): + continue + + if arg in ('rgb', 'depth') and fn_name == 'render': + num_outputs += 1 + continue + + arg_jax = arg + if mjwarp_field_info[arg].param_source == 'Data': + if arg.split('__')[0] not in mjx_types.Data.__annotations__: + arg_jax = f'_impl.{arg}' + arg_jax = 'd.' + arg_jax + elif mjwarp_field_info[arg].param_source == 'Model': + arg_jax = ( + arg.replace('__', '.') + if arg.startswith('opt') or arg.startswith('stat') + else arg + ) + public_field = arg.split('__')[0] in mjx_types.Model.__annotations__ + if not public_field: + arg_jax = f'_impl.{arg_jax}' + if ( + arg.startswith('opt') + and arg.split('__')[-1] not in mjx_types.Option.__annotations__ + ): + arg_jax = arg_jax.replace('opt', 'opt._impl') + arg_jax = 'm.' + arg_jax + else: + raise ValueError( + f'Unknown param source: {mjwarp_field_info[arg].param_source}' + ) + + if arg in field_usage.data_out_fields: + # all out fields are in_out, since JAX already allocated them + in_out_argnames.append(f"'{arg}'") + num_outputs += 1 + output_dims.append(f"'{arg}': {arg_jax}.shape") + + if '_impl' not in arg_jax or not _ONLY_PUBLIC_OUTPUT_FIELDS.value: + tree_replace.append(f'"{arg_jax[2:]}": out[{num_outputs - 1}]') + + if arg == 'geom_dataid': + jax_args.append(f'jax.numpy.expand_dims({arg_jax}, 0)') + else: + jax_args.append(arg_jax) + + if field_usage.render_context_in_caller: + jax_args.append('ctx.key') + + needs_dummy_output = not field_usage.data_out_fields + if needs_dummy_output and fn_name != 'render': + num_outputs = 1 + output_dims = ["'dummy': (d.qpos.shape[0],)"] + has_side_effect = True + + if fn_name == 'render': + output_dims = [ + "'rgb': render_ctx.rgb_data_shape", + "'depth': render_ctx.depth_data_shape", + ] + tree_replace = [] + + render_ctx_param = ( + 'ctx: RenderContextPytree' if field_usage.render_context_in_caller else '' + ) + fn_args = ['m: types.Model', 'd: types.Data'] + + if render_ctx_param: + fn_args.append(render_ctx_param) + + return ( + fn_args, + jax_args, + output_dims, + num_outputs, + tree_replace, + in_out_argnames, + stage_in_argnames, + stage_out_argnames, + has_side_effect, + ) + + +def create_jax_warp_shim( + fn_name: str, + field_usage: trace.FieldUsage, + mjwarp_field_info: Dict[str, trace.FieldInfo], + mjx_warp_field_info: Dict[str, trace.FieldInfo], + out_fpath: epath.Path, +): + """Creates a JAX-wrapped MJWarp function.""" + src = '' + old_src = ( + out_fpath.read_text() + if out_fpath.exists() + else '' + if out_fpath.exists() + else '' + ) + + # create top-level imports. + if not _APPEND_TO_OUTPUT_FILE.value: + src += _top_level_imports(field_usage) + '\n\n' + + # create global assignments. + assignments = _global_assignments() + already_in_src = re.sub(r'\s+', '', assignments) in re.sub( + r'\s+', '', old_src + ) + if not already_in_src or not _APPEND_TO_OUTPUT_FILE.value: + src += assignments + + # create warp function. + fn_args_raw, fn_assignments, fn_call = _warp_function( + fn_name, field_usage, mjwarp_field_info, mjx_warp_field_info + ) + fn_args_raw_str = '\n'.join([' ' + arg for arg in fn_args_raw]) + warp_fn_args = [arg.split(':')[0] for arg in fn_args_raw if '#' not in arg] # pytype: disable=attribute-error + + src += f""" +@ffi.format_args_for_warp +def _{fn_name}_shim( +{fn_args_raw_str} +): + _m.stat = _s + _m.opt = _o + _m.callback = _cb + _d.efc = _e + _d.contact = _c +{'\n'.join(fn_assignments)} + {fn_call} + """ + src += '\n\n' + + # create private jax function. + ( + fn_args, + jax_args, + output_dims, + num_outputs, + tree_replace, + in_out_argnames, + stage_in_argnames, + stage_out_argnames, + has_side_effect, + ) = _jax_shim_fn(fn_name, field_usage, warp_fn_args, mjwarp_field_info) + render_ctx_line = '' + return_stmt = 'return d' + if fn_name == 'render': + render_ctx_line = f' render_ctx = _MJX_RENDER_CONTEXT_BUFFERS[(ctx.key, None)]\n' + return_stmt = 'return out' + output_dims_str = '{' + ','.join(output_dims) + '}' + data_tree_replace = f"d = d.tree_replace({{ {','.join(tree_replace)} }})" + src += f""" +def _{fn_name}_jax_impl({','.join(fn_args)}): +{render_ctx_line} output_dims = {output_dims_str} + jf = ffi.jax_callable_variadic_tuple( + _{fn_name}_shim, num_outputs={num_outputs}, + output_dims=output_dims, + vmap_method=None, + in_out_argnames=set([{','.join(in_out_argnames)}]), + stage_in_argnames=set([{','.join(stage_in_argnames)}]), + stage_out_argnames=set([{','.join(stage_out_argnames)}]), + graph_mode=m.opt._impl.graph_mode, + has_side_effect={has_side_effect}, + ) + out = jf({','.join(jax_args)}) + {data_tree_replace} + {return_stmt} +""" + src += '\n' + + # create public jax functions. + fn_args_no_annotation = [arg.split(':')[0] for arg in fn_args] + fn_call_str = ','.join(fn_args_no_annotation) + + marshal_decorator = '@ffi.marshal_jax_warp_callable' + marshal_vmap_decorator = '@ffi.marshal_custom_vmap' + vmap_return_stmt = f'd = {fn_name}({fn_call_str})\n return d, is_batched[1]' + if fn_name == 'render': + marshal_decorator = ( + '@functools.partial(' + 'ffi.marshal_jax_warp_callable, tree_map_output=True)' + ) + marshal_vmap_decorator = ( + '@functools.partial(ffi.marshal_custom_vmap, tree_map_output=True)' + ) + vmap_return_stmt = ( + f'out = {fn_name}({fn_call_str})\n return out, [True, True]' + ) + + src += f""" +@jax.custom_batching.custom_vmap +{marshal_decorator} +def {fn_name}({','.join(fn_args)}): + return _{fn_name}_jax_impl({','.join(fn_args_no_annotation)}) +@{fn_name}.def_vmap +{marshal_vmap_decorator} +def {fn_name}_vmap(unused_axis_size, is_batched, {','.join(fn_args)}): + {vmap_return_stmt} +""" + src += '\n' + + if _APPEND_TO_OUTPUT_FILE.value: + src = old_src + '\n\n' + src + + out_fpath.write_text(src) + + +def main(argv: Sequence[str]) -> None: + del argv + logging.set_verbosity(logging.DEBUG) + + # Get mjwarp field annotations. + fpath = epath.Path(_MJWARP_TYPES.value) + mjwarp_field_info = trace.get_mjwarp_field_info( + fpath.read_text(), file.get_cls_type_annotations + ) + + # Trace function to get field usage. + fpath, fn_name = _MJWARP_FUNCTION.value.split(':') + field_usage = trace.trace_function(fpath, fn_name, mjwarp_field_info) + + base_path = file.get_base_path() + types_fpath = base_path / _MJX_WARP_OUTPUT_PATH.value / 'types.py' + mjx_warp_field_info = trace.get_mjx_warp_field_info( + types_fpath.read_text(), file.get_cls_type_annotations + ) + + base_path = file.get_base_path() + target_fpath = ( + base_path / _MJX_WARP_OUTPUT_PATH.value / epath.Path(fpath).name + ) + create_jax_warp_shim( + fn_name, field_usage, mjwarp_field_info, mjx_warp_field_info, target_fpath + ) + + if not _APPEND_TO_OUTPUT_FILE.value: + file.write_license(target_fpath) + file.format_file(target_fpath) + + +if __name__ == '__main__': + app.run(main) diff --git a/mjx/mujoco/mjx/codegen/generate_warp_types.py b/mjx/mujoco/mjx/codegen/generate_warp_types.py new file mode 100644 index 00000000..374e7464 --- /dev/null +++ b/mjx/mujoco/mjx/codegen/generate_warp_types.py @@ -0,0 +1,570 @@ +# Copyright 2026 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +"""Generate types for MJX warp integration.""" + +import ast +import dataclasses +import enum +import logging +import typing +from typing import Any, Callable, Dict, List, Optional, Set + +from absl import app +from absl import flags +from etils import epath +import mujoco +from mujoco.mjx.codegen import file +import mujoco.mjx.third_party.mujoco_warp as mjwarp +import numpy as np +import warp as wp +from mujoco.mjx.third_party.warp._src.jax_experimental import ffi + + +_MJX_WARP_TYPES_OUT_FPATH = flags.DEFINE_string( + 'mjx_warp_types_out_path', + 'third_party/py/mujoco/mjx/warp/types.py', + 'Path to write the mjWarp types into.', +) + +_MJX_TYPES_PATH = flags.DEFINE_string( + 'mjx_types_path', + 'third_party/py/mujoco/mjx/_src/types.py', + 'Path to read the MJX types from.', +) + +_DATA_SHAPE_PROPERTY_FIELD = 'cacc' +_DUMMY_XML = """ + + + + + + + + + + + + +""" + + +def _to_py_string(value, indent=0): + """Converts a dictionary/set/tuple/type to a Python code string.""" + indent_str = ' ' * indent + next_indent_str = ' ' * (indent + 1) + if isinstance(value, tuple): + items = [_to_py_string(item, indent) for item in value] + return f'({', '.join(items)})' + + if isinstance(value, type): + if value.__module__ == 'builtins': + return value.__name__ + return f'{value.__module__}.{value.__name__}' + + if isinstance(value, dict): + items = [ + f'\n{next_indent_str}{repr(k)}: {_to_py_string(v, indent + 1)}' + for k, v in sorted(value.items(), key=lambda x: x[0]) + ] + return f'{{{','.join(items)}\n{indent_str}}}' + + if isinstance(value, set): + items = sorted([_to_py_string(item, indent) for item in value]) + items = [f'\n{next_indent_str}{item}' for item in items] + return f'{{{",".join(items)}\n{indent_str}}}' + + return repr(value) + + +def _ast_parse_type(type_repr: str) -> ast.expr: + """Parses a string representation of a type into an AST node.""" + try: + return ast.parse(type_repr, mode='eval').body + except SyntaxError as e: + raise ValueError(f'Failed to parse type repr "{type_repr}": {e}') from e + + +def _get_target_annotation_node( + key: str, + target_annotations: Dict[str, Any], +) -> ast.expr: + """Determines the AST node for the target type annotation for MJX.""" + annotation = target_annotations.get(key) + if annotation == np.ndarray: + return _ast_parse_type('np.ndarray') + + if (isinstance(annotation, wp.array) or + type(annotation).__name__ == '_ArrayAnnotation'): + return _ast_parse_type('jax.Array') + + if annotation in (int, float, bool): + return _ast_parse_type(annotation.__name__) + + if annotation is ffi.GraphMode: + return _ast_parse_type('GraphMode') + + if isinstance(annotation, type) and issubclass(annotation, enum.Enum): + return _ast_parse_type('int') + + if dataclasses.is_dataclass(annotation): + return _ast_parse_type(annotation.__name__) + + is_tuple = typing.get_origin(annotation) == tuple + if is_tuple and typing.get_args(annotation)[1] != ...: + raise NotImplementedError( + 'Only variadic tuples are supported. Got annotation type' + f' {annotation} for key {key}.' + ) + + if is_tuple and typing.get_args(annotation)[0] in (int, float, bool): + type_ = typing.get_args(annotation)[0].__name__ + return _ast_parse_type(f'Tuple[{type_}, ...]') + + if is_tuple and ( + isinstance(typing.get_args(annotation)[0], wp.array) + or type(typing.get_args(annotation)[0]).__name__ == '_ArrayAnnotation' + ): + return _ast_parse_type('Tuple[np.ndarray, ...]') + + if is_tuple and dataclasses.is_dataclass(typing.get_args(annotation)[0]): + cls_type = typing.get_args(annotation)[0].__name__ + return _ast_parse_type(f'Tuple[{cls_type}, ...]') + + raise NotImplementedError( + f'Unhandled annotation type {annotation} for key {key}.' + ) + + +def _get_annotations_recursive( + annotations: Dict[str, Any], prefix: str = '' +) -> Dict[str, Any]: + """Recursively flattens type annotations, handling nested classes. + + Args: + annotations: A dictionary of type annotations (field_name: type). + prefix: The prefix to add to each key, for nested classes. + + Returns: + A dictionary of flattened annotations (e.g., 'contact__dist'). + """ + flattened = {} + for key, annotation in annotations.items(): + full_key = f'{prefix}{key}' + if hasattr( + annotation, '__annotations__' + ) and 'mujoco_warp' in annotation.__module__: + nested = _get_annotations_recursive( + dict(annotation.__annotations__), prefix=f'{full_key}__' + ) + flattened.update(nested) + else: + flattened[full_key] = annotation # Leaf node. + + return flattened + + +def _build_new_class_body_ast( + keys: Set[str], + cls_name: str, + target_annotations: Dict[str, Any], + shape_property: Optional[str] = None, + add_docstring: bool = True, +) -> List[ast.AST]: + """Builds the list of AST nodes for the new class body.""" + new_body_nodes: List[ast.AST] = [] + + if add_docstring: + docstring = f'Derived fields from {cls_name}.' + new_body_nodes.append(ast.Expr(value=ast.Constant(value=docstring))) + + # Sort keys alphabetically before creating AST nodes + sorted_keys = sorted(list(keys)) + for key in sorted_keys: + annotation_node = _get_target_annotation_node(key, target_annotations) + + new_body_nodes.append( + ast.AnnAssign( + target=ast.Name(id=key, ctx=ast.Store()), + annotation=annotation_node, + simple=1, # No value assignment + ) + ) + + if shape_property is not None: + property_string = ( + f'shape = property(lambda self: self.{shape_property}.shape)' + ) + shape_property_node = ast.parse(property_string).body[0] + new_body_nodes.append(shape_property_node) + + return new_body_nodes + + +def _write_class_in_file( + target_fpath: epath.Path, + target_cls_name: str, + target_base_name: str, + new_body_ast: List[ast.AST], +) -> None: + """Reads target file, writes the specified class, and saves.""" + target_code = target_fpath.read_text() + target_tree = ast.parse(target_code) + + if target_cls_name in target_code: + raise ValueError( + f'Class {target_cls_name} already exists in file: {target_fpath}' + ) + + new_class_def = ast.ClassDef( + name=target_cls_name, + bases=[ast.Name(id=target_base_name, ctx=ast.Load())], + body=new_body_ast, # pytype: disable=wrong-arg-types + decorator_list=[], + keywords=[], + type_params=[], + ) + target_tree.body.append(new_class_def) + + ast.fix_missing_locations(target_tree) + modified_code = ast.unparse(target_tree) + + logging.info('Writing modified code back to: %s', target_fpath) + with target_fpath.open('w') as f: + f.write(modified_code) + + logging.info('File successfully rewritten.') + + +def write_header(target_fpath: epath.Path): + """Writes imports and pre-defined class definitions to types.py.""" + header = ''' +"""MJX Warp types. +DO NOT EDIT. This file is auto-generated. +""" +import dataclasses +import typing +from typing import Tuple +import jax +from jax import tree_util +from jax.interpreters import batching +from mujoco.mjx._src import dataclasses as mjx_dataclasses +import numpy as np + +if typing.TYPE_CHECKING: + GraphMode = int # Type alias for pytype. + @dataclasses.dataclass + class Callback: + pass +else: + try: + from warp._src.jax_experimental.ffi import GraphMode + from mujoco.mjx.third_party.mujoco_warp._src import types as mjwp_types + Callback = mjwp_types.Callback + except ImportError: + GraphMode = int # Fallback when warp not installed. + Callback = None + +PyTreeNode = mjx_dataclasses.PyTreeNode +''' + target_fpath.write_text(header) + + +_FLATTEN_UNFLATTEN = """ + # flatten/unflatten all fields for custom jax_callable, but prevent the parent + # PyTreeNode from putting these fields on device. + def tree_flatten(self): + children = list(getattr(self, k) for k in self.__dataclass_fields__) + return (children, None) + @classmethod + def tree_unflatten(cls, aux_data, children): + del aux_data + return cls(*children) +""" + + +def write_nested_dataclass(target_fpath: epath.Path, cls: Any): + new_class_body = _build_new_class_body_ast( + set(cls.__annotations__.keys()), + cls.__name__, + dict(cls.__annotations__), + add_docstring=False, + ) + cls_str = '\n'.join([' ' + ast.unparse(node) for node in new_class_body]) + cls_str = cls_str.replace('jax.Array', 'np.ndarray') + with target_fpath.open('a') as f: + f.write(f''' +@dataclasses.dataclass(frozen=True) +@tree_util.register_pytree_node_class +class {cls.__name__}: + """{cls.__doc__}""" +{cls_str} +{_FLATTEN_UNFLATTEN} +''') + + +def _get_meta_fields(cls_name: str) -> Set[str]: + """Returns the set of fields that should be meta-fields in the pytree.""" + m = mujoco.MjModel.from_xml_string(_DUMMY_XML) + d = mujoco.MjData(m) + mujoco.mj_step(m, d) + + if cls_name == 'Data': + with wp.ScopedDevice('cpu'): + dw = mjwarp.put_data(m, d, nworld=113, naconmax=1113, njmax=1113) + cond_fn = lambda x: x.shape[0] not in {113, 1113} + dw_meta = _get_fields_with_cond(dw, cond_fn) + data_non_vmap = _get_non_vmap_data_fields() + return {k for k in dw_meta if k not in data_non_vmap} + + with wp.ScopedDevice('cpu'): + mw = mjwarp.put_model(m) + cond_fn = lambda x: not hasattr(x, '_is_batched') + mw_meta = _get_fields_with_cond(mw, cond_fn) + if cls_name == 'Model': + return mw_meta + + if cls_name == 'Option': + return {k[len('opt__') :] for k in mw_meta if k.startswith('opt')} + + if cls_name == 'Statistic': + return {k[len('stat__') :] for k in mw_meta if k.startswith('stat')} + + raise NotImplementedError(f'Unhandled class name {cls_name}.') + + +def write_core_cls( + cls_name: str, + target_fpath: epath.Path, + mjx_types_fpath: epath.Path, + flatten_fields: bool = False, + set_diff: bool = True, + extra_annotations: dict[str, type] | None = None, +): + """Writes a core API class (e.g. Model/Data/Option/Statistic).""" + cls = { + 'Model': mjwarp.Model, + 'Data': mjwarp.Data, + 'Option': mjwarp.Option, + 'Statistic': mjwarp.Statistic, + }[cls_name] + + annotations = dict(cls.__annotations__) # pytype: disable=attribute-error + if flatten_fields: + annotations = _get_annotations_recursive(annotations) + + meta_fields = _get_meta_fields(cls_name) + for k, v in annotations.items(): + if k not in meta_fields: + continue + if isinstance(v, wp.array) or type(v).__name__ == '_ArrayAnnotation': + annotations[k] = np.ndarray + + warp_keys = annotations.keys() + mjx_annotations = file.get_cls_type_annotations(mjx_types_fpath.read_text())[ + cls_name + ] + + keys = warp_keys + if set_diff: + # Take the set difference between warp and mjx annotation keys. + keys = warp_keys - mjx_annotations.keys() + + if extra_annotations: + annotations.update(extra_annotations) + keys = set(keys) | extra_annotations.keys() + + if not keys: + raise ValueError('No derived keys found') + + shape_property = None + if cls_name == 'Data': + shape_property = _DATA_SHAPE_PROPERTY_FIELD + + new_class_body = _build_new_class_body_ast( + keys, + cls_name, + annotations, + shape_property=shape_property, + ) + + _write_class_in_file( + target_fpath=target_fpath, + target_cls_name=f'{cls_name}Warp', + target_base_name='PyTreeNode', + new_body_ast=new_class_body, + ) + + +def _get_fields_with_cond( + d: Any, + cond_fn: Callable[[Any], bool], + s: Optional[Set[str]] = None, + prefix: str = '', + not_in: bool = False, + add_static: bool = False, +) -> Set[str]: + """Recursively finds fields given a condition on the leading dimensions.""" + if s is None: + s = set() + for f in dataclasses.fields(d): + attr = getattr(d, f.name) + if dataclasses.is_dataclass(f.type): + s = _get_fields_with_cond(attr, cond_fn, s, f.name + '__', not_in) + continue + if f.type in (int, float, bool) and add_static: + s.add(prefix + f.name) + continue + if not (isinstance(f.type, wp.array) or + type(f.type).__name__ == '_ArrayAnnotation'): + continue + if cond_fn(attr): + s.add(prefix + f.name) + return s + + +def _get_non_vmap_data_fields() -> Set[str]: + """Returns the fields that are not be vmapped but are still jax.Array.""" + m = mujoco.MjModel.from_xml_string(_DUMMY_XML) + d = mujoco.MjData(m) + mujoco.mj_step(m, d) + + dw = mjwarp.put_data(m, d, nworld=113, naconmax=1113, njmax=1113) + cond_fn = lambda x: x.shape[0] not in {113} + non_vmap = _get_fields_with_cond(dw, cond_fn, add_static=True) + return non_vmap + + +def write_register_vmappable(target_fpath: epath.Path): + """Writes register_vmappable to types.py.""" + data_non_vmap = _get_non_vmap_data_fields() + with target_fpath.open('a') as f: + f.write('\nDATA_NON_VMAP =' + _to_py_string(data_non_vmap)) + f.write("""\n +def _to_elt(cont, _, d, axis): + return DataWarp(**{f.name: cont(getattr(d, f.name), axis) + if f.name not in DATA_NON_VMAP + else getattr(d, f.name) for f in DataWarp.fields()}) +def _from_elt(cont, axis_size, d, axis_dest): + return DataWarp(**{f.name: cont(axis_size, getattr(d, f.name), axis_dest) + if f.name not in DATA_NON_VMAP + else getattr(d, f.name) for f in DataWarp.fields()}) +batching.register_vmappable(DataWarp, int, int, _to_elt, _from_elt, None) +""") + + +def _is_ffi_compatible(wp_type: Any) -> bool: + """Returns True if the type is an array, scalar, or variadic tuple.""" + if (isinstance(wp_type, wp.array) or + type(wp_type).__name__ == '_ArrayAnnotation'): + return True + if wp_type in wp._src.types.value_types: + return True + if typing.get_origin(wp_type) is tuple: + return True + return False + + +def _to_jax_ndim(name: str, wp_type: Any) -> int: + if typing.get_origin(wp_type) is tuple: + if typing.get_args(wp_type)[1] != ...: + raise NotImplementedError('Only variadic tuples are supported.') + return -1 # signals that dim should be untouched in downstream code. + ffi_arg = ffi.FfiArg(name, wp_type) + return ffi_arg.jax_ndim + + +def write_ndim_annotations(target_fpath: epath.Path): + """Writes a dictionary of ndim for every warp field.""" + ndim_annotations = {} + for cls in [mjwarp.Model, mjwarp.Data, mjwarp.Option, mjwarp.Statistic]: + ndim_annotations[cls.__name__] = {} + annotations = _get_annotations_recursive(cls.__annotations__) + for name, type_ in annotations.items(): + if not _is_ffi_compatible(type_): + continue + ndim = _to_jax_ndim(name, type_) + ndim_annotations[cls.__name__][name] = ndim + + with target_fpath.open('a') as f: + f.write('\n_NDIM = ' + _to_py_string(ndim_annotations)) + + +def write_nworld_leading_dim(target_fpath: epath.Path): + """Writes a dictionary of which MJW fields are batched.""" + # TODO(btaba): check that batch fields have MJX jax.Array annotations, and + # that non-batch fields have np.ndarray annotations. Fail early. + + m = mujoco.MjModel.from_xml_string(_DUMMY_XML) + d = mujoco.MjData(m) + + batched = {} + for cls in [mjwarp.Model, mjwarp.Data, mjwarp.Option, mjwarp.Statistic]: + batched[cls.__name__] = {} + + if cls.__name__ == 'Data': + with wp.ScopedDevice('cpu'): + dw = mjwarp.put_data(m, d, nworld=113, naconmax=1113, njmax=1113) + cond_fn = lambda x: x.shape[0] == 113 + batched_fields = _get_fields_with_cond(dw, cond_fn) + else: + with wp.ScopedDevice('cpu'): + obj = mjwarp.put_model(m) + if cls.__name__ == 'Option': + obj = obj.opt + elif cls.__name__ == 'Statistic': + obj = obj.stat + cond_fn = lambda x: hasattr(x, '_is_batched') + batched_fields = _get_fields_with_cond(obj, cond_fn) + + all_annotations = _get_annotations_recursive(cls.__annotations__) + for name, type_ in all_annotations.items(): + if not _is_ffi_compatible(type_): + continue + batched[cls.__name__][name] = name in batched_fields + + with target_fpath.open('a') as f: + f.write('\n_BATCH_DIM = ' + _to_py_string(batched)) + + +def main(argv): + del argv + + base_path = file.get_base_path() + target_fpath = base_path / _MJX_WARP_TYPES_OUT_FPATH.value + mjx_types_fpath = base_path / _MJX_TYPES_PATH.value + + write_header(target_fpath) + # TODO(btaba): consider automated grabbing of nested dataclasses from mjwarp. + write_nested_dataclass(target_fpath, mjwarp._src.types.TileSet) + write_nested_dataclass(target_fpath, mjwarp._src.types.BlockDim) + + write_core_cls('Statistic', target_fpath, mjx_types_fpath, set_diff=False) + write_core_cls( + 'Option', target_fpath, mjx_types_fpath, + extra_annotations={'graph_mode': ffi.GraphMode}, + ) + write_core_cls('Model', target_fpath, mjx_types_fpath) + write_core_cls('Data', target_fpath, mjx_types_fpath, flatten_fields=True) + write_register_vmappable(target_fpath) + write_ndim_annotations(target_fpath) + write_nworld_leading_dim(target_fpath) + + file.write_license(target_fpath) + file.format_file(target_fpath) + + +if __name__ == '__main__': + app.run(main) diff --git a/mjx/mujoco/mjx/codegen/trace.py b/mjx/mujoco/mjx/codegen/trace.py new file mode 100644 index 00000000..5ba8359d --- /dev/null +++ b/mjx/mujoco/mjx/codegen/trace.py @@ -0,0 +1,318 @@ +# Copyright 2026 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +"""Static AST tracing to find MuJoCo Model and Data field usages.""" + +import ast +import dataclasses +import functools +import importlib +import importlib.util +import os +from typing import Dict, Optional, Sequence, Set, Tuple + +from absl import logging +from etils import epath +from mujoco.mjx.codegen import file + + +def _get_imported_module_names(fpath: epath.Path) -> Sequence[Tuple[str, str]]: + """Returns set of (fully qualified module_name, alias) tuples.""" + module_names = set() + tree = ast.parse(fpath.read_text(), filename=fpath) + + for node in ast.walk(tree): + if isinstance(node, ast.Import): + for alias in node.names: + module_names.add((alias.name, alias.name)) + elif isinstance(node, ast.ImportFrom): + if node.module: + for name in node.names: + name_ = name.name if not name.asname else name.asname + module_names.add((f'{node.module}.{name.name}', f'{name_}')) + return list(module_names) + + +def _resolve_module_name_to_fpath(fully_qualified_name: str) -> Optional[str]: + """Resolves a fully qualified name to its file path using importlib.""" + name_parts = fully_qualified_name.split('.') + for i in range(len(name_parts), 0, -1): + module_name_to_try = '.'.join(name_parts[:i]) + try: + spec = importlib.util.find_spec(module_name_to_try) + if spec and spec.origin and spec.origin != 'built-in': + return os.path.abspath(spec.origin) + except (ModuleNotFoundError, ImportError): + continue + + +def _get_imported_module_fpaths(fpath: epath.Path) -> Dict[str, str]: + """Returns the file paths of all imported modules.""" + all_imported_names = _get_imported_module_names(fpath) + + all_resolved_fpaths = {} + for fully_qualified_name, alias in all_imported_names: + fpath = _resolve_module_name_to_fpath(fully_qualified_name) + if fpath: + all_resolved_fpaths[alias] = fpath + return all_resolved_fpaths + + +@dataclasses.dataclass +class FieldInfo: + param_source: str + expected_type: str + param_order: tuple[int, str] + + +class _FunctionFieldUsageVisitor(ast.NodeVisitor): + """AST visitor to find attribute usages on 'm' and 'd' variables.""" + + def __init__( + self, + current_fpath: epath.Path, + visited_fns: Set[Tuple[str, str]], + mjwarp_field_info: Dict[str, FieldInfo], + ): + self.model_fields = set() + self.data_fields = set() + self.data_out_fields = set() + self._current_fpath = current_fpath.as_posix() + self._visited_fns = visited_fns + self._mjwarp_field_info = mjwarp_field_info + self._module_fpaths = _get_imported_module_fpaths(current_fpath) + self._in_outputs_context = False + + def visit_FunctionDef(self, node: ast.FunctionDef): + """Visits nested function definitions.""" + self.generic_visit(node) + + def add_field_usage(self, node: ast.Attribute, is_output: bool): + """Adds field to the appropriate set.""" + attr_parts = [] + curr_node = node + while isinstance(curr_node, ast.Attribute): + attr_parts.append(curr_node.attr) + curr_node = curr_node.value + + if isinstance(curr_node, ast.Name): + attr_parts.reverse() + full_attribute_str = '__'.join(attr_parts) + in_field_info = full_attribute_str in self._mjwarp_field_info + if in_field_info: + if curr_node.id == 'm': + self.model_fields.add(full_attribute_str) + if curr_node.id == 'd': + self.data_fields.add(full_attribute_str) + if curr_node.id == 'd' and is_output: + self.data_out_fields.add(full_attribute_str) + + def visit_Attribute(self, node: ast.Attribute): + self.add_field_usage(node, self._in_outputs_context) + self.generic_visit(node) + + def visit_keyword(self, node: ast.keyword): + """Visit a keyword argument node (e.g., outputs=[...]).""" + previous_in_outputs_context = self._in_outputs_context + if node.arg == 'outputs': + self._in_outputs_context = True + try: + self.generic_visit(node) + finally: + self._in_outputs_context = previous_in_outputs_context + + def recurse_trace(self, next_fpath: str, called_fn_name: str): + """Recursively trace into a function.""" + try: + field_usage = trace_function( + next_fpath, + called_fn_name, + self._mjwarp_field_info, + self._visited_fns, + ) + self.model_fields.update(field_usage.model_fields) + self.data_fields.update(field_usage.data_fields) + self.data_out_fields.update(field_usage.data_out_fields) + except ValueError as e: + logging.warning( + 'Could not trace function %s in %s: %s', + called_fn_name, + next_fpath, + e, + ) + + def visit_Call(self, node: ast.Call): + """Visit a function call node and recursively find all attribute usages.""" + if isinstance(node.func, ast.Name): + called_fn_name = node.func.id + key = (hash(self._current_fpath), called_fn_name) + if key not in self._visited_fns: + self._visited_fns.add(key) + next_fpath = self._module_fpaths.get( + called_fn_name, self._current_fpath + ) + self.recurse_trace(next_fpath, called_fn_name) + elif isinstance(node.func, ast.Attribute): + parts = [] + current = node.func + while isinstance(current, ast.Attribute): + parts.append(current.attr) + current = current.value + if isinstance(current, ast.Name): + parts.append(current.id) + parts = parts[::-1] + + if len(parts) == 2 and parts[0] == 'wp' and parts[1] == 'copy': + if len(node.args) != 2: + raise ValueError(f'wp.copy() must have 2 arguments, got {node.args}.') + out_node, in_node = node.args + self.add_field_usage(out_node, True) + self.add_field_usage(in_node, False) + return + + for arg in node.args: + if isinstance(arg, ast.Attribute): + self.add_field_usage(arg, is_output=True) + + called_fn_name = '.'.join(parts[1:]) + key = (hash(self._current_fpath), called_fn_name) + next_fpath = self._module_fpaths.get(parts[0]) + if next_fpath and key not in self._visited_fns: + self._visited_fns.add(key) + self.recurse_trace(next_fpath, called_fn_name) + + self.generic_visit(node) + + +@dataclasses.dataclass +class FieldUsage: + model_fields: Sequence[str] = dataclasses.field(default_factory=list) + data_fields: Sequence[str] = dataclasses.field(default_factory=list) + data_out_fields: Sequence[str] = dataclasses.field(default_factory=list) + render_context_in_caller: bool = False + + +def trace_function( + fpath: str, + fn: str, + mjwarp_field_info: Dict[str, FieldInfo], + visited_fns: Set[Tuple[str, str]] | None = None, +) -> FieldUsage: + """Traces the function statically to find usages of model and data fields.""" + base_path = file.get_base_path() + fpath = base_path / fpath + logging.info('Tracing function: "%s" in "%s"', fn, fpath) + + src = fpath.read_text() + parsed_ast = ast.parse(src, filename=str(fpath)) + + target_fn_nodes = ( + node + for node in parsed_ast.body + if isinstance(node, ast.FunctionDef) and node.name == fn + ) + target_fn_node = next(target_fn_nodes, None) + if not target_fn_node: + raise ValueError(f'Function "{fn}" not found in "{fpath}".') + + args_node = target_fn_node.args + args_tuple = tuple( + map(functools.partial(ast.get_source_segment, src), args_node.args) + ) + check_args_tuple = ( + ('m: Model', 'd: Data'), + ('m: types.Model', 'd: types.Data'), + ('m', 'd'), + ('m: Model',), + ('m: types.Model',), + ('m',), + ) + if ( + args_tuple[:2] not in check_args_tuple + and args_tuple[:1] not in check_args_tuple + ): + raise ValueError( + f'Function "{fn}" in "{fpath}" must have arguments in' + f' {check_args_tuple} got {args_tuple}.' + ) + + if visited_fns is None: + visited_fns = set() + + visitor = _FunctionFieldUsageVisitor(fpath, visited_fns, mjwarp_field_info) + for body in target_fn_node.body: + visitor.visit(body) + + render_context_in_caller = False + if len(target_fn_node.args.args) > 2: + third_param = target_fn_node.args.args[2] + if third_param.annotation: + annotation_str = ast.unparse(third_param.annotation) + render_context_in_caller = 'RenderContext' in annotation_str + + logging.info( + 'End trace function "%s". Output fields: %s, RenderContext: %s', + fn, visitor.data_out_fields, render_context_in_caller + ) + return FieldUsage( + model_fields=sorted(list(visitor.model_fields)), + data_fields=sorted(list(visitor.data_fields)), + data_out_fields=sorted(list(visitor.data_out_fields)), + render_context_in_caller=render_context_in_caller, + ) + + +def get_mjwarp_field_info(src: str, get_cls_type_annotations) -> Dict[str, FieldInfo]: + """Return field info for mujoco_warp/_src/types.py.""" + dataclass_map = { + 'opt': 'Option', + 'stat': 'Statistic', + 'efc': 'Constraint', + 'contact': 'Contact', + } + field_info = {} + type_classes = get_cls_type_annotations(src) + for field, typ in type_classes['Model'].items(): + if field == 'callback': + continue + if field in field_info: + raise AssertionError(f'Field {field} is duplicated in Model.') + if field in dataclass_map: + for sfield, styp in type_classes[dataclass_map[field]].items(): + field_name = field + '__' + sfield + field_info[field_name] = FieldInfo('Model', styp, (1, field_name)) + else: + field_info[field] = FieldInfo('Model', typ, (0, field)) + + for field, typ in type_classes['Data'].items(): + if field in field_info: + raise AssertionError(f'Field {field} is duplicated.') + if field in dataclass_map: + for sfield, styp in type_classes[dataclass_map[field]].items(): + field_name = field + '__' + sfield + field_info[field_name] = FieldInfo('Data', styp, (3, field_name)) + else: + field_info[field] = FieldInfo('Data', typ, (2, field)) + return field_info + + +def get_mjx_warp_field_info(src: str, get_cls_type_annotations) -> Dict[str, FieldInfo]: + """Return field info for mjx/warp/types.py.""" + field_info = {} + type_classes = get_cls_type_annotations(src) + for field, typ in type_classes['DataWarp'].items(): + field_info[field] = FieldInfo('Data', typ, (0, field)) + for field, typ in type_classes['ModelWarp'].items(): + field_info[field] = FieldInfo('Model', typ, (1, field)) + return field_info diff --git a/mjx/mujoco/mjx/codegen/update_for_mujoco_warp.sh b/mjx/mujoco/mjx/codegen/update_for_mujoco_warp.sh new file mode 100755 index 00000000..5186c6f5 --- /dev/null +++ b/mjx/mujoco/mjx/codegen/update_for_mujoco_warp.sh @@ -0,0 +1,119 @@ +#!/bin/bash +# Copyright 2026 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== + +set -euo pipefail + +# --- Logging helpers --------------------------------------------------------- +log_stage() { echo -e "\n\033[1;34m==== $1 ====\033[0m"; } +log_ok() { echo -e "\033[1;32m ✓ $1\033[0m"; } +log_fail() { echo -e "\033[1;31m ✗ $1\033[0m"; } + +run_shim() { + local label="$1"; shift + log_stage "Generating shim: ${label}" + echo " → $*" + local output + if output=$("$@" --logtostderr 2>&1); then + # Show only Python-level log lines (filters noisy C++ infra logs). + echo "$output" | grep -E '\.py' || true + log_ok "${label}" + else + echo "$output" + log_fail "${label}" + exit 1 + fi +} + +# --- Path setup -------------------------------------------------------------- +mjwarp_base="mujoco/mjx/third_party/mujoco_warp/_src" +mjx_base="mujoco/mjx" + +# Derived paths (shared). +mjwarp="${mjwarp_base}" +mjx_warp_out="${mjx_base}/warp" +mjx_types="${mjx_base}/_src/types.py" +mjx_warp_types="${mjx_base}/warp/types.py" + +log_stage "Path configuration" +echo " mjwarp_base = ${mjwarp_base}" +echo " mjx_base = ${mjx_base}" +echo " mjx_types = ${mjx_types}" +echo " output dir = ${mjx_warp_out}" + +# --- Stage 1: Generate warp types ------------------------------------------- +log_stage "Stage 1/3: Generating warp types" +python mujoco/mjx/codegen/generate_warp_types.py \ + --mjx_warp_types_out_path=${mjx_warp_types} \ + --mjx_types_path=${mjx_types} +log_ok "Warp types written to ${mjx_warp_types}" + +# --- Stage 2: Build shim generator ------------------------------------------ +log_stage "Stage 2/3: Building shim generator" +generate_warp_shim="python mujoco/mjx/codegen/generate_warp_shim.py" +log_ok "Shim generator ready" + +# --- Stage 3: Generate shim code for each function -------------------------- +log_stage "Stage 3/3: Generating shim code" + +# Smooth. +run_shim "smooth:kinematics" ${generate_warp_shim} \ + --mjwarp_function=${mjwarp}/smooth.py:kinematics \ + --mjx_warp_output_path=${mjx_warp_out}/ \ + --mjwarp_types=${mjwarp}/types.py + +run_shim "smooth:tendon" ${generate_warp_shim} \ + --mjwarp_function=${mjwarp}/smooth.py:tendon \ + --mjx_warp_output_path=${mjx_warp_out}/ \ + --mjwarp_types=${mjwarp}/types.py \ + --append_to_output_file=True + +run_shim "smooth:com_pos" ${generate_warp_shim} \ + --mjwarp_function=${mjwarp}/smooth.py:com_pos \ + --mjx_warp_output_path=${mjx_warp_out}/ \ + --mjwarp_types=${mjwarp}/types.py \ + --append_to_output_file=True + +# Collision. +run_shim "collision_driver:collision" ${generate_warp_shim} \ + --mjwarp_function=${mjwarp}/collision_driver.py:collision \ + --mjx_warp_output_path=${mjx_warp_out}/ \ + --mjwarp_types=${mjwarp}/types.py + +# Forward. +run_shim "forward:forward" ${generate_warp_shim} \ + --mjwarp_function=${mjwarp}/forward.py:forward \ + --mjx_warp_output_path=${mjx_warp_out}/ \ + --mjwarp_types=${mjwarp}/types.py + +run_shim "forward:step" ${generate_warp_shim} \ + --mjwarp_function=${mjwarp}/forward.py:step \ + --mjx_warp_output_path=${mjx_warp_out}/ \ + --mjwarp_types=${mjwarp}/types.py \ + --append_to_output_file=True + +# Render and bvh. +run_shim "bvh:refit_bvh" ${generate_warp_shim} \ + --mjwarp_function=${mjwarp}/bvh.py:refit_bvh \ + --mjx_warp_output_path=${mjx_warp_out}/ \ + --mjwarp_types=${mjwarp}/types.py + +run_shim "render:render" ${generate_warp_shim} \ + --mjwarp_function=${mjwarp}/render.py:render \ + --mjx_warp_output_path=${mjx_warp_out}/ \ + --mjwarp_types=${mjwarp}/types.py + +log_stage "Done" +log_ok "All shims generated successfully" diff --git a/mjx/mujoco/mjx/warp/collision_driver.py b/mjx/mujoco/mjx/warp/collision_driver.py index dbecb9c1..47b94ed2 100644 --- a/mjx/mujoco/mjx/warp/collision_driver.py +++ b/mjx/mujoco/mjx/warp/collision_driver.py @@ -46,7 +46,6 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) - @ffi.format_args_for_warp def _collision_shim( # Model diff --git a/mjx/mujoco/mjx/warp/forward.py b/mjx/mujoco/mjx/warp/forward.py index 2f46f1f3..f260ea92 100644 --- a/mjx/mujoco/mjx/warp/forward.py +++ b/mjx/mujoco/mjx/warp/forward.py @@ -46,7 +46,6 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) - @ffi.format_args_for_warp def _forward_shim( # Model diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index 65b614b9..9d3e2f85 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -39,6 +39,10 @@ dependencies = [ warp = [ "warp-lang==1.12.1", ] +dev = [ + "isort", + "pyink", +] [project.scripts] mjx-testspeed = "mujoco.mjx.testspeed:main" From 767c607f58b702e91f2050ba141da23dc067c70d Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 4 May 2026 14:25:23 -0700 Subject: [PATCH 183/251] Add `mju_sym2dense`, document future breakage of `mj_fullM` PiperOrigin-RevId: 910242375 Change-Id: Ibfbdef9cfb66088723499ea257da09aee0d80938 --- doc/APIreference/functions.rst | 9 ++++++ doc/changelog.rst | 14 +++++++++ doc/includes/references.h | 2 ++ include/mujoco/mujoco.h | 4 +++ mjx/mujoco/mjx/_src/io.py | 8 ++++- mjx/mujoco/mjx/_src/io_test.py | 6 ++-- mjx/mujoco/mjx/_src/smooth_test.py | 2 +- mjx/mujoco/mjx/_src/support_test.py | 2 +- mjx/mujoco/mjx/warp/forward_test.py | 2 +- python/mujoco/functions.cc | 31 +++++++++++++++++++ python/mujoco/introspect/functions.py | 42 ++++++++++++++++++++++++++ src/engine/engine_island.c | 2 +- src/engine/engine_setconst.c | 2 +- src/engine/engine_util_sparse.c | 17 +++++++++++ src/engine/engine_util_sparse.h | 4 +++ test/engine/engine_support_test.cc | 7 +++-- test/engine/engine_util_sparse_test.cc | 36 ++++++++++++++++++++++ unity/Runtime/Bindings/MjBindings.cs | 3 ++ wasm/codegen/generated/bindings.cc | 10 ++++++ 19 files changed, 191 insertions(+), 12 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index fc513e20..a91344ac 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -3982,6 +3982,15 @@ Convert matrix from dense to sparse. Convert matrix from sparse to dense. +.. _mju_sym2dense: + +`mju_sym2dense <#mju_sym2dense>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mju_sym2dense + +Convert lower-triangular symmetric CSR matrix to full dense matrix. + .. _Quaternions: Quaternions diff --git a/doc/changelog.rst b/doc/changelog.rst index 26f1f591..92df67c5 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -5,6 +5,8 @@ Changelog Upcoming version (not yet released) ----------------------------------- +General +^^^^^^^ - Added island support for the :ref:`PGS solver`. - Added support for :ref:`elastic2d` for trilinear and quadratic flex :ref:`dofs`. @@ -13,6 +15,18 @@ Upcoming version (not yet released) (nonzero :ref:`density` or :ref:`viscosity`). Midpoint integration treats external forces as zero-order-hold constants, which causes energy gain in the presence of contacts and in fluid media. +- Added :ref:`mju_sym2dense`, converting a lower-triangular, implicitly symmetric CSR matrix to a dense + symmetric matrix. The inertia matrix ``mjData.M`` is an example of such a matrix. + +.. admonition:: Future breaking API changes + :class: warning + + - The introduction of :ref:`mju_sym2dense` is a step towards the removal of the legacy-format ``mjData.qM`` in favor + of the CSR-format ``mjData.M``. This removal will involve a future breaking change to :ref:`mj_fullM` (which + currently accepts a ``qM``-like matrix as an argument). To prevent a future breakage, replace + ``mj_fullM(m, dst, d->qM)`` with + |br| ``mju_sym2dense(dst, d->M, m->nv, m->M_rownnz, m->M_rowadr, m->M_colind)``. + Python ^^^^^^ diff --git a/doc/includes/references.h b/doc/includes/references.h index c594a32d..edc8b0c4 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3522,6 +3522,8 @@ int mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc, int* rownnz, int* rowadr, int* colind, int nnz); void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc, const int* rownnz, const int* rowadr, const int* colind); +void mju_sym2dense(mjtNum* res, const mjtNum* mat, int n, + const int* rownnz, const int* rowadr, const int* colind); void mju_rotVecQuat(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]); void mju_negQuat(mjtNum res[4], const mjtNum quat[4]); void mju_mulQuat(mjtNum res[4], const mjtNum quat1[4], const mjtNum quat2[4]); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index c8ca6ef5..45b56bb1 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1206,6 +1206,10 @@ MJAPI int mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc, MJAPI void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc, const int* rownnz, const int* rowadr, const int* colind); +// Convert lower-triangular symmetric CSR matrix to full dense matrix. +MJAPI void mju_sym2dense(mjtNum* res, const mjtNum* mat, int n, + const int* rownnz, const int* rowadr, const int* colind); + //---------------------------------- Quaternions --------------------------------------------------- diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 918b0fa4..a4630703 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -1058,7 +1058,13 @@ def _put_data_jax( # convert qM and qLD if jacobian is dense if not support.is_sparse(m): impl_fields['qM'] = np.zeros((m.nv, m.nv)) - mujoco.mj_fullM(m, impl_fields['qM'], d.qM) + mujoco.mju_sym2dense( + impl_fields['qM'], + d.M, + m.M_rownnz, + m.M_rowadr, + m.M_colind, + ) # TODO(erikfrey): derive L*L' from L'*D*L instead of recomputing try: impl_fields['qLD'], _ = scipy.linalg.cho_factor(impl_fields['qM']) diff --git a/mjx/mujoco/mjx/_src/io_test.py b/mjx/mujoco/mjx/_src/io_test.py index 542f0718..6c61e8d6 100644 --- a/mjx/mujoco/mjx/_src/io_test.py +++ b/mjx/mujoco/mjx/_src/io_test.py @@ -520,7 +520,7 @@ class DataIOTest(parameterized.TestCase): if impl == 'jax': # check that qM is transformed properly qm = np.zeros((m.nv, m.nv), dtype=np.float64) - mujoco.mj_fullM(m, qm, d.qM) + mujoco.mju_sym2dense(qm, d.M, m.M_rownnz, m.M_rowadr, m.M_colind) np.testing.assert_allclose(qm, mjx.full_m(mjx.put_model(m), dx)) elif impl == 'cpp': @@ -529,7 +529,7 @@ class DataIOTest(parameterized.TestCase): return # cpp does not populate other fields in _impl elif impl == 'warp': qm = np.zeros((m.nv, m.nv), dtype=np.float64) - mujoco.mj_fullM(m, qm, d.qM) + mujoco.mju_sym2dense(qm, d.M, m.M_rownnz, m.M_rowadr, m.M_colind) np.testing.assert_allclose(dx._impl.qM, qm) # TODO(taylorhowell): test efc__J np.testing.assert_allclose(dx._impl.efc__aref[:3], d.efc_aref[:3]) @@ -596,7 +596,7 @@ class DataIOTest(parameterized.TestCase): dx_from_dense = mjx.put_data(m, d, impl=impl) if impl == 'jax': qm = np.zeros((m.nv, m.nv)) - mujoco.mj_fullM(m, qm, d.qM) + mujoco.mju_sym2dense(qm, d.M, m.M_rownnz, m.M_rowadr, m.M_colind) np.testing.assert_allclose(dx_from_dense._impl.qM, qm, atol=1e-8) diff --git a/mjx/mujoco/mjx/_src/smooth_test.py b/mjx/mujoco/mjx/_src/smooth_test.py index 7f0d8c09..b421b68f 100644 --- a/mjx/mujoco/mjx/_src/smooth_test.py +++ b/mjx/mujoco/mjx/_src/smooth_test.py @@ -438,7 +438,7 @@ class TendonTest(parameterized.TestCase): if jacobian == JacobianType.DENSE: qM = np.zeros((m.nv, m.nv)) # pylint: disable=invalid-name - mujoco.mj_fullM(m, qM, d.qM) + mujoco.mju_sym2dense(qM, d.M, m.M_rownnz, m.M_rowadr, m.M_colind) else: qM = d.qM # pylint: disable=invalid-name _assert_eq(dx._impl.qM, qM, 'qM') diff --git a/mjx/mujoco/mjx/_src/support_test.py b/mjx/mujoco/mjx/_src/support_test.py index 6268f14b..a01fb004 100644 --- a/mjx/mujoco/mjx/_src/support_test.py +++ b/mjx/mujoco/mjx/_src/support_test.py @@ -64,7 +64,7 @@ class SupportTest(parameterized.TestCase): dx = mjx.put_data(m, d) mjx_full_m = jax.jit(support.full_m)(mx, dx) mj_full_m = np.zeros((m.nv, m.nv), dtype=np.float64) - mujoco.mj_fullM(m, mj_full_m, d.qM) + mujoco.mju_sym2dense(mj_full_m, d.M, m.M_rownnz, m.M_rowadr, m.M_colind) np.testing.assert_allclose(mjx_full_m, mj_full_m, atol=5e-5, rtol=5e-5) @parameterized.parameters('constraints.xml', 'pendula.xml') diff --git a/mjx/mujoco/mjx/warp/forward_test.py b/mjx/mujoco/mjx/warp/forward_test.py index 056a0c51..2c38891e 100644 --- a/mjx/mujoco/mjx/warp/forward_test.py +++ b/mjx/mujoco/mjx/warp/forward_test.py @@ -175,7 +175,7 @@ class ForwardTest(parameterized.TestCase): tu.assert_attr_eq(dx._impl, d, 'crb') qm = np.zeros((m.nv, m.nv)) - mujoco.mj_fullM(m, qm, d.qM) + mujoco.mju_sym2dense(qm, d.M, m.M_rownnz, m.M_rowadr, m.M_colind) # mjwarp adds padding to qM tu.assert_eq(qm, dx._impl.qM[: m.nv, : m.nv], 'qM') # qLD is fused in a cholesky factorize and solve, and not written to. diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index 03a1be1b..5787d756 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -1241,6 +1241,37 @@ PYBIND11_MODULE(_functions, pymodule) { colind.data()); }); + DEF_WITH_OMITTED_PY_ARGS(traits::mju_sym2dense, "n")( + pymodule, + [](Eigen::Ref res, + Eigen::Ref mat, + Eigen::Ref rownnz, + Eigen::Ref rowadr, + Eigen::Ref colind) { + if (res.rows() != res.cols()) { + throw py::type_error("res should be a square matrix"); + } + if (res.rows() != rownnz.size()) { + throw py::type_error("#rows in res should equal size of rownnz"); + } + if (res.rows() != rowadr.size()) { + throw py::type_error("#rows in res should equal size of rowadr"); + } + if (res.rows() > 0) { + int nnz = rowadr.array().tail(1)[0] + rownnz.array().tail(1)[0]; + if (mat.size() < nnz) { + throw py::type_error("mat size is too small for the given sparse " + "structure"); + } + if (colind.size() < nnz) { + throw py::type_error("colind size is too small for the given " + "sparse structure"); + } + } + return ::mju_sym2dense(res.data(), mat.data(), res.rows(), + rownnz.data(), rowadr.data(), colind.data()); + }); + // Quaternions Def(pymodule); Def(pymodule); diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index e16f434f..ff12151e 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -7755,6 +7755,48 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Convert matrix from sparse to dense.', )), + ('mju_sym2dense', + FunctionDecl( + name='mju_sym2dense', + return_type=ValueType(name='void'), + parameters=( + FunctionParameterDecl( + name='res', + type=PointerType( + inner_type=ValueType(name='mjtNum'), + ), + ), + FunctionParameterDecl( + name='mat', + type=PointerType( + inner_type=ValueType(name='mjtNum', is_const=True), + ), + ), + FunctionParameterDecl( + name='n', + type=ValueType(name='int'), + ), + FunctionParameterDecl( + name='rownnz', + type=PointerType( + inner_type=ValueType(name='int', is_const=True), + ), + ), + FunctionParameterDecl( + name='rowadr', + type=PointerType( + inner_type=ValueType(name='int', is_const=True), + ), + ), + FunctionParameterDecl( + name='colind', + type=PointerType( + inner_type=ValueType(name='int', is_const=True), + ), + ), + ), + doc='Convert lower-triangular symmetric CSR matrix to full dense matrix.', # pylint: disable=line-too-long + )), ('mju_rotVecQuat', FunctionDecl( name='mju_rotVecQuat', diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 399fcbc1..ebda019d 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -525,7 +525,7 @@ void mj_island(const mjModel* m, mjData* d) { d->island_dofadr[i] = d->map_idof2dof[d->island_idofadr[i]]; } - // inertia: block-diagonalize both iLD <- qLD and iM <- qM + // inertia: block-diagonalize both iLD <- qLD and iM <- M mju_blockDiagSparse(d->iLD, d->iM_rownnz, d->iM_rowadr, d->iM_colind, d->qLD, m->M_rownnz, m->M_rowadr, m->M_colind, nidof, nisland, diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 754aa29b..e3e41069 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -1307,7 +1307,7 @@ static void setStat(mjModel* m, mjData* d) { if (m->nv) { m->stat.meaninertia = 0; for (int i=0; i < m->nv; i++) { - m->stat.meaninertia += d->qM[m->dof_Madr[i]]; + m->stat.meaninertia += d->M[m->M_rowadr[i] + m->M_rownnz[i] - 1]; } m->stat.meaninertia /= m->nv; } diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index b4a74b1a..0c79a27f 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -140,6 +140,23 @@ void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc, } +// convert lower-triangular symmetric CSR matrix to full dense matrix +void mju_sym2dense(mjtNum* res, const mjtNum* mat, int n, + const int* rownnz, const int* rowadr, const int* colind) { + mju_zero(res, n*n); + for (int i = 0; i < n; i++) { + int adr = rowadr[i]; + for (int j = 0; j < rownnz[i]; j++) { + int col = colind[adr+j]; + if (col <= i) { + res[i*n+col] = mat[adr+j]; + res[col*n+i] = mat[adr+j]; + } + } + } +} + + // res[row, :] = mat[row, :] void mju_copySparse(mjtNum* res, const mjtNum* mat, const int* rownnz, const int* rowadr, const int* row, int nrow) { diff --git a/src/engine/engine_util_sparse.h b/src/engine/engine_util_sparse.h index 3d5badaa..038ac5ce 100644 --- a/src/engine/engine_util_sparse.h +++ b/src/engine/engine_util_sparse.h @@ -46,6 +46,10 @@ MJAPI int mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc, MJAPI void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc, const int* rownnz, const int* rowadr, const int* colind); +// convert lower-triangular symmetric CSR matrix to full dense matrix +MJAPI void mju_sym2dense(mjtNum* res, const mjtNum* mat, int n, + const int* rownnz, const int* rowadr, const int* colind); + // res[row, :] = mat[row, :] void mju_copySparse(mjtNum* res, const mjtNum* mat, const int* rownnz, const int* rowadr, const int* row, int nrow); diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index f52d374f..30bb5dfe 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -548,7 +548,8 @@ TEST_F(InertiaTest, mulM) { // dense M matrix vector Mdense(nv*nv); - mj_fullM(model, Mdense.data(), data->qM); + mju_sym2dense(Mdense.data(), data->M, nv, + model->M_rownnz, model->M_rowadr, model->M_colind); // arbitrary RHS vector vector vec(nv); @@ -611,9 +612,9 @@ TEST_F(InertiaTest, FullM) { mjData* d = mj_makeData(m); mj_forward(m, d); - // get dense mass matrix from qM using mj_fullM + // get dense mass matrix from M using mju_sym2dense vector M(nv * nv); - mj_fullM(m, M.data(), d->qM); + mju_sym2dense(M.data(), d->M, nv, m->M_rownnz, m->M_rowadr, m->M_colind); // get dense mass matrix from M using mju_sparse2dense vector M_CSR(nv * nv); diff --git a/test/engine/engine_util_sparse_test.cc b/test/engine/engine_util_sparse_test.cc index 67eadbbc..4e0dd3de 100644 --- a/test/engine/engine_util_sparse_test.cc +++ b/test/engine/engine_util_sparse_test.cc @@ -361,6 +361,42 @@ TEST_F(EngineUtilSparseTest, MjuCompressSparse) { EXPECT_EQ(AsVector(dense, 6), AsVector(dense_expected_minval1, 6)); } +TEST_F(EngineUtilSparseTest, MjuSym2Dense) { + // lower-triangular CSR for a 3x3 symmetric matrix: + // 1 2 0 + // 2 3 4 + // 0 4 5 + // stored as lower triangle: + // row 0: [1] (col 0) + // row 1: [2, 3] (cols 0, 1) + // row 2: [4, 5] (cols 1, 2) + mjtNum mat[] = {1, 2, 3, 4, 5}; + int rownnz[] = {1, 2, 2}; + int rowadr[] = {0, 1, 3}; + int colind[] = {0, 0, 1, 1, 2}; + + mjtNum dense[9]; + mju_sym2dense(dense, mat, 3, rownnz, rowadr, colind); + + mjtNum expected[] = {1, 2, 0, 2, 3, 4, 0, 4, 5}; + EXPECT_EQ(AsVector(dense, 9), AsVector(expected, 9)); +} + +TEST_F(EngineUtilSparseTest, MjuSym2DenseWithUpper) { + mjtNum mat[] = {1, 999, 2, 3, 4, 5}; + int rownnz[] = {2, 2, 2}; + int rowadr[] = {0, 2, 4}; + int colind[] = {0, 1, 0, 1, 1, 2}; + + mjtNum dense[9]; + mju_sym2dense(dense, mat, 3, rownnz, rowadr, colind); + + mjtNum expected[] = {1, 2, 0, + 2, 3, 4, + 0, 4, 5}; + EXPECT_EQ(AsVector(dense, 9), AsVector(expected, 9)); +} + // helper: run split-col approach and return dense result static void SqrMatTDSplitCol( std::vector& dense_result, int nr, int nc, diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 211665e7..c7cbbe66 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -7514,6 +7514,9 @@ public static unsafe extern int mju_dense2sparse(double* res, double* mat, int n [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mju_sparse2dense(double* res, double* mat, int nr, int nc, int* rownnz, int* rowadr, int* colind); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern void mju_sym2dense(double* res, double* mat, int n, int* rownnz, int* rowadr, int* colind); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mju_rotVecQuat(double* res, double* vec, double* quat); diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 2dd89505..eddd2024 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -10638,6 +10638,15 @@ mjtNum mju_sum_wrapper(const NumberArray& vec, int n) { return mju_sum(vec_.data(), n); } +void mju_sym2dense_wrapper(const val& res, const NumberArray& mat, int n, const NumberArray& rownnz, const NumberArray& rowadr, const NumberArray& colind) { + UNPACK_VALUE(mjtNum, res); + UNPACK_ARRAY(mjtNum, mat); + UNPACK_ARRAY(int, rownnz); + UNPACK_ARRAY(int, rowadr); + UNPACK_ARRAY(int, colind); + mju_sym2dense(res_.data(), mat_.data(), n, rownnz_.data(), rowadr_.data(), colind_.data()); +} + void mju_symmetrize_wrapper(const val& res, const NumberArray& mat, int n) { UNPACK_VALUE(mjtNum, res); UNPACK_ARRAY(mjtNum, mat); @@ -13461,6 +13470,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { function("mju_subFrom3", &mju_subFrom3_wrapper); function("mju_subQuat", &mju_subQuat_wrapper); function("mju_sum", &mju_sum_wrapper); + function("mju_sym2dense", &mju_sym2dense_wrapper); function("mju_symmetrize", &mju_symmetrize_wrapper); function("mju_transformSpatial", &mju_transformSpatial_wrapper); function("mju_transpose", &mju_transpose_wrapper); From a5de6506eb36f0f737f87f2104e0101ec3a80695 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 5 May 2026 02:37:54 -0700 Subject: [PATCH 184/251] Add backlinks from tech note to dcmotor docs. PiperOrigin-RevId: 910543352 Change-Id: I171c34a793f23b09feeb46ab7b02ebe7900902ba --- doc/_static/dcmotor.pdf | Bin 599178 -> 599453 bytes doc/dcmotor/dcmotor.tex | 4 ++-- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/_static/dcmotor.pdf b/doc/_static/dcmotor.pdf index cfee21c4ac9a76c928a047de74136c889e8645c5..be5c53de851ed4d10787c7db863b97a6499aef41 100644 GIT binary patch delta 30981 zcmX`SV{j!*v^5;tp4hfEu_v}|+sTRT6Wg|J+cqY~#LoNN@78y#R&_P1_ph$rtM}>^ zH9?*;MNUnjC?>(c!pH?jF~2;r4#&dEP0UQ}U~CP?&kx5WYi4iZYDvt&$<3O!01r-` zs7Qg_@ppk#YHG7?Z<-ebLz6f$gomha&#$GclKbRj&ERcst!v1hh^daYbkeM=_x)|> zg(`6y7tp2G#!Myq?6$Srr*BtBtIgQdWUgKiT2o^Thh0&=(o)=7UL1ImZ1C4WBP^{w zi1?)U%WcK~el3prb%$6fr_a9i51_%eYsB9T5hnAs)?(bCE8rK6WrHO9-J#_0NV&+G zI(k%lcO3-q#QDH{OoOr=t$(#)jBJA*X^c|n8E`z{=?D3caKED$eN}4qf!eR$Wf$HS za4QhQwDondMknTep0~X(aR9vhYUqkC3RKrDmLzsOfA}8E$mYp!Qee5`OWp9Y+{IQ{QHdphy8XVcWz0*h?yS|uzD;Oe> z_4H}uBTO(#_F6pxj-ozn?9scAkLB{?0L}$G!rg&K$2U+TAtP!KMfP|acF^}lG{V~u zhhai;YXn>exW@xdgnOE9{s(W6 zO^91>R&MPN%vsO#H$#@_{x8J#bnD;18Wj`KARS2?;ltIHj2ZBekI_H7r5!u10BDrH zC>yQbUJ)-y74#W(b5S=2({;r%*N6=!4a3sSIbo=;>6KEw{oQR2;+>gI!3U8J1CF&z z>cR2?6No;qm#ZT--084_7ASX_QEOqdhIM|QA2yoQJA`(``-*4YbOVbRt#gDedp}G21rW=^$-LT_!9?Z( zPm*mhutVc7+^SDpe2DL=BY#Ea2?z!Q`G;3TU$q)fb%e+0+7FL zmpNM@BNqW}c{}P6`GicJ?hVnDf47r}V6flResM;H0a#$$0ZAj?Cqm?Pu#{^91Sn|OelZx+ZgTfsn3?)?JJ>Jdzbx90>P|d&bItZ z6ho-Ne{);rP1>#oh9S`EfGexJjwU%vI}JT_3*c&=}* ze<|GiX;BK@M6A2WM%Az^bboP%!|=iLI;26{Rt`L2v_b)_08^r0rugrXJ@SqNRBJPv zTfe&Bgvc{?@{%~0WyNeLM|OgGNV13I zqtu_1M}RVM2LyU5DSb1r4fu?EazIdEs`28UOjIG*4)mQEgr4zhQZT|nX3oW`2x;T7 z|3Sgl$w)6TwE>&HmSX{X|3eAoZ$M*{VS;0aKczr5_?T&F2>z-}H_XnN(KxuMrJhdz zTXv1u4)%BWG(>kQd4WYr$(YFN@p(twWB&eyHasLBtQH9@dG}5YK+n#h=d&GR6B1Y0 z%T1Q;qraF>-$P%z6}SLJgPNRGVUnM;Hp$BT(Wp)p7Z?wOhsFc!!Y8aPRHQ%RnkyfG zZXx6*@XM$KY!L4%tR|n=mOIZURA5<+izJeBqVa<27&*;(&3LUp;Iy#GK{bkEBoJFvjxuXZ5so;#i1 zu9;Gz)$SJ7vn}v5ZVb24ha_anQ1BfK`PYWb4Z;s);E$oJ3W6lHZ5<4)rQ6C7?hDiG>gx=tunvW>`2}s%EGSguCIdTjtrPkK z&yBB2_)e#$Jv+*aMUE^$KIMxCBieBIw@xKXCk%Z{+9C?U0k7p{`E)V7gvl+O$CNLvzhAc*#bMjZ|Ccl7n9&wT;?$Kkfv zgxd>`bmdoLNBgm!o~b+5kFBqg0P;ZptRzU4VwHP}!K|MgC-xs@sV5%8pb|@1njft< z3oV~fv=%avZ~3YUpqlffo)V+AXbPR5iipW*_%0lm^MTA$y+o5cLSVrG3Zy;nqwvk- zC3#TmmOJ2sryChiTD$Y(O`&$q8q3ruCbfSl=t2pHPj~VN+P$c4aEJ+Y*g`>>GNPs) zq{wT2lt$k*Ak!GmLzCsC%oZ52wWFUz6o{n3kx1Y6f$mx^<7=LZcOtu)fD;Jxjm$$` z-`020Vk1RQ=F)N?S4qCSnlKiItjTB&HMZOg(*3FDlXU@Xp1@^WpFY@o+%MpN@&o{v zgC=mLoGdWH!dPDv&b&p1sSrCms;Y+y(dLXtI?8pF#@&~T&$uX?^ZuM|rYkZNP6|;O z-?de<;As>B8I<==0uqZne{+G3Czcku9{-oUcg)Yi$|cjG7izTZaDGdAWZTHTXw=`# z5-{rjpbi6U^2a0o3$@0r5Pd$wBw1;e3KQtJh1U;9B`!r6rF8@hGaBFTBIwMO8f zw>~EB{D$)L9E=r}e38F&;H=uyl?gfeR!xz<=PsJ;S97*=kq~&Q04H`TsO73TqY6wV z`gwaFQJ8xWMx2qnm{nho?!~|Cl19Kz+x|=dZfJQia53o@PK*h`Fb4@uV=Eb!lF-DS zt}_G9dOqIIXscbh4Qi8}N)ndVk+rYb7#jedYqp8UYbt_S8pNu5AHB#IK`ho~WD)zT*% z%;SU5@e;ONJAKjhy@EvR)s(MofqA)kA!WASvM#@vG%e=um3nWFzD;{*`dhH5MD1D$ zE#zDu8Q|7fKciL{tzuV=Zq^js9|`rx0rArU350s!ky&$WcFLaJZ*tOPmg02sYd*lT z>I6yV?Y!<0%lOU(mJb&>-kN9^^I?#GfI-{LqCv+j!Nimjc-XQ1y?f82gWC*=Pr1`} zxI6Kk+bsEk12Z_r_B5eV(mop2OoPjnG>A6UZS?m(X|X$>Yr%%N3Q0dXMarMHOWy1W;e>1N&cX~|1Tb8L zfoT#^0mPKPHcyY$ZR*VBbfj4-{2WQ6+LDOWZxp6HyLvVb9MASm#w|+#gy9$!W@LHv z)PbskR`vF@R)+6+zuU{dUKfPZb@sktaIR3!#k9bo4AV0*M+%0$J&P$j6iR@{(RCSX zEhvM=#Yh;@K04puZ>n;E9Cd6jF`(GLn z&e}s+;Z<>qeG;Fj@gkKgzCr^z`AH*Xenvv7FFnZJ)}XMUOGaeKGDP@6A6F5;3ehFx zC%z6l3+D0fanA8q0QmDw2eGygm^o@FJll(;!A;LWOH=yC4-7BJ-!OnQLn6`LZQGnK zG~}@^vuwlB23lmKid;s>r!^a4Z1Cmb##W()mBLlt7?vK9`S-_cw{nR!kaMOY{yG(%{BI)ePJDODXL?9xBz>8>6(cNfDUiiYzQ9JXvf z45?iVzmi26^&(#U!~{V5D`-W;*%rQ0S3sy3s^h#w8Rw$$Dhhu7zbw%uKPOA33Lz8a z=bF8>5>p&V6R8%|E5V!rkLAmz4ir*VSjs{;DHj(hT3TD=k9B0$ZUOs==@74JnMNlp zAf122_m%B(9GkGP%9Br9yos!Ys`Orz)E2j%_jrltiY(e9>I1knQtu34?gw0e!=gE& z>rUhgkm+T~*ElimDE!_ItH2cQ%KuzOL0}=46125GM-WTr-W|4btARN2*Pb1aSjwkQ zEB*Zs;dOC+7lG2wzOz9@nD);dNHW_qN`*)?_KblsBs2|BlO2R^3Nr$_ttXkHOb0b8 z_;QrA0zKEPs~A+n1MRSuK)7l?dbf4HFy%P>GiBQ;zAKzch;X0&o|$v_l*HS8Hc4Vsdu#lY z_t;XUq|xo*nnK!%^&*GA`vHz(E8*a;({T$CmvvX{;2ZBn%;^+gK_GW}`x&mWzF=Y@ z&t}mX8}ae2{4c=n^&7G)s+QJF{H>KIuG&S3Jlffcv<&dY~_> zM6=aoai;)U6LNyG=hv~aY<8MaKpzC#e4326XWih(Hwc68r>?w3_UXiC@OKBaDd_;X zbFOCkAX=M`+H9ZDHpNboUbRAhm`gpvJeuEfxn2Aox zj#OcE$n@V4hg0Xht!*^powUj7j9pS*F-?^AMvJI{vTou@TY}ZojG!h5U??Qn63eO} z0`Jf<0q$WRgp>iN@l~f#KNQj)FmVlV2VP#1FTN_={QX#nH zoC+l9a+pWH=;Ebw%4X=s%D~!zq*ysfHLRWL#i28AemHr;kSNx`p*V`8z^=oMK3&o` z77a6BCxFibkwH&Lo2=6o=C}XC<4djpH3-5J_Hn-?`zXA4*;EeCSSK#6I5-VG!KNZzJDlM4Y3^Ztw5hC*n#WviJ{Lc-(SDXr!i(Ke$MYY&*UL9f!#)?FRWlw2OJBTn&Vg;EQ z%yDp4h)kLUh5SZH=l$6LhdA2tI2?L;3Kzh}sa020i8r>3<#|y2XYGH{*mcMNd0_G| z@HiS|$)}UY>CnhlcUkPhH^2)?jB&r;`M7Xk9q-&B(#)hQhBY7e?HCk8_wDAIgj_}U z+}|k$n#`kxVI$zOXi>PH2SlNQgA^YkcCT!7%EprBd%ovC^Wz;!ynUZ7mm_^^V>^;{ zn3X6?9fG4!S?>U~SSDdOGka4YqzrNzXfP;xQ!$nk8z@g&Vh;>@6OD{FJ}B4!S6Bti zXz4g?jbr++)#2nbO^AlM^kIyG$#=_xlWUUAGPcUagp#tg;%d3lRQ~(^@DIn5B+NoH z(_w&H$5-a(KjmBFVKYfL473^`jZTsd6&x!zpVXuzPyr3L*xXlVCLmWy`sMI1SBpks zk`<41JY5Bc^kI{wCjAA69saS#WWX43Ckq!F4=E9kx@ZbBjT512BzE7dJPi?(&y^?* z&WM#jpP(7b0%Hz32MRA$9Bc{gPZ&h-3R#x{-bej79%z6N)&X*ib_*Ks`T$23AzYc< zfn2QfG}<`j8Tghge$y=dS9h7-QNdm@@qE0Ym}C z60d|PQPS4+5k3SKv?O^9u>uyENA%zT(&d$qD~U&86ciqN69#4)9@IGh1uWl?fhh=A z`gIOiN&K=H$b^562@8XP5N6MBPyG%)!gQ#n%&%Dg=ko$l3z6hDw#)#~Po>Gj%WXJ9 zL$|A*N2`wCfF13;C|kvo#Z82z`vjCu#W0$oEgpZ?p@l?~SID|9J#JtdLBtX*rkzH*uEx`YuR-7LXtxz# z;2v@J*a7IEmo<>z0OJ6TzUhuyH#{n~-@;Q6LV^%?G?5IzmAYI87W7SvwU>#^n)=J( zyU*HT$$*bgMoBxdYcOfMfaS_IsWkN~W9+ajS)J!IO)AVx(=gs1zZ7r{{x{Bn&(ka8Ry8-}Bb5VcGilk6DwJdU4 zFG-(iwMu5CS6QpbJ#9v>ags`zuH%v_(>`Pznpo1b=lW)XKQ;!v@2}dnIn`LN**9M1 zPAOJ5Z7*ea2`TA@t%qV7&Do}+hkt_{^}=l6rc};dm;FYV(tYpzYbMio?x795xd3Iq zK(*{GDW3p)kkbc{U;4P?g4E{?@)Hm98%*h04}nDG26-ll+?)kUSMjf|wpgczOZ-kw zOW4MCqk0SB^|Ni+60i35=JAZv>>1d*jJ)G@%6VQ(UfLcPB$Pe&7&OP0`c2}Q{wKGT z^p$L^#on&YBhi#=-Ist*ZkH?>`SE6|p=_DAttR#(@zD>Er})W-9EYHq;Q{}UOEGHf zQn9vWzSSa(@M9n|iupI#8yjG#f3>_RUKdy(idR zH|0Pv%|`QP=ECtY<+(^dCa2cNo2RRWV;}-7qcPzqlS4WMTT@~yH;gr#l`Jp+$y3z= zHDXAb$&4({Xin52upEkz+|+l8)1k@W%(a1R6m=I|pj?6~PsL6<3E~{tblpP6V)CrF zq22{NZ|v?@OJSl!t^5n zg$I;eh!~=+x#<`ad7OoTg}`zNzTy)btjun6>;t?IUl-jDtT1T5bSHBga(wMKSfJsi z-(y&od)bDIFka+<)3q^EoWoMInVz#}xwr}78G00EDEMWXJ36sdB~b^p$YYylFS^uX zq@IW#W>d(k2Es#$#e^PPo-7?Q<=~o7`D4f^VAZsG=7@P4z$#U1sEJIXs_MAy>g zE#L>*?ikvP=h|>Ct!ZOJf)~S1mVffhh*xv^s(+5xbEVUW7qdOyIboJsHo{)0HcAS> z9Is7HfNd@GE7Ue3OkP=WDi)V=_1X)ZvIVCV{ZGx?bp~`<&o{|RxjqR++mcn_^S|d1 zPbts*lc-;8t`qiD7#aCO(UpKb*ha$N{(8{xI zt2l9Cq4?l{h)c?%0ivJ1F9#r>6Y}KOI9u|F$kmfmg z-K>Y`D&th}WIwzN&U@=lmP+D?3Po(=nwENj-6$;GwWgensmqHL*VA%qe?Cd8tNzit ztf_jHP|2?uUi%EGEp^s=k%c`MyTX-)(8xnp)u5G9w)Bkh{bz-16dHGhOTdF|sE(7Q zP(OC(!PoMsMr=G?%T{HyWJ+6EtW9l%%j$kPe|;Tmug*@>)7j2Cl_I<7Mc8&|I&9gA z8r}~ms4;H({epf}Pj0B{V59u7>Z-j<9Ls7FxeRy#*elgeLe(!fj?3tR>u+;xeSsSy zz-xu^Jwc_d*bf2X{bS*E@wN}Oj?&)RsydYLVfQtnz+bvNmBR8y+##1A)9y$E=NdO{ zb;!}_z1j@+`Cnd%;?vy~?Eji=foZ+iwS3@$-d7uY&?i_32@GM%`g&+M-01Ufr*&DI z^7&W-*jMaAqtQbDMPr4|Zhp}2w%28uwO4}`5nBz$USB&ghekJhg~NBGz<_H&iHYYP zZ1mQE4d=P!+|oLu*VW-O_s>+~9#Lm&PRb&05ICe}oh<9nx`?KN$#Va(_xJg_YO~<) zt-uUco0cW{#q?;~;QxG(=Gz)#5y0IPn&Wp5AX{T2A(#>tdDyUiUTXS@M5FVDxHROM zC{{6QsjG!>REV}ceIXhLpKHv!_zC_~;ga=Lv>7=5+coJ=kp;n09P)xnXqE)>^Ad>n z;?n|K9A@{KriI4srgKvSLP9k(dkhrfP++W{z2i|-we{*#sV>RQ#j)Lq@^c``Epm(- zKnwz>&~Q==&P%Rg-QNA08;-Q_Zq^19dA(05XmhH`9owE{Nh!kA;b_OW^q}@>drEE~ zd#TB5Hj4COB-In|8SVQP5o~0jro?Elo*z3FcGGVwK0d>sFBX=MX zjWI?lk3ATNtXRCKPE$7w1Sf1DV74S`NDjH$OdCe?b0JfSnkFA1?+Hpg-vAW_KJ^dW zzTq4c$e6o0%v7}#Z1h^ziNBM?Brn#FyBb5kI(=u4mj7<@rMFYu2p|I9wj#$7kUAkE zki#U&Q@1bcl;}vfo?!la#H&<=%qibsZZnIHxwwk$(nlex-kjH?-9s5G$E|>SIuLmU z55wvS@iW3U$}voWbbh711ond zr_qVt#!XgkmUM%2nTdb-+LP7}fS>fQZoap?m%HA#e84^$BS%4j4Oq32#0C=tuEe=( zkAIeAYHPItqs~PO;Q;!T;f%<_I)$siNG-UO5AygJ@ed&CB|&GD0=bE@_aM3DjS}gx z8iew*&Ce~2L4QQ90-7SJnLcdLOLwmWA1e=T&>FA?n7_bhFM_(| znC$;{LtpcT*WQ7j)e1~V7E0gtNinEol;{r{#nBY}X3vjW&$le{Nj6Bx#iX}byipk1 zPGpW2K@sXBB@WubhJ<9A8tevGbo!_Vjd@YKfLF>|0Q-PLy) z1nX;BMnd6{A!QPnV|B&L>$>DvX4*=nhhKE(#*UYmVf%@^95mth5zBb4uwp5as5)_n z>v$L>H;1=)G8WdO9p7XE!(_h#xh;Nt>1r^kUxNfGAQLH(msE#k;bDiB!;AkUrf?C= z8O=`qlkEeU#h~>8ynJ!(h4O{l&hC~?F_XdQ=Ij_R$6|lUE+}aWOt;W37VjyB9q7rv zlc>sm8ezpe*uK<*@jSAP2|XlOTsrx>!9@1PDT%i954=zWZQ+|xFd4D%>=%d^JiYj4 zqAXu9&OjjoK;78j-)K=eK-n~@c=3o z`PWYLFT7s_;EY);gu@XBORD5|dugSJfLnvz;mr5;Yq2d-(UEwlZSXfry&6H|(Bm-4 zMseH(WghR74vkw9?1-?#1bzkPFXx&@sts1KVIQ;*c$7lOL>Zq@aMGsH<0pRt$Cz$s zIPd!EA^@cQr&5@3-yqELU?^ebqyTTeic$lS#6T%EAf4d(_#tJk-`m@y)*TwhNP@8L z5;4P2ecSvJ(6?p$sZg_B=Ia3}W@-&D`(@0Z7tIKHhN$S9&r|l$fscU99bhG}7Q{n~ zV4b^#luAE;;$+kz=%-&q<$RRW$WYHGxedj$4RqtJ)$N=&CEfNtIEU1qst7#0bKI{6 z-=|pvMC+;uS5J_7JIUQzCF>S6y@2Nu)J_Flbb-g87HIwVqKtHWe;|1Xgj{A}yLUIO z)HU5hB41oOCJw%*4i={h1~=`-TO}D{5;7-$%Ga!2YCc)$efgKV4^vLg?y2#u@AC}9 zN`sz(pE}Vlnrn}xrea^}u6H^OGODnD?9>JV;D#9m9b;X3+bPQhHTRx@r~i%|t`0_0ACB!OhrJ9-m z>$L;lAIha3<9A$hnO{(D20C?g?7=9OgS)m_qK-VetlctoYJ7M2zWsq8kXau*vWkRy zP+%3U+GA)oN5La+jZr_rB)2kAXtqsU&buD;fmxir`TWMWhL1kZRC-LKzfhhseFLWo z^OXN!UD=g24?qs`l<5&mb_o$ec(cF&4k)X$3(?!ft2(t=i>(5$VkpdDvrzRg&&8-% z9Gh+{U#U(g`OnPney&JMusLX}tuv4S9Yb0n z5L>B`yPSw8Z2+X<3DRf*tTw=7HO*;8%X|A)B8@HQH>VdxwLjhq<>q|=(n$DPJZuan zbYF!C_^c7h^FNIQ)Y?bA0gcU5NKx{p#B6gO>>b?P3$zq#EG=)wL6{`7erR-yy*w}q z2g5{4j;Ep1IrKIS4w=@Xm$piVII@f~Yz!k(A}m_T=13?Pu|95hla)#;Fs%K}oC+*! zKu|FwP|7yhtqPDqeS9Inc5;_D>VpR~)$t(=SOX6tNi&M1p=(wS7FCX@K8m-G^kM7?=D1c#pfIr zNIven5j1>0#q_p)X-45neXJ=KGtY73is3Km0O>u>MLgGE)Yeq^E{+R$AqOMi?U8B^ z+%LJjl`^S(Y4%>_win!t{mHaG#W=0BA(x=VBi8CLeIwLp{Ai{W#3B}7KX?Or%2k< zKYTEk?nB9!-z1E!R#j%1l%am7>q8)aM+3)Dt-owFr~B~%n^u(agdDBO*rr29^QN5m zCJS;akR?>-+wuUZ3Zpq;s>&dzqv+Yb-zKwyv4W0Oc)xGM=6sHVDbHxH8jKtm2mSOzqaTlyNAC4|RY={4s@j5+q)QaI$7; zUIfaL^ZC5wxCINBkqi~qr^wO6HzOqhNlBR8vByvMCI|!iEM5M zeqqBS{(JMJDf(vwJMOaK9{Q=!o+{c(Q8G#sr7vXw=%hqKs-3<#gpfPVRVXQM#4NwD!+RI z^j~Mu8S8B!5D2I_4b(`?wGxwufRQvOVbHjsDb9QLMlhUgMq-g|ckC7|MHlwapykN? zRR2D)o<(EBBm~h$XPp782kfpy^Lw47usfo1%0W61>Z+pioYrPXy?hG}?+w$(6_GXG zv4@d~nDic%4(44IWQfZJE;EvaFDG^YKAJww7Bl=>bQb^c8b8u5CKH`JsJJgl6@AK9 zfv2?SEB8L_&?>{HzxtBFgpWA}np)6)if=*QrfJth9hvqBPejPEoAvt?#GDu2pd9W` zjkwap?PMFuC9(gWDs=_Bi8;|=)b_vO#|(jCa`fl3xy>$im8mF#{0_5v2E_(|#U#+e zcFmOrA5C)2Ur=CB0nTn`E|@_=dn~btM5z-uI0IWt*#)kK(Ic`vBXBfj&S9RM(sIQJ zCE&>YT87S!7jp%i$V!DT*)QghcJ^ynEn- z-@YZv`wfJS8rXD^{BJ@V01mwTcj&&eX{X=y*x@1B{)ghY(bS8@>@GanYuXj@Y=os`Gc-zB@NoSZ+S=R zIhIjZ*l1_gU)8n-1`8$Fh|5=s#{jG-tiFOoYc*jLOE|ER1A1)>rh0~%Fd|@2z8`@B z$`h;uo!v7nH>rz$K)f0NrV!b(ddYqA={)G~y@(a4BXBW@QHecc*C#3r`{xJjRw`hm zDZgm0T{4&BeRrW5oU|B0QsEtG{GgnaFs8kCu+B6e-l^fo$P_8_38|VvoddCzeT35A zcX4@NcGw2RPy|FYFB8+^eHj6hDHQwnj-8KaT!{m1W2iK@fUZh7e&Gy?5 zf}#GCMxQkwpV!MBEQnTOOsUWMXa~5L>l%Y_k-M^w$_j~MxhMWG;OcZeF+Tfq23oa} zyYGjSn%xcWFK#m@q|;Z1)jF2@49M*t*a+QN~Y3sk#zv5H@@-3`ny1x-1B%MdX(xdGRH z&zrfTmpQmTkdlVVt*=I0hh#5tu1M_txgS_P{9ya8p*Htcmf<2k<#CsBp6Ym=3w-BJ zyGbt3M+t^Yw-({&`y5o&vdHC_Dr1P1Ba1+ZObY~Q;S>|VccJd;iFr0Sgn!RyWf}8@ zXII2B3LZ$@AP!}hMBqAeV^j_)qkB|LE~?-fCTr<#iz!@8%W%J%2jcs`^`u$1R{>S~ zMy{?#-4GqYWUe&o_t&r_$X=(DuTbQp&Q6?*3xmNfw^!FR!C@>vMj5=fTu)-v+wQce z8g@^g9Re0W^v{$0AD+8cU&bjFpT{og*Ig7z!I4hib;$Z}KBP$psG6s)zXzUIB-y(# zts*6uN8IL-0_B^YHI<*B1+OwYh%j_oSMht*gIg>|u1OK+6l4+vsgZR_6fMs?$pT~X zG(v`x1QN4Lc51}k7su@Kq(lVF@U(@OhP-Oqy|sq`!67IgI1&cdd_n17AEgPf3Xq+_ zc5vopYP^w_-nlOz^cEeN`~qYdD{=3#!p^1`k69;$KhmI$wU7e18a>IMV;5MqEKfO< z^~k#)mjqj-xQry79Pj+@QLyVsU|r3_F$96MKGo`5;yeD|XXp0%?@t}@4mkkzy}9*C$t{O0WCck$(S=-8`1Q8&Kj*BlO*KBZfU z)V_L2ln;pHmw#p+uKa^eJjm~OOx>NJ4xZTGJGNVnN?*I7PraGIkEQYNfM`MLYbKW2 zQORI*#pASFLtK*`;*@}w!wG%gcDl7423`OlGB`;6O54>F%Q+pZE9>uSKu7r|>TOv# z+Qx*yRq&&yA?@lZsR$I9L++zm=vKVO<||?O9n&bwYs*j~mNW_5`?frcB?_3HDGnv$ zrnwupv}EOrW9uh*R#<%820Q69WoK9Ip^gfjX!JuG4?QR{*Ii#st&^g7o{l2s_CpoP zB|qhFcF7;)Z9l%O|B<*|T%FB~?Ea_sKjtA;E}o|Ljvef@z})|Y?(ziyHYj%*6CNx+ zFv=VrnTz%RtVOJx%$)zLfcI$IIpB4me%BfZ=NsGzB>!>d_N9pdcTRiv?k7MQrg5}0 z#Axics8T9c)owfQyJcIR{I{un&AUsOkiNa6<;;yQ@GxC%`#4FRC9UPK;4OZ<(0I>M zghmd!#76}?(%T1=5b94|>TQKGT2kpWMOO{2al6vN#350ndx7eGV(Ycoko%$3xh z+$^VQlCK$5`4{r|D6be1Bs6EO87 zdVMK!g6@O{%2TOCH-D&+aj3d=>Lf^>@DVa`5>zzintVbb(Y6v)|JiDs9gPu!hP*q{ zBYqLEpwdTBjORqiC-6x_s#Iq!3=m<*z^OwGcMljFW0MRuG658!ZAf0hD-y(^a1EWS zY3rjn2YBb1lS17LSI@KvDKN!7uCk((MkDV!A)wtF+HM2= z)gzs@2BK)9Ac27%#wkdQnaZh1`g6y_Wb*@t=_Gg?3e#DLT^WC3kXn(C%yq;LmesQn zYN4sZTNj}^5&>$Io{dAP=0SRTt!Ji6l$BRK*&t=kgA3ezS!%c69~J5=RZrea)0NR( zRi(N=-MWBuwLOQ{wWsHv0VDRb6@$vWN}}B@XO^nn)o6*Hw$Te?zkA;Cb7m!<4QmBl z^YCptH}}#?c(t!~uQsI>n6PH>~t-JX7#nInReTFrz_tv-e_n{Tve?}a8JN{cu0jD~^q%a_; zQuQpkCII}>*bf9OCrQwHW@jhmvuY}-d8JsdPCTy$KsLSO2q?G<3uw9}W7BIPqr5J~)z( z4}$fbBtNBv?FQ3Z!xf-b3bMm+6!n+`oKHXsahFlI3$l&wSt zH0wu=MTe@JKVPt~xCo!IZt8huH&~8(zU|K7lIzi+ZQwX|J@JWO?Kq`W5%o^eo(v2=( z?+pe$Bc^-|MN&9Ees}eR?))bVejsFvgk9(+petk}V9Ag^{Gt#5f6k2ixBh?`mpTg3 zXUA~#V#d9xz1+#)sB7)C(pv*yzift$2H2f;xcC**V}Ie+1U{d9Hl=L$_SA z{a(_mKijwGs}4!a-Nd-CG8D@uv_In;O%hUeJp#Xe#oUp=sU*$4)!SlsxtY4T+Q!gk zXTA0Hk+u|&5ewqO^SNPVl5{6cAbb+UlG%6}-gNnPOw^ zZ!Vp-jp_uy@s}Fuv`jVPfSG>bFdrob=;vE1@)*G@J;@cu>vqyzEFW+#%u*bA=Zu^D zz=l8)yhJ}#@%w+U;+vN*U{PWpC0*`5CJ#o)zW_ zY%3}?|E{v!ahVlM+k394riR>F`GjT$GWrkbafsD~}HV>OcBpURc` zoe6jJn4Sk8+yL|aHSN)nHHTgGhIwzA>l_~Bt{e<`{+JIdkeC_|kF+Yx_xEmi%=7}q zDQsL-vNjJO`x_0O2Rn~230)s$Y#j9W6%3D2I?cRQhYu8bVrUW(jjX(j^HB&m+Le@h zHuEV23_eezqE#cA+p{ul8Ml0Tx2o?Dx6vs&+G`>&nc!$aM(ZTxx99;*-k-t8PvY|n z3?uE;@-U;hrRXB*i)fadZL5LFM9Rw5BvvTX;KPaXAbp6MQ z^S4P)@+JLVUS+ETY3*KVX!5Or_N9Wvvg(};V+1I<`ojb?(i0%nBP;tTwJo5`)LEVi zcD^6yZ9knnSITb>7Qk(XA!uYYY4fHGPo9>C7+@(0?pqHtN0X~=EQ2gv;CRu>Z(c$# z=3Dun!MFez0s*(giblhE&$CyUQg?HgweSDK6m>kKPl8XJy2sv6EM8D(h+mPA*T^qX z((kJn0m>G*&4!tICk33KpaDD>?kKHuwj%|ez+OVezJ0+*oK%suPD2;j73_iRCgMVME~qd9a7R3 zK6x9DoV`otb2t&>Z5l)0YS*07H*}^L2_rCY0UeYztzhjx!YTeBjRDHp77q>jhV~!v z6qbd+Y|F(3HL?6}(4Yp1(S{WaicAdlpR4`9CGi^x0-qu0`=F0@e!u9`iSU40OX3qa3v0QB2EdNVluXXKQ@Y@o*iL2_Ngs}CrpQp1>;V!* z3$dfs47GTCI=&X&DHdn8GWRh#T>7~c@cMf$*wKj==Z(=hcw@EXR0W>#nQT=}mKHv46O88&r?JJw=5~f9}jkQgi8o$Ea^rd2z4;uv?)g5~< z@XqU=lqz80gPv7*9YpJs610W64`Rp*)4rt*cFr&WTD-kO}S(+}+(ZH~|jt_x|61 zan8lLtF^mpP3@We^z?qJYV}MejePU$4Xt(jtWTZzYdDl3rZ!fHeHd`vK-ofGN;!2r zvYv#XAJJImj5~>k61HeA3@kM(b@>q# z9!4MMCMqWpx%UNM0U*c>lBbVtlM8hnM_eOlh?8r{tsZS3w^JAvF*C#?LQsj?PlXcz zyeUtGDXHOAvP$Jd7wgHZ_tQfi6+{fn;rvaP)$5LSH2r( zS3md8E!}y$EhGGqzkfVHxb}Km|IK_SlW}2Ao_foBFSF1z5%8;-?WEJ>`y}d@PsV)y zaslblO5^t~vmWYC%rnVtH%wt5BUve5Urad6e$iot>!PziAl>KNS7ZNC6#!pIwfL4) z?Q!p3J^mQJ@}acb`eJuh@+!R>!W@~3WA*$0}@cTrA)`O<6)-f+r<8Wt@WPwgR zm7n-02}AV@skj>VA7MhY!@s^RCQ)5i*cN(Hzh{C(p6f9(*nsoW;{)e<4*6{1@_( z+|+)}lNT%qF`1(Z@4>``cI3%;{qW43XRGL^Gsf^wpthC3TmRPO??Rz*p<7e??#R&r z#X=1PqFsKw^#u#~O}PC^Te0Va=8|*xYy6hnORF=QR1pdG3^TDrA9ik%=ioB{zvcdF zV$Gp&-%5|_^C;Sp9;sx=$C@QsP1}luRV+v>s}?u%3Sv(tsK8b)S9(flWO>r&uYEI~ zy}3M10I_>}`*sNUci`bt7`|Mq@%eWKLe2t>ZPUuCif)6IdXGE~(RP0h{JpK* zWxb57cQ+*p8Vzs3IPI4(Vr|jh#aHyMhsM9|G%P}}!1yR>FE*W&Fkm+VE}Vrv65l-oH@vIWml?#;{| zfKn%t1U{v?0X~69s%Wx|vp+708?m`jDC(@XvPy5n>fEs1eX_p{;N^bMg73pVZ?C3l zk{H(e4$n_#`+vd6XDgS!0j|7;*+wV_Cpx_& zy?j|``)#2105?~sz~R)dm1lhxjU*C)qc>~t89iCu@tt{Cw=bZu70f82#oAf000Ngy zZy!4|*;&0bfZW>77TDN6>G@Kyo?0m}Upn&9elLv1PCQ%l=8dlJKoDUf8aeRh%dP$> z0%*ln<$WPo%keIa3n0Aj!bZ1QEdn)2-*Ie=S$^pwX!P2_T{-Dq8FYsa$;AlB=W?#n zs$)ORLVb#fu45;|TY2g6o@`VAgYK8NTDMLQ=Rl(V6b6a|U1Xw%iLZa%@JPS!7xvC+ zhE-OhS2zAC!e3M6dX8XQ=p-Zmo)>W2Kd}r||I|m>=h{R%)BPm6j;x&-#83sbB(Xh9 zvedzuP%Dg9153MP)~ajn)I#4{O#o@w0moDv7){5FP+EgM(Kur&eAOfGugXE&Fn+$%{tE43n7M><)rrCbdLnj zQuw4#V!h#(wA@%?%?YBxx!LC6RiW({<`hQDn8W=%sgH_zvG$(RG5R;0GC-MV{YKay z*bT)+f^f;$-PG|AInPRZQ2(K@h0C&oGQP-43fk{;#%GZzfitnBTSs?IJoy)$yOWAn zdLOk39ox)D8r?l!*I5dWH0#SiaDs2@`N7jDFeme3C;OuYV}z`WSw3wB6BXP4AFX(D9S1LYOZ-qRUB$ znm{LN11@=8NXE>Mo@{e$3;k*~%M^Y}wMFYJM~<9gjqs+VpR-l4O|$1PD;=1HEN0w2 z;uT^lK;P`)15S^X&U$U>uTfq_osm)jtJy2lCh6RB=>je165$YS7c~?DGciWSZX%`0 z&jfHA6@~)HW1jfu!Du(Q}|j#azs?Xu48C(mM8A`sSkz zJFeZTO_qSo@a^ZRN{Pl}K+)$r9k&%po|r0j0s3vf-t>%Ac7*wPh7O$FpbpY@vGac zjR$_jZ=;+bslU`9*Lznpu!=KnS54W8$fm#7_@kg&G0a! zuws`-?qo14`8ij{NQ~>VS zXeGnc#8D>mNw~ElRHZr&tTQSB(b9zc=6def0vPa6#dUg8-(C!}%HyGt@&+Hn*(>N) zz&#J&Ns14)jMI%+i6}bE%)E?basscF)fLGeNlz0~foU8S11lO8a)dV% zOZ5X+JG*pNT<_<W-1wFl`F zvO!1)HyAFAz|ZA2<&G*?1{G(}$6Arn5sYXM(}CNY5G@Krt2%i$OY%qLil|C6Mn8U! z)DrkSeq$697;G&M71+=~V*93^YT_RY@_Vs~0xe{uKP0=SZzs_ZW3vX@f<5JMXL|dh zwf!oWDaD1TZ6gsYlHsE>2gr&?>}*SUQ9uCH`cZYGL{Hy*KEC1=l4R|*O+1@9fMamu z3=SGoz%*~eTRd)wG+j(3LU?oOGG_vUjW=G!pfq!ulp6o@Ya_f_Oq~#MIl=AP$?&`~ zJx3)a_J>IJqgOl_|BPKzT_Jh}-6BpixcRgvn+nG5Z$$Jd5)S=6J2ASo_Td>pB_N9o zlh$lNKqE1a#wbm{(KmEkM~w4zy^axw$8JO7Uk+xfvJo#b=39%y^M$Pp#S zxj1vAXdToul##?U&t102^~;gk>#^p*8AK2pCWsIhn@wei`BG7xEdS;FCiThyN_6E(N? zi;<#fThz*8mz91-ViNb3{{#k)X*UFWSYbYCw&vA2cNP59!eE`snKg@Cj=zUrbY|C$ zgvqk2`$qJY3q?XMkMUl(d6OL4#jir5y~pbeGhnejRmp95CnF@VyL<(NDGdUD9MdWL zOIOO~d^V(&W1J(w!cNDqv%^*V-_w;1)uma?pOZKCbQR%X;((gGigQM}eE5^iT!hu^xHiD0qHK$@Q zz}#t4!y+BgzwN?7DtucDOFl%?DRkR!hdseim-jwbwz(kP@hxrjmgKGktuieb{--~1 zttvm+ft&)Jp!q~Y0G_}hvM zhPZrDAoT~2vcQ&d`UptKIKOV@w3P7gVwEkLr}Rf7Iea8hlhFm;@F$oF_L@^5W4ky% zj1SZ^3|7zuAwtEa6Dx6b?im1P;8RS+7E((F_iP>ZwZBt*Nd@FBT4IQnE!yD= zI+lc2YHg>6?kI!in8=u9+8w1XZa85^W1-uV8B34T0PThFwV_*tgdOr)b{Ptp)V5qd zH$RZshK*rhV?5C?H)wV;H;-`tF&M8;E!L{tl_sKWf>YT;z19-S$m}Jo@*wu#o?Vh$ zj6M3vttvx!r|h_?gs@~@TY49~m?inL(YZ_}1?4!Nv)l?Yr!thb8>=E#b3nrVgI9!h zzi4WMfdwN8Tnw)}vS?~T_Jo$1VK%HS9*!wh;ncl_d&;22Hl@xfSHlrexo?Xf>iXxI)Y*Y|KPxt<8x5Zt@0QeFnodDA$bh;b?B*VM+;CI;wmJaj8%VFm- z)|l(iw$lLhsrWItv#h3M*k#D8vSWf(@VWnl26IT*mhm9rK*`_nvc^Nb*)me)V)3?@wE2r{ zZB9Kj2g1?LXfm;%>lLbyAWxCUeO(WLis6EmvJPLEzo&{YDe&h%x@plwNU0>6F<3=# zCdF_}W};A5?Ypl21h3vKkQr4s#W&-QSc$b#ylb!F1lvkl`DxQd@-2EeZk z308bO+oyFzE*)6jJOes-3%XypRHl@SIR4IaP)t=BD(kw}*>mlVclPz}S#8Q5JKb_OWNqHyyxNpcfk^^xgY+bKnZ4 zn|Z}|G0f**gTl{-&In9%KJLPF9HraRFaD&gp1Fo{)vPatGTt!oQ-WPb*i!s=+Evgs88)nnigs49! zk*9(ptH0eF(R4cuhZl$Ffy85?9&q44oufWwIBQ7m7Fgqx6f3Ma^T3kVyTFg+2y3l`;(hZpZfX}dIlh9u5%e8S zIg3Iz*X6>zD#$)l3bqr=$FHJn1=X21%d9uE18xa3T7sxTtZB^dxdCM>E-n{A&O>|` zH$h0P&_A;7Myceg+@NEz?&G0u9;@1x5qCX*CYocTa zAjnH!8XTMZ2)1ba3L8quqBSR{bZnC`WgPt`e)^eB>j$(c-XreJz+)yH=m`6L+m^cS zqIf2HSxK=3hoal7MEA=Kkxv8roYyM!42duPW3LDIW>5t_Cp4>tx8ZO0Ux`~TlpnRS z6_%DOky*y80`@51_icD&!8_w(StUB0>8GZdk1RXKb>c1Sl#bKczwyd)HZht%g|F?p zG`z2jhZFVQFVoK)K6^^vo(|C1zt@YZtm~Q2HKKbVvv1BEmR!kim zqEl_Lf$?t-gCFb&($a7leG&WUVUdhnJI0Dqm%lC}FnhPkg=7rBfd~#__`t=I1(!a7wu{JuVrM)7H73y>5le-b1*);5m1W)bRf50&XiHcu zqU21l$8H8ae7Y^c586t0T7UA@Gg0Ll^o6D2CgGKCsf3F}^61`{(c2u5ErIRl*_QH` zDwc+8(A`;9*$DoX3KUJJp3@6kKve`249i0a*Nbu}k!@9_6sbyWjJ^q^xvK})gS2TZ~>+ia@-=a-&2 zPQ>Ox&u+<7`TM~tcuI-6BYV@&v1P#=TlNL|6(b_6^gF|zbH?|F(|(>Q(+yNdBgmH; zl;&}7G0t3&lB1a-B{3w+0?A0I5T13}_Ic8|J)F?dHs>;ZFg~9nGI7Ch$gd|Io$P7+ z(HSV;l%4mhea8nGAGy3{#knKgY<)BU_whHn@%Q)_LFUhhyIMjYS*WePOOAhOC2?Xd@BgotI<2JM=b@6AU9P+hnPJ zg>$Pzpp}!15K8mO!G146vvYY8a8;>8L%G1F((RQj?8~x90t;ebMRulPM4Jiz$xZHe zzqTZEnkul#_EdVwKx=f?b^M+X`FQHZ0wf_Q_CUzWENI#_#Z83%!(@_SLbAPy2L(kY zmhTt}QYBdv@Q{^RE@3L!BpP>fuD%MMG^?c@4bF5&;E~%cCE?S?HkMusHTZjsElbcJ zR1=5K^^ou7;@lJKx8qw^KpdX;xna-2PcVz0gS{fKwD6wfR+w?t(gl%o4!=hZA4$Y% zu*d(dnMJEX^x!ER4%*Il!;G~mdr~+sBkCR+L`=YAsg%9|s0$rvj20JZD&=9ne|vi~ zc3S)WzQm{lgPX9?m{Sf5_%I8;IOasb5< z#{1rH+M`w=uw^&45hW~o`UaJ`c3~$d#rtU`!{bxMdgw9wuxCl1|7=IfudB!QvN|b> zMwtC`i>~#Ca-5JQ_A)Vn%FnB!FLgh|W0%7@Hb_Gacp=V?Wi=vhzE2bUcLO))59M?e zqE|$4tlYrUNO%<+8N(5(nCeK|Qo)^OP_ce)p&13FGZSDLy2!W1QkJ%5yvU{ua*V%bD!hNU(LB5!e$|9eiiQMCok4#VzEu6V z9b=jpIq{7o&#eTNf{{>>fy!kOa$7kNe(8?=bXs>a#{(4cHBgos>_;D<8H^DL1UyYTvVl>~J1Y_go{)+k3yp@Nw%lFKJH^0AtKaVikE4M$FFkuJjtDQHmFnI_0 zE9YMaUDT@Bv!}lg+u*JK%WZ#;hY?@DhQOwr=>r9KeiAf3x-5-gr}gRx5V+TO(!@`Y z&=|Eg0c3jIp0_Out9 zvgQAafOWnT>9v38c>A`F^k?nF!im|lCzs(hp9?D*-@(b&!q(ZPogcEig(>hk;ctn=gU?rsJ8RzrI!x$&>hF88hhKbUD%m^dMoRayrvvWzM@B%B@jLQ zNPN&&wwCH`walP4{(a%%!M|NZ)UVmcPwA!6BE#H3wV)G$ZRw|h%C5D=Lcc)xp62Es%8&T=l1p2)t$@g=C;IVVi{{z)3h==B?TV3F|w)O*wPYKVBk2Q-le8liCq{uTrM zy=tWc?TXEejeH?0mnV0~-=7Z-q?F8=?~N5SO!F>xKoU;7K^=2#x9F>mc|({96+Q#I zD8k_Un{SoTgC^$$CnLk)LHuH&XSu;jNQfxl8U|=Yh(#B)6AQEe-@mEYGGrJj@^^up z|Kw%g7-7OFV7S>i{)OKZVWy~|xc&u!&M@KB-~t9{TnIrL%sKhHkj{U6h?-zjKmL~> zU=b#Y7@Ff>fABlPfH2;`7!&`OBpV%;lJsAaX=YeQ@)XWLSm4B61Pq9|5-c0=Um8kC zV?Au4AvEWIXec0FXmEnE|1wZOrnTUjc>iI*0mrYxp+hR;;3ibxW7$eWpn$wfKg1}% zll%urGr|`pL345ZixAu3oxI;k{xkHA8$s6We@RdwQ0WNCwEvP&LYmqThUoqqN&YTE zvn`na8vy&CFkvCissMVK{}fq5s?q=nFyQC{05(MF8-N)aoKy_Jg~Y!L4?=^#l>r7I zvUvarXmCL#;4@^Y0PqVMTv78*lU)F4gy@v~GqhIs?ps~Pg4gd`UPHcbYZvFvqga-F@y%PvR08}vGZ+-8kQwx9+ z2JHCz{bSzEMu>LDKeHr90i%$_ZU7<-m|*hV1or=9p3S_^s_p}{K!k<>2r%IMh4%>F zaWe#c3;=)uYp(nk?@s=M0UH23uH z226eYZjx;P@?pRWXMkZy)DGYX8XR*47=j2M{4-bN79b9Or+I%kj{$}-;LC@1uDmP2 z4@my`KU0hT0)UY7D}V}2nl>U3{BBu75bgkn&|pMZM4a?lfE4)M(GAk__zxQs0wOl} z^DRIvtrZanad`QkXe^pq5mjMQlJT&?q6mmTAXBi2`p{6k|B-@%d_q7(g?X=D_@5Hl z_woWx{{Jm6;1b~dujqoIjngVG_UpMx_gB=I)NK}MX{e1WLh|}AtpFq+mfBlObe$^Y znb#+;h}mJmg{m42?w?Jma_@z)ms^Rm+PX5(RMpf}o0mG+W0!7n_83@%uwfkIn<3b= z#UaS@8T&-6+-AI8v5C&KG)bS66p8KS1`zpzgRta(0YAsuP=PWUoH4N|1q_`JrR`ww zNm_%CNdjR#V``XS`bk8?luS9)g0+a%oeOb#Fp>|&Bs`#^(vUW(sFw-K8DY$!yO?O` zWW-P@2{{=1a^VnRPM0>r(WYpzkXU%TXt5a05K|}-J<0*0NM`f|sq|R-*5AbW@Rr2v zSQb7*2j~se^?|Uea8j**0TX{FViGGf2U{8-$Jb-sR@*Y1F%U?%#m0V33g>T zHV2YMDV0Epgf#itJH4!vS7sHxsQUoV?_nVQ!gz0}_MJvrsXtfWF!F~nDqM5Ky~Rei zj8KqS){KIs7~6W(@Q@m>PHKs7W;P}`3jAi|e_~I-+gxWW&!FBvaufCD?5u=VwHD<; zR8`O<=*gvf36oxfYif?XuAlt~^e>uw>QN@+kvc&3lVeL1xfW6e-mXHvuNZW_bPohE zs4?xE?e{27WENBOg!U>CMFa}@o1C$O3w>fbd_F7;1l-p+C14ksz``u|SjQO{U%9V` z93mxFHz9$AQ-_ZXh4=34q+dTiiG3lm!X%vz3oMy&5so`_OSP2e_LUi&e zuK4};JmRI6Idc4M*cPrjkIhFWm6Rh%IxDxQ_`tF^O#$ox{%7x2FP~rN*+zBwIb0Y$ z*<1ugZy}AQ-5Eb~;?DX$QtY5q2gq%6=hAS2p4i6^HP+ zzjdGo8tXr|hwppnttifXhp0NDd37&?MpjrK2N`B;!KOj&GVP1V1GRj>;d}$=+`O41 zFDIAO(wWZejkUD<=YZ7d=SORtR(kmk=^}5=s3Z3HA{0v$9T%JlOE#UI$4$gVzg(b( zpb9Mp>&_#+Vy-OcppgkWq7t1o{V%r7O(II6TH2CvkFRfFTkG=@$4WtU;x9cAVT(Vp zI_nF38{_&=72k<~@_@^`B9X0c0|ew2uoHKjs3DiOVW89{@RySk6l;Bir-=e@rPoh8 zuLwXF&E3d0;?HVHWf#k0e~xS_;<#h6pJs{=D%sL+YBrhnYW8#X-?cYRrZ>>l+*-!A z=XEk`)E(L%mJ0;7mChV6><)uwWz0;AWC21jCh^VsfxI)<zKy48iRqep)7Mb0lb_|zIa5`NL!bL;()lH{Us zUf7JT54!r5f4{B}TQqhf83aYp|u;}E~cs~cEa%t3Ifk$=5& zH+Z|RfqZxY@6zLgk`X9Xm#8IJ2fbM4ucpr-O^+;}j;tR-S8u<3zR(Ulei}5M*{GPN zK11t_vZ43Qo;d8>ud80>uh^>4>YHhFk_$Sh=Ov9^bO(c8E9<5+HqI2Tp*-l_o9ge~ z<06K+KI>hkt9{qKy(c5)Fjeet9vn90Snb@8{%_8pKf_$u9pj)lbqY!e<7UFgEJ zjByx3`+ALTLzTi@dS!cSHa-ylh#}HG&ElEMdz~?qyE0Dc+Pm#uUk|^}Kf9)A%#d_W zXq~38?fFZA?pO*MvREAX3K>tix8Z&uvP#(hyNvz9W+ZHTb^S&Z$m;pm`2Hd9X5x`M zmi%1yJh&g>E$(Y5Pdn?AGSJHESx0F6WUUzonFH)+=%gbvZGFBSIw%|?i6iU}X>UraT3)xS(d9aLb zB@1G@RTfGULJHeIsm1QTaA)R|V!5;#Ftn=5D0xduUTyTHA$QKplBFrT@&i~{v`2}Du#f9n zXhyM=s%J@pw%r=bG24(J>Gi+UTzxS>QeUidmzjc;ivU!vS4ebJLzyd!nSsQb?jvl2m<`AzaU=8#m+ufX{dmgIA6uQpqYJcM&FYxE!VB+IMVTxYK? z_s$u6Ih6VB%H3~oqXn}q`4Nng%QIn&YYL|37VwU}(`6c2#~K^Lb9i7|UiHOERX=7| z-K5I;O~U?&5L-~$vGdlgL;1>FVfMWrgv)RD+wF{`9W9crgl@9rFiBb1uH5Ymr7c00 zwO2zW*d05;ST?I_aK?k*w*oRey?T>wESpJu(CZ-;?>%42lrn~0r%ub1g_OU5ak}5^ zh)UxmC2%424dIrFej)K{-^_*-`0=5nGAfj%25FIze&O@|J|>W~#88y7Q5}Z})PXe2 z!`RMIdGesgC4FE+jwS6+ z8vY1(bv0ueK5^`K$3=aej#*$@i&?Yd*$9cxnyDJ!8>^z<9LcoJUlBJGZhP(@i+#bK zeaW7>6*;`ZKR4Nbtc@{)JV^t%DwmXW>j*37E&Y|})f+cCE!G@$?aL1QQ~AV^okcBN zzh5W%)Y`V=o)R|}LpYxuFinxJ<8`N%zWL8cbTIs0_}NNmM7!}Lu@?E-!;ktolV$fL z#D6RuC7(0MT*>#^Bh6of(qzGx>9!moOuD`gSNT+80SVMDBMZwlEuM}tHToc zgWhN6Dje|h!QM{~q~(LY*;FMw$3!i@OIMO8yjTnI3U)=?^c}sQeU0=FxaRU) zz`&Kss1Os!)~qdHgEaC|8%0*t`+bb1d+);~WqY(lr!`4w7l(n1^jNj9jM*oYymWvA z-Yc5w7!#F!jd&*@<9(r(`6BY%y&ZyNkQN1bPNg||6~hbU37inAPXd$;>qzRHMVz(-Sp73jUI}9HfnIA7oJ-7=xZ|5(!T*Cu6^3bMO zlHQ1WS8=B^d6bbkU34mcaEO1%wq1w(RekX2Ty!i_^(qd8Fbb{D=HkH?iCx0r*w2L))sD0vMwln=1z zCnbkUZ%ua~SB4LG;yZfIRhN`&>_s8v>Lq5~BV&i8uNUaY?WqAO*dYASe5K`z!bV zH3ghUgjhw$$$`WsW8>=PPQk(dZvM|feEb6M%?t>L44^a}MW+>Zl#XYe!(S-6R7tbQ z^hhWt4yvavM83`jY{-+m;>l!p7qZK)C%O%dITrpn{TD^_YNe)W+Bj{TmPC34~|@#q8F&~c^9#6tOf;KVou=qd0YJ~BF=+BRup zERNU~QGm=h(La|DT_P}Y_g(QYz&igm3&+va7J3z{W@W9MD|;#sD9@`6beLCtw)8Q$ zNXRJ|Y6$urDykCXzf#U=J2WFVr7DJsX^sLCSdXnb6 z03##g+(dhq!KyGbl1_{v7z*|emLD!Nqb6+cZhb)}7lEC|L@bP75@Hw22c@15iz?vK z+4gxEGNeC@lj&ydaBj4afDM-oq2xI2zkdj!9@<4x$%q%;4--p;qeSkj%78YR;x2&MHb3PkgDJsr&t*67v35@g-E78?=ys#DD7m{9PQoU!3fq(e z_>F=PDKVjaq@DeDTuH?lMdnrtnjKV$F2aSRb*n=)6#Z&50w<)--9(#v4{t1ET}1Zt7y?K8#`JwxT%B>lq*5R^=7ovsLAq z5Q`cx`vHzJT8a&aI;oz5@<=AC(&@)}#r+_{E_UE9H%4kabx3tCCnu#MO%vfo;UB#r z>Pq`0{)=2^WXIv(;zhZf*9}RfaoN@Ugc@j#5pJ0kbz?bRrH{HBtDQge8NLUW$>1HS zF5@V>YTciCS|op;!5{lg{SsQ~QVC*Rt~^93@p&)!yiO%LsYge8bL;+IO?o7w~Z zoatVCB>~>Fc<5zl@}?TEq#BE-u9tvP&v9e+$;rmUMYDT~Er!r#`P7Lt9uB7uCw>kc zNt}J5LF9P#(y~}FonZ@Ch9sSx_ujo((w?PM8S^0I+P5NCBXqtk@z~ye7oERf@o0ML zL4OIQ+9)m54+s=ocOo;Oc9+ws{V8O&_V?>btL5#^ty)3EUcQt+?~AhGFC7;sQwsgWLv!x`fOgu;l1Wr}sd&4}pQWz(in zjw6R)P2`l!iNCwGx0O>oiU8*g=S@eCERJc6xeg12$JxQ%iPuOEQg6d*+eroHlQ_|= z69D1L`|P@iORAygysNHc=u3s0Lj70fS(# zL|0_%-l{<%v1VNR-bS~hB6|c)JO5gcW-9g3~$>aj?TadnI-?oM(L2V=rdvnKkJq zbh2lu=VGY$Sl3Sv93TQm*m6{ZqNvOe4V~;~L)G~N3-ZWDxJ8#`tD4(tI=Y@fr+Skn zqwx?9m$V68pAfjyu4={VG2UP_VdR98(QPYK2}PQ+F7Ax2B$8+>6`E`X>NGhQdNXzP(mvHA z%96-bY9>sbhT5B0PL@=T!clzSB|W4zV0-;}as4Z+T8k8+&2Y5_e39#wZ-iUD&dyz$ zkj}Z`**MEM8Xzi|UoMDxvoru{5j8^e;kqFp1o4!xSv1c|cLfi6plaenL?s&>5R^}% zc{#C$mGyT(n-bWeuxc^_q2(mfRxkCI(6(5L75xO2hymo%%srpyg>^`3AsOBdb z7jr8yP~$87(DJq4=Q|VwAGSJ__j@n}4~kvIVNUR6959V=@d=0aaN+oWBKV*LrFth) zf8QI_=ht&C+fBfaHqZ@A(V2Vb3wpl$()Oeg1a;9Y4$?0EzU1kyvKtLnR*q)&x3$&Z zjil7z!b*u$9#g2Un3&D(BdeAK=Iri^Y*ODX*QHU_lea4MQ|E$aG!3`Ma7(eU$h~JY z)wl0I*)hs)|EVce#{!j&Y3gr7STlyx9yUgdbE!FO5m^Evlgle5Eu6l@ted16lz-1?4ow%AMp2BzY0ARi(N*U;4co zO@)auj~2_(7M=2(obM|P-4XsiG^;U^_f>olcpdGRzsP19I*jof^X9M)RBT01)h*OT z0<57QnE-H+iu$Ndd=*MT)q*({|GmcnyY`g_DSh$lk~bhK~=1QO?xP+{J>3or8-ht$qxODnW@H zvHoP9L~3%Qc6W*g2u*`5A()%6XVcXjjI4U@5!rewmjv*Y7^`&BYw z3kT4x+ss5M_u{&_(|g0Aj!vJpuE|qq_5rg)dfeHG;R%qQd?AH%-;|GQ^`ByhX57?$y+k)^cv7@; zz`PrOzM0z{o)?0m6Qsk^aPVClR7NT3>ecssNo!l9*|}={S)FdR`XsQoZ0{~8-i&Pl zAdqgP=H6%xO~}_-VSaAAEz&2FgT_Q%d&N)@zQmFl5M%cJ92?G))N;F9AJ{kE_X6k{ zo{-h}fa;Y~rz^Ji;Jkl5=C3gz!OmY?N$?o;s5O=TIOl5^aHurLI?16LtkK9oQ~&PJ z)o{@R+P7^HGmj(a8=ITVD8$rK!US}>z7YVXo@*|jJBZh}1_PU&*M@O$g;R|LxoolN zvp;6_;sOXq{ZhZ}yRDaal&zdp6acy!5-iuvwPyaYxDqf&)Nou7c3*%-m~}klsHaci z<@cK1cF<$2a^gefFpo!8P{Dn)Tr8uchl-zNwDw)=s94 z;9dN1ZOBbPldF{#xx=Lez0_6*Porm^#jeM=HfL+58{&bwBmLy6I#sQKB%nZk!ltYy z1P=Ei8G-rp7{*5FlY1;Lw_U3hb}O?JqNr2z-CU2#UDvrgk|Y|3NBl0+eO5rA&_NnT z3row|pwy%aLlqg2ium zT*ln{!7nui#4FuQrO$Kknwpw2Q)jFGd4d7&Pf_u8K0$>CjVgyZAWufbuu?mJyt+qp zr;qPe7${9)@$Te%JKet9^vL>4F31Xu(7Uxopr*hH1_RF2t82UcU;I1xcl_M6i37d5 z&NhcRGSZ!8{fKmq)J^xuejL|!c~wZ}oNfdN<~W(GCI~kaz1OT?3n<2;#&!^c=dP-_ z5*C+|3W$qpxV@wIN{B~PKZ$0LvX&ynG0*Vmy?|@OK%VE|yrA$dUw4dnCShi0FEUV? zn_H0fAlN)kSgBYm09N1 zvGa$iLS={bpUgH)vZ5$7n(xM&?qkwy>kBX{KZkbs(T6U|NaEVC=w;utK!&hTkh*xT zpmk_QBQQtHp?OBRnklUDSlV)VJG$6=>1pZ+5NtPelrxXafI$>!h{O^$2kH`(saOIY zXuo|`Tr)gXUSfnVYkIb4vdGc+HGyz3Oj#r(11JTWJCWTkzm&q|2)H#4){T}?;g!wH0J(=i*zdaWIcd?(u1n!ROOET8xlx<#%wSWH={APnG#5%8u=@f2+iGmtX^`!D+gTE{*{^jr@? zlP9to!RX1$quYwf$~hvdr)kC2u9~qc){z_874#<70!o9ly9eXVhFxd+;?^!3Kg9gKvi(Bg7#9@(2#L&XREISUC z-j-LaYJbkL6P$4)cxMDwd?+B1KR+{5%zSIV-4#o64ZIs zljdlh?Y8iP*$9lg&7iGyFn39~2(8UJKx8ta3_q=(wu646@t0YPtN^@ifg-HvXL*75stJ-On2C)YXkBynxxG zaJn198CsFJYNZJ~qLKpgH#uzgK|)l?Lrz+4g9(3B3L(Hp6%;hewsinYzCC-GtyPm} z_2Ip?J4}OJZ*<$cHQSxr*Uuf8q_QYu^@N!R#!|DQFb_bZGGTUU)RnZ#RO5<+mykhNnW~zeq6wz+P=U^&SvIwR_#EFS)+<* z%|<(lgayD+l-etiBs{9=QXsMDv~+LH9sD%7*x0UBS59GAd?S$!2J87@H81F^S5_eG zqpf}>*E1y2(>7ApylOxRUT|db8*jj^BJ)m9YQUy+B|&oR$xc3r_8M*|%5c-v?w30qUx=3`~F5UmQ@ygp{R ztp})F`|eyT2xN2%LOLXBF=>KQ-$W10DV_X4>)KPD6$x{?r?_gU9tc-*VY<@7tjA`c zf0qO!I792=qQZBmDFSsPe`tnyi@37i{}T;22g zF|gl>A_Bx>vktSEI@E}Ypij*tiLmlN5{W73WkOJP-LVR>VyyNRcwz)9MhD|-p*)!Y z0e>(d@`ICeZw?G8qY=v++|eZBm@j(u{{kYV=un(829CC0Re@5i~$4^~*`~eu2 zWqZeh0+v7uesm|hy=i^so>QHxsf8&pIlu)D{we0m8v+@KhleO zoL3?`0E?;i{~&1$U$FIpfPC{Iy~By4GE7=@ekDcFf3gs2m!Yb%{z08#uwo!6AWMl1 zu+r%6z2(S6p6v8DDoU-CUI{wQSvY!Ou9Hp8hgq_tSv^@W93P7$Zad>&3I+hXxphUi zBs4yB#)OFlvxK5073s!h&~S@A@@H7{L9}1m)w_!}Sn@X^k}{k@zWv>#`%bysO6baB zGmyHWuveG;RolI448sZ2wSWZCKb1rIfnXKtpT0sOhh{LG$MYJ^0n2lJ=)po@c=(Wd z{Isi&BqGdvTcU`(%_uZjAO~Pvc3GjorHadFNreaz>U89^o?od3Gq819(14{E6*--| z>4vd#LpA6ssLAe*0RtMt2SWaZ6*9i=ylrSs zeFMPoft&}6QpXVAUN_BafEqGu~f$O&-=57JkWFRC`F5H4sriOFX zbQ=ag3w5m49W^gOrU4-z7T}q_wvt%rOCF->S0YWH28ZqWS0PPgm3J;5>a;|XKYwm* znIpl4Rjm~O7}+XV(K>;SQ$n`zLl7v|Z96<_Y?>{VwFJD1{U!YidX8F6!hY8IIhr{% zejT^VqnUiO2jd|08~_I023=#Knj29wUAn@RMo>avKe!Z`r>o>=1r>^gR7A+p>;gd| zhi8A#)S(vWBuIa1P-v-?D!WMF3E^XDWiOHBX4O$HB1|KD2b9eEf>bUNg*mNn1TIOP zq`?M6I|+^+4%r%%)PYFMl%3^n{;ikguEY0-UD1wxE;Fq)fezX(nPvo5*iZm`43sof z_~|`#?VqWO8Z*l+RuU`{7XW*w;=p=v>TkN&pDuixA8KUe5{`-Mpp%P;)KM&{UG5B> zYZyAue^5%uW351jX;fqh*AHV2Q+?*z_;UnxGdO?$s2DTt)NKt*X^y%Da4>uc<3~26 z9DvyZ(}08sXitjdUT3vnd&p#tcHqqWshErplA7eFz4&h~cD=9=)P4o5ryy1yiP8b&Vz&&iN(EoX za`~{C8i9I0wi?Cr&ZOg^hBe< zkh-Liy9&hZ&6^~o0s!&0*1N(|=gm&9OV4mp_8CP8rf#TMlktnapWmV2#$5|xl1uqS zn(v26nXY`tfp1|Y+I(}H{lVQ1p}zsE*Q54n4=PvHU{d})|4G2w5Pz{7OLp%gp`M{+ z9KU0RXTfH8TO2Jq;#%mfeqH5Rg|b?*eWou}kFEtvL2Ie=j5PAb6>jhbY^%p9+e zx;rj=oNt`4T2WL$=}!(uwn6vSdS11`TjMBA2I6_%xT3wRca0EqQ6H* zn7i4G;ys(mN9E0qph-kS6wc*H0Y!b(8>k`Osy&>l=2HlfXA(rhx@AT23@bC-)%yC5 zypkgQOq1P#Dqusz4aP~tSlgz?bx{Ot>UZmAI?~a*LR%>3ikYf+s)iD*M`S5tkXK{U z2s$kWW70*5||Kug3yc`W?H=%CGhq1KlP*&0fU zRgO0c*X!Xyx@u_yO=NXgBSc}qNYmw{kP;1nQbZxi4M0D(REPkbqgStDDYkTRB?hK9 z&npbQxw9_@H+s*w90Rc_Bny31ZG04XCdnY6S$AXri@WiyO6^AWX!!->#%=l|YSEQ>P7sQpXh|sJM}NYI$bmtkHLg6OJ&nvy(Cnu!5vvr5 zgcd51-snYNm@j60nn{Y7hDgpz&aRG0E*dzxH1F^X?PYwK74%R3ko`58nn;yCC>a!s z7gC#{1AD-bf*EOb3G^A4SH+SUaGw@y>L%A(GM7jG5eN9Uv9<09_;)B~-E0Pq(!sMy zQVuE!aBNaPDjMKWdD$jG(+efMTEeQ4J8cnKS0kK?41lJXXhS5Y67zVZrZR;-Lq>J1 zl0FDN`A+gvZOahF0*+yn-r={?=XV z7?BP_t@I-1$|WS8x2s8(t;ku(AOAPTB_`Fzil(gL%nXo1AgR9i$KioUH5nSujqV@9 z7W(YZG0J#cJI!I#=Vs>b+XZ=-Y2;?l-D{Lt+bR2^N0-8VoUjsjWxE#Lkiu`ZwyiqB zajQjLZyn%IMU?DAOUC5^Y#GR(4u@aM6VO>$wAb9!f6~vq+`#|yBHIZ-VZmGbmox0E zy-B81?AC;>@i7xMLZ7BI3u&?^rn2V3twAy4WdJfuQ(?I^2!gO1hE>w*u_j!Vzg~ra z&2E%RvUtWZlr%*c>rGqcw(`B5Die0LEc-xHj8TXcjV2B6@;C1LmPf{Xb!_0a2l#D~5tr-Ey@9;vLHsxu$^71N>GUFKlYs>mVyCpf$~5kEY1kA0luVp=)FM z<^T{!(&Z}P5C2V5KpQhxS_k2FFdyQSL^qLy!zfK`@4b3#(j8Xrj^b`|>6i3V2UP{* z(Im|Uoq>YIQb$czOc`fJBv{ zcG4o4=XkEKuj>wi!OFp6XeP_id~s8-tBkOmr^8tAEkfw_yTr>$n{^F)n-ybGPuheN z87t&ROj(aM8SNC#)6KQUf=2;HSgF|rFf91Bq>_l4jER&(kjEEgm{I2fcf=^*+cbn4 zgpAlXh|^PjB<~)PQ2H6MW=R0p^m*XDSill} zb|Na0Eb^q(a6A&xMRj%P*g}2a6QagYb2ubqeEBg&v1S_s(aHz3*7xm0>LzB;m1le9 zpw0=gOjK7FS5ig>O+@xE8XAY_?=e@DDWRa~u5{7zC&3sLB81Sd8p17NhfwhE>3*Y# z3q#04g`&xKsUhog5oZ98G9VK*R0vp#j4lIY5{L}xvAWPND^Q*UyolPW5k`mLX5qLo z))nAzFh)usjF}KcVX6SIe(+GezRn^p7q=BuW(zz`XhLp>G~s;I^1&?5JzOMY0z}QQ zz=BxuQO`&WFmQ3ct80>BRJO;!s?fbVkeTGW63z*gaAtdCR1`q|ez$UA5^Ab$-PPaw ze1mmt@0KabRzO;;JOIB9yX%Ez;^X^;$AY`()ZO+vt)UWhqx8XH7o8MwCwK`dlxt{t!2sTif%6 z%*ETl>J?7&sTe55%o{LEG>*5me<@CD77 zrMBV;cn36Ol1)mw;Q%>g4e=2T;nf@qv>O76D~kA}iQHU8NJrUKHMd*sz{faQ=!rP# zFLhrslr`E(qt1MOoDZD72s9yk91Tdq70nMZhxtFL?t)^{wbsfZNIk>TNF8YUK|VDbDFk)t zU;qwZ8n1dWWb+v`1v_#ey8?7gF_txqp46Ub(tDg)2rs^IykIl zILEYdY07#L%Nz;$6pjE+a8IH z&n(g4Jrxg5{YzeNi@whn1vu*5+Kj7(S9pKL@V8GxV^0oCPK3y0=raO}yaDmkM02`R zp`YB_7H1_2j!z>TpjBcUoS-~)h?xPSAH}%b;Z&9;q+trvxZKaCvZRnniG1w@LZNIm z*hoPV2)dT(5(wGHo52}1HaKj&5iD+%?uaEi`}&B1O*V?R4Oe_#do5t!BG3Msu31Jlriw$F904zvAnh z0^4Mg-Uctv);=!4odCPcdoxtLY39ayv_x7txl(v{7kqj?&SZ=jiTbTvh|2rruIWHO zYmBYb7ZciTp^B1Cr_sWy!Jx(Bw&pS6z!rGi5G&t?#iS%H5jQP++=i&D)z{kd-*x%F z+w{NxOj|1jBc=N%efX;F9xRzPA2bt=qii&TX6&LY1#;^$VC?MN|36{j+f3c$NWA@lG1W`;RRBYEpL71& z8y~*pnz`ZWemppOt0Q;gr0j5^s=&WpwMPe%0sC9Dq%hOfv__^ug?6C_e$v>fDd7E0 zNBH$~__6+Y8Y;IOCaPZoh(9X{Bph zLk5`LXsB+*#e4bk9ySN3jb?(P$({D>_aGqtHeXg%cQ*a|axT+0r9Ww-d0lhDr_J^} zy{xlFhv3=zYxp_+BIpKO#mYJflj#ihX;=fBK#zH8+i4qaDd^F5T(F(S>+aTQ?mis+AE&<^K*0e`S%HwcLH1BMmDv%9al zw|@Iv-lB@Fe4OkH_|)y98k{xi1W|sM^+@P5*R*Y+UjRyN+hlae@&1aFlj2vcaF>4G@p(Wmn%wk-c z6Z%b?%X&bBvudxcQwloqqb}6r4FMQ)tIY%jKXa*K3nc3hQj_F((A7RKWPo0q#@zgV z1NiRfKHGl;+md-&*ISnnj?VK@WpVf~)7^Z+b7;2Myy|ix=8|gSavZDn#7B8Ly;f5* z#ZCN8`11tV$`BE3uAG8!)lL33)^HxfII2_H)21~b!g&G=sz^!tkib15ij z5kw}kwDKe+BnsMkf3}G~M*zHGM}Wn)cfIepY%YZiyGlo)zod{0>!uu0%M3}phBh;f z#fj>qp}u4p{^Hqr8PG3^^c}>NM2biQo4!?XQh|I&`fJqbC<8%ya{zI*(28i7#mH)O z?a;kOu_~|Bzqwah0k;^lnyh2RF)LJe>P*PXIi@%_F0NANxYt zE~#f&vvDFozr9v=bOMNVvo&|Um4n$A+XuUo8qTUaQd+&^<$JQW*qjLEu5wzpJE6OkAc3Eltq)pJYw-au=Pd-wSDZKcP z#@#iu7bnQfry$GD6M)9c2SgCCG>39%cwJNjc-9F&;yEUn(#z*fzJ?~#Z>M$T+GnU7 z14wI($PtG8M`|~Gp$%^#0f_fs*{-lH76#9B_Ivo!s%cKgJacfS{odP3R-1boy932! z4UsVwi?q9UP-745kTJ%<^X z;3BYrqn*kxWmjep47QQJkNZ;Kze5n>xZ8zaflpNi+(~W#oRipKO|(Fu4|HJUs~#$< z@NvH!A-un-L@9W7&;Lv`YX})o;sGn2!)}DGrmt#G&O!E}a-C(&KD2ZD_|FxE+zkCA zfh8r}susR3mjHcN(r=gN60404yk+tY5aFT^S(gZoJ8RQZQvRz=N6!!VRbAho;GHuJ zP%V4Q9oz1-jw{O~NkOCKRQKh_v%woHw9eBXXr5&c=Rd7)x2Fl)auLw=5}P*al@-{W zUfJG{9aWrIN7|+h+b{<`7b;(f#;e?7`kF$>ho(LV>3{>%klCxgo*5WW9yu~T_6jaP z(&CX5l6tDeNEYWM8}_O@Rj4 zP3~NzW-``1kz+Cmg>);8#2^%oAx3kJju1YDleGHj+Q-x1Idx*<+XE_j*MU7LV=D8sjnCkuoPs}uCSOY`MEQy9Y~s&IJc+!E_uC9I#wiS;-rCbh~U|wsVd+8 zZ^Jd46%MYn!q=|=hI8J+fQk47;yLXEd{**z8yJdFQY&9 zY5TL?c?-(I>52`?Fb5)P;y3=7mBH=}uQKphJBE#1+(0}%W7G-pc`N&G2S7Zy%V+(9 zyX0%zT{8zKk_YYGXp7K)>@a>u)2(*h=X?hYd4p+lbIK&_>4FVav|)uWTNQ(daXLx! zgBaRHOD$73y))*yTMuTp@n&@&-W@c5JD%;*jb$#hWc>in5alOBuUXfcy#Rp;b(-ZC zfOieziF7th6r5S-6sfR>0jR#P8jmjlZ(vPLCbyAr`CEjVJ=8bdQ+be^oOzd*+5g)Z zm8`c^R##!lV!y8xs(j?94O!69)j|16F$To)xQ+wv4B1V1T(gYAU%XIL|;^lqj(VsYX;0z*gK_TfQ*f z#l|LrJuzw8i+rQ`$0Md2vh;ipZYk5UN6wEvDg{kXDvlSxeG*tCpD>kzDQ)&kPVb^q z@iks0pQvug+?hAtx$dW9J9#|1E&7-KT)Z&jJCcl{%}z&Y2q1sQPt0J0L0HTy7yuqRfTtTVcF5(TXm%=Ic=CG~ki#211Jkm%3GWN~k(8t$>I zfUj;84=`kErU#bR3u0_E)p z56Fs-B83_{VGEuU!3&tTb<@gnA%N#M?Qis+5+G_?`5bvO$F+m&{3w&A zJ6+#=@d4Gpvf_ihgRJP#e>^HyxONFBCh($h^^Sl z&$q<-+ppI{7D#9Z=0I;?opp(hp^Shg!VC$N{c^>z2M}6!A3?j?>%)|FxDqnl38ZWX zEBDf*8%^FPjw(iZr39w%-ITJQGtDc@c9`lHw?_)aMBJlLn?Bs~z9^&7ywT}6&hJUP zvGZwPw4djMYl5~+917eLh_Hnkc;T&nCI377pmNSJ*Fw4 z*!*M(RgP09Vm!9epND~a+X7yX`iGu~UXc5tsFD@H&y>Y$!TftmRJ94OQu7(swpLP| zYMFudl5LC#nvM%eAX)a@XvhreY*G4;%X4W>8K4&rA10i%?hq}``4=-er^uQ2o47cX zA2PcPW+aA@=l3U?`Ma1~H?kwmnpcRMc_vyf$JzuH^o!D1sqPhWTn; zk%AnA-(5zE31POgVq;Fg!VgDR9z_|4!r+mS;>hs}jQB9`F_=#zlkCVqIV1jCni9`3 z1CYnYo{~?}Nqsv=b+rrnM=k7uh+OG*R@LuF<9HOp5;%K>d@+ZJ&9k2@E$#N%@oPwL zRQUAu1IpH^7BXqUdpnnHx=#+d&)_k*J!{?ja1?qDo=4W7potm;j*=WYUXF)g>9^B^ zpT{Th=vf2wKaGM!R2QYqU2x`;6n$Le6X2x2%gv>d7+t6gTgZTnVO+Pn=VW*ule?F~ zb;ehVmoNV?@!x9IKavuOU&-*B3^N`l&mHgtg~R?E#*dw&IA=;vE)fMN^GM`Qqnj&? zFHzz|ora0O8j4Ca?L_C{bnE9!@UPJVm4qtt#lSl9>_I~nIIeX6=4&2{M5hE5NQqesHXiDpeV z(H{i&wGf6wTOM^bMO6g9oUhi}A|I+B^@&cfYCE7PW^*?KW7I6+u0M;7;T{D(O1Wou zh6p6yDC7819bR4k8ns8upSL3doO1?wSJnSZ@^!Eh=p?Xm*z2c9?)++x;yvmK21WTH zK%P5c;y@2#TBtAKE2!*@iECmI`Iz7D*^b5nWHiHty-$hEM&Cz7crw3*f(=2yh*~Ku zfAhRj>4v^4cE5!;)7p^&-`i>=Q51n;bcv_Mz&=o^efb!DPN6Y1I6y)H2xytL6-bR$ zQWN{*L&;8qpfUb3?e?uqV444z3Wl^jG8#AjxV8oXEkWiadHBJ8=1Yhc=0X{pw+sGr z!0A9dv)4@szR9Pc?xzQ#tsu6*ZEkSf%|7G&)-Zcm8dB?)xE~sWN#$MUY}{T-fVh}z zKOvp}d}zr|*00)NL0F9dVD%2J2qbK$)mJNmj{0pTr$tmNejk%`Zr7^q(`aQ| z`krE{tq2pU_T&+)n|?afl;DbfkBSy@(0M?I$8_ik#prfdNuWU8NVTY18vAt9tRgao z$AF2TxBU(Eq((&-dyQI9bQEyjGz|4{oZFBSb;hWzaW%PwA*+lwTdGHj&p)MDbbY=UC zTZFl@30ARMNO-RH#L~t(K|e!Qp&S~!YY+dXncJM|1rAQ18BSAA2ipC6w8I{}`6clV znw~S*m>4eb>ML|EqncBH?3Gu67(5s?V1?X?Z49iAqz0J+pjUNgo@_cAhOespSRv27 zmP$({4HUkD4%A=#OHG`3`K#WOl>Ej%8u)8KVcD zf#O^?Wjpx-I`Z1Pl(c}hikk9d(d0{Ba|-r{e|!6QQ4H?s&dykG63kkbglqc|ANfA|Fa|fl@Fqqua6+iWnjX zz%Dv0yDzTjz8RL)#3`4E=?hT~HDW>Whs5xcT%OWpZf0+Iij~`0;8551^L< zM_Hp}3;BFmsTC#uPx4$=C0!`-A{dmmHq$^t!1~kW=PCFl;*!`9FF%S#nKaiL$3f{APa6xo8P>IQ~ z$l&b*fTT^H-M4Uq>hLd=3oLNB2S-y@Q3$e$6#OeO;4$EPJ)QqF57GBu&{)3q(~RMq z=qkt@g1tNa4P=NFZr$G3=H0|Pn8&9+;y%J#8Krkc;NIpq#^v)WOnvUyAo6~nfvQ*( zvl{u!9C&fxC`3BV8d{V$!3_AraOuQ40}Rp&&@*0E!E)`=6|)M52a+^|OVTYAvcb|E zRzSw&6_=O|&%K1fR=L$;0Uyya+N!wb-QVW%Nt+}ziW0yqj`30CL#lA!ksVgU>FU0TO&|XCDlNwI@DWG@ z9GCNdY?JxehZp7==?YwdX#Cc|u1 zCQMmL`j!xJv%q{zJI((V0coy+5XMyM$^00*!nUM;D4?!E-hMyF+N!{(A#LOM67UX# z*+2&9Y8j2h3Z?L?RbCg^2?CxX-dWpp#rP`F<4N@Gw=BycJqY4q!FUTe2FMlwOf}mo zZJr{RvZHt)!-z0i3Rd4FR^LGqFT432_*P#tIKjJR?xd6YYoy`6AQPW&4ZJ)A`)yf} z-!Yha+y0zgu)lUKwryoUX9HgP)8pTkCV;+S!sM4UEc3&{erT$PNq450db@a;KJR-I zMuDxA%NvaB-XUROS~q$gK3HymT-3I_vvr@2>UH?*qG-gG3E_*d7atS)r86=yXaJ|; zN5#PPD6OpzqUsywA=cN%zEo6EJhrbLB^Vo200RpgDyD5CPgW7>vNxBuU!we=hdDVK z$J@6tfteah=Myl2Ig%XAC=oe0{$Jq`%YXO!e}A~O{@CNTBm1q^?it^PkZ@-aLxVwX z>1J35U_dc=wsAngtp7nhgrjXf=JSvgTgrcFC_#_-DtqnO*{#eEk<#9IePMf zW(Oc#D?KoFq1C^*rh^uNH0Xju9rrH(NCM7B6E9mYsxg0l1cxsdBF%t`Vaxk> zp(GxLTaBezt{Yvv0UzE9hnoXqqIFytYD+0_wjIoZk!_ll4^|Ya4a+bO<7&9B2@CKk zszLGwc|+kwIH4VQ1Z_^$@hELK(m~jv17}i+s8O|pCs>WQNr>d43X6*zYyp9IDTi@y zXLF%x>46~NRtR75Anm9Kz=tcGqkYn8MjduY^~b_Y=^UMxIYJc;lZ5T4Z1(;qnGP9h zuf73uRd^3`I^JBxr!midO|YMrwgkYK&7n`N{rz;jP8HnRvT2DruKbL$b^lJQ%-qG8 zB;;(pU1_xknPTeF{jq#MJyx_gay?KpXQgXsQ>`8E2fo{3A?{q~go`SS_>lZyZ)af#akL^;b@h5=9mF>v_L z18~fh2hi*8?bZF9Tpqfv%9IS-OWiZY#sF5Q;HkcBDabjZsMl7r2)R0zTcPKnspz` z4n~aLjhq{=6%JrM#?#$f(-`;KjTJs&r_xLTz)=gFr z-EUs>P0-n$-XLHTK|~*iz7lURC==wj57+{RPuRSbZ;L1Ck2^5G{2R%j z-qP9~-9BQ;O~W9ZG)8;QwAz(A{j-~Q*jAtJpC(uPFoyl(i*GMr(_NcY&P`sfj2l;+ zpY4;M7boAN&J3zghHG^-J2I|mR(^;Rqppa+F6XAbxDtf8Rz zFW2)v|$iA}K8>JuRN=nVo6@9qsJd z@zG?yEMU4xA7DVfb6B?J^9r@g*6^kegMiFEat~1 z`rD@a`+0$oc6-9h+3clc?qOfc%$s2duKToZvye~yNIafLog%lRHJaOCQu}WUZ%t0& zp#*Y)jnou`7w{63g$=oe!reh+Bk|x$SRsGl;He-qhJnPi_zNt|O`paHMiy6wy4X&= zWJv{ip$fb8RF=N4+Dcku- z02MQ`mxm@AZPEN~MS6g29q@Ofs_HC032tq|KM&4g%zr;? zpA3}1c}yC@=9rC_Zodfw4sbeCdc*;CyHtPu2Cs!3R-G z6ZSgtG@y>(02up><>1^PaV9-(QMCUO8(cCQS~lGw*yfZ$~1Kb^>x$wUGz5-3G8 z_8x{aQKQ%(zlwOH#D`!cx{DptBv2`cc>tm;A6ShPvfoiq7#*=dD?^1m&;vb?n>zYu zfQR@%9USz}n<;N?685?C&5M{}-auTtd)mIO>tqNX58k zdzgWKe4TU-72UYC{9-VCyF~(*yoPoNY!(-bX6aBwy0gJ{qa14L)Ipg%szN)-YIZnw zIe;wMY$p(@Ebz#9sWcI7oJtV8As$$eyNIlGQUJ8WC8H{<1~|KbIc`jSxagS58y79d zj0zq7Ga54GON^k1zxO=C5mx)#(A2JcxO;v&Fyk!4!5 zMlC9gN`goxN+!bQ2>!1CEykIp{fHKn%}4IgU3LUDbpZ8!(6jgm+nQ?~6A#YADd0KK zA`R#l2|34_8_{S6Rk`|b$(NkEArK_jwIubab52^=Db^BQx&y)wABkNQr2Q&n58$`f-ldKrluFhR(RPuy*-N=lGRVqUu>;t=z>Xfl^Cfg~eaBKtaBL%#!)bp(C@~M5tBB zrXD6G(8xM$#rATe#$rMT?LsWITNlAx*q;6ss|jf2GvkDNt{TeJfp`(*@`qvaZ3b-& zf3snZ08TlVZ!o5ICeAKSriQlv)B2xMa8_o{#^Bw&z6s{eui z4?vp+#G4U>>wn=a?jQs#AYA_oH!BBOV*4LBMyt{eh#fr$GuwaUk^+gFiS_@nOHOvC z|Ft7r>u5XUwbuKc)~1iPDequ!1*kBrCKQjQ8gn-sk@X}KsvSihF%O%L6&D|^miE>% zcVaoI>-^(Od33O;$BeRM^Oq?6@2G7YbmTV7XNG2msDhZ3lM>?-`psG}Sh%G~X-yiGBr^}@qLiqi73C;R zU_Vj?aO zlGszNPWfT_pmnLR8sc<^+OiaN<-h@4?XnowbROkWxe9N&_!{_fX(|%^6DZu?dx1&l z5oHO6@W6+CVlm@;|EI9G4vH)2+6RLK28ZAf+}$m>ySux)I}?JtGq}6^;O?$LgA<(K z4q5WOzxUg%+O67urjE>c?wOkD+jsi(c^YFSvF@H^Oolgs47={P`0$R{QSfJF9Vy?v z>gd5Su06K)$c9eiI?}Im(r}Q!&E(~%A-v2iuxR^rlVENslTcSl_(#Jb=)_9o;Iz=t zXjNrWOX22)fdP?zx@6bRpGtK0SbWedjBy|18^Ym1>K1n9URu`HwvA_SRQ?mYfxHbI zW^<2fIsn_c_xXNvK8AUb)b_7__p8Ex-vpVk^#2~c`n7#KGSu%exO(9#J?tU8HKb>X z!vYDSG-!qyKG!~5zU}2A^?bc$!Vasvu+;3AGvn6g1x67jnpt{pdt{3X`z-q3YcKbl zJTrl|m^$92AHs$NYYr z4~PNIXPKslXnVC@dEv$lwa7Z8n$!5vHj*Qlv8AcMqeH>7jaC6B?cxI;(=3#nG+<~O zUUouQffo?4D!cTf!#J>C%!c}N_R;{8Nf(y7k+T7-cY?2vs#LxL>&&tR(9^Xu(*dgy zhaJfaavodRRzdMe1OeDN*v3Hx?Fci&e00aie5PlUQ5p5u+7~%8G;F@u8Cp}g(K7HB zFy~F!F4WdAWX(jZg1-nxl`Y6C_r=-5?N_7ffFoamn}`}>2Iy1yX`&KypJ*JG1r2f- zm<;tNQLwWYD4Bj&F700HRpIr#AHSVUXjVSi%;9Q$y;TGq(#f2u9{J5}4f@hrOtr2Z zS;xA$+#(?UNE>?=Ka1cZ)A?RrW*D4^wyxUag_Cy`wUWS`7)s;3mjqZ>I()_$Hn}v# z2jVJdC1EY3EEkz`JAci*C82cuihQ=lO%xUw3r&Bv!CcTsUIRDk^!g}{NI6N+4G4O{ z22BiTA}B-jFHtFWyuS>+s6R}oRd%KkF(I4bPHRTpETwQuoPBBEv+?%#Zfz~79jrKv zDqMKNo_HW=j=|YZ$D~W|N~mVA*&ljU0ZO^mT?lPSK{ag($e2cgRjoI~(DA8#rZyt5 zBQyIPyU=HTrxVO)dlhV0Qfx&~zK_ZIMWTF6Wy6H3278g^n7_J-8XlwBl-*uPq6o4N z!Zym#`*Q3Sk4tRD0~$i8c%4Z9F2t-kv>&%PdY;$tlPPYCr`lJTRM@WR(|w!T2Rb{# zaMu(f11_0Is4k1D*Gs$xIc4KrYxOImGtP7@17>Q}nf7G9NeTs%kPyx*niCa}ycwK4 zJZ~J!0ljf*DKbC$t-atKC}k;hJ>#8=jx2Yh9PhL>Zk8>hR(D>1#L6Z+9)V_ZkG+_k}Jf9Vb{3?=~4yUZo4~c)bI_)nMv_OuMREM3Y!c}Kwww@%5&1t zsyNWydC8D*^Hws<5mb3_aAtOQf42FWOhJ@)K4{+fEd+J`?;Oc`zEsr#ni)b6TH1h* z@Ud{)Ypk+iu58@RK+0M6(A?Siu}uqi`@7kfz}7jpjRQHkOAlUjt~qkR=)D79=Aiag zP!49yfomte;yqey`NbeQu#l**1AQjhSR<7Gl3WDu9w!&4UwhKCaTUjT`0SV0(6m$2 zO?;nFZ^_fBkx{88v-24~l>dNu8aE2MtDrL>JTlsV0ao`ZQ|#93!=9vx4lC9Dqw>i0 zVeV<+z|-^Gj-TIP$y$yUHfW@_BX{}iG?zYrmtU{*^*YLs`P0=ikeqnmqd7Y@=&sR= zNz3|62_kGmNTjOmpr1LRp*NupoScbqht}?-n{pLB%2$Yph#SV^A#)YF-Amr%!WH2< zI)?eB`(~8fISIe2wV++J3}Ls>bTx5Z3*&xx2HUPgyWD4Ttk(V(3Fo!vR0bWCQwE<2 z%s?EYi9wlVW4TQXVABXqjnRxsn1DHdUQ+W1sFl@qhFfVbr`wVlOV#~F?|(HkfErtV4qm& zCP?3V0}{NIZCawZ4a?1|nfiXLR|Sfm(9UhYl|)17p+(hq0e{+L>ipSjmq=Cn8W*|9 zsZ_))#Omjv*VK9;mPJ+rn>Q zpCaz?B z?5l#M55z(Z+tZ_G^mS4aC$VfoNFPV~LF_`2m|)B}FLYBvuJ(%xYQ8|#h%(X2WA#J| zM$HOF-|sD4sv|OWhCW4wviL_QZmv%PgCR+-V~!L(IzHaZyEQ^c9_PC-2AOPnssOCj zNEVC^HFc65bpS3K9U+;D9Ev^TM9p(zChJkOIdCCnUn&h+tDru-;knI2nU?}X0Ogbk z9-VZ@qD5zZ4M~w(@(WVK!gc>L=qx<10LT^**WUQClg`fnPI>`W3o zC4hNeN&(DRU@Gb?){1w8gc0gjGYf3FQL%ZJdQ+rhOWDea43S(BPW7Vs$Wvqt+C-0y zRG_XLQ{=3MmUBjF?07@52hmEbym9vAr{;1cTes^kzVo2{rkDzStw}9n!BuBpv<34; zSViIHv_fbXROAzqq4H$u{M7Px_kDGpSi|D!ZFwc-%f0Q+dFAT1LlyyYwwdE zp9MJlXq>}{<@I0WH2E7EIbT2rzX)YiQ+%I39cC_p1qVfxHyuQs<^Q)Sq#xth6Ud2* zbJsC49f~HCu@wK=NGQ!{%ftvV=_`S)q`0)B8dlY8he%dx1*w55nR&62Z+&zt=|kmx zL0WLJS-W9ohLnF}dUI-%28~0`(Tc5@}JW@LY8#iM@xfer4S zzG@Sl@r%4H67u98PSio>v19lHRS9G&ni6QaGAKG7xuCM&TA)fQS!idUJwp%3OK5be z80Ji1WXs2u%L9+1WNi~8PGZt3n1>ar=GUbM-|M%=%4vs-&0CHFAPVy{jaUa%Y)~uc zX2V2&^iRRTOY6G@<;gaur-cQE8h>}xK5E>#|I*1ju}JkA5F$?X)y6R7_zO@=MG~Po z{SVfP6SLL`RHkm7Hr@;yVo0(8^#fn%HW8G!Z@Eas?m~6k(ZFxnu@ePvLRBk0=H6Fq zWjCIRmy=%L&}~S5mkZ`Z6vZHvDeJEQlbFFUWBq8p2RPC5;`_>#8UlD@PN8M#riFHX z@oETT_0h~kNsj_=1KoN~=Y1eq61$Gj07nBUG+uL_q#?ay&M}`>$xTm017xWxiT#?r z)VHZzXvrL7G(j~R%$Dl4__h%5??zjpFr_EnuS$*;{ex0fphW3_PF8W z(dc2Pz-GdSh@Sj}0*&-MBBE!`I^nB$F^YUOPvM9zZ=6!SG>?bCxCUS`<|L59vqNG; zg=%SB2uioxW<4`6(OyuceRWYZhYQGk;hgRuQ4oZ{rGt?|h0dNRd#nLhO5|LS5j7kT zJZ%#UZI9aO?qVf&N+*i#u(A;r4L6(n&I99hU_HG$7J(LThNY|iv^pyxgoxCW*SvrcJvDY} z6++(D&4Rp^+rSegu6Dgb>&)luoX=rK+Ez$w*;ds?6R-LFdBm6ut)0^jJiK)1o^oVh z<9$;l^L3oro-ZtBzqG$!6gz~(S)BhDU+TIK!{KhaElqONAR92EY1lfq{p_c?zYo#J z{Ax?c)tD=G#xqTbUod78Ca;BUF~~x#f$U zb150gqnC>u81OvLlHiFYBaxFcYTB4>_zCtyFlJ|tKz3u$j%n4iuWnAEtBMPv^n^!8 z*Q#7==+pd4t`FSyL3g=ZD;;NJKv!{4nW8Czzbb)zCywew9gHtx?w#ZsPnV>O-1FBH z9kR|9Dcy%_e(9B2Xm@8~2-b)&MT+Cf*W~j=EO4;+1Wqkq|MbdB8w-td*_IRqG-N}Q$>b{@(NX%rUT3r(QsK26860s(Y7?;_16`G(}php88-)2>jLhR4HiB{$}+zq}UaTQau zidLHAZhA{(hXbrS=&Ln;`qbRzt?O(ks|RYwp<Ira-z1hW*D}2Sh#+U<{?PLf^`Wfqp18y^$ zw99!7t2g$&MZ0Hi6C9CN)t}JZtGJ|P3ASWoDs21x3x9BRNOr6y>r{F4gh+&4X|r|O zL#^LgI>v~Ux1i~oqiB^{a&hdPwc>ksf4K$PMAy>Sv<+M26r{JC{FKde_bJI+yz>(Z z`c~W%`}e&4l2|=o=Mncs=Bd$4#qZuF1ou(f{m*p!8@mGpUMyZ=785Q&m z2Qw3so_&-We0R-oa-;1iHGAIP%q3IYZtWr4J}1gH4i&v9+oGtGsM-|>hg@lQ+dO0- za%;)g#Nb574(cL=J~mgkOnB3Lk)cGb;8SG*BP+Bz;J zvbDpK0HJ*DHou%BY|CHbsU`#4H*DMGeSTM0Qx>k4C^~{30xiOin&j;+wjQyHlH#q# zV+cX4*%FdJ)2zI#saQfT+8ztk(}`(P$5Mqb_vAQoL(9|3VtJQGnfuLvj0yQb_GhH5 z%}hAC0r41Do(3UTg7m`?-)=2J4m0MTf$N%JR(mzEk$~A%47^q;!=cN9?}^5k znZ2xh2kIC1;7;6Q)f8LEn8KZbGh)BPTv}}5+hI{}t0+uPM^c+EgMQC>j=65j+5FZG z`A#T36=D@jt{s`5!+s@|<%--^qG{Wb#r0{~GCXLr+UXX9x@x0F`Hvn>$upw9z6hG| zeWDdV(!#Mv+6J!@_d%v7cV2;dp2xr_eNvMax=(VaBZk$m+pm1|F9%Q}z)DH}c6Vx& z&1GG$mChW_+*|a6avYeK*u?RDaMQuP8=k_|mlEU+cH9>!=LK8nlKw1?2!fO1NR3aE zMcpQAt=$&+x0o7Pzsn^?7(Q-|nLo^^n4M5;c~}r-JJU0~z&!XFbert488xb~v1zUG zXkH`(HzJ`sA&4^wxaA+j592>I^sT%N&goE^L2Hh4WKNs7R+UZ^rsK#Aw*C{X-|Q1> zJi{{^7|o&<7@fjxa{Dc=-0JcDhOUP9M21`arqHuvQ|W4AcT>HNR<7W6c^AfGb*pO6 z7BcP4oDp>4Tc^3r-6k%uTKwgQXD>TLLX73~uaPvpuUuT$o4d4pfgPPFRXJDtSbsKS z9!UA0H+gfL{2k@X9c(d<4QV%i0VtXFZLk=A2`eiHGS(|Ug!LEFvfUWSnq9XZA(6?4 zHJ+n-o117Enjy-O2FqFU&emO5P!u3!I!@_cg~7!G{G%&DgNT#8MTw91k4 z?EF%2J7D0nNg)^b2gtODzb~yQV_oj$mh3hi_|4%vT(3N3E67Jgc0{N4Fy2DBYdDHD zVz{S;!;~1?o1Zf2C7~K|B8QPX+ohd3aOP}Zq_v-BtMl%zK|p~m%mXftHZHc_wghiCr?BO>av-Vc<)AYuU&z^t&nu)Q zO`PXL2UdT|1lc}`I=?d^w{X|i#hpkuquN5=<^#pzaAfsCi@`)hP$h=6e*Ugpj*HXB zw=Ps%sN`#%zr2jq6grXPrt8~&q;1%{!^8WT?S@a5MIX;=dth#Xmyl^zU1qK3T(wQ) zeTfhwDc*aR6<8kWcB!LbrG2VfvP46*#D{od!*h(ljUH`b{Pa6GU}ox4$8JbiFHbOd zXSmol=ftf^@5&tmEG9bQ;C0%bZEHSiKa5Vhmomm+M`O0f8C4~+JY5!;)V;>SuIFfM zk*0&)dEiWDrBkR>+eZ5bv&HXn(Q`TsU$^4mb@Uu*2B^!uDrCPNdc%NdYkE;RD(Sy- z@N>g)KqMuv(?+u>WITOcX}SIvaz`Gn+hG&0GwfmrmqhK|Sw5uO8JXaGOp5YKMT7>D z{C!`TPhexb&(y z(SjnbF<^ngiIGg!m4*qW#Dw-A8g0hX&B+B_x3i2=f<5lx0eZsJ%*>lP9|BMR3U8yt z(9ifC%8p}mbV~3#hED}|Lo6r`>8P_%fEN2pOxRR(taNjb10Th*SI&TGOk8kmi;gjJ zg)|qcG%455*8KKMRCh&(W8GZ7e(F%WCBJ$~BA^p?7*%H{S8Nax4!mH2Sta%xyj8jC3V8~Ka{CV= zL}?5^)1Esmz7ufd$Y(4Dam5U7{Qdjp_y!l|UV6;SmO;1#b-`Z4SM_B6Wl;YZ=d zMCs|)I+umjQJg$EIO+a8 zuyv-Ku;H4VGq14!xOWJzQM20bpY%5?vV8C-&@6U4MtvT3Fxg<%&HvT!WS-gWCik<& zX};ZR+H+t#b&5gnVWGvq)#r7B@Ba6-!(%u)YVIxGXC`*@HPL}QcD7pcbn)T*jPgD& z#&QA0IV)0dYT^=C+nvh{-h}n+P{*0QI(58s=F!_#LuYEUfpm*vN}c8FN-9I)8WvDe z*V^CcTGF;*j_YE@BWPw_NA8?$75f(@L^15UwUI#r;&n7nlBo*+%{{p)e`Hb`xhj0~ zWC-FQ1KKAHIFFXe}t!PGL%Q$U-0P^k+mZcf(XD zaxU*?ktg13J^f68bpR=hV8@l2vpdks+oxW~;AF~#QdSLV9 z=iU8!d$9ZXY=>Ob_;jUyi1~u>PQFyyy?>z^W%!KLNjtSw*!gWxQAJwHbyhIP(65=B zUqG8g&~3I*k?&;@&vRB#cWOofg(#aSm!W+p57?~J`Gz0xV=wP-K4)T@x)m^MlIhA7 zA9zG`XLuX;LiGlEYkZ%1<9fS!Uw>zMoA_Ar^#2Qf9|_o;snv*@Sg167S$;d6c-pCr zT3X$mvY|KpVFg?vVw_1mgxu=Revo`C7^&sYzH@x$D(zu--g_?+d^#K&+wBxQ|6BUE zS@3l0{_w5AFo)?m>RsfBDGvAyy#3p1P=h5|JM%p8UeTlTO8Ka1c~hAbuvDwSh#eJh z=dfyc>bR>%z{rIC@ni0I_y0TdZWuLjKs7#GTFUDAO747<;8y0>=- zeC|VB2MzfcJMa9t+Ld%p{6!x{w5E=m!4a|j`jg|zx{-V{-9x%}bqKiI6_8VaGCmWS$!E>HTg6O1|rUY>i}FTnaM<_4e`2VXu8MA*XjzWA6hF?O-v05353 zZVUeoenUWe`9pGPi4CN3O}Ljx({xSP+2iRZGGzSSKq3||9*wQ3`F4H2>xGTYl4BS}9kkgzdhzmZsG6D@XM9}fXyE@awKXN{W(&|h&7LLt zM2pZ|gTD(GPk>3KirwB%=xypAPL|4_-O4nNVMv>E`A*bE=UX`srX?CbJgF(x{1{rDJtqlin1HaD$I> zf%)$+(5}JQ-_l){Ia@pi57X+GsK!$|tAgfQw+RMXTiEj#M-rCbxgh>>NE+}W0+c8b z1m}Nhy!3QXLBvpOJly|Lhh?E=$sw5kDVX3YC#WFu4+@XTzo1qlQ z{tHmC0u_c2$^4&q(1(Ts*0_Z-!2b^j26zkwnwa3fAn)|h4nz>_|5Kd?_3pzWf@9^N znauxTeblS}WBF*hD$s*u`2hSsCIoOH^5<`o{{a8j@~-~5ne!hiEetVuG5YhI;(t(y z!NVJ$Bjq7D!5^Of1W4BZfgu5NcEUJ#{Re>S5Uk+hOaK51G??zjKfO8!Fbs|=0|-Ncn(_h4V1h~j7!uT3 z1i%2zbO9v5j5UBYNYG#@00aEB9smUenyUB!{BSjb51IfDkf5dN50@nvKmrAVtN)0a z?E*A_6*~U8h?+jIe{}y7Rly%o)uVuRa97{I48z<1t0xZq(>uF9GXC)QL4p4C0;a&c zQ~&ch20uj9xqo8%=m+-pGJp}J{{+ASJ1qkAp+Jt4A5kC1d?=9E><8e72nNe;08Su5 zxQhTxu*?=9?<3>o5Ap9VAP4FnKLIA_Lu>|H9s-UbK^$BE(dqx8Mu727{~1qq0psAQ z3&0U1DC_r!2)zDhRXhQRfHr>v!ojTf|8&R;02YXG@xxep0&oTYe)@OSmMef+$|0ON zIQ{LPYJCSVYd(ZigaUni`bf_P7On~W1`VeJ37UQV$oxY`h5~)tghWj8wZKQ^VP*f{ zO$%(?JRjR4KDhLOUAoq;AEZxj(}pZWd-62B8)+5Q)rs`VKdlK;8LTH#eo%+^XJ`t| zJKrxmJ|IdVGbV12JD;Iu-Q1n#aPfZ{jUgu4kwBN=@xvo+r6es9TL1{~u)qjX*yOG<{R9Zd^;1X+d>ON=C1PF#$%=m)$6DdWc_W6i=TGQ{9d%74%v zg+j-VeqvAq+L(x>W%mmX;IJA47LSlqqH*BtAfyMWY4UjED*~Nqq3h^Ap=b#YFhjD5 zl%e2?rb0N4B27+Q$w0;>n^wb&TS0gAk1F91=?HNk!Q|Z#g8Ej` z2Z52Nj0dswRnmea_PZH#Xh8pawB)vEuV{L5l_;hG(7H<8LJL|`#8f*NEzcQ)%t@h% zV$i6MdnaGT3QAA8q*tx;(=bwju~A&#@2s$FfUfDR_==WL%J8sC-)ByZpXFBAlX-RZ zp*U(Q+&^e#k>cfM6#%t>T^OdOW57gI45b?lH&Gkbr>wlIAEI(A;k{O(YaxiZ!pOmc zD}BT8K+D15wd+W1v{R#Xslq3tB4&7+J`WF8BxTaJYK|kBs-eqxoo&o$5~N`sEn&)qOQ>$ZFrVrQ457=B^ zB4M3@GQpLC+&F)U|85qNteSq&ga}Se-C0(s0}7r~6yhqw=ybrki2bY^-cf3`5Cz$*vSh*RZ9II~L_i%t%l+!MY`3{Z4gpmI))h8dff z)HmmrW5%5GJ&O^mqVuZGz_oWI-_ap3k)-ZU?{_8D_pP-&1Cm2~1cbE!UaQ z5$}76dRZW=yT1<%MM8i+<0zX+-qm2#y#H~AE4lSl{Z>tHE?!t_cS+$Jz*Er0Kbi?`V|@RRH_T2HeQhZ z>SQv*?3kGbZ%Ia;seR-j_fNGlk_&w1Lr!1vr+L-7$S(0Fb_80OpynzoRXP&BCog}X zyQ#g6NVoox1dvFKuyPNvcE3pB9Z4OQte8RF{s%Y+K{M z`(-xk3>vS@vZm&*%9_-y>CpH(UcG!>pHJ-{wh583anfJ86V#Wzwii;gpe zXIkpunUR-m=2n1DpKCmHZ5;|ES}Qh85^+$M1J z_>zWlH#9TX`aHnNh(Q3pz0cX{OIFZ1sBdJOwiV7HNU=}<5;w7gYP*n&uoo-5sb(bXE6jN z>k<#{fG0nmqPI=`{p?a0$V!l+nL5agQVOD+w!xjiDpyZ$BsHvtez@N|_-sd-%0!a! z_=Cl@`%+C2NNWh&y9~$dzE{5pMyUNNmGVe)*{c(Ax%&1?`vKakwGE(dy0M*~IM9pn z2M&iMV?mzJtLY#xY^c?Ff<6vi5>jP5*)p_-32#Hjl8C8hhG>fy8022qM>2z?K@SP8H6o_n1#QRyJpqLc3y5a8eJah(*)Oi%bi9xO>`0guqH zi)`#G6KV1a@pH)9>9_QCGu5~ud_yPkAD-jA>+gT<`g0I-_l|Ajqt8YrWZJkQz`V!w zQjXkE1ypBfvu&msz*}a5?=!|HO7cm6m}zZR+`fOZSrUA7ydxgcv4;fbDz^W^KNMcr z7q(#z`z|heH4WN%rt)?Yl$Ctb2=J@R@f+XW1e;k{PuCL6{Vk1xp}V@YboDF$&c~He97r}01}{Ia{94t1`j))7m?`&IC(zca6=>u^|@IMs3*_UnGk z$7JYl?7s*u&*G=YV6bae-|~C!{Cg18sMHwh%CZFM%HA15vK>!6IrFOmg(NvUCM|x& zt;ou_lHuX?8o2xR4yE;YY!+h_w&QXvqt(v)GTS5F=x|lT`ViHx(lvu^yO2R|c^9KC zOJixRg>>#vPqdM(|4WN9Q+#&xbHc47u}jME{V8%tl6Xselo(I-0Y_%3n?JZzQEc^jvYlIJe96pdhqKyxe9~;0V3hHS!a#|?xf_5H0OCt zqZ!q8kB0dttnF88iyjk_b7tuQui9b5VBnSTx`v z^Z%$(9+j-Rh1!CgwAK%u-$Ph&9%Y~Te6vM#Pk?KToo0qSbriY7o@9VumRc zLM8B{O>id}IP>QaAFSn@IcwNNCmQMHFzyg>_k0!=UIq7Fk&d9`9W@a=qDT%gC3&gL z4+~Rh^3@T1&Tu%EdcZ!ih`wJ!Vf09qClI6Hg8@RX)k%%0+@5FlH(fxT7mf##izo zoEfr*8YkckKS6J09X~mt*~gg&#>v(rjT79*oT!$tj@ytH`Bh_WZsxaKKO%QX-<3K@b_OF+sR zD2N5e-$y=zx+c&HTngKxT^2U?j?Du?SHCB|GsuWF_l%x{i>ovpvLb*d@+4F5gfu!( zNsdsB77uNaFJn#UokTNN$|A7cmR@Z7^dfKc{=*&J7nnRE?(fF|VnS&XoI==bFN+== z{`zdNju}C1qSQMV=Pco{E!OmngNGi7FPXVExp6nE#017%6BJ`_adK;05UY*|Zc#bM zcJX-A-J_S)j+=>M_yKA|uRLV6`*Uu7x~v>>pZ&mxU}TuDu)j zU9%WQQSj3o{ZCprT~jwFyl3SI(l%Ij|Xmg!T1~9l!4uAbM(T`?&+7> zvE-1&&H>M+F6nveuMaR+=m@ye`xmVT?m^yyvH`F?rK*q5(L$}7{L#Q@$)29*#vMOY zMQ5XfC7{B>P7}6b;#RKj0Ly-wR_)ufcB%kHm*ZlvRD9tqS3P8BiD6gscaEqY3ZumD z+Vayh0^kwNFS8$1?k>gv;xt-N0UMb<))NV) z!0Ikzj>N;6AUhQzxahGqEetbf!6>TQzjdG6?m9_A@V>xS)WmdmAB?>yf+%(y|L^t%4rX@Z|NBfkB^O6C zcT;m0kS89TJtZ>_Gcz*_D~ApZJd?bmnV7MgIWe`^H&$j=PGx|{`U_(7Y7@tZWKTROwkx~SZ7A^N{G&p>fE{P;?iUJR*X5EC*+EcX(MW{!l;tj1dmqEF>YmF4mJMC z=S0h#I~pzpE_CFZdEy-sWo@sQgp~%bnfT{29y%JQ3`#22OG>2?rLf;lKs=1#vT{>Q zG5d_lN{g#<|Fip(!%F)OTg?uSmzO(=A_TlNsXI;g=ye41uRrU$w~uW$m>q1s;(Bsi z;5j<_=c@Xa?kmzPxq=#ju{nqp+SanQH)@VOz2zoZ8di9H&_}iL+6F z`6R9|C#3rU0h3hKwvhxQv& z6ZCnMxvL3nqXKB9ilh}kTdxJH3e#$2?C%d$?POVEr1Ds5=Dgfr{hzgUCfPx(PEs*Hz;h@rXJa_)* zfRe!st?vHoDPH@yX5de6OGKR+wkSrs?E$(I;QYHgC zw^z!bRxF1{$?%t?SH?e3?a z;Eo)vfNIT|4s1Nbxn%pG1NY(ZV)tT(+xlzv44+*Bfmw_X&gYEX9&-l-f7)BP8G)n) z{AQ{w_Z=9TbC}pyXCzFJE%)OR@f}b^EVWWekBl z#MjYoZZ_!!1RqdNOCg+b$6l5W&{Ic%8ylWcL@yW2Y4a_=7S{6}Zi53Jd>)mk2x_1^G8RxCb2FHF&i<c*GdB!3{%m`8jCB=|#wX6d@Lo(ux$Y#~&3$d0FKnCjSV>rbjFaVT9!E>ERN5p8!q2OMCA&Pie5Sz{BHILX3XD_EMlXkh!x}zE58C#x%k#iT4`&90 zqg)WJhBBkUNiOs_VO*Gwum@hu5VZu3ya!^;aE=h0oa19g@SCKa)yb-T?41yu2II$h zNREP=SmU@rBg##Z&Z*?{ZGjpQ%jO6dth5--C~!gpl-uqxBB#wQ0uBkvCVC%v4bd#Xq0;BVbQ8S(e=^J5{PiSV>f>* z_bhH2XGF-CCipop-egcBzFdF>#gB)Qe(cPZ=Qg+i6vp0^SQKGKl7sjbBu{-_h;$_2 z*8c6@fG?I!SEvn>1G^ci7-81^Pu^noAF-)V-@;SAMofDn&;G4|Tgra3t`xMY956yX zYngp6xUYY1yvZjrdsj?Kfv(=Kr=x<}^ ziKNOvK&_Af=l?pV4SKHu9kqjO(4f`Iee3dBEA!SQs6VTPawJ zo25#fn^}`M`P1{}(t}$^<$Cj$_L-xA95V zRA_|rnplrcn5{8$2QRfndeJo>wC5o{Y-(nrx!%XN!|&MyHtPKN=>k_*C! z5IW;=d!LQ+K1F`djBW1|M%e1Cic1#$Y&Bb9id$Px$gE}xWn?#OF$*biy`!$t`c@6p zATe@o3K2Xk{|Yty!`X;*mM@0-2M%s=!lU>PbnDx)wi029!9BOeaops)c#=_1`rrM>UsD_*FQg5NSpl-+tf|KRyT7HgAq5Q9-J~lFoG9rAd2wcK$h|& zKXb-jLAw7gFpDzksOFYlnt*L+Rv_sBWF}Adl^- zf0BmzjZq3oXQ#JofuKR;K5wxOv&J6u&0**nq@-Q9*1l;pG_6ZJ6{)Kzi&^#fNTwoA zm%~<`h{*&h8rOKeGA_rXdv~9y)GC*b%T%vkbQiDqDYM>Qd{TA0qh0Pp$8|3YjH$cs zeNjQu5V&blD@<_|AXzfw$hKFR(r~2!+0>60B&m>9yIfF}D{Hj*II~3e@X-DtZiI8pmT$e4>F34%ve}V**%uQWcR}vAk#l+67d&Gi z)q3yUkL~w3A&K67BK_FTvD2{yM6YS=jp+`~rGx9ylOI(ifBl*2^zstSJ45l>MY^*k z_~qS~E!!VQidr3=mcU0ZgSilnt!s#*G44}ACQD7DFf(*0#M9IQBzlJyG1VXs$uyj^ zLb$6+<_!WdRXfkzl=su2*azwIhZNy3&GbF%_4huPWBuCI-=c$3KB9L<*w8$~|Bv=g cSGSKoR4*5E3wRD5E>3n1cnS(}MG5%-3)D3XX#fBK diff --git a/doc/dcmotor/dcmotor.tex b/doc/dcmotor/dcmotor.tex index 0af0fa2c..6d91e476 100644 --- a/doc/dcmotor/dcmotor.tex +++ b/doc/dcmotor/dcmotor.tex @@ -113,7 +113,7 @@ \maketitle -\noindent We review DC motors and describe MuJoCo's \texttt{dcmotor} actuator. The equations are derived for brushed motors but apply equally to brushless ones, where electronic commutation reduces to an equivalent circuit. +\noindent We review DC motors and describe MuJoCo's \href{https://mujoco.readthedocs.io/en/stable/XMLreference.html#actuator-dcmotor}{\texttt{dcmotor} actuator}. The equations are derived for brushed motors but apply equally to brushless ones, where electronic commutation reduces to an equivalent circuit. %============================================================================= % BACKGROUND @@ -789,7 +789,7 @@ $\beta$ & Damping decay exponent & dimensionless \\ \section{Implementation} \label{sec:implementation} -Here we describe MuJoCo's \texttt{dcmotor} actuator. Some scalars are grouped into vectors; we use a colon to denote such scalar sub-attributes, e.g.\ \texttt{cogging:phase} refers to the third element of the \texttt{cogging} attribute (see Tables \ref{tab:mjcf_attributes} and \ref{tab:cogging_impl}). +Here we describe MuJoCo's \href{https://mujoco.readthedocs.io/en/stable/XMLreference.html#actuator-dcmotor}{\texttt{dcmotor} actuator}. Some scalars are grouped into vectors; we use a colon to denote such scalar sub-attributes, e.g.\ \texttt{cogging:phase} refers to the third element of the \texttt{cogging} attribute (see Tables \ref{tab:mjcf_attributes} and \ref{tab:cogging_impl}). \begin{table}[H] \centering From 68fdc439e4e57a37d6feb9e0b0f95f5cf3cf0ec9 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 5 May 2026 02:47:13 -0700 Subject: [PATCH 185/251] Use mjrTextureTarget type instead of filament type. PiperOrigin-RevId: 910547104 Change-Id: I3e8aa30a72b9b5ec8c45e8fa6bac18353ce7b485 --- src/experimental/filament/compat/scene_geom_util.cc | 3 +-- src/experimental/filament/filament/renderable.cc | 3 +-- src/experimental/filament/filament/texture.h | 3 +++ 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/experimental/filament/compat/scene_geom_util.cc b/src/experimental/filament/compat/scene_geom_util.cc index 382e6c88..6e9939b3 100644 --- a/src/experimental/filament/compat/scene_geom_util.cc +++ b/src/experimental/filament/compat/scene_geom_util.cc @@ -420,8 +420,7 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, // the programmatic UVs. if (textures.color) { - if (Texture::downcast(textures.color)->GetFilamentTexture()->getTarget() == - filament::Texture::Sampler::SAMPLER_2D) { + if (Texture::downcast(textures.color)->GetTarget() == mjTEXTURE_2D) { // For 2D textures, `tex_repeat` specifies how many times the texture // image is repeated. The `tex_uniform` flag determines if the repetition // is applied at in object space (false) or in world space (true). diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 279d718c..a620be37 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -383,8 +383,7 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { } else { return ObjectManager::kPhongColor; } - } else if (color_texture->GetFilamentTexture()->getTarget() == - filament::Texture::Sampler::SAMPLER_CUBEMAP) { + } else if (color_texture->GetTarget() == mjTEXTURE_CUBE) { if (material_params_.color[3] < 1.0f) { return ObjectManager::kPhongCubeFade; } else if (material_params_.reflective) { diff --git a/src/experimental/filament/filament/texture.h b/src/experimental/filament/filament/texture.h index c0ec1e0b..ba94a161 100644 --- a/src/experimental/filament/filament/texture.h +++ b/src/experimental/filament/filament/texture.h @@ -52,6 +52,9 @@ class Texture : public mjrTexture { // Returns the height of the texture. int GetHeight() const { return config_.height; } + // Returns the target of the texture. + mjrTextureTarget GetTarget() const { return config_.target; } + // Returns the underlying filament texture. filament::Texture* GetFilamentTexture() const { return texture_; } From de030f4664c6488c946584880cf7cab28226f94a Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 5 May 2026 02:51:44 -0700 Subject: [PATCH 186/251] Allow SceneView to read configuration from mjModel directly. PiperOrigin-RevId: 910548865 Change-Id: I4db045e76cd36012810c75b9d150c6c5f5746488 --- .../filament/compat/mjr_filament_renderer.cc | 3 - .../filament/compat/scene_bridge.cc | 99 +---------------- .../filament/filament/scene_view.cc | 105 ++++++++++++++++++ .../filament/filament/scene_view.h | 4 + 4 files changed, 110 insertions(+), 101 deletions(-) diff --git a/src/experimental/filament/compat/mjr_filament_renderer.cc b/src/experimental/filament/compat/mjr_filament_renderer.cc index fcd8f31e..831b6636 100644 --- a/src/experimental/filament/compat/mjr_filament_renderer.cc +++ b/src/experimental/filament/compat/mjr_filament_renderer.cc @@ -64,9 +64,6 @@ void MjrFilamentRenderer::Init(const mjModel* model) { render_requests_[1].camera.frustum_top = 0.0f; render_requests_[1].camera.frustum_near = 0.0f; render_requests_[1].camera.frustum_far = 1.0f; - - filament_context_->SetClearColor(ReadElement( - model, "filament.clearColor", filament::math::float4(0, 0, 0, 1))); } void MjrFilamentRenderer::Render(const mjrRect& viewport, diff --git a/src/experimental/filament/compat/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc index bc247649..f47b9316 100644 --- a/src/experimental/filament/compat/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -90,109 +90,12 @@ SceneBridge::SceneBridge(FilamentContext* ctx, const mjModel* model) scene_view_ = std::make_unique(ctx_, params); model_objects_ = std::make_unique(model, ctx_); - // Configure options for the normal view. - auto cg = scene_view_->GetColorGradingOptions(); - cg.exposure = ReadElement(model, "filament.cg.exposure", cg.exposure); - cg.contrast = ReadElement(model, "filament.cg.contrast", cg.contrast); - cg.vibrance = ReadElement(model, "filament.cg.vibrance", cg.vibrance); - cg.saturation = ReadElement(model, "filament.cg.saturation", cg.saturation); - cg.temperature = - ReadElement(model, "filament.cg.temperature", cg.temperature); - cg.tint = ReadElement(model, "filament.cg.tint", cg.tint); - cg.gamut_mapping = - ReadElement(model, "filament.cg.gamut_mapping", cg.gamut_mapping); - cg.luminance_scaling = - ReadElement(model, "filament.cg.luminance_scaling", cg.luminance_scaling); - cg.slope = ReadElement(model, "filament.cg.slope", cg.slope); - cg.offset = ReadElement(model, "filament.cg.offset", cg.offset); - cg.power = ReadElement(model, "filament.cg.power", cg.power); - cg.shadow_gamma = - ReadElement(model, "filament.cg.shadow_gamma", cg.shadow_gamma); - cg.mid_point = ReadElement(model, "filament.cg.mid_point", cg.mid_point); - cg.highlight_scale = - ReadElement(model, "filament.cg.highlight_scale", cg.highlight_scale); - cg.shadows = ReadElement(model, "filament.cg.shadows", cg.shadows); - cg.midtones = ReadElement(model, "filament.cg.midtones", cg.midtones); - cg.highlights = ReadElement(model, "filament.cg.highlights", cg.highlights); - cg.tonal_ranges = - ReadElement(model, "filament.cg.tonal_ranges", cg.tonal_ranges); - - auto tone_mapping = - ReadElement(model, "filament.cg.tone_mapping"); - if (tone_mapping == "aces") { - cg.tone_mapper = ToneMapperType::kACES; - } else if (tone_mapping == "aces_legacy") { - cg.tone_mapper = ToneMapperType::kACESLegacy; - } else if (tone_mapping == "filmic") { - cg.tone_mapper = ToneMapperType::kFilmic; - } else if (tone_mapping == "linear") { - cg.tone_mapper = ToneMapperType::kLinear; - } else if (tone_mapping == "pbr_neutral") { - cg.tone_mapper = ToneMapperType::kPBRNeutral; - } - scene_view_->SetColorGradingOptions(cg); - - filament::View* fview = scene_view_->GetDefaultRenderView(); - auto ao = fview->getAmbientOcclusionOptions(); - ao.enabled = ReadElement(model, "filament.ao.enabled", true); - ao.bentNormals = ReadElement(model, "filament.ao.bent_normals", false); - ao.ssct.enabled = ReadElement(model, "filament.ao.ssct", ao.ssct.enabled); - ao.quality = - ReadElement(model, "filament.ao.quality", filament::QualityLevel::ULTRA); - ao.lowPassFilter = ReadElement(model, "filament.ao.low_pass_filter", - filament::QualityLevel::ULTRA); - ao.upsampling = ReadElement(model, "filament.ao.upsampling", - filament::QualityLevel::ULTRA); - ao.bilateralThreshold = - ReadElement(model, "filament.ao.bilateral_threshold", 0.5f); - fview->setAmbientOcclusionOptions(ao); - - auto bloom = fview->getBloomOptions(); - bloom.enabled = ReadElement(model, "filament.bloom.enabled", bloom.enabled); - bloom.strength = - ReadElement(model, "filament.bloom.strength", bloom.strength); - bloom.dirtStrength = - ReadElement(model, "filament.bloom.dirt_strength", bloom.dirtStrength); - bloom.quality = ReadElement(model, "filament.bloom.quality", bloom.quality); - bloom.resolution = - ReadElement(model, "filament.bloom.resolution", bloom.resolution); - bloom.levels = ReadElement(model, "filament.bloom.levels", bloom.levels); - fview->setBloomOptions(bloom); - - auto msaa = fview->getMultiSampleAntiAliasingOptions(); - msaa.enabled = ReadElement(model, "filament.msaa.enabled", true); - fview->setMultiSampleAntiAliasingOptions(msaa); + scene_view_->Configure(model); default_shadow_map_size_ = ReadElement( model, "filament.shadows.map_size", default_shadow_map_size_); default_vsm_blur_width_ = ReadElement( model, "filament.shadows.vsm_blur_width", default_vsm_blur_width_); - - auto shadow_type = fview->getShadowType(); - shadow_type = ReadElement(model, "filament.shadows.type", shadow_type); - fview->setShadowType(shadow_type); - - auto fog_opts = fview->getFogOptions(); - fog_opts.enabled = - ReadElement(model, "filament.fog.enabled", fog_opts.enabled); - fog_opts.color = ReadElement(model, "filament.fog.color", fog_opts.color); - fog_opts.distance = ReadElement( - model, "filament.fog.distance", fog_opts.distance); - fog_opts.density = ReadElement( - model, "filament.fog.density", fog_opts.density); - fog_opts.cutOffDistance = ReadElement( - model, "filament.fog.cutOffDistance", fog_opts.cutOffDistance); - fog_opts.maximumOpacity = ReadElement( - model, "filament.fog.maximumOpacity", fog_opts.maximumOpacity); - fog_opts.height = ReadElement(model, "filament.fog.height", fog_opts.height); - fog_opts.heightFalloff = ReadElement( - model, "filament.fog.heightFalloff", fog_opts.heightFalloff); - fog_opts.inScatteringStart = ReadElement( - model, "filament.fog.inScatteringStart", fog_opts.inScatteringStart); - fog_opts.inScatteringSize = ReadElement( - model, "filament.fog.inScatteringSize", fog_opts.inScatteringSize); - fview->setFogOptions(fog_opts); - fallback_head_light_intensity_ = ReadElement(model, "filament.fallback.head_light_intensity", fallback_head_light_intensity_); diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 594b842a..8546940b 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -43,6 +44,7 @@ #include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/light.h" #include "experimental/filament/filament/math_util.h" +#include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/render_target.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/texture.h" @@ -378,6 +380,109 @@ ColorGradingOptions SceneView::GetColorGradingOptions() const { return color_grading_options_; } +void SceneView::Configure(const mjModel* model) { + ctx_->SetClearColor(ReadElement(model, "filament.clearColor", + filament::math::float4(0, 0, 0, 1))); + + filament::View* view = views_[mjDRAW_MODE_COLOR]; + + auto cg = color_grading_options_; + cg.exposure = ReadElement(model, "filament.cg.exposure", cg.exposure); + cg.contrast = ReadElement(model, "filament.cg.contrast", cg.contrast); + cg.vibrance = ReadElement(model, "filament.cg.vibrance", cg.vibrance); + cg.saturation = ReadElement(model, "filament.cg.saturation", cg.saturation); + cg.temperature = + ReadElement(model, "filament.cg.temperature", cg.temperature); + cg.tint = ReadElement(model, "filament.cg.tint", cg.tint); + cg.gamut_mapping = + ReadElement(model, "filament.cg.gamut_mapping", cg.gamut_mapping); + cg.luminance_scaling = + ReadElement(model, "filament.cg.luminance_scaling", cg.luminance_scaling); + cg.slope = ReadElement(model, "filament.cg.slope", cg.slope); + cg.offset = ReadElement(model, "filament.cg.offset", cg.offset); + cg.power = ReadElement(model, "filament.cg.power", cg.power); + cg.shadow_gamma = + ReadElement(model, "filament.cg.shadow_gamma", cg.shadow_gamma); + cg.mid_point = ReadElement(model, "filament.cg.mid_point", cg.mid_point); + cg.highlight_scale = + ReadElement(model, "filament.cg.highlight_scale", cg.highlight_scale); + cg.shadows = ReadElement(model, "filament.cg.shadows", cg.shadows); + cg.midtones = ReadElement(model, "filament.cg.midtones", cg.midtones); + cg.highlights = ReadElement(model, "filament.cg.highlights", cg.highlights); + cg.tonal_ranges = + ReadElement(model, "filament.cg.tonal_ranges", cg.tonal_ranges); + + auto tone_mapping = + ReadElement(model, "filament.cg.tone_mapping"); + if (tone_mapping == "aces") { + cg.tone_mapper = ToneMapperType::kACES; + } else if (tone_mapping == "aces_legacy") { + cg.tone_mapper = ToneMapperType::kACESLegacy; + } else if (tone_mapping == "filmic") { + cg.tone_mapper = ToneMapperType::kFilmic; + } else if (tone_mapping == "linear") { + cg.tone_mapper = ToneMapperType::kLinear; + } else if (tone_mapping == "pbr_neutral") { + cg.tone_mapper = ToneMapperType::kPBRNeutral; + } + SetColorGradingOptions(cg); + + auto ao = view->getAmbientOcclusionOptions(); + ao.enabled = ReadElement(model, "filament.ao.enabled", true); + ao.bentNormals = ReadElement(model, "filament.ao.bent_normals", false); + ao.ssct.enabled = ReadElement(model, "filament.ao.ssct", ao.ssct.enabled); + ao.quality = + ReadElement(model, "filament.ao.quality", filament::QualityLevel::ULTRA); + ao.lowPassFilter = ReadElement(model, "filament.ao.low_pass_filter", + filament::QualityLevel::ULTRA); + ao.upsampling = ReadElement(model, "filament.ao.upsampling", + filament::QualityLevel::ULTRA); + ao.bilateralThreshold = + ReadElement(model, "filament.ao.bilateral_threshold", 0.5f); + view->setAmbientOcclusionOptions(ao); + + auto msaa = view->getMultiSampleAntiAliasingOptions(); + msaa.enabled = ReadElement(model, "filament.msaa.enabled", true); + view->setMultiSampleAntiAliasingOptions(msaa); + + auto shadow_type = view->getShadowType(); + shadow_type = ReadElement(model, "filament.shadows.type", shadow_type); + view->setShadowType(shadow_type); + + auto fog_opts = view->getFogOptions(); + fog_opts.enabled = + ReadElement(model, "filament.fog.enabled", fog_opts.enabled); + fog_opts.color = ReadElement(model, "filament.fog.color", fog_opts.color); + fog_opts.distance = ReadElement( + model, "filament.fog.distance", fog_opts.distance); + fog_opts.density = ReadElement( + model, "filament.fog.density", fog_opts.density); + fog_opts.cutOffDistance = ReadElement( + model, "filament.fog.cutOffDistance", fog_opts.cutOffDistance); + fog_opts.maximumOpacity = ReadElement( + model, "filament.fog.maximumOpacity", fog_opts.maximumOpacity); + fog_opts.height = ReadElement(model, "filament.fog.height", fog_opts.height); + fog_opts.heightFalloff = ReadElement( + model, "filament.fog.heightFalloff", fog_opts.heightFalloff); + fog_opts.inScatteringStart = ReadElement( + model, "filament.fog.inScatteringStart", fog_opts.inScatteringStart); + fog_opts.inScatteringSize = ReadElement( + model, "filament.fog.inScatteringSize", fog_opts.inScatteringSize); + view->setFogOptions(fog_opts); + + auto bloom = view->getBloomOptions(); + bloom.enabled = ReadElement(model, "filament.bloom.enabled", bloom.enabled); + bloom.strength = + ReadElement(model, "filament.bloom.strength", bloom.strength); + bloom.dirtStrength = + ReadElement(model, "filament.bloom.dirt_strength", bloom.dirtStrength); + bloom.quality = ReadElement(model, "filament.bloom.quality", bloom.quality); + bloom.resolution = + ReadElement(model, "filament.bloom.resolution", bloom.resolution); + bloom.levels = ReadElement(model, "filament.bloom.levels", bloom.levels); + view->setBloomOptions(bloom); +} + void DoRender(filament::Renderer* renderer, const mjrRenderRequest& request) { SceneView::RenderRequest scene_view_request; scene_view_request.draw_mode = request.draw_mode; diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index e64be22e..0187ee0f 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -94,6 +94,10 @@ class SceneView : public mjrScene { ColorGradingOptions GetColorGradingOptions() const; void SetColorGradingOptions(const ColorGradingOptions& opts); + // Reads filament-specific settings from the mjModel and configures the + // scene view accordingly. + void Configure(const mjModel* model); + static SceneView* downcast(mjrScene* scene) { return static_cast(scene); } From 88bff84cbb95df63697f3f7ec62af694ae672062 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 5 May 2026 05:07:40 -0700 Subject: [PATCH 187/251] Add API functions for creating and destroying objects. Also provide a header-only C++ library that wraps created objects into unique_ptrs. PiperOrigin-RevId: 910607824 Change-Id: I0427f6dedab6aa299b3128031b055bd4d0c8e497 --- src/experimental/filament/CMakeLists.txt | 1 + .../filament/render_context_filament.cc | 57 ++++++++++++++++ .../filament/render_context_filament.h | 39 +++++++++++ .../filament/render_context_filament_cpp.h | 66 +++++++++++++++++++ 4 files changed, 163 insertions(+) create mode 100644 src/experimental/filament/render_context_filament_cpp.h diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index 8b655e9a..9c9a2434 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -23,6 +23,7 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} PUBLIC render_context_filament.h render_context_filament.cc + render_context_filament_cpp.h filament/builtins.cc filament/builtins.h filament/color_grading_options.cc diff --git a/src/experimental/filament/render_context_filament.cc b/src/experimental/filament/render_context_filament.cc index dbe80ee1..7f70e700 100644 --- a/src/experimental/filament/render_context_filament.cc +++ b/src/experimental/filament/render_context_filament.cc @@ -23,6 +23,13 @@ #include #include #include "experimental/filament/compat/mjr_filament_renderer.h" +#include "experimental/filament/filament/filament_context.h" +#include "experimental/filament/filament/light.h" +#include "experimental/filament/filament/mesh.h" +#include "experimental/filament/filament/render_target.h" +#include "experimental/filament/filament/renderable.h" +#include "experimental/filament/filament/scene_view.h" +#include "experimental/filament/filament/texture.h" #if defined(TLS_FILAMENT_CONTEXT) @@ -136,6 +143,56 @@ void mjr_defaultReadPixelsRequest(mjrReadPixelsRequest* request) { memset(request, 0, sizeof(mjrReadPixelsRequest)); } +mjrTexture* mjrf_createTexture(mjrfContext* ctx, const mjrTextureConfig* cfg) { + return new mujoco::Texture(mujoco::FilamentContext::downcast(ctx), *cfg); +} + +void mjrf_destroyTexture(mjrTexture* texture) { + delete mujoco::Texture::downcast(texture); +} + +mjrMesh* mjrf_createMesh(mjrfContext* ctx, const mjrMeshData* data) { + return new mujoco::Mesh(mujoco::FilamentContext::downcast(ctx), *data); +} + +void mjrf_destroyMesh(mjrMesh* mesh) { delete mujoco::Mesh::downcast(mesh); } + +mjrScene* mjrf_createScene(mjrfContext* ctx, const mjrSceneParams* params) { + return new mujoco::SceneView(mujoco::FilamentContext::downcast(ctx), *params); +} + +void mjrf_destroyScene(mjrScene* scene) { + delete mujoco::SceneView::downcast(scene); +} + +mjrLight* mjrf_createLight(mjrfContext* ctx, const mjrLightParams* params) { + return new mujoco::Light(mujoco::FilamentContext::downcast(ctx), *params); +} + +void mjrf_destroyLight(mjrLight* light) { + delete mujoco::Light::downcast(light); +} + +mjrRenderable* mjrf_createRenderable(mjrfContext* ctx, + const mjrRenderableParams* params) { + return new mujoco::Renderable(mujoco::FilamentContext::downcast(ctx), + *params); +} + +void mjrf_destroyRenderable(mjrRenderable* renderable) { + delete mujoco::Renderable::downcast(renderable); +} + +mjrRenderTarget* mjrf_createRenderTarget(mjrfContext* ctx, + const mjrRenderTargetConfig* config) { + return new mujoco::RenderTarget(mujoco::FilamentContext::downcast(ctx), + *config); +} + +void mjrf_destroyRenderTarget(mjrRenderTarget* render_target) { + delete mujoco::RenderTarget::downcast(render_target); +} + void mjrf_makeFilamentContext(const mjModel* m, mjrContext* con, const mjrFilamentConfig* config) { // TODO: Support multiple contexts and multiple threads. For now, we'll just diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index 0edf75c2..377acebc 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -421,6 +421,45 @@ struct mjrFilamentConfig { bool force_software_rendering; }; +// Creates a texture for the filament renderer. +mjrTexture* mjrf_createTexture(mjrfContext* ctx, const mjrTextureConfig* cfg); + +// Destroys the texture. +void mjrf_destroyTexture(mjrTexture* texture); + +// Creates a mesh for the filament renderer. +mjrMesh* mjrf_createMesh(mjrfContext* ctx, const mjrMeshData* data); + +// Destroys the mesh. +void mjrf_destroyMesh(mjrMesh* mesh); + +// Creates a scene for the filament renderer. +mjrScene* mjrf_createScene(mjrfContext* ctx, const mjrSceneParams* params); + +// Destroys the scene. +void mjrf_destroyScene(mjrScene* scene); + +// Creates a light for the filament renderer. +mjrLight* mjrf_createLight(mjrfContext* ctx, const mjrLightParams* params); + +// Destroys the light. +void mjrf_destroyLight(mjrLight* light); + +// Creates a renderable for the filament renderer. +mjrRenderable* mjrf_createRenderable(mjrfContext* ctx, const mjrRenderableParams* params); + +// Destroys the renderable. +void mjrf_destroyRenderable(mjrRenderable* renderable); + +// Creates a render target for the filament renderer. +mjrRenderTarget* mjrf_createRenderTarget(mjrfContext* ctx, + const mjrRenderTargetConfig* config); + +// Destroys the render target. +void mjrf_destroyRenderTarget(mjrRenderTarget* render_target); + +// Legacy API, to be deprecated. + void mjrf_defaultFilamentConfig(mjrFilamentConfig* config); void mjrf_makeFilamentContext(const mjModel* m, mjrContext* con, diff --git a/src/experimental/filament/render_context_filament_cpp.h b/src/experimental/filament/render_context_filament_cpp.h new file mode 100644 index 00000000..8df24b64 --- /dev/null +++ b/src/experimental/filament/render_context_filament_cpp.h @@ -0,0 +1,66 @@ +// Copyright 2026 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_RENDER_CONTEXT_FILAMENT_CPP_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_RENDER_CONTEXT_FILAMENT_CPP_H_ + +#include + +#include "experimental/filament/render_context_filament.h" + +namespace mujoco { + +// A unique pointer to a mujoco object. +template +using UniquePtr = std::unique_ptr; + +inline UniquePtr CreateTexture(mjrfContext* ctx, + const mjrTextureConfig& config) { + mjrTexture* texture = mjrf_createTexture(ctx, &config); + return UniquePtr(texture, mjrf_destroyTexture); +} + +inline UniquePtr CreateMesh(mjrfContext* ctx, + const mjrMeshData& data) { + mjrMesh* mesh = mjrf_createMesh(ctx, &data); + return UniquePtr(mesh, mjrf_destroyMesh); +} + +inline UniquePtr CreateScene(mjrfContext* ctx, + const mjrSceneParams& params) { + mjrScene* scene = mjrf_createScene(ctx, ¶ms); + return UniquePtr(scene, mjrf_destroyScene); +} + +inline UniquePtr CreateLight(mjrfContext* ctx, + const mjrLightParams& params) { + mjrLight* light = mjrf_createLight(ctx, ¶ms); + return UniquePtr(light, mjrf_destroyLight); +} + +inline UniquePtr CreateRenderable( + mjrfContext* ctx, const mjrRenderableParams& params) { + mjrRenderable* renderable = mjrf_createRenderable(ctx, ¶ms); + return UniquePtr(renderable, mjrf_destroyRenderable); +} + +inline UniquePtr CreateRenderTarget( + mjrfContext* ctx, const mjrRenderTargetConfig& config) { + mjrRenderTarget* render_target = mjrf_createRenderTarget(ctx, &config); + return UniquePtr(render_target, mjrf_destroyRenderTarget); +} + +} // namespace mujoco + +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_RENDER_CONTEXT_FILAMENT_CPP_H_ From 4ed69b5ce76c0b2d75fd94ff5c4a3aad59e0c35c Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 5 May 2026 05:08:19 -0700 Subject: [PATCH 188/251] Randomize PGS constraint visitation order. Total testspeed runtime for `2humanoids100.xml` reduced by 19.6% (49.5 -> 39.8s). PiperOrigin-RevId: 910608140 Change-Id: Ided0ae5bdb8e4e8196f84e08354a9ae8cfa5b626 --- doc/changelog.rst | 2 + src/engine/engine_solver.c | 66 ++++++++++++++++++++++++++++--- test/engine/engine_island_test.cc | 6 +-- 3 files changed, 65 insertions(+), 9 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 92df67c5..7de551e3 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -8,6 +8,8 @@ Upcoming version (not yet released) General ^^^^^^^ - Added island support for the :ref:`PGS solver`. +- The :ref:`PGS solver` now iterates over constraints in pseudo-random order, improving performance by + ~20%. - Added support for :ref:`elastic2d` for trilinear and quadratic flex :ref:`dofs`. - :ref:`Midpoint integration` is now restricted to the ``implicitfast`` diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index 8a7b1d00..cd44350d 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -15,6 +15,7 @@ #include "engine/engine_solver.h" #include +#include #include #include @@ -234,6 +235,34 @@ static mjtNum costChange(const mjtNum* A, mjtNum* force, const mjtNum* oldforce, } +// PCG32 random number generator state +typedef struct { + uint64_t state; + uint64_t inc; +} pcg32_state; + + +// generate next 32-bit pseudorandom integer +static uint32_t pcg32_next(pcg32_state* rng) { + uint64_t oldstate = rng->state; + rng->state = oldstate * 6364136223846793005ULL + (rng->inc | 1); + uint32_t xorshifted = ((oldstate >> 18u) ^ oldstate) >> 27u; + uint32_t rot = oldstate >> 59u; + return (xorshifted >> rot) | (xorshifted << ((-rot) & 31)); +} + + +// Fisher-Yates shuffle of integer array +static void shuffle_int(int* array, int n, pcg32_state* rng) { + for (int i = n - 1; i > 0; i--) { + uint32_t j = pcg32_next(rng) % (i + 1); + int temp = array[i]; + array[i] = array[j]; + array[j] = temp; + } +} + + // set efc_state to dual constraint state; return nactive // iterates over efclist (or sequentially if NULL), classifies by ne/nf ranges static int dualState(const mjData* d, int* state, @@ -391,7 +420,8 @@ static void solPGS(const mjModel* m, mjData* d, int island, mjtNum *force = d->efc_force; mj_markStack(d); mjtNum* ARinv = mjSTACKALLOC(d, nefc, mjtNum); - int* oldstate = mjSTACKALLOC(d, nefc, int); + int* oldstate = mjSTACKALLOC(d, 2*nefc, int); + int* blockstart = oldstate + nefc; int island_stat = mjMAX(0, island); // island index for diagnostic stats mjtNum scale = 1 / (m->stat.meaninertia * mjMAX(1, m->nv)); @@ -402,14 +432,41 @@ static void solPGS(const mjModel* m, mjData* d, int island, // initial constraint state dualState(d, d->efc_state, ne, nf, nefc, efclist); + // build block-index array: one entry per constraint block + int nblocks = 0; + for (int c=0; c < nefc; ) { + blockstart[nblocks++] = c; + int i = efclist ? efclist[c] : c; + if (d->efc_type[i] == mjCNSTR_CONTACT_ELLIPTIC) { + c += d->contact[d->efc_id[i]].dim; + } else { + c++; + } + } + + // seed PCG32 RNG from simulation time + pcg32_state rng; + uint64_t seed = 0; + memcpy(&seed, &d->time, sizeof(d->time)); + rng.state = 0; + rng.inc = 1; + rng.state = seed; + pcg32_next(&rng); + rng.state += seed; + pcg32_next(&rng); + // main iteration int iter = 0; while (iter < maxiter) { // clear improvement mjtNum improvement = 0; - // perform one sweep - for (int c=0; c < nefc; c++) { + // shuffle constraint visitation order + shuffle_int(blockstart, nblocks, &rng); + + // perform one sweep over constraint blocks + for (int bi=0; bi < nblocks; bi++) { + int c = blockstart[bi]; int i = efclist ? efclist[c] : c; // get constraint dimensionality @@ -529,9 +586,6 @@ static void solPGS(const mjModel* m, mjData* d, int island, Athis[0] = 1/ARinv[c]; } improvement -= costChange(Athis, force+i, oldforce, res, dim); - - // skip the rest of this constraint - c += (dim-1); } // update constraint state diff --git a/test/engine/engine_island_test.cc b/test/engine/engine_island_test.cc index c8055293..e6592b2b 100644 --- a/test/engine/engine_island_test.cc +++ b/test/engine/engine_island_test.cc @@ -603,7 +603,7 @@ TEST_F(IslandTest, EqualityConstraintOfTendons) { mj_deleteModel(model); } -TEST_F(IslandTest, PGSIslandExact) { +TEST_F(IslandTest, PGSIsland) { const std::string xml_path = GetTestDataFilePath(kIlslandEfcPath); char error[1024]; mjModel* m = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error)); @@ -632,8 +632,8 @@ TEST_F(IslandTest, PGSIslandExact) { std::vector qfrc_mono(d->qfrc_constraint, d->qfrc_constraint + m->nv); - // expect exact match - EXPECT_EQ(qfrc_island, qfrc_mono); + // expect close match (inexact due to randomized constraint visitation order) + EXPECT_THAT(qfrc_island, Pointwise(MjNear(1e-3, 1e-3), qfrc_mono)); mj_deleteData(d); mj_deleteModel(m); From d168eb2d7de558ccc2fa3e25e305dd7a777243ca Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 5 May 2026 05:55:21 -0700 Subject: [PATCH 189/251] Expose several functions in the public C API. PiperOrigin-RevId: 910629209 Change-Id: I8623309e348c72350703f6874a887592f0607bba --- .../filament/render_context_filament.cc | 135 ++++++++++++++++++ .../filament/render_context_filament.h | 80 +++++++++++ 2 files changed, 215 insertions(+) diff --git a/src/experimental/filament/render_context_filament.cc b/src/experimental/filament/render_context_filament.cc index 7f70e700..ed85bc28 100644 --- a/src/experimental/filament/render_context_filament.cc +++ b/src/experimental/filament/render_context_filament.cc @@ -18,6 +18,8 @@ #include #include +#include +#include #include #include #include @@ -193,6 +195,139 @@ void mjrf_destroyRenderTarget(mjrRenderTarget* render_target) { delete mujoco::RenderTarget::downcast(render_target); } + +void mjrf_setTextureData(mjrTexture* texture, const mjrTextureData* data) { + mujoco::Texture::downcast(texture)->Upload(*data); +} + +int mjrf_getTextureWidth(const mjrTexture* texture) { + return mujoco::Texture::downcast(texture)->GetWidth(); +} + +int mjrf_getTextureHeight(const mjrTexture* texture) { + return mujoco::Texture::downcast(texture)->GetHeight(); +} + +mjrTextureTarget mjrf_getTextureTarget(const mjrTexture* texture) { + return mujoco::Texture::downcast(texture)->GetTarget(); +} + +void mjrf_setLightEnabled(mjrLight* light, bool enabled) { + if (enabled) { + mujoco::Light::downcast(light)->Enable(); + } else { + mujoco::Light::downcast(light)->Disable(); + } +} + +void mjrf_setLightIntensity(mjrLight* light, float intensity) { + mujoco::Light::downcast(light)->SetIntensity(intensity); +} + +void mjrf_setLightColor(mjrLight* light, const float color[3]) { + mujoco::Light::downcast(light)->SetColor({color[0], color[1], color[2]}); +} + +void mjrf_setLightTransform(mjrLight* light, const float position[3], + const float direction[3]) { + mujoco::Light::downcast(light)->SetTransform( + {position[0], position[1], position[2]}, + {direction[0], direction[1], direction[2]}); +} + +mjrLightType mjrf_getLightType(const mjrLight* light) { + return mujoco::Light::downcast(light)->GetType(); +} + +void mjrf_setRenderableMesh(mjrRenderable* renderable, const mjrMesh* mesh, + int elem_offset, int elem_count) { + mujoco::Renderable::downcast(renderable) + ->SetMesh(mujoco::Mesh::downcast(mesh), elem_offset, elem_count); +} + +void mjrf_setRenderableMaterial(mjrRenderable* renderable, + const mjrMaterialParams* params, + const mjrMaterialTextures* textures) { + mujoco::Renderable::downcast(renderable)->UpdateMaterial(*params, *textures); +} + +void mjrf_setRenderableTransform(mjrRenderable* renderable, + const float position[3], + const float rotation[9], const float size[3]) { + const filament::math::float3 fposition{position[0], position[1], position[2]}; + const filament::math::float3 fsize{size[0], size[1], size[2]}; + const filament::math::mat3f frotation{rotation[0], rotation[1], rotation[2], + rotation[3], rotation[4], rotation[5], + rotation[6], rotation[7], rotation[8]}; + mujoco::Renderable::downcast(renderable) + ->SetTransform({fposition, frotation, fsize}); +} + +void mjrf_setRenderableLayerMask(mjrRenderable* renderable, + uint8_t layer_mask) { + mujoco::Renderable::downcast(renderable)->SetLayerMask(layer_mask); +} + +void mjrf_setRenderableWireframe(mjrRenderable* renderable, bool wireframe) { + mujoco::Renderable::downcast(renderable)->SetWireframe(wireframe); +} + +void mjrf_setRenderableCastShadows(mjrRenderable* renderable, + bool cast_shadows) { + mujoco::Renderable::downcast(renderable)->SetCastShadows(cast_shadows); +} + +void mjrf_setRenderableReceiveShadows(mjrRenderable* renderable, + bool receive_shadows) { + mujoco::Renderable::downcast(renderable)->SetReceiveShadows(receive_shadows); +} + +void mjrf_addLightToScene(mjrScene* scene, mjrLight* light) { + mujoco::SceneView::downcast(scene)->AddToScene( + mujoco::Light::downcast(light)); +} + +void mjrf_removeLightFromScene(mjrScene* scene, mjrLight* light) { + mujoco::SceneView::downcast(scene)->RemoveFromScene( + mujoco::Light::downcast(light)); +} + +void mjrf_addRenderableToScene(mjrScene* scene, mjrRenderable* renderable) { + mujoco::SceneView::downcast(scene)->AddToScene( + mujoco::Renderable::downcast(renderable)); +} + +void mjrf_removeRenderableFromScene(mjrScene* scene, + mjrRenderable* renderable) { + mujoco::SceneView::downcast(scene)->RemoveFromScene( + mujoco::Renderable::downcast(renderable)); +} + +void mjrf_setSceneSkybox(mjrScene* scene, const mjrTexture* texture) { + mujoco::SceneView::downcast(scene)->SetSkybox( + mujoco::Texture::downcast(texture)); +} + +void mjrf_setSceneShadowsEnabled(mjrScene* scene, bool enabled) { + if (enabled) { + mujoco::SceneView::downcast(scene)->EnableShadows(); + } else { + mujoco::SceneView::downcast(scene)->DisableShadows(); + } +} + +void mjrf_setSceneReflectionsEnabled(mjrScene* scene, bool enabled) { + if (enabled) { + mujoco::SceneView::downcast(scene)->EnableReflections(); + } else { + mujoco::SceneView::downcast(scene)->DisableReflections(); + } +} + +void mjrf_configureSceneFromModel(mjrScene* scene, const mjModel* model) { + mujoco::SceneView::downcast(scene)->Configure(model); +} + void mjrf_makeFilamentContext(const mjModel* m, mjrContext* con, const mjrFilamentConfig* config) { // TODO: Support multiple contexts and multiple threads. For now, we'll just diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index 377acebc..73ab3b4a 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -458,6 +458,86 @@ mjrRenderTarget* mjrf_createRenderTarget(mjrfContext* ctx, // Destroys the render target. void mjrf_destroyRenderTarget(mjrRenderTarget* render_target); +// Uploads the given texture data to the texture. +void mjrf_setTextureData(mjrTexture* texture, const mjrTextureData* data); + +// Returns the width of the texture. +int mjrf_getTextureWidth(const mjrTexture* texture); + +// Returns the height of the texture. +int mjrf_getTextureHeight(const mjrTexture* texture); + +// Returns the target type of the texture. +mjrTextureTarget mjrf_getTextureTarget(const mjrTexture* texture); + +// Enables or disables the light. +void mjrf_setLightEnabled(mjrLight* light, bool enabled); + +// Sets the intensity of the light, in candela. +void mjrf_setLightIntensity(mjrLight* light, float intensity); + +// Sets the RGB color of the light. +void mjrf_setLightColor(mjrLight* light, const float color[3]); + +// Sets the position and direction of the light. +void mjrf_setLightTransform(mjrLight* light, const float position[3], + const float direction[3]); + +// Returns the type of the light. +mjrLightType mjrf_getLightType(const mjrLight* light); + +// Sets the mesh of the renderable. +void mjrf_setRenderableMesh(mjrRenderable* renderable, const mjrMesh* mesh, + int elem_offset, int elem_count); + +// Sets the material properties and textures of the renderable. +void mjrf_setRenderableMaterial(mjrRenderable* renderable, + const mjrMaterialParams* params, + const mjrMaterialTextures* textures); + +// Sets the transform (position, rotation, and size) of the renderable. +void mjrf_setRenderableTransform(mjrRenderable* renderable, + const float position[3], + const float rotation[9], const float size[3]); + +// Sets whether the renderable casts shadows or not. +void mjrf_setRenderableCastShadows(mjrRenderable* renderable, + bool cast_shadows); + +// Sets whether the renderable receives shadows or not. +void mjrf_setRenderableReceiveShadows(mjrRenderable* renderable, + bool receive_shadows); + +// Forces the renderable to be rendered using lines. +void mjrf_setRenderableWireframe(mjrRenderable* renderable, bool wireframe); + +// Sets the layer mask of the renderable. See mjrRenderableParams for details. +void mjrf_setRenderableLayerMask(mjrRenderable* renderable, uint8_t layer_mask); + +// Adds the light to the scene. +void mjrf_addLightToScene(mjrScene* scene, mjrLight* light); + +// Removes the light from the scene. +void mjrf_removeLightFromScene(mjrScene* scene, mjrLight* light); + +// Adds the renderable to the scene. +void mjrf_addRenderableToScene(mjrScene* scene, mjrRenderable* renderable); + +// Removes the renderable from the scene. +void mjrf_removeRenderableFromScene(mjrScene* scene, mjrRenderable* renderable); + +// Sets the skybox texture of the scene. +void mjrf_setSceneSkybox(mjrScene* scene, const mjrTexture* texture); + +// Enables (or disables) shadows in the scene.. +void mjrf_setSceneShadowsEnabled(mjrScene* scene, bool enabled); + +// Enables (or disables) reflections in the scene. +void mjrf_setSceneReflectionsEnabled(mjrScene* scene, bool enabled); + +// Configures the scene based on the parameters in the model. +void mjrf_configureSceneFromModel(mjrScene* scene, const mjModel* model); + // Legacy API, to be deprecated. void mjrf_defaultFilamentConfig(mjrFilamentConfig* config); From b9c1877ecb1861fe6fb0369ac872fd791dc39047 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 5 May 2026 06:00:30 -0700 Subject: [PATCH 190/251] Project out rigid body modes from flex strain equality constraints. Modify EigendecomposeStiffness to project out rigid body translations and rotations from the stiffness matrix eigenvectors. This prevents "ghost damping" from rigid body modes being incorrectly constrained. Update hollow_vs_solid.xml with solimp parameters for edge constraints. Adjust engine_core_constraint_test.cc to reflect the reduced number of equality constraints due to the projection. PiperOrigin-RevId: 910631207 Change-Id: Ibaaa59bc030cfc6ae657d8f0d1b2a51002cc8d4e --- doc/changelog.rst | 2 +- model/flex/hollow_vs_solid.xml | 4 +- src/user/user_mesh.cc | 126 ++++++++++++++++++++- test/engine/engine_core_constraint_test.cc | 4 +- 4 files changed, 128 insertions(+), 8 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 7de551e3..dfa1a99e 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -10,7 +10,7 @@ General - Added island support for the :ref:`PGS solver`. - The :ref:`PGS solver` now iterates over constraints in pseudo-random order, improving performance by ~20%. -- Added support for :ref:`elastic2d` for trilinear and quadratic flex +- Added support for :ref:`elastic2d` for trilinear and quadratic flex :ref:`dofs`. - :ref:`Midpoint integration` is now restricted to the ``implicitfast`` :ref:`integrator` and is disabled when fluid forces are active diff --git a/model/flex/hollow_vs_solid.xml b/model/flex/hollow_vs_solid.xml index 709fc114..811090b1 100644 --- a/model/flex/hollow_vs_solid.xml +++ b/model/flex/hollow_vs_solid.xml @@ -24,7 +24,7 @@ solref="0.001 1" friction="2 0.1 0.1"/> - + @@ -51,7 +51,7 @@ type="grid" name="soft_mesh_2" dim="3" spacing=".025 .05 .025" mass="0.43" radius="0.005" dof="trilinear" rgba="0.9 0.7 0.7 1"> - + & K, // Output layout in `out`: // [0]: neig (as double) // [1 .. neig*n]: sqrt(λ_phys_i) * v_i, row-major +// If pos is non-null (3*npe doubles), rigid body modes are projected out of +// each eigenvector to prevent ghost damping in the constraint solver. The +// constraint Jacobian freezes the corotational frame, so eigenvectors aligned +// with rigid rotation patterns produce spurious velocity-level forces. // Returns number of retained eigenmodes. static int EigendecomposeStiffness(const double* K_cell_data, - double* out, int ndof) { + double* out, int ndof, + const double* pos) { + int npe = ndof / 3; + // copy K_cell for in-place decomposition std::vector mat(K_cell_data, K_cell_data + ndof * ndof); std::vector eigval(ndof); @@ -3914,27 +3921,138 @@ static int EigendecomposeStiffness(const double* K_cell_data, mjuu_eigendecompose(mat.data(), eigval.data(), eigvec.data(), ndof); + // build orthonormal rigid body modes for projection + // 6 modes: 3 translations + 3 rotations about centroid + const int kMaxRigid = 6; + std::vector rigid(pos ? kMaxRigid * ndof : 0, 0); + + if (pos) { + // compute centroid + double centroid[3] = {0, 0, 0}; + for (int n = 0; n < npe; n++) { + for (int k = 0; k < 3; k++) { + centroid[k] += pos[3*n + k]; + } + } + for (int k = 0; k < 3; k++) { + centroid[k] /= npe; + } + + // translation modes: uniform displacement along each axis + for (int n = 0; n < npe; n++) { + rigid[0*ndof + 3*n + 0] = 1; + rigid[1*ndof + 3*n + 1] = 1; + rigid[2*ndof + 3*n + 2] = 1; + } + + // rotation modes: e_axis × (pos_n - centroid) + for (int n = 0; n < npe; n++) { + double rx = pos[3*n + 0] - centroid[0]; + double ry = pos[3*n + 1] - centroid[1]; + double rz = pos[3*n + 2] - centroid[2]; + + // rotation about x: [0, -rz, ry] + rigid[3*ndof + 3*n + 1] = -rz; + rigid[3*ndof + 3*n + 2] = ry; + + // rotation about y: [rz, 0, -rx] + rigid[4*ndof + 3*n + 0] = rz; + rigid[4*ndof + 3*n + 2] = -rx; + + // rotation about z: [-ry, rx, 0] + rigid[5*ndof + 3*n + 0] = -ry; + rigid[5*ndof + 3*n + 1] = rx; + } + + // orthonormalize via modified Gram-Schmidt + for (int i = 0; i < kMaxRigid; i++) { + double* ri = rigid.data() + i * ndof; + for (int j = 0; j < i; j++) { + const double* rj = rigid.data() + j * ndof; + double dot = 0; + for (int k = 0; k < ndof; k++) { + dot += ri[k] * rj[k]; + } + for (int k = 0; k < ndof; k++) { + ri[k] -= dot * rj[k]; + } + } + double norm2 = 0; + for (int k = 0; k < ndof; k++) { + norm2 += ri[k] * ri[k]; + } + if (norm2 > 1e-20) { + double inv_norm = 1.0 / std::sqrt(norm2); + for (int k = 0; k < ndof; k++) { + ri[k] *= inv_norm; + } + } else { + // degenerate mode (e.g., collinear nodes): zero out + std::fill(ri, ri + ndof, 0.0); + } + } + } + // K_stored = -K_physical, so physical eigenvalue = -eigval[i] // retain modes where physical eigenvalue > threshold double max_eigval = 0; for (int i = 0; i < ndof; i++) { max_eigval = std::max(max_eigval, std::abs(eigval[i])); } - double threshold = max_eigval * 1e-8; + double threshold = max_eigval * 1e-8; int neig = 0; for (int i = 0; i < ndof; i++) { double lambda_phys = -eigval[i]; // negate to get physical eigenvalue if (lambda_phys > threshold) { // store sqrt(λ) * eigenvector (column i of eigvec matrix) double scale = std::sqrt(lambda_phys); + double* w = out + 1 + neig * ndof; for (int j = 0; j < ndof; j++) { - out[1 + neig * ndof + j] = scale * eigvec[j * ndof + i]; + w[j] = scale * eigvec[j * ndof + i]; } + + // project out rigid body components + if (pos) { + for (int r = 0; r < kMaxRigid; r++) { + const double* rr = rigid.data() + r * ndof; + double dot = 0; + for (int j = 0; j < ndof; j++) { + dot += w[j] * rr[j]; + } + for (int j = 0; j < ndof; j++) { + w[j] -= dot * rr[j]; + } + } + + // discard if projected norm is negligible relative to original + double norm2 = 0; + for (int j = 0; j < ndof; j++) { + norm2 += w[j] * w[j]; + } + if (norm2 < lambda_phys * 1e-6) { + continue; // mode was mostly rigid body: skip + } + } + neig++; + + // guard: eigendecomposed data must fit within ndof*ndof slot + // (guaranteed by rigid-body projection discarding >= 6 modes) + if (1 + neig * ndof > ndof * ndof) { + mju_error("EigendecomposeStiffness: output size %d exceeds buffer %d", + 1 + neig * ndof, ndof * ndof); + } } } + // check that enough modes were discarded (rigid body + numerical artifacts) + // for 3D elements: expect ndof - neig == 6 + if (pos && ndof - neig != kMaxRigid) { + mju_warning("EigendecomposeStiffness: only %d modes discarded, expected " + "at least %d rigid body modes", ndof - neig, kMaxRigid); + } + out[0] = static_cast(neig); return neig; } @@ -4611,7 +4729,7 @@ void mjCFlex::Compile(const mjVFS* vfs) { if (has_strain_eq) { // eigendecompose: store [neig, sqrt(λ)*v_1, sqrt(λ)*v_2, ...] std::fill(out, out + ndof_elem * ndof_elem, 0.0); - EigendecomposeStiffness(K_elem.data(), out, ndof_elem); + EigendecomposeStiffness(K_elem.data(), out, ndof_elem, elem_pos.data()); } else { // store raw K for passive forces std::copy(K_elem.begin(), K_elem.end(), out); diff --git a/test/engine/engine_core_constraint_test.cc b/test/engine/engine_core_constraint_test.cc index 69b0a173..b52df109 100644 --- a/test/engine/engine_core_constraint_test.cc +++ b/test/engine/engine_core_constraint_test.cc @@ -710,7 +710,9 @@ TEST_F(CoreConstraintTest, ShellModeBendZeroForceAtRest) { EXPECT_EQ(m->neq, 6); // Check total number of scalar equality constraints - EXPECT_EQ(d->ne, 48); // 6 faces * 8 modes per face + // 6 faces * 6 physical modes per face = 36 + // (2 spurious rigid-rotation modes from transverse shear are projected out) + EXPECT_EQ(d->ne, 36); // all constraint residuals should be zero at rest for (int i = 0; i < d->ne; i++) { From 24ce1eff10b1a7369b1c03b08ec5fbb23d5c3250 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 5 May 2026 06:32:19 -0700 Subject: [PATCH 191/251] Update compat library to use public functions. There are still a few places where the compat library is downcasting to the internal types due to limitations in the public APIs. PiperOrigin-RevId: 910646480 Change-Id: I1ccb6714db08a2010d9535f59338e5eba06d1d35 --- .../filament/compat/imgui_bridge.cc | 68 +++++++----- .../filament/compat/imgui_bridge.h | 23 ++-- .../filament/compat/imgui_editor.cc | 2 +- .../filament/compat/mjr_filament_renderer.cc | 15 ++- .../filament/compat/model_objects.cc | 75 ++++++------- .../filament/compat/model_objects.h | 37 +++--- .../filament/compat/scene_bridge.cc | 105 ++++++++---------- .../filament/compat/scene_bridge.h | 23 ++-- .../filament/compat/scene_geom_util.cc | 62 ++++++----- .../filament/compat/scene_geom_util.h | 10 +- .../filament/filament/builtins.cc | 26 ++--- src/experimental/filament/filament/builtins.h | 26 ++--- .../filament/filament/renderable.cc | 4 +- .../filament/filament/renderable.h | 2 +- 14 files changed, 229 insertions(+), 249 deletions(-) diff --git a/src/experimental/filament/compat/imgui_bridge.cc b/src/experimental/filament/compat/imgui_bridge.cc index 26ca06c2..461ef955 100644 --- a/src/experimental/filament/compat/imgui_bridge.cc +++ b/src/experimental/filament/compat/imgui_bridge.cc @@ -18,31 +18,28 @@ #include #include #include +#include #include #include #include #include #include -#include "experimental/filament/filament/filament_context.h" -#include "experimental/filament/filament/mesh.h" -#include "experimental/filament/filament/renderable.h" -#include "experimental/filament/filament/scene_view.h" -#include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" +#include "experimental/filament/render_context_filament_cpp.h" namespace mujoco { using filament::math::float3; using filament::math::mat3f; -ImguiBridge::ImguiBridge(FilamentContext* ctx) : ctx_(ctx) { +ImguiBridge::ImguiBridge(mjrfContext* ctx) : ctx_(ctx) { mjrSceneParams params; mjr_defaultSceneParams(¶ms); params.enable_post_processing = false; params.enable_reflections = false; params.enable_shadows = false; - scene_view_ = std::make_unique(ctx_, params); + scene_ = CreateScene(ctx_, params); } ImguiBridge::~ImguiBridge() { @@ -77,12 +74,12 @@ uintptr_t ImguiBridge::UploadImage(uintptr_t tex_id, const uint8_t* pixels, tex_id = next_tex_id_++; } - std::unique_ptr& texture = textures_[tex_id]; + mjrTexture* texture = GetTexture(tex_id); // If the texture does not exist or the dimensions have changed, we create a // new texture. - if (texture == nullptr || texture->GetWidth() != width || - texture->GetHeight() != height) { + if (texture == nullptr || mjrf_getTextureWidth(texture) != width || + mjrf_getTextureHeight(texture) != height) { mjrTextureConfig config; mjr_defaultTextureConfig(&config); config.width = width; @@ -90,7 +87,9 @@ uintptr_t ImguiBridge::UploadImage(uintptr_t tex_id, const uint8_t* pixels, config.target = mjTEXTURE_2D; config.format = bpp == 4 ? mjPIXEL_FORMAT_RGBA8 : mjPIXEL_FORMAT_RGB8; config.color_space = mjCOLORSPACE_LINEAR; - texture = std::make_unique(ctx_, config); + UniquePtr new_texture = ::mujoco::CreateTexture(ctx_, config); + texture = new_texture.get(); + textures_.insert_or_assign(tex_id, std::move(new_texture)); } // Create a copy of the image to pass it to filament as we don't know the @@ -108,7 +107,7 @@ uintptr_t ImguiBridge::UploadImage(uintptr_t tex_id, const uint8_t* pixels, texture_data.release_callback = callback; std::memcpy(bytes, pixels, num_bytes); - texture->Upload(texture_data); + mjrf_setTextureData(texture, &texture_data); return tex_id; } @@ -126,7 +125,7 @@ void ImguiBridge::CreateTexture(ImTextureData* data) { config.color_space = mjCOLORSPACE_LINEAR; const uintptr_t tex_id = next_tex_id_++; - textures_[tex_id] = std::make_unique(ctx_, config); + textures_.insert_or_assign(tex_id, ::mujoco::CreateTexture(ctx_, config)); data->SetTexID((ImTextureID)tex_id); UpdateTexture(data); } @@ -143,7 +142,7 @@ void ImguiBridge::UpdateTexture(ImTextureData* data) { texture_data.nbytes = data->Width * data->Height * 4; texture_data.user_data = nullptr; texture_data.release_callback = nullptr; - iter->second->Upload(texture_data); + mjrf_setTextureData(iter->second.get(), &texture_data); data->SetStatus(ImTextureStatus_OK); } @@ -156,6 +155,14 @@ void ImguiBridge::DestroyTexture(ImTextureData* data) { } } +mjrTexture* ImguiBridge::GetTexture(uintptr_t tex_id) const { + auto iter = textures_.find(tex_id); + if (iter == textures_.end()) { + return nullptr; + } + return iter->second.get(); +} + void ImguiBridge::Update() { if (!ImGui::GetCurrentContext()) { PrepareRenderables(0); @@ -233,21 +240,22 @@ void ImguiBridge::Update() { data.indices = cmds->IdxBuffer.Data; data.index_type = mjINDEX_TYPE_U16; data.primitive_type = mjMESH_PRIMITIVE_TYPE_TRIANGLES; - meshes_.push_back(std::make_unique(ctx_, data)); + meshes_.push_back(CreateMesh(ctx_, data)); - const Mesh* mesh = meshes_.back().get(); + const mjrMesh* mesh = meshes_.back().get(); int index_offset = 0; for (const ImDrawCmd& command : cmds->CmdBuffer) { const int width = size.x * scale.x; const int height = size.y * scale.y; - auto& renderable = renderables_[renderable_index]; - renderable->SetMesh(mesh, index_offset, command.ElemCount); + UniquePtr& renderable = renderables_[renderable_index]; + mjrf_setRenderableMesh(renderable.get(), mesh, index_offset, + command.ElemCount); mjrMaterialTextures textures; mjr_defaultMaterialTextures(&textures); - textures.color = textures_[command.GetTexID()].get(); + textures.color = GetTexture(command.GetTexID()); mjrMaterialParams properties; mjr_defaultMaterialParams(&properties); @@ -264,9 +272,12 @@ void ImguiBridge::Update() { properties.scissor[2] = width; properties.scissor[3] = height; } - renderable->UpdateMaterial(properties, textures); - renderable->SetTransform( - {float3{0, 0, 0}, mat3f(), float3(scale.x, scale.y, 1.0f)}); + mjrf_setRenderableMaterial(renderable.get(), &properties, &textures); + + const float position[] = {0, 0, 0}; + const float rotation[] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; + const float size[] = {scale.x, scale.y, 1.0f}; + mjrf_setRenderableTransform(renderable.get(), position, rotation, size); index_offset += command.ElemCount; ++renderable_index; @@ -279,15 +290,14 @@ void ImguiBridge::PrepareRenderables(int count) { mjrRenderableParams params; mjr_defaultRenderableParams(¶ms); params.shading_model = mjSHADING_MODEL_UX; - auto& r = - renderables_.emplace_back(std::make_unique(ctx_, params)); - r->SetCastShadows(false); - r->SetReceiveShadows(false); - r->SetBlendOrder(static_cast(renderables_.size())); - scene_view_->AddToScene(r.get()); + params.cast_shadows = false; + params.receive_shadows = false; + params.blend_order = static_cast(renderables_.size() + 1); + auto& renderable = renderables_.emplace_back(CreateRenderable(ctx_, params)); + mjrf_addRenderableToScene(scene_.get(), renderable.get()); } while (renderables_.size() > count) { - scene_view_->RemoveFromScene(renderables_.back().get()); + mjrf_removeRenderableFromScene(scene_.get(), renderables_.back().get()); renderables_.pop_back(); } } diff --git a/src/experimental/filament/compat/imgui_bridge.h b/src/experimental/filament/compat/imgui_bridge.h index 756456fc..ac479292 100644 --- a/src/experimental/filament/compat/imgui_bridge.h +++ b/src/experimental/filament/compat/imgui_bridge.h @@ -16,23 +16,19 @@ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_IMGUI_BRIDGE_H_ #include -#include #include #include #include -#include "experimental/filament/filament/filament_context.h" -#include "experimental/filament/filament/mesh.h" -#include "experimental/filament/filament/renderable.h" -#include "experimental/filament/filament/scene_view.h" -#include "experimental/filament/filament/texture.h" +#include "experimental/filament/render_context_filament.h" +#include "experimental/filament/render_context_filament_cpp.h" namespace mujoco { // Creates and manages a SceneView using data read from ImGui. class ImguiBridge { public: - explicit ImguiBridge(FilamentContext* ctx); + explicit ImguiBridge(mjrfContext* ctx); ~ImguiBridge(); // Prepares the Renderables using data from the current ImGui state. This @@ -41,7 +37,7 @@ class ImguiBridge { void Update(); // Returns the managed UX scene. - SceneView* GetSceneView() const { return scene_view_.get(); } + mjrScene* GetScene() const { return scene_.get(); } // Uploads texture to be used with ImGui's Image and ImageButton functions. uintptr_t UploadImage(uintptr_t tex_id, const uint8_t* pixels, int width, @@ -58,12 +54,13 @@ class ImguiBridge { void CreateTexture(ImTextureData* data); void UpdateTexture(ImTextureData* data); void DestroyTexture(ImTextureData* data); + mjrTexture* GetTexture(uintptr_t tex_id) const; - FilamentContext* ctx_ = nullptr; - std::unique_ptr scene_view_; - std::vector> renderables_; - std::vector> meshes_; - std::unordered_map> textures_; + mjrfContext* ctx_ = nullptr; + UniquePtr scene_{nullptr, nullptr}; + std::vector> renderables_; + std::vector> meshes_; + std::unordered_map> textures_; uintptr_t next_tex_id_ = 1; }; diff --git a/src/experimental/filament/compat/imgui_editor.cc b/src/experimental/filament/compat/imgui_editor.cc index dd707914..54105aac 100644 --- a/src/experimental/filament/compat/imgui_editor.cc +++ b/src/experimental/filament/compat/imgui_editor.cc @@ -651,7 +651,7 @@ void DrawLightGui(filament::LightManager& lm, } void DrawGui(SceneBridge* scene_bridge) { - SceneView* scene_view = scene_bridge->GetSceneView(); + SceneView* scene_view = SceneView::downcast(scene_bridge->GetScene()); filament::View* view = scene_view->GetDefaultRenderView(); filament::Engine* engine = scene_view->GetEngine(); filament::LightManager& lm = engine->getLightManager(); diff --git a/src/experimental/filament/compat/mjr_filament_renderer.cc b/src/experimental/filament/compat/mjr_filament_renderer.cc index 831b6636..3046c9f3 100644 --- a/src/experimental/filament/compat/mjr_filament_renderer.cc +++ b/src/experimental/filament/compat/mjr_filament_renderer.cc @@ -27,8 +27,8 @@ #include "experimental/filament/compat/scene_bridge.h" #include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/model_util.h" -#include "experimental/filament/filament/render_target.h" #include "experimental/filament/render_context_filament.h" +#include "experimental/filament/render_context_filament_cpp.h" namespace mujoco { @@ -43,10 +43,10 @@ void MjrFilamentRenderer::Init(const mjModel* model) { mjr_defaultRenderRequest(&render_requests_[0]); mjr_defaultRenderRequest(&render_requests_[1]); - render_requests_[0].scene = scene_bridge_->GetSceneView(); + render_requests_[0].scene = scene_bridge_->GetScene(); render_requests_[0].draw_mode = mjDRAW_MODE_COLOR; - render_requests_[1].scene = imgui_bridge_->GetSceneView(); + render_requests_[1].scene = imgui_bridge_->GetScene(); render_requests_[1].draw_mode = mjDRAW_MODE_COLOR; // The UX camera is a fixed orthographic camera. We only need to change the @@ -135,8 +135,7 @@ void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, config.height = viewport.height; config.color_format = mjPIXEL_FORMAT_RGB8; config.depth_format = mjPIXEL_FORMAT_DEPTH32F; - auto target = - std::make_unique(filament_context_.get(), config); + auto target = CreateRenderTarget(filament_context_.get(), config); render_requests_[0].target = target.get(); render_requests_[1].target = target.get(); @@ -158,11 +157,11 @@ void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, if (depth) { mjrRenderTargetConfig config; mjr_defaultRenderTargetConfig(&config); + config.width = viewport.width; + config.height = viewport.height; config.color_format = mjPIXEL_FORMAT_R32F; config.depth_format = mjPIXEL_FORMAT_DEPTH32F; - auto target = - std::make_unique(filament_context_.get(), config); - target->Prepare(viewport.width, viewport.height); + auto target = CreateRenderTarget(filament_context_.get(), config); render_requests_[0].target = target.get(); render_requests_[1].target = target.get(); diff --git a/src/experimental/filament/compat/model_objects.cc b/src/experimental/filament/compat/model_objects.cc index d87dd4c0..23ced1e0 100644 --- a/src/experimental/filament/compat/model_objects.cc +++ b/src/experimental/filament/compat/model_objects.cc @@ -14,7 +14,6 @@ #include "experimental/filament/compat/model_objects.h" -#include #include #include #include @@ -25,18 +24,16 @@ #include #include -#include #include #include #include #include #include #include "experimental/filament/filament/builtins.h" -#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/math_util.h" -#include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/model_util.h" -#include "experimental/filament/filament/texture.h" +#include "experimental/filament/render_context_filament.h" +#include "experimental/filament/render_context_filament_cpp.h" namespace mujoco { @@ -407,7 +404,7 @@ static std::span GetIndices(const mjModel* model, } } -static void UpdatemjrMeshData(mjrMeshData* data, const mjModel* model, int id, +static void UpdateMeshData(mjrMeshData* data, const mjModel* model, int id, MeshType mesh_type) { if (!IsValidIndex(model, id, mesh_type)) { mju_error("Invalid index %d for type %d", id, mesh_type); @@ -462,7 +459,7 @@ static void UpdatemjrMeshData(mjrMeshData* data, const mjModel* model, int id, data->bounds_max[2] = builder->bounds_max.z; } -void UpdateSkinFlexmjrMeshData(mjrMeshData* data, const mjModel* model, +void UpdateSkinFlexMeshData(mjrMeshData* data, const mjModel* model, const mjvScene* scene, const mjvGeom& geom) { auto positions = GetPositions(model, scene, geom); auto normals = GetNormals(model, scene, geom); @@ -494,21 +491,21 @@ void UpdateSkinFlexmjrMeshData(mjrMeshData* data, const mjModel* model, data->user_data = nullptr; } -ModelObjects::ModelObjects(const mjModel* model, FilamentContext* ctx) +ModelObjects::ModelObjects(const mjModel* model, mjrfContext* ctx) : model_(model), ctx_(ctx) { const int nstack = model->vis.quality.numstacks; const int nslice = model->vis.quality.numslices; const int nquad = model->vis.quality.numquads; - shapes_[kLine] = CreateLine(ctx_); - shapes_[kBox] = CreateBox(ctx_, nquad); - shapes_[kLineBox] = CreateLineBox(ctx_); - shapes_[kCone] = CreateCone(ctx_, nstack, nslice); - shapes_[kDisk] = CreateDisk(ctx_, nslice); - shapes_[kDome] = CreateDome(ctx_, nstack / 2, nslice); - shapes_[kTube] = CreateTube(ctx_, nstack, nslice); - shapes_[kPlane] = CreatePlane(ctx_, nquad); - shapes_[kSphere] = CreateSphere(ctx_, nstack, nslice); - shapes_[kTriangle] = CreateTriangle(ctx_); + shapes_.insert({kLine, CreateLine(ctx_)}); + shapes_.insert({kBox, CreateBox(ctx_, nquad)}); + shapes_.insert({kLineBox, CreateLineBox(ctx_)}); + shapes_.insert({kCone, CreateCone(ctx_, nstack, nslice)}); + shapes_.insert({kDisk, CreateDisk(ctx_, nslice)}); + shapes_.insert({kDome, CreateDome(ctx_, nstack / 2, nslice)}); + shapes_.insert({kTube, CreateTube(ctx_, nstack, nslice)}); + shapes_.insert({kPlane, CreatePlane(ctx_, nquad)}); + shapes_.insert({kSphere, CreateSphere(ctx_, nstack, nslice)}); + shapes_.insert({kTriangle, CreateTriangle(ctx_)}); for (int i = 0; i < model_->ntex; ++i) { UploadTexture(model_, i); @@ -545,14 +542,14 @@ void ModelObjects::UploadMesh(const mjModel* model, int id) { mjrMeshData data; mjr_defaultMeshData(&data); - UpdatemjrMeshData(&data, model, id, MeshType::kNormal); - meshes_[id] = std::make_unique(ctx_, data); + UpdateMeshData(&data, model, id, MeshType::kNormal); + meshes_.insert_or_assign(id, CreateMesh(ctx_, data)); if (model->mesh_graphadr[id] >= 0) { mjrMeshData convex_hull_data; mjr_defaultMeshData(&convex_hull_data); - UpdatemjrMeshData(&convex_hull_data, model, id, MeshType::kConvexHull); - convex_hulls_[id] = std::make_unique(ctx_, convex_hull_data); + UpdateMeshData(&convex_hull_data, model, id, MeshType::kConvexHull); + convex_hulls_.insert_or_assign(id, CreateMesh(ctx_, convex_hull_data)); } } @@ -597,9 +594,9 @@ void ModelObjects::UploadTexture(const mjModel* model, int id) { payload.user_data = nullptr; payload.release_callback = nullptr; - auto texture = std::make_unique(ctx_, config); - texture->Upload(payload); - textures_[id] = std::move(texture); + auto texture = CreateTexture(ctx_, config); + mjrf_setTextureData(texture.get(), &payload); + textures_.insert_or_assign(id, std::move(texture)); } void ModelObjects::UploadHeightField(const mjModel* model, int id) { @@ -614,18 +611,18 @@ void ModelObjects::UploadHeightField(const mjModel* model, int id) { mjrMeshData data; mjr_defaultMeshData(&data); - UpdatemjrMeshData(&data, model, id, MeshType::kHeightField); - height_fields_[id] = std::make_unique(ctx_, data); + UpdateMeshData(&data, model, id, MeshType::kHeightField); + height_fields_.insert_or_assign(id, CreateMesh(ctx_, data)); } void ModelObjects::CreateSkinFlexMesh(const mjvScene* scene, const mjvGeom& geom) { mjrMeshData data; mjr_defaultMeshData(&data); - UpdateSkinFlexmjrMeshData(&data, model_, scene, geom); - dynamic_meshes_[geom.objid] = std::make_unique(ctx_, data); + UpdateSkinFlexMeshData(&data, model_, scene, geom); + dynamic_meshes_.insert_or_assign(geom.objid, CreateMesh(ctx_, data)); } -const Mesh* ModelObjects::GetMeshBuffer(int data_id) const { +const mjrMesh* ModelObjects::GetMeshBuffer(int data_id) const { // As defined by mjv_updateScene: // original mesh: mesh_id * 2 // convex hull: (mesh_id * 2) + 1 @@ -639,29 +636,27 @@ const Mesh* ModelObjects::GetMeshBuffer(int data_id) const { } } -const Mesh* ModelObjects::GetHeightFieldBuffer(int hfield_id) const { +const mjrMesh* ModelObjects::GetHeightFieldBuffer(int hfield_id) const { auto it = height_fields_.find(hfield_id); return it != height_fields_.end() ? it->second.get() : nullptr; } -const Mesh* ModelObjects::GetShapeBuffer(ShapeType shape) const { - if (shape < 0 || shape >= kNumShapes) { - mju_error("Invalid shape type: %d", shape); - } - return shapes_[shape].get(); +const mjrMesh* ModelObjects::GetShapeBuffer(ShapeType shape) const { + auto it = shapes_.find(shape); + return it != shapes_.end() ? it->second.get() : nullptr; } -const Mesh* ModelObjects::GetFlexSkinGeomMesh(int geom_id) const { +const mjrMesh* ModelObjects::GetFlexSkinGeomMesh(int geom_id) const { auto it = dynamic_meshes_.find(geom_id); return it != dynamic_meshes_.end() ? it->second.get() : nullptr; } -const Texture* ModelObjects::GetTexture(int tex_id) const { +const mjrTexture* ModelObjects::GetTexture(int tex_id) const { auto it = textures_.find(tex_id); return it != textures_.end() ? it->second.get() : nullptr; } -const Texture* ModelObjects::GetTexture(int mat_id, int role) const { +const mjrTexture* ModelObjects::GetTexture(int mat_id, int role) const { if (mat_id < 0 || mat_id >= model_->nmat || role < 0 || role >= mjNTEXROLE) { return nullptr; } @@ -669,7 +664,7 @@ const Texture* ModelObjects::GetTexture(int mat_id, int role) const { return GetTexture(tex_id); } -const Texture* ModelObjects::GetSkyboxTexture() const { +const mjrTexture* ModelObjects::GetSkyboxTexture() const { for (auto& iter : textures_) { if (model_->tex_type[iter.first] == mjTEXTURE_SKYBOX) { return iter.second.get(); diff --git a/src/experimental/filament/compat/model_objects.h b/src/experimental/filament/compat/model_objects.h index 2db285f7..0a7f980f 100644 --- a/src/experimental/filament/compat/model_objects.h +++ b/src/experimental/filament/compat/model_objects.h @@ -15,22 +15,19 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_MODEL_OBJECTS_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_MODEL_OBJECTS_H_ -#include -#include #include #include #include -#include "experimental/filament/filament/filament_context.h" -#include "experimental/filament/filament/mesh.h" -#include "experimental/filament/filament/texture.h" +#include "experimental/filament/render_context_filament.h" +#include "experimental/filament/render_context_filament_cpp.h" namespace mujoco { // Creates and owns various filament objects based on the mjModel. class ModelObjects { public: - ModelObjects(const mjModel* model, FilamentContext* ctx); + ModelObjects(const mjModel* model, mjrfContext* ctx); ~ModelObjects(); enum ShapeType { @@ -56,13 +53,13 @@ class ModelObjects { void CreateSkinFlexMesh(const mjvScene* scene, const mjvGeom& geom); // Returns the cached instance of a filament object created from the mjModel. - const Mesh* GetShapeBuffer(ShapeType shape) const; - const Mesh* GetMeshBuffer(int data_id) const; - const Mesh* GetHeightFieldBuffer(int hfield_id) const; - const Mesh* GetFlexSkinGeomMesh(int geom_id) const; - const Texture* GetTexture(int tex_id) const; - const Texture* GetTexture(int mat_id, int role) const; - const Texture* GetSkyboxTexture() const; + const mjrMesh* GetShapeBuffer(ShapeType shape) const; + const mjrMesh* GetMeshBuffer(int data_id) const; + const mjrMesh* GetHeightFieldBuffer(int hfield_id) const; + const mjrMesh* GetFlexSkinGeomMesh(int geom_id) const; + const mjrTexture* GetTexture(int tex_id) const; + const mjrTexture* GetTexture(int mat_id, int role) const; + const mjrTexture* GetSkyboxTexture() const; float GetSpecularMultiplier() const { return specular_multiplier_; } float GetShininessMultiplier() const { return shininess_multiplier_; } @@ -75,13 +72,13 @@ class ModelObjects { private: const mjModel* model_ = nullptr; - FilamentContext* ctx_ = nullptr; - std::array, kNumShapes> shapes_; - std::unordered_map> meshes_; - std::unordered_map> convex_hulls_; - std::unordered_map> height_fields_; - std::unordered_map> dynamic_meshes_; - std::unordered_map> textures_; + mjrfContext* ctx_ = nullptr; + std::unordered_map> shapes_; + std::unordered_map> meshes_; + std::unordered_map> convex_hulls_; + std::unordered_map> height_fields_; + std::unordered_map> dynamic_meshes_; + std::unordered_map> textures_; float specular_multiplier_ = 0.2f; float shininess_multiplier_ = 0.1f; float emissive_multiplier_ = 0.3f; diff --git a/src/experimental/filament/compat/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc index f47b9316..616cc726 100644 --- a/src/experimental/filament/compat/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -19,8 +19,6 @@ #include #include -#include -#include #include #include #include @@ -31,16 +29,13 @@ #include "experimental/filament/compat/imgui_bridge.h" #include "experimental/filament/compat/model_objects.h" #include "experimental/filament/compat/scene_geom_util.h" -#include "experimental/filament/filament/color_grading_options.h" #include "experimental/filament/filament/filament_context.h" -#include "experimental/filament/filament/light.h" +#include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/model_util.h" -#include "experimental/filament/filament/object_manager.h" -#include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/scene_view.h" -#include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" +#include "experimental/filament/render_context_filament_cpp.h" namespace mujoco { @@ -49,14 +44,14 @@ using filament::math::float4; using filament::math::mat3; using filament::math::mat4; -static std::unique_ptr CreateFallbackIndirectLightTexture( - FilamentContext* ctx, std::string_view filename = "") { +static UniquePtr CreateFallbackIndirectLightTexture( + mjrfContext* ctx, std::string_view filename = "") { if (filename.empty()) { filename = ObjectManager::kDefaultEnvironmentLight; } std::unique_ptr asset = - ctx->GetObjectManager()->LoadAsset(filename); + FilamentContext::downcast(ctx)->GetObjectManager()->LoadAsset(filename); mjrTextureConfig config; mjr_defaultTextureConfig(&config); @@ -66,7 +61,7 @@ static std::unique_ptr CreateFallbackIndirectLightTexture( config.format = mjPIXEL_FORMAT_KTX; config.color_space = mjCOLORSPACE_AUTO; - auto texture = std::make_unique(ctx, config); + auto texture = CreateTexture(ctx, config); mjrTextureData payload; mjr_defaultTextureData(&payload); @@ -77,20 +72,20 @@ static std::unique_ptr CreateFallbackIndirectLightTexture( }; payload.user_data = asset.release(); - texture->Upload(payload); + mjrf_setTextureData(texture.get(), &payload); return texture; } -SceneBridge::SceneBridge(FilamentContext* ctx, const mjModel* model) +SceneBridge::SceneBridge(mjrfContext* ctx, const mjModel* model) : ctx_(ctx) { mjrSceneParams params; mjr_defaultSceneParams(¶ms); params.layer_mask = mjCAT_ALL; params.reflection_layer_mask = mjCAT_DYNAMIC | mjCAT_STATIC; - scene_view_ = std::make_unique(ctx_, params); + scene_ = CreateScene(ctx_, params); model_objects_ = std::make_unique(model, ctx_); - scene_view_->Configure(model); + mjrf_configureSceneFromModel(scene_.get(), model); default_shadow_map_size_ = ReadElement( model, "filament.shadows.map_size", default_shadow_map_size_); @@ -111,15 +106,15 @@ SceneBridge::SceneBridge(FilamentContext* ctx, const mjModel* model) SceneBridge::~SceneBridge() { for (auto& iter : lights_) { - scene_view_->RemoveFromScene(iter.get()); + mjrf_removeLightFromScene(scene_.get(), iter.get()); } lights_.clear(); if (fallback_ibl_) { - scene_view_->RemoveFromScene(fallback_ibl_.get()); + mjrf_removeLightFromScene(scene_.get(), fallback_ibl_.get()); } fallback_ibl_.reset(); for (auto& iter : renderables_) { - scene_view_->RemoveFromScene(iter.get()); + mjrf_removeRenderableFromScene(scene_.get(), iter.get()); } renderables_.clear(); } @@ -127,14 +122,14 @@ SceneBridge::~SceneBridge() { void SceneBridge::SetEnvironmentLight(std::string_view filename, float intensity) { for (auto& light : lights_) { - if (light->GetType() == mjLIGHT_IMAGE) { - scene_view_->RemoveFromScene(light.get()); + if (mjrf_getLightType(light.get()) == mjLIGHT_IMAGE) { + mjrf_removeLightFromScene(scene_.get(), light.get()); light.reset(); break; } } if (fallback_ibl_) { - scene_view_->RemoveFromScene(fallback_ibl_.get()); + mjrf_removeLightFromScene(scene_.get(), fallback_ibl_.get()); fallback_ibl_.reset(); } @@ -145,8 +140,8 @@ void SceneBridge::SetEnvironmentLight(std::string_view filename, params.type = mjLIGHT_IMAGE; params.texture = fallback_ibl_texture_.get(); params.intensity = intensity; - fallback_ibl_ = std::make_unique(ctx_, params); - scene_view_->AddToScene(fallback_ibl_.get()); + fallback_ibl_ = CreateLight(ctx_, params); + mjrf_addLightToScene(scene_.get(), fallback_ibl_.get()); } std::optional SceneBridge::ClipFromWorld(const float3& pos) const{ @@ -171,8 +166,8 @@ void SceneBridge::PrepareLights() { params.type = mjLIGHT_IMAGE; params.texture = model_objects_->GetTexture(model->light_texid[i]); params.intensity = model->light_intensity[i]; - auto light_obj = std::make_unique(ctx_, params); - scene_view_->AddToScene(light_obj.get()); + auto light_obj = CreateLight(ctx_, params); + mjrf_addLightToScene(scene_.get(), light_obj.get()); lights_.emplace_back(std::move(light_obj)); has_image_based_light = true; } else { @@ -192,8 +187,8 @@ void SceneBridge::PrepareLights() { params.spot_cone_angle = model->light_cutoff[i]; } - auto light_obj = std::make_unique(ctx_, params); - scene_view_->AddToScene(light_obj.get()); + auto light_obj = CreateLight(ctx_, params); + mjrf_addLightToScene(scene_.get(), light_obj.get()); lights_.emplace_back(std::move(light_obj)); } } @@ -212,8 +207,8 @@ void SceneBridge::PrepareLights() { params.cast_shadows = 0; params.intensity = 0.0f; params.spot_cone_angle = 90.0f; - auto light_obj = std::make_unique(ctx_, params); - scene_view_->AddToScene(light_obj.get()); + auto light_obj = CreateLight(ctx_, params); + mjrf_addLightToScene(scene_.get(), light_obj.get()); lights_.emplace_back(std::move(light_obj)); } @@ -224,8 +219,8 @@ void SceneBridge::PrepareLights() { mjr_defaultLightParams(¶ms); params.type = mjLIGHT_IMAGE; params.intensity = 10.0f; - fallback_ibl_ = std::make_unique(ctx_, params); - scene_view_->AddToScene(fallback_ibl_.get()); + fallback_ibl_ = CreateLight(ctx_, params); + mjrf_addLightToScene(scene_.get(), fallback_ibl_.get()); } // There are no "physical" lights in the scene which means we're likely @@ -240,21 +235,22 @@ void SceneBridge::PrepareLights() { params.type = mjLIGHT_IMAGE; params.texture = fallback_ibl_texture_.get(); params.intensity = fallback_environment_light_intensity_; - fallback_ibl_ = std::make_unique(ctx_, params); - scene_view_->AddToScene(fallback_ibl_.get()); + fallback_ibl_ = CreateLight(ctx_, params); + mjrf_addLightToScene(scene_.get(), fallback_ibl_.get()); // Distribute the fallback scene light intensity among the lights. const float intensity = fallback_scene_light_intensity_ / lights_.size(); for (auto& light : lights_) { if (light) { const bool is_headlight = (light == lights_.back()); - light->SetIntensity(is_headlight ? fallback_head_light_intensity_ - : intensity); + mjrf_setLightIntensity(light.get(), + is_headlight ? fallback_head_light_intensity_ + : intensity); } } } - scene_view_->SetSkybox(model_objects_->GetSkyboxTexture()); + mjrf_setSceneSkybox(scene_.get(), model_objects_->GetSkyboxTexture()); } mat4 CalculateClipFromWorld(const mjrRect& viewport, const mjvGLCamera& cam) { @@ -285,16 +281,8 @@ mat4 CalculateClipFromWorld(const mjrRect& viewport, const mjvGLCamera& cam) { } void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { - if (scene->flags[mjRND_SHADOW]) { - scene_view_->EnableShadows(); - } else { - scene_view_->DisableShadows(); - } - if (scene->flags[mjRND_REFLECTION]) { - scene_view_->EnableReflections(); - } else { - scene_view_->DisableReflections(); - } + mjrf_setSceneShadowsEnabled(scene_.get(), scene->flags[mjRND_SHADOW]); + mjrf_setSceneReflectionsEnabled(scene_.get(), scene->flags[mjRND_REFLECTION]); mjtNum hpos[3], hfwd[3]; float headpos[3], gazedir[3]; @@ -308,7 +296,7 @@ void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { // Remove all drawables from previous render and prepare new ones. for (auto& iter : renderables_) { - scene_view_->RemoveFromScene(iter.get()); + mjrf_removeRenderableFromScene(scene_.get(), iter.get()); } renderables_.clear(); for (int i = 0; i < scene->ngeom; ++i) { @@ -324,10 +312,10 @@ void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { model_objects_->CreateSkinFlexMesh(scene, *geom); } - std::unique_ptr renderable = CreateGeomRenderable( + UniquePtr renderable = CreateGeomRenderable( *geom, scene, ctx_, model_objects_.get(), headpos); - scene_view_->AddToScene(renderable.get()); + mjrf_addRenderableToScene(scene_.get(), renderable.get()); renderables_.push_back(std::move(renderable)); } @@ -343,16 +331,15 @@ void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { headpos[2] -= gazedir[2] * 0.05f; // The headlight is always the "back" light. - std::unique_ptr& light = lights_.back(); - light->SetColor(ReadFloat3(scene_light.diffuse)); - light->SetTransform(ReadFloat3(headpos), ReadFloat3(gazedir)); + UniquePtr& light = lights_.back(); + mjrf_setLightColor(light.get(), scene_light.diffuse); + mjrf_setLightTransform(light.get(), headpos, gazedir); continue; } else if (scene_light.id < lights_.size() - 1) { - std::unique_ptr& light = lights_[scene_light.id]; + UniquePtr& light = lights_[scene_light.id]; if (light) { - light->SetColor(ReadFloat3(scene_light.diffuse)); - light->SetTransform(ReadFloat3(scene_light.pos), - ReadFloat3(scene_light.dir)); + mjrf_setLightColor(light.get(), scene_light.diffuse); + mjrf_setLightTransform(light.get(), scene_light.pos, scene_light.dir); } } else { mju_error("Unexpected light id: %d", scene_light.id); @@ -360,11 +347,7 @@ void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { } // Enable/disable the headlight based on whether or not it's in the scene. - if (headlight_enabled) { - lights_.back()->Enable(); - } else { - lights_.back()->Disable(); - } + mjrf_setLightEnabled(lights_.back().get(), headlight_enabled); } void SceneBridge::UploadMesh(const mjModel* model, int id) { diff --git a/src/experimental/filament/compat/scene_bridge.h b/src/experimental/filament/compat/scene_bridge.h index d2d1dfdb..ce6911b6 100644 --- a/src/experimental/filament/compat/scene_bridge.h +++ b/src/experimental/filament/compat/scene_bridge.h @@ -25,18 +25,15 @@ #include #include #include "experimental/filament/compat/model_objects.h" -#include "experimental/filament/filament/filament_context.h" -#include "experimental/filament/filament/light.h" -#include "experimental/filament/filament/renderable.h" -#include "experimental/filament/filament/scene_view.h" -#include "experimental/filament/filament/texture.h" +#include "experimental/filament/render_context_filament.h" +#include "experimental/filament/render_context_filament_cpp.h" namespace mujoco { // Manages all mjModel data and updates a SceneView using an mjvScene. class SceneBridge { public: - SceneBridge(FilamentContext* ctx, const mjModel* model); + SceneBridge(mjrfContext* ctx, const mjModel* model); ~SceneBridge(); // Updates the environment light using the KTX image at the given path. @@ -55,7 +52,7 @@ class SceneBridge { void UploadHeightField(const mjModel* model, int id); // Returns the managed scene. - SceneView* GetSceneView() const { return scene_view_.get(); } + mjrScene* GetScene() const { return scene_.get(); } SceneBridge(const SceneBridge&) = delete; SceneBridge& operator=(const SceneBridge&) = delete; @@ -68,13 +65,13 @@ class SceneBridge { std::optional ClipFromWorld( const filament::math::float3& pos) const; - FilamentContext* ctx_ = nullptr; - std::unique_ptr scene_view_; + mjrfContext* ctx_ = nullptr; std::unique_ptr model_objects_; - std::unique_ptr fallback_ibl_; - std::unique_ptr fallback_ibl_texture_; - std::vector> lights_; - std::vector> renderables_; + UniquePtr scene_{nullptr, nullptr}; + UniquePtr fallback_ibl_{nullptr, nullptr}; + UniquePtr fallback_ibl_texture_{nullptr, nullptr}; + std::vector> lights_; + std::vector> renderables_; filament::math::mat4 clip_from_world_; int default_shadow_map_size_ = 2048; float default_vsm_blur_width_ = 0.0f; diff --git a/src/experimental/filament/compat/scene_geom_util.cc b/src/experimental/filament/compat/scene_geom_util.cc index 6e9939b3..2a131781 100644 --- a/src/experimental/filament/compat/scene_geom_util.cc +++ b/src/experimental/filament/compat/scene_geom_util.cc @@ -16,6 +16,7 @@ #include #include +#include #include #include #include @@ -27,12 +28,10 @@ #include #include #include "experimental/filament/compat/model_objects.h" -#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/math_util.h" -#include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/renderable.h" -#include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" +#include "experimental/filament/render_context_filament_cpp.h" namespace mujoco { @@ -64,39 +63,39 @@ static bool IsBehind(const float* headpos, const float* pos, const float* mat) { 0.0f); } -static const Mesh* GetMesh(ModelObjects* model_objs, int data_id) { - const Mesh* mesh = model_objs->GetMeshBuffer(data_id); +static const mjrMesh* GetMesh(ModelObjects* model_objs, int data_id) { + const mjrMesh* mesh = model_objs->GetMeshBuffer(data_id); if (mesh == nullptr) { mju_error("Unknown mesh %d", data_id); } return mesh; } -static const Mesh* GetSkinFlexMesh(ModelObjects* model_objs, int objid) { +static const mjrMesh* GetSkinFlexMesh(ModelObjects* model_objs, int objid) { return model_objs->GetFlexSkinGeomMesh(objid); } -static const Mesh* GetHeightField(ModelObjects* model_objs, int hfield_id) { - const Mesh* mesh = model_objs->GetHeightFieldBuffer(hfield_id); +static const mjrMesh* GetHeightField(ModelObjects* model_objs, int hfield_id) { + const mjrMesh* mesh = model_objs->GetHeightFieldBuffer(hfield_id); if (mesh == nullptr) { mju_error("Unknown height field %d", hfield_id); } return mesh; } -static const Mesh* GetShape(ModelObjects* model_objs, +static const mjrMesh* GetShape(ModelObjects* model_objs, ModelObjects::ShapeType shape_type) { - const Mesh* mesh = model_objs->GetShapeBuffer(shape_type); + const mjrMesh* mesh = model_objs->GetShapeBuffer(shape_type); if (mesh == nullptr) { mju_error("Unknown shape %d", shape_type); } return mesh; } -static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, +static void PrepareGeomMeshes(mjrRenderable* renderable, const mjvGeom& geom, const mjvScene* scene, ModelObjects* model_objects) { - std::vector meshes; + std::vector meshes; Renderable::GetTransformFn get_transforms; Trs trs = { @@ -334,11 +333,18 @@ static void PrepareGeomMeshes(Renderable& renderable, const mjvGeom& geom, break; } - renderable.SetMeshes(meshes, get_transforms); - renderable.SetTransform(trs); + Renderable::downcast(renderable)->SetMeshes(meshes, get_transforms); + + float position[3]; + std::memcpy(position, &trs.translation[0], 3 * sizeof(float)); + float rotation[9]; + std::memcpy(rotation, &trs.rotation[0], 9 * sizeof(float)); + float size[3]; + std::memcpy(size, &trs.size[0], 3 * sizeof(float)); + mjrf_setRenderableTransform(renderable, position, rotation, size); } -static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, +static void UpdateGeomMaterial(mjrRenderable* renderable, const mjvGeom& geom, const mjvScene* scene, ModelObjects* model_objs, const float headpos[3]) { const mjModel* model = model_objs->GetModel(); @@ -353,19 +359,19 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, if (geom.type == mjGEOM_PLANE) { if (IsBehind(headpos, geom.pos, geom.mat)) { params.color[3] *= 0.3; - renderable.SetReceiveShadows(false); + mjrf_setRenderableReceiveShadows(renderable, false); params.reflective = false; } else { - renderable.SetReceiveShadows(true); + mjrf_setRenderableReceiveShadows(renderable, true); params.reflective = geom.reflectance > 0 && params.color[3] == 1.0f; } } - renderable.SetLayerMask(geom.category); + mjrf_setRenderableLayerMask(renderable, geom.category); if (geom.category == mjCAT_DECOR) { - renderable.SetCastShadows(false); - renderable.SetReceiveShadows(false); + mjrf_setRenderableCastShadows(renderable, false); + mjrf_setRenderableReceiveShadows(renderable, false); } else { - renderable.SetWireframe(scene->flags[mjRND_WIREFRAME]); + mjrf_setRenderableWireframe(renderable, scene->flags[mjRND_WIREFRAME]); } mjrMaterialTextures textures; @@ -420,7 +426,7 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, // the programmatic UVs. if (textures.color) { - if (Texture::downcast(textures.color)->GetTarget() == mjTEXTURE_2D) { + if (mjrf_getTextureTarget(textures.color) == mjTEXTURE_2D) { // For 2D textures, `tex_repeat` specifies how many times the texture // image is repeated. The `tex_uniform` flag determines if the repetition // is applied at in object space (false) or in world space (true). @@ -482,11 +488,11 @@ static void UpdateGeomMaterial(Renderable& renderable, const mjvGeom& geom, params.specular *= model_objs->GetSpecularMultiplier(); params.glossiness *= model_objs->GetShininessMultiplier(); - renderable.UpdateMaterial(params, textures); + mjrf_setRenderableMaterial(renderable, ¶ms, &textures); } -std::unique_ptr CreateGeomRenderable( - const mjvGeom& geom, const mjvScene* scene, FilamentContext* ctx, +UniquePtr CreateGeomRenderable( + const mjvGeom& geom, const mjvScene* scene, mjrfContext* ctx, ModelObjects* model_objs, const float headpos[3]) { mjrShadingModel shading_model = mjSHADING_MODEL_SCENE_OBJECT; if (geom.type == mjGEOM_LINE || geom.type == mjGEOM_LINEBOX) { @@ -498,9 +504,9 @@ std::unique_ptr CreateGeomRenderable( mjrRenderableParams params; mjr_defaultRenderableParams(¶ms); params.shading_model = shading_model; - auto renderable = std::make_unique(ctx, params); - PrepareGeomMeshes(*renderable, geom, scene, model_objs); - UpdateGeomMaterial(*renderable, geom, scene, model_objs, headpos); + auto renderable = CreateRenderable(ctx, params); + PrepareGeomMeshes(renderable.get(), geom, scene, model_objs); + UpdateGeomMaterial(renderable.get(), geom, scene, model_objs, headpos); return renderable; } } // namespace mujoco diff --git a/src/experimental/filament/compat/scene_geom_util.h b/src/experimental/filament/compat/scene_geom_util.h index 2a0ee2f5..7823dca3 100644 --- a/src/experimental/filament/compat/scene_geom_util.h +++ b/src/experimental/filament/compat/scene_geom_util.h @@ -15,18 +15,16 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_SCENE_GEOM_UTIL_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_SCENE_GEOM_UTIL_H_ -#include - #include #include "experimental/filament/compat/model_objects.h" -#include "experimental/filament/filament/filament_context.h" -#include "experimental/filament/filament/renderable.h" +#include "experimental/filament/render_context_filament.h" +#include "experimental/filament/render_context_filament_cpp.h" namespace mujoco { // Creates a Renderable from the given mjvGeom. -std::unique_ptr CreateGeomRenderable( - const mjvGeom& geom, const mjvScene* scene, FilamentContext* ctx, +UniquePtr CreateGeomRenderable( + const mjvGeom& geom, const mjvScene* scene, mjrfContext* ctx, ModelObjects* model_objs, const float headpos[3]); } // namespace mujoco diff --git a/src/experimental/filament/filament/builtins.cc b/src/experimental/filament/filament/builtins.cc index abde82df..90b96baf 100644 --- a/src/experimental/filament/filament/builtins.cc +++ b/src/experimental/filament/filament/builtins.cc @@ -28,6 +28,7 @@ #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/render_context_filament.h" +#include "experimental/filament/render_context_filament_cpp.h" namespace mujoco { @@ -64,15 +65,14 @@ class BuiltinBuilder : public mjrMeshData { virtual ~BuiltinBuilder() = default; template - static std::unique_ptr Create(FilamentContext* ctx, - Args&&... args) { + static UniquePtr Create(mjrfContext* ctx, Args&&... args) { auto builder = new T(std::forward(args)...); mjrMeshData* mesh_data = builder->PrepareMeshData(); mesh_data->release_callback = +[](void* user_data) { delete static_cast(user_data); }; mesh_data->user_data = builder; - return std::make_unique(ctx, *mesh_data); + return CreateMesh(ctx, *mesh_data); } mjrMeshData* PrepareMeshData() { @@ -617,43 +617,43 @@ class DomeBuilder : public BuiltinBuilder { } }; -std::unique_ptr CreateLine(FilamentContext* ctx) { +UniquePtr CreateLine(mjrfContext* ctx) { return BuiltinBuilder::Create(ctx); } -std::unique_ptr CreatePlane(FilamentContext* ctx, int nquad) { +UniquePtr CreatePlane(mjrfContext* ctx, int nquad) { return BuiltinBuilder::Create(ctx, nquad); } -std::unique_ptr CreateTriangle(FilamentContext* ctx) { +UniquePtr CreateTriangle(mjrfContext* ctx) { return BuiltinBuilder::Create(ctx); } -std::unique_ptr CreateBox(FilamentContext* ctx, int nquad) { +UniquePtr CreateBox(mjrfContext* ctx, int nquad) { return BuiltinBuilder::Create(ctx, nquad); } -std::unique_ptr CreateLineBox(FilamentContext* ctx) { +UniquePtr CreateLineBox(mjrfContext* ctx) { return BuiltinBuilder::Create(ctx); } -std::unique_ptr CreateSphere(FilamentContext* ctx, int nstack, int nslice) { +UniquePtr CreateSphere(mjrfContext* ctx, int nstack, int nslice) { return BuiltinBuilder::Create(ctx, nstack, nslice); } -std::unique_ptr CreateTube(FilamentContext* ctx, int nstack, int nslice) { +UniquePtr CreateTube(mjrfContext* ctx, int nstack, int nslice) { return BuiltinBuilder::Create(ctx, nstack, nslice); } -std::unique_ptr CreateDisk(FilamentContext* ctx, int nslice) { +UniquePtr CreateDisk(mjrfContext* ctx, int nslice) { return BuiltinBuilder::Create(ctx, nslice); } -std::unique_ptr CreateDome(FilamentContext* ctx, int nstack, int nslice) { +UniquePtr CreateDome(mjrfContext* ctx, int nstack, int nslice) { return BuiltinBuilder::Create(ctx, nstack, nslice); } -std::unique_ptr CreateCone(FilamentContext* ctx, int nstack, int nslice) { +UniquePtr CreateCone(mjrfContext* ctx, int nstack, int nslice) { return BuiltinBuilder::Create(ctx, nstack, nslice); } diff --git a/src/experimental/filament/filament/builtins.h b/src/experimental/filament/filament/builtins.h index 1727f378..2cf2c591 100644 --- a/src/experimental/filament/filament/builtins.h +++ b/src/experimental/filament/filament/builtins.h @@ -15,24 +15,22 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUILTINS_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUILTINS_H_ -#include - -#include "experimental/filament/filament/filament_context.h" -#include "experimental/filament/filament/mesh.h" +#include "experimental/filament/render_context_filament.h" +#include "experimental/filament/render_context_filament_cpp.h" // Generates buffers for built-in shapes. namespace mujoco { -std::unique_ptr CreateLine(FilamentContext* ctx); -std::unique_ptr CreatePlane(FilamentContext* ctx, int nquad); -std::unique_ptr CreateTriangle(FilamentContext* ctx); -std::unique_ptr CreateBox(FilamentContext* ctx, int nquad); -std::unique_ptr CreateLineBox(FilamentContext* ctx); -std::unique_ptr CreateSphere(FilamentContext* ctx, int nstack, int nslice); -std::unique_ptr CreateTube(FilamentContext* ctx, int nstack, int nslice); -std::unique_ptr CreateDisk(FilamentContext* ctx, int nslice); -std::unique_ptr CreateDome(FilamentContext* ctx, int nstack, int nslice); -std::unique_ptr CreateCone(FilamentContext* ctx, int nstack, int nslice); +UniquePtr CreateLine(mjrfContext* ctx); +UniquePtr CreatePlane(mjrfContext* ctx, int nquad); +UniquePtr CreateTriangle(mjrfContext* ctx); +UniquePtr CreateBox(mjrfContext* ctx, int nquad); +UniquePtr CreateLineBox(mjrfContext* ctx); +UniquePtr CreateSphere(mjrfContext* ctx, int nstack, int nslice); +UniquePtr CreateTube(mjrfContext* ctx, int nstack, int nslice); +UniquePtr CreateDisk(mjrfContext* ctx, int nslice); +UniquePtr CreateDome(mjrfContext* ctx, int nstack, int nslice); +UniquePtr CreateCone(mjrfContext* ctx, int nstack, int nslice); } // namespace mujoco diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index a620be37..636473f7 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -159,7 +159,7 @@ const mat4f& Renderable::GetTransform() const { return transform_; } -void Renderable::SetMeshes(std::span meshes, +void Renderable::SetMeshes(std::span meshes, GetTransformFn get_transform_fn) { if (!parts_.empty()) { mju_error("Cannot set meshes for renderable with multiple parts."); @@ -168,7 +168,7 @@ void Renderable::SetMeshes(std::span meshes, get_transform_fn_ = get_transform_fn; for (int i = 0; i < meshes.size(); ++i) { Part& part = parts_.emplace_back(); - part.mesh = meshes[i]; + part.mesh = Mesh::downcast(meshes[i]); part.elem_offset = 0; part.elem_count = part.mesh->GetFilamentIndexBuffer()->getIndexCount(); InitPartEntity(part); diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 6d284e92..e7a405f4 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -70,7 +70,7 @@ class Renderable : public mjrRenderable { // relative to the transform of the renderable itself. This allows users to // construct compound (but rigid) objects from multiple meshes. using GetTransformFn = std::function; - void SetMeshes(std::span meshes, + void SetMeshes(std::span meshes, GetTransformFn get_transform = nullptr); // Sets the layer mask for the managed filament Entities. Layer masks can be From 9f972aa42e756b4e887a31b6011c58ee9ba4c4f8 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 5 May 2026 07:53:42 -0700 Subject: [PATCH 192/251] Use cinematic cameras by default in cards.xml and leaves.xml PiperOrigin-RevId: 910687282 Change-Id: I382c06f36d79fdd36a8299c36460ca74cf277aae --- model/cards/cards.xml | 1 + model/replicate/leaves.xml | 5 +---- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/model/cards/cards.xml b/model/cards/cards.xml index 42f2306b..efaa230f 100644 --- a/model/cards/cards.xml +++ b/model/cards/cards.xml @@ -6,6 +6,7 @@ + diff --git a/model/replicate/leaves.xml b/model/replicate/leaves.xml index 96f62de4..5594ccc6 100644 --- a/model/replicate/leaves.xml +++ b/model/replicate/leaves.xml @@ -3,6 +3,7 @@ + @@ -17,10 +18,6 @@ - - - - From a692283db34a005fdcdfe36b68e6d79101068dbf Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 5 May 2026 10:19:00 -0700 Subject: [PATCH 193/251] Add functions to create context and render. Rename mjrf_render to mjrf_renderScene to avoid naming collision. This function is will be removed in a future CL. PiperOrigin-RevId: 910764245 Change-Id: If4d05e806a9699570ca4bbc882f28bfdd05dbd8d --- src/experimental/filament/mjr_compat.cc | 2 +- .../filament/render_context_filament.cc | 24 ++++++++++++++++++- .../filament/render_context_filament.h | 19 ++++++++++++++- .../filament/render_context_filament_cpp.h | 5 ++++ src/experimental/platform/hal/renderer.cc | 2 +- 5 files changed, 48 insertions(+), 4 deletions(-) diff --git a/src/experimental/filament/mjr_compat.cc b/src/experimental/filament/mjr_compat.cc index 9d06dea6..c3cd27e1 100644 --- a/src/experimental/filament/mjr_compat.cc +++ b/src/experimental/filament/mjr_compat.cc @@ -36,7 +36,7 @@ void mjr_freeContext(mjrContext* con) { mjrf_freeContext(con); } void mjr_render(mjrRect viewport, mjvScene* scn, const mjrContext* con) { - mjrf_render(viewport, scn, con); + mjrf_renderScene(viewport, scn, con); } void mjr_uploadMesh(const mjModel* m, const mjrContext* con, int meshid) { mjrf_uploadMesh(m, con, meshid); diff --git a/src/experimental/filament/render_context_filament.cc b/src/experimental/filament/render_context_filament.cc index ed85bc28..8c13c404 100644 --- a/src/experimental/filament/render_context_filament.cc +++ b/src/experimental/filament/render_context_filament.cc @@ -145,6 +145,14 @@ void mjr_defaultReadPixelsRequest(mjrReadPixelsRequest* request) { memset(request, 0, sizeof(mjrReadPixelsRequest)); } +mjrfContext* mjrf_createContext(const mjrFilamentConfig* config) { + return new mujoco::FilamentContext(config); +} + +void mjrf_destroyContext(mjrfContext* ctx) { + delete mujoco::FilamentContext::downcast(ctx); +} + mjrTexture* mjrf_createTexture(mjrfContext* ctx, const mjrTextureConfig* cfg) { return new mujoco::Texture(mujoco::FilamentContext::downcast(ctx), *cfg); } @@ -328,6 +336,20 @@ void mjrf_configureSceneFromModel(mjrScene* scene, const mjModel* model) { mujoco::SceneView::downcast(scene)->Configure(model); } +mjrFrameHandle mjrf_render(mjrfContext* ctx, const mjrRenderRequest* req, + int nreq, const mjrReadPixelsRequest* read_req, + int nread_req) { + return mujoco::FilamentContext::downcast(ctx)->Render( + {req, static_cast(nreq)}, + {read_req, static_cast(nread_req)}); +} + +void mjrf_waitForFrame(mjrfContext* ctx, mjrFrameHandle frame) { + mujoco::FilamentContext::downcast(ctx)->WaitForFrame(frame); +} + +// Legacy API, to be deprecated. + void mjrf_makeFilamentContext(const mjModel* m, mjrContext* con, const mjrFilamentConfig* config) { // TODO: Support multiple contexts and multiple threads. For now, we'll just @@ -361,7 +383,7 @@ void mjrf_freeContext(mjrContext* con) { mjrf_defaultContext(con); } -void mjrf_render(mjrRect viewport, mjvScene* scn, const mjrContext* con) { +void mjrf_renderScene(mjrRect viewport, mjvScene* scn, const mjrContext* con) { CheckFilamentContext(); g_filament_context->Render(viewport, scn); } diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index 73ab3b4a..04074d97 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -421,6 +421,15 @@ struct mjrFilamentConfig { bool force_software_rendering; }; +// Initializes the mjrFilamentConfig to default values. +void mjrf_defaultFilamentConfig(mjrFilamentConfig* config); + +// Creates a filament rendering context. +mjrfContext* mjrf_createContext(const mjrFilamentConfig* config); + +// Destroys the filament rendering context. +void mjrf_destroyContext(mjrfContext* ctx); + // Creates a texture for the filament renderer. mjrTexture* mjrf_createTexture(mjrfContext* ctx, const mjrTextureConfig* cfg); @@ -538,6 +547,14 @@ void mjrf_setSceneReflectionsEnabled(mjrScene* scene, bool enabled); // Configures the scene based on the parameters in the model. void mjrf_configureSceneFromModel(mjrScene* scene, const mjModel* model); +// Submits the given requests for rendering. +mjrFrameHandle mjrf_render(mjrfContext* ctx, const mjrRenderRequest* req, + int nreq, const mjrReadPixelsRequest* read_req, + int nread_req); + +// Waits for the rendering to complete for the given frame handle. +void mjrf_waitForFrame(mjrfContext* ctx, mjrFrameHandle frame); + // Legacy API, to be deprecated. void mjrf_defaultFilamentConfig(mjrFilamentConfig* config); @@ -551,7 +568,7 @@ void mjrf_makeContext(const mjModel* m, mjrContext* con, int fontscale); void mjrf_freeContext(mjrContext* con); -void mjrf_render(mjrRect viewport, mjvScene* scn, const mjrContext* con); +void mjrf_renderScene(mjrRect viewport, mjvScene* scn, const mjrContext* con); void mjrf_uploadMesh(const mjModel* m, const mjrContext* con, int meshid); diff --git a/src/experimental/filament/render_context_filament_cpp.h b/src/experimental/filament/render_context_filament_cpp.h index 8df24b64..d5b58ed5 100644 --- a/src/experimental/filament/render_context_filament_cpp.h +++ b/src/experimental/filament/render_context_filament_cpp.h @@ -25,6 +25,11 @@ namespace mujoco { template using UniquePtr = std::unique_ptr; +inline UniquePtr CreateContext(const mjrFilamentConfig& config) { + mjrfContext* context = mjrf_createContext(&config); + return UniquePtr(context, mjrf_destroyContext); +} + inline UniquePtr CreateTexture(mjrfContext* ctx, const mjrTextureConfig& config) { mjrTexture* texture = mjrf_createTexture(ctx, &config); diff --git a/src/experimental/platform/hal/renderer.cc b/src/experimental/platform/hal/renderer.cc index a023c54e..c7d8c75b 100644 --- a/src/experimental/platform/hal/renderer.cc +++ b/src/experimental/platform/hal/renderer.cc @@ -95,7 +95,7 @@ void Renderer::Init(const mjModel* model) { : mjGRAPHICS_API_VULKAN; mjrf_makeFilamentContext(model, &render_context_, &render_config); render_ = [&](mjrRect rect, mjvScene* scene) { - mjrf_render(rect, scene, &render_context_); + mjrf_renderScene(rect, scene, &render_context_); }; set_buffer_ = [&](int framebuffer) { mjrf_setBuffer(framebuffer, &render_context_); From d933b195eed3473ec5882156a313e3f786b2f950 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 5 May 2026 10:32:28 -0700 Subject: [PATCH 194/251] Implement bending forces for interpolated flex shells. This change adds a new passive force computation for flexes with elastic2d="bend" and dof="trilinear". The bending energy is based on the squared difference of normals between adjacent face elements at their shared edge midpoint. The edge data is precomputed during model compilation and stored in flex_bending. PiperOrigin-RevId: 910772638 Change-Id: I3b12c7b7f1ba6ac1875df495d89e8cfec921ca80 --- model/flex/flag.xml | 1 - model/flex/hollow_vs_solid.xml | 2 +- src/engine/engine_derivative.c | 5 + src/engine/engine_passive.c | 197 +++++++++++++++++++++- src/engine/engine_util_misc.c | 67 +++----- src/engine/engine_util_misc.h | 28 ++++ src/user/user_mesh.cc | 252 ++++++++++++++++++++++++++++- src/xml/xml_native_reader.cc | 7 +- test/engine/engine_passive_test.cc | 114 +++++++++++++ 9 files changed, 622 insertions(+), 51 deletions(-) diff --git a/model/flex/flag.xml b/model/flex/flag.xml index c7bfa371..d35e1ead 100644 --- a/model/flex/flag.xml +++ b/model/flex/flag.xml @@ -33,7 +33,6 @@ - diff --git a/model/flex/hollow_vs_solid.xml b/model/flex/hollow_vs_solid.xml index 811090b1..73606d55 100644 --- a/model/flex/hollow_vs_solid.xml +++ b/model/flex/hollow_vs_solid.xml @@ -50,7 +50,7 @@ origin="0 0 0" count="8 2 12" cellcount="6 1 6" type="grid" name="soft_mesh_2" dim="3" spacing=".025 .05 .025" mass="0.43" radius="0.005" dof="trilinear" rgba="0.9 0.7 0.7 1"> - + flex_interp[f]; int shell_mode = order < 0; order = order < 0 ? -order : order; + + // warn that bending derivatives are not yet implemented + if (shell_mode) { + mj_warning(d, mjWARN_INERTIA, f); // bending implicit derivatives missing + } int cx = m->flex_cellnum[3*f+0]; int cy = m->flex_cellnum[3*f+1]; int cz = m->flex_cellnum[3*f+2]; diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 7438f4c7..9a18b71c 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -201,6 +201,196 @@ static void mj_flexPassiveInterp(const mjModel* m, mjData* d, int f, } +// 2D shape function gradient: dir=0 returns dphi(s0,l0)*phi(s1,l1), +// dir=1 returns phi(s0,l0)*dphi(s1,l1) +static inline mjtNum mju_dphi2D(mjtNum s0, int l0, mjtNum s1, int l1, + int order, int dir) { + if (dir == 0) { + return mju_flexDphi(s0, l0, order) * mju_flexPhi(s1, l1, order); + } else { + return mju_flexPhi(s0, l0, order) * mju_flexDphi(s1, l1, order); + } +} + + +// per-edge data layout in flex_bending for interpolated shell bending +#define BEND_EDGE_SIZE 10 + + +// passive bending forces for interpolated flex shell +// +// Current approach: discrete Crouzeix-Raviart — point evaluation of the +// normal jump at each edge midpoint, with energy E = D/(2h) * |Δn - Δn₀|² * l. +// +// TODO(quaglino): upgrade to a Galerkin formulation by precomputing a bending +// stiffness matrix K_bend in the corotated frame using edge normal-jump residuals +// as DOFs and Gauss integration over each face. At runtime, rotate the residual +// vector into the corotated frame, multiply by K_bend, and rotate back. This +// would give implicit derivatives for free (K_bend is constant) and better +// accuracy for elements with varying curvature. +static void mj_flexPassiveBendInterp(const mjModel* m, mjData* d, int f, + int enbl_spring, int enbl_damper) { + // read bending edge data + const mjtNum* bdata = m->flex_bending + m->flex_bendingadr[f]; + int nedge = (int)bdata[0]; + if (nedge == 0) return; + + int order = -m->flex_interp[f]; // shell_mode: interp < 0 + int cx = m->flex_cellnum[3*f+0]; + int cy = m->flex_cellnum[3*f+1]; + int cz = m->flex_cellnum[3*f+2]; + int npe = (order+1)*(order+1); + int nodenum = m->flex_nodenum[f]; + + mj_markStack(d); + + // gather global state + mjtNum* xpos_g = mjSTACKALLOC(d, 3*nodenum, mjtNum); + mjtNum* vel_g = mjSTACKALLOC(d, 3*nodenum, mjtNum); + mjtNum* frc_g = mjSTACKALLOC(d, 3*nodenum, mjtNum); + mjtNum* dmp_g = mjSTACKALLOC(d, 3*nodenum, mjtNum); + mju_flexGatherState(m, d, f, xpos_g, vel_g); + mju_zero(frc_g, 3*nodenum); + mju_zero(dmp_g, 3*nodenum); + + // per-face temporaries + mjtNum* xpos_A = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* xpos_B = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* vel_A = mjSTACKALLOC(d, 3*npe, mjtNum); + mjtNum* vel_B = mjSTACKALLOC(d, 3*npe, mjtNum); + int* gidx_A = mjSTACKALLOC(d, npe, int); + int* gidx_B = mjSTACKALLOC(d, npe, int); + + mjtNum kD = m->opt.timestep > 0 ? m->flex_damping[f] / m->opt.timestep : 0; + if (enbl_damper && kD > 0) { + mju_warning("Bending damping is not yet supported for interpolated flex shells."); + } + + for (int e = 0; e < nedge; e++) { + const mjtNum* edata = bdata + 1 + e * BEND_EDGE_SIZE; + int fe_A = (int)edata[0]; + int fe_B = (int)edata[1]; + mjtNum local_A[2] = {edata[2], edata[3]}; + mjtNum local_B[2] = {edata[4], edata[5]}; + mjtNum stiffness = edata[6]; + mjtNum dn0[3] = {edata[7], edata[8], edata[9]}; + + if (stiffness == 0) continue; + + // gather face A and B positions + velocities + mju_flexGatherFaceState(order, cx, cy, cz, fe_A, xpos_g, + enbl_damper ? vel_g : NULL, NULL, + xpos_A, enbl_damper ? vel_A : NULL, NULL, + gidx_A, NULL); + mju_flexGatherFaceState(order, cx, cy, cz, fe_B, xpos_g, + enbl_damper ? vel_g : NULL, NULL, + xpos_B, enbl_damper ? vel_B : NULL, NULL, + gidx_B, NULL); + + // compute deformed normals at edge midpoint + mjtNum n_A[3], t1_A[3], t2_A[3]; + mjtNum n_B[3], t1_B[3], t2_B[3]; + mju_flexFaceNormal2D(n_A, t1_A, t2_A, order, xpos_A, local_A); + mju_flexFaceNormal2D(n_B, t1_B, t2_B, order, xpos_B, local_B); + + // normalize normals + mjtNum len_A = mju_norm3(n_A); + mjtNum len_B = mju_norm3(n_B); + if (len_A < mjMINVAL || len_B < mjMINVAL) continue; + mjtNum inv_A = 1.0 / len_A; + mjtNum inv_B = 1.0 / len_B; + mji_scl3(n_A, n_A, inv_A); + mji_scl3(n_B, n_B, inv_B); + + // normal jump residual: r = (n_A - n_B) - dn0 + mjtNum r[3]; + mji_sub3(r, n_A, n_B); + r[0] -= dn0[0]; r[1] -= dn0[1]; r[2] -= dn0[2]; + + // --- spring force --- + if (enbl_spring) { + // w_A = P_A * r = (r - n_A*(n_A.r)) / |c_A| + mjtNum dot_A = mju_dot3(n_A, r); + mjtNum w_A[3]; + w_A[0] = (r[0] - n_A[0]*dot_A) * inv_A; + w_A[1] = (r[1] - n_A[1]*dot_A) * inv_A; + w_A[2] = (r[2] - n_A[2]*dot_A) * inv_A; + + mjtNum dot_B = mju_dot3(n_B, r); + mjtNum w_B[3]; + w_B[0] = (r[0] - n_B[0]*dot_B) * inv_B; + w_B[1] = (r[1] - n_B[1]*dot_B) * inv_B; + w_B[2] = (r[2] - n_B[2]*dot_B) * inv_B; + + // precompute cross products: wA x t2_A, wA x t1_A + mjtNum wAt2[3], wAt1[3], wBt2[3], wBt1[3]; + mji_cross(wAt2, w_A, t2_A); + mji_cross(wAt1, w_A, t1_A); + mji_cross(wBt2, w_B, t2_B); + mji_cross(wBt1, w_B, t1_B); + + // force on face A nodes: f_k = stiffness * [g0_k * (wA x t2_A) - + // g1_k * (wA x t1_A)] + int idx = 0; + for (int l0 = 0; l0 <= order; l0++) { + for (int l1 = 0; l1 <= order; l1++) { + mjtNum g0 = mju_dphi2D(local_A[0], l0, local_A[1], l1, order, 0); + mjtNum g1 = mju_dphi2D(local_A[0], l0, local_A[1], l1, order, 1); + int gi = gidx_A[idx]; + for (int j = 0; j < 3; j++) { + frc_g[3*gi + j] += stiffness * (g0 * wAt2[j] - g1 * wAt1[j]); + } + idx++; + } + } + + // force on face B nodes (negative sign: ∂Δn/∂x = -∂n_B/∂x) + idx = 0; + for (int l0 = 0; l0 <= order; l0++) { + for (int l1 = 0; l1 <= order; l1++) { + mjtNum g0 = mju_dphi2D(local_B[0], l0, local_B[1], l1, order, 0); + mjtNum g1 = mju_dphi2D(local_B[0], l0, local_B[1], l1, order, 1); + int gi = gidx_B[idx]; + for (int j = 0; j < 3; j++) { + frc_g[3*gi + j] -= stiffness * (g0 * wBt2[j] - g1 * wBt1[j]); + } + idx++; + } + } + } + + // --- damping force --- + // TODO(quaglino): bending damping is disabled because at corner edges + // with nonzero rest normal jump, dΔn/dt = ω × Δn₀ ≠ 0 under rigid + // rotation, producing anti-conservative forces. A correct implementation + // would subtract the rigid-body velocity component before computing the + // damping residual. + } + + // apply accumulated forces to bodies + int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f]; + for (int i = 0; i < nodenum; i++) { + int bid = bodyid[i]; + int nidx = i + m->flex_nodeadr[f]; + + // fast path: node at body origin, direct DOF write + if (m->body_dofnum[bid] > 0 && + (m->flex_centered[f] || + (m->flex_node[3*nidx+0] == 0 && + m->flex_node[3*nidx+1] == 0 && + m->flex_node[3*nidx+2] == 0))) { + if (enbl_spring) mji_addTo3(d->qfrc_spring + m->body_dofadr[bid], frc_g+3*i); + if (enbl_damper) mji_addTo3(d->qfrc_damper + m->body_dofadr[bid], dmp_g+3*i); + } else { + if (enbl_spring) mj_applyFT(m, d, frc_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_spring); + if (enbl_damper) mj_applyFT(m, d, dmp_g+3*i, 0, xpos_g+3*i, bid, d->qfrc_damper); + } + } + + mj_freeStack(d); +} + + // passive forces for flex bending static void mj_flexPassiveBend(const mjModel* m, mjData* d, int f, int enbl_spring, int enbl_damper) { @@ -466,8 +656,13 @@ static void mj_springdamper(const mjModel* m, mjData* d) { } if (m->flex_interp[f]) { - // interpolated flex + // interpolated flex: stretch forces mj_flexPassiveInterp(m, d, f, enbl_spring, enbl_damper); + + // interpolated shell bending forces + if (m->flex_interp[f] < 0) { + mj_flexPassiveBendInterp(m, d, f, enbl_spring, enbl_damper); + } } else { // add bending forces mj_flexPassiveBend(m, d, f, enbl_spring, enbl_damper); diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index ef9916a0..723e2625 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -533,47 +533,9 @@ void mju_camPixelRay(mjtNum origin[3], mjtNum direction[3], // ----------------------------- flex interpolation ------------------------------------------------ -mjtNum static inline phi(mjtNum s, int i, int order) { - if (order == 1) { - return i == 0 ? 1 - s : s; - } else if (order == 2) { - switch (i) { - case 0: - return 2 * s * s - 3 * s + 1; - case 1: - return 4 * (s - s * s); - case 2: - return 2 * s * s - s; - default: - mjERROR("invalid index %d", i); - return 0; - } - } else { - mjERROR("order must be 1 or 2"); - return 0; - } -} - -mjtNum static inline dphi(mjtNum s, int i, int order) { - if (order == 1) { - return i == 0 ? -1 : 1; - } else if (order == 2) { - switch (i) { - case 0: - return 4 * s - 3; - case 1: - return 4 * (1 - 2 * s); - case 2: - return 4 * s - 1; - default: - mjERROR("invalid index %d, must be 0, 1, or 2", i); - return 0; - } - } else { - mjERROR("order must be 1 or 2"); - return 0; - } -} +// use shared shape functions from engine_util_misc.h +#define phi mju_flexPhi +#define dphi mju_flexDphi // evaluate the deformation gradient at p using the nodal dof values void mju_defGradient(mjtNum res[9], const mjtNum p[3], const mjtNum* dof, int order) { @@ -853,6 +815,29 @@ void mju_flexGatherFaceState(int order, int cx, int cy, int cz, } +// compute unnormalized surface normal and tangent vectors at a parametric point +// on a 2D face element; normal = t1 x t2 (unnormalized) +void mju_flexFaceNormal2D(mjtNum normal[3], mjtNum t1[3], mjtNum t2[3], + int order, const mjtNum* xpos_f, + const mjtNum local[2]) { + mju_zero3(t1); + mju_zero3(t2); + int idx = 0; + for (int l0 = 0; l0 <= order; l0++) { + for (int l1 = 0; l1 <= order; l1++) { + mjtNum grad0 = dphi(local[0], l0, order) * phi(local[1], l1, order); + mjtNum grad1 = phi(local[0], l0, order) * dphi(local[1], l1, order); + for (int d = 0; d < 3; d++) { + t1[d] += xpos_f[3*idx + d] * grad0; + t2[d] += xpos_f[3*idx + d] * grad1; + } + idx++; + } + } + mju_cross(normal, t1, t2); +} + + //------------------------------ actuator models --------------------------------------------------- // normalized muscle length-gain curve diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 374f54c2..ebf2ced7 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -116,6 +116,34 @@ MJAPI void mju_flexInterpRotation2D(int order, const mjtNum* xpos_f, int npe, int axis0, int axis1, int normal_axis, const mjtNum local[2], mjtNum* quat); +// compute unnormalized surface normal and tangent vectors at a parametric point +// on a 2D face element; normal = t1 x t2 (unnormalized) +MJAPI void mju_flexFaceNormal2D(mjtNum normal[3], mjtNum t1[3], mjtNum t2[3], + int order, const mjtNum* xpos_f, + const mjtNum local[2]); + + +// 1D shape function: order 1 (linear) or 2 (quadratic), node index i +static inline mjtNum mju_flexPhi(mjtNum s, int i, int order) { + if (order == 1) return i == 0 ? 1 - s : s; + switch (i) { + case 0: return 2*s*s - 3*s + 1; + case 1: return 4*(s - s*s); + case 2: return 2*s*s - s; + default: return 0; + } +} + +// 1D shape function gradient +static inline mjtNum mju_flexDphi(mjtNum s, int i, int order) { + if (order == 1) return i == 0 ? -1 : 1; + switch (i) { + case 0: return 4*s - 3; + case 1: return 4*(1 - 2*s); + case 2: return 4*s - 1; + default: return 0; + } +} // ----------------------------- Base64 ------------------------------------------------------------ diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index cf90b1a9..3083fb46 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4168,6 +4168,7 @@ void mjCFlex::DelTexcoord() { void mjCFlex::ResolveReferences(const mjCModel* m) { + interpolated = !nodebody_.empty(); vertbodyid.clear(); nodebodyid.clear(); for (const auto& vertbody : vertbody_) { @@ -4279,6 +4280,247 @@ void mjCFlex::CacheStiffness() { } +// compute interpolated shell bending edge data +// enumerates intra-surface and corner edges, stores per-edge metadata: +// [fe_A, fe_B, local_A[2], local_B[2], stiffness, dn0[3]] +static void ComputeInterpBending( + std::vector& bending, + const std::vector& nodexpos_local, + int order, const int cellcount[3], + double young, double poisson, double thickness) { + // bending modulus D = E * t^3 / (12 * (1 - nu^2)) + double D_bend = young * thickness * thickness * thickness / + (12.0 * (1.0 - poisson * poisson)); + + int cx = cellcount[0], cy = cellcount[1], cz = cellcount[2]; + int ny_global = cy * order + 1; + int nz_global = cz * order + 1; + int npe = (order + 1) * (order + 1); // nodes per 2D face element + + // face layout: 6 surfaces of the box + // face 0: x=0, face 1: x=max, face 2: y=0, face 3: y=max, + // face 4: z=0, face 5: z=max + int face_sizes[6] = {cy*cz, cy*cz, cx*cz, cx*cz, cx*cy, cx*cy}; + int face_normal[6] = {0, 0, 1, 1, 2, 2}; + int face_count1[6] = {cz, cz, cx, cx, cy, cy}; + int face_fixed[6] = {0, cx*order, 0, cy*order, 0, cz*order}; + + // gather node positions for one face element + auto gather_face_nodes = [&](int face_id, int within_face, + std::vector& fpos) { + int nax = face_normal[face_id]; + int a0 = (nax + 1) % 3; + int a1 = (nax + 2) % 3; + int c1 = face_count1[face_id]; + int gf = face_fixed[face_id]; + int q0 = within_face / c1; + int q1 = within_face % c1; + fpos.resize(3 * npe); + int loc = 0; + for (int l0 = 0; l0 <= order; l0++) { + for (int l1 = 0; l1 <= order; l1++) { + int g[3]; + g[nax] = gf; + g[a0] = q0 * order + l0; + g[a1] = q1 * order + l1; + int gidx = g[0] * ny_global * nz_global + g[1] * nz_global + g[2]; + mjuu_copyvec(fpos.data() + 3*loc, &nodexpos_local[3*gidx], 3); + loc++; + } + } + }; + + // compute unnormalized normal and tangents at a parametric point + auto compute_normal = [&](const std::vector& fpos, + const double local[2], + double normal[3], double t1[3], double t2[3]) { + mjuu_zerovec(t1, 3); + mjuu_zerovec(t2, 3); + int idx = 0; + for (int l0 = 0; l0 <= order; l0++) { + for (int l1 = 0; l1 <= order; l1++) { + double g0 = dphi(local[0], l0, order) * phi(local[1], l1, order); + double g1 = phi(local[0], l0, order) * dphi(local[1], l1, order); + for (int d = 0; d < 3; d++) { + t1[d] += fpos[3*idx + d] * g0; + t2[d] += fpos[3*idx + d] * g1; + } + idx++; + } + } + mjuu_crossvec(normal, t1, t2); + }; + + // face cumulative offsets + int face_cumul[6]; + face_cumul[0] = 0; + for (int f = 1; f < 6; f++) { + face_cumul[f] = face_cumul[f-1] + face_sizes[f-1]; + } + + int face_count0[6]; + for (int f = 0; f < 6; f++) { + face_count0[f] = face_sizes[f] / face_count1[f]; + } + + int cells[3] = {cx, cy, cz}; + + // find the neighbor of face element (fid, q0, q1) across the edge in + // direction dir (0=a0, 1=a1) at side (+1 or -1). + // returns (fid_B, within_B) and fills local_A, local_B with parametric + // midpoint coordinates on each side of the shared edge. + auto get_neighbor = [&](int fid, int q0, int q1, int dir, int side, double local_A[2], + double local_B[2]) -> std::pair { + int nax = fid / 2, sign_f = fid % 2; + int a0 = (nax+1)%3, a1 = (nax+2)%3; + int nc1 = face_count1[fid]; + + // parametric coordinates on face A at the shared edge + local_A[0] = (dir == 0) ? (side > 0 ? 1.0 : 0.0) : 0.5; + local_A[1] = (dir == 1) ? (side > 0 ? 1.0 : 0.0) : 0.5; + + // check if neighbor is on the same face (internal) + int q_nb = (dir == 0 ? q0 : q1) + side; + int q_max = (dir == 0) ? face_count0[fid] : nc1; + if (q_nb >= 0 && q_nb < q_max) { + // internal neighbor + int q0_B = (dir == 0) ? q_nb : q0; + int q1_B = (dir == 0) ? q1 : q_nb; + local_B[0] = (dir == 0) ? (side > 0 ? 0.0 : 1.0) : 0.5; + local_B[1] = (dir == 1) ? (side > 0 ? 0.0 : 1.0) : 0.5; + return {fid, q0_B * nc1 + q1_B}; + } + + // boundary neighbor: cross to adjacent face on the box + int ax = (dir == 0) ? a0 : a1; // axis being crossed + int fid_B = 2*ax + (side > 0 ? 1 : 0); // neighboring face + int nc1_B = face_count1[fid_B]; + + // the running coordinate along the shared edge maps to the neighbor face: + // dir=0: edge runs along a1, maps to a0_B = (ax+1)%3 = a1 → q0_B + // dir=1: edge runs along a0, maps to a1_B = (ax+2)%3 = a0 → q1_B + // the boundary position maps to the other axis on face B (= nax of face A): + // q_boundary = sign_f ? cells[nax]-1 : 0 + int q_run = (dir == 0) ? q1 : q0; + int q_boundary = sign_f ? (cells[nax]-1) : 0; + int q0_B, q1_B; + if (dir == 0) { + q0_B = q_run; + q1_B = q_boundary; + local_B[0] = 0.5; + local_B[1] = sign_f ? 1.0 : 0.0; + } else { + q0_B = q_boundary; + q1_B = q_run; + local_B[0] = sign_f ? 1.0 : 0.0; + local_B[1] = 0.5; + } + return {fid_B, q0_B * nc1_B + q1_B}; + }; + + struct BendEdge { + int fe_A, fe_B; // global face element indices (for runtime) + int fid_A, fid_B; // face id (0-5) + int within_A, within_B; // within-face element index + double local_A[2]; + double local_B[2]; + }; + std::vector edges; + + // enumerate all edges: for each face element, check 4 neighbors + // (2 directions × 2 sides). Add each edge once via fe_A < fe_B. + for (int f = 0; f < 6; f++) { + int nc0 = face_count0[f]; + int nc1 = face_count1[f]; + for (int q0 = 0; q0 < nc0; q0++) { + for (int q1 = 0; q1 < nc1; q1++) { + int within_A = q0 * nc1 + q1; + int fe_A = face_cumul[f] + within_A; + + for (int dir = 0; dir < 2; dir++) { + for (int side = -1; side <= 1; side += 2) { + double lA[2], lB[2]; + auto [fid_B, within_B] = get_neighbor(f, q0, q1, dir, side, lA, lB); + int fe_B = face_cumul[fid_B] + within_B; + if (fe_A < fe_B) { + BendEdge e; + e.fe_A = fe_A; e.fid_A = f; e.within_A = within_A; + e.fe_B = fe_B; e.fid_B = fid_B; e.within_B = within_B; + mjuu_copyvec(e.local_A, lA, 2); + mjuu_copyvec(e.local_B, lB, 2); + edges.push_back(e); + } + } + } + } + } + } + + // compute per-edge bending data + const int BEND_EDGE_SIZE = 10; // should match engine_passive.c + bending.resize(1 + edges.size() * BEND_EDGE_SIZE, 0); + bending[0] = static_cast(edges.size()); + + for (int e = 0; e < (int)edges.size(); e++) { + const BendEdge& edge = edges[e]; + std::vector fpos_A, fpos_B; + gather_face_nodes(edge.fid_A, edge.within_A, fpos_A); + gather_face_nodes(edge.fid_B, edge.within_B, fpos_B); + + // compute rest normals at edge midpoint + double n_A[3], t1_A[3], t2_A[3]; + double n_B[3], t1_B[3], t2_B[3]; + compute_normal(fpos_A, edge.local_A, n_A, t1_A, t2_A); + compute_normal(fpos_B, edge.local_B, n_B, t1_B, t2_B); + + // normalize + double len_A = mjuu_normvec(n_A, 3); + double len_B = mjuu_normvec(n_B, 3); + if (len_A < 1e-12 || len_B < 1e-12) continue; + + // rest normal jump + double dn0[3] = {n_A[0]-n_B[0], n_A[1]-n_B[1], n_A[2]-n_B[2]}; + + // stiffness coefficient: D * l_e / h_e + // determine which tangent is along vs across the edge for each face: + // local[k] == 0.5 means parametric direction k runs along the edge + double h_A, l_A, h_B, l_B; + if (edge.local_A[0] == 0.5) { + // edge runs along ξ on face A: t1 is along edge, t2 is across + l_A = mjuu_normvec(t1_A, 3); + h_A = mjuu_normvec(t2_A, 3); + } else { + // edge runs along η on face A: t2 is along edge, t1 is across + h_A = mjuu_normvec(t1_A, 3); + l_A = mjuu_normvec(t2_A, 3); + } + if (edge.local_B[0] == 0.5) { + l_B = mjuu_normvec(t1_B, 3); + h_B = mjuu_normvec(t2_B, 3); + } else { + h_B = mjuu_normvec(t1_B, 3); + l_B = mjuu_normvec(t2_B, 3); + } + double h_avg = (h_A + h_B) / 2; + double l_avg = (l_A + l_B) / 2; + double stiffness_coeff = D_bend * l_avg / mjMAX(h_avg, 1e-12); + + // pack into bending array + double* edata = bending.data() + 1 + e * BEND_EDGE_SIZE; + edata[0] = static_cast(edge.fe_A); + edata[1] = static_cast(edge.fe_B); + edata[2] = edge.local_A[0]; + edata[3] = edge.local_A[1]; + edata[4] = edge.local_B[0]; + edata[5] = edge.local_B[1]; + edata[6] = stiffness_coeff; + edata[7] = dn0[0]; + edata[8] = dn0[1]; + edata[9] = dn0[2]; + } +} + + // compiler void mjCFlex::Compile(const mjVFS* vfs) { CopyFromSpec(); @@ -4316,8 +4558,8 @@ void mjCFlex::Compile(const mjVFS* vfs) { if (thickness <= 0) { throw mjCError(this, "2d elasticity requires positive thickness"); } - if (interpolated && elastic2d != 2) { - mju_warning("bending passive force is not implemented for interpolated flex"); + if (poisson < 0.0 || poisson >= 0.5) { + throw mjCError(this, "Poisson ratio must be in [0, 0.5)"); } if (dim != 2 && !interpolated) { throw mjCError(this, "2d elasticity requires 2d flex"); @@ -4737,6 +4979,12 @@ void mjCFlex::Compile(const mjVFS* vfs) { } } + // compute interpolated shell bending edge data (independent of stiffness cache) + if (interpolated && (elastic2d == 1 || elastic2d == 3) && thickness > 0 && young > 0) { + ComputeInterpBending(bending, nodexpos_local, spec.order, spec.cellcount, + young, poisson, thickness); + } + // create bounding volume hierarchy CreateBVH(); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index af291787..c31b1bf2 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -2883,11 +2883,8 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) { } // check errors - if (dflex.elastic2d >= 2 && fcomp.equality) { - throw mjXError(elem, "elasticity and edge constraints cannot both be present"); - } - if (fcomp.equality == 3 && dflex.young > 0) { - throw mjXError(elem, "strain constraint and elasticity (young) cannot both be present"); + if (dflex.elastic2d != 1 && fcomp.equality && dflex.young > 0) { + throw mjXError(elem, "flex constraints and elasticity (young) cannot both be present"); } // contact diff --git a/test/engine/engine_passive_test.cc b/test/engine/engine_passive_test.cc index f87dc0a5..461b0090 100644 --- a/test/engine/engine_passive_test.cc +++ b/test/engine/engine_passive_test.cc @@ -883,5 +883,119 @@ TEST_F(ElasticityTest, ShellModeZeroForceAtRest) { mj_deleteModel(m); } +// interpolated shell bending must produce zero spring forces at rest +TEST_F(ElasticityTest, InterpBendingZeroForceAtRest) { + static constexpr char xml[] = R"( + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, testing::NotNull()) << error; + mjData* d = mj_makeData(m); + + // verify bending data was compiled + const mjtNum* bdata = m->flex_bending + m->flex_bendingadr[0]; + int nedge = (int)bdata[0]; + EXPECT_GT(nedge, 0) << "no bending edges compiled"; + + mj_forward(m, d); + + // all spring forces should be zero at rest + for (int i = 0; i < m->nv; i++) { + EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-10) + << "nonzero spring force at DOF " << i; + } + + // verify per-edge bending data + int n_flat = 0, n_corner = 0; + for (int e = 0; e < nedge; e++) { + const mjtNum* edata = bdata + 1 + e * 10; + mjtNum stiffness = edata[6]; + mjtNum dn0[3] = {edata[7], edata[8], edata[9]}; + mjtNum dn0_norm = mju_norm3(dn0); + + // stiffness must be positive + EXPECT_GT(stiffness, 0) << "edge " << e << " has non-positive stiffness"; + + if (dn0_norm < 1e-10) { + // intra-surface edge: coplanar faces, zero normal jump + n_flat++; + } else { + // corner edge: 90° between perpendicular face normals, |dn0| = sqrt(2) + n_corner++; + EXPECT_NEAR(dn0_norm, mju_sqrt(2.0), 1e-10) + << "corner edge " << e << " has unexpected |dn0|=" << dn0_norm; + } + } + + // for a 2x2x1 box: 12 intra-surface + 20 corner = 32 edges + EXPECT_GT(n_flat, 0) << "no intra-surface edges found"; + EXPECT_GT(n_corner, 0) << "no corner edges found"; + EXPECT_EQ(n_flat + n_corner, nedge); + + mj_deleteData(d); + mj_deleteModel(m); +} + +// interpolated shell bending must produce zero forces after a rigid rotation +TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) { + static constexpr char xml[] = R"( + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, testing::NotNull()) << error; + mjData* d = mj_makeData(m); + + // apply a rigid rotation by setting all body quats to a 30 degree rotation + // about z-axis (all flex node bodies get the same rotation) + mjtNum angle = 30 * 3.14159265358979 / 180.0; + mjtNum sa = mju_sin(angle / 2), ca = mju_cos(angle / 2); + for (int b = 1; b < m->nbody; b++) { + int qadr = m->jnt_qposadr[m->body_jntadr[b]]; + if (m->body_jntnum[b] > 0 && m->jnt_type[m->body_jntadr[b]] == mjJNT_FREE) { + d->qpos[qadr + 3] = ca; + d->qpos[qadr + 4] = 0; + d->qpos[qadr + 5] = 0; + d->qpos[qadr + 6] = sa; + } + } + + mj_forward(m, d); + + // spring forces should still be zero (or very small) after rigid rotation + for (int i = 0; i < m->nv; i++) { + EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-6) + << "nonzero spring force at DOF " << i << " after rigid rotation"; + } + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco From 5c156ebf157a687035634464f51e5c548c317ca8 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 5 May 2026 10:56:37 -0700 Subject: [PATCH 195/251] Refactor sparse constraint Jacobian supernode computation. - Compute efc_J_rowsuper incrementally in mj_addConstraint instead of post-hoc via mju_superSparse. - Better exploitation of supernodes in A matrix pipeline: precount and fill skip redundant chain traversals for supernode rows. - Redundant B_rowsuper computation via mju_superSparse is eliminated (indentical to efc_J_rowsuper). PiperOrigin-RevId: 910787825 Change-Id: I6eda9996659602b7051ee1090aeedb862603c84e --- src/engine/engine_core_constraint.c | 189 +++++++++++++++++----------- 1 file changed, 114 insertions(+), 75 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index b0285a33..b6c143a6 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -391,6 +391,18 @@ static void mj_addConstraint(const mjModel* m, mjData* d, mju_copy(J + adr[nefc+i], jac + i*NV, NV); } } + + // set J row supernodes; 1: next row has same pattern, 0: different pattern + + // cross-boundary: does previous row have same pattern? + if (nefc > 0 && NV == nnz[nefc-1] && + (NV == 0 || mju_compare(ind + adr[nefc], ind + adr[nefc-1], NV))) { + d->efc_J_rowsuper[nefc-1] = 1; + } + + // within-constraint: consecutive rows always share same pattern + mju_fillInt(d->efc_J_rowsuper + nefc, 1, size-1); + d->efc_J_rowsuper[nefc+size-1] = 0; } // all rows empty: skip constraint @@ -1162,7 +1174,7 @@ static inline int mj_addConstraintCount(const mjModel* m, int size, int NV) { } -// frictional dofs and tendons +// frictional DOFs and tendons // count_only: count constraints and Jacobian nonzeros without instantiating static int mj_instantiateFriction(const mjModel* m, mjData* d, int count_only, int* nnz) { int nv = m->nv, issparse = mj_isSparse(m); @@ -1184,7 +1196,7 @@ static int mj_instantiateFriction(const mjModel* m, mjData* d, int count_only, i jac = mjSTACKALLOC(d, nv, mjtNum); } - // find frictional dofs + // find frictional DOFs for (int i=0; i < nv; i++) { // no friction loss: skip if (!m->dof_frictionloss[i]) { @@ -2647,18 +2659,13 @@ void mj_makeConstraint(const mjModel* m, mjData* d) { return; } - // transpose sparse Jacobian, make row supernodes - if (mj_isSparse(m)) { -#ifdef mjUSEAVX - // compute supernodes of J; used by mju_mulMatVecSparse_avx - mju_superSparse(d->nefc, d->efc_J_rowsuper, - d->efc_J_rownnz, d->efc_J_rowadr, d->efc_J_colind); -#else - #ifdef MEMORY_SANITIZER - // tell msan to treat the entire J rowsuper as uninitialized - __msan_allocated_memory(d->efc_J_rowsuper, d->nefc); - #endif // MEMORY_SANITIZER -#endif // mjUSEAVX + // accumulate J row supernodes (reverse cumsum of 0/1 flags set at assembly time) + if (mj_isSparse(m) && d->nefc) { + for (int r=d->nefc-2; r >= 0; r--) { + if (d->efc_J_rowsuper[r]) { + d->efc_J_rowsuper[r] += d->efc_J_rowsuper[r+1]; + } + } } // compute diagApprox @@ -2704,35 +2711,43 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { B_rowadr[0] = 0; for (int r=0; r < nefc; r++) { - int nnz = 0; // nonzeros in row r of B - - // traverse row r of J in reverse, count unique nonzeros - int start = d->efc_J_rowadr[r]; - int end = start + d->efc_J_rownnz[r]; - for (int i=end-1; i >= start; i--) { - int j = d->efc_J_colind[i]; - - // if dof j is marked, it was already counted by a child dof: skip it - if (marker[j] == r) { - continue; - } - - // traverse row j of C, marking new unique nonzeros - int nnzC = m->M_rownnz[j]; - int adrC = m->M_rowadr[j]; - for (int k=0; k < nnzC; k++) { - int c = m->M_colind[adrC + k]; - if (marker[c] != r) { - marker[c] = r; - nnz++; - } - } + // supernode: same sparsity as previous row + if (r > 0 && d->efc_J_rowsuper[r-1] > 0) { + B_rownnz[r] = B_rownnz[r-1]; } - // update rownnz and rowadr - B_rownnz[r] = nnz; + // first row in supernode block: full chain traversal + else { + int nnz = 0; + + // traverse row r of J in reverse, count unique nonzeros + int start = d->efc_J_rowadr[r]; + int end = start + d->efc_J_rownnz[r]; + for (int i=end-1; i >= start; i--) { + int j = d->efc_J_colind[i]; + + // if dof j is marked, it was already counted by a child dof: skip it + if (marker[j] == r) { + continue; + } + + // traverse row j of M, marking new unique nonzeros + int nnzM = m->M_rownnz[j]; + int adrM = m->M_rowadr[j]; + for (int k=0; k < nnzM; k++) { + int c = m->M_colind[adrM + k]; + if (marker[c] != r) { + marker[c] = r; + nnz++; + } + } + } + B_rownnz[r] = nnz; + } + + // update rowadr if (r < nefc - 1) { - B_rowadr[r+1] = B_rowadr[r] + nnz; + B_rowadr[r+1] = B_rowadr[r] + B_rownnz[r]; } } @@ -2747,42 +2762,67 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { int* B_colind = mjSTACKALLOC(d, nB, int); for (int r=0; r < nefc; r++) { - // init row - int end = B_rowadr[r] + B_rownnz[r]; - int adrJ = d->efc_J_rowadr[r]; - int remainJ = d->efc_J_rownnz[r]; - int nnzB = 0; + // supernode: copy column indices, only update values from J + if (r > 0 && d->efc_J_rowsuper[r-1] > 0) { + int prevAdr = B_rowadr[r-1]; + int adrB = B_rowadr[r]; + int nnzB = B_rownnz[r]; + mju_copyInt(B_colind + adrB, B_colind + prevAdr, nnzB); + mju_zero(B + adrB, nnzB); - // complete chain in reverse - while (1) { - // get previous dof in src and dst - int prev_src = (remainJ > 0 ? d->efc_J_colind[adrJ + remainJ - 1] : -1); - int prev_dst = (nnzB > 0 ? m->dof_parentid[B_colind[end - nnzB]] : -1); - - // both finished: break - if (prev_src < 0 && prev_dst < 0) { - break; - } - - // add src - else if (prev_src >= prev_dst) { - nnzB++; - remainJ--; - B_colind[end - nnzB] = prev_src; - B[end - nnzB] = d->efc_J[adrJ + remainJ]; - } - - // add dst - else { - nnzB++; - B_colind[end - nnzB] = prev_dst; - B[end - nnzB] = 0; + // copy J values into correct positions + int adrJ = d->efc_J_rowadr[r]; + int jnnz = d->efc_J_rownnz[r]; + int bi = 0, ji = 0; + while (ji < jnnz && bi < nnzB) { + if (B_colind[adrB+bi] == d->efc_J_colind[adrJ+ji]) { + B[adrB+bi] = d->efc_J[adrJ+ji]; + bi++; + ji++; + } else { + bi++; + } } } - // compare with B_rownnz: SHOULD NOT OCCUR - if (nnzB != B_rownnz[r]) { - mjERROR("pre and post-count of B_rownnz are not equal on row %d", r); + // first row in supernode block: full chain completion + else { + int end = B_rowadr[r] + B_rownnz[r]; + int adrJ = d->efc_J_rowadr[r]; + int remainJ = d->efc_J_rownnz[r]; + int nnzB = 0; + + // complete chain in reverse + while (1) { + // get previous dof in src and dst + int prev_src = (remainJ > 0 ? d->efc_J_colind[adrJ + remainJ - 1] : -1); + int prev_dst = (nnzB > 0 ? m->dof_parentid[B_colind[end - nnzB]] : -1); + + // both finished: break + if (prev_src < 0 && prev_dst < 0) { + break; + } + + // add src + else if (prev_src >= prev_dst) { + nnzB++; + remainJ--; + B_colind[end - nnzB] = prev_src; + B[end - nnzB] = d->efc_J[adrJ + remainJ]; + } + + // add dst + else { + nnzB++; + B_colind[end - nnzB] = prev_dst; + B[end - nnzB] = 0; + } + } + + // compare with B_rownnz: SHOULD NOT OCCUR + if (nnzB != B_rownnz[r]) { + mjERROR("pre and post-count of B_rownnz are not equal on row %d", r); + } } } @@ -2814,9 +2854,8 @@ void mj_projectConstraint(const mjModel* m, mjData* d) { } } - // construct B supernodes - int* B_rowsuper = mjSTACKALLOC(d, nefc, int); - mju_superSparse(nefc, B_rowsuper, B_rownnz, B_rowadr, B_colind); + // B supernodes are identical to J supernodes + const int* B_rowsuper = d->efc_J_rowsuper; // construct B transposed int* BT_rownnz = mjSTACKALLOC(d, nv, int); From d249c882d59503e14db45c684726d7dfc1b026f2 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 5 May 2026 11:38:18 -0700 Subject: [PATCH 196/251] Fix gcc errors due to restrict. PiperOrigin-RevId: 910814591 Change-Id: I6858b1a36e21c6d47df803f49ceaed62c8e56248 --- src/engine/engine_passive.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 9a18b71c..0196fca5 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -299,8 +299,8 @@ static void mj_flexPassiveBendInterp(const mjModel* m, mjData* d, int f, if (len_A < mjMINVAL || len_B < mjMINVAL) continue; mjtNum inv_A = 1.0 / len_A; mjtNum inv_B = 1.0 / len_B; - mji_scl3(n_A, n_A, inv_A); - mji_scl3(n_B, n_B, inv_B); + n_A[0] *= inv_A; n_A[1] *= inv_A; n_A[2] *= inv_A; + n_B[0] *= inv_B; n_B[1] *= inv_B; n_B[2] *= inv_B; // normal jump residual: r = (n_A - n_B) - dn0 mjtNum r[3]; From 579a27e9d27459fdb492495fd46d91cca0b29b1a Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Tue, 5 May 2026 12:34:58 -0700 Subject: [PATCH 197/251] Make mjSpec related getter/find utilities take const parameters. PiperOrigin-RevId: 910848529 Change-Id: I1c966a6299fe47317cbf0bfb8f8b835cbcd4ed65 --- doc/includes/references.h | 42 +++++----- include/mujoco/mujoco.h | 42 +++++----- python/mujoco/introspect/functions.py | 46 +++++------ src/user/user_api.cc | 112 ++++++++++++-------------- src/user/user_api.h | 40 ++++----- src/user/user_model.cc | 14 ++-- src/user/user_model.h | 6 +- src/user/user_objects.cc | 8 +- src/user/user_objects.h | 4 +- wasm/codegen/generated/bindings.cc | 42 +++++----- 10 files changed, 175 insertions(+), 181 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index edc8b0c4..c96db9b3 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3686,27 +3686,27 @@ mjsSkin* mjs_addSkin(mjSpec* s); mjsTexture* mjs_addTexture(mjSpec* s); mjsMaterial* mjs_addMaterial(mjSpec* s, const mjsDefault* def); int mjs_makeMesh(mjsMesh* mesh, mjtMeshBuiltin builtin, double* params, int nparams); -mjSpec* mjs_getSpec(mjsElement* element); -mjsCompiler* mjs_getCompiler(mjsElement* element); -mjSpec* mjs_findSpec(mjSpec* spec, const char* name); -mjsBody* mjs_findBody(mjSpec* s, const char* name); -mjsElement* mjs_findElement(mjSpec* s, mjtObj type, const char* name); -mjsBody* mjs_findChild(mjsBody* body, const char* name); -mjsBody* mjs_getParent(mjsElement* element); -mjsFrame* mjs_getFrame(mjsElement* element); -mjsFrame* mjs_findFrame(mjSpec* s, const char* name); -mjsDefault* mjs_getDefault(mjsElement* element); -mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); -mjsDefault* mjs_getSpecDefault(mjSpec* s); -int mjs_getId(mjsElement* element); -mjsElement* mjs_firstChild(mjsBody* body, mjtObj type, int recurse); -mjsElement* mjs_nextChild(mjsBody* body, mjsElement* child, int recurse); -mjsElement* mjs_firstElement(mjSpec* s, mjtObj type); -mjsElement* mjs_nextElement(mjSpec* s, mjsElement* element); -mjsElement* mjs_getWrapTarget(mjsWrap* wrap); -mjsSite* mjs_getWrapSideSite(mjsWrap* wrap); -double mjs_getWrapDivisor(mjsWrap* wrap); -double mjs_getWrapCoef(mjsWrap* wrap); +mjSpec* mjs_getSpec(const mjsElement* element); +mjsCompiler* mjs_getCompiler(const mjsElement* element); +mjSpec* mjs_findSpec(const mjSpec* spec, const char* name); +mjsBody* mjs_findBody(const mjSpec* s, const char* name); +mjsElement* mjs_findElement(const mjSpec* s, mjtObj type, const char* name); +mjsBody* mjs_findChild(const mjsBody* body, const char* name); +mjsBody* mjs_getParent(const mjsElement* element); +mjsFrame* mjs_getFrame(const mjsElement* element); +mjsFrame* mjs_findFrame(const mjSpec* s, const char* name); +mjsDefault* mjs_getDefault(const mjsElement* element); +mjsDefault* mjs_findDefault(const mjSpec* s, const char* classname); +mjsDefault* mjs_getSpecDefault(const mjSpec* s); +int mjs_getId(const mjsElement* element); +mjsElement* mjs_firstChild(const mjsBody* body, mjtObj type, int recurse); +mjsElement* mjs_nextChild(const mjsBody* body, const mjsElement* child, int recurse); +mjsElement* mjs_firstElement(const mjSpec* s, mjtObj type); +mjsElement* mjs_nextElement(const mjSpec* s, const mjsElement* element); +mjsElement* mjs_getWrapTarget(const mjsWrap* wrap); +mjsSite* mjs_getWrapSideSite(const mjsWrap* wrap); +double mjs_getWrapDivisor(const mjsWrap* wrap); +double mjs_getWrapCoef(const mjsWrap* wrap); int mjs_setName(mjsElement* element, const char* name); void mjs_setBuffer(mjByteVec* dest, const void* array, int size); void mjs_setString(mjString* dest, const char* text); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 45b56bb1..c9600ba6 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1774,68 +1774,68 @@ MJAPI int mjs_makeMesh(mjsMesh* mesh, mjtMeshBuiltin builtin, double* params, in //---------------------------------- Find and get utilities ---------------------------------------- // Get spec from body. -MJAPI mjSpec* mjs_getSpec(mjsElement* element); +MJAPI mjSpec* mjs_getSpec(const mjsElement* element); // Get compiler associated with element's origin spec. -MJAPI mjsCompiler* mjs_getCompiler(mjsElement* element); +MJAPI mjsCompiler* mjs_getCompiler(const mjsElement* element); // Find spec (model asset) by name. -MJAPI mjSpec* mjs_findSpec(mjSpec* spec, const char* name); +MJAPI mjSpec* mjs_findSpec(const mjSpec* spec, const char* name); // Find body in spec by name. -MJAPI mjsBody* mjs_findBody(mjSpec* s, const char* name); +MJAPI mjsBody* mjs_findBody(const mjSpec* s, const char* name); // Find element in spec by name. -MJAPI mjsElement* mjs_findElement(mjSpec* s, mjtObj type, const char* name); +MJAPI mjsElement* mjs_findElement(const mjSpec* s, mjtObj type, const char* name); // Find child body by name. -MJAPI mjsBody* mjs_findChild(mjsBody* body, const char* name); +MJAPI mjsBody* mjs_findChild(const mjsBody* body, const char* name); // Get parent body. -MJAPI mjsBody* mjs_getParent(mjsElement* element); +MJAPI mjsBody* mjs_getParent(const mjsElement* element); // Get parent frame. -MJAPI mjsFrame* mjs_getFrame(mjsElement* element); +MJAPI mjsFrame* mjs_getFrame(const mjsElement* element); // Find frame by name. -MJAPI mjsFrame* mjs_findFrame(mjSpec* s, const char* name); +MJAPI mjsFrame* mjs_findFrame(const mjSpec* s, const char* name); // Get default corresponding to an element. -MJAPI mjsDefault* mjs_getDefault(mjsElement* element); +MJAPI mjsDefault* mjs_getDefault(const mjsElement* element); // Find default in model by class name. -MJAPI mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); +MJAPI mjsDefault* mjs_findDefault(const mjSpec* s, const char* classname); // Get global default from model. -MJAPI mjsDefault* mjs_getSpecDefault(mjSpec* s); +MJAPI mjsDefault* mjs_getSpecDefault(const mjSpec* s); // Get element id. -MJAPI int mjs_getId(mjsElement* element); +MJAPI int mjs_getId(const mjsElement* element); // Return body's first child of given type. If recurse is nonzero, also search the body's subtree. -MJAPI mjsElement* mjs_firstChild(mjsBody* body, mjtObj type, int recurse); +MJAPI mjsElement* mjs_firstChild(const mjsBody* body, mjtObj type, int recurse); // Return body's next child of the same type; return NULL if child is last. // If recurse is nonzero, also search the body's subtree. -MJAPI mjsElement* mjs_nextChild(mjsBody* body, mjsElement* child, int recurse); +MJAPI mjsElement* mjs_nextChild(const mjsBody* body, const mjsElement* child, int recurse); // Return spec's first element of selected type. -MJAPI mjsElement* mjs_firstElement(mjSpec* s, mjtObj type); +MJAPI mjsElement* mjs_firstElement(const mjSpec* s, mjtObj type); // Return spec's next element; return NULL if element is last. -MJAPI mjsElement* mjs_nextElement(mjSpec* s, mjsElement* element); +MJAPI mjsElement* mjs_nextElement(const mjSpec* s, const mjsElement* element); // Get wrapped element in tendon path. -MJAPI mjsElement* mjs_getWrapTarget(mjsWrap* wrap); +MJAPI mjsElement* mjs_getWrapTarget(const mjsWrap* wrap); // Get wrapped element side site in tendon path if it has one, nullptr otherwise. -MJAPI mjsSite* mjs_getWrapSideSite(mjsWrap* wrap); +MJAPI mjsSite* mjs_getWrapSideSite(const mjsWrap* wrap); // Get divisor of mjsWrap wrapping a puller. -MJAPI double mjs_getWrapDivisor(mjsWrap* wrap); +MJAPI double mjs_getWrapDivisor(const mjsWrap* wrap); // Get coefficient of mjsWrap wrapping a joint. -MJAPI double mjs_getWrapCoef(mjsWrap* wrap); +MJAPI double mjs_getWrapCoef(const mjsWrap* wrap); //---------------------------------- Attribute setters --------------------------------------------- diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index ff12151e..0bfc82b9 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -11120,7 +11120,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='element', type=PointerType( - inner_type=ValueType(name='mjsElement'), + inner_type=ValueType(name='mjsElement', is_const=True), ), ), ), @@ -11136,7 +11136,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='element', type=PointerType( - inner_type=ValueType(name='mjsElement'), + inner_type=ValueType(name='mjsElement', is_const=True), ), ), ), @@ -11152,7 +11152,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='spec', type=PointerType( - inner_type=ValueType(name='mjSpec'), + inner_type=ValueType(name='mjSpec', is_const=True), ), ), FunctionParameterDecl( @@ -11174,7 +11174,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='s', type=PointerType( - inner_type=ValueType(name='mjSpec'), + inner_type=ValueType(name='mjSpec', is_const=True), ), ), FunctionParameterDecl( @@ -11196,7 +11196,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='s', type=PointerType( - inner_type=ValueType(name='mjSpec'), + inner_type=ValueType(name='mjSpec', is_const=True), ), ), FunctionParameterDecl( @@ -11222,7 +11222,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='body', type=PointerType( - inner_type=ValueType(name='mjsBody'), + inner_type=ValueType(name='mjsBody', is_const=True), ), ), FunctionParameterDecl( @@ -11244,7 +11244,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='element', type=PointerType( - inner_type=ValueType(name='mjsElement'), + inner_type=ValueType(name='mjsElement', is_const=True), ), ), ), @@ -11260,7 +11260,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='element', type=PointerType( - inner_type=ValueType(name='mjsElement'), + inner_type=ValueType(name='mjsElement', is_const=True), ), ), ), @@ -11276,7 +11276,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='s', type=PointerType( - inner_type=ValueType(name='mjSpec'), + inner_type=ValueType(name='mjSpec', is_const=True), ), ), FunctionParameterDecl( @@ -11298,7 +11298,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='element', type=PointerType( - inner_type=ValueType(name='mjsElement'), + inner_type=ValueType(name='mjsElement', is_const=True), ), ), ), @@ -11314,7 +11314,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='s', type=PointerType( - inner_type=ValueType(name='mjSpec'), + inner_type=ValueType(name='mjSpec', is_const=True), ), ), FunctionParameterDecl( @@ -11336,7 +11336,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='s', type=PointerType( - inner_type=ValueType(name='mjSpec'), + inner_type=ValueType(name='mjSpec', is_const=True), ), ), ), @@ -11350,7 +11350,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='element', type=PointerType( - inner_type=ValueType(name='mjsElement'), + inner_type=ValueType(name='mjsElement', is_const=True), ), ), ), @@ -11366,7 +11366,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='body', type=PointerType( - inner_type=ValueType(name='mjsBody'), + inner_type=ValueType(name='mjsBody', is_const=True), ), ), FunctionParameterDecl( @@ -11390,13 +11390,13 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='body', type=PointerType( - inner_type=ValueType(name='mjsBody'), + inner_type=ValueType(name='mjsBody', is_const=True), ), ), FunctionParameterDecl( name='child', type=PointerType( - inner_type=ValueType(name='mjsElement'), + inner_type=ValueType(name='mjsElement', is_const=True), ), ), FunctionParameterDecl( @@ -11416,7 +11416,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='s', type=PointerType( - inner_type=ValueType(name='mjSpec'), + inner_type=ValueType(name='mjSpec', is_const=True), ), ), FunctionParameterDecl( @@ -11436,13 +11436,13 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='s', type=PointerType( - inner_type=ValueType(name='mjSpec'), + inner_type=ValueType(name='mjSpec', is_const=True), ), ), FunctionParameterDecl( name='element', type=PointerType( - inner_type=ValueType(name='mjsElement'), + inner_type=ValueType(name='mjsElement', is_const=True), ), ), ), @@ -11458,7 +11458,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='wrap', type=PointerType( - inner_type=ValueType(name='mjsWrap'), + inner_type=ValueType(name='mjsWrap', is_const=True), ), ), ), @@ -11474,7 +11474,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='wrap', type=PointerType( - inner_type=ValueType(name='mjsWrap'), + inner_type=ValueType(name='mjsWrap', is_const=True), ), ), ), @@ -11488,7 +11488,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='wrap', type=PointerType( - inner_type=ValueType(name='mjsWrap'), + inner_type=ValueType(name='mjsWrap', is_const=True), ), ), ), @@ -11502,7 +11502,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ FunctionParameterDecl( name='wrap', type=PointerType( - inner_type=ValueType(name='mjsWrap'), + inner_type=ValueType(name='mjsWrap', is_const=True), ), ), ), diff --git a/src/user/user_api.cc b/src/user/user_api.cc index f03b5e85..5b91a772 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -1302,61 +1302,56 @@ const char* mjs_setToDCMotor(mjsActuator* actuator, double motorconst[2], double // get spec from body -mjSpec* mjs_getSpec(mjsElement* element) { - return &(static_cast(element)->model->spec); +mjSpec* mjs_getSpec(const mjsElement* element) { + return &(static_cast(element)->model->spec); } -mjsCompiler* mjs_getCompiler(mjsElement* element) { - return static_cast(element)->compiler; +mjsCompiler* mjs_getCompiler(const mjsElement* element) { + return static_cast(element)->compiler; } // find spec (model asset) by name -mjSpec* mjs_findSpec(mjSpec* s, const char* name) { - mjCModel* model = static_cast(s->element); +mjSpec* mjs_findSpec(const mjSpec* s, const char* name) { + const mjCModel* model = static_cast(s->element); return model->FindSpec(name); } // get default -mjsDefault* mjs_getDefault(mjsElement* element) { - mjCModel* model = static_cast(element)->model; - std::string classname = static_cast(element)->classname; - return &(model->def_map[classname]->spec); +mjsDefault* mjs_getDefault(const mjsElement* element) { + const mjCModel* model = static_cast(element)->model; + std::string classname = static_cast(element)->classname; + auto it = model->def_map.find(classname); + return (it != model->def_map.end()) ? &it->second->spec : nullptr; } // Find default with given name in model. -mjsDefault* mjs_findDefault(mjSpec* s, const char* classname) { - mjCModel* modelC = static_cast(s->element); +mjsDefault* mjs_findDefault(const mjSpec* s, const char* classname) { + const mjCModel* modelC = static_cast(s->element); mjCDef* cdef = modelC->FindDefault(classname); - if (!cdef) { - return nullptr; - } - return &cdef->spec; + return cdef ? &cdef->spec : nullptr; } // get default[0] from model -mjsDefault* mjs_getSpecDefault(mjSpec* s) { - mjCModel* modelC = static_cast(s->element); +mjsDefault* mjs_getSpecDefault(const mjSpec* s) { + const mjCModel* modelC = static_cast(s->element); mjCDef* def = modelC->Default(); - if (!def) { - return nullptr; - } - return &def->spec; + return def ? &def->spec : nullptr; } // find body in model by name -mjsBody* mjs_findBody(mjSpec* s, const char* name) { +mjsBody* mjs_findBody(const mjSpec* s, const char* name) { mjsElement* body = mjs_findElement(s, mjOBJ_BODY, name); return body ? &(static_cast(body)->spec) : nullptr; } @@ -1364,7 +1359,7 @@ mjsBody* mjs_findBody(mjSpec* s, const char* name) { // find element in spec by name -mjsElement* mjs_findElement(mjSpec* s, mjtObj type, const char* name) { +mjsElement* mjs_findElement(const mjSpec* s, mjtObj type, const char* name) { mjCModel* model = static_cast(s->element); if (model->IsCompiled() && type != mjOBJ_FRAME) { return model->FindObject(type, std::string(name)); // fast lookup @@ -1390,8 +1385,8 @@ mjsElement* mjs_findElement(mjSpec* s, mjtObj type, const char* name) { // find child of a body by name -mjsBody* mjs_findChild(mjsBody* bodyspec, const char* name) { - mjCBody* body = static_cast(bodyspec->element); +mjsBody* mjs_findChild(const mjsBody* bodyspec, const char* name) { + const mjCBody* body = static_cast(bodyspec->element); mjCBase* child = body->FindObject(mjOBJ_BODY, std::string(name)); return child ? &(static_cast(child)->spec) : nullptr; } @@ -1399,22 +1394,22 @@ mjsBody* mjs_findChild(mjsBody* bodyspec, const char* name) { // get parent body -mjsBody* mjs_getParent(mjsElement* element) { +mjsBody* mjs_getParent(const mjsElement* element) { switch (element->elemtype) { case mjOBJ_BODY: - return &(static_cast(element)->GetParent()->spec); + return &(static_cast(element)->GetParent()->spec); case mjOBJ_FRAME: - return &(static_cast(element)->GetParent()->spec); + return &(static_cast(element)->GetParent()->spec); case mjOBJ_JOINT: - return &(static_cast(element)->GetParent()->spec); + return &(static_cast(element)->GetParent()->spec); case mjOBJ_GEOM: - return &(static_cast(element)->GetParent()->spec); + return &(static_cast(element)->GetParent()->spec); case mjOBJ_SITE: - return &(static_cast(element)->GetParent()->spec); + return &(static_cast(element)->GetParent()->spec); case mjOBJ_CAMERA: - return &(static_cast(element)->GetParent()->spec); + return &(static_cast(element)->GetParent()->spec); case mjOBJ_LIGHT: - return &(static_cast(element)->GetParent()->spec); + return &(static_cast(element)->GetParent()->spec); default: return nullptr; } @@ -1423,8 +1418,8 @@ mjsBody* mjs_getParent(mjsElement* element) { // get parent frame -mjsFrame* mjs_getFrame(mjsElement* element) { - mjCBase* base = static_cast(element); +mjsFrame* mjs_getFrame(const mjsElement* element) { + const mjCBase* base = static_cast(element); switch (element->elemtype) { case mjOBJ_BODY: case mjOBJ_FRAME: @@ -1442,7 +1437,7 @@ mjsFrame* mjs_getFrame(mjsElement* element) { // find frame by name -mjsFrame* mjs_findFrame(mjSpec* s, const char* name) { +mjsFrame* mjs_findFrame(const mjSpec* s, const char* name) { mjsElement* frame = mjs_findElement(s, mjOBJ_FRAME, name); return frame ? &(static_cast(frame)->spec) : nullptr; } @@ -1601,11 +1596,11 @@ int mjs_sensorDim(const mjsSensor* sensor) { // get id -int mjs_getId(mjsElement* element) { +int mjs_getId(const mjsElement* element) { if (!element) { return -1; } - return static_cast(element)->id; + return static_cast(element)->id; } @@ -1619,8 +1614,8 @@ void mjs_setDefault(mjsElement* element, const mjsDefault* defspec) { // return first child of selected type -mjsElement* mjs_firstChild(mjsBody* body, mjtObj type, int recurse) { - mjCBody* bodyC = static_cast(body->element); +mjsElement* mjs_firstChild(const mjsBody* body, mjtObj type, int recurse) { + const mjCBody* bodyC = static_cast(body->element); try { return bodyC->NextChild(NULL, type, recurse); } catch (mjCError& e) { @@ -1632,8 +1627,8 @@ mjsElement* mjs_firstChild(mjsBody* body, mjtObj type, int recurse) { // return body's next child; return NULL if child is last -mjsElement* mjs_nextChild(mjsBody* body, mjsElement* child, int recurse) { - mjCBody* bodyC = static_cast(body->element); +mjsElement* mjs_nextChild(const mjsBody* body, const mjsElement* child, int recurse) { + const mjCBody* bodyC = static_cast(body->element); try { return bodyC->NextChild(child, child->elemtype, recurse); } catch(mjCError& e) { @@ -1645,23 +1640,23 @@ mjsElement* mjs_nextChild(mjsBody* body, mjsElement* child, int recurse) { // return spec's first element of selected type -mjsElement* mjs_firstElement(mjSpec* s, mjtObj type) { - mjCModel* modelC = static_cast(s->element); +mjsElement* mjs_firstElement(const mjSpec* s, mjtObj type) { + const mjCModel* modelC = static_cast(s->element); return modelC->NextObject(NULL, type); } // return spec's next element; return NULL if element is last -mjsElement* mjs_nextElement(mjSpec* s, mjsElement* element) { - mjCModel* modelC = static_cast(s->element); +mjsElement* mjs_nextElement(const mjSpec* s, const mjsElement* element) { + const mjCModel* modelC = static_cast(s->element); return modelC->NextObject(element); } -mjsElement* mjs_getWrapTarget(mjsWrap* wrap) { - mjCWrap* cwrap = static_cast(wrap->element); +mjsElement* mjs_getWrapTarget(const mjsWrap* wrap) { + const mjCWrap* cwrap = static_cast(wrap->element); mjtObj type = mjOBJ_UNKNOWN; switch (cwrap->Type()) { case mjWRAP_SPHERE: @@ -1680,15 +1675,14 @@ mjsElement* mjs_getWrapTarget(mjsWrap* wrap) { default: return nullptr; } - mjSpec* spec = mjs_getSpec(wrap->element); - mjsElement* target = mjs_findElement(spec, type, cwrap->name.c_str()); - return target; + const mjSpec* spec = mjs_getSpec(wrap->element); + return mjs_findElement(spec, type, cwrap->name.c_str()); } -mjsSite* mjs_getWrapSideSite(mjsWrap* wrap) { - mjCWrap* cwrap = static_cast(wrap->element); +mjsSite* mjs_getWrapSideSite(const mjsWrap* wrap) { + const mjCWrap* cwrap = static_cast(wrap->element); // only sphere and cylinder (geoms) have side sites if ((cwrap->Type() != mjWRAP_SPHERE && cwrap->Type() != mjWRAP_CYLINDER) || @@ -1696,7 +1690,7 @@ mjsSite* mjs_getWrapSideSite(mjsWrap* wrap) { return nullptr; } - mjSpec* spec = mjs_getSpec(wrap->element); + const mjSpec* spec = mjs_getSpec(wrap->element); mjsElement* site = mjs_findElement(spec, mjOBJ_SITE, cwrap->sidesite.c_str()); if (site == nullptr) { mju_warning("Could not find side site %s for wrap %s in spec", @@ -1708,8 +1702,8 @@ mjsSite* mjs_getWrapSideSite(mjsWrap* wrap) { -double mjs_getWrapDivisor(mjsWrap* wrap) { - mjCWrap* cwrap = static_cast(wrap->element); +double mjs_getWrapDivisor(const mjsWrap* wrap) { + const mjCWrap* cwrap = static_cast(wrap->element); if (cwrap->Type() != mjWRAP_PULLEY) { mju_warning("Querying divisor attribute of non-pulley wrap: %s", cwrap->name.c_str()); return 1.0; @@ -1719,8 +1713,8 @@ double mjs_getWrapDivisor(mjsWrap* wrap) { -double mjs_getWrapCoef(mjsWrap* wrap) { - mjCWrap* cwrap = static_cast(wrap->element); +double mjs_getWrapCoef(const mjsWrap* wrap) { + const mjCWrap* cwrap = static_cast(wrap->element); if (cwrap->Type() != mjWRAP_JOINT) { mju_warning("Querying coef attribute of non-joint wrap: %s", cwrap->name.c_str()); return 1.0; diff --git a/src/user/user_api.h b/src/user/user_api.h index 2b73c12e..878f9045 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -222,68 +222,68 @@ MJAPI int mjs_makeMesh(mjsMesh* mesh, mjtMeshBuiltin builtin, double* params, in //---------------------------------- Find/get utilities -------------------------------------------- // Get spec from body. -MJAPI mjSpec* mjs_getSpec(mjsElement* element); +MJAPI mjSpec* mjs_getSpec(const mjsElement* element); // Find spec (model asset) by name. -MJAPI mjSpec* mjs_findSpec(mjSpec* spec, const char* name); +MJAPI mjSpec* mjs_findSpec(const mjSpec* spec, const char* name); // Find body in spec by name. -MJAPI mjsBody* mjs_findBody(mjSpec* s, const char* name); +MJAPI mjsBody* mjs_findBody(const mjSpec* s, const char* name); // Find element in spec by name. -MJAPI mjsElement* mjs_findElement(mjSpec* s, mjtObj type, const char* name); +MJAPI mjsElement* mjs_findElement(const mjSpec* s, mjtObj type, const char* name); // Find child body by name. -MJAPI mjsBody* mjs_findChild(mjsBody* body, const char* name); +MJAPI mjsBody* mjs_findChild(const mjsBody* body, const char* name); // Get parent body. -MJAPI mjsBody* mjs_getParent(mjsElement* element); +MJAPI mjsBody* mjs_getParent(const mjsElement* element); // Get parent frame. -MJAPI mjsFrame* mjs_getFrame(mjsElement* element); +MJAPI mjsFrame* mjs_getFrame(const mjsElement* element); // Find frame by name. -MJAPI mjsFrame* mjs_findFrame(mjSpec* s, const char* name); +MJAPI mjsFrame* mjs_findFrame(const mjSpec* s, const char* name); // Get default corresponding to an element. -MJAPI mjsDefault* mjs_getDefault(mjsElement* element); +MJAPI mjsDefault* mjs_getDefault(const mjsElement* element); // Find default in model by class name. -MJAPI mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); +MJAPI mjsDefault* mjs_findDefault(const mjSpec* s, const char* classname); // Get global default from model. -MJAPI mjsDefault* mjs_getSpecDefault(mjSpec* s); +MJAPI mjsDefault* mjs_getSpecDefault(const mjSpec* s); // Get element id. -MJAPI int mjs_getId(mjsElement* element); +MJAPI int mjs_getId(const mjsElement* element); //---------------------------------- Tree traversal ------------------------------------------------ // Return body's first child of given type. If recurse is nonzero, also search the body's subtree. -MJAPI mjsElement* mjs_firstChild(mjsBody* body, mjtObj type, int recurse); +MJAPI mjsElement* mjs_firstChild(const mjsBody* body, mjtObj type, int recurse); // Return body's next child of the same type; return NULL if child is last. // If recurse is nonzero, also search the body's subtree. -MJAPI mjsElement* mjs_nextChild(mjsBody* body, mjsElement* child, int recurse); +MJAPI mjsElement* mjs_nextChild(const mjsBody* body, const mjsElement* child, int recurse); // Return spec's first element of selected type. -MJAPI mjsElement* mjs_firstElement(mjSpec* s, mjtObj type); +MJAPI mjsElement* mjs_firstElement(const mjSpec* s, mjtObj type); // Return spec's next element; return NULL if element is last. -MJAPI mjsElement* mjs_nextElement(mjSpec* s, mjsElement* element); +MJAPI mjsElement* mjs_nextElement(const mjSpec* s, const mjsElement* element); // Get wrapped element in tendon path. -MJAPI mjsElement* mjs_getWrapTarget(mjsWrap* wrap); +MJAPI mjsElement* mjs_getWrapTarget(const mjsWrap* wrap); // Get wrapped element in tendon path. -MJAPI mjsSite* mjs_getWrapSideSite(mjsWrap* wrap); +MJAPI mjsSite* mjs_getWrapSideSite(const mjsWrap* wrap); // Get divisor of mjsWrap wrapping a puller. -MJAPI double mjs_getWrapDivisor(mjsWrap* wrap); +MJAPI double mjs_getWrapDivisor(const mjsWrap* wrap); // Get coefficient of mjsWrap wrapping a joint. -MJAPI double mjs_getWrapCoef(mjsWrap* wrap); +MJAPI double mjs_getWrapCoef(const mjsWrap* wrap); // Safely cast an element as mjsBody, or return NULL if the element is not an mjsBody. MJAPI mjsBody* mjs_asBody(mjsElement* element); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index e85cbe64..47d9a289 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1408,7 +1408,7 @@ mjCBase* mjCModel::GetObject(mjtObj type, int id) { template -static mjsElement* GetNext(std::vector& list, mjsElement* child) { +static mjsElement* GetNext(const std::vector& list, const mjsElement* child) { if (!child) { if (list.empty()) { return nullptr; @@ -1428,7 +1428,7 @@ static mjsElement* GetNext(std::vector& list, mjsElement* child) { // next object of specified type -mjsElement* mjCModel::NextObject(mjsElement* object, mjtObj type) { +mjsElement* mjCModel::NextObject(const mjsElement* object, mjtObj type) const { if (type == mjOBJ_UNKNOWN) { if (!object) { throw mjCError(nullptr, "type must be specified if no element is given"); @@ -1519,7 +1519,7 @@ mjCBody* mjCModel::GetWorld() { // find default class name in array -mjCDef* mjCModel::FindDefault(string name) { +mjCDef* mjCModel::FindDefault(const string& name) const { for (int i=0; i < (int)defaults_.size(); i++) { if (defaults_[i]->name == name) { return defaults_[i]; @@ -1697,13 +1697,13 @@ mjSpec* mjCModel::FindSpec(std::string name) const { // find spec by mjsCompiler pointer -mjSpec* mjCModel::FindSpec(const mjsCompiler* compiler_) { +mjSpec* mjCModel::FindSpec(const mjsCompiler* compiler_) const { if (compiler_ == &spec.compiler) { - return &spec; + return &const_cast(this)->spec; } - if (compiler2spec_.find(compiler_) != compiler2spec_.end()) { - return compiler2spec_[compiler_]; + if (auto it = compiler2spec_.find(compiler_); it != compiler2spec_.end()) { + return it->second; } for (auto s : specs_) { diff --git a/src/user/user_model.h b/src/user/user_model.h index b22e0dcb..70cc39fc 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -242,19 +242,19 @@ class mjCModel : public mjCModel_, private mjSpec { // API for access to model elements (outside tree) int NumObjects(mjtObj type); // number of objects in specified list mjCBase* GetObject(mjtObj type, int id); // pointer to specified object - mjsElement* NextObject(mjsElement* object, mjtObj type = mjOBJ_UNKNOWN); // next object of specified type + mjsElement* NextObject(const mjsElement* object, mjtObj type = mjOBJ_UNKNOWN) const; // next object of specified type // API for access to other variables bool IsCompiled() const; // is model already compiled const mjCError& GetError() const; // get reference of error object void SetError(const mjCError& error) { errInfo = error; } // set value of error object mjCBody* GetWorld(); // pointer to world body - mjCDef* FindDefault(std::string name); // find defaults class name + mjCDef* FindDefault(const std::string& name) const; // find defaults class name mjCDef* AddDefault(std::string name, mjCDef* parent = nullptr); // add defaults class to array mjCBase* FindObject(mjtObj type, std::string name) const; // find object given type and name mjCBase* FindTree(mjCBody* body, mjtObj type, std::string name); // find tree object given name mjSpec* FindSpec(std::string name) const; // find spec given name - mjSpec* FindSpec(const mjsCompiler* compiler_); // find spec given mjsCompiler + mjSpec* FindSpec(const mjsCompiler* compiler_) const; // find spec given mjsCompiler void ActivatePlugin(const mjpPlugin* plugin, int slot); // activate plugin // find asset given name checking both name and filename diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 75a7e679..d4f54ec6 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -2251,7 +2251,7 @@ mjCBase* mjCBody::GetObject(mjtObj type, int i) { // find object by name in given list template -static T* findobject(std::string name, std::vector& list) { +static T* findobject(const std::string& name, const std::vector& list) { for (unsigned int i=0; i < list.size(); i++) { if (list[i]->name == name) { return list[i]; @@ -2264,12 +2264,12 @@ static T* findobject(std::string name, std::vector& list) { // recursive find by name -mjCBase* mjCBody::FindObject(mjtObj type, std::string _name, bool recursive) { +mjCBase* mjCBody::FindObject(mjtObj type, const std::string& _name, bool recursive) const { mjCBase* res = 0; // check self: just in case if (name == _name) { - return this; + return const_cast(this); } // search elements of this body @@ -2406,7 +2406,7 @@ static mjsElement* GetNextBody(const mjCBody* body, const mjsElement* child, // get next child of given type -mjsElement* mjCBody::NextChild(const mjsElement* child, mjtObj type, bool recursive) { +mjsElement* mjCBody::NextChild(const mjsElement* child, mjtObj type, bool recursive) const { if (type == mjOBJ_UNKNOWN) { if (!child) { throw mjCError(this, "child type must be specified if no child element is given"); diff --git a/src/user/user_objects.h b/src/user/user_objects.h index c8e63938..c0d1be94 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -531,7 +531,7 @@ class mjCBody : public mjCBody_, private mjsBody { // API for accessing objects int NumObjects(mjtObj type); mjCBase* GetObject(mjtObj type, int id); - mjCBase* FindObject(mjtObj type, std::string name, bool recursive = true); + mjCBase* FindObject(mjtObj type, const std::string& name, bool recursive = true) const; // Propagate suffix and prefix to the whole tree void NameSpace(const mjCModel* m); @@ -556,7 +556,7 @@ class mjCBody : public mjCBody_, private mjsBody { // returns nullptr if the next child is not found or if `child` is the last element, returns // the next child after the input `child` otherwise mjsElement* NextChild(const mjsElement* child, mjtObj type = mjOBJ_UNKNOWN, - bool recursive = false); + bool recursive = false) const; // reset keyframe references for allowing self-attach void ForgetKeyframes() const; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index eddd2024..bbd29e12 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -9687,7 +9687,7 @@ void mjs_deleteUserValue_wrapper(MjsElement& element, const String& key) { mjs_deleteUserValue(element.get(), key.as().data()); } -std::optional mjs_findBody_wrapper(MjSpec& s, const String& name) { +std::optional mjs_findBody_wrapper(const MjSpec& s, const String& name) { CHECK_VAL(name); mjsBody* result = mjs_findBody(s.get(), name.as().data()); if (result == nullptr) { @@ -9696,7 +9696,7 @@ std::optional mjs_findBody_wrapper(MjSpec& s, const String& name) { return MjsBody(result); } -std::optional mjs_findChild_wrapper(MjsBody& body, const String& name) { +std::optional mjs_findChild_wrapper(const MjsBody& body, const String& name) { CHECK_VAL(name); mjsBody* result = mjs_findChild(body.get(), name.as().data()); if (result == nullptr) { @@ -9705,7 +9705,7 @@ std::optional mjs_findChild_wrapper(MjsBody& body, const String& name) return MjsBody(result); } -std::optional mjs_findDefault_wrapper(MjSpec& s, const String& classname) { +std::optional mjs_findDefault_wrapper(const MjSpec& s, const String& classname) { CHECK_VAL(classname); mjsDefault* result = mjs_findDefault(s.get(), classname.as().data()); if (result == nullptr) { @@ -9714,7 +9714,7 @@ std::optional mjs_findDefault_wrapper(MjSpec& s, const String& class return MjsDefault(result); } -std::optional mjs_findElement_wrapper(MjSpec& s, mjtObj type, const String& name) { +std::optional mjs_findElement_wrapper(const MjSpec& s, mjtObj type, const String& name) { CHECK_VAL(name); mjsElement* result = mjs_findElement(s.get(), type, name.as().data()); if (result == nullptr) { @@ -9723,7 +9723,7 @@ std::optional mjs_findElement_wrapper(MjSpec& s, mjtObj type, const return MjsElement(result); } -std::optional mjs_findFrame_wrapper(MjSpec& s, const String& name) { +std::optional mjs_findFrame_wrapper(const MjSpec& s, const String& name) { CHECK_VAL(name); mjsFrame* result = mjs_findFrame(s.get(), name.as().data()); if (result == nullptr) { @@ -9732,7 +9732,7 @@ std::optional mjs_findFrame_wrapper(MjSpec& s, const String& name) { return MjsFrame(result); } -std::optional mjs_findSpec_wrapper(MjSpec& spec, const String& name) { +std::optional mjs_findSpec_wrapper(const MjSpec& spec, const String& name) { CHECK_VAL(name); mjSpec* result = mjs_findSpec(spec.get(), name.as().data()); if (result == nullptr) { @@ -9741,7 +9741,7 @@ std::optional mjs_findSpec_wrapper(MjSpec& spec, const String& name) { return MjSpec(result); } -std::optional mjs_firstChild_wrapper(MjsBody& body, mjtObj type, int recurse) { +std::optional mjs_firstChild_wrapper(const MjsBody& body, mjtObj type, int recurse) { mjsElement* result = mjs_firstChild(body.get(), type, recurse); if (result == nullptr) { return std::nullopt; @@ -9749,7 +9749,7 @@ std::optional mjs_firstChild_wrapper(MjsBody& body, mjtObj type, int return MjsElement(result); } -std::optional mjs_firstElement_wrapper(MjSpec& s, mjtObj type) { +std::optional mjs_firstElement_wrapper(const MjSpec& s, mjtObj type) { mjsElement* result = mjs_firstElement(s.get(), type); if (result == nullptr) { return std::nullopt; @@ -9757,7 +9757,7 @@ std::optional mjs_firstElement_wrapper(MjSpec& s, mjtObj type) { return MjsElement(result); } -std::optional mjs_getCompiler_wrapper(MjsElement& element) { +std::optional mjs_getCompiler_wrapper(const MjsElement& element) { mjsCompiler* result = mjs_getCompiler(element.get()); if (result == nullptr) { return std::nullopt; @@ -9765,7 +9765,7 @@ std::optional mjs_getCompiler_wrapper(MjsElement& element) { return MjsCompiler(result); } -std::optional mjs_getDefault_wrapper(MjsElement& element) { +std::optional mjs_getDefault_wrapper(const MjsElement& element) { mjsDefault* result = mjs_getDefault(element.get()); if (result == nullptr) { return std::nullopt; @@ -9777,7 +9777,7 @@ std::string mjs_getError_wrapper(MjSpec& s) { return std::string(mjs_getError(s.get())); } -std::optional mjs_getFrame_wrapper(MjsElement& element) { +std::optional mjs_getFrame_wrapper(const MjsElement& element) { mjsFrame* result = mjs_getFrame(element.get()); if (result == nullptr) { return std::nullopt; @@ -9785,7 +9785,7 @@ std::optional mjs_getFrame_wrapper(MjsElement& element) { return MjsFrame(result); } -int mjs_getId_wrapper(MjsElement& element) { +int mjs_getId_wrapper(const MjsElement& element) { return mjs_getId(element.get()); } @@ -9793,7 +9793,7 @@ std::string mjs_getName_wrapper(MjsElement& element) { return *mjs_getName(element.get()); } -std::optional mjs_getParent_wrapper(MjsElement& element) { +std::optional mjs_getParent_wrapper(const MjsElement& element) { mjsBody* result = mjs_getParent(element.get()); if (result == nullptr) { return std::nullopt; @@ -9801,7 +9801,7 @@ std::optional mjs_getParent_wrapper(MjsElement& element) { return MjsBody(result); } -std::optional mjs_getSpec_wrapper(MjsElement& element) { +std::optional mjs_getSpec_wrapper(const MjsElement& element) { mjSpec* result = mjs_getSpec(element.get()); if (result == nullptr) { return std::nullopt; @@ -9809,7 +9809,7 @@ std::optional mjs_getSpec_wrapper(MjsElement& element) { return MjSpec(result); } -std::optional mjs_getSpecDefault_wrapper(MjSpec& s) { +std::optional mjs_getSpecDefault_wrapper(const MjSpec& s) { mjsDefault* result = mjs_getSpecDefault(s.get()); if (result == nullptr) { return std::nullopt; @@ -9825,11 +9825,11 @@ std::optional mjs_getWrap_wrapper(const MjsTendon& tendonspec, int i) { return MjsWrap(result); } -double mjs_getWrapCoef_wrapper(MjsWrap& wrap) { +double mjs_getWrapCoef_wrapper(const MjsWrap& wrap) { return mjs_getWrapCoef(wrap.get()); } -double mjs_getWrapDivisor_wrapper(MjsWrap& wrap) { +double mjs_getWrapDivisor_wrapper(const MjsWrap& wrap) { return mjs_getWrapDivisor(wrap.get()); } @@ -9837,7 +9837,7 @@ int mjs_getWrapNum_wrapper(const MjsTendon& tendonspec) { return mjs_getWrapNum(tendonspec.get()); } -std::optional mjs_getWrapSideSite_wrapper(MjsWrap& wrap) { +std::optional mjs_getWrapSideSite_wrapper(const MjsWrap& wrap) { mjsSite* result = mjs_getWrapSideSite(wrap.get()); if (result == nullptr) { return std::nullopt; @@ -9845,7 +9845,7 @@ std::optional mjs_getWrapSideSite_wrapper(MjsWrap& wrap) { return MjsSite(result); } -std::optional mjs_getWrapTarget_wrapper(MjsWrap& wrap) { +std::optional mjs_getWrapTarget_wrapper(const MjsWrap& wrap) { mjsElement* result = mjs_getWrapTarget(wrap.get()); if (result == nullptr) { return std::nullopt; @@ -9862,7 +9862,7 @@ int mjs_makeMesh_wrapper(MjsMesh& mesh, mjtMeshBuiltin builtin, const val& param return mjs_makeMesh(mesh.get(), builtin, params_.data(), nparams); } -std::optional mjs_nextChild_wrapper(MjsBody& body, MjsElement& child, int recurse) { +std::optional mjs_nextChild_wrapper(const MjsBody& body, const MjsElement& child, int recurse) { mjsElement* result = mjs_nextChild(body.get(), child.get(), recurse); if (result == nullptr) { return std::nullopt; @@ -9870,7 +9870,7 @@ std::optional mjs_nextChild_wrapper(MjsBody& body, MjsElement& child return MjsElement(result); } -std::optional mjs_nextElement_wrapper(MjSpec& s, MjsElement& element) { +std::optional mjs_nextElement_wrapper(const MjSpec& s, const MjsElement& element) { mjsElement* result = mjs_nextElement(s.get(), element.get()); if (result == nullptr) { return std::nullopt; From 6376e6707036790f09b5f6e20e8dc5acd98d3542 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 6 May 2026 05:25:15 -0700 Subject: [PATCH 198/251] Extract AVX code from engine_util_blas.c into engine_util_blas_avx.h PiperOrigin-RevId: 911274884 Change-Id: Iaac2789f9538f669c49b497d1e60947a783368d3 --- src/engine/CMakeLists.txt | 1 + src/engine/engine_util_blas.c | 275 ++---------------------------- src/engine/engine_util_blas_avx.h | 160 +++++++++++++++++ 3 files changed, 179 insertions(+), 257 deletions(-) create mode 100644 src/engine/engine_util_blas_avx.h diff --git a/src/engine/CMakeLists.txt b/src/engine/CMakeLists.txt index 3758ed70..e61c1dc4 100644 --- a/src/engine/CMakeLists.txt +++ b/src/engine/CMakeLists.txt @@ -77,6 +77,7 @@ set(MUJOCO_ENGINE_SRCS engine_support.h engine_util_blas.c engine_util_blas.h + engine_util_blas_avx.h engine_util_errmem.c engine_util_errmem.h engine_util_misc.c diff --git a/src/engine/engine_util_blas.c b/src/engine/engine_util_blas.c index 0041484f..8d4e7c3e 100644 --- a/src/engine/engine_util_blas.c +++ b/src/engine/engine_util_blas.c @@ -18,12 +18,7 @@ #include -#ifdef mjUSEPLATFORMSIMD - #if defined(__AVX__) && !defined(mjUSESINGLE) - #define mjUSEAVX - #include "immintrin.h" - #endif -#endif +#include "engine/engine_util_blas_avx.h" // IWYU pragma: keep @@ -339,42 +334,12 @@ void mju_scl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n) { int i = 0; #ifdef mjUSEAVX - int n_4 = n - 4; + i = mju_scl_avx(res, vec, scl, n); +#endif - // vector part - if (n_4 >= 0) { - __m256d sclpar, val1, val1scl; - - // init - sclpar = _mm256_set1_pd(scl); - - // parallel computation - while (i <= n_4) { - val1 = _mm256_loadu_pd(vec+i); - val1scl = _mm256_mul_pd(val1, sclpar); - _mm256_storeu_pd(res+i, val1scl); - i += 4; - } - } - - // process remaining - int n_i = n - i; - if (n_i == 3) { - res[i] = vec[i]*scl; - res[i+1] = vec[i+1]*scl; - res[i+2] = vec[i+2]*scl; - } else if (n_i == 2) { - res[i] = vec[i]*scl; - res[i+1] = vec[i+1]*scl; - } else if (n_i == 1) { - res[i] = vec[i]*scl; - } - -#else for (; i < n; i++) { res[i] = vec[i]*scl; } -#endif } @@ -383,40 +348,12 @@ void mju_add(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) { int i = 0; #ifdef mjUSEAVX - int n_4 = n - 4; + i = mju_add_avx(res, vec1, vec2, n); +#endif - // vector part - if (n_4 >= 0) { - __m256d sum, val1, val2; - - // parallel computation - while (i <= n_4) { - val1 = _mm256_loadu_pd(vec1+i); - val2 = _mm256_loadu_pd(vec2+i); - sum = _mm256_add_pd(val1, val2); - _mm256_storeu_pd(res+i, sum); - i += 4; - } - } - - // process remaining - int n_i = n - i; - if (n_i == 3) { - res[i] = vec1[i] + vec2[i]; - res[i+1] = vec1[i+1] + vec2[i+1]; - res[i+2] = vec1[i+2] + vec2[i+2]; - } else if (n_i == 2) { - res[i] = vec1[i] + vec2[i]; - res[i+1] = vec1[i+1] + vec2[i+1]; - } else if (n_i == 1) { - res[i] = vec1[i] + vec2[i]; - } - -#else for (; i < n; i++) { res[i] = vec1[i] + vec2[i]; } -#endif } @@ -434,40 +371,12 @@ void mju_sub(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) { int i = 0; #ifdef mjUSEAVX - int n_4 = n - 4; + i = mju_sub_avx(res, vec1, vec2, n); +#endif - // vector part - if (n_4 >= 0) { - __m256d dif, val1, val2; - - // parallel computation - while (i <= n_4) { - val1 = _mm256_loadu_pd(vec1+i); - val2 = _mm256_loadu_pd(vec2+i); - dif = _mm256_sub_pd(val1, val2); - _mm256_storeu_pd(res+i, dif); - i += 4; - } - } - - // process remaining - int n_i = n - i; - if (n_i == 3) { - res[i] = vec1[i] - vec2[i]; - res[i+1] = vec1[i+1] - vec2[i+1]; - res[i+2] = vec1[i+2] - vec2[i+2]; - } else if (n_i == 2) { - res[i] = vec1[i] - vec2[i]; - res[i+1] = vec1[i+1] - vec2[i+1]; - } else if (n_i == 1) { - res[i] = vec1[i] - vec2[i]; - } - -#else for (; i < n; i++) { res[i] = vec1[i] - vec2[i]; } -#endif } @@ -485,40 +394,12 @@ void mju_addTo(mjtNum* res, const mjtNum* vec, int n) { int i = 0; #ifdef mjUSEAVX - int n_4 = n - 4; + i = mju_addTo_avx(res, vec, n); +#endif - // vector part - if (n_4 >= 0) { - __m256d sum, val1, val2; - - // parallel computation - while (i <= n_4) { - val1 = _mm256_loadu_pd(res+i); - val2 = _mm256_loadu_pd(vec+i); - sum = _mm256_add_pd(val1, val2); - _mm256_storeu_pd(res+i, sum); - i += 4; - } - } - - // process remaining - int n_i = n - i; - if (n_i == 3) { - res[i] += vec[i]; - res[i+1] += vec[i+1]; - res[i+2] += vec[i+2]; - } else if (n_i == 2) { - res[i] += vec[i]; - res[i+1] += vec[i+1]; - } else if (n_i == 1) { - res[i] += vec[i]; - } - -#else for (; i < n; i++) { res[i] += vec[i]; } -#endif } @@ -536,40 +417,12 @@ void mju_subFrom(mjtNum* res, const mjtNum* vec, int n) { int i = 0; #ifdef mjUSEAVX - int n_4 = n - 4; + i = mju_subFrom_avx(res, vec, n); +#endif - // vector part - if (n_4 >= 0) { - __m256d dif, val1, val2; - - // parallel computation - while (i <= n_4) { - val1 = _mm256_loadu_pd(res+i); - val2 = _mm256_loadu_pd(vec+i); - dif = _mm256_sub_pd(val1, val2); - _mm256_storeu_pd(res+i, dif); - i += 4; - } - } - - // process remaining - int n_i = n - i; - if (n_i == 3) { - res[i] -= vec[i]; - res[i+1] -= vec[i+1]; - res[i+2] -= vec[i+2]; - } else if (n_i == 2) { - res[i] -= vec[i]; - res[i+1] -= vec[i+1]; - } else if (n_i == 1) { - res[i] -= vec[i]; - } - -#else for (; i < n; i++) { res[i] -= vec[i]; } -#endif } @@ -578,44 +431,12 @@ void mju_addToScl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n) { int i = 0; #ifdef mjUSEAVX - int n_4 = n - 4; + i = mju_addToScl_avx(res, vec, scl, n); +#endif - // vector part - if (n_4 >= 0) { - __m256d sclpar, sum, val1, val2, val2scl; - - // init - sclpar = _mm256_set1_pd(scl); - - // parallel computation - while (i <= n_4) { - val1 = _mm256_loadu_pd(res+i); - val2 = _mm256_loadu_pd(vec+i); - val2scl = _mm256_mul_pd(val2, sclpar); - sum = _mm256_add_pd(val1, val2scl); - _mm256_storeu_pd(res+i, sum); - i += 4; - } - } - - // process remaining - int n_i = n - i; - if (n_i == 3) { - res[i] += vec[i]*scl; - res[i+1] += vec[i+1]*scl; - res[i+2] += vec[i+2]*scl; - } else if (n_i == 2) { - res[i] += vec[i]*scl; - res[i+1] += vec[i+1]*scl; - } else if (n_i == 1) { - res[i] += vec[i]*scl; - } - -#else for (; i < n; i++) { res[i] += vec[i]*scl; } -#endif } @@ -634,45 +455,13 @@ void mju_addToSclInd(mjtNum* res, const mjtNum* vec, const int* ind, mjtNum scl, void mju_addScl(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, mjtNum scl, int n) { int i = 0; -#if defined(__AVX__) && defined(mjUSEAVX) && !defined(mjUSESINGLE) - int n_4 = n - 4; +#ifdef mjUSEAVX + i = mju_addScl_avx(res, vec1, vec2, scl, n); +#endif - // vector part - if (n_4 >= 0) { - __m256d sclpar, sum, val1, val2, val2scl; - - // init - sclpar = _mm256_set1_pd(scl); - - // parallel computation - while (i <= n_4) { - val1 = _mm256_loadu_pd(vec1+i); - val2 = _mm256_loadu_pd(vec2+i); - val2scl = _mm256_mul_pd(val2, sclpar); - sum = _mm256_add_pd(val1, val2scl); - _mm256_storeu_pd(res+i, sum); - i += 4; - } - } - - // process remaining - int n_i = n - i; - if (n_i == 3) { - res[i] = vec1[i] + vec2[i]*scl; - res[i+1] = vec1[i+1] + vec2[i+1]*scl; - res[i+2] = vec1[i+2] + vec2[i+2]*scl; - } else if (n_i == 2) { - res[i] = vec1[i] + vec2[i]*scl; - res[i+1] = vec1[i+1] + vec2[i+1]*scl; - } else if (n_i == 1) { - res[i] = vec1[i] + vec2[i]*scl; - } - -#else for (; i < n; i++) { res[i] = vec1[i] + vec2[i]*scl; } -#endif } @@ -704,41 +493,13 @@ mjtNum mju_norm(const mjtNum* res, int n) { mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n) { mjtNum res = 0; int i = 0; - int n_4 = n - 4; #ifdef mjUSEAVX - - // vector part - if (n_4 >= 0) { - __m256d sum, prod, val1, val2; - __m128d vlow, vhigh, high64; - - // init - val1 = _mm256_loadu_pd(vec1); - val2 = _mm256_loadu_pd(vec2); - sum = _mm256_mul_pd(val1, val2); - i = 4; - - // parallel computation - while (i <= n_4) { - val1 = _mm256_loadu_pd(vec1+i); - val2 = _mm256_loadu_pd(vec2+i); - prod = _mm256_mul_pd(val1, val2); - sum = _mm256_add_pd(sum, prod); - i += 4; - } - - // reduce - vlow = _mm256_castpd256_pd128(sum); - vhigh = _mm256_extractf128_pd(sum, 1); - vlow = _mm_add_pd(vlow, vhigh); - high64 = _mm_unpackhi_pd(vlow, vlow); - res = _mm_cvtsd_f64(_mm_add_sd(vlow, high64)); - } - + res = mju_dot_avx(vec1, vec2, n, &i); #else // do the same order of additions as the AVX intrinsics implementation. // this is faster than the simple for loop you'd expect for a dot product, // and produces exactly the same results. + int n_4 = n - 4; mjtNum res0 = 0; mjtNum res1 = 0; mjtNum res2 = 0; diff --git a/src/engine/engine_util_blas_avx.h b/src/engine/engine_util_blas_avx.h new file mode 100644 index 00000000..ff2af6c7 --- /dev/null +++ b/src/engine/engine_util_blas_avx.h @@ -0,0 +1,160 @@ +// Copyright 2026 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_SRC_ENGINE_ENGINE_UTIL_BLAS_AVX_H_ +#define MUJOCO_SRC_ENGINE_ENGINE_UTIL_BLAS_AVX_H_ + +#ifdef mjUSEPLATFORMSIMD +#if defined(__AVX__) && !defined(mjUSESINGLE) + +#define mjUSEAVX + +#include + +#include + +// res = vec*scl +static inline +int mju_scl_avx(mjtNum* res, const mjtNum* vec, mjtNum scl, int n) { + int i = 0; + int n_4 = n - 4; + if (n_4 >= 0) { + __m256d sclpar = _mm256_set1_pd(scl); + while (i <= n_4) { + _mm256_storeu_pd(res+i, _mm256_mul_pd(_mm256_loadu_pd(vec+i), sclpar)); + i += 4; + } + } + return i; +} + +// res = vec1 + vec2 +static inline +int mju_add_avx(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) { + int i = 0; + int n_4 = n - 4; + if (n_4 >= 0) { + while (i <= n_4) { + _mm256_storeu_pd(res+i, _mm256_add_pd(_mm256_loadu_pd(vec1+i), _mm256_loadu_pd(vec2+i))); + i += 4; + } + } + return i; +} + +// res = vec1 - vec2 +static inline +int mju_sub_avx(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n) { + int i = 0; + int n_4 = n - 4; + if (n_4 >= 0) { + while (i <= n_4) { + _mm256_storeu_pd(res+i, _mm256_sub_pd(_mm256_loadu_pd(vec1+i), _mm256_loadu_pd(vec2+i))); + i += 4; + } + } + return i; +} + +// res += vec +static inline +int mju_addTo_avx(mjtNum* res, const mjtNum* vec, int n) { + int i = 0; + int n_4 = n - 4; + if (n_4 >= 0) { + while (i <= n_4) { + _mm256_storeu_pd(res+i, _mm256_add_pd(_mm256_loadu_pd(res+i), _mm256_loadu_pd(vec+i))); + i += 4; + } + } + return i; +} + +// res -= vec +static inline +int mju_subFrom_avx(mjtNum* res, const mjtNum* vec, int n) { + int i = 0; + int n_4 = n - 4; + if (n_4 >= 0) { + while (i <= n_4) { + _mm256_storeu_pd(res+i, _mm256_sub_pd(_mm256_loadu_pd(res+i), _mm256_loadu_pd(vec+i))); + i += 4; + } + } + return i; +} + +// res += vec*scl +static inline +int mju_addToScl_avx(mjtNum* res, const mjtNum* vec, mjtNum scl, int n) { + int i = 0; + int n_4 = n - 4; + if (n_4 >= 0) { + __m256d sclpar = _mm256_set1_pd(scl); + while (i <= n_4) { + __m256d val1 = _mm256_loadu_pd(res+i); + __m256d val2 = _mm256_loadu_pd(vec+i); + _mm256_storeu_pd(res+i, _mm256_add_pd(val1, _mm256_mul_pd(val2, sclpar))); + i += 4; + } + } + return i; +} + +// res = vec1 + vec2*scl +static inline +int mju_addScl_avx(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, mjtNum scl, int n) { + int i = 0; + int n_4 = n - 4; + if (n_4 >= 0) { + __m256d sclpar = _mm256_set1_pd(scl); + while (i <= n_4) { + __m256d val1 = _mm256_loadu_pd(vec1+i); + __m256d val2 = _mm256_loadu_pd(vec2+i); + _mm256_storeu_pd(res+i, _mm256_add_pd(val1, _mm256_mul_pd(val2, sclpar))); + i += 4; + } + } + return i; +} + +// vector dot-product +static inline +mjtNum mju_dot_avx(const mjtNum* vec1, const mjtNum* vec2, int n, int* processed) { + mjtNum res = 0; + int i = 0; + int n_4 = n - 4; + if (n_4 >= 0) { + __m256d sum = _mm256_mul_pd(_mm256_loadu_pd(vec1), _mm256_loadu_pd(vec2)); + i = 4; + + while (i <= n_4) { + sum = _mm256_add_pd(sum, _mm256_mul_pd(_mm256_loadu_pd(vec1+i), _mm256_loadu_pd(vec2+i))); + i += 4; + } + + __m128d vlow = _mm256_castpd256_pd128(sum); + __m128d vhigh = _mm256_extractf128_pd(sum, 1); + vlow = _mm_add_pd(vlow, vhigh); + __m128d high64 = _mm_unpackhi_pd(vlow, vlow); + res = _mm_cvtsd_f64(_mm_add_sd(vlow, high64)); + } + *processed = i; + return res; +} + +#endif // defined(__AVX__) && !defined(mjUSESINGLE) +#endif // mjUSEPLATFORMSIMD + +#endif // MUJOCO_SRC_ENGINE_ENGINE_UTIL_BLAS_AVX_H_ From 9ffd50ce6a76b5cea34375a746fe23ff6405805f Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Wed, 6 May 2026 08:47:18 -0700 Subject: [PATCH 199/251] Import google-deepmind/mujoco_warp from GitHub. PiperOrigin-RevId: 911361969 Change-Id: I2dd1223a4279b3df2f11adc4f44778b8d153e599 --- .../mjx/third_party/mujoco_warp/_src/cli.py | 15 +++---- .../mjx/third_party/mujoco_warp/_src/io.py | 41 +++++++++++++++++++ .../third_party/mujoco_warp/_src/passive.py | 22 ++++++---- .../third_party/mujoco_warp/_src/sensor.py | 17 +++++++- .../mjx/third_party/mujoco_warp/_src/types.py | 16 ++++++-- .../third_party/mujoco_warp/pyproject.toml | 1 + mjx/mujoco/mjx/warp/forward.py | 21 ++++++++-- mjx/mujoco/mjx/warp/render.py | 1 - mjx/mujoco/mjx/warp/types.py | 16 +++++++- 9 files changed, 122 insertions(+), 28 deletions(-) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/cli.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/cli.py index 912df58f..0e427408 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/cli.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/cli.py @@ -21,14 +21,12 @@ from typing import Callable, Tuple, get_type_hints import mujoco import numpy as np import warp as wp -from absl import app from absl import flags from etils import epath import mujoco.mjx.third_party.mujoco_warp as mjw from mujoco.mjx.third_party.mujoco_warp._src import warp_util -from mujoco.mjx.third_party.mujoco_warp._src.io import find_keys -from mujoco.mjx.third_party.mujoco_warp._src.io import make_trajectory +from mujoco.mjx.third_party.mujoco_warp._src.io import load_trajectory from mujoco.mjx.third_party.mujoco_warp._src.io import override_model from mujoco.mjx.third_party.mujoco_warp._src.util_misc import halton @@ -46,7 +44,7 @@ NOISE_STD = flags.DEFINE_float("noise_std", 0.01, "add noise to ctrl signal (sta NOISE_RATE = flags.DEFINE_float("noise_rate", 0.1, "add noise to ctrl signal (noise rate)") DEVICE = flags.DEFINE_string("device", None, "override the default Warp device") -REPLAY = flags.DEFINE_string("replay", None, "keyframe sequence to replay, keyframe name must prefix match") +REPLAY = flags.DEFINE_string("replay", None, "NPZ file with ctrl sequence to replay") RENDER_WIDTH = flags.DEFINE_integer("render_width", 64, "render width (pixels)") RENDER_HEIGHT = flags.DEFINE_integer("render_height", 64, "render height (pixels)") @@ -133,11 +131,10 @@ def init_structs( mjd = mujoco.MjData(mjm) ctrls = None if REPLAY.value: - keys = find_keys(mjm, REPLAY.value) - if not keys: - raise app.UsageError(f"Key prefix not found: {REPLAY.value}") - ctrls = make_trajectory(mjm, keys) - mujoco.mj_resetDataKeyframe(mjm, mjd, keys[0]) + ctrls = load_trajectory(REPLAY.value, mjm, mjd) + # default nstep to trajectory length when not explicitly set + if flags.FLAGS["nstep"].using_default_value: + flags.FLAGS.nstep = len(ctrls) elif mjm.nkey > 0 and KEYFRAME.value > -1: mujoco.mj_resetDataKeyframe(mjm, mjd, KEYFRAME.value) ctrls = [mjd.ctrl.copy() for _ in range(NSTEP.value)] diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py index dc834e06..309f9e41 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/io.py @@ -660,6 +660,14 @@ def put_model(mjm: mujoco.MjModel) -> types.Model: m.flexedge_J_rowadr = mjm.flexedge_J_rowadr m.flexedge_J_colind = mjm.flexedge_J_colind.reshape(-1) + # flex_bendingadr backward compat: flatten old (nflexedge, 17) to 1D + if not check_version("mujoco>=3.8.1.dev909088123"): + m.flex_bendingadr = ( + np.array([mjm.flex_edgeadr[i] * 17 for i in range(mjm.nflex)], dtype=int) if mjm.nflex else np.zeros(0, dtype=int) + ) + m.flex_bending = mjm.flex_bending.ravel() + m.nflexbending = len(m.flex_bending) + # place m on device sizes = dict({"*": 1}, **{f.name: getattr(m, f.name) for f in dataclasses.fields(types.Model) if f.type is int}) for f in dataclasses.fields(types.Model): @@ -2654,6 +2662,39 @@ def make_trajectory(model: mujoco.MjModel, keys: list[int]) -> np.ndarray: return np.array(ctrls) +def load_trajectory(npz_path: str, mjm: mujoco.MjModel, mjd: mujoco.MjData) -> np.ndarray: + """Load ctrl sequence from NPZ and interpolate to model timestep. + + If the trajectory dt differs from mjm.opt.timestep, each ctrl value is held + constant (zero-order hold) for the appropriate number of physics steps. + + The NPZ file should contain: + - 'ctrl': array of shape (nstep, nu) with ctrl values + - 'times': array of shape (nstep,) with timestamps + - 'qpos' (optional): array of shape (1, nq) - initial state + - 'qvel' (optional): array of shape (1, nv) - initial state + """ + data = np.load(npz_path) + ctrl = data["ctrl"] + times = data["times"] + + if ctrl.shape[1] != mjm.nu: + raise ValueError(f"ctrl shape {ctrl.shape} does not match model nu={mjm.nu}") + + # set initial state from first frame if available + if "qpos" in data and data["qpos"].shape[1] == mjm.nq: + mjd.qpos[:] = data["qpos"][0] + if "qvel" in data and data["qvel"].shape[1] == mjm.nv: + mjd.qvel[:] = data["qvel"][0] + + # determine decimation from timing + ctrl_dt = (times[1] - times[0]) if len(times) > 1 else mjm.opt.timestep + decimation = max(1, round(ctrl_dt / mjm.opt.timestep)) + + # expand: each ctrl held constant for decimation physics steps + return np.repeat(ctrl, decimation, axis=0) + + @wp.kernel def _build_rays( # In: diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py index 31469590..2d261abb 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/passive.py @@ -604,12 +604,13 @@ def _flex_elasticity( flex_elemadr: wp.array[int], flex_elemnum: wp.array[int], flex_elemdataadr: wp.array[int], + flex_stiffnessadr: wp.array[int], flex_elemedgeadr: wp.array[int], flex_vertbodyid: wp.array[int], flex_elem: wp.array[int], flex_elemedge: wp.array[int], flexedge_length0: wp.array[float], - flex_stiffness: wp.array2d[float], + flex_stiffness: wp.array[float], flex_damping: wp.array[float], # Data in: flexvert_xpos_in: wp.array2d[wp.vec3], @@ -669,11 +670,13 @@ def _flex_elasticity( elongation[e] = deformed * deformed - reference * reference + (deformed * deformed - previous * previous) * kD metric = wp.matrix(0.0, shape=(6, 6)) + stiffness_size = nedge * (nedge + 1) / 2 + stiffness_adr = flex_stiffnessadr[f] + local_elemid * stiffness_size id = int(0) for ed1 in range(nedge): for ed2 in range(ed1, nedge): - metric[ed1, ed2] = flex_stiffness[elemid, id] - metric[ed2, ed1] = flex_stiffness[elemid, id] + metric[ed1, ed2] = flex_stiffness[stiffness_adr + id] + metric[ed2, ed1] = flex_stiffness[stiffness_adr + id] id += 1 force = wp.matrix(0.0, shape=(6, 3)) @@ -699,10 +702,11 @@ def _flex_bending( flex_vertadr: wp.array[int], flex_edgeadr: wp.array[int], flex_edgenum: wp.array[int], + flex_bendingadr: wp.array[int], flex_vertbodyid: wp.array[int], flex_edge: wp.array[wp.vec2i], flex_edgeflap: wp.array[wp.vec2i], - flex_bending: wp.array2d[float], + flex_bending: wp.array[float], # Data in: flexvert_xpos_in: wp.array2d[wp.vec3], # Data out: @@ -730,8 +734,10 @@ def _flex_bending( flex_vertadr[f] + flex_edgeflap[edgeid][1], ) + adr = flex_bendingadr[f] + frc = wp.matrix(0.0, shape=(4, 3)) - if flex_bending[edgeid, 16]: + if flex_bending[adr + 16]: v0 = flexvert_xpos_in[worldid, v[0]] v1 = flexvert_xpos_in[worldid, v[1]] v2 = flexvert_xpos_in[worldid, v[2]] @@ -746,8 +752,8 @@ def _flex_bending( for x in range(3): acc = float(0.0) for j in range(nvert): - acc += flex_bending[edgeid, 4 * i + j] * flexvert_xpos_in[worldid, v[j]][x] - force[i, x] = -(acc + flex_bending[edgeid, 16] * frc[i, x]) + acc += flex_bending[adr + 4 * i + j] * flexvert_xpos_in[worldid, v[j]][x] + force[i, x] = -(acc + flex_bending[adr + 16] * frc[i, x]) for i in range(nvert): bodyid = flex_vertbodyid[v[i]] @@ -827,6 +833,7 @@ def passive(m: Model, d: Data): m.flex_elemadr, m.flex_elemnum, m.flex_elemdataadr, + m.flex_stiffnessadr, m.flex_elemedgeadr, m.flex_vertbodyid, m.flex_elem, @@ -851,6 +858,7 @@ def passive(m: Model, d: Data): m.flex_vertadr, m.flex_edgeadr, m.flex_edgenum, + m.flex_bendingadr, m.flex_vertbodyid, m.flex_edge, m.flex_edgeflap, diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py index 381dfd66..0610612b 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/sensor.py @@ -464,6 +464,8 @@ def _sensor_pos( body_geomnum: wp.array[int], body_geomadr: wp.array[int], body_iquat: wp.array2d[wp.quat], + body_mass: wp.array2d[float], + body_subtreemass: wp.array2d[float], jnt_qposadr: wp.array[int], geom_type: wp.array[int], geom_bodyid: wp.array[int], @@ -684,7 +686,18 @@ def _sensor_pos( if objtype == ObjType.XBODY: xpos = xpos_in[worldid, objid] elif objtype == ObjType.BODY: - xpos = xipos_in[worldid, objid] + # for massless bodies with positive subtree mass (e.g., flex parents), + # xipos is the static body frame origin; use subtree_com instead + if objid > 0: + if ( + body_mass[worldid % body_mass.shape[0], objid] < MJ_MINVAL + and body_subtreemass[worldid % body_subtreemass.shape[0], objid] >= MJ_MINVAL + ): + xpos = subtree_com_in[worldid, objid] + else: + xpos = xipos_in[worldid, objid] + else: + xpos = xipos_in[worldid, objid] elif objtype == ObjType.GEOM: xpos = geom_xpos_in[worldid, objid] elif objtype == ObjType.SITE: @@ -830,6 +843,8 @@ def sensor_pos(m: Model, d: Data): m.body_geomnum, m.body_geomadr, m.body_iquat, + m.body_mass, + m.body_subtreemass, m.jnt_qposadr, m.geom_type, m.geom_bodyid, diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py index 2395a711..7a4c8242 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py @@ -867,6 +867,8 @@ class Model: nflexedge: number of edges in all flexes nflexelem: number of elements in all flexes nflexelemdata: number of element vertex ids in all flexes + nflexstiffness: number of stiffness parameters in all flexes + nflexbending: number of bending parameters in all flexes nflexelemedge: number of element edge ids in all flexes nflexshelldata: number of shell fragment vertex ids in all flexes nJfe: number of non-zeros in sparse flexedge Jacobian @@ -1022,7 +1024,9 @@ class Model: flex_elemadr: first element address (nflex,) flex_elemnum: number of elements (nflex,) flex_elemdataadr: first element vertex id address (nflex,) + flex_stiffnessadr: stiffness matrix address (nflex,) flex_elemedgeadr: first element edge id address (nflex,) + flex_bendingadr: first bending data address (nflex,) flex_shellnum: number of shells (nflex,) flex_shelldataadr: first shell data address (nflex,) flex_vertbodyid: vertex body ids (nflexvert,) @@ -1035,8 +1039,8 @@ class Model: flexedge_length0: edge lengths in qpos0 (nflexedge,) flexedge_invweight0: inv. inertia for the edge (nflexedge,) flex_radius: radius around primitive element (nflex,) - flex_stiffness: finite element stiffness matrix (nflexelem, 21) - flex_bending: bending stiffness (nflexedge, 17) + flex_stiffness: finite element stiffness matrix (nflexstiffness,) + flex_bending: bending stiffness (nflexbending,) flex_damping: Rayleigh's damping coefficient (nflex,) flex_centered: flex vertices are centered at body origin (nflex,) flexedge_J_rownnz: number of nonzeros in Jacobian row (nflexedge,) @@ -1266,6 +1270,8 @@ class Model: nflexedge: int nflexelem: int nflexelemdata: int + nflexstiffness: int + nflexbending: int nflexelemedge: int nflexshelldata: int nJfe: int @@ -1421,7 +1427,9 @@ class Model: flex_elemadr: array("nflex", int) flex_elemnum: array("nflex", int) flex_elemdataadr: array("nflex", int) + flex_stiffnessadr: array("nflex", int) flex_elemedgeadr: array("nflex", int) + flex_bendingadr: array("nflex", int) flex_shellnum: array("nflex", int) flex_shelldataadr: array("nflex", int) flex_vertbodyid: array("nflexvert", int) @@ -1434,8 +1442,8 @@ class Model: flexedge_length0: array("nflexedge", float) flexedge_invweight0: array("nflexedge", float) flex_radius: array("nflex", float) - flex_stiffness: array("nflexelem", 21, float) - flex_bending: array("nflexedge", 17, float) + flex_stiffness: array("nflexstiffness", float) + flex_bending: array("nflexbending", float) flex_damping: array("nflex", float) flex_centered: array("nflex", bool) flexedge_J_rownnz: array("nflexedge", int) diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/pyproject.toml b/mjx/mujoco/mjx/third_party/mujoco_warp/pyproject.toml index a5caa4b4..92750398 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/pyproject.toml +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/pyproject.toml @@ -22,6 +22,7 @@ classifiers = [ "Programming Language :: Python :: 3.11", "Programming Language :: Python :: 3.12", "Programming Language :: Python :: 3.13", + "Programming Language :: Python :: 3.14", "Topic :: Scientific/Engineering", ] requires-python = ">=3.10" diff --git a/mjx/mujoco/mjx/warp/forward.py b/mjx/mujoco/mjx/warp/forward.py index f260ea92..0adf9236 100644 --- a/mjx/mujoco/mjx/warp/forward.py +++ b/mjx/mujoco/mjx/warp/forward.py @@ -46,6 +46,7 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) + @ffi.format_args_for_warp def _forward_shim( # Model @@ -138,7 +139,8 @@ def _forward_shim( eq_ten_adr: wp.array[int], eq_type: wp.array[int], eq_wld_adr: wp.array[int], - flex_bending: wp.array2d[float], + flex_bending: wp.array[float], + flex_bendingadr: wp.array[int], flex_centered: wp.array[bool], flex_conaffinity: wp.array[int], flex_condim: wp.array[int], @@ -166,7 +168,8 @@ def _forward_shim( flex_solimp: wp.array[mjwp_types.vec5], flex_solmix: wp.array[float], flex_solref: wp.array[wp.vec2], - flex_stiffness: wp.array2d[float], + flex_stiffness: wp.array[float], + flex_stiffnessadr: wp.array[int], flex_vert: wp.array[wp.vec3], flex_vertadr: wp.array[int], flex_vertbodyid: wp.array[int], @@ -624,6 +627,7 @@ def _forward_shim( _m.eq_type = eq_type _m.eq_wld_adr = eq_wld_adr _m.flex_bending = flex_bending + _m.flex_bendingadr = flex_bendingadr _m.flex_centered = flex_centered _m.flex_conaffinity = flex_conaffinity _m.flex_condim = flex_condim @@ -652,6 +656,7 @@ def _forward_shim( _m.flex_solmix = flex_solmix _m.flex_solref = flex_solref _m.flex_stiffness = flex_stiffness + _m.flex_stiffnessadr = flex_stiffnessadr _m.flex_vert = flex_vert _m.flex_vertadr = flex_vertadr _m.flex_vertbodyid = flex_vertbodyid @@ -1504,6 +1509,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m.eq_type, m._impl.eq_wld_adr, m._impl.flex_bending, + m._impl.flex_bendingadr, m._impl.flex_centered, m._impl.flex_conaffinity, m._impl.flex_condim, @@ -1532,6 +1538,7 @@ def _forward_jax_impl(m: types.Model, d: types.Data): m._impl.flex_solmix, m._impl.flex_solref, m._impl.flex_stiffness, + m._impl.flex_stiffnessadr, m._impl.flex_vert, m.flex_vertadr, m._impl.flex_vertbodyid, @@ -2107,7 +2114,8 @@ def _step_shim( eq_ten_adr: wp.array[int], eq_type: wp.array[int], eq_wld_adr: wp.array[int], - flex_bending: wp.array2d[float], + flex_bending: wp.array[float], + flex_bendingadr: wp.array[int], flex_centered: wp.array[bool], flex_conaffinity: wp.array[int], flex_condim: wp.array[int], @@ -2135,7 +2143,8 @@ def _step_shim( flex_solimp: wp.array[mjwp_types.vec5], flex_solmix: wp.array[float], flex_solref: wp.array[wp.vec2], - flex_stiffness: wp.array2d[float], + flex_stiffness: wp.array[float], + flex_stiffnessadr: wp.array[int], flex_vert: wp.array[wp.vec3], flex_vertadr: wp.array[int], flex_vertbodyid: wp.array[int], @@ -2595,6 +2604,7 @@ def _step_shim( _m.eq_type = eq_type _m.eq_wld_adr = eq_wld_adr _m.flex_bending = flex_bending + _m.flex_bendingadr = flex_bendingadr _m.flex_centered = flex_centered _m.flex_conaffinity = flex_conaffinity _m.flex_condim = flex_condim @@ -2623,6 +2633,7 @@ def _step_shim( _m.flex_solmix = flex_solmix _m.flex_solref = flex_solref _m.flex_stiffness = flex_stiffness + _m.flex_stiffnessadr = flex_stiffnessadr _m.flex_vert = flex_vert _m.flex_vertadr = flex_vertadr _m.flex_vertbodyid = flex_vertbodyid @@ -3489,6 +3500,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): m.eq_type, m._impl.eq_wld_adr, m._impl.flex_bending, + m._impl.flex_bendingadr, m._impl.flex_centered, m._impl.flex_conaffinity, m._impl.flex_condim, @@ -3517,6 +3529,7 @@ def _step_jax_impl(m: types.Model, d: types.Data): m._impl.flex_solmix, m._impl.flex_solref, m._impl.flex_stiffness, + m._impl.flex_stiffnessadr, m._impl.flex_vert, m.flex_vertadr, m._impl.flex_vertbodyid, diff --git a/mjx/mujoco/mjx/warp/render.py b/mjx/mujoco/mjx/warp/render.py index 719e8df9..cf6754a1 100644 --- a/mjx/mujoco/mjx/warp/render.py +++ b/mjx/mujoco/mjx/warp/render.py @@ -48,7 +48,6 @@ _cb = mjwp_types.Callback( **{f.name: None for f in dataclasses.fields(mjwp_types.Callback) if f.init} ) - @ffi.format_args_for_warp def _render_shim( # Model diff --git a/mjx/mujoco/mjx/warp/types.py b/mjx/mujoco/mjx/warp/types.py index 81e77ec8..f64b6fba 100644 --- a/mjx/mujoco/mjx/warp/types.py +++ b/mjx/mujoco/mjx/warp/types.py @@ -143,6 +143,7 @@ class ModelWarp(PyTreeNode): eq_ten_adr: np.ndarray eq_wld_adr: np.ndarray flex_bending: np.ndarray + flex_bendingadr: np.ndarray flex_centered: np.ndarray flex_conaffinity: np.ndarray flex_condim: np.ndarray @@ -171,6 +172,7 @@ class ModelWarp(PyTreeNode): flex_solmix: np.ndarray flex_solref: np.ndarray flex_stiffness: np.ndarray + flex_stiffnessadr: np.ndarray flex_vert: np.ndarray flex_vertbodyid: np.ndarray flex_vertflexid: np.ndarray @@ -205,11 +207,13 @@ class ModelWarp(PyTreeNode): nJfe: int nacttrnbody: int nbranch: int + nflexbending: int nflexedge: int nflexelem: int nflexelemdata: int nflexelemedge: int nflexshelldata: int + nflexstiffness: int nflexvert: int nmaxcondim: int nmaxmeshdeg: int @@ -645,7 +649,8 @@ _NDIM = { 'eq_type': 1, 'eq_wld_adr': 1, 'exclude_signature': 1, - 'flex_bending': 2, + 'flex_bending': 1, + 'flex_bendingadr': 1, 'flex_centered': 1, 'flex_conaffinity': 1, 'flex_condim': 1, @@ -673,7 +678,8 @@ _NDIM = { 'flex_solimp': 2, 'flex_solmix': 1, 'flex_solref': 2, - 'flex_stiffness': 2, + 'flex_stiffness': 1, + 'flex_stiffnessadr': 1, 'flex_vert': 2, 'flex_vertadr': 1, 'flex_vertbodyid': 1, @@ -785,11 +791,13 @@ _NDIM = { 'neq': 0, 'nexclude': 0, 'nflex': 0, + 'nflexbending': 0, 'nflexedge': 0, 'nflexelem': 0, 'nflexelemdata': 0, 'nflexelemedge': 0, 'nflexshelldata': 0, + 'nflexstiffness': 0, 'nflexvert': 0, 'ngeom': 0, 'ngravcomp': 0, @@ -1224,6 +1232,7 @@ _BATCH_DIM = { 'eq_wld_adr': False, 'exclude_signature': False, 'flex_bending': False, + 'flex_bendingadr': False, 'flex_centered': False, 'flex_conaffinity': False, 'flex_condim': False, @@ -1252,6 +1261,7 @@ _BATCH_DIM = { 'flex_solmix': False, 'flex_solref': False, 'flex_stiffness': False, + 'flex_stiffnessadr': False, 'flex_vert': False, 'flex_vertadr': False, 'flex_vertbodyid': False, @@ -1363,11 +1373,13 @@ _BATCH_DIM = { 'neq': False, 'nexclude': False, 'nflex': False, + 'nflexbending': False, 'nflexedge': False, 'nflexelem': False, 'nflexelemdata': False, 'nflexelemedge': False, 'nflexshelldata': False, + 'nflexstiffness': False, 'nflexvert': False, 'ngeom': False, 'ngravcomp': False, From 2b67fe3c3a6d16de800b2fd2d4d84b41a31a8869 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 6 May 2026 08:56:49 -0700 Subject: [PATCH 200/251] Remove direct dependency between SceneBridge and ImguiBridge. PiperOrigin-RevId: 911366540 Change-Id: I05c348ea6b1d08c034f2bb26e4c770079073c61d --- .../filament/compat/mjr_filament_renderer.cc | 3 +++ src/experimental/filament/compat/scene_bridge.cc | 10 +++++++--- src/experimental/filament/compat/scene_bridge.h | 5 +++++ 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/src/experimental/filament/compat/mjr_filament_renderer.cc b/src/experimental/filament/compat/mjr_filament_renderer.cc index 3046c9f3..a23de31b 100644 --- a/src/experimental/filament/compat/mjr_filament_renderer.cc +++ b/src/experimental/filament/compat/mjr_filament_renderer.cc @@ -17,7 +17,9 @@ #include #include #include +#include +#include #include #include #include @@ -39,6 +41,7 @@ MjrFilamentRenderer::MjrFilamentRenderer(const mjrFilamentConfig* config) { void MjrFilamentRenderer::Init(const mjModel* model) { scene_bridge_ = std::make_unique(filament_context_.get(), model); imgui_bridge_ = std::make_unique(filament_context_.get()); + scene_bridge_->SetDrawTextFunction(DrawTextAt); mjr_defaultRenderRequest(&render_requests_[0]); mjr_defaultRenderRequest(&render_requests_[1]); diff --git a/src/experimental/filament/compat/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc index 616cc726..a7e6a5e7 100644 --- a/src/experimental/filament/compat/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -26,7 +26,6 @@ #include #include #include -#include "experimental/filament/compat/imgui_bridge.h" #include "experimental/filament/compat/model_objects.h" #include "experimental/filament/compat/scene_geom_util.h" #include "experimental/filament/filament/filament_context.h" @@ -302,9 +301,9 @@ void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { for (int i = 0; i < scene->ngeom; ++i) { const mjvGeom* geom = scene->geoms + i; - if (geom->label[0] != 0) { + if (draw_text_callback_ && geom->label[0] != 0) { if (auto pos = ClipFromWorld(ReadFloat3(geom->pos))) { - DrawTextAt(geom->label, pos->x, pos->y, pos->z); + draw_text_callback_(geom->label, pos->x, pos->y, pos->z); } } @@ -361,4 +360,9 @@ void SceneBridge::UploadTexture(const mjModel* model, int id) { void SceneBridge::UploadHeightField(const mjModel* model, int id) { model_objects_->UploadHeightField(model, id); } + +void SceneBridge::SetDrawTextFunction(DrawTextAtFn fn) { + draw_text_callback_ = std::move(fn); +} + } // namespace mujoco diff --git a/src/experimental/filament/compat/scene_bridge.h b/src/experimental/filament/compat/scene_bridge.h index ce6911b6..3de3f2f0 100644 --- a/src/experimental/filament/compat/scene_bridge.h +++ b/src/experimental/filament/compat/scene_bridge.h @@ -15,6 +15,7 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_SCENE_BRIDGE_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_SCENE_BRIDGE_H_ +#include #include #include #include @@ -51,6 +52,9 @@ class SceneBridge { void UploadTexture(const mjModel* model, int id); void UploadHeightField(const mjModel* model, int id); + using DrawTextAtFn = std::function; + void SetDrawTextFunction(DrawTextAtFn fn); + // Returns the managed scene. mjrScene* GetScene() const { return scene_.get(); } @@ -67,6 +71,7 @@ class SceneBridge { mjrfContext* ctx_ = nullptr; std::unique_ptr model_objects_; + DrawTextAtFn draw_text_callback_; UniquePtr scene_{nullptr, nullptr}; UniquePtr fallback_ibl_{nullptr, nullptr}; UniquePtr fallback_ibl_texture_{nullptr, nullptr}; From 2aa2131057ceac62f3b170a8e36d90adc0dac73d Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 6 May 2026 10:13:04 -0700 Subject: [PATCH 201/251] Refactor how render requests are generated. Get the camera and drawmode from the bridges rather than recalculating it unecessarily. PiperOrigin-RevId: 911406470 Change-Id: Ia295846bb80105a2b0ad5da21912ca82b25e04bd --- .../filament/compat/imgui_bridge.cc | 23 ++++ .../filament/compat/imgui_bridge.h | 3 +- .../filament/compat/mjr_filament_renderer.cc | 115 +++++++----------- .../filament/compat/mjr_filament_renderer.h | 3 +- .../filament/compat/scene_bridge.cc | 20 ++- .../filament/compat/scene_bridge.h | 6 +- .../filament/filament/scene_view.cc | 2 +- .../filament/filament/scene_view.h | 2 +- .../filament/render_context_filament.h | 3 +- 9 files changed, 95 insertions(+), 82 deletions(-) diff --git a/src/experimental/filament/compat/imgui_bridge.cc b/src/experimental/filament/compat/imgui_bridge.cc index 461ef955..9af52b74 100644 --- a/src/experimental/filament/compat/imgui_bridge.cc +++ b/src/experimental/filament/compat/imgui_bridge.cc @@ -302,6 +302,29 @@ void ImguiBridge::PrepareRenderables(int count) { } } +mjrScene* ImguiBridge::GetScene() const { return scene_.get(); } + +mjrCamera ImguiBridge::GetCamera(int width, int height) const { + mjrCamera camera; + camera.orthographic = true; + camera.pos[0] = 0.0f; + camera.pos[1] = 0.0f; + camera.pos[2] = 1.0f; + camera.forward[0] = 0.0f; + camera.forward[1] = 0.0f; + camera.forward[2] = -1.0f; + camera.up[0] = 0.0f; + camera.up[1] = 1.0f; + camera.up[2] = 0.0f; + camera.frustum_top = 0.0f; + camera.frustum_near = 0.0f; + camera.frustum_far = 1.0f; + camera.frustum_center = width / 2.0f; + camera.frustum_width = width / 2.0f; + camera.frustum_bottom = height; + return camera; +} + static ImVec2 ClipSpaceToWindowCoordinates(float x, float y) { const ImVec2& display_size = ImGui::GetIO().DisplaySize; const float pos_x = display_size.x * ((x + 1) * 0.5f); diff --git a/src/experimental/filament/compat/imgui_bridge.h b/src/experimental/filament/compat/imgui_bridge.h index ac479292..9625018b 100644 --- a/src/experimental/filament/compat/imgui_bridge.h +++ b/src/experimental/filament/compat/imgui_bridge.h @@ -37,7 +37,8 @@ class ImguiBridge { void Update(); // Returns the managed UX scene. - mjrScene* GetScene() const { return scene_.get(); } + mjrScene* GetScene() const; + mjrCamera GetCamera(int width, int height) const; // Uploads texture to be used with ImGui's Image and ImageButton functions. uintptr_t UploadImage(uintptr_t tex_id, const uint8_t* pixels, int width, diff --git a/src/experimental/filament/compat/mjr_filament_renderer.cc b/src/experimental/filament/compat/mjr_filament_renderer.cc index a23de31b..9331f93c 100644 --- a/src/experimental/filament/compat/mjr_filament_renderer.cc +++ b/src/experimental/filament/compat/mjr_filament_renderer.cc @@ -17,7 +17,6 @@ #include #include #include -#include #include #include @@ -42,31 +41,6 @@ void MjrFilamentRenderer::Init(const mjModel* model) { scene_bridge_ = std::make_unique(filament_context_.get(), model); imgui_bridge_ = std::make_unique(filament_context_.get()); scene_bridge_->SetDrawTextFunction(DrawTextAt); - - mjr_defaultRenderRequest(&render_requests_[0]); - mjr_defaultRenderRequest(&render_requests_[1]); - - render_requests_[0].scene = scene_bridge_->GetScene(); - render_requests_[0].draw_mode = mjDRAW_MODE_COLOR; - - render_requests_[1].scene = imgui_bridge_->GetScene(); - render_requests_[1].draw_mode = mjDRAW_MODE_COLOR; - - // The UX camera is a fixed orthographic camera. We only need to change the - // width/height based on the viewport per frame. - render_requests_[1].camera.orthographic = true; - render_requests_[1].camera.pos[0] = 0.0f; - render_requests_[1].camera.pos[1] = 0.0f; - render_requests_[1].camera.pos[2] = 1.0f; - render_requests_[1].camera.forward[0] = 0.0f; - render_requests_[1].camera.forward[1] = 0.0f; - render_requests_[1].camera.forward[2] = -1.0f; - render_requests_[1].camera.up[0] = 0.0f; - render_requests_[1].camera.up[1] = 1.0f; - render_requests_[1].camera.up[2] = 0.0f; - render_requests_[1].camera.frustum_top = 0.0f; - render_requests_[1].camera.frustum_near = 0.0f; - render_requests_[1].camera.frustum_far = 1.0f; } void MjrFilamentRenderer::Render(const mjrRect& viewport, @@ -78,29 +52,22 @@ void MjrFilamentRenderer::Render(const mjrRect& viewport, imgui_bridge_->Update(); } - if (scene->flags[mjRND_SEGMENT]) { - render_requests_[0].draw_mode = mjDRAW_MODE_SEGMENTATION; - } else if (scene->flags[mjRND_DEPTH]) { - render_requests_[0].draw_mode = mjDRAW_MODE_DEPTH; - } else { - render_requests_[0].draw_mode = mjDRAW_MODE_COLOR; - } - - render_requests_[0].width = viewport.width; - render_requests_[0].height = viewport.height; - render_requests_[1].width = viewport.width; - render_requests_[1].height = viewport.height; - - render_requests_[0].camera = - mjv_averageCamera(scene->camera, scene->camera + 1); - render_requests_[1].camera.frustum_center = viewport.width / 2.0f; - render_requests_[1].camera.frustum_width = viewport.width / 2.0f; - render_requests_[1].camera.frustum_bottom = viewport.height; - if (mode_ == FrameBufferMode::Window) { - render_requests_[0].target = nullptr; - render_requests_[1].target = nullptr; - filament_context_->Render(render_requests_); + mjrRenderRequest reqs[2]; + mjr_defaultRenderRequest(&reqs[0]); + reqs[0].scene = scene_bridge_->GetScene(); + reqs[0].draw_mode = scene_bridge_->GetDrawMode(); + reqs[0].camera = scene_bridge_->GetCamera(); + reqs[0].width = viewport.width; + reqs[0].height = viewport.height; + + mjr_defaultRenderRequest(&reqs[1]); + reqs[1].scene = imgui_bridge_->GetScene(); + reqs[1].draw_mode = mjDRAW_MODE_COLOR; + reqs[1].camera = imgui_bridge_->GetCamera(viewport.width, viewport.height); + reqs[1].width = viewport.width; + reqs[1].height = viewport.height; + filament_context_->Render(reqs); } } @@ -126,10 +93,21 @@ void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, mju_error("ReadPixels is only supported for offscreen rendering."); } - render_requests_[0].width = viewport.width; - render_requests_[0].height = viewport.height; - render_requests_[1].width = viewport.width; - render_requests_[1].height = viewport.height; + mjrRenderRequest reqs[2]; + mjr_defaultRenderRequest(&reqs[0]); + + reqs[0].scene = scene_bridge_->GetScene(); + reqs[0].draw_mode = scene_bridge_->GetDrawMode(); + reqs[0].camera = scene_bridge_->GetCamera(); + reqs[0].width = viewport.width; + reqs[0].height = viewport.height; + + mjr_defaultRenderRequest(&reqs[1]); + reqs[1].scene = imgui_bridge_->GetScene(); + reqs[1].draw_mode = mjDRAW_MODE_COLOR; + reqs[1].camera = imgui_bridge_->GetCamera(viewport.width, viewport.height); + reqs[1].width = viewport.width; + reqs[1].height = viewport.height; if (rgb) { mjrRenderTargetConfig config; @@ -139,22 +117,21 @@ void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, config.color_format = mjPIXEL_FORMAT_RGB8; config.depth_format = mjPIXEL_FORMAT_DEPTH32F; auto target = CreateRenderTarget(filament_context_.get(), config); - render_requests_[0].target = target.get(); - render_requests_[1].target = target.get(); - const size_t num_requests = - (mode_ == FrameBufferMode::OffScreenWithGui) ? 2 : 1; + reqs[0].target = target.get(); + reqs[1].target = target.get(); mjrReadPixelsRequest read_request; mjr_defaultReadPixelsRequest(&read_request); + read_request.target = target.get(); read_request.output = rgb; read_request.num_bytes = viewport.width * viewport.height * 3; - const mjrFrameHandle frame = filament_context_->Render( - {&render_requests_[0], num_requests}, {&read_request, 1}); - filament_context_->WaitForFrame(frame); - render_requests_[0].target = nullptr; - render_requests_[1].target = nullptr; + const size_t num_requests = + (mode_ == FrameBufferMode::OffScreenWithGui) ? 2 : 1; + const mjrFrameHandle frame = filament_context_->Render( + {&reqs[0], num_requests}, {&read_request, 1}); + filament_context_->WaitForFrame(frame); } if (depth) { @@ -165,23 +142,19 @@ void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, config.color_format = mjPIXEL_FORMAT_R32F; config.depth_format = mjPIXEL_FORMAT_DEPTH32F; auto target = CreateRenderTarget(filament_context_.get(), config); - render_requests_[0].target = target.get(); - render_requests_[1].target = target.get(); - mjrDrawMode last_draw_mode = render_requests_[0].draw_mode; - render_requests_[0].draw_mode = mjDRAW_MODE_DEPTH; + reqs[0].draw_mode = mjDRAW_MODE_DEPTH; + reqs[0].target = target.get(); mjrReadPixelsRequest read_request; mjr_defaultReadPixelsRequest(&read_request); + read_request.target = target.get(); read_request.output = reinterpret_cast(depth); read_request.num_bytes = viewport.width * viewport.height * sizeof(float); - const mjrFrameHandle frame = filament_context_->Render( - {&render_requests_[0], 1}, {&read_request, 1}); - filament_context_->WaitForFrame(frame); - render_requests_[0].target = nullptr; - render_requests_[1].target = nullptr; - render_requests_[0].draw_mode = last_draw_mode; + const mjrFrameHandle frame = filament_context_->Render( + {&reqs[0], 1}, {&read_request, 1}); + filament_context_->WaitForFrame(frame); } } diff --git a/src/experimental/filament/compat/mjr_filament_renderer.h b/src/experimental/filament/compat/mjr_filament_renderer.h index ab77f2c8..f1f2f69d 100644 --- a/src/experimental/filament/compat/mjr_filament_renderer.h +++ b/src/experimental/filament/compat/mjr_filament_renderer.h @@ -80,10 +80,9 @@ class MjrFilamentRenderer { }; std::unique_ptr filament_context_; - FrameBufferMode mode_ = FrameBufferMode::Window; - mjrRenderRequest render_requests_[2]; std::unique_ptr scene_bridge_; std::unique_ptr imgui_bridge_; + FrameBufferMode mode_ = FrameBufferMode::Window; }; } // namespace mujoco diff --git a/src/experimental/filament/compat/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc index a7e6a5e7..ad6d1518 100644 --- a/src/experimental/filament/compat/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -252,7 +252,7 @@ void SceneBridge::PrepareLights() { mjrf_setSceneSkybox(scene_.get(), model_objects_->GetSkyboxTexture()); } -mat4 CalculateClipFromWorld(const mjrRect& viewport, const mjvGLCamera& cam) { +mat4 CalculateClipFromWorld(const mjrRect& viewport, const mjrCamera& cam) { const float3 cam_pos(cam.pos[0], cam.pos[1], cam.pos[2]); const float3 cam_fwd(cam.forward[0], cam.forward[1], cam.forward[2]); const float3 cam_up(cam.up[0], cam.up[1], cam.up[2]); @@ -282,6 +282,13 @@ mat4 CalculateClipFromWorld(const mjrRect& viewport, const mjvGLCamera& cam) { void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { mjrf_setSceneShadowsEnabled(scene_.get(), scene->flags[mjRND_SHADOW]); mjrf_setSceneReflectionsEnabled(scene_.get(), scene->flags[mjRND_REFLECTION]); + if (scene->flags[mjRND_SEGMENT]) { + draw_mode_ = mjDRAW_MODE_SEGMENTATION; + } else if (scene->flags[mjRND_DEPTH]) { + draw_mode_ = mjDRAW_MODE_DEPTH; + } else { + draw_mode_ = mjDRAW_MODE_COLOR; + } mjtNum hpos[3], hfwd[3]; float headpos[3], gazedir[3]; @@ -289,9 +296,8 @@ void SceneBridge::Update(const mjrRect& viewport, const mjvScene* scene) { mju_n2f(headpos, hpos, 3); mju_n2f(gazedir, hfwd, 3); - const mjvGLCamera gl_camera = - mjv_averageCamera(scene->camera, scene->camera + 1); - clip_from_world_ = CalculateClipFromWorld(viewport, gl_camera); + camera_ = mjv_averageCamera(scene->camera, scene->camera + 1); + clip_from_world_ = CalculateClipFromWorld(viewport, camera_); // Remove all drawables from previous render and prepare new ones. for (auto& iter : renderables_) { @@ -365,4 +371,10 @@ void SceneBridge::SetDrawTextFunction(DrawTextAtFn fn) { draw_text_callback_ = std::move(fn); } +mjrScene* SceneBridge::GetScene() const { return scene_.get(); } + +mjrCamera SceneBridge::GetCamera() const { return camera_; } + +mjrDrawMode SceneBridge::GetDrawMode() const { return draw_mode_; } + } // namespace mujoco diff --git a/src/experimental/filament/compat/scene_bridge.h b/src/experimental/filament/compat/scene_bridge.h index 3de3f2f0..0f04e402 100644 --- a/src/experimental/filament/compat/scene_bridge.h +++ b/src/experimental/filament/compat/scene_bridge.h @@ -56,7 +56,9 @@ class SceneBridge { void SetDrawTextFunction(DrawTextAtFn fn); // Returns the managed scene. - mjrScene* GetScene() const { return scene_.get(); } + mjrScene* GetScene() const; + mjrCamera GetCamera() const; + mjrDrawMode GetDrawMode() const; SceneBridge(const SceneBridge&) = delete; SceneBridge& operator=(const SceneBridge&) = delete; @@ -71,6 +73,8 @@ class SceneBridge { mjrfContext* ctx_ = nullptr; std::unique_ptr model_objects_; + mjrCamera camera_; + mjrDrawMode draw_mode_ = mjDRAW_MODE_COLOR; DrawTextAtFn draw_text_callback_; UniquePtr scene_{nullptr, nullptr}; UniquePtr fallback_ibl_{nullptr, nullptr}; diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 8546940b..4ddd103b 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -76,7 +76,7 @@ static filament::ColorGrading::Builder ToBuilder( .curves(opts.shadow_gamma, opts.mid_point, opts.highlight_scale); } -static void SetupCamera(const mjvGLCamera& cam, +static void SetupCamera(const mjrCamera& cam, const filament::Viewport& viewport, filament::Camera* camera) { const filament::Camera::Projection type = diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index 0187ee0f..1522fe3c 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -63,7 +63,7 @@ class SceneView : public mjrScene { // The target viewport for the rendered image. mjrRect viewport; // The camera from which to render the scene. - mjvGLCamera camera; + mjrCamera camera; // An optional render target into which the scene will be rendered. RenderTarget* target = nullptr; }; diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index 04074d97..fadc10ca 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -122,6 +122,7 @@ typedef std::uint64_t mjrFrameHandle; typedef mjtTexture mjrTextureTarget; typedef mjtColorSpace mjrColorSpace; typedef mjtLightType mjrLightType; +typedef mjvGLCamera mjrCamera; // The textures that can be assigned to the drawable's material. struct mjrMaterialTextures { @@ -367,7 +368,7 @@ struct mjrRenderRequest { mjrDrawMode draw_mode; // The camera from which to render the scene. - mjvGLCamera camera; + mjrCamera camera; // The dimensions of the output image. int width; From a501731089a7563abf49337d1049dd6269cbcf9d Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Wed, 6 May 2026 10:21:18 -0700 Subject: [PATCH 202/251] Update multiccd default in mjcPhysics. PiperOrigin-RevId: 911410939 Change-Id: Ia20b5be87f85c4662b71994dd824d90c6b98b3ed --- doc/changelog.rst | 6 ++++++ include/mujoco/experimental/usd/mjcPhysics/sceneAPI.h | 2 +- src/experimental/usd/mjcPhysics/generatedSchema.usda | 2 +- src/experimental/usd/mjcPhysics/schema.usda | 2 +- test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc | 2 ++ 5 files changed, 11 insertions(+), 3 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index dfa1a99e..466c02fc 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -30,6 +30,12 @@ General |br| ``mju_sym2dense(dst, d->M, m->nv, m->M_rownnz, m->M_rowadr, m->M_colind)``. + +Bug fixes +^^^^^^^^^ + +- Fixed default for multiccd in :ref:`mjcPhysics`. + Python ^^^^^^ diff --git a/include/mujoco/experimental/usd/mjcPhysics/sceneAPI.h b/include/mujoco/experimental/usd/mjcPhysics/sceneAPI.h index 2ed55495..7d9b4b37 100644 --- a/include/mujoco/experimental/usd/mjcPhysics/sceneAPI.h +++ b/include/mujoco/experimental/usd/mjcPhysics/sceneAPI.h @@ -1319,7 +1319,7 @@ class MjcPhysicsSceneAPI : public UsdAPISchemaBase { /// /// | || /// | -- | -- | - /// | Declaration | `uniform bool mjc:flag:multiccd = 0` | + /// | Declaration | `uniform bool mjc:flag:multiccd = 1` | /// | C++ Type | bool | /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Bool | /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | diff --git a/src/experimental/usd/mjcPhysics/generatedSchema.usda b/src/experimental/usd/mjcPhysics/generatedSchema.usda index 633ec10d..48229205 100644 --- a/src/experimental/usd/mjcPhysics/generatedSchema.usda +++ b/src/experimental/usd/mjcPhysics/generatedSchema.usda @@ -138,7 +138,7 @@ class "MjcSceneAPI" ( displayName = "Mid-Phase Collision Filtering Toggle" doc = "Enables mid-phase collision filtering using a static AABB bounding volume hierarchy (BVH)." ) - uniform bool mjc:flag:multiccd = 0 ( + uniform bool mjc:flag:multiccd = 1 ( displayName = "Multiple Contact Collision Detection (CCD) Toggle" doc = "Enables multiple-contact collision detection for geom pairs using a general-purpose convex-convex collider." ) diff --git a/src/experimental/usd/mjcPhysics/schema.usda b/src/experimental/usd/mjcPhysics/schema.usda index 62c1d993..a0c50967 100644 --- a/src/experimental/usd/mjcPhysics/schema.usda +++ b/src/experimental/usd/mjcPhysics/schema.usda @@ -493,7 +493,7 @@ class "MjcSceneAPI" doc = """Enables discrete-time inverse dynamics with mj_inverse for integrators other than RK4.""" ) - uniform bool mjc:flag:multiccd = False ( + uniform bool mjc:flag:multiccd = True ( customData = { string apiName = "MultiCCDFlag" } diff --git a/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc b/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc index d7f106d4..59de7649 100644 --- a/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc +++ b/test/experimental/usd/mjcPhysics/mjc_physics_scene_test.cc @@ -172,6 +172,8 @@ TEST_F(MjcPhysicsSceneTest, TestDefaults) { mjDSBL_EULERDAMP); EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(AutoResetFlag, mjDSBL_AUTORESET); + EXPECT_DISABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(MultiCCDFlag, + mjDSBL_MULTICCD); EXPECT_ENABLE_FLAG_USD_FALLBACK_EQ_MODEL_DEFAULT(OverrideFlag, mjENBL_OVERRIDE); From 0f53f558538c04103e4478e148282bee5af5a319 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 6 May 2026 16:24:41 -0700 Subject: [PATCH 203/251] Add checks for degenerate meshes in convex hull computation. Fixes #3260 PiperOrigin-RevId: 911610731 Change-Id: I9efd15eee1bb5cb13639fd733a49535a147c3401 --- src/user/user_mesh.cc | 72 +++++++++++++++++++++++++++++++++++++ test/user/user_mesh_test.cc | 59 +++++++++++++++++++++++++++++- 2 files changed, 130 insertions(+), 1 deletion(-) diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 3083fb46..235d0a45 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -1715,6 +1715,78 @@ void mjCMesh::MakeGraph(const double* dvert) { return; } + // check for colocated/collinear/coplanar vertices + { + // find second vertex that is distinct from vertex 0 + int v1 = -1; + double len1 = 0; + for (int i = 1; i < nvert(); i++) { + len1 = mjuu_dist3(dvert+3*i, dvert); + if (len1 > mjMINVAL) { + v1 = i; + break; + } + } + + // no second vertex found: all vertices are colocated + if (v1 < 0) { + throw mjCError(this, + "mesh '%s' has colocated vertices, cannot compute convex hull." + " Consider using a small sphere instead", + name.c_str()); + } + + // find first non-collinear triple to define a plane + double edge1[3] = {dvert[3*v1+0] - dvert[0], + dvert[3*v1+1] - dvert[1], + dvert[3*v1+2] - dvert[2]}; + double normal[3] = {0, 0, 0}; + bool collinear = true; + for (int i = 1; i < nvert(); i++) { + if (i == v1) continue; + double edge2[3] = {dvert[3*i+0] - dvert[0], + dvert[3*i+1] - dvert[1], + dvert[3*i+2] - dvert[2]}; + double len2 = sqrt(mjuu_dot3(edge2, edge2)); + if (len2 < mjMINVAL) continue; + mjuu_crossvec(normal, edge1, edge2); + double norm = sqrt(mjuu_dot3(normal, normal)); + if (norm > mjMINVAL * len1 * len2) { + normal[0] /= norm; + normal[1] /= norm; + normal[2] /= norm; + collinear = false; + break; + } + } + + // vertices are collinear: cannot compute convex hull + if (collinear) { + throw mjCError(this, + "mesh '%s' has collinear vertices, cannot compute convex hull." + " Consider using a thin capsule instead", + name.c_str()); + } + + // find first vertex that is not on the plane + double d = mjuu_dot3(normal, dvert); + bool coplanar = true; + for (int i = 0; i < nvert(); i++) { + if (fabs(mjuu_dot3(normal, dvert+3*i) - d) > mjMINVAL * len1) { + coplanar = false; + break; + } + } + + // vertices are coplanar: cannot compute convex hull + if (coplanar) { + throw mjCError(this, + "mesh '%s' has coplanar vertices, cannot compute convex hull." + " Consider using a primitive geom type (plane or thin box) instead", + name.c_str()); + } + } + qhT qh_qh; qhT* qh = &qh_qh; qh_zero(qh, stderr); diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index 67d06da7..af5bc552 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -713,7 +713,7 @@ TEST_F(MjCMeshTest, AreaTooSmall) { )"; std::array error; mjModel* model = LoadModelFromString(xml, error.data(), error.size()); - EXPECT_THAT(model, testing::IsNull()); + EXPECT_THAT(model, IsNull()); EXPECT_THAT(error.data(), HasSubstr("mesh surface area is too small")); } @@ -1385,6 +1385,63 @@ TEST_F(MjCMeshTest, QhullCache) { mj_deleteVFS(&vfs); } +TEST_F(MjCMeshTest, ColocatedMeshError) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("colocated")); +} + +TEST_F(MjCMeshTest, CollinearMeshError) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("collinear")); +} + +TEST_F(MjCMeshTest, CoplanarMeshError) { + static constexpr char xml[] = R"( + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + EXPECT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("coplanar")); +} + TEST_F(MjCMeshTest, LoadSkin) { const std::string xml_path = GetTestDataFilePath(kCubeSkinPath); std::array error; From b6aac76cc4b1f3d8a0bedc1563a7daa508b574b8 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 6 May 2026 23:43:57 -0700 Subject: [PATCH 204/251] Pass filament::Engine instead of FilamentContext for textures, meshes, and render targets. PiperOrigin-RevId: 911770834 Change-Id: I081458e118f0e91091d6d4df7b760e9dbb321672 --- src/experimental/filament/filament/mesh.cc | 5 ++--- src/experimental/filament/filament/mesh.h | 4 +--- src/experimental/filament/filament/render_target.cc | 13 ++++++------- src/experimental/filament/filament/render_target.h | 5 ++--- src/experimental/filament/filament/scene_view.cc | 3 ++- src/experimental/filament/filament/texture.cc | 5 ++--- src/experimental/filament/filament/texture.h | 3 +-- .../filament/render_context_filament.cc | 11 ++++++----- 8 files changed, 22 insertions(+), 27 deletions(-) diff --git a/src/experimental/filament/filament/mesh.cc b/src/experimental/filament/filament/mesh.cc index c2024f3b..9c06cfc7 100644 --- a/src/experimental/filament/filament/mesh.cc +++ b/src/experimental/filament/filament/mesh.cc @@ -32,7 +32,6 @@ #include #include #include -#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/render_context_filament.h" @@ -103,8 +102,8 @@ int FillSequence(std::byte* buffer, std::size_t num_bytes) { return num; } -Mesh::Mesh(FilamentContext* ctx, const mjrMeshData& data) - : engine_(ctx->GetEngine()), shared_state_(std::make_shared()) { +Mesh::Mesh(filament::Engine* engine, const mjrMeshData& data) + : engine_(engine), shared_state_(std::make_shared()) { type_ = data.primitive_type == mjMESH_PRIMITIVE_TYPE_TRIANGLES ? filament::RenderableManager::PrimitiveType::TRIANGLES : filament::RenderableManager::PrimitiveType::LINES; diff --git a/src/experimental/filament/filament/mesh.h b/src/experimental/filament/filament/mesh.h index ba9945da..735d8717 100644 --- a/src/experimental/filament/filament/mesh.h +++ b/src/experimental/filament/filament/mesh.h @@ -29,7 +29,6 @@ #include #include #include -#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/render_context_filament.h" // Functions for creating filament vertex and index buffers. @@ -39,8 +38,7 @@ namespace mujoco { class Mesh : public mjrMesh { public: // Creates a Mesh from the given MeshData. - Mesh(FilamentContext* ctx, const mjrMeshData& data); - + Mesh(filament::Engine* engine, const mjrMeshData& data); ~Mesh(); Mesh(const Mesh&) = delete; diff --git a/src/experimental/filament/filament/render_target.cc b/src/experimental/filament/filament/render_target.cc index d8919a96..fa9bbfb9 100644 --- a/src/experimental/filament/filament/render_target.cc +++ b/src/experimental/filament/filament/render_target.cc @@ -26,15 +26,14 @@ #include #include #include -#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { -RenderTarget::RenderTarget(FilamentContext* ctx, +RenderTarget::RenderTarget(filament::Engine* engine, const mjrRenderTargetConfig& config) - : ctx_(ctx), config_(config) { + : engine_(engine), config_(config) { if (config_.width > 0 && config_.height > 0) { Prepare(config_.width, config_.height); } @@ -67,7 +66,7 @@ void RenderTarget::Prepare(int width, int height) { color_config.color_space = mjCOLORSPACE_LINEAR; color_config.format = mjPIXEL_FORMAT_RGB8; color_flags.color_attachment = true; - color_texture_ = std::make_unique(ctx_, color_config, color_flags); + color_texture_ = std::make_unique(engine_, color_config, color_flags); mjrTextureConfig depth_config; mjr_defaultTextureConfig(&depth_config); @@ -79,14 +78,14 @@ void RenderTarget::Prepare(int width, int height) { depth_config.color_space = mjCOLORSPACE_LINEAR; depth_config.format = mjPIXEL_FORMAT_DEPTH32F; depth_flags.depth_attachment = true; - depth_texture_ = std::make_unique(ctx_, depth_config, depth_flags); + depth_texture_ = std::make_unique(engine_, depth_config, depth_flags); filament::RenderTarget::Builder builder; builder.texture(filament::RenderTarget::AttachmentPoint::COLOR, color_texture_->GetFilamentTexture()); builder.texture(filament::RenderTarget::AttachmentPoint::DEPTH, depth_texture_->GetFilamentTexture()); - render_target_ = builder.build(*ctx_->GetEngine()); + render_target_ = builder.build(*engine_); } void RenderTarget::ReadColorPixels(filament::Renderer* renderer, uint8_t* bytes, @@ -120,7 +119,7 @@ void RenderTarget::ReadColorPixels(filament::Renderer* renderer, uint8_t* bytes, void RenderTarget::Destroy() { if (render_target_) { - ctx_->GetEngine()->destroy(render_target_); + engine_->destroy(render_target_); render_target_ = nullptr; } color_texture_.reset(); diff --git a/src/experimental/filament/filament/render_target.h b/src/experimental/filament/filament/render_target.h index 8e3fa83c..92e221b3 100644 --- a/src/experimental/filament/filament/render_target.h +++ b/src/experimental/filament/filament/render_target.h @@ -21,7 +21,6 @@ #include #include -#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" @@ -32,7 +31,7 @@ class RenderTarget : public mjrRenderTarget { public: // Defines the types of textures to create for the color and depth // attachments. - RenderTarget(FilamentContext* ctx, const mjrRenderTargetConfig& config); + RenderTarget(filament::Engine* engine, const mjrRenderTargetConfig& config); ~RenderTarget() noexcept; RenderTarget(const RenderTarget&) = delete; @@ -65,7 +64,7 @@ class RenderTarget : public mjrRenderTarget { private: void Destroy(); - FilamentContext* ctx_ = nullptr; + filament::Engine* engine_ = nullptr; mjrRenderTargetConfig config_; filament::RenderTarget* render_target_ = nullptr; std::unique_ptr color_texture_ = nullptr; diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 4ddd103b..e97395b7 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -308,7 +308,8 @@ void SceneView::AddReflectiveRenderable(Renderable* renderable) { config.color_format = mjPIXEL_FORMAT_RGBA8; config.depth_format = mjPIXEL_FORMAT_DEPTH32F; - reflect_targets_.push_back(std::make_unique(ctx_, config)); + reflect_targets_.push_back( + std::make_unique(ctx_->GetEngine(), config)); } // Prepare a render target for the reflective renderable. diff --git a/src/experimental/filament/filament/texture.cc b/src/experimental/filament/filament/texture.cc index af174a6e..0d7b29ef 100644 --- a/src/experimental/filament/filament/texture.cc +++ b/src/experimental/filament/filament/texture.cc @@ -24,7 +24,6 @@ #include #include #include -#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -112,9 +111,9 @@ static filament::Texture::InternalFormat GetTextureInternalFormat( } } -Texture::Texture(FilamentContext* ctx, const mjrTextureConfig& config, +Texture::Texture(filament::Engine* engine, const mjrTextureConfig& config, InternalFlags flags) - : engine_(ctx->GetEngine()), config_(config) { + : engine_(engine), config_(config) { if (IsCompressed(config_)) { // We defer creation of compressed textures until Upload() is called. In // the meantime, we don't really know anything about the texture (e.g. diff --git a/src/experimental/filament/filament/texture.h b/src/experimental/filament/filament/texture.h index ba94a161..bfe4fbf6 100644 --- a/src/experimental/filament/filament/texture.h +++ b/src/experimental/filament/filament/texture.h @@ -18,7 +18,6 @@ #include #include #include -#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/render_context_filament.h" // Functions for creating filament textures. @@ -35,7 +34,7 @@ class Texture : public mjrTexture { }; // Creates a texture with the given data. - Texture(FilamentContext* ctx, const mjrTextureConfig& config, + Texture(filament::Engine* engine, const mjrTextureConfig& config, InternalFlags flags = InternalFlags()); ~Texture(); diff --git a/src/experimental/filament/render_context_filament.cc b/src/experimental/filament/render_context_filament.cc index 8c13c404..68b58766 100644 --- a/src/experimental/filament/render_context_filament.cc +++ b/src/experimental/filament/render_context_filament.cc @@ -154,7 +154,8 @@ void mjrf_destroyContext(mjrfContext* ctx) { } mjrTexture* mjrf_createTexture(mjrfContext* ctx, const mjrTextureConfig* cfg) { - return new mujoco::Texture(mujoco::FilamentContext::downcast(ctx), *cfg); + return new mujoco::Texture( + mujoco::FilamentContext::downcast(ctx)->GetEngine(), *cfg); } void mjrf_destroyTexture(mjrTexture* texture) { @@ -162,7 +163,8 @@ void mjrf_destroyTexture(mjrTexture* texture) { } mjrMesh* mjrf_createMesh(mjrfContext* ctx, const mjrMeshData* data) { - return new mujoco::Mesh(mujoco::FilamentContext::downcast(ctx), *data); + return new mujoco::Mesh(mujoco::FilamentContext::downcast(ctx)->GetEngine(), + *data); } void mjrf_destroyMesh(mjrMesh* mesh) { delete mujoco::Mesh::downcast(mesh); } @@ -195,15 +197,14 @@ void mjrf_destroyRenderable(mjrRenderable* renderable) { mjrRenderTarget* mjrf_createRenderTarget(mjrfContext* ctx, const mjrRenderTargetConfig* config) { - return new mujoco::RenderTarget(mujoco::FilamentContext::downcast(ctx), - *config); + return new mujoco::RenderTarget( + mujoco::FilamentContext::downcast(ctx)->GetEngine(), *config); } void mjrf_destroyRenderTarget(mjrRenderTarget* render_target) { delete mujoco::RenderTarget::downcast(render_target); } - void mjrf_setTextureData(mjrTexture* texture, const mjrTextureData* data) { mujoco::Texture::downcast(texture)->Upload(*data); } From 0c05215e180a92161853317e75824882a1123852 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 6 May 2026 23:52:46 -0700 Subject: [PATCH 205/251] Reduce flex stiffness and bending memory size to the exact amount needed. PiperOrigin-RevId: 911774147 Change-Id: I3b18eba23e75ec91ece8342945f25e0cd7c63a1a --- src/engine/engine_core_constraint.c | 20 ++++--- src/engine/engine_derivative.c | 6 ++- src/engine/engine_passive.c | 35 +++++++++--- src/user/user_mesh.cc | 8 +++ src/user/user_model.cc | 82 ++++++----------------------- 5 files changed, 70 insertions(+), 81 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index b6c143a6..b74f19b2 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -832,9 +832,14 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) { // read eigenmode data from flex_stiffness int ndof_elem = 3 * npe; - const mjtNum* k_elem = m->flex_stiffness + m->flex_stiffnessadr[f] - + elem_idx * ndof_elem * ndof_elem; - int neig = (int)k_elem[0]; + int stiffnessadr = m->flex_stiffnessadr[f]; + int neig = 0; + const mjtNum* k_elem = NULL; + if (stiffnessadr >= 0) { + k_elem = m->flex_stiffness + stiffnessadr + + elem_idx * ndof_elem * ndof_elem; + neig = (int)k_elem[0]; + } // compute displacement in corotational frame mjtNum* displ_e = mjSTACKALLOC(d, ndof_elem, mjtNum); @@ -2387,9 +2392,12 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) { // read eigenmode count from flex_stiffness int ndof_elem = 3 * npe; - const mjtNum* k_elem = m->flex_stiffness + m->flex_stiffnessadr[f] - + elem_idx * ndof_elem * ndof_elem; - size = (int)k_elem[0]; // neig stored as first element + size = 0; + if (m->flex_stiffnessadr[f] >= 0) { + const mjtNum* k_elem = m->flex_stiffness + m->flex_stiffnessadr[f] + + elem_idx * ndof_elem * ndof_elem; + size = (int)k_elem[0]; // neig stored as first element + } if (nnz) { // get element node body IDs diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index d97c7a0b..ef6bc8b7 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -951,7 +951,11 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, } // get stiffness and damping - mjtNum* K = m->flex_stiffness + m->flex_stiffnessadr[f]; + int stiffnessadr = m->flex_stiffnessadr[f]; + if (stiffnessadr < 0) { + continue; + } + mjtNum* K = m->flex_stiffness + stiffnessadr; // skip if rigid or no stiffness if (m->flex_rigid[f] || K[0] == 0) { diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 0196fca5..b1470ec3 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -62,7 +62,12 @@ static void inline GradSquaredLengths(mjtNum gradient[6][2][3], // passive forces for interpolated flex (stretch + bending) static void mj_flexPassiveInterp(const mjModel* m, mjData* d, int f, int enbl_spring, int enbl_damper) { - mjtNum* k = m->flex_stiffness + m->flex_stiffnessadr[f]; + int stiffnessadr = m->flex_stiffnessadr[f]; + if (stiffnessadr < 0) { + return; + } + + mjtNum* k = m->flex_stiffness + stiffnessadr; int nodenum = m->flex_nodenum[f]; int order = m->flex_interp[f]; @@ -230,8 +235,17 @@ static inline mjtNum mju_dphi2D(mjtNum s0, int l0, mjtNum s1, int l1, // accuracy for elements with varying curvature. static void mj_flexPassiveBendInterp(const mjModel* m, mjData* d, int f, int enbl_spring, int enbl_damper) { + if (m->flex_interp[f] >= 0) { + return; + } + + int bendingadr = m->flex_bendingadr[f]; + if (bendingadr < 0) { + return; + } + // read bending edge data - const mjtNum* bdata = m->flex_bending + m->flex_bendingadr[f]; + const mjtNum* bdata = m->flex_bending + bendingadr; int nedge = (int)bdata[0]; if (nedge == 0) return; @@ -398,10 +412,15 @@ static void mj_flexPassiveBend(const mjModel* m, mjData* d, int f, return; } + int bendingadr = m->flex_bendingadr[f]; + if (bendingadr < 0) { + return; + } + int edgenum = m->flex_edgenum[f]; mjtNum* xpos = d->flexvert_xpos + 3*m->flex_vertadr[f]; int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f]; - mjtNum* b = m->flex_bending + 17*m->flex_edgeadr[f]; + mjtNum* b = m->flex_bending + bendingadr; for (int e = 0; e < edgenum; e++) { const int* edge = m->flex_edge + 2*(e+m->flex_edgeadr[f]); @@ -469,7 +488,11 @@ static void mj_flexPassiveBend(const mjModel* m, mjData* d, int f, // passive forces for flex stretch static void mj_flexPassiveStretch(const mjModel* m, mjData* d, int f, int enbl_spring, int enbl_damper) { - mjtNum* k = m->flex_stiffness + m->flex_stiffnessadr[f]; + int stiffnessadr = m->flex_stiffnessadr[f]; + if (stiffnessadr < 0) { + return; + } + mjtNum* k = m->flex_stiffness + stiffnessadr; if (k[0] == 0) { return; } @@ -660,9 +683,7 @@ static void mj_springdamper(const mjModel* m, mjData* d) { mj_flexPassiveInterp(m, d, f, enbl_spring, enbl_damper); // interpolated shell bending forces - if (m->flex_interp[f] < 0) { - mj_flexPassiveBendInterp(m, d, f, enbl_spring, enbl_damper); - } + mj_flexPassiveBendInterp(m, d, f, enbl_spring, enbl_damper); } else { // add bending forces mj_flexPassiveBend(m, d, f, enbl_spring, enbl_damper); diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 235d0a45..8db7a9f5 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -4918,6 +4918,14 @@ void mjCFlex::Compile(const mjVFS* vfs) { stiffness_cached = LoadCachedStiffness(); } + // check if any strain equality references this flex + for (auto* equality : model->Equalities()) { + if (equality->spec.type == mjEQ_FLEXSTRAIN && *equality->spec.name1 == name) { + has_strain_eq = true; + break; + } + } + if (!stiffness_cached && interpolated && (young > 0 || has_strain_eq)) { // use young=1 for strain constraints (eigenvectors are geometry-only) double K_young = has_strain_eq ? 1e1 : young; diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 47d9a289..7d87f3b5 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2106,21 +2106,6 @@ static size_t getpathslength(std::vector list) { return result; } -// compute extra stiffness/bending array size for an interpolated flex -static int flexInterpExtraSize(int order, const int cellcount[3], bool shell) { - int npe, nelem; - int cx = cellcount[0], cy = cellcount[1], cz = cellcount[2]; - if (shell) { - npe = (int)pow(order + 1, 2); - nelem = 2*(cy*cz + cx*cz + cx*cy); - } else { - npe = (int)pow(order + 1, 3); - nelem = cx * cy * cz; - } - int ndof_elem = 3 * npe; - return nelem * ndof_elem * ndof_elem; -} - // set array sizes void mjCModel::SetSizes() { // set from object list sizes @@ -2194,8 +2179,6 @@ void mjCModel::SetSizes() { } nbvh = nbvhstatic + nbvhdynamic; - int extra_stiffness_size = 0; - int extra_bending_size = 0; // flex counts for (int i=0; i < nflex; i++) { nflexnode += flexes_[i]->nnode; @@ -2207,13 +2190,8 @@ void mjCModel::SetSizes() { nflexshelldata += (int)flexes_[i]->shell.size(); nflexevpair += (int)flexes_[i]->evpair.size()/2; nflextexcoord += (flexes_[i]->HasTexcoord() ? flexes_[i]->get_texcoord().size()/2 : 0); - if (flexes_[i]->spec.order != 0) { - int extra_size = flexInterpExtraSize( - flexes_[i]->spec.order, flexes_[i]->spec.cellcount, - flexes_[i]->elastic2d != 0); - extra_stiffness_size += extra_size; - extra_bending_size += extra_size; - } + nflexstiffness += flexes_[i]->stiffness.size(); + nflexbending += flexes_[i]->bending.size(); if (flexes_[i]->interpolated || flexes_[i]->rigid) { continue; } @@ -2263,10 +2241,6 @@ void mjCModel::SetSizes() { } } } - // TODO: This can be compacted further when we update mjwarp to not rely on - // 21*elem_adr for non-interpolated flexes and 17*edge_adr for bending. - nflexstiffness = nflexelem * 21 + extra_stiffness_size; - nflexbending = nflexedge * 17 + extra_bending_size; // mesh counts for (int i=0; i < nmesh; i++) { @@ -3351,6 +3325,7 @@ int mjCModel::CountNJten(const mjModel* m) { // copy objects outside kinematic tree void mjCModel::CopyObjects(mjModel* m) { mjtSize adr, bone_adr, vert_adr, node_adr, normal_adr, face_adr, texcoord_adr, oct_adr; + mjtSize stiffness_adr, bending_adr; mjtSize edge_adr, elem_adr, elemdata_adr, elemedge_adr, shelldata_adr, evpair_adr; mjtSize bonevert_adr, graph_adr, data_adr, bvh_adr; mjtSize poly_adr, polymap_adr, polyvert_adr; @@ -3465,10 +3440,8 @@ void mjCModel::CopyObjects(mjModel* m) { shelldata_adr = 0; evpair_adr = 0; texcoord_adr = 0; - int standard_stiffness_size = 21 * m->nflexelem; - int current_extra_stiffness_adr = standard_stiffness_size; - int standard_bending_size = 17 * m->nflexedge; - int current_extra_bending_adr = standard_bending_size; + stiffness_adr = 0; + bending_adr = 0; for (int i=0; i < nflex; i++) { // get pointer mjCFlex* pfl = flexes_[i]; @@ -3491,46 +3464,19 @@ void mjCModel::CopyObjects(mjModel* m) { mjuu_copyvec(m->flex_rgba + 4 * i, pfl->rgba, 4); // elasticity - if (pfl->spec.order == 0) { - m->flex_stiffnessadr[i] = 21 * elem_adr; + if (pfl->stiffness.empty()) { + m->flex_stiffnessadr[i] = -1; } else { - m->flex_stiffnessadr[i] = current_extra_stiffness_adr; - current_extra_stiffness_adr += flexInterpExtraSize( - pfl->spec.order, pfl->spec.cellcount, pfl->elastic2d != 0); - } - - if (!pfl->stiffness.empty()) { + m->flex_stiffnessadr[i] = stiffness_adr; mjuu_copyvec(m->flex_stiffness + m->flex_stiffnessadr[i], pfl->stiffness.data(), pfl->stiffness.size()); - } else { - int stiff_size; - if (pfl->spec.order == 0) { - stiff_size = 21 * pfl->nelem; - } else { - stiff_size = flexInterpExtraSize( - pfl->spec.order, pfl->spec.cellcount, pfl->elastic2d != 0); - } - mjuu_zerovec(m->flex_stiffness + m->flex_stiffnessadr[i], stiff_size); } - if (pfl->spec.order == 0) { - m->flex_bendingadr[i] = 17 * edge_adr; + if (pfl->bending.empty()) { + m->flex_bendingadr[i] = -1; } else { - m->flex_bendingadr[i] = current_extra_bending_adr; - current_extra_bending_adr += flexInterpExtraSize( - pfl->spec.order, pfl->spec.cellcount, pfl->elastic2d != 0); - } - - if (!pfl->bending.empty()) { - mjuu_copyvec(m->flex_bending + m->flex_bendingadr[i], pfl->bending.data(), pfl->bending.size()); - } else { - int bending_size; - if (pfl->spec.order == 0) { - bending_size = 17 * pfl->nedge; - } else { - bending_size = flexInterpExtraSize( - pfl->spec.order, pfl->spec.cellcount, pfl->elastic2d != 0); - } - mjuu_zerovec(m->flex_bending + m->flex_bendingadr[i], bending_size); + m->flex_bendingadr[i] = bending_adr; + mjuu_copyvec(m->flex_bending + m->flex_bendingadr[i], + pfl->bending.data(), pfl->bending.size()); } m->flex_damping[i] = (mjtNum)pfl->damping; @@ -3704,6 +3650,8 @@ void mjCModel::CopyObjects(mjModel* m) { evpair_adr += (int)pfl->evpair.size()/2; texcoord_adr += (int)pfl->texcoord_.size()/2; bvh_adr += pfl->tree.Nbvh(); + stiffness_adr += pfl->stiffness.size(); + bending_adr += pfl->bending.size(); } // skins From 39c891e358d44f1b23f8d856f96fa5d20f4f35f1 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 7 May 2026 00:12:09 -0700 Subject: [PATCH 206/251] Add function for creating Renderable meshes from mjtGeom types. The Renderable class now internally knows which built-in meshes to use for a given geom type. It gets these meshes from the ObjectManager which now stores the collection of built-ins based on the nstack/nslice/nquad quality arguments. PiperOrigin-RevId: 911782530 Change-Id: I6698ec3964a5c657f6a9495809cc795eec2e346f --- .../filament/compat/model_objects.cc | 24 -- .../filament/compat/model_objects.h | 17 -- .../filament/compat/scene_geom_util.cc | 277 ++++-------------- .../filament/filament/builtins.cc | 67 ++--- src/experimental/filament/filament/builtins.h | 45 ++- .../filament/filament/object_manager.cc | 18 ++ .../filament/filament/object_manager.h | 7 + .../filament/filament/renderable.cc | 225 +++++++++++++- .../filament/filament/renderable.h | 17 +- .../filament/render_context_filament.cc | 12 +- .../filament/render_context_filament.h | 6 + 11 files changed, 371 insertions(+), 344 deletions(-) diff --git a/src/experimental/filament/compat/model_objects.cc b/src/experimental/filament/compat/model_objects.cc index 23ced1e0..2fd77a75 100644 --- a/src/experimental/filament/compat/model_objects.cc +++ b/src/experimental/filament/compat/model_objects.cc @@ -29,7 +29,6 @@ #include #include #include -#include "experimental/filament/filament/builtins.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/model_util.h" #include "experimental/filament/render_context_filament.h" @@ -493,19 +492,6 @@ void UpdateSkinFlexMeshData(mjrMeshData* data, const mjModel* model, ModelObjects::ModelObjects(const mjModel* model, mjrfContext* ctx) : model_(model), ctx_(ctx) { - const int nstack = model->vis.quality.numstacks; - const int nslice = model->vis.quality.numslices; - const int nquad = model->vis.quality.numquads; - shapes_.insert({kLine, CreateLine(ctx_)}); - shapes_.insert({kBox, CreateBox(ctx_, nquad)}); - shapes_.insert({kLineBox, CreateLineBox(ctx_)}); - shapes_.insert({kCone, CreateCone(ctx_, nstack, nslice)}); - shapes_.insert({kDisk, CreateDisk(ctx_, nslice)}); - shapes_.insert({kDome, CreateDome(ctx_, nstack / 2, nslice)}); - shapes_.insert({kTube, CreateTube(ctx_, nstack, nslice)}); - shapes_.insert({kPlane, CreatePlane(ctx_, nquad)}); - shapes_.insert({kSphere, CreateSphere(ctx_, nstack, nslice)}); - shapes_.insert({kTriangle, CreateTriangle(ctx_)}); for (int i = 0; i < model_->ntex; ++i) { UploadTexture(model_, i); @@ -525,11 +511,6 @@ ModelObjects::ModelObjects(const mjModel* model, mjrfContext* ctx) model_, "filament.phong.emissive_multiplier", emissive_multiplier_); } -ModelObjects::~ModelObjects() { - meshes_.clear(); - textures_.clear(); -} - void ModelObjects::UploadMesh(const mjModel* model, int id) { if (model != model_) { mju_error("Model mismatch."); @@ -641,11 +622,6 @@ const mjrMesh* ModelObjects::GetHeightFieldBuffer(int hfield_id) const { return it != height_fields_.end() ? it->second.get() : nullptr; } -const mjrMesh* ModelObjects::GetShapeBuffer(ShapeType shape) const { - auto it = shapes_.find(shape); - return it != shapes_.end() ? it->second.get() : nullptr; -} - const mjrMesh* ModelObjects::GetFlexSkinGeomMesh(int geom_id) const { auto it = dynamic_meshes_.find(geom_id); return it != dynamic_meshes_.end() ? it->second.get() : nullptr; diff --git a/src/experimental/filament/compat/model_objects.h b/src/experimental/filament/compat/model_objects.h index 0a7f980f..b0bf9036 100644 --- a/src/experimental/filament/compat/model_objects.h +++ b/src/experimental/filament/compat/model_objects.h @@ -28,21 +28,6 @@ namespace mujoco { class ModelObjects { public: ModelObjects(const mjModel* model, mjrfContext* ctx); - ~ModelObjects(); - - enum ShapeType { - kLine, - kLineBox, - kPlane, - kTriangle, - kBox, - kSphere, - kCone, - kDisk, - kDome, - kTube, - kNumShapes, - }; void UploadMesh(const mjModel* model, int id); @@ -53,7 +38,6 @@ class ModelObjects { void CreateSkinFlexMesh(const mjvScene* scene, const mjvGeom& geom); // Returns the cached instance of a filament object created from the mjModel. - const mjrMesh* GetShapeBuffer(ShapeType shape) const; const mjrMesh* GetMeshBuffer(int data_id) const; const mjrMesh* GetHeightFieldBuffer(int hfield_id) const; const mjrMesh* GetFlexSkinGeomMesh(int geom_id) const; @@ -73,7 +57,6 @@ class ModelObjects { private: const mjModel* model_ = nullptr; mjrfContext* ctx_ = nullptr; - std::unordered_map> shapes_; std::unordered_map> meshes_; std::unordered_map> convex_hulls_; std::unordered_map> height_fields_; diff --git a/src/experimental/filament/compat/scene_geom_util.cc b/src/experimental/filament/compat/scene_geom_util.cc index 2a131781..35c6f380 100644 --- a/src/experimental/filament/compat/scene_geom_util.cc +++ b/src/experimental/filament/compat/scene_geom_util.cc @@ -18,32 +18,15 @@ #include #include #include -#include -#include -#include -#include -#include -#include #include #include #include "experimental/filament/compat/model_objects.h" -#include "experimental/filament/filament/math_util.h" -#include "experimental/filament/filament/renderable.h" #include "experimental/filament/render_context_filament.h" #include "experimental/filament/render_context_filament_cpp.h" namespace mujoco { -using filament::math::float2; -using filament::math::float3; -using filament::math::float4; -using filament::math::mat4f; - -// An arbitrary scale factor for arrows. -static constexpr float kArrowScale = 1.f / 6.f; -static constexpr float kArrowHeadSize = 1.75f; - // Returns the tile size for infinite plane texture alignment. // This is duplicated from engine_vis_visualize.c (re-center infinite plane) // to ensure UV scaling matches the re-centering increments. @@ -83,245 +66,103 @@ static const mjrMesh* GetHeightField(ModelObjects* model_objs, int hfield_id) { return mesh; } -static const mjrMesh* GetShape(ModelObjects* model_objs, - ModelObjects::ShapeType shape_type) { - const mjrMesh* mesh = model_objs->GetShapeBuffer(shape_type); - if (mesh == nullptr) { - mju_error("Unknown shape %d", shape_type); - } - return mesh; -} - static void PrepareGeomMeshes(mjrRenderable* renderable, const mjvGeom& geom, const mjvScene* scene, ModelObjects* model_objects) { - std::vector meshes; - Renderable::GetTransformFn get_transforms; + const mjModel* model = model_objects->GetModel(); + const int nstack = model->vis.quality.numstacks; + const int nslice = model->vis.quality.numslices; + const int nquad = model->vis.quality.numquads; - Trs trs = { - .translation = ReadFloat3(geom.pos), - .rotation = ReadMat3(geom.mat), - .size = ReadFloat3(geom.size), - }; + float position[3]; + std::memcpy(position, &geom.pos, 3 * sizeof(float)); + float rotation[9]; + std::memcpy(rotation, &geom.mat, 9 * sizeof(float)); + float size[3]; + std::memcpy(size, &geom.size, 3 * sizeof(float)); switch ((mjtGeom)geom.type) { case mjGEOM_MESH: - meshes.push_back(GetMesh(model_objects, geom.dataid)); + mjrf_setRenderableMesh(renderable, GetMesh(model_objects, geom.dataid), 0, 0); // Ignore size for meshes. - trs.size = float3{1.0f, 1.0f, 1.0f}; + size[0] = 1.f; + size[1] = 1.f; + size[2] = 1.f; break; case mjGEOM_HFIELD: - meshes.push_back(GetHeightField(model_objects, geom.dataid)); - // Ignore size for height fields. - trs.size = float3{1.0f, 1.0f, 1.0f}; + mjrf_setRenderableMesh(renderable, GetHeightField(model_objects, geom.dataid), 0, 0); + // Ignore size for meshes. + size[0] = 1.f; + size[1] = 1.f; + size[2] = 1.f; break; case mjGEOM_PLANE: { - meshes.push_back(GetShape(model_objects, ModelObjects::kPlane)); - const bool is_infinite = !(trs.size.x > 0 && trs.size.y > 0); + mjrf_setRenderableGeomMesh(renderable, (mjtGeom)geom.type, nstack, nslice, nquad); + + const bool is_infinite = !(size[0] > 0 && size[1] > 0); if (is_infinite) { // Infinite planes are scaled to match the tile size used by // re-centering in engine_vis_visualize.c. const float plane_scale = static_cast(mjMAXPLANEGRID) / 2.0f; - trs.size.x = plane_scale; - trs.size.y = plane_scale; + size[0] = plane_scale; + size[1] = plane_scale; } // Planes only define an xy size, so set the z-dimension to 1.0f. - trs.size.z = 1.0f; + size[2] = 1.0f; break; } case mjGEOM_SPHERE: - meshes.push_back(GetShape(model_objects, ModelObjects::kSphere)); + mjrf_setRenderableGeomMesh(renderable, (mjtGeom)geom.type, nstack, nslice, nquad); break; case mjGEOM_ELLIPSOID: - meshes.push_back(GetShape(model_objects, ModelObjects::kSphere)); + mjrf_setRenderableGeomMesh(renderable, (mjtGeom)geom.type, nstack, nslice, nquad); break; case mjGEOM_BOX: - meshes.push_back(GetShape(model_objects, ModelObjects::kBox)); + mjrf_setRenderableGeomMesh(renderable, (mjtGeom)geom.type, nstack, nslice, nquad); break; - case mjGEOM_CAPSULE: { - // Capsules are a tube with two domes at the ends. - meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); - meshes.push_back(GetShape(model_objects, ModelObjects::kDome)); - meshes.push_back(GetShape(model_objects, ModelObjects::kDome)); - - get_transforms = [](int index, const Trs& trs) { - // We apply an inverse scale to the domes to counteract the capsule's - // overall scale so that the domes remain spherical in shape. - const float xz_size = 0.5f * (trs.size.x + trs.size.y); - if (index == 0) { - return trs.ToTransform(); - } else if (index == 1) { - // Move the first dome to the top of the capsule. - mat4f top = mat4f(trs.rotation, trs.translation); - top *= mat4f::translation(float3{0, 0, trs.size.z}); - top *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); - return top; - } else if (index == 2) { - // Move the second dome to the bottom of the capsule and rotate it 180 - // degrees so that it's facing the right way. - mat4f bottom = mat4f(trs.rotation, trs.translation); - bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); - bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - bottom *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); - return bottom; - } else { - mju_error("Invalid index for capsule geom: %d (expected [0,2])", index); - return trs.ToTransform(); - } - }; + case mjGEOM_CAPSULE: + mjrf_setRenderableGeomMesh(renderable, (mjtGeom)geom.type, nstack, nslice, nquad); break; - } - case mjGEOM_CYLINDER: { - // Cylinders are a tube with two disks at the ends. - meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); - meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); - meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); - - get_transforms = [](int index, const Trs& trs) { - if (index == 0) { - return trs.ToTransform(); - } else if (index == 1) { - // Move the first disk to the top of the cylinder. - mat4f top = mat4f(trs.rotation, trs.translation); - top *= mat4f::translation(float3{0, 0, trs.size.z}); - top *= mat4f::scaling(trs.size); - return top; - } else if (index == 2) { - // Move the second disk to the bottom of the cylinder. Rotate the disk - // 180 degrees so that the normals point outwards. - mat4f bottom = mat4f(trs.rotation, trs.translation); - bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); - bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - bottom *= mat4f::scaling(trs.size); - return bottom; - } else { - mju_error("Invalid index for cylinder geom: %d (expected [0,2])", index); - return trs.ToTransform(); - } - }; + case mjGEOM_CYLINDER: + mjrf_setRenderableGeomMesh(renderable, (mjtGeom)geom.type, nstack, nslice, nquad); break; - } - case mjGEOM_ARROW: { - meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); - meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); - meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); - meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); - - get_transforms = [](int index, const Trs& trs) { - mat4f base = mat4f(trs.rotation, trs.translation); - base *= mat4f::scaling(float3{1, 1, kArrowScale}); - base *= mat4f::translation(float3{0, 0, trs.size.z}); - if (index == 0) { - return base * mat4f::scaling(trs.size); - } else if (index == 1) { - mat4f top = base; - top *= mat4f::translation(float3{0, 0, trs.size.z}); - top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - return top * mat4f::scaling(trs.size); - } else if (index == 2) { - mat4f top_disk = base; - top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); - top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - return top_disk * mat4f::scaling(trs.size); - } else if (index == 3) { - mat4f bottom = base; - bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); - bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - return bottom * mat4f::scaling(trs.size); - } else { - mju_error("Invalid index for arrow geom: %d (expected [0,3])", index); - return trs.ToTransform(); - } - }; + case mjGEOM_ARROW: + mjrf_setRenderableGeomMesh(renderable, (mjtGeom)geom.type, nstack, nslice, nquad); break; - } - case mjGEOM_ARROW1: { - meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); - meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); - meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); - - get_transforms = [](int index, const Trs& trs) { - mat4f base = mat4f(trs.rotation, trs.translation); - base *= mat4f::scaling(float3{1, 1, kArrowScale}); - base *= mat4f::translation(float3{0, 0, trs.size.z}); - if (index == 0) { - return base * mat4f::scaling(trs.size); - } else if (index == 1) { - mat4f top = base; - top *= mat4f::translation(float3{0, 0, trs.size.z}); - return top * mat4f::scaling(trs.size); - } else if (index == 2) { - mat4f bottom = base; - bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); - bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - return bottom * mat4f::scaling(trs.size); - } else { - mju_error("Invalid index for arrow1 geom: %d (expected [0,2])", index); - return trs.ToTransform(); - } - }; + case mjGEOM_ARROW1: + mjrf_setRenderableGeomMesh(renderable, (mjtGeom)geom.type, nstack, nslice, nquad); break; - } - case mjGEOM_ARROW2: { - meshes.push_back(GetShape(model_objects, ModelObjects::kTube)); - meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); - meshes.push_back(GetShape(model_objects, ModelObjects::kCone)); - meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); - meshes.push_back(GetShape(model_objects, ModelObjects::kDisk)); - - get_transforms = [](int index, const Trs& trs) { - mat4f base = mat4f(trs.rotation, trs.translation); - base *= mat4f::scaling(float3{1, 1, kArrowScale}); - base *= mat4f::translation(float3{0, 0, trs.size.z}); - if (index == 0) { - return base * mat4f::scaling(trs.size); - } else if (index == 1) { - mat4f top = base; - top *= mat4f::translation(float3{0, 0, trs.size.z}); - top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - return top * mat4f::scaling(trs.size); - } else if (index == 2) { - mat4f bottom = base; - bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); - bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - bottom *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - return bottom * mat4f::scaling(trs.size); - } else if (index == 3) { - mat4f top_disk = base; - top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); - top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); - top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); - return top_disk * mat4f::scaling(trs.size); - } else if (index == 4) { - mat4f bottom_disk = base; - bottom_disk *= mat4f::translation(float3{0, 0, -trs.size.z}); - return bottom_disk * mat4f::scaling(trs.size); - } else { - mju_error("Invalid index for arrow2 geom: %d (expected [0,4])", index); - return trs.ToTransform(); - } - }; + case mjGEOM_ARROW2: + mjrf_setRenderableGeomMesh(renderable, (mjtGeom)geom.type, nstack, nslice, nquad); break; - } case mjGEOM_LINE: - meshes.push_back(GetShape(model_objects, ModelObjects::kLine)); + mjrf_setRenderableGeomMesh(renderable, (mjtGeom)geom.type, nstack, nslice, nquad); break; case mjGEOM_LINEBOX: - meshes.push_back(GetShape(model_objects, ModelObjects::kLineBox)); + mjrf_setRenderableGeomMesh(renderable, (mjtGeom)geom.type, nstack, nslice, nquad); break; case mjGEOM_TRIANGLE: - meshes.push_back(GetShape(model_objects, ModelObjects::kTriangle)); + mjrf_setRenderableGeomMesh(renderable, (mjtGeom)geom.type, nstack, nslice, nquad); break; case mjGEOM_FLEX: - meshes.push_back(GetSkinFlexMesh(model_objects, geom.objid)); + mjrf_setRenderableMesh(renderable, GetSkinFlexMesh(model_objects, geom.objid), 0, 0); // Flexes are defined in global space. - trs = Trs(); + std::memset(position, 0, sizeof(position)); + std::memset(rotation, 0, sizeof(rotation)); + rotation[0] = 1.f; + rotation[4] = 1.f; + rotation[8] = 1.f; + std::memset(size, 0, sizeof(size)); break; case mjGEOM_SKIN: - meshes.push_back(GetSkinFlexMesh(model_objects, geom.objid)); + mjrf_setRenderableMesh(renderable, GetSkinFlexMesh(model_objects, geom.objid), 0, 0); // Skins are defined in global space. - trs = Trs(); + std::memset(position, 0, sizeof(position)); + std::memset(rotation, 0, sizeof(rotation)); + rotation[0] = 1.f; + rotation[4] = 1.f; + rotation[8] = 1.f; + std::memset(size, 0, sizeof(size)); break; case mjGEOM_NONE: case mjGEOM_LABEL: @@ -333,14 +174,6 @@ static void PrepareGeomMeshes(mjrRenderable* renderable, const mjvGeom& geom, break; } - Renderable::downcast(renderable)->SetMeshes(meshes, get_transforms); - - float position[3]; - std::memcpy(position, &trs.translation[0], 3 * sizeof(float)); - float rotation[9]; - std::memcpy(rotation, &trs.rotation[0], 9 * sizeof(float)); - float size[3]; - std::memcpy(size, &trs.size[0], 3 * sizeof(float)); mjrf_setRenderableTransform(renderable, position, rotation, size); } diff --git a/src/experimental/filament/filament/builtins.cc b/src/experimental/filament/filament/builtins.cc index 90b96baf..28468e50 100644 --- a/src/experimental/filament/filament/builtins.cc +++ b/src/experimental/filament/filament/builtins.cc @@ -19,16 +19,16 @@ #include #include #include +#include #include +#include #include #include #include -#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/render_context_filament.h" -#include "experimental/filament/render_context_filament_cpp.h" namespace mujoco { @@ -65,14 +65,15 @@ class BuiltinBuilder : public mjrMeshData { virtual ~BuiltinBuilder() = default; template - static UniquePtr Create(mjrfContext* ctx, Args&&... args) { + static std::unique_ptr Create(filament::Engine* engine, + Args&&... args) { auto builder = new T(std::forward(args)...); mjrMeshData* mesh_data = builder->PrepareMeshData(); mesh_data->release_callback = +[](void* user_data) { delete static_cast(user_data); }; mesh_data->user_data = builder; - return CreateMesh(ctx, *mesh_data); + return std::make_unique(engine, *mesh_data); } mjrMeshData* PrepareMeshData() { @@ -617,44 +618,28 @@ class DomeBuilder : public BuiltinBuilder { } }; -UniquePtr CreateLine(mjrfContext* ctx) { - return BuiltinBuilder::Create(ctx); +Builtins::Builtins(filament::Engine* engine, int nstack, int nslice, int nquad) { + line_ = BuiltinBuilder::Create(engine); + plane_ = BuiltinBuilder::Create(engine, nquad); + triangle_ = BuiltinBuilder::Create(engine); + box_ = BuiltinBuilder::Create(engine, nquad); + line_box_ = BuiltinBuilder::Create(engine); + sphere_ = BuiltinBuilder::Create(engine, nstack, nslice); + tube_ = BuiltinBuilder::Create(engine, nstack, nslice); + disk_ = BuiltinBuilder::Create(engine, nslice); + dome_ = BuiltinBuilder::Create(engine, nstack, nslice); + cone_ = BuiltinBuilder::Create(engine, nstack, nslice); } -UniquePtr CreatePlane(mjrfContext* ctx, int nquad) { - return BuiltinBuilder::Create(ctx, nquad); -} - -UniquePtr CreateTriangle(mjrfContext* ctx) { - return BuiltinBuilder::Create(ctx); -} - -UniquePtr CreateBox(mjrfContext* ctx, int nquad) { - return BuiltinBuilder::Create(ctx, nquad); -} - -UniquePtr CreateLineBox(mjrfContext* ctx) { - return BuiltinBuilder::Create(ctx); -} - -UniquePtr CreateSphere(mjrfContext* ctx, int nstack, int nslice) { - return BuiltinBuilder::Create(ctx, nstack, nslice); -} - -UniquePtr CreateTube(mjrfContext* ctx, int nstack, int nslice) { - return BuiltinBuilder::Create(ctx, nstack, nslice); -} - -UniquePtr CreateDisk(mjrfContext* ctx, int nslice) { - return BuiltinBuilder::Create(ctx, nslice); -} - -UniquePtr CreateDome(mjrfContext* ctx, int nstack, int nslice) { - return BuiltinBuilder::Create(ctx, nstack, nslice); -} - -UniquePtr CreateCone(mjrfContext* ctx, int nstack, int nslice) { - return BuiltinBuilder::Create(ctx, nstack, nslice); -} +const Mesh* Builtins::Line() { return line_.get(); } +const Mesh* Builtins::LineBox() { return line_box_.get(); } +const Mesh* Builtins::Plane() { return plane_.get(); } +const Mesh* Builtins::Triangle() { return triangle_.get(); } +const Mesh* Builtins::Box() { return box_.get(); } +const Mesh* Builtins::Sphere() { return sphere_.get(); } +const Mesh* Builtins::Cone() { return cone_.get(); } +const Mesh* Builtins::Disk() { return disk_.get(); } +const Mesh* Builtins::Dome() { return dome_.get(); } +const Mesh* Builtins::Tube() { return tube_.get(); } } // namespace mujoco diff --git a/src/experimental/filament/filament/builtins.h b/src/experimental/filament/filament/builtins.h index 2cf2c591..8044b14b 100644 --- a/src/experimental/filament/filament/builtins.h +++ b/src/experimental/filament/filament/builtins.h @@ -15,22 +15,41 @@ #ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUILTINS_H_ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_BUILTINS_H_ -#include "experimental/filament/render_context_filament.h" -#include "experimental/filament/render_context_filament_cpp.h" +#include + +#include +#include "experimental/filament/filament/mesh.h" -// Generates buffers for built-in shapes. namespace mujoco { -UniquePtr CreateLine(mjrfContext* ctx); -UniquePtr CreatePlane(mjrfContext* ctx, int nquad); -UniquePtr CreateTriangle(mjrfContext* ctx); -UniquePtr CreateBox(mjrfContext* ctx, int nquad); -UniquePtr CreateLineBox(mjrfContext* ctx); -UniquePtr CreateSphere(mjrfContext* ctx, int nstack, int nslice); -UniquePtr CreateTube(mjrfContext* ctx, int nstack, int nslice); -UniquePtr CreateDisk(mjrfContext* ctx, int nslice); -UniquePtr CreateDome(mjrfContext* ctx, int nstack, int nslice); -UniquePtr CreateCone(mjrfContext* ctx, int nstack, int nslice); +// A collection of meshes that "built in" to the renderer. +class Builtins { + public: + Builtins(filament::Engine* engine, int nstack, int nslice, int nquad); + + const Mesh* Line(); + const Mesh* LineBox(); + const Mesh* Plane(); + const Mesh* Triangle(); + const Mesh* Box(); + const Mesh* Sphere(); + const Mesh* Cone(); + const Mesh* Disk(); + const Mesh* Dome(); + const Mesh* Tube(); + + private: + std::unique_ptr line_; + std::unique_ptr line_box_; + std::unique_ptr plane_; + std::unique_ptr triangle_; + std::unique_ptr box_; + std::unique_ptr sphere_; + std::unique_ptr cone_; + std::unique_ptr disk_; + std::unique_ptr dome_; + std::unique_ptr tube_; +}; } // namespace mujoco diff --git a/src/experimental/filament/filament/object_manager.cc b/src/experimental/filament/filament/object_manager.cc index 2db47f0d..fb2e31d6 100644 --- a/src/experimental/filament/filament/object_manager.cc +++ b/src/experimental/filament/filament/object_manager.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -27,6 +28,7 @@ #include #include #include +#include "experimental/filament/filament/builtins.h" #include "user/user_resource.h" namespace mujoco { @@ -131,6 +133,22 @@ filament::Material* ObjectManager::GetMaterial(MaterialType type) const { return materials_[type]; } +Builtins* ObjectManager::GetBuiltins(int nstack, int nslice, int nquad) { + // Assumes nstack, nslice, and nquad are non-negative and less than 2^20. + std::uint64_t key = (static_cast(nstack) << 20) | + (static_cast(nslice) << 40) | + static_cast(nquad); + + auto iter = builtins_.find(key); + if (iter == builtins_.end()) { + auto builtins = std::make_unique(engine_, nstack, nslice, nquad); + Builtins* ptr = builtins.get(); + builtins_[key] = std::move(builtins); + return ptr; + } + return iter->second.get(); +} + const filament::Texture* ObjectManager::GetFallbackTexture( mjtTextureRole role) const { if (role < 0 || role >= mjNTEXROLE) { diff --git a/src/experimental/filament/filament/object_manager.h b/src/experimental/filament/filament/object_manager.h index 140d08fe..05dab205 100644 --- a/src/experimental/filament/filament/object_manager.h +++ b/src/experimental/filament/filament/object_manager.h @@ -17,15 +17,18 @@ #include #include +#include #include #include #include +#include #include #include #include #include #include +#include "experimental/filament/filament/builtins.h" namespace mujoco { @@ -84,6 +87,9 @@ class ObjectManager { // Returns the fallback Texture with the given role. const filament::Texture* GetFallbackTexture(mjtTextureRole role) const; + // Returns the built-in mesh collection with the given parameters. + Builtins* GetBuiltins(int nstack, int nslice, int nquad); + // Loads the given asset from the filament resource directory. std::unique_ptr LoadAsset(std::string_view filename); @@ -97,6 +103,7 @@ class ObjectManager { filament::Engine* engine_ = nullptr; std::array materials_; std::array fallback_textures_; + std::unordered_map> builtins_; filament::Texture* fallback_white_ = nullptr; filament::Texture* fallback_black_ = nullptr; filament::Texture* fallback_normal_ = nullptr; diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 636473f7..8f4af490 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -16,7 +16,7 @@ #include #include -#include +#include #include #include @@ -24,8 +24,12 @@ #include #include #include +#include +#include +#include #include #include +#include "experimental/filament/filament/builtins.h" #include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/math_util.h" @@ -36,8 +40,15 @@ namespace mujoco { +using filament::math::float2; +using filament::math::float3; +using filament::math::float4; using filament::math::mat4f; +// An arbitrary scale factor for arrows. +static constexpr float kArrowScale = 1.f / 6.f; +static constexpr float kArrowHeadSize = 1.75f; + Renderable::Renderable(FilamentContext* ctx, const mjrRenderableParams& params) : object_mgr_(ctx->GetObjectManager()), params_(params) { mjr_defaultMaterialParams(&material_params_); @@ -159,20 +170,12 @@ const mat4f& Renderable::GetTransform() const { return transform_; } -void Renderable::SetMeshes(std::span meshes, - GetTransformFn get_transform_fn) { - if (!parts_.empty()) { - mju_error("Cannot set meshes for renderable with multiple parts."); - } - - get_transform_fn_ = get_transform_fn; - for (int i = 0; i < meshes.size(); ++i) { - Part& part = parts_.emplace_back(); - part.mesh = Mesh::downcast(meshes[i]); - part.elem_offset = 0; - part.elem_count = part.mesh->GetFilamentIndexBuffer()->getIndexCount(); - InitPartEntity(part); - } +void Renderable::AppendMesh(const Mesh* mesh) { + Part& part = parts_.emplace_back(); + part.mesh = Mesh::downcast(mesh); + part.elem_offset = 0; + part.elem_count = part.mesh->GetFilamentIndexBuffer()->getIndexCount(); + InitPartEntity(part); } void Renderable::AddToScene(filament::Scene* scene) { @@ -410,6 +413,198 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { } } +void Renderable::SetGeomMesh(mjtGeom type, int nstack, int nslice, int nquad) { + Builtins* builtins = object_mgr_->GetBuiltins(nstack, nslice, nquad); + + switch (type) { + case mjGEOM_PLANE: + AppendMesh(builtins->Plane()); + break; + case mjGEOM_SPHERE: + AppendMesh(builtins->Sphere()); + break; + case mjGEOM_ELLIPSOID: + AppendMesh(builtins->Sphere()); + break; + case mjGEOM_BOX: + AppendMesh(builtins->Box()); + break; + case mjGEOM_CAPSULE: + // Capsules are a tube with two domes at the ends. + AppendMesh(builtins->Tube()); + AppendMesh(builtins->Dome()); + AppendMesh(builtins->Dome()); + + get_transform_fn_ = [](int index, const Trs& trs) { + // We apply an inverse scale to the domes to counteract the capsule's + // overall scale so that the domes remain spherical in shape. + const float xz_size = 0.5f * (trs.size.x + trs.size.y); + if (index == 0) { + return trs.ToTransform(); + } else if (index == 1) { + // Move the first dome to the top of the capsule. + mat4f top = mat4f(trs.rotation, trs.translation); + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); + return top; + } else if (index == 2) { + // Move the second dome to the bottom of the capsule and rotate it 180 + // degrees so that it's facing the right way. + mat4f bottom = mat4f(trs.rotation, trs.translation); + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(float3{trs.size.x, trs.size.y, xz_size}); + return bottom; + } else { + mju_error("Invalid index for capsule geom: %d (expected [0,2])", index); + return trs.ToTransform(); + } + }; + break; + case mjGEOM_CYLINDER: + // Cylinders are a tube with two disks at the ends. + AppendMesh(builtins->Tube()); + AppendMesh(builtins->Disk()); + AppendMesh(builtins->Disk()); + + get_transform_fn_ = [](int index, const Trs& trs) { + if (index == 0) { + return trs.ToTransform(); + } else if (index == 1) { + // Move the first disk to the top of the cylinder. + mat4f top = mat4f(trs.rotation, trs.translation); + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(trs.size); + return top; + } else if (index == 2) { + // Move the second disk to the bottom of the cylinder. Rotate the disk + // 180 degrees so that the normals point outwards. + mat4f bottom = mat4f(trs.rotation, trs.translation); + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(trs.size); + return bottom; + } else { + mju_error("Invalid index for cylinder geom: %d (expected [0,2])", index); + return trs.ToTransform(); + } + }; + break; + case mjGEOM_ARROW: + AppendMesh(builtins->Tube()); + AppendMesh(builtins->Cone()); + AppendMesh(builtins->Disk()); + AppendMesh(builtins->Disk()); + + get_transform_fn_ = [](int index, const Trs& trs) { + mat4f base = mat4f(trs.rotation, trs.translation); + base *= mat4f::scaling(float3{1, 1, kArrowScale}); + base *= mat4f::translation(float3{0, 0, trs.size.z}); + if (index == 0) { + return base * mat4f::scaling(trs.size); + } else if (index == 1) { + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return top * mat4f::scaling(trs.size); + } else if (index == 2) { + mat4f top_disk = base; + top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); + top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return top_disk * mat4f::scaling(trs.size); + } else if (index == 3) { + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + return bottom * mat4f::scaling(trs.size); + } else { + mju_error("Invalid index for arrow geom: %d (expected [0,3])", index); + return trs.ToTransform(); + } + }; + break; + case mjGEOM_ARROW1: + AppendMesh(builtins->Tube()); + AppendMesh(builtins->Cone()); + AppendMesh(builtins->Disk()); + + get_transform_fn_ = [](int index, const Trs& trs) { + mat4f base = mat4f(trs.rotation, trs.translation); + base *= mat4f::scaling(float3{1, 1, kArrowScale}); + base *= mat4f::translation(float3{0, 0, trs.size.z}); + if (index == 0) { + return base * mat4f::scaling(trs.size); + } else if (index == 1) { + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + return top * mat4f::scaling(trs.size); + } else if (index == 2) { + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + return bottom * mat4f::scaling(trs.size); + } else { + mju_error("Invalid index for arrow1 geom: %d (expected [0,2])", index); + return trs.ToTransform(); + } + }; + break; + case mjGEOM_ARROW2: + AppendMesh(builtins->Tube()); + AppendMesh(builtins->Cone()); + AppendMesh(builtins->Cone()); + AppendMesh(builtins->Disk()); + AppendMesh(builtins->Disk()); + + get_transform_fn_ = [](int index, const Trs& trs) { + mat4f base = mat4f(trs.rotation, trs.translation); + base *= mat4f::scaling(float3{1, 1, kArrowScale}); + base *= mat4f::translation(float3{0, 0, trs.size.z}); + if (index == 0) { + return base * mat4f::scaling(trs.size); + } else if (index == 1) { + mat4f top = base; + top *= mat4f::translation(float3{0, 0, trs.size.z}); + top *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return top * mat4f::scaling(trs.size); + } else if (index == 2) { + mat4f bottom = base; + bottom *= mat4f::translation(float3{0, 0, -trs.size.z}); + bottom *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + bottom *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return bottom * mat4f::scaling(trs.size); + } else if (index == 3) { + mat4f top_disk = base; + top_disk *= mat4f::translation(float3{0, 0, trs.size.z}); + top_disk *= mat4f::rotation(std::numbers::pi, float3{1, 0, 0}); + top_disk *= mat4f::scaling(float3{kArrowHeadSize, kArrowHeadSize, 1.0f}); + return top_disk * mat4f::scaling(trs.size); + } else if (index == 4) { + mat4f bottom_disk = base; + bottom_disk *= mat4f::translation(float3{0, 0, -trs.size.z}); + return bottom_disk * mat4f::scaling(trs.size); + } else { + mju_error("Invalid index for arrow2 geom: %d (expected [0,4])", index); + return trs.ToTransform(); + } + }; + break; + case mjGEOM_LINE: + AppendMesh(builtins->Line()); + break; + case mjGEOM_LINEBOX: + AppendMesh(builtins->LineBox()); + break; + case mjGEOM_TRIANGLE: + AppendMesh(builtins->Triangle()); + break; + default: + mju_error("Unsupported geom type: %d", type); + break; + } +} + filament::Engine* Renderable::GetEngine() { return object_mgr_->GetEngine(); } } // namespace mujoco diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index e7a405f4..3d41b2a1 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -17,13 +17,13 @@ #include #include -#include #include #include #include #include #include +#include #include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" @@ -59,20 +59,15 @@ class Renderable : public mjrRenderable { // assumes the entire mesh should be appended. void SetMesh(const Mesh* mesh, int elem_offset = 0, int elem_count = 0); + // Sets the mesh of the renderable based on the given geom type. + void SetGeomMesh(mjtGeom type, int nstack, int nslice, int nquad); + // Sets the transform of the renderable. void SetTransform(const Trs& trs); // Returns the current transform of the renderable. const filament::math::mat4f& GetTransform() const; - // Sets multiple meshes for a renderable. Users can optionally provide a - // function that will be used to compute the transform for each (sub)mesh - // relative to the transform of the renderable itself. This allows users to - // construct compound (but rigid) objects from multiple meshes. - using GetTransformFn = std::function; - void SetMeshes(std::span meshes, - GetTransformFn get_transform = nullptr); - // Sets the layer mask for the managed filament Entities. Layer masks can be // used to show/hide the renderable in different views. Returns the previous // layer mask. @@ -128,6 +123,8 @@ class Renderable : public mjrRenderable { } private: + using GetTransformFn = std::function; + struct Part { utils::Entity entity; const Mesh* mesh = nullptr; @@ -135,6 +132,8 @@ class Renderable : public mjrRenderable { int elem_count = 0; }; + void AppendMesh(const Mesh* mesh); + void InitPartEntity(Part& part); void AssignMaterial(mjrDrawMode mode, ObjectManager::MaterialType material_type); diff --git a/src/experimental/filament/render_context_filament.cc b/src/experimental/filament/render_context_filament.cc index 68b58766..938f4d19 100644 --- a/src/experimental/filament/render_context_filament.cc +++ b/src/experimental/filament/render_context_filament.cc @@ -254,6 +254,12 @@ void mjrf_setRenderableMesh(mjrRenderable* renderable, const mjrMesh* mesh, ->SetMesh(mujoco::Mesh::downcast(mesh), elem_offset, elem_count); } +void mjrf_setRenderableGeomMesh(mjrRenderable* renderable, mjtGeom type, + int nstack, int nslice, int nquad) { + mujoco::Renderable::downcast(renderable)->SetGeomMesh(type, nstack, nslice, + nquad); +} + void mjrf_setRenderableMaterial(mjrRenderable* renderable, const mjrMaterialParams* params, const mjrMaterialTextures* textures) { @@ -265,9 +271,9 @@ void mjrf_setRenderableTransform(mjrRenderable* renderable, const float rotation[9], const float size[3]) { const filament::math::float3 fposition{position[0], position[1], position[2]}; const filament::math::float3 fsize{size[0], size[1], size[2]}; - const filament::math::mat3f frotation{rotation[0], rotation[1], rotation[2], - rotation[3], rotation[4], rotation[5], - rotation[6], rotation[7], rotation[8]}; + const filament::math::mat3f frotation{rotation[0], rotation[3], rotation[6], + rotation[1], rotation[4], rotation[7], + rotation[2], rotation[5], rotation[8]}; mujoco::Renderable::downcast(renderable) ->SetTransform({fposition, frotation, fsize}); } diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index fadc10ca..0881a0b6 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -500,6 +500,12 @@ mjrLightType mjrf_getLightType(const mjrLight* light); void mjrf_setRenderableMesh(mjrRenderable* renderable, const mjrMesh* mesh, int elem_offset, int elem_count); +// Sets the mesh of the renderable to a built-in mesh based on the geom type. +// Note: using the same parameters (nstack, nslice, nquad) will have better +// performance as the internal mesh data can be shared across renderables. +void mjrf_setRenderableGeomMesh(mjrRenderable* renderable, mjtGeom type, + int nstack, int nslice, int nquad); + // Sets the material properties and textures of the renderable. void mjrf_setRenderableMaterial(mjrRenderable* renderable, const mjrMaterialParams* params, From 8e214e513301bfe647d6b03f9b36b65ee5648be8 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 7 May 2026 01:18:58 -0700 Subject: [PATCH 207/251] Move imgui_editor into filament library. PiperOrigin-RevId: 911808107 Change-Id: I14318b1c7396e2eaf0df9d6afcc5298de01bcfa4 --- src/experimental/filament/CMakeLists.txt | 3 +- .../filament/compat/imgui_editor.h | 27 --------- .../filament/compat/mjr_filament_renderer.cc | 5 +- .../filament/compat/scene_bridge.cc | 35 +----------- .../filament/compat/scene_bridge.h | 6 -- .../{compat => filament}/imgui_editor.cc | 56 +++++++++---------- .../filament/render_context_filament.h | 5 ++ 7 files changed, 39 insertions(+), 98 deletions(-) delete mode 100644 src/experimental/filament/compat/imgui_editor.h rename src/experimental/filament/{compat => filament}/imgui_editor.cc (95%) diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index 9c9a2434..20ae8c5e 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -32,6 +32,7 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/filament_context.h filament/filament_platform_factory.cc filament/filament_platform_factory.h + filament/imgui_editor.cc filament/light.cc filament/light.h filament/material.cc @@ -53,8 +54,6 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/texture.h compat/imgui_bridge.cc compat/imgui_bridge.h - compat/imgui_editor.cc - compat/imgui_editor.h compat/mjr_filament_renderer.cc compat/mjr_filament_renderer.h compat/model_objects.cc diff --git a/src/experimental/filament/compat/imgui_editor.h b/src/experimental/filament/compat/imgui_editor.h deleted file mode 100644 index 3457fd26..00000000 --- a/src/experimental/filament/compat/imgui_editor.h +++ /dev/null @@ -1,27 +0,0 @@ -// Copyright 2025 DeepMind Technologies Limited -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_IMGUI_EDITOR_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_IMGUI_EDITOR_H_ - -#include "experimental/filament/compat/scene_bridge.h" - -namespace mujoco { - -// Generates a ImGui Window for the given scene views. -void DrawGui(SceneBridge* scene_bridge); - -} // namespace mujoco - -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_COMPAT_IMGUI_EDITOR_H_ diff --git a/src/experimental/filament/compat/mjr_filament_renderer.cc b/src/experimental/filament/compat/mjr_filament_renderer.cc index 9331f93c..c56731d8 100644 --- a/src/experimental/filament/compat/mjr_filament_renderer.cc +++ b/src/experimental/filament/compat/mjr_filament_renderer.cc @@ -24,7 +24,6 @@ #include #include #include "experimental/filament/compat/imgui_bridge.h" -#include "experimental/filament/compat/imgui_editor.h" #include "experimental/filament/compat/scene_bridge.h" #include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/model_util.h" @@ -185,6 +184,8 @@ uintptr_t MjrFilamentRenderer::UploadGuiImage(uintptr_t tex_id, return imgui_bridge_->UploadImage(tex_id, pixels, width, height, bpp); } -void MjrFilamentRenderer::UpdateGui() { DrawGui(scene_bridge_.get()); } +void MjrFilamentRenderer::UpdateGui() { + mjrf_DEBUG_drawImguiEditor(scene_bridge_->GetScene()); +} } // namespace mujoco diff --git a/src/experimental/filament/compat/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc index ad6d1518..9d9a355c 100644 --- a/src/experimental/filament/compat/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -32,7 +32,6 @@ #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/model_util.h" -#include "experimental/filament/filament/scene_view.h" #include "experimental/filament/render_context_filament.h" #include "experimental/filament/render_context_filament_cpp.h" @@ -44,13 +43,10 @@ using filament::math::mat3; using filament::math::mat4; static UniquePtr CreateFallbackIndirectLightTexture( - mjrfContext* ctx, std::string_view filename = "") { - if (filename.empty()) { - filename = ObjectManager::kDefaultEnvironmentLight; - } - + mjrfContext* ctx) { std::unique_ptr asset = - FilamentContext::downcast(ctx)->GetObjectManager()->LoadAsset(filename); + FilamentContext::downcast(ctx)->GetObjectManager()->LoadAsset( + ObjectManager::kDefaultEnvironmentLight); mjrTextureConfig config; mjr_defaultTextureConfig(&config); @@ -118,31 +114,6 @@ SceneBridge::~SceneBridge() { renderables_.clear(); } -void SceneBridge::SetEnvironmentLight(std::string_view filename, - float intensity) { - for (auto& light : lights_) { - if (mjrf_getLightType(light.get()) == mjLIGHT_IMAGE) { - mjrf_removeLightFromScene(scene_.get(), light.get()); - light.reset(); - break; - } - } - if (fallback_ibl_) { - mjrf_removeLightFromScene(scene_.get(), fallback_ibl_.get()); - fallback_ibl_.reset(); - } - - fallback_ibl_texture_ = CreateFallbackIndirectLightTexture(ctx_, filename); - - mjrLightParams params; - mjr_defaultLightParams(¶ms); - params.type = mjLIGHT_IMAGE; - params.texture = fallback_ibl_texture_.get(); - params.intensity = intensity; - fallback_ibl_ = CreateLight(ctx_, params); - mjrf_addLightToScene(scene_.get(), fallback_ibl_.get()); -} - std::optional SceneBridge::ClipFromWorld(const float3& pos) const{ const float4 clip_pos = clip_from_world_ * float4(pos, 1.0f); if (clip_pos.w == 0.0f) { diff --git a/src/experimental/filament/compat/scene_bridge.h b/src/experimental/filament/compat/scene_bridge.h index 0f04e402..19dca38a 100644 --- a/src/experimental/filament/compat/scene_bridge.h +++ b/src/experimental/filament/compat/scene_bridge.h @@ -37,12 +37,6 @@ class SceneBridge { SceneBridge(mjrfContext* ctx, const mjModel* model); ~SceneBridge(); - // Updates the environment light using the KTX image at the given path. - void SetEnvironmentLight(std::string_view filename, float intensity); - - // Updates the environment light to the fallback light - void SetFallbackEnvironmentLight(float intensity); - // Updates the Entities in the filament Scene to match the current mjvScene // state. void Update(const mjrRect& viewport, const mjvScene* scene); diff --git a/src/experimental/filament/compat/imgui_editor.cc b/src/experimental/filament/filament/imgui_editor.cc similarity index 95% rename from src/experimental/filament/compat/imgui_editor.cc rename to src/experimental/filament/filament/imgui_editor.cc index 54105aac..fe084307 100644 --- a/src/experimental/filament/compat/imgui_editor.cc +++ b/src/experimental/filament/filament/imgui_editor.cc @@ -12,7 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/filament/compat/imgui_editor.h" #include #include @@ -34,9 +33,9 @@ #include #include #include -#include "experimental/filament/compat/scene_bridge.h" #include "experimental/filament/filament/color_grading_options.h" #include "experimental/filament/filament/scene_view.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -561,7 +560,7 @@ void DrawCameraGui(SceneView* scene_view) { Ui("Direction", &direction); } -void DrawIndirectLightGui(SceneBridge* scene_bridge, SceneView* scene_view) { +void DrawIndirectLightGui(SceneView* scene_view) { filament::View* view = scene_view->GetDefaultRenderView(); auto ibl = view->getScene()->getIndirectLight(); @@ -572,12 +571,6 @@ void DrawIndirectLightGui(SceneBridge* scene_bridge, SceneView* scene_view) { ibl->setIntensity(intensity); } } - - static char filename[256]; - ImGui::InputText("Filename", filename, sizeof(filename)); - if (ImGui::Button("Load")) { - scene_bridge->SetEnvironmentLight(filename, intensity); - } } void DrawLightGui(filament::LightManager& lm, @@ -650,75 +643,79 @@ void DrawLightGui(filament::LightManager& lm, } } -void DrawGui(SceneBridge* scene_bridge) { - SceneView* scene_view = SceneView::downcast(scene_bridge->GetScene()); +} // namespace mujoco + +extern "C" { + +void mjrf_DEBUG_drawImguiEditor(mjrScene* scene) { + mujoco::SceneView* scene_view = mujoco::SceneView::downcast(scene); filament::View* view = scene_view->GetDefaultRenderView(); filament::Engine* engine = scene_view->GetEngine(); filament::LightManager& lm = engine->getLightManager(); if (ImGui::TreeNodeEx("Ambient Occlusion")) { - DrawAmbientOcclusionGui(scene_view); + mujoco::DrawAmbientOcclusionGui(scene_view); ImGui::TreePop(); } if (ImGui::TreeNodeEx("Screen Space")) { - DrawScreenSpaceGui(scene_view); + mujoco::DrawScreenSpaceGui(scene_view); ImGui::TreePop(); } if (ImGui::TreeNodeEx("Shadowing")) { - DrawShadowingGui(scene_view); + mujoco::DrawShadowingGui(scene_view); ImGui::TreePop(); } if (ImGui::TreeNodeEx("Post Processing")) { - DrawPostProcessingGui(scene_view); + mujoco::DrawPostProcessingGui(scene_view); if (ImGui::TreeNodeEx("Anti Aliasing (FXAA)")) { - DrawFxaaGui(scene_view); + mujoco::DrawFxaaGui(scene_view); ImGui::TreePop(); } if (ImGui::TreeNodeEx("Anti Aliasing (MSAA)")) { - DrawMsaaGui(scene_view); + mujoco::DrawMsaaGui(scene_view); ImGui::TreePop(); } if (ImGui::TreeNodeEx("Anti Aliasing (Temporal)")) { - DrawTaaGui(scene_view); + mujoco::DrawTaaGui(scene_view); ImGui::TreePop(); } if (ImGui::TreeNodeEx("Bloom")) { - DrawBloomGui(scene_view); + mujoco::DrawBloomGui(scene_view); ImGui::TreePop(); } if (ImGui::TreeNodeEx("Color Grading")) { - DrawColorGradingGui(scene_view); + mujoco::DrawColorGradingGui(scene_view); ImGui::TreePop(); } if (ImGui::TreeNodeEx("Depth of Field")) { - DrawDepthOfFieldGui(scene_view); + mujoco::DrawDepthOfFieldGui(scene_view); ImGui::TreePop(); } if (ImGui::TreeNodeEx("Dithering")) { - DrawDitheringGui(scene_view); + mujoco::DrawDitheringGui(scene_view); ImGui::TreePop(); } if (ImGui::TreeNodeEx("Fog")) { - DrawFogGui(scene_view); + mujoco::DrawFogGui(scene_view); ImGui::TreePop(); } if (ImGui::TreeNodeEx("Vignette")) { - DrawVignetteGui(scene_view); + mujoco::DrawVignetteGui(scene_view); ImGui::TreePop(); } ImGui::TreePop(); } if (ImGui::TreeNodeEx("Visibility Layers")) { - DrawVisibleLayersGui(scene_view); + mujoco::DrawVisibleLayersGui(scene_view); ImGui::TreePop(); } if (ImGui::TreeNodeEx("Camera")) { - DrawCameraGui(scene_view); + mujoco::DrawCameraGui(scene_view); ImGui::TreePop(); } if (ImGui::TreeNodeEx("Lights")) { if (ImGui::TreeNodeEx("Indirect (Image-based) Light")) { - DrawIndirectLightGui(scene_bridge, scene_view); + mujoco::DrawIndirectLightGui(scene_view); ImGui::TreePop(); } view->getScene()->forEach([&](utils::Entity entity) { @@ -732,11 +729,12 @@ void DrawGui(SceneBridge* scene_bridge) { : " (S)"; const std::string name = "Light " + std::to_string(entity.getId()) + type; if (ImGui::TreeNodeEx(name.c_str())) { - DrawLightGui(lm, li); + mujoco::DrawLightGui(lm, li); ImGui::TreePop(); } }); ImGui::TreePop(); } } -} // namespace mujoco + +} // extern "C" diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index 0881a0b6..29c403de 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -562,6 +562,11 @@ mjrFrameHandle mjrf_render(mjrfContext* ctx, const mjrRenderRequest* req, // Waits for the rendering to complete for the given frame handle. void mjrf_waitForFrame(mjrfContext* ctx, mjrFrameHandle frame); +// Draws an ImGui editor for the given scene, exposing filament-specific +// settings. +void mjrf_DEBUG_drawImguiEditor(mjrScene* scene); + + // Legacy API, to be deprecated. void mjrf_defaultFilamentConfig(mjrFilamentConfig* config); From 607970a9004a13e488c02aca89c58136b5538813 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 7 May 2026 01:55:11 -0700 Subject: [PATCH 208/251] Remove dependency between SceneBridge and ObjectManager. PiperOrigin-RevId: 911824153 Change-Id: Ie87d6d9e77b0b3f68a41a0c7d13a36fb2029e4c2 --- .../filament/compat/scene_bridge.cc | 25 +++--- .../filament/filament/object_manager.cc | 81 ++++++++----------- .../filament/filament/object_manager.h | 20 ----- .../filament/render_context_filament_cpp.h | 3 + 4 files changed, 52 insertions(+), 77 deletions(-) diff --git a/src/experimental/filament/compat/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc index 9d9a355c..d78dc09c 100644 --- a/src/experimental/filament/compat/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -28,8 +29,6 @@ #include #include "experimental/filament/compat/model_objects.h" #include "experimental/filament/compat/scene_geom_util.h" -#include "experimental/filament/filament/filament_context.h" -#include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/model_util.h" #include "experimental/filament/render_context_filament.h" @@ -44,9 +43,17 @@ using filament::math::mat4; static UniquePtr CreateFallbackIndirectLightTexture( mjrfContext* ctx) { - std::unique_ptr asset = - FilamentContext::downcast(ctx)->GetObjectManager()->LoadAsset( - ObjectManager::kDefaultEnvironmentLight); + const std::string filename = ResolveFilamentAssetPath("ibl.ktx"); + mjResource* resource = + mju_openResource("", filename.c_str(), nullptr, nullptr, 0); + if (!resource) { + mju_error("Failed to open resource: %s", filename.c_str()); + } + const void* bytes = nullptr; + const int nbytes = mju_readResource(resource, &bytes); + if (bytes == nullptr || nbytes <= 0) { + mju_error("Failed to read resource: %s", filename.c_str()); + } mjrTextureConfig config; mjr_defaultTextureConfig(&config); @@ -60,12 +67,12 @@ static UniquePtr CreateFallbackIndirectLightTexture( mjrTextureData payload; mjr_defaultTextureData(&payload); - payload.bytes = asset->GetBytes().data(); - payload.nbytes = asset->GetBytes().size(); + payload.bytes = bytes; + payload.nbytes = nbytes; payload.release_callback = +[](void* user_data) { - delete static_cast(user_data); + mju_closeResource((mjResource*)user_data); }; - payload.user_data = asset.release(); + payload.user_data = resource; mjrf_setTextureData(texture.get(), &payload); return texture; diff --git a/src/experimental/filament/filament/object_manager.cc b/src/experimental/filament/filament/object_manager.cc index fb2e31d6..487f95fd 100644 --- a/src/experimental/filament/filament/object_manager.cc +++ b/src/experimental/filament/filament/object_manager.cc @@ -33,55 +33,45 @@ namespace mujoco { -static std::string GetAssetPath(std::string_view filename) { - std::string path = "filament:" + std::string(filename); +std::string ResolveFilamentAssetPath(const std::string& filename) { + std::string path = "filament:" + filename; return path; } -ObjectManager::Asset::Asset(std::string_view filename) { - std::string path = GetAssetPath(filename); - resource = mju_openResource("", path.c_str(), nullptr, nullptr, 0); - size = mju_readResource(resource, const_cast(&payload)); -} - -ObjectManager::Asset::~Asset() { - if (resource) { - mju_closeResource(resource); - } -} - -std::span ObjectManager::Asset::GetBytes() const { - return {reinterpret_cast(payload), size}; -} +static filament::Material* LoadMaterial(filament::Engine* engine, + std::string_view filename) { + const std::string path = ResolveFilamentAssetPath(std::string(filename)); + mjResource* resource = mju_openResource("", path.c_str(), nullptr, nullptr, 0); + void* payload = nullptr; + int size = mju_readResource(resource, const_cast(&payload)); + filament::Material::Builder material_builder; + material_builder.package(payload, size); + filament::Material* material = material_builder.build(*engine); + mju_closeResource(resource); + return material; +}; ObjectManager::ObjectManager(filament::Engine* engine) : engine_(engine) { - auto LoadMaterial = [this](std::string_view filename) { - Asset asset(filename); - filament::Material::Builder material_builder; - material_builder.package(asset.payload, asset.size); - return material_builder.build(*this->engine_); - }; - - materials_[kPbr] = LoadMaterial("pbr.filamat"); - materials_[kPbrPacked] = LoadMaterial("pbr_packed.filamat"); - materials_[kPhong2d] = LoadMaterial("phong_2d.filamat"); - materials_[kPhong2dFade] = LoadMaterial("phong_2d_fade.filamat"); - materials_[kPhong2dReflect] = LoadMaterial("phong_2d_reflect.filamat"); - materials_[kPhong2dUv] = LoadMaterial("phong_2d_uv.filamat"); - materials_[kPhong2dUvFade] = LoadMaterial("phong_2d_uv_fade.filamat"); - materials_[kPhong2dUvReflect] = LoadMaterial("phong_2d_uv_reflect.filamat"); - materials_[kPhongColor] = LoadMaterial("phong_color.filamat"); - materials_[kPhongColorFade] = LoadMaterial("phong_color_fade.filamat"); - materials_[kPhongColorReflect] = LoadMaterial("phong_color_reflect.filamat"); - materials_[kPhongCube] = LoadMaterial("phong_cube.filamat"); - materials_[kPhongCubeFade] = LoadMaterial("phong_cube_fade.filamat"); - materials_[kPhongCubeReflect] = LoadMaterial("phong_cube_reflect.filamat"); - materials_[kUnlitSegmentation] = LoadMaterial("unlit_segmentation.filamat"); - materials_[kUnlitLine] = LoadMaterial("unlit_line.filamat"); - materials_[kUnlitDecor] = LoadMaterial("unlit_decor.filamat"); - materials_[kUnlitDepth] = LoadMaterial("unlit_depth.filamat"); - materials_[kUnlitUi] = LoadMaterial("unlit_ui.filamat"); + materials_[kPbr] = LoadMaterial(engine, "pbr.filamat"); + materials_[kPbrPacked] = LoadMaterial(engine, "pbr_packed.filamat"); + materials_[kPhong2d] = LoadMaterial(engine, "phong_2d.filamat"); + materials_[kPhong2dFade] = LoadMaterial(engine, "phong_2d_fade.filamat"); + materials_[kPhong2dReflect] = LoadMaterial(engine, "phong_2d_reflect.filamat"); + materials_[kPhong2dUv] = LoadMaterial(engine, "phong_2d_uv.filamat"); + materials_[kPhong2dUvFade] = LoadMaterial(engine, "phong_2d_uv_fade.filamat"); + materials_[kPhong2dUvReflect] = LoadMaterial(engine, "phong_2d_uv_reflect.filamat"); + materials_[kPhongColor] = LoadMaterial(engine, "phong_color.filamat"); + materials_[kPhongColorFade] = LoadMaterial(engine, "phong_color_fade.filamat"); + materials_[kPhongColorReflect] = LoadMaterial(engine, "phong_color_reflect.filamat"); + materials_[kPhongCube] = LoadMaterial(engine, "phong_cube.filamat"); + materials_[kPhongCubeFade] = LoadMaterial(engine, "phong_cube_fade.filamat"); + materials_[kPhongCubeReflect] = LoadMaterial(engine, "phong_cube_reflect.filamat"); + materials_[kUnlitSegmentation] = LoadMaterial(engine, "unlit_segmentation.filamat"); + materials_[kUnlitLine] = LoadMaterial(engine, "unlit_line.filamat"); + materials_[kUnlitDecor] = LoadMaterial(engine, "unlit_decor.filamat"); + materials_[kUnlitDepth] = LoadMaterial(engine, "unlit_depth.filamat"); + materials_[kUnlitUi] = LoadMaterial(engine, "unlit_ui.filamat"); static uint8_t black_rgb[3] = {0, 0, 0}; static uint8_t white_rgb[3] = {255, 255, 255}; @@ -156,9 +146,4 @@ const filament::Texture* ObjectManager::GetFallbackTexture( } return fallback_textures_[role]; } - -std::unique_ptr ObjectManager::LoadAsset( - std::string_view filename) { - return std::unique_ptr(new Asset(filename)); -} } // namespace mujoco diff --git a/src/experimental/filament/filament/object_manager.h b/src/experimental/filament/filament/object_manager.h index 05dab205..4365029b 100644 --- a/src/experimental/filament/filament/object_manager.h +++ b/src/experimental/filament/filament/object_manager.h @@ -35,23 +35,6 @@ namespace mujoco { // Creates and owns various filament objects based on the data in a mjrContext. class ObjectManager { public: - class Asset { - public: - ~Asset(); - - std::span GetBytes() const; - - Asset(const Asset&) = delete; - Asset& operator=(const Asset&) = delete; - private: - friend class ObjectManager; - explicit Asset(std::string_view filename); - - std::size_t size = 0; - void* payload = nullptr; - mjResource* resource = nullptr; - }; - ObjectManager(filament::Engine* engine); ~ObjectManager(); @@ -90,9 +73,6 @@ class ObjectManager { // Returns the built-in mesh collection with the given parameters. Builtins* GetBuiltins(int nstack, int nslice, int nquad); - // Loads the given asset from the filament resource directory. - std::unique_ptr LoadAsset(std::string_view filename); - // The default environment light to use if no environment light is specified. static constexpr const char* kDefaultEnvironmentLight = "ibl.ktx"; diff --git a/src/experimental/filament/render_context_filament_cpp.h b/src/experimental/filament/render_context_filament_cpp.h index d5b58ed5..56c1c2d5 100644 --- a/src/experimental/filament/render_context_filament_cpp.h +++ b/src/experimental/filament/render_context_filament_cpp.h @@ -16,6 +16,7 @@ #define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_RENDER_CONTEXT_FILAMENT_CPP_H_ #include +#include #include "experimental/filament/render_context_filament.h" @@ -66,6 +67,8 @@ inline UniquePtr CreateRenderTarget( return UniquePtr(render_target, mjrf_destroyRenderTarget); } +std::string ResolveFilamentAssetPath(const std::string& filename); + } // namespace mujoco #endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_RENDER_CONTEXT_FILAMENT_CPP_H_ From 7ade42ca88fdd03215c349c11d8aa0e33c5543b1 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 7 May 2026 03:48:22 -0700 Subject: [PATCH 209/251] Internal changes. PiperOrigin-RevId: 911871922 Change-Id: I82452ffb4c96dca0206d2284193e0756e3dc830e --- .../filament/filament_platform_factory.cc | 1 + .../platform/hal/graphics_mode.cc | 9 +- src/experimental/platform/hal/graphics_mode.h | 3 + .../platform/hal/renderer_test.cc | 127 ++++++++++++++++++ src/experimental/studio/app.cc | 5 + 5 files changed, 143 insertions(+), 2 deletions(-) create mode 100644 src/experimental/platform/hal/renderer_test.cc diff --git a/src/experimental/filament/filament/filament_platform_factory.cc b/src/experimental/filament/filament/filament_platform_factory.cc index a7dc9709..9d9863cf 100644 --- a/src/experimental/filament/filament/filament_platform_factory.cc +++ b/src/experimental/filament/filament/filament_platform_factory.cc @@ -14,6 +14,7 @@ #include "experimental/filament/filament/filament_platform_factory.h" +#include #include #include diff --git a/src/experimental/platform/hal/graphics_mode.cc b/src/experimental/platform/hal/graphics_mode.cc index d992f1f4..87547e5a 100644 --- a/src/experimental/platform/hal/graphics_mode.cc +++ b/src/experimental/platform/hal/graphics_mode.cc @@ -28,6 +28,7 @@ bool IsClassic(GraphicsMode gfx_mode) { bool IsFilament(GraphicsMode gfx_mode) { return gfx_mode == GraphicsMode::FilamentOpenGl || gfx_mode == GraphicsMode::FilamentVulkan || + gfx_mode == GraphicsMode::FilamentVulkanSoftware || gfx_mode == GraphicsMode::FilamentWebGl || gfx_mode == GraphicsMode::FilamentOpenGlHeadless || gfx_mode == GraphicsMode::FilamentOpenGlSoftware; @@ -42,7 +43,8 @@ bool IsOpenGl(GraphicsMode gfx_mode) { } bool IsVulkan(GraphicsMode gfx_mode) { - return gfx_mode == GraphicsMode::FilamentVulkan; + return gfx_mode == GraphicsMode::FilamentVulkan || + gfx_mode == GraphicsMode::FilamentVulkanSoftware; } bool IsWebGl(GraphicsMode gfx_mode) { @@ -56,7 +58,8 @@ bool IsHeadless(GraphicsMode gfx_mode) { } bool IsSoftware(GraphicsMode gfx_mode) { - return gfx_mode == GraphicsMode::FilamentOpenGlSoftware; + return gfx_mode == GraphicsMode::FilamentOpenGlSoftware || + gfx_mode == GraphicsMode::FilamentVulkanSoftware; } GraphicsMode GraphicsModeFromString(std::string_view str, @@ -69,6 +72,8 @@ GraphicsMode GraphicsModeFromString(std::string_view str, return GraphicsMode::FilamentOpenGl; } else if (str == "vulkan") { return GraphicsMode::FilamentVulkan; + } else if (str == "vulkan_software") { + return GraphicsMode::FilamentVulkanSoftware; } else if (str == "webgl") { return GraphicsMode::FilamentWebGl; } else if (str == "opengl_headless") { diff --git a/src/experimental/platform/hal/graphics_mode.h b/src/experimental/platform/hal/graphics_mode.h index d81dfa96..9b8045fc 100644 --- a/src/experimental/platform/hal/graphics_mode.h +++ b/src/experimental/platform/hal/graphics_mode.h @@ -33,6 +33,9 @@ enum class GraphicsMode { // The Filament-based renderer running on Vulkan. FilamentVulkan, + // The Filament-based renderer running on Vulkan using software rendering. + FilamentVulkanSoftware, + // The Filament-based renderer running on WebGL. FilamentWebGl, diff --git a/src/experimental/platform/hal/renderer_test.cc b/src/experimental/platform/hal/renderer_test.cc new file mode 100644 index 00000000..0e020162 --- /dev/null +++ b/src/experimental/platform/hal/renderer_test.cc @@ -0,0 +1,127 @@ +// Copyright 2026 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include "experimental/platform/hal/renderer.h" + +#include +#include +#include + +#include "third_party/mujoco/google/gfx/opengl_dynamic_loader.h" +#include +#include "experimental/platform/hal/graphics_mode.h" +#include "experimental/platform/sim/model_holder.h" + +#include "testing/base/public/gunit.h" + +namespace mujoco::platform { +namespace { + +class RendererTest : public ::testing::Test { + public: + void SetUp() { + mjSpec* spec = mj_makeSpec(); + // Set a clear color. + const double clear_color[] = {1.0, 1.0, 1.0, 1.0}; + mjsNumeric* numeric = mjs_addNumeric(spec); + mjs_setName(numeric->element, "filament.clearColor"); + numeric->size = 4; + mjs_setDouble(numeric->data, clear_color, numeric->size); + holder_ = ModelHolder::FromSpec(spec); + holder_->model()->vis.global.offwidth = width_; + holder_->model()->vis.global.offheight = height_; + } + + void Test(); + + int width_ = 2; + int height_ = 2; + std::unique_ptr holder_; +}; + +TEST_F(RendererTest, OpengGlSoftware) { + Renderer renderer(nullptr, GraphicsMode::FilamentOpenGlSoftware); + renderer.Init(holder_->model()); + + gl::DriverType driver_type = gl::GetLoadedDriverType(); + ASSERT_EQ(driver_type, gl::DriverType::kOsMesa); + + std::vector pixels(width_ * height_ * 3); + renderer.Render(holder_->model(), holder_->data(), nullptr, nullptr, nullptr, + width_, height_, pixels); + // We set the clear color to white, but we don't know the exact color due to + // post processing, but it should definitely not be black. + for (int i = 0; i < pixels.size(); i += 3) { + EXPECT_NE((int)pixels[i + 0], 0); + EXPECT_NE((int)pixels[i + 1], 0); + EXPECT_NE((int)pixels[i + 2], 0); + } +} + +TEST_F(RendererTest, OpengGlHeadless) { + Renderer renderer(nullptr, GraphicsMode::FilamentOpenGlHeadless); + renderer.Init(holder_->model()); + + gl::DriverType driver_type = gl::GetLoadedDriverType(); + #if TEST_HAS_GPU + ASSERT_EQ(driver_type, gl::DriverType::kEgl); + #else + ASSERT_EQ(driver_type, gl::DriverType::kOsMesa); + #endif + + std::vector pixels(width_ * height_ * 3); + renderer.Render(holder_->model(), holder_->data(), nullptr, nullptr, nullptr, + width_, height_, pixels); + // We set the clear color to white, but we don't know the exact color due to + // post processing, but it should definitely not be black. + for (int i = 0; i < pixels.size(); i += 3) { + EXPECT_NE((int)pixels[i + 0], 0); + EXPECT_NE((int)pixels[i + 1], 0); + EXPECT_NE((int)pixels[i + 2], 0); + } +} + +TEST_F(RendererTest, VulkanSoftware) { + Renderer renderer(nullptr, GraphicsMode::FilamentVulkanSoftware); + renderer.Init(holder_->model()); + + std::vector pixels(width_ * height_ * 3); + renderer.Render(holder_->model(), holder_->data(), nullptr, nullptr, nullptr, + width_, height_, pixels); + // We set the clear color to white, but we don't know the exact color due to + // post processing, but it should definitely not be black. + for (int i = 0; i < pixels.size(); i += 3) { + EXPECT_NE((int)pixels[i + 0], 0); + EXPECT_NE((int)pixels[i + 1], 0); + EXPECT_NE((int)pixels[i + 2], 0); + } +} + +TEST_F(RendererTest, VulkanHeadless) { + Renderer renderer(nullptr, GraphicsMode::FilamentVulkan); + renderer.Init(holder_->model()); + + std::vector pixels(width_ * height_ * 3); + renderer.Render(holder_->model(), holder_->data(), nullptr, nullptr, nullptr, + width_, height_, pixels); + // We set the clear color to white, but we don't know the exact color due to + // post processing, but it should definitely not be black. + for (int i = 0; i < pixels.size(); i += 3) { + EXPECT_NE((int)pixels[i + 0], 0); + EXPECT_NE((int)pixels[i + 1], 0); + EXPECT_NE((int)pixels[i + 2], 0); + } +} + +} // namespace +} // namespace mujoco::platform diff --git a/src/experimental/studio/app.cc b/src/experimental/studio/app.cc index f3cf2f62..1bed7a85 100644 --- a/src/experimental/studio/app.cc +++ b/src/experimental/studio/app.cc @@ -1712,6 +1712,11 @@ void App::MainMenuGui() { gfx_mode_ == platform::GraphicsMode::FilamentVulkan)) { mode = platform::GraphicsMode::FilamentVulkan; } + if (ImGui::MenuItem( + "Filament Vulkan Software", nullptr, + gfx_mode_ == platform::GraphicsMode::FilamentVulkanSoftware)) { + mode = platform::GraphicsMode::FilamentVulkanSoftware; + } if (mode.has_value()) { pending_op_ = [=, this]() { const int width = window_->GetWidth(); From e1b27afcdaa7838b25a9ec56fadefd8344d585de Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 7 May 2026 04:19:55 -0700 Subject: [PATCH 210/251] Merge math_util and model_util and rename to filament_util. Move filament_util to the main folder so as to indicate that these utilities can be used by anyone and aren't internal to the filament renderer implementation. PiperOrigin-RevId: 911884375 Change-Id: Ieb58184263bdac7e44a07f1e44a4649c68c6bf77 --- src/experimental/filament/CMakeLists.txt | 4 +- .../filament/compat/mjr_filament_renderer.cc | 5 +- .../filament/compat/model_objects.cc | 5 +- .../filament/compat/scene_bridge.cc | 7 +- .../filament/filament/builtins.cc | 2 +- src/experimental/filament/filament/light.cc | 2 +- .../filament/filament/material.cc | 4 +- src/experimental/filament/filament/mesh.cc | 2 +- .../filament/filament/model_util.h | 75 ------------------- .../filament/filament/renderable.cc | 2 +- .../filament/filament/renderable.h | 2 +- .../filament/filament/scene_view.cc | 5 +- .../math_util.cc => filament_util.cc} | 2 +- .../{filament/math_util.h => filament_util.h} | 53 ++++++++++++- 14 files changed, 67 insertions(+), 103 deletions(-) delete mode 100644 src/experimental/filament/filament/model_util.h rename src/experimental/filament/{filament/math_util.cc => filament_util.cc} (98%) rename src/experimental/filament/{filament/math_util.h => filament_util.h} (59%) diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index 20ae8c5e..cfa322c2 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -24,6 +24,7 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} render_context_filament.h render_context_filament.cc render_context_filament_cpp.h + filament_util.h filament/builtins.cc filament/builtins.h filament/color_grading_options.cc @@ -37,11 +38,8 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} filament/light.h filament/material.cc filament/material.h - filament/math_util.cc - filament/math_util.h filament/mesh.cc filament/mesh.h - filament/model_util.h filament/object_manager.cc filament/object_manager.h filament/render_target.cc diff --git a/src/experimental/filament/compat/mjr_filament_renderer.cc b/src/experimental/filament/compat/mjr_filament_renderer.cc index c56731d8..d43153d8 100644 --- a/src/experimental/filament/compat/mjr_filament_renderer.cc +++ b/src/experimental/filament/compat/mjr_filament_renderer.cc @@ -18,17 +18,14 @@ #include #include -#include -#include #include #include #include #include "experimental/filament/compat/imgui_bridge.h" #include "experimental/filament/compat/scene_bridge.h" #include "experimental/filament/filament/filament_context.h" -#include "experimental/filament/filament/model_util.h" -#include "experimental/filament/render_context_filament.h" #include "experimental/filament/render_context_filament_cpp.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { diff --git a/src/experimental/filament/compat/model_objects.cc b/src/experimental/filament/compat/model_objects.cc index 2fd77a75..e1c33456 100644 --- a/src/experimental/filament/compat/model_objects.cc +++ b/src/experimental/filament/compat/model_objects.cc @@ -29,10 +29,9 @@ #include #include #include -#include "experimental/filament/filament/math_util.h" -#include "experimental/filament/filament/model_util.h" -#include "experimental/filament/render_context_filament.h" +#include "experimental/filament/filament_util.h" #include "experimental/filament/render_context_filament_cpp.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { diff --git a/src/experimental/filament/compat/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc index d78dc09c..16c8e26e 100644 --- a/src/experimental/filament/compat/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -23,16 +23,15 @@ #include #include #include +#include #include #include -#include #include #include "experimental/filament/compat/model_objects.h" #include "experimental/filament/compat/scene_geom_util.h" -#include "experimental/filament/filament/math_util.h" -#include "experimental/filament/filament/model_util.h" -#include "experimental/filament/render_context_filament.h" +#include "experimental/filament/filament_util.h" #include "experimental/filament/render_context_filament_cpp.h" +#include "experimental/filament/render_context_filament.h" namespace mujoco { diff --git a/src/experimental/filament/filament/builtins.cc b/src/experimental/filament/filament/builtins.cc index 28468e50..b18e28e5 100644 --- a/src/experimental/filament/filament/builtins.cc +++ b/src/experimental/filament/filament/builtins.cc @@ -26,7 +26,7 @@ #include #include #include -#include "experimental/filament/filament/math_util.h" +#include "experimental/filament/filament_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/render_context_filament.h" diff --git a/src/experimental/filament/filament/light.cc b/src/experimental/filament/filament/light.cc index a4cc0ed1..396aee42 100644 --- a/src/experimental/filament/filament/light.cc +++ b/src/experimental/filament/filament/light.cc @@ -25,8 +25,8 @@ #include #include #include +#include "experimental/filament/filament_util.h" #include "experimental/filament/filament/filament_context.h" -#include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" diff --git a/src/experimental/filament/filament/material.cc b/src/experimental/filament/filament/material.cc index 925e88fb..672f7a40 100644 --- a/src/experimental/filament/filament/material.cc +++ b/src/experimental/filament/filament/material.cc @@ -20,9 +20,9 @@ #include #include #include -#include "experimental/filament/filament/math_util.h" -#include "experimental/filament/filament/texture.h" +#include "experimental/filament/filament_util.h" #include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { diff --git a/src/experimental/filament/filament/mesh.cc b/src/experimental/filament/filament/mesh.cc index 9c06cfc7..9d08789e 100644 --- a/src/experimental/filament/filament/mesh.cc +++ b/src/experimental/filament/filament/mesh.cc @@ -32,7 +32,7 @@ #include #include #include -#include "experimental/filament/filament/math_util.h" +#include "experimental/filament/filament_util.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { diff --git a/src/experimental/filament/filament/model_util.h b/src/experimental/filament/filament/model_util.h deleted file mode 100644 index 06501ae7..00000000 --- a/src/experimental/filament/filament/model_util.h +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2025 DeepMind Technologies Limited -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_UTIL_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_UTIL_H_ - -#include - -#include -#include -#include -#include -#include - -namespace mujoco { - -// Reads a value with the given name from the mjModel's data sections. The -// default_value is returned if the named element is not found. -template -T ReadElement(const mjModel* model, const char* name, T default_value = T()) { - constexpr bool is_string = - std::is_same_v || std::is_same_v; - - const int type = is_string ? mjOBJ_TEXT : mjOBJ_NUMERIC; - const int id = mj_name2id(model, type, name); - if (id < 0) { - return default_value; - } - - if constexpr (std::is_same_v) { - const char* ptr = model->text_data + model->text_adr[id]; - return ptr; - } else if constexpr (std::is_same_v) { - const char* ptr = model->text_data + model->text_adr[id]; - // Do not include the null terminator in the string view. - return std::string_view(ptr, model->text_size[id] - 1); - } else if constexpr (std::is_arithmetic_v) { - const mjtNum* ptr = model->numeric_data + model->numeric_adr[id]; - return static_cast(*ptr); - } else if constexpr (std::is_enum_v) { - const mjtNum* ptr = model->numeric_data + model->numeric_adr[id]; - return static_cast(static_cast(*ptr)); - } else if constexpr (std::is_same_v) { - const mjtNum* ptr = model->numeric_data + model->numeric_adr[id]; - if (model->numeric_size[id] != 2) mju_error("Invalid numeric size."); - return T{ptr[0], ptr[1]}; - } else if constexpr (std::is_same_v) { - const mjtNum* ptr = model->numeric_data + model->numeric_adr[id]; - if (model->numeric_size[id] != 3) mju_error("Invalid numeric size."); - return T{ptr[0], ptr[1], ptr[2]}; - } else if constexpr (std::is_same_v) { - const mjtNum* ptr = model->numeric_data + model->numeric_adr[id]; - if (model->numeric_size[id] != 4) mju_error("Invalid numeric size."); - return T{ptr[0], ptr[1], ptr[2], ptr[3]}; - } else if constexpr (std::is_same_v) { - const mjtNum* ptr = model->numeric_data + model->numeric_adr[id]; - return static_cast(*ptr != 0); - } - return default_value; -} - -} // namespace mujoco - -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MODEL_UTIL_H_ diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 8f4af490..bfa0a57f 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -29,10 +29,10 @@ #include #include #include +#include "experimental/filament/filament_util.h" #include "experimental/filament/filament/builtins.h" #include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/material.h" -#include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/filament/texture.h" diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 3d41b2a1..58310d98 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -24,8 +24,8 @@ #include #include #include +#include "experimental/filament/filament_util.h" #include "experimental/filament/filament/filament_context.h" -#include "experimental/filament/filament/math_util.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/render_context_filament.h" diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index e97395b7..c3f132ab 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -35,16 +35,15 @@ #include #include #include +#include #include #include -#include #include #include +#include "experimental/filament/filament_util.h" #include "experimental/filament/filament/color_grading_options.h" #include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/light.h" -#include "experimental/filament/filament/math_util.h" -#include "experimental/filament/filament/model_util.h" #include "experimental/filament/filament/render_target.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/texture.h" diff --git a/src/experimental/filament/filament/math_util.cc b/src/experimental/filament/filament_util.cc similarity index 98% rename from src/experimental/filament/filament/math_util.cc rename to src/experimental/filament/filament_util.cc index 34e0cb24..bd092a69 100644 --- a/src/experimental/filament/filament/math_util.cc +++ b/src/experimental/filament/filament_util.cc @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "experimental/filament/filament/math_util.h" +#include "experimental/filament/filament_util.h" #include #include diff --git a/src/experimental/filament/filament/math_util.h b/src/experimental/filament/filament_util.h similarity index 59% rename from src/experimental/filament/filament/math_util.h rename to src/experimental/filament/filament_util.h index 5438b5e4..39c4c640 100644 --- a/src/experimental/filament/filament/math_util.h +++ b/src/experimental/filament/filament_util.h @@ -12,14 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MATH_UTIL_H_ -#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MATH_UTIL_H_ +#ifndef MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_UTIL_H_ +#define MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_UTIL_H_ #include #include #include #include #include +#include +#include namespace mujoco { @@ -96,6 +98,51 @@ filament::math::float4 CalculateOrientation( const filament::math::float3& p2, const filament::math::float3& p3); +// Reads a value with the given name from the mjModel's data sections. The +// default_value is returned if the named element is not found. +template +T ReadElement(const mjModel* model, const char* name, T default_value = T()) { + constexpr bool is_string = + std::is_same_v || std::is_same_v; + + const int type = is_string ? mjOBJ_TEXT : mjOBJ_NUMERIC; + const int id = mj_name2id(model, type, name); + if (id < 0) { + return default_value; + } + + if constexpr (std::is_same_v) { + const char* ptr = model->text_data + model->text_adr[id]; + return ptr; + } else if constexpr (std::is_same_v) { + const char* ptr = model->text_data + model->text_adr[id]; + // Do not include the null terminator in the string view. + return std::string_view(ptr, model->text_size[id] - 1); + } else if constexpr (std::is_arithmetic_v) { + const mjtNum* ptr = model->numeric_data + model->numeric_adr[id]; + return static_cast(*ptr); + } else if constexpr (std::is_enum_v) { + const mjtNum* ptr = model->numeric_data + model->numeric_adr[id]; + return static_cast(static_cast(*ptr)); + } else if constexpr (std::is_same_v) { + const mjtNum* ptr = model->numeric_data + model->numeric_adr[id]; + if (model->numeric_size[id] != 2) mju_error("Invalid numeric size."); + return T{ptr[0], ptr[1]}; + } else if constexpr (std::is_same_v) { + const mjtNum* ptr = model->numeric_data + model->numeric_adr[id]; + if (model->numeric_size[id] != 3) mju_error("Invalid numeric size."); + return T{ptr[0], ptr[1], ptr[2]}; + } else if constexpr (std::is_same_v) { + const mjtNum* ptr = model->numeric_data + model->numeric_adr[id]; + if (model->numeric_size[id] != 4) mju_error("Invalid numeric size."); + return T{ptr[0], ptr[1], ptr[2], ptr[3]}; + } else if constexpr (std::is_same_v) { + const mjtNum* ptr = model->numeric_data + model->numeric_adr[id]; + return static_cast(*ptr != 0); + } + return default_value; +} + } // namespace mujoco -#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_MATH_UTIL_H_ +#endif // MUJOCO_SRC_EXPERIMENTAL_FILAMENT_FILAMENT_UTIL_H_ From f2da6cabdf224ed36d4eab2e5b343306dc3220b8 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 7 May 2026 04:56:50 -0700 Subject: [PATCH 211/251] Add API function for querying frame stats. Replaces the current GetFrameRate function. PiperOrigin-RevId: 911897360 Change-Id: I4199ba9ce40d1d8bc37b551fa7058d964186641d --- .../filament/compat/mjr_filament_renderer.cc | 7 +++++++ .../filament/compat/mjr_filament_renderer.h | 4 +--- .../filament/filament/filament_context.cc | 12 +++++++----- .../filament/filament/filament_context.h | 4 ++-- .../filament/render_context_filament.cc | 9 +++++++++ src/experimental/filament/render_context_filament.h | 13 +++++++++++++ 6 files changed, 39 insertions(+), 10 deletions(-) diff --git a/src/experimental/filament/compat/mjr_filament_renderer.cc b/src/experimental/filament/compat/mjr_filament_renderer.cc index d43153d8..f01d3f5a 100644 --- a/src/experimental/filament/compat/mjr_filament_renderer.cc +++ b/src/experimental/filament/compat/mjr_filament_renderer.cc @@ -181,6 +181,13 @@ uintptr_t MjrFilamentRenderer::UploadGuiImage(uintptr_t tex_id, return imgui_bridge_->UploadImage(tex_id, pixels, width, height, bpp); } +double MjrFilamentRenderer::GetFrameRate() const { + mjrFrameStats stats; + mjr_defaultFrameStats(&stats); + filament_context_->GetFrameStats(0, &stats); + return stats.frame_rate; +} + void MjrFilamentRenderer::UpdateGui() { mjrf_DEBUG_drawImguiEditor(scene_bridge_->GetScene()); } diff --git a/src/experimental/filament/compat/mjr_filament_renderer.h b/src/experimental/filament/compat/mjr_filament_renderer.h index f1f2f69d..d986a16d 100644 --- a/src/experimental/filament/compat/mjr_filament_renderer.h +++ b/src/experimental/filament/compat/mjr_filament_renderer.h @@ -65,9 +65,7 @@ class MjrFilamentRenderer { // Renders an ImGui window containing Filament-specific editor UI. void UpdateGui(); - double GetFrameRate() const { - return filament_context_->GetFrameRate(); - } + double GetFrameRate() const; MjrFilamentRenderer(const MjrFilamentRenderer&) = delete; MjrFilamentRenderer& operator=(const MjrFilamentRenderer&) = delete; diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index 3166bea8..e55db060 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -183,14 +183,16 @@ void FilamentContext::SetClearColor(const filament::math::float4& color) { renderer_->setClearOptions(opts); } -double FilamentContext::GetFrameRate() const { +void FilamentContext::GetFrameStats(mjrFrameHandle frame, + mjrFrameStats* stats_out) const { utils::FixedCapacityVector frame_info = renderer_->getFrameInfoHistory(1); - if (frame_info.empty()) { - return 0; + if (!frame_info.empty()) { + const int64_t ns = frame_info[0].denoisedGpuFrameDuration; + stats_out->frame_rate = 1.0e9 / static_cast(ns); + } else { + stats_out->frame_rate = 0.0; } - const int64_t ns = frame_info[0].denoisedGpuFrameDuration; - return 1.0e9 / static_cast(ns); } } // namespace mujoco diff --git a/src/experimental/filament/filament/filament_context.h b/src/experimental/filament/filament/filament_context.h index 732229e0..db817395 100644 --- a/src/experimental/filament/filament/filament_context.h +++ b/src/experimental/filament/filament/filament_context.h @@ -52,8 +52,8 @@ class FilamentContext : public mjrfContext { // Sets the clear color for the renderer. void SetClearColor(const filament::math::float4& color); - // Returns the current frame rate of the renderer. - double GetFrameRate() const; + // Returns information about the frame. + void GetFrameStats(mjrFrameHandle frame, mjrFrameStats* stats_out) const; filament::Engine* GetEngine() const { return engine_; } diff --git a/src/experimental/filament/render_context_filament.cc b/src/experimental/filament/render_context_filament.cc index 938f4d19..eeecf054 100644 --- a/src/experimental/filament/render_context_filament.cc +++ b/src/experimental/filament/render_context_filament.cc @@ -145,6 +145,10 @@ void mjr_defaultReadPixelsRequest(mjrReadPixelsRequest* request) { memset(request, 0, sizeof(mjrReadPixelsRequest)); } +void mjr_defaultFrameStats(mjrFrameStats* stats) { + memset(stats, 0, sizeof(mjrFrameStats)); +} + mjrfContext* mjrf_createContext(const mjrFilamentConfig* config) { return new mujoco::FilamentContext(config); } @@ -355,6 +359,11 @@ void mjrf_waitForFrame(mjrfContext* ctx, mjrFrameHandle frame) { mujoco::FilamentContext::downcast(ctx)->WaitForFrame(frame); } +void mjrf_getFrameStats(mjrfContext* ctx, mjrFrameHandle frame, + mjrFrameStats* stats_out) { + mujoco::FilamentContext::downcast(ctx)->GetFrameStats(frame, stats_out); +} + // Legacy API, to be deprecated. void mjrf_makeFilamentContext(const mjModel* m, mjrContext* con, diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index 29c403de..5cd7faf2 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -406,6 +406,15 @@ struct mjrReadPixelsRequest { // Initializes the mjrReadPixelsRequest to default values. void mjr_defaultReadPixelsRequest(mjrReadPixelsRequest* request); +// Information about a single frame of rendering. +struct mjrFrameStats { + // The frame rate of the renderer, in frames per second. + double frame_rate; +}; + +// Initializes the mjrFrameStats to default values. +void mjr_defaultFrameStats(mjrFrameStats* stats); + // Configuration parameters for the filament rendering context. struct mjrFilamentConfig { // The native window handle into which we can render directly. @@ -562,6 +571,10 @@ mjrFrameHandle mjrf_render(mjrfContext* ctx, const mjrRenderRequest* req, // Waits for the rendering to complete for the given frame handle. void mjrf_waitForFrame(mjrfContext* ctx, mjrFrameHandle frame); +// Returns the stats for the given frame but updating the given `stats_out`. +void mjrf_getFrameStats(mjrfContext* ctx, mjrFrameHandle frame, + mjrFrameStats* stats_out); + // Draws an ImGui editor for the given scene, exposing filament-specific // settings. void mjrf_DEBUG_drawImguiEditor(mjrScene* scene); From 034167f4cd901deb5769ccff28713c86a4ead008 Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Thu, 7 May 2026 05:27:59 -0700 Subject: [PATCH 212/251] Fix MjVfs changelog item accidentally placed in last release PiperOrigin-RevId: 911908806 Change-Id: I44514152c2084ca39d8ef218d9f00de66448c3ac --- doc/changelog.rst | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 466c02fc..b99151a1 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -40,6 +40,13 @@ Python ^^^^^^ - Added ``MjSpec.encode`` method, wrapping :ref:`mj_encode`. +- Added ``mujoco.MjVfs`` Python binding to interact with the Virtual File System directly from Python. + See :ref:`Virtual File System ` for usage details. + + .. warning:: + The previous way of passing assets via a dictionary mapping asset names to bytes is **deprecated** and will be + removed in an upcoming release. You cannot specify both the ``assets`` dictionary and the ``vfs`` argument at the same + time. ``MjVfs`` should be used as a drop-in replacement. Version 3.8.0 (April 24, 2026) ------------------------------ @@ -77,19 +84,6 @@ Bug fixes than the parent spec. This prevents the origin of the parent spec to affect the resolution of asset paths in the child spec. -Python -^^^^^^ - -- Added ``mujoco.MjVfs`` Python binding to interact with the Virtual File System directly from Python. - See :ref:`Virtual File System ` for usage details. - - .. warning:: - The previous way of passing assets via a dictionary mapping asset names to bytes is **deprecated** and will be - removed in an upcoming release. You cannot specify both the ``assets`` dictionary and the ``vfs`` argument at the same - time. ``MjVfs`` should be used as a drop-in replacement. - - - Version 3.7.0 (April 14, 2026) ------------------------------ From 75f1b81df769d92f058787895d7dfc0b9664fd65 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 7 May 2026 05:42:19 -0700 Subject: [PATCH 213/251] Missing file in CMakeLists.txt PiperOrigin-RevId: 911913781 Change-Id: I080298ff26fa2d7a3646f80ed15396bbe868c76b --- src/experimental/filament/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index cfa322c2..37105703 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -25,6 +25,7 @@ target_sources(${MUJOCO_FILAMENT_TARGET_NAME} render_context_filament.cc render_context_filament_cpp.h filament_util.h + filament_util.cc filament/builtins.cc filament/builtins.h filament/color_grading_options.cc From 364c716f088c38b7298a150a6253e6394e198fd9 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 7 May 2026 05:55:38 -0700 Subject: [PATCH 214/251] Rename TextureTarget to SamplerType. PiperOrigin-RevId: 911918372 Change-Id: If711d5909499b68316acd10a2de3a2d83436e18d --- src/experimental/filament/compat/imgui_bridge.cc | 4 ++-- src/experimental/filament/compat/model_objects.cc | 2 +- src/experimental/filament/compat/scene_bridge.cc | 2 +- src/experimental/filament/compat/scene_geom_util.cc | 2 +- src/experimental/filament/filament/render_target.cc | 4 ++-- src/experimental/filament/filament/renderable.cc | 2 +- src/experimental/filament/filament/texture.cc | 3 ++- src/experimental/filament/filament/texture.h | 2 +- src/experimental/filament/render_context_filament.cc | 4 ++-- src/experimental/filament/render_context_filament.h | 8 ++++---- 10 files changed, 17 insertions(+), 16 deletions(-) diff --git a/src/experimental/filament/compat/imgui_bridge.cc b/src/experimental/filament/compat/imgui_bridge.cc index 9af52b74..f7fbdd65 100644 --- a/src/experimental/filament/compat/imgui_bridge.cc +++ b/src/experimental/filament/compat/imgui_bridge.cc @@ -84,7 +84,7 @@ uintptr_t ImguiBridge::UploadImage(uintptr_t tex_id, const uint8_t* pixels, mjr_defaultTextureConfig(&config); config.width = width; config.height = height; - config.target = mjTEXTURE_2D; + config.sampler_type = mjTEXTURE_2D; config.format = bpp == 4 ? mjPIXEL_FORMAT_RGBA8 : mjPIXEL_FORMAT_RGB8; config.color_space = mjCOLORSPACE_LINEAR; UniquePtr new_texture = ::mujoco::CreateTexture(ctx_, config); @@ -120,7 +120,7 @@ void ImguiBridge::CreateTexture(ImTextureData* data) { mjr_defaultTextureConfig(&config); config.width = data->Width; config.height = data->Height; - config.target = mjTEXTURE_2D; + config.sampler_type = mjTEXTURE_2D; config.format = mjPIXEL_FORMAT_RGBA8; config.color_space = mjCOLORSPACE_LINEAR; diff --git a/src/experimental/filament/compat/model_objects.cc b/src/experimental/filament/compat/model_objects.cc index e1c33456..a619e73d 100644 --- a/src/experimental/filament/compat/model_objects.cc +++ b/src/experimental/filament/compat/model_objects.cc @@ -545,7 +545,7 @@ void ModelObjects::UploadTexture(const mjModel* model, int id) { mjr_defaultTextureConfig(&config); config.width = model->tex_width[id]; config.height = model->tex_height[id]; - config.target = (mjtTexture)model->tex_type[id]; + config.sampler_type = (mjtTexture)model->tex_type[id]; config.color_space = (mjtColorSpace)model->tex_colorspace[id]; switch (model->tex_nchannel[id]) { case 1: diff --git a/src/experimental/filament/compat/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc index 16c8e26e..fdc69936 100644 --- a/src/experimental/filament/compat/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -58,7 +58,7 @@ static UniquePtr CreateFallbackIndirectLightTexture( mjr_defaultTextureConfig(&config); config.width = 1; config.height = 1; - config.target = mjTEXTURE_CUBE; + config.sampler_type = mjTEXTURE_CUBE; config.format = mjPIXEL_FORMAT_KTX; config.color_space = mjCOLORSPACE_AUTO; diff --git a/src/experimental/filament/compat/scene_geom_util.cc b/src/experimental/filament/compat/scene_geom_util.cc index 35c6f380..d255583a 100644 --- a/src/experimental/filament/compat/scene_geom_util.cc +++ b/src/experimental/filament/compat/scene_geom_util.cc @@ -259,7 +259,7 @@ static void UpdateGeomMaterial(mjrRenderable* renderable, const mjvGeom& geom, // the programmatic UVs. if (textures.color) { - if (mjrf_getTextureTarget(textures.color) == mjTEXTURE_2D) { + if (mjrf_getSamplerType(textures.color) == mjTEXTURE_2D) { // For 2D textures, `tex_repeat` specifies how many times the texture // image is repeated. The `tex_uniform` flag determines if the repetition // is applied at in object space (false) or in world space (true). diff --git a/src/experimental/filament/filament/render_target.cc b/src/experimental/filament/filament/render_target.cc index fa9bbfb9..ed9b7fe4 100644 --- a/src/experimental/filament/filament/render_target.cc +++ b/src/experimental/filament/filament/render_target.cc @@ -61,7 +61,7 @@ void RenderTarget::Prepare(int width, int height) { Texture::InternalFlags color_flags; color_config.width = width; color_config.height = height; - color_config.target = mjTEXTURE_2D; + color_config.sampler_type = mjTEXTURE_2D; color_config.format = config_.color_format; color_config.color_space = mjCOLORSPACE_LINEAR; color_config.format = mjPIXEL_FORMAT_RGB8; @@ -73,7 +73,7 @@ void RenderTarget::Prepare(int width, int height) { Texture::InternalFlags depth_flags; depth_config.width = width; depth_config.height = height; - depth_config.target = mjTEXTURE_2D; + depth_config.sampler_type = mjTEXTURE_2D; depth_config.format = config_.depth_format; depth_config.color_space = mjCOLORSPACE_LINEAR; depth_config.format = mjPIXEL_FORMAT_DEPTH32F; diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index bfa0a57f..ad729b46 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -386,7 +386,7 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { } else { return ObjectManager::kPhongColor; } - } else if (color_texture->GetTarget() == mjTEXTURE_CUBE) { + } else if (color_texture->GetSamplerType() == mjTEXTURE_CUBE) { if (material_params_.color[3] < 1.0f) { return ObjectManager::kPhongCubeFade; } else if (material_params_.reflective) { diff --git a/src/experimental/filament/filament/texture.cc b/src/experimental/filament/filament/texture.cc index 0d7b29ef..de75a812 100644 --- a/src/experimental/filament/filament/texture.cc +++ b/src/experimental/filament/filament/texture.cc @@ -35,7 +35,8 @@ static bool IsCompressed(const mjrTextureConfig& config) { } static bool IsCubeMap(const mjrTextureConfig& config) { - return config.target == mjTEXTURE_CUBE || config.target == mjTEXTURE_SKYBOX; + return config.sampler_type == mjTEXTURE_CUBE || + config.sampler_type == mjTEXTURE_SKYBOX; } static int GetFaceHeight(const mjrTextureConfig& config) { diff --git a/src/experimental/filament/filament/texture.h b/src/experimental/filament/filament/texture.h index bfe4fbf6..9947395c 100644 --- a/src/experimental/filament/filament/texture.h +++ b/src/experimental/filament/filament/texture.h @@ -52,7 +52,7 @@ class Texture : public mjrTexture { int GetHeight() const { return config_.height; } // Returns the target of the texture. - mjrTextureTarget GetTarget() const { return config_.target; } + mjrSamplerType GetSamplerType() const { return config_.sampler_type; } // Returns the underlying filament texture. filament::Texture* GetFilamentTexture() const { return texture_; } diff --git a/src/experimental/filament/render_context_filament.cc b/src/experimental/filament/render_context_filament.cc index eeecf054..c58a3b2d 100644 --- a/src/experimental/filament/render_context_filament.cc +++ b/src/experimental/filament/render_context_filament.cc @@ -221,8 +221,8 @@ int mjrf_getTextureHeight(const mjrTexture* texture) { return mujoco::Texture::downcast(texture)->GetHeight(); } -mjrTextureTarget mjrf_getTextureTarget(const mjrTexture* texture) { - return mujoco::Texture::downcast(texture)->GetTarget(); +mjrSamplerType mjrf_getSamplerType(const mjrTexture* texture) { + return mujoco::Texture::downcast(texture)->GetSamplerType(); } void mjrf_setLightEnabled(mjrLight* light, bool enabled) { diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index 5cd7faf2..03cbecbe 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -119,7 +119,7 @@ typedef enum mjrGraphicsApi_ { // backend graphics API to use typedef std::uint64_t mjrFrameHandle; // Bring some legacy mjt types into the mjr namespace. -typedef mjtTexture mjrTextureTarget; +typedef mjtTexture mjrSamplerType; typedef mjtColorSpace mjrColorSpace; typedef mjtLightType mjrLightType; typedef mjvGLCamera mjrCamera; @@ -191,7 +191,7 @@ struct mjrTextureConfig { int height; // The target of the texture (e.g. 2D, cube, etc.) - mjrTextureTarget target; + mjrSamplerType sampler_type; // The format of the pixels in the texture (e.g. RGB8, RGBA8, KTX, etc.) mjrPixelFormat format; @@ -486,8 +486,8 @@ int mjrf_getTextureWidth(const mjrTexture* texture); // Returns the height of the texture. int mjrf_getTextureHeight(const mjrTexture* texture); -// Returns the target type of the texture. -mjrTextureTarget mjrf_getTextureTarget(const mjrTexture* texture); +// Returns the sampler type of the texture. +mjrSamplerType mjrf_getSamplerType(const mjrTexture* texture); // Enables or disables the light. void mjrf_setLightEnabled(mjrLight* light, bool enabled); From 8b06fe4c164ea61ec59ac216015f8d12f896b553 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 7 May 2026 06:12:34 -0700 Subject: [PATCH 215/251] Remove invalid dlfcn.h include. PiperOrigin-RevId: 911924226 Change-Id: Id5c05745ee8dbdd4e5518e8c6ff5feb0b9972bb5 --- src/experimental/filament/filament/filament_platform_factory.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/experimental/filament/filament/filament_platform_factory.cc b/src/experimental/filament/filament/filament_platform_factory.cc index 9d9863cf..a7dc9709 100644 --- a/src/experimental/filament/filament/filament_platform_factory.cc +++ b/src/experimental/filament/filament/filament_platform_factory.cc @@ -14,7 +14,6 @@ #include "experimental/filament/filament/filament_platform_factory.h" -#include #include #include From ec50260e265a5b188e899c02614e036902bb8b38 Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Thu, 7 May 2026 06:32:54 -0700 Subject: [PATCH 216/251] Add mjs_getOriginSpec to retrieve the original spec of an element. The new function mjs_getOriginSpec returns the mjSpec that was used to define a given mjsElement. Unlike mjs_getSpec, this value remains constant even after the element has been attached to a different model. PiperOrigin-RevId: 911930951 Change-Id: Ia9cd79d9dfabc513d6121eadcffc5d41075224c9 --- doc/APIreference/functions.rst | 10 ++++++++ doc/changelog.rst | 3 +++ doc/includes/references.h | 1 + include/mujoco/mujoco.h | 4 ++++ python/mujoco/introspect/functions.py | 16 +++++++++++++ src/user/user_api.cc | 10 ++++++++ src/user/user_api.h | 4 ++++ test/user/user_api_test.cc | 33 +++++++++++++++++++++++++++ wasm/codegen/generated/bindings.cc | 9 ++++++++ 9 files changed, 90 insertions(+) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index a91344ac..fc8a07c4 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -4783,6 +4783,16 @@ Find and get utilities Get spec from body. +.. _mjs_getOriginSpec: + +`mjs_getOriginSpec <#mjs_getOriginSpec>`__ +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mjs_getOriginSpec + +get spec that originally defined an element +contrary to mjs_getSpec, this does not change after attachment + .. _mjs_getCompiler: `mjs_getCompiler <#mjs_getCompiler>`__ diff --git a/doc/changelog.rst b/doc/changelog.rst index b99151a1..50f0e2b1 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -19,6 +19,9 @@ General energy gain in the presence of contacts and in fluid media. - Added :ref:`mju_sym2dense`, converting a lower-triangular, implicitly symmetric CSR matrix to a dense symmetric matrix. The inertia matrix ``mjData.M`` is an example of such a matrix. +- Added :ref:`mjs_getOriginSpec`, returning the spec that originally defined an element, prior to attachment. This is in + contrast to :ref:`mjs_getSpec` which returns the spec currently owning the element. If the element is not the result + of an attach operation, the functions are identical. .. admonition:: Future breaking API changes :class: warning diff --git a/doc/includes/references.h b/doc/includes/references.h index c96db9b3..6342e21b 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -3687,6 +3687,7 @@ mjsTexture* mjs_addTexture(mjSpec* s); mjsMaterial* mjs_addMaterial(mjSpec* s, const mjsDefault* def); int mjs_makeMesh(mjsMesh* mesh, mjtMeshBuiltin builtin, double* params, int nparams); mjSpec* mjs_getSpec(const mjsElement* element); +mjSpec* mjs_getOriginSpec(const mjsElement* element); mjsCompiler* mjs_getCompiler(const mjsElement* element); mjSpec* mjs_findSpec(const mjSpec* spec, const char* name); mjsBody* mjs_findBody(const mjSpec* s, const char* name); diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index c9600ba6..682bb87e 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -1776,6 +1776,10 @@ MJAPI int mjs_makeMesh(mjsMesh* mesh, mjtMeshBuiltin builtin, double* params, in // Get spec from body. MJAPI mjSpec* mjs_getSpec(const mjsElement* element); +// get spec that originally defined an element +// contrary to mjs_getSpec, this does not change after attachment +MJAPI mjSpec* mjs_getOriginSpec(const mjsElement* element); + // Get compiler associated with element's origin spec. MJAPI mjsCompiler* mjs_getCompiler(const mjsElement* element); diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index 0bfc82b9..882c323f 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -11126,6 +11126,22 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Get spec from body.', )), + ('mjs_getOriginSpec', + FunctionDecl( + name='mjs_getOriginSpec', + return_type=PointerType( + inner_type=ValueType(name='mjSpec'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement', is_const=True), + ), + ), + ), + doc='get spec that originally defined an element contrary to mjs_getSpec, this does not change after attachment', # pylint: disable=line-too-long + )), ('mjs_getCompiler', FunctionDecl( name='mjs_getCompiler', diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 5b91a772..fe25fae2 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -1308,6 +1308,16 @@ mjSpec* mjs_getSpec(const mjsElement* element) { +// get spec that originally defined an element +// contrary to mjs_getSpec, this does not change after attachment +mjSpec* mjs_getOriginSpec(const mjsElement* element) { + const mjCModel* model = static_cast(element)->model; + const mjsCompiler* compiler = static_cast(element)->compiler; + return model->FindSpec(compiler); +} + + + mjsCompiler* mjs_getCompiler(const mjsElement* element) { return static_cast(element)->compiler; } diff --git a/src/user/user_api.h b/src/user/user_api.h index 878f9045..f1f216a7 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -224,6 +224,10 @@ MJAPI int mjs_makeMesh(mjsMesh* mesh, mjtMeshBuiltin builtin, double* params, in // Get spec from body. MJAPI mjSpec* mjs_getSpec(const mjsElement* element); +// get spec that originally defined an element +// contrary to mjs_getSpec, this does not change after attachment +MJAPI mjSpec* mjs_getOriginSpec(const mjsElement* element); + // Find spec (model asset) by name. MJAPI mjSpec* mjs_findSpec(const mjSpec* spec, const char* name); diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 57e889bb..b26ebf87 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -205,6 +205,39 @@ TEST_F(MujocoTest, AttachAndChildDeletion) { mj_deleteSpec(parent_spec); } +TEST_F(MujocoTest, OriginSpecInvariantToAttachment) { + mjSpec* child_spec = mj_makeSpec(); + mjsBody* child_world = mjs_findBody(child_spec, "world"); + mjsBody* child_body = mjs_addBody(child_world, 0); + mjsJoint* freejoint = mjs_addJoint(child_body, 0); + freejoint->type = mjJNT_FREE; + mjs_setName(freejoint->element, "child_freejoint"); + + mjSpec* parent_spec = mj_makeSpec(); + mjsBody* parent_world = mjs_findBody(parent_spec, "world"); + mjsBody* parent_body = mjs_addBody(parent_world, 0); + mjs_setName(parent_body->element, "parent_body"); + + // Attach child spec to parent_body + mjsElement* attached = + mjs_attach(parent_body->element, child_spec->element, "pre_", ""); + ASSERT_THAT(attached, NotNull()); + + // The freejoint should still be in parent_spec because deletion failed + mjsElement* child_spec_joint = + mjs_findElement(parent_spec, mjOBJ_JOINT, "pre_child_freejoint"); + EXPECT_EQ(mjs_getSpec(child_spec_joint), parent_spec); + EXPECT_EQ(mjs_getOriginSpec(child_spec_joint), child_spec); + + mjsElement* parent_spec_body = + mjs_findElement(parent_spec, mjOBJ_BODY, "parent_body"); + EXPECT_EQ(mjs_getOriginSpec(parent_spec_body), parent_spec); + + + mj_deleteSpec(child_spec); + mj_deleteSpec(parent_spec); +} + int open_mock(mjResource* resource) { static const char parent_xml[] = R"( diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index bbd29e12..324092ef 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -9793,6 +9793,14 @@ std::string mjs_getName_wrapper(MjsElement& element) { return *mjs_getName(element.get()); } +std::optional mjs_getOriginSpec_wrapper(const MjsElement& element) { + mjSpec* result = mjs_getOriginSpec(element.get()); + if (result == nullptr) { + return std::nullopt; + } + return MjSpec(result); +} + std::optional mjs_getParent_wrapper(const MjsElement& element) { mjsBody* result = mjs_getParent(element.get()); if (result == nullptr) { @@ -13343,6 +13351,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { function("mjs_getFrame", &mjs_getFrame_wrapper); function("mjs_getId", &mjs_getId_wrapper); function("mjs_getName", &mjs_getName_wrapper); + function("mjs_getOriginSpec", &mjs_getOriginSpec_wrapper); function("mjs_getParent", &mjs_getParent_wrapper); function("mjs_getSpec", &mjs_getSpec_wrapper); function("mjs_getSpecDefault", &mjs_getSpecDefault_wrapper); From d5c5a989f21eaabc06ec9753d4a1804ff02936b5 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 7 May 2026 07:06:02 -0700 Subject: [PATCH 217/251] Use viewport instead of width/height. PiperOrigin-RevId: 911942457 Change-Id: I2d6d52a8bb6b1b10d69956cce9e5d7f62e7d1f1c --- .../filament/compat/mjr_filament_renderer.cc | 12 ++++-------- .../filament/filament/filament_context.cc | 7 ++++--- src/experimental/filament/filament/scene_view.cc | 4 ++-- src/experimental/filament/render_context_filament.h | 5 ++--- 4 files changed, 12 insertions(+), 16 deletions(-) diff --git a/src/experimental/filament/compat/mjr_filament_renderer.cc b/src/experimental/filament/compat/mjr_filament_renderer.cc index f01d3f5a..b94fa596 100644 --- a/src/experimental/filament/compat/mjr_filament_renderer.cc +++ b/src/experimental/filament/compat/mjr_filament_renderer.cc @@ -54,15 +54,13 @@ void MjrFilamentRenderer::Render(const mjrRect& viewport, reqs[0].scene = scene_bridge_->GetScene(); reqs[0].draw_mode = scene_bridge_->GetDrawMode(); reqs[0].camera = scene_bridge_->GetCamera(); - reqs[0].width = viewport.width; - reqs[0].height = viewport.height; + reqs[0].viewport = viewport; mjr_defaultRenderRequest(&reqs[1]); reqs[1].scene = imgui_bridge_->GetScene(); reqs[1].draw_mode = mjDRAW_MODE_COLOR; reqs[1].camera = imgui_bridge_->GetCamera(viewport.width, viewport.height); - reqs[1].width = viewport.width; - reqs[1].height = viewport.height; + reqs[1].viewport = viewport; filament_context_->Render(reqs); } } @@ -95,15 +93,13 @@ void MjrFilamentRenderer::ReadPixels(mjrRect viewport, unsigned char* rgb, reqs[0].scene = scene_bridge_->GetScene(); reqs[0].draw_mode = scene_bridge_->GetDrawMode(); reqs[0].camera = scene_bridge_->GetCamera(); - reqs[0].width = viewport.width; - reqs[0].height = viewport.height; + reqs[0].viewport = viewport; mjr_defaultRenderRequest(&reqs[1]); reqs[1].scene = imgui_bridge_->GetScene(); reqs[1].draw_mode = mjDRAW_MODE_COLOR; reqs[1].camera = imgui_bridge_->GetCamera(viewport.width, viewport.height); - reqs[1].width = viewport.width; - reqs[1].height = viewport.height; + reqs[1].viewport = viewport; if (rgb) { mjrRenderTargetConfig config; diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index e55db060..d7f81cb4 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -110,13 +110,14 @@ mjrFrameHandle FilamentContext::Render( } // If the window size has changed, we need to reacquire the swap chain. - if (request.width != window_width_ || request.height != window_height_) { + if (request.viewport.width != window_width_ || + request.viewport.height != window_height_) { if (window_width_ != 0 && window_height_ != 0) { engine_->destroy(window_swap_chain_); window_swap_chain_ = engine_->createSwapChain(config_.native_window); } - window_width_ = request.width; - window_height_ = request.height; + window_width_ = request.viewport.width; + window_height_ = request.viewport.height; } if (!render_began) { diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index c3f132ab..3a217c59 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -486,7 +486,7 @@ void SceneView::Configure(const mjModel* model) { void DoRender(filament::Renderer* renderer, const mjrRenderRequest& request) { SceneView::RenderRequest scene_view_request; scene_view_request.draw_mode = request.draw_mode; - scene_view_request.viewport = {0, 0, request.width, request.height}; + scene_view_request.viewport = request.viewport; scene_view_request.camera = request.camera; SceneView* scene_view = SceneView::downcast(request.scene); scene_view->Render(renderer, scene_view_request); @@ -499,7 +499,7 @@ void DoReadPixels(filament::Renderer* renderer, SceneView::RenderRequest scene_view_request; scene_view_request.draw_mode = request.draw_mode; - scene_view_request.viewport = {0, 0, request.width, request.height}; + scene_view_request.viewport = request.viewport; scene_view_request.camera = request.camera; scene_view_request.target = render_target; SceneView* scene_view = SceneView::downcast(request.scene); diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index 03cbecbe..b960244d 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -370,9 +370,8 @@ struct mjrRenderRequest { // The camera from which to render the scene. mjrCamera camera; - // The dimensions of the output image. - int width; - int height; + // The viewport into which to render the image. + mjrRect viewport; // The render target into which to render the image. If nullptr, the image // will be rendered to the window (as previously configured in From abea65b0be1c2fdd38dfff88b449af6cc7aea7ce Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 7 May 2026 07:50:30 -0700 Subject: [PATCH 218/251] Simplify materials. Merge shading model, material params, and material textures into a single "mjrMaterial" type. Remove redundant decor_line material. PiperOrigin-RevId: 911960222 Change-Id: Icce3772a8ea48467dfd610799c0c2f2ba752323e --- src/experimental/filament/CMakeLists.txt | 1 - .../filament/assets/unlit_line.mat | 29 ---- .../filament/compat/imgui_bridge.cc | 30 ++--- .../filament/compat/scene_geom_util.cc | 126 +++++++++--------- .../filament/filament/material.cc | 69 +++++----- src/experimental/filament/filament/material.h | 3 +- .../filament/filament/object_manager.cc | 1 - .../filament/filament/object_manager.h | 1 - .../filament/filament/renderable.cc | 64 ++++----- .../filament/filament/renderable.h | 23 +--- .../filament/filament/scene_view.cc | 20 +-- .../filament/render_context_filament.cc | 46 +++---- .../filament/render_context_filament.h | 99 ++++++++------ src/experimental/studio/index.html | 1 - 14 files changed, 228 insertions(+), 285 deletions(-) delete mode 100644 src/experimental/filament/assets/unlit_line.mat diff --git a/src/experimental/filament/CMakeLists.txt b/src/experimental/filament/CMakeLists.txt index 37105703..42261f11 100644 --- a/src/experimental/filament/CMakeLists.txt +++ b/src/experimental/filament/CMakeLists.txt @@ -117,7 +117,6 @@ set(MATERIAL_FILES phong_cube_reflect.mat unlit_decor.mat unlit_depth.mat - unlit_line.mat unlit_segmentation.mat unlit_ui.mat ) diff --git a/src/experimental/filament/assets/unlit_line.mat b/src/experimental/filament/assets/unlit_line.mat deleted file mode 100644 index 359bb319..00000000 --- a/src/experimental/filament/assets/unlit_line.mat +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2025 DeepMind Technologies Limited -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -material { - name : unlit_segmentation, - shadingModel : unlit, - culling: none, - parameters : [ - { type : float4, name : BaseColorFactor } - ] -} - -fragment { - void material(inout MaterialInputs material) { - prepareMaterial(material); - material.baseColor = materialParams.BaseColorFactor; - } -} diff --git a/src/experimental/filament/compat/imgui_bridge.cc b/src/experimental/filament/compat/imgui_bridge.cc index f7fbdd65..30a2fdcf 100644 --- a/src/experimental/filament/compat/imgui_bridge.cc +++ b/src/experimental/filament/compat/imgui_bridge.cc @@ -253,26 +253,25 @@ void ImguiBridge::Update() { mjrf_setRenderableMesh(renderable.get(), mesh, index_offset, command.ElemCount); - mjrMaterialTextures textures; - mjr_defaultMaterialTextures(&textures); - textures.color = GetTexture(command.GetTexID()); + mjrMaterial material; + mjr_defaultMaterial(&material); + material.color_texture = GetTexture(command.GetTexID()); - mjrMaterialParams properties; - mjr_defaultMaterialParams(&properties); - properties.scissor[0] = command.ClipRect.x; - properties.scissor[1] = height - command.ClipRect.w; - properties.scissor[2] = command.ClipRect.z - command.ClipRect.x; - properties.scissor[3] = command.ClipRect.w - command.ClipRect.y; + material.decor_ux = true; + material.scissor[0] = command.ClipRect.x; + material.scissor[1] = height - command.ClipRect.w; + material.scissor[2] = command.ClipRect.z - command.ClipRect.x; + material.scissor[3] = command.ClipRect.w - command.ClipRect.y; // Modal dialogs try to cover the whole window, but also a little outside // of it. This doesn't work well with filament's scissor test, so we clip // them to the window. - if (properties.scissor[0] < 0 || properties.scissor[1] < 0) { - properties.scissor[0] = 0; - properties.scissor[1] = 0; - properties.scissor[2] = width; - properties.scissor[3] = height; + if (material.scissor[0] < 0 || material.scissor[1] < 0) { + material.scissor[0] = 0; + material.scissor[1] = 0; + material.scissor[2] = width; + material.scissor[3] = height; } - mjrf_setRenderableMaterial(renderable.get(), &properties, &textures); + mjrf_setRenderableMaterial(renderable.get(), &material); const float position[] = {0, 0, 0}; const float rotation[] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; @@ -289,7 +288,6 @@ void ImguiBridge::PrepareRenderables(int count) { while (renderables_.size() < count) { mjrRenderableParams params; mjr_defaultRenderableParams(¶ms); - params.shading_model = mjSHADING_MODEL_UX; params.cast_shadows = false; params.receive_shadows = false; params.blend_order = static_cast(renderables_.size() + 1); diff --git a/src/experimental/filament/compat/scene_geom_util.cc b/src/experimental/filament/compat/scene_geom_util.cc index d255583a..e66a8630 100644 --- a/src/experimental/filament/compat/scene_geom_util.cc +++ b/src/experimental/filament/compat/scene_geom_util.cc @@ -183,20 +183,25 @@ static void UpdateGeomMaterial(mjrRenderable* renderable, const mjvGeom& geom, const mjModel* model = model_objs->GetModel(); const bool use_segid_color = scene->flags[mjRND_IDCOLOR]; - mjrMaterialParams params; - mjr_defaultMaterialParams(¶ms); - params.color[0] = geom.rgba[0]; - params.color[1] = geom.rgba[1]; - params.color[2] = geom.rgba[2]; - params.color[3] = geom.rgba[3]; + mjrMaterial material; + mjr_defaultMaterial(&material); + + if (geom.category == mjCAT_DECOR) { + material.decor_ux = true; + } + + material.color[0] = geom.rgba[0]; + material.color[1] = geom.rgba[1]; + material.color[2] = geom.rgba[2]; + material.color[3] = geom.rgba[3]; if (geom.type == mjGEOM_PLANE) { if (IsBehind(headpos, geom.pos, geom.mat)) { - params.color[3] *= 0.3; + material.color[3] *= 0.3; mjrf_setRenderableReceiveShadows(renderable, false); - params.reflective = false; + material.reflective = false; } else { mjrf_setRenderableReceiveShadows(renderable, true); - params.reflective = geom.reflectance > 0 && params.color[3] == 1.0f; + material.reflective = geom.reflectance > 0 && material.color[3] == 1.0f; } } mjrf_setRenderableLayerMask(renderable, geom.category); @@ -207,30 +212,28 @@ static void UpdateGeomMaterial(mjrRenderable* renderable, const mjvGeom& geom, mjrf_setRenderableWireframe(renderable, scene->flags[mjRND_WIREFRAME]); } - mjrMaterialTextures textures; - mjr_defaultMaterialTextures(&textures); if (geom.matid >= 0) { - textures.color = model_objs->GetTexture(geom.matid, mjTEXROLE_RGB); - textures.normal = model_objs->GetTexture(geom.matid, mjTEXROLE_NORMAL); - textures.emissive = model_objs->GetTexture(geom.matid, mjTEXROLE_EMISSIVE); - textures.orm = model_objs->GetTexture(geom.matid, mjTEXROLE_ORM); - textures.metallic = model_objs->GetTexture(geom.matid, mjTEXROLE_METALLIC); - textures.roughness = + material.color_texture = model_objs->GetTexture(geom.matid, mjTEXROLE_RGB); + material.normal_texture = + model_objs->GetTexture(geom.matid, mjTEXROLE_NORMAL); + material.emissive_texture = + model_objs->GetTexture(geom.matid, mjTEXROLE_EMISSIVE); + material.orm_texture = model_objs->GetTexture(geom.matid, mjTEXROLE_ORM); + material.metallic_texture = + model_objs->GetTexture(geom.matid, mjTEXROLE_METALLIC); + material.roughness_texture = model_objs->GetTexture(geom.matid, mjTEXROLE_ROUGHNESS); - textures.occlusion = + material.occlusion_texture = model_objs->GetTexture(geom.matid, mjTEXROLE_OCCLUSION); } - params.reflectance = geom.reflectance; - params.emissive = geom.emission; - params.specular = geom.specular; - params.glossiness = geom.shininess; + material.reflectance = geom.reflectance; + material.emissive = geom.emission; + material.specular = geom.specular; + material.glossiness = geom.shininess; if (geom.matid >= 0) { - params.metallic = model->mat_metallic[geom.matid]; - params.roughness = model->mat_roughness[geom.matid]; - params.tex_uniform = model->mat_texuniform[geom.matid]; - params.tex_repeat[0] = model->mat_texrepeat[(geom.matid * 2) + 0]; - params.tex_repeat[1] = model->mat_texrepeat[(geom.matid * 2) + 1]; + material.metallic = model->mat_metallic[geom.matid]; + material.roughness = model->mat_roughness[geom.matid]; } if (geom.segid >= 0) { @@ -246,9 +249,9 @@ static void UpdateGeomMaterial(mjrRenderable* renderable, const mjvGeom& geom, const uint8_t red = (segmentation_color >> 0) & 0xff; const uint8_t green = (segmentation_color >> 8) & 0xff; const uint8_t blue = (segmentation_color >> 16) & 0xff; - params.segmentation_color[0] = static_cast(red) / 255.0f; - params.segmentation_color[1] = static_cast(green) / 255.0f; - params.segmentation_color[2] = static_cast(blue) / 255.0f; + material.segmentation_color[0] = static_cast(red) / 255.0f; + material.segmentation_color[1] = static_cast(green) / 255.0f; + material.segmentation_color[2] = static_cast(blue) / 255.0f; } // UvScale only applies to objects that don't have explicit UV coordinates @@ -258,28 +261,33 @@ static void UpdateGeomMaterial(mjrRenderable* renderable, const mjvGeom& geom, // The material's `texuniform` and `texrepeat` parameters allow us to scale // the programmatic UVs. - if (textures.color) { - if (mjrf_getSamplerType(textures.color) == mjTEXTURE_2D) { + if (material.color_texture) { + const bool tex_uniform = model->mat_texuniform[geom.matid]; + if (mjrf_getSamplerType(material.color_texture) == mjTEXTURE_2D) { // For 2D textures, `tex_repeat` specifies how many times the texture // image is repeated. The `tex_uniform` flag determines if the repetition // is applied at in object space (false) or in world space (true). - params.uv_scale[0] = params.tex_repeat[0]; - params.uv_scale[1] = params.tex_repeat[1]; + float tex_repeat[2]; + tex_repeat[0] = model->mat_texrepeat[(geom.matid * 2) + 0]; + tex_repeat[1] = model->mat_texrepeat[(geom.matid * 2) + 1]; + material.uv_scale[0] = tex_repeat[0]; + material.uv_scale[1] = tex_repeat[1]; if (geom.dataid >= 0 && geom.type != mjGEOM_PLANE) { if (geom.size[0] > mjMINVAL) { - params.uv_scale[0] /= geom.size[0]; + material.uv_scale[0] /= geom.size[0]; } if (geom.size[1] > mjMINVAL) { - params.uv_scale[1] /= geom.size[1]; + material.uv_scale[1] /= geom.size[1]; } } - if (params.tex_uniform) { + + if (tex_uniform) { if (geom.size[0] > 0) { - params.uv_scale[0] *= geom.size[0]; + material.uv_scale[0] *= geom.size[0]; } if (geom.size[1] > 0) { - params.uv_scale[1] *= geom.size[1]; + material.uv_scale[1] *= geom.size[1]; } } const bool is_infinite_plane = @@ -289,11 +297,11 @@ static void UpdateGeomMaterial(mjrRenderable* renderable, const mjvGeom& geom, // re-centering in engine_vis_visualize.c. const float plane_scale = static_cast(mjMAXPLANEGRID) / 2.0f; const float tile_size_x = - GetPlaneTileSize(model, geom.matid, params.tex_repeat[0]); + GetPlaneTileSize(model, geom.matid, tex_repeat[0]); const float tile_size_y = - GetPlaneTileSize(model, geom.matid, params.tex_repeat[1]); - params.uv_scale[0] = 2.0f * plane_scale / tile_size_x; - params.uv_scale[1] = 2.0f * plane_scale / tile_size_y; + GetPlaneTileSize(model, geom.matid, tex_repeat[1]); + material.uv_scale[0] = 2.0f * plane_scale / tile_size_x; + material.uv_scale[1] = 2.0f * plane_scale / tile_size_y; } // We want to do the equivalent of: @@ -301,42 +309,34 @@ static void UpdateGeomMaterial(mjrRenderable* renderable, const mjvGeom& geom, // mjr_setf4(tplane, 0, -0.5 * scl.y, 0, -0.5); // glTexGenfv(GL_S, GL_OBJECT_PLANE, splane); // glTexGenfv(GL_T, GL_OBJECT_PLANE, tplane); - params.uv_scale[0] = 0.5f * params.uv_scale[0]; - params.uv_scale[1] = -0.5f * params.uv_scale[1]; - params.uv_offset[0] = -0.5f; - params.uv_offset[1] = -0.5f; + material.uv_scale[0] = 0.5f * material.uv_scale[0]; + material.uv_scale[1] = -0.5f * material.uv_scale[1]; + material.uv_offset[0] = -0.5f; + material.uv_offset[1] = -0.5f; } else { // For cube maps, if `tex_uniform` is true, then scale the texture so that // it covers a 1x1 area of world space rather than the area of the object. - if (params.tex_uniform) { - params.uv_scale[0] = 1.0f / (geom.size[0] ? geom.size[0] : 1.0f); - params.uv_scale[1] = 1.0f / (geom.size[1] ? geom.size[1] : 1.0f); - params.uv_scale[2] = 1.0f / (geom.size[2] ? geom.size[2] : 1.0f); + if (tex_uniform) { + material.uv_scale[0] = 1.0f / (geom.size[0] ? geom.size[0] : 1.0f); + material.uv_scale[1] = 1.0f / (geom.size[1] ? geom.size[1] : 1.0f); + material.uv_scale[2] = 1.0f / (geom.size[2] ? geom.size[2] : 1.0f); } } } // Apply material multipliers from the model. - params.emissive *= model_objs->GetEmissiveMultiplier(); - params.specular *= model_objs->GetSpecularMultiplier(); - params.glossiness *= model_objs->GetShininessMultiplier(); + material.emissive *= model_objs->GetEmissiveMultiplier(); + material.specular *= model_objs->GetSpecularMultiplier(); + material.glossiness *= model_objs->GetShininessMultiplier(); - mjrf_setRenderableMaterial(renderable, ¶ms, &textures); + mjrf_setRenderableMaterial(renderable, &material); } UniquePtr CreateGeomRenderable( const mjvGeom& geom, const mjvScene* scene, mjrfContext* ctx, ModelObjects* model_objs, const float headpos[3]) { - mjrShadingModel shading_model = mjSHADING_MODEL_SCENE_OBJECT; - if (geom.type == mjGEOM_LINE || geom.type == mjGEOM_LINEBOX) { - shading_model = mjSHADING_MODEL_DECOR_LINES; - } else if (geom.category == mjCAT_DECOR) { - shading_model = mjSHADING_MODEL_DECOR; - } - mjrRenderableParams params; mjr_defaultRenderableParams(¶ms); - params.shading_model = shading_model; auto renderable = CreateRenderable(ctx, params); PrepareGeomMeshes(renderable.get(), geom, scene, model_objs); UpdateGeomMaterial(renderable.get(), geom, scene, model_objs, headpos); diff --git a/src/experimental/filament/filament/material.cc b/src/experimental/filament/filament/material.cc index 672f7a40..5e074d68 100644 --- a/src/experimental/filament/filament/material.cc +++ b/src/experimental/filament/filament/material.cc @@ -28,48 +28,47 @@ namespace mujoco { void UpdateMaterialInstance(filament::MaterialInstance* instance, - const mjrMaterialParams& params, - const mjrMaterialTextures& textures, + const mjrMaterial& material, ObjectManager* object_mgr) { - if (params.scissor[2] != 0 && params.scissor[3] != 0) { - instance->setScissor(params.scissor[0], params.scissor[1], - params.scissor[2], params.scissor[3]); + if (material.scissor[2] != 0 && material.scissor[3] != 0) { + instance->setScissor(material.scissor[0], material.scissor[1], + material.scissor[2], material.scissor[3]); } - const filament::Material* material = instance->getMaterial(); - if (material->hasParameter("BaseColorFactor")) { + const filament::Material* fmaterial = instance->getMaterial(); + if (fmaterial->hasParameter("BaseColorFactor")) { instance->setParameter("BaseColorFactor", filament::RgbaType::sRGB, - ReadFloat4(params.color)); + ReadFloat4(material.color)); } - if (material->hasParameter("SegmentationColor")) { + if (fmaterial->hasParameter("SegmentationColor")) { instance->setParameter("SegmentationColor", filament::RgbaType::LINEAR, - ReadFloat4(params.segmentation_color)); + ReadFloat4(material.segmentation_color)); } - if (material->hasParameter("EmissiveFactor")) { - instance->setParameter("EmissiveFactor", params.emissive); + if (fmaterial->hasParameter("EmissiveFactor")) { + instance->setParameter("EmissiveFactor", material.emissive); } - if (material->hasParameter("SpecularFactor")) { - instance->setParameter("SpecularFactor", params.specular); + if (fmaterial->hasParameter("SpecularFactor")) { + instance->setParameter("SpecularFactor", material.specular); } - if (material->hasParameter("GlossinessFactor")) { - instance->setParameter("GlossinessFactor", params.glossiness); + if (fmaterial->hasParameter("GlossinessFactor")) { + instance->setParameter("GlossinessFactor", material.glossiness); } - if (material->hasParameter("MetallicFactor")) { + if (fmaterial->hasParameter("MetallicFactor")) { instance->setParameter("MetallicFactor", - params.metallic >= 0 ? params.metallic : 1.0f); + material.metallic >= 0 ? material.metallic : 1.0f); } - if (material->hasParameter("RoughnessFactor")) { + if (fmaterial->hasParameter("RoughnessFactor")) { instance->setParameter("RoughnessFactor", - params.roughness >= 0 ? params.roughness : 1.0f); + material.roughness >= 0 ? material.roughness : 1.0f); } - if (material->hasParameter("UvScale")) { - instance->setParameter("UvScale", ReadFloat3(params.uv_scale)); + if (fmaterial->hasParameter("UvScale")) { + instance->setParameter("UvScale", ReadFloat3(material.uv_scale)); } - if (material->hasParameter("UvOffset")) { - instance->setParameter("UvOffset", ReadFloat3(params.uv_offset)); + if (fmaterial->hasParameter("UvOffset")) { + instance->setParameter("UvOffset", ReadFloat3(material.uv_offset)); } - if (material->hasParameter("Reflectance")) { - instance->setParameter("Reflectance", params.reflectance); + if (fmaterial->hasParameter("Reflectance")) { + instance->setParameter("Reflectance", material.reflectance); } // All textures use the same default sampler. @@ -83,7 +82,7 @@ void UpdateMaterialInstance(filament::MaterialInstance* instance, auto TrySetTexture = [&](const char* name, const mjrTexture* texture, mjtTextureRole role) { - if (material->hasParameter(name)) { + if (fmaterial->hasParameter(name)) { if (texture != nullptr) { instance->setParameter( name, Texture::downcast(texture)->GetFilamentTexture(), sampler); @@ -94,14 +93,14 @@ void UpdateMaterialInstance(filament::MaterialInstance* instance, } }; - TrySetTexture("BaseColor", textures.color, mjTEXROLE_RGB); - TrySetTexture("Normal", textures.normal, mjTEXROLE_NORMAL); - TrySetTexture("Metallic", textures.metallic, mjTEXROLE_METALLIC); - TrySetTexture("Roughness", textures.roughness, mjTEXROLE_ROUGHNESS); - TrySetTexture("Occlusion", textures.occlusion, mjTEXROLE_OCCLUSION); - TrySetTexture("ORM", textures.orm, mjTEXROLE_ORM); - TrySetTexture("Emissive", textures.emissive, mjTEXROLE_EMISSIVE); - TrySetTexture("Reflection", textures.reflection, mjTEXROLE_USER); + TrySetTexture("BaseColor", material.color_texture, mjTEXROLE_RGB); + TrySetTexture("Normal", material.normal_texture, mjTEXROLE_NORMAL); + TrySetTexture("Metallic", material.metallic_texture, mjTEXROLE_METALLIC); + TrySetTexture("Roughness", material.roughness_texture, mjTEXROLE_ROUGHNESS); + TrySetTexture("Occlusion", material.occlusion_texture, mjTEXROLE_OCCLUSION); + TrySetTexture("ORM", material.orm_texture, mjTEXROLE_ORM); + TrySetTexture("Emissive", material.emissive_texture, mjTEXROLE_EMISSIVE); + TrySetTexture("Reflection", material.reflection_texture, mjTEXROLE_USER); } } // namespace mujoco diff --git a/src/experimental/filament/filament/material.h b/src/experimental/filament/filament/material.h index abc0f9a1..8e6ac65d 100644 --- a/src/experimental/filament/filament/material.h +++ b/src/experimental/filament/filament/material.h @@ -25,8 +25,7 @@ namespace mujoco { // Updates the material instances based on the currently set parameters and // textures. void UpdateMaterialInstance(filament::MaterialInstance* instance, - const mjrMaterialParams& params, - const mjrMaterialTextures& textures, + const mjrMaterial& material, ObjectManager* object_mgr); } // namespace mujoco diff --git a/src/experimental/filament/filament/object_manager.cc b/src/experimental/filament/filament/object_manager.cc index 487f95fd..1816b627 100644 --- a/src/experimental/filament/filament/object_manager.cc +++ b/src/experimental/filament/filament/object_manager.cc @@ -68,7 +68,6 @@ ObjectManager::ObjectManager(filament::Engine* engine) materials_[kPhongCubeFade] = LoadMaterial(engine, "phong_cube_fade.filamat"); materials_[kPhongCubeReflect] = LoadMaterial(engine, "phong_cube_reflect.filamat"); materials_[kUnlitSegmentation] = LoadMaterial(engine, "unlit_segmentation.filamat"); - materials_[kUnlitLine] = LoadMaterial(engine, "unlit_line.filamat"); materials_[kUnlitDecor] = LoadMaterial(engine, "unlit_decor.filamat"); materials_[kUnlitDepth] = LoadMaterial(engine, "unlit_depth.filamat"); materials_[kUnlitUi] = LoadMaterial(engine, "unlit_ui.filamat"); diff --git a/src/experimental/filament/filament/object_manager.h b/src/experimental/filament/filament/object_manager.h index 4365029b..00fc006c 100644 --- a/src/experimental/filament/filament/object_manager.h +++ b/src/experimental/filament/filament/object_manager.h @@ -56,7 +56,6 @@ class ObjectManager { kUnlitSegmentation, kUnlitDecor, kUnlitDepth, - kUnlitLine, kUnlitUi, kNumMaterials, }; diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index ad729b46..14655e0e 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -51,8 +51,7 @@ static constexpr float kArrowHeadSize = 1.75f; Renderable::Renderable(FilamentContext* ctx, const mjrRenderableParams& params) : object_mgr_(ctx->GetObjectManager()), params_(params) { - mjr_defaultMaterialParams(&material_params_); - mjr_defaultMaterialTextures(&material_textures_); + mjr_defaultMaterial(&material_); } Renderable::~Renderable() noexcept { @@ -202,21 +201,18 @@ void Renderable::RemoveFromScene(filament::Scene* scene) { assigned_scene_ = nullptr; } -void Renderable::UpdateMaterial(const mjrMaterialParams& params, - const mjrMaterialTextures& textures) { - material_params_ = params; - material_textures_ = textures; +void Renderable::UpdateMaterial(const mjrMaterial& material) { + material_ = material; AssignMaterial(mjDRAW_MODE_COLOR, GetColorMaterialType()); - if (params_.shading_model == mjSHADING_MODEL_SCENE_OBJECT) { + if (!material_.decor_ux) { AssignMaterial(mjDRAW_MODE_DEPTH, ObjectManager::kUnlitDepth); AssignMaterial(mjDRAW_MODE_SEGMENTATION, ObjectManager::kUnlitSegmentation); } for (int i = 0; i < mjNUM_DRAW_MODES; ++i) { if (instances_[i]) { - UpdateMaterialInstance(instances_[i], material_params_, - material_textures_, object_mgr_); + UpdateMaterialInstance(instances_[i], material_, object_mgr_); } } SetDrawMode(draw_mode_); @@ -241,17 +237,13 @@ void Renderable::AssignMaterial(mjrDrawMode mode, } } -const mjrMaterialParams& Renderable::GetMaterialParams() const { - return material_params_; -} - -const mjrMaterialTextures& Renderable::GetMaterialTextures() const { - return material_textures_; +const mjrMaterial& Renderable::GetMaterial() const { + return material_; } void Renderable::SetDrawMode(mjrDrawMode mode) { // Only SceneObjects support non-color draw modes. - if (params_.shading_model != mjSHADING_MODEL_SCENE_OBJECT) { + if (!material_.decor_ux) { mode = mjDRAW_MODE_COLOR; } @@ -347,21 +339,21 @@ void Renderable::SetWireframe(bool wireframe) { } ObjectManager::MaterialType Renderable::GetColorMaterialType() const { - if (params_.shading_model == mjSHADING_MODEL_DECOR_LINES) { - return ObjectManager::kUnlitLine; - } else if (params_.shading_model == mjSHADING_MODEL_DECOR) { - return ObjectManager::kUnlitDecor; - } else if (params_.shading_model == mjSHADING_MODEL_UX) { - return ObjectManager::kUnlitUi; - } else if (material_textures_.orm) { + if (material_.decor_ux) { + if (material_.color_texture) { + return ObjectManager::kUnlitUi; + } else { + return ObjectManager::kUnlitDecor; + } + } else if (material_.orm_texture) { return ObjectManager::kPbrPacked; - } else if (material_textures_.metallic) { + } else if (material_.metallic_texture) { return ObjectManager::kPbr; - } else if (material_textures_.roughness) { + } else if (material_.roughness_texture) { return ObjectManager::kPbr; - } else if (material_params_.metallic >= 0) { + } else if (material_.metallic >= 0) { return ObjectManager::kPbr; - } else if (material_params_.roughness >= 0) { + } else if (material_.roughness >= 0) { return ObjectManager::kPbr; } @@ -370,7 +362,7 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { // geometry) and `mesh_texcoordadr` stores the address of the mesh uvs if // it has them. bool has_texcoords = false; - const Texture* color_texture = Texture::downcast(material_textures_.color); + const Texture* color_texture = Texture::downcast(material_.color_texture); if (!parts_.empty()) { const auto attribs = parts_[0].mesh->GetVertexAttributes(); auto it = std::find(attribs.begin(), attribs.end(), @@ -379,33 +371,33 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { } if (color_texture == nullptr) { - if (material_params_.color[3] < 1.0f) { + if (material_.color[3] < 1.0f) { return ObjectManager::kPhongColorFade; - } else if (material_params_.reflective) { + } else if (material_.reflective) { return ObjectManager::kPhongColorReflect; } else { return ObjectManager::kPhongColor; } } else if (color_texture->GetSamplerType() == mjTEXTURE_CUBE) { - if (material_params_.color[3] < 1.0f) { + if (material_.color[3] < 1.0f) { return ObjectManager::kPhongCubeFade; - } else if (material_params_.reflective) { + } else if (material_.reflective) { return ObjectManager::kPhongCubeReflect; } else { return ObjectManager::kPhongCube; } } else if (has_texcoords) { - if (material_params_.color[3] < 1.0f) { + if (material_.color[3] < 1.0f) { return ObjectManager::kPhong2dUvFade; - } else if (material_params_.reflective) { + } else if (material_.reflective) { return ObjectManager::kPhong2dUvReflect; } else { return ObjectManager::kPhong2dUv; } } else { - if (material_params_.color[3] < 1.0f) { + if (material_.color[3] < 1.0f) { return ObjectManager::kPhong2dFade; - } else if (material_params_.reflective) { + } else if (material_.reflective) { return ObjectManager::kPhong2dReflect; } else { return ObjectManager::kPhong2d; diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 58310d98..6e4f32c1 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -37,15 +37,6 @@ namespace mujoco { // The mesh describes the surface geometry of the object and the material // describes how that surface interacts with light (i.e. the color of each point // on the surface). -// -// Defining the mesh is easy; just call SetMesh. -// -// Defining a Material happens in two stages. First, the user specifies the -// ShadingModel to use for Rendering. This describes the overall intent of -// how the Renderable will appear (e.g. lit, unlit, wireframe, etc.). Next, -// the user specifies the MaterialParams and MaterialTextures to use with the -// ShadingModel. Its these properties that ultimately define the actual material -// of the Renderable. class Renderable : public mjrRenderable { public: Renderable(FilamentContext* ctx, const mjrRenderableParams& params); @@ -98,19 +89,14 @@ class Renderable : public mjrRenderable { // Removes the renderable from the given filament Scene. void RemoveFromScene(filament::Scene* scene); - // Further defines the material of the renderable. Only applies to renderables - // with a SceneObject shading model. + // Further defines the material of the renderable. void SetDrawMode(mjrDrawMode mode); // Updates the parameters for the material. - void UpdateMaterial(const mjrMaterialParams& params, - const mjrMaterialTextures& textures); + void UpdateMaterial(const mjrMaterial& material); // Returns the current material parameters. - const mjrMaterialParams& GetMaterialParams() const; - - // Returns the current material textures. - const mjrMaterialTextures& GetMaterialTextures() const; + const mjrMaterial& GetMaterial() const; // Returns the filament Engine managing the renderables. filament::Engine* GetEngine(); @@ -143,8 +129,7 @@ class Renderable : public mjrRenderable { ObjectManager* object_mgr_; mjrRenderableParams params_; filament::MaterialInstance* instances_[mjNUM_DRAW_MODES] = {nullptr}; - mjrMaterialParams material_params_; - mjrMaterialTextures material_textures_; + mjrMaterial material_; mjrDrawMode draw_mode_ = mjDRAW_MODE_COLOR; filament::Scene* assigned_scene_ = nullptr; std::vector parts_; diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 3a217c59..3622f0ab 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -208,7 +208,7 @@ void SceneView::RemoveFromScene(Light* light) { void SceneView::AddToScene(Renderable* renderable) { if (renderables_.insert(renderable).second) { renderable->AddToScene(scene_); - if (renderable->GetMaterialParams().reflective) { + if (renderable->GetMaterial().reflective) { AddReflectiveRenderable(renderable); } } @@ -317,9 +317,9 @@ void SceneView::AddReflectiveRenderable(Renderable* renderable) { target->Prepare(viewport.width, viewport.height); if (reflections_enabled_) { - mjrMaterialTextures textures = renderable->GetMaterialTextures(); - textures.reflection = target->GetColorTexture(); - renderable->UpdateMaterial(renderable->GetMaterialParams(), textures); + mjrMaterial material = renderable->GetMaterial(); + material.reflection_texture = target->GetColorTexture(); + renderable->UpdateMaterial(material); } } @@ -349,18 +349,18 @@ void SceneView::EnableReflections() { for (int i = 0; i < reflectives_.size(); ++i) { Renderable* renderable = reflectives_[i]; - mjrMaterialTextures textures = renderable->GetMaterialTextures(); - textures.reflection = reflect_targets_[i]->GetColorTexture(); - renderable->UpdateMaterial(renderable->GetMaterialParams(), textures); + mjrMaterial material = renderable->GetMaterial(); + material.reflection_texture = reflect_targets_[i]->GetColorTexture(); + renderable->UpdateMaterial(material); } } void SceneView::DisableReflections() { reflections_enabled_ = false; for (Renderable* renderable : reflectives_) { - mjrMaterialTextures textures = renderable->GetMaterialTextures(); - textures.reflection = nullptr; - renderable->UpdateMaterial(renderable->GetMaterialParams(), textures); + mjrMaterial material = renderable->GetMaterial(); + material.reflection_texture = nullptr; + renderable->UpdateMaterial(material); } } diff --git a/src/experimental/filament/render_context_filament.cc b/src/experimental/filament/render_context_filament.cc index c58a3b2d..b923ed16 100644 --- a/src/experimental/filament/render_context_filament.cc +++ b/src/experimental/filament/render_context_filament.cc @@ -69,10 +69,11 @@ void mjr_defaultTextureConfig(mjrTextureConfig* config) { } void mjr_defaultMeshData(mjrMeshData* data) { - std::memset(data, 0, sizeof(mjrMeshData)); + memset(data, 0, sizeof(mjrMeshData)); } void mjr_defaultSceneParams(mjrSceneParams* params) { + memset(params, 0, sizeof(mjrSceneParams)); params->enable_post_processing = true; params->enable_reflections = true; params->enable_shadows = true; @@ -81,6 +82,7 @@ void mjr_defaultSceneParams(mjrSceneParams* params) { } void mjr_defaultLightParams(mjrLightParams* params) { + memset(params, 0, sizeof(mjrLightParams)); params->type = mjLIGHT_POINT; params->texture = nullptr; params->color[0] = 0; @@ -95,35 +97,20 @@ void mjr_defaultLightParams(mjrLightParams* params) { params->vsm_blur_width = 0.0f; } -void mjr_defaultMaterialTextures(mjrMaterialTextures* textures) { - textures->color = nullptr; - textures->normal = nullptr; - textures->metallic = nullptr; - textures->roughness = nullptr; - textures->occlusion = nullptr; - textures->orm = nullptr; - textures->emissive = nullptr; - textures->reflection = nullptr; -} - -void mjr_defaultMaterialParams(mjrMaterialParams* params) { - setf(params->color, {1.f, 1.f, 1.f, 1.f}); - setf(params->segmentation_color, {1, 1, 1, 1}); - setf(params->uv_scale, {1, 1, 1}); - setf(params->uv_offset, {0, 0, 0}); - setf(params->scissor, {0, 0, 0, 0}); - params->emissive = -1.0f; - params->specular = -1.0f; - params->glossiness = -1.0f; - params->metallic = -1.0f; - params->roughness = -1.0f; - params->reflectance = 0.0f; - params->tex_uniform = false; - params->reflective = false; +void mjr_defaultMaterial(mjrMaterial* material) { + memset(material, 0, sizeof(mjrMaterial)); + setf(material->color, {1.f, 1.f, 1.f, 1.f}); + setf(material->segmentation_color, {1, 1, 1, 1}); + setf(material->uv_scale, {1, 1, 1}); + material->emissive = -1.0f; + material->specular = -1.0f; + material->glossiness = -1.0f; + material->metallic = -1.0f; + material->roughness = -1.0f; } void mjr_defaultRenderableParams(mjrRenderableParams* params) { - params->shading_model = mjSHADING_MODEL_SCENE_OBJECT; + memset(params, 0, sizeof(mjrRenderableParams)); params->cast_shadows = true; params->receive_shadows = true; params->layer_mask = 0x01; @@ -265,9 +252,8 @@ void mjrf_setRenderableGeomMesh(mjrRenderable* renderable, mjtGeom type, } void mjrf_setRenderableMaterial(mjrRenderable* renderable, - const mjrMaterialParams* params, - const mjrMaterialTextures* textures) { - mujoco::Renderable::downcast(renderable)->UpdateMaterial(*params, *textures); + const mjrMaterial* material) { + mujoco::Renderable::downcast(renderable)->UpdateMaterial(*material); } void mjrf_setRenderableTransform(mjrRenderable* renderable, diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index b960244d..85ec2856 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -53,19 +53,6 @@ typedef enum mjrDrawMode_ { enum { mjNUM_DRAW_MODES = 3 }; -// The shading model (material) for a Renderable. -typedef enum mjrShadingModel_ { - // For renderables in the main 3D scene. - mjSHADING_MODEL_SCENE_OBJECT = 0, - // For UX renderables. - mjSHADING_MODEL_UX, - // For decorative elements in a Scene (e.g. contact points, force vectors, - // etc.). These objects will not be affected by lighting. - mjSHADING_MODEL_DECOR, - // As above, but uses a line primitives for drawing. - mjSHADING_MODEL_DECOR_LINES, -} mjrShadingModel; - // The type of data stored in an index buffer. typedef enum mjrIndexType_ { mjINDEX_TYPE_U16 = 0, @@ -124,41 +111,74 @@ typedef mjtColorSpace mjrColorSpace; typedef mjtLightType mjrLightType; typedef mjvGLCamera mjrCamera; -// The textures that can be assigned to the drawable's material. -struct mjrMaterialTextures { - const mjrTexture* color; - const mjrTexture* normal; - const mjrTexture* metallic; - const mjrTexture* roughness; - const mjrTexture* occlusion; - const mjrTexture* orm; - const mjrTexture* emissive; - const mjrTexture* reflection; -}; - -// Initializes the mjrMaterialTextures to default values. -void mjr_defaultMaterialTextures(mjrMaterialTextures* textures); - -// The parameters that can be applied to the drawable's material. -struct mjrMaterialParams { +// The material to be applied to a renderable. +struct mjrMaterial { + // The color of the object. Defaults to white. float color[4]; + + // The color to use for segmentation rendering. Defaults to white. float segmentation_color[4]; - float tex_repeat[2]; + + // Applies an addition scale to the UV coordinates of the object. Defaults to + // (1, 1, 1). float uv_scale[3]; + + // Applies an offset to the UV coordinates of the object. Defaults to (0, 0, + // 0). float uv_offset[3]; + + // Applies a scissor test to the object. float scissor[4]; - float specular; - float glossiness; + + // Factors for PBR metallic-roughness materials. float metallic; float roughness; + + // Factors for (non-PBR) specular-glossiness materials. + float specular; + float glossiness; + + // The emissive (glow) factor of the object. float emissive; - float reflectance; - mjtByte tex_uniform; + + // Whether or not the object is a reflective surface. Only applies to planes. mjtByte reflective; + // The blend factor to use for reflective surfaces. A value of 1.0 means that + // the surface is fully reflective (i.e. a mirror). + float reflectance; + + // If true, does not apply any lighting to the object. (Assumes the object is + // used for UX or decorative elements like contact forces and labels.) + mjtByte decor_ux; + + // The texture containing the base color of the object. + const mjrTexture* color_texture; + + // The normal map of the object. + const mjrTexture* normal_texture; + + // The metallic map of the object. + const mjrTexture* metallic_texture; + + // The roughness map of the object. + const mjrTexture* roughness_texture; + + // The occlusion map of the object. + const mjrTexture* occlusion_texture; + + // A texture containing the occlusion, roughness, and metallic maps packed + // into the R, G, B channels, respectively. + const mjrTexture* orm_texture; + + // An emissive texture for the object. + const mjrTexture* emissive_texture; + + // The reflection texture to use for the object. For internal use only. + const mjrTexture* reflection_texture; }; -// Initializes the mjrMaterialParams to default values. -void mjr_defaultMaterialParams(mjrMaterialParams* params); +// Initializes the mjrMaterial to default values. +void mjr_defaultMaterial(mjrMaterial* material); // The binary contents of a texture. struct mjrTextureData { @@ -205,8 +225,6 @@ void mjr_defaultTextureConfig(mjrTextureConfig* config); // Configuration parameters for a Renderable. struct mjrRenderableParams { - // The shading model to use for the Renderable. - mjrShadingModel shading_model; // Whether or not the Renderable casts shadows. mjtByte cast_shadows; // Whether or not the Renderable receives shadows. @@ -516,8 +534,7 @@ void mjrf_setRenderableGeomMesh(mjrRenderable* renderable, mjtGeom type, // Sets the material properties and textures of the renderable. void mjrf_setRenderableMaterial(mjrRenderable* renderable, - const mjrMaterialParams* params, - const mjrMaterialTextures* textures); + const mjrMaterial* material); // Sets the transform (position, rotation, and size) of the renderable. void mjrf_setRenderableTransform(mjrRenderable* renderable, diff --git a/src/experimental/studio/index.html b/src/experimental/studio/index.html index ec40e14e..8ab12f48 100644 --- a/src/experimental/studio/index.html +++ b/src/experimental/studio/index.html @@ -89,7 +89,6 @@ "assets/phong_cube_reflect.filamat", "assets/unlit_decor.filamat", "assets/unlit_depth.filamat", - "assets/unlit_line.filamat", "assets/unlit_segmentation.filamat", "assets/unlit_ui.filamat" ]; From 730ecceda1f66337a302ad10dcaea4d8a24c41ce Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 7 May 2026 09:36:44 -0700 Subject: [PATCH 219/251] Add SDF support. PiperOrigin-RevId: 912007379 Change-Id: I89ebf6bbabca08656932d028b27f1fbe48178986 --- src/experimental/filament/compat/scene_geom_util.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/experimental/filament/compat/scene_geom_util.cc b/src/experimental/filament/compat/scene_geom_util.cc index e66a8630..9a733a29 100644 --- a/src/experimental/filament/compat/scene_geom_util.cc +++ b/src/experimental/filament/compat/scene_geom_util.cc @@ -83,6 +83,7 @@ static void PrepareGeomMeshes(mjrRenderable* renderable, const mjvGeom& geom, switch ((mjtGeom)geom.type) { case mjGEOM_MESH: + case mjGEOM_SDF: mjrf_setRenderableMesh(renderable, GetMesh(model_objects, geom.dataid), 0, 0); // Ignore size for meshes. size[0] = 1.f; @@ -168,7 +169,6 @@ static void PrepareGeomMeshes(mjrRenderable* renderable, const mjvGeom& geom, case mjGEOM_LABEL: // Do nothing. break; - case mjGEOM_SDF: case mjNGEOMTYPES: mju_warning("Unsupported geom type: %d", geom.type); break; From 5f36cb9e64dbb7605d5fbe59df43c6cf8a68d2d3 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Thu, 7 May 2026 09:51:33 -0700 Subject: [PATCH 220/251] Add/cleanup documentation. PiperOrigin-RevId: 912014433 Change-Id: I8f05d482e07ed58588f5847d470e5420e5c09f3a --- src/experimental/filament/filament/builtins.h | 1 + .../filament/filament/filament_context.h | 7 +- src/experimental/filament/filament/light.h | 15 +- src/experimental/filament/filament/material.h | 5 +- src/experimental/filament/filament/mesh.h | 3 +- .../filament/filament/object_manager.h | 13 +- .../filament/filament/renderable.h | 59 +- .../filament/render_context_filament.cc | 17 +- .../filament/render_context_filament.h | 853 +++++++++++------- 9 files changed, 567 insertions(+), 406 deletions(-) diff --git a/src/experimental/filament/filament/builtins.h b/src/experimental/filament/filament/builtins.h index 8044b14b..c5746e64 100644 --- a/src/experimental/filament/filament/builtins.h +++ b/src/experimental/filament/filament/builtins.h @@ -27,6 +27,7 @@ class Builtins { public: Builtins(filament::Engine* engine, int nstack, int nslice, int nquad); + // Returns a mesh for the corresponding built-in shape. const Mesh* Line(); const Mesh* LineBox(); const Mesh* Plane(); diff --git a/src/experimental/filament/filament/filament_context.h b/src/experimental/filament/filament/filament_context.h index db817395..d1b175ce 100644 --- a/src/experimental/filament/filament/filament_context.h +++ b/src/experimental/filament/filament/filament_context.h @@ -29,7 +29,7 @@ namespace mujoco { -// Manages the filament renderer and provides APIs for rendering scenes. +// Manages the filament::Renderer and provides APIs for rendering scenes. class FilamentContext : public mjrfContext { public: explicit FilamentContext(const mjrFilamentConfig* config); @@ -43,8 +43,9 @@ class FilamentContext : public mjrfContext { // immediately afterwards. The renderer thread will then perform the actual // rendering on the GPU. Callers can use WaitForFrame to block until the // rendering is complete. - mjrFrameHandle Render(std::span render_requests, - std::span read_requests = {}); + mjrFrameHandle Render( + std::span render_requests, + std::span read_requests = {}); // Blocks until the given frame has completed rendering. void WaitForFrame(mjrFrameHandle frame_handle); diff --git a/src/experimental/filament/filament/light.h b/src/experimental/filament/filament/light.h index cc2a16a6..106dcba8 100644 --- a/src/experimental/filament/filament/light.h +++ b/src/experimental/filament/filament/light.h @@ -25,7 +25,8 @@ namespace mujoco { -// Manages the filament Entities for a single mjvLight. +// Wrapper around both a "normal" filament Light Entity and a filament +// IndirectLight. class Light : public mjrLight { public: Light(FilamentContext* ctx, const mjrLightParams& params); @@ -34,20 +35,20 @@ class Light : public mjrLight { Light(const Light&) = delete; Light& operator=(const Light&) = delete; - // Adds the filament light Entities to the given filament Scene. + // Adds this light to the filament Scene. void AddToScene(filament::Scene* scene); - // Removes the filament light Entities from the given filament Scene. + // Removes this light from the filament Scene. void RemoveFromScene(filament::Scene* scene); - // Updates the light's position/rotation. + // Updates this light's position and rotation. void SetTransform(filament::math::float3 position, filament::math::float3 direction); - // Sets the color of the light. + // Sets the color of this light. void SetColor(const filament::math::float3& color); - // Sets the intensity of the light in candela. + // Sets the intensity of this light, in candela. void SetIntensity(float intensity); // Returns the type of the light. @@ -68,8 +69,8 @@ class Light : public mjrLight { filament::Engine* engine_ = nullptr; filament::IndirectLight* ibl_ = nullptr; utils::Entity entity_; - bool enabled_ = true; mjrLightParams params_; + bool enabled_ = true; }; } // namespace mujoco diff --git a/src/experimental/filament/filament/material.h b/src/experimental/filament/filament/material.h index 8e6ac65d..25469924 100644 --- a/src/experimental/filament/filament/material.h +++ b/src/experimental/filament/filament/material.h @@ -22,8 +22,9 @@ namespace mujoco { -// Updates the material instances based on the currently set parameters and -// textures. +// Updates the material instance using the given parameters and texture data. In +// some cases where a material needs a texture, but a specific texture is not +// provided, a default texture from the ObjectManager will be used instead. void UpdateMaterialInstance(filament::MaterialInstance* instance, const mjrMaterial& material, ObjectManager* object_mgr); diff --git a/src/experimental/filament/filament/mesh.h b/src/experimental/filament/filament/mesh.h index 735d8717..8e79053b 100644 --- a/src/experimental/filament/filament/mesh.h +++ b/src/experimental/filament/filament/mesh.h @@ -31,10 +31,9 @@ #include #include "experimental/filament/render_context_filament.h" -// Functions for creating filament vertex and index buffers. namespace mujoco { -// Owns a Vertex and Index buffer representing a geometry mesh. +// Owns a filament Vertex and Index buffer representing a geometry mesh. class Mesh : public mjrMesh { public: // Creates a Mesh from the given MeshData. diff --git a/src/experimental/filament/filament/object_manager.h b/src/experimental/filament/filament/object_manager.h index 00fc006c..509b3505 100644 --- a/src/experimental/filament/filament/object_manager.h +++ b/src/experimental/filament/filament/object_manager.h @@ -38,6 +38,8 @@ class ObjectManager { ObjectManager(filament::Engine* engine); ~ObjectManager(); + // The different filament::Materials that are loaded and managed by the + // ObjectManager. enum MaterialType { kPbr, kPbrPacked, @@ -60,20 +62,19 @@ class ObjectManager { kNumMaterials, }; - // Returns the filament Engine that owns the assets. - filament::Engine* GetEngine() const { return engine_; } - // Returns the Material of the given type. filament::Material* GetMaterial(MaterialType type) const; // Returns the fallback Texture with the given role. const filament::Texture* GetFallbackTexture(mjtTextureRole role) const; - // Returns the built-in mesh collection with the given parameters. + // Returns the built-in mesh collection with the given dimensions. For + // performance reasons, you should consider always using the same dimensions + // in order to reuse the same meshes. Builtins* GetBuiltins(int nstack, int nslice, int nquad); - // The default environment light to use if no environment light is specified. - static constexpr const char* kDefaultEnvironmentLight = "ibl.ktx"; + // Returns the filament Engine that owns the assets. + filament::Engine* GetEngine() const { return engine_; } ObjectManager(const ObjectManager&) = delete; ObjectManager& operator=(const ObjectManager&) = delete; diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 6e4f32c1..dfed9db0 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -45,62 +45,57 @@ class Renderable : public mjrRenderable { Renderable(const Renderable&) = delete; Renderable& operator=(const Renderable&) = delete; - // Sets the mesh of the renderable. The elem_offset and elem_count parameters + // Sets the mesh of this renderable. The elem_offset and elem_count parameters // can be used to specify a submesh within the mesh. If elem_count is 0, // assumes the entire mesh should be appended. void SetMesh(const Mesh* mesh, int elem_offset = 0, int elem_count = 0); - // Sets the mesh of the renderable based on the given geom type. + // Sets the mesh of this renderable to a built-in mesh based on the geom type. void SetGeomMesh(mjtGeom type, int nstack, int nslice, int nquad); - // Sets the transform of the renderable. + // Sets the transform of this renderable. void SetTransform(const Trs& trs); - // Returns the current transform of the renderable. + // Returns the current transform of this renderable. const filament::math::mat4f& GetTransform() const; - // Sets the layer mask for the managed filament Entities. Layer masks can be - // used to show/hide the renderable in different views. Returns the previous - // layer mask. + // Sets the layer mask for this renderable. Layer masks can be used to + // show/hide groups of renderables in scenes. Returns the previous layer mask. std::uint8_t SetLayerMask(std::uint8_t mask); - // Sets the priority for the managed filament Entities. The priority - // determines the order in which renderables are rendered. Returns the - // previous priority. + // Sets the draw priority this renderable. The priority determines the order + // in which renderables are rendered. Returns the previous priority. std::uint8_t SetPriority(std::uint8_t priority); - // Sets the blend order of the managed filament entities. This determines the - // order in which renderables are blended together. Returns the previous blend - // order. + // Sets the blend order for this renderable. This determines the order in + // which transparent renderables are blended together. Returns the previous + // blend order. std::uint16_t SetBlendOrder(std::uint16_t blend_order); - // Disables the renderable from casting shadows. + // Disables this renderable from casting shadows. void SetCastShadows(bool cast_shadows); - // Disables the renderable from receiving shadows. + // Disables this renderable from receiving shadows. void SetReceiveShadows(bool receive_shadows); - // If true, forces all meshes to be rendered using Lines primitives. + // If true, forces this renderable to use wireframe rendering. void SetWireframe(bool wireframe); - // Adds the renderable to the given filament Scene. + // Adds this renderable to the filament Scene. void AddToScene(filament::Scene* scene); - // Removes the renderable from the given filament Scene. + // Removes this renderable from the filament Scene. void RemoveFromScene(filament::Scene* scene); - // Further defines the material of the renderable. + // Determines how this renderable will be drawn. See mjrDrawMode for details. void SetDrawMode(mjrDrawMode mode); - // Updates the parameters for the material. + // Updates the parameters and textures of the material for this renderable. void UpdateMaterial(const mjrMaterial& material); - // Returns the current material parameters. + // Returns this renderable's current material. const mjrMaterial& GetMaterial() const; - // Returns the filament Engine managing the renderables. - filament::Engine* GetEngine(); - static Renderable* downcast(mjrRenderable* renderable) { return static_cast(renderable); } @@ -109,8 +104,9 @@ class Renderable : public mjrRenderable { } private: - using GetTransformFn = std::function; - + // In most cases, a Renderable will be composed of a single filament Entity. + // However, for some built-in geom types (e.g. capsules) we compose the + // renderable out of multiple Entities. struct Part { utils::Entity entity; const Mesh* mesh = nullptr; @@ -118,13 +114,18 @@ class Renderable : public mjrRenderable { int elem_count = 0; }; - void AppendMesh(const Mesh* mesh); + // When composing a multi-part renderable, each Entity will have its own + // transform offset based on the transform of the Renderable itself. + using GetTransformFn = std::function; + void AppendMesh(const Mesh* mesh); void InitPartEntity(Part& part); - void AssignMaterial(mjrDrawMode mode, ObjectManager::MaterialType material_type); - ObjectManager::MaterialType GetColorMaterialType() const; + void AssignMaterial(mjrDrawMode mode, + ObjectManager::MaterialType material_type); + + filament::Engine* GetEngine(); ObjectManager* object_mgr_; mjrRenderableParams params_; diff --git a/src/experimental/filament/render_context_filament.cc b/src/experimental/filament/render_context_filament.cc index b923ed16..220f87cc 100644 --- a/src/experimental/filament/render_context_filament.cc +++ b/src/experimental/filament/render_context_filament.cc @@ -144,9 +144,10 @@ void mjrf_destroyContext(mjrfContext* ctx) { delete mujoco::FilamentContext::downcast(ctx); } -mjrTexture* mjrf_createTexture(mjrfContext* ctx, const mjrTextureConfig* cfg) { +mjrTexture* mjrf_createTexture(mjrfContext* ctx, + const mjrTextureConfig* config) { return new mujoco::Texture( - mujoco::FilamentContext::downcast(ctx)->GetEngine(), *cfg); + mujoco::FilamentContext::downcast(ctx)->GetEngine(), *config); } void mjrf_destroyTexture(mjrTexture* texture) { @@ -212,7 +213,7 @@ mjrSamplerType mjrf_getSamplerType(const mjrTexture* texture) { return mujoco::Texture::downcast(texture)->GetSamplerType(); } -void mjrf_setLightEnabled(mjrLight* light, bool enabled) { +void mjrf_setLightEnabled(mjrLight* light, mjtByte enabled) { if (enabled) { mujoco::Light::downcast(light)->Enable(); } else { @@ -273,17 +274,17 @@ void mjrf_setRenderableLayerMask(mjrRenderable* renderable, mujoco::Renderable::downcast(renderable)->SetLayerMask(layer_mask); } -void mjrf_setRenderableWireframe(mjrRenderable* renderable, bool wireframe) { +void mjrf_setRenderableWireframe(mjrRenderable* renderable, mjtByte wireframe) { mujoco::Renderable::downcast(renderable)->SetWireframe(wireframe); } void mjrf_setRenderableCastShadows(mjrRenderable* renderable, - bool cast_shadows) { + mjtByte cast_shadows) { mujoco::Renderable::downcast(renderable)->SetCastShadows(cast_shadows); } void mjrf_setRenderableReceiveShadows(mjrRenderable* renderable, - bool receive_shadows) { + mjtByte receive_shadows) { mujoco::Renderable::downcast(renderable)->SetReceiveShadows(receive_shadows); } @@ -313,7 +314,7 @@ void mjrf_setSceneSkybox(mjrScene* scene, const mjrTexture* texture) { mujoco::Texture::downcast(texture)); } -void mjrf_setSceneShadowsEnabled(mjrScene* scene, bool enabled) { +void mjrf_setSceneShadowsEnabled(mjrScene* scene, mjtByte enabled) { if (enabled) { mujoco::SceneView::downcast(scene)->EnableShadows(); } else { @@ -321,7 +322,7 @@ void mjrf_setSceneShadowsEnabled(mjrScene* scene, bool enabled) { } } -void mjrf_setSceneReflectionsEnabled(mjrScene* scene, bool enabled) { +void mjrf_setSceneReflectionsEnabled(mjrScene* scene, mjtByte enabled) { if (enabled) { mujoco::SceneView::downcast(scene)->EnableReflections(); } else { diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index 85ec2856..0d7d0cbd 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -29,7 +29,37 @@ extern "C" { // IMPORTANT: This API should still be considered experimental and is likely // change frequently. -// Opaque types. +// This library provides a C API for the filament rendering library +// (https://github.com/google/filament) that is designed to work with the +// MuJoCo library for visualizing simulations. +// +// The filament renderer is a real-time physically based rendering (PBR) engine +// developed by Google. It is designed to be as small as possible and as +// efficient as possible, while still providing high-quality results. It works +// across all major platforms (Linux, Windows, macOS, Android, iOS, Web) and +// supports OpenGL, Vulkan, and Metal. +// +// For the purposes of this API, we assume the reader has a basic understanding +// of rendering concepts (e.g. textures, vertices, cameras, framebuffers, etc.). +// We will also highlight some of the key differences between this renderer and +// the legacy/classic MuJoCo (mjr) renderer. +// +// ## API Overview +// +// There are seven key components: Context, Texture, Mesh, Scene, Light, +// Renderable, and RenderTarget. We'll describe these in detail further below. +// +// Each object is created using a `create` function and destroyed using a +// `destroy` function, e.g. `mjrf_createTexture` and `mjrf_destroyTexture`. +// The `create` functions accept a pointer to a configuration struct (e.g. +// `mjrTextureConfig`) which describes the parameters for the object to be +// created. Each of these structs has a corresponding `default` function (e.g. +// `mjr_defaultTextureConfig`) which can be used to initialize the struct to +// default values. Default values are assumed to be 0/NULL unless otherwise +// specified. +// +// For now, we'll just define opaque handles for each of our components. +struct mjrfContext {}; struct mjrTexture {}; struct mjrMesh {}; struct mjrScene {}; @@ -37,10 +67,66 @@ struct mjrLight {}; struct mjrRenderable {}; struct mjrRenderTarget {}; -// Opaque type for the filament rendering context. -struct mjrfContext {}; -// The different modes that can be used to render a scene. +// ## Rendering Context (mjrfContext) +// +// The Context is the main entry point for the library. It manages all the +// core filament objects that are responsible for the rendering of an image. +// +// Filament uses a separate thread for doing the actual rendering. However, +// despite that, this API is not thread-safe; calls are expected to be made +// from a single thread. Also, due to the asynchronous nature of filament, +// some APIs provide handles or callbacks to signal when an operation is +// complete. (Note: for WASM builds, filament does not use a separate thread.) +// +// All other objects (e.g. Textures, Meshes, Scenes, etc.) need a Context in +// order to be created. Otherwise, the main function to use with the Context is +// `mjrf_render()` which does the actual rendering. +// +// There are two key differences between the mjrfContext and the classic +// mjrContext. Firstly, the filament context will manage the underlying graphics +// context itself. This means users do not need to initialize EGL or similar +// libraries beforehand. Secondly, the filament context is independent of a +// MuJoCo model. That means you can use a single mjrfContext to render images +// for multiple models. + +// Underlying graphics API library to use for the Context. +typedef enum mjrGraphicsApi_ { + // Default, based on current platform. + mjGRAPHICS_API_DEFAULT = 0, + // OpenGL (desktop), GLES (mobile), WebGL (web) + mjGRAPHICS_API_OPENGL, + // Vulkan + mjGRAPHICS_API_VULKAN, +} mjrGraphicsApi; + +// Configuration parameters for the filament rendering context. +struct mjrFilamentConfig { + // The native window handle into which we can render directly. If nullptr, + // rendering will be done to an offscreen framebuffer. + void* native_window; + + // The initial width and height of the offscreen framebuffer. + int width; + int height; + + // The backend graphics API to use. + mjrGraphicsApi graphics_api; + + // Use software rendering even if the platform supports hardware rendering. + mjtByte force_software_rendering; +}; + +// Initializes the mjrFilamentConfig to default values. +void mjrf_defaultFilamentConfig(mjrFilamentConfig* config); + +// Creates a filament rendering context. +mjrfContext* mjrf_createContext(const mjrFilamentConfig* config); + +// Destroys the filament rendering context. +void mjrf_destroyContext(mjrfContext* ctx); + +// Describes the look/intention of the final rendered image. typedef enum mjrDrawMode_ { // Render the scene with "normal" colors and lighting. mjDRAW_MODE_COLOR, @@ -51,19 +137,198 @@ typedef enum mjrDrawMode_ { mjDRAW_MODE_SEGMENTATION, } mjrDrawMode; -enum { mjNUM_DRAW_MODES = 3 }; +enum { mjNUM_DRAW_MODES = 3 }; // Number of modes in `mjrDrawMode`. -// The type of data stored in an index buffer. -typedef enum mjrIndexType_ { - mjINDEX_TYPE_U16 = 0, - mjINDEX_TYPE_U32, -} mjrIndexType; +// Parameters describing the camera to use for rendering an image. +typedef mjvGLCamera mjrCamera; -// The type of primitive to be drawn by vertex data. -typedef enum mjrMeshPrimitiveType_ { - mjMESH_PRIMITIVE_TYPE_TRIANGLES = 0, - mjMESH_PRIMITIVE_TYPE_LINES, -} mjrMeshPrimitiveType; +// Describes a single rendering operation; used by `mjrf_render()`. +struct mjrRenderRequest { + // The scene to render. + mjrScene* scene; + + // The camera from which to render the scene. + mjrCamera camera; + + // The method (e.g. Color, Depth, Segmentation, etc.) to use for rendering. + mjrDrawMode draw_mode; + + // The viewport into which to render the image. + mjrRect viewport; + + // The render target into which to render the image. If nullptr, the image + // will be rendered to the window (as previously configured in + // mjrFilamentConfig::native_window). + mjrRenderTarget* target; +}; + +// Initializes the mjrRenderRequest to default values. +void mjr_defaultRenderRequest(mjrRenderRequest* request); + +// Information needed to read pixels; used by `mjrf_render()`. +struct mjrReadPixelsRequest { + // The render target from which to read the image pixels. + mjrRenderTarget* target; + + // The buffer into which the read pixels will be written. + void* output; + + // The number of bytes in the output buffer. This should match the size of + // the render target texture. + mjtSize num_bytes; + + // Callback when the read pixels operation is complete. This function can + // optionally be used to free the output buffer if needed. + void (*read_completed_callback)(void* user_data); + + // User data to pass to the completion callback. + void* user_data; +}; + +// Initializes the mjrReadPixelsRequest to default values. +void mjr_defaultReadPixelsRequest(mjrReadPixelsRequest* request); + +// Because rendering is asynchronous, each render request is assigned a +// unique Handle which can be used to query the status of the request. The +// Handle can also be used to block until the request is completed. +typedef std::uint64_t mjrFrameHandle; + +// Submits the given requests for rendering. Because rendering may happen +// asynchronously, we have to submit both the render and read requests in the +// same call. This function is also when any callbacks will be triggered, +// though there is no guarantee on when exactly that will be done. +// +// Multiple requests and reads can be submitted in a single call. These +// requests will be processed in order, so some care must be taken. Firstly, +// requests should be grouped by target. Next, the combined area of the +// viewports for all requests for a given target must be contained within the +// dimensions of the target itself. +mjrFrameHandle mjrf_render(mjrfContext* ctx, const mjrRenderRequest* req, + int nreq, const mjrReadPixelsRequest* read_req, + int nread_req); + +// Waits for all rendering operations to complete for the given frame handle, +// triggering any callbacks as needed. +void mjrf_waitForFrame(mjrfContext* ctx, mjrFrameHandle frame); + +// Information about a single frame of rendering. +struct mjrFrameStats { + // The frame rate of the renderer, in frames per second. + double frame_rate; +}; + +// Initializes the mjrFrameStats to default values. +void mjr_defaultFrameStats(mjrFrameStats* stats); + +// Returns the stats for the given frame but updating the given `stats_out`. +void mjrf_getFrameStats(mjrfContext* ctx, mjrFrameHandle frame, + mjrFrameStats* stats_out); + +// ## Textures (mjrTexture) +// +// A texture is a 2D or 3D (cubemap) image that adds visual detail to a rendered +// model, such as color or bumpiness, without increasing geometric complexity. +// +// For textures intended to be used for image-based lights (see `mjrLight` +// below), you should use filament's `cmgen` tool to generate a KTX image from +// your source image. This tool will calculate additional data (i.e. the +// spherical harmonics) and encode that information into the KTX file. + +// Pixel formats for textures. +typedef enum mjrPixelFormat_ { + mjPIXEL_FORMAT_UNKNOWN = 0, + mjPIXEL_FORMAT_R8, + mjPIXEL_FORMAT_RGB8, + mjPIXEL_FORMAT_RGBA8, + mjPIXEL_FORMAT_R32F, + mjPIXEL_FORMAT_DEPTH32F, + mjPIXEL_FORMAT_KTX, +} mjrPixelFormat; + +// Type of texture. +typedef mjtTexture mjrSamplerType; + +// Type of color space encoding. +typedef mjtColorSpace mjrColorSpace; + +// Defines the basic properties of a texture. +struct mjrTextureConfig { + // The width of the texture. For compressed textures (e.g. KTX), this is the + // number of bytes in the compressed data. + int width; + + // The height of the texture. For compressed textures (e.g. KTX), this should + // be 0. + int height; + + // How the texture will be interpreted by the renderer (e.g. 2D, cube, etc.). + mjrSamplerType sampler_type; + + // The format of the pixels in the texture (e.g. RGB8, RGBA8, KTX, etc.) + mjrPixelFormat format; + + // The color space of the texture (e.g. LINEAR, sRGB, etc.) + mjrColorSpace color_space; +}; + +// Initializes the mjrTextureConfig to default values. +void mjr_defaultTextureConfig(mjrTextureConfig* config); + +// Creates a texture with the given configuration. Note that the texture will +// not be created on the GPU until `mjrf_setTextureData()` is called. +mjrTexture* mjrf_createTexture(mjrfContext* ctx, const mjrTextureConfig* config); + +// Destroys the texture. +void mjrf_destroyTexture(mjrTexture* texture); + +// The binary data for a texture. +struct mjrTextureData { + // Pointer to the data. If null, an empty texture will be created. + const void* bytes; + + // The number of bytes in the image data. + mjtSize nbytes; + + // Because rendering may be multithreaded, we cannot make assumptions about + // when the image data will finish uploading to the GPU. As such, we will use + // this callback to notify callers when it is safe to free the image data. + void (*release_callback)(void* user_data); + + // User data to pass to the release callback. + void* user_data; +}; + +// Initializes the mjrTextureData to default values. +void mjr_defaultTextureData(mjrTextureData* data); + +// Uploads the given texture data to the texture. +void mjrf_setTextureData(mjrTexture* texture, const mjrTextureData* data); + +// Returns the width of the texture. +int mjrf_getTextureWidth(const mjrTexture* texture); + +// Returns the height of the texture. +int mjrf_getTextureHeight(const mjrTexture* texture); + +// Returns the target type of the texture. +mjrSamplerType mjrf_getSamplerType(const mjrTexture* texture); + +// ## Meshes (mjrMesh) +// +// A mesh describes the surface geometry of an object to be rendered. It is +// defined as a collection of vertices (i.e. a VertexBuffer), a set of indices +// (i.e. an IndexBuffer) that describes the order in which the vertices should +// be processed, and a primitive type that defined how the vertices are to be +// interpreted (e.g. triangles, lines, etc.) when rendering the surface. +// +// Filament does not directly support normals. Instead, it encodes the normal, +// tangen, and bitangent into a 4-component quaternion describing the +// "orientation" of the vertex. Ideally, you should preprocess your assets +// to generate this data offline, but we will compute it on the fly if needed +// (at a performance cost). +// +// We also suggest precomputing the bounds of the mesh, otherwise we will also +// compute it on the fly. // The usage/purpose of an attribute of a vertex. typedef enum mjrVertexAttributeUsage_ { @@ -82,167 +347,17 @@ typedef enum mjrVertexAttributeType_ { mjVERTEX_ATTRIBUTE_TYPE_UBYTE4, } mjrVertexAttributeType; -// Pixel formats for textures. -typedef enum mjrPixelFormat_ { - mjPIXEL_FORMAT_UNKNOWN = 0, - mjPIXEL_FORMAT_R8, - mjPIXEL_FORMAT_RGB8, - mjPIXEL_FORMAT_RGBA8, - mjPIXEL_FORMAT_R32F, - mjPIXEL_FORMAT_DEPTH32F, - mjPIXEL_FORMAT_KTX, -} mjrPixelFormat; +// The type of data stored in an index buffer. +typedef enum mjrIndexType_ { + mjINDEX_TYPE_U16 = 0, + mjINDEX_TYPE_U32, +} mjrIndexType; -typedef enum mjrGraphicsApi_ { // backend graphics API to use - mjGRAPHICS_API_DEFAULT = 0, // default based on platform - mjGRAPHICS_API_OPENGL, // OpenGL (desktop) / WebGL - mjGRAPHICS_API_VULKAN // Vulkan -} mjrGraphicsApi; - - -// Rendering is asynchronous by nature. Each render request is assigned a -// unique Handle which can be used to query the status of the request. The -// Handle can also be used to block until the request is completed. -typedef std::uint64_t mjrFrameHandle; - -// Bring some legacy mjt types into the mjr namespace. -typedef mjtTexture mjrSamplerType; -typedef mjtColorSpace mjrColorSpace; -typedef mjtLightType mjrLightType; -typedef mjvGLCamera mjrCamera; - -// The material to be applied to a renderable. -struct mjrMaterial { - // The color of the object. Defaults to white. - float color[4]; - - // The color to use for segmentation rendering. Defaults to white. - float segmentation_color[4]; - - // Applies an addition scale to the UV coordinates of the object. Defaults to - // (1, 1, 1). - float uv_scale[3]; - - // Applies an offset to the UV coordinates of the object. Defaults to (0, 0, - // 0). - float uv_offset[3]; - - // Applies a scissor test to the object. - float scissor[4]; - - // Factors for PBR metallic-roughness materials. - float metallic; - float roughness; - - // Factors for (non-PBR) specular-glossiness materials. - float specular; - float glossiness; - - // The emissive (glow) factor of the object. - float emissive; - - // Whether or not the object is a reflective surface. Only applies to planes. - mjtByte reflective; - // The blend factor to use for reflective surfaces. A value of 1.0 means that - // the surface is fully reflective (i.e. a mirror). - float reflectance; - - // If true, does not apply any lighting to the object. (Assumes the object is - // used for UX or decorative elements like contact forces and labels.) - mjtByte decor_ux; - - // The texture containing the base color of the object. - const mjrTexture* color_texture; - - // The normal map of the object. - const mjrTexture* normal_texture; - - // The metallic map of the object. - const mjrTexture* metallic_texture; - - // The roughness map of the object. - const mjrTexture* roughness_texture; - - // The occlusion map of the object. - const mjrTexture* occlusion_texture; - - // A texture containing the occlusion, roughness, and metallic maps packed - // into the R, G, B channels, respectively. - const mjrTexture* orm_texture; - - // An emissive texture for the object. - const mjrTexture* emissive_texture; - - // The reflection texture to use for the object. For internal use only. - const mjrTexture* reflection_texture; -}; - -// Initializes the mjrMaterial to default values. -void mjr_defaultMaterial(mjrMaterial* material); - -// The binary contents of a texture. -struct mjrTextureData { - // Pointer to the image data. If null, an empty texture will be created. - const void* bytes; - - // The number of bytes in the image data. - mjtSize nbytes; - - // Because rendering may be multithreaded, we cannot make assumptions about - // when the image data will finish uploading to the GPU. As such, we will use - // this callback to notify callers when it is safe to free the image data. - void (*release_callback)(void* user_data); - - // User data to pass to the release callback. - void* user_data; -}; - -// Initializes the mjrTextureData to default values. -void mjr_defaultTextureData(mjrTextureData* data); - -// Defines the basic properties of a texture. -struct mjrTextureConfig { - // The width of the texture. For compressed textures (e.g. KTX), this is the - // number of bytes in the compressed data. - int width; - - // The height of the texture. For compressed textures (e.g. KTX), this should - // be 0. - int height; - - // The target of the texture (e.g. 2D, cube, etc.) - mjrSamplerType sampler_type; - - // The format of the pixels in the texture (e.g. RGB8, RGBA8, KTX, etc.) - mjrPixelFormat format; - - // The color space of the texture (e.g. LINEAR, sRGB, etc.) - mjrColorSpace color_space; -}; - -// Initializes the mjrTextureConfig to default values. -void mjr_defaultTextureConfig(mjrTextureConfig* config); - -// Configuration parameters for a Renderable. -struct mjrRenderableParams { - // Whether or not the Renderable casts shadows. - mjtByte cast_shadows; - // Whether or not the Renderable receives shadows. - mjtByte receive_shadows; - // The layers to which the Renderable belongs. This mask is used in - // conjunction with the layer mask in the Scene to determine which - // Renderables to render. Defaults to 0xff. - uint8_t layer_mask; - // Controls the order in which the Renderable is drawn relative to other - // Renderables; defaults to 4. - uint8_t priority; - // Similar to priority, but provides finer-grained control for Renderables - // with transparency; defaults to 0. - uint16_t blend_order; -}; - -// Initializes the mjrRenderableParams to default values. -void mjr_defaultRenderableParams(mjrRenderableParams* params); +// The type of primitive to be drawn by vertex data. +typedef enum mjrMeshPrimitiveType_ { + mjMESH_PRIMITIVE_TYPE_TRIANGLES = 0, + mjMESH_PRIMITIVE_TYPE_LINES, +} mjrMeshPrimitiveType; // Information about a single attribute of a vertex. struct mjrVertexAttribute { @@ -317,17 +432,33 @@ struct mjrMeshData { // Initializes the mjrMeshData to default values. void mjr_defaultMeshData(mjrMeshData* data); +// Creates a mesh with the given data. +mjrMesh* mjrf_createMesh(mjrfContext* ctx, const mjrMeshData* data); + +// Destroys the mesh. +void mjrf_destroyMesh(mjrMesh* mesh); + +// ## Scenes (mjrScene) +// +// A scene is a collection of entities (Lights and Renderables) that defines +// what is to be rendered. It also specifies the various effects that are to be +// applied to the rendering (e.g. shadows, reflections, post-processing, etc.) + // Configuration parameters for a Scene. struct mjrSceneParams { // Whether or not to enable post processing; enabled by default. mjtByte enable_post_processing; + // Whether or not to enable reflections; enabled by default. mjtByte enable_reflections; + // Whether or not to enable shadows; enabled by default. mjtByte enable_shadows; + // This mask, in conjunction with the layer mask in the Renderable, determines // which Renderables to render within the Scene. uint8_t layer_mask; + // The layer mask to use for reflections. uint8_t reflection_layer_mask; }; @@ -335,6 +466,59 @@ struct mjrSceneParams { // Initializes the mjrSceneParams to default values. void mjr_defaultSceneParams(mjrSceneParams* params); +// Creates a scene with the given parameters. +mjrScene* mjrf_createScene(mjrfContext* ctx, const mjrSceneParams* params); + +// Destroys the scene. +void mjrf_destroyScene(mjrScene* scene); + +// Adds a light to the scene. +void mjrf_addLightToScene(mjrScene* scene, mjrLight* light); + +// Removes the light from the scene. +void mjrf_removeLightFromScene(mjrScene* scene, mjrLight* light); + +// Adds a renderable to the scene. +void mjrf_addRenderableToScene(mjrScene* scene, mjrRenderable* renderable); + +// Removes the renderable from the scene. +void mjrf_removeRenderableFromScene(mjrScene* scene, mjrRenderable* renderable); + +// Sets the skybox (cube texture) for the scene. +void mjrf_setSceneSkybox(mjrScene* scene, const mjrTexture* texture); + +// Enables (or disables) shadows in the scene. +void mjrf_setSceneShadowsEnabled(mjrScene* scene, mjtByte enabled); + +// Enables (or disables) reflections in the scene. +void mjrf_setSceneReflectionsEnabled(mjrScene* scene, mjtByte enabled); + +// Configures the scene based on the parameters in an mjModel. +void mjrf_configureSceneFromModel(mjrScene* scene, const mjModel* model); + +// ## Lights (mjrLight) +// +// A light is a source of illumination in the scene. (Without lights, a scene +// will be completely black.) There are several different types of lights such +// as directional, spot, point, and image lights. +// +// The primary light in a scene is the image light (also sometimes known as the +// environment light). This is a light that "surrounds" the entire scene and +// is defined as a 3D texture. Each "pixel" of the cubemap is interpreted as the +// color of projected into the scene from a particular direction. +// +// Directional lights are the next most common type of light and is usually +// used to simulate the sun; a uniformly colored light that is emitted in a +// single direction. +// +// Filament only supports a single image and directional light. You can define +// as many point or spot lights as you want. Each light source (except image +// based lights) may or may not cast shadows. Each shadow-casting light incurs a +// performance cost. + +// The type of light (spot, directional, image, etc.). +typedef mjtLightType mjrLightType; + // Configuration parameters for a light. struct mjrLightParams { // The type of light (e.g. spot, point, directional, etc.) @@ -362,152 +546,14 @@ struct mjrLightParams { // Initializes the mjrLightParams to default values. void mjr_defaultLightParams(mjrLightParams* params); -// Defines the basic properties of a render target. -struct mjrRenderTargetConfig { - // The width of the render target. - int width; - // The height of the render target. - int height; - // The format of the color buffer in the render target. - mjrPixelFormat color_format; - // The format of the depth buffer in the render target. - mjrPixelFormat depth_format; -}; - -// Initializes the RenderTargetConfig to default values. -void mjr_defaultRenderTargetConfig(mjrRenderTargetConfig* config); - -// Information needed to render a single image of a scene. -struct mjrRenderRequest { - // The scene to render. - mjrScene* scene; - - // The method (e.g. Color, Depth, Segmentation, etc.) to use for rendering. - mjrDrawMode draw_mode; - - // The camera from which to render the scene. - mjrCamera camera; - - // The viewport into which to render the image. - mjrRect viewport; - - // The render target into which to render the image. If nullptr, the image - // will be rendered to the window (as previously configured in - // mjrFilamentConfig::native_window). - mjrRenderTarget* target; -}; - -// Initializes the mjrRenderRequest to default values. -void mjr_defaultRenderRequest(mjrRenderRequest* request); - -// Information needed to read pixels from a render target. -struct mjrReadPixelsRequest { - mjrRenderTarget* target; - - // The buffer into which the read pixels will be written. - void* output; - - // The number of bytes in the output buffer. This should match the size of - // the render target texture. - mjtSize num_bytes; - - // Callback when the read pixels operation is complete. This will be called - // during WaitForFrame() or in a subsequent call to Render(). This function - // can optionally be used to free the output buffer if needed. - void (*read_completed_callback)(void* user_data); - - // User data to pass to the completion callback. - void* user_data; -}; - -// Initializes the mjrReadPixelsRequest to default values. -void mjr_defaultReadPixelsRequest(mjrReadPixelsRequest* request); - -// Information about a single frame of rendering. -struct mjrFrameStats { - // The frame rate of the renderer, in frames per second. - double frame_rate; -}; - -// Initializes the mjrFrameStats to default values. -void mjr_defaultFrameStats(mjrFrameStats* stats); - -// Configuration parameters for the filament rendering context. -struct mjrFilamentConfig { - // The native window handle into which we can render directly. - void* native_window; - - // The initial width and height of the offscreen framebuffer. - int width; - int height; - - // The backend graphics API to use. - int graphics_api; - - // Use software rendering even if the platform supports hardware rendering. - bool force_software_rendering; -}; - -// Initializes the mjrFilamentConfig to default values. -void mjrf_defaultFilamentConfig(mjrFilamentConfig* config); - -// Creates a filament rendering context. -mjrfContext* mjrf_createContext(const mjrFilamentConfig* config); - -// Destroys the filament rendering context. -void mjrf_destroyContext(mjrfContext* ctx); - -// Creates a texture for the filament renderer. -mjrTexture* mjrf_createTexture(mjrfContext* ctx, const mjrTextureConfig* cfg); - -// Destroys the texture. -void mjrf_destroyTexture(mjrTexture* texture); - -// Creates a mesh for the filament renderer. -mjrMesh* mjrf_createMesh(mjrfContext* ctx, const mjrMeshData* data); - -// Destroys the mesh. -void mjrf_destroyMesh(mjrMesh* mesh); - -// Creates a scene for the filament renderer. -mjrScene* mjrf_createScene(mjrfContext* ctx, const mjrSceneParams* params); - -// Destroys the scene. -void mjrf_destroyScene(mjrScene* scene); - // Creates a light for the filament renderer. mjrLight* mjrf_createLight(mjrfContext* ctx, const mjrLightParams* params); // Destroys the light. void mjrf_destroyLight(mjrLight* light); -// Creates a renderable for the filament renderer. -mjrRenderable* mjrf_createRenderable(mjrfContext* ctx, const mjrRenderableParams* params); - -// Destroys the renderable. -void mjrf_destroyRenderable(mjrRenderable* renderable); - -// Creates a render target for the filament renderer. -mjrRenderTarget* mjrf_createRenderTarget(mjrfContext* ctx, - const mjrRenderTargetConfig* config); - -// Destroys the render target. -void mjrf_destroyRenderTarget(mjrRenderTarget* render_target); - -// Uploads the given texture data to the texture. -void mjrf_setTextureData(mjrTexture* texture, const mjrTextureData* data); - -// Returns the width of the texture. -int mjrf_getTextureWidth(const mjrTexture* texture); - -// Returns the height of the texture. -int mjrf_getTextureHeight(const mjrTexture* texture); - -// Returns the sampler type of the texture. -mjrSamplerType mjrf_getSamplerType(const mjrTexture* texture); - // Enables or disables the light. -void mjrf_setLightEnabled(mjrLight* light, bool enabled); +void mjrf_setLightEnabled(mjrLight* light, mjtByte enabled); // Sets the intensity of the light, in candela. void mjrf_setLightIntensity(mjrLight* light, float intensity); @@ -522,6 +568,120 @@ void mjrf_setLightTransform(mjrLight* light, const float position[3], // Returns the type of the light. mjrLightType mjrf_getLightType(const mjrLight* light); +// ## Renderables (mjrRenderable) +// +// A renderable is a single drawable object in the scene. It is defined as a +// combination of a mesh (i.e. surface geometry) and a material (i.e. surface +// appearance and properties). +// +// In terms of materials, there are three lighting models currently supported: +// +// 1. Metallic-roughness (PBR): this is the preferred model for rendering +// models based standard metallic-roughness workflows. +// 2. Specular-glossiness (non-PBR): this is a legacy model designed to be +// compatible with classic mjr renderer, though it is not 100% identical. +// 3. Unlit: this model ignores lighting and used for rendering UX or decorative +// elements like contact forces and labels. +// +// Which lighting model is used is determined by the mjrMaterial properties. + +// The material to be applied to a renderable. +struct mjrMaterial { + // The color of the object. Defaults to white. + float color[4]; + + // The color to use for segmentation rendering. Defaults to white. + float segmentation_color[4]; + + // Applies an addition scale to the UV coordinates of the object. Defaults to + // (1, 1, 1). + float uv_scale[3]; + + // Applies an offset to the UV coordinates of the object. Defaults to (0, 0, + // 0). + float uv_offset[3]; + + // Applies a scissor test to the object. + float scissor[4]; + + // Factors for PBR metallic-roughness materials. + float metallic; + float roughness; + + // Factors for (non-PBR) specular-glossiness materials. + float specular; + float glossiness; + + // The emissive (glow) factor of the object. + float emissive; + + // Whether or not the object is a reflective surface. Only applies to planes. + mjtByte reflective; + // The blend factor to use for reflective surfaces. A value of 1.0 means that + // the surface is fully reflective (i.e. a mirror). + float reflectance; + + // If true, does not apply any lighting to the object. Assumes the object is + // used for UX or decorative elements like contact forces and labels. + mjtByte decor_ux; + + // The texture containing the base color of the object. + const mjrTexture* color_texture; + + // The normal map of the object. + const mjrTexture* normal_texture; + + // The metallic map of the object. + const mjrTexture* metallic_texture; + + // The roughness map of the object. + const mjrTexture* roughness_texture; + + // The occlusion map of the object. + const mjrTexture* occlusion_texture; + + // A texture containing the occlusion, roughness, and metallic maps packed + // into the R, G, B channels, respectively. + const mjrTexture* orm_texture; + + // An emissive texture for the object. + const mjrTexture* emissive_texture; + + // The reflection texture to use for the object. For internal use only. + const mjrTexture* reflection_texture; +}; + +// Initializes the mjrMaterial to default values. +void mjr_defaultMaterial(mjrMaterial* material); + +// Configuration parameters for a Renderable. +struct mjrRenderableParams { + // Whether or not the Renderable casts shadows. + mjtByte cast_shadows; + // Whether or not the Renderable receives shadows. + mjtByte receive_shadows; + // The layers to which the Renderable belongs. This mask is used in + // conjunction with the layer mask in the Scene to determine which + // Renderables to render. Defaults to 0xff. + uint8_t layer_mask; + // Controls the order in which the Renderable is drawn relative to other + // Renderables; defaults to 4. + uint8_t priority; + // Similar to priority, but provides finer-grained control for Renderables + // with transparency; defaults to 0. + uint16_t blend_order; +}; + +// Initializes the mjrRenderableParams to default values. +void mjr_defaultRenderableParams(mjrRenderableParams* params); + +// Creates a renderable with the given parameters. +mjrRenderable* mjrf_createRenderable(mjrfContext* ctx, + const mjrRenderableParams* params); + +// Destroys the renderable. +void mjrf_destroyRenderable(mjrRenderable* renderable); + // Sets the mesh of the renderable. void mjrf_setRenderableMesh(mjrRenderable* renderable, const mjrMesh* mesh, int elem_offset, int elem_count); @@ -536,66 +696,61 @@ void mjrf_setRenderableGeomMesh(mjrRenderable* renderable, mjtGeom type, void mjrf_setRenderableMaterial(mjrRenderable* renderable, const mjrMaterial* material); -// Sets the transform (position, rotation, and size) of the renderable. +// Sets the transform (position, rotation, and size) of the renderable. Note +// that `size` is not the same as `scale`. For example, the z-size of a capsule +// only scales the tubular-portion of its geometry, but not the spherical caps. void mjrf_setRenderableTransform(mjrRenderable* renderable, const float position[3], const float rotation[9], const float size[3]); // Sets whether the renderable casts shadows or not. void mjrf_setRenderableCastShadows(mjrRenderable* renderable, - bool cast_shadows); + mjtByte cast_shadows); // Sets whether the renderable receives shadows or not. void mjrf_setRenderableReceiveShadows(mjrRenderable* renderable, - bool receive_shadows); + mjtByte receive_shadows); // Forces the renderable to be rendered using lines. -void mjrf_setRenderableWireframe(mjrRenderable* renderable, bool wireframe); +void mjrf_setRenderableWireframe(mjrRenderable* renderable, mjtByte wireframe); // Sets the layer mask of the renderable. See mjrRenderableParams for details. void mjrf_setRenderableLayerMask(mjrRenderable* renderable, uint8_t layer_mask); -// Adds the light to the scene. -void mjrf_addLightToScene(mjrScene* scene, mjrLight* light); +// ## Render Targets (mjrRenderTarget) +// +// A render target is a memory buffer that holds the results of a rendering +// operation. (This is an alternative to rendering directly to the screen.) +// See mjrf_render for more details. -// Removes the light from the scene. -void mjrf_removeLightFromScene(mjrScene* scene, mjrLight* light); +// Defines the basic properties of a render target. +struct mjrRenderTargetConfig { + // The width of the render target. + int width; + // The height of the render target. + int height; + // The format of the color buffer in the render target. + mjrPixelFormat color_format; + // The format of the depth buffer in the render target. + mjrPixelFormat depth_format; +}; -// Adds the renderable to the scene. -void mjrf_addRenderableToScene(mjrScene* scene, mjrRenderable* renderable); +// Initializes the RenderTargetConfig to default values. +void mjr_defaultRenderTargetConfig(mjrRenderTargetConfig* config); -// Removes the renderable from the scene. -void mjrf_removeRenderableFromScene(mjrScene* scene, mjrRenderable* renderable); +// Creates a render target for the filament renderer. +mjrRenderTarget* mjrf_createRenderTarget(mjrfContext* ctx, + const mjrRenderTargetConfig* config); -// Sets the skybox texture of the scene. -void mjrf_setSceneSkybox(mjrScene* scene, const mjrTexture* texture); +// Destroys the render target. +void mjrf_destroyRenderTarget(mjrRenderTarget* render_target); -// Enables (or disables) shadows in the scene.. -void mjrf_setSceneShadowsEnabled(mjrScene* scene, bool enabled); - -// Enables (or disables) reflections in the scene. -void mjrf_setSceneReflectionsEnabled(mjrScene* scene, bool enabled); - -// Configures the scene based on the parameters in the model. -void mjrf_configureSceneFromModel(mjrScene* scene, const mjModel* model); - -// Submits the given requests for rendering. -mjrFrameHandle mjrf_render(mjrfContext* ctx, const mjrRenderRequest* req, - int nreq, const mjrReadPixelsRequest* read_req, - int nread_req); - -// Waits for the rendering to complete for the given frame handle. -void mjrf_waitForFrame(mjrfContext* ctx, mjrFrameHandle frame); - -// Returns the stats for the given frame but updating the given `stats_out`. -void mjrf_getFrameStats(mjrfContext* ctx, mjrFrameHandle frame, - mjrFrameStats* stats_out); +// ## Debug-only functions. // Draws an ImGui editor for the given scene, exposing filament-specific // settings. void mjrf_DEBUG_drawImguiEditor(mjrScene* scene); - // Legacy API, to be deprecated. void mjrf_defaultFilamentConfig(mjrFilamentConfig* config); From eddfbcfc5053e4c65687e27a67d494cda6ea6be4 Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Thu, 7 May 2026 10:51:28 -0700 Subject: [PATCH 221/251] Move mjz_decoder out of experimental to xml/mjz. Since the mjz format is just an archive for MJCF assets this seems like a sensible place. The upcoming mjz encoder will also need to make use of the full precision XML utility that is local to src/xml. PiperOrigin-RevId: 912044746 Change-Id: I6a9fef24b1c3fec5b5edc3a8d4c8e22584273264 --- CMakeLists.txt | 2 +- src/{experimental => xml}/mjz/CMakeLists.txt | 24 ++++--------------- src/{experimental => xml}/mjz/mjz_decoder.cc | 0 test/xml/mjz/CMakeLists.txt | 19 +++++++++++++++ .../mjz/mjz_decoder_test.cc | 0 5 files changed, 24 insertions(+), 21 deletions(-) rename src/{experimental => xml}/mjz/CMakeLists.txt (60%) rename src/{experimental => xml}/mjz/mjz_decoder.cc (100%) create mode 100644 test/xml/mjz/CMakeLists.txt rename test/{experimental => xml}/mjz/mjz_decoder_test.cc (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 14fb6eec..08e0bd0b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -256,7 +256,7 @@ if(MUJOCO_BUILD_SIMULATE) endif() if(MUJOCO_BUILD_STUDIO) - add_subdirectory(src/experimental/mjz) + add_subdirectory(src/xml/mjz) add_subdirectory(src/experimental/platform) add_subdirectory(src/experimental/studio) endif() diff --git a/src/experimental/mjz/CMakeLists.txt b/src/xml/mjz/CMakeLists.txt similarity index 60% rename from src/experimental/mjz/CMakeLists.txt rename to src/xml/mjz/CMakeLists.txt index c1dcc4f7..198b4281 100644 --- a/src/experimental/mjz/CMakeLists.txt +++ b/src/xml/mjz/CMakeLists.txt @@ -11,27 +11,11 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. - -cmake_minimum_required(VERSION 3.16) - -set(MUJOCO_MJZ_TARGET_NAME mujoco_mjz) - -add_library(${MUJOCO_MJZ_TARGET_NAME} STATIC) - -target_include_directories(${MUJOCO_MJZ_TARGET_NAME} PRIVATE - ../.. -) - -target_sources(${MUJOCO_MJZ_TARGET_NAME} - PUBLIC - mjz_decoder.cc -) - include(third_party_deps/miniz) -target_link_libraries(${MUJOCO_MJZ_TARGET_NAME} - miniz - mujoco::mujoco +set(MUJOCO_MJZ_SRCS + mjz_decoder.cc ) -add_library(mujoco::mjz ALIAS ${MUJOCO_MJZ_TARGET_NAME}) +target_sources(mujoco PRIVATE ${MUJOCO_MJZ_SRCS}) +target_link_libraries(mujoco miniz) diff --git a/src/experimental/mjz/mjz_decoder.cc b/src/xml/mjz/mjz_decoder.cc similarity index 100% rename from src/experimental/mjz/mjz_decoder.cc rename to src/xml/mjz/mjz_decoder.cc diff --git a/test/xml/mjz/CMakeLists.txt b/test/xml/mjz/CMakeLists.txt new file mode 100644 index 00000000..721be4fc --- /dev/null +++ b/test/xml/mjz/CMakeLists.txt @@ -0,0 +1,19 @@ +# Copyright 2021 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# https://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +mujoco_test(mjz_api_test) + +mujoco_test( + mjz_decoder_test +) diff --git a/test/experimental/mjz/mjz_decoder_test.cc b/test/xml/mjz/mjz_decoder_test.cc similarity index 100% rename from test/experimental/mjz/mjz_decoder_test.cc rename to test/xml/mjz/mjz_decoder_test.cc From 3e960ba3d3296ce778731f890dc9f65a4ac33592 Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Thu, 7 May 2026 11:06:37 -0700 Subject: [PATCH 222/251] Fix mjcPhysics doc link in changelog. PiperOrigin-RevId: 912053636 Change-Id: Ie711995e171ae9dc97766a82902ba6917b08cd79 --- doc/changelog.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 50f0e2b1..19436ea2 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -37,7 +37,7 @@ General Bug fixes ^^^^^^^^^ -- Fixed default for multiccd in :ref:`mjcPhysics`. +- Fixed default for multiccd in :doc:`mjcPhysics `. Python ^^^^^^ From f4ab9aa93d00d171c406e48d64580ae6db12c69e Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Thu, 7 May 2026 11:32:48 -0700 Subject: [PATCH 223/251] Make miniz a PRIVATE dependency of mujoco in CMake. PiperOrigin-RevId: 912068267 Change-Id: I5d4f3bbd294c6d4fcc519ae4f36f13005d51f9b7 --- src/xml/mjz/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/xml/mjz/CMakeLists.txt b/src/xml/mjz/CMakeLists.txt index 198b4281..5d07c7bb 100644 --- a/src/xml/mjz/CMakeLists.txt +++ b/src/xml/mjz/CMakeLists.txt @@ -18,4 +18,4 @@ set(MUJOCO_MJZ_SRCS ) target_sources(mujoco PRIVATE ${MUJOCO_MJZ_SRCS}) -target_link_libraries(mujoco miniz) +target_link_libraries(mujoco PRIVATE miniz) From b7b96d2accc3cffe10c7252b5105002b9aefb741 Mon Sep 17 00:00:00 2001 From: Sam Haves Date: Thu, 7 May 2026 14:34:43 -0700 Subject: [PATCH 224/251] Remove explicit mjz linking from studio. PiperOrigin-RevId: 912157162 Change-Id: Ie5a229836cfe2c4af1b2f3cc9de9f03623b5d815 --- src/experimental/studio/CMakeLists.txt | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/experimental/studio/CMakeLists.txt b/src/experimental/studio/CMakeLists.txt index 6dfd90f5..11681425 100644 --- a/src/experimental/studio/CMakeLists.txt +++ b/src/experimental/studio/CMakeLists.txt @@ -54,14 +54,6 @@ function(configure_studio_target TARGET_NAME) mujoco::platform ) - # TODO: re-enable mjz support on Windows builds once DllMain issue is resolved. - if (NOT WIN32) - target_link_libraries(${TARGET_NAME} - PRIVATE - mujoco::mjz - ) - endif() - file(MAKE_DIRECTORY ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/assets) add_custom_command( From 3ef083eecc777c58c63d39dd68d214c7ae366bb5 Mon Sep 17 00:00:00 2001 From: Michael Moss Date: Fri, 8 May 2026 02:51:24 -0700 Subject: [PATCH 225/251] Allow the Studio build script to be run outside the git checkout. PiperOrigin-RevId: 912421771 Change-Id: Ie355d2f59cd43166ab06aa49349d87a0f5f9151c --- src/experimental/studio/build.sh | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/experimental/studio/build.sh b/src/experimental/studio/build.sh index 8b8b116b..59b521ca 100755 --- a/src/experimental/studio/build.sh +++ b/src/experimental/studio/build.sh @@ -51,7 +51,8 @@ if [[ "$explicit_action" == false ]]; then do_build=true fi -cd "$(git rev-parse --show-toplevel)" +GIT_ROOT="${GIT_ROOT:-$(git rev-parse --show-toplevel)}" +cd "${GIT_ROOT}" # Configure MuJoCo Studio if [[ "$do_configure" == true ]]; then @@ -97,8 +98,8 @@ if [[ "$do_build" == true ]]; then echo "Use the following command to run mujoco_studio" echo "" if [[ "${OS_NAME}" == "windows" ]]; then - echo " cd $(git rev-parse --show-toplevel)/build/bin && ./${build_type}/mujoco_studio.exe " + echo " cd ${GIT_ROOT}/build/bin && ./${build_type}/mujoco_studio.exe " else - echo " cd $(git rev-parse --show-toplevel)/build/bin && ./mujoco_studio " + echo " cd ${GIT_ROOT}/build/bin && ./mujoco_studio " fi fi From 2ad01960c6e660a930fdb18dca4179ec2eaad8b2 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 8 May 2026 03:32:13 -0700 Subject: [PATCH 226/251] Studio UI tweaks - RHS: Make Inspector be focused by default. - RHS: Remove Noise pane, put the functionality in the Controls pane. - Improve Frame and Label dropdowns. PiperOrigin-RevId: 912434712 Change-Id: I2ee5b647febfc4a90a0cf9387b1d416f5a0dd4a1 --- src/experimental/platform/ux/gui.cc | 10 ++++++---- src/experimental/studio/app.cc | 29 +++++++++++++---------------- 2 files changed, 19 insertions(+), 20 deletions(-) diff --git a/src/experimental/platform/ux/gui.cc b/src/experimental/platform/ux/gui.cc index b952688e..b0ffbadf 100644 --- a/src/experimental/platform/ux/gui.cc +++ b/src/experimental/platform/ux/gui.cc @@ -464,7 +464,8 @@ bool LabelSelectionGui(mjvOption* opts) { bool changed = false; const std::string label_preview = - std::string(ICON_LABEL) + " " + kLabelNames[opts->label]; + opts->label == 0 ? std::string(ICON_LABEL) + " Label" + : std::string(ICON_LABEL) + " " + kLabelNames[opts->label]; if (ImGui::BeginCombo("##Label", label_preview.c_str(), ImGuiComboFlags_NoArrowButton)) { for (int n = 0; n < IM_ARRAYSIZE(kLabelNames); n++) { @@ -486,7 +487,8 @@ bool FrameSelectionGui(mjvOption* opts) { bool changed = false; const std::string frame_preview = - std::string(ICON_FRAME) + " " + kFrameNames[opts->frame]; + opts->frame == 0 ? std::string(ICON_FRAME) + " Frame" + : std::string(ICON_FRAME) + " " + kFrameNames[opts->frame]; if (ImGui::BeginCombo("##Frame", frame_preview.c_str(), ImGuiComboFlags_NoArrowButton)) { for (int n = 0; n < IM_ARRAYSIZE(kFrameNames); n++) { @@ -1071,8 +1073,8 @@ void NoiseGui(const mjModel* model, const mjData* data, float& noise_scale, float& noise_rate) { const float item_width = ImGui::GetWindowWidth() * .6f; ImGui::PushItemWidth(item_width); - ImGui::SliderFloat("Scale", &noise_scale, 0, 1); - ImGui::SliderFloat("Rate", &noise_rate, 0, 4); + ImGui::SliderFloat("Noise scale", &noise_scale, 0, 1); + ImGui::SliderFloat("Noise rate", &noise_rate, 0, 4); ImGui::PopItemWidth(); } diff --git a/src/experimental/studio/app.cc b/src/experimental/studio/app.cc index 1bed7a85..3952c9f9 100644 --- a/src/experimental/studio/app.cc +++ b/src/experimental/studio/app.cc @@ -872,11 +872,6 @@ void App::BuildGui() { } if (tmp_.inspector_panel) { - if (ImGui::Begin("Inspector", &tmp_.inspector_panel)) { - DataInspectorGui(); - } - ImGui::End(); - if (ImGui::Begin("Explorer", &tmp_.inspector_panel)) { SpecExplorerGui(); } @@ -886,6 +881,11 @@ void App::BuildGui() { SpecEditorGui(); } ImGui::End(); + + if (ImGui::Begin("Inspector", &tmp_.inspector_panel)) { + DataInspectorGui(); + } + ImGui::End(); } if (tmp_.chart_performance) { @@ -1071,17 +1071,6 @@ void App::DataInspectorGui() { const ImGuiTreeNodeFlags node_flags = ImGuiTreeNodeFlags_SpanAvailWidth | ImGuiTreeNodeFlags_Framed; - ImGui::BeginChild("NoiseGui", {0, 0}, child_flags); - if (ImGui::TreeNodeEx("Noise", node_flags)) { - float noise_scale = 0; - float noise_rate = 0; - step_control_.GetNoiseParameters(noise_scale, noise_rate); - platform::NoiseGui(model(), data(), noise_scale, noise_rate); - step_control_.SetNoiseParameters(noise_scale, noise_rate); - ImGui::TreePop(); - } - ImGui::EndChild(); - ImGui::BeginChild("JointsGui", {0, 0}, child_flags); if (ImGui::TreeNodeEx("Joints", node_flags)) { platform::JointsGui(model(), data(), &vis_options_); @@ -1091,6 +1080,14 @@ void App::DataInspectorGui() { ImGui::BeginChild("ControlsGui", {0, 0}, child_flags); if (ImGui::TreeNodeEx("Controls", node_flags)) { + + float noise_scale = 0; + float noise_rate = 0; + step_control_.GetNoiseParameters(noise_scale, noise_rate); + platform::NoiseGui(model(), data(), noise_scale, noise_rate); + step_control_.SetNoiseParameters(noise_scale, noise_rate); + ImGui::Separator(); + platform::ControlsGui(model(), data(), &vis_options_); ImGui::TreePop(); } From 395a0028018f496fcc513a146e25d5783e79b067 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Fri, 8 May 2026 04:49:44 -0700 Subject: [PATCH 227/251] Small usability fixes. - allow reloading of files that failed to load - clear errors on successful reload PiperOrigin-RevId: 912458744 Change-Id: Ie6af87508f10e83461bc998c99c9c66e2d58312e --- src/experimental/studio/app.cc | 23 +++++++++++------------ src/experimental/studio/app.h | 3 --- 2 files changed, 11 insertions(+), 15 deletions(-) diff --git a/src/experimental/studio/app.cc b/src/experimental/studio/app.cc index 3952c9f9..f30c6551 100644 --- a/src/experimental/studio/app.cc +++ b/src/experimental/studio/app.cc @@ -106,17 +106,6 @@ void App::SwitchGraphicsMode(int width, int height, window_->GetNativeWindowHandle(), gfx_mode_); } -void App::ClearModel() { - model_holder_.reset(); - window_->SetTitle("MuJoCo Studio"); - step_control_.SetSpeed(100.f); - profiler_.Clear(); - tmp_ = UiTempState(); - load_error_ = ""; - step_error_ = ""; - edit_error_ = ""; -} - void App::Recompile() { mj_recompile(model_holder_->spec(), model_holder_->vfs(), model_holder_->model(), model_holder_->data()); @@ -129,7 +118,8 @@ void App::RequestModelLoad(std::string model_file) { } void App::RequestModelReload() { - if (model_kind_ == kModelFromFile) { + if (model_kind_ == kModelFromFile || + (model_kind_ == kEmptyModel && !model_path_.empty())) { pending_load_ = model_path_; preserve_camera_on_load_ = true; } @@ -158,6 +148,9 @@ void App::LoadModelFromFile(const std::string& filepath) { } } else { SetLoadError(std::string(model_holder_->error())); + // Keep track of the attempted load in case the user fixes the error and + // tries to reload the same file again. + model_path_ = resolved_file; } } @@ -175,6 +168,10 @@ void App::LoadModelFromBuffer(std::span buffer, } void App::OnModelLoaded(std::string filename, ModelKind model_kind) { + load_error_ = ""; + step_error_ = ""; + edit_error_ = ""; + model_path_ = std::move(filename); if (model_kind_ == kEmptyModel) { @@ -243,6 +240,8 @@ void App::UpdateFilePaths(const std::string& resolved_path) { void App::SetLoadError(std::string error) { InitEmptyModel(); load_error_ = std::move(error); + step_error_ = ""; + edit_error_ = ""; } void App::ResetPhysics() { diff --git a/src/experimental/studio/app.h b/src/experimental/studio/app.h index bef5f6ed..52059644 100644 --- a/src/experimental/studio/app.h +++ b/src/experimental/studio/app.h @@ -175,9 +175,6 @@ class App { // Requests that the currently loaded model be reloaded at the next update. void RequestModelReload(); - // Clears the currently loaded model and all associated state. - void ClearModel(); - // Recompiles the spec, updating the model and data. void Recompile(); From 531a571ddae91b98464214fb1166c05be7a3b4cc Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 8 May 2026 06:24:53 -0700 Subject: [PATCH 228/251] studio: replace separate Solver and Performance charts with a unified Profiler, bound to F3 Combines the "Solver" and "Performance" chart windows into a single "Profiler" window for better ergonomics and visual layout: - Unified Dashboard: Groups the Solver block (Counts & Convergence) and the Performance block (Dimensions & CPU Time) in a single viewable window that scales cleanly without vertical scrollbars. - Adaptive Stacking: Uses ImGui grouping so each block internally stacks its charts vertically, but tiles side-by-side horizontally when the window is stretched wide. - Default Docking & Width: Integrates the Profiler into the default dockspace layout attached to the right of the main viewable area, with an optimized initial width scaling factor of 0.42. - Shortcut and Menus: Adds a toggle shortcut bound to `F3`, updates the Help Shortcuts overlay, and replaces old separate entries in the Charts menu. PiperOrigin-RevId: 912491435 Change-Id: I1f9215e5498866a01ce48b6aeb4a7461191fa01d --- src/experimental/platform/ux/gui.cc | 57 +++++++++++++++++++ src/experimental/platform/ux/gui.h | 4 ++ src/experimental/platform/ux/imgui_widgets.cc | 10 ++-- src/experimental/studio/app.cc | 47 ++++----------- src/experimental/studio/app.h | 3 +- 5 files changed, 77 insertions(+), 44 deletions(-) diff --git a/src/experimental/platform/ux/gui.cc b/src/experimental/platform/ux/gui.cc index b0ffbadf..2892b185 100644 --- a/src/experimental/platform/ux/gui.cc +++ b/src/experimental/platform/ux/gui.cc @@ -28,6 +28,7 @@ #include #include #include "experimental/platform/helpers.h" +#include "experimental/platform/sim/sim_profiler.h" #include "experimental/platform/sim/step_control.h" #include "experimental/platform/ux/imgui_widgets.h" #include "experimental/platform/ux/interaction.h" @@ -275,6 +276,9 @@ ImVec4 ConfigureDockingLayout() { ImGui::DockBuilderSplitNode(inspector, ImGuiDir_Down, kStatsRelHeight, &properties, &inspector); + ImGuiID profiler = 0; + ImGui::DockBuilderSplitNode(main, ImGuiDir_Right, 0.42f, &profiler, &main); + ImGui::DockBuilderDockWindow("Dockspace", main); ImGui::DockBuilderDockWindow("Options", options); ImGui::DockBuilderDockWindow("Explorer", inspector); @@ -282,6 +286,7 @@ ImVec4 ConfigureDockingLayout() { ImGui::DockBuilderDockWindow("Inspector", inspector); ImGui::DockBuilderDockWindow("Properties", properties); ImGui::DockBuilderDockWindow("Stats", stats); + ImGui::DockBuilderDockWindow("Profiler", profiler); ImGui::DockBuilderFinish(root); } @@ -1392,4 +1397,56 @@ void StatsGui(const mjModel* model, const mjData* data, bool paused, ImGui::Columns(); } +void ProfilerGui(const mjModel* model, mjData* data, SimProfiler* profiler) { + ImGui::SetWindowFontScale(0.8f); + ImVec2 avail = ImGui::GetContentRegionAvail(); + const float pad = ImGui::GetStyle().ItemSpacing.x; + const float aspect = avail.y > 0 ? avail.x / avail.y : 1.0f; + + ImVec2 plot_size; + int cols; + if (aspect < 0.8f) { + plot_size.x = avail.x; + plot_size.y = (avail.y - pad * 3.0f) * 0.25f; + cols = 1; + } else if (aspect < 1.8f) { + plot_size.x = (avail.x - pad) * 0.5f; + plot_size.y = (avail.y - pad) * 0.5f; + cols = 2; + } else { + plot_size.x = (avail.x - pad * 3.0f) * 0.25f; + plot_size.y = avail.y; + cols = 4; + } + + int current_col = 0; + auto advance = [&]() { + current_col++; + if (current_col < cols) { + ImGui::SameLine(); + } else { + current_col = 0; + } + }; + + if (cols == 2) { + // In 2x2 layout, vertically stack charts with the same x-axis. + CountsGui(model, data, plot_size); + advance(); + profiler->DimensionsGraph(plot_size); + advance(); + ConvergenceGui(model, data, plot_size); + advance(); + profiler->CpuTimeGraph(plot_size); + } else { + CountsGui(model, data, plot_size); + advance(); + ConvergenceGui(model, data, plot_size); + advance(); + profiler->DimensionsGraph(plot_size); + advance(); + profiler->CpuTimeGraph(plot_size); + } +} + } // namespace mujoco::platform diff --git a/src/experimental/platform/ux/gui.h b/src/experimental/platform/ux/gui.h index cc2ae08f..3599a93d 100644 --- a/src/experimental/platform/ux/gui.h +++ b/src/experimental/platform/ux/gui.h @@ -28,6 +28,7 @@ #include #include +#include "experimental/platform/sim/sim_profiler.h" #include "experimental/platform/sim/step_control.h" namespace mujoco::platform { @@ -148,6 +149,9 @@ void ConvergenceGui(const mjModel* model, mjData* data, void CountsGui(const mjModel* model, mjData* data, ImVec2 plot_size = ImVec2(-1, 0)); +// UX for Profiler panel combining Solver and Performance metrics. +void ProfilerGui(const mjModel* model, mjData* data, SimProfiler* profiler); + // UX for displaying basic simulation information. Note that the pause state and // FPS needs to be tracked by the caller and passed here to be displayed. void StatsGui(const mjModel* model, const mjData* data, bool paused, float fps); diff --git a/src/experimental/platform/ux/imgui_widgets.cc b/src/experimental/platform/ux/imgui_widgets.cc index 305f292c..cf33bc8d 100644 --- a/src/experimental/platform/ux/imgui_widgets.cc +++ b/src/experimental/platform/ux/imgui_widgets.cc @@ -382,10 +382,10 @@ ImPlotFlags ImPlot_SetupPlotFlags(ImVec2 plot_size) { ImPlotFlags flags = ImPlotFlags_None; if (plot_size.x > 0 && plot_size.y > 0) { const float min_dim = std::min(plot_size.x, plot_size.y); - if (min_dim < 300) { + if (min_dim < 150) { flags |= ImPlotFlags_NoTitle; } - if (min_dim < 200) { + if (min_dim < 140) { flags |= ImPlotFlags_NoLegend; } } @@ -395,7 +395,7 @@ ImPlotFlags ImPlot_SetupPlotFlags(ImVec2 plot_size) { void ImPlot_SetupTimeAxis(ImVec2 plot_size, const char* label, ImPlotAxisFlags extra_flags) { ImPlotAxisFlags flags = extra_flags; - if (plot_size.x > 0 && plot_size.x < 300) { + if (plot_size.x > 0 && plot_size.x < 180) { flags |= ImPlotAxisFlags_NoTickLabels; } ImPlot::SetupAxis(ImAxis_X1, label, flags); @@ -404,7 +404,7 @@ void ImPlot_SetupTimeAxis(ImVec2 plot_size, const char* label, void ImPlot_SetupValueAxis(ImVec2 plot_size, const char* label, const char* format, ImPlotAxisFlags extra_flags) { ImPlotAxisFlags flags = extra_flags; - if (plot_size.y > 0 && plot_size.y < 150) { + if (plot_size.y > 0 && plot_size.y < 90) { flags |= ImPlotAxisFlags_NoTickLabels; } ImPlot::SetupAxis(ImAxis_Y1, label, flags); @@ -418,7 +418,7 @@ void ImPlot_SetupFixedAxis(ImVec2 plot_size, double y_min, double y_max, const double* tick_values, const char* const* tick_labels, int n_ticks) { ImPlotAxisFlags flags = ImPlotAxisFlags_None; - if (plot_size.y > 0 && plot_size.y < 150) { + if (plot_size.y > 0 && plot_size.y < 90) { flags |= ImPlotAxisFlags_NoTickLabels; } ImPlot::SetupAxis(ImAxis_Y1, label, flags); diff --git a/src/experimental/studio/app.cc b/src/experimental/studio/app.cc index f30c6551..44442808 100644 --- a/src/experimental/studio/app.cc +++ b/src/experimental/studio/app.cc @@ -625,14 +625,12 @@ void App::HandleKeyboardEvents() { ToggleWindow(tmp_.help); } else if (ImGui_IsChordJustPressed(ImGuiKey_F2)) { ToggleWindow(tmp_.stats); + } else if (ImGui_IsChordJustPressed(ImGuiKey_F3)) { + ToggleWindow(tmp_.profiler); } else if (ImGui_IsChordJustPressed(ImGuiKey_F6)) { vis_options_.frame = (vis_options_.frame + 1) % mjNFRAME; } else if (ImGui_IsChordJustPressed(ImGuiKey_F7)) { vis_options_.label = (vis_options_.label + 1) % mjNLABEL; - } else if (ImGui_IsChordJustPressed(ImGuiKey_F9)) { - tmp_.chart_solver = !tmp_.chart_solver; - } else if (ImGui_IsChordJustPressed(ImGuiKey_F10)) { - tmp_.chart_performance = !tmp_.chart_performance; } else if (ImGui_IsChordJustPressed(ImGuiKey_F11)) { tmp_.full_screen = !tmp_.full_screen; } else if (ImGui_IsChordJustPressed(ImGuiKey_H)) { @@ -887,30 +885,10 @@ void App::BuildGui() { ImGui::End(); } - if (tmp_.chart_performance) { - ImGui::SetNextWindowPos(chart_pos, ImGuiCond_FirstUseEver); - ImGui::SetNextWindowSize(chart_size, ImGuiCond_FirstUseEver); - if (ImGui::Begin("Performance", &tmp_.chart_performance)) { - auto layout = platform::ImPlot_ComputePairLayout(); - profiler_.CpuTimeGraph(layout.plot_size); - if (layout.direction == platform::ImPlotLayoutDirection::kHorizontal) { - ImGui::SameLine(); - } - profiler_.DimensionsGraph(layout.plot_size); - } - ImGui::End(); - } - - if (tmp_.chart_solver) { - ImGui::SetNextWindowPos(chart_pos, ImGuiCond_FirstUseEver); - ImGui::SetNextWindowSize(chart_size, ImGuiCond_FirstUseEver); - if (ImGui::Begin("Solver", &tmp_.chart_solver)) { - auto layout = platform::ImPlot_ComputePairLayout(); - platform::CountsGui(model(), data(), layout.plot_size); - if (layout.direction == platform::ImPlotLayoutDirection::kHorizontal) { - ImGui::SameLine(); - } - platform::ConvergenceGui(model(), data(), layout.plot_size); + if (tmp_.profiler) { + if (ImGui::Begin("Profiler", &tmp_.profiler, + ImGuiWindowFlags_NoScrollbar | ImGuiWindowFlags_NoScrollWithMouse)) { + platform::ProfilerGui(model(), data(), &profiler_); } ImGui::End(); } @@ -1311,10 +1289,9 @@ void App::HelpGui() { ImGui::Text("Help"); ImGui::Text("Stats"); + ImGui::Text("Profiler"); ImGui::Text("Cycle Frames"); ImGui::Text("Cycle Labels"); - ImGui::Text("Solver Charts"); - ImGui::Text("Perf. Charts"); ImGui::Text("Toggle Fullscreen"); ImGui::Text("Free Camera"); ImGui::Text("Toggle Pause"); @@ -1338,10 +1315,9 @@ void App::HelpGui() { ImGui::Indent(indent); ImGui::Text("F1"); ImGui::Text("F2"); + ImGui::Text("F3"); ImGui::Text("F6"); ImGui::Text("F7"); - ImGui::Text("F9"); - ImGui::Text("F10"); ImGui::Text("F11"); ImGui::Text("Esc"); ImGui::Text("Spc"); @@ -1733,11 +1709,8 @@ void App::MainMenuGui() { } if (ImGui::BeginMenu("Charts")) { - if (ImGui::MenuItem("Solver", "F9")) { - tmp_.chart_solver = !tmp_.chart_solver; - } - if (ImGui::MenuItem("Performance", "F10")) { - tmp_.chart_performance = !tmp_.chart_performance; + if (ImGui::MenuItem("Profiler", "F3")) { + ToggleWindow(tmp_.profiler); } ImGui::EndMenu(); } diff --git a/src/experimental/studio/app.h b/src/experimental/studio/app.h index 52059644..4f2b734c 100644 --- a/src/experimental/studio/app.h +++ b/src/experimental/studio/app.h @@ -120,8 +120,7 @@ class App { // Windows. bool help = false; bool stats = false; - bool chart_solver = false; - bool chart_performance = false; + bool profiler = false; bool picture_in_picture = false; bool options_panel = true; bool inspector_panel = true; From 8cef5bb978986f88bc929143fdb8a2ea5378d432 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 8 May 2026 06:35:07 -0700 Subject: [PATCH 229/251] Add configurable octree max depth for meshes. The maximum depth of the octree used for SDF generation can now be specified in the mesh definition via `mjsMesh::octree_maxdepth`. The default value is 6. PiperOrigin-RevId: 912494999 Change-Id: I6d828d1d3999b99d1210a6829a05b04fac591e1f --- doc/includes/references.h | 1 + include/mujoco/mjspec.h | 1 + python/mujoco/introspect/structs.py | 5 +++++ src/user/user_init.c | 1 + src/user/user_mesh.cc | 2 ++ src/user/user_objects.cc | 2 +- src/user/user_objects.h | 6 ++++++ test/user/user_mesh_test.cc | 4 ++++ wasm/codegen/generated/bindings.cc | 7 +++++++ 9 files changed, 28 insertions(+), 1 deletion(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 6342e21b..d106b4b6 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -2294,6 +2294,7 @@ typedef struct mjsMesh_ { // mesh specification mjIntVec* userfacetexcoord; // user texcoord indices mjsPlugin plugin; // sdf plugin mjString* material; // name of material + int octree_maxdepth; // max octree depth mjString* info; // message appended to compiler errors } mjsMesh; typedef struct mjsHField_ { // height field specification diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index 73d7c74f..5dbf1908 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -490,6 +490,7 @@ typedef struct mjsMesh_ { // mesh specification mjIntVec* userfacetexcoord; // user texcoord indices mjsPlugin plugin; // sdf plugin mjString* material; // name of material + int octree_maxdepth; // max octree depth mjString* info; // message appended to compiler errors } mjsMesh; diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 6ce4cb62..84ff1341 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -8461,6 +8461,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name of material', ), + StructFieldDecl( + name='octree_maxdepth', + type=ValueType(name='int'), + doc='max octree depth', + ), StructFieldDecl( name='info', type=PointerType( diff --git a/src/user/user_init.c b/src/user/user_init.c index 1e522776..9a7274e8 100644 --- a/src/user/user_init.c +++ b/src/user/user_init.c @@ -243,6 +243,7 @@ void mjs_defaultMesh(mjsMesh* mesh) { mesh->scale[0] = mesh->scale[1] = mesh->scale[2] = 1; mesh->maxhullvert = -1; mesh->inertia = mjMESH_INERTIA_LEGACY; + mesh->octree_maxdepth = 6; } diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 8db7a9f5..23d71d90 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -782,6 +782,7 @@ void mjCMesh::TryCompile(const mjVFS* vfs) { } else if (octree_.NumNodes() == 0) { std::vector dvert(vert_.begin(), vert_.end()); octree_.SetFace(dvert, face_); + octree_.SetMaxDepth(spec.octree_maxdepth); octree_.CreateOctree(aamm_); if (!plugin.active) { octree_.ComputeSdfCoeffs(dvert.data(), nvert(), face_.data(), nface(), tree_); @@ -1532,6 +1533,7 @@ void mjCMesh::Process() { // make octree if (needsdf) { octree_.SetFace(dvert, face_); + octree_.SetMaxDepth(spec.octree_maxdepth); octree_.CreateOctree(aamm_); if (!plugin.active) { diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index d4f54ec6..b4d76a10 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -1285,7 +1285,7 @@ void mjCOctree::MakeOctree(const std::vector& elements, const double } // skip if the box is empty - if (colliding.empty() || task.lev >= 6) { + if (colliding.empty() || task.lev >= max_depth_) { continue; } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index c0d1be94..a2566b4d 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -270,6 +270,7 @@ struct OctreeTask { struct mjCOctree_ { int nnode_ = 0; int nvert_ = 0; + int max_depth_ = 6; // max octree depth (default 6) std::vector node_; std::vector face_; // mesh faces (nmeshface x 3) std::vector vert_; // octree vertices (nvert x 3) @@ -308,6 +309,10 @@ class mjCOctree : public mjCOctree_ { void AddCoeff(int n, int v, double coeff) { node_[n].coeff[v] = coeff; } double Coeff(int n, int v) const { return node_[n].coeff[v]; } + // Set max octree depth (default 6) + void SetMaxDepth(int depth) { max_depth_ = depth; } + int MaxDepth() const { return max_depth_; } + // Set number of Laplacian smoothing iterations (0 = disabled, default) void SetSmoothingIterations(int iterations) { smoothing_iterations_ = iterations; } int SmoothingIterations() const { return smoothing_iterations_; } @@ -1208,6 +1213,7 @@ class mjCMesh: public mjCMesh_, private mjsMesh { // octree const mjCOctree& octree() { return octree_; } + mjCOctree& mutable_octree() { return octree_; } void Compile(const mjVFS* vfs); // compiler double* GetPosPtr(); // get position diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index af5bc552..185b2678 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -1501,6 +1501,8 @@ TEST_F(MjCMeshTest, OctreeIsBalanced) { mjSpec* spec = mj_parseXML(xml_path.c_str(), 0, error.data(), error.size()); mjsGeom* geom = mjs_asGeom(mjs_firstElement(spec, mjOBJ_GEOM)); geom->type = mjGEOM_SDF; + mjsMesh* mesh = mjs_asMesh(mjs_firstElement(spec, mjOBJ_MESH)); + mesh->octree_maxdepth = 5; mjModel* model = mj_compile(spec, 0); ASSERT_THAT(model, NotNull()) << error.data(); EXPECT_GT(model->mesh_octnum[0], 0); @@ -1564,6 +1566,8 @@ TEST_F(MjCMeshTest, OctreeHangingNodeInterpolation) { mjSpec* spec = mj_parseXML(xml_path.c_str(), 0, error.data(), error.size()); mjsGeom* geom = mjs_asGeom(mjs_firstElement(spec, mjOBJ_GEOM)); geom->type = mjGEOM_SDF; + mjsMesh* mesh = mjs_asMesh(mjs_firstElement(spec, mjOBJ_MESH)); + mesh->octree_maxdepth = 5; mjModel* model = mj_compile(spec, 0); ASSERT_THAT(model, NotNull()) << error.data(); EXPECT_GT(model->mesh_octnum[0], 0); diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index 324092ef..f0c11146 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -6246,6 +6246,12 @@ struct MjsMesh { *(ptr_->material) = value; } } + int octree_maxdepth() const { + return ptr_->octree_maxdepth; + } + void set_octree_maxdepth(int value) { + ptr_->octree_maxdepth = value; + } mjString info() const { return (ptr_ && ptr_->info) ? *(ptr_->info) : ""; } @@ -12782,6 +12788,7 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("material", &MjsMesh::material, &MjsMesh::set_material, reference()) .property("maxhullvert", &MjsMesh::maxhullvert, &MjsMesh::set_maxhullvert, reference()) .property("needsdf", &MjsMesh::needsdf, &MjsMesh::set_needsdf, reference()) + .property("octree_maxdepth", &MjsMesh::octree_maxdepth, &MjsMesh::set_octree_maxdepth, reference()) .property("plugin", &MjsMesh::plugin, reference()) .property("refpos", &MjsMesh::refpos) .property("refquat", &MjsMesh::refquat) From fa912dffa08c9437bdbd790e8de2ed7caa635a4f Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 8 May 2026 08:47:10 -0700 Subject: [PATCH 230/251] Add flex arrays to CompareModel check. Due to a bug, all flex mjModel fields were previously ignored. PiperOrigin-RevId: 912546202 Change-Id: Ica95ecfcbbe366fd81de6e8f7d83ed57a7d23033 --- src/engine/engine_setconst.c | 2 + src/user/user_flexcomp.cc | 113 +--------------------------- src/user/user_mesh.cc | 139 ++++++++++++++++++++++++++++++++++- src/user/user_objects.h | 3 + src/xml/xml_native_reader.cc | 2 +- src/xml/xml_native_writer.cc | 3 +- test/fixture.cc | 13 +++- 7 files changed, 158 insertions(+), 117 deletions(-) diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index e3e41069..81a0f155 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -447,6 +447,8 @@ static void makeFlexSparse(mjModel* m, mjData* d) { mju_zeroInt(m->flex_vertedge, 2 * m->nflexedge); mju_zeroInt(m->flex_vertedge, 2 * m->nflexedge); mju_zero(m->flex_vertmetric, 4 * m->nflexvert); + mju_zeroInt(m->flexedge_J_colind, m->nJfe); + mju_zeroInt(m->flexvert_J_colind, 2 * m->nJfv); int current_adj_offset = 0; // compute lengths and Jacobians of edges diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 85b900ae..7030768c 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -13,14 +13,12 @@ // limitations under the License. #include -#include #include #include #include #include #include #include -#include #include #include #include @@ -107,116 +105,11 @@ void mjCFlexcomp::MarkEmptyCells(mjCFlex* flex, const double* points, int cx = flex->spec.cellcount[0]; int cy = flex->spec.cellcount[1]; int cz = flex->spec.cellcount[2]; - int ncells = cx * cy * cz; int order = flex->spec.order; - // determine which cells contain mesh elements (not just vertices) - // for each element, compute its AABB and mark all overlapping cells - std::vector has_element(ncells, false); - - double dx = minmax[3] - minmax[0]; - double dy = minmax[4] - minmax[1]; - double dz = minmax[5] - minmax[2]; - - // vertices per element: dim+1 (edges=2, triangles=3, tets=4) - int nvpe = flex->spec.dim + 1; - - if (nvpe > 0 && !element.empty()) { - int nelem = element.size() / nvpe; - for (int e = 0; e < nelem; e++) { - // compute element AABB - double elo[3] = {1e30, 1e30, 1e30}; - double ehi[3] = {-1e30, -1e30, -1e30}; - for (int v = 0; v < nvpe; v++) { - int vid = element[nvpe * e + v]; - for (int j = 0; j < 3; j++) { - elo[j] = std::min(elo[j], points[3 * vid + j]); - ehi[j] = std::max(ehi[j], points[3 * vid + j]); - } - } - - // map element AABB to cell range - auto cellIdx = [](double coord, double lo, double d, int nc) { - if (d <= 0) return 0; - int c = (int)((coord - lo) / d * nc); - return std::max(0, std::min(nc - 1, c)); - }; - - int ci0 = cellIdx(elo[0], minmax[0], dx, cx); - int ci1 = cellIdx(ehi[0], minmax[0], dx, cx); - int cj0 = cellIdx(elo[1], minmax[1], dy, cy); - int cj1 = cellIdx(ehi[1], minmax[1], dy, cy); - int ck0 = cellIdx(elo[2], minmax[2], dz, cz); - int ck1 = cellIdx(ehi[2], minmax[2], dz, cz); - - // mark all overlapping cells as containing elements - for (int ci = ci0; ci <= ci1; ci++) { - for (int cj = cj0; cj <= cj1; cj++) { - for (int ck = ck0; ck <= ck1; ck++) { - has_element[ci * cy * cz + cj * cz + ck] = true; - } - } - } - } - } - - // default: all cells non-empty (only exterior cells will be empty) - flex->cell_empty.assign(ncells, false); - - // for dim=2 (surface mesh): check watertightness and flood-fill - if (flex->spec.dim == 2 && nvpe == 3 && !element.empty()) { - // flood-fill from grid boundary to find exterior cells - // cells reachable from the boundary through non-element cells - // are outside the mesh volume; cells NOT reachable are interior - std::vector visited(ncells, false); - std::queue> bfs; - - // seed BFS from boundary cells that have no elements - for (int ci = 0; ci < cx; ci++) { - for (int cj = 0; cj < cy; cj++) { - for (int ck = 0; ck < cz; ck++) { - if (ci == 0 || ci == cx - 1 || - cj == 0 || cj == cy - 1 || - ck == 0 || ck == cz - 1) { - int idx = ci * cy * cz + cj * cz + ck; - if (!has_element[idx] && !visited[idx]) { - visited[idx] = true; - flex->cell_empty[idx] = true; - bfs.push({ci, cj, ck}); - } - } - } - } - } - - // BFS: spread through non-element cells - const int dirs[6][3] = { - {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, - {0, 1, 0}, {0, 0, -1}, {0, 0, 1}}; - while (!bfs.empty()) { - auto [ci, cj, ck] = bfs.front(); - bfs.pop(); - for (auto& d : dirs) { - int ni = ci + d[0], nj = cj + d[1], nk = ck + d[2]; - if (ni < 0 || ni >= cx || - nj < 0 || nj >= cy || - nk < 0 || nk >= cz) { - continue; - } - int nidx = ni * cy * cz + nj * cz + nk; - if (!visited[nidx] && !has_element[nidx]) { - visited[nidx] = true; - flex->cell_empty[nidx] = true; - bfs.push({ni, nj, nk}); - } - } - } - } else { - // dim!=2 (e.g., tet mesh): cells without element overlap are empty - for (int c = 0; c < ncells; c++) { - flex->cell_empty[c] = !has_element[c]; - } - } + // delegate cell_empty computation to mjCFlex + int nelem = element.size() / (flex->spec.dim + 1); + flex->ComputeCellEmpty(points, element.data(), npnt, nelem, flex->spec.dim, minmax); // pin nodes that belong exclusively to empty cells for (int gi = 0; gi < nx; gi++) { diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 23d71d90..d9b454f5 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -4698,8 +4699,8 @@ void mjCFlex::Compile(const mjVFS* vfs) { // no elemtexcoord: copy from faces if (elemtexcoord_.empty() && !texcoord_.empty()) { - elemtexcoord_.assign(3*nelem, 0); - memcpy(elemtexcoord_.data(), elem_.data(), 3*nelem*sizeof(int)); + elemtexcoord_.assign((dim + 1) * nelem, 0); + memcpy(elemtexcoord_.data(), elem_.data(), (dim + 1) * nelem * sizeof(int)); } // resolve material name @@ -4914,6 +4915,15 @@ void mjCFlex::Compile(const mjVFS* vfs) { // create shell fragments and element-vertex collision pairs CreateShellPair(); + // recompute cell_empty from vertex/element geometry + // (survives XML round-trips where flexcomp data is lost) + if (interpolated && cell_empty.empty()) { + int cx = spec.cellcount[0], cy = spec.cellcount[1], cz = spec.cellcount[2]; + if (cx * cy * cz > 1) { + ComputeCellEmpty(vertxpos.data(), elem_.data(), nvert, nelem, dim); + } + } + // compute linear stiffness for interpolated elements (cached) bool stiffness_cached = false; if (young > 0 && interpolated) { @@ -5223,6 +5233,131 @@ std::vector mjCFlex::ComputeUnrotatedNodePositions( } +// identify cells with no mesh content from vertex/element geometry +void mjCFlex::ComputeCellEmpty(const double* vpos, const int* elems, + int nv, int ne, int fdim, + const double* bbox) { + int cx = spec.cellcount[0]; + int cy = spec.cellcount[1]; + int cz = spec.cellcount[2]; + int ncells = cx * cy * cz; + + // use precomputed bounding box if provided, otherwise compute from vertices + double minmax[6]; + if (bbox) { + for (int j = 0; j < 6; j++) minmax[j] = bbox[j]; + } else { + minmax[0] = minmax[1] = minmax[2] = 1e30; + minmax[3] = minmax[4] = minmax[5] = -1e30; + for (int i = 0; i < nv; i++) { + for (int j = 0; j < 3; j++) { + minmax[j+0] = std::min(minmax[j+0], vpos[3*i+j]); + minmax[j+3] = std::max(minmax[j+3], vpos[3*i+j]); + } + } + } + + double dx = minmax[3] - minmax[0]; + double dy = minmax[4] - minmax[1]; + double dz = minmax[5] - minmax[2]; + + // determine which cells contain mesh elements + std::vector has_element(ncells, false); + int nvpe = fdim + 1; + + if (nvpe > 0 && ne > 0) { + for (int e = 0; e < ne; e++) { + // compute element AABB + double elo[3] = {1e30, 1e30, 1e30}; + double ehi[3] = {-1e30, -1e30, -1e30}; + for (int v = 0; v < nvpe; v++) { + int vid = elems[nvpe * e + v]; + for (int j = 0; j < 3; j++) { + elo[j] = std::min(elo[j], vpos[3 * vid + j]); + ehi[j] = std::max(ehi[j], vpos[3 * vid + j]); + } + } + + // map element AABB to cell range + auto cellIdx = [](double coord, double lo, double d, int nc) { + if (d <= 0) return 0; + int c = (int)((coord - lo) / d * nc); + return std::max(0, std::min(nc - 1, c)); + }; + + int ci0 = cellIdx(elo[0], minmax[0], dx, cx); + int ci1 = cellIdx(ehi[0], minmax[0], dx, cx); + int cj0 = cellIdx(elo[1], minmax[1], dy, cy); + int cj1 = cellIdx(ehi[1], minmax[1], dy, cy); + int ck0 = cellIdx(elo[2], minmax[2], dz, cz); + int ck1 = cellIdx(ehi[2], minmax[2], dz, cz); + + for (int ci = ci0; ci <= ci1; ci++) { + for (int cj = cj0; cj <= cj1; cj++) { + for (int ck = ck0; ck <= ck1; ck++) { + has_element[ci * cy * cz + cj * cz + ck] = true; + } + } + } + } + } + + cell_empty.assign(ncells, false); + + // for dim=2 (surface mesh): flood-fill from boundary to find exterior cells + if (fdim == 2 && nvpe == 3 && ne > 0) { + std::vector visited(ncells, false); + std::queue> bfs; + + // seed BFS from boundary cells that have no elements + for (int ci = 0; ci < cx; ci++) { + for (int cj = 0; cj < cy; cj++) { + for (int ck = 0; ck < cz; ck++) { + if (ci == 0 || ci == cx - 1 || + cj == 0 || cj == cy - 1 || + ck == 0 || ck == cz - 1) { + int idx = ci * cy * cz + cj * cz + ck; + if (!has_element[idx] && !visited[idx]) { + visited[idx] = true; + cell_empty[idx] = true; + bfs.push({ci, cj, ck}); + } + } + } + } + } + + // BFS: spread through non-element cells + const int dirs[6][3] = { + {-1, 0, 0}, {1, 0, 0}, {0, -1, 0}, + {0, 1, 0}, {0, 0, -1}, {0, 0, 1}}; + while (!bfs.empty()) { + auto [ci, cj, ck] = bfs.front(); + bfs.pop(); + for (auto& d : dirs) { + int ni = ci + d[0], nj = cj + d[1], nk = ck + d[2]; + if (ni < 0 || ni >= cx || + nj < 0 || nj >= cy || + nk < 0 || nk >= cz) { + continue; + } + int nidx = ni * cy * cz + nj * cz + nk; + if (!visited[nidx] && !has_element[nidx]) { + visited[nidx] = true; + cell_empty[nidx] = true; + bfs.push({ni, nj, nk}); + } + } + } + } else { + // dim!=2: cells without element overlap are empty + for (int c = 0; c < ncells; c++) { + cell_empty[c] = !has_element[c]; + } + } +} + + // create flex BVH void mjCFlex::CreateBVH() { int nbvh = 0; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index a2566b4d..ac58c810 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1055,6 +1055,9 @@ class mjCFlex: public mjCFlex_, private mjsFlex { void Compile(const mjVFS* vfs); // compiler void CreateBVH(void); // create flex BVH void CreateShellPair(void); // create shells and evpairs + void ComputeCellEmpty(const double* vpos, const int* elems, // identify cells + int nv, int ne, int fdim, // with no mesh content + const double* bbox = nullptr); // optional precomputed bbox std::vector vert0_; // vertex positions in [0, 1]^d in the bounding box std::vector node0_; // node Cartesian positions diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index c31b1bf2..f249df3f 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -1553,7 +1553,7 @@ void mjXReader::OneFlex(XMLElement* elem, mjsFlex* flex) { flex->internal = (n == 1); } MapValue(cont, "selfcollide", &flex->selfcollide, flexself_map, 5); - if (MapValue(cont, "passive", &flex->passive, bool_map, 2)) { + if (MapValue(cont, "passive", &n, bool_map, 2)) { flex->passive = (n == 1); } ReadAttrInt(cont, "activelayers", &flex->activelayers); diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 0bc4c07a..ff17f141 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -190,6 +190,7 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) { WriteAttrKey(cont, "internal", bool_map, 2, flex->internal, defflex.internal); WriteAttrKey(cont, "selfcollide", flexself_map, 5, flex->selfcollide, defflex.selfcollide); WriteAttrInt(cont, "activelayers", flex->activelayers, defflex.activelayers); + WriteAttrKey(cont, "passive", bool_map, 2, flex->passive, defflex.passive); // remove contact is no attributes if (!cont->FirstAttribute()) { @@ -202,7 +203,7 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) { WriteAttr(elastic, "poisson", 1, &flex->poisson, &defflex.poisson); WriteAttr(elastic, "thickness", 1, &flex->thickness, &defflex.thickness); WriteAttr(elastic, "damping", 1, &flex->damping, &defflex.damping); - WriteAttrKey(elastic, "elastic2d", elastic2d_map, 2, flex->elastic2d, defflex.elastic2d); + WriteAttrKey(elastic, "elastic2d", elastic2d_map, 4, flex->elastic2d, defflex.elastic2d); // edge subelement XMLElement* edge = InsertEnd(elem, "edge"); diff --git a/test/fixture.cc b/test/fixture.cc index f621330e..6a6b1732 100644 --- a/test/fixture.cc +++ b/test/fixture.cc @@ -251,10 +251,17 @@ mjtNum CompareModel(const mjModel* m1, const mjModel* m2, // compare arrays, apart from bvh-related ones (which includes flex_vert0), as // those are sensitive to numerical differences when meshes are perfectly - // symmetric. + // symmetric. Also skip flex fields derived from node local positions and + // cell geometry that are not fully serialized to XML. #define X(type, name, nr, nc) \ - if (strncmp(#name, "bvh_", 4) && strncmp(#name, "flex_vert0", 4) && \ - strncmp(#name, "mesh_poly", 4)) { \ + if (strncmp(#name, "bvh_", 4) && \ + strncmp(#name, "flex_vert", 9) && \ + strncmp(#name, "mesh_poly", 9) && \ + strcmp(#name, "flex_centered") && \ + strcmp(#name, "flex_size") && \ + strcmp(#name, "flexedge_length0") && \ + strcmp(#name, "flexedge_invweight0") && \ + strncmp(#name, "flex_node", 9)) { \ for (int r = 0; r < m1->nr; r++) { \ for (int c = 0; c < nc; c++) { \ dif = Compare(m1->name[r * nc + c], m2->name[r * nc + c]); \ From fefbc2c40786baa46f1c70239c7b78242dc1d740 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Fri, 24 Apr 2026 16:08:09 +0100 Subject: [PATCH 231/251] mujoco introspect --- .readthedocs.yml | 40 ++------- doc/make_mujoco_stubs.py | 176 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 182 insertions(+), 34 deletions(-) create mode 100644 doc/make_mujoco_stubs.py diff --git a/.readthedocs.yml b/.readthedocs.yml index 5f9792b3..89ef662f 100644 --- a/.readthedocs.yml +++ b/.readthedocs.yml @@ -20,46 +20,18 @@ build: - asdf install uv latest - asdf global uv latest - uv venv $READTHEDOCS_VIRTUALENV_PATH - # install doc requirements and build tools + # install doc requirements - | UV_PROJECT_ENVIRONMENT=$READTHEDOCS_VIRTUALENV_PATH \ uv pip install \ -r doc/requirements.txt \ - cmake pip build setuptools absl-py - # build and install MuJoCo C library + pip setuptools absl-py + # generate and install doc-only mujoco stubs (no C build required) + - python doc/make_mujoco_stubs.py python/mujoco_doc - | - VENV=$READTHEDOCS_VIRTUALENV_PATH && \ - $VENV/bin/cmake -B build \ - -DCMAKE_BUILD_TYPE=Release \ - -DCMAKE_INSTALL_PREFIX=$VENV \ - -DMUJOCO_BUILD_EXAMPLES=OFF \ - -DMUJOCO_BUILD_SIMULATE=OFF \ - -DMUJOCO_BUILD_TESTS=OFF \ - -DMUJOCO_TEST_PYTHON_UTIL=OFF && \ - $VENV/bin/cmake --build build --parallel && \ - $VENV/bin/cmake --install build - # copy plugins - - | - mkdir -p $READTHEDOCS_VIRTUALENV_PATH/mujoco_plugin && \ - cp build/lib/libactuator.* \ - build/lib/libelasticity.* \ - build/lib/libsensor.* \ - $READTHEDOCS_VIRTUALENV_PATH/mujoco_plugin/ && \ - cp build/lib/libsdf_plugin.* \ - $READTHEDOCS_VIRTUALENV_PATH/mujoco_plugin/ || true - # build and install Python bindings from source - - | - export VIRTUAL_ENV=$READTHEDOCS_VIRTUALENV_PATH \ - PATH=$READTHEDOCS_VIRTUALENV_PATH/bin:$PATH && \ - cd python && bash make_sdist.sh && cd dist && \ - MUJOCO_PATH=$READTHEDOCS_VIRTUALENV_PATH \ - MUJOCO_PLUGIN_PATH=$READTHEDOCS_VIRTUALENV_PATH/mujoco_plugin \ - MUJOCO_CMAKE_ARGS="-DCMAKE_INTERPROCEDURAL_OPTIMIZATION:BOOL=OFF \ - -DGLFW_BUILD_WAYLAND=OFF -DGLFW_BUILD_X11=OFF" \ UV_PROJECT_ENVIRONMENT=$READTHEDOCS_VIRTUALENV_PATH \ - uv pip install mujoco-*.tar.gz && \ - cd ../.. - # install mjx and mujoco_warp + uv pip install --no-deps python/mujoco_doc + # install mjx and mujoco_warp (mujoco dep satisfied by stubs above) - UV_PROJECT_ENVIRONMENT=$READTHEDOCS_VIRTUALENV_PATH uv pip install -e mjx - | find mjx/mujoco/mjx/third_party/mujoco_warp -type f -exec \ diff --git a/doc/make_mujoco_stubs.py b/doc/make_mujoco_stubs.py new file mode 100644 index 00000000..e8f781e9 --- /dev/null +++ b/doc/make_mujoco_stubs.py @@ -0,0 +1,176 @@ +# Copyright 2026 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +"""Generates a pip-installable doc-only mujoco stub package. + +This creates a lightweight pure-Python package that provides the mujoco +public API surface (enums, constants) without C extensions, for use by +Sphinx autodoc during documentation builds. + +Usage: + python doc/make_mujoco_stubs.py +""" + +import glob +import os +import re +import shutil +import sys +import textwrap + + +def _read_version(repo_root): + """Reads the mujoco version from python/pyproject.toml.""" + with open(os.path.join(repo_root, 'python', 'pyproject.toml')) as f: + for line in f: + m = re.match(r'version\s*=\s*"([^"]+)"', line.strip()) + if m: + return m.group(1) + raise RuntimeError('Could not find version in python/pyproject.toml') + + +def _parse_constants(repo_root): + """Parses numeric #define constants from MuJoCo C headers.""" + consts = {} + for path in glob.glob(os.path.join(repo_root, 'include', 'mujoco', '*.h')): + with open(path) as f: + for line in f: + m = re.match( + r'\s*#define\s+(mj[A-Z]\w*)\s+([\d.eE+\-]+)\s', line) + if m and not m.group(2).endswith('f'): + consts[m.group(1)] = m.group(2) + return consts + + +_PYPROJECT_TEMPLATE = textwrap.dedent("""\ + [build-system] + requires = ["setuptools"] + build-backend = "setuptools.build_meta" + + [project] + name = "mujoco" + version = "{version}" + requires-python = ">=3.10" + dependencies = [] + + [tool.setuptools] + include-package-data = false + + [tool.setuptools.packages.find] + include = ["mujoco*"] +""") + +_INIT_PY_TEMPLATE = textwrap.dedent("""\ + \"\"\"Doc-only stub for MuJoCo. Provides enums and constants for autodoc.\"\"\" + import enum + import sys + + __path__ = __import__('pkgutil').extend_path(__path__, __name__) + + from mujoco.introspect.enums import ENUMS + + _mod = sys.modules[__name__] + for _n, _d in ENUMS.items(): + _cls = enum.IntEnum(_n, list(_d.values.items())) + setattr(_mod, _n, _cls) + for _vn, _vv in _d.values.items(): + setattr(_mod, _vn, _vv) + + {constants} + + try: + from importlib.metadata import version as _v + __version__ = _v('mujoco') + except Exception: + __version__ = '0.0.0' + + def mj_versionString(): + return __version__ + + # Stub types for C extensions (MjModel, MjData, mj_* functions, etc.) + # needed by MJX and mujoco_warp imports during Sphinx autodoc. + # Each accessed name gets a dynamically created class so that Sphinx + # renders the real type name instead of "Mock". + _mock_cache = {{}} + + def _make_mock_meta(mock_name): + class _MockMeta(type): + def __getattr__(cls, name): + return _make_mock(f'{{mock_name}}.{{name}}') + def __instancecheck__(cls, instance): + return True + def __repr__(cls): + return mock_name + return _MockMeta + + def _make_mock(qualname): + if qualname in _mock_cache: + return _mock_cache[qualname] + basename = qualname.rsplit('.', 1)[-1] + meta = _make_mock_meta(qualname) + cls = meta(basename, (), {{ + '__init__': lambda self, *a, **kw: None, + '__call__': lambda self, *a, **kw: _make_mock(qualname)(), + '__getattr__': lambda self, name: _make_mock(f'{{qualname}}.{{name}}'), + '__class_getitem__': classmethod(lambda cls, item: cls), + '__iter__': lambda self: iter([]), + '__bool__': lambda self: False, + '__repr__': lambda self: qualname, + '__module__': 'mujoco', + '__qualname__': basename, + }}) + _mock_cache[qualname] = cls + return cls + + def __getattr__(name): + return _make_mock(name) +""") + + + +def main(): + if len(sys.argv) != 2: + print(f'Usage: {sys.argv[0]} ', file=sys.stderr) + sys.exit(1) + + output_dir = sys.argv[1] + repo_root = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + version = _read_version(repo_root) + consts = _parse_constants(repo_root) + + mujoco_dir = os.path.join(output_dir, 'mujoco') + os.makedirs(mujoco_dir, exist_ok=True) + + # Write pyproject.toml. + with open(os.path.join(output_dir, 'pyproject.toml'), 'w') as f: + f.write(_PYPROJECT_TEMPLATE.format(version=version)) + + # Write mujoco/__init__.py with constants inlined. + constants_str = '\n'.join( + f'{k} = {v}' for k, v in sorted(consts.items())) + with open(os.path.join(mujoco_dir, '__init__.py'), 'w') as f: + f.write(_INIT_PY_TEMPLATE.format(constants=constants_str)) + + # Copy introspect/ into the package. + introspect_src = os.path.join(repo_root, 'python', 'mujoco', 'introspect') + introspect_dst = os.path.join(mujoco_dir, 'introspect') + if os.path.exists(introspect_dst): + shutil.rmtree(introspect_dst) + shutil.copytree(introspect_src, introspect_dst) + + print(f'Generated doc-only mujoco {version} package at {output_dir}') + + +if __name__ == '__main__': + main() From b9a1fad99bfdb065b3ee522530e602f49f01ee16 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Fri, 8 May 2026 13:39:56 -0700 Subject: [PATCH 232/251] Fix segmentation/depth rendering. PiperOrigin-RevId: 912674769 Change-Id: Iba525679630e7996bc27d916e61e5a265a147061 --- src/experimental/filament/filament/renderable.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 14655e0e..7597182a 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -243,7 +243,7 @@ const mjrMaterial& Renderable::GetMaterial() const { void Renderable::SetDrawMode(mjrDrawMode mode) { // Only SceneObjects support non-color draw modes. - if (!material_.decor_ux) { + if (material_.decor_ux) { mode = mjDRAW_MODE_COLOR; } From 4e2064b26fce6930a76879fa60231e999bc943a7 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Sat, 9 May 2026 08:38:41 -0700 Subject: [PATCH 233/251] Fix skin/flex rendering by setting size to 1 (not 0). PiperOrigin-RevId: 912993245 Change-Id: Ib81f58bd8d474decea469cafa40d57f053eee516 --- src/experimental/filament/compat/scene_geom_util.cc | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/experimental/filament/compat/scene_geom_util.cc b/src/experimental/filament/compat/scene_geom_util.cc index 9a733a29..99839645 100644 --- a/src/experimental/filament/compat/scene_geom_util.cc +++ b/src/experimental/filament/compat/scene_geom_util.cc @@ -153,7 +153,9 @@ static void PrepareGeomMeshes(mjrRenderable* renderable, const mjvGeom& geom, rotation[0] = 1.f; rotation[4] = 1.f; rotation[8] = 1.f; - std::memset(size, 0, sizeof(size)); + size[0] = 1.f; + size[1] = 1.f; + size[2] = 1.f; break; case mjGEOM_SKIN: mjrf_setRenderableMesh(renderable, GetSkinFlexMesh(model_objects, geom.objid), 0, 0); @@ -163,7 +165,9 @@ static void PrepareGeomMeshes(mjrRenderable* renderable, const mjvGeom& geom, rotation[0] = 1.f; rotation[4] = 1.f; rotation[8] = 1.f; - std::memset(size, 0, sizeof(size)); + size[0] = 1.f; + size[1] = 1.f; + size[2] = 1.f; break; case mjGEOM_NONE: case mjGEOM_LABEL: From cb5a9caa5c8b4a8c0691e2801540c05e294a92a9 Mon Sep 17 00:00:00 2001 From: Michael Moss Date: Sun, 10 May 2026 00:50:58 -0700 Subject: [PATCH 234/251] Update dependencies ahead of the 3.8.1 release. PiperOrigin-RevId: 913212199 Change-Id: I97e5a2367010c492e6277a8c333ae8eb8155f7f2 --- cmake/MujocoDependencies.cmake | 2 +- python/mujoco/CMakeLists.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/MujocoDependencies.cmake b/cmake/MujocoDependencies.cmake index d2404bc1..4381a759 100644 --- a/cmake/MujocoDependencies.cmake +++ b/cmake/MujocoDependencies.cmake @@ -39,7 +39,7 @@ set(MUJOCO_DEP_VERSION_qhull CACHE STRING "Version of `qhull` to be fetched." ) set(MUJOCO_DEP_VERSION_Eigen3 - 75bcd155c40cb48e647c87c3f29052360255bc9e + ea13a98decd497a8c5588fb5de71b57bcf10d864 CACHE STRING "Version of `Eigen3` to be fetched." ) diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index 35eeb831..e26be0dc 100644 --- a/python/mujoco/CMakeLists.txt +++ b/python/mujoco/CMakeLists.txt @@ -175,7 +175,7 @@ findorfetch( GIT_REPO https://gitlab.com/libeigen/eigen GIT_TAG - 75bcd155c40cb48e647c87c3f29052360255bc9e + ea13a98decd497a8c5588fb5de71b57bcf10d864 TARGETS Eigen3::Eigen EXCLUDE_FROM_ALL From 5ee8bd7b9c3147f1094816882903e741e53c26bf Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Sun, 10 May 2026 02:59:22 -0700 Subject: [PATCH 235/251] Rotate undeformed normals before using them in the bending passive forces. The normal jump residual in flex elasticity calculations now rotates the rest-frame normal jump into the current frame using the face's corotational quaternion before subtracting the jump from the current normal difference. PiperOrigin-RevId: 913242127 Change-Id: Ia62b28ecccd59e79225737d8225ef4131b333c96 --- src/engine/engine_passive.c | 27 ++++++++++--- test/engine/engine_passive_test.cc | 64 ++++++++++++++++++++++-------- 2 files changed, 70 insertions(+), 21 deletions(-) diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index b1470ec3..b7ae4280 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -291,15 +291,16 @@ static void mj_flexPassiveBendInterp(const mjModel* m, mjData* d, int f, if (stiffness == 0) continue; - // gather face A and B positions + velocities + // gather face A and B positions + corotational quats + mjtNum quat_A[4], quat_B[4]; mju_flexGatherFaceState(order, cx, cy, cz, fe_A, xpos_g, enbl_damper ? vel_g : NULL, NULL, xpos_A, enbl_damper ? vel_A : NULL, NULL, - gidx_A, NULL); + gidx_A, quat_A); mju_flexGatherFaceState(order, cx, cy, cz, fe_B, xpos_g, enbl_damper ? vel_g : NULL, NULL, xpos_B, enbl_damper ? vel_B : NULL, NULL, - gidx_B, NULL); + gidx_B, quat_B); // compute deformed normals at edge midpoint mjtNum n_A[3], t1_A[3], t2_A[3]; @@ -316,10 +317,26 @@ static void mj_flexPassiveBendInterp(const mjModel* m, mjData* d, int f, n_A[0] *= inv_A; n_A[1] *= inv_A; n_A[2] *= inv_A; n_B[0] *= inv_B; n_B[1] *= inv_B; n_B[2] *= inv_B; - // normal jump residual: r = (n_A - n_B) - dn0 + // average corotational frame: symmetric under face swap + // quat_A and quat_B encode R^{-1}; average them, then negate to get R_avg + // ensure quaternions are in the same hemisphere before averaging + if (mju_dot(quat_A, quat_B, 4) < 0) { + mju_scl(quat_B, quat_B, -1, 4); + } + mjtNum quat_avg[4]; + mju_add(quat_avg, quat_A, quat_B, 4); + mju_normalize(quat_avg, 4); // NLERP = SLERP at t=0.5 for two quaternions + // negate to get R_avg (from rest frame to current frame) + mju_negQuat(quat_avg, quat_avg); + + // rotate dn0 from rest frame to current frame using average corotational R + mjtNum dn0_rot[3]; + mju_rotVecQuat(dn0_rot, dn0, quat_avg); + + // normal jump residual: r = (n_A - n_B) - R_avg * dn0 mjtNum r[3]; mji_sub3(r, n_A, n_B); - r[0] -= dn0[0]; r[1] -= dn0[1]; r[2] -= dn0[2]; + r[0] -= dn0_rot[0]; r[1] -= dn0_rot[1]; r[2] -= dn0_rot[2]; // --- spring force --- if (enbl_spring) { diff --git a/test/engine/engine_passive_test.cc b/test/engine/engine_passive_test.cc index 461b0090..be0f2850 100644 --- a/test/engine/engine_passive_test.cc +++ b/test/engine/engine_passive_test.cc @@ -955,11 +955,11 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) { - - + @@ -971,25 +971,57 @@ TEST_F(ElasticityTest, InterpBendingRigidRotationInvariance) { ASSERT_THAT(m, testing::NotNull()) << error; mjData* d = mj_makeData(m); - // apply a rigid rotation by setting all body quats to a 30 degree rotation - // about z-axis (all flex node bodies get the same rotation) - mjtNum angle = 30 * 3.14159265358979 / 180.0; - mjtNum sa = mju_sin(angle / 2), ca = mju_cos(angle / 2); + // compute geometric center from body positions (skip world body) + mjtNum center[3] = {0, 0, 0}; + int nnodes = 0; for (int b = 1; b < m->nbody; b++) { - int qadr = m->jnt_qposadr[m->body_jntadr[b]]; - if (m->body_jntnum[b] > 0 && m->jnt_type[m->body_jntadr[b]] == mjJNT_FREE) { - d->qpos[qadr + 3] = ca; - d->qpos[qadr + 4] = 0; - d->qpos[qadr + 5] = 0; - d->qpos[qadr + 6] = sa; + center[0] += m->body_pos[3*b + 0]; + center[1] += m->body_pos[3*b + 1]; + center[2] += m->body_pos[3*b + 2]; + nnodes++; + } + ASSERT_GT(nnodes, 0); + center[0] /= nnodes; center[1] /= nnodes; center[2] /= nnodes; + + // rotation: 45 degrees about (1,1,1)/sqrt(3) + mjtNum angle = 45 * 3.14159265358979 / 180.0; + mjtNum sa = mju_sin(angle / 2), ca = mju_cos(angle / 2); + mjtNum inv_sqrt3 = 1.0 / mju_sqrt(3.0); + mjtNum quat[4] = {ca, sa * inv_sqrt3, sa * inv_sqrt3, sa * inv_sqrt3}; + mjtNum neg_quat[4]; + mju_negQuat(neg_quat, quat); + + // apply rigid rotation via slide joint displacements: + // new_pos = center + R * (body_pos - center) + // qpos = new_pos - body_pos + for (int b = 1; b < m->nbody; b++) { + mjtNum rel[3] = {m->body_pos[3*b+0] - center[0], + m->body_pos[3*b+1] - center[1], + m->body_pos[3*b+2] - center[2]}; + mjtNum rotated[3]; + mju_rotVecQuat(rotated, rel, neg_quat); + + // each body has 3 slide joints (x, y, z) + for (int j = 0; j < m->body_jntnum[b] && j < 3; j++) { + int jid = m->body_jntadr[b] + j; + int qadr = m->jnt_qposadr[jid]; + int axis = -1; + for (int a = 0; a < 3; a++) { + if (m->jnt_axis[3*jid + a] != 0) { axis = a; break; } + } + if (axis >= 0) { + d->qpos[qadr] = + (center[axis] + rotated[axis]) - m->body_pos[3 * b + axis]; + } } } mj_forward(m, d); - // spring forces should still be zero (or very small) after rigid rotation + // spring forces should still be zero after rigid rotation + constexpr mjtNum tol = MjTol(1e-6, 1e-3); for (int i = 0; i < m->nv; i++) { - EXPECT_NEAR(d->qfrc_spring[i], 0, 1e-6) + EXPECT_NEAR(d->qfrc_spring[i], 0, tol) << "nonzero spring force at DOF " << i << " after rigid rotation"; } From 688209574d35555fb897be5795f1ce4ecca9f29a Mon Sep 17 00:00:00 2001 From: Michael Moss Date: Mon, 11 May 2026 02:14:17 -0700 Subject: [PATCH 236/251] Update changelog for the 3.8.1 release. PiperOrigin-RevId: 913578392 Change-Id: I7367ff9044fd52039cec8dd492b37df2153a6d7f --- doc/changelog.rst | 62 +++++++++++++++++++++++------------------------ 1 file changed, 31 insertions(+), 31 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 19436ea2..e408b4fa 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,54 +2,54 @@ Changelog ========= -Upcoming version (not yet released) ------------------------------------ +Version 3.8.1 (May 11, 2026) +---------------------------- General ^^^^^^^ -- Added island support for the :ref:`PGS solver`. -- The :ref:`PGS solver` now iterates over constraints in pseudo-random order, improving performance by - ~20%. -- Added support for :ref:`elastic2d` for trilinear and quadratic flex - :ref:`dofs`. -- :ref:`Midpoint integration` is now restricted to the ``implicitfast`` - :ref:`integrator` and is disabled when fluid forces are active - (nonzero :ref:`density` or :ref:`viscosity`). - Midpoint integration treats external forces as zero-order-hold constants, which causes - energy gain in the presence of contacts and in fluid media. -- Added :ref:`mju_sym2dense`, converting a lower-triangular, implicitly symmetric CSR matrix to a dense - symmetric matrix. The inertia matrix ``mjData.M`` is an example of such a matrix. -- Added :ref:`mjs_getOriginSpec`, returning the spec that originally defined an element, prior to attachment. This is in - contrast to :ref:`mjs_getSpec` which returns the spec currently owning the element. If the element is not the result - of an attach operation, the functions are identical. +1. Added island support for the :ref:`PGS solver`. +2. The :ref:`PGS solver` now iterates over constraints in pseudo-random order, improving performance by + ~20%. +3. Added support for :ref:`elastic2d` for trilinear and quadratic flex + :ref:`dofs`. +4. :ref:`Midpoint integration` is now restricted to the ``implicitfast`` + :ref:`integrator` and is disabled when fluid forces are active + (nonzero :ref:`density` or :ref:`viscosity`). + Midpoint integration treats external forces as zero-order-hold constants, which causes + energy gain in the presence of contacts and in fluid media. +5. Added :ref:`mjs_getOriginSpec`, returning the spec that originally defined an element, prior to attachment. This is + in contrast to :ref:`mjs_getSpec` which returns the spec currently owning the element. If the element is not the + result of an attach operation, the functions are identical. +6. Added :ref:`mju_sym2dense`, converting a lower-triangular, implicitly symmetric CSR matrix to a dense symmetric + matrix. The inertia matrix ``mjData.M`` is an example of such a matrix. .. admonition:: Future breaking API changes :class: warning - - The introduction of :ref:`mju_sym2dense` is a step towards the removal of the legacy-format ``mjData.qM`` in favor - of the CSR-format ``mjData.M``. This removal will involve a future breaking change to :ref:`mj_fullM` (which - currently accepts a ``qM``-like matrix as an argument). To prevent a future breakage, replace - ``mj_fullM(m, dst, d->qM)`` with - |br| ``mju_sym2dense(dst, d->M, m->nv, m->M_rownnz, m->M_rowadr, m->M_colind)``. - + 7. The introduction of :ref:`mju_sym2dense` is a step towards the removal of the legacy-format ``mjData.qM`` in favor + of the CSR-format ``mjData.M``. This removal will involve a future breaking change to :ref:`mj_fullM` (which + currently accepts a ``qM``-like matrix as an argument). To prevent a future breakage, replace + ``mj_fullM(m, dst, d->qM)`` with + |br| ``mju_sym2dense(dst, d->M, m->nv, m->M_rownnz, m->M_rowadr, m->M_colind)``. Bug fixes ^^^^^^^^^ -- Fixed default for multiccd in :doc:`mjcPhysics `. +8. Fixed default for multiccd in :doc:`mjcPhysics `. Python ^^^^^^ -- Added ``MjSpec.encode`` method, wrapping :ref:`mj_encode`. -- Added ``mujoco.MjVfs`` Python binding to interact with the Virtual File System directly from Python. - See :ref:`Virtual File System ` for usage details. +9. Added ``MjSpec.encode`` method, wrapping :ref:`mj_encode`. +10. Added ``mujoco.MjVfs`` Python binding to interact with the Virtual File System directly from Python. + See :ref:`Virtual File System ` for usage details. + + .. warning:: + The previous way of passing assets via a dictionary mapping asset names to bytes is **deprecated** and will be + removed in an upcoming release. You cannot specify both the ``assets`` dictionary and the ``vfs`` argument at the + same time. ``MjVfs`` should be used as a drop-in replacement. - .. warning:: - The previous way of passing assets via a dictionary mapping asset names to bytes is **deprecated** and will be - removed in an upcoming release. You cannot specify both the ``assets`` dictionary and the ``vfs`` argument at the same - time. ``MjVfs`` should be used as a drop-in replacement. Version 3.8.0 (April 24, 2026) ------------------------------ From df59e7d0f16520247e0396be187c86a0e28614b9 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 11 May 2026 02:44:50 -0700 Subject: [PATCH 237/251] Fix mj_makeConstraint error for flex trilinear vs trilinear contacts. The coordinates for flex interpolation are now computed using the absolute values of the vertex weights. The sign of the first vertex weight is then applied to the resulting barycentric weights. This correctly handles cases where the flex is both the first and the second entity in the contact pair. PiperOrigin-RevId: 913590928 Change-Id: I970b35fba3d209e13b5b33bb5f945e3c6a43d487 --- src/engine/engine_core_constraint.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index b74f19b2..2f94818f 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -268,10 +268,13 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v, return 0; } - // compute parametric coordinates of the vertex in [0, 1]^3 + // determine sign: vweight may be negative for side-0 of a contact pair + mjtNum sign = vweight[0] < 0 ? -1 : 1; + + // compute parametric coordinates using absolute weights mjtNum coord[3] = {0, 0, 0}; for (int i = 0; i < nw; i++) { - mju_addToScl3(coord, m->flex_vert0 + 3*v[i], vweight[i]); + mju_addToScl3(coord, m->flex_vert0 + 3*v[i], mju_abs(vweight[i])); } int order = m->flex_interp[f]; @@ -292,7 +295,7 @@ static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int* v, if (w < 1e-5) { continue; } - if (bweight) bweight[nb] = w; + if (bweight) bweight[nb] = sign * w; body[nb++] = m->flex_nodebodyid[nstart + nodeindices[j]]; } From 28548ce51c2e26d7623464ff86a7867c81060a54 Mon Sep 17 00:00:00 2001 From: Michael Moss Date: Mon, 11 May 2026 02:58:23 -0700 Subject: [PATCH 238/251] Update MuJoCo version to 3.9.0 following the 3.8.1 release PiperOrigin-RevId: 913596257 Change-Id: Ib8b27fd2224e38560a209054c1d508a9bba21bbf --- CMakeLists.txt | 2 +- dist/mujoco.rc | 8 ++++---- dist/simulate.rc | 8 ++++---- doc/APIreference/APIglobals.rst | 2 +- doc/unity.rst | 4 ++-- include/mujoco/mujoco.h | 2 +- mjx/pyproject.toml | 8 ++++---- python/mujoco/CMakeLists.txt | 4 ++-- python/mujoco/mjpython/Info.plist | 8 ++++---- python/pyproject.toml | 6 +++--- sample/CMakeLists.txt | 2 +- simulate/CMakeLists.txt | 2 +- src/engine/engine_support.c | 4 ++-- unity/Editor/Bindings/MujocoBinaryRetriever.cs | 4 ++-- unity/Runtime/Bindings/MjBindings.cs | 2 +- unity/package.json | 2 +- 16 files changed, 34 insertions(+), 34 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 08e0bd0b..92254c2d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco - VERSION 3.8.1 + VERSION 3.9.0 DESCRIPTION "MuJoCo Physics Simulator" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/dist/mujoco.rc b/dist/mujoco.rc index 6400ab0d..b9f1eff7 100644 --- a/dist/mujoco.rc +++ b/dist/mujoco.rc @@ -1,6 +1,6 @@ 1 VERSIONINFO -FILEVERSION 3,8,1,0 -PRODUCTVERSION 3,8,1,0 +FILEVERSION 3,9,0,0 +PRODUCTVERSION 3,9,0,0 FILEOS 0x4 FILETYPE 0x1 { @@ -9,9 +9,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.8.1" + VALUE "ProductVersion", "3.9.0" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.8.1" + VALUE "FileVersion", "3.9.0" VALUE "InternalName", "mujoco.dll" VALUE "OriginalFilename", "mujoco.dll" VALUE "CompanyName", "Google DeepMind" diff --git a/dist/simulate.rc b/dist/simulate.rc index a7696780..dd3f0778 100644 --- a/dist/simulate.rc +++ b/dist/simulate.rc @@ -1,8 +1,8 @@ MUJOCO ICON "mujoco.ico" 1 VERSIONINFO -FILEVERSION 3,8,1,0 -PRODUCTVERSION 3,8,1,0 +FILEVERSION 3,9,0,0 +PRODUCTVERSION 3,9,0,0 FILEOS 0x4 FILETYPE 0x1 { @@ -11,9 +11,9 @@ FILETYPE 0x1 BLOCK "040904b0" { VALUE "ProductName", "MuJoCo" - VALUE "ProductVersion", "3.8.1" + VALUE "ProductVersion", "3.9.0" VALUE "FileDescription", "MuJoCo" - VALUE "FileVersion", "3.8.1" + VALUE "FileVersion", "3.9.0" VALUE "InternalName", "simulate.exe" VALUE "OriginalFilename", "simulate.exe" VALUE "CompanyName", "Google DeepMind" diff --git a/doc/APIreference/APIglobals.rst b/doc/APIreference/APIglobals.rst index dae8fccd..ba2d63ae 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -388,7 +388,7 @@ Defined in `mujoco.h diff --git a/mjx/pyproject.toml b/mjx/pyproject.toml index 9d3e2f85..aa5f1573 100644 --- a/mjx/pyproject.toml +++ b/mjx/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name="mujoco-mjx" -version = "3.8.1" +version = "3.9.0" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -30,7 +30,7 @@ dependencies = [ "etils[epath]", "jax", "jaxlib", - "mujoco>=3.8.1.dev0", + "mujoco>=3.9.0.dev0", "scipy", "trimesh", ] @@ -50,9 +50,9 @@ mjx-viewer = "mujoco.mjx.viewer:main" [project.urls] Homepage = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Documentation = "https://mujoco.readthedocs.io/en/3.8.1" +Documentation = "https://mujoco.readthedocs.io/en/3.9.0" Repository = "https://github.com/google-deepmind/mujoco/tree/main/mjx" -Changelog = "https://mujoco.readthedocs.io/en/3.8.1/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.9.0/changelog.html" [tool.isort] force_single_line = true diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index e26be0dc..8810b3b1 100644 --- a/python/mujoco/CMakeLists.txt +++ b/python/mujoco/CMakeLists.txt @@ -86,7 +86,7 @@ if(NOT TARGET mujoco) if(MUJOCO_FRAMEWORK) message("MuJoCo framework is at ${MUJOCO_FRAMEWORK}/mujoco.framework") set(MUJOCO_LIBRARY - ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.8.1.dylib + ${MUJOCO_FRAMEWORK}/mujoco.framework/Versions/A/libmujoco.3.9.0.dylib ) target_compile_options(mujoco INTERFACE -F${MUJOCO_FRAMEWORK}) endif() @@ -94,7 +94,7 @@ if(NOT TARGET mujoco) if(NOT MUJOCO_FRAMEWORK) find_library( - MUJOCO_LIBRARY mujoco mujoco.3.8.1 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED + MUJOCO_LIBRARY mujoco mujoco.3.9.0 HINTS ${MUJOCO_LIBRARY_DIR} REQUIRED ) find_path(MUJOCO_INCLUDE mujoco/mujoco.h HINTS ${MUJOCO_INCLUDE_DIR} REQUIRED) message("MuJoCo is at ${MUJOCO_LIBRARY}") diff --git a/python/mujoco/mjpython/Info.plist b/python/mujoco/mjpython/Info.plist index f18c2a35..3bb2afb2 100644 --- a/python/mujoco/mjpython/Info.plist +++ b/python/mujoco/mjpython/Info.plist @@ -7,13 +7,13 @@ CFBundleIdentifier org.mujoco.mjpython CFBundleVersion - 3.8.1 + 3.9.0 CFBundleGetInfoString - 3.8.1 + 3.9.0 CFBundleLongVersionString - 3.8.1 + 3.9.0 CFBundleShortVersionString - 3.8.1 + 3.9.0 CFBundleExecutable mjpython CFBundleIconFile diff --git a/python/pyproject.toml b/python/pyproject.toml index 8621f35c..1e7e3632 100644 --- a/python/pyproject.toml +++ b/python/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "mujoco" -version = "3.8.1" +version = "3.9.0" authors = [ {name = "Google DeepMind", email = "mujoco@deepmind.com"}, ] @@ -35,9 +35,9 @@ dynamic = ["readme", "scripts"] [project.urls] Homepage = "https://github.com/google-deepmind/mujoco" -Documentation = "https://mujoco.readthedocs.io/en/3.8.1" +Documentation = "https://mujoco.readthedocs.io/en/3.9.0" Repository = "https://github.com/google-deepmind/mujoco" -Changelog = "https://mujoco.readthedocs.io/en/3.8.1/changelog.html" +Changelog = "https://mujoco.readthedocs.io/en/3.9.0/changelog.html" [tool.setuptools] include-package-data = false diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt index deb9cf63..88e9cb87 100644 --- a/sample/CMakeLists.txt +++ b/sample/CMakeLists.txt @@ -24,7 +24,7 @@ set(MSVC_INCREMENTAL_DEFAULT ON) project( mujoco_samples - VERSION 3.8.1 + VERSION 3.9.0 DESCRIPTION "MuJoCo samples binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/simulate/CMakeLists.txt b/simulate/CMakeLists.txt index 2e8cee6a..f12701bb 100644 --- a/simulate/CMakeLists.txt +++ b/simulate/CMakeLists.txt @@ -29,7 +29,7 @@ set(MUJOCO_DEP_VERSION_lodepng project( mujoco_simulate - VERSION 3.8.1 + VERSION 3.9.0 DESCRIPTION "MuJoCo simulate binaries" HOMEPAGE_URL "https://mujoco.org" ) diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index c68a5d5c..18edd0de 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -43,8 +43,8 @@ //-------------------------- Constants ------------------------------------------------------------- - #define mjVERSION 3008001 -#define mjVERSIONSTRING "3.8.1" + #define mjVERSION 3009000 +#define mjVERSIONSTRING "3.9.0" // names of disable flags const char* mjDISABLESTRING[mjNDISABLE] = { diff --git a/unity/Editor/Bindings/MujocoBinaryRetriever.cs b/unity/Editor/Bindings/MujocoBinaryRetriever.cs index be5112ab..1ab0f725 100644 --- a/unity/Editor/Bindings/MujocoBinaryRetriever.cs +++ b/unity/Editor/Bindings/MujocoBinaryRetriever.cs @@ -37,7 +37,7 @@ public class MujocoBinaryRetriever { if (AssetDatabase.LoadMainAssetAtPath(mujocoPath + "/mujoco.dylib") == null) { File.Copy( "/Applications/MuJoCo.app/Contents/Frameworks" + - "/mujoco.framework/Versions/Current/libmujoco.3.8.1.dylib", + "/mujoco.framework/Versions/Current/libmujoco.3.9.0.dylib", mujocoPath + "/mujoco.dylib"); AssetDatabase.Refresh(); } @@ -45,7 +45,7 @@ public class MujocoBinaryRetriever { if (AssetDatabase.LoadMainAssetAtPath(mujocoPath + "/libmujoco.so") == null) { File.Copy( Environment.GetFolderPath(Environment.SpecialFolder.UserProfile) + - "/.mujoco/mujoco-3.8.1/lib/libmujoco.so.3.8.1", + "/.mujoco/mujoco-3.9.0/lib/libmujoco.so.3.9.0", mujocoPath + "/libmujoco.so"); AssetDatabase.Refresh(); } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index c7cbbe66..a655529c 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -113,7 +113,7 @@ public const int mjMAXLINEPNT = 1001; public const int mjMAXPLANEGRID = 200; public const bool THIRD_PARTY_MUJOCO_MJXMACRO_H_ = true; public const bool THIRD_PARTY_MUJOCO_MUJOCO_H_ = true; -public const int mjVERSION_HEADER = 3008001; +public const int mjVERSION_HEADER = 3009000; // ------------------------------------Enums------------------------------------ diff --git a/unity/package.json b/unity/package.json index 4e9bf364..d533ede4 100644 --- a/unity/package.json +++ b/unity/package.json @@ -1,7 +1,7 @@ { "name": "org.mujoco", "displayName": "MuJoCo", - "version": "3.8.1", + "version": "3.9.0", "description": "MuJoCo importer and runtime plug-in", "dependencies": {}, "author": { From ff4a5673f8bde00049baadcdcb10977404d51d23 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Mon, 11 May 2026 05:25:09 -0700 Subject: [PATCH 239/251] Delete testspeed_test.sh This shell test was extremely slow (taking up to 26 minutes under ASAN). In any case the compilation pipeline takes one kick-the-tires step, so the RecompileCompare test effectively does the same thing. PiperOrigin-RevId: 913646840 Change-Id: If659705b88d0ac64722f88ddf2ae377932e5ef0f --- test/sample/CMakeLists.txt | 1 - test/sample/testspeed_test.sh | 91 ----------------------------------- 2 files changed, 92 deletions(-) delete mode 100755 test/sample/testspeed_test.sh diff --git a/test/sample/CMakeLists.txt b/test/sample/CMakeLists.txt index 0d1061e3..15f87dc2 100644 --- a/test/sample/CMakeLists.txt +++ b/test/sample/CMakeLists.txt @@ -16,5 +16,4 @@ if(MUJOCO_BUILD_EXAMPLES) include(ShellTests) add_mujoco_shell_test(compile_test compile) - add_mujoco_shell_test(testspeed_test testspeed) endif() diff --git a/test/sample/testspeed_test.sh b/test/sample/testspeed_test.sh deleted file mode 100755 index 79d6c8c5..00000000 --- a/test/sample/testspeed_test.sh +++ /dev/null @@ -1,91 +0,0 @@ -#!/bin/bash -# Copyright 2021 DeepMind Technologies Limited -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -MODEL_DIRS=( - "${CMAKE_SOURCE_DIR}/model" - "${CMAKE_SOURCE_DIR}/test" -) - -die() { echo "$*" 1>&2 ; exit 1; } - -test_model() { - local EXPECTED_STR='Simulation time' - local model="$1" - echo "Testing $model" >&2 - - local iterations=10 - # for particularly slow models, only run 2 steps under ASAN, or skip. - if [[ ${TESTSPEED_ASAN:-0} != 0 ]]; then - if [[ "$model" == */humanoid/100_humanoids.xml || - "$model" == */composite/particle.xml || - "$model" == */replicate/bunnies.xml || - "$model" == */replicate/leaves.xml || - "$model" == */replicate/particle.xml || - "$model" == */perf/* - ]]; then - # these tests can take several minutes under ASAN - return 0 - fi - if [[ "$model" == */benchmark/testdata/humanoid200.xml || - "$model" == */engine/testdata/collision_convex/stacked_boxes.xml || - "$model" == */user/testdata/shark_22_ascii_fTetWild.xml || - "$model" == */user/testdata/shark_22_binary_fTetWild.xml - ]]; then - iterations=2 - fi - fi - - # run testspeed, writing its output to stderr. - # die if testspeed returns a failure code, or if it doesn't have the string - # "Simulation time" in the output. - ("$TARGET_BINARY" "$model" "$iterations" || die "testspeed failed") \ - | tee >(cat 1>&2) | grep -q "$EXPECTED_STR" - - if [ "$?" != 0 ]; then - die "Expected string not found in output ($EXPECTED_STR)." - fi -} - -if [ -z "$TARGET_BINARY" ]; then - die "Expecting environment variable TARGET_BINARY." -fi - -if [ -z "$MUJOCO_DLL_DIR" ]; then - # Extend PATH to include the directory containing the mujoco DLL. - # This is needed on Windows. - PATH=$PATH:$MUJOCO_DLL_DIR -fi - -shopt -s globstar -for model_dir in ${MODEL_DIRS[@]}; do - echo "Looking in $model_dir" - for model in $model_dir/**/*.xml; do - if [[ $(basename $model) == malformed* ]]; then - echo "Skipping $model" >&2 - continue - fi - if [[ $(basename $model) == *_fail.xml ]]; then - echo "Skipping $model" >&2 - continue - fi - if grep -q "plugin" $model; then - continue - fi - test_model "$model" - done -done - -cd $CURRENT_DIR -echo "PASS" From 465e574c42005348c0863d70c911619b15eea5d6 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Mon, 11 May 2026 07:42:14 -0700 Subject: [PATCH 240/251] Test scene for shape generators. PiperOrigin-RevId: 913694536 Change-Id: I49a2a3a4f918f9b80650ce884d3d442581e60d6b --- test/user/testdata/shapes.xml | 62 ++++++++++++++++++++++++++++ test/user/testdata/test-pattern.png | Bin 0 -> 3148 bytes 2 files changed, 62 insertions(+) create mode 100644 test/user/testdata/shapes.xml create mode 100644 test/user/testdata/test-pattern.png diff --git a/test/user/testdata/shapes.xml b/test/user/testdata/shapes.xml new file mode 100644 index 00000000..06b4b7d1 --- /dev/null +++ b/test/user/testdata/shapes.xml @@ -0,0 +1,62 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/user/testdata/test-pattern.png b/test/user/testdata/test-pattern.png new file mode 100644 index 0000000000000000000000000000000000000000..ff5eaa85f944fbbaf1aae0ec15b5e06a42846ae2 GIT binary patch literal 3148 zcmZvedo+~m9>;(0yu*xfnF*CjjJA!6nj*PW#-$wzlWx;0x3--wNC-QO7r9kRY^f7w zQpxU!DJA7Hqm+uGvfW0R6ml6%!eFL3qqFupXRUMIwchnUzvub=oY zb<$!I0I0h-J9z>iiXj3-RPi}l%-lH_g?VmqKssKS#wh}o5a+#N0IFJZF9QF_)lp2E zGq&yA+@5@-=k{q)%Pr|->#MH1{KsV{%1hmD-KzIo(R!NMTbnfdwXWjTKf{YMZF0^+ zQj(%>YjA^^VxPM%PWC&aaz%ORyV2!pNdvr5`MvV;6M23ZJVdUVzF(L7DYdXXs}xPF zc4@!tAdroU^b2HRQ*jSgVjadbK?xjW^8+h!mtt{cB9iZ<(I8J{nh(UozFmj+-tO3M%9p zH9KHZ5YTdlExfB&TAqV4W!}0|dF?eb)m9_6cVrcMqbjF;r&uV=PDsu=Gb*R95XSCQ zJ%DlX1UfIm)dWn9Q>~saeNj#(dDYGJYX8w(W^Q+2{gx z`f9rT8d!%LZrDq$W75#`Bw{1m=aYmtk!~_U>*_`T)$Dw#z{2JjaIDW z=z%SP0Raa!sL_LvlzA`^weppqM75(Us_d0$ z?Ikdb79FfIcd{La;mC#d8L@$SY5i3v1FuWsoz1Y9Z#E8Yx80?xFBlE6SsuTP%AHVY357fuR`nY$R&TI(B(lCrwe95 zLJHBFcfbG-J6D~e0*`{r5Ps1r7oo&~nS2HH2s#qpIUkFr|G{eN?an&i5ZPGY=WXs4SKFc=V%-)arC-0a z=H+(JII0KF*kx)<->+fJI6i_Q`0pwS;asf~Xm^_YLxWOPFnvNZVOjgo5IN&3T6b=c z90-};qD$Gy#;M_Dy8M)@iyr7N!kCtydk8U|Yj(L;ZEVpWD{%2APY|BZdJG@`!jDHN zH@im{fxCqgq#lx7@Ikh}Tz*6yTibFL>ym<>nh4YuFLS)5Grmt z;fE-O`0_|I9)Mj>h(0 zhg6i|lbYs3$lxM+(EYvHvqG$*%?~7K({FnHH|NQ+L#u~|6oTdRvM#GdNWM@&8=__bl7&NkfQcrGa)u(`NI`{^icwk?p5%nsF zf@hfC?y*c&_|}M?va0ZessV694e-d$zw{}s$O|_{&SQmZn1DjJ&uTE-B@qW_(k`#s zz{-_sY~wj-E2g4xij2nCSVf99ze|guKs+*J3x1yhtYsTsZTL3nKXml^lE8;;CM-Vj z<%OU<8IjKA-<6};f{QKz*04t09Hr!I<@ip&pF!77C?hyh z7c$-%Wc1}5Ir95IklrpTru&`UlhHxa?%Nf8$!U!f3KY2jB&(F07diEoCKnn2oFb|D zTijDe7Jw{7eRD=d{?7OUHI9YK0wkk@^P8pxjpb_sf?L6f^M|k!0kz1Xto&;b8q*1{Q#q4f(?MvY*Z}kq?GSBX-k_W z&1rH>p~+2^ij+hjAB8Gw6{?&WCJB9Tl(-{}*|*c}thPkQ39dKXjagMk*5T*|@iN9W z-c|Femkugh!<~>jS?*h*`C0w^NA`w#N2j_dAL9)-W|V@jf(DsF6rw1o!!jE>uuX{tS$SC-;|O=iK&3L6Pk8jj@V_ z42Yd`O=aF3!9*WA@~xvDrHIDIXH)9Q!>;70u;}$1*%fm9M@J{5>S5yCB{x@K!}j?* zoNPlTUsMFGAz+I2+^V8IZ z_VseTCgm4~QDgQNQo}E+VuOk8Z)G}IHR21Fm>MGaD=5Q@P&H=x94X?de|5xyefhX3 zv4aU=>`D@aK=mg6mPOr56c1r0r&Ce>g8^4Bnc`6X2RRE0Vr!%u_v^7`8ty!YgKUoC z_@HxV5-E=GB~&P9pu9UKjt zRV`ie86d$lr)jIfU Date: Mon, 11 May 2026 07:43:29 -0700 Subject: [PATCH 241/251] Save/restore window size in UX settings. PiperOrigin-RevId: 913695007 Change-Id: I3366b11a5de53292d4ffe0e4c46f866381624788 --- src/experimental/platform/hal/window.cc | 12 ++++++++++++ src/experimental/platform/hal/window.h | 3 +++ src/experimental/studio/app.cc | 17 ++++++++++++++++- src/experimental/studio/app.h | 2 ++ 4 files changed, 33 insertions(+), 1 deletion(-) diff --git a/src/experimental/platform/hal/window.cc b/src/experimental/platform/hal/window.cc index 054fcb88..4d481b15 100644 --- a/src/experimental/platform/hal/window.cc +++ b/src/experimental/platform/hal/window.cc @@ -177,6 +177,18 @@ void Window::SetTitle(std::string_view title) { SDL_SetWindowTitle(sdl_window_, title.data()); } +void Window::Resize(int width, int height) { + SDL_SetWindowSize(sdl_window_, width, height); + SDL_SetWindowPosition(sdl_window_, SDL_WINDOWPOS_CENTERED, + SDL_WINDOWPOS_CENTERED); + + SDL_GetWindowSize(sdl_window_, &width_, &height_); + int drawable_width = width_; + int drawable_height = height_; + SDL_GL_GetDrawableSize(sdl_window_, &drawable_width, &drawable_height); + scale_ = (float)drawable_width / (float)width_; +} + void Window::DisableWindowResizing() { SDL_SetWindowResizable(sdl_window_, SDL_FALSE); } diff --git a/src/experimental/platform/hal/window.h b/src/experimental/platform/hal/window.h index 4e9f504a..2cc61452 100644 --- a/src/experimental/platform/hal/window.h +++ b/src/experimental/platform/hal/window.h @@ -79,6 +79,9 @@ class Window { // Returns the graphics configuration of the window. GraphicsMode GetGraphicsMode() const; + // Resizes the window to the given width and height. + void Resize(int width, int height); + // Enables window resizing. void EnableWindowResizing(); diff --git a/src/experimental/studio/app.cc b/src/experimental/studio/app.cc index 44442808..ece64f22 100644 --- a/src/experimental/studio/app.cc +++ b/src/experimental/studio/app.cc @@ -104,6 +104,11 @@ void App::SwitchGraphicsMode(int width, int height, window_config); renderer_ = std::make_unique( window_->GetNativeWindowHandle(), gfx_mode_); + + LoadSettings(); + if (ui_.window_width > 0 && ui_.window_height > 0) { + window_->Resize(ui_.window_width, ui_.window_height); + } } void App::Recompile() { @@ -791,6 +796,10 @@ void App::LoadSettings() { void App::SaveSettings() { if (!ini_path_.empty()) { std::string settings = ImGui::SaveIniSettingsToMemory(); + if (window_) { + ui_.window_width = window_->GetWidth(); + ui_.window_height = window_->GetHeight(); + } platform::AppendIniSection(settings, "[Studio][UX]", ui_.ToDict()); platform::KeyValues plugin_names; @@ -1839,12 +1848,18 @@ App::UiState::Dict App::UiState::ToDict() const { return { {"theme", std::to_string(static_cast(theme))}, {"font_scale", std::to_string(font_scale)}, + {"window_width", std::to_string(window_width)}, + {"window_height", std::to_string(window_height)}, }; } void App::UiState::FromDict(const Dict& dict) { + using platform::ReadIniValue; + *this = UiState(); theme = ReadIniValue(dict, "theme", theme); - font_scale = platform::ReadIniValue(dict, "font_scale", font_scale); + window_width = ReadIniValue(dict, "window_width", window_width); + window_height = ReadIniValue(dict, "window_height", window_height); + font_scale = ReadIniValue(dict, "font_scale", font_scale); } } // namespace mujoco::studio diff --git a/src/experimental/studio/app.h b/src/experimental/studio/app.h index 4f2b734c..b38d551b 100644 --- a/src/experimental/studio/app.h +++ b/src/experimental/studio/app.h @@ -106,6 +106,8 @@ class App { int key_idx = 0; platform::GuiTheme theme = platform::GuiTheme::kLight; float font_scale = 1.0f; + int window_width = 0; + int window_height = 0; using Dict = std::unordered_map; Dict ToDict() const; From f9f1db1e0a176da2c398458f3b17cd2c47527b38 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Mon, 11 May 2026 10:09:37 -0700 Subject: [PATCH 242/251] Replace the banded Cholesky solver for implicit flex interpolation with a preconditioned Conjugate Gradient (CG) solver that operates on the full system matrix. MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The previous approach extracted flex DOFs into a reduced banded system, factored it separately, and overwrote the global solve. This required precomputed bandwidth (makeFlexBandwidth), parent-joint detection, coupling corrections, and a FlexInterpContext struct — and only worked for standalone flex trees without parent joints. The new CG solver uses the already-factored global system (M - h*qDeriv) as a preconditioner and adds the flex stiffness contribution via matrix-free products (mjd_flexInterp_mulKD/mulK). This handles any kinematic configuration — including flexes attached to articulated chains or with parent joints — without sparsity pattern restrictions. Before (`bunny_multicell`): ``` Simulation time : 50.80 s Steps per second : 197 Realtime factor : 0.20 x Time per step : 5080.3 µs CG iters / step : 3.16 Contacts / step : 31.04 Constraints / step : 124.15 Degrees of freedom : 178 Dynamic memory usage : 0.4% of 100M ``` After: ``` Simulation time : 9.52 s Steps per second : 1051 Realtime factor : 1.05 x Time per step : 951.7 µs CG iters / step : 3.21 Contacts / step : 30.90 Constraints / step : 123.61 Degrees of freedom : 178 Dynamic memory usage : 0.3% of 100M ``` PiperOrigin-RevId: 913758038 Change-Id: If5aa617b2d535c86aec9bd71c9e0003a2b38bdd7 --- doc/includes/references.h | 1 - include/mujoco/mjmodel.h | 1 - include/mujoco/mjxmacro.h | 1 - model/flex/bunny_multicell.xml | 4 +- python/mujoco/introspect/structs.py | 8 - src/engine/engine_derivative.c | 12 +- src/engine/engine_derivative.h | 8 +- src/engine/engine_forward.c | 347 ++++++++------------------ src/engine/engine_setconst.c | 131 +--------- test/engine/engine_derivative_test.cc | 56 +++-- unity/Runtime/Bindings/MjBindings.cs | 1 - wasm/codegen/generated/bindings.cc | 4 - 12 files changed, 152 insertions(+), 422 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index d106b4b6..c226304b 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1316,7 +1316,6 @@ struct mjModel_ { int* flex_matid; // material id for rendering (nflex x 1) int* flex_group; // group for visibility (nflex x 1) int* flex_interp; // interpolation (0: vertex, 1: nodes) (nflex x 1) - int* flex_bandwidth; // precomputed solver bandwidth (nflex x 1) int* flex_cellnum; // finite cell num per dimension (nflex x 3) int* flex_nodeadr; // first node address (nflex x 1) int* flex_nodenum; // number of nodes (nflex x 1) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index ec2c8250..565358d0 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -979,7 +979,6 @@ struct mjModel_ { int* flex_matid; // material id for rendering (nflex x 1) int* flex_group; // group for visibility (nflex x 1) int* flex_interp; // interpolation (0: vertex, 1: nodes) (nflex x 1) - int* flex_bandwidth; // precomputed solver bandwidth (nflex x 1) int* flex_cellnum; // finite cell num per dimension (nflex x 3) int* flex_nodeadr; // first node address (nflex x 1) int* flex_nodenum; // number of nodes (nflex x 1) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 080cbc07..c11ff844 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -455,7 +455,6 @@ X ( int, flex_matid, nflex, 1 ) \ X ( int, flex_group, nflex, 1 ) \ X ( int, flex_interp, nflex, 1 ) \ - X ( int, flex_bandwidth, nflex, 1 ) \ X ( int, flex_cellnum, nflex, 3 ) \ X ( int, flex_nodeadr, nflex, 1 ) \ X ( int, flex_nodenum, nflex, 1 ) \ diff --git a/model/flex/bunny_multicell.xml b/model/flex/bunny_multicell.xml index 730cbc5d..5f4d9f1d 100644 --- a/model/flex/bunny_multicell.xml +++ b/model/flex/bunny_multicell.xml @@ -16,9 +16,9 @@ - + - + diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 84ff1341..5ed73b61 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -2673,14 +2673,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='interpolation (0: vertex, 1: nodes)', array_extent=('nflex',), ), - StructFieldDecl( - name='flex_bandwidth', - type=PointerType( - inner_type=ValueType(name='int'), - ), - doc='precomputed solver bandwidth', - array_extent=('nflex',), - ), StructFieldDecl( name='flex_cellnum', type=PointerType( diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index ef6bc8b7..85f91e33 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -1134,18 +1134,18 @@ void mjd_flexInterp_mulKD(const mjModel* m, mjData* d, mjtNum* res, const mjtNum } -// add (h^2 + h*damping) * J'*K*J to banded matrix H, for all interpolated flexes -// H: banded ndof x nband matrix (lower triangle, band storage) -// dof_indices: maps local indices to global DOFs -void mjd_flexInterp_addH(const mjModel* m, mjData* d, mjtNum* H, const int* dof_indices, - int ndof, int nband, mjtNum h) { - mjd_flexInterp_kernel(m, d, mjFLEXOP_ADDH, H, NULL, h * h, h, dof_indices, ndof, nband); +// compute res += h * J'*K*J * vec, for all interpolated flexes (stiffness only, no damping) +void mjd_flexInterp_mulK(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h) { + // s1=h, s2=0 => scale = h (no damping contribution) + mjd_flexInterp_kernel(m, d, mjFLEXOP_VEC, res, vec, h, 0, NULL, 0, 0); } + + // add (d qfrc_actuator / d qvel) to qDeriv void mjd_actuator_vel(const mjModel* m, mjData* d) { int nu = m->nu; diff --git a/src/engine/engine_derivative.h b/src/engine/engine_derivative.h index 65a136f5..1ddb3af9 100644 --- a/src/engine/engine_derivative.h +++ b/src/engine/engine_derivative.h @@ -47,9 +47,11 @@ MJAPI void mjd_rne_vel_dense(const mjModel* m, mjData* d); // res and vec are vectors of size m->nv MJAPI void mjd_flexInterp_mulKD(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h); -// assemble flex stiffness matrix H_flex: H += h*h*K + h*D -// H is a dense matrix of size ndof x ndof, dof_indices maps local rows/cols to global DOFs -MJAPI void mjd_flexInterp_addH(const mjModel* m, mjData* d, mjtNum* H, const int* dof_indices, int ndof, int nband, mjtNum h); +// derivative of flex_interp generalized force w.r.t position (stiffness only, no damping) +MJAPI void mjd_flexInterp_mulK(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h); + + + #ifdef __cplusplus diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index ef15c37a..5e285f39 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -1371,236 +1371,121 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { } -// return 1 if flex f needs implicit interp treatment -static int flexInterp_active(const mjModel* m, int f) { - return m->flex_interp[f] && !m->flex_rigid[f] && - m->flex_edgeequality[f] != 3 && - m->flex_stiffness[m->flex_stiffnessadr[f]] != 0; +// return 1 if any flex needs implicit interp treatment +static int flexInterp_has_active(const mjModel* m) { + for (int f=0; f < m->nflex; f++) { + if (m->flex_interp[f] && !m->flex_rigid[f] && + m->flex_edgeequality[f] != 3 && + m->flex_stiffness[m->flex_stiffnessadr[f]] != 0) { + return 1; + } + } + return 0; } -// context for flex interp reduced banded factorization/solve -typedef struct { - mjtNum* H; // banded Cholesky-factored matrix (ndof x nband) - int* dof_indices; // global DOF index for each local flex DOF - int ndof; // number of flex DOFs - int nband; // half-bandwidth + 1 (number of band columns) - int ncoupling; // number of off-diagonal coupling terms - mjtNum* coupling_val; // coupling coefficient values - int* coupling_row; // local flex row index for each coupling term - int* coupling_col; // global DOF column index for each coupling term -} FlexInterpContext; - - -// collect flex DOFs for one flex, marking seen_dof and incrementing count -static void flexInterp_collect(const mjModel* m, int f, - int* chain_dofs, int* seen_dof, int* count) { - int nodenum = m->flex_nodenum[f]; - int nodeadr = m->flex_nodeadr[f]; - for (int n=0; n < nodenum; n++) { - int b = m->flex_nodebodyid[nodeadr+n]; - int chain_nnz; - if (m->body_dofnum[b] == 0) { - // pinned node: use bodyChain to get parent DOFs - chain_nnz = mj_bodyChain(m, b, chain_dofs); - } else { - // regular flex node: use body's own DOFs only - chain_nnz = m->body_dofnum[b]; - for (int j=0; j < chain_nnz; j++) { - chain_dofs[j] = m->body_dofadr[b] + j; - } - } - for (int i=0; i < chain_nnz; i++) { - int dof = chain_dofs[i]; - if (!seen_dof[dof]) { - seen_dof[dof] = 1; - (*count)++; - } - } - } -} - - -// build and factor the reduced banded matrix for flex interp DOFs -// mark/free stack handled by caller -static FlexInterpContext flexInterp_factor(const mjModel* m, mjData* d, int nv) { - FlexInterpContext ctx = {0}; - - int* chain_dofs = mjSTACKALLOC(d, nv, int); - int* seen_dof = mjSTACKALLOC(d, nv, int); - mju_fillInt(seen_dof, 0, nv); - - // count flex DOFs - int ndof = 0; - for (int f=0; f < m->nflex; f++) { - if (flexInterp_active(m, f)) { - flexInterp_collect(m, f, chain_dofs, seen_dof, &ndof); - } - } - if (ndof == 0) { - return ctx; - } - - // allocate and build global-to-local mapping - int* dof_indices = mjSTACKALLOC(d, ndof, int); - int* global2local = mjSTACKALLOC(d, nv, int); - mju_fillInt(global2local, -1, nv); - - // collect unique DOFs in order - int cnt = 0; - mju_fillInt(seen_dof, 0, nv); - for (int f=0; f < m->nflex; f++) { - if (flexInterp_active(m, f)) { - int nodenum = m->flex_nodenum[f]; - int nodeadr = m->flex_nodeadr[f]; - for (int n=0; n < nodenum; n++) { - int b = m->flex_nodebodyid[nodeadr+n]; - int chain_nnz; - if (m->body_dofnum[b] == 0) { - // pinned node: use bodyChain to get parent DOFs - chain_nnz = mj_bodyChain(m, b, chain_dofs); - } else { - // regular flex node: use body's own DOFs only - chain_nnz = m->body_dofnum[b]; - for (int j=0; j < chain_nnz; j++) { - chain_dofs[j] = m->body_dofadr[b] + j; - } - } - for (int i=0; i < chain_nnz; i++) { - int dof = chain_dofs[i]; - if (!seen_dof[dof]) { - seen_dof[dof] = 1; - dof_indices[cnt] = dof; - global2local[dof] = cnt; - cnt++; - } - } - } - } - } - - // select sparse matrix format based on integrator - int implicit = (m->opt.integrator == mjINT_IMPLICIT); - const int* rownnz = implicit ? m->D_rownnz : m->M_rownnz; - const int* rowadr = implicit ? m->D_rowadr : m->M_rowadr; - const int* colind = implicit ? m->D_colind : m->M_colind; - const mjtNum* source = implicit ? d->qLU : d->qH; - - // get precomputed bandwidth - int bandwidth = 0; - for (int f=0; f < m->nflex; f++) { - if (flexInterp_active(m, f)) { - if (m->flex_bandwidth[f] > bandwidth) { - bandwidth = m->flex_bandwidth[f]; - } - } - } - - // compute ncoupling from sparse matrix entries - int ncoupling = 0; - for (int i=0; i < ndof; i++) { - int row = dof_indices[i]; - int start = rowadr[row]; - int end = start + rownnz[row]; - for (int k=start; k < end; k++) { - int local_j = global2local[colind[k]]; - if (local_j < 0) { - ncoupling++; - } - } - } - - // nband = bandwidth + 1 (includes diagonal) - int nband = bandwidth + 1; - - // cap nband at ndof (dense fallback for small systems) - if (nband > ndof) nband = ndof; - - // allocate coupling storage - mjtNum* coupling_val = NULL; - int* coupling_row = NULL; - int* coupling_col = NULL; - if (ncoupling > 0) { - coupling_val = mjSTACKALLOC(d, ncoupling, mjtNum); - coupling_row = mjSTACKALLOC(d, ncoupling, int); - coupling_col = mjSTACKALLOC(d, ncoupling, int); - } - - // build H_flex (banded) from qLU (implicit) or qH (implicitfast) - mjtNum* H = mjSTACKALLOC(d, ndof*nband, mjtNum); - mju_zero(H, ndof*nband); - - int coup_cnt = 0; - for (int i=0; i < ndof; i++) { - int row = dof_indices[i]; - int start = rowadr[row]; - int end = start + rownnz[row]; - for (int k=start; k < end; k++) { - int col = colind[k]; - int local_j = global2local[col]; - if (local_j >= 0) { - // store lower triangle only: row i, col local_j, where i >= local_j - if (i >= local_j) { - H[i*nband + nband-1-(i-local_j)] = source[k]; - } else { - // upper triangle entry: store symmetrically in lower triangle - H[local_j*nband + nband-1-(local_j-i)] = source[k]; - } - } else if (coup_cnt < ncoupling) { - coupling_val[coup_cnt] = source[k]; - coupling_row[coup_cnt] = i; - coupling_col[coup_cnt] = col; - coup_cnt++; - } - } - } - - // add flex stiffness in banded format and factorize - mjd_flexInterp_addH(m, d, H, dof_indices, ndof, nband, m->opt.timestep); - mju_cholFactorBand(H, ndof, nband, 0, 0, 0); - - // store results in context - ctx.H = H; - ctx.dof_indices = dof_indices; - ctx.ndof = ndof; - ctx.nband = nband; - ctx.ncoupling = ncoupling; - ctx.coupling_val = coupling_val; - ctx.coupling_row = coupling_row; - ctx.coupling_col = coupling_col; - return ctx; -} - - -// solve the reduced banded system for flex interp DOFs, overwrite qacc -static void flexInterp_solve(const mjModel* m, mjData* d, const FlexInterpContext* ctx, - mjtNum* qacc, const mjtNum* qfrc, int nv) { - int ndof = ctx->ndof; - mjtNum* qfrc_flex = mjSTACKALLOC(d, ndof, mjtNum); - mjtNum* res = mjSTACKALLOC(d, nv, mjtNum); - +// preconditioned CG solve for implicit flex interp +// solves (M - h*qDeriv - (h^2+h*d)*K) * qacc = qfrc - h*K*qvel +// where K is the flex stiffness, using the already-factored standard system +// (M - h*qDeriv) as a preconditioner +static void flexInterp_cgsolve(const mjModel* m, mjData* d, + mjtNum* qacc, const mjtNum* qfrc, int nv) { mjtNum h = m->opt.timestep; - mjtNum damp = (m->nflex > 0 && m->flex_damping) ? m->flex_damping[0] : 0; - mjtNum scl = h*h + h*damp; - mjtNum factor = (scl > mjMINVAL) ? (h/scl) : 0; + int implicit = (m->opt.integrator == mjINT_IMPLICIT); - // velocity correction: -h * K * v - mju_zero(res, nv); - mjd_flexInterp_mulKD(m, d, res, d->qvel, h); + mj_markStack(d); - for (int i=0; i < ndof; i++) { - int global_dof = ctx->dof_indices[i]; - qfrc_flex[i] = qfrc[global_dof] + res[global_dof] * factor; + // allocate CG work vectors + mjtNum* rhs = mjSTACKALLOC(d, nv, mjtNum); + mjtNum* r = mjSTACKALLOC(d, nv, mjtNum); + mjtNum* z = mjSTACKALLOC(d, nv, mjtNum); + mjtNum* p = mjSTACKALLOC(d, nv, mjtNum); + mjtNum* Ap = mjSTACKALLOC(d, nv, mjtNum); + mjtNum* temp = mjSTACKALLOC(d, nv, mjtNum); + + // build RHS: rhs = qfrc - h*K*qvel (velocity correction from flex stiffness) + mju_copy(rhs, qfrc, nv); + mju_zero(temp, nv); + mjd_flexInterp_mulK(m, d, temp, d->qvel, h); // temp = h*K*v (stiffness only) + mju_addToScl(rhs, temp, -1.0, nv); // rhs -= h*K*v + + // --- helper lambda-style inline: compute Ap = A*x --- + // A*x = (M - h*qDeriv)*x - (h^2+h*d)*K*x + #define FLEX_CG_MATVEC(Ap_out, x_in) \ + mju_mulMatVecSparse(Ap_out, d->qDeriv, x_in, nv, m->D_rownnz, m->D_rowadr, \ + m->D_colind, NULL); \ + mju_zero(temp, nv); \ + mju_mulSymVecSparse(temp, d->M, x_in, nv, m->M_rownnz, m->M_rowadr, \ + m->M_colind); \ + mju_addScl(Ap_out, temp, Ap_out, -h, nv); \ + mju_zero(temp, nv); \ + mjd_flexInterp_mulKD(m, d, temp, x_in, h); \ + mju_addToScl(Ap_out, temp, -1.0, nv) + + // --- helper: preconditioner solve z = (M - h*qDeriv)^{-1} * r --- + #define FLEX_CG_PRECOND(z_out, r_in) \ + if (implicit) { \ + mju_solveLUSparse(z_out, d->qLU, r_in, nv, m->D_rownnz, m->D_rowadr, \ + m->D_diag, m->D_colind, NULL); \ + } else { \ + mju_copy(z_out, r_in, nv); \ + mj_solveLD(z_out, d->qH, d->qHDiagInv, nv, 1, m->M_rownnz, m->M_rowadr, \ + m->M_colind, NULL); \ + } + + // initial residual: r = rhs - A*qacc + FLEX_CG_MATVEC(Ap, qacc); + mju_sub(r, rhs, Ap, nv); + + // check if already converged + mjtNum rnorm = mju_dot(r, r, nv); + mjtNum tol = 1e-10 * mju_dot(rhs, rhs, nv); + if (rnorm < tol || rnorm < mjMINVAL) { + mj_freeStack(d); + return; } - // coupling correction: qfrc_flex -= H_coupling * qacc_parent - for (int k=0; k < ctx->ncoupling; k++) { - qfrc_flex[ctx->coupling_row[k]] -= ctx->coupling_val[k] * qacc[ctx->coupling_col[k]]; + // z = precond(r), p = z + FLEX_CG_PRECOND(z, r); + mju_copy(p, z, nv); + mjtNum rz = mju_dot(r, z, nv); + + // CG iterations + int maxiter = 50; + for (int iter=0; iter < maxiter; iter++) { + FLEX_CG_MATVEC(Ap, p); + + // alpha = rz / dot(p, Ap) + mjtNum pAp = mju_dot(p, Ap, nv); + if (mju_abs(pAp) < mjMINVAL) break; + mjtNum alpha = rz / pAp; + + // qacc += alpha * p + mju_addToScl(qacc, p, alpha, nv); + + // r -= alpha * Ap + mju_addToScl(r, Ap, -alpha, nv); + + // check convergence + rnorm = mju_dot(r, r, nv); + if (rnorm < tol || rnorm < mjMINVAL) break; + + // z = precond(r) + FLEX_CG_PRECOND(z, r); + + // beta = rz_new / rz + mjtNum rz_new = mju_dot(r, z, nv); + mjtNum beta = rz_new / mju_max(mjMINVAL, rz); + + // p = z + beta * p + mju_addScl(p, z, p, beta, nv); + rz = rz_new; } - // solve with banded Cholesky and scatter back - mju_cholSolveBand(qfrc_flex, ctx->H, qfrc_flex, ndof, ctx->nband, 0); - mju_scatter(qacc, qfrc_flex, ctx->dof_indices, ndof); + #undef FLEX_CG_MATVEC + #undef FLEX_CG_PRECOND + + mj_freeStack(d); } @@ -1972,16 +1857,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { } // check for flex_interp that needs implicit treatment - int has_flex_interp = 0; - for (int f=0; f < m->nflex; f++) { - if (flexInterp_active(m, f)) { - has_flex_interp = 1; - break; - } - } - - // flex interp context (populated during factorization) - FlexInterpContext flex = {0}; + int has_flex_interp = !sleep_filter && flexInterp_has_active(m); // factorization if (!skipfactor) { @@ -2011,11 +1887,6 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { mjERROR("integrator must be implicit or implicitfast"); } - // flex: reduced dense factorization - if (has_flex_interp && !sleep_filter) { - flex = flexInterp_factor(m, d, nv); - } - // standard factorization (implicit / implicitfast) if (m->opt.integrator == mjINT_IMPLICIT) { int* scratch = mjSTACKALLOC(d, nv, int); @@ -2039,9 +1910,9 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1, m->M_rownnz, m->M_rowadr, m->M_colind, dof_awake_ind); } - // flex: reduced dense solve - if (flex.H) { - flexInterp_solve(m, d, &flex, qacc, qfrc, nv); + // flex: CG correction for implicit flex stiffness + if (has_flex_interp) { + flexInterp_cgsolve(m, d, qacc, qfrc, m->nv); } // count and list joints of free bodies eligible for midpoint integration diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 81a0f155..d47c570e 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -638,135 +638,6 @@ static void makeFlexSparse(mjModel* m, mjData* d) { mj_freeStack(d); } -// compute flex bandwidth for trilinear interpolation -static void makeFlexBandwidth(mjModel* m, mjData* d) { - if (!m->nflex) { - return; - } - - mj_markStack(d); - int* chain_dofs = mjSTACKALLOC(d, m->nv, int); - int* seen_dof = mjSTACKALLOC(d, m->nv, int); - int* dof_indices = mjSTACKALLOC(d, m->nv, int); - int* global2local = mjSTACKALLOC(d, m->nv, int); - - mju_zeroInt(seen_dof, m->nv); - for (int i = 0; i < m->nv; i++) { - global2local[i] = -1; - } - - int ndof = 0; - for (int f = 0; f < m->nflex; f++) { - if (m->flex_interp[f]) { - int nodenum = m->flex_nodenum[f]; - int nodeadr = m->flex_nodeadr[f]; - for (int n = 0; n < nodenum; n++) { - int b = m->flex_nodebodyid[nodeadr + n]; - // only the body's own DOFs enter the reduced banded flex system; - // ancestor DOFs are solved by the global factorization and coupled - // via off-diagonal correction (see flexInterp_solve in engine_forward) - int chain_nnz; - if (m->body_dofnum[b] == 0) { - chain_nnz = mj_bodyChain(m, b, chain_dofs); - } else { - chain_nnz = m->body_dofnum[b]; - for (int j = 0; j < chain_nnz; j++) { - chain_dofs[j] = m->body_dofadr[b] + j; - } - } - for (int i = 0; i < chain_nnz; i++) { - int dof = chain_dofs[i]; - if (!seen_dof[dof]) { - seen_dof[dof] = 1; - dof_indices[ndof] = dof; - global2local[dof] = ndof++; - } - } - } - } - } - - int bandwidth = 0; - if (ndof > 0) { - // check sparse matrix coupling (both D and M) - for (int integrator = 0; integrator < 2; integrator++) { - const int* rownnz = (integrator == 0) ? m->D_rownnz : m->M_rownnz; - const int* rowadr = (integrator == 0) ? m->D_rowadr : m->M_rowadr; - const int* colind = (integrator == 0) ? m->D_colind : m->M_colind; - - // D arrays are only allocated for implicit integrators - if (!rownnz) continue; - - for (int i = 0; i < ndof; i++) { - int row = dof_indices[i]; - int start = rowadr[row]; - int end = start + rownnz[row]; - for (int k = start; k < end; k++) { - int local_j = global2local[colind[k]]; - if (local_j >= 0) { - int diff = i - local_j; - if (diff < 0) diff = -diff; - if (diff > bandwidth) bandwidth = diff; - } - } - } - } - - // check stiffness coupling - for (int f = 0; f < m->nflex; f++) { - if (!m->flex_interp[f]) continue; - int order = m->flex_interp[f]; - order = order < 0 ? -order : order; - int nodeadr = m->flex_nodeadr[f]; - int nodenum = m->flex_nodenum[f]; - int cx = m->flex_cellnum[3*f+0]; - int cy = m->flex_cellnum[3*f+1]; - int cz = m->flex_cellnum[3*f+2]; - int ny = cy * order + 1; - int nz = cz * order + 1; - - for (int icx = 0; icx < cx; icx++) { - for (int icy = 0; icy < cy; icy++) { - for (int icz = 0; icz < cz; icz++) { - int min_local = ndof, max_local = -1; - for (int lx = 0; lx <= order; lx++) { - for (int ly = 0; ly <= order; ly++) { - for (int lz = 0; lz <= order; lz++) { - int gx = icx * order + lx; - int gy = icy * order + ly; - int gz = icz * order + lz; - int node_idx = gx * ny * nz + gy * nz + gz; // non-negative by construction - if (node_idx < nodenum) { - int b = m->flex_nodebodyid[nodeadr + node_idx]; - int chain_nnz = mj_bodyChain(m, b, chain_dofs); - for (int i = 0; i < chain_nnz; i++) { - int dof = chain_dofs[i]; - int local = global2local[dof]; - if (local >= 0) { - if (local < min_local) min_local = local; - if (local > max_local) max_local = local; - } - } - } - } - } - } - if (max_local >= 0 && max_local - min_local > bandwidth) { - bandwidth = max_local - min_local; - } - } - } - } - } - } - - // store bandwidth for all flexes (global max) - for (int f = 0; f < m->nflex; f++) { - m->flex_bandwidth[f] = bandwidth; - } - - mj_freeStack(d); -} // align 2D flexes to the XY plane static void mj_alignFlex(mjModel* m, mjData* d) { @@ -819,7 +690,7 @@ static void mj_alignFlex(mjModel* m, mjData* d) { static void set0(mjModel* m, mjData* d) { makeTendonSparse(m); makeFlexSparse(m, d); - makeFlexBandwidth(m, d); + mj_alignFlex(m, d); int nv = m->nv; mjtNum A[36] = {0}, pos[3], quat[4]; diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 0e5c1849..fb9d777f 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -1473,15 +1473,24 @@ void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name, } } -// Helper: assemble flex stiffness into dense matrix via banded addH -// This wraps the banded API and converts to dense for test verification. -static void addH_dense(mjModel* m, mjData* d, mjtNum* H_dense, - const int* dof_indices, int ndof, mjtNum h) { - // use full bandwidth (ndof) for exact dense equivalence - std::vector H_band(ndof * ndof, 0); - mjd_flexInterp_addH(m, d, H_band.data(), dof_indices, ndof, ndof, h); - // convert banded to dense (lower triangle), then symmetrize - mju_band2Dense(H_dense, H_band.data(), ndof, ndof, 0, 1); +// Helper: assemble flex stiffness into dense matrix via matrix-vector products. +// Builds K column-by-column using mjd_flexInterp_mulKD. +// Result is -(h^2 + h*damping) * J'KJ (negative sign matches the old addH +// convention where stiffness is subtracted from the system matrix). +static void mulKD_dense(mjModel* m, mjData* d, mjtNum* H_dense, + int nv, mjtNum h) { + std::vector e_i(nv, 0); + std::vector col(nv, 0); + for (int i = 0; i < nv; i++) { + mju_zero(e_i.data(), nv); + mju_zero(col.data(), nv); + e_i[i] = 1.0; + mjd_flexInterp_mulKD(m, d, col.data(), e_i.data(), h); + // col = +(h^2 + h*damp)*K*e_i, negate to match addH convention (H -= K) + for (int j = 0; j < nv; j++) { + H_dense[j * nv + i] = -col[j]; + } + } } // compare analytic and fin-diff d_qfrc_passive/d_qvel for flex interp @@ -1525,18 +1534,16 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) { vec[i] = mju_Halton(i, 2) - 0.5; } - // use addH to compute K * vec - // addH adds (h^2*K + h*D) to H - // if we set h=1, damping=0, we get K added to H + // use mulKD to compute K * vec + // mulKD adds (h^2*K + h*D)*vec to res + // if we set h=1, damping=0, we get K*vec mjtNum save_damping = model->flex_damping[0]; model->flex_damping[0] = 0; std::vector H(nv * nv, 0); - std::vector dof_indices(nv); - for (int i = 0; i < nv; i++) dof_indices[i] = i; - // assemble K into H - addH_dense(model, data, H.data(), dof_indices.data(), nv, 1.0); + // assemble K into H column-by-column + mulKD_dense(model, data, H.data(), nv, 1.0); // restore damping model->flex_damping[0] = save_damping; @@ -1623,16 +1630,13 @@ TEST_F(DerivativeTest, FlexInterpDerivatives) { // check that we have non-zero damping (FD should find it) EXPECT_GT(mju_norm(qDerivFD.data(), nD), 1e-3); - // compute expected flex damping using mjd_flexInterp_addH + // compute expected flex damping using mulKD_dense // D = 4*H(0.5) - H(1) - vector dof_indices(nv); - for (int i = 0; i < nv; i++) dof_indices[i] = i; - vector H1(nv * nv, 0); - addH_dense(model, data, H1.data(), dof_indices.data(), nv, 1.0); + mulKD_dense(model, data, H1.data(), nv, 1.0); vector H2(nv * nv, 0); - addH_dense(model, data, H2.data(), dof_indices.data(), nv, 0.5); + mulKD_dense(model, data, H2.data(), nv, 0.5); vector D(nv * nv); for (int i = 0; i < nv * nv; i++) { @@ -1700,13 +1704,11 @@ TEST_F(DerivativeTest, FlexInterpDerivativesDeformed) { mj_forward(model, data); // 1. Compute Analytic Jacobian (Approximate) - // We use mjd_flexInterp_addH to get K_approx + // We use mulKD_dense to get K_approx std::vector H_approx(nv * nv, 0); - std::vector dof_indices(nv); - for (int i = 0; i < nv; i++) dof_indices[i] = i; - // h=1, damping=0 => adds K to H - addH_dense(model, data, H_approx.data(), dof_indices.data(), nv, 1.0); + // h=1, damping=0 => gives K + mulKD_dense(model, data, H_approx.data(), nv, 1.0); // 2. Compute Finite Difference Jacobian (Ground Truth) // qfrc_passive = -dV/dq diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index a655529c..d546c345 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -1201,7 +1201,6 @@ public unsafe struct mjModel_ { public int* flex_matid; public int* flex_group; public int* flex_interp; - public int* flex_bandwidth; public int* flex_cellnum; public int* flex_nodeadr; public int* flex_nodenum; diff --git a/wasm/codegen/generated/bindings.cc b/wasm/codegen/generated/bindings.cc index f0c11146..e78ece4f 100644 --- a/wasm/codegen/generated/bindings.cc +++ b/wasm/codegen/generated/bindings.cc @@ -4655,9 +4655,6 @@ struct MjModel { emscripten::val flex_interp() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nflex, ptr_->flex_interp)); } - emscripten::val flex_bandwidth() const { - return emscripten::val(emscripten::typed_memory_view(ptr_->nflex, ptr_->flex_bandwidth)); - } emscripten::val flex_cellnum() const { return emscripten::val(emscripten::typed_memory_view(ptr_->nflex * 3, ptr_->flex_cellnum)); } @@ -11854,7 +11851,6 @@ EMSCRIPTEN_BINDINGS(mujoco_bindings) { .property("eq_type", &MjModel::eq_type) .property("exclude_signature", &MjModel::exclude_signature) .property("flex_activelayers", &MjModel::flex_activelayers) - .property("flex_bandwidth", &MjModel::flex_bandwidth) .property("flex_bending", &MjModel::flex_bending) .property("flex_bendingadr", &MjModel::flex_bendingadr) .property("flex_bvhadr", &MjModel::flex_bvhadr) From 5339d9154e788ee32dd41e15e911768734e96adb Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 12 May 2026 01:26:50 -0700 Subject: [PATCH 243/251] Ensure faces for meshes are correctly oriented. This is in preparation for a future CL that will disable backface culling. PiperOrigin-RevId: 914136370 Change-Id: Ie8db1268faaa306c3f3356efc5e1305d791cf525 --- .../filament/compat/model_objects.cc | 12 ++++++------ src/experimental/filament/filament/builtins.cc | 16 ++++++++++------ 2 files changed, 16 insertions(+), 12 deletions(-) diff --git a/src/experimental/filament/compat/model_objects.cc b/src/experimental/filament/compat/model_objects.cc index a619e73d..dfd7d644 100644 --- a/src/experimental/filament/compat/model_objects.cc +++ b/src/experimental/filament/compat/model_objects.cc @@ -254,17 +254,17 @@ static void FillHeightFieldBuffer(MeshBuilder& builder, const mjModel* model, // Build the front edge. for (int col = 0; col < ncol - 1; ++col) { const float3 a = get_pos(0, col); - const float3 b = get_pos(0, col + 1); - const float3 c = {b.x, b.y, -sz[3]}; - const float3 d = {a.x, a.y, -sz[3]}; + const float3 b = {a.x, a.y, -sz[3]}; + const float3 d = get_pos(0, col + 1); + const float3 c = {d.x, d.y, -sz[3]}; append_quad(a, b, c, d); } // Build the back edge. for (int col = 0; col < ncol - 1; ++col) { const float3 a = get_pos(nrow - 1, col + 1); - const float3 b = get_pos(nrow - 1, col); - const float3 c = {b.x, b.y, -sz[3]}; - const float3 d = {a.x, a.y, -sz[3]}; + const float3 b = {a.x, a.y, -sz[3]}; + const float3 d = get_pos(nrow - 1, col); + const float3 c = {d.x, d.y, -sz[3]}; append_quad(a, b, c, d); } // Build the base. We use the visualization quality as the size rather than diff --git a/src/experimental/filament/filament/builtins.cc b/src/experimental/filament/filament/builtins.cc index b18e28e5..98a14296 100644 --- a/src/experimental/filament/filament/builtins.cc +++ b/src/experimental/filament/filament/builtins.cc @@ -153,7 +153,7 @@ class PlaneBuilder : public BuiltinBuilder { const int i1 = base_idx + 1; const int i2 = base_idx + num_quads_per_axis + 2; const int i3 = base_idx + num_quads_per_axis + 1; - AppendQuadIndices(indices_, i0, i1, i2, i3); + AppendQuadIndices(indices_, i0, i3, i2, i1); } } @@ -268,7 +268,11 @@ class BoxBuilder : public BuiltinBuilder { const int i1 = base_idx + 1; const int i2 = base_idx + num_quads_per_axis_ + 2; const int i3 = base_idx + num_quads_per_axis_ + 1; - AppendQuadIndices(indices_, i0, i1, i2, i3); + if (i == 2 || i == 1 || i == 4) { + AppendQuadIndices(indices_, i0, i3, i2, i1); + } else { + AppendQuadIndices(indices_, i0, i1, i2, i3); + } } } } @@ -326,7 +330,7 @@ class TubeBuilder : public BuiltinBuilder { const int i1 = base_idx + 1; const int i2 = (base_idx + num_stacks + 2) % num_vertices; const int i3 = (base_idx + num_stacks + 1) % num_vertices; - AppendQuadIndices(indices_, i0, i1, i2, i3); + AppendQuadIndices(indices_, i0, i3, i2, i1); } } @@ -494,8 +498,8 @@ class SphereBuilder : public BuiltinBuilder { for (int lon = 0; lon < num_slices; ++lon) { const int next = lon < (num_slices - 1) ? lon + 1 : 0; indices_.push_back(kNorthPoleIndex); - indices_.push_back(row_start + next); indices_.push_back(row_start + lon); + indices_.push_back(row_start + next); } // Latitudinal triangle strips. @@ -519,8 +523,8 @@ class SphereBuilder : public BuiltinBuilder { for (int lon = 0; lon < num_slices; ++lon) { const int adjacent = lon < (num_slices - 1) ? lon + 1 : 0; indices_.push_back(kSouthPoleIndex); - indices_.push_back(row_start + lon); indices_.push_back(row_start + adjacent); + indices_.push_back(row_start + lon); } SetBounds({-1, -1, -1}, {1, 1, 1}); @@ -585,8 +589,8 @@ class DomeBuilder : public BuiltinBuilder { for (int lon = 0; lon < num_slices; ++lon) { const int next = lon < (num_slices - 1) ? lon + 1 : 0; indices_.push_back(kPoleIndex); - indices_.push_back(row_start + next); indices_.push_back(row_start + lon); + indices_.push_back(row_start + next); } // Latitudinal quad strips. The first "stack" was handled above, so we From 3f3ff85a59b9ce68cfb9d9a5222bf7e050d966c1 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 12 May 2026 04:40:39 -0700 Subject: [PATCH 244/251] Enable backface culling by default. Disable only for geom types that need it (planes and triangles). PiperOrigin-RevId: 914211570 Change-Id: Id95e32924122789111f561d922110128b3f56c77 --- src/experimental/filament/assets/pbr.mat | 1 - src/experimental/filament/assets/pbr_packed.mat | 1 - src/experimental/filament/assets/phong_2d.mat | 1 - src/experimental/filament/assets/phong_2d_fade.mat | 1 - src/experimental/filament/assets/phong_2d_reflect.mat | 1 - src/experimental/filament/assets/phong_2d_uv.mat | 1 - src/experimental/filament/assets/phong_2d_uv_fade.mat | 1 - .../filament/assets/phong_2d_uv_reflect.mat | 1 - src/experimental/filament/assets/phong_color.mat | 1 - src/experimental/filament/assets/phong_color_fade.mat | 1 - .../filament/assets/phong_color_reflect.mat | 1 - src/experimental/filament/assets/phong_cube.mat | 1 - src/experimental/filament/assets/phong_cube_fade.mat | 1 - .../filament/assets/phong_cube_reflect.mat | 1 - src/experimental/filament/assets/unlit_decor.mat | 1 - src/experimental/filament/assets/unlit_depth.mat | 1 - .../filament/assets/unlit_segmentation.mat | 1 - src/experimental/filament/filament/renderable.cc | 10 ++++++++++ src/experimental/filament/filament/renderable.h | 1 + src/experimental/filament/filament/scene_view.cc | 1 + 20 files changed, 12 insertions(+), 17 deletions(-) diff --git a/src/experimental/filament/assets/pbr.mat b/src/experimental/filament/assets/pbr.mat index 19855986..2c1611f4 100644 --- a/src/experimental/filament/assets/pbr.mat +++ b/src/experimental/filament/assets/pbr.mat @@ -15,7 +15,6 @@ material { name : pbr, shadingModel : lit, - culling: none, flipUV: false, parameters : [ { type : sampler2d, name : BaseColor }, diff --git a/src/experimental/filament/assets/pbr_packed.mat b/src/experimental/filament/assets/pbr_packed.mat index 252af662..8626bf3e 100644 --- a/src/experimental/filament/assets/pbr_packed.mat +++ b/src/experimental/filament/assets/pbr_packed.mat @@ -15,7 +15,6 @@ material { name : pbr_packed, shadingModel : lit, - culling: none, flipUV: false, parameters : [ { type : sampler2d, name : BaseColor }, diff --git a/src/experimental/filament/assets/phong_2d.mat b/src/experimental/filament/assets/phong_2d.mat index df4788cc..e3aacba5 100644 --- a/src/experimental/filament/assets/phong_2d.mat +++ b/src/experimental/filament/assets/phong_2d.mat @@ -15,7 +15,6 @@ material { name : phong_2d, shadingModel : specularGlossiness, - culling : none, flipUV : false, parameters : [ { type : float4, name : BaseColorFactor }, diff --git a/src/experimental/filament/assets/phong_2d_fade.mat b/src/experimental/filament/assets/phong_2d_fade.mat index 15c18d73..2c934425 100644 --- a/src/experimental/filament/assets/phong_2d_fade.mat +++ b/src/experimental/filament/assets/phong_2d_fade.mat @@ -15,7 +15,6 @@ material { name : phong_2d_fade, shadingModel : specularGlossiness, - culling : none, flipUV : false, blending: fade, parameters : [ diff --git a/src/experimental/filament/assets/phong_2d_reflect.mat b/src/experimental/filament/assets/phong_2d_reflect.mat index 94dd2a0e..63a2c640 100644 --- a/src/experimental/filament/assets/phong_2d_reflect.mat +++ b/src/experimental/filament/assets/phong_2d_reflect.mat @@ -15,7 +15,6 @@ material { name : phong_2d_reflect, shadingModel : specularGlossiness, - culling : none, flipUV : false, parameters : [ { type : float4, name : BaseColorFactor }, diff --git a/src/experimental/filament/assets/phong_2d_uv.mat b/src/experimental/filament/assets/phong_2d_uv.mat index cad22805..7641564a 100644 --- a/src/experimental/filament/assets/phong_2d_uv.mat +++ b/src/experimental/filament/assets/phong_2d_uv.mat @@ -15,7 +15,6 @@ material { name : phong_2d_uv, shadingModel : specularGlossiness, - culling : none, flipUV : false, parameters : [ { type : float4, name : BaseColorFactor }, diff --git a/src/experimental/filament/assets/phong_2d_uv_fade.mat b/src/experimental/filament/assets/phong_2d_uv_fade.mat index 83464418..96430d08 100644 --- a/src/experimental/filament/assets/phong_2d_uv_fade.mat +++ b/src/experimental/filament/assets/phong_2d_uv_fade.mat @@ -15,7 +15,6 @@ material { name : phong_2d_uv_fade, shadingModel : specularGlossiness, - culling : none, flipUV : false, blending: fade, parameters : [ diff --git a/src/experimental/filament/assets/phong_2d_uv_reflect.mat b/src/experimental/filament/assets/phong_2d_uv_reflect.mat index 51ace511..8843be8f 100644 --- a/src/experimental/filament/assets/phong_2d_uv_reflect.mat +++ b/src/experimental/filament/assets/phong_2d_uv_reflect.mat @@ -15,7 +15,6 @@ material { name : phong_2d_uv_reflect, shadingModel : specularGlossiness, - culling : none, flipUV : false, parameters : [ { type : float4, name : BaseColorFactor }, diff --git a/src/experimental/filament/assets/phong_color.mat b/src/experimental/filament/assets/phong_color.mat index beaec101..778c6ab6 100644 --- a/src/experimental/filament/assets/phong_color.mat +++ b/src/experimental/filament/assets/phong_color.mat @@ -15,7 +15,6 @@ material { name : phong_color, shadingModel : specularGlossiness, - culling: none, parameters : [ { type : float4, name : BaseColorFactor }, { type : float, name : SpecularFactor }, diff --git a/src/experimental/filament/assets/phong_color_fade.mat b/src/experimental/filament/assets/phong_color_fade.mat index 2fd0721a..34fd8c42 100644 --- a/src/experimental/filament/assets/phong_color_fade.mat +++ b/src/experimental/filament/assets/phong_color_fade.mat @@ -15,7 +15,6 @@ material { name : phong_color_fade, shadingModel : specularGlossiness, - culling: none, blending: fade, parameters : [ { type : float4, name : BaseColorFactor }, diff --git a/src/experimental/filament/assets/phong_color_reflect.mat b/src/experimental/filament/assets/phong_color_reflect.mat index 80b7e09d..1bd3bee0 100644 --- a/src/experimental/filament/assets/phong_color_reflect.mat +++ b/src/experimental/filament/assets/phong_color_reflect.mat @@ -15,7 +15,6 @@ material { name : phong_color_reflect, shadingModel : specularGlossiness, - culling: none, parameters : [ { type : float4, name : BaseColorFactor }, { type : float, name : SpecularFactor }, diff --git a/src/experimental/filament/assets/phong_cube.mat b/src/experimental/filament/assets/phong_cube.mat index 0a39c3c6..5240ef1d 100644 --- a/src/experimental/filament/assets/phong_cube.mat +++ b/src/experimental/filament/assets/phong_cube.mat @@ -15,7 +15,6 @@ material { name : phong_cube, shadingModel : specularGlossiness, - culling : none, flipUV : false, parameters : [ { type : float4, name : BaseColorFactor }, diff --git a/src/experimental/filament/assets/phong_cube_fade.mat b/src/experimental/filament/assets/phong_cube_fade.mat index dec2b3af..4f42711d 100644 --- a/src/experimental/filament/assets/phong_cube_fade.mat +++ b/src/experimental/filament/assets/phong_cube_fade.mat @@ -15,7 +15,6 @@ material { name : phong_cube_fade, shadingModel : specularGlossiness, - culling : none, flipUV : false, blending: fade, parameters : [ diff --git a/src/experimental/filament/assets/phong_cube_reflect.mat b/src/experimental/filament/assets/phong_cube_reflect.mat index c4f02ac0..c575246c 100644 --- a/src/experimental/filament/assets/phong_cube_reflect.mat +++ b/src/experimental/filament/assets/phong_cube_reflect.mat @@ -15,7 +15,6 @@ material { name : phong_cube_reflect, shadingModel : specularGlossiness, - culling : none, flipUV : false, parameters : [ { type : float4, name : BaseColorFactor }, diff --git a/src/experimental/filament/assets/unlit_decor.mat b/src/experimental/filament/assets/unlit_decor.mat index 73a08e50..d9cd4aff 100644 --- a/src/experimental/filament/assets/unlit_decor.mat +++ b/src/experimental/filament/assets/unlit_decor.mat @@ -15,7 +15,6 @@ material { name : unlit_decor, shadingModel : unlit, - culling: none, parameters : [ { type : float4, name : BaseColorFactor } ] diff --git a/src/experimental/filament/assets/unlit_depth.mat b/src/experimental/filament/assets/unlit_depth.mat index 681319d8..d3f35604 100644 --- a/src/experimental/filament/assets/unlit_depth.mat +++ b/src/experimental/filament/assets/unlit_depth.mat @@ -16,7 +16,6 @@ material { name : unlit_depth, shadingModel : unlit, blending : opaque, - culling: none, depthWrite: true } diff --git a/src/experimental/filament/assets/unlit_segmentation.mat b/src/experimental/filament/assets/unlit_segmentation.mat index b57aca03..1fb6c1b5 100644 --- a/src/experimental/filament/assets/unlit_segmentation.mat +++ b/src/experimental/filament/assets/unlit_segmentation.mat @@ -15,7 +15,6 @@ material { name : unlit_segmentation, shadingModel : unlit, - culling: none, parameters : [ { type : float4, name : SegmentationColor } ] diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index 7597182a..ddd45503 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -77,6 +77,11 @@ void Renderable::SetMesh(const Mesh* mesh, int elem_offset, int elem_count) { if (mesh == nullptr) { mju_error("Cannot set mesh to nullptr."); } + + // We use MESH, even though it could be any mesh-like geom type, e.g. + // heightfields, flex, skin, sdf, etc. + geom_type_ = mjGEOM_MESH; + filament::VertexBuffer* vertex_buffer = mesh->GetFilamentVertexBuffer(); if (vertex_buffer == nullptr) { mju_error("Invalid (null) vertex buffer."); @@ -234,6 +239,10 @@ void Renderable::AssignMaterial(mjrDrawMode mode, } if (material) { instances_[index] = material->createInstance(); + if (geom_type_ == mjGEOM_PLANE || geom_type_ == mjGEOM_TRIANGLE) { + instances_[index]->setCullingMode( + filament::MaterialInstance::CullingMode::NONE); + } } } @@ -407,6 +416,7 @@ ObjectManager::MaterialType Renderable::GetColorMaterialType() const { void Renderable::SetGeomMesh(mjtGeom type, int nstack, int nslice, int nquad) { Builtins* builtins = object_mgr_->GetBuiltins(nstack, nslice, nquad); + geom_type_ = type; switch (type) { case mjGEOM_PLANE: diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index dfed9db0..3d7707da 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -130,6 +130,7 @@ class Renderable : public mjrRenderable { ObjectManager* object_mgr_; mjrRenderableParams params_; filament::MaterialInstance* instances_[mjNUM_DRAW_MODES] = {nullptr}; + mjtGeom geom_type_ = mjGEOM_NONE; mjrMaterial material_; mjrDrawMode draw_mode_ = mjDRAW_MODE_COLOR; filament::Scene* assigned_scene_ = nullptr; diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index 3622f0ab..d7ff12e6 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -141,6 +141,7 @@ SceneView::SceneView(FilamentContext* ctx, const mjrSceneParams& params) reflect_view_->setCamera(reflect_camera_); reflect_view_->setShadowingEnabled(false); reflect_view_->setPostProcessingEnabled(false); + reflect_view_->setFrontFaceWindingInverted(true); reflect_view_->setVisibleLayers(0xff, params.reflection_layer_mask); // Disable post processing for the depth and segmentation views to preserve From 58abf3d4ae2d42c788da198943aa0afe0c8948d4 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Tue, 12 May 2026 06:37:09 -0700 Subject: [PATCH 245/251] Fix dependency between FilamentContext and SceneView. Removes the DoRender and DoReadPixels functions which allowed FilamentContext to "call" SceneView functions even though SceneView depended on FilamentContext. Adds a mjrf_setClearColor function so that we no longer rely on SceneView setting the clear color. PiperOrigin-RevId: 914253362 Change-Id: I33c1cc9dfc7cb2f4c3cc855a600cccb0c54ae31c --- .../filament/compat/scene_bridge.cc | 4 ++ .../filament/filament/filament_context.cc | 18 ++--- src/experimental/filament/filament/light.cc | 5 +- src/experimental/filament/filament/light.h | 3 +- .../filament/filament/renderable.cc | 7 +- .../filament/filament/renderable.h | 4 +- .../filament/filament/scene_view.cc | 67 ++++++------------- .../filament/filament/scene_view.h | 22 ++---- .../filament/render_context_filament.cc | 16 +++-- .../filament/render_context_filament.h | 3 + 10 files changed, 60 insertions(+), 89 deletions(-) diff --git a/src/experimental/filament/compat/scene_bridge.cc b/src/experimental/filament/compat/scene_bridge.cc index fdc69936..ac3a9d37 100644 --- a/src/experimental/filament/compat/scene_bridge.cc +++ b/src/experimental/filament/compat/scene_bridge.cc @@ -88,6 +88,10 @@ SceneBridge::SceneBridge(mjrfContext* ctx, const mjModel* model) mjrf_configureSceneFromModel(scene_.get(), model); + auto clear_color = ReadElement(model, "filament.clearColor", + filament::math::float4(0, 0, 0, 1)); + mjrf_setClearColor(ctx_, &clear_color[0]); + default_shadow_map_size_ = ReadElement( model, "filament.shadows.map_size", default_shadow_map_size_); default_vsm_blur_width_ = ReadElement( diff --git a/src/experimental/filament/filament/filament_context.cc b/src/experimental/filament/filament/filament_context.cc index d7f81cb4..fdee15c2 100644 --- a/src/experimental/filament/filament/filament_context.cc +++ b/src/experimental/filament/filament/filament_context.cc @@ -36,16 +36,12 @@ #include #include "experimental/filament/filament/filament_platform_factory.h" #include "experimental/filament/filament/object_manager.h" +#include "experimental/filament/filament/render_target.h" +#include "experimental/filament/filament/scene_view.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { -// Forward declarations of functions defined in scene_view.cc to prevent -// circular dependencies. -void DoRender(filament::Renderer* renderer, const mjrRenderRequest& request); -void DoReadPixels(filament::Renderer* renderer, const mjrRenderRequest& request, - const mjrReadPixelsRequest& read_request); - FilamentContext::FilamentContext(const mjrFilamentConfig* config) : config_(*config) { FilamentPlatformSetup setup = CreateFilamentPlatform(config_); @@ -127,7 +123,8 @@ mjrFrameHandle FilamentContext::Render( break; } if (render_began) { - DoRender(renderer_, request); + SceneView* scene_view = SceneView::downcast(request.scene); + scene_view->Render(renderer_, request); } } else { if (read_requests.empty()) { @@ -147,8 +144,11 @@ mjrFrameHandle FilamentContext::Render( break; } if (render_began) { - DoRender(renderer_, request); - DoReadPixels(renderer_, request, read_request); + SceneView* scene_view = SceneView::downcast(request.scene); + scene_view->Render(renderer_, request); + RenderTarget* render_target = RenderTarget::downcast(request.target); + render_target->ReadColorPixels(renderer_, (uint8_t*)read_request.output, + read_request.num_bytes); } } } diff --git a/src/experimental/filament/filament/light.cc b/src/experimental/filament/filament/light.cc index 396aee42..9132c6fa 100644 --- a/src/experimental/filament/filament/light.cc +++ b/src/experimental/filament/filament/light.cc @@ -26,7 +26,6 @@ #include #include #include "experimental/filament/filament_util.h" -#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/texture.h" #include "experimental/filament/render_context_filament.h" @@ -35,8 +34,8 @@ namespace mujoco { using filament::math::float3; using filament::math::mat3f; -Light::Light(FilamentContext* ctx, const mjrLightParams& params) - : engine_(ctx->GetEngine()), params_(params) { +Light::Light(filament::Engine* engine, const mjrLightParams& params) + : engine_(engine), params_(params) { // Filament treats image-based lights (IBLs) as separate objects (i.e. // filament::IndirectLight) and so we need to handle IBLs specially. if (params.type == mjLIGHT_IMAGE) { diff --git a/src/experimental/filament/filament/light.h b/src/experimental/filament/filament/light.h index 106dcba8..73b092d2 100644 --- a/src/experimental/filament/filament/light.h +++ b/src/experimental/filament/filament/light.h @@ -20,7 +20,6 @@ #include #include #include -#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/render_context_filament.h" namespace mujoco { @@ -29,7 +28,7 @@ namespace mujoco { // IndirectLight. class Light : public mjrLight { public: - Light(FilamentContext* ctx, const mjrLightParams& params); + Light(filament::Engine* engine, const mjrLightParams& params); ~Light() noexcept; Light(const Light&) = delete; diff --git a/src/experimental/filament/filament/renderable.cc b/src/experimental/filament/filament/renderable.cc index ddd45503..232cdfb9 100644 --- a/src/experimental/filament/filament/renderable.cc +++ b/src/experimental/filament/filament/renderable.cc @@ -31,7 +31,6 @@ #include #include "experimental/filament/filament_util.h" #include "experimental/filament/filament/builtins.h" -#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/material.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/object_manager.h" @@ -49,8 +48,10 @@ using filament::math::mat4f; static constexpr float kArrowScale = 1.f / 6.f; static constexpr float kArrowHeadSize = 1.75f; -Renderable::Renderable(FilamentContext* ctx, const mjrRenderableParams& params) - : object_mgr_(ctx->GetObjectManager()), params_(params) { +Renderable::Renderable(filament::Engine* engine, + const mjrRenderableParams& params, + ObjectManager* object_mgr) + : object_mgr_(object_mgr), params_(params) { mjr_defaultMaterial(&material_); } diff --git a/src/experimental/filament/filament/renderable.h b/src/experimental/filament/filament/renderable.h index 3d7707da..39a267d5 100644 --- a/src/experimental/filament/filament/renderable.h +++ b/src/experimental/filament/filament/renderable.h @@ -25,7 +25,6 @@ #include #include #include "experimental/filament/filament_util.h" -#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/mesh.h" #include "experimental/filament/filament/object_manager.h" #include "experimental/filament/render_context_filament.h" @@ -39,7 +38,8 @@ namespace mujoco { // on the surface). class Renderable : public mjrRenderable { public: - Renderable(FilamentContext* ctx, const mjrRenderableParams& params); + Renderable(filament::Engine* engine, const mjrRenderableParams& params, + ObjectManager* object_mgr); ~Renderable() noexcept; Renderable(const Renderable&) = delete; diff --git a/src/experimental/filament/filament/scene_view.cc b/src/experimental/filament/filament/scene_view.cc index d7ff12e6..c761a099 100644 --- a/src/experimental/filament/filament/scene_view.cc +++ b/src/experimental/filament/filament/scene_view.cc @@ -42,7 +42,6 @@ #include #include "experimental/filament/filament_util.h" #include "experimental/filament/filament/color_grading_options.h" -#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/light.h" #include "experimental/filament/filament/render_target.h" #include "experimental/filament/filament/renderable.h" @@ -122,9 +121,8 @@ static void SetupReflectionCamera(const mat4& surface_xform, reflection_camera->setCustomProjection(oblique, near, far); } -SceneView::SceneView(FilamentContext* ctx, const mjrSceneParams& params) - : ctx_(ctx) { - filament::Engine* engine = ctx_->GetEngine(); +SceneView::SceneView(filament::Engine* engine, const mjrSceneParams& params) + : engine_(engine) { scene_ = engine->createScene(); camera_ = engine->createCamera(utils::EntityManager::get().create()); reflect_camera_ = engine->createCamera(utils::EntityManager::get().create()); @@ -168,10 +166,9 @@ SceneView::SceneView(FilamentContext* ctx, const mjrSceneParams& params) } SceneView::~SceneView() { - filament::Engine* engine = ctx_->GetEngine(); if (skybox_) { scene_->setSkybox(nullptr); - engine->destroy(skybox_); + engine_->destroy(skybox_); } for (auto& light : lights_) { light->RemoveFromScene(scene_); @@ -182,15 +179,15 @@ SceneView::~SceneView() { lights_.clear(); renderables_.clear(); reflect_targets_.clear(); - engine->destroyCameraComponent(reflect_camera_->getEntity()); - engine->destroy(reflect_view_); - engine->destroyCameraComponent(camera_->getEntity()); + engine_->destroyCameraComponent(reflect_camera_->getEntity()); + engine_->destroy(reflect_view_); + engine_->destroyCameraComponent(camera_->getEntity()); if (color_grading_) { - engine->destroy(color_grading_); + engine_->destroy(color_grading_); } - engine->destroy(scene_); + engine_->destroy(scene_); for (auto& view : views_) { - engine->destroy(view); + engine_->destroy(view); } } @@ -239,8 +236,11 @@ void SceneView::SetSkybox(const Texture* skybox_texture) { } } -void SceneView::Render(filament::Renderer* renderer, - const RenderRequest& request) { +void SceneView::Render(filament::Renderer* renderer, const mjrRenderRequest& request) { + if (request.scene != this) { + mju_error("Invalid scene for SceneView::Render."); + } + filament::Viewport viewport(request.viewport.left, request.viewport.bottom, request.viewport.width, request.viewport.height); for (auto& view : views_) { @@ -258,8 +258,8 @@ void SceneView::Render(filament::Renderer* renderer, filament::MultiSampleAntiAliasingOptions options = view->getMultiSampleAntiAliasingOptions(); - filament::RenderTarget* render_target = - request.target ? request.target->GetFilamentRenderTarget() : nullptr; + + RenderTarget* render_target = RenderTarget::downcast(request.target); if (render_target) { // We need to disable msaa in order to render to texture. view->setMultiSampleAntiAliasingOptions({.enabled = false}); @@ -287,7 +287,8 @@ void SceneView::Render(filament::Renderer* renderer, } } - view->setRenderTarget(render_target); + view->setRenderTarget(render_target ? render_target->GetFilamentRenderTarget() + : nullptr); renderer->render(view); view->setRenderTarget(nullptr); @@ -308,8 +309,7 @@ void SceneView::AddReflectiveRenderable(Renderable* renderable) { config.color_format = mjPIXEL_FORMAT_RGBA8; config.depth_format = mjPIXEL_FORMAT_DEPTH32F; - reflect_targets_.push_back( - std::make_unique(ctx_->GetEngine(), config)); + reflect_targets_.push_back(std::make_unique(engine_, config)); } // Prepare a render target for the reflective renderable. @@ -382,9 +382,6 @@ ColorGradingOptions SceneView::GetColorGradingOptions() const { } void SceneView::Configure(const mjModel* model) { - ctx_->SetClearColor(ReadElement(model, "filament.clearColor", - filament::math::float4(0, 0, 0, 1))); - filament::View* view = views_[mjDRAW_MODE_COLOR]; auto cg = color_grading_options_; @@ -483,30 +480,4 @@ void SceneView::Configure(const mjModel* model) { bloom.levels = ReadElement(model, "filament.bloom.levels", bloom.levels); view->setBloomOptions(bloom); } - -void DoRender(filament::Renderer* renderer, const mjrRenderRequest& request) { - SceneView::RenderRequest scene_view_request; - scene_view_request.draw_mode = request.draw_mode; - scene_view_request.viewport = request.viewport; - scene_view_request.camera = request.camera; - SceneView* scene_view = SceneView::downcast(request.scene); - scene_view->Render(renderer, scene_view_request); -} - -void DoReadPixels(filament::Renderer* renderer, - const mjrRenderRequest& request, - const mjrReadPixelsRequest& read_request) { - RenderTarget* render_target = RenderTarget::downcast(request.target); - - SceneView::RenderRequest scene_view_request; - scene_view_request.draw_mode = request.draw_mode; - scene_view_request.viewport = request.viewport; - scene_view_request.camera = request.camera; - scene_view_request.target = render_target; - SceneView* scene_view = SceneView::downcast(request.scene); - scene_view->Render(renderer, scene_view_request); - render_target->ReadColorPixels(renderer, (uint8_t*)read_request.output, - read_request.num_bytes); -} - } // namespace mujoco diff --git a/src/experimental/filament/filament/scene_view.h b/src/experimental/filament/filament/scene_view.h index 1522fe3c..f17b4022 100644 --- a/src/experimental/filament/filament/scene_view.h +++ b/src/experimental/filament/filament/scene_view.h @@ -27,7 +27,6 @@ #include #include #include "experimental/filament/filament/color_grading_options.h" -#include "experimental/filament/filament/filament_context.h" #include "experimental/filament/filament/light.h" #include "experimental/filament/filament/renderable.h" #include "experimental/filament/filament/render_target.h" @@ -43,7 +42,7 @@ namespace mujoco { // (e.g. normal, depth, segmentation, etc.) as well as reflective surfaces. class SceneView : public mjrScene { public: - SceneView(FilamentContext* ctx, const mjrSceneParams& params); + SceneView(filament::Engine* engine, const mjrSceneParams& params); ~SceneView(); SceneView(const SceneView&) = delete; @@ -56,23 +55,10 @@ class SceneView : public mjrScene { void RemoveFromScene(Renderable* renderable); void SetSkybox(const Texture* skybox_texture); - // Parameters for rendering the scene. - struct RenderRequest { - // The draw mode (e.g. normal, depth, segmentation) to render. - mjrDrawMode draw_mode = mjDRAW_MODE_COLOR; - // The target viewport for the rendered image. - mjrRect viewport; - // The camera from which to render the scene. - mjrCamera camera; - // An optional render target into which the scene will be rendered. - RenderTarget* target = nullptr; - }; - - // Renders the scene. - void Render(filament::Renderer* renderer, const RenderRequest& request); + void Render(filament::Renderer* renderer, const mjrRenderRequest& request); // Returns the filament Engine managing the scene. - filament::Engine* GetEngine() const { return ctx_->GetEngine(); } + filament::Engine* GetEngine() const { return engine_; } // Enables/disables shadows for the default render view. void EnableShadows(); @@ -110,7 +96,7 @@ class SceneView : public mjrScene { // rendered in their own passes to create the reflective texture. void AddReflectiveRenderable(Renderable* renderable); - FilamentContext* ctx_ = nullptr; + filament::Engine* engine_ = nullptr; filament::Scene* scene_ = nullptr; filament::Camera* camera_ = nullptr; filament::ColorGrading* color_grading_ = nullptr; diff --git a/src/experimental/filament/render_context_filament.cc b/src/experimental/filament/render_context_filament.cc index 220f87cc..5cc8ddf5 100644 --- a/src/experimental/filament/render_context_filament.cc +++ b/src/experimental/filament/render_context_filament.cc @@ -162,7 +162,8 @@ mjrMesh* mjrf_createMesh(mjrfContext* ctx, const mjrMeshData* data) { void mjrf_destroyMesh(mjrMesh* mesh) { delete mujoco::Mesh::downcast(mesh); } mjrScene* mjrf_createScene(mjrfContext* ctx, const mjrSceneParams* params) { - return new mujoco::SceneView(mujoco::FilamentContext::downcast(ctx), *params); + return new mujoco::SceneView( + mujoco::FilamentContext::downcast(ctx)->GetEngine(), *params); } void mjrf_destroyScene(mjrScene* scene) { @@ -170,7 +171,8 @@ void mjrf_destroyScene(mjrScene* scene) { } mjrLight* mjrf_createLight(mjrfContext* ctx, const mjrLightParams* params) { - return new mujoco::Light(mujoco::FilamentContext::downcast(ctx), *params); + return new mujoco::Light(mujoco::FilamentContext::downcast(ctx)->GetEngine(), + *params); } void mjrf_destroyLight(mjrLight* light) { @@ -179,8 +181,9 @@ void mjrf_destroyLight(mjrLight* light) { mjrRenderable* mjrf_createRenderable(mjrfContext* ctx, const mjrRenderableParams* params) { - return new mujoco::Renderable(mujoco::FilamentContext::downcast(ctx), - *params); + return new mujoco::Renderable( + mujoco::FilamentContext::downcast(ctx)->GetEngine(), *params, + mujoco::FilamentContext::downcast(ctx)->GetObjectManager()); } void mjrf_destroyRenderable(mjrRenderable* renderable) { @@ -346,6 +349,11 @@ void mjrf_waitForFrame(mjrfContext* ctx, mjrFrameHandle frame) { mujoco::FilamentContext::downcast(ctx)->WaitForFrame(frame); } +void mjrf_setClearColor(mjrfContext* ctx, const float color[3]) { + mujoco::FilamentContext::downcast(ctx)->SetClearColor( + {color[0], color[1], color[2], 1.0f}); +} + void mjrf_getFrameStats(mjrfContext* ctx, mjrFrameHandle frame, mjrFrameStats* stats_out) { mujoco::FilamentContext::downcast(ctx)->GetFrameStats(frame, stats_out); diff --git a/src/experimental/filament/render_context_filament.h b/src/experimental/filament/render_context_filament.h index 0d7d0cbd..f864a325 100644 --- a/src/experimental/filament/render_context_filament.h +++ b/src/experimental/filament/render_context_filament.h @@ -211,6 +211,9 @@ mjrFrameHandle mjrf_render(mjrfContext* ctx, const mjrRenderRequest* req, // triggering any callbacks as needed. void mjrf_waitForFrame(mjrfContext* ctx, mjrFrameHandle frame); +// Sets the clear color for the renderer. +void mjrf_setClearColor(mjrfContext* ctx, const float color[3]); + // Information about a single frame of rendering. struct mjrFrameStats { // The frame rate of the renderer, in frames per second. From 7f5be412a0e7606ff0b590f103853267b177ef82 Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Tue, 12 May 2026 09:26:26 -0700 Subject: [PATCH 246/251] Put miniz in MujocoDependencies.cmake. PiperOrigin-RevId: 914322421 Change-Id: I3f6e821164eede979802c51a6f8ee00d3346d585 --- cmake/MujocoDependencies.cmake | 13 +++++++++ cmake/third_party_deps/miniz.cmake | 29 --------------------- cmake/third_party_deps/miniz/CMakeLists.txt | 22 ---------------- src/xml/mjz/CMakeLists.txt | 1 - 4 files changed, 13 insertions(+), 52 deletions(-) delete mode 100644 cmake/third_party_deps/miniz.cmake delete mode 100644 cmake/third_party_deps/miniz/CMakeLists.txt diff --git a/cmake/MujocoDependencies.cmake b/cmake/MujocoDependencies.cmake index 4381a759..eaf8e51b 100644 --- a/cmake/MujocoDependencies.cmake +++ b/cmake/MujocoDependencies.cmake @@ -38,6 +38,10 @@ set(MUJOCO_DEP_VERSION_qhull 62ccc56af071eaa478bef6ed41fd7a55d3bb2d80 CACHE STRING "Version of `qhull` to be fetched." ) +set(MUJOCO_DEP_VERSION_miniz + 3.1.1 + CACHE STRING "Version of `miniz` to be fetched." +) set(MUJOCO_DEP_VERSION_Eigen3 ea13a98decd497a8c5588fb5de71b57bcf10d864 CACHE STRING "Version of `Eigen3` to be fetched." @@ -243,6 +247,15 @@ if(WIN32) endif() endif() +set(BUILD_TESTS OFF) +fetchpackage( + PACKAGE_NAME miniz + GIT_REPO https://github.com/richgel999/miniz.git + GIT_TAG ${MUJOCO_DEP_VERSION_miniz} + TARGETS miniz +) + + if(MUJOCO_BUILD_TESTS OR MUJOCO_BUILD_STUDIO OR MUJOCO_USE_FILAMENT) set(ABSL_PROPAGATE_CXX_STD ON) diff --git a/cmake/third_party_deps/miniz.cmake b/cmake/third_party_deps/miniz.cmake deleted file mode 100644 index bbaf5a2a..00000000 --- a/cmake/third_party_deps/miniz.cmake +++ /dev/null @@ -1,29 +0,0 @@ -# Copyright 2026 DeepMind Technologies Limited -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -set(MUJOCO_DEP_VERSION_miniz - 2.1.0 - CACHE STRING "Tag/version of `miniz` to be fetched." -) -mark_as_advanced(MUJOCO_DEP_VERSION_miniz) - -include(FindOrFetch) - -fetchpackage( - PACKAGE_NAME miniz - GIT_REPO https://github.com/richgel999/miniz.git - GIT_TAG ${MUJOCO_DEP_VERSION_miniz} - TARGETS miniz - CUSTOM_CMAKE "${CMAKE_SOURCE_DIR}/cmake/third_party_deps/miniz/CMakeLists.txt" -) diff --git a/cmake/third_party_deps/miniz/CMakeLists.txt b/cmake/third_party_deps/miniz/CMakeLists.txt deleted file mode 100644 index 696d37ed..00000000 --- a/cmake/third_party_deps/miniz/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -# Copyright 2026 DeepMind Technologies Limited -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# https://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -cmake_minimum_required(VERSION 3.16) - -PROJECT(miniz C) - -set(miniz_SOURCE miniz.c miniz_zip.c miniz_tinfl.c miniz_tdef.c) -add_library(miniz STATIC ${miniz_SOURCE}) - -target_include_directories(miniz PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}") diff --git a/src/xml/mjz/CMakeLists.txt b/src/xml/mjz/CMakeLists.txt index 5d07c7bb..a44e096b 100644 --- a/src/xml/mjz/CMakeLists.txt +++ b/src/xml/mjz/CMakeLists.txt @@ -11,7 +11,6 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. -include(third_party_deps/miniz) set(MUJOCO_MJZ_SRCS mjz_decoder.cc From a4e49f2dff3e7f26b6cd003e51097ace8f4d9732 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 12 May 2026 09:43:07 -0700 Subject: [PATCH 247/251] Margin and gap redesign (breaking change) PiperOrigin-RevId: 914329812 Change-Id: I905665e4c1965bdb8b90587e5b1277cfbfe5cce0 --- doc/XMLreference.rst | 51 +- doc/changelog.rst | 51 + doc/computation/index.rst | 55 +- doc/images/modeling/margin_gap_dark.svg | 887 ++++++++++++++++++ doc/images/modeling/margin_gap_light.svg | 886 +++++++++++++++++ doc/includes/references.h | 20 +- doc/modeling.rst | 3 +- .../usd/mjcPhysics/collisionAPI.h | 11 +- include/mujoco/mjdata.h | 2 +- include/mujoco/mjmodel.h | 12 +- include/mujoco/mjspec.h | 6 +- mjx/mujoco/mjx/_src/collision_driver.py | 5 +- mjx/mujoco/mjx/_src/types.py | 2 +- .../mujoco_warp/_src/collision_convex.py | 14 +- .../mujoco_warp/_src/collision_core.py | 11 +- .../mujoco_warp/_src/collision_driver.py | 40 +- .../mjx/third_party/mujoco_warp/_src/types.py | 9 +- .../third_party/mujoco_warp/_src/util_pkg.py | 20 +- model/adhesion/active_adhesion.xml | 12 +- python/mujoco/introspect/structs.py | 20 +- src/engine/engine_collision_driver.c | 117 +-- src/experimental/platform/ux/gui_spec.cc | 6 +- .../usd/mjcPhysics/generatedSchema.usda | 4 +- src/experimental/usd/mjcPhysics/schema.usda | 4 +- test/engine/engine_collision_gjk_test.cc | 4 +- .../testdata/collision_box/boxbox_bad0.xml | 2 +- .../collision_primitive/sphere_cylinder.xml | 2 +- test/engine/testdata/solver/model.xml | 2 +- test/testdata/model.xml | 2 +- 29 files changed, 2082 insertions(+), 178 deletions(-) create mode 100644 doc/images/modeling/margin_gap_dark.svg create mode 100644 doc/images/modeling/margin_gap_light.svg diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 55c220b7..d136291a 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -376,8 +376,8 @@ adjust it properly through the XML. :at:`o_margin`: :at-val:`real, "0"` This attribute replaces the margin parameter of all active contact pairs when :ref:`Contact override ` is enabled. Otherwise MuJoCo uses the element-specific margin attribute of :ref:`geom` or - :ref:`pair` depending on how the contact pair was generated. See also :ref:`Collision` in the - Computation chapter. The related gap parameter does not have a global override. + :ref:`pair` depending on how the contact pair was generated. See :ref:`margin and gap` in + the Computation chapter. The related gap parameter does not have a global override. .. _option-o_solref: .. _option-o_solimp: @@ -2702,18 +2702,20 @@ helps clarify the role of bodies and geoms in MuJoCo. .. _body-geom-margin: :at:`margin`: :at-val:`real, "0"` - Distance threshold below which contacts are detected and included in the global array mjData.contact. This however - does not mean that contact force will be generated. A contact is considered active only if the distance between the - two geom surfaces is below margin-gap. Recall that constraint impedance can be a function of distance, as explained - in :ref:`CSolver`. The quantity this function is applied to is the distance between - the two geoms minus the margin plus the gap. + Geometric inflation of the geom surface for the purpose of contact force generation. When the distance between two + geom surfaces is below ``margin``, the contact is considered active and contact forces are generated. The constraint + impedance can be a function of distance, as explained in :ref:`CSolver`. The quantity this function is applied to is + the distance between the two geoms minus the ``margin``. See :ref:`margin and gap`. .. _body-geom-gap: :at:`gap`: :at-val:`real, "0"` - This attribute is used to enable the generation of inactive contacts, i.e., contacts that are ignored by the - constraint solver but are included in mjData.contact for the purpose of custom computations. When this value is - positive, geom distances between margin and margin-gap correspond to such inactive contacts. + Additional contact detection buffer beyond ``margin``. When this value is positive, contacts are detected at + distance ``margin + gap`` but forces are only generated at distance ``margin``. Contacts with distance between + ``margin`` and ``margin + gap`` are included in ``mjData.contact`` as inactive contacts (with ``efc_address`` = -1). + These inactive contacts can be used for custom computations, for example by :ref:`adhesion` + actuators which use contacts in the gap zone to generate adhesive forces without producing contact forces. + See :ref:`margin and gap`. .. _body-geom-fromto: @@ -4110,14 +4112,15 @@ friction can only be created with this element. .. _contact-pair-margin: :at:`margin`: :at-val:`real, "0"` - Distance threshold below which contacts are detected and included in the global array mjData.contact. + Geometric inflation for the purpose of contact force generation. Contacts are detected at distance ``margin + gap`` + and forces are generated at distance ``margin``. .. _contact-pair-gap: :at:`gap`: :at-val:`real, "0"` - This attribute is used to enable the generation of inactive contacts, i.e., contacts that are ignored by the - constraint solver but are included in mjData.contact for the purpose of custom computations. When this value is - positive, geom distances between margin and margin-gap correspond to such inactive contacts. + Additional contact detection buffer beyond ``margin``. When this value is positive, contacts with distance between + ``margin`` and ``margin + gap`` are included in ``mjData.contact`` as inactive contacts but no contact forces are + generated. .. _contact-exclude: @@ -6286,16 +6289,16 @@ This element has nine custom attributes in addition to the common attributes: This element defines an active adhesion actuator which injects forces at contacts in the normal direction, see illustration video. The model shown in the video can be found `here -`_ and includes inline annotations. The transmission target -is a :el:`body`, and adhesive forces are injected into all contacts involving geoms which belong to this body. The force -is divided equally between multiple contacts. When the :at:`gap` attribute is not used, this actuator requires active -contacts and cannot apply a force at a distance, more like the active adhesion on the feet of geckos and insects rather -than an industrial vacuum gripper. In order to enable "suction at a distance", "inflate" the body's geoms by -:at:`margin` and add a corresponding :at:`gap` which activates contacts only after :at:`gap` penetration distance. This -will create a layer around the geom where contacts are detected but are inactive, and can be used for -applying the adhesive force. In the video above, such inactive contacts are blue, while active contacts are orange. -An adhesion actuator's length is always 0. :at:`ctrlrange` is required and must also be nonnegative (no repulsive forces -are allowed). The underlying :el:`general` attributes are set as follows: +`_ and includes inline annotations. The transmission +target is a :el:`body`, and adhesive forces are injected into all contacts involving geoms which belong to this body. +The force is divided equally between multiple contacts. When the :ref:`gap` attribute is not used, this +actuator requires active contacts and cannot apply a force at a distance, more like the active adhesion on the feet of +geckos and insects rather than an industrial vacuum gripper. In order to enable "suction at a distance", set the +:ref:`gap` attribute of the body's geoms to a positive value. This creates a layer around each geom where +contacts are detected but no contact forces are generated, and the adhesive force can act across this gap. In the video +above, such inactive contacts are blue, while active contacts are orange. An adhesion actuator's length is always 0. +:at:`ctrlrange` is required and must also be nonnegative (no repulsive forces are allowed). The underlying :el:`general` +attributes are set as follows: =========== ======= =========== ======== Attribute Setting Attribute Setting diff --git a/doc/changelog.rst b/doc/changelog.rst index e408b4fa..437cdac0 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -2,6 +2,57 @@ Changelog ========= +Upcoming version (not yet released) +----------------------------------- + +.. admonition:: Breaking API changes + :class: attention + + - The semantics of the contact ``margin`` and ``gap`` parameters have been redesigned for conceptual clarity and + consistency with `NVIDIA Newton `__. See the new + :ref:`margin and gap` documentation section for details. + + Previously, ``margin`` controlled the *detection threshold* (contacts exist when ``dist < margin``) and ``gap`` + was subtracted from it to produce the *force threshold* (forces generated when ``dist < margin - gap``). This was + unintuitive: users expected ``margin`` to mean geometric inflation and ``gap`` to mean a spatial gap. + + Under the new semantics, ``margin`` is the geometric inflation of the geom surface and ``gap`` is an additional + detection buffer beyond the inflated surface: + + - **Detection**: contacts are created when ``dist < margin + gap``. + - **Force generation**: constraint forces are applied when ``dist < margin``. + - **Inactive contacts**: contacts with ``margin < dist ≤ margin + gap`` are included in ``mjData.contact`` but + generate no force (``efc_address = -1``). This is useful for :ref:`adhesion` actuators and + custom callbacks. + + With the default values ``margin = 0``, ``gap = 0``, the behavior is unchanged. + + .. image:: images/modeling/margin_gap_light.svg + :width: 80% + :align: center + :class: only-light + + .. image:: images/modeling/margin_gap_dark.svg + :width: 80% + :align: center + :class: only-dark + + | + + **Migration:** Models that use the default ``gap="0"`` (the vast majority) require no changes. For models with + ``gap > 0``, apply the following transformation to preserve identical behavior: + + .. code-block:: + + margin_new = margin_old - gap_old + gap_new = gap_old + + For example, a geom with the old attributes ``margin="0.1" gap="0.1"`` should be changed to + ``margin="0" gap="0.1"``. + + Negative ``margin`` values are now permitted (corresponding to ``gap > margin`` under the old semantics). The + constraint ``margin + gap >= 0`` should be maintained to ensure valid collision detection. + Version 3.8.1 (May 11, 2026) ---------------------------- diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 8ef76cb8..059558ee 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -909,7 +909,7 @@ In addition to the above quantities which are computed online, each contact has model definition. .. list-table:: - :widths: 1 5 + :widths: 2 6 :header-rows: 1 * - Parameter @@ -920,15 +920,53 @@ model definition. - Vector of friction coefficients with dimensionality ``condim-1``. See below for semantics of the specific coefficients. * - ``margin`` - - The distance margin used to determine if the contact should be included in the global contact array - ``mjData.contact``. + - The geometric inflation of the geom surfaces. Contacts are detected when the distance is below + ``margin + gap``, and contact forces are generated when the distance is below ``margin``. * - ``gap`` - - For custom computations it is sometimes convenient to include contacts in ``mjData.contact`` but not generate - contact forces. This is what ``gap`` does: contact forces are generated only when the normal distance is below - (margin - gap). + - An additional detection buffer beyond ``margin``. Contacts with distance between ``margin`` and + ``margin + gap`` are included in ``mjData.contact`` as inactive contacts, but no contact forces are generated. + This is useful for action-at-a-distance effects, for example by :ref:`adhesion` actuators. * - ``solref`` and ``solimp`` - :ref:`Solver ` parameters, explained later. +.. _coMarginGap: + +margin and gap +^^^^^^^^^^^^^^ + +Each geom has a ``margin`` and a ``gap`` parameter, defined in the table above. The values for both parameters are +:ref:`summed` when considering contact between the two geoms. Together they define three regimes of contact +detection and force generation, illustrated in the figure below. + +.. image:: ../images/modeling/margin_gap_light.svg + :width: 90% + :align: center + :class: only-light + +.. image:: ../images/modeling/margin_gap_dark.svg + :width: 90% + :align: center + :class: only-dark + +The distance between two geom surfaces determines which regime applies: + +- **No contact** (distance > ``margin + gap``): The geom surfaces, including their gap buffers, are not overlapping. + No contact is generated. + +- **Inactive contact** (``margin`` < distance ≤ ``margin + gap``): A contact is detected and included in + ``mjData.contact``, but no contact force is generated (``efc_address = -1``). These contacts can be used for custom + computations, for example by :ref:`adhesion` actuators. + +- **Active contact** (distance ≤ ``margin``): The contact is active and constraint forces are generated. The constraint + impedance function is applied to the quantity ``distance - margin``, which is non-positive in this regime. + +Negative ``margin`` values, corresponding "shrinkgage" of the geometric shape, are permitted. In this case +``margin + gap >= 0`` must be maintained for collision detection to work correctly. + +.. _coCondim: + +condim +^^^^^^ The contact friction cone can be either elliptic or pyramidal. This is a global setting determined by the choice of constraint solver: the elliptic solvers work with elliptic cones, while the pyramidal solvers work with pyramidal cones, as defined later. The ``condim`` parameter determines the contact type, and has the following meaning: @@ -961,6 +999,11 @@ Note that condim cannot be 2 or 5. This is because the two tangential directions treated as pairs. The friction coefficients within a pair can be different though, which can be used to model skating for example. +.. _coCones: + +Friction cones +^^^^^^^^^^^^^^ + Now we describe the friction cones and the corresponding Jacobians more formally. In this section only, let :math:`f` denote the vector of constraint forces for a single contact (as opposed to the system-level vector of constraint forces), :math:`\mu` the vector of friction coefficients, and :math:`n` the contact dimensionality condim. For diff --git a/doc/images/modeling/margin_gap_dark.svg b/doc/images/modeling/margin_gap_dark.svg new file mode 100644 index 00000000..adddae01 --- /dev/null +++ b/doc/images/modeling/margin_gap_dark.svg @@ -0,0 +1,887 @@ + + + + + + + + 2026-05-12T11:57:09.015687 + image/svg+xml + + + Matplotlib v3.10.6, https://matplotlib.org/ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/images/modeling/margin_gap_light.svg b/doc/images/modeling/margin_gap_light.svg new file mode 100644 index 00000000..93f162c0 --- /dev/null +++ b/doc/images/modeling/margin_gap_light.svg @@ -0,0 +1,886 @@ + + + + + + + + 2026-05-12T11:57:08.937609 + image/svg+xml + + + Matplotlib v3.10.6, https://matplotlib.org/ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/doc/includes/references.h b/doc/includes/references.h index c226304b..3034e6c9 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -108,7 +108,7 @@ struct mjContact_ { // result of collision detection functions mjtNum frame[9]; // normal is in [0-2], points from geom[0] to geom[1] // contact parameters set by mj_collideGeoms - mjtNum includemargin; // include if dist`-:ref:`gap`. + constraint becomes active; for contacts this margin is :ref:`margin`. Limit and contact constraints are active when :math:`r < 0` (penetration). For frictional constraints, see :ref:`Friction`. @@ -516,6 +516,7 @@ are as follows: **margin**, **gap** The sum of the two geom margins (or gaps respectively) is used. The geom priority is ignored here, because the margin and gap are distance properties and a one-sided specification makes little sense. + See :ref:`margin and gap`. .. _solmixing: diff --git a/include/mujoco/experimental/usd/mjcPhysics/collisionAPI.h b/include/mujoco/experimental/usd/mjcPhysics/collisionAPI.h index 1203ad5a..3aee44ed 100644 --- a/include/mujoco/experimental/usd/mjcPhysics/collisionAPI.h +++ b/include/mujoco/experimental/usd/mjcPhysics/collisionAPI.h @@ -321,8 +321,8 @@ class MjcPhysicsCollisionAPI : public UsdAPISchemaBase { // --------------------------------------------------------------------- // // MARGIN // --------------------------------------------------------------------- // - /// Distance threshold below which contacts are detected and included in the - /// global array mjData.contact. + /// Geometric inflation of the geom surface for the purpose of contact force + /// generation. /// /// | || /// | -- | -- | @@ -346,11 +346,8 @@ class MjcPhysicsCollisionAPI : public UsdAPISchemaBase { // --------------------------------------------------------------------- // // GAP // --------------------------------------------------------------------- // - /// This attribute is used to enable the generation of inactive contacts, - /// i.e., contacts that are ignored by the constraint solver but are included - /// in mjData.contact for the purpose of custom computations. When this value - /// is positive, geom distances between margin and margin-gap correspond to - /// such inactive contacts. + /// Additional contact detection buffer beyond margin. Contacts are detected + /// at distance margin + gap but forces are only generated at distance margin. /// /// | || /// | -- | -- | diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index f98de218..d092a27d 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -130,7 +130,7 @@ struct mjContact_ { // result of collision detection functions mjtNum frame[9]; // normal is in [0-2], points from geom[0] to geom[1] // contact parameters set by mj_collideGeoms - mjtNum includemargin; // include if dist Dict[FunctionKey, Contact]: if ip.size > 0: # pair contacts get their params from m.pair_* fields params.append(( - m.pair_margin[ip] - m.pair_gap[ip], + m.pair_margin[ip], jp.clip(m.pair_friction[ip], min=eps), m.pair_solref[ip], m.pair_solreffriction[ip], @@ -284,7 +284,6 @@ def _contact_groups(m: Model, d: Data) -> Dict[FunctionKey, Contact]: if geom1.size > 0 and geom2.size > 0: # other contacts get their params from geom fields margin = m.geom_margin[geom1] + m.geom_margin[geom2] - gap = m.geom_gap[geom1] + m.geom_gap[geom2] solmix1, solmix2 = m.geom_solmix[geom1], m.geom_solmix[geom2] mix = solmix1 / (solmix1 + solmix2) mix = jp.where((solmix1 < eps) & (solmix2 < eps), 0.5, mix) @@ -315,7 +314,7 @@ def _contact_groups(m: Model, d: Data) -> Dict[FunctionKey, Contact]: # unpack 5d friction: friction = friction[:, [0, 0, 1, 2, 2]] - params.append((margin - gap, friction, solref, solreffriction, solimp)) + params.append((margin, friction, solref, solreffriction, solimp)) params = map(jp.concatenate, zip(*params)) includemargin, friction, solref, solreffriction, solimp = params diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index e29d4aca..bc51e6e5 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -942,7 +942,7 @@ class Contact(PyTreeNode): dist: distance between nearest points; neg: penetration pos: position of contact point: midpoint between geoms (3,) frame: normal is in [0-2] (9,) - includemargin: include if dist= 0.0 and pairid[1] == -1: - return 0 + if wp.static(_NEW_GAP_SEMANTICS): + if dist >= gap and pairid[1] == -1: + return 0 + else: + if dist >= 0.0 and pairid[1] == -1: + return 0 # CCD operates on margin-inflated shapes (support() inflates each geom by # 0.5 * margin). The returned dist is therefore relative to the inflated diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_core.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_core.py index 96da38e0..41feb836 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_core.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_core.py @@ -22,6 +22,7 @@ import warp as wp from mujoco.mjx.third_party.mujoco_warp._src.math import safe_div from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINMU +from mujoco.mjx.third_party.mujoco_warp._src.types import _NEW_GAP_SEMANTICS from mujoco.mjx.third_party.mujoco_warp._src.types import MJ_MINVAL from mujoco.mjx.third_party.mujoco_warp._src.types import ContactType from mujoco.mjx.third_party.mujoco_warp._src.types import GeomType @@ -197,14 +198,15 @@ def write_contact( Returns 1 if the contact is active (dist < margin), 0 otherwise. """ active = dist_in < margin_in + detected = dist_in < margin_in + gap_in # skip contact and no collision sensor - if (pairid_in[0] == -2 or not active) and pairid_in[1] == -1: + if (pairid_in[0] == -2 or not detected) and pairid_in[1] == -1: return 0 contact_type = 0 - if pairid_in[0] >= -1 and active: + if pairid_in[0] >= -1 and detected: contact_type |= ContactType.CONSTRAINT if pairid_in[1] >= 0: @@ -217,7 +219,10 @@ def write_contact( contact_frame_out[cid] = frame_in contact_geom_out[cid] = geoms_in contact_worldid_out[cid] = worldid_in - includemargin = margin_in - gap_in + if wp.static(_NEW_GAP_SEMANTICS): + includemargin = margin_in + else: + includemargin = margin_in - gap_in contact_includemargin_out[cid] = includemargin contact_dim_out[cid] = condim_in contact_friction_out[cid] = friction_in diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py index c87b21f1..df75adc7 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/collision_driver.py @@ -271,13 +271,14 @@ def _obb_filter( return True -def _broadphase_filter(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int): +def _broadphase_filter(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int, ngeom_gap: int): @wp.func def func( # Model: geom_aabb: wp.array3d[wp.vec3], geom_rbound: wp.array2d[float], geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], # Data in: geom_xpos_in: wp.array2d[wp.vec3], geom_xmat_in: wp.array2d[wp.mat33], @@ -299,21 +300,25 @@ def _broadphase_filter(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound rbound1, rbound2 = geom_rbound[rbound_id, geom1], geom_rbound[rbound_id, geom2] # kernel_analyzer: ignore margin_id = worldid % ngeom_margin if wp.static(ngeom_margin > 1) else 0 margin1, margin2 = geom_margin[margin_id, geom1], geom_margin[margin_id, geom2] # kernel_analyzer: ignore + gap_id = worldid % ngeom_gap if wp.static(ngeom_gap > 1) else 0 + gap1, gap2 = geom_gap[gap_id, geom1], geom_gap[gap_id, geom2] # kernel_analyzer: ignore + effective_margin1 = margin1 + gap1 + effective_margin2 = margin2 + gap2 xpos1, xpos2 = geom_xpos_in[worldid, geom1], geom_xpos_in[worldid, geom2] xmat1, xmat2 = geom_xmat_in[worldid, geom1], geom_xmat_in[worldid, geom2] if rbound1 == 0.0 or rbound2 == 0.0: if wp.static(opt_broadphase_filter & BroadphaseFilter.PLANE): - return _plane_filter(rbound1, rbound2, margin1, margin2, xpos1, xpos2, xmat1, xmat2) + return _plane_filter(rbound1, rbound2, effective_margin1, effective_margin2, xpos1, xpos2, xmat1, xmat2) else: if wp.static(opt_broadphase_filter & BroadphaseFilter.SPHERE): - if not _sphere_filter(rbound1, rbound2, margin1, margin2, xpos1, xpos2): + if not _sphere_filter(rbound1, rbound2, effective_margin1, effective_margin2, xpos1, xpos2): return False if wp.static(opt_broadphase_filter & BroadphaseFilter.AABB): - if not _aabb_filter(center1, center2, size1, size2, margin1, margin2, xpos1, xpos2, xmat1, xmat2): + if not _aabb_filter(center1, center2, size1, size2, effective_margin1, effective_margin2, xpos1, xpos2, xmat1, xmat2): return False if wp.static(opt_broadphase_filter & BroadphaseFilter.OBB): - if not _obb_filter(center1, center2, size1, size2, margin1, margin2, xpos1, xpos2, xmat1, xmat2): + if not _obb_filter(center1, center2, size1, size2, effective_margin1, effective_margin2, xpos1, xpos2, xmat1, xmat2): return False return True @@ -377,6 +382,7 @@ def _sap_project(opt_broadphase: int): ngeom: int, geom_rbound: wp.array2d[float], geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], # Data in: geom_xpos_in: wp.array2d[wp.vec3], nworld_in: int, @@ -397,7 +403,7 @@ def _sap_project(opt_broadphase: int): # geom is a plane rbound = MJ_MAXVAL - radius = rbound + geom_margin[worldid % geom_margin.shape[0], geomid] + radius = rbound + geom_margin[worldid % geom_margin.shape[0], geomid] + geom_gap[worldid % geom_gap.shape[0], geomid] center = wp.dot(direction_in, xpos) sort_index_out[worldid, geomid] = geomid @@ -443,7 +449,7 @@ def _sap_range( @cache_kernel -def _sap_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int): +def _sap_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int, ngeom_gap: int): @wp.kernel(module="unique", enable_backward=False) def kernel( # Model: @@ -452,6 +458,7 @@ def _sap_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: i geom_aabb: wp.array3d[wp.vec3], geom_rbound: wp.array2d[float], geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], nxn_pairid: wp.array[wp.vec2i], # Data in: geom_xpos_in: wp.array2d[wp.vec3], @@ -502,8 +509,8 @@ def _sap_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: i continue if ( - wp.static(_broadphase_filter(opt_broadphase_filter, ngeom_aabb, ngeom_rbound, ngeom_margin))( - geom_aabb, geom_rbound, geom_margin, geom_xpos_in, geom_xmat_in, geom1, geom2, worldid + wp.static(_broadphase_filter(opt_broadphase_filter, ngeom_aabb, ngeom_rbound, ngeom_margin, ngeom_gap))( + geom_aabb, geom_rbound, geom_margin, geom_gap, geom_xpos_in, geom_xmat_in, geom1, geom2, worldid ) or pairid[1] >= 0 ): @@ -586,7 +593,7 @@ def sap_broadphase(m: Model, d: Data, ctx: CollisionContext): wp.launch( kernel=_sap_project(m.opt.broadphase), dim=(d.nworld, m.ngeom), - inputs=[m.ngeom, m.geom_rbound, m.geom_margin, d.geom_xpos, d.nworld, direction], + inputs=[m.ngeom, m.geom_rbound, m.geom_margin, m.geom_gap, d.geom_xpos, d.nworld, direction], outputs=[ projection_lower.reshape((-1, m.ngeom)), projection_upper, @@ -622,7 +629,7 @@ def sap_broadphase(m: Model, d: Data, ctx: CollisionContext): # assumes each geom has 5 other geoms (batched over all worlds) nsweep = 5 * nworldgeom wp.launch( - kernel=_sap_broadphase(m.opt.broadphase_filter, m.geom_aabb.shape[0], m.geom_rbound.shape[0], m.geom_margin.shape[0]), + kernel=_sap_broadphase(m.opt.broadphase_filter, m.geom_aabb.shape[0], m.geom_rbound.shape[0], m.geom_margin.shape[0], m.geom_gap.shape[0]), dim=nsweep, inputs=[ m.ngeom, @@ -630,6 +637,7 @@ def sap_broadphase(m: Model, d: Data, ctx: CollisionContext): m.geom_aabb, m.geom_rbound, m.geom_margin, + m.geom_gap, m.nxn_pairid, d.geom_xpos, d.geom_xmat, @@ -644,7 +652,7 @@ def sap_broadphase(m: Model, d: Data, ctx: CollisionContext): @cache_kernel -def _nxn_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int): +def _nxn_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: int, ngeom_margin: int, ngeom_gap: int): @wp.kernel(module="unique", enable_backward=False) def kernel( # Model: @@ -652,6 +660,7 @@ def _nxn_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: i geom_aabb: wp.array3d[wp.vec3], geom_rbound: wp.array2d[float], geom_margin: wp.array2d[float], + geom_gap: wp.array2d[float], nxn_geom_pair: wp.array[wp.vec2i], nxn_pairid: wp.array[wp.vec2i], # Data in: @@ -672,8 +681,8 @@ def _nxn_broadphase(opt_broadphase_filter: int, ngeom_aabb: int, ngeom_rbound: i geom2 = geom[1] if ( - wp.static(_broadphase_filter(opt_broadphase_filter, ngeom_aabb, ngeom_rbound, ngeom_margin))( - geom_aabb, geom_rbound, geom_margin, geom_xpos_in, geom_xmat_in, geom1, geom2, worldid + wp.static(_broadphase_filter(opt_broadphase_filter, ngeom_aabb, ngeom_rbound, ngeom_margin, ngeom_gap))( + geom_aabb, geom_rbound, geom_margin, geom_gap, geom_xpos_in, geom_xmat_in, geom1, geom2, worldid ) or nxn_pairid[elementid][1] >= 0 ): @@ -709,13 +718,14 @@ def nxn_broadphase(m: Model, d: Data, ctx: CollisionContext): `contype`/`conaffinity`, parent-child relationships, and explicit `` tags. """ wp.launch( - _nxn_broadphase(m.opt.broadphase_filter, m.geom_aabb.shape[0], m.geom_rbound.shape[0], m.geom_margin.shape[0]), + _nxn_broadphase(m.opt.broadphase_filter, m.geom_aabb.shape[0], m.geom_rbound.shape[0], m.geom_margin.shape[0], m.geom_gap.shape[0]), dim=(d.nworld, m.nxn_geom_pair_filtered.shape[0]), inputs=[ m.geom_type, m.geom_aabb, m.geom_rbound, m.geom_margin, + m.geom_gap, m.nxn_geom_pair_filtered, m.nxn_pairid_filtered, d.geom_xpos, diff --git a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py index 7a4c8242..01c974c0 100644 --- a/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py +++ b/mjx/mujoco/mjx/third_party/mujoco_warp/_src/types.py @@ -17,6 +17,7 @@ import enum from typing import Callable import mujoco +from mujoco.mjx.third_party.mujoco_warp._src.util_pkg import check_version import numpy as np import warp as wp @@ -26,6 +27,8 @@ MJ_MINIMP = mujoco.mjMINIMP # minimum constraint impedance MJ_MAXIMP = mujoco.mjMAXIMP # maximum constraint impedance MJ_MAXCONPAIR = mujoco.mjMAXCONPAIR MJ_MINMU = mujoco.mjMINMU # minimum friction +# True if MuJoCo >= 3.9.0 (new margin/gap semantics: includemargin = margin) +_NEW_GAP_SEMANTICS = check_version("mujoco>=3.9.0") # maximum size (by number of edges) of an horizon in EPA algorithm MJ_MAX_EPAHORIZON = 24 # maximum average number of trianglarfaces EPA can insert at each iteration @@ -974,7 +977,7 @@ class Model: geom_quat: local orientation offset rel. to body (*, ngeom, 4) geom_friction: friction for (slide, spin, roll) (*, ngeom, 3) geom_margin: detect contact if dist tuple[tuple[int, int | str], ...]: """Parse a version string into comparable components. - Both '.' and '-' are treated as separators. Each component is wrapped in a - tuple: (0, int) for numeric parts, (-1, str) for non-numeric. A (0, 0) - sentinel is appended so that stable releases sort above pre-release suffixes - during Python tuple comparison (e.g., 1.2.3 >= 1.2.3.dev). Non-numeric - components are compared lexicographically (e.g., b >= a). + Dot-separated components form the version. Hyphen-separated suffixes (e.g., + "-foo3") are treated as local build identifiers and stripped before + parsing. Each component is wrapped in a tuple: (0, int) for numeric parts, + (-1, str) for non-numeric. A (0, 0) sentinel is appended so that stable + releases sort above pre-release suffixes during Python tuple comparison + (e.g., 1.2.3 >= 1.2.3.dev). Non-numeric components are compared + lexicographically (e.g., b >= a). Args: - version_str: Version string like "3.5.0" or "3.5.0.dev869102767". + version_str: Version string like "3.5.0", "3.5.0.dev869102767", or + "3.9.0-foo3". Returns: Tuple of (type_order, value) pairs for comparison, where type_order is 0 for integers and -1 for strings, followed by a (0, 0) sentinel. """ - # Split on both '.' and '-' - parts = re.split(r"[.\-]", version_str) + # Strip local build identifier (e.g., "3.9.0-foo3" -> "3.9.0") + version_str = version_str.split("-", 1)[0] + parts = version_str.split(".") return tuple([(0, int(p)) if p.isdigit() else (-1, p) for p in parts] + [(0, 0)]) diff --git a/model/adhesion/active_adhesion.xml b/model/adhesion/active_adhesion.xml index db99db98..8678311c 100644 --- a/model/adhesion/active_adhesion.xml +++ b/model/adhesion/active_adhesion.xml @@ -22,11 +22,11 @@ - + @@ -87,9 +87,9 @@ frictionless point particles. In order to make them stick to the sphere we give the sphere priority 2, to force condim=3. - Also note the sphere has a margin+gap of 3cm as opposed to the 1cm of the arm box. + Also note the sphere has a gap of 3cm as opposed to the 1cm of the arm box. --> - + diff --git a/python/mujoco/introspect/structs.py b/python/mujoco/introspect/structs.py index 5ed73b61..a9ee204a 100644 --- a/python/mujoco/introspect/structs.py +++ b/python/mujoco/introspect/structs.py @@ -2118,7 +2118,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='detect contact if distgeom_margin[nodeid1] + m->geom_margin[nodeid2]); + mjtNum gap = m->geom_gap[nodeid1] + m->geom_gap[nodeid2]; - if (!mj_filterSphere(m, d, nodeid1, nodeid2, margin)) { + if (!mj_filterSphere(m, d, nodeid1, nodeid2, margin + gap)) { if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2, d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1, d->geom_xpos + 3*nodeid2, d->geom_xmat + 9*nodeid2, - margin, NULL, NULL, &initialize)) { + margin + gap, NULL, NULL, &initialize)) { mj_collideGeomPair(m, d, nodeid1, nodeid2, merged, startadr, pairadr); if (mark_active) { d->bvh_active[node1 + bvhadr1] = 1; @@ -913,15 +914,16 @@ void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, // both are leaves if (isleaf1 && isleaf2) { mjtNum margin = mj_assignMargin(m, m->geom_margin[nodeid1] + m->flex_margin[f2]); + mjtNum gap = m->geom_gap[nodeid1] + m->flex_gap[f2]; if (!filterBitmask(m->geom_contype[nodeid1], m->geom_conaffinity[nodeid1], m->flex_contype[f2], m->flex_conaffinity[f2]) && - !filterSphereBox(d->geom_xpos + 3*nodeid1, m->geom_rbound[nodeid1] + margin, + !filterSphereBox(d->geom_xpos + 3*nodeid1, m->geom_rbound[nodeid1] + margin + gap, bvh2 + 6*node2)) { if (mj_collideOBB(m->geom_aabb + 6*nodeid1, bvh2 + 6*node2, d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1, NULL, NULL, - margin, NULL, NULL, &initialize)) { + margin + gap, NULL, NULL, &initialize)) { // collide unless geom is plane or SDF (handled separately) if (m->geom_type[nodeid1] != mjGEOM_PLANE && m->geom_type[nodeid1] != mjGEOM_SDF) { @@ -1058,7 +1060,8 @@ static void makeAAMM(const mjModel* m, mjData* d, // process all body geoms (body is collidable, should have geoms) for (int i=0; i < body_geomnum; i++) { int geom = m->body_geomadr[body]+i; - mjtNum margin = override_margin ? override_margin : m->geom_margin[geom]; + mjtNum margin = override_margin ? override_margin + : m->geom_margin[geom] + m->geom_gap[geom]; mjtNum _aamm[6]; const mjtNum* aabb = m->geom_aabb + 6*geom; @@ -1124,7 +1127,8 @@ static void makeAAMM(const mjModel* m, mjData* d, } // correct for flex radius and margin - mjtNum margin = override_margin ? override_margin : m->flex_margin[f]; + mjtNum margin = override_margin ? override_margin + : m->flex_margin[f] + m->flex_gap[f]; mjtNum bound = m->flex_radius[f] + margin; aamm[0] -= bound; aamm[1] -= bound; @@ -1513,8 +1517,8 @@ int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair) { //----------------------------- narrow-phase collision detection ----------------------------------- -// compute contact condim, gap, solref, solimp, friction -static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap, +// compute contact condim, solref, solimp, friction +static void mj_contactParam(const mjModel* m, int* condim, mjtNum* solref, mjtNum* solimp, mjtNum* friction, int g1, int g2, int f1, int f2) { mjtNum fri[3]; @@ -1522,7 +1526,6 @@ static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap, // get parameters from geom1 or flex1 int priority1 = (f1 < 0) ? m->geom_priority[g1] : m->flex_priority[f1]; int condim1 = (f1 < 0) ? m->geom_condim[g1] : m->flex_condim[f1]; - mjtNum gap1 = (f1 < 0) ? m->geom_gap[g1] : m->flex_gap[f1]; mjtNum solmix1 = (f1 < 0) ? m->geom_solmix[g1] : m->flex_solmix[f1]; const mjtNum* solref1 = (f1 < 0) ? m->geom_solref+g1*mjNREF : m->flex_solref+f1*mjNREF; const mjtNum* solimp1 = (f1 < 0) ? m->geom_solimp+g1*mjNIMP : m->flex_solimp+f1*mjNIMP; @@ -1531,15 +1534,11 @@ static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap, // get parameters from geom2 or flex2 int priority2 = (f2 < 0) ? m->geom_priority[g2] : m->flex_priority[f2]; int condim2 = (f2 < 0) ? m->geom_condim[g2] : m->flex_condim[f2]; - mjtNum gap2 = (f2 < 0) ? m->geom_gap[g2] : m->flex_gap[f2]; mjtNum solmix2 = (f2 < 0) ? m->geom_solmix[g2] : m->flex_solmix[f2]; const mjtNum* solref2 = (f2 < 0) ? m->geom_solref+g2*mjNREF : m->flex_solref+f2*mjNREF; const mjtNum* solimp2 = (f2 < 0) ? m->geom_solimp+g2*mjNIMP : m->flex_solimp+f2*mjNIMP; const mjtNum* friction2 = (f2 < 0) ? m->geom_friction+g2*3 : m->flex_friction+f2*3; - // gap: add - *gap = gap1 + gap2; - // different priority: copy from item with higher priority if (priority1 > priority2) { *condim = condim1; @@ -1719,8 +1718,15 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) { margin = mj_assignMargin(m, m->pair_margin[ipair]); } + // set gap: dynamic or pair + if (ipair < 0) { + gap = m->geom_gap[g1] + m->geom_gap[g2]; + } else { + gap = m->pair_gap[ipair]; + } + // bounding sphere filter - if (mj_filterSphere(m, d, g1, g2, margin)) { + if (mj_filterSphere(m, d, g1, g2, margin + gap)) { return; } @@ -1733,7 +1739,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) { } // call collision detector to generate contacts - num = collisionFunc(m, d, con, g1, g2, margin); + num = collisionFunc(m, d, con, g1, g2, margin + gap); // check contacts if (!num) { @@ -1746,15 +1752,14 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) { mjERROR("too many contacts returned by collision function"); } - // set condim, gap, solref, solimp, friction: dynamic + // set condim, solref, solimp, friction: dynamic if (ipair < 0) { - mj_contactParam(m, &condim, &gap, solref, solimp, friction, g1, g2, -1, -1); + mj_contactParam(m, &condim, solref, solimp, friction, g1, g2, -1, -1); } - // set condim, gap, solref, solimp, friction: pair + // set condim, solref, solimp, friction: pair else { condim = m->pair_dim[ipair]; - gap = m->pair_gap[ipair]; mju_copy(solref, m->pair_solref+mjNREF*ipair, mjNREF); mju_copy(solimp, m->pair_solimp+mjNIMP*ipair, mjNIMP); mju_copy(friction, m->pair_friction+5*ipair, 5); @@ -1778,7 +1783,7 @@ void mj_collideGeoms(const mjModel* m, mjData* d, int ipair, int g1, int g2) { con[i].vert[1] = -1; // set remaining contact parameters - mj_setContact(m, con + i, condim, margin-gap, solref, solreffriction, solimp, friction); + mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction); } // add to ncon @@ -1801,9 +1806,10 @@ void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) { mjtNum margin = mj_assignMargin(m, m->geom_margin[g] + m->flex_margin[f]); int condim; int flex_vertnum = m->flex_vertnum[f]; - mjtNum gap, solref[mjNREF], solimp[mjNIMP], friction[5]; + mjtNum gap = m->geom_gap[g] + m->flex_gap[f]; + mjtNum solref[mjNREF], solimp[mjNIMP], friction[5]; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, &gap, solref, solimp, friction, g, -1, -1, f); + mj_contactParam(m, &condim, solref, solimp, friction, g, -1, -1, f); // collide all flex vertices with plane for (int i=0; i < flex_vertnum; i++) { @@ -1814,7 +1820,7 @@ void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) { mjtNum dist = mju_dot3(dif, nrm); // no contact - if (dist > margin + radius) { + if (dist > margin + gap + radius) { continue; } @@ -1835,7 +1841,7 @@ void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f) { con.vert[1] = i; // set remaining contact parameters - mj_setContact(m, &con, condim, margin-gap, solref, solreffriction, solimp, friction); + mj_setContact(m, &con, condim, margin, solref, solreffriction, solimp, friction); // add to mjData, abort if too many contacts if (mj_addContact(m, d, &con)) { @@ -1854,10 +1860,11 @@ void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) { // prepare contact parameters (same for all contacts) mjtNum margin = mj_assignMargin(m, m->geom_margin[g] + m->flex_margin[f]); + mjtNum gap = m->geom_gap[g] + m->flex_gap[f]; int condim; - mjtNum gap, solref[mjNREF], solimp[mjNIMP], friction[5]; + mjtNum solref[mjNREF], solimp[mjNIMP], friction[5]; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, &gap, solref, solimp, friction, g, -1, -1, f); + mj_contactParam(m, &condim, solref, solimp, friction, g, -1, -1, f); // allocate temporary contact array on stack (zero-initialized) mj_markStack(d); @@ -1865,7 +1872,7 @@ void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) { memset(con, 0, mjMAXCONPAIR * sizeof(mjContact)); // call batched flex-SDF collision - int num = mjc_FlexSDF(m, d, con, g, f, margin); + int num = mjc_FlexSDF(m, d, con, g, f, margin + gap); // add contacts to mjData for (int i = 0; i < num; i++) { @@ -1880,7 +1887,7 @@ void mj_collideSdfFlex(const mjModel* m, mjData* d, int g, int f) { con[i].vert[1] = -1; // set remaining contact parameters - mj_setContact(m, con + i, condim, margin-gap, solref, solreffriction, solimp, friction); + mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction); // add to mjData, abort if too many contacts if (mj_addContact(m, d, con + i)) { @@ -1949,9 +1956,9 @@ void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) { int condim; int flex_elemnum = m->flex_elemnum[f]; mjtNum radius = m->flex_radius[f]; - mjtNum gap, solref[mjNREF], solimp[mjNIMP], friction[5]; + mjtNum solref[mjNREF], solimp[mjNIMP], friction[5]; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, &gap, solref, solimp, friction, -1, -1, f, f); + mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f, f); condim = 1; // process all elements @@ -2048,13 +2055,14 @@ void mj_collideFlexSAP(const mjModel* m, mjData* d, int f) { // test a geom and an elem for collision, add to contact list void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { mjtNum margin = mj_assignMargin(m, m->geom_margin[g] + m->flex_margin[f]); + mjtNum gap = m->geom_gap[g] + m->flex_gap[f]; int dim = m->flex_dim[f], type = m->geom_type[g]; int num; // bounding sphere test: only if midphase is disabled if (mjDISABLED(mjDSBL_MIDPHASE)) { int eglobal = m->flex_elemadr[f] + e; - if (filterSphereBox(d->geom_xpos+3*g, m->geom_rbound[g]+margin, + if (filterSphereBox(d->geom_xpos+3*g, m->geom_rbound[g]+margin+gap, d->flexelem_aabb+6*eglobal)) { return; } @@ -2087,17 +2095,17 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { // call raw primitive for corresponding geom type if (type == mjGEOM_SPHERE) { - num = mjraw_SphereCapsule(con, margin, + num = mjraw_SphereCapsule(con, margin + gap, d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, pos, mat, size); } else if (type == mjGEOM_CAPSULE) { - num = mjraw_CapsuleCapsule(con, margin, + num = mjraw_CapsuleCapsule(con, margin + gap, d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, pos, mat, size); } else { - num = mjraw_CapsuleBox(con, margin, + num = mjraw_CapsuleBox(con, margin + gap, pos, mat, size, d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g); @@ -2110,13 +2118,13 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { // heightfield : elem else if (type == mjGEOM_HFIELD) { - num = mjc_HFieldElem(m, d, con, g, f, e, margin); + num = mjc_HFieldElem(m, d, con, g, f, e, margin + gap); } // sphere : triangle else if (type == mjGEOM_SPHERE && dim == 2) { const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f]; - num = mjraw_SphereTriangle(con, margin, + num = mjraw_SphereTriangle(con, margin + gap, d->geom_xpos+3*g, m->geom_size[3*g], vertxpos + 3*edata[0], vertxpos + 3*edata[1], vertxpos + 3*edata[2], m->flex_radius[f]); @@ -2125,7 +2133,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { // box : triangle else if (type == mjGEOM_BOX && dim == 2) { const mjtNum* vertxpos = d->flexvert_xpos + 3 * m->flex_vertadr[f]; - num = mjraw_BoxTriangle(con, margin, d->geom_xpos + 3 * g, + num = mjraw_BoxTriangle(con, margin + gap, d->geom_xpos + 3 * g, d->geom_xmat + 9 * g, m->geom_size + 3 * g, vertxpos + 3 * edata[0], vertxpos + 3 * edata[1], vertxpos + 3 * edata[2], m->flex_radius[f]); @@ -2135,14 +2143,14 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { else if (type == mjGEOM_CAPSULE && dim == 2) { const mjtNum* vertxpos = d->flexvert_xpos + 3 * m->flex_vertadr[f]; num = mjraw_CapsuleTriangle( - con, margin, d->geom_xpos + 3 * g, d->geom_xmat + 9 * g, + con, margin + gap, d->geom_xpos + 3 * g, d->geom_xmat + 9 * g, m->geom_size + 3 * g, vertxpos + 3 * edata[0], vertxpos + 3 * edata[1], vertxpos + 3 * edata[2], m->flex_radius[f]); } // general geom : elem else { - num = mjc_ConvexElem(m, d, con, g, -1, -1, -1, f, e, margin); + num = mjc_ConvexElem(m, d, con, g, -1, -1, -1, f, e, margin + gap); } // check contacts @@ -2153,9 +2161,9 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { // get contact parameters int condim; - mjtNum gap, friction[5], solref[mjNREF], solimp[mjNIMP]; + mjtNum friction[5], solref[mjNREF], solimp[mjNIMP]; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, &gap, solref, solimp, friction, g, -1, -1, f); + mj_contactParam(m, &condim, solref, solimp, friction, g, -1, -1, f); // add contacts for (int i=0; i < num; i++) { @@ -2170,7 +2178,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { con[i].vert[1] = -1; // set remaining contact parameters - mj_setContact(m, con + i, condim, margin-gap, solref, solreffriction, solimp, friction); + mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction); } // add to ncon @@ -2184,17 +2192,19 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) { // test two elems for collision, add to contact list void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2) { mjtNum margin = mj_assignMargin(m, m->flex_margin[f1] + m->flex_margin[f2]); + mjtNum gap = m->flex_gap[f1] + m->flex_gap[f2]; int dim1 = m->flex_dim[f1], dim2 = m->flex_dim[f2]; int num; - // ignore margin in self-collisions + // ignore margin and gap in self-collisions if (f1 == f2) { margin = 0; + gap = 0; } // bounding box filter (not applied in midphase) if (filterBox(d->flexelem_aabb+6*(m->flex_elemadr[f1]+e1), - d->flexelem_aabb+6*(m->flex_elemadr[f2]+e2), margin)) { + d->flexelem_aabb+6*(m->flex_elemadr[f2]+e2), margin + gap)) { return; } @@ -2231,12 +2241,12 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2 pos2, mat2, size2); // raw primitive - num = mjraw_CapsuleCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2); + num = mjraw_CapsuleCapsule(con, margin + gap, pos1, mat1, size1, pos2, mat2, size2); } // general convex collision else { - num = mjc_ConvexElem(m, d, con, -1, f1, e1, -1, f2, e2, margin); + num = mjc_ConvexElem(m, d, con, -1, f1, e1, -1, f2, e2, margin + gap); } // check contacts @@ -2247,14 +2257,9 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2 // get contact parameters int condim; - mjtNum gap, friction[5], solref[mjNREF], solimp[mjNIMP]; + mjtNum friction[5], solref[mjNREF], solimp[mjNIMP]; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, &gap, solref, solimp, friction, -1, -1, f1, f2); - - // ignore gap in self collision, since margin is ignored - if (f1 == f2) { - gap = 0; - } + mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f1, f2); // add contacts for (int i=0; i < num; i++) { @@ -2269,7 +2274,7 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2 con[i].vert[1] = -1; // set remaining contact parameters - mj_setContact(m, con + i, condim, margin-gap, solref, solreffriction, solimp, friction); + mj_setContact(m, con + i, condim, margin, solref, solreffriction, solimp, friction); } // add to ncon @@ -2336,9 +2341,9 @@ void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) { // get contact parameters int condim; - mjtNum gap, friction[5], solref[mjNREF], solimp[mjNIMP]; + mjtNum friction[5], solref[mjNREF], solimp[mjNIMP]; mjtNum solreffriction[mjNREF] = {0}; - mj_contactParam(m, &condim, &gap, solref, solimp, friction, -1, -1, f, f); + mj_contactParam(m, &condim, solref, solimp, friction, -1, -1, f, f); // add contacts for (int i=0; i < num; i++) { diff --git a/src/experimental/platform/ux/gui_spec.cc b/src/experimental/platform/ux/gui_spec.cc index d845ab09..a723cbfe 100644 --- a/src/experimental/platform/ux/gui_spec.cc +++ b/src/experimental/platform/ux/gui_spec.cc @@ -421,7 +421,7 @@ void ElementSpecGui(mjsElement* element, SpecEditor* editor) { FIELD(solref, "solver reference"); FIELD(solimp, "solver impedance"); FIELD(margin, "margin for contact detection"); - FIELD(gap, "include in solver if dist < margin-gap"); + FIELD(gap, "additional contact detection buffer"); FIELD(mass, "used to compute density"); FIELD(density, "used to compute mass and inertia from volume or surface"); FIELD(typeinertia, "selects between surface and volume inertia"); @@ -530,7 +530,7 @@ void ElementSpecGui(mjsElement* element, SpecEditor* editor) { FIELD(solref, "solver reference"); FIELD(solimp, "solver impedance"); FIELD(margin, "margin for contact detection"); - FIELD(gap, "include in solver if distncon, 1); - EXPECT_LT(data->contact[0].efc_address, 0); + EXPECT_GE(data->contact[0].efc_address, 0); mj_deleteData(data); mj_deleteModel(model); diff --git a/test/engine/testdata/collision_box/boxbox_bad0.xml b/test/engine/testdata/collision_box/boxbox_bad0.xml index a3e08973..8d436c6f 100644 --- a/test/engine/testdata/collision_box/boxbox_bad0.xml +++ b/test/engine/testdata/collision_box/boxbox_bad0.xml @@ -2,7 +2,7 @@ - + diff --git a/test/engine/testdata/collision_primitive/sphere_cylinder.xml b/test/engine/testdata/collision_primitive/sphere_cylinder.xml index 3023d060..0e17f2dd 100644 --- a/test/engine/testdata/collision_primitive/sphere_cylinder.xml +++ b/test/engine/testdata/collision_primitive/sphere_cylinder.xml @@ -16,7 +16,7 @@ - + diff --git a/test/engine/testdata/solver/model.xml b/test/engine/testdata/solver/model.xml index 82bc2906..1f98808d 100644 --- a/test/engine/testdata/solver/model.xml +++ b/test/engine/testdata/solver/model.xml @@ -43,7 +43,7 @@ - + diff --git a/test/testdata/model.xml b/test/testdata/model.xml index 9bd11249..595eaae4 100644 --- a/test/testdata/model.xml +++ b/test/testdata/model.xml @@ -43,7 +43,7 @@ - + From 99e626096f2280faeaeeba41ac29636bb1eef62b Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Tue, 12 May 2026 15:50:48 -0700 Subject: [PATCH 248/251] Fix miniz include. PiperOrigin-RevId: 914519929 Change-Id: I0dc00ef3f4518571165e4be0e85016511e0cdd60 --- cmake/MujocoDependencies.cmake | 20 ++++++++++++++++---- src/xml/mjz/mjz_decoder.cc | 15 ++++++++++++++- 2 files changed, 30 insertions(+), 5 deletions(-) diff --git a/cmake/MujocoDependencies.cmake b/cmake/MujocoDependencies.cmake index eaf8e51b..a6a0a4ca 100644 --- a/cmake/MujocoDependencies.cmake +++ b/cmake/MujocoDependencies.cmake @@ -247,13 +247,25 @@ if(WIN32) endif() endif() +if(DEFINED BUILD_TESTS) + set(_OLD_BUILD_TESTS "${BUILD_TESTS}") + set(_BUILD_TESTS_WAS_DEFINED TRUE) +else() + set(_BUILD_TESTS_WAS_DEFINED FALSE) +endif() set(BUILD_TESTS OFF) fetchpackage( - PACKAGE_NAME miniz - GIT_REPO https://github.com/richgel999/miniz.git - GIT_TAG ${MUJOCO_DEP_VERSION_miniz} - TARGETS miniz + PACKAGE_NAME miniz + GIT_REPO https://github.com/richgel999/miniz.git + GIT_TAG ${MUJOCO_DEP_VERSION_miniz} + TARGETS miniz ) +if(_BUILD_TESTS_WAS_DEFINED) + set(BUILD_TESTS "${_OLD_BUILD_TESTS}") +else() + unset(BUILD_TESTS) +endif() +unset(_BUILD_TESTS_WAS_DEFINED) if(MUJOCO_BUILD_TESTS OR MUJOCO_BUILD_STUDIO OR MUJOCO_USE_FILAMENT) diff --git a/src/xml/mjz/mjz_decoder.cc b/src/xml/mjz/mjz_decoder.cc index 12fc0ba5..f2864518 100644 --- a/src/xml/mjz/mjz_decoder.cc +++ b/src/xml/mjz/mjz_decoder.cc @@ -24,7 +24,20 @@ #include #include -#include +#if defined(__clang__) +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wunused-function" +#elif defined(__GNUC__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-function" +#endif +#include +#if defined(__clang__) +#pragma clang diagnostic pop +#elif defined(__GNUC__) +#pragma GCC diagnostic pop +#endif + #include #include #include "user/user_resource.h" From 7bfdbad80be9a3cc4d2bd3b782b6923814d7fff8 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 13 May 2026 02:33:50 -0700 Subject: [PATCH 249/251] Make PGS solver constraint visitation order time-independent. PiperOrigin-RevId: 914741347 Change-Id: I7889a42543e9a9f3ff883e25ee9eafaceedc29e1 --- doc/changelog.rst | 6 ++++++ src/engine/engine_solver.c | 7 +------ 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 437cdac0..5261a0b3 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -5,6 +5,12 @@ Changelog Upcoming version (not yet released) ----------------------------------- +General +^^^^^^^ +- The pseudo-random constraint visitation order in the :ref:`PGS solver`, introduced in the previous + release, now uses a fixed seed. The previous implementation seeded with ``mjData.time``, which introduced subtle yet + undesirable time dependence. + .. admonition:: Breaking API changes :class: attention diff --git a/src/engine/engine_solver.c b/src/engine/engine_solver.c index cd44350d..dd18ae55 100644 --- a/src/engine/engine_solver.c +++ b/src/engine/engine_solver.c @@ -444,15 +444,10 @@ static void solPGS(const mjModel* m, mjData* d, int island, } } - // seed PCG32 RNG from simulation time + // seed PCG32 RNG with a fixed seed pcg32_state rng; - uint64_t seed = 0; - memcpy(&seed, &d->time, sizeof(d->time)); rng.state = 0; rng.inc = 1; - rng.state = seed; - pcg32_next(&rng); - rng.state += seed; pcg32_next(&rng); // main iteration From 35cdc779e640ce9e7bda81c4c30c2cfad197d541 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 13 May 2026 03:57:41 -0700 Subject: [PATCH 250/251] Add implicit bending stiffness for standard flex. Standard flex (flex_interp=0) with thin-plate bending treated bending forces purely explicitly. This caused contact-induced vertex vibrations and non-physical energy injection for flat resting sheets, because the solver treated each vertex as an independent mass during contact and contact normals are orthogonal to stretch constraints. Fix: extend the existing preconditioned CG solver to include the constant bending stiffness K_bend in the implicit operator via matrix-free mat-vec. PiperOrigin-RevId: 914774020 Change-Id: I45e0d6749abb6f873566203bccae956514b2576b --- model/flex/poncho.xml | 4 +- model/flex/poncho_edgeequality.xml | 4 +- src/engine/engine_derivative.c | 66 ++++++++++++-- src/engine/engine_derivative.h | 15 ++- src/engine/engine_forward.c | 40 +++++--- test/engine/engine_derivative_test.cc | 4 +- test/engine/engine_forward_test.cc | 126 +++++++++++++++++++++++++- 7 files changed, 221 insertions(+), 38 deletions(-) diff --git a/model/flex/poncho.xml b/model/flex/poncho.xml index ffa8aa28..72acc750 100644 --- a/model/flex/poncho.xml +++ b/model/flex/poncho.xml @@ -15,7 +15,7 @@ - @@ -1414,7 +1414,7 @@ 398 399 418 398 376 378"> - + diff --git a/model/flex/poncho_edgeequality.xml b/model/flex/poncho_edgeequality.xml index d14378df..20ffecc2 100644 --- a/model/flex/poncho_edgeequality.xml +++ b/model/flex/poncho_edgeequality.xml @@ -15,7 +15,7 @@ - @@ -1414,7 +1414,7 @@ 398 399 418 398 376 378"> - + diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 85f91e33..34625963 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -1127,25 +1127,71 @@ static void mjd_flexInterp_kernel(const mjModel* m, mjData* d, mjtFlexOp op, -// compute res += (h^2 + h*damping) * J'*K*J * vec, for all interpolated flexes -void mjd_flexInterp_mulKD(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h) { - // s1=h*h, s2=h => scale = h*h + h*damping - mjd_flexInterp_kernel(m, d, mjFLEXOP_VEC, res, vec, h * h, h, NULL, 0, 0); +// compute res += (s1 + s2*damping) * J'*K*J * vec, for all interpolated flexes +void mjd_flexInterp_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, + mjtNum s1, mjtNum s2) { + mjd_flexInterp_kernel(m, d, mjFLEXOP_VEC, res, vec, s1, s2, NULL, 0, 0); } -// compute res += h * J'*K*J * vec, for all interpolated flexes (stiffness only, no damping) -void mjd_flexInterp_mulK(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h) { - // s1=h, s2=0 => scale = h (no damping contribution) - mjd_flexInterp_kernel(m, d, mjFLEXOP_VEC, res, vec, h, 0, NULL, 0, 0); + +// compute res += scale * K_bend * vec for standard (non-interp) flex bending +// scale = s1 + s2 * flex_damping[f] per flex +// for stiffness+damping: s1=h^2, s2=h => scale = h^2 + h*damping +// for stiffness only: s1=h, s2=0 => scale = h +void mjd_flexBend_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, + mjtNum s1, mjtNum s2) { + for (int f = 0; f < m->nflex; f++) { + // skip interp, rigid, or non-2D + if (m->flex_interp[f] || m->flex_rigid[f] || m->flex_dim[f] != 2) { + continue; + } + + int bendingadr = m->flex_bendingadr[f]; + if (bendingadr < 0) { + continue; + } + + mjtNum scale = s1 + s2 * m->flex_damping[f]; + if (!scale) { + continue; + } + + const mjtNum* b = m->flex_bending + bendingadr; + const int* bodyid = m->flex_vertbodyid + m->flex_vertadr[f]; + int edgenum = m->flex_edgenum[f]; + int edgeadr = m->flex_edgeadr[f]; + + for (int e = 0; e < edgenum; e++) { + const int* edge = m->flex_edge + 2*(e + edgeadr); + const int* flap = m->flex_edgeflap + 2*(e + edgeadr); + int v[4] = {edge[0], edge[1], flap[0], flap[1]}; + + // skip boundary edges (no second flap vertex) + if (v[3] == -1) { + continue; + } + + // apply 4x4 bending stencil, coordinate-wise + for (int i = 0; i < 4; i++) { + int dof_i = m->body_dofadr[bodyid[v[i]]]; + for (int x = 0; x < 3; x++) { + mjtNum val = 0; + for (int j = 0; j < 4; j++) { + int dof_j = m->body_dofadr[bodyid[v[j]]]; + val += b[17*e + 4*i + j] * vec[dof_j + x]; + } + res[dof_i + x] += scale * val; + } + } + } + } } - - // add (d qfrc_actuator / d qvel) to qDeriv void mjd_actuator_vel(const mjModel* m, mjData* d) { int nu = m->nu; diff --git a/src/engine/engine_derivative.h b/src/engine/engine_derivative.h index 1ddb3af9..09a5e659 100644 --- a/src/engine/engine_derivative.h +++ b/src/engine/engine_derivative.h @@ -43,15 +43,14 @@ MJAPI void mjd_passive_vel(const mjModel* m, mjData* d); // subtract (d qfrc_bias / d qvel) from qDeriv (dense version) MJAPI void mjd_rne_vel_dense(const mjModel* m, mjData* d); -// derivative of flex_interp generalized force w.r.t position: res = (d qfrc_flexinterp / d qpos) * vec -// res and vec are vectors of size m->nv -MJAPI void mjd_flexInterp_mulKD(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h); - -// derivative of flex_interp generalized force w.r.t position (stiffness only, no damping) -MJAPI void mjd_flexInterp_mulK(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, mjtNum h); - - +// compute res += (s1 + s2*damping) * J'*K*J * vec, for all interpolated flexes +MJAPI void mjd_flexInterp_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, + mjtNum s1, mjtNum s2); +// compute res += scale * K_bend * vec for standard (non-interp) flex bending +// scale = s1 + s2 * flex_damping[f] per flex +MJAPI void mjd_flexBend_mul(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, + mjtNum s1, mjtNum s2); #ifdef __cplusplus diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 5e285f39..61171337 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -1371,14 +1371,25 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) { } -// return 1 if any flex needs implicit interp treatment -static int flexInterp_has_active(const mjModel* m) { +// return 1 if any flex needs implicit stiffness treatment (interp or bending) +static int flex_has_implicit_stiffness(const mjModel* m) { for (int f=0; f < m->nflex; f++) { - if (m->flex_interp[f] && !m->flex_rigid[f] && + if (m->flex_rigid[f]) { + continue; + } + + // interpolated flex with stiffness + if (m->flex_interp[f] && m->flex_edgeequality[f] != 3 && m->flex_stiffness[m->flex_stiffnessadr[f]] != 0) { return 1; } + + // standard flex with bending + if (!m->flex_interp[f] && m->flex_dim[f] == 2 && + m->flex_bendingadr[f] >= 0) { + return 1; + } } return 0; } @@ -1403,24 +1414,27 @@ static void flexInterp_cgsolve(const mjModel* m, mjData* d, mjtNum* Ap = mjSTACKALLOC(d, nv, mjtNum); mjtNum* temp = mjSTACKALLOC(d, nv, mjtNum); - // build RHS: rhs = qfrc - h*K*qvel (velocity correction from flex stiffness) + // build RHS: rhs = qfrc mju_copy(rhs, qfrc, nv); + + // flex_interp velocity correction: rhs -= h*K_interp*qvel mju_zero(temp, nv); - mjd_flexInterp_mulK(m, d, temp, d->qvel, h); // temp = h*K*v (stiffness only) - mju_addToScl(rhs, temp, -1.0, nv); // rhs -= h*K*v + mjd_flexInterp_mul(m, d, temp, d->qvel, h, 0); // temp = h*K_interp*v + mju_addToScl(rhs, temp, -1.0, nv); // rhs -= h*K_interp*v + + // standard flex bending velocity correction: rhs -= h*K_bend*qvel + mjd_flexBend_mul(m, d, rhs, d->qvel, -h, 0); // rhs -= h*K_bend*v // --- helper lambda-style inline: compute Ap = A*x --- - // A*x = (M - h*qDeriv)*x - (h^2+h*d)*K*x + // A*x = (M - h*qDeriv)*x - (h^2+h*d)*K_interp*x + (h^2+h*d)*K_bend*x #define FLEX_CG_MATVEC(Ap_out, x_in) \ mju_mulMatVecSparse(Ap_out, d->qDeriv, x_in, nv, m->D_rownnz, m->D_rowadr, \ m->D_colind, NULL); \ - mju_zero(temp, nv); \ mju_mulSymVecSparse(temp, d->M, x_in, nv, m->M_rownnz, m->M_rowadr, \ m->M_colind); \ mju_addScl(Ap_out, temp, Ap_out, -h, nv); \ - mju_zero(temp, nv); \ - mjd_flexInterp_mulKD(m, d, temp, x_in, h); \ - mju_addToScl(Ap_out, temp, -1.0, nv) + mjd_flexInterp_mul(m, d, Ap_out, x_in, -(h*h), -h); \ + mjd_flexBend_mul(m, d, Ap_out, x_in, h*h, h) // --- helper: preconditioner solve z = (M - h*qDeriv)^{-1} * r --- #define FLEX_CG_PRECOND(z_out, r_in) \ @@ -1857,7 +1871,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { } // check for flex_interp that needs implicit treatment - int has_flex_interp = !sleep_filter && flexInterp_has_active(m); + int has_flex_stiffness = !sleep_filter && flex_has_implicit_stiffness(m); // factorization if (!skipfactor) { @@ -1911,7 +1925,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) { } // flex: CG correction for implicit flex stiffness - if (has_flex_interp) { + if (has_flex_stiffness) { flexInterp_cgsolve(m, d, qacc, qfrc, m->nv); } diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index fb9d777f..0b3be449 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -1474,7 +1474,7 @@ void RotateFlexGrid(mjModel* model, mjData* data, const char* flex_name, } // Helper: assemble flex stiffness into dense matrix via matrix-vector products. -// Builds K column-by-column using mjd_flexInterp_mulKD. +// Builds K column-by-column using mjd_flexInterp_mul. // Result is -(h^2 + h*damping) * J'KJ (negative sign matches the old addH // convention where stiffness is subtracted from the system matrix). static void mulKD_dense(mjModel* m, mjData* d, mjtNum* H_dense, @@ -1485,7 +1485,7 @@ static void mulKD_dense(mjModel* m, mjData* d, mjtNum* H_dense, mju_zero(e_i.data(), nv); mju_zero(col.data(), nv); e_i[i] = 1.0; - mjd_flexInterp_mulKD(m, d, col.data(), e_i.data(), h); + mjd_flexInterp_mul(m, d, col.data(), e_i.data(), h * h, h); // col = +(h^2 + h*damp)*K*e_i, negate to match addH convention (H -= K) for (int j = 0; j < nv; j++) { H_dense[j * nv + i] = -col[j]; diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index dddf612b..9566abaf 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -3147,7 +3147,7 @@ TEST_F(ForwardTest, FlexTrilinearInstability) { // using mulKD for legacy check consistency, but we know it applies h^2+h*d // scaling; actually, let's stick to the high-level property checks from // FlexStiffnessSign which used mulKD - mjd_flexInterp_mulKD(model, data, flex_Kv.data(), v.data(), h); + mjd_flexInterp_mul(model, data, flex_Kv.data(), v.data(), h * h, h); // compute v^T*M*v and v^T*scale*K*v mjtNum vMv = mju_dot(v.data(), Mv.data(), nv); @@ -3836,5 +3836,129 @@ TEST_F(ActuatorDampingTest, DampingVsKvGearScaling) { mj_deleteModel(m); } +// flex sheet dropping on a plane should not gain energy from implicit bending +TEST_F(ImplicitIntegratorTest, FlexContactEnergy) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + ASSERT_EQ(m->nflex, 1); + + mjData* d = mj_makeData(m); + + // compute initial energy + mj_forward(m, d); + mjtNum initial_energy = d->energy[0] + d->energy[1]; + ASSERT_GT(initial_energy, 0); + + // simulate + mjtNum max_energy = initial_energy; + int max_energy_step = 0; + int nsteps = 500; + for (int i = 0; i < nsteps; i++) { + mj_step(m, d); + mjtNum total_energy = d->energy[0] + d->energy[1]; + if (total_energy > max_energy) { + max_energy = total_energy; + max_energy_step = i + 1; + } + } + + mjtNum energy_ratio = max_energy / initial_energy; + + EXPECT_LE(energy_ratio, 1.01) + << "contact solver injected energy: max_energy/initial_energy = " + << energy_ratio << " (max at step " << max_energy_step << ")" + << "\n initial_energy = " << initial_energy + << "\n max_energy = " << max_energy; + + mj_deleteData(d); + mj_deleteModel(m); +} + +// bending damping on a flat flex must dissipate energy with implicit integrator +TEST_F(ImplicitIntegratorTest, BendingDampingDecaysEnergy) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + + char error[1024] = {0}; + mjModel* m = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(m, NotNull()) << error; + ASSERT_EQ(m->nflex, 1); + ASSERT_GT(m->flex_damping[0], 0) << "flex_damping not set"; + + mjData* d = mj_makeData(m); + + // perturb a central vertex with upward velocity + // vertex layout is 6x6 grid; pick a central vertex (row=3, col=3 -> id=21) + int center_vert = 21; + int bid = m->flex_vertbodyid[m->flex_vertadr[0] + center_vert]; + int dofadr = m->body_dofadr[bid]; + d->qvel[dofadr + 2] = 1.0; // z-velocity + + // initial forward to compute energy + mj_forward(m, d); + mjtNum initial_energy = d->energy[0] + d->energy[1]; + ASSERT_GT(initial_energy, 0) << "initial energy should be nonzero"; + + // step forward and check energy decay + mjtNum max_energy = initial_energy; + int nsteps = 100; + for (int i = 0; i < nsteps; i++) { + mj_step(m, d); + mjtNum total_energy = d->energy[0] + d->energy[1]; + max_energy = mju_max(max_energy, total_energy); + } + + // energy must never exceed initial (system must not go unstable) + EXPECT_LE(max_energy, initial_energy * 1.01) + << "energy exceeded initial by more than 1%: max=" << max_energy + << ", initial=" << initial_energy; + + // after 100 steps (0.1 seconds), energy should have decayed significantly + mjtNum final_energy = d->energy[0] + d->energy[1]; + EXPECT_LT(final_energy, 0.5 * initial_energy) + << "energy did not decay by at least 50% after " << nsteps << " steps" + << " (initial=" << initial_energy << ", final=" << final_energy << ")"; + + mj_deleteData(d); + mj_deleteModel(m); +} + } // namespace } // namespace mujoco From 69a1087e9ee6d6db1f33b1d32b3d3f69dbe38791 Mon Sep 17 00:00:00 2001 From: Haroon Qureshi Date: Wed, 13 May 2026 03:59:17 -0700 Subject: [PATCH 251/251] Disable window resizing on load as it breaks on some platforms. PiperOrigin-RevId: 914774489 Change-Id: I9aea71b386de93f4d2fc3da5310dd7a3018c8c1c --- src/experimental/studio/app.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/experimental/studio/app.cc b/src/experimental/studio/app.cc index ece64f22..48b038f3 100644 --- a/src/experimental/studio/app.cc +++ b/src/experimental/studio/app.cc @@ -105,7 +105,8 @@ void App::SwitchGraphicsMode(int width, int height, renderer_ = std::make_unique( window_->GetNativeWindowHandle(), gfx_mode_); - LoadSettings(); + // TODO: Figure out why this breaks on some platforms. + // LoadSettings(); if (ui_.window_width > 0 && ui_.window_height > 0) { window_->Resize(ui_.window_width, ui_.window_height); }