-
Notifications
You must be signed in to change notification settings - Fork 232
SolverMuJoCo: custom attributes for MJCF/MJC schema options #1211
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
base: main
Are you sure you want to change the base?
Changes from 10 commits
678d66e
d8876d3
aecba1b
77fb71c
c0a0a84
3eefc67
d0950c4
96fd15d
4949342
215861b
7de0ec8
ac634a2
1fbff89
32e934b
5ddc2d3
45005e2
f17e568
1ec7f74
1f68ab8
32d68c1
ff87480
61ef331
63534cf
0ef28e0
1cbe491
c3c61de
0ea8f4d
905a81b
993e8d8
11dd497
95a18dc
58af13f
a7a8279
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -64,6 +64,7 @@ | |
| update_mocap_transforms_kernel, | ||
| update_model_properties_kernel, | ||
| update_shape_mappings_kernel, | ||
| update_solver_options_kernel, | ||
| ) | ||
|
|
||
| if TYPE_CHECKING: | ||
|
|
@@ -285,6 +286,18 @@ def register_custom_attributes(cls, builder: ModelBuilder) -> None: | |
| mjcf_attribute_name="actuatorgravcomp", | ||
| ) | ||
| ) | ||
| # Solver options (frequency WORLD for per-world values) | ||
| builder.add_custom_attribute( | ||
| ModelBuilder.CustomAttribute( | ||
| name="impratio", | ||
| frequency=ModelAttributeFrequency.WORLD, | ||
| assignment=ModelAttributeAssignment.MODEL, | ||
| dtype=wp.float32, | ||
| default=1.0, | ||
| namespace="mujoco", | ||
| usd_attribute_name="mjc:option:impratio", | ||
| ) | ||
| ) | ||
|
|
||
| def __init__( | ||
| self, | ||
|
|
@@ -300,7 +313,7 @@ def __init__( | |
| solver: int | str = "cg", | ||
| integrator: int | str = "implicitfast", | ||
| cone: int | str = "pyramidal", | ||
| impratio: float = 1.0, | ||
| impratio: float | None = None, | ||
| use_mujoco_cpu: bool = False, | ||
| disable_contacts: bool = False, | ||
| default_actuator_gear: float | None = None, | ||
|
|
@@ -326,7 +339,7 @@ def __init__( | |
| solver (int | str): Solver type. Can be "cg" or "newton", or their corresponding MuJoCo integer constants. | ||
| integrator (int | str): Integrator type. Can be "euler", "rk4", or "implicitfast", or their corresponding MuJoCo integer constants. | ||
| cone (int | str): The type of contact friction cone. Can be "pyramidal", "elliptic", or their corresponding MuJoCo integer constants. | ||
| impratio (float): Frictional-to-normal constraint impedance ratio. | ||
| impratio (float | None): Frictional-to-normal constraint impedance ratio. If None, uses the value from model custom attribute ``mujoco:impratio`` or MuJoCo's default (1.0). | ||
| use_mujoco_cpu (bool): If True, use the MuJoCo-C CPU backend instead of `mujoco_warp`. | ||
| disable_contacts (bool): If True, disable contact computation in MuJoCo. | ||
| register_collision_groups (bool): If True, register collision groups from the Newton model in MuJoCo. | ||
|
|
@@ -894,7 +907,7 @@ def _convert_to_mjc( | |
| integrator: int | str = "implicitfast", | ||
| disableflags: int = 0, | ||
| disable_contacts: bool = False, | ||
| impratio: float = 1.0, | ||
| impratio: float | None = None, | ||
| tolerance: float = 1e-6, | ||
| ls_tolerance: float = 0.01, | ||
| cone: int | str = "pyramidal", | ||
|
|
@@ -996,6 +1009,14 @@ def fill_arr_from_dict(arr: nparray, d: dict[int, Any]): | |
| else: | ||
| arr[tuple(keys.T)] = vals | ||
|
|
||
| # Resolve solver options: constructor arg > custom attribute > MuJoCo default | ||
| # Track if overridden (tile() handles expansion) vs needs per-world update from Newton model | ||
| impratio_overridden = impratio is not None | ||
| if impratio is None: | ||
| mujoco_attrs = getattr(model, "mujoco", None) | ||
| if mujoco_attrs and hasattr(mujoco_attrs, "impratio"): | ||
| impratio = float(mujoco_attrs.impratio.numpy()[0]) | ||
|
|
||
| spec = mujoco.MjSpec() | ||
| spec.option.disableflags = disableflags | ||
| spec.option.gravity = np.array([*model.gravity.numpy()[0]]) | ||
|
|
@@ -1004,7 +1025,8 @@ def fill_arr_from_dict(arr: nparray, d: dict[int, Any]): | |
| spec.option.iterations = iterations | ||
| spec.option.ls_iterations = ls_iterations | ||
| spec.option.cone = cone | ||
| spec.option.impratio = impratio | ||
| if impratio is not None: | ||
| spec.option.impratio = impratio | ||
| spec.option.tolerance = tolerance | ||
| spec.option.ls_tolerance = ls_tolerance | ||
| spec.option.jacobian = mujoco.mjtJacobian.mjJAC_AUTO | ||
|
|
@@ -1905,6 +1927,9 @@ def add_geoms(newton_body_id: int): | |
| # expand model fields that can be expanded: | ||
| self.expand_model_fields(self.mjw_model, nworld) | ||
|
|
||
| # update solver options from Newton model (only if not overridden by constructor) | ||
| self.update_solver_options(impratio_overridden=impratio_overridden) | ||
|
||
|
|
||
coderabbitai[bot] marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| # so far we have only defined the first world, | ||
| # now complete the data from the Newton model | ||
| self.notify_model_changed(SolverNotifyFlags.ALL) | ||
|
|
@@ -1987,6 +2012,20 @@ def expand_model_fields(self, mj_model: MjWarpModel, nworld: int): | |
| # "mat_rgba", | ||
| } | ||
|
|
||
| # Solver option fields to expand (nested in mj_model.opt) | ||
| opt_fields_to_expand = { | ||
| # "timestep", | ||
| "impratio", | ||
| # "tolerance", | ||
| # "ls_tolerance", | ||
| # "ccd_tolerance", | ||
| # "density", | ||
| # "viscosity", | ||
| # "gravity", | ||
| # "wind", | ||
| # "magnetic", | ||
| } | ||
|
|
||
| def tile(x: wp.array): | ||
| # Create new array with same shape but first dim multiplied by nworld | ||
| new_shape = list(x.shape) | ||
|
|
@@ -2014,6 +2053,41 @@ def tile(x: wp.array): | |
| array = getattr(mj_model, field) | ||
| setattr(mj_model, field, tile(array)) | ||
|
|
||
| for field in opt_fields_to_expand: | ||
| if hasattr(mj_model.opt, field): | ||
| array = getattr(mj_model.opt, field) | ||
| setattr(mj_model.opt, field, tile(array)) | ||
|
|
||
| def update_solver_options(self, impratio_overridden: bool = False): | ||
eric-heiden marked this conversation as resolved.
Outdated
Show resolved
Hide resolved
|
||
| """Update solver options from Newton model to MuJoCo Warp. | ||
|
|
||
| Copies per-world values from Newton custom attributes to the MuJoCo Warp model. | ||
| If a value was overridden by constructor, tile() already handled expansion so we skip it. | ||
|
|
||
| Args: | ||
| impratio_overridden: If True, impratio was set by constructor and tile() handled it. | ||
| """ | ||
| mujoco_attrs = getattr(self.model, "mujoco", None) | ||
| nworld = self.model.num_worlds | ||
|
|
||
| # Get Newton arrays - pass None if overridden or not available (kernel checks for None) | ||
| if not impratio_overridden and mujoco_attrs and hasattr(mujoco_attrs, "impratio"): | ||
| newton_impratio = mujoco_attrs.impratio | ||
| else: | ||
| newton_impratio = None | ||
|
|
||
| # Skip kernel if all options are None (add more checks here as options are added) | ||
| all_none = newton_impratio is None # and other_option is None and ... | ||
| if all_none: | ||
| return | ||
|
|
||
| wp.launch( | ||
| update_solver_options_kernel, | ||
| dim=nworld, | ||
| inputs=[newton_impratio], | ||
| outputs=[self.mjw_model.opt.impratio], | ||
| ) | ||
|
|
||
eric-heiden marked this conversation as resolved.
Outdated
Show resolved
Hide resolved
|
||
| def update_model_inertial_properties(self): | ||
| if self.model.body_count == 0: | ||
| return | ||
|
|
||
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -143,6 +143,18 @@ def parse_mjcf( | |
| else: | ||
| mesh_dir = "." | ||
|
|
||
| # Parse MJCF option tag for ONCE and WORLD frequency custom attributes (solver options) | ||
| # WORLD frequency attributes use index 0 here; they get remapped during add_world() | ||
| builder_custom_attr_option: list[ModelBuilder.CustomAttribute] = builder.get_custom_attributes_by_frequency( | ||
| [ModelAttributeFrequency.ONCE, ModelAttributeFrequency.WORLD] | ||
| ) | ||
| option_elem = root.find("option") | ||
| if option_elem is not None and builder_custom_attr_option: | ||
| option_attrs = parse_custom_attributes(option_elem.attrib, builder_custom_attr_option, "mjcf") | ||
| for key, value in option_attrs.items(): | ||
| if key in builder.custom_attributes: | ||
| builder.custom_attributes[key].values[0] = value | ||
|
||
|
|
||
| mesh_assets = {} | ||
| for asset in root.findall("asset"): | ||
| for mesh in asset.findall("mesh"): | ||
|
|
||
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -956,13 +956,21 @@ def define_joint_targets(dof, joint_desc): | |
| ) | ||
|
|
||
| if physics_scene_prim is not None: | ||
| # Extract custom attributes for model (ONCE) frequency from the PhysicsScene prim | ||
| # Extract custom attributes for model (ONCE and WORLD frequency) from the PhysicsScene prim | ||
| # WORLD frequency attributes use index 0 here; they get remapped during add_world() | ||
| builder_custom_attr_model: list[ModelBuilder.CustomAttribute] = [ | ||
| attr for attr in builder.custom_attributes.values() if attr.frequency == ModelAttributeFrequency.ONCE | ||
| attr | ||
|
Member
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. not sure if you are aware of the schema resolver capability (
Member
Author
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. What happens if a mjc:* option is not in the schema? Will it still be there? I think we still kind of need this code because there could be other custom attributes defined on the scene prim that are not part of the schema. That is the flexibility we get from the custom attribute system. I will change to use the schemaresolver for schema attributes though, thanks for the pointer. |
||
| for attr in builder.custom_attributes.values() | ||
| if attr.frequency in (ModelAttributeFrequency.ONCE, ModelAttributeFrequency.WORLD) | ||
| ] | ||
| scene_custom_attrs = usd.get_custom_attribute_values(physics_scene_prim, builder_custom_attr_model) | ||
| scene_attributes.update(scene_custom_attrs) | ||
|
|
||
| # Set values on builder's custom attributes (similar to MJCF parsing) | ||
| for key, value in scene_custom_attrs.items(): | ||
| if key in builder.custom_attributes: | ||
| builder.custom_attributes[key].values[0] = value | ||
|
|
||
| joint_descriptions = {} | ||
| # maps from joint prim path to joint index in builder | ||
| path_joint_map: dict[str, int] = {} | ||
|
|
||
Uh oh!
There was an error while loading. Please reload this page.