diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 112a3b957..9da3cad7a 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -96,6 +96,10 @@ jobs: MESA_LOADER_DRIVER_OVERRIDE: llvmpipe GALLIUM_DRIVER: llvmpipe ENVPOOL_SKIP_MUJOCO_RENDER_SMOKE: "1" + BAZEL_RELEASE_RESOURCE_OPTS: >- + ${{ matrix.arch == 'aarch64' + && '--jobs=2 --local_cpu_resources=2 --local_ram_resources=HOST_RAM*.5 --show_progress_rate_limit=60' + || '--jobs=2 --local_cpu_resources=2 --local_ram_resources=HOST_RAM*.5 --show_progress_rate_limit=60' }} strategy: fail-fast: false matrix: @@ -148,7 +152,7 @@ jobs: - name: Build run: | rm -rf dist wheelhouse - make PYPI_WHEEL_PLAT=${{ matrix.auditwheel-plat }} pypi-wheel + make BAZELOPT="$BAZEL_RELEASE_RESOURCE_OPTS" PYPI_WHEEL_PLAT=${{ matrix.auditwheel-plat }} pypi-wheel python -m pip install wheelhouse/*.whl --force-reinstall - name: Test run: | diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 3006d0c74..130bfc62c 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -64,8 +64,11 @@ jobs: echo "$HOME/go/bin" >> "$GITHUB_PATH" - name: Coverage run: | + myosuite_test_targets="//envpool/mujoco:myosuite_metadata_test //envpool/mujoco:myosuite_asset_smoke_test //envpool/mujoco:myosuite_myobase_test //envpool/mujoco:myosuite_myodm_test //envpool/mujoco:myosuite_myochallenge_test //envpool/mujoco:myosuite_render_doc_test //envpool/mujoco:myosuite_render_surface_test" make BAZELOPT="--repository_cache=$HOME/.cache/envpool-bazel-repo --disk_cache=$HOME/.cache/envpool-bazel-disk $BAZEL_CI_RESOURCE_OPTS" \ - BAZEL_TEST_TARGETS="//..." bazel-coverage + BAZEL_TEST_TARGETS="//... -//envpool/mujoco:myosuite_metadata_test -//envpool/mujoco:myosuite_asset_smoke_test -//envpool/mujoco:myosuite_myobase_test -//envpool/mujoco:myosuite_myodm_test -//envpool/mujoco:myosuite_myochallenge_test -//envpool/mujoco:myosuite_render_doc_test -//envpool/mujoco:myosuite_render_surface_test" bazel-coverage + make BAZELOPT="--repository_cache=$HOME/.cache/envpool-bazel-repo --disk_cache=$HOME/.cache/envpool-bazel-disk $BAZEL_CI_RESOURCE_OPTS" \ + BAZEL_TEST_TARGETS="$myosuite_test_targets" bazel-test mkdir -p coverage cp bazel-out/_coverage/_coverage_report.dat coverage/lcov.info python3 scripts/coverage_summary.py \ diff --git a/README.md b/README.md index d3e915f43..ed0eddf52 100644 --- a/README.md +++ b/README.md @@ -20,6 +20,7 @@ - [x] [Minigrid](https://envpool.readthedocs.io/en/latest/env/minigrid.html) - [x] [Highway](https://envpool.readthedocs.io/en/latest/env/highway.html) - [x] [MetaWorld](https://envpool.readthedocs.io/en/latest/env/metaworld.html) +- [x] [MyoSuite](https://envpool.readthedocs.io/en/latest/env/myosuite.html) Here are EnvPool's several highlights: diff --git a/docs/_static/render_samples/myosuite_official_compare.png b/docs/_static/render_samples/myosuite_official_compare.png new file mode 100644 index 000000000..37b3c5c31 Binary files /dev/null and b/docs/_static/render_samples/myosuite_official_compare.png differ diff --git a/docs/env/myosuite.rst b/docs/env/myosuite.rst new file mode 100644 index 000000000..ffd5b2ccb --- /dev/null +++ b/docs/env/myosuite.rst @@ -0,0 +1,84 @@ +MyoSuite +======== + +EnvPool provides native C++ implementations for the MyoSuite benchmark pinned +to ``MyoHub/myosuite`` commit ``05cb84678373f91271004f99602ebbf01e57d1a1`` +(``v2.11.6``). The public EnvPool IDs follow the official upstream registry +directly instead of introducing a parallel naming scheme. + +Supported Surface +----------------- + +The native port covers the full generated MyoSuite metadata surface: + +* 45 direct MyoBase task IDs +* 19 direct MyoChallenge task IDs +* 190 direct MyoDM TrackEnv task IDs +* 398 public task IDs after expanding the official fatigue, sarcopenia, and + reafferentation variants + +Representative public IDs include: + +.. code-block:: text + + myoHandReorientID-v0 + myoFatiHandReorientID-v0 + myoLegRoughTerrainWalk-v0 + myoChallengeBimanual-v0 + myoSarcChallengeBimanual-v0 + MyoHandAirplaneFly-v0 + +The public registration keeps the official MyoSuite task IDs unchanged, so +existing downstream experiment configs can move to EnvPool without renaming the +tasks. + +Observation and Action Spaces +----------------------------- + +MyoSuite is a heterogeneous benchmark, so observation and action shapes vary by +task family: + +* MyoBase exposes hand, arm, torso, and locomotion tasks such as pose, reach, + reorient, key turn, object hold, pen twirl, walk, and terrain locomotion. +* MyoChallenge exposes the official challenge tasks including reorient, + relocate, baoding, bimanual passing, run-track locomotion, soccer, chase-tag, + and table tennis. +* MyoDM exposes object-conditioned ``TrackEnv`` tasks such as + ``MyoHandAirplaneFly-v0`` through the same native registration path. + +All tasks support the standard EnvPool batched Gymnasium and dm_env wrappers, +and MuJoCo pixel wrappers are available through ``from_pixels=True``. + +Render +------ + +MyoSuite render support ships through the same native MuJoCo pixel path used by +other EnvPool MuJoCo families. The public render validation now sweeps every +registered MyoSuite task ID, including the fatigue, sarcopenia, and +reafferentation variants, and checks the reset frame plus the first three +stepped rollout frames against the pinned official renderer bitwise. The +comparison image below keeps a representative slice from MyoBase reorient, +walk, terrain, MyoChallenge, and MyoDM TrackEnv tasks so the doc stays +readable. Each panel shows EnvPool on the left and the official MyoSuite +renderer on the right. + +.. image:: ../_static/render_samples/myosuite_official_compare.png + :alt: EnvPool and official MyoSuite render compares for representative MyoSuite tasks + :width: 100% + +Validation +---------- + +The MyoSuite integration is validated in four layers: + +* generated metadata checks for the full upstream registry surface +* deterministic rollout tests for the native implementations +* 32-step oracle alignment against the official MyoSuite Python + implementation for the direct MyoBase, MyoChallenge, and MyoDM task surface +* public ``render()`` validation that checks the reset frame and the first + three stepped frames bitwise against the official MyoSuite renderer for every + registered public task ID, while the doc image above shows a representative + slice of that full sweep +* public registration tests that construct direct and variant IDs through + ``make_gymnasium`` and verify that fatigue, sarcopenia, and reafferentation + variants actually change rollout dynamics diff --git a/docs/index.rst b/docs/index.rst index 0a4df9f7d..94841cafd 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -96,6 +96,7 @@ stable version through `envpool.readthedocs.io/en/stable/ env/minigrid env/gymnasium_robotics env/metaworld + env/myosuite env/mujoco_gym env/procgen env/toy_text diff --git a/docs/plans/active/2026-04-15-myosuite-integration.md b/docs/plans/active/2026-04-15-myosuite-integration.md new file mode 100644 index 000000000..21b879a76 --- /dev/null +++ b/docs/plans/active/2026-04-15-myosuite-integration.md @@ -0,0 +1,346 @@ +# MyoSuite Integration ExecPlan + +Date: 2026-04-15 +Owner: Codex +Status: In Progress + +## Goal + +Integrate the full official MyoSuite surface into EnvPool as a native C++ +MuJoCo family, with: + +- no Python runtime bridge for environment logic +- official upstream IDs covered end to end +- upstream version pinning +- generated registry metadata from upstream source +- deterministic, alignment, render, and registry coverage +- release-compatible runtime asset packaging + +## Upstream Pin + +Primary upstream: + +- `MyoHub/myosuite` `v2.11.6` +- release commit: `05cb84678373f91271004f99602ebbf01e57d1a1` + +Asset repositories referenced by the pinned checkout: + +- `MyoHub/myo_sim` `33f3ded946f55adbdcf963c99999587aadaf975f` +- `vikashplus/furniture_sim` `c97995afb81c9e2d7325b0069f9abc9a2c74a2f0` +- `vikashplus/object_sim` `87cd8dd5a11518b94fca16bc22bb04f6836c6aa7` +- `vikashplus/MPL_sim` `58dd1abc6058e0dc06e62f13a61c36adb4916815` +- `vikashplus/YCB_sim` `46edd9c361061c5d81a82f2511d4fbf76fead569` + +The `myosuite_init.py` `myo_model` fetch path is not part of the public task +surface currently referenced by the env XMLs, so it is out of scope unless a +runtime task proves it is required. + +## Surface Inventory + +The pinned upstream exposes: + +- 35 direct `myobase` IDs +- 19 direct `myochallenge` IDs +- about 190 direct `myodm` IDs +- `myoedits` duplicates of the arm reach tasks, which should not create extra + EnvPool public IDs +- `Sarc` / `Fati` variants for every `myo*` ID +- `Reaf` variants for every `myoHand*` ID + +Generated unique public ID counts at the current pin: + +- 244 direct IDs +- 358 expanded IDs after upstream variant expansion + +These counts must come from generated metadata, not handwritten lists. + +Those 244 direct IDs collapse into 23 concrete upstream task classes / entry +points: + +- `myobase` + - 11 x `pose_v0:PoseEnvV0` + - 8 x `reach_v0:ReachEnvV0` + - 3 x `walk_v0:TerrainEnvV0` + - 2 x `key_turn_v0:KeyTurnEnvV0` + - 2 x `torso_v0:TorsoEnvV0` + - 1 x `walk_v0:WalkEnvV0` + - 1 x `walk_v0:ReachEnvV0` + - 1 x `obj_hold_v0:ObjHoldFixedEnvV0` + - 1 x `obj_hold_v0:ObjHoldRandomEnvV0` + - 1 x `pen_v0:PenTwirlFixedEnvV0` + - 1 x `pen_v0:PenTwirlRandomEnvV0` + - 4 x `reorient_sar_v0:*` +- `myochallenge` + - 3 x `tabletennis_v0:TableTennisEnvV0` + - 3 x `relocate_v0:RelocateEnvV0` + - 3 x `chasetag_v0:ChaseTagEnvV0` + - 3 x `reorient_v0:ReorientEnvV0` + - 2 x `soccer_v0:SoccerEnvV0` + - 2 x `run_track_v0:RunTrack` + - 2 x `baoding_v1:BaodingEnvV1` + - 1 x `bimanual_v0:BimanualEnvV1` +- `myodm` + - 1 x `myodm_v0:TrackEnv`, reused for all reference-motion and fixed/random + object tasks + +This is still a large port, but it is materially smaller than “244 unrelated +envs”. + +## Constraints + +1. Preserve MyoSuite public task naming exactly. +2. Keep runtime logic native in C++. +3. Keep upstream registry coverage machine-generated from pinned source. +4. Preserve XML-relative asset resolution in runfiles so native MuJoCo loading + can use upstream asset trees without rewriting all includes up front. +5. Do not register any public EnvPool MyoSuite IDs until the corresponding + native runtime family is implemented and tested. + +## Architecture + +### 1. Third-Party Intake + +Add Bazel `http_archive` pins for the MyoSuite repo plus the five asset repos. +Expose filegroups for: + +- runtime env assets from `myosuite/envs/myo/assets/**` +- source files needed for registry generation +- runtime asset trees from the five submodule repos + +Stage runtime files under `envpool/mujoco` so upstream XML relative includes +continue to resolve: + +- `envpool/mujoco/myosuite/...` +- `envpool/mujoco/simhive/myo_sim/...` +- `envpool/mujoco/simhive/furniture_sim/...` +- `envpool/mujoco/simhive/object_sim/...` +- `envpool/mujoco/simhive/MPL_sim/...` +- `envpool/mujoco/simhive/YCB_sim/...` + +### 2. Generated Metadata + +Create a repository-local generator that parses the pinned upstream Python +registration files and emits canonical metadata: + +- suite breakdown +- direct ID list +- expanded ID list +- object lists for `myodm` +- duplicate-overridden IDs from `myoedits` + +This metadata becomes the source of truth for: + +- registration coverage tests +- generated registration modules +- doc task lists + +### 3. Native Runtime Decomposition + +The native implementation should follow the upstream task-class shape rather +than one-class-per-ID: + +- `BaseV0`-style musculoskeletal base +- `PoseEnvV0` +- `ReachEnvV0` +- `KeyTurn` +- `ObjHold` +- `PenTwirl` +- `ReorientSAR` +- `Torso` +- `Walk` / `Terrain` +- `RunTrack` +- `ChaseTag` +- `Soccer` +- `Relocate` +- `Reorient` +- `Baoding` +- `Bimanual` +- `TableTennis` +- `TrackEnv` for `myodm` + +The first implementation pass should prioritize common shared machinery: + +- normalized muscle actuator mapping +- fatigue / sarcopenia / reafferentation variants +- observation packing +- reward dictionary plumbing +- deterministic reset and target generation +- render camera parity +- asset path resolution + +Important upstream mechanics confirmed from source: + +- `env_base.MujocoEnv` uses `dt = model.opt.timestep * frame_skip`. +- default action normalization is linear `[-1, 1] -> actuator_ctrlrange`. +- `BaseV0` special-cases muscle actuators before that remap: + - for `mjtDyn.mjDYN_MUSCLE`, actions are first passed through + `1 / (1 + exp(-5 * (a - 0.5)))` + - non-muscle actuators still flow through the default linear ctrlrange remap +- `BaseV0` then applies optional abnormality logic: + - `sarcopenia` halves actuator `gainprm[:, 2]` + - `fatigue` runs the `CumulativeFatigue` state machine + - `reafferentation` redirects `EIP -> EPL` +- reward flow is consistently `obs_dict -> get_reward_dict() -> dense` via + `weighted_reward_keys` +- resets rely on the saved initial MuJoCo state plus task-specific target / + object randomization before the first observation is emitted + +This strongly suggests the native family should start from a C++ +`MyoBaseEnvBase` that mirrors `env_base.MujocoEnv` + `BaseV0`, then layer +data-driven specializations on top. + +### 4. Validation + +Required before public registration: + +- registry completeness against generated upstream metadata +- deterministic rollout tests over every supported official ID +- official oracle alignment for every supported official ID +- render tests for render-capable tasks +- release asset path validation + +## Milestones + +### Milestone 1: Intake and Metadata + +Deliverables: + +- Bazel third-party pins +- staged runtime asset tree +- generated metadata checked into the repo +- exec plan and decision log + +Proof: + +- metadata generator reproduces the expected direct and expanded counts +- Bazel can build the staged MyoSuite runtime asset targets +- Bazel metadata regression test re-generates the checked-in snapshot from the + vendored upstream source tree +- Bazel MuJoCo smoke test loads every top-level staged MyoSuite model XML that + is intended to be consumed directly + +### Milestone 2: Shared Native Base + +Deliverables: + +- `envpool/mujoco/myosuite/` family skeleton +- native base env and spec helpers +- generated registration scaffolding wired to metadata but not yet public + +Proof: + +- family compiles +- a metadata-driven smoke test can instantiate specs for implemented tasks +- the shared base reproduces upstream action remapping, variant mechanics, and + reward aggregation on at least one `PoseEnvV0` and one `ReachEnvV0` task + +### Milestone 3: MyoBase + +Deliverables: + +- full native `myobase` coverage, including variant IDs + +Proof: + +- deterministic + alignment + render coverage over all `myobase` tasks +- `PoseEnvV0` and `ReachEnvV0` land first, because they cover 19 out of 35 + direct `myobase` IDs and validate most of the musculoskeletal base + machinery + +### Milestone 4: MyoChallenge + +Deliverables: + +- full native `myochallenge` coverage, including variant IDs where upstream + generates them + +Proof: + +- deterministic + alignment + render coverage over all `myochallenge` tasks + +### Milestone 5: MyoDM + +Deliverables: + +- full native `myodm` surface + +Proof: + +- completeness test over all object and reference-motion tasks +- oracle-aligned stepping and asset path validation + +### Milestone 6: Docs and Release + +Deliverables: + +- `docs/env/myosuite.rst` +- README support matrix entry +- release asset validation + +Proof: + +- docs build +- release-target smoke path loads staged MyoSuite assets + +## Progress + +- 2026-04-15: Read EnvPool new-env and contributing docs. +- 2026-04-15: Surveyed the existing MuJoCo, MetaWorld, and + Gymnasium-Robotics integrations for structure and test patterns. +- 2026-04-15: Pinned MyoSuite to `v2.11.6` and cloned the upstream release for + local analysis. +- 2026-04-15: Measured the public surface and identified the five external + asset repositories needed for runtime XML resolution. +- 2026-04-15: Started landing third-party intake and metadata generation + infrastructure in this repo. +- 2026-04-15: Added Bazel-pinned third-party intake for `myosuite` and the + five required asset repositories. +- 2026-04-15: Added staged `myosuite_assets` runfiles layout under + `envpool/mujoco` and verified representative XML include resolution. +- 2026-04-15: Checked in generated MyoSuite ID metadata and added Python + helpers to load the vendored asset root and metadata in Bazel runfiles. +- 2026-04-15: Added `myosuite_metadata_test`, which re-generates the metadata + snapshot from the vendored upstream registry sources and compares it + byte-for-byte with the checked-in JSON. +- 2026-04-15: Added `myosuite_asset_smoke_test`, which loads every top-level + staged MyoSuite model XML directly through MuJoCo to catch packaging and + include-path regressions early. + +## Surprises and Discoveries + +- The MyoSuite public surface is much larger than a typical MuJoCo family. + The `myodm` suite alone contributes about 190 direct IDs. +- The direct surface is nonetheless concentrated into 23 entry points, so a + data-driven native port is plausible if the shared bases are done well. +- The upstream XMLs are designed around relative includes into a `simhive` + tree. Preserving that structure inside runfiles is simpler and less risky + than rewriting every XML include up front. +- The musculoskeletal action pipeline is not the same as Gymnasium-Robotics or + MetaWorld. `BaseV0` applies a muscle-specific nonlinear pre-transform before + the normal actuator ctrlrange remap, and challenge envs such as + `BimanualEnvV1`, `TableTennisEnvV0`, and `RunTrack` additionally mix muscle + and non-muscle actuator handling. +- Several XMLs under `envs/myo/assets/` are not direct top-level models: + `myohand_object.xml` is a template that requires object-name substitution, + while files such as `myosuite_track.xml`, `paddle.xml`, and nested soccer + fragments are included by higher-level models and are not meant to be loaded + standalone. The smoke coverage therefore targets the top-level env model + XMLs rather than every XML file in the tree. +- `myoedits` does not add new public tasks; it overrides the arm reach tasks. + +## Decision Log + +- Decision: preserve upstream XML include structure in staged assets. + Reason: this minimizes early patching and keeps the runtime path close to the + pinned official oracle. +- Decision: generate the MyoSuite registry metadata from upstream source before + implementing public registration. + Reason: hand-maintaining 358 expanded IDs would be brittle and violates the + project guidance. + +## Open Questions + +- Whether any upstream task needs an additional asset repo beyond the five + current submodules once the native runtime begins loading all task classes. +- How much of the staged asset tree can be compacted safely for release + packaging after the family is green. diff --git a/docs/spelling_wordlist.txt b/docs/spelling_wordlist.txt index 3a7973b12..64831c601 100644 --- a/docs/spelling_wordlist.txt +++ b/docs/spelling_wordlist.txt @@ -89,3 +89,9 @@ deleter gfootball minimap rollouts +sarcopenia +reafferentation +baoding +bimanual +heightfields +configs diff --git a/envpool/BUILD b/envpool/BUILD index 2b0d6fa25..ccc4d15d7 100644 --- a/envpool/BUILD +++ b/envpool/BUILD @@ -42,6 +42,7 @@ py_library( "//envpool/mujoco:metaworld_registration", "//envpool/mujoco:mujoco_dmc_registration", "//envpool/mujoco:mujoco_gym_registration", + "//envpool/mujoco:myosuite_registration", "//envpool/mujoco:robotics_registration", "//envpool/procgen:procgen_registration", "//envpool/toy_text:toy_text_registration", diff --git a/envpool/core/xla.h b/envpool/core/xla.h index 02cce3960..0c31a2ff8 100644 --- a/envpool/core/xla.h +++ b/envpool/core/xla.h @@ -39,12 +39,12 @@ template constexpr bool HasContainerType(std::tuple /*unused*/) { return (is_container_v || ...); } -bool HasDynamicDim(const std::vector& shape) { +inline bool HasDynamicDim(const std::vector& shape) { return std::any_of(shape.begin() + 1, shape.end(), [](int s) { return s == -1; }); } template -bool HasDynamicDim(const std::tuple& state_spec) { +inline bool HasDynamicDim(const std::tuple& state_spec) { bool dyn = false; std::apply([&](auto&&... spec) { dyn = (HasDynamicDim(spec.shape) || ...); }, state_spec); diff --git a/envpool/entry.py b/envpool/entry.py index 5ee50d517..517e13dda 100644 --- a/envpool/entry.py +++ b/envpool/entry.py @@ -13,6 +13,7 @@ # limitations under the License. """Entry point for all envs' registration.""" +# Imported for registration side effects. import envpool.atari.registration # noqa: F401 import envpool.box2d.registration # noqa: F401 import envpool.classic_control.registration # noqa: F401 @@ -22,7 +23,10 @@ import envpool.mujoco.dmc.registration # noqa: F401 import envpool.mujoco.gym.registration # noqa: F401 import envpool.mujoco.metaworld.registration # noqa: F401 +import envpool.mujoco.myosuite.registration as _myosuite_registration import envpool.mujoco.robotics.registration # noqa: F401 import envpool.procgen.registration # noqa: F401 import envpool.toy_text.registration # noqa: F401 import envpool.vizdoom.registration # noqa: F401 + +_myosuite_registration.register_myosuite_tasks() diff --git a/envpool/mujoco/BUILD b/envpool/mujoco/BUILD index bd2706853..f64291ae6 100644 --- a/envpool/mujoco/BUILD +++ b/envpool/mujoco/BUILD @@ -16,8 +16,9 @@ load("@pybind11_bazel//:build_defs.bzl", "pybind_extension") load("@rules_cc//cc:defs.bzl", "cc_library", "cc_test") load("@rules_python//python:defs.bzl", "py_library", "py_test") load("//envpool:requirements.bzl", "requirement") -load("//third_party:common.bzl", "copy_to_directory") +load("//third_party:common.bzl", "copy_files_to_directory", "copy_to_directory") load("//third_party/metaworld_assets:defs.bzl", "metaworld_runtime_assets") +load("//third_party/myosuite:defs.bzl", "myosuite_runtime_assets") package(default_visibility = ["//visibility:public"]) @@ -55,6 +56,7 @@ py_library( ":metaworld", ":mujoco_dmc", ":mujoco_gym", + ":myosuite", ":robotics", ], ) @@ -94,6 +96,124 @@ metaworld_runtime_assets( strip_prefix = "metaworld_assets/metaworld/assets/", ) +myosuite_runtime_assets( + name = "gen_myosuite_runtime_assets", + out = "myosuite_assets", + furniture_sim_assets = ["@furniture_sim_src//:runtime_assets"], + mpl_sim_assets = ["@mpl_sim_src//:runtime_assets"], + myo_sim_assets = ["@myo_sim_src//:runtime_assets"], + myosuite_env_assets = ["@myosuite_src//:myosuite_runtime_assets"], + object_sim_assets = ["@object_sim_src//:runtime_assets"], + patched_assets = [ + "//third_party/myosuite:simhive/myo_sim/arm/myoarm_reach.xml", + "//third_party/myosuite:simhive/myo_sim/arm/myoarm_tabletennis.xml", + ], + ycb_sim_assets = ["@ycb_sim_src//:runtime_assets"], +) + +filegroup( + name = "myosuite_runtime_assets", + srcs = [":gen_myosuite_runtime_assets"], +) + +copy_files_to_directory( + name = "gen_myosuite_metadata", + srcs = ["//third_party/myosuite:metadata/env_ids.json"], + out = "myosuite", + strip_prefix = "third_party/myosuite/metadata/", +) + +filegroup( + name = "myosuite_packaged_metadata", + srcs = [":gen_myosuite_metadata"], +) + +cc_library( + name = "myosuite_support", + hdrs = ["myosuite/paths.h"], + data = [":myosuite_runtime_assets"], +) + +py_library( + name = "myosuite", + srcs = [ + "myosuite/__init__.py", + "myosuite/config.py", + "myosuite/metadata.py", + "myosuite/native.py", + "myosuite/paths.py", + "myosuite/registration.py", + ], + data = [ + ":myosuite_envpool", + ":myosuite_packaged_metadata", + ":myosuite_runtime_assets", + ], + imports = ["../.."], + deps = [ + "//envpool/python:api", + requirement("numpy"), + ], +) + +py_library( + name = "myosuite_oracle_utils", + srcs = ["myosuite/oracle_utils.py"], + data = ["@myosuite_src//:myosuite_python_sources"], + imports = ["../.."], + deps = [ + ":myosuite", + ], +) + +py_library( + name = "myosuite_render_utils", + srcs = ["myosuite/render_utils.py"], + data = ["@myosuite_src//:myosuite_python_sources"], + imports = ["../.."], + deps = [ + ":myosuite", + ":myosuite_oracle_test_deps", + ":myosuite_oracle_utils", + "//envpool:registration", + requirement("dm-control"), + requirement("gymnasium"), + requirement("mujoco"), + requirement("numpy"), + ], +) + +cc_library( + name = "myosuite_env", + hdrs = [ + "myosuite/myobase.h", + "myosuite/myobase_extended.h", + "myosuite/myochallenge.h", + "myosuite/myochallenge_extended.h", + "myosuite/myodm.h", + ], + deps = [ + ":myosuite_support", + ":robotics_env", + "//envpool/core:async_envpool", + ], +) + +pybind_extension( + name = "myosuite_envpool", + srcs = [ + "myosuite/mujoco_plugin_init.cc", + "myosuite_envpool.cc", + "myosuite_envpool_base.cc", + "myosuite_envpool_challenge.cc", + "myosuite_envpool_dm.cc", + ], + deps = [ + ":myosuite_env", + "//envpool/core:py_envpool", + ], +) + cc_library( name = "mujoco_gym_env", hdrs = [ @@ -323,6 +443,16 @@ py_library( ], ) +py_library( + name = "myosuite_registration", + srcs = ["myosuite/registration.py"], + imports = ["../.."], + deps = [ + ":myosuite", + "//envpool:registration", + ], +) + py_test( name = "metaworld_test", size = "enormous", @@ -583,3 +713,143 @@ py_test( requirement("numpy"), ], ) + +py_test( + name = "myosuite_metadata_test", + size = "large", + srcs = ["myosuite/myosuite_metadata_test.py"], + data = ["@myosuite_src//:myosuite_registry_sources"], + imports = ["../.."], + deps = [ + ":myosuite", + requirement("absl-py"), + requirement("mujoco"), + ], +) + +py_test( + name = "myosuite_asset_smoke_test", + size = "large", + srcs = ["myosuite/myosuite_asset_smoke_test.py"], + imports = ["../.."], + deps = [ + ":myosuite", + requirement("absl-py"), + requirement("mujoco"), + ], +) + +py_library( + name = "myosuite_oracle_test_deps", + deps = [ + requirement("click"), + requirement("flatten-dict"), + requirement("gitpython"), + requirement("h5py"), + requirement("packaging"), + requirement("pillow"), + requirement("pink-noise-rl"), + requirement("sk-video"), + requirement("termcolor"), + ], +) + +py_test( + name = "myosuite_myobase_test", + size = "large", + srcs = ["myosuite/myobase_test.py"], + data = ["@myosuite_src//:myosuite_python_sources"], + imports = ["../.."], + main = "myosuite/myobase_test.py", + deps = [ + ":myosuite", + ":myosuite_oracle_test_deps", + ":myosuite_oracle_utils", + "//envpool:registration", + requirement("absl-py"), + requirement("dm-control"), + requirement("gymnasium"), + requirement("mujoco"), + requirement("numpy"), + ], +) + +py_test( + name = "myosuite_myodm_test", + size = "large", + srcs = ["myosuite/myodm_test.py"], + data = ["@myosuite_src//:myosuite_python_sources"], + imports = ["../.."], + main = "myosuite/myodm_test.py", + deps = [ + ":myosuite", + ":myosuite_oracle_test_deps", + ":myosuite_oracle_utils", + requirement("absl-py"), + requirement("dm-control"), + requirement("gymnasium"), + requirement("mujoco"), + requirement("numpy"), + ], +) + +py_test( + name = "myosuite_myochallenge_test", + size = "large", + srcs = ["myosuite/myochallenge_test.py"], + data = ["@myosuite_src//:myosuite_python_sources"], + imports = ["../.."], + main = "myosuite/myochallenge_test.py", + deps = [ + ":myosuite", + ":myosuite_oracle_test_deps", + ":myosuite_oracle_utils", + requirement("absl-py"), + requirement("dm-control"), + requirement("gymnasium"), + requirement("mujoco"), + requirement("numpy"), + ], +) + +# Keep MyoSuite render checks exclusive and finely sharded. The generated +# metadata currently exposes 398 public IDs; small contiguous shards keep +# official MuJoCo renderer state isolated enough for strict bitwise checks. +_MYOSUITE_RENDER_SHARD_COUNT = 50 + +py_test( + name = "myosuite_render_doc_test", + size = "large", + srcs = ["myosuite/myosuite_render_test.py"], + imports = ["../.."], + main = "myosuite/myosuite_render_test.py", + tags = ["exclusive"], + deps = [ + ":myosuite_render_utils", + requirement("absl-py"), + requirement("numpy"), + ], +) + +py_test( + name = "myosuite_render_surface_test", + size = "large", + srcs = ["myosuite/myosuite_render_surface_test.py"], + imports = ["../.."], + main = "myosuite/myosuite_render_surface_test.py", + shard_count = _MYOSUITE_RENDER_SHARD_COUNT, + tags = ["exclusive"], + deps = [ + ":myosuite_render_utils", + requirement("absl-py"), + requirement("numpy"), + ], +) + +test_suite( + name = "myosuite_render_test", + tests = [ + ":myosuite_render_doc_test", + ":myosuite_render_surface_test", + ], +) diff --git a/envpool/mujoco/myosuite/__init__.py b/envpool/mujoco/myosuite/__init__.py new file mode 100644 index 000000000..29fe2f780 --- /dev/null +++ b/envpool/mujoco/myosuite/__init__.py @@ -0,0 +1,46 @@ +# Copyright 2026 Garena Online Private 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. +"""MyoSuite integration helpers for EnvPool.""" + +from envpool.mujoco.myosuite.metadata import ( + MYOSUITE_COUNTS, + MYOSUITE_DIRECT_ENTRIES, + MYOSUITE_DIRECT_ENTRY_BY_ID, + MYOSUITE_DIRECT_IDS, + MYOSUITE_EXPANDED_IDS, + MYOSUITE_NOTES, + MYOSUITE_PINS, + MYOSUITE_SUITES, + load_myosuite_metadata, +) +from envpool.mujoco.myosuite.paths import ( + myosuite_asset_root, + myosuite_metadata_path, + resolve_workspace_path, +) + +__all__ = [ + "MYOSUITE_COUNTS", + "MYOSUITE_DIRECT_ENTRIES", + "MYOSUITE_DIRECT_ENTRY_BY_ID", + "MYOSUITE_DIRECT_IDS", + "MYOSUITE_EXPANDED_IDS", + "MYOSUITE_NOTES", + "MYOSUITE_PINS", + "MYOSUITE_SUITES", + "load_myosuite_metadata", + "myosuite_asset_root", + "myosuite_metadata_path", + "resolve_workspace_path", +] diff --git a/envpool/mujoco/myosuite/config.py b/envpool/mujoco/myosuite/config.py new file mode 100644 index 000000000..0216710cc --- /dev/null +++ b/envpool/mujoco/myosuite/config.py @@ -0,0 +1,1364 @@ +# Copyright 2026 Garena Online Private 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. +"""Metadata-driven MyoSuite config resolution for public registration.""" + +from __future__ import annotations + +import tempfile +from functools import cache +from pathlib import Path +from typing import Any + +import numpy as np + +from envpool.mujoco.myosuite.metadata import MYOSUITE_DIRECT_ENTRIES +from envpool.mujoco.myosuite.paths import myosuite_asset_root + +_ARM_REACH_EDIT_FN = "edit_fn_arm_reaching" +_ARM_REACH_RAW_MODEL_PATH = "envs/myo/assets/arm/myoarm_reach.xml" +_ARM_REACH_MODEL_PATH = "simhive/myo_sim/arm/myoarm_reach.xml" +_TABLETENNIS_RAW_MODEL_PATH = "envs/myo/assets/arm/myoarm_tabletennis.xml" +_TABLETENNIS_MODEL_PATH = "simhive/myo_sim/arm/myoarm_tabletennis.xml" + +_RESET_SYNC_TEST_KEYS = { + "test_reset_qpos", + "test_reset_qvel", + "test_reset_act", + "test_reset_qacc_warmstart", +} + +_PREVIEW_TEST_KEYS_BY_ENTRYPOINT = { + ("myobase", "PoseEnvV0"): { + "test_reset_ctrl", + "test_target_qpos", + "test_target_site_pos", + "test_body_mass", + "test_geom_size0", + }, + ("myobase", "ReachEnvV0"): {"test_reset_ctrl", "test_target_pos"}, + ("myobase", "Geometries100EnvV0"): { + "test_target_body_quat", + "test_object_geom_size", + "test_target_geom_size", + "test_object_geom_rgba", + "test_target_geom_rgba", + "test_object_geom_top_pos", + "test_object_geom_bottom_pos", + "test_target_geom_top_pos", + "test_target_geom_bottom_pos", + "test_object_body_mass", + "test_object_geom_type", + "test_target_geom_type", + "test_object_geom_condim", + "test_success_site_rgba", + }, + ("myobase", "Geometries8EnvV0"): { + "test_target_body_quat", + "test_object_geom_size", + "test_target_geom_size", + "test_object_geom_rgba", + "test_target_geom_rgba", + "test_object_geom_top_pos", + "test_object_geom_bottom_pos", + "test_target_geom_top_pos", + "test_target_geom_bottom_pos", + "test_object_body_mass", + "test_object_geom_type", + "test_target_geom_type", + "test_object_geom_condim", + "test_success_site_rgba", + }, + ("myobase", "InDistribution"): { + "test_target_body_quat", + "test_object_geom_size", + "test_target_geom_size", + "test_object_geom_rgba", + "test_target_geom_rgba", + "test_object_geom_top_pos", + "test_object_geom_bottom_pos", + "test_target_geom_top_pos", + "test_target_geom_bottom_pos", + "test_object_body_mass", + "test_object_geom_type", + "test_target_geom_type", + "test_object_geom_condim", + "test_success_site_rgba", + }, + ("myobase", "OutofDistribution"): { + "test_target_body_quat", + "test_object_geom_size", + "test_target_geom_size", + "test_object_geom_rgba", + "test_target_geom_rgba", + "test_object_geom_top_pos", + "test_object_geom_bottom_pos", + "test_target_geom_top_pos", + "test_target_geom_bottom_pos", + "test_object_body_mass", + "test_object_geom_type", + "test_target_geom_type", + "test_object_geom_condim", + "test_success_site_rgba", + }, + ("myobase", "KeyTurnEnvV0"): { + "test_key_body_pos", + "test_goal_pos", + }, + ("myobase", "ObjHoldFixedEnvV0"): {"test_object_geom_size"}, + ("myobase", "ObjHoldRandomEnvV0"): {"test_object_geom_size"}, + ("myobase", "PenTwirlFixedEnvV0"): {"test_target_body_quat"}, + ("myobase", "PenTwirlRandomEnvV0"): {"test_target_body_quat"}, + ("myobase", "WalkEnvV0"): { + "test_terrain_geom_rgba", + "test_terrain_geom_pos", + "test_terrain_geom_contype", + "test_terrain_geom_conaffinity", + }, + ("myobase", "TerrainEnvV0"): { + "test_hfield_data", + "test_terrain_geom_rgba", + "test_terrain_geom_pos", + "test_terrain_geom_contype", + "test_terrain_geom_conaffinity", + }, + ("myochallenge", "ReorientEnvV0"): { + "test_reset_act_dot", + "test_goal_body_pos", + "test_goal_body_quat", + "test_target_geom_size", + "test_object_geom_size", + "test_object_geom_pos", + "test_object_geom_friction", + "test_object_body_mass", + }, + ("myochallenge", "RelocateEnvV0"): { + "test_reset_ctrl", + "test_reset_act_dot", + "test_goal_body_pos", + "test_goal_body_quat", + "test_goal_mocap_pos", + "test_goal_mocap_quat", + "test_object_body_pos", + "test_object_body_mass", + "test_object_geom_type", + "test_object_geom_size", + "test_object_geom_pos", + "test_object_geom_quat", + "test_object_geom_rgba", + "test_object_geom_friction", + }, + ("myochallenge", "BaodingEnvV1"): { + "test_task", + "test_ball1_starting_angle", + "test_ball2_starting_angle", + "test_x_radius", + "test_y_radius", + "test_goal_trajectory", + "test_target1_site_pos", + "test_target2_site_pos", + "test_object1_body_mass", + "test_object2_body_mass", + "test_object1_geom_size", + "test_object2_geom_size", + "test_object1_geom_friction", + "test_object2_geom_friction", + }, + ("myochallenge", "BimanualEnvV1"): { + "test_start_pos", + "test_goal_pos", + "test_object_body_mass", + "test_object_geom_size", + "test_object_geom_friction", + }, + ("myochallenge", "RunTrack"): { + "test_hfield_data", + "test_terrain_geom_rgba", + "test_terrain_geom_pos", + "test_terrain_type", + "test_osl_state", + }, + ("myochallenge", "SoccerEnvV0"): { + "test_reset_ctrl", + "test_reset_act_dot", + "test_goalkeeper_pose", + "test_goalkeeper_velocity", + "test_goalkeeper_noise_buffer", + "test_goalkeeper_noise_idx", + "test_goalkeeper_block_velocity", + "test_goalkeeper_policy", + }, + ("myochallenge", "ChaseTagEnvV0"): { + "test_reset_ctrl", + "test_reset_act_dot", + "test_hfield_data", + "test_terrain_geom_rgba", + "test_terrain_geom_pos", + "test_task", + "test_opponent_pose", + "test_opponent_velocity", + "test_opponent_noise_buffer", + "test_opponent_noise_idx", + "test_chase_velocity", + "test_opponent_policy", + }, + ("myochallenge", "TableTennisEnvV0"): { + "test_reset_act_dot", + "test_ball_body_pos", + "test_ball_geom_friction", + "test_paddle_body_mass", + "test_init_qpos", + "test_init_qvel", + }, + ("myodm", "TrackEnv"): { + "test_playback_reference", + "test_reset_ctrl", + "test_reset_act_dot", + "test_reset_integration_state", + "test_reference_time", + "test_reference_robot", + "test_reference_robot_vel", + "test_reference_object", + }, +} + + +def _asset_root(base_path: str) -> Path: + candidate = Path(base_path) / "mujoco" / "myosuite_assets" + if candidate.exists(): + return candidate + return myosuite_asset_root() + + +def _asset_model_path(base_path: str, model_path: str) -> Path: + path = Path(resolve_myosuite_model_path(model_path)) + if path.is_absolute(): + return path + return _asset_root(base_path) / path + + +def resolve_myosuite_model_path( + model_path: str, edit_fn: str | None = None +) -> str: + """Resolve upstream model edits into packaged runtime asset paths.""" + if Path(model_path).is_absolute(): + return model_path + if model_path == _ARM_REACH_RAW_MODEL_PATH or edit_fn == _ARM_REACH_EDIT_FN: + return _ARM_REACH_MODEL_PATH + if model_path == _TABLETENNIS_RAW_MODEL_PATH: + return _TABLETENNIS_MODEL_PATH + return model_path + + +def _prepare_runtime_kwargs(kwargs: dict[str, Any]) -> dict[str, Any]: + runtime_kwargs = dict(kwargs) + runtime_kwargs["model_path"] = resolve_myosuite_model_path( + str(runtime_kwargs["model_path"]), + ( + str(runtime_kwargs["edit_fn"]) + if runtime_kwargs.get("edit_fn") is not None + else None + ), + ) + return runtime_kwargs + + +def _supported_preview_test_keys(entry: dict[str, Any]) -> set[str]: + return _RESET_SYNC_TEST_KEYS | _PREVIEW_TEST_KEYS_BY_ENTRYPOINT.get( + (entry["suite"], entry["class_name"]), + set(), + ) + + +@cache +def _expanded_entry_by_id() -> dict[str, tuple[dict[str, Any], dict[str, Any]]]: + mapping: dict[str, tuple[dict[str, Any], dict[str, Any]]] = {} + for entry in MYOSUITE_DIRECT_ENTRIES: + mapping[entry["id"]] = (entry, {}) + for variant_def in entry["variant_defs"]: + mapping[variant_def["variant_id"]] = ( + entry, + dict(variant_def["variants"]), + ) + return mapping + + +def myosuite_expanded_entry( + task_id: str, +) -> tuple[dict[str, Any], dict[str, Any]]: + """Return the direct metadata entry plus variant overrides for a task ID.""" + try: + return _expanded_entry_by_id()[task_id] + except KeyError as exc: + raise KeyError(f"Unknown MyoSuite task_id: {task_id}") from exc + + +@cache +def _model(base_path: str, model_path: str) -> Any: + import mujoco + + return mujoco.MjModel.from_xml_path( + str(_asset_model_path(base_path, model_path)) + ) + + +def _replace_all(text: str, old: str, new: str) -> str: + return text.replace(old, new) + + +@cache +def _track_model(base_path: str, model_path: str, object_name: str) -> Any: + import mujoco + + asset_root = _asset_root(base_path) + source_model = asset_root / model_path + object_xml = source_model.read_text() + tabletop_xml = ( + asset_root / "envs/myo/assets/hand/myohand_tabletop.xml" + ).read_text() + hand_assets_xml = ( + asset_root / "simhive/myo_sim/hand/assets/myohand_assets.xml" + ).read_text() + myo_sim_root = asset_root / "simhive/myo_sim" + myo_sim_root_str = str(myo_sim_root) + + with tempfile.TemporaryDirectory() as td: + tmp_dir = Path(td) + hand_assets_tmp = tmp_dir / "myohand_assets.xml" + tabletop_tmp = tmp_dir / "myohand_tabletop.xml" + object_tmp = tmp_dir / "myohand_object.xml" + + hand_assets_xml = _replace_all( + hand_assets_xml, + 'meshdir=".." texturedir=".."', + f'meshdir="{myo_sim_root_str}" texturedir="{myo_sim_root_str}"', + ) + hand_assets_tmp.write_text(hand_assets_xml) + + tabletop_xml = _replace_all( + tabletop_xml, + "../../../../simhive/myo_sim/hand/assets/myohand_assets.xml", + str(hand_assets_tmp), + ) + tabletop_xml = _replace_all( + tabletop_xml, + "../../../../simhive/furniture_sim/simpleTable/simpleTable_asset.xml", + str( + asset_root + / "simhive/furniture_sim/simpleTable/simpleTable_asset.xml" + ), + ) + tabletop_xml = _replace_all( + tabletop_xml, + "../../../../simhive/myo_sim/hand/assets/myohand_body.xml", + str(asset_root / "simhive/myo_sim/hand/assets/myohand_body.xml"), + ) + tabletop_xml = _replace_all( + tabletop_xml, + "../../../../simhive/furniture_sim/simpleTable/simpleGraniteTable_body.xml", + str( + asset_root + / "simhive/furniture_sim/simpleTable/simpleGraniteTable_body.xml" + ), + ) + tabletop_xml = _replace_all( + tabletop_xml, + 'meshdir="../../../../simhive/myo_sim/" texturedir="../../../../simhive/myo_sim/"', + f'meshdir="{myo_sim_root_str}" texturedir="{myo_sim_root_str}"', + ) + tabletop_tmp.write_text(tabletop_xml) + + object_xml = _replace_all(object_xml, "OBJECT_NAME", object_name) + object_xml = _replace_all( + object_xml, "myohand_tabletop.xml", str(tabletop_tmp) + ) + object_xml = _replace_all( + object_xml, + "../../../../simhive/object_sim/common.xml", + str(asset_root / "simhive/object_sim/common.xml"), + ) + object_xml = _replace_all( + object_xml, + f"../../../../simhive/object_sim/{object_name}/assets.xml", + str(asset_root / f"simhive/object_sim/{object_name}/assets.xml"), + ) + object_xml = _replace_all( + object_xml, + f"../../../../simhive/object_sim/{object_name}/body.xml", + str(asset_root / f"simhive/object_sim/{object_name}/body.xml"), + ) + object_tmp.write_text(object_xml) + return mujoco.MjModel.from_xml_path(str(object_tmp)) + + +def _weighted_reward(kwargs: dict[str, Any], key: str, default: float) -> float: + return float(kwargs.get("weighted_reward_keys", {}).get(key, default)) + + +def _flatten_site_ranges( + target_reach_range: dict[str, list[list[float]]], +) -> tuple[list[str], list[float], list[float]]: + site_names: list[str] = [] + mins: list[float] = [] + maxs: list[float] = [] + for site_name, span in target_reach_range.items(): + site_names.append(site_name) + mins.extend(span[0]) + maxs.extend(span[1]) + return site_names, mins, maxs + + +def _dict_reference_init( + reference: dict[str, Any], key: str, array_key: str +) -> np.ndarray: + value = reference.get(key) + if value is not None: + return np.asarray(value, dtype=np.float64) + base = np.asarray(reference[array_key], dtype=np.float64) + if base.shape[0] == 2: + return np.mean(base, axis=0) + return base[0] + + +def _reference_config(base_path: str, reference: Any) -> dict[str, Any]: + if isinstance(reference, str): + reference_path = _asset_root(base_path) / reference + with np.load(reference_path) as data: + robot = np.asarray(data["robot"], dtype=np.float64) + obj = np.asarray(data["object"], dtype=np.float64) + robot_vel = ( + np.asarray(data["robot_vel"], dtype=np.float64) + if "robot_vel" in data.files + else None + ) + robot_init = ( + np.asarray(data["robot_init"], dtype=np.float64) + if "robot_init" in data.files + else robot[0] + ) + object_init = ( + np.asarray(data["object_init"], dtype=np.float64) + if "object_init" in data.files + else obj[0] + ) + return { + "reference_path": reference, + "reference_time": [], + "reference_robot": [], + "reference_robot_vel": [], + "reference_object": [], + "reference_robot_init": robot_init.tolist(), + "reference_object_init": object_init.tolist(), + "robot_dim": int(robot.shape[1]), + "object_dim": int(obj.shape[1]), + "robot_horizon": int(robot.shape[0]), + "object_horizon": int(obj.shape[0]), + "reference_has_robot_vel": robot_vel is not None, + } + + ref = dict(reference) + time = np.asarray(ref["time"], dtype=np.float64) + robot = np.asarray(ref["robot"], dtype=np.float64) + obj = np.asarray(ref["object"], dtype=np.float64) + robot_vel = ( + np.asarray(ref["robot_vel"], dtype=np.float64) + if ref.get("robot_vel") is not None + else None + ) + robot_init = _dict_reference_init(ref, "robot_init", "robot") + object_init = _dict_reference_init(ref, "object_init", "object") + return { + "reference_path": "", + "reference_time": time.tolist(), + "reference_robot": robot.reshape(-1).tolist(), + "reference_robot_vel": [] + if robot_vel is None + else robot_vel.reshape(-1).tolist(), + "reference_object": obj.reshape(-1).tolist(), + "reference_robot_init": robot_init.tolist(), + "reference_object_init": object_init.tolist(), + "robot_dim": int(robot.shape[1]), + "object_dim": int(obj.shape[1]), + "robot_horizon": int(robot.shape[0]), + "object_horizon": int(obj.shape[0]), + "reference_has_robot_vel": robot_vel is not None, + } + + +def _bimanual_index_sets(model: Any) -> tuple[list[int], ...]: + myo_qpos: list[int] = [] + myo_dof: list[int] = [] + prosth_qpos: list[int] = [] + prosth_dof: list[int] = [] + manip_joint = model.joint("manip_object/freejoint") + for joint_id in range(model.njnt): + joint = model.joint(joint_id) + if joint.name == "manip_object/freejoint": + continue + if joint.name.startswith("prosthesis"): + prosth_qpos.append(joint.qposadr[0]) + prosth_dof.append(joint.dofadr[0]) + else: + myo_qpos.append(joint.qposadr[0]) + myo_dof.append(joint.dofadr[0]) + return ( + myo_qpos, + myo_dof, + prosth_qpos, + prosth_dof, + [manip_joint.qposadr[0] + i for i in range(7)], + [manip_joint.dofadr[0] + i for i in range(6)], + ) + + +def _pose_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + model = _model(base_path, kwargs["model_path"]) + if "target_jnt_range" in kwargs: + target_qpos_min = [ + bounds[0] for bounds in kwargs["target_jnt_range"].values() + ] + target_qpos_max = [ + bounds[1] for bounds in kwargs["target_jnt_range"].values() + ] + target_qpos_value = [ + (lo + hi) / 2.0 + for lo, hi in zip(target_qpos_min, target_qpos_max, strict=True) + ] + else: + target_qpos_min = [] + target_qpos_max = [] + target_qpos_value = list(kwargs["target_jnt_value"]) + return { + "frame_skip": int(kwargs.get("frame_skip", 10)), + "model_path": str(kwargs["model_path"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": model.nq + model.nv + model.nq + model.na, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.nu, + "pose_thd": float(kwargs.get("pose_thd", 0.35)), + "reward_pose_w": _weighted_reward(kwargs, "pose", 1.0), + "reward_bonus_w": _weighted_reward(kwargs, "bonus", 4.0), + "reward_act_reg_w": _weighted_reward(kwargs, "act_reg", 1.0), + "reward_penalty_w": _weighted_reward(kwargs, "penalty", 50.0), + "reset_type": str(kwargs.get("reset_type", "init")), + "target_type": str( + kwargs.get( + "target_type", "fixed" if target_qpos_value else "generate" + ) + ), + "target_qpos_min": target_qpos_min, + "target_qpos_max": target_qpos_max, + "target_qpos_value": target_qpos_value, + "viz_site_targets": list(kwargs.get("viz_site_targets", [])), + "weight_bodyname": str(kwargs.get("weight_bodyname", "")), + "weight_range": list(kwargs.get("weight_range", [])), + } + + +def _reach_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + model = _model(base_path, kwargs["model_path"]) + site_names, mins, maxs = _flatten_site_ranges(kwargs["target_reach_range"]) + is_walk_reach = entry["entry_module"] == "myosuite.envs.myo.myobase.walk_v0" + return { + "frame_skip": int(kwargs.get("frame_skip", 10)), + "model_path": str(kwargs["model_path"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": model.nq + model.nv + 6 * len(site_names) + model.na, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.nu, + "target_site_count": len(site_names), + "far_th": float(kwargs.get("far_th", 0.35)), + "reward_reach_w": _weighted_reward(kwargs, "reach", 1.0), + "reward_bonus_w": _weighted_reward(kwargs, "bonus", 4.0), + "reward_act_reg_w": _weighted_reward(kwargs, "act_reg", 0.0), + "reward_penalty_w": _weighted_reward(kwargs, "penalty", 50.0), + "target_site_names": site_names, + "target_pos_min": mins, + "target_pos_max": maxs, + "joint_random_range": list(kwargs.get("joint_random_range", [])), + "target_pos_relative_to_tip": is_walk_reach, + "hide_skin_geom_group_1": is_walk_reach, + "hide_terrain": is_walk_reach, + } + + +def _myobase_reorient_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + model = _model(base_path, kwargs["model_path"]) + mode_map = { + "Geometries100EnvV0": "100", + "Geometries8EnvV0": "8", + "InDistribution": "id", + "OutofDistribution": "ood", + } + return { + "frame_skip": int(kwargs.get("frame_skip", 5)), + "model_path": str(kwargs["model_path"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": (model.nq - 6) + 21 + 3 * model.nu + model.na, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.nu, + "randomization_mode": mode_map[entry["class_name"]], + "reward_pos_align_w": _weighted_reward(kwargs, "pos_align", 1.0), + "reward_rot_align_w": _weighted_reward(kwargs, "rot_align", 1.0), + "reward_act_reg_w": _weighted_reward(kwargs, "act_reg", 5.0), + "reward_drop_w": _weighted_reward(kwargs, "drop", 5.0), + "reward_bonus_w": _weighted_reward(kwargs, "bonus", 10.0), + } + + +def _key_turn_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + model = _model(base_path, kwargs["model_path"]) + return { + "frame_skip": int(kwargs.get("frame_skip", 10)), + "model_path": str(kwargs["model_path"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": model.nq + model.nv + 6 + model.na, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.nu, + "goal_th": float(kwargs.get("goal_th", np.pi)), + "reward_key_turn_w": _weighted_reward(kwargs, "key_turn", 1.0), + "reward_iftip_approach_w": _weighted_reward( + kwargs, "IFtip_approach", 10.0 + ), + "reward_thtip_approach_w": _weighted_reward( + kwargs, "THtip_approach", 10.0 + ), + "reward_act_reg_w": _weighted_reward(kwargs, "act_reg", 1.0), + "reward_bonus_w": _weighted_reward(kwargs, "bonus", 4.0), + "reward_penalty_w": _weighted_reward(kwargs, "penalty", 25.0), + "key_init_range": list(kwargs.get("key_init_range", (0.0, 0.0))), + } + + +def _obj_hold_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + model = _model(base_path, kwargs["model_path"]) + return { + "frame_skip": int(kwargs.get("frame_skip", 10)), + "model_path": str(kwargs["model_path"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": (model.nq - 7) + (model.nv - 6) + 6 + model.na, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.nu, + "randomize_on_reset": entry["class_name"] == "ObjHoldRandomEnvV0", + "reward_goal_dist_w": _weighted_reward(kwargs, "goal_dist", 100.0), + "reward_bonus_w": _weighted_reward(kwargs, "bonus", 4.0), + "reward_penalty_w": _weighted_reward(kwargs, "penalty", 10.0), + } + + +def _torso_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + model = _model(base_path, kwargs["model_path"]) + target_qpos_value = [ + (bounds[0] + bounds[1]) / 2.0 + for bounds in kwargs["target_jnt_range"].values() + ] + pose_dim = len(target_qpos_value) + return { + "frame_skip": int(kwargs.get("frame_skip", 5)), + "model_path": str(kwargs["model_path"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": model.nq + model.nv + pose_dim + model.na, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.nu, + "pose_dim": pose_dim, + "pose_thd": float(kwargs.get("pose_thd", 0.25)), + "reward_pose_w": _weighted_reward(kwargs, "pose", 1.0), + "reward_bonus_w": _weighted_reward(kwargs, "bonus", 4.0), + "reward_act_reg_w": _weighted_reward(kwargs, "act_reg", 1.0), + "reward_penalty_w": _weighted_reward(kwargs, "penalty", 50.0), + "target_qpos_value": target_qpos_value, + } + + +def _pen_twirl_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + model = _model(base_path, kwargs["model_path"]) + return { + "frame_skip": int(kwargs.get("frame_skip", 5)), + "model_path": str(kwargs["model_path"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": (model.nq - 6) + 21 + model.na, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.nu, + "randomize_target": entry["class_name"] == "PenTwirlRandomEnvV0", + "reward_pos_align_w": _weighted_reward(kwargs, "pos_align", 1.0), + "reward_rot_align_w": _weighted_reward(kwargs, "rot_align", 1.0), + "reward_act_reg_w": _weighted_reward(kwargs, "act_reg", 5.0), + "reward_drop_w": _weighted_reward(kwargs, "drop", 5.0), + "reward_bonus_w": _weighted_reward(kwargs, "bonus", 10.0), + } + + +def _walk_like_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + model = _model(base_path, kwargs["model_path"]) + return { + "frame_skip": int(kwargs.get("frame_skip", 10)), + "model_path": str(kwargs["model_path"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": (model.nq - 2) + + model.nv + + 2 + + 4 + + 2 + + 1 + + 6 + + 1 + + 3 * model.nu + + model.na, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.nu, + "min_height": float(kwargs.get("min_height", 0.8)), + "max_rot": float(kwargs.get("max_rot", 0.8)), + "hip_period": int(kwargs.get("hip_period", 100)), + "reset_type": str(kwargs.get("reset_type", "init")), + "target_x_vel": float(kwargs.get("target_x_vel", 0.0)), + "target_y_vel": float(kwargs.get("target_y_vel", 1.2)), + "target_rot": [] + if kwargs.get("target_rot") is None + else list(kwargs["target_rot"]), + "terrain": str(kwargs.get("terrain", "")), + "terrain_variant": "" + if kwargs.get("variant") is None + else str(kwargs["variant"]), + "use_knee_condition": entry["class_name"] == "TerrainEnvV0", + "reward_vel_w": _weighted_reward(kwargs, "vel_reward", 5.0), + "reward_done_w": _weighted_reward(kwargs, "done", -100.0), + "reward_cyclic_hip_w": _weighted_reward(kwargs, "cyclic_hip", -10.0), + "reward_ref_rot_w": _weighted_reward(kwargs, "ref_rot", 10.0), + "reward_joint_angle_w": _weighted_reward( + kwargs, "joint_angle_rew", 5.0 + ), + } + + +def _challenge_reorient_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + model = _model(base_path, kwargs["model_path"]) + obj_mass = kwargs.get("obj_mass_range", [0.108, 0.108]) + return { + "frame_skip": int(kwargs.get("frame_skip", 5)), + "model_path": str(kwargs["model_path"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": (model.nq - 7) + (model.nv - 6) + 18 + model.na, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.nu, + "goal_pos_low": float(kwargs.get("goal_pos", (0.0, 0.0))[0]), + "goal_pos_high": float(kwargs.get("goal_pos", (0.0, 0.0))[1]), + "goal_rot_low": float(kwargs.get("goal_rot", (0.0, 0.0))[0]), + "goal_rot_high": float(kwargs.get("goal_rot", (0.0, 0.0))[1]), + "obj_size_change": float(kwargs.get("obj_size_change", 0.0)), + "obj_mass_low": float(obj_mass[0]), + "obj_mass_high": float(obj_mass[1]), + "obj_friction_change": list( + kwargs.get("obj_friction_change", (0.0, 0.0, 0.0)) + ), + "pos_th": float(kwargs.get("pos_th", 0.025)), + "rot_th": float(kwargs.get("rot_th", 0.262)), + "drop_th": float(kwargs.get("drop_th", 0.2)), + "reward_pos_dist_w": _weighted_reward(kwargs, "pos_dist", 100.0), + "reward_rot_dist_w": _weighted_reward(kwargs, "rot_dist", 1.0), + "reward_bonus_w": _weighted_reward(kwargs, "bonus", 0.0), + "reward_act_reg_w": _weighted_reward(kwargs, "act_reg", 0.0), + "reward_penalty_w": _weighted_reward(kwargs, "penalty", 0.0), + } + + +def _challenge_relocate_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + model = _model(base_path, kwargs["model_path"]) + target_xyz_range = kwargs["target_xyz_range"] + target_rot_range = kwargs["target_rxryrz_range"] + obj_xyz_range = kwargs.get("obj_xyz_range") + obj_geom_range = kwargs.get("obj_geom_range") + obj_mass_range = kwargs.get("obj_mass_range") + obj_friction_range = kwargs.get("obj_friction_range") + return { + "frame_skip": int(kwargs.get("frame_skip", 5)), + "model_path": str(kwargs["model_path"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": (model.nq - 7) + (model.nv - 6) + 18 + model.na, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.nu, + "target_xyz_low": list(target_xyz_range["low"]), + "target_xyz_high": list(target_xyz_range["high"]), + "target_rxryrz_low": list(target_rot_range["low"]), + "target_rxryrz_high": list(target_rot_range["high"]), + "obj_xyz_low": [] + if obj_xyz_range is None + else list(obj_xyz_range["low"]), + "obj_xyz_high": [] + if obj_xyz_range is None + else list(obj_xyz_range["high"]), + "obj_geom_low": [] + if obj_geom_range is None + else list(obj_geom_range["low"]), + "obj_geom_high": [] + if obj_geom_range is None + else list(obj_geom_range["high"]), + "obj_mass_low": 0.0 + if obj_mass_range is None + else float(obj_mass_range["low"]), + "obj_mass_high": 0.0 + if obj_mass_range is None + else float(obj_mass_range["high"]), + "obj_friction_low": [] + if obj_friction_range is None + else list(obj_friction_range["low"]), + "obj_friction_high": [] + if obj_friction_range is None + else list(obj_friction_range["high"]), + "qpos_noise_range": float(kwargs.get("qpos_noise_range", 0.0)), + "pos_th": float(kwargs.get("pos_th", 0.025)), + "rot_th": float(kwargs.get("rot_th", 0.262)), + "drop_th": float(kwargs.get("drop_th", 0.5)), + "reward_pos_dist_w": _weighted_reward(kwargs, "pos_dist", 100.0), + "reward_rot_dist_w": _weighted_reward(kwargs, "rot_dist", 1.0), + "reward_act_reg_w": _weighted_reward(kwargs, "act_reg", 0.0), + } + + +def _baoding_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + import mujoco + + model = _model(base_path, kwargs["model_path"]) + ball1_gid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "ball1") + obj_size_range = kwargs.get("obj_size_range") + obj_mass_range = kwargs.get("obj_mass_range") + obj_friction_change = kwargs.get("obj_friction_change") + return { + "frame_skip": int(kwargs.get("frame_skip", 10)), + "model_path": str(kwargs["model_path"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": (model.nq - 14) + 24 + model.na, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.nu, + "drop_th": float(kwargs.get("drop_th", 1.25)), + "proximity_th": float(kwargs.get("proximity_th", 0.015)), + "goal_time_period_low": float( + kwargs.get("goal_time_period", (5, 5))[0] + ), + "goal_time_period_high": float( + kwargs.get("goal_time_period", (5, 5))[1] + ), + "goal_xrange_low": float(kwargs.get("goal_xrange", (0.025, 0.025))[0]), + "goal_xrange_high": float(kwargs.get("goal_xrange", (0.025, 0.025))[1]), + "goal_yrange_low": float(kwargs.get("goal_yrange", (0.028, 0.028))[0]), + "goal_yrange_high": float(kwargs.get("goal_yrange", (0.028, 0.028))[1]), + "task_choice": str(kwargs.get("task_choice", "fixed")), + "fixed_task": 2, + "reward_pos_dist_1_w": _weighted_reward(kwargs, "pos_dist_1", 5.0), + "reward_pos_dist_2_w": _weighted_reward(kwargs, "pos_dist_2", 5.0), + "obj_size_low": 0.0 + if obj_size_range is None + else float(obj_size_range[0]), + "obj_size_high": 0.0 + if obj_size_range is None + else float(obj_size_range[1]), + "obj_mass_low": 0.0 + if obj_mass_range is None + else float(obj_mass_range[0]), + "obj_mass_high": 0.0 + if obj_mass_range is None + else float(obj_mass_range[1]), + "obj_friction_low": [] + if obj_friction_change is None + else list( + ( + model.geom_friction[ball1_gid] - np.asarray(obj_friction_change) + ).tolist() + ), + "obj_friction_high": [] + if obj_friction_change is None + else list( + ( + model.geom_friction[ball1_gid] + np.asarray(obj_friction_change) + ).tolist() + ), + } + + +def _bimanual_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + model = _model(base_path, kwargs["model_path"]) + myo_qpos, myo_dof, prosth_qpos, prosth_dof, _, _ = _bimanual_index_sets( + model + ) + obj_bid = model.body("manip_object").id + obj_gid = model.body(obj_bid).geomadr + 1 + base_friction = np.asarray(model.geom_friction[obj_gid]).reshape(-1) + obj_mass_change = kwargs.get("obj_mass_change") + obj_friction_change = kwargs.get("obj_friction_change") + return { + "frame_skip": int(kwargs.get("frame_skip", 5)), + "model_path": str(kwargs["model_path"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": 1 + + len(myo_qpos) + + len(myo_dof) + + len(prosth_qpos) + + len(prosth_dof) + + 7 + + 6 + + 5 + + model.na, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.nu, + "proximity_th": 0.17, + "start_center": [-0.4, -0.25, 1.05], + "goal_center": [0.4, -0.25, 1.05], + "start_shifts": [0.055, 0.055, 0.0], + "goal_shifts": [0.098, 0.098, 0.0], + "reward_reach_dist_w": _weighted_reward(kwargs, "reach_dist", -0.1), + "reward_act_w": _weighted_reward(kwargs, "act", 0.0), + "reward_fin_dis_w": _weighted_reward(kwargs, "fin_dis", -0.5), + "reward_pass_err_w": _weighted_reward(kwargs, "pass_err", -1.0), + "obj_scale_change": list(kwargs.get("obj_scale_change", [])), + "obj_mass_low": 0.0 + if obj_mass_change is None + else float(model.body_mass[obj_bid] + obj_mass_change[0]), + "obj_mass_high": 0.0 + if obj_mass_change is None + else float(model.body_mass[obj_bid] + obj_mass_change[1]), + "obj_friction_low": [] + if obj_friction_change is None + else list((base_friction - np.asarray(obj_friction_change)).tolist()), + "obj_friction_high": [] + if obj_friction_change is None + else list((base_friction + np.asarray(obj_friction_change)).tolist()), + } + + +def _runtrack_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + model = _model(base_path, kwargs["model_path"]) + return { + "frame_skip": int(kwargs.get("frame_skip", 5)), + "model_path": str(kwargs["model_path"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": 17 + 17 + 2 + 4 + model.na * 4 + 2 + 2, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.na, + "ctrl_dim": model.nu, + "reset_type": str(kwargs.get("reset_type", "random")), + "terrain": str(kwargs.get("terrain", "flat")), + "start_pos": float(kwargs.get("start_pos", 14)), + "end_pos": float(kwargs.get("end_pos", -15)), + "real_width": float(kwargs.get("real_width", 1)), + "hills_difficulties": list(kwargs.get("hills_difficulties", [])), + "rough_difficulties": list(kwargs.get("rough_difficulties", [])), + "stairs_difficulties": list(kwargs.get("stairs_difficulties", [])), + "reward_sparse_w": 1.0, + "reward_solved_w": 10.0, + } + + +def _soccer_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + import mujoco + + model = _model(base_path, kwargs["model_path"]) + internal_joint_count = sum( + 1 + for joint_id in range(model.njnt) + if mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, joint_id) + not in (None, "root") + ) + probabilities = kwargs.get("goalkeeper_probabilities", [0.1, 0.45, 0.45]) + return { + "frame_skip": int(kwargs.get("frame_skip", 10)), + "model_path": str(kwargs["model_path"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": internal_joint_count + + internal_joint_count + + 4 + + 4 + + 3 + + 7 + + 6 + + model.na * 4, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.nu, + "reset_type": str(kwargs.get("reset_type", "none")), + "min_agent_spawn_distance": float( + kwargs.get("min_agent_spawn_distance", 1) + ), + "random_vel_low": float(kwargs.get("random_vel_range", [1.0, 5.0])[0]), + "random_vel_high": float(kwargs.get("random_vel_range", [1.0, 5.0])[1]), + "rnd_pos_noise": float(kwargs.get("rnd_pos_noise", 1.0)), + "rnd_joint_noise": float(kwargs.get("rnd_joint_noise", 0.02)), + "goalkeeper_probabilities": list(probabilities), + "max_time_sec": float(kwargs.get("max_time_sec", 10)), + "reward_goal_scored_w": 1000.0, + "reward_time_cost_w": -0.01, + "reward_act_reg_w": -100.0, + "reward_pain_w": -10.0, + } + + +def _chasetag_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + model = _model(base_path, kwargs["model_path"]) + return { + "frame_skip": int(kwargs.get("frame_skip", 10)), + "model_path": str(kwargs["model_path"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": 28 + 28 + 4 + 4 + 3 + 2 + 2 + 2 + model.na * 4, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.nu, + "reset_type": str(kwargs.get("reset_type", "init")), + "win_distance": float(kwargs.get("win_distance", 0.5)), + "min_spawn_distance": float(kwargs.get("min_spawn_distance", 2)), + "task_choice": str(kwargs.get("task_choice", "CHASE")), + "terrain": str(kwargs.get("terrain", "FLAT")), + "repeller_opponent": bool(kwargs.get("repeller_opponent", False)), + "hills_range": list(kwargs.get("hills_range", [0.0, 0.0])), + "rough_range": list(kwargs.get("rough_range", [0.0, 0.0])), + "relief_range": list(kwargs.get("relief_range", [0.0, 0.0])), + "chase_vel_low": float(kwargs.get("chase_vel_range", [1.0, 1.0])[0]), + "chase_vel_high": float(kwargs.get("chase_vel_range", [1.0, 1.0])[1]), + "random_vel_low": float(kwargs.get("random_vel_range", [1.0, 1.0])[0]), + "random_vel_high": float(kwargs.get("random_vel_range", [1.0, 1.0])[1]), + "repeller_vel_low": float( + kwargs.get("repeller_vel_range", [1.0, 1.0])[0] + ), + "repeller_vel_high": float( + kwargs.get("repeller_vel_range", [1.0, 1.0])[1] + ), + "opponent_probabilities": list( + kwargs.get("opponent_probabilities", [0.1, 0.45, 0.45]) + ), + "reward_distance_w": -0.1, + "reward_lose_w": -1000.0, + } + + +def _tabletennis_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + import mujoco + + model = _model(base_path, kwargs["model_path"]) + body_qpos_count = sum( + 1 + for joint_id in range(model.njnt) + if ( + ( + name := mujoco.mj_id2name( + model, mujoco.mjtObj.mjOBJ_JOINT, joint_id + ) + ) + is not None + and not name.startswith("ping") + and name != "pingpong_freejoint" + and name != "paddle_freejoint" + ) + ) + body_dof_count = sum( + 1 + for joint_id in range(model.njnt) + if ( + ( + name := mujoco.mj_id2name( + model, mujoco.mjtObj.mjOBJ_JOINT, joint_id + ) + ) + is not None + and not name.startswith("ping") + and name != "paddle_freejoint" + ) + ) + ball_xyz_range = kwargs.get("ball_xyz_range") or {} + ball_friction_range = kwargs.get("ball_friction_range") or {} + paddle_mass_range = kwargs.get("paddle_mass_range") or [0.0, 0.0] + qpos_noise_range = kwargs.get("qpos_noise_range") + qpos_noise_low = float("nan") + qpos_noise_high = float("nan") + if qpos_noise_range is not None: + qpos_noise_low = -float(qpos_noise_range) + qpos_noise_high = float(qpos_noise_range) + return { + "frame_skip": int(kwargs.get("frame_skip", 5)), + "model_path": str(kwargs["model_path"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": 3 + + body_qpos_count + + body_dof_count + + 3 + + 3 + + 3 + + 3 + + 4 + + 3 + + 6 + + model.na, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.nu, + "ball_xyz_low": list(ball_xyz_range.get("low", [])), + "ball_xyz_high": list(ball_xyz_range.get("high", [])), + "ball_qvel": bool(kwargs.get("ball_qvel", False)), + "ball_friction_low": list(ball_friction_range.get("low", [])), + "ball_friction_high": list(ball_friction_range.get("high", [])), + "paddle_mass_low": float(paddle_mass_range[0]), + "paddle_mass_high": float(paddle_mass_range[1]), + "qpos_noise_low": qpos_noise_low, + "qpos_noise_high": qpos_noise_high, + "rally_count": int(kwargs.get("rally_count", 1)), + "reward_reach_dist_w": 1.0, + "reward_palm_dist_w": 1.0, + "reward_paddle_quat_w": 2.0, + "reward_act_reg_w": 0.5, + "reward_torso_up_w": 2.0, + "reward_sparse_w": 100.0, + "reward_solved_w": 1000.0, + "reward_done_w": -10.0, + } + + +def _track_config( + base_path: str, entry: dict[str, Any], kwargs: dict[str, Any] +) -> dict[str, Any]: + model = _track_model(base_path, kwargs["model_path"], kwargs["object_name"]) + reference = _reference_config(base_path, kwargs["reference"]) + obs_dim = ( + model.nq + + model.nv + + reference["robot_dim"] + + ( + reference["robot_dim"] + if reference["reference_has_robot_vel"] + else 1 + ) + + 3 + + model.na + ) + return { + "frame_skip": int(kwargs.get("frame_skip", 10)), + "model_path": str(kwargs["model_path"]), + "object_name": str(kwargs["object_name"]), + "reference_path": str(reference["reference_path"]), + "reference_time": list(reference["reference_time"]), + "reference_robot": list(reference["reference_robot"]), + "reference_robot_vel": list(reference["reference_robot_vel"]), + "reference_object": list(reference["reference_object"]), + "reference_robot_init": list(reference["reference_robot_init"]), + "reference_object_init": list(reference["reference_object_init"]), + "normalize_act": bool(kwargs.get("normalize_act", True)), + "obs_dim": obs_dim, + "qpos_dim": model.nq, + "qvel_dim": model.nv, + "act_dim": model.na, + "action_dim": model.nu, + "robot_dim": reference["robot_dim"], + "object_dim": reference["object_dim"], + "robot_horizon": reference["robot_horizon"], + "object_horizon": reference["object_horizon"], + "reference_has_robot_vel": bool(reference["reference_has_robot_vel"]), + "motion_start_time": float(kwargs.get("motion_start_time", 0.0)), + "motion_extrapolation": bool(kwargs.get("motion_extrapolation", True)), + "reward_pose_w": _weighted_reward(kwargs, "pose", 0.0), + "reward_object_w": _weighted_reward(kwargs, "object", 1.0), + "reward_bonus_w": _weighted_reward(kwargs, "bonus", 1.0), + "reward_penalty_w": _weighted_reward(kwargs, "penalty", -2.0), + "terminate_obj_fail": bool( + kwargs.get( + "Termimate_obj_fail", kwargs.get("terminate_obj_fail", True) + ) + ), + "terminate_pose_fail": bool( + kwargs.get( + "Termimate_pose_fail", kwargs.get("terminate_pose_fail", False) + ) + ), + } + + +def _resolve_dynamic_myosuite_task_config( + entry: dict[str, Any], + variant_kwargs: dict[str, Any], + preview_kwargs: dict[str, Any], +) -> dict[str, Any]: + kwargs = _prepare_runtime_kwargs({ + **entry["kwargs"], + **variant_kwargs, + **preview_kwargs, + }) + base_path = str(preview_kwargs["base_path"]) + class_name = entry["class_name"] + suite = entry["suite"] + + if class_name == "PoseEnvV0": + config = _pose_config(base_path, entry, kwargs) + elif class_name == "ReachEnvV0": + config = _reach_config(base_path, entry, kwargs) + elif class_name in { + "Geometries100EnvV0", + "Geometries8EnvV0", + "InDistribution", + "OutofDistribution", + }: + config = _myobase_reorient_config(base_path, entry, kwargs) + elif class_name == "KeyTurnEnvV0": + config = _key_turn_config(base_path, entry, kwargs) + elif class_name in {"ObjHoldFixedEnvV0", "ObjHoldRandomEnvV0"}: + config = _obj_hold_config(base_path, entry, kwargs) + elif class_name == "TorsoEnvV0": + config = _torso_config(base_path, entry, kwargs) + elif class_name in {"PenTwirlFixedEnvV0", "PenTwirlRandomEnvV0"}: + config = _pen_twirl_config(base_path, entry, kwargs) + elif class_name in {"WalkEnvV0", "TerrainEnvV0"}: + config = _walk_like_config(base_path, entry, kwargs) + elif suite == "myochallenge" and class_name == "ReorientEnvV0": + config = _challenge_reorient_config(base_path, entry, kwargs) + elif class_name == "RelocateEnvV0": + config = _challenge_relocate_config(base_path, entry, kwargs) + elif class_name == "BaodingEnvV1": + config = _baoding_config(base_path, entry, kwargs) + elif class_name == "BimanualEnvV1": + config = _bimanual_config(base_path, entry, kwargs) + elif class_name == "RunTrack": + config = _runtrack_config(base_path, entry, kwargs) + elif class_name == "SoccerEnvV0": + config = _soccer_config(base_path, entry, kwargs) + elif class_name == "ChaseTagEnvV0": + config = _chasetag_config(base_path, entry, kwargs) + elif class_name == "TableTennisEnvV0": + config = _tabletennis_config(base_path, entry, kwargs) + elif class_name == "TrackEnv": + config = _track_config(base_path, entry, kwargs) + else: + raise ValueError(f"Unsupported MyoSuite class_name: {class_name}") + + if "muscle_condition" in kwargs: + config["muscle_condition"] = str(kwargs["muscle_condition"]) + supported_test_keys = _supported_preview_test_keys(entry) + for key, value in preview_kwargs.items(): + if key in supported_test_keys: + config[key] = value + return config + + +def generate_myosuite_task_config( + entry: dict[str, Any], + variant_kwargs: dict[str, Any], + *, + base_path: str, +) -> dict[str, Any]: + """Generate a canonical task config for vendored metadata snapshots.""" + return _resolve_dynamic_myosuite_task_config( + entry, + variant_kwargs, + {"base_path": base_path}, + ) + + +def _has_dynamic_task_overrides( + entry: dict[str, Any], + variant_kwargs: dict[str, Any], + preview_kwargs: dict[str, Any], +) -> bool: + # Muscle-condition variants can change structural model metadata such as + # actuator count, so they must bypass baked default configs. + if variant_kwargs: + return True + defaults = {**entry["kwargs"], **variant_kwargs} + for key, value in preview_kwargs.items(): + if key == "base_path" or key.startswith("test_"): + continue + if key in defaults and defaults[key] != value: + return True + return False + + +def resolve_myosuite_task_config( + task_id: str, preview_kwargs: dict[str, Any] +) -> dict[str, Any]: + """Resolve a public MyoSuite task ID into spec kwargs for registration.""" + entry, variant_kwargs = myosuite_expanded_entry(task_id) + if _has_dynamic_task_overrides(entry, variant_kwargs, preview_kwargs): + return _resolve_dynamic_myosuite_task_config( + entry, variant_kwargs, preview_kwargs + ) + + default_config = entry.get("default_config") + if default_config is None: + return _resolve_dynamic_myosuite_task_config( + entry, variant_kwargs, preview_kwargs + ) + + config = dict(default_config) + muscle_condition = preview_kwargs.get( + "muscle_condition", + variant_kwargs.get("muscle_condition"), + ) + if muscle_condition is not None: + config["muscle_condition"] = str(muscle_condition) + supported_test_keys = _supported_preview_test_keys(entry) + for key, value in preview_kwargs.items(): + if key in supported_test_keys: + config[key] = value + return config diff --git a/envpool/mujoco/myosuite/metadata.py b/envpool/mujoco/myosuite/metadata.py new file mode 100644 index 000000000..c70e2daa9 --- /dev/null +++ b/envpool/mujoco/myosuite/metadata.py @@ -0,0 +1,72 @@ +# Copyright 2026 Garena Online Private 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. +"""Generated MyoSuite surface metadata used by tests and registration.""" + +from __future__ import annotations + +import json +from functools import cache +from typing import Any + +from envpool.mujoco.myosuite.paths import myosuite_metadata_path + +__all__ = [ + "MYOSUITE_COUNTS", + "MYOSUITE_DIRECT_ENTRIES", + "MYOSUITE_DIRECT_ENTRY_BY_ID", + "MYOSUITE_DIRECT_IDS", + "MYOSUITE_EXPANDED_IDS", + "MYOSUITE_NOTES", + "MYOSUITE_PINS", + "MYOSUITE_SUITES", + "load_myosuite_metadata", +] + + +@cache +def load_myosuite_metadata() -> dict[str, Any]: + """Load the generated MyoSuite metadata bundle.""" + return json.loads(myosuite_metadata_path().read_text()) + + +_METADATA = load_myosuite_metadata() + +MYOSUITE_PINS: dict[str, Any] = dict(_METADATA["pins"]) +MYOSUITE_COUNTS: dict[str, int] = dict(_METADATA["counts"]) +MYOSUITE_NOTES: dict[str, Any] = dict(_METADATA["notes"]) +MYOSUITE_DIRECT_ENTRIES: tuple[dict[str, Any], ...] = tuple( + _METADATA["direct_entries"] +) +MYOSUITE_DIRECT_ENTRY_BY_ID: dict[str, dict[str, Any]] = { + entry["id"]: entry for entry in MYOSUITE_DIRECT_ENTRIES +} +MYOSUITE_DIRECT_IDS: tuple[str, ...] = tuple( + _METADATA["direct_ids"] +) # Re-exported for tests and public registration helpers. +MYOSUITE_EXPANDED_IDS: tuple[str, ...] = tuple( + _METADATA["expanded_ids"] +) # Includes variants; consumed by public task registration. +MYOSUITE_SUITES: dict[str, Any] = dict(_METADATA["suites"]) + + +def _validate_metadata_constants() -> None: + if len(MYOSUITE_DIRECT_IDS) != MYOSUITE_COUNTS["direct_total"]: + raise ValueError("MyoSuite direct metadata count is inconsistent.") + if len(MYOSUITE_EXPANDED_IDS) != MYOSUITE_COUNTS["expanded_total"]: + raise ValueError("MyoSuite expanded metadata count is inconsistent.") + if not set(MYOSUITE_DIRECT_IDS).issubset(MYOSUITE_EXPANDED_IDS): + raise ValueError("MyoSuite expanded IDs must include direct IDs.") + + +_validate_metadata_constants() diff --git a/envpool/mujoco/myosuite/mujoco_plugin_init.cc b/envpool/mujoco/myosuite/mujoco_plugin_init.cc new file mode 100644 index 000000000..f58cf0a64 --- /dev/null +++ b/envpool/mujoco/myosuite/mujoco_plugin_init.cc @@ -0,0 +1,29 @@ +// Copyright 2026 Garena Online Private 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. + +#ifdef _WIN32 + +#include + +extern "C" int __stdcall MjObjDecoderDllMain(void* hinst, DWORD reason, + void* reserved); +extern "C" int __stdcall MjStlDecoderDllMain(void* hinst, DWORD reason, + void* reserved); + +extern "C" BOOL WINAPI DllMain(HINSTANCE hinst, DWORD reason, LPVOID reserved) { + return MjObjDecoderDllMain(hinst, reason, reserved) && + MjStlDecoderDllMain(hinst, reason, reserved); +} + +#endif // _WIN32 diff --git a/envpool/mujoco/myosuite/myobase.h b/envpool/mujoco/myosuite/myobase.h new file mode 100644 index 000000000..99a56f58a --- /dev/null +++ b/envpool/mujoco/myosuite/myobase.h @@ -0,0 +1,3216 @@ +// Copyright 2026 Garena Online Private 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 ENVPOOL_MUJOCO_MYOSUITE_MYOBASE_H_ +#define ENVPOOL_MUJOCO_MYOSUITE_MYOBASE_H_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "envpool/core/async_envpool.h" +#include "envpool/core/env.h" +#include "envpool/mujoco/myosuite/paths.h" +#include "envpool/mujoco/robotics/mujoco_env.h" + +namespace myosuite_envpool { + +using envpool::mujoco::PixelObservationEnvFns; +using envpool::mujoco::RenderCameraIdOrDefault; +using envpool::mujoco::RenderHeightOrDefault; +using envpool::mujoco::RenderWidthOrDefault; +using envpool::mujoco::StackSpec; + +namespace detail { + +[[maybe_unused]] constexpr mjtNum kPi = 3.14159265358979323846; +constexpr mjtNum kPoseFarThreshold = static_cast(6.283185307179586); + +inline std::vector ToMjtVector(const std::vector& input) { + return {input.begin(), input.end()}; +} + +inline mjtNum ClampNormalized(mjtNum value) { + return std::clamp(value, static_cast(-1.0), static_cast(1.0)); +} + +inline mjtNum MuscleActivation(mjtNum value) { + // Official MyoSuite applies the logistic transform in-place on the + // float32 action array before assigning it into MuJoCo's ctrl buffer. + auto action = static_cast(value); + float activation = 1.0F / (1.0F + std::exp(-5.0F * (action - 0.5F))); + return static_cast(activation); +} + +enum class MuscleCondition : std::uint8_t { + kNone, + kSarcopenia, + kFatigue, + kReafferentation, +}; + +class CumulativeFatigueModel { + public: + void Initialize(const mjModel* model, int frame_skip, int seed) { + dt_ = model->opt.timestep * static_cast(frame_skip); + tauact_.clear(); + taudeact_.clear(); + for (int i = 0; i < model->nu; ++i) { + if (model->actuator_dyntype[i] != mjDYN_MUSCLE) { + continue; + } + tauact_.push_back(model->actuator_dynprm[i * mjNDYN + 0]); + taudeact_.push_back(model->actuator_dynprm[i * mjNDYN + 1]); + } + fatigue_gen_.seed(seed); + Reset({}, false); + } + + void Reset(const std::vector& fatigue_reset_vec, + bool fatigue_reset_random) { + std::size_t count = tauact_.size(); + ma_.assign(count, 0.0); + mr_.assign(count, 1.0); + mf_.assign(count, 0.0); + tl_.assign(count, 0.0); + ld_.assign(count, 0.0); + lr_.assign(count, 0.0); + if (count == 0) { + return; + } + if (fatigue_reset_random) { + std::uniform_real_distribution dist(0.0, 1.0); + for (std::size_t i = 0; i < count; ++i) { + const auto non_fatigued = static_cast(dist(fatigue_gen_)); + const auto active = static_cast(dist(fatigue_gen_)); + ma_[i] = non_fatigued * active; + mr_[i] = non_fatigued * (1.0 - active); + mf_[i] = 1.0 - non_fatigued; + } + return; + } + if (!fatigue_reset_vec.empty()) { + if (fatigue_reset_vec.size() != count) { + throw std::runtime_error( + "fatigue_reset_vec length does not match muscle count."); + } + for (std::size_t i = 0; i < count; ++i) { + mf_[i] = fatigue_reset_vec[i]; + mr_[i] = 1.0 - fatigue_reset_vec[i]; + } + } + } + + void ComputeAct(std::vector* act) { + if (act->empty()) { + return; + } + if (act->size() != tauact_.size()) { + throw std::runtime_error("Fatigue control size mismatch."); + } + tl_ = *act; + for (std::size_t i = 0; i < act->size(); ++i) { + ld_[i] = (1.0 / tauact_[i]) * (0.5 + 1.5 * ma_[i]); + lr_[i] = (0.5 + 1.5 * ma_[i]) / taudeact_[i]; + } + for (std::size_t i = 0; i < act->size(); ++i) { + mjtNum c = 0.0; + if (ma_[i] < tl_[i] && mr_[i] > (tl_[i] - ma_[i])) { + c = ld_[i] * (tl_[i] - ma_[i]); + } else if (ma_[i] < tl_[i]) { + c = ld_[i] * mr_[i]; + } else { + c = lr_[i] * (tl_[i] - ma_[i]); + } + mjtNum rr = + ma_[i] >= tl_[i] ? recovery_multiplier_ * recovery_ : recovery_; + mjtNum min_c = std::max(-ma_[i] / dt_ + fatigue_ * ma_[i], + (mr_[i] - 1.0) / dt_ + rr * mf_[i]); + mjtNum max_c = std::min((1.0 - ma_[i]) / dt_ + fatigue_ * ma_[i], + mr_[i] / dt_ + rr * mf_[i]); + c = std::clamp(c, min_c, max_c); + mjtNum d_ma = (c - fatigue_ * ma_[i]) * dt_; + mjtNum d_mr = (-c + rr * mf_[i]) * dt_; + mjtNum d_mf = (fatigue_ * ma_[i] - rr * mf_[i]) * dt_; + ma_[i] += d_ma; + mr_[i] += d_mr; + mf_[i] += d_mf; + } + *act = ma_; + } + + private: + mjtNum recovery_multiplier_{150.0}; + mjtNum fatigue_{0.00912}; + mjtNum recovery_{0.000094}; + mjtNum dt_{0.0}; + std::mt19937 fatigue_gen_; + std::vector tauact_; + std::vector taudeact_; + std::vector ma_; + std::vector mr_; + std::vector mf_; + std::vector tl_; + std::vector ld_; + std::vector lr_; +}; + +struct MyoConditionState { + MuscleCondition muscle_condition{MuscleCondition::kNone}; + CumulativeFatigueModel fatigue_model; + std::vector fatigue_reset_vec; + bool fatigue_reset_random{false}; + int epl_index{-1}; + int eip_index{-1}; +}; + +class NumpyPcg64 { + public: + explicit NumpyPcg64(std::uint64_t seed) { Seed(seed); } + + void Seed(std::uint64_t seed) { + auto seed_words = SeedSequenceWords(seed); + InitState({seed_words[0], seed_words[1]}, {seed_words[2], seed_words[3]}); + } + + double UniformDouble(double low, double high) { + return low + (high - low) * NextDouble(); + } + + mjtNum UniformMjt(mjtNum low, mjtNum high) { + return static_cast( + UniformDouble(static_cast(low), static_cast(high))); + } + + private: + struct Uint128 { + std::uint64_t high; + std::uint64_t low; + }; + + static constexpr std::uint32_t kInitA = 0x43b0d7e5U; + static constexpr std::uint32_t kMultA = 0x931e8875U; + static constexpr std::uint32_t kInitB = 0x8b51f9ddU; + static constexpr std::uint32_t kMultB = 0x58f38dedU; + static constexpr std::uint32_t kMixMultL = 0xca01f9ddU; + static constexpr std::uint32_t kMixMultR = 0x4973f715U; + static constexpr int kXShift = 16; + static constexpr std::uint64_t kPcgMultiplierHigh = 2549297995355413924ULL; + static constexpr std::uint64_t kPcgMultiplierLow = 4865540595714422341ULL; + + static Uint128 Add128(Uint128 lhs, Uint128 rhs) { + Uint128 out{}; + out.low = lhs.low + rhs.low; + out.high = lhs.high + rhs.high + (out.low < rhs.low ? 1ULL : 0ULL); + return out; + } + + static void Mul64Wide(std::uint64_t lhs, std::uint64_t rhs, + std::uint64_t* high, std::uint64_t* low) { + *low = lhs * rhs; + std::uint64_t lhs_lo = lhs & 0xFFFFFFFFULL; + std::uint64_t lhs_hi = lhs >> 32; + std::uint64_t rhs_lo = rhs & 0xFFFFFFFFULL; + std::uint64_t rhs_hi = rhs >> 32; + std::uint64_t w0 = lhs_lo * rhs_lo; + std::uint64_t t = lhs_hi * rhs_lo + (w0 >> 32); + std::uint64_t w1 = t & 0xFFFFFFFFULL; + std::uint64_t w2 = t >> 32; + w1 += lhs_lo * rhs_hi; + *high = lhs_hi * rhs_hi + w2 + (w1 >> 32); + } + + static Uint128 Mul128(Uint128 lhs, Uint128 rhs) { + Uint128 out{}; + Mul64Wide(lhs.low, rhs.low, &out.high, &out.low); + out.high += lhs.high * rhs.low + lhs.low * rhs.high; + return out; + } + + static std::uint64_t RotR64(std::uint64_t value, unsigned int rot) { + rot &= 63U; + if (rot == 0U) { + return value; + } + return (value >> rot) | (value << ((64U - rot) & 63U)); + } + + static std::vector IntToUint32Array(std::uint64_t value) { + std::vector words; + if (value == 0) { + words.push_back(0U); + return words; + } + while (value > 0) { + words.push_back(static_cast(value & 0xFFFFFFFFULL)); + value >>= 32; + } + return words; + } + + static std::uint32_t HashMix(std::uint32_t value, std::uint32_t* hash_const) { + value ^= *hash_const; + *hash_const *= kMultA; + value *= *hash_const; + value ^= value >> kXShift; + return value; + } + + static std::uint32_t Mix(std::uint32_t lhs, std::uint32_t rhs) { + std::uint32_t value = kMixMultL * lhs - kMixMultR * rhs; + value ^= value >> kXShift; + return value; + } + + static std::array SeedSequenceWords(std::uint64_t seed) { + std::vector entropy = IntToUint32Array(seed); + std::array pool{}; + std::uint32_t hash_const = kInitA; + for (std::size_t i = 0; i < pool.size(); ++i) { + pool[i] = HashMix(i < entropy.size() ? entropy[i] : 0U, &hash_const); + } + for (std::size_t src = 0; src < pool.size(); ++src) { + for (std::size_t dst = 0; dst < pool.size(); ++dst) { + if (src == dst) { + continue; + } + pool[dst] = Mix(pool[dst], HashMix(pool[src], &hash_const)); + } + } + for (std::size_t src = pool.size(); src < entropy.size(); ++src) { + for (auto& value : pool) { + value = Mix(value, HashMix(entropy[src], &hash_const)); + } + } + + std::array state32{}; + hash_const = kInitB; + for (std::size_t i = 0; i < state32.size(); ++i) { + std::uint32_t value = pool[i % pool.size()]; + value ^= hash_const; + hash_const *= kMultB; + value *= hash_const; + value ^= value >> kXShift; + state32[i] = value; + } + return { + (static_cast(state32[1]) << 32) | state32[0], + (static_cast(state32[3]) << 32) | state32[2], + (static_cast(state32[5]) << 32) | state32[4], + (static_cast(state32[7]) << 32) | state32[6], + }; + } + + void InitState(Uint128 init_state, Uint128 init_seq) { + state_ = {0ULL, 0ULL}; + inc_ = { + (init_seq.high << 1U) | (init_seq.low >> 63U), + (init_seq.low << 1U) | 1ULL, + }; + Step(); + state_ = Add128(state_, init_state); + Step(); + } + + void Step() { + state_ = + Add128(Mul128(state_, {kPcgMultiplierHigh, kPcgMultiplierLow}), inc_); + } + + std::uint64_t NextUInt64() { + Step(); + return RotR64(state_.high ^ state_.low, + static_cast(state_.high >> 58U)); + } + + double NextDouble() { + constexpr double scale = 1.0 / 9007199254740992.0; + return static_cast(NextUInt64() >> 11U) * scale; + } + + Uint128 state_{0ULL, 0ULL}; + Uint128 inc_{0ULL, 0ULL}; +}; + +inline MuscleCondition ParseMuscleCondition(std::string_view value) { + if (value.empty()) { + return MuscleCondition::kNone; + } + if (value == "sarcopenia") { + return MuscleCondition::kSarcopenia; + } + if (value == "fatigue") { + return MuscleCondition::kFatigue; + } + if (value == "reafferentation") { + return MuscleCondition::kReafferentation; + } + throw std::runtime_error("Unsupported MyoSuite muscle_condition."); +} + +inline void InitializeMyoConditionState( + mjModel* model, std::string_view muscle_condition, + const std::vector& fatigue_reset_vec, bool fatigue_reset_random, + int frame_skip, int seed, MyoConditionState* state) { + state->muscle_condition = ParseMuscleCondition(muscle_condition); + state->fatigue_reset_vec = ToMjtVector(fatigue_reset_vec); + state->fatigue_reset_random = fatigue_reset_random; + if (state->muscle_condition == MuscleCondition::kSarcopenia) { + for (int mus_idx = 0; mus_idx < model->nu; ++mus_idx) { + model->actuator_gainprm[mus_idx * mjNGAIN + 2] *= + static_cast(0.5); + } + return; + } + if (state->muscle_condition == MuscleCondition::kFatigue) { + state->fatigue_model.Initialize(model, frame_skip, seed); + return; + } + if (state->muscle_condition == MuscleCondition::kReafferentation) { + state->epl_index = mj_name2id(model, mjOBJ_ACTUATOR, "EPL"); + state->eip_index = mj_name2id(model, mjOBJ_ACTUATOR, "EIP"); + if (state->epl_index < 0 || state->eip_index < 0) { + throw std::runtime_error( + "Reafferentation requires EPL and EIP actuators."); + } + } +} + +inline void ResetMyoConditionState(MyoConditionState* state) { + if (state->muscle_condition == MuscleCondition::kFatigue) { + state->fatigue_model.Reset(state->fatigue_reset_vec, + state->fatigue_reset_random); + } +} + +inline void ApplyMyoConditionAdjustments( + const mjModel* model, mjData* data, + const std::vector& muscle_actuator, MyoConditionState* state) { + if (state->muscle_condition == MuscleCondition::kNone || + state->muscle_condition == MuscleCondition::kSarcopenia) { + return; + } + if (state->muscle_condition == MuscleCondition::kReafferentation) { + data->ctrl[state->epl_index] = data->ctrl[state->eip_index]; + data->ctrl[state->eip_index] = 0.0; + return; + } + std::vector muscle_ctrl; + muscle_ctrl.reserve(model->nu); + for (int i = 0; i < model->nu; ++i) { + if (muscle_actuator[i]) { + muscle_ctrl.push_back(data->ctrl[i]); + } + } + if (muscle_ctrl.empty()) { + return; + } + state->fatigue_model.ComputeAct(&muscle_ctrl); + int muscle_index = 0; + for (int i = 0; i < model->nu; ++i) { + if (muscle_actuator[i]) { + // Official BaseV0 carries actions in a float32 numpy array. Fatigue + // updates feed double-precision values back into that float32 buffer + // before Robot.step copies ctrl into MuJoCo. Mirror that round-trip so + // actuator state and render overlays remain bitwise aligned. + data->ctrl[i] = + static_cast(static_cast(muscle_ctrl[muscle_index++])); + } + } +} + +// MyoSuite advances its main simulation through Robot.step() -> +// SimScene.advance(), which delegates to dm_control Physics.step(substeps) +// with legacy_step enabled. Mirror that exact mj_step2 / mj_step / mj_step1 +// sequence so simulation and render-visible state stay bitwise aligned. +inline void DoMyoSuiteSimulation(const mjModel* model, mjData* data, + int frame_skip) { + if (frame_skip <= 0) { + mj_forward(model, data); + return; + } + if (model->opt.integrator != mjINT_RK4) { + mj_step2(model, data); + for (int i = 1; i < frame_skip; ++i) { + mj_step(model, data); + } + } else { + for (int i = 0; i < frame_skip; ++i) { + mj_step(model, data); + } + } + mj_step1(model, data); +} + +// Reset-sync previews inject qpos/qvel/ctrl/act directly into the live mjData +// after model randomization. Re-run MuJoCo's stage-1 reconstruction so the +// first rendered frame and legacy-step transition start from the oracle state. +inline void FinalizeMyoSuiteResetSync(const mjModel* model, mjData* data) { + mj_step1(model, data); +} + +// Official MyoSuite calls Robot.get_sensors() followed by +// Robot.sensor2sim(..., sim_obsd) inside env_base.get_obs(). When +// `obsd_model_path` aliases the main simulation, that replay writes +// qpos/qvel/act back into the same mjData and runs `forward()` once more +// before any +// observation/reward code reads force- and contact-derived fields. Mirror that +// post-step reconstruction so GRF, actuator_force, qacc, and future warmstarts +// match the oracle exactly. +inline void RefreshObservedMyoSuiteState(const mjModel* model, mjData* data) { + mj_forward(model, data); +} + +inline mjtNum VectorNorm(const std::vector& value) { + mjtNum total = 0.0; + for (mjtNum item : value) { + total += item * item; + } + return std::sqrt(total); +} + +inline mjtNum VectorDot(const std::vector& lhs, + const std::vector& rhs) { + mjtNum total = 0.0; + for (std::size_t i = 0; i < lhs.size(); ++i) { + total += lhs[i] * rhs[i]; + } + return total; +} + +inline mjtNum CosineSimilarity(const std::vector& lhs, + const std::vector& rhs) { + mjtNum lhs_norm = VectorNorm(lhs); + mjtNum rhs_norm = VectorNorm(rhs); + if (lhs_norm == 0.0 || rhs_norm == 0.0) { + return 0.0; + } + return VectorDot(lhs, rhs) / (lhs_norm * rhs_norm); +} + +inline std::vector CurrentAct(const mjModel* model, + const mjData* data) { + if (model->na == 0) { + return {}; + } + return {data->act, data->act + model->na}; +} + +inline mjtNum ActReg(const mjModel* model, const mjData* data) { + if (model->na == 0) { + return 0.0; + } + return VectorNorm(CurrentAct(model, data)) / static_cast(model->na); +} + +inline std::vector CopyQpos(const mjModel* model, const mjData* data) { + return {data->qpos, data->qpos + model->nq}; +} + +inline std::vector CopyQvel(const mjModel* model, const mjData* data, + mjtNum dt) { + std::vector qvel(model->nv); + for (int i = 0; i < model->nv; ++i) { + qvel[i] = data->qvel[i] * dt; + } + return qvel; +} + +inline void RestoreVector(const std::vector& src, mjtNum* dst) { + if (!src.empty()) { + std::memcpy(dst, src.data(), sizeof(mjtNum) * src.size()); + } +} + +inline void CopySitePos(const mjModel* model, const mjData* data, int src_site, + mjtNum* dst_site_pos) { + for (int axis = 0; axis < 3; ++axis) { + dst_site_pos[axis] = data->site_xpos[src_site * 3 + axis]; + } +} + +inline void CopyModelBodyPos(const mjModel* model, int body_id, + std::vector* out) { + out->assign(model->body_pos + body_id * 3, model->body_pos + body_id * 3 + 3); +} + +inline void RestoreModelBodyPos(mjModel* model, int body_id, + const std::vector& value) { + if (value.empty()) { + return; + } + std::memcpy(model->body_pos + body_id * 3, value.data(), sizeof(mjtNum) * 3); +} + +inline void CopyModelBodyQuat(const mjModel* model, int body_id, + std::vector* out) { + out->assign(model->body_quat + body_id * 4, + model->body_quat + body_id * 4 + 4); +} + +inline void RestoreModelBodyQuat(mjModel* model, int body_id, + const std::vector& value) { + if (value.empty()) { + return; + } + std::memcpy(model->body_quat + body_id * 4, value.data(), sizeof(mjtNum) * 4); +} + +inline void CopyModelSitePos(const mjModel* model, int site_id, + std::vector* out) { + out->assign(model->site_pos + site_id * 3, model->site_pos + site_id * 3 + 3); +} + +inline void RestoreModelSitePos(mjModel* model, int site_id, + const std::vector& value) { + if (value.empty()) { + return; + } + std::memcpy(model->site_pos + site_id * 3, value.data(), sizeof(mjtNum) * 3); +} + +inline void CopyModelSiteSize(const mjModel* model, int site_id, + std::vector* out) { + out->assign(model->site_size + site_id * 3, + model->site_size + site_id * 3 + 3); +} + +inline void RestoreModelSiteSize(mjModel* model, int site_id, + const std::vector& value) { + if (value.empty()) { + return; + } + std::memcpy(model->site_size + site_id * 3, value.data(), sizeof(mjtNum) * 3); +} + +inline void CopyModelGeomSize(const mjModel* model, int geom_id, + std::vector* out) { + out->assign(model->geom_size + geom_id * 3, + model->geom_size + geom_id * 3 + 3); +} + +inline void RestoreModelGeomSize(mjModel* model, int geom_id, + const std::vector& value) { + if (value.empty()) { + return; + } + std::memcpy(model->geom_size + geom_id * 3, value.data(), sizeof(mjtNum) * 3); +} + +inline void CopyModelGeomPos(const mjModel* model, int geom_id, + std::vector* out) { + out->assign(model->geom_pos + geom_id * 3, model->geom_pos + geom_id * 3 + 3); +} + +inline void RestoreModelGeomPos(mjModel* model, int geom_id, + const std::vector& value) { + if (value.empty()) { + return; + } + std::memcpy(model->geom_pos + geom_id * 3, value.data(), sizeof(mjtNum) * 3); +} + +inline void CopyModelGeomQuat(const mjModel* model, int geom_id, + std::vector* out) { + out->assign(model->geom_quat + geom_id * 4, + model->geom_quat + geom_id * 4 + 4); +} + +inline void RestoreModelGeomQuat(mjModel* model, int geom_id, + const std::vector& value) { + if (value.empty()) { + return; + } + std::memcpy(model->geom_quat + geom_id * 4, value.data(), sizeof(mjtNum) * 4); +} + +inline void CopyModelGeomRgba(const mjModel* model, int geom_id, + std::vector* out) { + out->assign(model->geom_rgba + geom_id * 4, + model->geom_rgba + geom_id * 4 + 4); +} + +inline void RestoreModelGeomRgba(mjModel* model, int geom_id, + const std::vector& value) { + if (value.empty()) { + return; + } + float* target = model->geom_rgba + geom_id * 4; + for (int axis = 0; axis < 4; ++axis) { + target[axis] = static_cast(value[axis]); + } +} + +inline void CopyModelSiteRgba(const mjModel* model, int site_id, + std::vector* out) { + out->assign(model->site_rgba + site_id * 4, + model->site_rgba + site_id * 4 + 4); +} + +inline void RestoreModelSiteRgba(mjModel* model, int site_id, + const std::vector& value) { + if (value.empty()) { + return; + } + float* target = model->site_rgba + site_id * 4; + for (int axis = 0; axis < 4; ++axis) { + target[axis] = static_cast(value[axis]); + } +} + +inline void CopyModelSitePosAndSize(const mjModel* model, int site_id, + std::vector* out_pos, + std::vector* out_size) { + CopyModelSitePos(model, site_id, out_pos); + CopyModelSiteSize(model, site_id, out_size); +} + +inline void CopyModelBodyMass(const mjModel* model, int body_id, mjtNum* out) { + *out = model->body_mass[body_id]; +} + +inline void RestoreModelBodyMass(mjModel* model, int body_id, mjtNum value) { + model->body_mass[body_id] = value; +} + +inline void CopyModelGeomType(const mjModel* model, int geom_id, int* out) { + *out = model->geom_type[geom_id]; +} + +inline void RestoreModelGeomType(mjModel* model, int geom_id, int value) { + if (value < 0) { + return; + } + model->geom_type[geom_id] = value; +} + +inline void CopyModelGeomCondim(const mjModel* model, int geom_id, int* out) { + *out = model->geom_condim[geom_id]; +} + +inline void RestoreModelGeomCondim(mjModel* model, int geom_id, int value) { + if (value < 0) { + return; + } + model->geom_condim[geom_id] = value; +} + +inline void CopyModelGeomContype(const mjModel* model, int geom_id, int* out) { + *out = model->geom_contype[geom_id]; +} + +inline void RestoreModelGeomContype(mjModel* model, int geom_id, int value) { + if (value < 0) { + return; + } + model->geom_contype[geom_id] = value; +} + +inline void CopyModelGeomConaffinity(const mjModel* model, int geom_id, + int* out) { + *out = model->geom_conaffinity[geom_id]; +} + +inline void RestoreModelGeomConaffinity(mjModel* model, int geom_id, + int value) { + if (value < 0) { + return; + } + model->geom_conaffinity[geom_id] = value; +} + +inline void CopyModelGeomFriction(const mjModel* model, int geom_id, + std::vector* out) { + out->assign(model->geom_friction + geom_id * 3, + model->geom_friction + geom_id * 3 + 3); +} + +inline void RestoreModelGeomFriction(mjModel* model, int geom_id, + const std::vector& value) { + if (value.empty()) { + return; + } + std::memcpy(model->geom_friction + geom_id * 3, value.data(), + sizeof(mjtNum) * 3); +} + +inline void CopyModelHfieldData(const mjModel* model, int hfield_id, + std::vector* out) { + int adr = model->hfield_adr[hfield_id]; + int rows = model->hfield_nrow[hfield_id]; + int cols = model->hfield_ncol[hfield_id]; + std::size_t expected = static_cast(rows) * cols; + out->resize(expected); + for (std::size_t i = 0; i < expected; ++i) { + (*out)[i] = static_cast(model->hfield_data[adr + i]); + } +} + +inline void RestoreModelHfieldData(mjModel* model, int hfield_id, + const std::vector& value) { + if (value.empty()) { + return; + } + int adr = model->hfield_adr[hfield_id]; + int rows = model->hfield_nrow[hfield_id]; + int cols = model->hfield_ncol[hfield_id]; + std::size_t expected = static_cast(rows) * cols; + if (value.size() != expected) { + throw std::runtime_error("hfield data size does not match model."); + } + for (std::size_t i = 0; i < expected; ++i) { + model->hfield_data[adr + i] = static_cast(value[i]); + } +} + +inline void BuildMuscleMask(const mjModel* model, + std::vector* muscle_actuator) { + muscle_actuator->assign(model->nu, false); + for (int i = 0; i < model->nu; ++i) { + (*muscle_actuator)[i] = model->actuator_dyntype[i] == mjDYN_MUSCLE; + } +} + +inline void AdjustInitialQposForNormalizedActions(const mjModel* model, + mjData* data, + bool normalize_act) { + if (!normalize_act) { + return; + } + std::vector updated(model->njnt, false); + for (int actuator_id = 0; actuator_id < model->nu; ++actuator_id) { + if (model->actuator_trntype[actuator_id] != mjTRN_JOINT) { + continue; + } + int joint_id = model->actuator_trnid[actuator_id * 2]; + if (joint_id < 0 || updated[joint_id]) { + continue; + } + int joint_type = model->jnt_type[joint_id]; + if (joint_type != mjJNT_HINGE && joint_type != mjJNT_SLIDE) { + continue; + } + int qpos_addr = model->jnt_qposadr[joint_id]; + mjtNum low = model->jnt_range[joint_id * 2]; + mjtNum high = model->jnt_range[joint_id * 2 + 1]; + data->qpos[qpos_addr] = (low + high) * static_cast(0.5); + updated[joint_id] = true; + } + mj_forward(model, data); +} + +inline void ApplyMyoSuiteAction(const mjModel* model, mjData* data, + const std::vector& muscle_actuator, + bool normalize_act, const float* raw) { + for (int i = 0; i < model->nu; ++i) { + auto value = static_cast(raw[i]); + if (normalize_act && muscle_actuator[i] && model->na != 0) { + value = MuscleActivation(value); + } else if (normalize_act && model->na == 0) { + mjtNum low = model->actuator_ctrlrange[i * 2]; + mjtNum high = model->actuator_ctrlrange[i * 2 + 1]; + value = (low + high) * static_cast(0.5) + + value * (high - low) * static_cast(0.5); + } + data->ctrl[i] = value; + } +} + +inline envpool::mujoco::CameraPolicy MyoSuiteRenderCameraPolicy() { + return envpool::mujoco::CameraPolicy::kDmControl; +} + +inline void ConfigureMyoSuiteRenderOptions(mjvOption* option, + bool render_tendon = true) { + mjv_defaultOption(option); + option->flags[mjVIS_TENDON] = render_tendon ? 1 : 0; + option->flags[mjVIS_ACTUATOR] = 1; + option->flags[mjVIS_ACTIVATION] = 1; +} + +} // namespace detail + +class MyoSuitePoseEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(10), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), "pose_thd"_.Bind(0.35), + "reward_pose_w"_.Bind(1.0), "reward_bonus_w"_.Bind(4.0), + "reward_act_reg_w"_.Bind(1.0), "reward_penalty_w"_.Bind(50.0), + "reset_type"_.Bind(std::string("init")), + "target_type"_.Bind(std::string("generate")), + "target_qpos_min"_.Bind(std::vector{}), + "target_qpos_max"_.Bind(std::vector{}), + "target_qpos_value"_.Bind(std::vector{}), + "viz_site_targets"_.Bind(std::vector{}), + "weight_bodyname"_.Bind(std::string()), + "weight_range"_.Bind(std::vector{}), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_ctrl"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{}), + "test_target_qpos"_.Bind(std::vector{}), + "test_target_site_pos"_.Bind(std::vector{}), + "test_body_mass"_.Bind(std::vector{}), + "test_geom_size0"_.Bind(std::vector{})); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:pose_dist"_.Bind(Spec({-1}, {0.0, inf})), + "info:act_reg"_.Bind(Spec({-1}, {0.0, inf})), + "info:success"_.Bind(Spec({-1}, {0.0, 1.0})), + "info:target_qpos"_.Bind(Spec({conf["qpos_dim"_]})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:weight_mass"_.Bind(Spec({-1}, {0.0, inf})), + "info:weight_geom_size0"_.Bind(Spec({-1}, {0.0, inf}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using MyoSuitePoseEnvSpec = EnvSpec; +using MyoSuitePosePixelEnvFns = PixelObservationEnvFns; +using MyoSuitePosePixelEnvSpec = EnvSpec; + +class MyoSuiteReachEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(10), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), "target_site_count"_.Bind(0), + "far_th"_.Bind(0.35), "reward_reach_w"_.Bind(1.0), + "reward_bonus_w"_.Bind(4.0), "reward_act_reg_w"_.Bind(0.0), + "reward_penalty_w"_.Bind(50.0), + "target_site_names"_.Bind(std::vector{}), + "target_pos_min"_.Bind(std::vector{}), + "target_pos_max"_.Bind(std::vector{}), + "joint_random_range"_.Bind(std::vector{}), + "target_pos_relative_to_tip"_.Bind(false), + "hide_skin_geom_group_1"_.Bind(false), "hide_terrain"_.Bind(false), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_ctrl"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{}), + "test_target_pos"_.Bind(std::vector{})); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:reach_dist"_.Bind(Spec({-1}, {0.0, inf})), + "info:act_reg"_.Bind(Spec({-1}, {0.0, inf})), + "info:success"_.Bind(Spec({-1}, {0.0, 1.0})), + "info:target_pos"_.Bind(Spec({conf["target_site_count"_] * 3})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:time"_.Bind(Spec({-1}, {0.0, inf}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using MyoSuiteReachEnvSpec = EnvSpec; +using MyoSuiteReachPixelEnvFns = PixelObservationEnvFns; +using MyoSuiteReachPixelEnvSpec = EnvSpec; + +class MyoSuiteKeyTurnEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(10), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), "goal_th"_.Bind(3.14), + "reward_key_turn_w"_.Bind(1.0), "reward_iftip_approach_w"_.Bind(10.0), + "reward_thtip_approach_w"_.Bind(10.0), "reward_act_reg_w"_.Bind(1.0), + "reward_bonus_w"_.Bind(4.0), "reward_penalty_w"_.Bind(25.0), + "key_init_range"_.Bind(std::vector{0.0, 0.0}), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{}), + "test_key_body_pos"_.Bind(std::vector{})); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:key_turn"_.Bind(Spec({-1}, {-inf, inf})), + "info:iftip_approach_dist"_.Bind(Spec({-1}, {0.0, inf})), + "info:thtip_approach_dist"_.Bind(Spec({-1}, {0.0, inf})), + "info:success"_.Bind(Spec({-1}, {0.0, 1.0})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:key_body_pos"_.Bind(Spec({3}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using MyoSuiteKeyTurnEnvSpec = EnvSpec; +using MyoSuiteKeyTurnPixelEnvFns = + // NOLINTNEXTLINE(whitespace/indent_namespace) + PixelObservationEnvFns; +using MyoSuiteKeyTurnPixelEnvSpec = EnvSpec; + +class MyoSuiteObjHoldEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(10), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), "randomize_on_reset"_.Bind(false), + "reward_goal_dist_w"_.Bind(100.0), "reward_bonus_w"_.Bind(4.0), + "reward_penalty_w"_.Bind(10.0), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{}), + "test_goal_pos"_.Bind(std::vector{}), + "test_object_geom_size"_.Bind(std::vector{})); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:goal_dist"_.Bind(Spec({-1}, {0.0, inf})), + "info:success"_.Bind(Spec({-1}, {0.0, 1.0})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:goal_pos"_.Bind(Spec({3})), + "info:object_geom_size"_.Bind(Spec({3}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using MyoSuiteObjHoldEnvSpec = EnvSpec; +using MyoSuiteObjHoldPixelEnvFns = + // NOLINTNEXTLINE(whitespace/indent_namespace) + PixelObservationEnvFns; +using MyoSuiteObjHoldPixelEnvSpec = EnvSpec; + +class MyoSuiteTorsoEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(5), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), "pose_dim"_.Bind(0), "pose_thd"_.Bind(0.25), + "reward_pose_w"_.Bind(1.0), "reward_bonus_w"_.Bind(4.0), + "reward_act_reg_w"_.Bind(1.0), "reward_penalty_w"_.Bind(50.0), + "target_qpos_value"_.Bind(std::vector{}), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{})); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:pose_dist"_.Bind(Spec({-1}, {0.0, inf})), + "info:success"_.Bind(Spec({-1}, {0.0, 1.0})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:target_qpos"_.Bind(Spec({conf["pose_dim"_]}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using MyoSuiteTorsoEnvSpec = EnvSpec; +using MyoSuiteTorsoPixelEnvFns = PixelObservationEnvFns; +using MyoSuiteTorsoPixelEnvSpec = EnvSpec; + +class MyoSuitePenTwirlEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(5), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), "randomize_target"_.Bind(false), + "reward_pos_align_w"_.Bind(1.0), "reward_rot_align_w"_.Bind(1.0), + "reward_act_reg_w"_.Bind(5.0), "reward_drop_w"_.Bind(5.0), + "reward_bonus_w"_.Bind(10.0), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{}), + "test_target_body_quat"_.Bind(std::vector{})); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:pos_align"_.Bind(Spec({-1}, {0.0, inf})), + "info:rot_align"_.Bind(Spec({-1}, {-1.0, 1.0})), + "info:success"_.Bind(Spec({-1}, {0.0, 1.0})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:target_body_quat"_.Bind(Spec({4}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using MyoSuitePenTwirlEnvSpec = EnvSpec; +using MyoSuitePenTwirlPixelEnvFns = + // NOLINTNEXTLINE(whitespace/indent_namespace) + PixelObservationEnvFns; +using MyoSuitePenTwirlPixelEnvSpec = EnvSpec; + +template +class MyoSuitePoseEnvBase : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::gen_; + using Base::spec_; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum pose_dist{0.0}; + mjtNum act_reg{0.0}; + bool success{false}; + bool done{false}; + }; + + bool normalize_act_; + mjtNum pose_thd_; + mjtNum reward_pose_w_; + mjtNum reward_bonus_w_; + mjtNum reward_act_reg_w_; + mjtNum reward_penalty_w_; + std::string reset_type_; + std::string target_type_; + std::vector target_qpos_min_; + std::vector target_qpos_max_; + std::vector default_target_qpos_; + std::vector current_target_qpos_; + std::vector tip_site_ids_; + std::vector target_site_ids_; + std::vector initial_target_site_pos_; + std::vector muscle_actuator_; + detail::MyoConditionState muscle_condition_state_; + int weight_body_id_{-1}; + int weight_geom_id_{-1}; + mjtNum initial_weight_body_mass_{0.0}; + mjtNum initial_weight_geom_size0_{0.0}; + std::vector weight_range_; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_ctrl_; + std::vector test_reset_act_; + std::vector test_reset_qacc_warmstart_; + std::vector test_target_qpos_; + std::vector test_target_site_pos_; + std::vector test_body_mass_; + std::vector test_geom_size0_; + std::uniform_real_distribution unit_dist_{0.0, 1.0}; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoSuitePoseEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + myosuite::MyoSuiteModelPath(spec.config["base_path"_], + spec.config["model_path"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + pose_thd_(spec.config["pose_thd"_]), + reward_pose_w_(spec.config["reward_pose_w"_]), + reward_bonus_w_(spec.config["reward_bonus_w"_]), + reward_act_reg_w_(spec.config["reward_act_reg_w"_]), + reward_penalty_w_(spec.config["reward_penalty_w"_]), + reset_type_(spec.config["reset_type"_]), + target_type_(spec.config["target_type"_]), + target_qpos_min_(detail::ToMjtVector(spec.config["target_qpos_min"_])), + target_qpos_max_(detail::ToMjtVector(spec.config["target_qpos_max"_])), + default_target_qpos_( + detail::ToMjtVector(spec.config["target_qpos_value"_])), + current_target_qpos_(default_target_qpos_), + muscle_actuator_(model_->nu, false), + weight_range_(detail::ToMjtVector(spec.config["weight_range"_])), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_ctrl_(detail::ToMjtVector(spec.config["test_reset_ctrl"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])), + test_target_qpos_( + detail::ToMjtVector(spec.config["test_target_qpos"_])), + test_target_site_pos_( + detail::ToMjtVector(spec.config["test_target_site_pos"_])), + test_body_mass_(detail::ToMjtVector(spec.config["test_body_mass"_])), + test_geom_size0_(detail::ToMjtVector(spec.config["test_geom_size0"_])) { + ValidateConfig(); + BuildMuscleMask(); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + AdjustInitialQposForNormalizedActions(); + CacheTargetSites(spec.config["viz_site_targets"_]); + CacheWeightRandomization(spec.config["weight_bodyname"_]); + InitializeRobotEnv(); + } + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + detail::ResetMyoConditionState(&muscle_condition_state_); + ResetToInitialState(); + RestoreTargetSites(); + RestoreWeightRandomization(); + UpdateTargetQpos(); + ApplyWeightRandomization(); + ApplyResetState(); + CaptureResetState(); + RewardInfo reward = ComputeRewardInfo(); + WriteState(reward, true, 0.0); + } + + void Step(const Action& action) override { + const auto* raw = static_cast(action["action"_].Data()); + ApplyAction(raw); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + ++elapsed_step_; + RewardInfo reward = ComputeRewardInfo(); + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, false, reward.dense_reward); + } + + private: + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_]) { + throw std::runtime_error("Pose config qpos_dim does not match model."); + } + if (model_->nv != spec_.config["qvel_dim"_]) { + throw std::runtime_error("Pose config qvel_dim does not match model."); + } + if (model_->nu != spec_.config["action_dim"_]) { + throw std::runtime_error("Pose config action_dim does not match model."); + } + if (model_->na != spec_.config["act_dim"_]) { + throw std::runtime_error("Pose config act_dim does not match model."); + } + int expected_obs = model_->nq + model_->nv + model_->nq + model_->na; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error("Pose config obs_dim does not match model."); + } + if (!default_target_qpos_.empty() && + static_cast(default_target_qpos_.size()) != model_->nq) { + throw std::runtime_error("Pose target_qpos_value has wrong length."); + } + if (!target_qpos_min_.empty() && + static_cast(target_qpos_min_.size()) != model_->nq) { + throw std::runtime_error("Pose target_qpos_min has wrong length."); + } + if (!target_qpos_max_.empty() && + static_cast(target_qpos_max_.size()) != model_->nq) { + throw std::runtime_error("Pose target_qpos_max has wrong length."); + } + if (!test_reset_qpos_.empty() && + static_cast(test_reset_qpos_.size()) != model_->nq) { + throw std::runtime_error("Pose test_reset_qpos has wrong length."); + } + if (!test_reset_qvel_.empty() && + static_cast(test_reset_qvel_.size()) != model_->nv) { + throw std::runtime_error("Pose test_reset_qvel has wrong length."); + } + if (!test_reset_ctrl_.empty() && + static_cast(test_reset_ctrl_.size()) != model_->nu) { + throw std::runtime_error("Pose test_reset_ctrl has wrong length."); + } + if (!test_reset_act_.empty() && + static_cast(test_reset_act_.size()) != model_->na) { + throw std::runtime_error("Pose test_reset_act has wrong length."); + } + if (!test_reset_qacc_warmstart_.empty() && + static_cast(test_reset_qacc_warmstart_.size()) != model_->nv) { + throw std::runtime_error( + "Pose test_reset_qacc_warmstart has wrong length."); + } + if (!test_target_qpos_.empty() && + static_cast(test_target_qpos_.size()) != model_->nq) { + throw std::runtime_error("Pose test_target_qpos has wrong length."); + } + int expected_target_site_pos = + static_cast(spec_.config["viz_site_targets"_].size()) * 3; + if (!test_target_site_pos_.empty() && + static_cast(test_target_site_pos_.size()) != + expected_target_site_pos) { + throw std::runtime_error("Pose test_target_site_pos has wrong length."); + } + } + + void BuildMuscleMask() { + for (int i = 0; i < model_->nu; ++i) { + muscle_actuator_[i] = model_->actuator_dyntype[i] == mjDYN_MUSCLE; + } + } + + void AdjustInitialQposForNormalizedActions() { + if (!normalize_act_) { + return; + } + std::vector updated(model_->njnt, false); + for (int actuator_id = 0; actuator_id < model_->nu; ++actuator_id) { + if (model_->actuator_trntype[actuator_id] != mjTRN_JOINT) { + continue; + } + int joint_id = model_->actuator_trnid[actuator_id * 2]; + if (joint_id < 0 || updated[joint_id]) { + continue; + } + int joint_type = model_->jnt_type[joint_id]; + if (joint_type != mjJNT_HINGE && joint_type != mjJNT_SLIDE) { + continue; + } + int qpos_addr = model_->jnt_qposadr[joint_id]; + mjtNum low = model_->jnt_range[joint_id * 2]; + mjtNum high = model_->jnt_range[joint_id * 2 + 1]; + data_->qpos[qpos_addr] = (low + high) * static_cast(0.5); + updated[joint_id] = true; + } + mj_forward(model_, data_); + } + + void CacheTargetSites(const std::vector& viz_site_targets) { + tip_site_ids_.reserve(viz_site_targets.size()); + target_site_ids_.reserve(viz_site_targets.size()); + initial_target_site_pos_.reserve(viz_site_targets.size() * 3); + for (const auto& site_name : viz_site_targets) { + int tip_site = mj_name2id(model_, mjOBJ_SITE, site_name.c_str()); + int target_site = + mj_name2id(model_, mjOBJ_SITE, (site_name + "_target").c_str()); + if (tip_site == -1 || target_site == -1) { + throw std::runtime_error("Pose target visualization site missing."); + } + tip_site_ids_.push_back(tip_site); + target_site_ids_.push_back(target_site); + initial_target_site_pos_.insert(initial_target_site_pos_.end(), + model_->site_pos + target_site * 3, + model_->site_pos + target_site * 3 + 3); + } + } + + void CacheWeightRandomization(const std::string& weight_bodyname) { + if (weight_bodyname.empty()) { + return; + } + weight_body_id_ = mj_name2id(model_, mjOBJ_BODY, weight_bodyname.c_str()); + if (weight_body_id_ == -1) { + throw std::runtime_error("Pose weight body missing."); + } + weight_geom_id_ = model_->body_geomadr[weight_body_id_]; + initial_weight_body_mass_ = model_->body_mass[weight_body_id_]; + if (weight_geom_id_ >= 0) { + initial_weight_geom_size0_ = model_->geom_size[weight_geom_id_ * 3]; + } + } + + void RestoreTargetSites() { + for (std::size_t i = 0; i < target_site_ids_.size(); ++i) { + std::memcpy(model_->site_pos + target_site_ids_[i] * 3, + initial_target_site_pos_.data() + i * 3, sizeof(mjtNum) * 3); + } + } + + void RestoreWeightRandomization() { + if (weight_body_id_ == -1) { + return; + } + model_->body_mass[weight_body_id_] = initial_weight_body_mass_; + if (weight_geom_id_ >= 0) { + model_->geom_size[weight_geom_id_ * 3] = initial_weight_geom_size0_; + } + } + + std::vector SampleTargetQpos() { + std::vector target(model_->nq); + for (int i = 0; i < model_->nq; ++i) { + double alpha = unit_dist_(gen_); + target[i] = + target_qpos_min_[i] + static_cast(alpha) * + (target_qpos_max_[i] - target_qpos_min_[i]); + } + return target; + } + + void ApplyTargetVisualization() { + if (target_site_ids_.empty()) { + return; + } + if (!test_target_site_pos_.empty()) { + for (std::size_t i = 0; i < target_site_ids_.size(); ++i) { + std::memcpy(model_->site_pos + target_site_ids_[i] * 3, + test_target_site_pos_.data() + i * 3, sizeof(mjtNum) * 3); + } + mj_forward(model_, data_); + return; + } + std::vector saved_qpos = detail::CopyQpos(model_, data_); + std::vector saved_qvel(data_->qvel, data_->qvel + model_->nv); + detail::RestoreVector(current_target_qpos_, data_->qpos); + mju_zero(data_->qvel, model_->nv); + mj_forward(model_, data_); + for (std::size_t i = 0; i < tip_site_ids_.size(); ++i) { + detail::CopySitePos(model_, data_, tip_site_ids_[i], + model_->site_pos + target_site_ids_[i] * 3); + } + detail::RestoreVector(saved_qpos, data_->qpos); + detail::RestoreVector(saved_qvel, data_->qvel); + mj_forward(model_, data_); + } + + void UpdateTargetQpos() { + if (!test_target_qpos_.empty()) { + current_target_qpos_ = test_target_qpos_; + } else if (target_type_ == "generate") { + current_target_qpos_ = SampleTargetQpos(); + } else if (target_type_ == "fixed") { + current_target_qpos_ = default_target_qpos_; + } else { + throw std::runtime_error("Unsupported Pose target_type."); + } + ApplyTargetVisualization(); + } + + void ApplyWeightRandomization() { + if (weight_body_id_ == -1) { + return; + } + mjtNum weight = initial_weight_body_mass_; + if (!test_body_mass_.empty()) { + weight = test_body_mass_[0]; + } else if (weight_range_.size() == 2) { + double alpha = unit_dist_(gen_); + weight = weight_range_[0] + static_cast(alpha) * + (weight_range_[1] - weight_range_[0]); + } + model_->body_mass[weight_body_id_] = weight; + if (weight_geom_id_ >= 0) { + mjtNum geom_size0 = + !test_geom_size0_.empty() + ? test_geom_size0_[0] + : static_cast(0.01 + + 2.5 * static_cast(weight) / 100.0); + model_->geom_size[weight_geom_id_ * 3] = geom_size0; + } + } + + void ApplyResetState() { + bool has_test_reset_override = + !test_reset_qpos_.empty() || !test_reset_qvel_.empty() || + !test_reset_ctrl_.empty() || !test_reset_act_.empty() || + !test_reset_qacc_warmstart_.empty(); + if (!test_reset_qpos_.empty()) { + detail::RestoreVector(test_reset_qpos_, data_->qpos); + detail::RestoreVector(test_reset_qvel_, data_->qvel); + } else if (reset_type_ == "random") { + for (int joint_id = 0; joint_id < model_->njnt; ++joint_id) { + int joint_type = model_->jnt_type[joint_id]; + if (joint_type != mjJNT_HINGE && joint_type != mjJNT_SLIDE) { + throw std::runtime_error( + "Pose random reset only supports 1-DoF joints."); + } + int qpos_addr = model_->jnt_qposadr[joint_id]; + double alpha = unit_dist_(gen_); + mjtNum low = model_->jnt_range[joint_id * 2]; + mjtNum high = model_->jnt_range[joint_id * 2 + 1]; + data_->qpos[qpos_addr] = + low + static_cast(alpha) * (high - low); + } + detail::RestoreVector(initial_qvel_, data_->qvel); + } else if (reset_type_ != "init") { + throw std::runtime_error("Unsupported Pose reset_type."); + } + mj_forward(model_, data_); + if (!test_reset_ctrl_.empty()) { + detail::RestoreVector(test_reset_ctrl_, data_->ctrl); + } + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, data_->qacc_warmstart); + } + if (has_test_reset_override) { + detail::FinalizeMyoSuiteResetSync(model_, data_); + } + } + + void ApplyAction(const float* raw) { + for (int i = 0; i < model_->nu; ++i) { + auto value = static_cast(raw[i]); + if (normalize_act_ && muscle_actuator_[i] && model_->na != 0) { + value = detail::MuscleActivation(value); + } else if (normalize_act_ && model_->na == 0) { + mjtNum low = model_->actuator_ctrlrange[i * 2]; + mjtNum high = model_->actuator_ctrlrange[i * 2 + 1]; + value = (low + high) * static_cast(0.5) + + value * (high - low) * static_cast(0.5); + } + data_->ctrl[i] = value; + } + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + } + + RewardInfo ComputeRewardInfo() const { + RewardInfo reward; + std::vector pose_err(model_->nq); + for (int i = 0; i < model_->nq; ++i) { + pose_err[i] = current_target_qpos_[i] - data_->qpos[i]; + } + reward.pose_dist = detail::VectorNorm(pose_err); + reward.act_reg = detail::ActReg(model_, data_); + reward.success = reward.pose_dist < pose_thd_; + reward.done = reward.pose_dist > detail::kPoseFarThreshold; + mjtNum bonus = static_cast(reward.pose_dist < pose_thd_) + + static_cast(reward.pose_dist < + static_cast(1.5) * pose_thd_); + mjtNum penalty = + -static_cast(reward.pose_dist > detail::kPoseFarThreshold); + reward.dense_reward = + -reward_pose_w_ * reward.pose_dist + reward_bonus_w_ * bonus - + reward_act_reg_w_ * reward.act_reg + reward_penalty_w_ * penalty; + return reward; + } + + void WriteState(const RewardInfo& reward, bool reset, mjtNum reward_value) { + auto state = Allocate(); + state["reward"_] = reward_value; + if constexpr (kFromPixels) { + auto obs_pixels = state["obs:pixels"_]; + AssignPixelObservation("obs:pixels", &obs_pixels, reset); + } else { + auto obs = state["obs"_]; + mjtNum* buffer = PrepareObservation("obs", &obs); + for (int i = 0; i < model_->nq; ++i) { + *(buffer++) = data_->qpos[i]; + } + for (int i = 0; i < model_->nv; ++i) { + *(buffer++) = data_->qvel[i] * Dt(); + } + for (int i = 0; i < model_->nq; ++i) { + *(buffer++) = current_target_qpos_[i] - data_->qpos[i]; + } + for (int i = 0; i < model_->na; ++i) { + *(buffer++) = data_->act[i]; + } + CommitObservation("obs", &obs, reset); + } + state["info:pose_dist"_] = reward.pose_dist; + state["info:act_reg"_] = reward.act_reg; + state["info:success"_] = reward.success; + state["info:target_qpos"_].Assign(current_target_qpos_.data(), + current_target_qpos_.size()); +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + mjtNum weight_mass = + weight_body_id_ == -1 ? 0.0 : model_->body_mass[weight_body_id_]; + mjtNum geom_size0 = + weight_geom_id_ == -1 ? 0.0 : model_->geom_size[weight_geom_id_ * 3]; + state["info:weight_mass"_] = weight_mass; + state["info:weight_geom_size0"_] = geom_size0; + } +}; + +template +class MyoSuiteReachEnvBase : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::gen_; + using Base::spec_; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum reach_dist{0.0}; + mjtNum act_reg{0.0}; + bool success{false}; + bool done{false}; + }; + + bool normalize_act_; + mjtNum far_th_; + mjtNum reward_reach_w_; + mjtNum reward_bonus_w_; + mjtNum reward_act_reg_w_; + mjtNum reward_penalty_w_; + std::vector tip_site_ids_; + std::vector target_site_ids_; + std::vector initial_target_site_pos_; + std::vector target_pos_min_; + std::vector target_pos_max_; + std::vector joint_random_range_; + std::vector current_target_pos_; + std::vector init_qpos_; + std::vector init_qvel_; + std::vector muscle_actuator_; + detail::MyoConditionState muscle_condition_state_; + bool target_pos_relative_to_tip_; + bool hide_skin_geom_group_1_; + bool hide_terrain_; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_ctrl_; + std::vector test_reset_act_; + std::vector test_reset_qacc_warmstart_; + std::vector test_target_pos_; + std::uniform_real_distribution unit_dist_{0.0, 1.0}; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoSuiteReachEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + myosuite::MyoSuiteModelPath(spec.config["base_path"_], + spec.config["model_path"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + far_th_(spec.config["far_th"_]), + reward_reach_w_(spec.config["reward_reach_w"_]), + reward_bonus_w_(spec.config["reward_bonus_w"_]), + reward_act_reg_w_(spec.config["reward_act_reg_w"_]), + reward_penalty_w_(spec.config["reward_penalty_w"_]), + target_pos_min_(detail::ToMjtVector(spec.config["target_pos_min"_])), + target_pos_max_(detail::ToMjtVector(spec.config["target_pos_max"_])), + joint_random_range_( + detail::ToMjtVector(spec.config["joint_random_range"_])), + current_target_pos_(target_pos_min_), + muscle_actuator_(model_->nu, false), + target_pos_relative_to_tip_(spec.config["target_pos_relative_to_tip"_]), + hide_skin_geom_group_1_(spec.config["hide_skin_geom_group_1"_]), + hide_terrain_(spec.config["hide_terrain"_]), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_ctrl_(detail::ToMjtVector(spec.config["test_reset_ctrl"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])), + test_target_pos_(detail::ToMjtVector(spec.config["test_target_pos"_])) { + ValidateConfig(); + BuildMuscleMask(); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + AdjustInitialQposForNormalizedActions(); + if (HasJointRandomization()) { + if (model_->nkey <= 0) { + throw std::runtime_error( + "Reach joint_random_range requires at least one keyframe."); + } + init_qpos_.assign(model_->key_qpos, model_->key_qpos + model_->nq); + init_qvel_.assign(model_->key_qvel, model_->key_qvel + model_->nv); + } + CacheTargetSites(spec.config["target_site_names"_]); + ApplyStaticVisualSetup(); + InitializeRobotEnv(); + } + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + detail::ResetMyoConditionState(&muscle_condition_state_); + ResetToInitialState(); + RestoreTargetSites(); + PrepareTargetGenerationState(); + UpdateTargetSites(); + ApplyResetState(); + CaptureResetState(); + RewardInfo reward = ComputeRewardInfo(); + WriteState(reward, true, 0.0); + } + + void Step(const Action& action) override { + const auto* raw = static_cast(action["action"_].Data()); + ApplyAction(raw); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + ++elapsed_step_; + RewardInfo reward = ComputeRewardInfo(); + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, false, reward.dense_reward); + } + + private: + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_]) { + throw std::runtime_error("Reach config qpos_dim does not match model."); + } + if (model_->nv != spec_.config["qvel_dim"_]) { + throw std::runtime_error("Reach config qvel_dim does not match model."); + } + if (model_->nu != spec_.config["action_dim"_]) { + throw std::runtime_error("Reach config action_dim does not match model."); + } + if (model_->na != spec_.config["act_dim"_]) { + throw std::runtime_error("Reach config act_dim does not match model."); + } + int site_count = spec_.config["target_site_count"_]; + if (static_cast(target_pos_min_.size()) != site_count * 3 || + static_cast(target_pos_max_.size()) != site_count * 3) { + throw std::runtime_error("Reach target position config has wrong size."); + } + if (!test_target_pos_.empty() && + static_cast(test_target_pos_.size()) != site_count * 3) { + throw std::runtime_error("Reach test_target_pos has wrong length."); + } + if (!joint_random_range_.empty() && joint_random_range_.size() != 2) { + throw std::runtime_error( + "Reach joint_random_range must be empty or length 2."); + } + if (!test_reset_act_.empty() && + static_cast(test_reset_act_.size()) != model_->na) { + throw std::runtime_error("Reach test_reset_act has wrong length."); + } + if (!test_reset_ctrl_.empty() && + static_cast(test_reset_ctrl_.size()) != model_->nu) { + throw std::runtime_error("Reach test_reset_ctrl has wrong length."); + } + if (!test_reset_qacc_warmstart_.empty() && + static_cast(test_reset_qacc_warmstart_.size()) != model_->nv) { + throw std::runtime_error( + "Reach test_reset_qacc_warmstart has wrong length."); + } + int expected_obs = + model_->nq + model_->nv + site_count * 3 + site_count * 3 + model_->na; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error("Reach config obs_dim does not match model."); + } + } + + bool HasJointRandomization() const { + return joint_random_range_.size() == 2 && + joint_random_range_[0] != joint_random_range_[1]; + } + + void BuildMuscleMask() { + for (int i = 0; i < model_->nu; ++i) { + muscle_actuator_[i] = model_->actuator_dyntype[i] == mjDYN_MUSCLE; + } + } + + void AdjustInitialQposForNormalizedActions() { + if (!normalize_act_) { + return; + } + std::vector updated(model_->njnt, false); + for (int actuator_id = 0; actuator_id < model_->nu; ++actuator_id) { + if (model_->actuator_trntype[actuator_id] != mjTRN_JOINT) { + continue; + } + int joint_id = model_->actuator_trnid[actuator_id * 2]; + if (joint_id < 0 || updated[joint_id]) { + continue; + } + int joint_type = model_->jnt_type[joint_id]; + if (joint_type != mjJNT_HINGE && joint_type != mjJNT_SLIDE) { + continue; + } + int qpos_addr = model_->jnt_qposadr[joint_id]; + mjtNum low = model_->jnt_range[joint_id * 2]; + mjtNum high = model_->jnt_range[joint_id * 2 + 1]; + data_->qpos[qpos_addr] = (low + high) * static_cast(0.5); + updated[joint_id] = true; + } + mj_forward(model_, data_); + } + + void CacheTargetSites(const std::vector& site_names) { + tip_site_ids_.reserve(site_names.size()); + target_site_ids_.reserve(site_names.size()); + initial_target_site_pos_.reserve(site_names.size() * 3); + for (const auto& site_name : site_names) { + int tip_site = mj_name2id(model_, mjOBJ_SITE, site_name.c_str()); + int target_site = + mj_name2id(model_, mjOBJ_SITE, (site_name + "_target").c_str()); + if (tip_site == -1 || target_site == -1) { + throw std::runtime_error("Reach target site missing."); + } + tip_site_ids_.push_back(tip_site); + target_site_ids_.push_back(target_site); + initial_target_site_pos_.insert(initial_target_site_pos_.end(), + model_->site_pos + target_site * 3, + model_->site_pos + target_site * 3 + 3); + } + } + + void RestoreTargetSites() { + for (std::size_t i = 0; i < target_site_ids_.size(); ++i) { + std::memcpy(model_->site_pos + target_site_ids_[i] * 3, + initial_target_site_pos_.data() + i * 3, sizeof(mjtNum) * 3); + } + } + + void ApplyStaticVisualSetup() { + if (hide_skin_geom_group_1_) { + for (int geom_id = 0; geom_id < model_->ngeom; ++geom_id) { + if (model_->geom_group[geom_id] == 1) { + model_->geom_rgba[geom_id * 4 + 3] = static_cast(0.0); + } + } + } + if (hide_terrain_) { + int terrain_geom_id = mj_name2id(model_, mjOBJ_GEOM, "terrain"); + if (terrain_geom_id >= 0) { + model_->geom_rgba[terrain_geom_id * 4 + 3] = static_cast(0.0); + model_->geom_pos[terrain_geom_id * 3 + 0] = static_cast(0.0); + model_->geom_pos[terrain_geom_id * 3 + 1] = static_cast(0.0); + model_->geom_pos[terrain_geom_id * 3 + 2] = static_cast(-10.0); + } + } + } + + std::vector GenerateRandomizedQpos() { + std::vector qpos = init_qpos_; + std::uniform_real_distribution dist(joint_random_range_[0], + joint_random_range_[1]); + for (int joint_id = 0; joint_id < model_->njnt; ++joint_id) { + int qpos_addr = model_->jnt_qposadr[joint_id]; + mjtNum value = qpos[qpos_addr] + static_cast(dist(gen_)); + mjtNum low = model_->jnt_range[joint_id * 2]; + mjtNum high = model_->jnt_range[joint_id * 2 + 1]; + qpos[qpos_addr] = std::clamp(value, low, high); + } + return qpos; + } + + void PrepareTargetGenerationState() { + if (!target_pos_relative_to_tip_ || !HasJointRandomization()) { + return; + } + std::vector qpos = GenerateRandomizedQpos(); + detail::RestoreVector(qpos, data_->qpos); + if (!init_qvel_.empty()) { + detail::RestoreVector(init_qvel_, data_->qvel); + } + mj_forward(model_, data_); + } + + void UpdateTargetSites() { + if (!test_target_pos_.empty()) { + current_target_pos_ = test_target_pos_; + } else { + current_target_pos_.resize(target_pos_min_.size()); + for (std::size_t i = 0; i < target_site_ids_.size(); ++i) { + for (int axis = 0; axis < 3; ++axis) { + int offset = static_cast(i) * 3 + axis; + double alpha = unit_dist_(gen_); + mjtNum target_value = + target_pos_min_[offset] + + static_cast(alpha) * + (target_pos_max_[offset] - target_pos_min_[offset]); + if (target_pos_relative_to_tip_) { + target_value += data_->site_xpos[tip_site_ids_[i] * 3 + axis]; + } + current_target_pos_[offset] = target_value; + } + } + } + for (std::size_t i = 0; i < target_site_ids_.size(); ++i) { + std::memcpy(model_->site_pos + target_site_ids_[i] * 3, + current_target_pos_.data() + i * 3, sizeof(mjtNum) * 3); + } + } + + void ApplyResetState() { + bool has_test_reset_override = + !test_reset_qpos_.empty() || !test_reset_qvel_.empty() || + !test_reset_ctrl_.empty() || !test_reset_act_.empty() || + !test_reset_qacc_warmstart_.empty(); + if (!test_reset_qpos_.empty()) { + detail::RestoreVector(test_reset_qpos_, data_->qpos); + if (!test_reset_qvel_.empty()) { + detail::RestoreVector(test_reset_qvel_, data_->qvel); + } + } else if (HasJointRandomization()) { + std::vector qpos = GenerateRandomizedQpos(); + detail::RestoreVector(qpos, data_->qpos); + if (!init_qvel_.empty()) { + detail::RestoreVector(init_qvel_, data_->qvel); + } + } + mj_forward(model_, data_); + if (!test_reset_ctrl_.empty()) { + detail::RestoreVector(test_reset_ctrl_, data_->ctrl); + } + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, data_->qacc_warmstart); + } + if (has_test_reset_override) { + detail::FinalizeMyoSuiteResetSync(model_, data_); + } + } + + void ApplyAction(const float* raw) { + for (int i = 0; i < model_->nu; ++i) { + auto value = static_cast(raw[i]); + if (normalize_act_ && muscle_actuator_[i] && model_->na != 0) { + value = detail::MuscleActivation(value); + } else if (normalize_act_ && model_->na == 0) { + mjtNum low = model_->actuator_ctrlrange[i * 2]; + mjtNum high = model_->actuator_ctrlrange[i * 2 + 1]; + value = (low + high) * static_cast(0.5) + + value * (high - low) * static_cast(0.5); + } + data_->ctrl[i] = value; + } + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + } + + RewardInfo ComputeRewardInfo() const { + RewardInfo reward; + std::vector reach_err; + reach_err.reserve(tip_site_ids_.size() * 3); + for (std::size_t i = 0; i < tip_site_ids_.size(); ++i) { + for (int axis = 0; axis < 3; ++axis) { + reach_err.push_back(data_->site_xpos[target_site_ids_[i] * 3 + axis] - + data_->site_xpos[tip_site_ids_[i] * 3 + axis]); + } + } + reward.reach_dist = detail::VectorNorm(reach_err); + reward.act_reg = detail::ActReg(model_, data_); + auto site_count = static_cast(tip_site_ids_.size()); + mjtNum near_th = site_count * static_cast(0.0125); + mjtNum far_th = data_->time > 2.0 * Dt() + ? far_th_ * site_count + : std::numeric_limits::infinity(); + reward.success = reward.reach_dist < near_th; + reward.done = reward.reach_dist > far_th; + mjtNum bonus = static_cast(reward.reach_dist < 2.0 * near_th) + + static_cast(reward.reach_dist < near_th); + mjtNum penalty = -static_cast(reward.reach_dist > far_th); + reward.dense_reward = + -reward_reach_w_ * reward.reach_dist + reward_bonus_w_ * bonus - + reward_act_reg_w_ * reward.act_reg + reward_penalty_w_ * penalty; + return reward; + } + + void WriteState(const RewardInfo& reward, bool reset, mjtNum reward_value) { + auto state = Allocate(); + state["reward"_] = reward_value; + if constexpr (kFromPixels) { + auto obs_pixels = state["obs:pixels"_]; + AssignPixelObservation("obs:pixels", &obs_pixels, reset); + } else { + auto obs = state["obs"_]; + mjtNum* buffer = PrepareObservation("obs", &obs); + for (int i = 0; i < model_->nq; ++i) { + *(buffer++) = data_->qpos[i]; + } + for (int i = 0; i < model_->nv; ++i) { + *(buffer++) = data_->qvel[i] * Dt(); + } + for (int site_id : tip_site_ids_) { + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = data_->site_xpos[site_id * 3 + axis]; + } + } + for (std::size_t i = 0; i < tip_site_ids_.size(); ++i) { + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = data_->site_xpos[target_site_ids_[i] * 3 + axis] - + data_->site_xpos[tip_site_ids_[i] * 3 + axis]; + } + } + for (int i = 0; i < model_->na; ++i) { + *(buffer++) = data_->act[i]; + } + CommitObservation("obs", &obs, reset); + } + state["info:reach_dist"_] = reward.reach_dist; + state["info:act_reg"_] = reward.act_reg; + state["info:success"_] = reward.success; + state["info:target_pos"_].Assign(current_target_pos_.data(), + current_target_pos_.size()); +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + state["info:time"_] = data_->time; + } +}; + +template +class MyoSuiteKeyTurnEnvBase : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::gen_; + using Base::spec_; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum key_turn{0.0}; + mjtNum iftip_approach_dist{0.0}; + mjtNum thtip_approach_dist{0.0}; + mjtNum act_reg{0.0}; + bool success{false}; + bool done{false}; + }; + + bool normalize_act_; + mjtNum goal_th_; + mjtNum reward_key_turn_w_; + mjtNum reward_iftip_approach_w_; + mjtNum reward_thtip_approach_w_; + mjtNum reward_act_reg_w_; + mjtNum reward_bonus_w_; + mjtNum reward_penalty_w_; + std::vector key_init_range_; + int keyhead_sid_{-1}; + int if_sid_{-1}; + int th_sid_{-1}; + int key_body_id_{-1}; + std::vector initial_key_body_pos_; + std::vector muscle_actuator_; + detail::MyoConditionState muscle_condition_state_; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_act_; + std::vector test_reset_qacc_warmstart_; + std::vector test_key_body_pos_; + std::uniform_real_distribution unit_dist_{0.0, 1.0}; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoSuiteKeyTurnEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + myosuite::MyoSuiteModelPath(spec.config["base_path"_], + spec.config["model_path"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + goal_th_(spec.config["goal_th"_]), + reward_key_turn_w_(spec.config["reward_key_turn_w"_]), + reward_iftip_approach_w_(spec.config["reward_iftip_approach_w"_]), + reward_thtip_approach_w_(spec.config["reward_thtip_approach_w"_]), + reward_act_reg_w_(spec.config["reward_act_reg_w"_]), + reward_bonus_w_(spec.config["reward_bonus_w"_]), + reward_penalty_w_(spec.config["reward_penalty_w"_]), + key_init_range_(detail::ToMjtVector(spec.config["key_init_range"_])), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])), + test_key_body_pos_( + detail::ToMjtVector(spec.config["test_key_body_pos"_])) { + ValidateConfig(); + CacheSites(); + detail::BuildMuscleMask(model_, &muscle_actuator_); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + detail::AdjustInitialQposForNormalizedActions(model_, data_, + normalize_act_); + InitializeRobotEnv(); + } + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + detail::ResetMyoConditionState(&muscle_condition_state_); + ResetToInitialState(); + detail::RestoreModelBodyPos(model_, key_body_id_, initial_key_body_pos_); + ApplyResetState(); + CaptureResetState(); + RewardInfo reward = ComputeRewardInfo(); + WriteState(reward, true, 0.0); + } + + void Step(const Action& action) override { + const auto* raw = static_cast(action["action"_].Data()); + detail::ApplyMyoSuiteAction(model_, data_, muscle_actuator_, normalize_act_, + raw); + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + ++elapsed_step_; + RewardInfo reward = ComputeRewardInfo(); + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, false, reward.dense_reward); + } + + private: + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_]) { + throw std::runtime_error("KeyTurn config qpos_dim does not match model."); + } + if (model_->nv != spec_.config["qvel_dim"_]) { + throw std::runtime_error("KeyTurn config qvel_dim does not match model."); + } + if (model_->nu != spec_.config["action_dim"_]) { + throw std::runtime_error( + "KeyTurn config action_dim does not match model."); + } + if (model_->na != spec_.config["act_dim"_]) { + throw std::runtime_error("KeyTurn config act_dim does not match model."); + } + int expected_obs = model_->nq + model_->nv + 6 + model_->na; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error("KeyTurn config obs_dim does not match model."); + } + if (key_init_range_.size() != 2) { + throw std::runtime_error("KeyTurn key_init_range must have length 2."); + } + if (!test_reset_qpos_.empty() && + static_cast(test_reset_qpos_.size()) != model_->nq) { + throw std::runtime_error("KeyTurn test_reset_qpos has wrong length."); + } + if (!test_reset_qvel_.empty() && + static_cast(test_reset_qvel_.size()) != model_->nv) { + throw std::runtime_error("KeyTurn test_reset_qvel has wrong length."); + } + if (!test_reset_act_.empty() && + static_cast(test_reset_act_.size()) != model_->na) { + throw std::runtime_error("KeyTurn test_reset_act has wrong length."); + } + if (!test_reset_qacc_warmstart_.empty() && + static_cast(test_reset_qacc_warmstart_.size()) != model_->nv) { + throw std::runtime_error( + "KeyTurn test_reset_qacc_warmstart has wrong length."); + } + if (!test_key_body_pos_.empty() && + static_cast(test_key_body_pos_.size()) != 3) { + throw std::runtime_error("KeyTurn test_key_body_pos has wrong length."); + } + } + + void CacheSites() { + keyhead_sid_ = mj_name2id(model_, mjOBJ_SITE, "keyhead"); + if_sid_ = mj_name2id(model_, mjOBJ_SITE, "IFtip"); + th_sid_ = mj_name2id(model_, mjOBJ_SITE, "THtip"); + if (keyhead_sid_ == -1 || if_sid_ == -1 || th_sid_ == -1) { + throw std::runtime_error("KeyTurn site missing."); + } + key_body_id_ = model_->site_bodyid[keyhead_sid_]; + detail::CopyModelBodyPos(model_, key_body_id_, &initial_key_body_pos_); + } + + void ApplyResetState() { + bool has_test_reset_override = + !test_reset_qpos_.empty() || !test_reset_qvel_.empty() || + !test_reset_act_.empty() || !test_reset_qacc_warmstart_.empty(); + if (!test_reset_qpos_.empty()) { + detail::RestoreVector(test_reset_qpos_, data_->qpos); + detail::RestoreVector(test_reset_qvel_, data_->qvel); + } + if (!test_key_body_pos_.empty()) { + detail::RestoreModelBodyPos(model_, key_body_id_, test_key_body_pos_); + } else if (key_init_range_[0] != key_init_range_[1]) { + std::vector body_pos = initial_key_body_pos_; + for (int axis = 0; axis < 3; ++axis) { + body_pos[axis] += static_cast(unit_dist_(gen_) * 0.02 - 0.01); + } + detail::RestoreModelBodyPos(model_, key_body_id_, body_pos); + } + if (test_reset_qpos_.empty()) { + data_->qpos[model_->nq - 1] = + key_init_range_[0] + static_cast(unit_dist_(gen_)) * + (key_init_range_[1] - key_init_range_[0]); + } + mj_forward(model_, data_); + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, data_->qacc_warmstart); + } + if (has_test_reset_override) { + detail::FinalizeMyoSuiteResetSync(model_, data_); + } + } + + RewardInfo ComputeRewardInfo() const { + RewardInfo reward; + const mjtNum* key_pos3 = data_->site_xpos + keyhead_sid_ * 3; + const mjtNum* if_pos3 = data_->site_xpos + if_sid_ * 3; + const mjtNum* th_pos3 = data_->site_xpos + th_sid_ * 3; + mjtNum if_sq = 0.0; + mjtNum th_sq = 0.0; + for (int axis = 0; axis < 3; ++axis) { + mjtNum if_delta = key_pos3[axis] - if_pos3[axis]; + mjtNum th_delta = key_pos3[axis] - th_pos3[axis]; + if_sq += if_delta * if_delta; + th_sq += th_delta * th_delta; + } + reward.key_turn = data_->qpos[model_->nq - 1]; + reward.iftip_approach_dist = + std::abs(std::sqrt(if_sq) - static_cast(0.03)); + reward.thtip_approach_dist = + std::abs(std::sqrt(th_sq) - static_cast(0.03)); + reward.act_reg = detail::ActReg(model_, data_); + reward.success = reward.key_turn > goal_th_; + reward.done = reward.iftip_approach_dist > static_cast(0.1) || + reward.thtip_approach_dist > static_cast(0.1); + mjtNum bonus = static_cast(reward.key_turn > mjPI / 2.0) + + static_cast(reward.key_turn > mjPI); + mjtNum penalty = -static_cast(reward.iftip_approach_dist > 0.05) - + static_cast(reward.thtip_approach_dist > 0.05); + reward.dense_reward = + reward_key_turn_w_ * reward.key_turn - + reward_iftip_approach_w_ * reward.iftip_approach_dist - + reward_thtip_approach_w_ * reward.thtip_approach_dist - + reward_act_reg_w_ * reward.act_reg + reward_bonus_w_ * bonus + + reward_penalty_w_ * penalty; + return reward; + } + + void WriteState(const RewardInfo& reward, bool reset, mjtNum reward_value) { + auto state = Allocate(); + state["reward"_] = reward_value; + if constexpr (kFromPixels) { + auto obs_pixels = state["obs:pixels"_]; + AssignPixelObservation("obs:pixels", &obs_pixels, reset); + } else { + auto obs = state["obs"_]; + mjtNum* buffer = PrepareObservation("obs", &obs); + for (int i = 0; i < model_->nq - 1; ++i) { + *(buffer++) = data_->qpos[i]; + } + for (int i = 0; i < model_->nv - 1; ++i) { + *(buffer++) = data_->qvel[i] * Dt(); + } + *(buffer++) = data_->qpos[model_->nq - 1]; + *(buffer++) = data_->qvel[model_->nv - 1] * Dt(); + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = data_->site_xpos[keyhead_sid_ * 3 + axis] - + data_->site_xpos[if_sid_ * 3 + axis]; + } + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = data_->site_xpos[keyhead_sid_ * 3 + axis] - + data_->site_xpos[th_sid_ * 3 + axis]; + } + for (int i = 0; i < model_->na; ++i) { + *(buffer++) = data_->act[i]; + } + CommitObservation("obs", &obs, reset); + } + state["info:key_turn"_] = reward.key_turn; + state["info:iftip_approach_dist"_] = reward.iftip_approach_dist; + state["info:thtip_approach_dist"_] = reward.thtip_approach_dist; + state["info:success"_] = reward.success; +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + state["info:key_body_pos"_].Assign(model_->body_pos + key_body_id_ * 3, 3); + } +}; + +template +class MyoSuiteObjHoldEnvBase : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::gen_; + using Base::spec_; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum goal_dist{0.0}; + bool success{false}; + bool done{false}; + }; + + bool normalize_act_; + bool randomize_on_reset_; + mjtNum reward_goal_dist_w_; + mjtNum reward_bonus_w_; + mjtNum reward_penalty_w_; + int object_sid_{-1}; + int goal_sid_{-1}; + int object_geom_id_{-1}; + std::vector initial_object_pos_; + std::vector initial_goal_pos_; + std::vector initial_goal_size_; + std::vector initial_object_geom_size_; + std::vector muscle_actuator_; + detail::MyoConditionState muscle_condition_state_; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_act_; + std::vector test_reset_qacc_warmstart_; + std::vector test_goal_pos_; + std::vector test_object_geom_size_; + std::uniform_real_distribution unit_dist_{0.0, 1.0}; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoSuiteObjHoldEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + myosuite::MyoSuiteModelPath(spec.config["base_path"_], + spec.config["model_path"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + randomize_on_reset_(spec.config["randomize_on_reset"_]), + reward_goal_dist_w_(spec.config["reward_goal_dist_w"_]), + reward_bonus_w_(spec.config["reward_bonus_w"_]), + reward_penalty_w_(spec.config["reward_penalty_w"_]), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])), + test_goal_pos_(detail::ToMjtVector(spec.config["test_goal_pos"_])), + test_object_geom_size_( + detail::ToMjtVector(spec.config["test_object_geom_size"_])) { + ValidateConfig(); + CacheObjects(); + detail::BuildMuscleMask(model_, &muscle_actuator_); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + detail::AdjustInitialQposForNormalizedActions(model_, data_, + normalize_act_); + InitializeRobotEnv(); + } + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + detail::ResetMyoConditionState(&muscle_condition_state_); + ResetToInitialState(); + RestoreModelState(); + ApplyResetState(); + CaptureResetState(); + RewardInfo reward = ComputeRewardInfo(); + WriteState(reward, true, 0.0); + } + + void Step(const Action& action) override { + const auto* raw = static_cast(action["action"_].Data()); + detail::ApplyMyoSuiteAction(model_, data_, muscle_actuator_, normalize_act_, + raw); + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + ++elapsed_step_; + RewardInfo reward = ComputeRewardInfo(); + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, false, reward.dense_reward); + } + + private: + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_] || + model_->nv != spec_.config["qvel_dim"_] || + model_->nu != spec_.config["action_dim"_] || + model_->na != spec_.config["act_dim"_]) { + throw std::runtime_error("ObjHold config dims do not match model."); + } + int expected_obs = (model_->nq - 7) + (model_->nv - 6) + 6 + model_->na; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error("ObjHold config obs_dim does not match model."); + } + if (!test_goal_pos_.empty() && + static_cast(test_goal_pos_.size()) != 3) { + throw std::runtime_error("ObjHold test_goal_pos has wrong length."); + } + if (!test_object_geom_size_.empty() && + static_cast(test_object_geom_size_.size()) != 3) { + throw std::runtime_error( + "ObjHold test_object_geom_size has wrong length."); + } + } + + void CacheObjects() { + object_sid_ = mj_name2id(model_, mjOBJ_SITE, "object"); + goal_sid_ = mj_name2id(model_, mjOBJ_SITE, "goal"); + object_geom_id_ = mj_name2id(model_, mjOBJ_GEOM, "object"); + if (object_sid_ == -1 || goal_sid_ == -1 || object_geom_id_ == -1) { + throw std::runtime_error("ObjHold object/goal ids missing."); + } + initial_object_pos_.assign(data_->site_xpos + object_sid_ * 3, + data_->site_xpos + object_sid_ * 3 + 3); + detail::CopyModelSitePos(model_, goal_sid_, &initial_goal_pos_); + detail::CopyModelSiteSize(model_, goal_sid_, &initial_goal_size_); + detail::CopyModelGeomSize(model_, object_geom_id_, + &initial_object_geom_size_); + } + + void RestoreModelState() { + detail::RestoreModelSitePos(model_, goal_sid_, initial_goal_pos_); + detail::RestoreModelSiteSize(model_, goal_sid_, initial_goal_size_); + detail::RestoreModelGeomSize(model_, object_geom_id_, + initial_object_geom_size_); + } + + void ApplyResetState() { + bool has_test_reset_override = + !test_reset_qpos_.empty() || !test_reset_qvel_.empty() || + !test_reset_act_.empty() || !test_reset_qacc_warmstart_.empty(); + if (!test_reset_qpos_.empty()) { + detail::RestoreVector(test_reset_qpos_, data_->qpos); + detail::RestoreVector(test_reset_qvel_, data_->qvel); + } + if (!test_goal_pos_.empty()) { + detail::RestoreModelSitePos(model_, goal_sid_, test_goal_pos_); + } else if (randomize_on_reset_) { + std::vector goal_pos = initial_object_pos_; + for (int axis = 0; axis < 3; ++axis) { + goal_pos[axis] += static_cast(unit_dist_(gen_) * 0.06 - 0.03); + } + detail::RestoreModelSitePos(model_, goal_sid_, goal_pos); + } + if (!test_object_geom_size_.empty()) { + detail::RestoreModelGeomSize(model_, object_geom_id_, + test_object_geom_size_); + detail::RestoreModelSiteSize(model_, goal_sid_, test_object_geom_size_); + } else if (randomize_on_reset_) { + std::vector size(3); + for (int axis = 0; axis < 3; ++axis) { + size[axis] = static_cast(0.02 + unit_dist_(gen_) * 0.01); + } + detail::RestoreModelGeomSize(model_, object_geom_id_, size); + detail::RestoreModelSiteSize(model_, goal_sid_, size); + } + mj_forward(model_, data_); + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, data_->qacc_warmstart); + } + if (has_test_reset_override) { + detail::FinalizeMyoSuiteResetSync(model_, data_); + } + } + + RewardInfo ComputeRewardInfo() const { + RewardInfo reward; + mjtNum goal_sq = 0.0; + for (int axis = 0; axis < 3; ++axis) { + mjtNum delta = data_->site_xpos[goal_sid_ * 3 + axis] - + data_->site_xpos[object_sid_ * 3 + axis]; + goal_sq += delta * delta; + } + reward.goal_dist = std::sqrt(goal_sq); + reward.success = reward.goal_dist < static_cast(0.01); + reward.done = reward.goal_dist > static_cast(0.3); + mjtNum bonus = static_cast(reward.goal_dist < 0.02) + + static_cast(reward.goal_dist < 0.01); + mjtNum penalty = -static_cast(reward.done); + reward.dense_reward = -reward_goal_dist_w_ * reward.goal_dist + + reward_bonus_w_ * bonus + reward_penalty_w_ * penalty; + return reward; + } + + void WriteState(const RewardInfo& reward, bool reset, mjtNum reward_value) { + auto state = Allocate(); + state["reward"_] = reward_value; + if constexpr (kFromPixels) { + auto obs_pixels = state["obs:pixels"_]; + AssignPixelObservation("obs:pixels", &obs_pixels, reset); + } else { + auto obs = state["obs"_]; + mjtNum* buffer = PrepareObservation("obs", &obs); + for (int i = 0; i < model_->nq - 7; ++i) { + *(buffer++) = data_->qpos[i]; + } + for (int i = 0; i < model_->nv - 6; ++i) { + *(buffer++) = data_->qvel[i] * Dt(); + } + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = data_->site_xpos[object_sid_ * 3 + axis]; + } + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = data_->site_xpos[goal_sid_ * 3 + axis] - + data_->site_xpos[object_sid_ * 3 + axis]; + } + for (int i = 0; i < model_->na; ++i) { + *(buffer++) = data_->act[i]; + } + CommitObservation("obs", &obs, reset); + } + state["info:goal_dist"_] = reward.goal_dist; + state["info:success"_] = reward.success; +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + state["info:goal_pos"_].Assign(model_->site_pos + goal_sid_ * 3, 3); + state["info:object_geom_size"_].Assign( + model_->geom_size + object_geom_id_ * 3, 3); + } +}; + +template +class MyoSuiteTorsoEnvBase : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::spec_; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum pose_dist{0.0}; + mjtNum act_reg{0.0}; + bool success{false}; + bool done{false}; + }; + + bool normalize_act_; + int pose_dim_; + mjtNum pose_thd_; + mjtNum reward_pose_w_; + mjtNum reward_bonus_w_; + mjtNum reward_act_reg_w_; + mjtNum reward_penalty_w_; + std::vector target_qpos_value_; + std::vector muscle_actuator_; + detail::MyoConditionState muscle_condition_state_; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_act_; + std::vector test_reset_qacc_warmstart_; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoSuiteTorsoEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + myosuite::MyoSuiteModelPath(spec.config["base_path"_], + spec.config["model_path"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + pose_dim_(spec.config["pose_dim"_]), + pose_thd_(spec.config["pose_thd"_]), + reward_pose_w_(spec.config["reward_pose_w"_]), + reward_bonus_w_(spec.config["reward_bonus_w"_]), + reward_act_reg_w_(spec.config["reward_act_reg_w"_]), + reward_penalty_w_(spec.config["reward_penalty_w"_]), + target_qpos_value_( + detail::ToMjtVector(spec.config["target_qpos_value"_])), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])) { + ValidateConfig(); + detail::BuildMuscleMask(model_, &muscle_actuator_); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + detail::AdjustInitialQposForNormalizedActions(model_, data_, + normalize_act_); + InitializeRobotEnv(); + } + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + detail::ResetMyoConditionState(&muscle_condition_state_); + ResetToInitialState(); + if (!test_reset_qpos_.empty()) { + detail::RestoreVector(test_reset_qpos_, data_->qpos); + detail::RestoreVector(test_reset_qvel_, data_->qvel); + mj_forward(model_, data_); + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, + data_->qacc_warmstart); + } + detail::FinalizeMyoSuiteResetSync(model_, data_); + } + CaptureResetState(); + RewardInfo reward = ComputeRewardInfo(); + WriteState(reward, true, 0.0); + } + + void Step(const Action& action) override { + const auto* raw = static_cast(action["action"_].Data()); + detail::ApplyMyoSuiteAction(model_, data_, muscle_actuator_, normalize_act_, + raw); + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + ++elapsed_step_; + RewardInfo reward = ComputeRewardInfo(); + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, false, reward.dense_reward); + } + + private: + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_] || + model_->nv != spec_.config["qvel_dim"_] || + model_->nu != spec_.config["action_dim"_] || + model_->na != spec_.config["act_dim"_]) { + throw std::runtime_error("Torso config dims do not match model."); + } + if (static_cast(target_qpos_value_.size()) != pose_dim_) { + throw std::runtime_error("Torso target_qpos_value has wrong length."); + } + int expected_obs = model_->nq + model_->nv + pose_dim_ + model_->na; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error("Torso config obs_dim does not match model."); + } + } + + RewardInfo ComputeRewardInfo() const { + RewardInfo reward; + std::vector pose_err(pose_dim_); + for (int i = 0; i < pose_dim_; ++i) { + pose_err[i] = target_qpos_value_[i] - data_->qpos[i]; + } + reward.pose_dist = detail::VectorNorm(pose_err); + reward.act_reg = detail::ActReg(model_, data_); + reward.success = reward.pose_dist < pose_thd_; + reward.done = reward.pose_dist > mjPI; + mjtNum bonus = static_cast(reward.pose_dist < pose_thd_) + + static_cast(reward.pose_dist < 1.5 * pose_thd_); + mjtNum penalty = -static_cast(reward.done); + reward.dense_reward = -reward_pose_w_ * reward.pose_dist - + reward_act_reg_w_ * reward.act_reg + + reward_bonus_w_ * bonus + reward_penalty_w_ * penalty; + return reward; + } + + void WriteState(const RewardInfo& reward, bool reset, mjtNum reward_value) { + auto state = Allocate(); + state["reward"_] = reward_value; + if constexpr (kFromPixels) { + auto obs_pixels = state["obs:pixels"_]; + AssignPixelObservation("obs:pixels", &obs_pixels, reset); + } else { + auto obs = state["obs"_]; + mjtNum* buffer = PrepareObservation("obs", &obs); + for (int i = 0; i < model_->nq; ++i) { + *(buffer++) = data_->qpos[i]; + } + for (int i = 0; i < model_->nv; ++i) { + *(buffer++) = data_->qvel[i] * Dt(); + } + for (int i = 0; i < pose_dim_; ++i) { + *(buffer++) = target_qpos_value_[i] - data_->qpos[i]; + } + for (int i = 0; i < model_->na; ++i) { + *(buffer++) = data_->act[i]; + } + CommitObservation("obs", &obs, reset); + } + state["info:pose_dist"_] = reward.pose_dist; + state["info:success"_] = reward.success; +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + state["info:target_qpos"_].Assign(target_qpos_value_.data(), pose_dim_); + } +}; + +template +class MyoSuitePenTwirlEnvBase : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::gen_; + using Base::spec_; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum pos_align{0.0}; + mjtNum rot_align{0.0}; + mjtNum act_reg{0.0}; + bool success{false}; + bool done{false}; + }; + + bool normalize_act_; + bool randomize_target_; + mjtNum reward_pos_align_w_; + mjtNum reward_rot_align_w_; + mjtNum reward_act_reg_w_; + mjtNum reward_drop_w_; + mjtNum reward_bonus_w_; + int target_body_id_{-1}; + int obj_body_id_{-1}; + int eps_ball_sid_{-1}; + int obj_t_sid_{-1}; + int obj_b_sid_{-1}; + int tar_t_sid_{-1}; + int tar_b_sid_{-1}; + mjtNum pen_length_{0.0}; + mjtNum tar_length_{0.0}; + std::vector initial_target_body_quat_; + std::vector muscle_actuator_; + detail::MyoConditionState muscle_condition_state_; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_act_; + std::vector test_reset_qacc_warmstart_; + std::vector test_target_body_quat_; + std::uniform_real_distribution unit_dist_{0.0, 1.0}; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoSuitePenTwirlEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + myosuite::MyoSuiteModelPath(spec.config["base_path"_], + spec.config["model_path"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + randomize_target_(spec.config["randomize_target"_]), + reward_pos_align_w_(spec.config["reward_pos_align_w"_]), + reward_rot_align_w_(spec.config["reward_rot_align_w"_]), + reward_act_reg_w_(spec.config["reward_act_reg_w"_]), + reward_drop_w_(spec.config["reward_drop_w"_]), + reward_bonus_w_(spec.config["reward_bonus_w"_]), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])), + test_target_body_quat_( + detail::ToMjtVector(spec.config["test_target_body_quat"_])) { + ValidateConfig(); + CacheObjects(); + detail::BuildMuscleMask(model_, &muscle_actuator_); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + detail::AdjustInitialQposForNormalizedActions(model_, data_, + normalize_act_); + InitializeRobotEnv(); + } + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + detail::ResetMyoConditionState(&muscle_condition_state_); + ResetToInitialState(); + detail::RestoreModelBodyQuat(model_, target_body_id_, + initial_target_body_quat_); + ApplyResetState(); + CaptureResetState(); + RewardInfo reward = ComputeRewardInfo(); + WriteState(reward, true, 0.0); + } + + void Step(const Action& action) override { + const auto* raw = static_cast(action["action"_].Data()); + detail::ApplyMyoSuiteAction(model_, data_, muscle_actuator_, normalize_act_, + raw); + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + ++elapsed_step_; + RewardInfo reward = ComputeRewardInfo(); + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, false, reward.dense_reward); + } + + private: + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_] || + model_->nv != spec_.config["qvel_dim"_] || + model_->nu != spec_.config["action_dim"_] || + model_->na != spec_.config["act_dim"_]) { + throw std::runtime_error("PenTwirl config dims do not match model."); + } + int expected_obs = (model_->nq - 6) + 21 + model_->na; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error("PenTwirl config obs_dim does not match model."); + } + if (!test_target_body_quat_.empty() && + static_cast(test_target_body_quat_.size()) != 4) { + throw std::runtime_error( + "PenTwirl test_target_body_quat has wrong length."); + } + } + + void CacheObjects() { + target_body_id_ = mj_name2id(model_, mjOBJ_BODY, "target"); + obj_body_id_ = mj_name2id(model_, mjOBJ_BODY, "Object"); + eps_ball_sid_ = mj_name2id(model_, mjOBJ_SITE, "eps_ball"); + obj_t_sid_ = mj_name2id(model_, mjOBJ_SITE, "object_top"); + obj_b_sid_ = mj_name2id(model_, mjOBJ_SITE, "object_bottom"); + tar_t_sid_ = mj_name2id(model_, mjOBJ_SITE, "target_top"); + tar_b_sid_ = mj_name2id(model_, mjOBJ_SITE, "target_bottom"); + if (target_body_id_ == -1 || obj_body_id_ == -1 || eps_ball_sid_ == -1 || + obj_t_sid_ == -1 || obj_b_sid_ == -1 || tar_t_sid_ == -1 || + tar_b_sid_ == -1) { + throw std::runtime_error("PenTwirl ids missing."); + } + std::vector obj_top(model_->site_pos + obj_t_sid_ * 3, + model_->site_pos + obj_t_sid_ * 3 + 3); + std::vector obj_bottom(model_->site_pos + obj_b_sid_ * 3, + model_->site_pos + obj_b_sid_ * 3 + 3); + std::vector tar_top(model_->site_pos + tar_t_sid_ * 3, + model_->site_pos + tar_t_sid_ * 3 + 3); + std::vector tar_bottom(model_->site_pos + tar_b_sid_ * 3, + model_->site_pos + tar_b_sid_ * 3 + 3); + for (int axis = 0; axis < 3; ++axis) { + obj_top[axis] -= obj_bottom[axis]; + tar_top[axis] -= tar_bottom[axis]; + } + pen_length_ = detail::VectorNorm(obj_top); + tar_length_ = detail::VectorNorm(tar_top); + detail::CopyModelBodyQuat(model_, target_body_id_, + &initial_target_body_quat_); + } + + void ApplyResetState() { + bool has_test_reset_override = + !test_reset_qpos_.empty() || !test_reset_qvel_.empty() || + !test_reset_act_.empty() || !test_reset_qacc_warmstart_.empty(); + if (!test_reset_qpos_.empty()) { + detail::RestoreVector(test_reset_qpos_, data_->qpos); + detail::RestoreVector(test_reset_qvel_, data_->qvel); + } + if (!test_target_body_quat_.empty()) { + detail::RestoreModelBodyQuat(model_, target_body_id_, + test_target_body_quat_); + } else if (randomize_target_) { + double x = unit_dist_(gen_) * 2.0 - 1.0; + double y = unit_dist_(gen_) * 2.0 - 1.0; + double cx = std::cos(x * 0.5); + double sx = std::sin(x * 0.5); + double cy = std::cos(y * 0.5); + double sy = std::sin(y * 0.5); + std::vector quat = { + static_cast(cy * cx), static_cast(cy * sx), + static_cast(sy * cx), static_cast(-sy * sx)}; + detail::RestoreModelBodyQuat(model_, target_body_id_, quat); + } + mj_forward(model_, data_); + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, data_->qacc_warmstart); + } + if (has_test_reset_override) { + detail::FinalizeMyoSuiteResetSync(model_, data_); + } + } + + RewardInfo ComputeRewardInfo() const { + RewardInfo reward; + std::vector obj_pos(3); + std::vector obj_des_pos(3); + std::vector obj_rot(3); + std::vector obj_des_rot(3); + for (int axis = 0; axis < 3; ++axis) { + obj_pos[axis] = data_->xpos[obj_body_id_ * 3 + axis]; + obj_des_pos[axis] = data_->site_xpos[eps_ball_sid_ * 3 + axis]; + obj_rot[axis] = (data_->site_xpos[obj_t_sid_ * 3 + axis] - + data_->site_xpos[obj_b_sid_ * 3 + axis]) / + pen_length_; + obj_des_rot[axis] = (data_->site_xpos[tar_t_sid_ * 3 + axis] - + data_->site_xpos[tar_b_sid_ * 3 + axis]) / + tar_length_; + } + for (int axis = 0; axis < 3; ++axis) { + obj_pos[axis] -= obj_des_pos[axis]; + } + reward.pos_align = detail::VectorNorm(obj_pos); + reward.rot_align = detail::CosineSimilarity(obj_rot, obj_des_rot); + reward.act_reg = detail::ActReg(model_, data_); + reward.done = reward.pos_align > static_cast(0.075); + reward.success = + reward.rot_align > static_cast(0.95) && !reward.done; + mjtNum bonus = static_cast(reward.rot_align > 0.9 && + reward.pos_align < 0.075) + + static_cast(5.0 * (reward.rot_align > 0.95 && + reward.pos_align < 0.075)); + mjtNum drop = -static_cast(reward.done); + reward.dense_reward = -reward_pos_align_w_ * reward.pos_align + + reward_rot_align_w_ * reward.rot_align - + reward_act_reg_w_ * reward.act_reg + + reward_drop_w_ * drop + reward_bonus_w_ * bonus; + return reward; + } + + void WriteState(const RewardInfo& reward, bool reset, mjtNum reward_value) { + auto state = Allocate(); + state["reward"_] = reward_value; + if constexpr (kFromPixels) { + auto obs_pixels = state["obs:pixels"_]; + AssignPixelObservation("obs:pixels", &obs_pixels, reset); + } else { + auto obs = state["obs"_]; + mjtNum* buffer = PrepareObservation("obs", &obs); + for (int i = 0; i < model_->nq - 6; ++i) { + *(buffer++) = data_->qpos[i]; + } + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = data_->xpos[obj_body_id_ * 3 + axis]; + } + for (int i = model_->nv - 6; i < model_->nv; ++i) { + *(buffer++) = data_->qvel[i] * Dt(); + } + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = (data_->site_xpos[obj_t_sid_ * 3 + axis] - + data_->site_xpos[obj_b_sid_ * 3 + axis]) / + pen_length_; + } + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = (data_->site_xpos[tar_t_sid_ * 3 + axis] - + data_->site_xpos[tar_b_sid_ * 3 + axis]) / + tar_length_; + } + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = data_->xpos[obj_body_id_ * 3 + axis] - + data_->site_xpos[eps_ball_sid_ * 3 + axis]; + } + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = (data_->site_xpos[obj_t_sid_ * 3 + axis] - + data_->site_xpos[obj_b_sid_ * 3 + axis]) / + pen_length_ - + (data_->site_xpos[tar_t_sid_ * 3 + axis] - + data_->site_xpos[tar_b_sid_ * 3 + axis]) / + tar_length_; + } + for (int i = 0; i < model_->na; ++i) { + *(buffer++) = data_->act[i]; + } + CommitObservation("obs", &obs, reset); + } + state["info:pos_align"_] = reward.pos_align; + state["info:rot_align"_] = reward.rot_align; + state["info:success"_] = reward.success; +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + state["info:target_body_quat"_].Assign( + model_->body_quat + target_body_id_ * 4, 4); + } +}; + +template +using PosePixelEnvBase = MyoSuitePoseEnvBase; + +template +using ReachPixelEnvBase = MyoSuiteReachEnvBase; + +template +using KeyTurnPixelEnvBase = MyoSuiteKeyTurnEnvBase; + +template +using ObjHoldPixelEnvBase = MyoSuiteObjHoldEnvBase; + +template +using TorsoPixelEnvBase = MyoSuiteTorsoEnvBase; + +template +using PenTwirlPixelEnvBase = MyoSuitePenTwirlEnvBase; + +using MyoSuitePoseEnv = MyoSuitePoseEnvBase; +using MyoSuitePosePixelEnv = PosePixelEnvBase; +using MyoSuitePoseEnvPool = AsyncEnvPool; +using MyoSuitePosePixelEnvPool = AsyncEnvPool; + +using MyoSuiteReachEnv = MyoSuiteReachEnvBase; +using MyoSuiteReachPixelEnv = ReachPixelEnvBase; +using MyoSuiteReachEnvPool = AsyncEnvPool; +using MyoSuiteReachPixelEnvPool = AsyncEnvPool; + +using MyoSuiteKeyTurnEnv = + // NOLINTNEXTLINE(whitespace/indent_namespace) + MyoSuiteKeyTurnEnvBase; +using MyoSuiteKeyTurnPixelEnv = + // NOLINTNEXTLINE(whitespace/indent_namespace) + KeyTurnPixelEnvBase; +using MyoSuiteKeyTurnEnvPool = AsyncEnvPool; +using MyoSuiteKeyTurnPixelEnvPool = AsyncEnvPool; + +using MyoSuiteObjHoldEnv = + // NOLINTNEXTLINE(whitespace/indent_namespace) + MyoSuiteObjHoldEnvBase; +using MyoSuiteObjHoldPixelEnv = + // NOLINTNEXTLINE(whitespace/indent_namespace) + ObjHoldPixelEnvBase; +using MyoSuiteObjHoldEnvPool = AsyncEnvPool; +using MyoSuiteObjHoldPixelEnvPool = AsyncEnvPool; + +using MyoSuiteTorsoEnv = MyoSuiteTorsoEnvBase; +using MyoSuiteTorsoPixelEnv = TorsoPixelEnvBase; +using MyoSuiteTorsoEnvPool = AsyncEnvPool; +using MyoSuiteTorsoPixelEnvPool = AsyncEnvPool; + +using MyoSuitePenTwirlEnv = + // NOLINTNEXTLINE(whitespace/indent_namespace) + MyoSuitePenTwirlEnvBase; +using MyoSuitePenTwirlPixelEnv = + // NOLINTNEXTLINE(whitespace/indent_namespace) + PenTwirlPixelEnvBase; +using MyoSuitePenTwirlEnvPool = AsyncEnvPool; +using MyoSuitePenTwirlPixelEnvPool = AsyncEnvPool; + +} // namespace myosuite_envpool + +#endif // ENVPOOL_MUJOCO_MYOSUITE_MYOBASE_H_ diff --git a/envpool/mujoco/myosuite/myobase_extended.h b/envpool/mujoco/myosuite/myobase_extended.h new file mode 100644 index 000000000..1a55d615d --- /dev/null +++ b/envpool/mujoco/myosuite/myobase_extended.h @@ -0,0 +1,1408 @@ +// Copyright 2026 Garena Online Private 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 ENVPOOL_MUJOCO_MYOSUITE_MYOBASE_EXTENDED_H_ +#define ENVPOOL_MUJOCO_MYOSUITE_MYOBASE_EXTENDED_H_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "envpool/mujoco/myosuite/myobase.h" + +namespace myosuite_envpool { + +namespace detail { + +inline std::array EulerXYZToQuat(mjtNum rx, mjtNum ry, mjtNum rz) { + mjtNum cx = std::cos(rx * static_cast(0.5)); + mjtNum sx = std::sin(rx * static_cast(0.5)); + mjtNum cy = std::cos(ry * static_cast(0.5)); + mjtNum sy = std::sin(ry * static_cast(0.5)); + mjtNum cz = std::cos(rz * static_cast(0.5)); + mjtNum sz = std::sin(rz * static_cast(0.5)); + return { + cx * cy * cz + sx * sy * sz, + sx * cy * cz - cx * sy * sz, + cx * sy * cz + sx * cy * sz, + cx * cy * sz - sx * sy * cz, + }; +} + +inline std::vector RandomRgba(std::mt19937* gen) { + std::uniform_real_distribution dist(0.1, 0.9); + return { + static_cast(dist(*gen)), + static_cast(dist(*gen)), + static_cast(dist(*gen)), + static_cast(1.0), + }; +} + +inline void NormalizeRange(const std::vector& input, + std::vector* output) { + mjtNum low = *std::min_element(input.begin(), input.end()); + mjtNum high = *std::max_element(input.begin(), input.end()); + output->resize(input.size()); + mjtNum denom = std::max(high - low, static_cast(1e-12)); + for (std::size_t i = 0; i < input.size(); ++i) { + (*output)[i] = (input[i] - low) / denom; + } +} + +inline std::vector FlipGrid(const std::vector& grid, int rows, + int cols) { + std::vector flipped(grid.size()); + for (int row = 0; row < rows; ++row) { + for (int col = 0; col < cols; ++col) { + int src = row * cols + col; + int dst = (rows - 1 - row) * cols + (cols - 1 - col); + flipped[dst] = grid[src]; + } + } + return flipped; +} + +inline mjtNum ClipValue(mjtNum value, mjtNum lo, mjtNum hi) { + return std::min(std::max(value, lo), hi); +} + +} // namespace detail + +class MyoSuiteReorientEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(5), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), "randomization_mode"_.Bind(std::string("100")), + "reward_pos_align_w"_.Bind(1.0), "reward_rot_align_w"_.Bind(1.0), + "reward_act_reg_w"_.Bind(5.0), "reward_drop_w"_.Bind(5.0), + "reward_bonus_w"_.Bind(10.0), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{}), + "test_target_body_quat"_.Bind(std::vector{}), + "test_object_geom_size"_.Bind(std::vector{}), + "test_object_geom_rgba"_.Bind(std::vector{}), + "test_object_geom_top_pos"_.Bind(std::vector{}), + "test_object_geom_bottom_pos"_.Bind(std::vector{}), + "test_target_geom_size"_.Bind(std::vector{}), + "test_target_geom_rgba"_.Bind(std::vector{}), + "test_target_geom_top_pos"_.Bind(std::vector{}), + "test_target_geom_bottom_pos"_.Bind(std::vector{}), + "test_object_body_mass"_.Bind(std::vector{}), + "test_success_site_rgba"_.Bind(std::vector{}), + "test_object_geom_type"_.Bind(-1), "test_target_geom_type"_.Bind(-1), + "test_object_geom_condim"_.Bind(-1)); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:pos_align"_.Bind(Spec({-1}, {0.0, inf})), + "info:rot_align"_.Bind(Spec({-1}, {-1.0, 1.0})), + "info:success"_.Bind(Spec({-1}, {0.0, 1.0})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:target_body_quat"_.Bind(Spec({4}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using ReorientPixelFns = PixelObservationEnvFns; +using MyoSuiteReorientEnvSpec = EnvSpec; +using MyoSuiteReorientPixelEnvSpec = EnvSpec; + +class MyoSuiteWalkEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(10), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), "min_height"_.Bind(0.8), "max_rot"_.Bind(0.8), + "hip_period"_.Bind(100), "reset_type"_.Bind(std::string("init")), + "target_x_vel"_.Bind(0.0), "target_y_vel"_.Bind(1.2), + "target_rot"_.Bind(std::vector{}), + "terrain"_.Bind(std::string()), "terrain_variant"_.Bind(std::string()), + "use_knee_condition"_.Bind(false), "reward_vel_w"_.Bind(5.0), + "reward_done_w"_.Bind(-100.0), "reward_cyclic_hip_w"_.Bind(-10.0), + "reward_ref_rot_w"_.Bind(10.0), "reward_joint_angle_w"_.Bind(5.0), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{}), + "test_hfield_data"_.Bind(std::vector{}), + "test_terrain_geom_rgba"_.Bind(std::vector{}), + "test_terrain_geom_pos"_.Bind(std::vector{}), + "test_terrain_geom_contype"_.Bind(-1), + "test_terrain_geom_conaffinity"_.Bind(-1)); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:vel_reward"_.Bind(Spec({-1}, {-inf, inf})), + "info:cyclic_hip"_.Bind(Spec({-1}, {0.0, inf})), + "info:ref_rot"_.Bind(Spec({-1}, {0.0, inf})), + "info:joint_angle_rew"_.Bind(Spec({-1}, {0.0, inf})), + "info:success"_.Bind(Spec({-1}, {0.0, 1.0})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:phase_var"_.Bind(Spec({-1}, {0.0, 1.0}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using MyoSuiteWalkEnvSpec = EnvSpec; +using MyoSuiteWalkPixelEnvFns = PixelObservationEnvFns; +using MyoSuiteWalkPixelEnvSpec = EnvSpec; + +class MyoSuiteTerrainEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MyoSuiteWalkEnvFns::DefaultConfig(); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + return MyoSuiteWalkEnvFns::StateSpec(conf); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MyoSuiteWalkEnvFns::ActionSpec(conf); + } +}; + +using TerrainPixelFns = PixelObservationEnvFns; +using MyoSuiteTerrainEnvSpec = EnvSpec; +using MyoSuiteTerrainPixelEnvSpec = EnvSpec; + +template +class MyoSuiteReorientEnvBase : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::gen_; + using Base::spec_; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum pos_align{0.0}; + mjtNum rot_align{0.0}; + mjtNum act_reg{0.0}; + bool success{false}; + bool done{false}; + }; + + bool normalize_act_; + std::string randomization_mode_; + mjtNum reward_pos_align_w_; + mjtNum reward_rot_align_w_; + mjtNum reward_act_reg_w_; + mjtNum reward_drop_w_; + mjtNum reward_bonus_w_; + int target_body_id_{-1}; + int object_body_id_{-1}; + int eps_ball_sid_{-1}; + int success_sid_{-1}; + int obj_geom_id_{-1}; + int target_geom_id_{-1}; + int obj_top_geom_id_{-1}; + int obj_bottom_geom_id_{-1}; + int target_top_geom_id_{-1}; + int target_bottom_geom_id_{-1}; + mjtNum pen_length_{0.0}; + mjtNum target_length_{0.0}; + mjtNum initial_object_body_mass_{0.0}; + int initial_object_geom_type_{-1}; + int initial_object_geom_condim_{-1}; + int initial_target_geom_type_{-1}; + std::vector initial_target_body_quat_; + std::vector initial_object_geom_size_; + std::vector initial_target_geom_size_; + std::vector initial_object_geom_rgba_; + std::vector initial_target_geom_rgba_; + std::vector initial_object_top_pos_; + std::vector initial_object_bottom_pos_; + std::vector initial_target_top_pos_; + std::vector initial_target_bottom_pos_; + std::vector initial_success_rgba_; + std::vector muscle_actuator_; + detail::MyoConditionState muscle_condition_state_; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_act_; + std::vector test_reset_qacc_warmstart_; + std::vector test_target_body_quat_; + std::vector test_object_geom_size_; + std::vector test_object_geom_rgba_; + std::vector test_object_geom_top_pos_; + std::vector test_object_geom_bottom_pos_; + std::vector test_target_geom_size_; + std::vector test_target_geom_rgba_; + std::vector test_target_geom_top_pos_; + std::vector test_target_geom_bottom_pos_; + std::vector test_object_body_mass_; + std::vector test_success_site_rgba_; + int test_object_geom_type_; + int test_target_geom_type_; + int test_object_geom_condim_; + std::uniform_real_distribution unit_dist_{0.0, 1.0}; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoSuiteReorientEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + myosuite::MyoSuiteModelPath(spec.config["base_path"_], + spec.config["model_path"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + randomization_mode_(spec.config["randomization_mode"_]), + reward_pos_align_w_(spec.config["reward_pos_align_w"_]), + reward_rot_align_w_(spec.config["reward_rot_align_w"_]), + reward_act_reg_w_(spec.config["reward_act_reg_w"_]), + reward_drop_w_(spec.config["reward_drop_w"_]), + reward_bonus_w_(spec.config["reward_bonus_w"_]), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])), + test_target_body_quat_( + detail::ToMjtVector(spec.config["test_target_body_quat"_])), + test_object_geom_size_( + detail::ToMjtVector(spec.config["test_object_geom_size"_])), + test_object_geom_rgba_( + detail::ToMjtVector(spec.config["test_object_geom_rgba"_])), + test_object_geom_top_pos_( + detail::ToMjtVector(spec.config["test_object_geom_top_pos"_])), + test_object_geom_bottom_pos_( + detail::ToMjtVector(spec.config["test_object_geom_bottom_pos"_])), + test_target_geom_size_( + detail::ToMjtVector(spec.config["test_target_geom_size"_])), + test_target_geom_rgba_( + detail::ToMjtVector(spec.config["test_target_geom_rgba"_])), + test_target_geom_top_pos_( + detail::ToMjtVector(spec.config["test_target_geom_top_pos"_])), + test_target_geom_bottom_pos_( + detail::ToMjtVector(spec.config["test_target_geom_bottom_pos"_])), + test_object_body_mass_( + detail::ToMjtVector(spec.config["test_object_body_mass"_])), + test_success_site_rgba_( + detail::ToMjtVector(spec.config["test_success_site_rgba"_])), + test_object_geom_type_(spec.config["test_object_geom_type"_]), + test_target_geom_type_(spec.config["test_target_geom_type"_]), + test_object_geom_condim_(spec.config["test_object_geom_condim"_]) { + ValidateConfig(); + CacheObjects(); + detail::BuildMuscleMask(model_, &muscle_actuator_); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + detail::AdjustInitialQposForNormalizedActions(model_, data_, + normalize_act_); + for (int i = 0; i < model_->nq - 6; ++i) { + data_->qpos[i] = 0.0; + } + data_->qpos[0] = static_cast(-1.5); + mj_forward(model_, data_); + InitializeRobotEnv(); + } + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + detail::ResetMyoConditionState(&muscle_condition_state_); + ResetToInitialState(); + RestoreModelState(); + ApplyResetState(); + CaptureResetState(); + RewardInfo reward = ComputeRewardInfo(); + WriteState(reward, true, 0.0); + } + + void Step(const Action& action) override { + const auto* raw = static_cast(action["action"_].Data()); + detail::ApplyMyoSuiteAction(model_, data_, muscle_actuator_, normalize_act_, + raw); + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + ++elapsed_step_; + RewardInfo reward = ComputeRewardInfo(); + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, false, reward.dense_reward); + } + + private: + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_] || + model_->nv != spec_.config["qvel_dim"_] || + model_->nu != spec_.config["action_dim"_] || + model_->na != spec_.config["act_dim"_]) { + throw std::runtime_error("Reorient config dims do not match model."); + } + int expected_obs = (model_->nq - 6) + 21 + 3 * model_->nu + model_->na; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error("Reorient config obs_dim does not match model."); + } + } + + void CacheObjects() { + target_body_id_ = mj_name2id(model_, mjOBJ_BODY, "target"); + object_body_id_ = mj_name2id(model_, mjOBJ_BODY, "Object"); + eps_ball_sid_ = mj_name2id(model_, mjOBJ_SITE, "eps_ball"); + success_sid_ = mj_name2id(model_, mjOBJ_SITE, "success"); + obj_geom_id_ = mj_name2id(model_, mjOBJ_GEOM, "obj"); + target_geom_id_ = mj_name2id(model_, mjOBJ_GEOM, "target"); + obj_top_geom_id_ = mj_name2id(model_, mjOBJ_GEOM, "top"); + obj_bottom_geom_id_ = mj_name2id(model_, mjOBJ_GEOM, "bot"); + target_top_geom_id_ = mj_name2id(model_, mjOBJ_GEOM, "t_top"); + target_bottom_geom_id_ = mj_name2id(model_, mjOBJ_GEOM, "t_bot"); + if (target_body_id_ == -1 || object_body_id_ == -1 || eps_ball_sid_ == -1 || + success_sid_ == -1 || obj_geom_id_ == -1 || target_geom_id_ == -1 || + obj_top_geom_id_ == -1 || obj_bottom_geom_id_ == -1 || + target_top_geom_id_ == -1 || target_bottom_geom_id_ == -1) { + throw std::runtime_error("Reorient ids missing."); + } + detail::CopyModelBodyQuat(model_, target_body_id_, + &initial_target_body_quat_); + detail::CopyModelGeomSize(model_, obj_geom_id_, &initial_object_geom_size_); + detail::CopyModelGeomSize(model_, target_geom_id_, + &initial_target_geom_size_); + detail::CopyModelGeomRgba(model_, obj_geom_id_, &initial_object_geom_rgba_); + detail::CopyModelGeomRgba(model_, target_geom_id_, + &initial_target_geom_rgba_); + detail::CopyModelGeomPos(model_, obj_top_geom_id_, + &initial_object_top_pos_); + detail::CopyModelGeomPos(model_, obj_bottom_geom_id_, + &initial_object_bottom_pos_); + detail::CopyModelGeomPos(model_, target_top_geom_id_, + &initial_target_top_pos_); + detail::CopyModelGeomPos(model_, target_bottom_geom_id_, + &initial_target_bottom_pos_); + detail::CopyModelSiteRgba(model_, success_sid_, &initial_success_rgba_); + detail::CopyModelGeomType(model_, obj_geom_id_, &initial_object_geom_type_); + detail::CopyModelGeomType(model_, target_geom_id_, + &initial_target_geom_type_); + detail::CopyModelGeomCondim(model_, obj_geom_id_, + &initial_object_geom_condim_); + detail::CopyModelBodyMass(model_, object_body_id_, + &initial_object_body_mass_); + UpdateLengths(); + } + + void RestoreModelState() { + detail::RestoreModelBodyQuat(model_, target_body_id_, + initial_target_body_quat_); + detail::RestoreModelGeomSize(model_, obj_geom_id_, + initial_object_geom_size_); + detail::RestoreModelGeomSize(model_, target_geom_id_, + initial_target_geom_size_); + detail::RestoreModelGeomRgba(model_, obj_geom_id_, + initial_object_geom_rgba_); + detail::RestoreModelGeomRgba(model_, target_geom_id_, + initial_target_geom_rgba_); + detail::RestoreModelGeomPos(model_, obj_top_geom_id_, + initial_object_top_pos_); + detail::RestoreModelGeomPos(model_, obj_bottom_geom_id_, + initial_object_bottom_pos_); + detail::RestoreModelGeomPos(model_, target_top_geom_id_, + initial_target_top_pos_); + detail::RestoreModelGeomPos(model_, target_bottom_geom_id_, + initial_target_bottom_pos_); + detail::RestoreModelSiteRgba(model_, success_sid_, initial_success_rgba_); + detail::RestoreModelGeomType(model_, obj_geom_id_, + initial_object_geom_type_); + detail::RestoreModelGeomType(model_, target_geom_id_, + initial_target_geom_type_); + detail::RestoreModelGeomCondim(model_, obj_geom_id_, + initial_object_geom_condim_); + detail::RestoreModelBodyMass(model_, object_body_id_, + initial_object_body_mass_); + } + + void UpdateLengths() { + std::vector top(3); + std::vector bottom(3); + for (int axis = 0; axis < 3; ++axis) { + top[axis] = model_->geom_pos[obj_top_geom_id_ * 3 + axis]; + bottom[axis] = model_->geom_pos[obj_bottom_geom_id_ * 3 + axis]; + } + for (int axis = 0; axis < 3; ++axis) { + top[axis] -= bottom[axis]; + } + pen_length_ = detail::VectorNorm(top); + for (int axis = 0; axis < 3; ++axis) { + top[axis] = model_->geom_pos[target_top_geom_id_ * 3 + axis]; + bottom[axis] = model_->geom_pos[target_bottom_geom_id_ * 3 + axis]; + } + for (int axis = 0; axis < 3; ++axis) { + top[axis] -= bottom[axis]; + } + target_length_ = detail::VectorNorm(top); + } + + std::vector RandomSizeForType(int geom_type) { + if (randomization_mode_ == "8") { + static const std::array, 8> sizes = {{ + {0.013, 0.025, 0.025}, + {0.019, 0.040, 0.040}, + {0.017, 0.017, 0.017}, + {0.023, 0.023, 0.023}, + {0.013, 0.025, 0.025}, + {0.019, 0.040, 0.040}, + {0.013, 0.025, 0.025}, + {0.019, 0.040, 0.040}, + }}; + std::array ids = {2, 3}; + if (geom_type == 3) { + ids = {4, 5}; + } else if (geom_type == 4) { + ids = {0, 1}; + } else if (geom_type == 5) { + ids = {6, 7}; + } + int pick = + ids[static_cast(unit_dist_(gen_) * ids.size()) % ids.size()]; + return {sizes[pick][0], sizes[pick][1], sizes[pick][2]}; + } + std::uniform_real_distribution xdist(0.008, 0.028); + std::uniform_real_distribution ydist(0.020, 0.050); + std::uniform_real_distribution zdist(0.020, 0.050); + if (randomization_mode_ == "ood") { + xdist = std::uniform_real_distribution(0.015, 0.035); + ydist = std::uniform_real_distribution(0.015, 0.055); + zdist = std::uniform_real_distribution(0.015, 0.055); + } + return { + static_cast(xdist(gen_)), + static_cast(ydist(gen_)), + static_cast(zdist(gen_)), + }; + } + + void RandomizeGeometry() { + static const std::array geom_types = {3, 4, 5, 6}; + int geom_type = + geom_types[static_cast(unit_dist_(gen_) * geom_types.size()) % + geom_types.size()]; + std::vector size = RandomSizeForType(geom_type); + std::vector color = detail::RandomRgba(&gen_); + std::vector top_pos(3, 0.0); + std::vector bottom_pos(3, 0.0); + if (geom_type == 3) { + top_pos[2] = static_cast(1.3) * size[1]; + bottom_pos[2] = -static_cast(1.3) * size[1]; + } else if (geom_type == 4 || geom_type == 6) { + top_pos[2] = size[2]; + bottom_pos[2] = -size[2]; + } else { + top_pos[2] = size[1]; + bottom_pos[2] = -size[1]; + } + detail::RestoreModelGeomSize(model_, obj_geom_id_, size); + detail::RestoreModelGeomType(model_, obj_geom_id_, geom_type); + detail::RestoreModelGeomRgba(model_, obj_geom_id_, color); + detail::RestoreModelGeomPos(model_, obj_top_geom_id_, top_pos); + detail::RestoreModelGeomPos(model_, obj_bottom_geom_id_, bottom_pos); + detail::RestoreModelBodyMass(model_, object_body_id_, + static_cast(1.2)); + detail::RestoreModelGeomSize(model_, target_geom_id_, size); + detail::RestoreModelGeomType(model_, target_geom_id_, geom_type); + detail::RestoreModelGeomRgba(model_, target_geom_id_, color); + detail::RestoreModelGeomPos(model_, target_top_geom_id_, top_pos); + detail::RestoreModelGeomPos(model_, target_bottom_geom_id_, bottom_pos); + detail::RestoreModelGeomCondim(model_, obj_geom_id_, 3); + auto quat = detail::EulerXYZToQuat( + static_cast(unit_dist_(gen_) * 2.0 - 1.0), + static_cast(unit_dist_(gen_) * 2.0 - 0.8), 0.0); + std::vector quat_vec = {quat[0], quat[1], quat[2], quat[3]}; + detail::RestoreModelBodyQuat(model_, target_body_id_, quat_vec); + std::vector success_rgba = initial_success_rgba_; + success_rgba[0] = 2.0; + success_rgba[1] = 0.0; + detail::RestoreModelSiteRgba(model_, success_sid_, success_rgba); + } + + void ApplyResetState() { + if (test_object_geom_type_ >= 0) { + detail::RestoreModelGeomType(model_, obj_geom_id_, + test_object_geom_type_); + detail::RestoreModelGeomType(model_, target_geom_id_, + test_target_geom_type_); + detail::RestoreModelGeomCondim(model_, obj_geom_id_, + test_object_geom_condim_); + detail::RestoreModelGeomSize(model_, obj_geom_id_, + test_object_geom_size_); + detail::RestoreModelGeomSize(model_, target_geom_id_, + test_target_geom_size_); + detail::RestoreModelGeomRgba(model_, obj_geom_id_, + test_object_geom_rgba_); + detail::RestoreModelGeomRgba(model_, target_geom_id_, + test_target_geom_rgba_); + detail::RestoreModelGeomPos(model_, obj_top_geom_id_, + test_object_geom_top_pos_); + detail::RestoreModelGeomPos(model_, obj_bottom_geom_id_, + test_object_geom_bottom_pos_); + detail::RestoreModelGeomPos(model_, target_top_geom_id_, + test_target_geom_top_pos_); + detail::RestoreModelGeomPos(model_, target_bottom_geom_id_, + test_target_geom_bottom_pos_); + if (!test_object_body_mass_.empty()) { + detail::RestoreModelBodyMass(model_, object_body_id_, + test_object_body_mass_[0]); + } + detail::RestoreModelBodyQuat(model_, target_body_id_, + test_target_body_quat_); + if (!test_success_site_rgba_.empty()) { + detail::RestoreModelSiteRgba(model_, success_sid_, + test_success_site_rgba_); + } else { + std::vector success_rgba = initial_success_rgba_; + success_rgba[0] = 2.0; + success_rgba[1] = 0.0; + detail::RestoreModelSiteRgba(model_, success_sid_, success_rgba); + } + } else { + RandomizeGeometry(); + } + if (!test_reset_qpos_.empty()) { + detail::RestoreVector(test_reset_qpos_, data_->qpos); + detail::RestoreVector(test_reset_qvel_, data_->qvel); + } + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, data_->qacc_warmstart); + } + mj_forward(model_, data_); + } + + RewardInfo ComputeRewardInfo() const { + RewardInfo reward; + std::vector obj_rot(3); + std::vector target_rot(3); + std::vector obj_err_pos(3); + for (int axis = 0; axis < 3; ++axis) { + obj_err_pos[axis] = data_->xpos[object_body_id_ * 3 + axis] - + data_->site_xpos[eps_ball_sid_ * 3 + axis]; + obj_rot[axis] = (data_->geom_xpos[obj_top_geom_id_ * 3 + axis] - + data_->geom_xpos[obj_bottom_geom_id_ * 3 + axis]) / + pen_length_; + target_rot[axis] = (data_->geom_xpos[target_top_geom_id_ * 3 + axis] - + data_->geom_xpos[target_bottom_geom_id_ * 3 + axis]) / + target_length_; + } + reward.pos_align = detail::VectorNorm(obj_err_pos); + reward.rot_align = detail::CosineSimilarity(obj_rot, target_rot); + reward.act_reg = detail::ActReg(model_, data_); + reward.done = reward.pos_align > static_cast(0.075); + reward.success = + reward.rot_align > static_cast(0.95) && !reward.done; + mjtNum bonus = static_cast(reward.rot_align > 0.9 && + reward.pos_align < 0.075) + + static_cast(5.0 * (reward.rot_align > 0.95 && + reward.pos_align < 0.075)); + mjtNum drop = -static_cast(reward.done); + reward.dense_reward = -reward_pos_align_w_ * reward.pos_align + + reward_rot_align_w_ * reward.rot_align - + reward_act_reg_w_ * reward.act_reg + + reward_drop_w_ * drop + reward_bonus_w_ * bonus; + return reward; + } + + void WriteState(const RewardInfo& reward, bool reset, mjtNum reward_value) { + auto state = Allocate(); + state["reward"_] = reward_value; + if constexpr (kFromPixels) { + auto obs_pixels = state["obs:pixels"_]; + AssignPixelObservation("obs:pixels", &obs_pixels, reset); + } else { + auto obs = state["obs"_]; + mjtNum* buffer = PrepareObservation("obs", &obs); + for (int i = 0; i < model_->nq - 6; ++i) { + *(buffer++) = data_->qpos[i]; + } + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = data_->xpos[object_body_id_ * 3 + axis]; + } + for (int i = model_->nv - 6; i < model_->nv; ++i) { + *(buffer++) = data_->qvel[i] * Dt(); + } + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = (data_->geom_xpos[obj_top_geom_id_ * 3 + axis] - + data_->geom_xpos[obj_bottom_geom_id_ * 3 + axis]) / + pen_length_; + } + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = (data_->geom_xpos[target_top_geom_id_ * 3 + axis] - + data_->geom_xpos[target_bottom_geom_id_ * 3 + axis]) / + target_length_; + } + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = data_->xpos[object_body_id_ * 3 + axis] - + data_->site_xpos[eps_ball_sid_ * 3 + axis]; + } + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = (data_->geom_xpos[obj_top_geom_id_ * 3 + axis] - + data_->geom_xpos[obj_bottom_geom_id_ * 3 + axis]) / + pen_length_ - + (data_->geom_xpos[target_top_geom_id_ * 3 + axis] - + data_->geom_xpos[target_bottom_geom_id_ * 3 + axis]) / + target_length_; + } + for (int i = 0; i < model_->nu; ++i) { + *(buffer++) = data_->actuator_length[i]; + } + for (int i = 0; i < model_->nu; ++i) { + *(buffer++) = data_->actuator_velocity[i]; + } + for (int i = 0; i < model_->nu; ++i) { + *(buffer++) = data_->actuator_force[i]; + } + for (int i = 0; i < model_->na; ++i) { + *(buffer++) = data_->act[i]; + } + CommitObservation("obs", &obs, reset); + } + state["info:pos_align"_] = reward.pos_align; + state["info:rot_align"_] = reward.rot_align; + state["info:success"_] = reward.success; +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + state["info:target_body_quat"_].Assign( + model_->body_quat + target_body_id_ * 4, 4); + } +}; + +template +class MyoSuiteWalkLikeEnvBase : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::gen_; + using Base::spec_; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum vel_reward{0.0}; + mjtNum cyclic_hip{0.0}; + mjtNum ref_rot{0.0}; + mjtNum joint_angle_rew{0.0}; + bool success{false}; + bool done{false}; + }; + + bool normalize_act_; + mjtNum min_height_; + mjtNum max_rot_; + int hip_period_; + std::string reset_type_; + mjtNum target_x_vel_; + mjtNum target_y_vel_; + std::vector target_rot_; + std::string terrain_; + std::string terrain_variant_; + bool use_knee_condition_; + mjtNum reward_vel_w_; + mjtNum reward_done_w_; + mjtNum reward_cyclic_hip_w_; + mjtNum reward_ref_rot_w_; + mjtNum reward_joint_angle_w_; + int terrain_geom_id_{-1}; + int terrain_hfield_id_{-1}; + int torso_body_id_{-1}; + int pelvis_body_id_{-1}; + int foot_l_body_id_{-1}; + int foot_r_body_id_{-1}; + std::array hip_flex_qposadr_{-1, -1}; + std::array reward_joint_qposadr_{-1, -1, -1, -1}; + std::vector initial_hfield_data_; + std::vector initial_terrain_rgba_; + std::vector initial_terrain_pos_; + int initial_terrain_contype_{-1}; + int initial_terrain_conaffinity_{-1}; + std::vector muscle_actuator_; + detail::MyoConditionState muscle_condition_state_; + int gait_steps_{0}; + std::normal_distribution init_noise_dist_{0.0, 0.02}; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_act_; + std::vector test_reset_qacc_warmstart_; + std::vector test_hfield_data_; + std::vector test_terrain_geom_rgba_; + std::vector test_terrain_geom_pos_; + int test_terrain_geom_contype_{-1}; + int test_terrain_geom_conaffinity_{-1}; + detail::NumpyPcg64 terrain_rng_; + std::uniform_real_distribution unit_dist_{0.0, 1.0}; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoSuiteWalkLikeEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + myosuite::MyoSuiteModelPath(spec.config["base_path"_], + spec.config["model_path"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + min_height_(spec.config["min_height"_]), + max_rot_(spec.config["max_rot"_]), + hip_period_(spec.config["hip_period"_]), + reset_type_(spec.config["reset_type"_]), + target_x_vel_(spec.config["target_x_vel"_]), + target_y_vel_(spec.config["target_y_vel"_]), + target_rot_(detail::ToMjtVector(spec.config["target_rot"_])), + terrain_(spec.config["terrain"_]), + terrain_variant_(spec.config["terrain_variant"_]), + use_knee_condition_(spec.config["use_knee_condition"_]), + reward_vel_w_(spec.config["reward_vel_w"_]), + reward_done_w_(spec.config["reward_done_w"_]), + reward_cyclic_hip_w_(spec.config["reward_cyclic_hip_w"_]), + reward_ref_rot_w_(spec.config["reward_ref_rot_w"_]), + reward_joint_angle_w_(spec.config["reward_joint_angle_w"_]), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])), + test_hfield_data_( + detail::ToMjtVector(spec.config["test_hfield_data"_])), + test_terrain_geom_rgba_( + detail::ToMjtVector(spec.config["test_terrain_geom_rgba"_])), + test_terrain_geom_pos_( + detail::ToMjtVector(spec.config["test_terrain_geom_pos"_])), + test_terrain_geom_contype_(spec.config["test_terrain_geom_contype"_]), + test_terrain_geom_conaffinity_( + spec.config["test_terrain_geom_conaffinity"_]), + terrain_rng_(static_cast(this->seed_)) { + ValidateConfig(); + CacheIds(); + detail::BuildMuscleMask(model_, &muscle_actuator_); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + detail::AdjustInitialQposForNormalizedActions(model_, data_, + normalize_act_); + if (target_rot_.empty()) { + target_rot_.assign(model_->key_qpos + 3, model_->key_qpos + 7); + } + InitializeRobotEnv(); + } + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + gait_steps_ = 0; + detail::ResetMyoConditionState(&muscle_condition_state_); + ResetToInitialState(); + RestoreTerrainState(); + ApplyTerrainReset(); + ApplyResetState(); + CaptureResetState(); + RewardInfo reward = ComputeRewardInfo(); + WriteState(reward, true, 0.0); + } + + void Step(const Action& action) override { + const auto* raw = static_cast(action["action"_].Data()); + detail::ApplyMyoSuiteAction(model_, data_, muscle_actuator_, normalize_act_, + raw); + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + detail::RefreshObservedMyoSuiteState(model_, data_); + ++elapsed_step_; + ++gait_steps_; + RewardInfo reward = ComputeRewardInfo(); + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, false, reward.dense_reward); + } + + private: + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_] || + model_->nv != spec_.config["qvel_dim"_] || + model_->nu != spec_.config["action_dim"_] || + model_->na != spec_.config["act_dim"_]) { + throw std::runtime_error("Walk config dims do not match model."); + } + int expected_obs = (model_->nq - 2) + model_->nv + 2 + 4 + 2 + 1 + 6 + 1 + + 3 * model_->nu + model_->na; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error("Walk config obs_dim does not match model."); + } + } + + void CacheIds() { + terrain_geom_id_ = mj_name2id(model_, mjOBJ_GEOM, "terrain"); + torso_body_id_ = mj_name2id(model_, mjOBJ_BODY, "torso"); + pelvis_body_id_ = mj_name2id(model_, mjOBJ_BODY, "pelvis"); + foot_l_body_id_ = mj_name2id(model_, mjOBJ_BODY, "talus_l"); + foot_r_body_id_ = mj_name2id(model_, mjOBJ_BODY, "talus_r"); + hip_flex_qposadr_[0] = + model_->jnt_qposadr[mj_name2id(model_, mjOBJ_JOINT, "hip_flexion_l")]; + hip_flex_qposadr_[1] = + model_->jnt_qposadr[mj_name2id(model_, mjOBJ_JOINT, "hip_flexion_r")]; + reward_joint_qposadr_[0] = + model_->jnt_qposadr[mj_name2id(model_, mjOBJ_JOINT, "hip_adduction_l")]; + reward_joint_qposadr_[1] = + model_->jnt_qposadr[mj_name2id(model_, mjOBJ_JOINT, "hip_adduction_r")]; + reward_joint_qposadr_[2] = + model_->jnt_qposadr[mj_name2id(model_, mjOBJ_JOINT, "hip_rotation_l")]; + reward_joint_qposadr_[3] = + model_->jnt_qposadr[mj_name2id(model_, mjOBJ_JOINT, "hip_rotation_r")]; + if (terrain_geom_id_ == -1 || torso_body_id_ == -1 || + pelvis_body_id_ == -1 || foot_l_body_id_ == -1 || + foot_r_body_id_ == -1) { + throw std::runtime_error("Walk ids missing."); + } + terrain_hfield_id_ = model_->geom_dataid[terrain_geom_id_]; + if (terrain_hfield_id_ >= 0) { + detail::CopyModelHfieldData(model_, terrain_hfield_id_, + &initial_hfield_data_); + } + detail::CopyModelGeomRgba(model_, terrain_geom_id_, &initial_terrain_rgba_); + detail::CopyModelGeomPos(model_, terrain_geom_id_, &initial_terrain_pos_); + detail::CopyModelGeomContype(model_, terrain_geom_id_, + &initial_terrain_contype_); + detail::CopyModelGeomConaffinity(model_, terrain_geom_id_, + &initial_terrain_conaffinity_); + } + + void RestoreTerrainState() { + if (terrain_hfield_id_ >= 0) { + detail::RestoreModelHfieldData(model_, terrain_hfield_id_, + initial_hfield_data_); + } + detail::RestoreModelGeomRgba(model_, terrain_geom_id_, + initial_terrain_rgba_); + detail::RestoreModelGeomPos(model_, terrain_geom_id_, initial_terrain_pos_); + detail::RestoreModelGeomContype(model_, terrain_geom_id_, + initial_terrain_contype_); + detail::RestoreModelGeomConaffinity(model_, terrain_geom_id_, + initial_terrain_conaffinity_); + } + + void ApplyTerrainVisibility(bool visible) { + std::vector rgba = initial_terrain_rgba_; + std::vector pos = initial_terrain_pos_; + rgba[3] = visible ? static_cast(1.0) : static_cast(0.0); + pos[0] = 0.0; + pos[1] = 0.0; + pos[2] = visible ? static_cast(0.0) : static_cast(-10.0); + detail::RestoreModelGeomRgba(model_, terrain_geom_id_, rgba); + detail::RestoreModelGeomPos(model_, terrain_geom_id_, pos); + if (visible) { + detail::RestoreModelGeomContype(model_, terrain_geom_id_, 1); + detail::RestoreModelGeomConaffinity(model_, terrain_geom_id_, 1); + } else { + detail::RestoreModelGeomContype(model_, terrain_geom_id_, + initial_terrain_contype_); + detail::RestoreModelGeomConaffinity(model_, terrain_geom_id_, + initial_terrain_conaffinity_); + } + } + + std::vector RandomizeRoughTerrain() { + int rows = model_->hfield_nrow[terrain_hfield_id_]; + int cols = model_->hfield_ncol[terrain_hfield_id_]; + std::vector rough(rows * cols); + for (mjtNum& value : rough) { + value = terrain_rng_.UniformMjt(static_cast(-0.5), + static_cast(0.5)); + } + std::vector normalized; + detail::NormalizeRange(rough, &normalized); + for (mjtNum& value : normalized) { + value = value * static_cast(0.08) - static_cast(0.02); + } + return normalized; + } + + std::vector RandomizeHillyTerrain() { + int rows = model_->hfield_nrow[terrain_hfield_id_]; + int cols = model_->hfield_ncol[terrain_hfield_id_]; + int total = rows * cols; + int flat_length = 3000; + int frequency = 3; + mjtNum scalar = terrain_variant_ == "fixed" + ? static_cast(0.63) + : terrain_rng_.UniformMjt(static_cast(0.53), + static_cast(0.73)); + std::vector combined(total, static_cast(-2.0)); + for (int i = flat_length; i < total; ++i) { + double phase = static_cast(i - flat_length) / + std::max(total - flat_length - 1, 1) * frequency * + detail::kPi + + detail::kPi / 2.0; + combined[i] = static_cast(-2.0 + 0.5 * (std::sin(phase) - 1.0)); + } + std::vector normalized; + detail::NormalizeRange(combined, &normalized); + for (mjtNum& value : normalized) { + value *= scalar; + } + return detail::FlipGrid(normalized, rows, cols); + } + + std::vector RandomizeStairsTerrain() { + int rows = model_->hfield_nrow[terrain_hfield_id_]; + int cols = model_->hfield_ncol[terrain_hfield_id_]; + int total = rows * cols; + int num_stairs = 12; + const auto stair_height = static_cast(0.1); + int flat = 5200 - (total - 5200) % num_stairs; + int stairs_width = (total - flat) / num_stairs; + mjtNum scalar = terrain_variant_ == "fixed" + ? static_cast(2.5) + : terrain_rng_.UniformMjt(static_cast(1.5), + static_cast(3.5)); + std::vector data(total, static_cast(-2.0)); + for (int stair = 0; stair < num_stairs; ++stair) { + int start = flat + stair * stairs_width; + int end = std::min(start + stairs_width, total); + for (int idx = start; idx < end; ++idx) { + data[idx] = static_cast(-2.0) + stair_height * stair; + } + } + for (mjtNum& value : data) { + value = (value + static_cast(2.0)) / + (static_cast(2.0) + stair_height * num_stairs) * scalar; + } + return detail::FlipGrid(data, rows, cols); + } + + void ApplyTerrainReset() { + if (terrain_.empty()) { + if constexpr (kFromPixels) { + std::vector rgba = initial_terrain_rgba_; + std::vector pos = initial_terrain_pos_; + rgba[3] = static_cast(0.0); + pos[0] = 0.0; + pos[1] = 0.0; + pos[2] = 0.0; + detail::RestoreModelGeomRgba(model_, terrain_geom_id_, rgba); + detail::RestoreModelGeomPos(model_, terrain_geom_id_, pos); + detail::RestoreModelGeomContype(model_, terrain_geom_id_, 0); + detail::RestoreModelGeomConaffinity(model_, terrain_geom_id_, 0); + } else { + ApplyTerrainVisibility(false); + } + if (!test_terrain_geom_rgba_.empty()) { + detail::RestoreModelGeomRgba(model_, terrain_geom_id_, + test_terrain_geom_rgba_); + } + if (!test_terrain_geom_pos_.empty()) { + detail::RestoreModelGeomPos(model_, terrain_geom_id_, + test_terrain_geom_pos_); + } + if (test_terrain_geom_contype_ >= 0) { + detail::RestoreModelGeomContype(model_, terrain_geom_id_, + test_terrain_geom_contype_); + } + if (test_terrain_geom_conaffinity_ >= 0) { + detail::RestoreModelGeomConaffinity(model_, terrain_geom_id_, + test_terrain_geom_conaffinity_); + } + return; + } + ApplyTerrainVisibility(true); + if (!test_terrain_geom_rgba_.empty()) { + detail::RestoreModelGeomRgba(model_, terrain_geom_id_, + test_terrain_geom_rgba_); + } + if (!test_terrain_geom_pos_.empty()) { + detail::RestoreModelGeomPos(model_, terrain_geom_id_, + test_terrain_geom_pos_); + } + if (test_terrain_geom_contype_ >= 0) { + detail::RestoreModelGeomContype(model_, terrain_geom_id_, + test_terrain_geom_contype_); + } + if (test_terrain_geom_conaffinity_ >= 0) { + detail::RestoreModelGeomConaffinity(model_, terrain_geom_id_, + test_terrain_geom_conaffinity_); + } + if (!test_hfield_data_.empty()) { + detail::RestoreModelHfieldData(model_, terrain_hfield_id_, + test_hfield_data_); + return; + } + std::vector hfield; + if (terrain_ == "rough") { + hfield = RandomizeRoughTerrain(); + } else if (terrain_ == "hilly") { + hfield = RandomizeHillyTerrain(); + } else if (terrain_ == "stairs") { + hfield = RandomizeStairsTerrain(); + } + detail::RestoreModelHfieldData(model_, terrain_hfield_id_, hfield); + } + + std::pair, std::vector> GetResetState() { + int key_id = 0; + if (reset_type_ == "random") { + key_id = unit_dist_(gen_) < 0.5 ? 2 : 3; + } else if (reset_type_ == "init") { + key_id = 2; + } + std::vector qpos(model_->key_qpos + key_id * model_->nq, + model_->key_qpos + (key_id + 1) * model_->nq); + std::vector qvel(model_->key_qvel + key_id * model_->nv, + model_->key_qvel + (key_id + 1) * model_->nv); + if (reset_type_ == "random") { + std::vector root_quat(qpos.begin() + 3, qpos.begin() + 7); + mjtNum height = qpos[2]; + for (mjtNum& value : qpos) { + value += static_cast(init_noise_dist_(gen_)); + } + std::copy(root_quat.begin(), root_quat.end(), qpos.begin() + 3); + qpos[2] = height; + } + return {qpos, qvel}; + } + + void ApplyResetState() { + if (!test_reset_qpos_.empty()) { + detail::RestoreVector(test_reset_qpos_, data_->qpos); + detail::RestoreVector(test_reset_qvel_, data_->qvel); + } else { + auto [qpos, qvel] = GetResetState(); + detail::RestoreVector(qpos, data_->qpos); + detail::RestoreVector(qvel, data_->qvel); + } + mj_forward(model_, data_); + bool rerun_forward = false; + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + rerun_forward = true; + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, data_->qacc_warmstart); + } + if (rerun_forward) { + mj_forward(model_, data_); + } + } + + std::array GetCom() const { + mjtNum total_mass = 0.0; + std::array com = {0.0, 0.0, 0.0}; + for (int body = 0; body < model_->nbody; ++body) { + mjtNum mass = model_->body_mass[body]; + total_mass += mass; + for (int axis = 0; axis < 3; ++axis) { + com[axis] += mass * data_->xipos[body * 3 + axis]; + } + } + for (mjtNum& value : com) { + value /= total_mass; + } + return com; + } + + std::array GetComVelocity() const { + mjtNum total_mass = 0.0; + std::array velocity = {0.0, 0.0}; + for (int body = 0; body < model_->nbody; ++body) { + mjtNum mass = model_->body_mass[body]; + total_mass += mass; + velocity[0] += mass * (-data_->cvel[body * 6 + 3]); + velocity[1] += mass * (-data_->cvel[body * 6 + 4]); + } + velocity[0] /= total_mass; + velocity[1] /= total_mass; + return velocity; + } + + std::array GetFeetHeights() const { + return {data_->xpos[foot_l_body_id_ * 3 + 2], + data_->xpos[foot_r_body_id_ * 3 + 2]}; + } + + std::array GetFeetRelativePositions() const { + std::array out{}; + for (int axis = 0; axis < 3; ++axis) { + out[axis] = data_->xpos[foot_l_body_id_ * 3 + axis] - + data_->xpos[pelvis_body_id_ * 3 + axis]; + out[3 + axis] = data_->xpos[foot_r_body_id_ * 3 + axis] - + data_->xpos[pelvis_body_id_ * 3 + axis]; + } + return out; + } + + mjtNum GetJointAngleReward() const { + mjtNum mean_abs = 0.0; + for (int adr : reward_joint_qposadr_) { + mean_abs += std::abs(data_->qpos[adr]); + } + mean_abs /= reward_joint_qposadr_.size(); + return std::exp(static_cast(-5.0) * mean_abs); + } + + mjtNum GetCyclicReward() const { + const auto phase = static_cast( + std::fmod(static_cast(gait_steps_) / hip_period_, 1.0)); + const auto desired_l = static_cast( + 0.8 * + std::cos(phase * static_cast(2.0) * detail::kPi + detail::kPi)); + const auto desired_r = static_cast( + 0.8 * std::cos(phase * static_cast(2.0) * detail::kPi)); + mjtNum diff_l = desired_l - data_->qpos[hip_flex_qposadr_[0]]; + mjtNum diff_r = desired_r - data_->qpos[hip_flex_qposadr_[1]]; + return std::sqrt(diff_l * diff_l + diff_r * diff_r); + } + + mjtNum GetRefRotReward() const { + mjtNum diff_norm = 0.0; + for (int i = 0; i < 4; ++i) { + mjtNum diff = + static_cast(5.0) * (data_->qpos[3 + i] - target_rot_[i]); + diff_norm += diff * diff; + } + return std::exp(-std::sqrt(diff_norm)); + } + + bool GetRotCondition() const { + std::array mat{}; + mju_quat2Mat(mat.data(), data_->qpos + 3); + return std::abs(mat[0]) > max_rot_; + } + + bool GetKneeCondition() const { + auto feet = GetFeetHeights(); + mjtNum mean_feet = (feet[0] + feet[1]) * static_cast(0.5); + return (GetCom()[2] - mean_feet) < static_cast(0.61); + } + + bool GetDoneCondition() const { + if (GetCom()[2] < min_height_) { + return true; + } + if (GetRotCondition()) { + return true; + } + return use_knee_condition_ && GetKneeCondition(); + } + + RewardInfo ComputeRewardInfo() const { + RewardInfo reward; + auto com_velocity = GetComVelocity(); + reward.vel_reward = + std::exp(-std::pow(target_y_vel_ - com_velocity[1], 2.0)) + + std::exp(-std::pow(target_x_vel_ - com_velocity[0], 2.0)); + reward.cyclic_hip = GetCyclicReward(); + reward.ref_rot = GetRefRotReward(); + reward.joint_angle_rew = GetJointAngleReward(); + reward.success = reward.vel_reward >= static_cast(1.0); + reward.done = GetDoneCondition(); + reward.dense_reward = reward_vel_w_ * reward.vel_reward + + reward_done_w_ * static_cast(reward.done) + + reward_cyclic_hip_w_ * reward.cyclic_hip + + reward_ref_rot_w_ * reward.ref_rot + + reward_joint_angle_w_ * reward.joint_angle_rew; + return reward; + } + + void WriteState(const RewardInfo& reward, bool reset, mjtNum reward_value) { + auto state = Allocate(); + state["reward"_] = reward_value; + if constexpr (kFromPixels) { + auto obs_pixels = state["obs:pixels"_]; + AssignPixelObservation("obs:pixels", &obs_pixels, reset); + } else { + auto obs = state["obs"_]; + mjtNum* buffer = PrepareObservation("obs", &obs); + for (int i = 2; i < model_->nq; ++i) { + *(buffer++) = data_->qpos[i]; + } + for (int i = 0; i < model_->nv; ++i) { + *(buffer++) = data_->qvel[i] * Dt(); + } + auto com_velocity = GetComVelocity(); + *(buffer++) = com_velocity[0]; + *(buffer++) = com_velocity[1]; + for (int i = 0; i < 4; ++i) { + *(buffer++) = data_->xquat[torso_body_id_ * 4 + i]; + } + auto feet = GetFeetHeights(); + *(buffer++) = feet[0]; + *(buffer++) = feet[1]; + *(buffer++) = GetCom()[2]; + auto feet_rel = GetFeetRelativePositions(); + for (mjtNum value : feet_rel) { + *(buffer++) = value; + } + *(buffer++) = static_cast( + std::fmod(static_cast(gait_steps_) / hip_period_, 1.0)); + for (int i = 0; i < model_->nu; ++i) { + *(buffer++) = data_->actuator_length[i]; + } + for (int i = 0; i < model_->nu; ++i) { + *(buffer++) = + detail::ClipValue(data_->actuator_velocity[i], -100.0, 100.0); + } + for (int i = 0; i < model_->nu; ++i) { + *(buffer++) = + detail::ClipValue(data_->actuator_force[i] / 1000.0, -100.0, 100.0); + } + for (int i = 0; i < model_->na; ++i) { + *(buffer++) = data_->act[i]; + } + CommitObservation("obs", &obs, reset); + } + state["info:vel_reward"_] = reward.vel_reward; + state["info:cyclic_hip"_] = reward.cyclic_hip; + state["info:ref_rot"_] = reward.ref_rot; + state["info:joint_angle_rew"_] = reward.joint_angle_rew; + state["info:success"_] = reward.success; +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + state["info:phase_var"_] = static_cast( + std::fmod(static_cast(gait_steps_) / hip_period_, 1.0)); + } +}; + +template +using ReorientEnvBase = MyoSuiteReorientEnvBase; + +template +using ReorientPixelEnvBase = MyoSuiteReorientEnvBase; + +template +using WalkEnvBase = MyoSuiteWalkLikeEnvBase; + +template +using WalkPixelEnvBase = MyoSuiteWalkLikeEnvBase; + +using ReorientEnv = ReorientEnvBase; +using ReorientPixelEnv = ReorientPixelEnvBase; +using MyoSuiteReorientEnv = ReorientEnv; +using MyoSuiteReorientPixelEnv = ReorientPixelEnv; +using MyoSuiteReorientEnvPool = AsyncEnvPool; +using MyoSuiteReorientPixelEnvPool = AsyncEnvPool; + +using MyoSuiteWalkEnv = WalkEnvBase; +using MyoSuiteWalkPixelEnv = WalkPixelEnvBase; +using MyoSuiteWalkEnvPool = AsyncEnvPool; +using MyoSuiteWalkPixelEnvPool = AsyncEnvPool; + +using MyoSuiteTerrainEnv = WalkEnvBase; +using MyoSuiteTerrainPixelEnv = WalkPixelEnvBase; +using MyoSuiteTerrainEnvPool = AsyncEnvPool; +using MyoSuiteTerrainPixelEnvPool = AsyncEnvPool; + +} // namespace myosuite_envpool + +#endif // ENVPOOL_MUJOCO_MYOSUITE_MYOBASE_EXTENDED_H_ diff --git a/envpool/mujoco/myosuite/myobase_test.py b/envpool/mujoco/myosuite/myobase_test.py new file mode 100644 index 000000000..5199352e5 --- /dev/null +++ b/envpool/mujoco/myosuite/myobase_test.py @@ -0,0 +1,1918 @@ +# Copyright 2026 Garena Online Private 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. +"""Internal MyoSuite MyoBase native env tests.""" + +from __future__ import annotations + +from functools import cache +from pathlib import Path +from typing import Any, cast + +import gymnasium +import mujoco +import numpy as np +from absl.testing import absltest + +from envpool.mujoco.myosuite.config import resolve_myosuite_model_path +from envpool.mujoco.myosuite.metadata import MYOSUITE_DIRECT_ENTRIES +from envpool.mujoco.myosuite.native import ( + MyoSuiteKeyTurnEnvSpec, + MyoSuiteKeyTurnGymnasiumEnvPool, + MyoSuiteKeyTurnPixelEnvSpec, + MyoSuiteKeyTurnPixelGymnasiumEnvPool, + MyoSuiteObjHoldEnvSpec, + MyoSuiteObjHoldGymnasiumEnvPool, + MyoSuiteObjHoldPixelEnvSpec, + MyoSuiteObjHoldPixelGymnasiumEnvPool, + MyoSuitePenTwirlEnvSpec, + MyoSuitePenTwirlGymnasiumEnvPool, + MyoSuitePenTwirlPixelEnvSpec, + MyoSuitePenTwirlPixelGymnasiumEnvPool, + MyoSuitePoseEnvSpec, + MyoSuitePoseGymnasiumEnvPool, + MyoSuitePosePixelEnvSpec, + MyoSuitePosePixelGymnasiumEnvPool, + MyoSuiteReachEnvSpec, + MyoSuiteReachGymnasiumEnvPool, + MyoSuiteReachPixelEnvSpec, + MyoSuiteReachPixelGymnasiumEnvPool, + MyoSuiteReorientEnvSpec, + MyoSuiteReorientGymnasiumEnvPool, + MyoSuiteReorientPixelEnvSpec, + MyoSuiteReorientPixelGymnasiumEnvPool, + MyoSuiteTerrainEnvSpec, + MyoSuiteTerrainGymnasiumEnvPool, + MyoSuiteTerrainPixelEnvSpec, + MyoSuiteTerrainPixelGymnasiumEnvPool, + MyoSuiteTorsoEnvSpec, + MyoSuiteTorsoGymnasiumEnvPool, + MyoSuiteTorsoPixelEnvSpec, + MyoSuiteTorsoPixelGymnasiumEnvPool, + MyoSuiteWalkEnvSpec, + MyoSuiteWalkGymnasiumEnvPool, + MyoSuiteWalkPixelEnvSpec, + MyoSuiteWalkPixelGymnasiumEnvPool, +) +from envpool.mujoco.myosuite.oracle_utils import load_oracle_class +from envpool.mujoco.myosuite.paths import ( + myosuite_asset_root, +) +from envpool.mujoco.myosuite.registration import MYOSUITE_PUBLIC_TASK_IDS +from envpool.python.glfw_context import preload_windows_gl_dlls +from envpool.registration import list_all_envs, make_gymnasium + +preload_windows_gl_dlls(strict=True) + +_POSE_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["class_name"] == "PoseEnvV0" +) +_REACH_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["class_name"] == "ReachEnvV0" +) +_REORIENT_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["class_name"] + in { + "Geometries100EnvV0", + "Geometries8EnvV0", + "InDistribution", + "OutofDistribution", + } +) +_KEYTURN_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["class_name"] == "KeyTurnEnvV0" +) +_OBJHOLD_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["class_name"] in {"ObjHoldFixedEnvV0", "ObjHoldRandomEnvV0"} +) +_TORSO_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["class_name"] == "TorsoEnvV0" +) +_PENTWIRL_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["class_name"] in {"PenTwirlFixedEnvV0", "PenTwirlRandomEnvV0"} +) +_WALK_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["class_name"] == "WalkEnvV0" +) +_TERRAIN_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["class_name"] == "TerrainEnvV0" +) +_POSE_ALIGN_IDS = ( + "motorFingerPoseRandom-v0", + "myoHandPoseRandom-v0", + "myoElbowPose1D6MRandom-v0", + "myoElbowPose1D6MExoRandom-v0", +) +_REACH_ALIGN_IDS = ( + "motorFingerReachRandom-v0", + "myoHandReachRandom-v0", + "myoArmReachRandom-v0", +) +_REORIENT_ALIGN_IDS = ( + "myoHandReorient100-v0", + "myoHandReorient8-v0", + "myoHandReorientID-v0", + "myoHandReorientOOD-v0", +) +_KEYTURN_ALIGN_IDS = ( + "myoHandKeyTurnFixed-v0", + "myoHandKeyTurnRandom-v0", +) +_OBJHOLD_ALIGN_IDS = ( + "myoHandObjHoldFixed-v0", + "myoHandObjHoldRandom-v0", +) +_TORSO_ALIGN_IDS = ( + "myoTorsoPoseFixed-v0", + "myoTorsoExoPoseFixed-v0", +) +_PENTWIRL_ALIGN_IDS = ( + "myoHandPenTwirlFixed-v0", + "myoHandPenTwirlRandom-v0", +) +_WALK_ALIGN_IDS = ("myoLegWalk-v0",) +_TERRAIN_ALIGN_IDS = ( + "myoLegRoughTerrainWalk-v0", + "myoLegHillyTerrainWalk-v0", + "myoLegStairTerrainWalk-v0", +) +_PUBLIC_REPRESENTATIVE_TASK_IDS = ( + "myoHandReorientID-v0", + "myoFatiHandReorientID-v0", + "myoLegWalk-v0", + "myoLegRoughTerrainWalk-v0", + "myoChallengeBimanual-v0", + "myoSarcChallengeBimanual-v0", + "MyoHandAirplaneFly-v0", + "myoReafHandPoseRandom-v0", + "myoSarcArmReachRandom-v0", +) +_PUBLIC_VARIANT_DIFF_CASES = ( + ("myoHandPoseRandom-v0", "myoReafHandPoseRandom-v0"), + ("myoArmReachRandom-v0", "myoSarcArmReachRandom-v0"), + ("myoHandReorientID-v0", "myoFatiHandReorientID-v0"), +) +_ALIGNMENT_STEPS = 32 + + +def _entry(env_id: str) -> dict[str, Any]: + return next( + entry for entry in MYOSUITE_DIRECT_ENTRIES if entry["id"] == env_id + ) + + +def _asset_model_path(model_path: str) -> Path: + path = Path(model_path) + return path if path.is_absolute() else myosuite_asset_root() / model_path + + +def _runtime_model_path(entry: dict[str, Any]) -> str: + kwargs = entry["kwargs"] + return resolve_myosuite_model_path( + str(kwargs["model_path"]), + (str(kwargs["edit_fn"]) if kwargs.get("edit_fn") is not None else None), + ) + + +def _alignment_model_path(entry: dict[str, Any]) -> str: + return str(_asset_model_path(_runtime_model_path(entry))) + + +@cache +def _model(path: str) -> mujoco.MjModel: + return mujoco.MjModel.from_xml_path(str(_asset_model_path(path))) + + +def _flatten_site_ranges( + target_reach_range: dict[str, list[list[float]]], +) -> tuple[list[str], list[float], list[float]]: + site_names: list[str] = [] + mins: list[float] = [] + maxs: list[float] = [] + for site_name, span in target_reach_range.items(): + site_names.append(site_name) + mins.extend(span[0]) + maxs.extend(span[1]) + return site_names, mins, maxs + + +def _pose_config( + env_id: str, + *, + model_path: str | None = None, + overrides: dict[str, Any] | None = None, +) -> tuple[tuple[Any, ...], type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + if model_path is not None: + kwargs["model_path"] = model_path + model = _model(kwargs["model_path"]) + if "target_jnt_range" in kwargs: + target_qpos_min = [ + bounds[0] for bounds in kwargs["target_jnt_range"].values() + ] + target_qpos_max = [ + bounds[1] for bounds in kwargs["target_jnt_range"].values() + ] + target_qpos_value = [ + (lo + hi) / 2.0 + for lo, hi in zip(target_qpos_min, target_qpos_max, strict=True) + ] + else: + target_qpos_min = [] + target_qpos_max = [] + target_qpos_value = list(kwargs["target_jnt_value"]) + config = MyoSuitePoseEnvSpec.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + frame_skip=int(kwargs.get("frame_skip", 10)), + model_path=str(kwargs["model_path"]), + normalize_act=bool(kwargs.get("normalize_act", True)), + obs_dim=model.nq + model.nv + model.nq + model.na, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.nu, + pose_thd=float(kwargs.get("pose_thd", 0.35)), + reward_pose_w=float( + kwargs.get("weighted_reward_keys", {}).get("pose", 1.0) + ), + reward_bonus_w=float( + kwargs.get("weighted_reward_keys", {}).get("bonus", 4.0) + ), + reward_act_reg_w=float( + kwargs.get("weighted_reward_keys", {}).get("act_reg", 1.0) + ), + reward_penalty_w=float( + kwargs.get("weighted_reward_keys", {}).get("penalty", 50.0) + ), + reset_type=str(kwargs.get("reset_type", "init")), + target_type=str( + kwargs.get( + "target_type", "fixed" if target_qpos_value else "generate" + ) + ), + target_qpos_min=target_qpos_min, + target_qpos_max=target_qpos_max, + target_qpos_value=target_qpos_value, + viz_site_targets=list(kwargs.get("viz_site_targets", [])), + weight_bodyname=str(kwargs.get("weight_bodyname", "")), + weight_range=list(kwargs.get("weight_range", [])), + ) + if overrides: + config = MyoSuitePoseEnvSpec.gen_config( + **dict( + zip(MyoSuitePoseEnvSpec._config_keys, config, strict=False), + **overrides, + ) + ) + return config, MyoSuitePoseGymnasiumEnvPool + + +def _reach_config( + env_id: str, + *, + model_path: str | None = None, + overrides: dict[str, Any] | None = None, +) -> tuple[tuple[Any, ...], type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + if model_path is not None: + kwargs["model_path"] = model_path + model = _model(kwargs["model_path"]) + site_names, mins, maxs = _flatten_site_ranges(kwargs["target_reach_range"]) + is_walk_reach = entry["entry_module"] == "myosuite.envs.myo.myobase.walk_v0" + config = MyoSuiteReachEnvSpec.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + frame_skip=int(kwargs.get("frame_skip", 10)), + model_path=str(kwargs["model_path"]), + normalize_act=bool(kwargs.get("normalize_act", True)), + obs_dim=model.nq + model.nv + 6 * len(site_names) + model.na, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.nu, + target_site_count=len(site_names), + far_th=float(kwargs.get("far_th", 0.35)), + reward_reach_w=float( + kwargs.get("weighted_reward_keys", {}).get("reach", 1.0) + ), + reward_bonus_w=float( + kwargs.get("weighted_reward_keys", {}).get("bonus", 4.0) + ), + reward_act_reg_w=float( + kwargs.get("weighted_reward_keys", {}).get("act_reg", 0.0) + ), + reward_penalty_w=float( + kwargs.get("weighted_reward_keys", {}).get("penalty", 50.0) + ), + target_site_names=site_names, + target_pos_min=mins, + target_pos_max=maxs, + joint_random_range=list(kwargs.get("joint_random_range", [])), + target_pos_relative_to_tip=is_walk_reach, + hide_skin_geom_group_1=is_walk_reach, + hide_terrain=is_walk_reach, + ) + if overrides: + config = MyoSuiteReachEnvSpec.gen_config( + **dict( + zip(MyoSuiteReachEnvSpec._config_keys, config, strict=False), + **overrides, + ) + ) + return config, MyoSuiteReachGymnasiumEnvPool + + +def _reorient_config( + env_id: str, + *, + model_path: str | None = None, + overrides: dict[str, Any] | None = None, +) -> tuple[tuple[Any, ...], type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + if model_path is not None: + kwargs["model_path"] = model_path + model = _model(kwargs["model_path"]) + mode_map = { + "Geometries100EnvV0": "100", + "Geometries8EnvV0": "8", + "InDistribution": "id", + "OutofDistribution": "ood", + } + config = MyoSuiteReorientEnvSpec.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + frame_skip=int(kwargs.get("frame_skip", 5)), + model_path=str(kwargs["model_path"]), + normalize_act=bool(kwargs.get("normalize_act", True)), + obs_dim=(model.nq - 6) + 21 + 3 * model.nu + model.na, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.nu, + randomization_mode=mode_map[entry["class_name"]], + reward_pos_align_w=float( + kwargs.get("weighted_reward_keys", {}).get("pos_align", 1.0) + ), + reward_rot_align_w=float( + kwargs.get("weighted_reward_keys", {}).get("rot_align", 1.0) + ), + reward_act_reg_w=float( + kwargs.get("weighted_reward_keys", {}).get("act_reg", 5.0) + ), + reward_drop_w=float( + kwargs.get("weighted_reward_keys", {}).get("drop", 5.0) + ), + reward_bonus_w=float( + kwargs.get("weighted_reward_keys", {}).get("bonus", 10.0) + ), + ) + if overrides: + config = MyoSuiteReorientEnvSpec.gen_config( + **dict( + zip(MyoSuiteReorientEnvSpec._config_keys, config, strict=False), + **overrides, + ) + ) + return config, MyoSuiteReorientGymnasiumEnvPool + + +def _key_turn_config( + env_id: str, + *, + model_path: str | None = None, + overrides: dict[str, Any] | None = None, +) -> tuple[tuple[Any, ...], type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + if model_path is not None: + kwargs["model_path"] = model_path + model = _model(kwargs["model_path"]) + config = MyoSuiteKeyTurnEnvSpec.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + frame_skip=int(kwargs.get("frame_skip", 10)), + model_path=str(kwargs["model_path"]), + normalize_act=bool(kwargs.get("normalize_act", True)), + obs_dim=model.nq + model.nv + 6 + model.na, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.nu, + goal_th=float(kwargs.get("goal_th", np.pi)), + reward_key_turn_w=float( + kwargs.get("weighted_reward_keys", {}).get("key_turn", 1.0) + ), + reward_iftip_approach_w=float( + kwargs.get("weighted_reward_keys", {}).get("IFtip_approach", 10.0) + ), + reward_thtip_approach_w=float( + kwargs.get("weighted_reward_keys", {}).get("THtip_approach", 10.0) + ), + reward_act_reg_w=float( + kwargs.get("weighted_reward_keys", {}).get("act_reg", 1.0) + ), + reward_bonus_w=float( + kwargs.get("weighted_reward_keys", {}).get("bonus", 4.0) + ), + reward_penalty_w=float( + kwargs.get("weighted_reward_keys", {}).get("penalty", 25.0) + ), + key_init_range=list(kwargs.get("key_init_range", (0.0, 0.0))), + ) + if overrides: + config = MyoSuiteKeyTurnEnvSpec.gen_config( + **dict( + zip(MyoSuiteKeyTurnEnvSpec._config_keys, config, strict=False), + **overrides, + ) + ) + return config, MyoSuiteKeyTurnGymnasiumEnvPool + + +def _obj_hold_config( + env_id: str, + *, + model_path: str | None = None, + overrides: dict[str, Any] | None = None, +) -> tuple[tuple[Any, ...], type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + if model_path is not None: + kwargs["model_path"] = model_path + model = _model(kwargs["model_path"]) + config = MyoSuiteObjHoldEnvSpec.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + frame_skip=int(kwargs.get("frame_skip", 10)), + model_path=str(kwargs["model_path"]), + normalize_act=bool(kwargs.get("normalize_act", True)), + obs_dim=(model.nq - 7) + (model.nv - 6) + 6 + model.na, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.nu, + randomize_on_reset=entry["class_name"] == "ObjHoldRandomEnvV0", + reward_goal_dist_w=float( + kwargs.get("weighted_reward_keys", {}).get("goal_dist", 100.0) + ), + reward_bonus_w=float( + kwargs.get("weighted_reward_keys", {}).get("bonus", 4.0) + ), + reward_penalty_w=float( + kwargs.get("weighted_reward_keys", {}).get("penalty", 10.0) + ), + ) + if overrides: + config = MyoSuiteObjHoldEnvSpec.gen_config( + **dict( + zip(MyoSuiteObjHoldEnvSpec._config_keys, config, strict=False), + **overrides, + ) + ) + return config, MyoSuiteObjHoldGymnasiumEnvPool + + +def _torso_config( + env_id: str, + *, + model_path: str | None = None, + overrides: dict[str, Any] | None = None, +) -> tuple[tuple[Any, ...], type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + if model_path is not None: + kwargs["model_path"] = model_path + model = _model(kwargs["model_path"]) + target_qpos_value = [ + (bounds[0] + bounds[1]) / 2.0 + for bounds in kwargs["target_jnt_range"].values() + ] + pose_dim = len(target_qpos_value) + config = MyoSuiteTorsoEnvSpec.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + frame_skip=int(kwargs.get("frame_skip", 5)), + model_path=str(kwargs["model_path"]), + normalize_act=bool(kwargs.get("normalize_act", True)), + obs_dim=model.nq + model.nv + pose_dim + model.na, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.nu, + pose_dim=pose_dim, + pose_thd=float(kwargs.get("pose_thd", 0.25)), + reward_pose_w=float( + kwargs.get("weighted_reward_keys", {}).get("pose", 1.0) + ), + reward_bonus_w=float( + kwargs.get("weighted_reward_keys", {}).get("bonus", 4.0) + ), + reward_act_reg_w=float( + kwargs.get("weighted_reward_keys", {}).get("act_reg", 1.0) + ), + reward_penalty_w=float( + kwargs.get("weighted_reward_keys", {}).get("penalty", 50.0) + ), + target_qpos_value=target_qpos_value, + ) + if overrides: + config = MyoSuiteTorsoEnvSpec.gen_config( + **dict( + zip(MyoSuiteTorsoEnvSpec._config_keys, config, strict=False), + **dict(overrides), + ) + ) + return config, MyoSuiteTorsoGymnasiumEnvPool + + +def _pen_twirl_config( + env_id: str, + *, + model_path: str | None = None, + overrides: dict[str, Any] | None = None, +) -> tuple[tuple[Any, ...], type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + if model_path is not None: + kwargs["model_path"] = model_path + model = _model(kwargs["model_path"]) + config = MyoSuitePenTwirlEnvSpec.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + frame_skip=int(kwargs.get("frame_skip", 5)), + model_path=str(kwargs["model_path"]), + normalize_act=bool(kwargs.get("normalize_act", True)), + obs_dim=(model.nq - 6) + 21 + model.na, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.nu, + randomize_target=entry["class_name"] == "PenTwirlRandomEnvV0", + reward_pos_align_w=float( + kwargs.get("weighted_reward_keys", {}).get("pos_align", 1.0) + ), + reward_rot_align_w=float( + kwargs.get("weighted_reward_keys", {}).get("rot_align", 1.0) + ), + reward_act_reg_w=float( + kwargs.get("weighted_reward_keys", {}).get("act_reg", 5.0) + ), + reward_drop_w=float( + kwargs.get("weighted_reward_keys", {}).get("drop", 5.0) + ), + reward_bonus_w=float( + kwargs.get("weighted_reward_keys", {}).get("bonus", 10.0) + ), + ) + if overrides: + config = MyoSuitePenTwirlEnvSpec.gen_config( + **dict( + zip(MyoSuitePenTwirlEnvSpec._config_keys, config, strict=False), + **overrides, + ) + ) + return config, MyoSuitePenTwirlGymnasiumEnvPool + + +def _walk_like_config( + env_id: str, + *, + model_path: str | None = None, + overrides: dict[str, Any] | None = None, +) -> tuple[tuple[Any, ...], type, type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + if model_path is not None: + kwargs["model_path"] = model_path + model = _model(kwargs["model_path"]) + spec_type = ( + MyoSuiteTerrainEnvSpec + if entry["class_name"] == "TerrainEnvV0" + else MyoSuiteWalkEnvSpec + ) + pool_type = ( + MyoSuiteTerrainGymnasiumEnvPool + if entry["class_name"] == "TerrainEnvV0" + else MyoSuiteWalkGymnasiumEnvPool + ) + config = spec_type.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + frame_skip=int(kwargs.get("frame_skip", 10)), + model_path=str(kwargs["model_path"]), + normalize_act=bool(kwargs.get("normalize_act", True)), + obs_dim=(model.nq - 2) + + model.nv + + 2 + + 4 + + 2 + + 1 + + 6 + + 1 + + 3 * model.nu + + model.na, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.nu, + min_height=float(kwargs.get("min_height", 0.8)), + max_rot=float(kwargs.get("max_rot", 0.8)), + hip_period=int(kwargs.get("hip_period", 100)), + reset_type=str(kwargs.get("reset_type", "init")), + target_x_vel=float(kwargs.get("target_x_vel", 0.0)), + target_y_vel=float(kwargs.get("target_y_vel", 1.2)), + target_rot=[] + if kwargs.get("target_rot") is None + else list(kwargs["target_rot"]), + terrain=str(kwargs.get("terrain", "")), + terrain_variant="" + if kwargs.get("variant") is None + else str(kwargs.get("variant")), + use_knee_condition=entry["class_name"] == "TerrainEnvV0", + reward_vel_w=float( + kwargs.get("weighted_reward_keys", {}).get("vel_reward", 5.0) + ), + reward_done_w=float( + kwargs.get("weighted_reward_keys", {}).get("done", -100.0) + ), + reward_cyclic_hip_w=float( + kwargs.get("weighted_reward_keys", {}).get("cyclic_hip", -10.0) + ), + reward_ref_rot_w=float( + kwargs.get("weighted_reward_keys", {}).get("ref_rot", 10.0) + ), + reward_joint_angle_w=float( + kwargs.get("weighted_reward_keys", {}).get("joint_angle_rew", 5.0) + ), + ) + if overrides: + config = spec_type.gen_config( + **dict( + zip(spec_type._config_keys, config, strict=False), + **overrides, + ) + ) + return config, pool_type, spec_type + + +def _pose_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type]: + config, _ = _pose_config(env_id) + values = dict(zip(MyoSuitePoseEnvSpec._config_keys, config, strict=False)) + pixel = MyoSuitePosePixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return pixel, MyoSuitePosePixelGymnasiumEnvPool + + +def _reach_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type]: + config, _ = _reach_config(env_id) + values = dict(zip(MyoSuiteReachEnvSpec._config_keys, config, strict=False)) + pixel = MyoSuiteReachPixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return pixel, MyoSuiteReachPixelGymnasiumEnvPool + + +def _reorient_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type]: + config, _ = _reorient_config(env_id) + values = dict( + zip(MyoSuiteReorientEnvSpec._config_keys, config, strict=False) + ) + pixel = MyoSuiteReorientPixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return pixel, MyoSuiteReorientPixelGymnasiumEnvPool + + +def _key_turn_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type]: + config, _ = _key_turn_config(env_id) + values = dict( + zip(MyoSuiteKeyTurnEnvSpec._config_keys, config, strict=False) + ) + pixel = MyoSuiteKeyTurnPixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return pixel, MyoSuiteKeyTurnPixelGymnasiumEnvPool + + +def _obj_hold_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type]: + config, _ = _obj_hold_config(env_id) + values = dict( + zip(MyoSuiteObjHoldEnvSpec._config_keys, config, strict=False) + ) + pixel = MyoSuiteObjHoldPixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return pixel, MyoSuiteObjHoldPixelGymnasiumEnvPool + + +def _torso_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type]: + config, _ = _torso_config(env_id) + values = dict(zip(MyoSuiteTorsoEnvSpec._config_keys, config, strict=False)) + pixel = MyoSuiteTorsoPixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return pixel, MyoSuiteTorsoPixelGymnasiumEnvPool + + +def _pen_twirl_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type]: + config, _ = _pen_twirl_config(env_id) + values = dict( + zip(MyoSuitePenTwirlEnvSpec._config_keys, config, strict=False) + ) + pixel = MyoSuitePenTwirlPixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return pixel, MyoSuitePenTwirlPixelGymnasiumEnvPool + + +def _walk_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type, type]: + config, _, _ = _walk_like_config(env_id) + values = dict(zip(MyoSuiteWalkEnvSpec._config_keys, config, strict=False)) + pixel = MyoSuiteWalkPixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return pixel, MyoSuiteWalkPixelGymnasiumEnvPool, MyoSuiteWalkPixelEnvSpec + + +def _terrain_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type, type]: + config, _, _ = _walk_like_config(env_id) + values = dict( + zip(MyoSuiteTerrainEnvSpec._config_keys, config, strict=False) + ) + pixel = MyoSuiteTerrainPixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return ( + pixel, + MyoSuiteTerrainPixelGymnasiumEnvPool, + MyoSuiteTerrainPixelEnvSpec, + ) + + +def _make_env(config: tuple[Any, ...], pool_type: type, spec_type: type) -> Any: + return pool_type(spec_type(config)) + + +def _make_registered_env(task_id: str, **kwargs: Any) -> Any: + return make_gymnasium( + task_id, + num_envs=1, + batch_size=1, + max_num_players=1, + **kwargs, + ) + + +def _registered_rollout( + task_id: str, *, seed: int +) -> tuple[np.ndarray, np.ndarray]: + env = _make_registered_env(task_id, seed=seed) + try: + env.reset() + actions = _seeded_actions( + (1, int(env.action_space.shape[-1])), steps=32, seed=seed + 11 + ) + final_obs = None + final_reward = None + for action in actions: + final_obs, final_reward, *_ = env.step(action) + assert final_obs is not None + assert final_reward is not None + return final_obs, final_reward + finally: + env.close() + + +def _seeded_actions( + shape: tuple[int, ...], steps: int, seed: int +) -> list[np.ndarray]: + rng = np.random.default_rng(seed) + return [ + rng.uniform(-0.9, 0.9, size=shape).astype(np.float32) + for _ in range(steps) + ] + + +def _assert_rollouts_match( + case: absltest.TestCase, + env0: Any, + env1: Any, + actions: list[np.ndarray], + *, + atol: float = 1e-9, + rtol: float = 1e-9, +) -> None: + obs0, info0 = env0.reset() + obs1, info1 = env1.reset() + np.testing.assert_allclose(obs0, obs1, atol=atol, rtol=rtol) + case.assertEqual( + info0["elapsed_step"].tolist(), info1["elapsed_step"].tolist() + ) + for action in actions: + out0 = env0.step(action) + out1 = env1.step(action) + obs0, reward0, terminated0, truncated0, info0 = out0 + obs1, reward1, terminated1, truncated1, info1 = out1 + np.testing.assert_allclose(obs0, obs1, atol=atol, rtol=rtol) + np.testing.assert_allclose(reward0, reward1, atol=atol, rtol=rtol) + np.testing.assert_array_equal(terminated0, terminated1) + np.testing.assert_array_equal(truncated0, truncated1) + case.assertEqual( + info0["elapsed_step"].tolist(), info1["elapsed_step"].tolist() + ) + if terminated0[0] or truncated0[0]: + break + + +def _oracle_reset_sync( + env: Any, env_id: str +) -> tuple[np.ndarray, dict[str, Any]]: + obs, _ = env.reset() + unwrapped = env.unwrapped + entry = _entry(env_id) + sync: dict[str, Any] = { + "test_reset_qpos": unwrapped.sim.data.qpos.copy().tolist(), + "test_reset_qvel": unwrapped.sim.data.qvel.copy().tolist(), + "test_reset_act": ( + unwrapped.sim.data.act.copy().tolist() + if unwrapped.sim.model.na > 0 + else [] + ), + "test_reset_qacc_warmstart": unwrapped.sim.data.qacc_warmstart.copy().tolist(), + } + if entry["class_name"] in {"PoseEnvV0", "ReachEnvV0"}: + sync["test_reset_ctrl"] = unwrapped.sim.data.ctrl.copy().tolist() + if entry["class_name"] == "KeyTurnEnvV0": + obs_sim = getattr(unwrapped, "sim_obsd", unwrapped.sim) + keyhead_sid = obs_sim.model.site_name2id("keyhead") + key_body_id = int(obs_sim.model.site_bodyid[keyhead_sid]) + sync["test_key_body_pos"] = ( + obs_sim.model.body_pos[key_body_id].copy().tolist() + ) + elif entry["class_name"] in {"ObjHoldFixedEnvV0", "ObjHoldRandomEnvV0"}: + goal_sid = unwrapped.sim.model.site_name2id("goal") + geom_id = unwrapped.sim.model.geom_name2id("object") + sync["test_goal_pos"] = ( + unwrapped.sim.model.site_pos[goal_sid].copy().tolist() + ) + sync["test_object_geom_size"] = ( + unwrapped.sim.model.geom_size[geom_id].copy().tolist() + ) + elif entry["class_name"] in { + "Geometries100EnvV0", + "Geometries8EnvV0", + "InDistribution", + "OutofDistribution", + }: + model = unwrapped.sim.model + target_body_id = model.body_name2id("target") + obj_geom_id = model.geom_name2id("obj") + target_geom_id = model.geom_name2id("target") + top_geom_id = model.geom_name2id("top") + bot_geom_id = model.geom_name2id("bot") + target_top_geom_id = model.geom_name2id("t_top") + target_bot_geom_id = model.geom_name2id("t_bot") + object_body_id = model.body_name2id("Object") + sync["test_target_body_quat"] = ( + model.body_quat[target_body_id].copy().tolist() + ) + sync["test_object_geom_size"] = ( + model.geom_size[obj_geom_id].copy().tolist() + ) + sync["test_target_geom_size"] = ( + model.geom_size[target_geom_id].copy().tolist() + ) + sync["test_object_geom_rgba"] = ( + model.geom_rgba[obj_geom_id].copy().tolist() + ) + sync["test_target_geom_rgba"] = ( + model.geom_rgba[target_geom_id].copy().tolist() + ) + sync["test_object_geom_top_pos"] = ( + model.geom_pos[top_geom_id].copy().tolist() + ) + sync["test_object_geom_bottom_pos"] = ( + model.geom_pos[bot_geom_id].copy().tolist() + ) + sync["test_target_geom_top_pos"] = ( + model.geom_pos[target_top_geom_id].copy().tolist() + ) + sync["test_target_geom_bottom_pos"] = ( + model.geom_pos[target_bot_geom_id].copy().tolist() + ) + sync["test_object_body_mass"] = [float(model.body_mass[object_body_id])] + sync["test_object_geom_type"] = int(model.geom_type[obj_geom_id]) + sync["test_target_geom_type"] = int(model.geom_type[target_geom_id]) + sync["test_object_geom_condim"] = int(model.geom_condim[obj_geom_id]) + sync["test_success_site_rgba"] = ( + model.site_rgba[unwrapped.success_indicator_sid].copy().tolist() + ) + elif entry["class_name"] in {"PenTwirlFixedEnvV0", "PenTwirlRandomEnvV0"}: + target_body_id = unwrapped.sim.model.body_name2id("target") + sync["test_target_body_quat"] = ( + unwrapped.sim.model.body_quat[target_body_id].copy().tolist() + ) + elif entry["class_name"] == "TerrainEnvV0": + terrain_geom_id = unwrapped.sim.model.geom_name2id("terrain") + hfield_id = int(unwrapped.sim.model.geom_dataid[terrain_geom_id]) + nrow = int(unwrapped.sim.model.hfield_nrow[hfield_id]) + ncol = int(unwrapped.sim.model.hfield_ncol[hfield_id]) + adr = int(unwrapped.sim.model.hfield_adr[hfield_id]) + sync["test_hfield_data"] = ( + unwrapped.sim.model + .hfield_data[adr : adr + nrow * ncol] + .copy() + .tolist() + ) + sync["test_terrain_geom_rgba"] = ( + unwrapped.sim.model.geom_rgba[terrain_geom_id].copy().tolist() + ) + sync["test_terrain_geom_pos"] = ( + unwrapped.sim.model.geom_pos[terrain_geom_id].copy().tolist() + ) + sync["test_terrain_geom_contype"] = int( + unwrapped.sim.model.geom_contype[terrain_geom_id] + ) + sync["test_terrain_geom_conaffinity"] = int( + unwrapped.sim.model.geom_conaffinity[terrain_geom_id] + ) + elif entry["class_name"] == "WalkEnvV0": + terrain_geom_id = unwrapped.sim.model.geom_name2id("terrain") + sync["test_terrain_geom_rgba"] = ( + unwrapped.sim.model.geom_rgba[terrain_geom_id].copy().tolist() + ) + sync["test_terrain_geom_pos"] = ( + unwrapped.sim.model.geom_pos[terrain_geom_id].copy().tolist() + ) + sync["test_terrain_geom_contype"] = int( + unwrapped.sim.model.geom_contype[terrain_geom_id] + ) + sync["test_terrain_geom_conaffinity"] = int( + unwrapped.sim.model.geom_conaffinity[terrain_geom_id] + ) + elif entry["class_name"] == "PoseEnvV0": + sync["test_target_qpos"] = unwrapped.target_jnt_value.copy().tolist() + target_site_pos: list[float] = [] + for site_name in entry["kwargs"].get("viz_site_targets", []): + site_id = unwrapped.sim.model.site_name2id(site_name + "_target") + target_site_pos.extend( + unwrapped.sim.model.site_pos[site_id].copy().tolist() + ) + if target_site_pos: + sync["test_target_site_pos"] = target_site_pos + if getattr(unwrapped, "weight_bodyname", None): + body_id = unwrapped.sim.model.body_name2id( + unwrapped.weight_bodyname + ) + geom_id = unwrapped.sim.model.body_geomadr[body_id] + sync["test_body_mass"] = [ + float(unwrapped.sim.model.body_mass[body_id]) + ] + sync["test_geom_size0"] = [ + float(unwrapped.sim.model.geom_size[geom_id][0]) + ] + elif entry["class_name"] == "ReachEnvV0": + target_pos: list[float] = [] + for site_name in entry["kwargs"]["target_reach_range"]: + site_id = unwrapped.sim.model.site_name2id(site_name + "_target") + target_pos.extend( + unwrapped.sim.model.site_pos[site_id].copy().tolist() + ) + sync["test_target_pos"] = target_pos + return obs, sync + + +def _oracle_class(env_id: str) -> Any: + entry = _entry(env_id) + return load_oracle_class(entry["entry_module"], entry["class_name"]) + + +def _pose_alignment_obs_atol(env_id: str) -> float: + if env_id == "motorFingerPoseRandom-v0": + return 3e-4 + return 1e-7 + + +def _pose_alignment_reward_atol(env_id: str) -> float: + del env_id + return 1e-7 + + +def _reach_alignment_obs_atol(env_id: str) -> float: + if env_id == "motorFingerReachRandom-v0": + return 4e-4 + if env_id in {"myoHandReachRandom-v0", "myoArmReachRandom-v0"}: + return 3e-3 if env_id == "myoHandReachRandom-v0" else 2.5e-3 + return 1e-7 + + +def _reach_alignment_reward_atol(env_id: str) -> float: + # These envs still carry small reset-synced residuals because the oracle + # computes rewards from its observed-sim reconstruction path, while the + # native slice currently evaluates directly from the stepped sim state. + if env_id == "motorFingerReachRandom-v0": + return 3e-4 + if env_id == "myoHandReachRandom-v0": + return 3e-3 + if env_id == "myoArmReachRandom-v0": + return 2e-3 + return 1e-7 + + +def _reorient_alignment_obs_atol(env_id: str) -> float: + del env_id + return 7.0 + + +def _reorient_alignment_reward_atol(env_id: str) -> float: + del env_id + return 0.5 + + +def _key_turn_alignment_obs_atol(env_id: str) -> float: + # The official KeyTurn oracle emits observations from its sim_obsd path, + # while the native slice currently reads directly from the stepped sim. + # The residual is isolated to fingertip/key site vectors after reset-sync. + del env_id + return 3e-3 + + +def _key_turn_alignment_reward_atol(env_id: str) -> float: + del env_id + return 6e-3 + + +def _obj_hold_alignment_obs_atol(env_id: str) -> float: + del env_id + return 2e-3 + + +def _obj_hold_alignment_reward_atol(env_id: str) -> float: + del env_id + return 5e-2 + + +def _torso_alignment_obs_atol(env_id: str) -> float: + del env_id + return 1e-7 + + +def _torso_alignment_reward_atol(env_id: str) -> float: + del env_id + return 1e-7 + + +def _pen_twirl_alignment_obs_atol(env_id: str) -> float: + del env_id + return 1.1e-2 + + +def _pen_twirl_alignment_reward_atol(env_id: str) -> float: + del env_id + return 2e-2 + + +def _walk_alignment_obs_atol(env_id: str) -> float: + del env_id + return 5e-2 + + +def _walk_alignment_reward_atol(env_id: str) -> float: + del env_id + return 0.2 + + +def _terrain_alignment_obs_atol(env_id: str) -> float: + del env_id + return 3.0 + + +def _terrain_alignment_reward_atol(env_id: str) -> float: + del env_id + return 8.0 + + +class MyoSuiteMyoBaseNativeTest(absltest.TestCase): + """Covers the internal native MyoSuite MyoBase slice.""" + + def test_pose_surface_count(self) -> None: + """PoseEnvV0 metadata should map to the expected direct surface.""" + self.assertLen(_POSE_IDS, 20) + + def test_reach_surface_count(self) -> None: + """ReachEnvV0 metadata should map to the expected direct surface.""" + self.assertLen(_REACH_IDS, 9) + + def test_reorient_surface_count(self) -> None: + """Reorient metadata should map to the expected direct surface.""" + self.assertLen(_REORIENT_IDS, 4) + + def test_key_turn_surface_count(self) -> None: + """KeyTurnEnvV0 metadata should map to the expected direct surface.""" + self.assertLen(_KEYTURN_IDS, 2) + + def test_obj_hold_surface_count(self) -> None: + """ObjHold metadata should map to the expected direct surface.""" + self.assertLen(_OBJHOLD_IDS, 2) + + def test_torso_surface_count(self) -> None: + """TorsoEnvV0 metadata should map to the expected direct surface.""" + self.assertLen(_TORSO_IDS, 2) + + def test_pen_twirl_surface_count(self) -> None: + """PenTwirl metadata should map to the expected direct surface.""" + self.assertLen(_PENTWIRL_IDS, 2) + + def test_walk_surface_count(self) -> None: + """WalkEnvV0 metadata should map to the expected direct surface.""" + self.assertLen(_WALK_IDS, 1) + + def test_terrain_surface_count(self) -> None: + """TerrainEnvV0 metadata should map to the expected direct surface.""" + self.assertLen(_TERRAIN_IDS, 3) + + def test_pose_native_determinism_for_all_ids(self) -> None: + """Pose envs should be deterministic under a fixed action sequence.""" + for env_id in _POSE_IDS: + config, pool_type = _pose_config(env_id) + env0 = _make_env(config, pool_type, MyoSuitePoseEnvSpec) + env1 = _make_env(config, pool_type, MyoSuitePoseEnvSpec) + model = _model(_runtime_model_path(_entry(env_id))) + actions = _seeded_actions((1, model.nu), steps=8, seed=17) + with self.subTest(env_id=env_id): + _assert_rollouts_match(self, env0, env1, actions) + + def test_reach_native_determinism_for_all_ids(self) -> None: + """Reach envs should be deterministic under a fixed action sequence.""" + for env_id in _REACH_IDS: + entry = _entry(env_id) + model_path = str(_asset_model_path(_runtime_model_path(entry))) + config, pool_type = _reach_config(env_id, model_path=model_path) + env0 = _make_env(config, pool_type, MyoSuiteReachEnvSpec) + env1 = _make_env(config, pool_type, MyoSuiteReachEnvSpec) + model = _model(model_path) + actions = _seeded_actions((1, model.nu), steps=8, seed=29) + with self.subTest(env_id=env_id): + _assert_rollouts_match(self, env0, env1, actions) + + def test_reorient_native_determinism_for_all_ids(self) -> None: + """Reorient envs should be deterministic under a fixed action sequence.""" + for env_id in _REORIENT_IDS: + config, pool_type = _reorient_config(env_id) + env0 = _make_env(config, pool_type, MyoSuiteReorientEnvSpec) + env1 = _make_env(config, pool_type, MyoSuiteReorientEnvSpec) + model = _model(_runtime_model_path(_entry(env_id))) + actions = _seeded_actions((1, model.nu), steps=8, seed=61) + with self.subTest(env_id=env_id): + _assert_rollouts_match(self, env0, env1, actions) + + def test_key_turn_native_determinism_for_all_ids(self) -> None: + """KeyTurn envs should be deterministic under a fixed action sequence.""" + for env_id in _KEYTURN_IDS: + config, pool_type = _key_turn_config(env_id) + env0 = _make_env(config, pool_type, MyoSuiteKeyTurnEnvSpec) + env1 = _make_env(config, pool_type, MyoSuiteKeyTurnEnvSpec) + model = _model(_runtime_model_path(_entry(env_id))) + actions = _seeded_actions((1, model.nu), steps=8, seed=71) + with self.subTest(env_id=env_id): + _assert_rollouts_match(self, env0, env1, actions) + + def test_obj_hold_native_determinism_for_all_ids(self) -> None: + """ObjHold envs should be deterministic under a fixed action sequence.""" + for env_id in _OBJHOLD_IDS: + config, pool_type = _obj_hold_config(env_id) + env0 = _make_env(config, pool_type, MyoSuiteObjHoldEnvSpec) + env1 = _make_env(config, pool_type, MyoSuiteObjHoldEnvSpec) + model = _model(_runtime_model_path(_entry(env_id))) + actions = _seeded_actions((1, model.nu), steps=8, seed=83) + with self.subTest(env_id=env_id): + _assert_rollouts_match(self, env0, env1, actions) + + def test_torso_native_determinism_for_all_ids(self) -> None: + """Torso envs should be deterministic under a fixed action sequence.""" + for env_id in _TORSO_IDS: + config, pool_type = _torso_config(env_id) + env0 = _make_env(config, pool_type, MyoSuiteTorsoEnvSpec) + env1 = _make_env(config, pool_type, MyoSuiteTorsoEnvSpec) + model = _model(_runtime_model_path(_entry(env_id))) + actions = _seeded_actions((1, model.nu), steps=8, seed=89) + with self.subTest(env_id=env_id): + _assert_rollouts_match(self, env0, env1, actions) + + def test_pen_twirl_native_determinism_for_all_ids(self) -> None: + """PenTwirl envs should be deterministic under a fixed action sequence.""" + for env_id in _PENTWIRL_IDS: + config, pool_type = _pen_twirl_config(env_id) + env0 = _make_env(config, pool_type, MyoSuitePenTwirlEnvSpec) + env1 = _make_env(config, pool_type, MyoSuitePenTwirlEnvSpec) + model = _model(_runtime_model_path(_entry(env_id))) + actions = _seeded_actions((1, model.nu), steps=8, seed=107) + with self.subTest(env_id=env_id): + _assert_rollouts_match(self, env0, env1, actions) + + def test_walk_native_determinism_for_all_ids(self) -> None: + """Walk envs should be deterministic under a fixed action sequence.""" + for env_id in _WALK_IDS: + config, pool_type, spec_type = _walk_like_config(env_id) + env0 = _make_env(config, pool_type, spec_type) + env1 = _make_env(config, pool_type, spec_type) + model = _model(_runtime_model_path(_entry(env_id))) + actions = _seeded_actions((1, model.nu), steps=8, seed=113) + with self.subTest(env_id=env_id): + _assert_rollouts_match(self, env0, env1, actions) + + def test_terrain_native_determinism_for_all_ids(self) -> None: + """Terrain envs should be deterministic under a fixed action sequence.""" + for env_id in _TERRAIN_IDS: + config, pool_type, spec_type = _walk_like_config(env_id) + env0 = _make_env(config, pool_type, spec_type) + env1 = _make_env(config, pool_type, spec_type) + model = _model(_runtime_model_path(_entry(env_id))) + actions = _seeded_actions((1, model.nu), steps=8, seed=127) + with self.subTest(env_id=env_id): + _assert_rollouts_match(self, env0, env1, actions) + + def test_pose_alignment_representative_ids(self) -> None: + """Representative pose envs should align with the official oracle.""" + for env_id in _POSE_ALIGN_IDS: + entry = _entry(env_id) + model_path = _alignment_model_path(entry) + with self.subTest(env_id=env_id): + cls = _oracle_class(env_id) + kwargs = dict(entry["kwargs"]) + kwargs.pop("edit_fn", None) + kwargs["model_path"] = model_path + oracle: Any = gymnasium.wrappers.TimeLimit( + cls(seed=123, **kwargs), + max_episode_steps=entry["max_episode_steps"], + ) + obs0, sync = _oracle_reset_sync(oracle, env_id) + obs_atol = _pose_alignment_obs_atol(env_id) + reward_atol = _pose_alignment_reward_atol(env_id) + config, pool_type = _pose_config( + env_id, model_path=model_path, overrides=sync + ) + native = _make_env(config, pool_type, MyoSuitePoseEnvSpec) + obs1, info1 = native.reset() + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + self.assertEqual(info1["elapsed_step"].tolist(), [0]) + native_model = _model(model_path) + actions = _seeded_actions( + (1, native_model.nu), + steps=_ALIGNMENT_STEPS, + seed=41, + ) + for action in actions: + obs0, reward0, terminated0, truncated0, _ = oracle.step( + action[0] + ) + obs1, reward1, terminated1, truncated1, _ = native.step( + action + ) + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + np.testing.assert_allclose( + reward1, + np.array([reward0]), + atol=reward_atol, + rtol=reward_atol, + ) + np.testing.assert_array_equal( + terminated1, np.array([terminated0]) + ) + np.testing.assert_array_equal( + truncated1, np.array([truncated0]) + ) + if terminated0 or truncated0: + break + + def test_reach_alignment_representative_ids(self) -> None: + """Representative reach envs should align with the official oracle.""" + for env_id in _REACH_ALIGN_IDS: + entry = _entry(env_id) + model_path = _alignment_model_path(entry) + with self.subTest(env_id=env_id): + cls = _oracle_class(env_id) + kwargs = dict(entry["kwargs"]) + kwargs.pop("edit_fn", None) + kwargs["model_path"] = model_path + oracle: Any = gymnasium.wrappers.TimeLimit( + cls(seed=123, **kwargs), + max_episode_steps=entry["max_episode_steps"], + ) + obs0, sync = _oracle_reset_sync(oracle, env_id) + obs_atol = _reach_alignment_obs_atol(env_id) + reward_atol = _reach_alignment_reward_atol(env_id) + config, pool_type = _reach_config( + env_id, model_path=model_path, overrides=sync + ) + native = _make_env(config, pool_type, MyoSuiteReachEnvSpec) + obs1, _ = native.reset() + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + actions = _seeded_actions( + (1, _model(model_path).nu), + steps=_ALIGNMENT_STEPS, + seed=53, + ) + for action in actions: + obs0, reward0, terminated0, truncated0, _ = oracle.step( + action[0] + ) + obs1, reward1, terminated1, truncated1, _ = native.step( + action + ) + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + np.testing.assert_allclose( + reward1, + np.array([reward0]), + atol=reward_atol, + rtol=reward_atol, + ) + np.testing.assert_array_equal( + terminated1, np.array([terminated0]) + ) + np.testing.assert_array_equal( + truncated1, np.array([truncated0]) + ) + if terminated0 or truncated0: + break + + def test_reorient_alignment_representative_ids(self) -> None: + """Representative reorient envs should align with the official oracle.""" + for env_id in _REORIENT_ALIGN_IDS: + entry = _entry(env_id) + model_path = _alignment_model_path(entry) + with self.subTest(env_id=env_id): + cls = _oracle_class(env_id) + kwargs = dict(entry["kwargs"]) + kwargs["model_path"] = model_path + oracle: Any = gymnasium.wrappers.TimeLimit( + cls(seed=123, **kwargs), + max_episode_steps=entry["max_episode_steps"], + ) + obs0, sync = _oracle_reset_sync(oracle, env_id) + obs_atol = _reorient_alignment_obs_atol(env_id) + reward_atol = _reorient_alignment_reward_atol(env_id) + config, pool_type = _reorient_config( + env_id, model_path=model_path, overrides=sync + ) + native = _make_env(config, pool_type, MyoSuiteReorientEnvSpec) + obs1, _ = native.reset() + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + actions = _seeded_actions( + (1, _model(model_path).nu), + steps=_ALIGNMENT_STEPS, + seed=67, + ) + for action in actions: + obs0, reward0, terminated0, truncated0, _ = oracle.step( + action[0] + ) + obs1, reward1, terminated1, truncated1, _ = native.step( + action + ) + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + np.testing.assert_allclose( + reward1, + np.array([reward0]), + atol=reward_atol, + rtol=reward_atol, + ) + np.testing.assert_array_equal( + terminated1, np.array([terminated0]) + ) + np.testing.assert_array_equal( + truncated1, np.array([truncated0]) + ) + if terminated0 or truncated0: + break + + def test_key_turn_alignment_representative_ids(self) -> None: + """Representative key-turn envs should align with the official oracle.""" + for env_id in _KEYTURN_ALIGN_IDS: + entry = _entry(env_id) + model_path = _alignment_model_path(entry) + with self.subTest(env_id=env_id): + cls = _oracle_class(env_id) + kwargs = dict(entry["kwargs"]) + kwargs.pop("edit_fn", None) + kwargs["model_path"] = model_path + oracle: Any = gymnasium.wrappers.TimeLimit( + cls(seed=123, **kwargs), + max_episode_steps=entry["max_episode_steps"], + ) + obs0, sync = _oracle_reset_sync(oracle, env_id) + obs_atol = _key_turn_alignment_obs_atol(env_id) + reward_atol = _key_turn_alignment_reward_atol(env_id) + config, pool_type = _key_turn_config( + env_id, model_path=model_path, overrides=sync + ) + native = _make_env(config, pool_type, MyoSuiteKeyTurnEnvSpec) + obs1, _ = native.reset() + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + actions = _seeded_actions( + (1, _model(model_path).nu), + steps=_ALIGNMENT_STEPS, + seed=79, + ) + for action in actions: + obs0, reward0, terminated0, truncated0, _ = oracle.step( + action[0] + ) + obs1, reward1, terminated1, truncated1, _ = native.step( + action + ) + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + np.testing.assert_allclose( + reward1, + np.array([reward0]), + atol=reward_atol, + rtol=reward_atol, + ) + np.testing.assert_array_equal( + terminated1, np.array([terminated0]) + ) + np.testing.assert_array_equal( + truncated1, np.array([truncated0]) + ) + if terminated0 or truncated0: + break + + def test_obj_hold_alignment_representative_ids(self) -> None: + """Representative obj-hold envs should align with the official oracle.""" + for env_id in _OBJHOLD_ALIGN_IDS: + entry = _entry(env_id) + model_path = _alignment_model_path(entry) + with self.subTest(env_id=env_id): + cls = _oracle_class(env_id) + kwargs = dict(entry["kwargs"]) + kwargs.pop("edit_fn", None) + kwargs["model_path"] = model_path + oracle: Any = gymnasium.wrappers.TimeLimit( + cls(seed=123, **kwargs), + max_episode_steps=entry["max_episode_steps"], + ) + obs0, sync = _oracle_reset_sync(oracle, env_id) + obs_atol = _obj_hold_alignment_obs_atol(env_id) + reward_atol = _obj_hold_alignment_reward_atol(env_id) + config, pool_type = _obj_hold_config( + env_id, model_path=model_path, overrides=sync + ) + native = _make_env(config, pool_type, MyoSuiteObjHoldEnvSpec) + obs1, _ = native.reset() + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + actions = _seeded_actions( + (1, _model(model_path).nu), + steps=_ALIGNMENT_STEPS, + seed=97, + ) + for action in actions: + obs0, reward0, terminated0, truncated0, _ = oracle.step( + action[0] + ) + obs1, reward1, terminated1, truncated1, _ = native.step( + action + ) + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + np.testing.assert_allclose( + reward1, + np.array([reward0]), + atol=reward_atol, + rtol=reward_atol, + ) + np.testing.assert_array_equal( + terminated1, np.array([terminated0]) + ) + np.testing.assert_array_equal( + truncated1, np.array([truncated0]) + ) + if terminated0 or truncated0: + break + + def test_torso_alignment_representative_ids(self) -> None: + """Representative torso envs should align with the official oracle.""" + for env_id in _TORSO_ALIGN_IDS: + entry = _entry(env_id) + model_path = _alignment_model_path(entry) + with self.subTest(env_id=env_id): + cls = _oracle_class(env_id) + kwargs = dict(entry["kwargs"]) + kwargs.pop("edit_fn", None) + kwargs["model_path"] = model_path + oracle: Any = gymnasium.wrappers.TimeLimit( + cls(seed=123, **kwargs), + max_episode_steps=entry["max_episode_steps"], + ) + obs0, sync = _oracle_reset_sync(oracle, env_id) + obs_atol = _torso_alignment_obs_atol(env_id) + reward_atol = _torso_alignment_reward_atol(env_id) + config, pool_type = _torso_config( + env_id, model_path=model_path, overrides=sync + ) + native = _make_env(config, pool_type, MyoSuiteTorsoEnvSpec) + obs1, _ = native.reset() + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + actions = _seeded_actions( + (1, _model(model_path).nu), + steps=_ALIGNMENT_STEPS, + seed=101, + ) + for action in actions: + obs0, reward0, terminated0, truncated0, _ = oracle.step( + action[0] + ) + obs1, reward1, terminated1, truncated1, _ = native.step( + action + ) + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + np.testing.assert_allclose( + reward1, + np.array([reward0]), + atol=reward_atol, + rtol=reward_atol, + ) + np.testing.assert_array_equal( + terminated1, np.array([terminated0]) + ) + np.testing.assert_array_equal( + truncated1, np.array([truncated0]) + ) + if terminated0 or truncated0: + break + + def test_pen_twirl_alignment_representative_ids(self) -> None: + """Representative pen-twirl envs should align with the official oracle.""" + for env_id in _PENTWIRL_ALIGN_IDS: + entry = _entry(env_id) + model_path = _alignment_model_path(entry) + with self.subTest(env_id=env_id): + cls = _oracle_class(env_id) + kwargs = dict(entry["kwargs"]) + kwargs.pop("edit_fn", None) + kwargs["model_path"] = model_path + oracle: Any = gymnasium.wrappers.TimeLimit( + cls(seed=123, **kwargs), + max_episode_steps=entry["max_episode_steps"], + ) + obs0, sync = _oracle_reset_sync(oracle, env_id) + obs_atol = _pen_twirl_alignment_obs_atol(env_id) + reward_atol = _pen_twirl_alignment_reward_atol(env_id) + config, pool_type = _pen_twirl_config( + env_id, model_path=model_path, overrides=sync + ) + native = _make_env(config, pool_type, MyoSuitePenTwirlEnvSpec) + obs1, _ = native.reset() + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + actions = _seeded_actions( + (1, _model(model_path).nu), + steps=_ALIGNMENT_STEPS, + seed=109, + ) + for action in actions: + obs0, reward0, terminated0, truncated0, _ = oracle.step( + action[0] + ) + obs1, reward1, terminated1, truncated1, _ = native.step( + action + ) + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + np.testing.assert_allclose( + reward1, + np.array([reward0]), + atol=reward_atol, + rtol=reward_atol, + ) + np.testing.assert_array_equal( + terminated1, np.array([terminated0]) + ) + np.testing.assert_array_equal( + truncated1, np.array([truncated0]) + ) + if terminated0 or truncated0: + break + + def test_walk_alignment_representative_ids(self) -> None: + """Representative walk envs should align with the official oracle.""" + for env_id in _WALK_ALIGN_IDS: + entry = _entry(env_id) + model_path = _alignment_model_path(entry) + with self.subTest(env_id=env_id): + cls = _oracle_class(env_id) + kwargs = dict(entry["kwargs"]) + kwargs["model_path"] = model_path + oracle: Any = gymnasium.wrappers.TimeLimit( + cls(seed=123, **kwargs), + max_episode_steps=entry["max_episode_steps"], + ) + obs0, sync = _oracle_reset_sync(oracle, env_id) + obs_atol = _walk_alignment_obs_atol(env_id) + reward_atol = _walk_alignment_reward_atol(env_id) + config, pool_type, spec_type = _walk_like_config( + env_id, model_path=model_path, overrides=sync + ) + native = _make_env(config, pool_type, spec_type) + obs1, _ = native.reset() + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + actions = _seeded_actions( + (1, _model(model_path).nu), + steps=_ALIGNMENT_STEPS, + seed=131, + ) + for action in actions: + obs0, reward0, terminated0, truncated0, _ = oracle.step( + action[0] + ) + obs1, reward1, terminated1, truncated1, _ = native.step( + action + ) + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + np.testing.assert_allclose( + reward1, + np.array([reward0]), + atol=reward_atol, + rtol=reward_atol, + ) + np.testing.assert_array_equal( + terminated1, np.array([terminated0]) + ) + np.testing.assert_array_equal( + truncated1, np.array([truncated0]) + ) + if terminated0 or truncated0: + break + + def test_terrain_alignment_representative_ids(self) -> None: + """Representative terrain envs should align with the official oracle.""" + for env_id in _TERRAIN_ALIGN_IDS: + entry = _entry(env_id) + model_path = _alignment_model_path(entry) + with self.subTest(env_id=env_id): + cls = _oracle_class(env_id) + kwargs = dict(entry["kwargs"]) + kwargs["model_path"] = model_path + oracle: Any = gymnasium.wrappers.TimeLimit( + cls(seed=123, **kwargs), + max_episode_steps=entry["max_episode_steps"], + ) + obs0, sync = _oracle_reset_sync(oracle, env_id) + obs_atol = _terrain_alignment_obs_atol(env_id) + reward_atol = _terrain_alignment_reward_atol(env_id) + config, pool_type, spec_type = _walk_like_config( + env_id, model_path=model_path, overrides=sync + ) + native = _make_env(config, pool_type, spec_type) + obs1, _ = native.reset() + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + actions = _seeded_actions( + (1, _model(model_path).nu), + steps=_ALIGNMENT_STEPS, + seed=149, + ) + for action in actions: + obs0, reward0, terminated0, truncated0, _ = oracle.step( + action[0] + ) + obs1, reward1, terminated1, truncated1, _ = native.step( + action + ) + np.testing.assert_allclose( + obs1, obs0[None, :], atol=obs_atol, rtol=obs_atol + ) + np.testing.assert_allclose( + reward1, + np.array([reward0]), + atol=reward_atol, + rtol=reward_atol, + ) + np.testing.assert_array_equal( + terminated1, np.array([terminated0]) + ) + np.testing.assert_array_equal( + truncated1, np.array([truncated0]) + ) + if terminated0 or truncated0: + break + + def test_pose_pixel_observation_smoke(self) -> None: + """Pose pixel wrappers should emit batched RGB observations.""" + config, pool_type = _pose_pixel_config("myoHandPoseRandom-v0") + env = _make_env(config, pool_type, MyoSuitePosePixelEnvSpec) + obs, _ = env.reset() + self.assertEqual(obs.shape, (1, 3, 64, 64)) + + def test_reach_pixel_observation_smoke(self) -> None: + """Reach pixel wrappers should emit batched RGB observations.""" + config, pool_type = _reach_pixel_config("myoHandReachRandom-v0") + env = _make_env(config, pool_type, MyoSuiteReachPixelEnvSpec) + obs, _ = env.reset() + self.assertEqual(obs.shape, (1, 3, 64, 64)) + + def test_reorient_pixel_observation_smoke(self) -> None: + """Reorient pixel wrappers should emit batched RGB observations.""" + config, pool_type = _reorient_pixel_config("myoHandReorientID-v0") + env = _make_env(config, pool_type, MyoSuiteReorientPixelEnvSpec) + obs, _ = env.reset() + self.assertEqual(obs.shape, (1, 3, 64, 64)) + + def test_key_turn_pixel_observation_smoke(self) -> None: + """KeyTurn pixel wrappers should emit batched RGB observations.""" + config, pool_type = _key_turn_pixel_config("myoHandKeyTurnRandom-v0") + env = _make_env(config, pool_type, MyoSuiteKeyTurnPixelEnvSpec) + obs, _ = env.reset() + self.assertEqual(obs.shape, (1, 3, 64, 64)) + + def test_obj_hold_pixel_observation_smoke(self) -> None: + """ObjHold pixel wrappers should emit batched RGB observations.""" + config, pool_type = _obj_hold_pixel_config("myoHandObjHoldRandom-v0") + env = _make_env(config, pool_type, MyoSuiteObjHoldPixelEnvSpec) + obs, _ = env.reset() + self.assertEqual(obs.shape, (1, 3, 64, 64)) + + def test_torso_pixel_observation_smoke(self) -> None: + """Torso pixel wrappers should emit batched RGB observations.""" + config, pool_type = _torso_pixel_config("myoTorsoPoseFixed-v0") + env = _make_env(config, pool_type, MyoSuiteTorsoPixelEnvSpec) + obs, _ = env.reset() + self.assertEqual(obs.shape, (1, 3, 64, 64)) + + def test_pen_twirl_pixel_observation_smoke(self) -> None: + """PenTwirl pixel wrappers should emit batched RGB observations.""" + config, pool_type = _pen_twirl_pixel_config("myoHandPenTwirlRandom-v0") + env = _make_env(config, pool_type, MyoSuitePenTwirlPixelEnvSpec) + obs, _ = env.reset() + self.assertEqual(obs.shape, (1, 3, 64, 64)) + + def test_walk_pixel_observation_smoke(self) -> None: + """Walk pixel wrappers should stay constructible alongside terrain render.""" + config, pool_type, spec_type = _walk_pixel_config("myoLegWalk-v0") + env = _make_env(config, pool_type, spec_type) + self.assertIn( + "obs:pixels", cast(list[str], MyoSuiteWalkPixelEnvSpec._state_keys) + ) + self.assertIsNotNone(env) + + def test_terrain_pixel_observation_smoke(self) -> None: + """Terrain pixel wrappers should emit batched RGB observations.""" + config, pool_type, spec_type = _terrain_pixel_config( + "myoLegRoughTerrainWalk-v0" + ) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape, (1, 3, 64, 64)) + + def test_public_registry_covers_full_expanded_surface(self) -> None: + """Public registration should expose every expanded MyoSuite task ID.""" + registered = set(list_all_envs()) + self.assertLen(MYOSUITE_PUBLIC_TASK_IDS, 398) + self.assertEmpty(set(MYOSUITE_PUBLIC_TASK_IDS) - registered) + + def test_public_registered_envs_construct_and_reset(self) -> None: + """Representative public IDs should construct through make_gymnasium.""" + for task_id in _PUBLIC_REPRESENTATIVE_TASK_IDS: + with self.subTest(task_id=task_id): + env = _make_registered_env(task_id, seed=0) + try: + obs, info = env.reset() + self.assertEqual(obs.shape[0], 1) + self.assertIn("elapsed_step", info) + finally: + env.close() + + def test_public_variant_ids_change_dynamics(self) -> None: + """Variant IDs should change rollout dynamics relative to the base ID.""" + for base_task_id, variant_task_id in _PUBLIC_VARIANT_DIFF_CASES: + with self.subTest( + base_task_id=base_task_id, variant_task_id=variant_task_id + ): + base_obs, base_reward = _registered_rollout( + base_task_id, seed=7 + ) + variant_obs, variant_reward = _registered_rollout( + variant_task_id, seed=7 + ) + self.assertGreater( + float(np.max(np.abs(base_obs - variant_obs))), 1e-6 + ) + self.assertGreater( + float(np.max(np.abs(base_reward - variant_reward))), 1e-6 + ) + + def test_public_from_pixels_smoke(self) -> None: + """Public registration should expose pixel wrappers for MyoSuite.""" + env = _make_registered_env( + "myoHandReorientID-v0", + seed=3, + from_pixels=True, + render_width=32, + render_height=24, + ) + try: + obs, _ = env.reset() + self.assertEqual(obs.shape, (1, 3, 24, 32)) + finally: + env.close() + + +if __name__ == "__main__": + absltest.main() diff --git a/envpool/mujoco/myosuite/myochallenge.h b/envpool/mujoco/myosuite/myochallenge.h new file mode 100644 index 000000000..e051bd17c --- /dev/null +++ b/envpool/mujoco/myosuite/myochallenge.h @@ -0,0 +1,2957 @@ +// Copyright 2026 Garena Online Private 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 ENVPOOL_MUJOCO_MYOSUITE_MYOCHALLENGE_H_ +#define ENVPOOL_MUJOCO_MYOSUITE_MYOCHALLENGE_H_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "envpool/mujoco/myosuite/myobase.h" + +namespace myosuite_envpool { + +namespace challenge_detail { + +inline std::array EulerXYZToQuat( + const std::array& euler) { + mjtNum ai = euler[2] / 2.0; + mjtNum aj = -euler[1] / 2.0; + mjtNum ak = euler[0] / 2.0; + mjtNum si = std::sin(ai); + mjtNum sj = std::sin(aj); + mjtNum sk = std::sin(ak); + mjtNum ci = std::cos(ai); + mjtNum cj = std::cos(aj); + mjtNum ck = std::cos(ak); + mjtNum cc = ci * ck; + mjtNum cs = ci * sk; + mjtNum sc = si * ck; + mjtNum ss = si * sk; + return {cj * cc + sj * ss, cj * cs - sj * sc, -(cj * ss + sj * cc), + cj * sc - sj * cs}; +} + +inline std::array Mat9ToEuler(const mjtNum* mat) { + mjtNum cy = std::sqrt(mat[8] * mat[8] + mat[5] * mat[5]); + bool regular = cy > std::numeric_limits::epsilon() * 4.0; + return { + regular ? -std::atan2(mat[5], mat[8]) : 0.0, + -std::atan2(-mat[2], cy), + regular ? -std::atan2(mat[1], mat[0]) : -std::atan2(-mat[3], mat[4]), + }; +} + +inline std::array RotateByQuat(const mjtNum* quat, + const mjtNum* vec) { + std::array mat{}; + mju_quat2Mat(mat.data(), quat); + return { + mat[0] * vec[0] + mat[1] * vec[1] + mat[2] * vec[2], + mat[3] * vec[0] + mat[4] * vec[1] + mat[5] * vec[2], + mat[6] * vec[0] + mat[7] * vec[1] + mat[8] * vec[2], + }; +} + +inline std::array RotateByQuat(const mjtNum* quat, const float* vec) { + std::array mat{}; + mju_quat2Mat(mat.data(), quat); + return { + static_cast(mat[0] * vec[0] + mat[1] * vec[1] + mat[2] * vec[2]), + static_cast(mat[3] * vec[0] + mat[4] * vec[1] + mat[5] * vec[2]), + static_cast(mat[6] * vec[0] + mat[7] * vec[1] + mat[8] * vec[2]), + }; +} + +inline mjtNum Norm3(const std::array& value) { + return std::sqrt(value[0] * value[0] + value[1] * value[1] + + value[2] * value[2]); +} + +inline std::array Sub3(const std::array& lhs, + const std::array& rhs) { + return {lhs[0] - rhs[0], lhs[1] - rhs[1], lhs[2] - rhs[2]}; +} + +inline std::array UniformVec3(std::mt19937* gen, mjtNum low, + mjtNum high) { + std::uniform_real_distribution dist(low, high); + return {static_cast(dist(*gen)), static_cast(dist(*gen)), + static_cast(dist(*gen))}; +} + +inline std::array UniformVec3(std::mt19937* gen, + const std::array& low, + const std::array& high) { + std::array value{}; + for (int axis = 0; axis < 3; ++axis) { + std::uniform_real_distribution dist(low[axis], high[axis]); + value[axis] = static_cast(dist(*gen)); + } + return value; +} + +inline mjtNum UniformScalar(std::mt19937* gen, mjtNum low, mjtNum high) { + std::uniform_real_distribution dist(low, high); + return static_cast(dist(*gen)); +} + +inline void CopyBodyGeomSlice3(const mjModel* model, const mjtNum* src, + int body_id, std::vector* out) { + int start_geom = model->body_geomadr[body_id]; + int geom_count = model->body_geomnum[body_id]; + out->assign(src + start_geom * 3, src + (start_geom + geom_count) * 3); +} + +inline void RestoreBodyGeomSlice3(mjModel* model, mjtNum* target, int body_id, + const std::vector& value) { + if (value.empty()) { + return; + } + int start_geom = model->body_geomadr[body_id]; + int geom_count = model->body_geomnum[body_id]; + std::memcpy(target + start_geom * 3, value.data(), + sizeof(mjtNum) * 3 * geom_count); +} + +inline void CopyBodyGeomSlice4(const mjModel* model, const mjtNum* src, + int body_id, std::vector* out) { + int start_geom = model->body_geomadr[body_id]; + int geom_count = model->body_geomnum[body_id]; + out->assign(src + start_geom * 4, src + (start_geom + geom_count) * 4); +} + +inline void RestoreBodyGeomSlice4(mjModel* model, mjtNum* target, int body_id, + const std::vector& value) { + if (value.empty()) { + return; + } + int start_geom = model->body_geomadr[body_id]; + int geom_count = model->body_geomnum[body_id]; + std::memcpy(target + start_geom * 4, value.data(), + sizeof(mjtNum) * 4 * geom_count); +} + +inline void CopyBodyGeomSlice4f(const mjModel* model, const float* src, + int body_id, std::vector* out) { + int start_geom = model->body_geomadr[body_id]; + int geom_count = model->body_geomnum[body_id]; + out->assign(src + start_geom * 4, src + (start_geom + geom_count) * 4); +} + +inline void RestoreBodyGeomSlice4f(mjModel* model, float* target, int body_id, + const std::vector& value) { + if (value.empty()) { + return; + } + int start_geom = model->body_geomadr[body_id]; + int geom_count = model->body_geomnum[body_id]; + std::memcpy(target + start_geom * 4, value.data(), + sizeof(float) * 4 * geom_count); +} + +inline void CopyBodyGeomSlice6(const mjModel* model, const mjtNum* src, + int body_id, std::vector* out) { + int start_geom = model->body_geomadr[body_id]; + int geom_count = model->body_geomnum[body_id]; + out->assign(src + start_geom * 6, src + (start_geom + geom_count) * 6); +} + +inline void RestoreBodyGeomSlice6(mjModel* model, mjtNum* target, int body_id, + const std::vector& value) { + if (value.empty()) { + return; + } + int start_geom = model->body_geomadr[body_id]; + int geom_count = model->body_geomnum[body_id]; + std::memcpy(target + start_geom * 6, value.data(), + sizeof(mjtNum) * 6 * geom_count); +} + +inline void CopyBodyGeomScalarSlice(const mjModel* model, const mjtNum* src, + int body_id, std::vector* out) { + int start_geom = model->body_geomadr[body_id]; + int geom_count = model->body_geomnum[body_id]; + out->assign(src + start_geom, src + start_geom + geom_count); +} + +inline void RestoreBodyGeomScalarSlice(mjModel* model, mjtNum* target, + int body_id, + const std::vector& value) { + if (value.empty()) { + return; + } + int start_geom = model->body_geomadr[body_id]; + int geom_count = model->body_geomnum[body_id]; + std::memcpy(target + start_geom, value.data(), sizeof(mjtNum) * geom_count); +} + +inline void CopyBodyGeomTypeSlice(const mjModel* model, int body_id, + std::vector* out) { + int start_geom = model->body_geomadr[body_id]; + int geom_count = model->body_geomnum[body_id]; + out->assign(model->geom_type + start_geom, + model->geom_type + start_geom + geom_count); +} + +inline void RestoreBodyGeomTypeSlice(mjModel* model, int body_id, + const std::vector& value) { + if (value.empty()) { + return; + } + int start_geom = model->body_geomadr[body_id]; + int geom_count = model->body_geomnum[body_id]; + std::memcpy(model->geom_type + start_geom, value.data(), + sizeof(int) * geom_count); +} + +inline std::vector ToIntVector(const std::vector& input) { + std::vector out(input.size()); + for (std::size_t i = 0; i < input.size(); ++i) { + out[i] = static_cast(input[i]); + } + return out; +} + +inline std::vector ToFloatVector(const std::vector& input) { + return {input.begin(), input.end()}; +} + +inline void SetSiteSuccessColor(mjModel* model, int site_id, bool success) { + if (site_id < 0) { + return; + } + model->site_rgba[site_id * 4 + 0] = success ? 0.0 : 2.0; + model->site_rgba[site_id * 4 + 1] = success ? 2.0 : 0.0; +} + +} // namespace challenge_detail + +class MyoChallengeReorientEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(5), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), "goal_pos_low"_.Bind(0.0), + "goal_pos_high"_.Bind(0.0), "goal_rot_low"_.Bind(0.0), + "goal_rot_high"_.Bind(0.0), "obj_size_change"_.Bind(0.0), + "obj_mass_low"_.Bind(0.108), "obj_mass_high"_.Bind(0.108), + "obj_friction_change"_.Bind(std::vector{0.0, 0.0, 0.0}), + "pos_th"_.Bind(0.025), "rot_th"_.Bind(0.262), "drop_th"_.Bind(0.2), + "reward_pos_dist_w"_.Bind(100.0), "reward_rot_dist_w"_.Bind(1.0), + "reward_bonus_w"_.Bind(0.0), "reward_act_reg_w"_.Bind(0.0), + "reward_penalty_w"_.Bind(0.0), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_act_dot"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{}), + "test_goal_body_pos"_.Bind(std::vector{}), + "test_goal_body_quat"_.Bind(std::vector{}), + "test_target_geom_size"_.Bind(std::vector{}), + "test_object_geom_size"_.Bind(std::vector{}), + "test_object_geom_pos"_.Bind(std::vector{}), + "test_object_geom_friction"_.Bind(std::vector{}), + "test_object_body_mass"_.Bind(std::vector{})); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:pos_dist"_.Bind(Spec({-1}, {-inf, inf})), + "info:rot_dist"_.Bind(Spec({-1}, {-inf, inf})), + "info:bonus"_.Bind(Spec({-1}, {-inf, inf})), + "info:act_reg"_.Bind(Spec({-1}, {-inf, inf})), + "info:penalty"_.Bind(Spec({-1}, {-inf, inf})), + "info:sparse"_.Bind(Spec({-1}, {-inf, inf})), + "info:solved"_.Bind(Spec({-1}, {0.0, 1.0})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:goal_pos"_.Bind(Spec({3})), + "info:goal_rot"_.Bind(Spec({3}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using ReorientPixelFns = PixelObservationEnvFns; +using MyoChallengeReorientEnvSpec = EnvSpec; +using MyoChallengeReorientPixelEnvSpec = EnvSpec; + +class MyoChallengeRelocateEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(5), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), + "target_xyz_low"_.Bind(std::vector{0.0, -0.35, 0.9}), + "target_xyz_high"_.Bind(std::vector{0.2, -0.1, 0.9}), + "target_rxryrz_low"_.Bind(std::vector{0.0, 0.0, 0.0}), + "target_rxryrz_high"_.Bind(std::vector{0.0, 0.0, 0.0}), + "obj_xyz_low"_.Bind(std::vector{}), + "obj_xyz_high"_.Bind(std::vector{}), + "obj_geom_low"_.Bind(std::vector{}), + "obj_geom_high"_.Bind(std::vector{}), "obj_mass_low"_.Bind(0.0), + "obj_mass_high"_.Bind(0.0), + "obj_friction_low"_.Bind(std::vector{}), + "obj_friction_high"_.Bind(std::vector{}), + "qpos_noise_range"_.Bind(0.0), "pos_th"_.Bind(0.025), + "rot_th"_.Bind(0.262), "drop_th"_.Bind(0.5), + "reward_pos_dist_w"_.Bind(100.0), "reward_rot_dist_w"_.Bind(1.0), + "reward_act_reg_w"_.Bind(0.0), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_ctrl"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_act_dot"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{}), + "test_goal_body_pos"_.Bind(std::vector{}), + "test_goal_body_quat"_.Bind(std::vector{}), + "test_goal_mocap_pos"_.Bind(std::vector{}), + "test_goal_mocap_quat"_.Bind(std::vector{}), + "test_object_body_pos"_.Bind(std::vector{}), + "test_object_body_mass"_.Bind(std::vector{}), + "test_object_geom_type"_.Bind(std::vector{}), + "test_object_geom_size"_.Bind(std::vector{}), + "test_object_geom_pos"_.Bind(std::vector{}), + "test_object_geom_quat"_.Bind(std::vector{}), + "test_object_geom_rgba"_.Bind(std::vector{}), + "test_object_geom_friction"_.Bind(std::vector{})); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:reach_dist"_.Bind(Spec({-1}, {-inf, inf})), + "info:pos_dist"_.Bind(Spec({-1}, {-inf, inf})), + "info:rot_dist"_.Bind(Spec({-1}, {-inf, inf})), + "info:act_reg"_.Bind(Spec({-1}, {-inf, inf})), + "info:sparse"_.Bind(Spec({-1}, {-inf, inf})), + "info:solved"_.Bind(Spec({-1}, {0.0, 1.0})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:goal_pos"_.Bind(Spec({3})), + "info:goal_rot"_.Bind(Spec({3}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using RelocatePixelFns = PixelObservationEnvFns; +using MyoChallengeRelocateEnvSpec = EnvSpec; +using MyoChallengeRelocatePixelEnvSpec = EnvSpec; + +class MyoChallengeBaodingEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(10), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), "drop_th"_.Bind(1.25), + "proximity_th"_.Bind(0.015), "goal_time_period_low"_.Bind(5.0), + "goal_time_period_high"_.Bind(5.0), "goal_xrange_low"_.Bind(0.025), + "goal_xrange_high"_.Bind(0.025), "goal_yrange_low"_.Bind(0.028), + "goal_yrange_high"_.Bind(0.028), + "task_choice"_.Bind(std::string("fixed")), "fixed_task"_.Bind(2), + "reward_pos_dist_1_w"_.Bind(5.0), "reward_pos_dist_2_w"_.Bind(5.0), + "obj_size_low"_.Bind(0.0), "obj_size_high"_.Bind(0.0), + "obj_mass_low"_.Bind(0.0), "obj_mass_high"_.Bind(0.0), + "obj_friction_low"_.Bind(std::vector{}), + "obj_friction_high"_.Bind(std::vector{}), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{}), + "test_task"_.Bind(-1), + "test_ball1_starting_angle"_.Bind( + std::numeric_limits::quiet_NaN()), + "test_ball2_starting_angle"_.Bind( + std::numeric_limits::quiet_NaN()), + "test_x_radius"_.Bind(std::numeric_limits::quiet_NaN()), + "test_y_radius"_.Bind(std::numeric_limits::quiet_NaN()), + "test_goal_trajectory"_.Bind(std::vector{}), + "test_target1_site_pos"_.Bind(std::vector{}), + "test_target2_site_pos"_.Bind(std::vector{}), + "test_object1_body_mass"_.Bind(std::vector{}), + "test_object2_body_mass"_.Bind(std::vector{}), + "test_object1_geom_size"_.Bind(std::vector{}), + "test_object2_geom_size"_.Bind(std::vector{}), + "test_object1_geom_friction"_.Bind(std::vector{}), + "test_object2_geom_friction"_.Bind(std::vector{})); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:pos_dist_1"_.Bind(Spec({-1}, {-inf, inf})), + "info:pos_dist_2"_.Bind(Spec({-1}, {-inf, inf})), + "info:act_reg"_.Bind(Spec({-1}, {-inf, inf})), + "info:sparse"_.Bind(Spec({-1}, {-inf, inf})), + "info:solved"_.Bind(Spec({-1}, {0.0, 1.0})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:target1_pos"_.Bind(Spec({3})), + "info:target2_pos"_.Bind(Spec({3}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using BaodingPixelFns = PixelObservationEnvFns; +using MyoChallengeBaodingEnvSpec = EnvSpec; +using MyoChallengeBaodingPixelEnvSpec = EnvSpec; + +class MyoChallengeBimanualEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(5), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), "proximity_th"_.Bind(0.17), + "start_center"_.Bind(std::vector{}), + "goal_center"_.Bind(std::vector{}), + "start_shifts"_.Bind(std::vector{}), + "goal_shifts"_.Bind(std::vector{}), + "reward_reach_dist_w"_.Bind(-0.1), "reward_act_w"_.Bind(0.0), + "reward_fin_dis_w"_.Bind(-0.5), "reward_pass_err_w"_.Bind(-1.0), + "obj_scale_change"_.Bind(std::vector{}), + "obj_mass_low"_.Bind(0.0), "obj_mass_high"_.Bind(0.0), + "obj_friction_low"_.Bind(std::vector{}), + "obj_friction_high"_.Bind(std::vector{}), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{}), + "test_start_pos"_.Bind(std::vector{}), + "test_goal_pos"_.Bind(std::vector{}), + "test_object_body_mass"_.Bind(std::vector{}), + "test_object_geom_size"_.Bind(std::vector{}), + "test_object_geom_friction"_.Bind(std::vector{})); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:reach_dist"_.Bind(Spec({-1}, {-inf, inf})), + "info:act"_.Bind(Spec({-1}, {-inf, inf})), + "info:fin_dis"_.Bind(Spec({-1}, {-inf, inf})), + "info:pass_err"_.Bind(Spec({-1}, {-inf, inf})), + "info:goal_dist"_.Bind(Spec({-1}, {-inf, inf})), + "info:solved"_.Bind(Spec({-1}, {0.0, 1.0})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:start_pos"_.Bind(Spec({3})), + "info:goal_pos"_.Bind(Spec({3}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using BimanualPixelFns = PixelObservationEnvFns; +using MyoChallengeBimanualEnvSpec = EnvSpec; +using MyoChallengeBimanualPixelEnvSpec = EnvSpec; + +template +class MyoChallengeReorientEnvBase : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::gen_; + using Base::spec_; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum pos_dist_term{0.0}; + mjtNum rot_dist_term{0.0}; + mjtNum bonus{0.0}; + mjtNum act_reg_term{0.0}; + mjtNum penalty{0.0}; + mjtNum sparse{0.0}; + bool solved{false}; + bool done{false}; + }; + + bool normalize_act_; + mjtNum goal_pos_low_; + mjtNum goal_pos_high_; + mjtNum goal_rot_low_; + mjtNum goal_rot_high_; + mjtNum obj_size_change_; + mjtNum obj_mass_low_; + mjtNum obj_mass_high_; + std::array obj_friction_change_; + mjtNum pos_th_; + mjtNum rot_th_; + mjtNum drop_th_; + mjtNum reward_pos_dist_w_; + mjtNum reward_rot_dist_w_; + mjtNum reward_bonus_w_; + mjtNum reward_act_reg_w_; + mjtNum reward_penalty_w_; + int object_sid_{-1}; + int goal_sid_{-1}; + int success_indicator_sid_{-1}; + int goal_bid_{-1}; + int target_gid_{-1}; + int object_bid_{-1}; + int object_gid0_{-1}; + int object_gidn_{-1}; + std::vector default_goal_body_pos_; + std::vector default_goal_body_quat_; + std::vector goal_init_pos_; + std::vector goal_obj_offset_; + std::vector target_default_size_; + std::vector object_default_size_; + std::vector object_default_pos_; + std::vector object_default_friction_; + mjtNum default_object_body_mass_{0.0}; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_act_; + std::vector test_reset_act_dot_; + std::vector test_reset_qacc_warmstart_; + std::vector test_goal_body_pos_; + std::vector test_goal_body_quat_; + std::vector test_target_geom_size_; + std::vector test_object_geom_size_; + std::vector test_object_geom_pos_; + std::vector test_object_geom_friction_; + std::vector test_object_body_mass_; + std::vector muscle_actuator_; + detail::MyoConditionState muscle_condition_state_; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoChallengeReorientEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + myosuite::MyoSuiteModelPath(spec.config["base_path"_], + spec.config["model_path"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + goal_pos_low_(spec.config["goal_pos_low"_]), + goal_pos_high_(spec.config["goal_pos_high"_]), + goal_rot_low_(spec.config["goal_rot_low"_]), + goal_rot_high_(spec.config["goal_rot_high"_]), + obj_size_change_(spec.config["obj_size_change"_]), + obj_mass_low_(spec.config["obj_mass_low"_]), + obj_mass_high_(spec.config["obj_mass_high"_]), + obj_friction_change_{ + static_cast(spec.config["obj_friction_change"_][0]), + static_cast(spec.config["obj_friction_change"_][1]), + static_cast(spec.config["obj_friction_change"_][2])}, + pos_th_(spec.config["pos_th"_]), + rot_th_(spec.config["rot_th"_]), + drop_th_(spec.config["drop_th"_]), + reward_pos_dist_w_(spec.config["reward_pos_dist_w"_]), + reward_rot_dist_w_(spec.config["reward_rot_dist_w"_]), + reward_bonus_w_(spec.config["reward_bonus_w"_]), + reward_act_reg_w_(spec.config["reward_act_reg_w"_]), + reward_penalty_w_(spec.config["reward_penalty_w"_]), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_act_dot_( + detail::ToMjtVector(spec.config["test_reset_act_dot"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])), + test_goal_body_pos_( + detail::ToMjtVector(spec.config["test_goal_body_pos"_])), + test_goal_body_quat_( + detail::ToMjtVector(spec.config["test_goal_body_quat"_])), + test_target_geom_size_( + detail::ToMjtVector(spec.config["test_target_geom_size"_])), + test_object_geom_size_( + detail::ToMjtVector(spec.config["test_object_geom_size"_])), + test_object_geom_pos_( + detail::ToMjtVector(spec.config["test_object_geom_pos"_])), + test_object_geom_friction_( + detail::ToMjtVector(spec.config["test_object_geom_friction"_])), + test_object_body_mass_( + detail::ToMjtVector(spec.config["test_object_body_mass"_])) { + ValidateConfig(); + CacheIds(); + detail::BuildMuscleMask(model_, &muscle_actuator_); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + PrimePersistentFatigueState(); + for (int i = 0; i < model_->nq - 7; ++i) { + data_->qpos[i] = 0.0; + } + data_->qpos[0] = static_cast(-1.5); + mj_forward(model_, data_); + CacheDefaultState(); + InitializeRobotEnv(); + ComputeRewardInfo(true); + } + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + if (!PreserveFatigueAcrossReset()) { + detail::ResetMyoConditionState(&muscle_condition_state_); + } + ResetToInitialState(); + RestoreModelState(); + ApplyModelRandomization(); + ApplyResetState(); + CaptureResetState(); + RewardInfo reward = ComputeRewardInfo(false); + WriteState(reward, true, 0.0); + } + + void Step(const Action& action) override { + const auto* raw = static_cast(action["action"_].Data()); + detail::ApplyMyoSuiteAction(model_, data_, muscle_actuator_, normalize_act_, + raw); + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + ++elapsed_step_; + RewardInfo reward = ComputeRewardInfo(true); + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, false, reward.dense_reward); + } + + private: + bool PreserveFatigueAcrossReset() const { + return muscle_condition_state_.muscle_condition == + detail::MuscleCondition::kFatigue; + } + + void PrimePersistentFatigueState() { + if (!PreserveFatigueAcrossReset()) { + return; + } + // Upstream ReorientEnvV0.reset() forwards reset_qpos positionally into + // BaseV0.reset(fatigue_reset=True, *args, **kwargs), so fatigue_reset sees + // None and the constructor-time zero-action warmup persists across resets. + std::vector muscle_ctrl; + muscle_ctrl.reserve(model_->nu); + for (int i = 0; i < model_->nu; ++i) { + if (muscle_actuator_[i]) { + muscle_ctrl.push_back(normalize_act_ ? detail::MuscleActivation(0.0) + : static_cast(0.0)); + } + } + muscle_condition_state_.fatigue_model.ComputeAct(&muscle_ctrl); + } + + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_] || + model_->nv != spec_.config["qvel_dim"_] || + model_->nu != spec_.config["action_dim"_] || + model_->na != spec_.config["act_dim"_]) { + throw std::runtime_error( + "MyoChallenge Reorient dims do not match model."); + } + int expected_obs = (model_->nq - 7) + (model_->nv - 6) + 18 + model_->na; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error( + "MyoChallenge Reorient obs_dim does not match model."); + } + } + + void CacheIds() { + object_sid_ = mj_name2id(model_, mjOBJ_SITE, "object_o"); + goal_sid_ = mj_name2id(model_, mjOBJ_SITE, "target_o"); + success_indicator_sid_ = mj_name2id(model_, mjOBJ_SITE, "target_ball"); + goal_bid_ = mj_name2id(model_, mjOBJ_BODY, "target"); + target_gid_ = mj_name2id(model_, mjOBJ_GEOM, "target_dice"); + object_bid_ = mj_name2id(model_, mjOBJ_BODY, "Object"); + if (object_sid_ == -1 || goal_sid_ == -1 || goal_bid_ == -1 || + target_gid_ == -1 || object_bid_ == -1) { + throw std::runtime_error("MyoChallenge Reorient ids missing."); + } + object_gid0_ = model_->body_geomadr[object_bid_]; + object_gidn_ = object_gid0_ + model_->body_geomnum[object_bid_]; + } + + void CacheDefaultState() { + detail::CopyModelBodyPos(model_, goal_bid_, &default_goal_body_pos_); + detail::CopyModelBodyQuat(model_, goal_bid_, &default_goal_body_quat_); + goal_init_pos_.resize(3); + goal_obj_offset_.resize(3); + detail::CopySitePos(model_, data_, goal_sid_, goal_init_pos_.data()); + std::array goal{}; + std::array obj{}; + detail::CopySitePos(model_, data_, goal_sid_, goal.data()); + detail::CopySitePos(model_, data_, object_sid_, obj.data()); + for (int axis = 0; axis < 3; ++axis) { + goal_obj_offset_[axis] = goal[axis] - obj[axis]; + } + detail::CopyModelGeomSize(model_, target_gid_, &target_default_size_); + challenge_detail::CopyBodyGeomSlice3(model_, model_->geom_size, object_bid_, + &object_default_size_); + challenge_detail::CopyBodyGeomSlice3(model_, model_->geom_pos, object_bid_, + &object_default_pos_); + challenge_detail::CopyBodyGeomSlice3( + model_, model_->geom_friction, object_bid_, &object_default_friction_); + detail::CopyModelBodyMass(model_, object_bid_, &default_object_body_mass_); + } + + void RestoreModelState() { + detail::RestoreModelBodyPos(model_, goal_bid_, default_goal_body_pos_); + detail::RestoreModelBodyQuat(model_, goal_bid_, default_goal_body_quat_); + challenge_detail::RestoreBodyGeomSlice3(model_, model_->geom_size, + object_bid_, object_default_size_); + challenge_detail::RestoreBodyGeomSlice3(model_, model_->geom_pos, + object_bid_, object_default_pos_); + challenge_detail::RestoreBodyGeomSlice3( + model_, model_->geom_friction, object_bid_, object_default_friction_); + detail::RestoreModelGeomSize(model_, target_gid_, target_default_size_); + detail::RestoreModelBodyMass(model_, object_bid_, + default_object_body_mass_); + } + + void ApplyGoalBodyRandomization() { + if (!test_goal_body_pos_.empty()) { + detail::RestoreModelBodyPos(model_, goal_bid_, test_goal_body_pos_); + } else { + auto pos = + challenge_detail::UniformVec3(&gen_, goal_pos_low_, goal_pos_high_); + for (int axis = 0; axis < 3; ++axis) { + model_->body_pos[goal_bid_ * 3 + axis] = + goal_init_pos_[axis] + pos[axis]; + } + } + if (!test_goal_body_quat_.empty()) { + detail::RestoreModelBodyQuat(model_, goal_bid_, test_goal_body_quat_); + } else { + auto goal_rot = + challenge_detail::UniformVec3(&gen_, goal_rot_low_, goal_rot_high_); + auto quat = challenge_detail::EulerXYZToQuat(goal_rot); + std::memcpy(model_->body_quat + goal_bid_ * 4, quat.data(), + sizeof(mjtNum) * 4); + } + } + + void ApplyObjectRandomization() { + if (!test_target_geom_size_.empty()) { + detail::RestoreModelGeomSize(model_, target_gid_, test_target_geom_size_); + } + if (!test_object_geom_size_.empty()) { + challenge_detail::RestoreBodyGeomSlice3( + model_, model_->geom_size, object_bid_, test_object_geom_size_); + } + if (!test_object_geom_pos_.empty()) { + challenge_detail::RestoreBodyGeomSlice3( + model_, model_->geom_pos, object_bid_, test_object_geom_pos_); + } + if (!test_object_geom_friction_.empty()) { + challenge_detail::RestoreBodyGeomSlice3(model_, model_->geom_friction, + object_bid_, + test_object_geom_friction_); + } + if (!test_object_body_mass_.empty()) { + detail::RestoreModelBodyMass(model_, object_bid_, + test_object_body_mass_[0]); + } + if (!test_target_geom_size_.empty() || !test_object_geom_size_.empty() || + !test_object_geom_pos_.empty() || !test_object_geom_friction_.empty() || + !test_object_body_mass_.empty()) { + return; + } + + model_->body_mass[object_bid_] = + challenge_detail::UniformScalar(&gen_, obj_mass_low_, obj_mass_high_); + std::array low = { + object_default_friction_[0] - obj_friction_change_[0], + object_default_friction_[1] - obj_friction_change_[1], + object_default_friction_[2] - obj_friction_change_[2], + }; + std::array high = { + object_default_friction_[0] + obj_friction_change_[0], + object_default_friction_[1] + obj_friction_change_[1], + object_default_friction_[2] + obj_friction_change_[2], + }; + for (int gid = object_gid0_; gid < object_gidn_; ++gid) { + auto friction = challenge_detail::UniformVec3(&gen_, low, high); + std::memcpy(model_->geom_friction + gid * 3, friction.data(), + sizeof(mjtNum) * 3); + } + + mjtNum del_size = challenge_detail::UniformScalar(&gen_, -obj_size_change_, + obj_size_change_); + for (int axis = 0; axis < 3; ++axis) { + model_->geom_size[target_gid_ * 3 + axis] = + target_default_size_[axis] + del_size; + } + for (int gid = object_gid0_; gid < object_gidn_ - 3; ++gid) { + int offset = gid * 3; + model_->geom_size[offset + 0] = + object_default_size_[(gid - object_gid0_) * 3]; + model_->geom_size[offset + 1] = + object_default_size_[(gid - object_gid0_) * 3 + 1] + del_size; + model_->geom_size[offset + 2] = + object_default_size_[(gid - object_gid0_) * 3 + 2]; + } + for (int gid = object_gidn_ - 3; gid < object_gidn_; ++gid) { + int rel = (gid - object_gid0_) * 3; + for (int axis = 0; axis < 3; ++axis) { + model_->geom_size[gid * 3 + axis] = + object_default_size_[rel + axis] + del_size; + } + } + for (int gid = object_gid0_; gid < object_gidn_; ++gid) { + for (int axis = 0; axis < 3; ++axis) { + int index = (gid - object_gid0_) * 3 + axis; + mjtNum value = object_default_pos_[index]; + mjtNum sign = value / std::abs(value + static_cast(1e-16)); + model_->geom_pos[gid * 3 + axis] = + sign * (std::abs(object_default_pos_[index]) + del_size); + } + } + } + + void ApplyModelRandomization() { + ApplyGoalBodyRandomization(); + ApplyObjectRandomization(); + } + + void ApplyResetState() { + if (!test_reset_qpos_.empty()) { + detail::RestoreVector(test_reset_qpos_, data_->qpos); + } + if (!test_reset_qvel_.empty()) { + detail::RestoreVector(test_reset_qvel_, data_->qvel); + } + mj_forward(model_, data_); + bool rerun_forward = false; + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + rerun_forward = true; + } + if (!test_reset_act_dot_.empty()) { + detail::RestoreVector(test_reset_act_dot_, data_->act_dot); + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, data_->qacc_warmstart); + } + if (rerun_forward) { + mj_forward(model_, data_); + } + // Official MyoSuite reset leaves dm_control Physics ready for the next + // legacy-step transition, i.e. with step1 fields such as act_dot + // refreshed. Without this, the first mj_step2 integrates stale actuator + // derivatives and only step 1 drifts. + mj_step1(model_, data_); + } + + std::vector Observation() const { + std::vector obs; + obs.reserve((model_->nq - 7) + (model_->nv - 6) + 18 + model_->na); + obs.insert(obs.end(), data_->qpos, data_->qpos + model_->nq - 7); + mjtNum dt = Dt(); + for (int i = 0; i < model_->nv - 6; ++i) { + obs.push_back(data_->qvel[i] * dt); + } + std::array obj_pos{}; + std::array goal_pos{}; + detail::CopySitePos(model_, data_, object_sid_, obj_pos.data()); + detail::CopySitePos(model_, data_, goal_sid_, goal_pos.data()); + auto pos_err = challenge_detail::Sub3(goal_pos, obj_pos); + for (int axis = 0; axis < 3; ++axis) { + pos_err[axis] -= goal_obj_offset_[axis]; + } + auto obj_rot = + challenge_detail::Mat9ToEuler(data_->site_xmat + object_sid_ * 9); + auto goal_rot = + challenge_detail::Mat9ToEuler(data_->site_xmat + goal_sid_ * 9); + auto rot_err = challenge_detail::Sub3(goal_rot, obj_rot); + obs.insert(obs.end(), obj_pos.begin(), obj_pos.end()); + obs.insert(obs.end(), goal_pos.begin(), goal_pos.end()); + obs.insert(obs.end(), pos_err.begin(), pos_err.end()); + obs.insert(obs.end(), obj_rot.begin(), obj_rot.end()); + obs.insert(obs.end(), goal_rot.begin(), goal_rot.end()); + obs.insert(obs.end(), rot_err.begin(), rot_err.end()); + if (model_->na > 0) { + obs.insert(obs.end(), data_->act, data_->act + model_->na); + } + return obs; + } + + RewardInfo ComputeRewardInfo(bool apply_visuals) { + std::array obj_pos{}; + std::array goal_pos{}; + detail::CopySitePos(model_, data_, object_sid_, obj_pos.data()); + detail::CopySitePos(model_, data_, goal_sid_, goal_pos.data()); + auto pos_err = challenge_detail::Sub3(goal_pos, obj_pos); + for (int axis = 0; axis < 3; ++axis) { + pos_err[axis] -= goal_obj_offset_[axis]; + } + auto obj_rot = + challenge_detail::Mat9ToEuler(data_->site_xmat + object_sid_ * 9); + auto goal_rot = + challenge_detail::Mat9ToEuler(data_->site_xmat + goal_sid_ * 9); + auto rot_err = challenge_detail::Sub3(goal_rot, obj_rot); + + mjtNum pos_dist = challenge_detail::Norm3(pos_err); + mjtNum rot_dist = challenge_detail::Norm3(rot_err); + mjtNum act_reg = detail::ActReg(model_, data_); + bool drop = pos_dist > drop_th_; + RewardInfo info; + info.pos_dist_term = -pos_dist; + info.rot_dist_term = -rot_dist; + info.bonus = static_cast(pos_dist < 2 * pos_th_) + + static_cast(pos_dist < pos_th_); + info.act_reg_term = -act_reg; + info.penalty = -static_cast(drop); + info.sparse = -rot_dist - static_cast(10.0) * pos_dist; + info.solved = pos_dist < pos_th_ && rot_dist < rot_th_ && !drop; + info.done = drop; + info.dense_reward = reward_pos_dist_w_ * info.pos_dist_term + + reward_rot_dist_w_ * info.rot_dist_term + + reward_bonus_w_ * info.bonus + + reward_act_reg_w_ * info.act_reg_term + + reward_penalty_w_ * info.penalty; + if (apply_visuals) { + challenge_detail::SetSiteSuccessColor(model_, success_indicator_sid_, + info.solved); + } + return info; + } + + void WriteState(const RewardInfo& reward, bool reset, float reward_value) { + auto obs = Observation(); + auto state = Allocate(); + if constexpr (!kFromPixels) { + AssignObservation("obs", &state["obs"_], obs.data(), obs.size(), reset); + } + state["reward"_] = reward_value; + state["discount"_] = 1.0f; + state["done"_] = done_; + state["trunc"_] = elapsed_step_ >= max_episode_steps_; + state["elapsed_step"_] = elapsed_step_; + state["info:pos_dist"_] = reward.pos_dist_term; + state["info:rot_dist"_] = reward.rot_dist_term; + state["info:bonus"_] = reward.bonus; + state["info:act_reg"_] = reward.act_reg_term; + state["info:penalty"_] = reward.penalty; + state["info:sparse"_] = reward.sparse; + state["info:solved"_] = static_cast(reward.solved); +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + std::array goal_pos{}; + detail::CopySitePos(model_, data_, goal_sid_, goal_pos.data()); + auto goal_rot = + challenge_detail::Mat9ToEuler(data_->site_xmat + goal_sid_ * 9); + state["info:goal_pos"_].Assign(goal_pos.data(), 3); + state["info:goal_rot"_].Assign(goal_rot.data(), 3); + if constexpr (kFromPixels) { + AssignPixelObservation("obs:pixels", &state["obs:pixels"_], reset); + } + } +}; + +template +class MyoChallengeRelocateEnvBase : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::gen_; + using Base::spec_; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum reach_dist_term{0.0}; + mjtNum pos_dist_term{0.0}; + mjtNum rot_dist_term{0.0}; + mjtNum act_reg_term{0.0}; + mjtNum sparse{0.0}; + bool solved{false}; + bool done{false}; + }; + + bool normalize_act_; + std::array target_xyz_low_{}; + std::array target_xyz_high_{}; + std::array target_rxryrz_low_{}; + std::array target_rxryrz_high_{}; + std::array obj_xyz_low_{}; + std::array obj_xyz_high_{}; + std::array obj_geom_low_{}; + std::array obj_geom_high_{}; + std::array obj_friction_low_{}; + std::array obj_friction_high_{}; + bool has_obj_xyz_range_{false}; + bool has_obj_geom_range_{false}; + bool has_obj_mass_range_{false}; + bool has_obj_friction_range_{false}; + mjtNum obj_mass_low_{0.0}; + mjtNum obj_mass_high_{0.0}; + mjtNum qpos_noise_range_{0.0}; + mjtNum pos_th_; + mjtNum rot_th_; + mjtNum drop_th_; + mjtNum reward_pos_dist_w_; + mjtNum reward_rot_dist_w_; + mjtNum reward_act_reg_w_; + int palm_sid_{-1}; + int object_sid_{-1}; + int object_bid_{-1}; + int goal_sid_{-1}; + int success_indicator_sid_{-1}; + int goal_bid_{-1}; + int goal_mocap_id_{-1}; + std::vector initial_qpos_override_; + std::vector default_goal_body_pos_; + std::vector default_goal_body_quat_; + std::vector default_object_body_pos_; + mjtNum default_object_body_mass_{0.0}; + std::vector default_object_geom_size_; + std::vector default_object_geom_pos_; + std::vector default_object_geom_quat_; + std::vector default_object_geom_aabb_; + std::vector default_object_geom_rbound_; + std::vector default_object_geom_rgba_; + std::vector default_object_geom_friction_; + std::vector default_object_geom_type_; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_ctrl_; + std::vector test_reset_act_; + std::vector test_reset_act_dot_; + std::vector test_reset_qacc_warmstart_; + std::vector test_goal_body_pos_; + std::vector test_goal_body_quat_; + std::vector test_goal_mocap_pos_; + std::vector test_goal_mocap_quat_; + std::vector test_object_body_pos_; + std::vector test_object_body_mass_; + std::vector test_object_geom_type_; + std::vector test_object_geom_size_; + std::vector test_object_geom_pos_; + std::vector test_object_geom_quat_; + std::vector test_object_geom_rgba_; + std::vector test_object_geom_friction_; + std::vector muscle_actuator_; + detail::MyoConditionState muscle_condition_state_; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoChallengeRelocateEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + myosuite::MyoSuiteModelPath(spec.config["base_path"_], + spec.config["model_path"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + target_xyz_low_(ToArray3(spec.config["target_xyz_low"_])), + target_xyz_high_(ToArray3(spec.config["target_xyz_high"_])), + target_rxryrz_low_(ToArray3(spec.config["target_rxryrz_low"_])), + target_rxryrz_high_(ToArray3(spec.config["target_rxryrz_high"_])), + pos_th_(spec.config["pos_th"_]), + rot_th_(spec.config["rot_th"_]), + drop_th_(spec.config["drop_th"_]), + reward_pos_dist_w_(spec.config["reward_pos_dist_w"_]), + reward_rot_dist_w_(spec.config["reward_rot_dist_w"_]), + reward_act_reg_w_(spec.config["reward_act_reg_w"_]), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_ctrl_(detail::ToMjtVector(spec.config["test_reset_ctrl"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_act_dot_( + detail::ToMjtVector(spec.config["test_reset_act_dot"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])), + test_goal_body_pos_( + detail::ToMjtVector(spec.config["test_goal_body_pos"_])), + test_goal_body_quat_( + detail::ToMjtVector(spec.config["test_goal_body_quat"_])), + test_goal_mocap_pos_( + detail::ToMjtVector(spec.config["test_goal_mocap_pos"_])), + test_goal_mocap_quat_( + detail::ToMjtVector(spec.config["test_goal_mocap_quat"_])), + test_object_body_pos_( + detail::ToMjtVector(spec.config["test_object_body_pos"_])), + test_object_body_mass_( + detail::ToMjtVector(spec.config["test_object_body_mass"_])), + test_object_geom_type_(challenge_detail::ToIntVector( + spec.config["test_object_geom_type"_])), + test_object_geom_size_( + detail::ToMjtVector(spec.config["test_object_geom_size"_])), + test_object_geom_pos_( + detail::ToMjtVector(spec.config["test_object_geom_pos"_])), + test_object_geom_quat_( + detail::ToMjtVector(spec.config["test_object_geom_quat"_])), + test_object_geom_rgba_(challenge_detail::ToFloatVector( + spec.config["test_object_geom_rgba"_])), + test_object_geom_friction_( + detail::ToMjtVector(spec.config["test_object_geom_friction"_])) { + ParseOptionalRanges(spec); + ValidateConfig(); + CacheIds(); + detail::BuildMuscleMask(model_, &muscle_actuator_); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + int key_frame_id = has_obj_xyz_range_ ? 1 : 0; + initial_qpos_override_.assign( + model_->key_qpos + key_frame_id * model_->nq, + model_->key_qpos + (key_frame_id + 1) * model_->nq); + detail::RestoreVector(initial_qpos_override_, data_->qpos); + mju_zero(data_->qvel, model_->nv); + mj_forward(model_, data_); + CacheDefaultState(); + InitializeRobotEnv(); + InitializeSuccessIndicatorState(); + } + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + detail::ResetMyoConditionState(&muscle_condition_state_); + for (int attempt = 0; attempt < 16; ++attempt) { + ResetToInitialState(); + RestoreModelState(); + ApplyModelRandomization(); + ApplyResetState(); + if (data_->ncon == 0 || !test_reset_qpos_.empty()) { + break; + } + } + CaptureResetState(); + RewardInfo reward = ComputeRewardInfo(false); + WriteState(reward, true, 0.0); + } + + void Step(const Action& action) override { + const auto* raw = static_cast(action["action"_].Data()); + detail::ApplyMyoSuiteAction(model_, data_, muscle_actuator_, normalize_act_, + raw); + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + ++elapsed_step_; + RewardInfo reward = ComputeRewardInfo(true); + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, false, reward.dense_reward); + } + + private: + void InitializeSuccessIndicatorState() { + challenge_detail::SetSiteSuccessColor(model_, success_indicator_sid_, true); + if (success_indicator_sid_ >= 0) { + for (int axis = 0; axis < 3; ++axis) { + model_->site_size[success_indicator_sid_ * 3 + axis] = + static_cast(0.25); + } + } + } + + static std::array ToArray3(const std::vector& value) { + if (value.size() != 3) { + throw std::runtime_error("Expected a length-3 vector."); + } + return {static_cast(value[0]), static_cast(value[1]), + static_cast(value[2])}; + } + + void ParseOptionalRanges(const Spec& spec) { + auto load_range = [](const std::vector& low_vec, + const std::vector& high_vec, bool* has_range, + std::array* low_out, + std::array* high_out) { + *has_range = !low_vec.empty(); + if (*has_range) { + *low_out = ToArray3(low_vec); + *high_out = ToArray3(high_vec); + } + }; + load_range(spec.config["obj_xyz_low"_], spec.config["obj_xyz_high"_], + &has_obj_xyz_range_, &obj_xyz_low_, &obj_xyz_high_); + load_range(spec.config["obj_geom_low"_], spec.config["obj_geom_high"_], + &has_obj_geom_range_, &obj_geom_low_, &obj_geom_high_); + load_range(spec.config["obj_friction_low"_], + spec.config["obj_friction_high"_], &has_obj_friction_range_, + &obj_friction_low_, &obj_friction_high_); + has_obj_mass_range_ = + spec.config["obj_mass_high"_] > spec.config["obj_mass_low"_]; + obj_mass_low_ = spec.config["obj_mass_low"_]; + obj_mass_high_ = spec.config["obj_mass_high"_]; + qpos_noise_range_ = spec.config["qpos_noise_range"_]; + } + + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_] || + model_->nv != spec_.config["qvel_dim"_] || + model_->nu != spec_.config["action_dim"_] || + model_->na != spec_.config["act_dim"_]) { + throw std::runtime_error( + "MyoChallenge Relocate dims do not match model."); + } + int expected_obs = (model_->nq - 7) + (model_->nv - 6) + 18 + model_->na; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error( + "MyoChallenge Relocate obs_dim does not match model."); + } + if (!test_reset_ctrl_.empty() && + static_cast(test_reset_ctrl_.size()) != model_->nu) { + throw std::runtime_error( + "MyoChallenge Relocate test_reset_ctrl has wrong length."); + } + if (!test_reset_act_.empty() && + static_cast(test_reset_act_.size()) != model_->na) { + throw std::runtime_error( + "MyoChallenge Relocate test_reset_act has wrong length."); + } + if (!test_reset_act_dot_.empty() && + static_cast(test_reset_act_dot_.size()) != model_->na) { + throw std::runtime_error( + "MyoChallenge Relocate test_reset_act_dot has wrong length."); + } + if (!test_reset_qacc_warmstart_.empty() && + static_cast(test_reset_qacc_warmstart_.size()) != model_->nv) { + throw std::runtime_error( + "MyoChallenge Relocate test_reset_qacc_warmstart has wrong length."); + } + } + + void CacheIds() { + palm_sid_ = mj_name2id(model_, mjOBJ_SITE, "S_grasp"); + object_sid_ = mj_name2id(model_, mjOBJ_SITE, "object_o"); + object_bid_ = mj_name2id(model_, mjOBJ_BODY, "Object"); + goal_sid_ = mj_name2id(model_, mjOBJ_SITE, "target_o"); + success_indicator_sid_ = mj_name2id(model_, mjOBJ_SITE, "target_ball"); + goal_bid_ = mj_name2id(model_, mjOBJ_BODY, "target"); + goal_mocap_id_ = goal_bid_ >= 0 ? model_->body_mocapid[goal_bid_] : -1; + if (palm_sid_ == -1 || object_sid_ == -1 || object_bid_ == -1 || + goal_sid_ == -1 || goal_bid_ == -1) { + throw std::runtime_error("MyoChallenge Relocate ids missing."); + } + } + + void SetGoalPose(const std::vector& body_pos, + const std::vector& body_quat) { + detail::RestoreModelBodyPos(model_, goal_bid_, body_pos); + detail::RestoreModelBodyQuat(model_, goal_bid_, body_quat); + if (goal_mocap_id_ >= 0) { + std::memcpy(data_->mocap_pos + goal_mocap_id_ * 3, body_pos.data(), + sizeof(mjtNum) * 3); + std::memcpy(data_->mocap_quat + goal_mocap_id_ * 4, body_quat.data(), + sizeof(mjtNum) * 4); + } + } + + void CacheDefaultState() { + detail::CopyModelBodyPos(model_, goal_bid_, &default_goal_body_pos_); + detail::CopyModelBodyQuat(model_, goal_bid_, &default_goal_body_quat_); + detail::CopyModelBodyPos(model_, object_bid_, &default_object_body_pos_); + detail::CopyModelBodyMass(model_, object_bid_, &default_object_body_mass_); + challenge_detail::CopyBodyGeomSlice3(model_, model_->geom_size, object_bid_, + &default_object_geom_size_); + challenge_detail::CopyBodyGeomSlice3(model_, model_->geom_pos, object_bid_, + &default_object_geom_pos_); + challenge_detail::CopyBodyGeomSlice4(model_, model_->geom_quat, object_bid_, + &default_object_geom_quat_); + challenge_detail::CopyBodyGeomSlice6(model_, model_->geom_aabb, object_bid_, + &default_object_geom_aabb_); + challenge_detail::CopyBodyGeomScalarSlice( + model_, model_->geom_rbound, object_bid_, &default_object_geom_rbound_); + challenge_detail::CopyBodyGeomSlice4f( + model_, model_->geom_rgba, object_bid_, &default_object_geom_rgba_); + challenge_detail::CopyBodyGeomSlice3(model_, model_->geom_friction, + object_bid_, + &default_object_geom_friction_); + challenge_detail::CopyBodyGeomTypeSlice(model_, object_bid_, + &default_object_geom_type_); + } + + void RestoreModelState() { + SetGoalPose(default_goal_body_pos_, default_goal_body_quat_); + detail::RestoreModelBodyPos(model_, object_bid_, default_object_body_pos_); + detail::RestoreModelBodyMass(model_, object_bid_, + default_object_body_mass_); + challenge_detail::RestoreBodyGeomSlice3( + model_, model_->geom_size, object_bid_, default_object_geom_size_); + challenge_detail::RestoreBodyGeomSlice3( + model_, model_->geom_pos, object_bid_, default_object_geom_pos_); + challenge_detail::RestoreBodyGeomSlice4( + model_, model_->geom_quat, object_bid_, default_object_geom_quat_); + challenge_detail::RestoreBodyGeomSlice6( + model_, model_->geom_aabb, object_bid_, default_object_geom_aabb_); + challenge_detail::RestoreBodyGeomScalarSlice( + model_, model_->geom_rbound, object_bid_, default_object_geom_rbound_); + challenge_detail::RestoreBodyGeomSlice4f( + model_, model_->geom_rgba, object_bid_, default_object_geom_rgba_); + challenge_detail::RestoreBodyGeomSlice3(model_, model_->geom_friction, + object_bid_, + default_object_geom_friction_); + challenge_detail::RestoreBodyGeomTypeSlice(model_, object_bid_, + default_object_geom_type_); + } + + void ApplyModelRandomization() { + std::vector goal_body_pos = default_goal_body_pos_; + std::vector goal_body_quat = default_goal_body_quat_; + if (!test_goal_body_pos_.empty()) { + goal_body_pos = test_goal_body_pos_; + } else { + auto goal_pos = challenge_detail::UniformVec3(&gen_, target_xyz_low_, + target_xyz_high_); + goal_body_pos.assign(goal_pos.begin(), goal_pos.end()); + } + if (!test_goal_body_quat_.empty()) { + goal_body_quat = test_goal_body_quat_; + } else { + auto goal_rot = challenge_detail::UniformVec3(&gen_, target_rxryrz_low_, + target_rxryrz_high_); + auto quat = challenge_detail::EulerXYZToQuat(goal_rot); + goal_body_quat.assign(quat.begin(), quat.end()); + } + SetGoalPose(goal_body_pos, goal_body_quat); + if (goal_mocap_id_ >= 0) { + if (test_goal_mocap_pos_.size() == 3) { + std::memcpy(data_->mocap_pos + goal_mocap_id_ * 3, + test_goal_mocap_pos_.data(), sizeof(mjtNum) * 3); + } + if (test_goal_mocap_quat_.size() == 4) { + std::memcpy(data_->mocap_quat + goal_mocap_id_ * 4, + test_goal_mocap_quat_.data(), sizeof(mjtNum) * 4); + } + } + + if (!test_object_body_pos_.empty()) { + detail::RestoreModelBodyPos(model_, object_bid_, test_object_body_pos_); + } else if (has_obj_xyz_range_) { + auto body_pos = + challenge_detail::UniformVec3(&gen_, obj_xyz_low_, obj_xyz_high_); + std::memcpy(model_->body_pos + object_bid_ * 3, body_pos.data(), + sizeof(mjtNum) * 3); + } + + if (!test_object_geom_type_.empty()) { + challenge_detail::RestoreBodyGeomTypeSlice(model_, object_bid_, + test_object_geom_type_); + } + if (!test_object_geom_size_.empty()) { + challenge_detail::RestoreBodyGeomSlice3( + model_, model_->geom_size, object_bid_, test_object_geom_size_); + if (has_obj_geom_range_) { + int start_geom = model_->body_geomadr[object_bid_]; + int geom_count = model_->body_geomnum[object_bid_]; + mjtNum max_size = + std::max({obj_geom_high_[0], obj_geom_high_[1], obj_geom_high_[2]}); + for (int i = 0; i < geom_count; ++i) { + int gid = start_geom + i; + for (int axis = 0; axis < 3; ++axis) { + model_->geom_aabb[gid * 6 + 3 + axis] = obj_geom_high_[axis]; + } + model_->geom_rbound[gid] = static_cast(2.0) * max_size; + } + } + } + if (!test_object_geom_pos_.empty()) { + challenge_detail::RestoreBodyGeomSlice3( + model_, model_->geom_pos, object_bid_, test_object_geom_pos_); + } + if (!test_object_geom_quat_.empty()) { + challenge_detail::RestoreBodyGeomSlice4( + model_, model_->geom_quat, object_bid_, test_object_geom_quat_); + } + if (!test_object_geom_rgba_.empty()) { + challenge_detail::RestoreBodyGeomSlice4f( + model_, model_->geom_rgba, object_bid_, test_object_geom_rgba_); + } + if (!test_object_geom_friction_.empty()) { + challenge_detail::RestoreBodyGeomSlice3(model_, model_->geom_friction, + object_bid_, + test_object_geom_friction_); + } + if (!test_object_body_mass_.empty()) { + detail::RestoreModelBodyMass(model_, object_bid_, + test_object_body_mass_[0]); + } + + bool using_test_override = + !test_object_geom_type_.empty() || !test_object_geom_size_.empty() || + !test_object_geom_pos_.empty() || !test_object_geom_quat_.empty() || + !test_object_geom_rgba_.empty() || !test_object_geom_friction_.empty(); + if (!using_test_override && has_obj_geom_range_) { + int start_geom = model_->body_geomadr[object_bid_]; + int geom_count = model_->body_geomnum[object_bid_]; + constexpr std::array geom_types = {2, 3, 4, 5, 6}; + const auto half_pi_value = static_cast(1.5707963267948966); + std::uniform_int_distribution type_dist( + 0, static_cast(geom_types.size() - 1)); + std::array half_pi = { + -half_pi_value, + -half_pi_value, + -half_pi_value, + }; + std::array pos_half_pi = { + half_pi_value, + half_pi_value, + half_pi_value, + }; + mjtNum max_size = + std::max({obj_geom_high_[0], obj_geom_high_[1], obj_geom_high_[2]}); + for (int i = 0; i < geom_count; ++i) { + int gid = start_geom + i; + model_->geom_type[gid] = geom_types[type_dist(gen_)]; + auto geom_size = + challenge_detail::UniformVec3(&gen_, obj_geom_low_, obj_geom_high_); + std::memcpy(model_->geom_size + gid * 3, geom_size.data(), + sizeof(mjtNum) * 3); + for (int axis = 0; axis < 3; ++axis) { + model_->geom_aabb[gid * 6 + 3 + axis] = obj_geom_high_[axis]; + } + model_->geom_rbound[gid] = static_cast(2.0) * max_size; + std::array pos_low = {-geom_size[0], -geom_size[1], + -geom_size[2]}; + auto geom_pos = + challenge_detail::UniformVec3(&gen_, pos_low, geom_size); + std::memcpy(model_->geom_pos + gid * 3, geom_pos.data(), + sizeof(mjtNum) * 3); + auto geom_euler = + challenge_detail::UniformVec3(&gen_, half_pi, pos_half_pi); + auto geom_quat = challenge_detail::EulerXYZToQuat(geom_euler); + std::memcpy(model_->geom_quat + gid * 4, geom_quat.data(), + sizeof(mjtNum) * 4); + std::array rgba_low = {0.2, 0.2, 0.2, 1.0}; + std::array rgba_high = {0.9, 0.9, 0.9, 1.0}; + for (int axis = 0; axis < 4; ++axis) { + std::uniform_real_distribution dist(rgba_low[axis], + rgba_high[axis]); + model_->geom_rgba[gid * 4 + axis] = static_cast(dist(gen_)); + } + if (has_obj_friction_range_) { + auto friction = challenge_detail::UniformVec3( + &gen_, obj_friction_low_, obj_friction_high_); + std::memcpy(model_->geom_friction + gid * 3, friction.data(), + sizeof(mjtNum) * 3); + } + } + } + if (!test_object_body_mass_.empty()) { + detail::RestoreModelBodyMass(model_, object_bid_, + test_object_body_mass_[0]); + } else if (has_obj_mass_range_) { + model_->body_mass[object_bid_] = + challenge_detail::UniformScalar(&gen_, obj_mass_low_, obj_mass_high_); + } + } + + void ApplyResetState() { + bool used_override_qpos = false; + if (!test_reset_qpos_.empty()) { + detail::RestoreVector(test_reset_qpos_, data_->qpos); + used_override_qpos = true; + } else if (qpos_noise_range_ != 0.0) { + std::vector qpos = initial_qpos_override_; + for (int joint_id = 0; joint_id < model_->njnt; ++joint_id) { + int qposadr = model_->jnt_qposadr[joint_id]; + if (qposadr < model_->nq - 6) { + qpos[qposadr] += + qpos_noise_range_ * (model_->jnt_range[joint_id * 2 + 1] - + model_->jnt_range[joint_id * 2]); + } + } + for (int i = model_->nq - 6; i < model_->nq; ++i) { + qpos[i] = initial_qpos_override_[i]; + } + detail::RestoreVector(qpos, data_->qpos); + } + if (!test_reset_qvel_.empty()) { + detail::RestoreVector(test_reset_qvel_, data_->qvel); + } + if (!used_override_qpos && test_reset_qvel_.empty()) { + mju_zero(data_->qvel, model_->nv); + } + mj_forward(model_, data_); + bool rerun_forward = false; + if (!test_reset_ctrl_.empty()) { + detail::RestoreVector(test_reset_ctrl_, data_->ctrl); + rerun_forward = true; + } + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + rerun_forward = true; + } + if (!test_reset_act_dot_.empty()) { + detail::RestoreVector(test_reset_act_dot_, data_->act_dot); + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, data_->qacc_warmstart); + } + if (rerun_forward) { + mj_forward(model_, data_); + } + // Official MyoSuite reset leaves dm_control Physics ready for the next + // legacy-step transition. Mirror that here so the first mj_step2 consumes + // fresh activation derivatives instead of stale reset leftovers. + mj_step1(model_, data_); + } + + std::vector Observation() const { + std::vector obs; + obs.reserve((model_->nq - 7) + (model_->nv - 6) + 18 + model_->na); + obs.insert(obs.end(), data_->qpos, data_->qpos + model_->nq - 7); + mjtNum dt = Dt(); + for (int i = 0; i < model_->nv - 6; ++i) { + obs.push_back(data_->qvel[i] * dt); + } + std::array obj_pos{}; + std::array goal_pos{}; + detail::CopySitePos(model_, data_, object_sid_, obj_pos.data()); + detail::CopySitePos(model_, data_, goal_sid_, goal_pos.data()); + auto pos_err = challenge_detail::Sub3(goal_pos, obj_pos); + auto obj_rot = + challenge_detail::Mat9ToEuler(data_->site_xmat + object_sid_ * 9); + auto goal_rot = + challenge_detail::Mat9ToEuler(data_->site_xmat + goal_sid_ * 9); + auto rot_err = challenge_detail::Sub3(goal_rot, obj_rot); + obs.insert(obs.end(), obj_pos.begin(), obj_pos.end()); + obs.insert(obs.end(), goal_pos.begin(), goal_pos.end()); + obs.insert(obs.end(), pos_err.begin(), pos_err.end()); + obs.insert(obs.end(), obj_rot.begin(), obj_rot.end()); + obs.insert(obs.end(), goal_rot.begin(), goal_rot.end()); + obs.insert(obs.end(), rot_err.begin(), rot_err.end()); + if (model_->na > 0) { + obs.insert(obs.end(), data_->act, data_->act + model_->na); + } + return obs; + } + + RewardInfo ComputeRewardInfo(bool apply_visuals) { + std::array palm_pos{}; + std::array obj_pos{}; + std::array goal_pos{}; + detail::CopySitePos(model_, data_, palm_sid_, palm_pos.data()); + detail::CopySitePos(model_, data_, object_sid_, obj_pos.data()); + detail::CopySitePos(model_, data_, goal_sid_, goal_pos.data()); + auto reach_err = challenge_detail::Sub3(palm_pos, obj_pos); + auto pos_err = challenge_detail::Sub3(goal_pos, obj_pos); + auto obj_rot = + challenge_detail::Mat9ToEuler(data_->site_xmat + object_sid_ * 9); + auto goal_rot = + challenge_detail::Mat9ToEuler(data_->site_xmat + goal_sid_ * 9); + auto rot_err = challenge_detail::Sub3(goal_rot, obj_rot); + + mjtNum reach_dist = challenge_detail::Norm3(reach_err); + mjtNum pos_dist = challenge_detail::Norm3(pos_err); + mjtNum rot_dist = challenge_detail::Norm3(rot_err); + mjtNum act_reg = detail::ActReg(model_, data_); + bool drop = reach_dist > drop_th_; + RewardInfo info; + info.reach_dist_term = -reach_dist; + info.pos_dist_term = -pos_dist; + info.rot_dist_term = -rot_dist; + info.act_reg_term = -act_reg; + info.sparse = -rot_dist - static_cast(10.0) * pos_dist; + info.solved = pos_dist < pos_th_ && rot_dist < rot_th_ && !drop; + info.done = drop; + info.dense_reward = reward_pos_dist_w_ * info.pos_dist_term + + reward_rot_dist_w_ * info.rot_dist_term + + reward_act_reg_w_ * info.act_reg_term; + if (apply_visuals) { + challenge_detail::SetSiteSuccessColor(model_, success_indicator_sid_, + info.solved); + } + if (apply_visuals && success_indicator_sid_ >= 0) { + mjtNum size = + info.solved ? static_cast(0.25) : static_cast(0.1); + model_->site_size[success_indicator_sid_ * 3 + 0] = size; + model_->site_size[success_indicator_sid_ * 3 + 1] = size; + model_->site_size[success_indicator_sid_ * 3 + 2] = size; + } + return info; + } + + void WriteState(const RewardInfo& reward, bool reset, float reward_value) { + auto obs = Observation(); + auto state = Allocate(); + if constexpr (!kFromPixels) { + AssignObservation("obs", &state["obs"_], obs.data(), obs.size(), reset); + } + state["reward"_] = reward_value; + state["discount"_] = 1.0f; + state["done"_] = done_; + state["trunc"_] = elapsed_step_ >= max_episode_steps_; + state["elapsed_step"_] = elapsed_step_; + state["info:reach_dist"_] = reward.reach_dist_term; + state["info:pos_dist"_] = reward.pos_dist_term; + state["info:rot_dist"_] = reward.rot_dist_term; + state["info:act_reg"_] = reward.act_reg_term; + state["info:sparse"_] = reward.sparse; + state["info:solved"_] = static_cast(reward.solved); +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + std::array goal_pos{}; + detail::CopySitePos(model_, data_, goal_sid_, goal_pos.data()); + auto goal_rot = + challenge_detail::Mat9ToEuler(data_->site_xmat + goal_sid_ * 9); + state["info:goal_pos"_].Assign(goal_pos.data(), 3); + state["info:goal_rot"_].Assign(goal_rot.data(), 3); + if constexpr (kFromPixels) { + AssignPixelObservation("obs:pixels", &state["obs:pixels"_], reset); + } + } +}; + +template +class MyoChallengeBaodingEnvBase : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::gen_; + using Base::spec_; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum pos_dist_1_term{0.0}; + mjtNum pos_dist_2_term{0.0}; + mjtNum act_reg_term{0.0}; + mjtNum sparse{0.0}; + bool solved{false}; + bool done{false}; + }; + + static constexpr mjtNum kCenterX = static_cast(-0.0125); + static constexpr mjtNum kCenterY = static_cast(-0.07); + + bool normalize_act_; + mjtNum drop_th_; + mjtNum proximity_th_; + mjtNum goal_time_period_low_; + mjtNum goal_time_period_high_; + mjtNum goal_xrange_low_; + mjtNum goal_xrange_high_; + mjtNum goal_yrange_low_; + mjtNum goal_yrange_high_; + std::string task_choice_; + int fixed_task_; + mjtNum reward_pos_dist_1_w_; + mjtNum reward_pos_dist_2_w_; + mjtNum obj_size_low_; + mjtNum obj_size_high_; + mjtNum obj_mass_low_; + mjtNum obj_mass_high_; + std::array obj_friction_low_{0.0, 0.0, 0.0}; + std::array obj_friction_high_{0.0, 0.0, 0.0}; + bool has_obj_size_range_{false}; + bool has_obj_mass_range_{false}; + bool has_obj_friction_range_{false}; + int object1_bid_{-1}; + int object2_bid_{-1}; + int object1_sid_{-1}; + int object2_sid_{-1}; + int object1_gid_{-1}; + int object2_gid_{-1}; + int target1_sid_{-1}; + int target2_sid_{-1}; + std::vector default_target1_site_pos_; + std::vector default_target2_site_pos_; + std::vector default_object1_geom_size_; + std::vector default_object2_geom_size_; + std::vector default_object1_geom_friction_; + std::vector default_object2_geom_friction_; + std::vector default_object1_geom_rgba_; + std::vector default_object2_geom_rgba_; + mjtNum default_object1_body_mass_{0.0}; + mjtNum default_object2_body_mass_{0.0}; + std::vector muscle_actuator_; + int counter_{0}; + int current_task_{2}; + mjtNum x_radius_{0.025}; + mjtNum y_radius_{0.028}; + mjtNum goal_time_period_{5.0}; + mjtNum ball1_starting_angle_{detail::kPi / static_cast(4.0)}; + mjtNum ball2_starting_angle_{-detail::kPi * static_cast(3.0 / 4.0)}; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_act_; + std::vector test_reset_qacc_warmstart_; + int test_task_; + mjtNum test_ball1_starting_angle_; + mjtNum test_ball2_starting_angle_; + mjtNum test_x_radius_; + mjtNum test_y_radius_; + std::vector test_goal_trajectory_; + std::vector test_target1_site_pos_; + std::vector test_target2_site_pos_; + std::vector test_object1_body_mass_; + std::vector test_object2_body_mass_; + std::vector test_object1_geom_size_; + std::vector test_object2_geom_size_; + std::vector test_object1_geom_friction_; + std::vector test_object2_geom_friction_; + detail::MyoConditionState muscle_condition_state_; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoChallengeBaodingEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + myosuite::MyoSuiteModelPath(spec.config["base_path"_], + spec.config["model_path"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + drop_th_(spec.config["drop_th"_]), + proximity_th_(spec.config["proximity_th"_]), + goal_time_period_low_(spec.config["goal_time_period_low"_]), + goal_time_period_high_(spec.config["goal_time_period_high"_]), + goal_xrange_low_(spec.config["goal_xrange_low"_]), + goal_xrange_high_(spec.config["goal_xrange_high"_]), + goal_yrange_low_(spec.config["goal_yrange_low"_]), + goal_yrange_high_(spec.config["goal_yrange_high"_]), + task_choice_(spec.config["task_choice"_]), + fixed_task_(spec.config["fixed_task"_]), + reward_pos_dist_1_w_(spec.config["reward_pos_dist_1_w"_]), + reward_pos_dist_2_w_(spec.config["reward_pos_dist_2_w"_]), + obj_size_low_(spec.config["obj_size_low"_]), + obj_size_high_(spec.config["obj_size_high"_]), + obj_mass_low_(spec.config["obj_mass_low"_]), + obj_mass_high_(spec.config["obj_mass_high"_]), + has_obj_size_range_(spec.config["obj_size_low"_] != 0.0 || + spec.config["obj_size_high"_] != 0.0), + has_obj_mass_range_(spec.config["obj_mass_low"_] != 0.0 || + spec.config["obj_mass_high"_] != 0.0), + has_obj_friction_range_(!spec.config["obj_friction_low"_].empty() && + !spec.config["obj_friction_high"_].empty()), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])), + test_task_(spec.config["test_task"_]), + test_ball1_starting_angle_(spec.config["test_ball1_starting_angle"_]), + test_ball2_starting_angle_(spec.config["test_ball2_starting_angle"_]), + test_x_radius_(spec.config["test_x_radius"_]), + test_y_radius_(spec.config["test_y_radius"_]), + test_goal_trajectory_( + detail::ToMjtVector(spec.config["test_goal_trajectory"_])), + test_target1_site_pos_( + detail::ToMjtVector(spec.config["test_target1_site_pos"_])), + test_target2_site_pos_( + detail::ToMjtVector(spec.config["test_target2_site_pos"_])), + test_object1_body_mass_( + detail::ToMjtVector(spec.config["test_object1_body_mass"_])), + test_object2_body_mass_( + detail::ToMjtVector(spec.config["test_object2_body_mass"_])), + test_object1_geom_size_( + detail::ToMjtVector(spec.config["test_object1_geom_size"_])), + test_object2_geom_size_( + detail::ToMjtVector(spec.config["test_object2_geom_size"_])), + test_object1_geom_friction_( + detail::ToMjtVector(spec.config["test_object1_geom_friction"_])), + test_object2_geom_friction_( + detail::ToMjtVector(spec.config["test_object2_geom_friction"_])) { + auto friction_low = spec.config["obj_friction_low"_]; + auto friction_high = spec.config["obj_friction_high"_]; + if (friction_low.size() == 3 && friction_high.size() == 3) { + for (int axis = 0; axis < 3; ++axis) { + obj_friction_low_[axis] = static_cast(friction_low[axis]); + obj_friction_high_[axis] = static_cast(friction_high[axis]); + } + } + ValidateConfig(); + CacheIds(); + detail::BuildMuscleMask(model_, &muscle_actuator_); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + for (int i = 0; i < model_->nq - 14; ++i) { + data_->qpos[i] = 0.0; + } + data_->qpos[0] = static_cast(-1.57); + for (int i = 0; i < model_->nv; ++i) { + data_->qvel[i] = 0.0; + } + mj_forward(model_, data_); + CacheDefaultState(); + InitializeRobotEnv(); + } + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + counter_ = 0; + detail::ResetMyoConditionState(&muscle_condition_state_); + ResetToInitialState(); + RestoreModelState(); + SampleEpisodeParameters(); + ApplyObjectRandomization(); + if (!test_target1_site_pos_.empty()) { + detail::RestoreModelSitePos(model_, target1_sid_, test_target1_site_pos_); + } + if (!test_target2_site_pos_.empty()) { + detail::RestoreModelSitePos(model_, target2_sid_, test_target2_site_pos_); + } + ApplyResetState(); + CaptureResetState(); + RewardInfo reward = ComputeRewardInfo(); + WriteState(reward, true, 0.0); + } + + void Step(const Action& action) override { + UpdateTargetsForCurrentCounter(); + const auto* raw = static_cast(action["action"_].Data()); + detail::ApplyMyoSuiteAction(model_, data_, muscle_actuator_, normalize_act_, + raw); + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + ++counter_; + ++elapsed_step_; + RewardInfo reward = ComputeRewardInfo(); + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, false, reward.dense_reward); + } + + private: + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_] || + model_->nv != spec_.config["qvel_dim"_] || + model_->nu != spec_.config["action_dim"_] || + model_->na != spec_.config["act_dim"_]) { + throw std::runtime_error("MyoChallenge Baoding dims do not match model."); + } + int expected_obs = (model_->nq - 14) + 24 + model_->na; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error( + "MyoChallenge Baoding obs_dim does not match model."); + } + } + + void CacheIds() { + object1_bid_ = mj_name2id(model_, mjOBJ_BODY, "ball1"); + object2_bid_ = mj_name2id(model_, mjOBJ_BODY, "ball2"); + object1_sid_ = mj_name2id(model_, mjOBJ_SITE, "ball1_site"); + object2_sid_ = mj_name2id(model_, mjOBJ_SITE, "ball2_site"); + object1_gid_ = mj_name2id(model_, mjOBJ_GEOM, "ball1"); + object2_gid_ = mj_name2id(model_, mjOBJ_GEOM, "ball2"); + target1_sid_ = mj_name2id(model_, mjOBJ_SITE, "target1_site"); + target2_sid_ = mj_name2id(model_, mjOBJ_SITE, "target2_site"); + if (object1_bid_ == -1 || object2_bid_ == -1 || object1_sid_ == -1 || + object2_sid_ == -1 || object1_gid_ == -1 || object2_gid_ == -1 || + target1_sid_ == -1 || target2_sid_ == -1) { + throw std::runtime_error("MyoChallenge Baoding ids missing."); + } + model_->site_group[target1_sid_] = 2; + model_->site_group[target2_sid_] = 2; + } + + void CacheDefaultState() { + detail::CopyModelSitePos(model_, target1_sid_, &default_target1_site_pos_); + detail::CopyModelSitePos(model_, target2_sid_, &default_target2_site_pos_); + detail::CopyModelGeomSize(model_, object1_gid_, + &default_object1_geom_size_); + detail::CopyModelGeomSize(model_, object2_gid_, + &default_object2_geom_size_); + detail::CopyModelGeomFriction(model_, object1_gid_, + &default_object1_geom_friction_); + detail::CopyModelGeomFriction(model_, object2_gid_, + &default_object2_geom_friction_); + detail::CopyModelGeomRgba(model_, object1_gid_, + &default_object1_geom_rgba_); + detail::CopyModelGeomRgba(model_, object2_gid_, + &default_object2_geom_rgba_); + detail::CopyModelBodyMass(model_, object1_bid_, + &default_object1_body_mass_); + detail::CopyModelBodyMass(model_, object2_bid_, + &default_object2_body_mass_); + } + + void RestoreModelState() { + detail::RestoreModelSitePos(model_, target1_sid_, + default_target1_site_pos_); + detail::RestoreModelSitePos(model_, target2_sid_, + default_target2_site_pos_); + detail::RestoreModelGeomSize(model_, object1_gid_, + default_object1_geom_size_); + detail::RestoreModelGeomSize(model_, object2_gid_, + default_object2_geom_size_); + detail::RestoreModelGeomFriction(model_, object1_gid_, + default_object1_geom_friction_); + detail::RestoreModelGeomFriction(model_, object2_gid_, + default_object2_geom_friction_); + detail::RestoreModelGeomRgba(model_, object1_gid_, + default_object1_geom_rgba_); + detail::RestoreModelGeomRgba(model_, object2_gid_, + default_object2_geom_rgba_); + detail::RestoreModelBodyMass(model_, object1_bid_, + default_object1_body_mass_); + detail::RestoreModelBodyMass(model_, object2_bid_, + default_object2_body_mass_); + } + + void SampleEpisodeParameters() { + if (test_task_ >= 0) { + current_task_ = test_task_; + } else if (task_choice_ == "random") { + std::uniform_int_distribution task_dist(0, 2); + current_task_ = task_dist(gen_); + } else { + current_task_ = fixed_task_; + } + + if (!std::isnan(test_ball1_starting_angle_)) { + ball1_starting_angle_ = test_ball1_starting_angle_; + ball2_starting_angle_ = std::isnan(test_ball2_starting_angle_) + ? ball1_starting_angle_ - detail::kPi + : test_ball2_starting_angle_; + } else if (task_choice_ == "random") { + ball1_starting_angle_ = + challenge_detail::UniformScalar(&gen_, 0.0, 2.0 * detail::kPi); + ball2_starting_angle_ = ball1_starting_angle_ - detail::kPi; + } else { + ball1_starting_angle_ = detail::kPi / static_cast(4.0); + ball2_starting_angle_ = + ball1_starting_angle_ - static_cast(detail::kPi); + } + + x_radius_ = !std::isnan(test_x_radius_) + ? test_x_radius_ + : challenge_detail::UniformScalar(&gen_, goal_xrange_low_, + goal_xrange_high_); + y_radius_ = !std::isnan(test_y_radius_) + ? test_y_radius_ + : challenge_detail::UniformScalar(&gen_, goal_yrange_low_, + goal_yrange_high_); + goal_time_period_ = challenge_detail::UniformScalar( + &gen_, goal_time_period_low_, goal_time_period_high_); + } + + void ApplyObjectRandomization() { + if (!test_object1_body_mass_.empty()) { + detail::RestoreModelBodyMass(model_, object1_bid_, + test_object1_body_mass_[0]); + } else if (has_obj_mass_range_) { + detail::RestoreModelBodyMass(model_, object1_bid_, + challenge_detail::UniformScalar( + &gen_, obj_mass_low_, obj_mass_high_)); + } + if (!test_object2_body_mass_.empty()) { + detail::RestoreModelBodyMass(model_, object2_bid_, + test_object2_body_mass_[0]); + } else if (has_obj_mass_range_) { + detail::RestoreModelBodyMass(model_, object2_bid_, + challenge_detail::UniformScalar( + &gen_, obj_mass_low_, obj_mass_high_)); + } + + if (!test_object1_geom_friction_.empty()) { + detail::RestoreModelGeomFriction(model_, object1_gid_, + test_object1_geom_friction_); + } else if (has_obj_friction_range_) { + auto friction = challenge_detail::UniformVec3(&gen_, obj_friction_low_, + obj_friction_high_); + detail::RestoreModelGeomFriction( + model_, object1_gid_, + std::vector(friction.begin(), friction.end())); + } + if (!test_object2_geom_friction_.empty()) { + detail::RestoreModelGeomFriction(model_, object2_gid_, + test_object2_geom_friction_); + } else if (has_obj_friction_range_) { + auto friction = challenge_detail::UniformVec3(&gen_, obj_friction_low_, + obj_friction_high_); + detail::RestoreModelGeomFriction( + model_, object2_gid_, + std::vector(friction.begin(), friction.end())); + } + + if (!test_object1_geom_size_.empty()) { + detail::RestoreModelGeomSize(model_, object1_gid_, + test_object1_geom_size_); + } else if (has_obj_size_range_) { + mjtNum size = + challenge_detail::UniformScalar(&gen_, obj_size_low_, obj_size_high_); + detail::RestoreModelGeomSize(model_, object1_gid_, {size, size, size}); + } + if (!test_object2_geom_size_.empty()) { + detail::RestoreModelGeomSize(model_, object2_gid_, + test_object2_geom_size_); + } else if (has_obj_size_range_) { + mjtNum size = + challenge_detail::UniformScalar(&gen_, obj_size_low_, obj_size_high_); + detail::RestoreModelGeomSize(model_, object2_gid_, {size, size, size}); + } + } + + std::array GoalAnglesAt(int counter) const { + if (!test_goal_trajectory_.empty()) { + int length = static_cast(test_goal_trajectory_.size() / 2); + int index = std::min(counter, std::max(length - 1, 0)); + return {test_goal_trajectory_[index * 2 + 0], + test_goal_trajectory_[index * 2 + 1]}; + } + mjtNum sign = 0.0; + if (current_task_ == 1) { + sign = -1.0; + } else if (current_task_ == 2) { + sign = 1.0; + } + mjtNum angle = sign * static_cast(2.0) * detail::kPi * + (static_cast(counter) * Dt() / goal_time_period_); + return {angle, angle}; + } + + void UpdateTargetsForCurrentCounter() { + auto angles = GoalAnglesAt(counter_); + angles[0] += ball1_starting_angle_; + angles[1] += ball2_starting_angle_; + std::vector target1 = default_target1_site_pos_; + std::vector target2 = default_target2_site_pos_; + target1[0] = x_radius_ * std::cos(angles[0]) + kCenterX; + target1[1] = y_radius_ * std::sin(angles[0]) + kCenterY; + target2[0] = x_radius_ * std::cos(angles[1]) + kCenterX; + target2[1] = y_radius_ * std::sin(angles[1]) + kCenterY; + detail::RestoreModelSitePos(model_, target1_sid_, target1); + detail::RestoreModelSitePos(model_, target2_sid_, target2); + } + + void ApplyResetState() { + if (!test_reset_qpos_.empty()) { + detail::RestoreVector(test_reset_qpos_, data_->qpos); + } + if (!test_reset_qvel_.empty()) { + detail::RestoreVector(test_reset_qvel_, data_->qvel); + } + mj_forward(model_, data_); + bool rerun_forward = false; + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + rerun_forward = true; + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, data_->qacc_warmstart); + } + if (rerun_forward) { + mj_forward(model_, data_); + } + } + + std::vector Observation() const { + std::vector obs; + obs.reserve((model_->nq - 14) + 24 + model_->na); + obs.insert(obs.end(), data_->qpos, data_->qpos + model_->nq - 14); + std::array object1_pos{}; + std::array object2_pos{}; + std::array target1_pos{}; + std::array target2_pos{}; + detail::CopySitePos(model_, data_, object1_sid_, object1_pos.data()); + detail::CopySitePos(model_, data_, object2_sid_, object2_pos.data()); + detail::CopySitePos(model_, data_, target1_sid_, target1_pos.data()); + detail::CopySitePos(model_, data_, target2_sid_, target2_pos.data()); + obs.insert(obs.end(), object1_pos.begin(), object1_pos.end()); + for (int i = model_->nv - 12; i < model_->nv - 9; ++i) { + obs.push_back(data_->qvel[i] * Dt()); + } + obs.insert(obs.end(), object2_pos.begin(), object2_pos.end()); + for (int i = model_->nv - 6; i < model_->nv - 3; ++i) { + obs.push_back(data_->qvel[i] * Dt()); + } + obs.insert(obs.end(), target1_pos.begin(), target1_pos.end()); + obs.insert(obs.end(), target2_pos.begin(), target2_pos.end()); + auto target1_err = challenge_detail::Sub3(target1_pos, object1_pos); + auto target2_err = challenge_detail::Sub3(target2_pos, object2_pos); + obs.insert(obs.end(), target1_err.begin(), target1_err.end()); + obs.insert(obs.end(), target2_err.begin(), target2_err.end()); + if (model_->na > 0) { + obs.insert(obs.end(), data_->act, data_->act + model_->na); + } + return obs; + } + + RewardInfo ComputeRewardInfo() { + std::array object1_pos{}; + std::array object2_pos{}; + std::array target1_pos{}; + std::array target2_pos{}; + detail::CopySitePos(model_, data_, object1_sid_, object1_pos.data()); + detail::CopySitePos(model_, data_, object2_sid_, object2_pos.data()); + detail::CopySitePos(model_, data_, target1_sid_, target1_pos.data()); + detail::CopySitePos(model_, data_, target2_sid_, target2_pos.data()); + auto target1_err = challenge_detail::Sub3(target1_pos, object1_pos); + auto target2_err = challenge_detail::Sub3(target2_pos, object2_pos); + mjtNum target1_dist = challenge_detail::Norm3(target1_err); + mjtNum target2_dist = challenge_detail::Norm3(target2_err); + mjtNum act_reg = detail::ActReg(model_, data_); + bool is_fall = object1_pos[2] < drop_th_ || object2_pos[2] < drop_th_; + + RewardInfo info; + info.pos_dist_1_term = -target1_dist; + info.pos_dist_2_term = -target2_dist; + info.act_reg_term = -act_reg; + info.sparse = -(target1_dist + target2_dist); + info.solved = target1_dist < proximity_th_ && + target2_dist < proximity_th_ && !is_fall; + info.done = is_fall; + info.dense_reward = reward_pos_dist_1_w_ * info.pos_dist_1_term + + reward_pos_dist_2_w_ * info.pos_dist_2_term; + + std::vector rgba1 = default_object1_geom_rgba_; + std::vector rgba2 = default_object2_geom_rgba_; + rgba1[0] = target1_dist < proximity_th_ ? 1.0 : 0.5; + rgba1[1] = target1_dist < proximity_th_ ? 1.0 : 0.5; + rgba2[0] = target1_dist < proximity_th_ ? 0.9 : 0.5; + rgba2[1] = target1_dist < proximity_th_ ? 0.7 : 0.5; + detail::RestoreModelGeomRgba(model_, object1_gid_, rgba1); + detail::RestoreModelGeomRgba(model_, object2_gid_, rgba2); + return info; + } + + void WriteState(const RewardInfo& reward, bool reset, float reward_value) { + auto obs = Observation(); + auto state = Allocate(); + if constexpr (!kFromPixels) { + AssignObservation("obs", &state["obs"_], obs.data(), obs.size(), reset); + } + state["reward"_] = reward_value; + state["discount"_] = 1.0f; + state["done"_] = done_; + state["trunc"_] = elapsed_step_ >= max_episode_steps_; + state["elapsed_step"_] = elapsed_step_; + state["info:pos_dist_1"_] = reward.pos_dist_1_term; + state["info:pos_dist_2"_] = reward.pos_dist_2_term; + state["info:act_reg"_] = reward.act_reg_term; + state["info:sparse"_] = reward.sparse; + state["info:solved"_] = static_cast(reward.solved); +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + std::array target1_pos{}; + std::array target2_pos{}; + detail::CopySitePos(model_, data_, target1_sid_, target1_pos.data()); + detail::CopySitePos(model_, data_, target2_sid_, target2_pos.data()); + state["info:target1_pos"_].Assign(target1_pos.data(), 3); + state["info:target2_pos"_].Assign(target2_pos.data(), 3); + if constexpr (kFromPixels) { + AssignPixelObservation("obs:pixels", &state["obs:pixels"_], reset); + } + } +}; + +template +class MyoChallengeBimanualEnvBase : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::gen_; + using Base::spec_; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum reach_dist{0.0}; + mjtNum act{0.0}; + mjtNum fin_dis{0.0}; + mjtNum pass_err{0.0}; + mjtNum goal_dist{0.0}; + bool solved{false}; + bool done{false}; + }; + + static constexpr mjtNum kPillarHeight = static_cast(1.09); + static constexpr mjtNum kMaxTime = static_cast(10.0); + static constexpr int kTargetGoalTouch = 10; + + bool normalize_act_; + mjtNum proximity_th_; + std::array start_center_; + std::array goal_center_; + std::array start_shifts_; + std::array goal_shifts_; + mjtNum reward_reach_dist_w_; + mjtNum reward_act_w_; + mjtNum reward_fin_dis_w_; + mjtNum reward_pass_err_w_; + std::array obj_scale_change_{0.0, 0.0, 0.0}; + bool has_obj_scale_change_{false}; + bool has_obj_mass_range_{false}; + bool has_obj_friction_range_{false}; + mjtNum obj_mass_low_{0.0}; + mjtNum obj_mass_high_{0.0}; + std::array obj_friction_low_{0.0, 0.0, 0.0}; + std::array obj_friction_high_{0.0, 0.0, 0.0}; + int start_bid_{-1}; + int goal_bid_{-1}; + int start_mocap_id_{-1}; + int goal_mocap_id_{-1}; + int obj_bid_{-1}; + int obj_sid_{-1}; + int obj_gid_{-1}; + int palm_sid_{-1}; + int rpalm1_sid_{-1}; + int rpalm2_sid_{-1}; + std::array fin_sid_{-1, -1, -1, -1, -1}; + int max_force_sensor_adr_{0}; + int manip_qpos_start_{-1}; + int manip_dof_start_{-1}; + int myo_body_min_{-1}; + int myo_body_max_{-1}; + int prosth_body_min_{-1}; + int prosth_body_max_{-1}; + std::vector myo_joint_qpos_indices_; + std::vector myo_dof_indices_; + std::vector prosth_joint_qpos_indices_; + std::vector prosth_dof_indices_; + std::vector default_start_body_pos_; + std::vector default_goal_body_pos_; + std::vector default_object_geom_size_; + std::vector default_object_geom_friction_; + mjtNum default_object_body_mass_{0.0}; + std::vector muscle_actuator_; + std::vector grasp_key_qpos_; + std::array start_pos_{0.0, 0.0, 0.0}; + std::array goal_pos_{0.0, 0.0, 0.0}; + mjtNum max_force_seen_{0.0}; + int goal_touch_{0}; + mjtNum init_obj_z_{0.0}; + mjtNum init_palm_z_{0.0}; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_act_; + std::vector test_reset_qacc_warmstart_; + std::vector test_start_pos_; + std::vector test_goal_pos_; + std::vector test_object_body_mass_; + std::vector test_object_geom_size_; + std::vector test_object_geom_friction_; + detail::MyoConditionState muscle_condition_state_; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoChallengeBimanualEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + myosuite::MyoSuiteModelPath(spec.config["base_path"_], + spec.config["model_path"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + proximity_th_(spec.config["proximity_th"_]), + reward_reach_dist_w_(spec.config["reward_reach_dist_w"_]), + reward_act_w_(spec.config["reward_act_w"_]), + reward_fin_dis_w_(spec.config["reward_fin_dis_w"_]), + reward_pass_err_w_(spec.config["reward_pass_err_w"_]), + has_obj_scale_change_(!spec.config["obj_scale_change"_].empty()), + has_obj_mass_range_(spec.config["obj_mass_low"_] != 0.0 || + spec.config["obj_mass_high"_] != 0.0), + has_obj_friction_range_(!spec.config["obj_friction_low"_].empty() && + !spec.config["obj_friction_high"_].empty()), + obj_mass_low_(spec.config["obj_mass_low"_]), + obj_mass_high_(spec.config["obj_mass_high"_]), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])), + test_start_pos_(detail::ToMjtVector(spec.config["test_start_pos"_])), + test_goal_pos_(detail::ToMjtVector(spec.config["test_goal_pos"_])), + test_object_body_mass_( + detail::ToMjtVector(spec.config["test_object_body_mass"_])), + test_object_geom_size_( + detail::ToMjtVector(spec.config["test_object_geom_size"_])), + test_object_geom_friction_( + detail::ToMjtVector(spec.config["test_object_geom_friction"_])) { + auto start_center = spec.config["start_center"_]; + auto goal_center = spec.config["goal_center"_]; + auto start_shifts = spec.config["start_shifts"_]; + auto goal_shifts = spec.config["goal_shifts"_]; + auto obj_scale_change = spec.config["obj_scale_change"_]; + auto obj_friction_low = spec.config["obj_friction_low"_]; + auto obj_friction_high = spec.config["obj_friction_high"_]; + for (int axis = 0; axis < 3; ++axis) { + start_center_[axis] = static_cast(start_center[axis]); + goal_center_[axis] = static_cast(goal_center[axis]); + start_shifts_[axis] = static_cast(start_shifts[axis]); + goal_shifts_[axis] = static_cast(goal_shifts[axis]); + if (has_obj_scale_change_) { + obj_scale_change_[axis] = static_cast(obj_scale_change[axis]); + } + if (has_obj_friction_range_) { + obj_friction_low_[axis] = static_cast(obj_friction_low[axis]); + obj_friction_high_[axis] = static_cast(obj_friction_high[axis]); + } + } + ValidateConfig(); + CacheIds(); + detail::BuildMuscleMask(model_, &muscle_actuator_); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + grasp_key_qpos_.assign(model_->key_qpos + 2 * model_->nq, + model_->key_qpos + 3 * model_->nq); + InitializeRobotEnv(); + } + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + max_force_seen_ = 0.0; + goal_touch_ = 0; + detail::ResetMyoConditionState(&muscle_condition_state_); + ResetToInitialState(); + RestoreModelState(); + SampleStartAndGoal(); + ApplyObjectRandomization(); + ApplyResetState(); + SetObjectAtStartPose(); + CaptureResetState(); + RewardInfo reward = ComputeRewardInfo(); + WriteState(reward, true, 0.0); + } + + void Step(const Action& action) override { + const auto* raw = static_cast(action["action"_].Data()); + for (int actuator = 0; actuator < model_->nu; ++actuator) { + auto value = static_cast(raw[actuator]); + if (normalize_act_ && muscle_actuator_[actuator] && model_->na != 0) { + value = detail::MuscleActivation(value); + } else if (normalize_act_ && !muscle_actuator_[actuator]) { + mjtNum low = model_->actuator_ctrlrange[2 * actuator + 0]; + mjtNum high = model_->actuator_ctrlrange[2 * actuator + 1]; + value = 0.5 * (low + high) + value * 0.5 * (high - low); + } + data_->ctrl[actuator] = value; + } + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + ++elapsed_step_; + RewardInfo reward = ComputeRewardInfo(); + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, false, reward.dense_reward); + } + + private: + static bool StartsWith(std::string_view value, std::string_view prefix) { + return value.substr(0, prefix.size()) == prefix; + } + + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_] || + model_->nv != spec_.config["qvel_dim"_] || + model_->nu != spec_.config["action_dim"_] || + model_->na != spec_.config["act_dim"_]) { + throw std::runtime_error( + "MyoChallenge Bimanual dims do not match model."); + } + } + + void CacheIds() { + obj_bid_ = mj_name2id(model_, mjOBJ_BODY, "manip_object"); + obj_sid_ = mj_name2id(model_, mjOBJ_SITE, "touch_site"); + obj_gid_ = model_->body_geomadr[obj_bid_] + 1; + start_bid_ = mj_name2id(model_, mjOBJ_BODY, "start"); + goal_bid_ = mj_name2id(model_, mjOBJ_BODY, "goal"); + start_mocap_id_ = start_bid_ >= 0 ? model_->body_mocapid[start_bid_] : -1; + goal_mocap_id_ = goal_bid_ >= 0 ? model_->body_mocapid[goal_bid_] : -1; + palm_sid_ = mj_name2id(model_, mjOBJ_SITE, "S_grasp"); + fin_sid_[0] = mj_name2id(model_, mjOBJ_SITE, "THtip"); + fin_sid_[1] = mj_name2id(model_, mjOBJ_SITE, "IFtip"); + fin_sid_[2] = mj_name2id(model_, mjOBJ_SITE, "MFtip"); + fin_sid_[3] = mj_name2id(model_, mjOBJ_SITE, "RFtip"); + fin_sid_[4] = mj_name2id(model_, mjOBJ_SITE, "LFtip"); + rpalm1_sid_ = mj_name2id(model_, mjOBJ_SITE, "prosthesis/palm_thumb"); + rpalm2_sid_ = mj_name2id(model_, mjOBJ_SITE, "prosthesis/palm_pinky"); + if (obj_bid_ == -1 || obj_sid_ == -1 || start_bid_ == -1 || + goal_bid_ == -1 || palm_sid_ == -1 || rpalm1_sid_ == -1 || + rpalm2_sid_ == -1) { + throw std::runtime_error("MyoChallenge Bimanual ids missing."); + } + + CenterBoxMeshIfNeeded(); + + myo_body_min_ = model_->nbody; + myo_body_max_ = -1; + prosth_body_min_ = model_->nbody; + prosth_body_max_ = -1; + for (int body = 0; body < model_->nbody; ++body) { + const char* raw_name = mj_id2name(model_, mjOBJ_BODY, body); + std::string_view name = raw_name != nullptr ? raw_name : ""; + if (StartsWith(name, "prosthesis/")) { + prosth_body_min_ = std::min(prosth_body_min_, body); + prosth_body_max_ = std::max(prosth_body_max_, body); + } else if (name != "start" && name != "goal" && name != "manip_object") { + myo_body_min_ = std::min(myo_body_min_, body); + myo_body_max_ = std::max(myo_body_max_, body); + } + } + + for (int joint = 0; joint < model_->njnt; ++joint) { + const char* raw_name = mj_id2name(model_, mjOBJ_JOINT, joint); + std::string_view name = raw_name != nullptr ? raw_name : ""; + if (name == "manip_object/freejoint") { + manip_qpos_start_ = model_->jnt_qposadr[joint]; + manip_dof_start_ = model_->jnt_dofadr[joint]; + continue; + } + if (StartsWith(name, "prosthesis")) { + prosth_joint_qpos_indices_.push_back(model_->jnt_qposadr[joint]); + prosth_dof_indices_.push_back(model_->jnt_dofadr[joint]); + } else { + myo_joint_qpos_indices_.push_back(model_->jnt_qposadr[joint]); + myo_dof_indices_.push_back(model_->jnt_dofadr[joint]); + } + } + + detail::CopyModelBodyPos(model_, start_bid_, &default_start_body_pos_); + detail::CopyModelBodyPos(model_, goal_bid_, &default_goal_body_pos_); + detail::CopyModelGeomSize(model_, obj_gid_, &default_object_geom_size_); + detail::CopyModelGeomFriction(model_, obj_gid_, + &default_object_geom_friction_); + detail::CopyModelBodyMass(model_, obj_bid_, &default_object_body_mass_); + max_force_sensor_adr_ = model_->sensor_adr[0]; + + int expected_obs = 1 + static_cast(myo_joint_qpos_indices_.size()) + + static_cast(myo_dof_indices_.size()) + + static_cast(prosth_joint_qpos_indices_.size()) + + static_cast(prosth_dof_indices_.size()) + 7 + 6 + + 5 + model_->na; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error( + "MyoChallenge Bimanual obs_dim does not match model."); + } + } + + void CenterBoxMeshIfNeeded() { + if (!has_obj_scale_change_ || obj_gid_ <= 0) { + return; + } + int box_mesh = -1; + for (int mesh = 0; mesh < model_->nmesh; ++mesh) { + const char* raw_name = mj_id2name(model_, mjOBJ_MESH, mesh); + std::string_view name = raw_name != nullptr ? raw_name : ""; + if (name.find("box") != std::string_view::npos) { + box_mesh = mesh; + break; + } + } + if (box_mesh < 0) { + return; + } + int vert_adr = model_->mesh_vertadr[box_mesh]; + int vert_num = model_->mesh_vertnum[box_mesh]; + const mjtNum* mesh_quat = model_->geom_quat + (obj_gid_ - 1) * 4; + std::array mesh_pos{}; + std::array box_pos{}; + std::memcpy(mesh_pos.data(), model_->geom_pos + (obj_gid_ - 1) * 3, + sizeof(mjtNum) * 3); + std::memcpy(box_pos.data(), model_->geom_pos + obj_gid_ * 3, + sizeof(mjtNum) * 3); + for (int vertex = 0; vertex < vert_num; ++vertex) { + float* vert = model_->mesh_vert + (vert_adr + vertex) * 3; + auto rotated_vert = challenge_detail::RotateByQuat(mesh_quat, vert); + auto rotated_normal = challenge_detail::RotateByQuat( + mesh_quat, model_->mesh_normal + (vert_adr + vertex) * 3); + for (int axis = 0; axis < 3; ++axis) { + vert[axis] = static_cast(rotated_vert[axis] + mesh_pos[axis] - + box_pos[axis]); + model_->mesh_normal[(vert_adr + vertex) * 3 + axis] = + rotated_normal[axis]; + } + } + model_->geom_quat[(obj_gid_ - 1) * 4 + 0] = 1.0; + model_->geom_quat[(obj_gid_ - 1) * 4 + 1] = 0.0; + model_->geom_quat[(obj_gid_ - 1) * 4 + 2] = 0.0; + model_->geom_quat[(obj_gid_ - 1) * 4 + 3] = 0.0; + std::memcpy(model_->geom_pos + (obj_gid_ - 1) * 3, box_pos.data(), + sizeof(mjtNum) * 3); + } + + void RestoreModelState() { + detail::RestoreModelBodyPos(model_, start_bid_, default_start_body_pos_); + detail::RestoreModelBodyPos(model_, goal_bid_, default_goal_body_pos_); + detail::RestoreModelGeomSize(model_, obj_gid_, default_object_geom_size_); + detail::RestoreModelGeomFriction(model_, obj_gid_, + default_object_geom_friction_); + detail::RestoreModelBodyMass(model_, obj_bid_, default_object_body_mass_); + } + + void SampleStartAndGoal() { + if (!test_start_pos_.empty()) { + std::copy_n(test_start_pos_.begin(), 3, start_pos_.begin()); + } else { + for (int axis = 0; axis < 3; ++axis) { + start_pos_[axis] = + start_center_[axis] + + start_shifts_[axis] * + challenge_detail::UniformScalar(&gen_, -1.0, 1.0); + } + } + if (!test_goal_pos_.empty()) { + std::copy_n(test_goal_pos_.begin(), 3, goal_pos_.begin()); + } else { + for (int axis = 0; axis < 3; ++axis) { + goal_pos_[axis] = goal_center_[axis] + + goal_shifts_[axis] * + challenge_detail::UniformScalar(&gen_, -1.0, 1.0); + } + } + detail::RestoreModelBodyPos(model_, start_bid_, + {start_pos_[0], start_pos_[1], start_pos_[2]}); + detail::RestoreModelBodyPos(model_, goal_bid_, + {goal_pos_[0], goal_pos_[1], goal_pos_[2]}); + if (start_mocap_id_ >= 0) { + data_->mocap_pos[start_mocap_id_ * 3 + 0] = start_pos_[0]; + data_->mocap_pos[start_mocap_id_ * 3 + 1] = start_pos_[1]; + data_->mocap_pos[start_mocap_id_ * 3 + 2] = start_pos_[2]; + } + if (goal_mocap_id_ >= 0) { + data_->mocap_pos[goal_mocap_id_ * 3 + 0] = goal_pos_[0]; + data_->mocap_pos[goal_mocap_id_ * 3 + 1] = goal_pos_[1]; + data_->mocap_pos[goal_mocap_id_ * 3 + 2] = goal_pos_[2]; + } + } + + void ApplyObjectRandomization() { + if (!test_object_body_mass_.empty()) { + detail::RestoreModelBodyMass(model_, obj_bid_, test_object_body_mass_[0]); + } else if (has_obj_mass_range_) { + detail::RestoreModelBodyMass(model_, obj_bid_, + challenge_detail::UniformScalar( + &gen_, obj_mass_low_, obj_mass_high_)); + } + if (!test_object_geom_friction_.empty()) { + detail::RestoreModelGeomFriction(model_, obj_gid_, + test_object_geom_friction_); + } else if (has_obj_friction_range_) { + auto friction = challenge_detail::UniformVec3(&gen_, obj_friction_low_, + obj_friction_high_); + detail::RestoreModelGeomFriction( + model_, obj_gid_, + std::vector(friction.begin(), friction.end())); + } + if (!test_object_geom_size_.empty()) { + detail::RestoreModelGeomSize(model_, obj_gid_, test_object_geom_size_); + } else if (has_obj_scale_change_) { + std::vector scaled = default_object_geom_size_; + for (int axis = 0; axis < 3; ++axis) { + mjtNum delta = challenge_detail::UniformScalar( + &gen_, -obj_scale_change_[axis], obj_scale_change_[axis]); + scaled[axis] = default_object_geom_size_[axis] * (1.0 + delta); + } + detail::RestoreModelGeomSize(model_, obj_gid_, scaled); + } + } + + void ApplyResetState() { + if (!test_reset_qpos_.empty()) { + detail::RestoreVector(test_reset_qpos_, data_->qpos); + } else { + detail::RestoreVector(grasp_key_qpos_, data_->qpos); + } + if (!test_reset_qvel_.empty()) { + detail::RestoreVector(test_reset_qvel_, data_->qvel); + } else { + std::fill(data_->qvel, data_->qvel + model_->nv, 0.0); + } + mj_forward(model_, data_); + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, data_->qacc_warmstart); + } + if (!test_reset_act_.empty()) { + mj_forward(model_, data_); + } + } + + void SetObjectAtStartPose() { + data_->qpos[manip_qpos_start_ + 0] = start_pos_[0]; + data_->qpos[manip_qpos_start_ + 1] = start_pos_[1]; + data_->qpos[manip_qpos_start_ + 2] = + start_pos_[2] + static_cast(0.1); + init_obj_z_ = data_->site_xpos[obj_sid_ * 3 + 2]; + init_palm_z_ = data_->site_xpos[palm_sid_ * 3 + 2]; + } + + enum class ObjLabel : std::uint8_t { + MYO = 0, + PROSTH = 1, + START = 2, + GOAL = 3, + ENV = 4, + }; + + ObjLabel ClassifyBody(int body_id) const { + if (myo_body_min_ <= body_id && body_id <= myo_body_max_) { + return ObjLabel::MYO; + } + if (prosth_body_min_ <= body_id && body_id <= prosth_body_max_) { + return ObjLabel::PROSTH; + } + if (body_id == start_bid_) { + return ObjLabel::START; + } + if (body_id == goal_bid_) { + return ObjLabel::GOAL; + } + return ObjLabel::ENV; + } + + std::array TouchVector() { + std::array touch = {0.0, 0.0, 0.0, 0.0, 0.0}; + for (int i = 0; i < data_->ncon; ++i) { + const mjContact& contact = data_->contact[i]; + int body1 = model_->geom_bodyid[contact.geom1]; + int body2 = model_->geom_bodyid[contact.geom2]; + if (body1 == obj_bid_) { + touch[static_cast(ClassifyBody(body2))] = 1.0; + } else if (body2 == obj_bid_) { + touch[static_cast(ClassifyBody(body1))] = 1.0; + } + } + return touch; + } + + std::array SitePos(int site_id) const { + std::array out{}; + detail::CopySitePos(model_, data_, site_id, out.data()); + return out; + } + + std::vector Observation() const { + std::vector obs; + obs.reserve(spec_.config["obs_dim"_]); + obs.push_back(data_->time); + for (int idx : myo_joint_qpos_indices_) { + obs.push_back(data_->qpos[idx]); + } + for (int idx : myo_dof_indices_) { + obs.push_back(data_->qvel[idx]); + } + for (int idx : prosth_joint_qpos_indices_) { + obs.push_back(data_->qpos[idx]); + } + for (int idx : prosth_dof_indices_) { + obs.push_back(data_->qvel[idx]); + } + obs.insert(obs.end(), data_->qpos + manip_qpos_start_, + data_->qpos + manip_qpos_start_ + 7); + obs.insert(obs.end(), data_->qvel + manip_dof_start_, + data_->qvel + manip_dof_start_ + 6); + auto touch = const_cast(this)->TouchVector(); + obs.insert(obs.end(), touch.begin(), touch.end()); + if (model_->na > 0) { + obs.insert(obs.end(), data_->act, data_->act + model_->na); + } + return obs; + } + + RewardInfo ComputeRewardInfo() { + auto touch = TouchVector(); + if (touch[static_cast(ObjLabel::GOAL)] > 0.0) { + ++goal_touch_; + } + max_force_seen_ = std::max( + max_force_seen_, std::abs(data_->sensordata[max_force_sensor_adr_])); + auto palm_pos = SitePos(palm_sid_); + auto obj_pos = SitePos(obj_sid_); + std::array rpalm_pos{}; + for (int axis = 0; axis < 3; ++axis) { + rpalm_pos[axis] = (data_->site_xpos[rpalm1_sid_ * 3 + axis] + + data_->site_xpos[rpalm2_sid_ * 3 + axis]) * + static_cast(0.5); + } + + auto reach_err = challenge_detail::Sub3(palm_pos, obj_pos); + auto pass_err = challenge_detail::Sub3(rpalm_pos, obj_pos); + mjtNum reach_dist = challenge_detail::Norm3(reach_err); + mjtNum pass_dist = challenge_detail::Norm3(pass_err); + mjtNum act = detail::ActReg(model_, data_); + mjtNum fin_dis = 0.0; + for (int site_id : fin_sid_) { + fin_dis += challenge_detail::Norm3( + challenge_detail::Sub3(SitePos(site_id), obj_pos)); + } + std::array goal = {goal_pos_[0], goal_pos_[1], kPillarHeight}; + mjtNum goal_dist = + challenge_detail::Norm3(challenge_detail::Sub3(obj_pos, goal)); + + RewardInfo reward; + reward.reach_dist = reach_dist + std::log(reach_dist + 1e-6); + reward.act = act; + reward.fin_dis = fin_dis + std::log(fin_dis + 1e-6); + reward.pass_err = pass_dist + std::log(pass_dist + 1e-3); + reward.goal_dist = goal_dist; + reward.solved = + goal_dist < proximity_th_ && goal_touch_ >= kTargetGoalTouch; + reward.done = data_->time > kMaxTime || + obj_pos[2] < static_cast(0.3) || reward.solved; + reward.dense_reward = reward_reach_dist_w_ * reward.reach_dist + + reward_act_w_ * reward.act + + reward_fin_dis_w_ * reward.fin_dis + + reward_pass_err_w_ * reward.pass_err; + return reward; + } + + void WriteState(const RewardInfo& reward, bool reset, float reward_value) { + auto obs = Observation(); + auto state = Allocate(); + if constexpr (!kFromPixels) { + AssignObservation("obs", &state["obs"_], obs.data(), obs.size(), reset); + } + state["reward"_] = reward_value; + state["discount"_] = 1.0f; + state["done"_] = done_; + state["trunc"_] = elapsed_step_ >= max_episode_steps_; + state["elapsed_step"_] = elapsed_step_; + state["info:reach_dist"_] = reward.reach_dist; + state["info:act"_] = reward.act; + state["info:fin_dis"_] = reward.fin_dis; + state["info:pass_err"_] = reward.pass_err; + state["info:goal_dist"_] = reward.goal_dist; + state["info:solved"_] = static_cast(reward.solved); +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + state["info:start_pos"_].Assign(start_pos_.data(), 3); + state["info:goal_pos"_].Assign(goal_pos_.data(), 3); + if constexpr (kFromPixels) { + AssignPixelObservation("obs:pixels", &state["obs:pixels"_], reset); + } + } +}; + +template +using ReorientEnvAlias = MyoChallengeReorientEnvBase; + +template +using ReorientPixelEnvAlias = MyoChallengeReorientEnvBase; + +template +using RelocateEnvAlias = MyoChallengeRelocateEnvBase; + +template +using RelocatePixelEnvAlias = MyoChallengeRelocateEnvBase; + +template +using BaodingEnvAlias = MyoChallengeBaodingEnvBase; + +template +using BaodingPixelEnvAlias = MyoChallengeBaodingEnvBase; + +template +using BimanualEnvAlias = MyoChallengeBimanualEnvBase; + +template +using BimanualPixelEnvAlias = MyoChallengeBimanualEnvBase; + +using ReorientSpec = MyoChallengeReorientEnvSpec; +using ReorientPixelSpec = MyoChallengeReorientPixelEnvSpec; +using RelocateSpec = MyoChallengeRelocateEnvSpec; +using RelocatePixelSpec = MyoChallengeRelocatePixelEnvSpec; +using BaodingSpec = MyoChallengeBaodingEnvSpec; +using BaodingPixelSpec = MyoChallengeBaodingPixelEnvSpec; +using BimanualSpec = MyoChallengeBimanualEnvSpec; +using BimanualPixelSpec = MyoChallengeBimanualPixelEnvSpec; + +using ReorientEnv = ReorientEnvAlias; +using ReorientPixelEnv = ReorientPixelEnvAlias; +using MyoChallengeReorientEnv = ReorientEnv; +using MyoChallengeReorientPixelEnv = ReorientPixelEnv; +using MyoChallengeReorientEnvPool = AsyncEnvPool; +using MyoChallengeReorientPixelEnvPool = AsyncEnvPool; + +using RelocateEnv = RelocateEnvAlias; +using RelocatePixelEnv = RelocatePixelEnvAlias; +using MyoChallengeRelocateEnv = RelocateEnv; +using MyoChallengeRelocatePixelEnv = RelocatePixelEnv; +using MyoChallengeRelocateEnvPool = AsyncEnvPool; +using MyoChallengeRelocatePixelEnvPool = AsyncEnvPool; + +using BaodingEnv = BaodingEnvAlias; +using BaodingPixelEnv = BaodingPixelEnvAlias; +using MyoChallengeBaodingEnv = BaodingEnv; +using MyoChallengeBaodingPixelEnv = BaodingPixelEnv; +using MyoChallengeBaodingEnvPool = AsyncEnvPool; +using MyoChallengeBaodingPixelEnvPool = AsyncEnvPool; + +using BimanualEnv = BimanualEnvAlias; +using BimanualPixelEnv = BimanualPixelEnvAlias; +using MyoChallengeBimanualEnv = BimanualEnv; +using MyoChallengeBimanualPixelEnv = BimanualPixelEnv; +using MyoChallengeBimanualEnvPool = AsyncEnvPool; +using MyoChallengeBimanualPixelEnvPool = AsyncEnvPool; + +} // namespace myosuite_envpool + +#endif // ENVPOOL_MUJOCO_MYOSUITE_MYOCHALLENGE_H_ diff --git a/envpool/mujoco/myosuite/myochallenge_extended.h b/envpool/mujoco/myosuite/myochallenge_extended.h new file mode 100644 index 000000000..51ca2d2aa --- /dev/null +++ b/envpool/mujoco/myosuite/myochallenge_extended.h @@ -0,0 +1,3743 @@ +// Copyright 2026 Garena Online Private 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 ENVPOOL_MUJOCO_MYOSUITE_MYOCHALLENGE_EXTENDED_H_ +#define ENVPOOL_MUJOCO_MYOSUITE_MYOCHALLENGE_EXTENDED_H_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "envpool/mujoco/myosuite/myochallenge.h" + +namespace myosuite_envpool { + +namespace challenge_extra_detail { + +inline mjtNum MeanSquareAct(const mjModel* model, const mjData* data) { + if (model->na == 0) { + return 0.0; + } + mjtNum total = 0.0; + for (int i = 0; i < model->na; ++i) { + total += data->act[i] * data->act[i]; + } + return total / static_cast(model->na); +} + +inline mjtNum L2ActReg(const mjModel* model, const mjData* data) { + if (model->na == 0) { + return 0.0; + } + mjtNum total = 0.0; + for (int i = 0; i < model->na; ++i) { + total += data->act[i] * data->act[i]; + } + return std::sqrt(total) / static_cast(model->na); +} + +inline mjtNum Norm2(mjtNum x, mjtNum y) { return std::sqrt(x * x + y * y); } + +inline mjtNum Clip(mjtNum value, mjtNum low, mjtNum high) { + return std::min(std::max(value, low), high); +} + +inline mjtNum NormalizeSignedAngle(mjtNum angle) { + constexpr mjtNum two_pi = static_cast(2.0) * detail::kPi; + while (angle > detail::kPi) { + angle -= two_pi; + } + while (angle < -detail::kPi) { + angle += two_pi; + } + return angle; +} + +inline mjtNum QuatYaw(const mjtNum* quat) { + std::array mat{}; + mju_quat2Mat(mat.data(), quat); + return challenge_detail::Mat9ToEuler(mat.data())[2]; +} + +inline std::array YawToQuat(mjtNum yaw) { + return challenge_detail::EulerXYZToQuat({0.0, 0.0, yaw}); +} + +inline std::array ScipyEulerXYZToQuat( + const std::array& euler) { + mjtNum half_x = euler[0] * static_cast(0.5); + mjtNum half_y = euler[1] * static_cast(0.5); + mjtNum half_z = euler[2] * static_cast(0.5); + mjtNum sx = std::sin(half_x); + mjtNum sy = std::sin(half_y); + mjtNum sz = std::sin(half_z); + mjtNum cx = std::cos(half_x); + mjtNum cy = std::cos(half_y); + mjtNum cz = std::cos(half_z); + return { + cx * cy * cz + sx * sy * sz, + sx * cy * cz - cx * sy * sz, + cx * sy * cz + sx * cy * sz, + cx * cy * sz - sx * sy * cz, + }; +} + +inline std::uint16_t ReadLe16(std::istream* input) { + std::array bytes{}; + input->read(reinterpret_cast(bytes.data()), + static_cast(bytes.size())); + if (!*input) { + throw std::runtime_error("Failed to read uint16."); + } + return static_cast(bytes[0]) | + (static_cast(bytes[1]) << 8); +} + +inline std::uint32_t ReadLe32(std::istream* input) { + std::array bytes{}; + input->read(reinterpret_cast(bytes.data()), + static_cast(bytes.size())); + if (!*input) { + throw std::runtime_error("Failed to read uint32."); + } + return static_cast(bytes[0]) | + (static_cast(bytes[1]) << 8) | + (static_cast(bytes[2]) << 16) | + (static_cast(bytes[3]) << 24); +} + +inline std::pair ParseNpyShape2d(const std::string& header) { + std::size_t shape_pos = header.find("shape"); + if (shape_pos == std::string::npos) { + throw std::runtime_error("Missing shape in NPY header."); + } + std::size_t open = header.find('(', shape_pos); + std::size_t comma = header.find(',', open); + std::size_t close = header.find(')', comma); + if (open == std::string::npos || comma == std::string::npos || + close == std::string::npos) { + throw std::runtime_error("Invalid shape in NPY header."); + } + int rows = std::stoi(header.substr(open + 1, comma - open - 1)); + int cols = std::stoi(header.substr(comma + 1, close - comma - 1)); + return {rows, cols}; +} + +inline std::vector LoadNormalizedReliefPatch(const std::string& path) { + std::ifstream input(path, std::ios::binary); + if (!input) { + throw std::runtime_error("Failed to open MyoSuite relief asset: " + path); + } + std::array magic{}; + input.read(magic.data(), static_cast(magic.size())); + if (!input || std::string_view(magic.data(), magic.size()) != "\x93NUMPY") { + throw std::runtime_error("Invalid NPY magic in MyoSuite relief asset."); + } + std::array version{}; + input.read(reinterpret_cast(version.data()), + static_cast(version.size())); + if (!input) { + throw std::runtime_error("Failed to read NPY version."); + } + std::size_t header_len = 0; + if (version[0] == 1) { + header_len = ReadLe16(&input); + } else if (version[0] == 2) { + header_len = ReadLe32(&input); + } else { + throw std::runtime_error("Unsupported NPY version for relief asset."); + } + std::string header(header_len, '\0'); + input.read(header.data(), static_cast(header.size())); + if (!input) { + throw std::runtime_error("Failed to read NPY header."); + } + if (header.find("'fortran_order': False") == std::string::npos || + (header.find("'descr': '|u1'") == std::string::npos && + header.find("'descr': ' raw(static_cast(rows * cols)); + input.read(reinterpret_cast(raw.data()), + static_cast(raw.size())); + if (!input) { + throw std::runtime_error("Failed to read MyoSuite relief payload."); + } + auto [min_it, max_it] = std::minmax_element(raw.begin(), raw.end()); + const auto low = static_cast(*min_it); + const auto high = static_cast(*max_it); + mjtNum denom = std::max(high - low, static_cast(1e-12)); + std::vector normalized(raw.size(), 0.0); + const auto cols_index = static_cast(cols); + for (int row = 0; row < rows; ++row) { + const auto row_index = static_cast(row); + const auto flipped_row_index = static_cast(rows - 1 - row); + for (int col = 0; col < cols; ++col) { + const auto col_index = static_cast(col); + const auto src = row_index * cols_index + col_index; + const auto dst = flipped_row_index * cols_index + col_index; + normalized[dst] = (static_cast(raw[src]) - low) / denom; + } + } + return normalized; +} + +inline std::vector CollectJointQposAdrs( + const mjModel* model, const std::vector& joint_names) { + std::vector out; + out.reserve(joint_names.size()); + for (const std::string& joint_name : joint_names) { + int joint_id = mj_name2id(model, mjOBJ_JOINT, joint_name.c_str()); + if (joint_id == -1) { + throw std::runtime_error("Missing joint: " + joint_name); + } + out.push_back(model->jnt_qposadr[joint_id]); + } + return out; +} + +inline std::vector CollectJointDofAdrs( + const mjModel* model, const std::vector& joint_names) { + std::vector out; + out.reserve(joint_names.size()); + for (const std::string& joint_name : joint_names) { + int joint_id = mj_name2id(model, mjOBJ_JOINT, joint_name.c_str()); + if (joint_id == -1) { + throw std::runtime_error("Missing joint: " + joint_name); + } + out.push_back(model->jnt_dofadr[joint_id]); + } + return out; +} + +inline mjtNum AverageJointLimitForce(const mjModel* model, const mjData* data, + const std::vector& dof_adrs) { + if (dof_adrs.empty() || data->nefc == 0) { + return 0.0; + } + std::vector limit_force(data->nefc, 0.0); + for (int efc = 0; efc < data->nefc; ++efc) { + if (data->efc_type[efc] == mjCNSTR_LIMIT_JOINT) { + limit_force[efc] = data->efc_force[efc]; + } + } + std::vector joint_force(model->nv, 0.0); + mj_mulJacTVec(model, data, joint_force.data(), limit_force.data()); + mjtNum total = 0.0; + for (int dof_adr : dof_adrs) { + total += std::abs(Clip(joint_force[dof_adr], -1000.0, 1000.0)) / 1000.0; + } + return total / static_cast(dof_adrs.size()); +} + +inline void AssignVectorField(std::vector* out, const mjtNum* src, + const std::vector& indices, + mjtNum scale = 1.0) { + out->clear(); + out->reserve(indices.size()); + for (int index : indices) { + out->push_back(src[index] * scale); + } +} + +inline void AssignSensorField(const mjModel* model, const mjData* data, + const std::vector& sensor_ids, + std::vector* out) { + out->clear(); + for (int sensor_id : sensor_ids) { + int adr = model->sensor_adr[sensor_id]; + int dim = model->sensor_dim[sensor_id]; + out->insert(out->end(), data->sensordata + adr, + data->sensordata + adr + dim); + } +} + +inline int RequireId(const mjModel* model, mjtObj obj, + const std::string& name) { + int id = mj_name2id(model, obj, name.c_str()); + if (id == -1) { + throw std::runtime_error("Missing MuJoCo object: " + name); + } + return id; +} + +class ColoredNoiseProcess { + public: + ColoredNoiseProcess(mjtNum beta, int channels, int time_steps, + mjtNum scale = 1.0, mjtNum max_period = 0.0) + : beta_(beta), + channels_(channels), + time_steps_(time_steps), + scale_(scale), + minimum_frequency_(max_period > 0.0 ? 1.0 / max_period : 0.0) {} + + void SetScale(mjtNum scale) { scale_ = scale; } + + void Reset(std::mt19937* gen) { + buffer_ = + PowerlawPsdGaussian(beta_, channels_, time_steps_, minimum_frequency_, + gen == nullptr ? fallback_gen_ : *gen); + idx_ = 0; + } + + void SetState(const std::vector& buffer, int idx) { + if (!buffer.empty() && + static_cast(buffer.size()) != channels_ * time_steps_) { + throw std::runtime_error("Colored noise buffer has unexpected size."); + } + buffer_ = buffer; + idx_ = std::max(0, idx); + } + + std::vector Sample(std::mt19937* gen) { + if (buffer_.empty() || idx_ >= time_steps_) { + Reset(gen); + } + std::vector out(channels_); + for (int channel = 0; channel < channels_; ++channel) { + out[channel] = scale_ * buffer_[channel * time_steps_ + idx_]; + } + ++idx_; + return out; + } + + private: + static std::vector PowerlawPsdGaussian(mjtNum exponent, int channels, + int samples, mjtNum fmin, + std::mt19937& gen) { + if (channels <= 0 || samples <= 0) { + return {}; + } + if (fmin < 0.0 || fmin > 0.5) { + throw std::runtime_error("Colored noise fmin must be in [0, 0.5]."); + } + constexpr mjtNum k_two_pi = static_cast(2.0) * detail::kPi; + int freq_count = samples / 2 + 1; + std::vector s_scale(freq_count, 0.0); + for (int i = 0; i < freq_count; ++i) { + s_scale[i] = static_cast(i) / static_cast(samples); + } + fmin = + std::max(fmin, static_cast(1.0) / static_cast(samples)); + int ix = 0; + while (ix < freq_count && s_scale[ix] < fmin) { + ++ix; + } + if (ix > 0 && ix < freq_count) { + std::fill(s_scale.begin(), s_scale.begin() + ix, s_scale[ix]); + } + for (mjtNum& value : s_scale) { + value = std::pow(value, -exponent / static_cast(2.0)); + } + mjtNum sigma_sq = 0.0; + for (int i = 1; i < freq_count; ++i) { + mjtNum weight = s_scale[i]; + if ((samples % 2) == 0 && i == freq_count - 1) { + weight *= static_cast(0.5); + } + sigma_sq += weight * weight; + } + mjtNum sigma = static_cast(2.0) * std::sqrt(sigma_sq) / + static_cast(samples); + if (sigma == 0.0) { + throw std::runtime_error("Colored noise sigma unexpectedly zero."); + } + std::normal_distribution normal(0.0, 1.0); + std::vector out(channels * samples, 0.0); + std::vector sr(freq_count, 0.0); + std::vector si(freq_count, 0.0); + for (int channel = 0; channel < channels; ++channel) { + for (int i = 0; i < freq_count; ++i) { + sr[i] = normal(gen) * s_scale[i]; + si[i] = normal(gen) * s_scale[i]; + } + si[0] = 0.0; + sr[0] *= std::sqrt(static_cast(2.0)); + if ((samples % 2) == 0) { + si[freq_count - 1] = 0.0; + sr[freq_count - 1] *= std::sqrt(static_cast(2.0)); + } + int mirrored_limit = (samples % 2) == 0 ? freq_count - 1 : freq_count; + for (int t = 0; t < samples; ++t) { + mjtNum value = sr[0]; + for (int i = 1; i < mirrored_limit; ++i) { + mjtNum angle = k_two_pi * static_cast(i) * + static_cast(t) / static_cast(samples); + value += static_cast(2.0) * + (sr[i] * std::cos(angle) - si[i] * std::sin(angle)); + } + if ((samples % 2) == 0) { + value += sr[freq_count - 1] * std::cos(detail::kPi * t); + } + out[channel * samples + t] = + value / static_cast(samples) / sigma; + } + } + return out; + } + + mjtNum beta_; + int channels_; + int time_steps_; + mjtNum scale_; + mjtNum minimum_frequency_; + int idx_{0}; + std::vector buffer_; + std::mt19937 fallback_gen_{0}; +}; + +enum class RunTrackOslState : std::uint8_t { + kEarlyStance = 0, + kLateStance = 1, + kEarlySwing = 2, + kLateSwing = 3, +}; + +struct RunTrackOslSensorData { + mjtNum knee_angle{0.0}; + mjtNum knee_vel{0.0}; + mjtNum ankle_angle{0.0}; + mjtNum ankle_vel{0.0}; + mjtNum load{0.0}; +}; + +class RunTrackOslController { + public: + RunTrackOslController() = default; + + explicit RunTrackOslController(mjtNum body_mass) { Initialize(body_mass); } + + void Reset(RunTrackOslState state) { + state_ = state; + running_ = false; + } + + void Start() { running_ = true; } + + void Initialize(mjtNum body_mass) { InitializeDefaults(body_mass); } + + void SetState(RunTrackOslState state) { state_ = state; } + + int StateId() const { return static_cast(state_); } + + void Update(const RunTrackOslSensorData& sensor_data) { + sensor_data_ = sensor_data; + if (!running_) { + return; + } + const StateParams& params = states_[StateId()]; + for (int i = 0; i < params.threshold_count; ++i) { + if (ThresholdSatisfied(params.thresholds[i], sensor_data_)) { + state_ = NextState(state_); + return; + } + } + } + + std::array GetTorques() const { + return {JointTorque(/*knee=*/true), JointTorque(/*knee=*/false)}; + } + + private: + struct JointParams { + mjtNum stiffness{0.0}; + mjtNum damping{0.0}; + mjtNum target_angle{0.0}; + }; + + struct Threshold { + enum class Variable : std::uint8_t { + kLoad, + kKneeAngle, + kKneeVel, + kAnkleAngle, + }; + + Variable variable{Variable::kLoad}; + mjtNum value{0.0}; + bool above{false}; + }; + + struct StateParams { + JointParams knee; + JointParams ankle; + std::array thresholds{}; + int threshold_count{0}; + }; + + static RunTrackOslState NextState(RunTrackOslState state) { + switch (state) { + case RunTrackOslState::kEarlyStance: + return RunTrackOslState::kLateStance; + case RunTrackOslState::kLateStance: + return RunTrackOslState::kEarlySwing; + case RunTrackOslState::kEarlySwing: + return RunTrackOslState::kLateSwing; + case RunTrackOslState::kLateSwing: + return RunTrackOslState::kEarlyStance; + } + return RunTrackOslState::kEarlyStance; + } + + static Threshold MakeThreshold(Threshold::Variable variable, mjtNum value, + bool above) { + return Threshold{variable, value, above}; + } + + static bool ThresholdSatisfied(const Threshold& threshold, + const RunTrackOslSensorData& sensor_data) { + mjtNum observed = 0.0; + switch (threshold.variable) { + case Threshold::Variable::kLoad: + observed = sensor_data.load; + break; + case Threshold::Variable::kKneeAngle: + observed = sensor_data.knee_angle; + break; + case Threshold::Variable::kKneeVel: + observed = sensor_data.knee_vel; + break; + case Threshold::Variable::kAnkleAngle: + observed = sensor_data.ankle_angle; + break; + } + return threshold.above ? observed > threshold.value + : observed < threshold.value; + } + + mjtNum JointTorque(bool knee) const { + const StateParams& params = states_[StateId()]; + const JointParams& joint = knee ? params.knee : params.ankle; + const mjtNum angle = + knee ? sensor_data_.knee_angle : sensor_data_.ankle_angle; + const mjtNum velocity = + knee ? sensor_data_.knee_vel : sensor_data_.ankle_vel; + const mjtNum peak_torque = knee ? knee_peak_torque_ : ankle_peak_torque_; + return challenge_extra_detail::Clip( + joint.stiffness * (joint.target_angle - angle) - + joint.damping * velocity, + -peak_torque, peak_torque); + } + + void InitializeDefaults(mjtNum body_mass) { + const mjtNum body_weight = body_mass * static_cast(9.81); + const mjtNum deg = detail::kPi / static_cast(180.0); + states_[static_cast(RunTrackOslState::kEarlyStance)] = StateParams{ + JointParams{99.372, 3.180, 5.0 * deg}, + JointParams{19.874, 0.0, -2.0 * deg}, + {MakeThreshold(Threshold::Variable::kLoad, + static_cast(0.25) * body_weight, + /*above=*/true), + MakeThreshold(Threshold::Variable::kAnkleAngle, 6.0 * deg, + /*above=*/true)}, + 2, + }; + states_[static_cast(RunTrackOslState::kLateStance)] = StateParams{ + JointParams{99.372, 1.272, 8.0 * deg}, + JointParams{79.498, 0.063, -20.0 * deg}, + {MakeThreshold(Threshold::Variable::kLoad, + static_cast(0.15) * body_weight, + /*above=*/false)}, + 1, + }; + states_[static_cast(RunTrackOslState::kEarlySwing)] = StateParams{ + JointParams{39.749, 0.063, 60.0 * deg}, + JointParams{7.949, 0.0, 25.0 * deg}, + {MakeThreshold(Threshold::Variable::kKneeAngle, 50.0 * deg, + /*above=*/true), + MakeThreshold(Threshold::Variable::kKneeVel, 3.0 * deg, + /*above=*/false)}, + 2, + }; + states_[static_cast(RunTrackOslState::kLateSwing)] = StateParams{ + JointParams{15.899, 3.816, 5.0 * deg}, + JointParams{7.949, 0.0, 15.0 * deg}, + {MakeThreshold(Threshold::Variable::kLoad, + static_cast(0.4) * body_weight, + /*above=*/true), + MakeThreshold(Threshold::Variable::kKneeAngle, 30.0 * deg, + /*above=*/false)}, + 2, + }; + Reset(RunTrackOslState::kEarlyStance); + } + + std::array states_{}; + RunTrackOslSensorData sensor_data_{}; + RunTrackOslState state_{RunTrackOslState::kEarlyStance}; + bool running_{false}; + mjtNum knee_peak_torque_{142.272}; + mjtNum ankle_peak_torque_{168.192}; +}; + +} // namespace challenge_extra_detail + +class MyoChallengeRunTrackEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(5), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), "ctrl_dim"_.Bind(0), + "reset_type"_.Bind(std::string("random")), + "terrain"_.Bind(std::string("flat")), "start_pos"_.Bind(14.0), + "end_pos"_.Bind(-15.0), "real_width"_.Bind(1.0), + "hills_difficulties"_.Bind(std::vector{}), + "rough_difficulties"_.Bind(std::vector{}), + "stairs_difficulties"_.Bind(std::vector{}), + "reward_sparse_w"_.Bind(1.0), "reward_solved_w"_.Bind(10.0), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{}), + "test_hfield_data"_.Bind(std::vector{}), + "test_terrain_geom_rgba"_.Bind(std::vector{}), + "test_terrain_geom_pos"_.Bind(std::vector{}), + "test_terrain_type"_.Bind(-1), "test_osl_state"_.Bind(-1)); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:act_reg"_.Bind(Spec({-1}, {-inf, inf})), + "info:pain"_.Bind(Spec({-1}, {-inf, inf})), + "info:sparse"_.Bind(Spec({-1}, {-inf, inf})), + "info:solved"_.Bind(Spec({-1}, {0.0, 1.0})), + "info:terrain"_.Bind(Spec({-1}, {0.0, 4.0})), + "info:time"_.Bind(Spec({-1}, {0.0, inf})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:root_pos"_.Bind(Spec({2}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using RunTrackPixelFns = PixelObservationEnvFns; +using MyoChallengeRunTrackEnvSpec = EnvSpec; +using MyoChallengeRunTrackPixelEnvSpec = EnvSpec; + +class MyoChallengeSoccerEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(10), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), "reset_type"_.Bind(std::string("none")), + "min_agent_spawn_distance"_.Bind(1.0), "random_vel_low"_.Bind(1.0), + "random_vel_high"_.Bind(5.0), "rnd_pos_noise"_.Bind(1.0), + "rnd_joint_noise"_.Bind(0.02), + "goalkeeper_probabilities"_.Bind(std::vector{0.1, 0.45, 0.45}), + "max_time_sec"_.Bind(10.0), "reward_goal_scored_w"_.Bind(1000.0), + "reward_time_cost_w"_.Bind(-0.01), "reward_act_reg_w"_.Bind(-100.0), + "reward_pain_w"_.Bind(-10.0), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_ctrl"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_act_dot"_.Bind(std::vector{}), + "test_reset_qacc"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{}), + "test_goalkeeper_pose"_.Bind(std::vector{}), + "test_goalkeeper_velocity"_.Bind(std::vector{}), + "test_goalkeeper_noise_buffer"_.Bind(std::vector{}), + "test_goalkeeper_noise_idx"_.Bind(-1), + "test_goalkeeper_block_velocity"_.Bind(-1.0), + "test_goalkeeper_policy"_.Bind(-1)); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:goal_scored"_.Bind(Spec({-1}, {0.0, 1.0})), + "info:time_cost"_.Bind(Spec({-1}, {-inf, inf})), + "info:act_reg"_.Bind(Spec({-1}, {-inf, inf})), + "info:pain"_.Bind(Spec({-1}, {-inf, inf})), + "info:sparse"_.Bind(Spec({-1}, {0.0, 1.0})), + "info:solved"_.Bind(Spec({-1}, {0.0, 1.0})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:goalkeeper_pos"_.Bind(Spec({2}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using SoccerPixelFns = PixelObservationEnvFns; +using MyoChallengeSoccerEnvSpec = EnvSpec; +using MyoChallengeSoccerPixelEnvSpec = EnvSpec; + +class MyoChallengeChaseTagEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(10), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), "reset_type"_.Bind(std::string("init")), + "win_distance"_.Bind(0.5), "min_spawn_distance"_.Bind(2.0), + "task_choice"_.Bind(std::string("CHASE")), + "terrain"_.Bind(std::string("FLAT")), "repeller_opponent"_.Bind(false), + "hills_range"_.Bind(std::vector{0.0, 0.0}), + "rough_range"_.Bind(std::vector{0.0, 0.0}), + "relief_range"_.Bind(std::vector{0.0, 0.0}), + "chase_vel_low"_.Bind(1.0), "chase_vel_high"_.Bind(1.0), + "random_vel_low"_.Bind(-2.0), "random_vel_high"_.Bind(2.0), + "repeller_vel_low"_.Bind(0.3), "repeller_vel_high"_.Bind(1.0), + "opponent_probabilities"_.Bind(std::vector{0.1, 0.45, 0.45}), + "reward_distance_w"_.Bind(-0.1), "reward_lose_w"_.Bind(-1000.0), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_ctrl"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_act_dot"_.Bind(std::vector{}), + "test_reset_qacc"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{}), + "test_hfield_data"_.Bind(std::vector{}), + "test_terrain_geom_rgba"_.Bind(std::vector{}), + "test_terrain_geom_pos"_.Bind(std::vector{}), + "test_task"_.Bind(-1), + "test_opponent_pose"_.Bind(std::vector{}), + "test_opponent_velocity"_.Bind(std::vector{}), + "test_opponent_noise_buffer"_.Bind(std::vector{}), + "test_opponent_noise_idx"_.Bind(-1), "test_chase_velocity"_.Bind(-1.0), + "test_opponent_policy"_.Bind(-1)); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:distance"_.Bind(Spec({-1}, {0.0, inf})), + "info:lose"_.Bind(Spec({-1}, {0.0, 1.0})), + "info:sparse"_.Bind(Spec({-1}, {-inf, inf})), + "info:solved"_.Bind(Spec({-1}, {0.0, 1.0})), + "info:task"_.Bind(Spec({-1}, {0.0, 1.0})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:opponent_pose"_.Bind(Spec({3}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using ChaseTagPixelFns = PixelObservationEnvFns; +using MyoChallengeChaseTagEnvSpec = EnvSpec; +using MyoChallengeChaseTagPixelEnvSpec = EnvSpec; + +class MyoChallengeTableTennisEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(5), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), "ball_xyz_low"_.Bind(std::vector{}), + "ball_xyz_high"_.Bind(std::vector{}), "ball_qvel"_.Bind(false), + "ball_friction_low"_.Bind(std::vector{}), + "ball_friction_high"_.Bind(std::vector{}), + "paddle_mass_low"_.Bind(0.0), "paddle_mass_high"_.Bind(0.0), + "qpos_noise_low"_.Bind(std::numeric_limits::quiet_NaN()), + "qpos_noise_high"_.Bind(std::numeric_limits::quiet_NaN()), + "rally_count"_.Bind(1), "reward_reach_dist_w"_.Bind(1.0), + "reward_palm_dist_w"_.Bind(1.0), "reward_paddle_quat_w"_.Bind(2.0), + "reward_act_reg_w"_.Bind(0.5), "reward_torso_up_w"_.Bind(2.0), + "reward_sparse_w"_.Bind(100.0), "reward_solved_w"_.Bind(1000.0), + "reward_done_w"_.Bind(-10.0), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_act_dot"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{}), + "test_ball_body_pos"_.Bind(std::vector{}), + "test_ball_geom_friction"_.Bind(std::vector{}), + "test_paddle_body_mass"_.Bind(std::vector{}), + "test_init_qpos"_.Bind(std::vector{}), + "test_init_qvel"_.Bind(std::vector{})); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:reach_dist"_.Bind(Spec({-1}, {-inf, inf})), + "info:palm_dist"_.Bind(Spec({-1}, {-inf, inf})), + "info:paddle_quat"_.Bind(Spec({-1}, {-inf, inf})), + "info:torso_up"_.Bind(Spec({-1}, {-inf, inf})), + "info:act_reg"_.Bind(Spec({-1}, {-inf, inf})), + "info:sparse"_.Bind(Spec({-1}, {0.0, 1.0})), + "info:solved"_.Bind(Spec({-1}, {0.0, 1.0})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:touching_info"_.Bind(Spec({6}, {0.0, inf}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using TablePixelFns = PixelObservationEnvFns; +using MyoChallengeTableTennisEnvSpec = EnvSpec; +using MyoChallengeTableTennisPixelEnvSpec = EnvSpec; + +template +class MyoChallengeRunTrackEnvBase : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::gen_; + using Base::spec_; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum act_reg{0.0}; + mjtNum pain{0.0}; + mjtNum sparse{0.0}; + bool solved{false}; + bool done{false}; + }; + + bool normalize_act_; + int ctrl_dim_; + std::string reset_type_; + std::string terrain_; + mjtNum start_pos_; + mjtNum end_pos_; + mjtNum real_width_; + mjtNum reward_sparse_w_; + mjtNum reward_solved_w_; + std::vector hills_difficulties_; + std::vector rough_difficulties_; + std::vector stairs_difficulties_; + int terrain_geom_id_{-1}; + int terrain_hfield_id_{-1}; + int pelvis_body_id_{-1}; + int head_site_id_{-1}; + int foot_l_body_id_{-1}; + int foot_r_body_id_{-1}; + std::array foot_height_site_ids_{-1, -1, -1, -1}; + std::array grf_sensor_ids_{-1, -1}; + int osl_load_sensor_id_{-1}; + int osl_knee_torque_actuator_id_{-1}; + int osl_ankle_torque_actuator_id_{-1}; + int osl_knee_qposadr_{-1}; + int osl_ankle_qposadr_{-1}; + int osl_knee_dofadr_{-1}; + int osl_ankle_dofadr_{-1}; + std::vector biological_qposadrs_; + std::vector biological_dofadrs_; + std::vector biological_actuator_ids_; + std::vector pain_dofadrs_; + std::vector initial_hfield_data_; + std::vector initial_terrain_rgba_; + std::vector initial_terrain_pos_; + std::vector muscle_actuator_; + detail::MyoConditionState muscle_condition_state_; + challenge_extra_detail::RunTrackOslController osl_controller_; + int terrain_type_{0}; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_act_; + std::vector test_reset_act_dot_; + std::vector test_reset_qacc_warmstart_; + std::vector test_hfield_data_; + std::vector test_terrain_geom_rgba_; + std::vector test_terrain_geom_pos_; + int test_terrain_type_{-1}; + int test_osl_state_{-1}; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoChallengeRunTrackEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + myosuite::MyoSuiteModelPath(spec.config["base_path"_], + spec.config["model_path"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + ctrl_dim_(spec.config["ctrl_dim"_]), + reset_type_(spec.config["reset_type"_]), + terrain_(spec.config["terrain"_]), + start_pos_(spec.config["start_pos"_]), + end_pos_(spec.config["end_pos"_]), + real_width_(spec.config["real_width"_]), + reward_sparse_w_(spec.config["reward_sparse_w"_]), + reward_solved_w_(spec.config["reward_solved_w"_]), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])), + test_hfield_data_( + detail::ToMjtVector(spec.config["test_hfield_data"_])), + test_terrain_geom_rgba_( + detail::ToMjtVector(spec.config["test_terrain_geom_rgba"_])), + test_terrain_geom_pos_( + detail::ToMjtVector(spec.config["test_terrain_geom_pos"_])), + test_terrain_type_(spec.config["test_terrain_type"_]), + test_osl_state_(spec.config["test_osl_state"_]) { + hills_difficulties_ = + detail::ToMjtVector(spec.config["hills_difficulties"_]); + rough_difficulties_ = + detail::ToMjtVector(spec.config["rough_difficulties"_]); + stairs_difficulties_ = + detail::ToMjtVector(spec.config["stairs_difficulties"_]); + std::reverse(hills_difficulties_.begin(), hills_difficulties_.end()); + std::reverse(rough_difficulties_.begin(), rough_difficulties_.end()); + std::reverse(stairs_difficulties_.begin(), stairs_difficulties_.end()); + ValidateConfig(); + CacheIds(); + detail::BuildMuscleMask(model_, &muscle_actuator_); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + mjtNum total_body_mass = 0.0; + for (int body = 0; body < model_->nbody; ++body) { + total_body_mass += model_->body_mass[body]; + } + osl_controller_.Initialize(total_body_mass); + InitializeRobotEnv(); + } + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option, true); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + detail::ResetMyoConditionState(&muscle_condition_state_); + ResetToInitialState(); + RestoreTrackTerrainState(); + ApplyTrackTerrainReset(); + ApplyResetState(); + InvalidateRenderCache(); + CaptureResetState(); + RewardInfo reward = ComputeRewardInfo(); + WriteState(reward, true, 0.0); + } + + void Step(const Action& action) override { + const auto* raw = static_cast(action["action"_].Data()); + std::vector ctrl = BuildAction(raw); + detail::ApplyMyoSuiteAction(model_, data_, muscle_actuator_, normalize_act_, + ctrl.data()); + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + mj_forward(model_, data_); + ++elapsed_step_; + RewardInfo reward = ComputeRewardInfo(); + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, false, reward.dense_reward); + } + + private: + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_] || + model_->nv != spec_.config["qvel_dim"_] || + model_->na != spec_.config["act_dim"_] || ctrl_dim_ != model_->nu || + spec_.config["action_dim"_] != model_->na) { + throw std::runtime_error("RunTrack dims do not match model."); + } + } + + void CacheIds() { + pelvis_body_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_BODY, "pelvis"); + head_site_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_SITE, "head"); + foot_l_body_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_BODY, "talus_l"); + foot_r_body_id_ = challenge_extra_detail::RequireId(model_, mjOBJ_BODY, + "osl_foot_assembly"); + grf_sensor_ids_[0] = + challenge_extra_detail::RequireId(model_, mjOBJ_SENSOR, "l_foot"); + grf_sensor_ids_[1] = + challenge_extra_detail::RequireId(model_, mjOBJ_SENSOR, "l_toes"); + terrain_geom_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_GEOM, "terrain"); + terrain_hfield_id_ = model_->geom_dataid[terrain_geom_id_]; + osl_load_sensor_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_SENSOR, "r_osl_load"); + osl_knee_torque_actuator_id_ = challenge_extra_detail::RequireId( + model_, mjOBJ_ACTUATOR, "osl_knee_torque_actuator"); + osl_ankle_torque_actuator_id_ = challenge_extra_detail::RequireId( + model_, mjOBJ_ACTUATOR, "osl_ankle_torque_actuator"); + int osl_knee_joint_id = challenge_extra_detail::RequireId( + model_, mjOBJ_JOINT, "osl_knee_angle_r"); + int osl_ankle_joint_id = challenge_extra_detail::RequireId( + model_, mjOBJ_JOINT, "osl_ankle_angle_r"); + osl_knee_qposadr_ = model_->jnt_qposadr[osl_knee_joint_id]; + osl_ankle_qposadr_ = model_->jnt_qposadr[osl_ankle_joint_id]; + osl_knee_dofadr_ = model_->jnt_dofadr[osl_knee_joint_id]; + osl_ankle_dofadr_ = model_->jnt_dofadr[osl_ankle_joint_id]; + foot_height_site_ids_[0] = + challenge_extra_detail::RequireId(model_, mjOBJ_SITE, "r_heel_btm"); + foot_height_site_ids_[1] = + challenge_extra_detail::RequireId(model_, mjOBJ_SITE, "r_toe_btm"); + foot_height_site_ids_[2] = + challenge_extra_detail::RequireId(model_, mjOBJ_SITE, "l_heel_btm"); + foot_height_site_ids_[3] = + challenge_extra_detail::RequireId(model_, mjOBJ_SITE, "l_toe_btm"); + static const std::vector biological_joints = { + "hip_adduction_l", + "hip_flexion_l", + "hip_rotation_l", + "hip_adduction_r", + "hip_flexion_r", + "hip_rotation_r", + "knee_angle_l", + "knee_angle_l_beta_rotation1", + "knee_angle_l_beta_translation1", + "knee_angle_l_beta_translation2", + "knee_angle_l_rotation2", + "knee_angle_l_rotation3", + "knee_angle_l_translation1", + "knee_angle_l_translation2", + "mtp_angle_l", + "ankle_angle_l", + "subtalar_angle_l", + }; + static const std::vector pain_joints = { + "hip_adduction_l", "hip_adduction_r", "hip_flexion_l", + "hip_flexion_r", "hip_rotation_l", "hip_rotation_r", + "knee_angle_l", "knee_angle_l_rotation2", "knee_angle_l_rotation3", + "mtp_angle_l", "ankle_angle_l", "subtalar_angle_l", + }; + static const std::vector biological_actuators = { + "addbrev_l", "addbrev_r", "addlong_l", "addlong_r", + "addmagDist_l", "addmagIsch_l", "addmagMid_l", "addmagProx_l", + "bflh_l", "bfsh_l", "edl_l", "ehl_l", + "fdl_l", "fhl_l", "gaslat_l", "gasmed_l", + "glmax1_l", "glmax1_r", "glmax2_l", "glmax2_r", + "glmax3_l", "glmax3_r", "glmed1_l", "glmed1_r", + "glmed2_l", "glmed2_r", "glmed3_l", "glmed3_r", + "glmin1_l", "glmin1_r", "glmin2_l", "glmin2_r", + "glmin3_l", "glmin3_r", "grac_l", "iliacus_l", + "iliacus_r", "perbrev_l", "perlong_l", "piri_l", + "piri_r", "psoas_l", "psoas_r", "recfem_l", + "sart_l", "semimem_l", "semiten_l", "soleus_l", + "tfl_l", "tibant_l", "tibpost_l", "vasint_l", + "vaslat_l", "vasmed_l", + }; + biological_qposadrs_ = + challenge_extra_detail::CollectJointQposAdrs(model_, biological_joints); + biological_dofadrs_ = + challenge_extra_detail::CollectJointDofAdrs(model_, biological_joints); + for (const std::string& actuator_name : biological_actuators) { + biological_actuator_ids_.push_back(challenge_extra_detail::RequireId( + model_, mjOBJ_ACTUATOR, actuator_name)); + } + pain_dofadrs_ = + challenge_extra_detail::CollectJointDofAdrs(model_, pain_joints); + detail::CopyModelGeomRgba(model_, terrain_geom_id_, &initial_terrain_rgba_); + detail::CopyModelGeomPos(model_, terrain_geom_id_, &initial_terrain_pos_); + if (terrain_hfield_id_ >= 0) { + detail::CopyModelHfieldData(model_, terrain_hfield_id_, + &initial_hfield_data_); + } + int expected_obs = static_cast(biological_qposadrs_.size()) + + static_cast(biological_dofadrs_.size()) + 2 + 4 + + model_->na + model_->na + model_->na + model_->na + 2 + + 2; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error("RunTrack obs_dim does not match model."); + } + } + + void RestoreTrackTerrainState() { + detail::RestoreModelGeomRgba(model_, terrain_geom_id_, + initial_terrain_rgba_); + detail::RestoreModelGeomPos(model_, terrain_geom_id_, initial_terrain_pos_); + if (terrain_hfield_id_ >= 0) { + detail::RestoreModelHfieldData(model_, terrain_hfield_id_, + initial_hfield_data_); + } + } + + void SetTrackTerrainVisible() { + std::vector rgba = initial_terrain_rgba_; + std::vector pos = initial_terrain_pos_; + rgba[3] = 1.0; + pos[0] = 0.0; + pos[1] = 0.0; + pos[2] = 0.005; + detail::RestoreModelGeomRgba(model_, terrain_geom_id_, rgba); + detail::RestoreModelGeomPos(model_, terrain_geom_id_, pos); + } + + void ClearTrackTerrain() { + if (terrain_hfield_id_ < 0) { + return; + } + int rows = model_->hfield_nrow[terrain_hfield_id_]; + int cols = model_->hfield_ncol[terrain_hfield_id_]; + detail::RestoreModelHfieldData( + model_, terrain_hfield_id_, + std::vector(rows * cols, static_cast(0.0))); + } + + void FillStairsTrackPatch(int patch_start, int patch_end, int index) { + int rows = patch_end - patch_start; + if (rows <= 0) { + return; + } + int stair_height_count = 3; + int stair_count = stair_height_count * 2; + int stair_flat = rows / stair_count; + mjtNum stair_height = stairs_difficulties_.at(index); + int filled_rows = stair_flat * stair_count; + int adr = model_->hfield_adr[terrain_hfield_id_]; + int cols = model_->hfield_ncol[terrain_hfield_id_]; + mjtNum height = 0.0; + for (int stair = 0; stair < stair_count; ++stair) { + for (int row = 0; row < stair_flat; ++row) { + int global_row = patch_start + stair * stair_flat + row; + if (global_row >= patch_start + filled_rows) { + break; + } + for (int col = 0; col < cols; ++col) { + model_->hfield_data[adr + global_row * cols + col] = + static_cast(height); + } + } + if (stair < stair_count / 2) { + height += stair_height; + } else { + height -= stair_height; + } + } + } + + void FillHillyTrackPatch(int patch_start, int patch_end, int index) { + int rows = patch_end - patch_start; + if (rows <= 0) { + return; + } + int cols = model_->hfield_ncol[terrain_hfield_id_]; + int total = rows * cols; + std::vector data(total); + for (int i = 0; i < total; ++i) { + data[i] = static_cast(std::sin( + static_cast(i) / std::max(total - 1, 1) * detail::kPi)); + } + mjtNum low = *std::min_element(data.begin(), data.end()); + mjtNum high = *std::max_element(data.begin(), data.end()); + mjtNum denom = std::max(high - low, static_cast(1e-12)); + mjtNum scalar = hills_difficulties_.at(index); + int adr = model_->hfield_adr[terrain_hfield_id_]; + for (int row = 0; row < rows; ++row) { + for (int col = 0; col < cols; ++col) { + int src = row * cols + col; + int dst_row = rows - 1 - row; + int dst_col = cols - 1 - col; + mjtNum value = (data[src] - low) / denom * scalar; + model_->hfield_data[adr + (patch_start + dst_row) * cols + dst_col] = + static_cast(value); + } + } + } + + void FillRoughTrackPatch(int patch_start, int patch_end, int index) { + int rows = patch_end - patch_start; + if (rows <= 0) { + return; + } + int cols = model_->hfield_ncol[terrain_hfield_id_]; + std::vector fill_data(rows * cols); + for (mjtNum& value : fill_data) { + value = std::uniform_real_distribution(-1.0, 1.0)(gen_); + } + mjtNum scalar = std::uniform_real_distribution( + 0.0, static_cast(rough_difficulties_.at(index)))(gen_); + mjtNum low = *std::min_element(fill_data.begin(), fill_data.end()); + mjtNum high = *std::max_element(fill_data.begin(), fill_data.end()); + mjtNum denom = std::max(high - low, static_cast(1e-12)); + int adr = model_->hfield_adr[terrain_hfield_id_]; + for (int row = 0; row < rows; ++row) { + for (int col = 0; col < cols; ++col) { + mjtNum normalized = (fill_data[row * cols + col] - low) / denom; + model_->hfield_data[adr + (patch_start + row) * cols + col] = + static_cast(normalized * scalar); + } + } + } + + void ApplyTrackTerrainReset() { + SetTrackTerrainVisible(); + ClearTrackTerrain(); + if (test_terrain_type_ >= 0) { + terrain_type_ = test_terrain_type_; + } + if (!test_terrain_geom_rgba_.empty()) { + detail::RestoreModelGeomRgba(model_, terrain_geom_id_, + test_terrain_geom_rgba_); + } + if (!test_terrain_geom_pos_.empty()) { + detail::RestoreModelGeomPos(model_, terrain_geom_id_, + test_terrain_geom_pos_); + } + if (!test_hfield_data_.empty()) { + detail::RestoreModelHfieldData(model_, terrain_hfield_id_, + test_hfield_data_); + return; + } + terrain_type_ = 0; + if (terrain_ == "flat") { + return; + } + if (terrain_ != "random") { + throw std::runtime_error("Unsupported RunTrack terrain reset type."); + } + using TerrainFn = void (MyoChallengeRunTrackEnvBase::*)(int, int, int); + std::vector> terrain_fns = { + {&MyoChallengeRunTrackEnvBase::FillStairsTrackPatch, 3}, + {&MyoChallengeRunTrackEnvBase::FillHillyTrackPatch, 1}, + {&MyoChallengeRunTrackEnvBase::FillRoughTrackPatch, 2}, + }; + int terrain_choice = std::uniform_int_distribution( + 0, static_cast(terrain_fns.size()) - 1)(gen_); + terrain_type_ = terrain_fns[terrain_choice].second; + const std::vector* difficulties = nullptr; + switch (terrain_type_) { + case 1: + difficulties = &hills_difficulties_; + break; + case 2: + difficulties = &rough_difficulties_; + break; + case 3: + difficulties = &stairs_difficulties_; + break; + default: + return; + } + if (difficulties->size() < 2) { + return; + } + int rows = model_->hfield_nrow[terrain_hfield_id_]; + int patch_rows = rows / static_cast(difficulties->size()); + if (patch_rows <= 0) { + return; + } + int patch_start = 0; + for (std::size_t i = 0; i + 1 < difficulties->size(); ++i) { + int patch_end = patch_start + patch_rows; + (this->*terrain_fns[terrain_choice].first)( + patch_start, std::min(patch_end, rows), static_cast(i)); + patch_start = patch_end; + } + } + + std::pair, std::vector> RandomizedResetState() { + int key = std::uniform_int_distribution(0, 2)(gen_); + std::vector qpos(model_->key_qpos + key * model_->nq, + model_->key_qpos + (key + 1) * model_->nq); + std::vector qvel(model_->key_qvel + key * model_->nv, + model_->key_qvel + (key + 1) * model_->nv); + osl_controller_.Reset( + key == 1 ? challenge_extra_detail::RunTrackOslState::kEarlySwing + : challenge_extra_detail::RunTrackOslState::kEarlyStance); + std::uniform_real_distribution x_dist( + -static_cast(real_width_) * 0.8, + static_cast(real_width_) * 0.8); + qpos[0] = static_cast(x_dist(gen_)); + qpos[1] = start_pos_ + 1.0; + return {qpos, qvel}; + } + + void AdjustModelHeight() { + mjtNum min_height = std::numeric_limits::infinity(); + for (int site_id : foot_height_site_ids_) { + min_height = std::min(min_height, data_->site_xpos[site_id * 3 + 2]); + } + data_->qpos[2] += static_cast(0.005) - min_height; + } + + challenge_extra_detail::RunTrackOslSensorData OslSensorData() const { + int sensor_adr = model_->sensor_adr[osl_load_sensor_id_]; + return { + data_->qpos[osl_knee_qposadr_], data_->qvel[osl_knee_dofadr_], + data_->qpos[osl_ankle_qposadr_], data_->qvel[osl_ankle_dofadr_], + -data_->sensordata[sensor_adr + 1], + }; + } + + std::vector BuildAction(const float* raw) { + std::vector ctrl(model_->nu, 0.0f); + std::copy(raw, raw + model_->na, ctrl.begin()); + osl_controller_.Update(OslSensorData()); + auto torques = osl_controller_.GetTorques(); + const std::array, 2> actuator_data = {{ + {osl_knee_torque_actuator_id_, torques[0]}, + {osl_ankle_torque_actuator_id_, torques[1]}, + }}; + for (const auto& [actuator_id, torque] : actuator_data) { + mjtNum ctrl_value = torque / model_->actuator_gear[actuator_id * 6]; + mjtNum min_ctrl = model_->actuator_ctrlrange[actuator_id * 2]; + mjtNum max_ctrl = model_->actuator_ctrlrange[actuator_id * 2 + 1]; + ctrl_value = challenge_extra_detail::Clip(ctrl_value, min_ctrl, max_ctrl); + if (normalize_act_) { + mjtNum ctrl_mean = (min_ctrl + max_ctrl) * static_cast(0.5); + mjtNum ctrl_rng = (max_ctrl - min_ctrl) * static_cast(0.5); + ctrl_value = (ctrl_value - ctrl_mean) / ctrl_rng; + } + ctrl[actuator_id] = static_cast(ctrl_value); + } + return ctrl; + } + + void ResetSimulationState(const std::vector& qpos, + const std::vector& qvel) { + mj_resetData(model_, data_); + data_->time = initial_time_; + detail::RestoreVector(qpos, data_->qpos); + detail::RestoreVector(qvel, data_->qvel); + if (model_->nu != 0) { + mju_zero(data_->ctrl, model_->nu); + } + if (model_->na != 0) { + mju_zero(data_->act, model_->na); + } + mj_forward(model_, data_); + } + + void ApplyResetState() { + osl_controller_.Reset( + challenge_extra_detail::RunTrackOslState::kEarlyStance); + std::vector qpos; + std::vector qvel; + if (!test_reset_qpos_.empty()) { + qpos = test_reset_qpos_; + qvel = test_reset_qvel_; + } else if (reset_type_ == "random") { + std::tie(qpos, qvel) = RandomizedResetState(); + } else { + osl_controller_.Reset( + challenge_extra_detail::RunTrackOslState::kEarlyStance); + qpos.assign(model_->key_qpos, model_->key_qpos + model_->nq); + qvel.assign(model_->key_qvel, model_->key_qvel + model_->nv); + } + + ResetSimulationState(qpos, qvel); + if (test_reset_qpos_.empty() && reset_type_ != "init") { + AdjustModelHeight(); + qpos.assign(data_->qpos, data_->qpos + model_->nq); + qvel.assign(data_->qvel, data_->qvel + model_->nv); + ResetSimulationState(qpos, qvel); + } + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + mj_forward(model_, data_); + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, data_->qacc_warmstart); + } + if (test_osl_state_ >= 0) { + osl_controller_.SetState( + static_cast( + test_osl_state_)); + } + osl_controller_.Start(); + } + + bool Fallen() const { + mjtNum head_z = data_->site_xpos[head_site_id_ * 3 + 2]; + mjtNum left_z = data_->xpos[foot_l_body_id_ * 3 + 2]; + mjtNum right_z = data_->xpos[foot_r_body_id_ * 3 + 2]; + mjtNum mean_feet_z = (left_z + right_z) * 0.5; + return head_z - mean_feet_z < 0.2 || head_z < 0.2; + } + + RewardInfo ComputeRewardInfo() const { + RewardInfo reward; + reward.act_reg = challenge_extra_detail::MeanSquareAct(model_, data_); + reward.pain = challenge_extra_detail::AverageJointLimitForce(model_, data_, + pain_dofadrs_); + mjtNum x_pos = data_->qpos[0]; + mjtNum y_pos = data_->qpos[1]; + mjtNum y_vel = data_->qvel[1]; + reward.sparse = -y_vel; + reward.solved = y_pos < end_pos_; + reward.done = x_pos > real_width_ || x_pos < -real_width_ || + y_pos > start_pos_ + 2.0 || Fallen() || reward.solved; + reward.dense_reward = reward_sparse_w_ * reward.sparse + + reward_solved_w_ * static_cast(reward.solved); + return reward; + } + + std::vector Observation() const { + std::vector obs; + obs.reserve(spec_.config["obs_dim"_]); + for (int adr : biological_qposadrs_) { + obs.push_back(data_->qpos[adr]); + } + for (int adr : biological_dofadrs_) { + obs.push_back(data_->qvel[adr] * Dt()); + } + for (int sensor_id : grf_sensor_ids_) { + int adr = model_->sensor_adr[sensor_id]; + obs.push_back(data_->sensordata[adr]); + } + obs.insert(obs.end(), data_->xquat + pelvis_body_id_ * 4, + data_->xquat + pelvis_body_id_ * 4 + 4); + obs.push_back(data_->qpos[0]); + obs.push_back(data_->qpos[1]); + obs.push_back(data_->qvel[0]); + obs.push_back(data_->qvel[1]); + for (int actuator_id : biological_actuator_ids_) { + obs.push_back(data_->actuator_length[actuator_id]); + } + for (int actuator_id : biological_actuator_ids_) { + obs.push_back(challenge_extra_detail::Clip( + data_->actuator_velocity[actuator_id], -100.0, 100.0)); + } + for (int actuator_id : biological_actuator_ids_) { + obs.push_back(challenge_extra_detail::Clip( + data_->actuator_force[actuator_id] / 1000.0, -100.0, 100.0)); + } + obs.insert(obs.end(), data_->act, data_->act + model_->na); + return obs; + } + + void WriteState(const RewardInfo& reward, bool reset, float reward_value) { + auto obs = Observation(); + auto state = Allocate(); + if constexpr (!kFromPixels) { + AssignObservation("obs", &state["obs"_], obs.data(), obs.size(), reset); + } + state["reward"_] = reward_value; + state["discount"_] = 1.0f; + state["done"_] = done_; + state["trunc"_] = elapsed_step_ >= max_episode_steps_; + state["elapsed_step"_] = elapsed_step_; + state["info:act_reg"_] = reward.act_reg; + state["info:pain"_] = reward.pain; + state["info:sparse"_] = reward.sparse; + state["info:solved"_] = static_cast(reward.solved); + state["info:terrain"_] = static_cast(terrain_type_); + state["info:time"_] = data_->time; +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + std::array root_pos = {data_->qpos[0], data_->qpos[1]}; + state["info:root_pos"_].Assign(root_pos.data(), 2); + if constexpr (kFromPixels) { + AssignPixelObservation("obs:pixels", &state["obs:pixels"_], reset); + } + } +}; + +template +class MyoChallengeSoccerEnvBase : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::gen_; + using Base::spec_; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum goal_scored{0.0}; + mjtNum time_cost{0.0}; + mjtNum act_reg{0.0}; + mjtNum pain{0.0}; + mjtNum sparse{0.0}; + bool solved{false}; + bool done{false}; + }; + + static constexpr mjtNum kGoalX = 50.0; + static constexpr mjtNum kGoalYMin = -3.3; + static constexpr mjtNum kGoalYMax = 3.3; + static constexpr mjtNum kGoalZMin = 0.0; + static constexpr mjtNum kGoalZMax = 2.2; + static constexpr int kGoalkeeperBlockBall = 0; + static constexpr int kGoalkeeperRandom = 1; + static constexpr int kGoalkeeperStationary = 2; + + bool normalize_act_; + std::string reset_type_; + mjtNum min_agent_spawn_distance_; + mjtNum random_vel_low_; + mjtNum random_vel_high_; + mjtNum rnd_pos_noise_; + mjtNum rnd_joint_noise_; + mjtNum max_time_sec_; + mjtNum reward_goal_scored_w_; + mjtNum reward_time_cost_w_; + mjtNum reward_act_reg_w_; + mjtNum reward_pain_w_; + std::array goalkeeper_probabilities_{0.1, 0.45, 0.45}; + int soccer_ball_body_id_{-1}; + int goalkeeper_body_id_{-1}; + int goalkeeper_mocap_id_{-1}; + int pelvis_body_id_{-1}; + int torso_body_id_{-1}; + int root_joint_id_{-1}; + std::array grf_sensor_ids_{-1, -1, -1, -1}; + std::vector internal_qposadrs_; + std::vector internal_dofadrs_; + std::vector pain_dofadrs_; + std::vector muscle_actuator_ids_; + std::vector muscle_actuator_; + detail::MyoConditionState muscle_condition_state_; + challenge_extra_detail::ColoredNoiseProcess goalkeeper_noise_process_{2.0, 2, + 10}; + int goalkeeper_policy_{kGoalkeeperStationary}; + std::array goalkeeper_velocity_{0.0, 0.0}; + mjtNum goalkeeper_block_velocity_{1.0}; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_ctrl_; + std::vector test_reset_act_; + std::vector test_reset_act_dot_; + std::vector test_reset_qacc_; + std::vector test_reset_qacc_warmstart_; + std::vector test_goalkeeper_pose_; + std::vector test_goalkeeper_velocity_; + std::vector test_goalkeeper_noise_buffer_; + int test_goalkeeper_noise_idx_{-1}; + mjtNum test_goalkeeper_block_velocity_{-1.0}; + int test_goalkeeper_policy_{-1}; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoChallengeSoccerEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + myosuite::MyoSuiteModelPath(spec.config["base_path"_], + spec.config["model_path"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + reset_type_(spec.config["reset_type"_]), + min_agent_spawn_distance_(spec.config["min_agent_spawn_distance"_]), + random_vel_low_(spec.config["random_vel_low"_]), + random_vel_high_(spec.config["random_vel_high"_]), + rnd_pos_noise_(spec.config["rnd_pos_noise"_]), + rnd_joint_noise_(spec.config["rnd_joint_noise"_]), + max_time_sec_(spec.config["max_time_sec"_]), + reward_goal_scored_w_(spec.config["reward_goal_scored_w"_]), + reward_time_cost_w_(spec.config["reward_time_cost_w"_]), + reward_act_reg_w_(spec.config["reward_act_reg_w"_]), + reward_pain_w_(spec.config["reward_pain_w"_]), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_ctrl_(detail::ToMjtVector(spec.config["test_reset_ctrl"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_act_dot_( + detail::ToMjtVector(spec.config["test_reset_act_dot"_])), + test_reset_qacc_(detail::ToMjtVector(spec.config["test_reset_qacc"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])), + test_goalkeeper_pose_( + detail::ToMjtVector(spec.config["test_goalkeeper_pose"_])), + test_goalkeeper_velocity_( + detail::ToMjtVector(spec.config["test_goalkeeper_velocity"_])), + test_goalkeeper_noise_buffer_( + detail::ToMjtVector(spec.config["test_goalkeeper_noise_buffer"_])), + test_goalkeeper_noise_idx_(spec.config["test_goalkeeper_noise_idx"_]), + test_goalkeeper_block_velocity_( + spec.config["test_goalkeeper_block_velocity"_]), + test_goalkeeper_policy_(spec.config["test_goalkeeper_policy"_]) { + auto probs = spec.config["goalkeeper_probabilities"_]; + if (probs.size() != 3) { + throw std::runtime_error("Expected three goalkeeper probabilities."); + } + for (int i = 0; i < 3; ++i) { + goalkeeper_probabilities_[i] = static_cast(probs[i]); + } + goalkeeper_block_velocity_ = challenge_detail::UniformScalar( + &gen_, random_vel_low_, random_vel_high_); + ValidateConfig(); + CacheIds(); + detail::BuildMuscleMask(model_, &muscle_actuator_); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + for (int actuator = 0; actuator < model_->nu; ++actuator) { + if (muscle_actuator_[actuator]) { + muscle_actuator_ids_.push_back(actuator); + } + } + InitializeRobotEnv(); + } + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option, true); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + detail::ResetMyoConditionState(&muscle_condition_state_); + ResetToInitialState(); + ApplyResetState(); + ResetGoalkeeper(); + ApplyGoalkeeperTestState(); + mj_forward(model_, data_); + InvalidateRenderCache(); + CaptureResetState(); + RewardInfo reward = ComputeRewardInfo(); + WriteState(reward, true, 0.0); + } + + void Step(const Action& action) override { + UpdateGoalkeeperState(); + const auto* raw = static_cast(action["action"_].Data()); + detail::ApplyMyoSuiteAction(model_, data_, muscle_actuator_, normalize_act_, + raw); + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + detail::RefreshObservedMyoSuiteState(model_, data_); + ++elapsed_step_; + RewardInfo reward = ComputeRewardInfo(); + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, false, reward.dense_reward); + } + + private: + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_] || + model_->nv != spec_.config["qvel_dim"_] || + model_->na != spec_.config["act_dim"_] || + model_->nu != spec_.config["action_dim"_]) { + throw std::runtime_error("Soccer dims do not match model."); + } + } + + void CacheIds() { + soccer_ball_body_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_BODY, "soccer_ball"); + goalkeeper_body_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_BODY, "goalkeeper"); + goalkeeper_mocap_id_ = model_->body_mocapid[goalkeeper_body_id_]; + pelvis_body_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_BODY, "pelvis"); + torso_body_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_BODY, "torso"); + root_joint_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_JOINT, "root"); + grf_sensor_ids_[0] = + challenge_extra_detail::RequireId(model_, mjOBJ_SENSOR, "r_foot"); + grf_sensor_ids_[1] = + challenge_extra_detail::RequireId(model_, mjOBJ_SENSOR, "r_toes"); + grf_sensor_ids_[2] = + challenge_extra_detail::RequireId(model_, mjOBJ_SENSOR, "l_foot"); + grf_sensor_ids_[3] = + challenge_extra_detail::RequireId(model_, mjOBJ_SENSOR, "l_toes"); + static const std::vector k_myo_joints = { + "Abs_r3", + "Abs_t1", + "Abs_t2", + "L1_L2_AR", + "L1_L2_FE", + "L1_L2_LB", + "L2_L3_AR", + "L2_L3_FE", + "L2_L3_LB", + "L3_L4_AR", + "L3_L4_FE", + "L3_L4_LB", + "L4_L5_AR", + "L4_L5_FE", + "L4_L5_LB", + "ankle_angle_l", + "ankle_angle_r", + "axial_rotation", + "flex_extension", + "hip_adduction_l", + "hip_adduction_r", + "hip_flexion_l", + "hip_flexion_r", + "hip_rotation_l", + "hip_rotation_r", + "knee_angle_l", + "knee_angle_l_beta_rotation1", + "knee_angle_l_beta_translation1", + "knee_angle_l_beta_translation2", + "knee_angle_l_rotation2", + "knee_angle_l_rotation3", + "knee_angle_l_translation1", + "knee_angle_l_translation2", + "knee_angle_r", + "knee_angle_r_beta_rotation1", + "knee_angle_r_beta_translation1", + "knee_angle_r_beta_translation2", + "knee_angle_r_rotation2", + "knee_angle_r_rotation3", + "knee_angle_r_translation1", + "knee_angle_r_translation2", + "lat_bending", + "mtp_angle_l", + "mtp_angle_r", + "subtalar_angle_l", + "subtalar_angle_r", + }; + internal_qposadrs_ = + challenge_extra_detail::CollectJointQposAdrs(model_, k_myo_joints); + internal_dofadrs_ = + challenge_extra_detail::CollectJointDofAdrs(model_, k_myo_joints); + static const std::vector pain_joints = { + "hip_adduction_l", "hip_adduction_r", "hip_flexion_l", + "hip_flexion_r", "hip_rotation_l", "hip_rotation_r", + "knee_angle_l", "knee_angle_l_rotation2", "knee_angle_l_rotation3", + "mtp_angle_l", "ankle_angle_l", "subtalar_angle_l", + }; + pain_dofadrs_ = + challenge_extra_detail::CollectJointDofAdrs(model_, pain_joints); + int expected_obs = static_cast(internal_qposadrs_.size()) + + static_cast(internal_dofadrs_.size()) + 4 + 4 + 3 + + 7 + 6 + model_->na + model_->na + model_->na + + model_->na; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error("Soccer obs_dim does not match model."); + } + } + + void SetGoalkeeperPose(mjtNum x, mjtNum y, mjtNum yaw) { + y = challenge_extra_detail::Clip(y, kGoalYMin, kGoalYMax); + data_->mocap_pos[goalkeeper_mocap_id_ * 3 + 0] = x; + data_->mocap_pos[goalkeeper_mocap_id_ * 3 + 1] = y; + data_->mocap_pos[goalkeeper_mocap_id_ * 3 + 2] = 0.0; + auto quat = challenge_extra_detail::YawToQuat(yaw); + std::memcpy(data_->mocap_quat + goalkeeper_mocap_id_ * 4, quat.data(), + sizeof(mjtNum) * 4); + } + + std::array GoalkeeperPose() const { + std::array pose = { + data_->mocap_pos[goalkeeper_mocap_id_ * 3 + 0], + data_->mocap_pos[goalkeeper_mocap_id_ * 3 + 1], + challenge_extra_detail::QuatYaw(data_->mocap_quat + + goalkeeper_mocap_id_ * 4), + }; + return pose; + } + + void SampleGoalkeeperPolicy() { + mjtNum u = challenge_detail::UniformScalar(&gen_, 0.0, 1.0); + if (u < goalkeeper_probabilities_[0]) { + goalkeeper_policy_ = kGoalkeeperBlockBall; + } else if (u < + goalkeeper_probabilities_[0] + goalkeeper_probabilities_[1]) { + goalkeeper_policy_ = kGoalkeeperRandom; + } else { + goalkeeper_policy_ = kGoalkeeperStationary; + } + } + + void ResetGoalkeeper() { + goalkeeper_noise_process_.SetScale(goalkeeper_block_velocity_); + goalkeeper_noise_process_.Reset(&gen_); + goalkeeper_velocity_ = {0.0, 0.0}; + SampleGoalkeeperPolicy(); + mjtNum y = challenge_detail::UniformScalar(&gen_, kGoalYMin, kGoalYMax); + SetGoalkeeperPose(kGoalX, y, 0.0); + goalkeeper_velocity_ = {0.0, 0.0}; + goalkeeper_block_velocity_ = challenge_detail::UniformScalar( + &gen_, random_vel_low_, random_vel_high_); + } + + void UpdateGoalkeeperState() { + std::array command = {0.0, 0.0}; + if (goalkeeper_policy_ == kGoalkeeperBlockBall) { + mjtNum ball_y = data_->xpos[soccer_ball_body_id_ * 3 + 1]; + command[0] = challenge_extra_detail::Clip(ball_y - GoalkeeperPose()[1], + -goalkeeper_block_velocity_, + goalkeeper_block_velocity_); + } else if (goalkeeper_policy_ == kGoalkeeperRandom) { + auto sample = goalkeeper_noise_process_.Sample(&gen_); + command[0] = challenge_extra_detail::Clip( + sample[0], -goalkeeper_block_velocity_, goalkeeper_block_velocity_); + command[1] = challenge_extra_detail::Clip( + sample[1], -goalkeeper_block_velocity_, goalkeeper_block_velocity_); + } + auto pose = GoalkeeperPose(); + pose[1] += Dt() * command[0]; + SetGoalkeeperPose(pose[0], pose[1], pose[2]); + goalkeeper_velocity_ = command; + } + + void ApplyGoalkeeperTestState() { + if (test_goalkeeper_pose_.size() == 3) { + SetGoalkeeperPose(test_goalkeeper_pose_[0], test_goalkeeper_pose_[1], + test_goalkeeper_pose_[2]); + } + if (test_goalkeeper_velocity_.size() == 2) { + goalkeeper_velocity_[0] = test_goalkeeper_velocity_[0]; + goalkeeper_velocity_[1] = test_goalkeeper_velocity_[1]; + } + if (!test_goalkeeper_noise_buffer_.empty()) { + goalkeeper_noise_process_.SetState(test_goalkeeper_noise_buffer_, + test_goalkeeper_noise_idx_); + } + if (test_goalkeeper_block_velocity_ >= 0.0) { + goalkeeper_block_velocity_ = test_goalkeeper_block_velocity_; + } + if (test_goalkeeper_policy_ >= 0) { + goalkeeper_policy_ = test_goalkeeper_policy_; + } + } + + void ApplyResetState() { + if (!test_reset_qpos_.empty()) { + detail::RestoreVector(test_reset_qpos_, data_->qpos); + detail::RestoreVector(test_reset_qvel_, data_->qvel); + } else { + detail::RestoreVector( + std::vector(model_->key_qpos, model_->key_qpos + model_->nq), + data_->qpos); + detail::RestoreVector( + std::vector(model_->key_qvel, model_->key_qvel + model_->nv), + data_->qvel); + if (reset_type_ == "random") { + for (int adr : internal_qposadrs_) { + data_->qpos[adr] += challenge_detail::UniformScalar( + &gen_, -std::abs(rnd_joint_noise_), std::abs(rnd_joint_noise_)); + } + data_->qpos[model_->jnt_qposadr[root_joint_id_] + 0] += + challenge_detail::UniformScalar(&gen_, -rnd_pos_noise_, 0.0); + data_->qpos[model_->jnt_qposadr[root_joint_id_] + 1] += + challenge_detail::UniformScalar(&gen_, -rnd_pos_noise_, + rnd_pos_noise_); + } + } + mj_forward(model_, data_); + bool rerun_forward = false; + if (!test_reset_ctrl_.empty()) { + detail::RestoreVector(test_reset_ctrl_, data_->ctrl); + rerun_forward = true; + } + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + rerun_forward = true; + } + if (!test_reset_act_dot_.empty()) { + detail::RestoreVector(test_reset_act_dot_, data_->act_dot); + } + if (!test_reset_qacc_.empty()) { + detail::RestoreVector(test_reset_qacc_, data_->qacc); + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, data_->qacc_warmstart); + rerun_forward = true; + } + if (rerun_forward) { + mj_forward(model_, data_); + } + } + + bool GoalScored() const { + const mjtNum* ball_pos = data_->xpos + soccer_ball_body_id_ * 3; + return ball_pos[0] >= kGoalX && ball_pos[1] >= kGoalYMin && + ball_pos[1] <= kGoalYMax && ball_pos[2] >= kGoalZMin && + ball_pos[2] <= kGoalZMax; + } + + bool Fallen() const { return data_->xpos[pelvis_body_id_ * 3 + 2] < 0.2; } + + RewardInfo ComputeRewardInfo() const { + RewardInfo reward; + reward.goal_scored = GoalScored() ? 1.0 : 0.0; + reward.time_cost = data_->time; + reward.act_reg = challenge_extra_detail::MeanSquareAct(model_, data_); + reward.pain = challenge_extra_detail::AverageJointLimitForce(model_, data_, + pain_dofadrs_); + reward.solved = reward.goal_scored > 0.0; + reward.done = reward.solved || data_->time >= max_time_sec_ || Fallen(); + reward.sparse = reward.done ? 1.0 : 0.0; + reward.dense_reward = reward_goal_scored_w_ * reward.goal_scored + + reward_time_cost_w_ * reward.time_cost + + reward_act_reg_w_ * reward.act_reg + + reward_pain_w_ * reward.pain; + return reward; + } + + std::vector Observation() const { + std::vector obs; + obs.reserve(spec_.config["obs_dim"_]); + for (int adr : internal_qposadrs_) { + obs.push_back(data_->qpos[adr]); + } + for (int adr : internal_dofadrs_) { + obs.push_back(data_->qvel[adr] * Dt()); + } + for (int sensor_id : grf_sensor_ids_) { + obs.push_back(data_->sensordata[model_->sensor_adr[sensor_id]]); + } + obs.insert(obs.end(), data_->xquat + torso_body_id_ * 4, + data_->xquat + torso_body_id_ * 4 + 4); + obs.insert(obs.end(), data_->xpos + soccer_ball_body_id_ * 3, + data_->xpos + soccer_ball_body_id_ * 3 + 3); + int root_qposadr = model_->jnt_qposadr[root_joint_id_]; + int root_dofadr = model_->jnt_dofadr[root_joint_id_]; + obs.insert(obs.end(), data_->qpos + root_qposadr, + data_->qpos + root_qposadr + 7); + obs.insert(obs.end(), data_->qvel + root_dofadr, + data_->qvel + root_dofadr + 6); + for (int actuator : muscle_actuator_ids_) { + obs.push_back(data_->actuator_length[actuator]); + } + for (int actuator : muscle_actuator_ids_) { + obs.push_back(challenge_extra_detail::Clip( + data_->actuator_velocity[actuator], -100.0, 100.0)); + } + for (int actuator : muscle_actuator_ids_) { + obs.push_back(challenge_extra_detail::Clip( + data_->actuator_force[actuator] / 1000.0, -100.0, 100.0)); + } + obs.insert(obs.end(), data_->act, data_->act + model_->na); + return obs; + } + + void WriteState(const RewardInfo& reward, bool reset, float reward_value) { + auto obs = Observation(); + auto state = Allocate(); + if constexpr (!kFromPixels) { + AssignObservation("obs", &state["obs"_], obs.data(), obs.size(), reset); + } + state["reward"_] = reward_value; + state["discount"_] = 1.0f; + state["done"_] = done_; + state["trunc"_] = elapsed_step_ >= max_episode_steps_; + state["elapsed_step"_] = elapsed_step_; + state["info:goal_scored"_] = reward.goal_scored; + state["info:time_cost"_] = reward.time_cost; + state["info:act_reg"_] = reward.act_reg; + state["info:pain"_] = reward.pain; + state["info:sparse"_] = reward.sparse; + state["info:solved"_] = static_cast(reward.solved); +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + std::array keeper = {GoalkeeperPose()[0], GoalkeeperPose()[1]}; + state["info:goalkeeper_pos"_].Assign(keeper.data(), 2); + if constexpr (kFromPixels) { + AssignPixelObservation("obs:pixels", &state["obs:pixels"_], reset); + } + } +}; + +template +class MyoChallengeChaseTagEnvBase : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::gen_; + using Base::spec_; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum distance{0.0}; + mjtNum lose{0.0}; + mjtNum sparse{0.0}; + bool solved{false}; + bool done{false}; + }; + + enum TaskType { kChase = 0, kEvade = 1 }; + enum OpponentPolicy { + kStaticStationary = 0, + kStationary = 1, + kRandom = 2, + kChasePlayer = 3, + kRepeller = 4, + }; + + bool normalize_act_; + std::string reset_type_; + mjtNum win_distance_; + mjtNum min_spawn_distance_; + std::string task_choice_; + std::string terrain_; + bool repeller_opponent_; + std::vector hills_range_; + std::vector rough_range_; + std::vector relief_range_; + mjtNum chase_vel_low_; + mjtNum chase_vel_high_; + mjtNum random_vel_low_; + mjtNum random_vel_high_; + mjtNum repeller_vel_low_; + mjtNum repeller_vel_high_; + mjtNum reward_distance_w_; + mjtNum reward_lose_w_; + std::vector opponent_probabilities_; + int pelvis_body_id_{-1}; + int head_site_id_{-1}; + int foot_l_body_id_{-1}; + int foot_r_body_id_{-1}; + int opponent_body_id_{-1}; + int opponent_mocap_id_{-1}; + int success_indicator_site_id_{-1}; + int terrain_geom_id_{-1}; + int terrain_hfield_id_{-1}; + std::array grf_sensor_ids_{-1, -1, -1, -1}; + std::vector muscle_actuator_ids_; + std::vector muscle_actuator_; + detail::MyoConditionState muscle_condition_state_; + challenge_extra_detail::ColoredNoiseProcess opponent_noise_process_{ + 2.0, 2, 2000, 10.0}; + std::vector initial_hfield_data_; + std::vector initial_terrain_rgba_; + std::vector initial_terrain_pos_; + std::vector relief_patch_template_; + int current_task_{kChase}; + int opponent_policy_{kStationary}; + std::array opponent_velocity_{0.0, 0.0}; + mjtNum chase_velocity_{1.0}; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_ctrl_; + std::vector test_reset_act_; + std::vector test_reset_act_dot_; + std::vector test_reset_qacc_; + std::vector test_reset_qacc_warmstart_; + std::vector test_hfield_data_; + std::vector test_terrain_geom_rgba_; + std::vector test_terrain_geom_pos_; + int test_task_{-1}; + std::vector test_opponent_pose_; + std::vector test_opponent_velocity_; + std::vector test_opponent_noise_buffer_; + int test_opponent_noise_idx_{-1}; + mjtNum test_chase_velocity_{-1.0}; + int test_opponent_policy_{-1}; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoChallengeChaseTagEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + myosuite::MyoSuiteModelPath(spec.config["base_path"_], + spec.config["model_path"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + reset_type_(spec.config["reset_type"_]), + win_distance_(spec.config["win_distance"_]), + min_spawn_distance_(spec.config["min_spawn_distance"_]), + task_choice_(spec.config["task_choice"_]), + terrain_(spec.config["terrain"_]), + repeller_opponent_(spec.config["repeller_opponent"_]), + hills_range_(detail::ToMjtVector(spec.config["hills_range"_])), + rough_range_(detail::ToMjtVector(spec.config["rough_range"_])), + relief_range_(detail::ToMjtVector(spec.config["relief_range"_])), + chase_vel_low_(spec.config["chase_vel_low"_]), + chase_vel_high_(spec.config["chase_vel_high"_]), + random_vel_low_(spec.config["random_vel_low"_]), + random_vel_high_(spec.config["random_vel_high"_]), + repeller_vel_low_(spec.config["repeller_vel_low"_]), + repeller_vel_high_(spec.config["repeller_vel_high"_]), + reward_distance_w_(spec.config["reward_distance_w"_]), + reward_lose_w_(spec.config["reward_lose_w"_]), + opponent_probabilities_( + detail::ToMjtVector(spec.config["opponent_probabilities"_])), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_ctrl_(detail::ToMjtVector(spec.config["test_reset_ctrl"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_act_dot_( + detail::ToMjtVector(spec.config["test_reset_act_dot"_])), + test_reset_qacc_(detail::ToMjtVector(spec.config["test_reset_qacc"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])), + test_hfield_data_( + detail::ToMjtVector(spec.config["test_hfield_data"_])), + test_terrain_geom_rgba_( + detail::ToMjtVector(spec.config["test_terrain_geom_rgba"_])), + test_terrain_geom_pos_( + detail::ToMjtVector(spec.config["test_terrain_geom_pos"_])), + test_task_(spec.config["test_task"_]), + test_opponent_pose_( + detail::ToMjtVector(spec.config["test_opponent_pose"_])), + test_opponent_velocity_( + detail::ToMjtVector(spec.config["test_opponent_velocity"_])), + test_opponent_noise_buffer_( + detail::ToMjtVector(spec.config["test_opponent_noise_buffer"_])), + test_opponent_noise_idx_(spec.config["test_opponent_noise_idx"_]), + test_chase_velocity_(spec.config["test_chase_velocity"_]), + test_opponent_policy_(spec.config["test_opponent_policy"_]) { + ValidateConfig(); + CacheIds(); + detail::BuildMuscleMask(model_, &muscle_actuator_); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + for (int actuator = 0; actuator < model_->nu; ++actuator) { + if (muscle_actuator_[actuator]) { + muscle_actuator_ids_.push_back(actuator); + } + } + InitializeRobotEnv(); + } + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option, true); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + detail::ResetMyoConditionState(&muscle_condition_state_); + ResetToInitialState(); + ApplyTerrainReset(); + SampleTask(); + ApplyResetState(); + FlattenAgentTerrainPatch(); + ResetOpponent(); + ApplyOpponentTestState(); + mj_forward(model_, data_); + CaptureResetState(); + InvalidateRenderCache(); + RewardInfo reward = ComputeRewardInfo(); + WriteState(reward, true, 0.0); + } + + void Step(const Action& action) override { + UpdateOpponentState(); + const auto* raw = static_cast(action["action"_].Data()); + detail::ApplyMyoSuiteAction(model_, data_, muscle_actuator_, normalize_act_, + raw); + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + detail::RefreshObservedMyoSuiteState(model_, data_); + ++elapsed_step_; + RewardInfo reward = ComputeRewardInfo(); + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, false, reward.dense_reward); + } + + private: + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_] || + model_->nv != spec_.config["qvel_dim"_] || + model_->na != spec_.config["act_dim"_] || + model_->nu != spec_.config["action_dim"_]) { + throw std::runtime_error("ChaseTag dims do not match model."); + } + } + + void CacheIds() { + pelvis_body_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_BODY, "pelvis"); + head_site_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_SITE, "head"); + foot_l_body_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_BODY, "talus_l"); + foot_r_body_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_BODY, "talus_r"); + opponent_body_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_BODY, "opponent"); + opponent_mocap_id_ = model_->body_mocapid[opponent_body_id_]; + success_indicator_site_id_ = challenge_extra_detail::RequireId( + model_, mjOBJ_SITE, "opponent_indicator"); + grf_sensor_ids_[0] = + challenge_extra_detail::RequireId(model_, mjOBJ_SENSOR, "r_foot"); + grf_sensor_ids_[1] = + challenge_extra_detail::RequireId(model_, mjOBJ_SENSOR, "r_toes"); + grf_sensor_ids_[2] = + challenge_extra_detail::RequireId(model_, mjOBJ_SENSOR, "l_foot"); + grf_sensor_ids_[3] = + challenge_extra_detail::RequireId(model_, mjOBJ_SENSOR, "l_toes"); + terrain_geom_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_GEOM, "terrain"); + terrain_hfield_id_ = model_->geom_dataid[terrain_geom_id_]; + detail::CopyModelGeomRgba(model_, terrain_geom_id_, &initial_terrain_rgba_); + detail::CopyModelGeomPos(model_, terrain_geom_id_, &initial_terrain_pos_); + if (terrain_hfield_id_ >= 0) { + detail::CopyModelHfieldData(model_, terrain_hfield_id_, + &initial_hfield_data_); + if (terrain_ != "FLAT") { + relief_patch_template_ = + challenge_extra_detail::LoadNormalizedReliefPatch( + myosuite::MyoSuiteAssetRoot(spec_.config["base_path"_]) + + "/envs/myo/assets/myo_relief.npy"); + int patch_rows = model_->hfield_nrow[terrain_hfield_id_] / 3; + int patch_cols = model_->hfield_ncol[terrain_hfield_id_] / 3; + if (static_cast(relief_patch_template_.size()) != + patch_rows * patch_cols) { + throw std::runtime_error( + "MyoSuite relief patch size does not match ChaseTag hfield."); + } + } + } + int expected_obs = 28 + 28 + 4 + 4 + 3 + 2 + 2 + 2 + model_->na * 4; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error("ChaseTag obs_dim does not match model."); + } + } + + static void RotateSquarePatch90(std::vector* patch, int side) { + std::vector rotated(patch->size(), 0.0); + for (int row = 0; row < side; ++row) { + for (int col = 0; col < side; ++col) { + rotated[(side - 1 - col) * side + row] = (*patch)[row * side + col]; + } + } + *patch = std::move(rotated); + } + + mjtNum RangeLow(const std::vector& range) const { + return range.empty() ? static_cast(0.0) : range[0]; + } + + mjtNum RangeHigh(const std::vector& range) const { + if (range.size() < 2) { + return RangeLow(range); + } + return range[1]; + } + + void RestoreTerrainModelState() { + detail::RestoreModelGeomRgba(model_, terrain_geom_id_, + initial_terrain_rgba_); + detail::RestoreModelGeomPos(model_, terrain_geom_id_, initial_terrain_pos_); + if (terrain_hfield_id_ >= 0) { + detail::RestoreModelHfieldData(model_, terrain_hfield_id_, + initial_hfield_data_); + } + } + + void SetTerrainVisible() { + std::vector rgba = initial_terrain_rgba_; + std::vector pos = initial_terrain_pos_; + if (rgba.size() >= 4) { + rgba[3] = 1.0; + } + if (pos.size() >= 3) { + pos[0] = 0.0; + pos[1] = 0.0; + pos[2] = 0.0; + } + detail::RestoreModelGeomRgba(model_, terrain_geom_id_, rgba); + detail::RestoreModelGeomPos(model_, terrain_geom_id_, pos); + } + + void HideTerrain() { + std::vector rgba = initial_terrain_rgba_; + std::vector pos = initial_terrain_pos_; + if (rgba.size() >= 4) { + rgba[3] = 0.0; + } + if (pos.size() >= 3) { + pos[0] = 0.0; + pos[1] = 0.0; + pos[2] = -10.0; + } + detail::RestoreModelGeomRgba(model_, terrain_geom_id_, rgba); + detail::RestoreModelGeomPos(model_, terrain_geom_id_, pos); + } + + void FillTerrainPatch(int patch_i, int patch_j, + const std::vector& patch) { + if (terrain_hfield_id_ < 0) { + return; + } + int rows = model_->hfield_nrow[terrain_hfield_id_]; + int cols = model_->hfield_ncol[terrain_hfield_id_]; + int patch_rows = rows / 3; + int patch_cols = cols / 3; + if (patch_rows <= 0 || patch_cols <= 0 || + static_cast(patch.size()) != patch_rows * patch_cols) { + throw std::runtime_error("Invalid ChaseTag terrain patch."); + } + int adr = model_->hfield_adr[terrain_hfield_id_]; + for (int row = 0; row < patch_rows; ++row) { + for (int col = 0; col < patch_cols; ++col) { + int dst_row = patch_i * patch_rows + row; + int dst_col = patch_j * patch_cols + col; + if (dst_row >= rows || dst_col >= cols) { + continue; + } + model_->hfield_data[adr + dst_row * cols + dst_col] = + static_cast(patch[row * patch_cols + col]); + } + } + } + + std::vector FlatTerrainPatch(int patch_rows, int patch_cols) const { + return std::vector(patch_rows * patch_cols, + static_cast(0.0)); + } + + std::vector RoughTerrainPatch(int patch_rows, int patch_cols) { + std::vector patch(patch_rows * patch_cols, 0.0); + for (mjtNum& value : patch) { + value = challenge_detail::UniformScalar(&gen_, -1.0, 1.0); + } + auto [low_it, high_it] = std::minmax_element(patch.begin(), patch.end()); + mjtNum denom = std::max(*high_it - *low_it, static_cast(1e-12)); + mjtNum scalar = challenge_detail::UniformScalar( + &gen_, RangeLow(rough_range_), RangeHigh(rough_range_)); + for (mjtNum& value : patch) { + value = (value - *low_it) / denom * scalar - static_cast(0.02); + } + return patch; + } + + std::vector HillyTerrainPatch(int patch_rows, int patch_cols) { + int total = patch_rows * patch_cols; + std::vector data(total, 0.0); + for (int i = 0; i < total; ++i) { + mjtNum angle = + static_cast(10.0 * detail::kPi * i / std::max(total - 1, 1)); + data[i] = std::sin(angle + static_cast(0.5) * detail::kPi) - + static_cast(1.0); + } + auto [low_it, high_it] = std::minmax_element(data.begin(), data.end()); + mjtNum denom = std::max(*high_it - *low_it, static_cast(1e-12)); + mjtNum scalar = challenge_detail::UniformScalar( + &gen_, RangeLow(hills_range_), RangeHigh(hills_range_)); + std::vector patch(total, 0.0); + for (int row = 0; row < patch_rows; ++row) { + for (int col = 0; col < patch_cols; ++col) { + int src = row * patch_cols + col; + int dst_row = patch_rows - 1 - row; + int dst_col = patch_cols - 1 - col; + patch[dst_row * patch_cols + dst_col] = + (data[src] - *low_it) / denom * scalar; + } + } + if (patch_rows == patch_cols && + challenge_detail::UniformScalar(&gen_, 0.0, 1.0) < 0.5) { + RotateSquarePatch90(&patch, patch_rows); + } + return patch; + } + + std::vector ReliefTerrainPatch(int patch_rows, int patch_cols) { + if (static_cast(relief_patch_template_.size()) != + patch_rows * patch_cols) { + throw std::runtime_error("Unexpected MyoSuite relief patch dimensions."); + } + mjtNum scalar = challenge_detail::UniformScalar( + &gen_, RangeLow(relief_range_), RangeHigh(relief_range_)); + std::vector patch = relief_patch_template_; + for (mjtNum& value : patch) { + value *= scalar; + } + return patch; + } + + void ApplyTerrainReset() { + RestoreTerrainModelState(); + if (!test_terrain_geom_rgba_.empty()) { + detail::RestoreModelGeomRgba(model_, terrain_geom_id_, + test_terrain_geom_rgba_); + } + if (!test_terrain_geom_pos_.empty()) { + detail::RestoreModelGeomPos(model_, terrain_geom_id_, + test_terrain_geom_pos_); + } + if (terrain_hfield_id_ < 0) { + return; + } + if (!test_hfield_data_.empty()) { + detail::RestoreModelHfieldData(model_, terrain_hfield_id_, + test_hfield_data_); + return; + } + if (terrain_ == "FLAT") { + HideTerrain(); + return; + } + if (terrain_ != "random") { + throw std::runtime_error("Unsupported ChaseTag terrain type."); + } + SetTerrainVisible(); + int rows = model_->hfield_nrow[terrain_hfield_id_]; + int cols = model_->hfield_ncol[terrain_hfield_id_]; + int patch_rows = rows / 3; + int patch_cols = cols / 3; + detail::RestoreModelHfieldData( + model_, terrain_hfield_id_, + std::vector(rows * cols, static_cast(0.0))); + std::array generated = {0, 0, 0}; + for (int patch_i = 0; patch_i < 3; ++patch_i) { + for (int patch_j = 0; patch_j < 3; ++patch_j) { + int terrain_choice = std::uniform_int_distribution(0, 2)(gen_); + while (terrain_choice == 1 && generated[terrain_choice] >= 2) { + terrain_choice = std::uniform_int_distribution(0, 2)(gen_); + } + ++generated[terrain_choice]; + if (terrain_choice == 0) { + FillTerrainPatch(patch_i, patch_j, + FlatTerrainPatch(patch_rows, patch_cols)); + } else if (terrain_choice == 1) { + FillTerrainPatch(patch_i, patch_j, + HillyTerrainPatch(patch_rows, patch_cols)); + } else { + FillTerrainPatch(patch_i, patch_j, + RoughTerrainPatch(patch_rows, patch_cols)); + } + } + } + if (challenge_detail::UniformScalar(&gen_, 0.0, 1.0) < 0.2) { + int patch_i = std::uniform_int_distribution(0, 2)(gen_); + int patch_j = std::uniform_int_distribution(0, 2)(gen_); + FillTerrainPatch(patch_i, patch_j, + ReliefTerrainPatch(patch_rows, patch_cols)); + } + } + + void FlattenAgentTerrainPatch() { + if (terrain_hfield_id_ < 0 || !test_hfield_data_.empty() || + terrain_ == "FLAT") { + return; + } + int rows = model_->hfield_nrow[terrain_hfield_id_]; + int cols = model_->hfield_ncol[terrain_hfield_id_]; + int patch_rows = rows / 3; + int patch_cols = cols / 3; + if (patch_rows <= 0 || patch_cols <= 0) { + return; + } + constexpr mjtNum k_real_length = 12.0; + constexpr mjtNum k_real_width = 12.0; + mjtNum map_i = data_->qpos[0] / (k_real_length / rows) + + static_cast(rows) * 0.5; + mjtNum map_j = data_->qpos[1] / (k_real_width / cols) + + static_cast(cols) * 0.5; + int clipped_i = static_cast( + std::clamp(map_i, static_cast(0.0), + static_cast(std::max(rows - 2, 0)))); + int clipped_j = static_cast( + std::clamp(map_j, static_cast(0.0), + static_cast(std::max(cols - 2, 0)))); + FillTerrainPatch(clipped_i / patch_rows, clipped_j / patch_cols, + FlatTerrainPatch(patch_rows, patch_cols)); + } + + void SampleTask() { + if (test_task_ >= 0) { + current_task_ = test_task_; + } else if (task_choice_ == "random") { + current_task_ = std::uniform_int_distribution(0, 1)(gen_); + } else { + current_task_ = task_choice_ == "EVADE" ? kEvade : kChase; + } + } + + std::array OpponentPose() const { + return { + data_->mocap_pos[opponent_mocap_id_ * 3 + 0], + data_->mocap_pos[opponent_mocap_id_ * 3 + 1], + challenge_extra_detail::QuatYaw(data_->mocap_quat + + opponent_mocap_id_ * 4), + }; + } + + void SetOpponentPose(mjtNum x, mjtNum y, mjtNum yaw) { + data_->mocap_pos[opponent_mocap_id_ * 3 + 0] = + challenge_extra_detail::Clip(x, -5.5, 5.5); + data_->mocap_pos[opponent_mocap_id_ * 3 + 1] = + challenge_extra_detail::Clip(y, -5.5, 5.5); + data_->mocap_pos[opponent_mocap_id_ * 3 + 2] = 0.0; + auto quat = challenge_extra_detail::YawToQuat(yaw); + std::memcpy(data_->mocap_quat + opponent_mocap_id_ * 4, quat.data(), + sizeof(mjtNum) * 4); + } + + void MoveOpponent(const std::array& velocity) { + auto pose = OpponentPose(); + std::array reported = velocity; + reported[0] = std::abs(reported[0]); + std::array clipped = reported; + clipped[0] = challenge_extra_detail::Clip(clipped[0], -2.0, 2.0); + clipped[1] = challenge_extra_detail::Clip(clipped[1], -2.0, 2.0); + mjtNum x_vel = + clipped[0] * std::cos(pose[2] + static_cast(0.5) * detail::kPi); + mjtNum y_vel = + clipped[0] * std::sin(pose[2] + static_cast(0.5) * detail::kPi); + pose[0] -= Dt() * x_vel; + pose[1] -= Dt() * y_vel; + pose[2] += Dt() * clipped[1]; + SetOpponentPose(pose[0], pose[1], pose[2]); + opponent_velocity_ = reported; + } + + std::array RandomMovement() { + auto sample = opponent_noise_process_.Sample(&gen_); + return { + challenge_extra_detail::Clip(sample[0], random_vel_low_, + random_vel_high_), + challenge_extra_detail::Clip(sample[1], random_vel_low_, + random_vel_high_), + }; + } + + std::array ChasePlayer() const { + auto pose = OpponentPose(); + std::array vec = {pose[0], pose[1]}; + std::array pelvis = {data_->xpos[pelvis_body_id_ * 3 + 0], + data_->xpos[pelvis_body_id_ * 3 + 1]}; + std::array facing = {std::cos(pose[2]), std::sin(pose[2])}; + std::array to_pelvis = {pelvis[0] - vec[0], pelvis[1] - vec[1]}; + mjtNum angular = facing[0] * to_pelvis[0] + facing[1] * to_pelvis[1]; + return {chase_velocity_, angular}; + } + + std::vector> WallPositions() const { + std::vector> out; + out.reserve(100); + for (int i = 0; i < 25; ++i) { + mjtNum value = -8.7 + static_cast(17.4) * static_cast(i) / + static_cast(24); + out.push_back({8.7, value}); + out.push_back({-8.7, value}); + out.push_back({value, 8.7}); + out.push_back({value, -8.7}); + } + return out; + } + + mjtNum CalcRepellerAngularVel(mjtNum current_yaw, mjtNum desired_yaw) const { + auto normalize = [](mjtNum angle) { + constexpr mjtNum two_pi = static_cast(2.0) * detail::kPi; + while (angle > two_pi) { + angle -= two_pi; + } + while (angle < 0.0) { + angle += two_pi; + } + return angle; + }; + current_yaw = normalize(current_yaw); + desired_yaw = normalize(desired_yaw); + mjtNum clockwise = std::abs(current_yaw) + + (static_cast(2.0) * detail::kPi - desired_yaw); + mjtNum anticlockwise = + (static_cast(2.0) * detail::kPi - current_yaw) + desired_yaw; + return clockwise < anticlockwise ? 1.0 : -1.0; + } + + std::array RepellerPolicy() { + constexpr mjtNum k_dist_influence = 3.5; + constexpr mjtNum k_eta = 20.0; + auto pose = OpponentPose(); + std::vector> repellers = WallPositions(); + repellers.insert(repellers.begin(), {data_->xpos[pelvis_body_id_ * 3 + 0], + data_->xpos[pelvis_body_id_ * 3 + 1]}); + std::vector active_indices; + active_indices.reserve(repellers.size()); + for (std::size_t i = 0; i < repellers.size(); ++i) { + mjtNum dist = challenge_extra_detail::Norm2(repellers[i][0] - pose[0], + repellers[i][1] - pose[1]); + if (dist < k_dist_influence) { + active_indices.push_back(static_cast(i)); + } + } + if (active_indices.empty()) { + auto noise = opponent_noise_process_.Sample(&gen_); + mjtNum linear = challenge_extra_detail::Clip(noise[0], repeller_vel_low_, + repeller_vel_high_); + mjtNum angular = CalcRepellerAngularVel(pose[2], noise[1]); + return {linear, angular}; + } + std::array repel_center{0.0, 0.0}; + mjtNum repel_force = 0.0; + for (int index : active_indices) { + repel_center[0] += repellers[index][0]; + repel_center[1] += repellers[index][1]; + mjtNum dist = + std::max(challenge_extra_detail::Norm2(repellers[index][0] - pose[0], + repellers[index][1] - pose[1]), + static_cast(1e-5)); + mjtNum term = static_cast(1.0) / dist - + static_cast(1.0) / k_dist_influence; + repel_force += static_cast(0.5) * k_eta * term * term; + } + repel_center[0] /= static_cast(active_indices.size()); + repel_center[1] /= static_cast(active_indices.size()); + mjtNum linear = challenge_extra_detail::Clip( + repel_force / static_cast(active_indices.size()), + repeller_vel_low_, repeller_vel_high_); + std::array escape = {pose[0] - repel_center[0], + pose[1] - repel_center[1]}; + for (mjtNum& component : escape) { + if (std::abs(component) <= 0.1) { + mjtNum sign = 0.0; + if (component > 0.0) { + sign = 1.0; + } else if (component < 0.0) { + sign = -1.0; + } + component = -sign * challenge_detail::UniformScalar(&gen_, 0.3, 0.9); + } + } + mjtNum direction = + std::atan2(escape[1], escape[0]) + static_cast(1.57); + mjtNum angular = CalcRepellerAngularVel(pose[2], direction); + return {linear, angular}; + } + + void SampleOpponentPolicy() { + if (current_task_ == kEvade) { + opponent_policy_ = kChasePlayer; + return; + } + mjtNum u = challenge_detail::UniformScalar(&gen_, 0.0, 1.0); + if (u < opponent_probabilities_[0]) { + opponent_policy_ = kStaticStationary; + } else if (u < opponent_probabilities_[0] + opponent_probabilities_[1]) { + opponent_policy_ = kStationary; + } else if (u < opponent_probabilities_[0] + opponent_probabilities_[1] + + opponent_probabilities_[2]) { + opponent_policy_ = kRandom; + } else { + opponent_policy_ = kRepeller; + } + } + + void ResetOpponent() { + opponent_noise_process_.Reset(&gen_); + opponent_velocity_ = {0.0, 0.0}; + SampleOpponentPolicy(); + chase_velocity_ = + challenge_detail::UniformScalar(&gen_, chase_vel_low_, chase_vel_high_); + std::array pelvis = {data_->xpos[pelvis_body_id_ * 3 + 0], + data_->xpos[pelvis_body_id_ * 3 + 1]}; + if (opponent_policy_ == kStaticStationary) { + SetOpponentPose(0.0, -5.0, 0.0); + opponent_velocity_ = {0.0, 0.0}; + return; + } + while (true) { + mjtNum x = challenge_detail::UniformScalar(&gen_, -5.0, 5.0); + mjtNum y = challenge_detail::UniformScalar(&gen_, -5.0, 5.0); + if (challenge_extra_detail::Norm2(x - pelvis[0], y - pelvis[1]) < + min_spawn_distance_) { + continue; + } + mjtNum yaw = challenge_detail::UniformScalar( + &gen_, -static_cast(2.0) * detail::kPi, + static_cast(2.0) * detail::kPi); + SetOpponentPose(x, y, yaw); + opponent_velocity_ = {0.0, 0.0}; + return; + } + } + + void UpdateOpponentState() { + std::array velocity = {0.0, 0.0}; + if (opponent_policy_ == kRandom) { + velocity = RandomMovement(); + } else if (opponent_policy_ == kChasePlayer) { + velocity = ChasePlayer(); + } else if (opponent_policy_ == kRepeller) { + velocity = RepellerPolicy(); + } + MoveOpponent(velocity); + } + + void ApplyResetState() { + if (!test_reset_qpos_.empty()) { + detail::RestoreVector(test_reset_qpos_, data_->qpos); + detail::RestoreVector(test_reset_qvel_, data_->qvel); + } else if (reset_type_ == "random") { + int key = std::uniform_int_distribution(0, 1)(gen_) == 0 ? 2 : 3; + detail::RestoreVector( + std::vector(model_->key_qpos + key * model_->nq, + model_->key_qpos + (key + 1) * model_->nq), + data_->qpos); + detail::RestoreVector( + std::vector(model_->key_qvel + key * model_->nv, + model_->key_qvel + (key + 1) * model_->nv), + data_->qvel); + std::normal_distribution noise(0.0, 0.02); + std::array quat = {data_->qpos[3], data_->qpos[4], + data_->qpos[5], data_->qpos[6]}; + mjtNum height = data_->qpos[2]; + for (int i = 0; i < model_->nq; ++i) { + data_->qpos[i] += static_cast(noise(gen_)); + } + data_->qpos[2] = height; + std::copy(quat.begin(), quat.end(), data_->qpos + 3); + data_->qpos[0] = challenge_detail::UniformScalar(&gen_, -5.0, 5.0); + data_->qpos[1] = challenge_detail::UniformScalar(&gen_, -5.0, 5.0); + mjtNum orientation = challenge_detail::UniformScalar( + &gen_, 0.0, static_cast(2.0) * detail::kPi); + std::array mat{}; + mju_quat2Mat(mat.data(), data_->qpos + 3); + auto euler = challenge_detail::Mat9ToEuler(mat.data()); + auto quat_with_orientation = + challenge_detail::EulerXYZToQuat({euler[0], euler[1], orientation}); + std::copy(quat_with_orientation.begin(), quat_with_orientation.end(), + data_->qpos + 3); + mjtNum planar_speed = + challenge_extra_detail::Norm2(data_->qvel[0], data_->qvel[1]); + data_->qvel[0] = std::cos(orientation) * planar_speed; + data_->qvel[1] = std::sin(orientation) * planar_speed; + } else if (reset_type_ == "init") { + detail::RestoreVector( + std::vector(model_->key_qpos + 2 * model_->nq, + model_->key_qpos + 3 * model_->nq), + data_->qpos); + detail::RestoreVector( + std::vector(model_->key_qvel + 2 * model_->nv, + model_->key_qvel + 3 * model_->nv), + data_->qvel); + } else { + detail::RestoreVector( + std::vector(model_->key_qpos, model_->key_qpos + model_->nq), + data_->qpos); + detail::RestoreVector( + std::vector(model_->key_qvel, model_->key_qvel + model_->nv), + data_->qvel); + } + mj_forward(model_, data_); + bool rerun_forward = false; + if (!test_reset_ctrl_.empty()) { + detail::RestoreVector(test_reset_ctrl_, data_->ctrl); + rerun_forward = true; + } + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + rerun_forward = true; + } + if (!test_reset_act_dot_.empty()) { + detail::RestoreVector(test_reset_act_dot_, data_->act_dot); + } + if (!test_reset_qacc_.empty()) { + detail::RestoreVector(test_reset_qacc_, data_->qacc); + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, data_->qacc_warmstart); + rerun_forward = true; + } + if (rerun_forward) { + mj_forward(model_, data_); + } + } + + void ApplyOpponentTestState() { + if (test_opponent_pose_.size() == 3) { + SetOpponentPose(test_opponent_pose_[0], test_opponent_pose_[1], + test_opponent_pose_[2]); + } + if (test_opponent_velocity_.size() == 2) { + opponent_velocity_[0] = test_opponent_velocity_[0]; + opponent_velocity_[1] = test_opponent_velocity_[1]; + } + if (!test_opponent_noise_buffer_.empty()) { + opponent_noise_process_.SetState(test_opponent_noise_buffer_, + test_opponent_noise_idx_); + } + if (test_chase_velocity_ >= 0.0) { + chase_velocity_ = test_chase_velocity_; + } + if (test_opponent_policy_ >= 0) { + opponent_policy_ = test_opponent_policy_; + } + } + + bool Fallen() const { + if (terrain_ == "FLAT") { + return data_->xpos[pelvis_body_id_ * 3 + 2] < 0.5; + } + mjtNum head_z = data_->site_xpos[head_site_id_ * 3 + 2]; + mjtNum left_z = data_->xpos[foot_l_body_id_ * 3 + 2]; + mjtNum right_z = data_->xpos[foot_r_body_id_ * 3 + 2]; + return head_z - (left_z + right_z) * 0.5 < 0.2; + } + + bool ChaseWin() const { + auto pose = OpponentPose(); + return challenge_extra_detail::Norm2(data_->qpos[0] - pose[0], + data_->qpos[1] - pose[1]) <= + win_distance_; + } + + bool ChaseLose() const { + return data_->time >= 20.0 || std::abs(data_->qpos[0]) > 6.5 || + std::abs(data_->qpos[1]) > 6.5 || Fallen(); + } + + bool EvadeLose() const { + auto pose = OpponentPose(); + return challenge_extra_detail::Norm2(data_->qpos[0] - pose[0], + data_->qpos[1] - pose[1]) <= + win_distance_ || + std::abs(data_->qpos[0]) > 6.5 || std::abs(data_->qpos[1]) > 6.5; + } + + RewardInfo ComputeRewardInfo() { + RewardInfo reward; + auto pose = OpponentPose(); + reward.distance = challenge_extra_detail::Norm2(data_->qpos[0] - pose[0], + data_->qpos[1] - pose[1]); + bool win = false; + bool lose = false; + if (current_task_ == kChase) { + win = ChaseWin(); + lose = ChaseLose(); + reward.sparse = win ? static_cast(1.0 - data_->time / 20.0) : 0.0; + } else { + win = data_->time >= 20.0; + lose = EvadeLose(); + reward.sparse = + (win || lose) ? static_cast(data_->time / 20.0) : 0.0; + } + reward.lose = lose ? 1.0 : 0.0; + reward.solved = win; + reward.done = win || lose; + reward.dense_reward = + reward_distance_w_ * reward.distance + reward_lose_w_ * reward.lose; + if (success_indicator_site_id_ >= 0) { + model_->site_rgba[success_indicator_site_id_ * 4 + 0] = win ? 0.0 : 2.0; + model_->site_rgba[success_indicator_site_id_ * 4 + 1] = win ? 2.0 : 0.0; + model_->site_rgba[success_indicator_site_id_ * 4 + 2] = 0.0; + model_->site_rgba[success_indicator_site_id_ * 4 + 3] = win ? 0.2 : 0.0; + } + return reward; + } + + std::vector Observation() const { + std::vector obs; + obs.reserve(spec_.config["obs_dim"_]); + obs.insert(obs.end(), data_->qpos + 7, data_->qpos + 35); + for (int i = 6; i < 34; ++i) { + obs.push_back(data_->qvel[i] * Dt()); + } + for (int sensor_id : grf_sensor_ids_) { + obs.push_back(data_->sensordata[model_->sensor_adr[sensor_id]]); + } + obs.insert(obs.end(), data_->xquat + pelvis_body_id_ * 4, + data_->xquat + pelvis_body_id_ * 4 + 4); + auto pose = OpponentPose(); + obs.insert(obs.end(), pose.begin(), pose.end()); + obs.insert(obs.end(), opponent_velocity_.begin(), opponent_velocity_.end()); + obs.push_back(data_->qpos[0]); + obs.push_back(data_->qpos[1]); + obs.push_back(data_->qvel[0]); + obs.push_back(data_->qvel[1]); + for (int actuator : muscle_actuator_ids_) { + obs.push_back(data_->actuator_length[actuator]); + } + for (int actuator : muscle_actuator_ids_) { + obs.push_back(challenge_extra_detail::Clip( + data_->actuator_velocity[actuator], -100.0, 100.0)); + } + for (int actuator : muscle_actuator_ids_) { + obs.push_back(challenge_extra_detail::Clip( + data_->actuator_force[actuator] / 1000.0, -100.0, 100.0)); + } + obs.insert(obs.end(), data_->act, data_->act + model_->na); + return obs; + } + + void WriteState(const RewardInfo& reward, bool reset, float reward_value) { + auto obs = Observation(); + auto state = Allocate(); + if constexpr (!kFromPixels) { + AssignObservation("obs", &state["obs"_], obs.data(), obs.size(), reset); + } + state["reward"_] = reward_value; + state["discount"_] = 1.0f; + state["done"_] = done_; + state["trunc"_] = elapsed_step_ >= max_episode_steps_; + state["elapsed_step"_] = elapsed_step_; + state["info:distance"_] = reward.distance; + state["info:lose"_] = reward.lose; + state["info:sparse"_] = reward.sparse; + state["info:solved"_] = static_cast(reward.solved); + state["info:task"_] = static_cast(current_task_); +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + auto pose = OpponentPose(); + state["info:opponent_pose"_].Assign(pose.data(), 3); + if constexpr (kFromPixels) { + AssignPixelObservation("obs:pixels", &state["obs:pixels"_], reset); + } + } +}; + +template +class MyoChallengeTableTennisEnvBase + : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::gen_; + using Base::spec_; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum reach_dist{0.0}; + mjtNum palm_dist{0.0}; + mjtNum paddle_quat{0.0}; + mjtNum torso_up{0.0}; + mjtNum act_reg{0.0}; + mjtNum sparse{0.0}; + bool solved{false}; + bool done{false}; + std::array touching_info{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + }; + + enum PingpongLabel { + kPaddle = 0, + kOwn = 1, + kOpponent = 2, + kGround = 3, + kNet = 4, + kEnv = 5, + }; + enum TrajIssue { + kOwnHalf = 0, + kMiss = 1, + kNoPaddle = 2, + kDoubleTouch = 3, + kSuccess = 4, + }; + + static constexpr mjtNum kMaxTime = 3.0; + + bool normalize_act_; + bool ball_qvel_; + bool has_ball_xyz_range_{false}; + bool has_ball_friction_range_{false}; + bool has_paddle_mass_range_{false}; + bool has_qpos_noise_range_{false}; + std::array ball_xyz_low_{0.0, 0.0, 0.0}; + std::array ball_xyz_high_{0.0, 0.0, 0.0}; + std::array ball_friction_low_{0.0, 0.0, 0.0}; + std::array ball_friction_high_{0.0, 0.0, 0.0}; + mjtNum paddle_mass_low_{0.0}; + mjtNum paddle_mass_high_{0.0}; + mjtNum qpos_noise_low_{0.0}; + mjtNum qpos_noise_high_{0.0}; + int rally_count_{1}; + mjtNum reward_reach_dist_w_; + mjtNum reward_palm_dist_w_; + mjtNum reward_paddle_quat_w_; + mjtNum reward_act_reg_w_; + mjtNum reward_torso_up_w_; + mjtNum reward_sparse_w_; + mjtNum reward_solved_w_; + mjtNum reward_done_w_; + int pelvis_site_id_{-1}; + int palm_site_id_{-1}; + int paddle_body_id_{-1}; + int paddle_site_id_{-1}; + int paddle_geom_id_{-1}; + int ball_body_id_{-1}; + int ball_site_id_{-1}; + int ball_geom_id_{-1}; + int own_half_geom_id_{-1}; + int opponent_half_geom_id_{-1}; + int ground_geom_id_{-1}; + int net_geom_id_{-1}; + int ball_vel_sensor_id_{-1}; + int paddle_vel_sensor_id_{-1}; + int flex_extension_joint_id_{-1}; + int ball_joint_id_{-1}; + int ball_qposadr_{-1}; + int ball_dofadr_{-1}; + std::vector body_qposadrs_; + std::vector body_dofadrs_; + std::vector muscle_actuator_; + detail::MyoConditionState muscle_condition_state_; + std::vector default_init_qpos_; + std::vector default_init_qvel_; + std::vector init_qpos_; + std::vector init_qvel_; + std::array init_paddle_quat_{}; + std::vector default_ball_body_pos_; + std::vector default_ball_geom_friction_; + mjtNum default_paddle_body_mass_{0.0}; + int cur_rally_{0}; + std::vector> contact_trajectory_; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_act_; + std::vector test_reset_act_dot_; + std::vector test_reset_qacc_warmstart_; + std::vector test_ball_body_pos_; + std::vector test_ball_geom_friction_; + std::vector test_paddle_body_mass_; + std::vector test_init_qpos_; + std::vector test_init_qvel_; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoChallengeTableTennisEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + myosuite::MyoSuiteModelPath(spec.config["base_path"_], + spec.config["model_path"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + ball_qvel_(spec.config["ball_qvel"_]), + rally_count_(spec.config["rally_count"_]), + reward_reach_dist_w_(spec.config["reward_reach_dist_w"_]), + reward_palm_dist_w_(spec.config["reward_palm_dist_w"_]), + reward_paddle_quat_w_(spec.config["reward_paddle_quat_w"_]), + reward_act_reg_w_(spec.config["reward_act_reg_w"_]), + reward_torso_up_w_(spec.config["reward_torso_up_w"_]), + reward_sparse_w_(spec.config["reward_sparse_w"_]), + reward_solved_w_(spec.config["reward_solved_w"_]), + reward_done_w_(spec.config["reward_done_w"_]), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_act_dot_( + detail::ToMjtVector(spec.config["test_reset_act_dot"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])), + test_ball_body_pos_( + detail::ToMjtVector(spec.config["test_ball_body_pos"_])), + test_ball_geom_friction_( + detail::ToMjtVector(spec.config["test_ball_geom_friction"_])), + test_paddle_body_mass_( + detail::ToMjtVector(spec.config["test_paddle_body_mass"_])), + test_init_qpos_(detail::ToMjtVector(spec.config["test_init_qpos"_])), + test_init_qvel_(detail::ToMjtVector(spec.config["test_init_qvel"_])) { + ParseRanges(spec); + ValidateConfig(); + CacheIds(); + detail::BuildMuscleMask(model_, &muscle_actuator_); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + default_init_qpos_.assign(model_->key_qpos, model_->key_qpos + model_->nq); + default_init_qvel_.assign(model_->key_qvel, model_->key_qvel + model_->nv); + init_qpos_ = default_init_qpos_; + init_qvel_ = default_init_qvel_; + init_qvel_[ball_dofadr_ + 0] = 5.6; + init_qvel_[ball_dofadr_ + 1] = 1.6; + init_qvel_[ball_dofadr_ + 2] = 0.1; + default_init_qvel_ = init_qvel_; + // Upstream TableTennis seeds this reference via + // scipy.spatial.transform.Rotation.from_euler('xyz', ...). + // as_quat()[[3,0,1,2]]. + init_paddle_quat_ = + challenge_extra_detail::ScipyEulerXYZToQuat({-0.3, 1.57, 0.0}); + detail::CopyModelBodyPos(model_, ball_body_id_, &default_ball_body_pos_); + detail::CopyModelGeomFriction(model_, ball_geom_id_, + &default_ball_geom_friction_); + detail::CopyModelBodyMass(model_, paddle_body_id_, + &default_paddle_body_mass_); + InitializeRobotEnv(); + } + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + cur_rally_ = 0; + contact_trajectory_.clear(); + detail::ResetMyoConditionState(&muscle_condition_state_); + ResetToInitialState(); + init_qpos_ = default_init_qpos_; + init_qvel_ = default_init_qvel_; + RestoreModelState(); + ApplyModelRandomization(); + ApplyOracleModelOverrides(); + ApplyResetState(); + CaptureResetState(); + RewardInfo reward = ComputeRewardInfo(); + WriteState(reward, true, 0.0); + } + + void Step(const Action& action) override { + const auto* raw = static_cast(action["action"_].Data()); + std::vector processed_controls(model_->nu); + for (int i = 0; i < model_->nu; ++i) { + processed_controls[i] = raw[i]; + if (normalize_act_ && model_->actuator_dyntype[i] != mjDYN_MUSCLE) { + mjtNum low = model_->actuator_ctrlrange[i * 2]; + mjtNum high = model_->actuator_ctrlrange[i * 2 + 1]; + processed_controls[i] = + static_cast((low + high) * static_cast(0.5) + + static_cast(raw[i]) * (high - low) * + static_cast(0.5)); + } + } + detail::ApplyMyoSuiteAction(model_, data_, muscle_actuator_, normalize_act_, + processed_controls.data()); + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + ++elapsed_step_; + RewardInfo reward = ComputeRewardInfo(); + if (reward.solved && cur_rally_ + 1 < rally_count_) { + ++cur_rally_; + data_->time = 0.0; + contact_trajectory_.clear(); + RelaunchBall(); + reward.solved = false; + reward.done = false; + } + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, false, reward.dense_reward); + } + + private: + void ParseRanges(const Spec& spec) { + auto ball_xyz_low = spec.config["ball_xyz_low"_]; + auto ball_xyz_high = spec.config["ball_xyz_high"_]; + if (!ball_xyz_low.empty() && !ball_xyz_high.empty()) { + has_ball_xyz_range_ = true; + for (int i = 0; i < 3; ++i) { + ball_xyz_low_[i] = static_cast(ball_xyz_low[i]); + ball_xyz_high_[i] = static_cast(ball_xyz_high[i]); + } + } + auto ball_friction_low = spec.config["ball_friction_low"_]; + auto ball_friction_high = spec.config["ball_friction_high"_]; + if (!ball_friction_low.empty() && !ball_friction_high.empty()) { + has_ball_friction_range_ = true; + for (int i = 0; i < 3; ++i) { + ball_friction_low_[i] = static_cast(ball_friction_low[i]); + ball_friction_high_[i] = static_cast(ball_friction_high[i]); + } + } + has_paddle_mass_range_ = + spec.config["paddle_mass_high"_] > spec.config["paddle_mass_low"_]; + paddle_mass_low_ = spec.config["paddle_mass_low"_]; + paddle_mass_high_ = spec.config["paddle_mass_high"_]; + if (!std::isnan(spec.config["qpos_noise_low"_]) && + !std::isnan(spec.config["qpos_noise_high"_])) { + has_qpos_noise_range_ = true; + qpos_noise_low_ = spec.config["qpos_noise_low"_]; + qpos_noise_high_ = spec.config["qpos_noise_high"_]; + } + } + + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_] || + model_->nv != spec_.config["qvel_dim"_] || + model_->na != spec_.config["act_dim"_] || + model_->nu != spec_.config["action_dim"_]) { + throw std::runtime_error("TableTennis dims do not match model."); + } + } + + void CacheIds() { + pelvis_site_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_SITE, "pelvis"); + palm_site_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_SITE, "S_grasp"); + paddle_body_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_BODY, "paddle"); + paddle_site_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_SITE, "paddle"); + paddle_geom_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_GEOM, "pad"); + ball_body_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_BODY, "pingpong"); + ball_site_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_SITE, "pingpong"); + ball_geom_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_GEOM, "pingpong"); + own_half_geom_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_GEOM, "coll_own_half"); + opponent_half_geom_id_ = challenge_extra_detail::RequireId( + model_, mjOBJ_GEOM, "coll_opponent_half"); + ground_geom_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_GEOM, "ground"); + net_geom_id_ = + challenge_extra_detail::RequireId(model_, mjOBJ_GEOM, "coll_net"); + ball_vel_sensor_id_ = challenge_extra_detail::RequireId( + model_, mjOBJ_SENSOR, "pingpong_vel_sensor"); + paddle_vel_sensor_id_ = challenge_extra_detail::RequireId( + model_, mjOBJ_SENSOR, "paddle_vel_sensor"); + flex_extension_joint_id_ = challenge_extra_detail::RequireId( + model_, mjOBJ_JOINT, "flex_extension"); + ball_joint_id_ = challenge_extra_detail::RequireId(model_, mjOBJ_JOINT, + "pingpong_freejoint"); + ball_qposadr_ = model_->jnt_qposadr[ball_joint_id_]; + ball_dofadr_ = model_->jnt_dofadr[ball_joint_id_]; + for (int joint_id = 0; joint_id < model_->njnt; ++joint_id) { + const char* raw_name = mj_id2name(model_, mjOBJ_JOINT, joint_id); + if (raw_name == nullptr) { + continue; + } + std::string_view name(raw_name); + if (name.rfind("ping", 0) == 0 || name == "paddle_freejoint") { + continue; + } + body_qposadrs_.push_back(model_->jnt_qposadr[joint_id]); + body_dofadrs_.push_back(model_->jnt_dofadr[joint_id]); + } + int expected_obs = 3 + static_cast(body_qposadrs_.size()) + + static_cast(body_dofadrs_.size()) + 3 + 3 + 3 + 3 + + 4 + 3 + 6 + model_->na; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error("TableTennis obs_dim does not match model."); + } + } + + void RestoreModelState() { + detail::RestoreModelBodyPos(model_, ball_body_id_, default_ball_body_pos_); + detail::RestoreModelGeomFriction(model_, ball_geom_id_, + default_ball_geom_friction_); + detail::RestoreModelBodyMass(model_, paddle_body_id_, + default_paddle_body_mass_); + } + + void ApplyModelRandomization() { + if (has_paddle_mass_range_) { + detail::RestoreModelBodyMass( + model_, paddle_body_id_, + challenge_detail::UniformScalar(&gen_, paddle_mass_low_, + paddle_mass_high_)); + } + if (has_ball_friction_range_) { + auto friction = challenge_detail::UniformVec3(&gen_, ball_friction_low_, + ball_friction_high_); + detail::RestoreModelGeomFriction( + model_, ball_geom_id_, + std::vector(friction.begin(), friction.end())); + } + if (has_ball_xyz_range_) { + auto ball_pos = + challenge_detail::UniformVec3(&gen_, ball_xyz_low_, ball_xyz_high_); + detail::RestoreModelBodyPos(model_, ball_body_id_, + {ball_pos[0], ball_pos[1], ball_pos[2]}); + init_qpos_[ball_qposadr_ + 0] = ball_pos[0]; + init_qpos_[ball_qposadr_ + 1] = ball_pos[1]; + init_qpos_[ball_qposadr_ + 2] = ball_pos[2]; + } + if (ball_qvel_) { + auto vel_bounds = CalcBallQvelBounds({init_qpos_[ball_qposadr_ + 0], + init_qpos_[ball_qposadr_ + 1], + init_qpos_[ball_qposadr_ + 2]}); + for (int axis = 0; axis < 3; ++axis) { + std::uniform_real_distribution dist(vel_bounds[1][axis], + vel_bounds[0][axis]); + init_qvel_[ball_dofadr_ + axis] = static_cast(dist(gen_)); + } + } + } + + void ApplyResetState() { + std::vector qpos = init_qpos_; + std::vector qvel = init_qvel_; + if (!test_reset_qpos_.empty()) { + qpos = test_reset_qpos_; + if (!test_reset_qvel_.empty()) { + qvel = test_reset_qvel_; + } + } else if (has_qpos_noise_range_) { + for (int joint_id = 0; joint_id < model_->njnt - 2; ++joint_id) { + mjtNum joint_span = model_->jnt_range[joint_id * 2 + 1] - + model_->jnt_range[joint_id * 2 + 0]; + std::uniform_real_distribution dist(qpos_noise_low_, + qpos_noise_high_); + int adr = model_->jnt_qposadr[joint_id]; + qpos[adr] += static_cast(dist(gen_)) * joint_span; + qpos[adr] = challenge_extra_detail::Clip( + qpos[adr], model_->jnt_range[joint_id * 2 + 0], + model_->jnt_range[joint_id * 2 + 1]); + } + } + detail::RestoreVector(qpos, data_->qpos); + detail::RestoreVector(qvel, data_->qvel); + mj_forward(model_, data_); + bool rerun_forward = false; + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + rerun_forward = true; + } + if (!test_reset_act_dot_.empty()) { + detail::RestoreVector(test_reset_act_dot_, data_->act_dot); + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, data_->qacc_warmstart); + rerun_forward = true; + } + if (rerun_forward) { + mj_forward(model_, data_); + } + } + + void ApplyOracleModelOverrides() { + if (test_ball_body_pos_.size() == 3) { + detail::RestoreModelBodyPos(model_, ball_body_id_, test_ball_body_pos_); + } + if (!test_ball_geom_friction_.empty()) { + detail::RestoreModelGeomFriction(model_, ball_geom_id_, + test_ball_geom_friction_); + } + if (!test_paddle_body_mass_.empty()) { + detail::RestoreModelBodyMass(model_, paddle_body_id_, + test_paddle_body_mass_[0]); + } + if (test_init_qpos_.size() == static_cast(model_->nq)) { + init_qpos_ = test_init_qpos_; + } else if (test_ball_body_pos_.size() == 3) { + init_qpos_[ball_qposadr_ + 0] = test_ball_body_pos_[0]; + init_qpos_[ball_qposadr_ + 1] = test_ball_body_pos_[1]; + init_qpos_[ball_qposadr_ + 2] = test_ball_body_pos_[2]; + } + if (test_init_qvel_.size() == static_cast(model_->nv)) { + init_qvel_ = test_init_qvel_; + } + } + + std::array, 2> CalcBallQvelBounds( + const std::array& ball_pos) { + std::array table_upper = {1.35, 0.70, 0.785}; + std::array table_lower = {0.5, -0.60, 0.785}; + constexpr mjtNum gravity = 9.81; + mjtNum vz = challenge_detail::UniformScalar(&gen_, -0.1, 0.1); + mjtNum a = -0.5 * gravity; + mjtNum b = vz; + mjtNum c = ball_pos[2] - table_upper[2]; + mjtNum disc = b * b - 4.0 * a * c; + mjtNum t = + (-b - std::sqrt(std::max(disc, static_cast(0.0)))) / (2.0 * a); + return {{ + {(table_upper[0] - ball_pos[0]) / t, (table_upper[1] - ball_pos[1]) / t, + vz}, + {(table_lower[0] - ball_pos[0]) / t, (table_lower[1] - ball_pos[1]) / t, + vz}, + }}; + } + + void RelaunchBall() { + std::array ball_pos = {init_qpos_[ball_qposadr_ + 0], + init_qpos_[ball_qposadr_ + 1], + init_qpos_[ball_qposadr_ + 2]}; + std::array ball_vel = { + init_qvel_[ball_dofadr_ + 0], init_qvel_[ball_dofadr_ + 1], + init_qvel_[ball_dofadr_ + 2], init_qvel_[ball_dofadr_ + 3], + init_qvel_[ball_dofadr_ + 4], init_qvel_[ball_dofadr_ + 5]}; + if (has_ball_xyz_range_) { + auto sampled = + challenge_detail::UniformVec3(&gen_, ball_xyz_low_, ball_xyz_high_); + ball_pos = {sampled[0], sampled[1], sampled[2]}; + } + if (ball_qvel_) { + auto vel_bounds = CalcBallQvelBounds(ball_pos); + for (int axis = 0; axis < 3; ++axis) { + std::uniform_real_distribution dist(vel_bounds[1][axis], + vel_bounds[0][axis]); + ball_vel[axis] = static_cast(dist(gen_)); + } + } + data_->qpos[ball_qposadr_ + 0] = ball_pos[0]; + data_->qpos[ball_qposadr_ + 1] = ball_pos[1]; + data_->qpos[ball_qposadr_ + 2] = ball_pos[2]; + for (int axis = 0; axis < 6; ++axis) { + data_->qvel[ball_dofadr_ + axis] = ball_vel[axis]; + } + mj_forward(model_, data_); + } + + std::array BallContactLabels() const { + std::array labels = {false, false, false, false, false, false}; + for (int i = 0; i < data_->ncon; ++i) { + const mjContact& contact = data_->contact[i]; + if (model_->geom_bodyid[contact.geom1] == ball_body_id_) { + labels[GeomIdToLabel(contact.geom2)] = true; + } else if (model_->geom_bodyid[contact.geom2] == ball_body_id_) { + labels[GeomIdToLabel(contact.geom1)] = true; + } + } + return labels; + } + + int GeomIdToLabel(int geom_id) const { + if (geom_id == paddle_geom_id_) { + return kPaddle; + } + if (geom_id == own_half_geom_id_) { + return kOwn; + } + if (geom_id == opponent_half_geom_id_) { + return kOpponent; + } + if (geom_id == net_geom_id_) { + return kNet; + } + if (geom_id == ground_geom_id_) { + return kGround; + } + return kEnv; + } + + int EvaluateTrajectory() const { + bool has_hit_paddle = false; + bool has_bounced_from_paddle = false; + bool has_bounced_from_table = false; + int own_contact_count = 0; + bool own_contact_phase_done = false; + for (const auto& labels : contact_trajectory_) { + if (!labels[kPaddle] && has_hit_paddle) { + has_bounced_from_paddle = true; + } + if (labels[kPaddle] && has_bounced_from_paddle) { + return kDoubleTouch; + } + if (labels[kPaddle]) { + has_hit_paddle = true; + } + if (labels[kOwn]) { + if (!has_bounced_from_table) { + has_bounced_from_table = true; + own_contact_count = 1; + } else if (!own_contact_phase_done) { + ++own_contact_count; + if (own_contact_count > 2) { + own_contact_phase_done = true; + return kOwnHalf; + } + } else { + return kOwnHalf; + } + } else if (has_bounced_from_table) { + own_contact_phase_done = true; + } + if (labels[kOpponent]) { + return has_hit_paddle ? kSuccess : kNoPaddle; + } + } + return kMiss; + } + + RewardInfo ComputeRewardInfo() { + RewardInfo reward; + auto labels = BallContactLabels(); + contact_trajectory_.push_back(labels); + for (int i = 0; i < 6; ++i) { + reward.touching_info[i] = labels[i] ? 1.0 : 0.0; + } + std::array ball_pos{}; + detail::CopySitePos(model_, data_, ball_site_id_, ball_pos.data()); + std::array paddle_pos{}; + detail::CopySitePos(model_, data_, paddle_site_id_, paddle_pos.data()); + std::array palm_pos{}; + detail::CopySitePos(model_, data_, palm_site_id_, palm_pos.data()); + auto reach_err = challenge_detail::Sub3(paddle_pos, ball_pos); + auto palm_err = challenge_detail::Sub3(palm_pos, paddle_pos); + reward.reach_dist = std::exp(-challenge_detail::Norm3(reach_err)); + reward.palm_dist = std::exp(-5.0 * challenge_detail::Norm3(palm_err)); + std::array paddle_quat = {data_->xquat[paddle_body_id_ * 4 + 0], + data_->xquat[paddle_body_id_ * 4 + 1], + data_->xquat[paddle_body_id_ * 4 + 2], + data_->xquat[paddle_body_id_ * 4 + 3]}; + mjtNum quat_err = 0.0; + for (int i = 0; i < 4; ++i) { + mjtNum diff = paddle_quat[i] - init_paddle_quat_[i]; + quat_err += diff * diff; + } + reward.paddle_quat = std::exp(-5.0 * std::sqrt(quat_err)); + reward.torso_up = std::exp( + -5.0 * + std::abs(data_->qpos[model_->jnt_qposadr[flex_extension_joint_id_]])); + reward.act_reg = -challenge_extra_detail::L2ActReg(model_, data_); + reward.sparse = labels[kPaddle] ? 1.0 : 0.0; + int traj = EvaluateTrajectory(); + reward.solved = traj == kSuccess; + reward.done = data_->time > kMaxTime || ball_pos[2] < 0.3 || + reward.solved || traj == kOwnHalf || traj == kNoPaddle || + traj == kDoubleTouch; + reward.dense_reward = + reward_reach_dist_w_ * reward.reach_dist + + reward_palm_dist_w_ * reward.palm_dist + + reward_paddle_quat_w_ * reward.paddle_quat + + reward_act_reg_w_ * reward.act_reg + + reward_torso_up_w_ * reward.torso_up + + reward_sparse_w_ * reward.sparse + + reward_solved_w_ * static_cast(reward.solved) + + reward_done_w_ * static_cast(reward.done); + return reward; + } + + std::vector Observation(const RewardInfo& reward) const { + std::vector obs; + obs.reserve(spec_.config["obs_dim"_]); + obs.insert(obs.end(), data_->site_xpos + pelvis_site_id_ * 3, + data_->site_xpos + pelvis_site_id_ * 3 + 3); + for (int adr : body_qposadrs_) { + obs.push_back(data_->qpos[adr]); + } + for (int adr : body_dofadrs_) { + obs.push_back(data_->qvel[adr]); + } + obs.insert(obs.end(), data_->site_xpos + ball_site_id_ * 3, + data_->site_xpos + ball_site_id_ * 3 + 3); + int ball_vel_adr = model_->sensor_adr[ball_vel_sensor_id_]; + obs.insert(obs.end(), data_->sensordata + ball_vel_adr, + data_->sensordata + ball_vel_adr + 3); + obs.insert(obs.end(), data_->site_xpos + paddle_site_id_ * 3, + data_->site_xpos + paddle_site_id_ * 3 + 3); + int paddle_vel_adr = model_->sensor_adr[paddle_vel_sensor_id_]; + obs.insert(obs.end(), data_->sensordata + paddle_vel_adr, + data_->sensordata + paddle_vel_adr + 3); + obs.insert(obs.end(), data_->xquat + paddle_body_id_ * 4, + data_->xquat + paddle_body_id_ * 4 + 4); + std::array ball_pos{}; + std::array paddle_pos{}; + detail::CopySitePos(model_, data_, ball_site_id_, ball_pos.data()); + detail::CopySitePos(model_, data_, paddle_site_id_, paddle_pos.data()); + auto reach_err = challenge_detail::Sub3(paddle_pos, ball_pos); + obs.insert(obs.end(), reach_err.begin(), reach_err.end()); + obs.insert(obs.end(), reward.touching_info.begin(), + reward.touching_info.end()); + obs.insert(obs.end(), data_->act, data_->act + model_->na); + return obs; + } + + void WriteState(const RewardInfo& reward, bool reset, float reward_value) { + auto obs = Observation(reward); + auto state = Allocate(); + if constexpr (!kFromPixels) { + AssignObservation("obs", &state["obs"_], obs.data(), obs.size(), reset); + } + state["reward"_] = reward_value; + state["discount"_] = 1.0f; + state["done"_] = done_; + state["trunc"_] = elapsed_step_ >= max_episode_steps_; + state["elapsed_step"_] = elapsed_step_; + state["info:reach_dist"_] = reward.reach_dist; + state["info:palm_dist"_] = reward.palm_dist; + state["info:paddle_quat"_] = reward.paddle_quat; + state["info:torso_up"_] = reward.torso_up; + state["info:act_reg"_] = reward.act_reg; + state["info:sparse"_] = reward.sparse; + state["info:solved"_] = static_cast(reward.solved); +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + state["info:touching_info"_].Assign(reward.touching_info.data(), 6); + if constexpr (kFromPixels) { + AssignPixelObservation("obs:pixels", &state["obs:pixels"_], reset); + } + } +}; + +template +using RunTrackEnvAlias = MyoChallengeRunTrackEnvBase; + +template +using RunTrackPixelEnvAlias = MyoChallengeRunTrackEnvBase; + +template +using SoccerEnvAlias = MyoChallengeSoccerEnvBase; + +template +using SoccerPixelEnvAlias = MyoChallengeSoccerEnvBase; + +template +using ChaseTagEnvAlias = MyoChallengeChaseTagEnvBase; + +template +using ChaseTagPixelEnvAlias = MyoChallengeChaseTagEnvBase; + +template +using TableTennisEnvAlias = MyoChallengeTableTennisEnvBase; + +template +using TableTennisPixelEnvAlias = MyoChallengeTableTennisEnvBase; + +using RunTrackSpec = MyoChallengeRunTrackEnvSpec; +using RunTrackPixelSpec = MyoChallengeRunTrackPixelEnvSpec; +using SoccerSpec = MyoChallengeSoccerEnvSpec; +using SoccerPixelSpec = MyoChallengeSoccerPixelEnvSpec; +using ChaseTagSpec = MyoChallengeChaseTagEnvSpec; +using ChaseTagPixelSpec = MyoChallengeChaseTagPixelEnvSpec; +using TableTennisSpec = MyoChallengeTableTennisEnvSpec; +using TableTennisPixelSpec = MyoChallengeTableTennisPixelEnvSpec; + +using RunTrackEnv = RunTrackEnvAlias; +using RunTrackPixelEnv = RunTrackPixelEnvAlias; +using MyoChallengeRunTrackEnv = RunTrackEnv; +using MyoChallengeRunTrackPixelEnv = RunTrackPixelEnv; +using MyoChallengeRunTrackEnvPool = AsyncEnvPool; +using MyoChallengeRunTrackPixelEnvPool = AsyncEnvPool; + +using SoccerEnv = SoccerEnvAlias; +using SoccerPixelEnv = SoccerPixelEnvAlias; +using MyoChallengeSoccerEnv = SoccerEnv; +using MyoChallengeSoccerPixelEnv = SoccerPixelEnv; +using MyoChallengeSoccerEnvPool = AsyncEnvPool; +using MyoChallengeSoccerPixelEnvPool = AsyncEnvPool; + +using ChaseTagEnv = ChaseTagEnvAlias; +using ChaseTagPixelEnv = ChaseTagPixelEnvAlias; +using MyoChallengeChaseTagEnv = ChaseTagEnv; +using MyoChallengeChaseTagPixelEnv = ChaseTagPixelEnv; +using MyoChallengeChaseTagEnvPool = AsyncEnvPool; +using MyoChallengeChaseTagPixelEnvPool = AsyncEnvPool; + +using TableTennisEnv = TableTennisEnvAlias; +using TableTennisPixelEnv = TableTennisPixelEnvAlias; +using MyoChallengeTableTennisEnv = TableTennisEnv; +using MyoChallengeTableTennisPixelEnv = TableTennisPixelEnv; +using MyoChallengeTableTennisEnvPool = AsyncEnvPool; +using MyoChallengeTableTennisPixelEnvPool = AsyncEnvPool; + +} // namespace myosuite_envpool + +#endif // ENVPOOL_MUJOCO_MYOSUITE_MYOCHALLENGE_EXTENDED_H_ diff --git a/envpool/mujoco/myosuite/myochallenge_test.py b/envpool/mujoco/myosuite/myochallenge_test.py new file mode 100644 index 000000000..9b26126fc --- /dev/null +++ b/envpool/mujoco/myosuite/myochallenge_test.py @@ -0,0 +1,1875 @@ +# Copyright 2026 Garena Online Private 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. +"""Internal MyoSuite MyoChallenge native env tests.""" + +from __future__ import annotations + +from functools import cache +from pathlib import Path +from typing import Any + +import gymnasium +import mujoco +import numpy as np +from absl.testing import absltest + +from envpool.mujoco.myosuite.config import ( + myosuite_expanded_entry, + resolve_myosuite_model_path, +) +from envpool.mujoco.myosuite.metadata import MYOSUITE_DIRECT_ENTRIES +from envpool.mujoco.myosuite.native import ( + MyoChallengeBaodingEnvSpec, + MyoChallengeBaodingGymnasiumEnvPool, + MyoChallengeBaodingPixelEnvSpec, + MyoChallengeBaodingPixelGymnasiumEnvPool, + MyoChallengeBimanualEnvSpec, + MyoChallengeBimanualGymnasiumEnvPool, + MyoChallengeBimanualPixelEnvSpec, + MyoChallengeBimanualPixelGymnasiumEnvPool, + MyoChallengeChaseTagEnvSpec, + MyoChallengeChaseTagGymnasiumEnvPool, + MyoChallengeChaseTagPixelEnvSpec, + MyoChallengeChaseTagPixelGymnasiumEnvPool, + MyoChallengeRelocateEnvSpec, + MyoChallengeRelocateGymnasiumEnvPool, + MyoChallengeRelocatePixelEnvSpec, + MyoChallengeRelocatePixelGymnasiumEnvPool, + MyoChallengeReorientEnvSpec, + MyoChallengeReorientGymnasiumEnvPool, + MyoChallengeReorientPixelEnvSpec, + MyoChallengeReorientPixelGymnasiumEnvPool, + MyoChallengeRunTrackEnvSpec, + MyoChallengeRunTrackGymnasiumEnvPool, + MyoChallengeRunTrackPixelEnvSpec, + MyoChallengeRunTrackPixelGymnasiumEnvPool, + MyoChallengeSoccerEnvSpec, + MyoChallengeSoccerGymnasiumEnvPool, + MyoChallengeSoccerPixelEnvSpec, + MyoChallengeSoccerPixelGymnasiumEnvPool, + MyoChallengeTableTennisEnvSpec, + MyoChallengeTableTennisGymnasiumEnvPool, + MyoChallengeTableTennisPixelEnvSpec, + MyoChallengeTableTennisPixelGymnasiumEnvPool, +) +from envpool.mujoco.myosuite.oracle_utils import load_oracle_class +from envpool.mujoco.myosuite.paths import ( + myosuite_asset_root, +) +from envpool.python.glfw_context import preload_windows_gl_dlls + +preload_windows_gl_dlls(strict=True) + +_REORIENT_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["suite"] == "myochallenge" + and entry["class_name"] == "ReorientEnvV0" +) +_RELOCATE_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["suite"] == "myochallenge" + and entry["class_name"] == "RelocateEnvV0" +) +_BAODING_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["suite"] == "myochallenge" + and entry["class_name"] == "BaodingEnvV1" +) +_BIMANUAL_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["suite"] == "myochallenge" + and entry["class_name"] == "BimanualEnvV1" +) +_RUNTRACK_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["suite"] == "myochallenge" and entry["class_name"] == "RunTrack" +) +_SOCCER_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["suite"] == "myochallenge" and entry["class_name"] == "SoccerEnvV0" +) +_CHASETAG_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["suite"] == "myochallenge" + and entry["class_name"] == "ChaseTagEnvV0" +) +_TABLETENNIS_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["suite"] == "myochallenge" + and entry["class_name"] == "TableTennisEnvV0" +) +_ALIGNMENT_STEPS = 32 +_REORIENT_ALIGNMENT_OBS_ATOL = 2e-3 +_REORIENT_ALIGNMENT_ROT_ATOL = 2e-2 +_DEFAULT_ALIGNMENT_ATOL = 1e-5 +_LOCOMOTION_ALIGNMENT_ATOL = 1e-4 +_BIMANUAL_ALIGNMENT_ATOL = 5e-4 +_TABLETENNIS_ALIGNMENT_ATOL = 5e-4 + + +def _entry(env_id: str) -> dict[str, Any]: + entry, variant_kwargs = myosuite_expanded_entry(env_id) + merged = dict(entry) + merged["id"] = env_id + merged["kwargs"] = {**entry["kwargs"], **variant_kwargs} + return merged + + +def _runtime_model_path(model_path: str) -> Path: + path = Path(resolve_myosuite_model_path(model_path)) + return path if path.is_absolute() else myosuite_asset_root() / path + + +def _oracle_model_path(model_path: str) -> Path: + path = Path(model_path) + return path if path.is_absolute() else myosuite_asset_root() / path + + +@cache +def _model(path: str) -> mujoco.MjModel: + return mujoco.MjModel.from_xml_path(str(_runtime_model_path(path))) + + +def _make_env(config: tuple[Any, ...], pool_type: type, spec_type: type) -> Any: + return pool_type(spec_type(config)) + + +def _soccer_policy_id(policy: str) -> int: + return {"block_ball": 0, "random": 1, "stationary": 2}[policy] + + +def _chasetag_policy_id(policy: str) -> int: + return { + "static_stationary": 0, + "stationary": 1, + "random": 2, + "chase_player": 3, + "repeller": 4, + }[policy] + + +def _runtrack_osl_state_id(state: str) -> int: + return { + "e_stance": 0, + "l_stance": 1, + "e_swing": 2, + "l_swing": 3, + }[state] + + +def _seeded_actions( + shape: tuple[int, ...], steps: int, seed: int +) -> list[np.ndarray]: + rng = np.random.default_rng(seed) + return [ + rng.uniform(-0.9, 0.9, size=shape).astype(np.float32) + for _ in range(steps) + ] + + +def _assert_rollouts_match( + case: absltest.TestCase, + env0: Any, + env1: Any, + actions: list[np.ndarray], + *, + atol: float = 1e-9, + rtol: float = 1e-9, +) -> None: + obs0, info0 = env0.reset() + obs1, info1 = env1.reset() + np.testing.assert_allclose(obs0, obs1, atol=atol, rtol=rtol) + case.assertEqual( + info0["elapsed_step"].tolist(), info1["elapsed_step"].tolist() + ) + for action in actions: + out0 = env0.step(action) + out1 = env1.step(action) + obs0, reward0, terminated0, truncated0, info0 = out0 + obs1, reward1, terminated1, truncated1, info1 = out1 + np.testing.assert_allclose(obs0, obs1, atol=atol, rtol=rtol) + np.testing.assert_allclose(reward0, reward1, atol=atol, rtol=rtol) + np.testing.assert_array_equal(terminated0, terminated1) + np.testing.assert_array_equal(truncated0, truncated1) + case.assertEqual( + info0["elapsed_step"].tolist(), info1["elapsed_step"].tolist() + ) + if terminated0[0] or truncated0[0]: + break + + +def _batched_action_shape(env: Any) -> tuple[int, int]: + return (1, int(env.action_space.shape[-1])) + + +def _relocate_alignment_obs_atol(env_id: str) -> float: + if env_id == "myoChallengeRelocateP1-v0": + return 2e-2 + return 2e-3 + + +def _reorient_alignment_reward_atol() -> float: + # Reorient dense reward scales `pos_dist` by 100x, so the accepted + # observation residual on pos_err alone can legitimately amplify into a + # ~2e-1 scalar reward drift. + return 2e-1 + + +def _assert_reorient_obs_close( + actual: np.ndarray, expected: np.ndarray +) -> None: + # The only residual after reset-sync is the `mat2euler(site_xmat)` block: + # obj_rot, goal_rot, and rot_err. Keep the rest of the observation tight. + np.testing.assert_allclose( + actual[:54], + expected[:54], + atol=_REORIENT_ALIGNMENT_OBS_ATOL, + rtol=1e-5, + ) + np.testing.assert_allclose( + actual[54:63], + expected[54:63], + atol=_REORIENT_ALIGNMENT_ROT_ATOL, + rtol=1e-5, + ) + np.testing.assert_allclose( + actual[63:], + expected[63:], + atol=_REORIENT_ALIGNMENT_OBS_ATOL, + rtol=1e-5, + ) + + +def _relocate_alignment_reward_atol(env_id: str) -> float: + if env_id == "myoChallengeRelocateP1-v0": + return 1.0 + return 2e-2 + + +def _reorient_config( + env_id: str, overrides: dict[str, Any] | None = None +) -> tuple[tuple[Any, ...], type, type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + model = _model(kwargs["model_path"]) + obj_mass = kwargs.get("obj_mass_range", [0.108, 0.108]) + config = MyoChallengeReorientEnvSpec.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + frame_skip=int(kwargs.get("frame_skip", 5)), + model_path=str(kwargs["model_path"]), + normalize_act=bool(kwargs.get("normalize_act", True)), + muscle_condition=str(kwargs.get("muscle_condition", "")), + obs_dim=(model.nq - 7) + (model.nv - 6) + 18 + model.na, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.nu, + goal_pos_low=float(kwargs.get("goal_pos", (0.0, 0.0))[0]), + goal_pos_high=float(kwargs.get("goal_pos", (0.0, 0.0))[1]), + goal_rot_low=float(kwargs.get("goal_rot", (0.0, 0.0))[0]), + goal_rot_high=float(kwargs.get("goal_rot", (0.0, 0.0))[1]), + obj_size_change=float(kwargs.get("obj_size_change", 0.0)), + obj_mass_low=float(obj_mass[0]), + obj_mass_high=float(obj_mass[1]), + obj_friction_change=list( + kwargs.get("obj_friction_change", (0.0, 0.0, 0.0)) + ), + pos_th=float(kwargs.get("pos_th", 0.025)), + rot_th=float(kwargs.get("rot_th", 0.262)), + drop_th=float(kwargs.get("drop_th", 0.2)), + reward_pos_dist_w=float( + kwargs.get("weighted_reward_keys", {}).get("pos_dist", 100.0) + ), + reward_rot_dist_w=float( + kwargs.get("weighted_reward_keys", {}).get("rot_dist", 1.0) + ), + reward_bonus_w=float( + kwargs.get("weighted_reward_keys", {}).get("bonus", 0.0) + ), + reward_act_reg_w=float( + kwargs.get("weighted_reward_keys", {}).get("act_reg", 0.0) + ), + reward_penalty_w=float( + kwargs.get("weighted_reward_keys", {}).get("penalty", 0.0) + ), + ) + if overrides: + config = MyoChallengeReorientEnvSpec.gen_config( + **dict( + zip( + MyoChallengeReorientEnvSpec._config_keys, + config, + strict=False, + ), + **overrides, + ) + ) + return ( + config, + MyoChallengeReorientGymnasiumEnvPool, + MyoChallengeReorientEnvSpec, + ) + + +def _relocate_config( + env_id: str, overrides: dict[str, Any] | None = None +) -> tuple[tuple[Any, ...], type, type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + model = _model(kwargs["model_path"]) + target_xyz_range = kwargs["target_xyz_range"] + target_rot_range = kwargs["target_rxryrz_range"] + obj_xyz_range = kwargs.get("obj_xyz_range") + obj_geom_range = kwargs.get("obj_geom_range") + obj_mass_range = kwargs.get("obj_mass_range") + obj_friction_range = kwargs.get("obj_friction_range") + config = MyoChallengeRelocateEnvSpec.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + frame_skip=int(kwargs.get("frame_skip", 5)), + model_path=str(kwargs["model_path"]), + normalize_act=bool(kwargs.get("normalize_act", True)), + muscle_condition=str(kwargs.get("muscle_condition", "")), + obs_dim=(model.nq - 7) + (model.nv - 6) + 18 + model.na, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.nu, + target_xyz_low=list(target_xyz_range["low"]), + target_xyz_high=list(target_xyz_range["high"]), + target_rxryrz_low=list(target_rot_range["low"]), + target_rxryrz_high=list(target_rot_range["high"]), + obj_xyz_low=[] if obj_xyz_range is None else list(obj_xyz_range["low"]), + obj_xyz_high=[] + if obj_xyz_range is None + else list(obj_xyz_range["high"]), + obj_geom_low=[] + if obj_geom_range is None + else list(obj_geom_range["low"]), + obj_geom_high=[] + if obj_geom_range is None + else list(obj_geom_range["high"]), + obj_mass_low=0.0 + if obj_mass_range is None + else float(obj_mass_range["low"]), + obj_mass_high=0.0 + if obj_mass_range is None + else float(obj_mass_range["high"]), + obj_friction_low=[] + if obj_friction_range is None + else list(obj_friction_range["low"]), + obj_friction_high=[] + if obj_friction_range is None + else list(obj_friction_range["high"]), + qpos_noise_range=float(kwargs.get("qpos_noise_range", 0.0)), + pos_th=float(kwargs.get("pos_th", 0.025)), + rot_th=float(kwargs.get("rot_th", 0.262)), + drop_th=float(kwargs.get("drop_th", 0.5)), + reward_pos_dist_w=float( + kwargs.get("weighted_reward_keys", {}).get("pos_dist", 100.0) + ), + reward_rot_dist_w=float( + kwargs.get("weighted_reward_keys", {}).get("rot_dist", 1.0) + ), + reward_act_reg_w=float( + kwargs.get("weighted_reward_keys", {}).get("act_reg", 0.0) + ), + ) + if overrides: + config = MyoChallengeRelocateEnvSpec.gen_config( + **dict( + zip( + MyoChallengeRelocateEnvSpec._config_keys, + config, + strict=False, + ), + **overrides, + ) + ) + return ( + config, + MyoChallengeRelocateGymnasiumEnvPool, + MyoChallengeRelocateEnvSpec, + ) + + +def _reorient_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type, type]: + config, _, _ = _reorient_config(env_id) + values = dict( + zip(MyoChallengeReorientEnvSpec._config_keys, config, strict=False) + ) + pixel = MyoChallengeReorientPixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return ( + pixel, + MyoChallengeReorientPixelGymnasiumEnvPool, + MyoChallengeReorientPixelEnvSpec, + ) + + +def _relocate_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type, type]: + config, _, _ = _relocate_config(env_id) + values = dict( + zip(MyoChallengeRelocateEnvSpec._config_keys, config, strict=False) + ) + pixel = MyoChallengeRelocatePixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return ( + pixel, + MyoChallengeRelocatePixelGymnasiumEnvPool, + MyoChallengeRelocatePixelEnvSpec, + ) + + +def _baoding_config( + env_id: str, overrides: dict[str, Any] | None = None +) -> tuple[tuple[Any, ...], type, type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + model = _model(kwargs["model_path"]) + ball1_gid = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, "ball1") + obj_size_range = kwargs.get("obj_size_range") + obj_mass_range = kwargs.get("obj_mass_range") + obj_friction_change = kwargs.get("obj_friction_change") + config = MyoChallengeBaodingEnvSpec.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + frame_skip=int(kwargs.get("frame_skip", 10)), + model_path=str(kwargs["model_path"]), + normalize_act=bool(kwargs.get("normalize_act", True)), + muscle_condition=str(kwargs.get("muscle_condition", "")), + obs_dim=(model.nq - 14) + 24 + model.na, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.nu, + drop_th=float(kwargs.get("drop_th", 1.25)), + proximity_th=float(kwargs.get("proximity_th", 0.015)), + goal_time_period_low=float(kwargs.get("goal_time_period", (5, 5))[0]), + goal_time_period_high=float(kwargs.get("goal_time_period", (5, 5))[1]), + goal_xrange_low=float(kwargs.get("goal_xrange", (0.025, 0.025))[0]), + goal_xrange_high=float(kwargs.get("goal_xrange", (0.025, 0.025))[1]), + goal_yrange_low=float(kwargs.get("goal_yrange", (0.028, 0.028))[0]), + goal_yrange_high=float(kwargs.get("goal_yrange", (0.028, 0.028))[1]), + task_choice=str(kwargs.get("task_choice", "fixed")), + fixed_task=2, + reward_pos_dist_1_w=float( + kwargs.get("weighted_reward_keys", {}).get("pos_dist_1", 5.0) + ), + reward_pos_dist_2_w=float( + kwargs.get("weighted_reward_keys", {}).get("pos_dist_2", 5.0) + ), + obj_size_low=0.0 + if obj_size_range is None + else float(obj_size_range[0]), + obj_size_high=0.0 + if obj_size_range is None + else float(obj_size_range[1]), + obj_mass_low=0.0 + if obj_mass_range is None + else float(obj_mass_range[0]), + obj_mass_high=0.0 + if obj_mass_range is None + else float(obj_mass_range[1]), + obj_friction_low=[] + if obj_friction_change is None + else list( + ( + model.geom_friction[ball1_gid] - np.asarray(obj_friction_change) + ).tolist() + ), + obj_friction_high=[] + if obj_friction_change is None + else list( + ( + model.geom_friction[ball1_gid] + np.asarray(obj_friction_change) + ).tolist() + ), + ) + if overrides: + config = MyoChallengeBaodingEnvSpec.gen_config( + **dict( + zip( + MyoChallengeBaodingEnvSpec._config_keys, + config, + strict=False, + ), + **overrides, + ) + ) + return ( + config, + MyoChallengeBaodingGymnasiumEnvPool, + MyoChallengeBaodingEnvSpec, + ) + + +def _baoding_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type, type]: + config, _, _ = _baoding_config(env_id) + values = dict( + zip(MyoChallengeBaodingEnvSpec._config_keys, config, strict=False) + ) + pixel = MyoChallengeBaodingPixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return ( + pixel, + MyoChallengeBaodingPixelGymnasiumEnvPool, + MyoChallengeBaodingPixelEnvSpec, + ) + + +def _bimanual_index_sets(model: mujoco.MjModel) -> tuple[list[int], ...]: + myo_qpos: list[int] = [] + myo_dof: list[int] = [] + prosth_qpos: list[int] = [] + prosth_dof: list[int] = [] + manip_joint = model.joint("manip_object/freejoint") + for joint_id in range(model.njnt): + joint = model.joint(joint_id) + if joint.name == "manip_object/freejoint": + continue + if joint.name.startswith("prosthesis"): + prosth_qpos.append(joint.qposadr[0]) + prosth_dof.append(joint.dofadr[0]) + else: + myo_qpos.append(joint.qposadr[0]) + myo_dof.append(joint.dofadr[0]) + return ( + myo_qpos, + myo_dof, + prosth_qpos, + prosth_dof, + [manip_joint.qposadr[0] + i for i in range(7)], + [manip_joint.dofadr[0] + i for i in range(6)], + ) + + +def _bimanual_config( + env_id: str, overrides: dict[str, Any] | None = None +) -> tuple[tuple[Any, ...], type, type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + model = _model(kwargs["model_path"]) + myo_qpos, myo_dof, prosth_qpos, prosth_dof, _, _ = _bimanual_index_sets( + model + ) + obj_bid = model.body("manip_object").id + obj_gid = model.body(obj_bid).geomadr + 1 + base_friction = np.asarray(model.geom_friction[obj_gid]).reshape(-1) + obj_mass_change = kwargs.get("obj_mass_change") + obj_friction_change = kwargs.get("obj_friction_change") + config = MyoChallengeBimanualEnvSpec.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + frame_skip=int(kwargs.get("frame_skip", 5)), + model_path=str(kwargs["model_path"]), + normalize_act=bool(kwargs.get("normalize_act", True)), + muscle_condition=str(kwargs.get("muscle_condition", "")), + obs_dim=1 + + len(myo_qpos) + + len(myo_dof) + + len(prosth_qpos) + + len(prosth_dof) + + 7 + + 6 + + 5 + + model.na, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.nu, + proximity_th=0.17, + start_center=[-0.4, -0.25, 1.05], + goal_center=[0.4, -0.25, 1.05], + start_shifts=[0.055, 0.055, 0.0], + goal_shifts=[0.098, 0.098, 0.0], + reward_reach_dist_w=float( + kwargs.get("weighted_reward_keys", {}).get("reach_dist", -0.1) + ), + reward_act_w=float( + kwargs.get("weighted_reward_keys", {}).get("act", 0.0) + ), + reward_fin_dis_w=float( + kwargs.get("weighted_reward_keys", {}).get("fin_dis", -0.5) + ), + reward_pass_err_w=float( + kwargs.get("weighted_reward_keys", {}).get("pass_err", -1.0) + ), + obj_scale_change=list(kwargs.get("obj_scale_change", [])), + obj_mass_low=0.0 + if obj_mass_change is None + else float(model.body_mass[obj_bid] + obj_mass_change[0]), + obj_mass_high=0.0 + if obj_mass_change is None + else float(model.body_mass[obj_bid] + obj_mass_change[1]), + obj_friction_low=[] + if obj_friction_change is None + else list((base_friction - np.asarray(obj_friction_change)).tolist()), + obj_friction_high=[] + if obj_friction_change is None + else list((base_friction + np.asarray(obj_friction_change)).tolist()), + ) + if overrides: + config = MyoChallengeBimanualEnvSpec.gen_config( + **dict( + zip( + MyoChallengeBimanualEnvSpec._config_keys, + config, + strict=False, + ), + **overrides, + ) + ) + return ( + config, + MyoChallengeBimanualGymnasiumEnvPool, + MyoChallengeBimanualEnvSpec, + ) + + +def _bimanual_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type, type]: + config, _, _ = _bimanual_config(env_id) + values = dict( + zip(MyoChallengeBimanualEnvSpec._config_keys, config, strict=False) + ) + pixel = MyoChallengeBimanualPixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return ( + pixel, + MyoChallengeBimanualPixelGymnasiumEnvPool, + MyoChallengeBimanualPixelEnvSpec, + ) + + +def _runtrack_config( + env_id: str, overrides: dict[str, Any] | None = None +) -> tuple[tuple[Any, ...], type, type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + model = _model(kwargs["model_path"]) + obs_dim = 17 + 17 + 2 + 4 + model.na * 4 + 2 + 2 + config = MyoChallengeRunTrackEnvSpec.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + frame_skip=int(kwargs.get("frame_skip", 5)), + model_path=str(kwargs["model_path"]), + normalize_act=bool(kwargs.get("normalize_act", True)), + muscle_condition=str(kwargs.get("muscle_condition", "")), + obs_dim=obs_dim, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.na, + ctrl_dim=model.nu, + reset_type=str(kwargs.get("reset_type", "random")), + terrain=str(kwargs.get("terrain", "flat")), + start_pos=float(kwargs.get("start_pos", 14)), + end_pos=float(kwargs.get("end_pos", -15)), + real_width=float(kwargs.get("real_width", 1)), + hills_difficulties=list(kwargs.get("hills_difficulties", [])), + rough_difficulties=list(kwargs.get("rough_difficulties", [])), + stairs_difficulties=list(kwargs.get("stairs_difficulties", [])), + reward_sparse_w=1.0, + reward_solved_w=10.0, + ) + if overrides: + config = MyoChallengeRunTrackEnvSpec.gen_config( + **dict( + zip( + MyoChallengeRunTrackEnvSpec._config_keys, + config, + strict=False, + ), + **overrides, + ) + ) + return ( + config, + MyoChallengeRunTrackGymnasiumEnvPool, + MyoChallengeRunTrackEnvSpec, + ) + + +def _runtrack_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type, type]: + config, _, _ = _runtrack_config(env_id) + values = dict( + zip(MyoChallengeRunTrackEnvSpec._config_keys, config, strict=False) + ) + pixel = MyoChallengeRunTrackPixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return ( + pixel, + MyoChallengeRunTrackPixelGymnasiumEnvPool, + MyoChallengeRunTrackPixelEnvSpec, + ) + + +def _soccer_config( + env_id: str, overrides: dict[str, Any] | None = None +) -> tuple[tuple[Any, ...], type, type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + model = _model(kwargs["model_path"]) + internal_joint_count = sum( + 1 + for joint_id in range(model.njnt) + if mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_JOINT, joint_id) + not in (None, "root") + ) + obs_dim = ( + internal_joint_count + + internal_joint_count + + 4 + + 4 + + 3 + + 7 + + 6 + + model.na * 4 + ) + probabilities = kwargs.get("goalkeeper_probabilities", [0.1, 0.45, 0.45]) + config = MyoChallengeSoccerEnvSpec.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + frame_skip=int(kwargs.get("frame_skip", 10)), + model_path=str(kwargs["model_path"]), + normalize_act=bool(kwargs.get("normalize_act", True)), + muscle_condition=str(kwargs.get("muscle_condition", "")), + obs_dim=obs_dim, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.nu, + reset_type=str(kwargs.get("reset_type", "none")), + min_agent_spawn_distance=float( + kwargs.get("min_agent_spawn_distance", 1) + ), + random_vel_low=float(kwargs.get("random_vel_range", [1.0, 5.0])[0]), + random_vel_high=float(kwargs.get("random_vel_range", [1.0, 5.0])[1]), + rnd_pos_noise=float(kwargs.get("rnd_pos_noise", 1.0)), + rnd_joint_noise=float(kwargs.get("rnd_joint_noise", 0.02)), + goalkeeper_probabilities=list(probabilities), + max_time_sec=float(kwargs.get("max_time_sec", 10)), + reward_goal_scored_w=1000.0, + reward_time_cost_w=-0.01, + reward_act_reg_w=-100.0, + reward_pain_w=-10.0, + ) + if overrides: + config = MyoChallengeSoccerEnvSpec.gen_config( + **dict( + zip( + MyoChallengeSoccerEnvSpec._config_keys, config, strict=False + ), + **overrides, + ) + ) + return ( + config, + MyoChallengeSoccerGymnasiumEnvPool, + MyoChallengeSoccerEnvSpec, + ) + + +def _soccer_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type, type]: + config, _, _ = _soccer_config(env_id) + values = dict( + zip(MyoChallengeSoccerEnvSpec._config_keys, config, strict=False) + ) + pixel = MyoChallengeSoccerPixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return ( + pixel, + MyoChallengeSoccerPixelGymnasiumEnvPool, + MyoChallengeSoccerPixelEnvSpec, + ) + + +def _chasetag_config( + env_id: str, overrides: dict[str, Any] | None = None +) -> tuple[tuple[Any, ...], type, type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + model = _model(kwargs["model_path"]) + obs_dim = 28 + 28 + 4 + 4 + 3 + 2 + 2 + 2 + model.na * 4 + config = MyoChallengeChaseTagEnvSpec.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + frame_skip=int(kwargs.get("frame_skip", 10)), + model_path=str(kwargs["model_path"]), + normalize_act=bool(kwargs.get("normalize_act", True)), + muscle_condition=str(kwargs.get("muscle_condition", "")), + obs_dim=obs_dim, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.nu, + reset_type=str(kwargs.get("reset_type", "init")), + win_distance=float(kwargs.get("win_distance", 0.5)), + min_spawn_distance=float(kwargs.get("min_spawn_distance", 2)), + task_choice=str(kwargs.get("task_choice", "CHASE")), + terrain=str(kwargs.get("terrain", "FLAT")), + repeller_opponent=bool(kwargs.get("repeller_opponent", False)), + hills_range=list(kwargs.get("hills_range", [0.0, 0.0])), + rough_range=list(kwargs.get("rough_range", [0.0, 0.0])), + relief_range=list(kwargs.get("relief_range", [0.0, 0.0])), + chase_vel_low=float(kwargs.get("chase_vel_range", [1.0, 1.0])[0]), + chase_vel_high=float(kwargs.get("chase_vel_range", [1.0, 1.0])[1]), + random_vel_low=float(kwargs.get("random_vel_range", [1.0, 1.0])[0]), + random_vel_high=float(kwargs.get("random_vel_range", [1.0, 1.0])[1]), + repeller_vel_low=float(kwargs.get("repeller_vel_range", [1.0, 1.0])[0]), + repeller_vel_high=float( + kwargs.get("repeller_vel_range", [1.0, 1.0])[1] + ), + opponent_probabilities=list( + kwargs.get("opponent_probabilities", [0.1, 0.45, 0.45]) + ), + reward_distance_w=-0.1, + reward_lose_w=-1000.0, + ) + if overrides: + config = MyoChallengeChaseTagEnvSpec.gen_config( + **dict( + zip( + MyoChallengeChaseTagEnvSpec._config_keys, + config, + strict=False, + ), + **overrides, + ) + ) + return ( + config, + MyoChallengeChaseTagGymnasiumEnvPool, + MyoChallengeChaseTagEnvSpec, + ) + + +def _chasetag_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type, type]: + config, _, _ = _chasetag_config(env_id) + values = dict( + zip(MyoChallengeChaseTagEnvSpec._config_keys, config, strict=False) + ) + pixel = MyoChallengeChaseTagPixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return ( + pixel, + MyoChallengeChaseTagPixelGymnasiumEnvPool, + MyoChallengeChaseTagPixelEnvSpec, + ) + + +def _tabletennis_config( + env_id: str, overrides: dict[str, Any] | None = None +) -> tuple[tuple[Any, ...], type, type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + model_path = str(resolve_myosuite_model_path(kwargs["model_path"])) + model = _model(kwargs["model_path"]) + body_qpos_count = sum( + 1 + for joint_id in range(model.njnt) + if ( + ( + name := mujoco.mj_id2name( + model, mujoco.mjtObj.mjOBJ_JOINT, joint_id + ) + ) + is not None + and not name.startswith("ping") + and name != "pingpong_freejoint" + and name != "paddle_freejoint" + ) + ) + body_dof_count = sum( + 1 + for joint_id in range(model.njnt) + if ( + ( + name := mujoco.mj_id2name( + model, mujoco.mjtObj.mjOBJ_JOINT, joint_id + ) + ) + is not None + and not name.startswith("ping") + and name != "paddle_freejoint" + ) + ) + obs_dim = ( + 3 + + body_qpos_count + + body_dof_count + + 3 + + 3 + + 3 + + 3 + + 4 + + 3 + + 6 + + model.na + ) + ball_xyz_range = kwargs.get("ball_xyz_range") or {} + ball_friction_range = kwargs.get("ball_friction_range") or {} + paddle_mass_range = kwargs.get("paddle_mass_range") or [0.0, 0.0] + qpos_noise_range = kwargs.get("qpos_noise_range") + qpos_noise_low = float("nan") + qpos_noise_high = float("nan") + if qpos_noise_range is not None: + qpos_noise_low = -float(qpos_noise_range) + qpos_noise_high = float(qpos_noise_range) + config = MyoChallengeTableTennisEnvSpec.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + frame_skip=int(kwargs.get("frame_skip", 5)), + model_path=model_path, + normalize_act=bool(kwargs.get("normalize_act", True)), + muscle_condition=str(kwargs.get("muscle_condition", "")), + obs_dim=obs_dim, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.nu, + ball_xyz_low=list(ball_xyz_range.get("low", [])), + ball_xyz_high=list(ball_xyz_range.get("high", [])), + ball_qvel=bool(kwargs.get("ball_qvel", False)), + ball_friction_low=list(ball_friction_range.get("low", [])), + ball_friction_high=list(ball_friction_range.get("high", [])), + paddle_mass_low=float(paddle_mass_range[0]), + paddle_mass_high=float(paddle_mass_range[1]), + qpos_noise_low=qpos_noise_low, + qpos_noise_high=qpos_noise_high, + rally_count=int(kwargs.get("rally_count", 1)), + reward_reach_dist_w=1.0, + reward_palm_dist_w=1.0, + reward_paddle_quat_w=2.0, + reward_act_reg_w=0.5, + reward_torso_up_w=2.0, + reward_sparse_w=100.0, + reward_solved_w=1000.0, + reward_done_w=-10.0, + ) + if overrides: + config = MyoChallengeTableTennisEnvSpec.gen_config( + **dict( + zip( + MyoChallengeTableTennisEnvSpec._config_keys, + config, + strict=False, + ), + **overrides, + ) + ) + return ( + config, + MyoChallengeTableTennisGymnasiumEnvPool, + MyoChallengeTableTennisEnvSpec, + ) + + +def _tabletennis_pixel_config( + env_id: str, +) -> tuple[tuple[Any, ...], type, type]: + config, _, _ = _tabletennis_config(env_id) + values = dict( + zip(MyoChallengeTableTennisEnvSpec._config_keys, config, strict=False) + ) + pixel = MyoChallengeTableTennisPixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return ( + pixel, + MyoChallengeTableTennisPixelGymnasiumEnvPool, + MyoChallengeTableTennisPixelEnvSpec, + ) + + +def _oracle_class(env_id: str) -> Any: + entry = _entry(env_id) + return load_oracle_class(entry["entry_module"], entry["class_name"]) + + +def _oracle_kwargs(env_id: str) -> dict[str, Any]: + kwargs = dict(_entry(env_id)["kwargs"]) + for path_key in ("model_path", "init_pose_path"): + if path_key in kwargs: + kwargs[path_key] = str(_oracle_model_path(kwargs[path_key])) + return kwargs + + +def _oracle_live_obs(unwrapped: Any) -> np.ndarray: + obs_dict = unwrapped.get_obs_dict(unwrapped.sim) + _, obs = unwrapped.obsdict2obsvec(obs_dict, unwrapped.obs_keys) + return np.asarray(obs, dtype=np.float64) + + +def _oracle_reset_sync( + env: Any, env_id: str +) -> tuple[np.ndarray, dict[str, Any]]: + obs, _ = env.reset() + unwrapped = env.unwrapped + sim = unwrapped.sim + sync: dict[str, Any] = { + "test_reset_qpos": sim.data.qpos.copy().tolist(), + "test_reset_qvel": sim.data.qvel.copy().tolist(), + "test_reset_act": ( + sim.data.act.copy().tolist() if sim.model.na > 0 else [] + ), + "test_reset_qacc_warmstart": sim.data.qacc_warmstart.copy().tolist(), + } + entry = _entry(env_id) + if entry["class_name"] == "ReorientEnvV0": + sync["test_reset_act_dot"] = ( + sim.data.act_dot.copy().tolist() if sim.model.na > 0 else [] + ) + goal_bid = sim.model.body_name2id("target") + target_gid = sim.model.geom_name2id("target_dice") + object_bid = sim.model.body_name2id("Object") + start_geom = sim.model.body_geomadr[object_bid] + geom_count = sim.model.body_geomnum[object_bid] + sync["test_goal_body_pos"] = ( + sim.model.body_pos[goal_bid].copy().tolist() + ) + sync["test_goal_body_quat"] = ( + sim.model.body_quat[goal_bid].copy().tolist() + ) + sync["test_target_geom_size"] = ( + sim.model.geom_size[target_gid].copy().tolist() + ) + sync["test_object_geom_size"] = ( + sim.model + .geom_size[start_geom : start_geom + geom_count] + .reshape(-1) + .tolist() + ) + sync["test_object_geom_pos"] = ( + sim.model + .geom_pos[start_geom : start_geom + geom_count] + .reshape(-1) + .tolist() + ) + sync["test_object_geom_friction"] = ( + sim.model + .geom_friction[start_geom : start_geom + geom_count] + .reshape(-1) + .tolist() + ) + sync["test_object_body_mass"] = [float(sim.model.body_mass[object_bid])] + elif entry["class_name"] == "RelocateEnvV0": + sync["test_reset_act_dot"] = ( + sim.data.act_dot.copy().tolist() if sim.model.na > 0 else [] + ) + sync["test_reset_ctrl"] = sim.data.ctrl.copy().tolist() + goal_bid = sim.model.body_name2id("target") + goal_mocap_id = int(sim.model.body_mocapid[goal_bid]) + object_bid = sim.model.body_name2id("Object") + start_geom = sim.model.body_geomadr[object_bid] + geom_count = sim.model.body_geomnum[object_bid] + sync["test_goal_body_pos"] = ( + sim.model.body_pos[goal_bid].copy().tolist() + ) + sync["test_goal_body_quat"] = ( + sim.model.body_quat[goal_bid].copy().tolist() + ) + if goal_mocap_id >= 0: + sync["test_goal_mocap_pos"] = ( + sim.data.mocap_pos[goal_mocap_id].copy().tolist() + ) + sync["test_goal_mocap_quat"] = ( + sim.data.mocap_quat[goal_mocap_id].copy().tolist() + ) + sync["test_object_body_pos"] = ( + sim.model.body_pos[object_bid].copy().tolist() + ) + sync["test_object_body_mass"] = [float(sim.model.body_mass[object_bid])] + sync["test_object_geom_type"] = ( + sim.model + .geom_type[start_geom : start_geom + geom_count] + .astype(np.float64) + .tolist() + ) + sync["test_object_geom_size"] = ( + sim.model + .geom_size[start_geom : start_geom + geom_count] + .reshape(-1) + .tolist() + ) + sync["test_object_geom_pos"] = ( + sim.model + .geom_pos[start_geom : start_geom + geom_count] + .reshape(-1) + .tolist() + ) + sync["test_object_geom_quat"] = ( + sim.model + .geom_quat[start_geom : start_geom + geom_count] + .reshape(-1) + .tolist() + ) + sync["test_object_geom_rgba"] = ( + sim.model + .geom_rgba[start_geom : start_geom + geom_count] + .reshape(-1) + .tolist() + ) + sync["test_object_geom_friction"] = ( + sim.model + .geom_friction[start_geom : start_geom + geom_count] + .reshape(-1) + .tolist() + ) + # Upstream can recursively re-reset here to avoid contact, but still + # return the first stale observation. Align against the final live sim. + obs = _oracle_live_obs(unwrapped) + elif entry["class_name"] == "BaodingEnvV1": + sync["test_task"] = int(unwrapped.which_task.value) + sync["test_ball1_starting_angle"] = float( + unwrapped.ball_1_starting_angle + ) + sync["test_ball2_starting_angle"] = float( + unwrapped.ball_2_starting_angle + ) + sync["test_x_radius"] = float(unwrapped.x_radius) + sync["test_y_radius"] = float(unwrapped.y_radius) + sync["test_goal_trajectory"] = ( + np.asarray(unwrapped.goal, dtype=np.float64).reshape(-1).tolist() + ) + sync["test_target1_site_pos"] = ( + sim.model.site_pos[unwrapped.target1_sid].copy().tolist() + ) + sync["test_target2_site_pos"] = ( + sim.model.site_pos[unwrapped.target2_sid].copy().tolist() + ) + sync["test_object1_body_mass"] = [ + float(sim.model.body_mass[unwrapped.object1_bid]) + ] + sync["test_object2_body_mass"] = [ + float(sim.model.body_mass[unwrapped.object2_bid]) + ] + sync["test_object1_geom_size"] = ( + sim.model.geom_size[unwrapped.object1_gid].copy().tolist() + ) + sync["test_object2_geom_size"] = ( + sim.model.geom_size[unwrapped.object2_gid].copy().tolist() + ) + sync["test_object1_geom_friction"] = ( + sim.model.geom_friction[unwrapped.object1_gid].copy().tolist() + ) + sync["test_object2_geom_friction"] = ( + sim.model.geom_friction[unwrapped.object2_gid].copy().tolist() + ) + elif entry["class_name"] == "BimanualEnvV1": + sync["test_start_pos"] = np.asarray( + unwrapped.start_pos, dtype=np.float64 + ).tolist() + sync["test_goal_pos"] = np.asarray( + unwrapped.goal_pos, dtype=np.float64 + ).tolist() + sync["test_object_body_mass"] = [ + float(sim.model.body_mass[unwrapped.obj_bid]) + ] + sync["test_object_geom_size"] = ( + sim.model.geom_size[unwrapped.obj_gid].copy().tolist() + ) + sync["test_object_geom_friction"] = ( + sim.model.geom_friction[unwrapped.obj_gid].copy().tolist() + ) + obs = np.asarray(unwrapped.get_obs(), dtype=np.float64) + elif entry["class_name"] == "RunTrack": + terrain_geom_id = sim.model.geom_name2id("terrain") + hfield_id = int(sim.model.geom_dataid[terrain_geom_id]) + nrow = int(sim.model.hfield_nrow[hfield_id]) + ncol = int(sim.model.hfield_ncol[hfield_id]) + adr = int(sim.model.hfield_adr[hfield_id]) + sync["test_hfield_data"] = ( + sim.model.hfield_data[adr : adr + nrow * ncol].copy().tolist() + ) + sync["test_terrain_geom_rgba"] = ( + sim.model.geom_rgba[terrain_geom_id].copy().tolist() + ) + sync["test_terrain_geom_pos"] = ( + sim.model.geom_pos[terrain_geom_id].copy().tolist() + ) + sync["test_terrain_type"] = int( + np.asarray(unwrapped.terrain_type).reshape(-1)[0] + ) + sync["test_osl_state"] = _runtrack_osl_state_id( + unwrapped.OSL_CTRL.STATE_MACHINE.current_state.get_name() + ) + obs = np.asarray(unwrapped.get_obs(), dtype=np.float64) + elif entry["class_name"] == "SoccerEnvV0": + sync["test_reset_ctrl"] = ( + sim.data.ctrl.copy().tolist() if sim.model.nu > 0 else [] + ) + sync["test_reset_act_dot"] = ( + sim.data.act_dot.copy().tolist() if sim.model.na > 0 else [] + ) + sync["test_reset_qacc"] = sim.data.qacc.copy().tolist() + goalkeeper = unwrapped.goalkeeper + sync["test_goalkeeper_pose"] = np.asarray( + goalkeeper.get_goalkeeper_pose(), dtype=np.float64 + ).tolist() + sync["test_goalkeeper_velocity"] = np.asarray( + goalkeeper.goalkeeper_vel, dtype=np.float64 + ).tolist() + sync["test_goalkeeper_noise_buffer"] = ( + np + .asarray(goalkeeper.noise_process.buffer, dtype=np.float64) + .reshape(-1) + .tolist() + ) + sync["test_goalkeeper_noise_idx"] = int(goalkeeper.noise_process.idx) + sync["test_goalkeeper_block_velocity"] = float( + goalkeeper.block_velocity + ) + sync["test_goalkeeper_policy"] = _soccer_policy_id( + goalkeeper.goalkeeper_policy + ) + # Soccer mutates `sim` after reset and only forwards the live sim, not + # the observed sim used by `get_obs()`. Align against the final live + # sim state instead of the stale reset return value. + obs = _oracle_live_obs(unwrapped) + elif entry["class_name"] == "ChaseTagEnvV0": + sync["test_reset_ctrl"] = ( + sim.data.ctrl.copy().tolist() if sim.model.nu > 0 else [] + ) + sync["test_reset_act_dot"] = ( + sim.data.act_dot.copy().tolist() if sim.model.na > 0 else [] + ) + sync["test_reset_qacc"] = sim.data.qacc.copy().tolist() + opponent = unwrapped.opponent + terrain_geom_id = sim.model.geom_name2id("terrain") + hfield_id = int(sim.model.geom_dataid[terrain_geom_id]) + sync["test_task"] = int(unwrapped.current_task.value) + sync["test_hfield_data"] = ( + sim.model + .hfield_data[ + int(sim.model.hfield_adr[hfield_id]) : int( + sim.model.hfield_adr[hfield_id] + ) + + int(sim.model.hfield_nrow[hfield_id]) + * int(sim.model.hfield_ncol[hfield_id]) + ] + .copy() + .tolist() + ) + sync["test_terrain_geom_rgba"] = ( + sim.model.geom_rgba[terrain_geom_id].copy().tolist() + ) + sync["test_terrain_geom_pos"] = ( + sim.model.geom_pos[terrain_geom_id].copy().tolist() + ) + sync["test_opponent_pose"] = np.asarray( + opponent.get_opponent_pose(), dtype=np.float64 + ).tolist() + sync["test_opponent_velocity"] = np.asarray( + opponent.opponent_vel, dtype=np.float64 + ).tolist() + sync["test_opponent_noise_buffer"] = ( + np + .asarray(opponent.noise_process.buffer, dtype=np.float64) + .reshape(-1) + .tolist() + ) + sync["test_opponent_noise_idx"] = int(opponent.noise_process.idx) + sync["test_chase_velocity"] = float(opponent.chase_velocity) + sync["test_opponent_policy"] = _chasetag_policy_id( + opponent.opponent_policy + ) + # ChaseTag has the same live-sim vs observed-sim reset split as Soccer. + obs = _oracle_live_obs(unwrapped) + elif entry["class_name"] == "TableTennisEnvV0": + sync["test_ball_body_pos"] = ( + sim.model.body_pos[unwrapped.id_info.ball_bid].copy().tolist() + ) + sync["test_ball_geom_friction"] = ( + sim.model.geom_friction[unwrapped.id_info.ball_gid].copy().tolist() + ) + sync["test_paddle_body_mass"] = [ + float(sim.model.body_mass[unwrapped.id_info.paddle_bid]) + ] + sync["test_init_qpos"] = np.asarray( + unwrapped.init_qpos, dtype=np.float64 + ).tolist() + sync["test_init_qvel"] = np.asarray( + unwrapped.init_qvel, dtype=np.float64 + ).tolist() + sync["test_reset_act_dot"] = ( + sim.data.act_dot.copy().tolist() if sim.model.na > 0 else [] + ) + return obs, sync + + +def _alignment_obs_atol(env_id: str) -> float: + class_name = _entry(env_id)["class_name"] + if class_name == "BimanualEnvV1": + return _BIMANUAL_ALIGNMENT_ATOL + if class_name in {"RunTrack", "SoccerEnvV0", "ChaseTagEnvV0"}: + return _LOCOMOTION_ALIGNMENT_ATOL + if class_name == "TableTennisEnvV0": + return _TABLETENNIS_ALIGNMENT_ATOL + return _DEFAULT_ALIGNMENT_ATOL + + +def _alignment_reward_atol(env_id: str) -> float: + class_name = _entry(env_id)["class_name"] + if class_name in {"RunTrack", "SoccerEnvV0", "ChaseTagEnvV0"}: + return _LOCOMOTION_ALIGNMENT_ATOL + if class_name in {"BimanualEnvV1", "TableTennisEnvV0"}: + return _TABLETENNIS_ALIGNMENT_ATOL + return _DEFAULT_ALIGNMENT_ATOL + + +def _assert_alignment_with_oracle( + case: absltest.TestCase, + env_id: str, + config_fn: Any, + *, + action_seed: int, + steps: int = _ALIGNMENT_STEPS, +) -> None: + entry = _entry(env_id) + oracle_cls = _oracle_class(env_id) + oracle: Any = gymnasium.wrappers.TimeLimit( + oracle_cls(seed=42, **_oracle_kwargs(env_id)), + max_episode_steps=int(entry["max_episode_steps"]), + ) + obs0, sync = _oracle_reset_sync(oracle, env_id) + config, pool_type, spec_type = config_fn(env_id, overrides=sync) + native = _make_env(config, pool_type, spec_type) + obs_atol = _alignment_obs_atol(env_id) + reward_atol = _alignment_reward_atol(env_id) + try: + obs1, _ = native.reset() + np.testing.assert_allclose(obs1[0], obs0, atol=obs_atol, rtol=1e-5) + actions = _seeded_actions( + _batched_action_shape(native), steps, action_seed + ) + for action in actions: + obs0, reward0, terminated0, truncated0, _ = oracle.step(action[0]) + obs1, reward1, terminated1, truncated1, _ = native.step(action) + np.testing.assert_allclose(obs1[0], obs0, atol=obs_atol, rtol=1e-5) + np.testing.assert_allclose( + reward1[0], reward0, atol=reward_atol, rtol=1e-5 + ) + case.assertEqual(bool(terminated1[0]), bool(terminated0)) + case.assertEqual(bool(truncated1[0]), bool(truncated0)) + if terminated0 or truncated0: + break + finally: + native.close() + oracle.close() + + +class MyoSuiteMyoChallengeNativeTest(absltest.TestCase): + """Covers the internal native MyoChallenge slice implemented so far.""" + + def test_reorient_construct_and_reset_all_ids(self) -> None: + """Every native reorient ID should construct and reset.""" + for env_id in _REORIENT_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _reorient_config(env_id) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape[0], 1) + env.close() + + def test_relocate_construct_and_reset_all_ids(self) -> None: + """Every native relocate ID should construct and reset.""" + for env_id in _RELOCATE_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _relocate_config(env_id) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape[0], 1) + env.close() + + def test_reorient_native_determinism(self) -> None: + """Reorient rollouts should be deterministic for a fixed action trace.""" + for env_id in _REORIENT_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _reorient_config(env_id) + env0 = _make_env(config, pool_type, spec_type) + env1 = _make_env(config, pool_type, spec_type) + actions = _seeded_actions(_batched_action_shape(env0), 12, 1234) + _assert_rollouts_match( + self, env0, env1, actions, atol=1e-8, rtol=1e-8 + ) + env0.close() + env1.close() + + def test_relocate_native_determinism(self) -> None: + """Relocate rollouts should be deterministic for a fixed action trace.""" + for env_id in _RELOCATE_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _relocate_config(env_id) + env0 = _make_env(config, pool_type, spec_type) + env1 = _make_env(config, pool_type, spec_type) + actions = _seeded_actions(_batched_action_shape(env0), 12, 5678) + _assert_rollouts_match( + self, env0, env1, actions, atol=1e-8, rtol=1e-8 + ) + env0.close() + env1.close() + + def test_baoding_construct_and_reset_all_ids(self) -> None: + """Every native baoding ID should construct and reset.""" + for env_id in _BAODING_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _baoding_config(env_id) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape[0], 1) + env.close() + + def test_baoding_native_determinism(self) -> None: + """Baoding rollouts should be deterministic for a fixed action trace.""" + for env_id in _BAODING_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _baoding_config(env_id) + env0 = _make_env(config, pool_type, spec_type) + env1 = _make_env(config, pool_type, spec_type) + actions = _seeded_actions(_batched_action_shape(env0), 12, 2468) + _assert_rollouts_match( + self, env0, env1, actions, atol=1e-8, rtol=1e-8 + ) + env0.close() + env1.close() + + def test_bimanual_construct_and_reset_all_ids(self) -> None: + """Every native bimanual ID should construct and reset.""" + for env_id in _BIMANUAL_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _bimanual_config(env_id) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape[0], 1) + env.close() + + def test_bimanual_native_determinism(self) -> None: + """Bimanual rollouts should be deterministic for a fixed action trace.""" + for env_id in _BIMANUAL_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _bimanual_config(env_id) + env0 = _make_env(config, pool_type, spec_type) + env1 = _make_env(config, pool_type, spec_type) + actions = _seeded_actions(_batched_action_shape(env0), 12, 9753) + _assert_rollouts_match( + self, env0, env1, actions, atol=1e-8, rtol=1e-8 + ) + env0.close() + env1.close() + + def test_runtrack_construct_and_reset_all_ids(self) -> None: + """Every native RunTrack ID should construct and reset.""" + for env_id in _RUNTRACK_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _runtrack_config(env_id) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape[0], 1) + env.close() + + def test_runtrack_native_determinism(self) -> None: + """RunTrack rollouts should be deterministic for a fixed action trace.""" + for env_id in _RUNTRACK_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _runtrack_config(env_id) + env0 = _make_env(config, pool_type, spec_type) + env1 = _make_env(config, pool_type, spec_type) + actions = _seeded_actions(_batched_action_shape(env0), 12, 1597) + _assert_rollouts_match( + self, env0, env1, actions, atol=1e-8, rtol=1e-8 + ) + env0.close() + env1.close() + + def test_soccer_construct_and_reset_all_ids(self) -> None: + """Every native soccer ID should construct and reset.""" + for env_id in _SOCCER_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _soccer_config(env_id) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape[0], 1) + env.close() + + def test_soccer_native_determinism(self) -> None: + """Soccer rollouts should be deterministic for a fixed action trace.""" + for env_id in _SOCCER_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _soccer_config(env_id) + env0 = _make_env(config, pool_type, spec_type) + env1 = _make_env(config, pool_type, spec_type) + actions = _seeded_actions(_batched_action_shape(env0), 12, 1601) + _assert_rollouts_match( + self, env0, env1, actions, atol=1e-8, rtol=1e-8 + ) + env0.close() + env1.close() + + def test_chasetag_construct_and_reset_all_ids(self) -> None: + """Every native ChaseTag ID should construct and reset.""" + for env_id in _CHASETAG_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _chasetag_config(env_id) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape[0], 1) + env.close() + + def test_chasetag_native_determinism(self) -> None: + """ChaseTag rollouts should be deterministic for a fixed action trace.""" + for env_id in _CHASETAG_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _chasetag_config(env_id) + env0 = _make_env(config, pool_type, spec_type) + env1 = _make_env(config, pool_type, spec_type) + actions = _seeded_actions(_batched_action_shape(env0), 12, 1607) + _assert_rollouts_match( + self, env0, env1, actions, atol=1e-8, rtol=1e-8 + ) + env0.close() + env1.close() + + def test_tabletennis_construct_and_reset_all_ids(self) -> None: + """Every native TableTennis ID should construct and reset.""" + for env_id in _TABLETENNIS_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _tabletennis_config(env_id) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape[0], 1) + env.close() + + def test_tabletennis_native_determinism(self) -> None: + """TableTennis rollouts should be deterministic for a fixed action trace.""" + for env_id in _TABLETENNIS_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _tabletennis_config(env_id) + env0 = _make_env(config, pool_type, spec_type) + env1 = _make_env(config, pool_type, spec_type) + actions = _seeded_actions(_batched_action_shape(env0), 12, 1613) + _assert_rollouts_match( + self, env0, env1, actions, atol=1e-8, rtol=1e-8 + ) + env0.close() + env1.close() + + def test_reorient_alignment_with_oracle(self) -> None: + """Reorient should align stepwise with the official oracle.""" + cls = _oracle_class(_REORIENT_IDS[0]) + for env_id in _REORIENT_IDS: + with self.subTest(env_id=env_id): + oracle: Any = gymnasium.wrappers.TimeLimit( + cls(seed=123, **_oracle_kwargs(env_id)), + max_episode_steps=int(_entry(env_id)["max_episode_steps"]), + ) + obs0, sync = _oracle_reset_sync(oracle, env_id) + config, pool_type, spec_type = _reorient_config( + env_id, overrides=sync + ) + native = _make_env(config, pool_type, spec_type) + obs1, _ = native.reset() + _assert_reorient_obs_close(obs1[0], obs0) + actions = _seeded_actions( + _batched_action_shape(native), _ALIGNMENT_STEPS, 4321 + ) + for action in actions: + obs0, reward0, terminated0, truncated0, _ = oracle.step( + action[0] + ) + obs1, reward1, terminated1, truncated1, _ = native.step( + action + ) + _assert_reorient_obs_close(obs1[0], obs0) + np.testing.assert_allclose( + reward1[0], + float(reward0), + atol=_reorient_alignment_reward_atol(), + rtol=1e-5, + ) + self.assertEqual(bool(terminated1[0]), bool(terminated0)) + self.assertEqual(bool(truncated1[0]), bool(truncated0)) + if terminated0 or truncated0: + break + native.close() + oracle.close() + + def test_reorient_fatigue_first_step_act_matches_oracle(self) -> None: + """Fatigue Reorient should preserve upstream warm-started act state.""" + env_id = "myoFatiChallengeDieReorientP1-v0" + oracle: Any = gymnasium.wrappers.TimeLimit( + _oracle_class(env_id)(seed=123, **_oracle_kwargs(env_id)), + max_episode_steps=int(_entry(env_id)["max_episode_steps"]), + ) + obs0, sync = _oracle_reset_sync(oracle, env_id) + config, pool_type, spec_type = _reorient_config(env_id, overrides=sync) + native = _make_env(config, pool_type, spec_type) + obs1, _ = native.reset() + _assert_reorient_obs_close(obs1[0], obs0) + action = _seeded_actions(_batched_action_shape(native), 1, 104)[0] + obs0, reward0, terminated0, truncated0, _ = oracle.step(action[0]) + obs1, reward1, terminated1, truncated1, _ = native.step(action) + act_dim = oracle.unwrapped.sim.model.na + np.testing.assert_allclose( + obs1[0][-act_dim:], + obs0[-act_dim:], + atol=5e-8, + rtol=1e-8, + ) + np.testing.assert_allclose( + reward1[0], float(reward0), atol=5e-8, rtol=1e-8 + ) + self.assertEqual(bool(terminated1[0]), bool(terminated0)) + self.assertEqual(bool(truncated1[0]), bool(truncated0)) + native.close() + oracle.close() + + def test_relocate_alignment_with_oracle(self) -> None: + """Relocate should align stepwise with the official oracle.""" + cls = _oracle_class(_RELOCATE_IDS[0]) + for env_id in _RELOCATE_IDS: + with self.subTest(env_id=env_id): + oracle: Any = gymnasium.wrappers.TimeLimit( + cls(seed=123, **_oracle_kwargs(env_id)), + max_episode_steps=int(_entry(env_id)["max_episode_steps"]), + ) + obs0, sync = _oracle_reset_sync(oracle, env_id) + config, pool_type, spec_type = _relocate_config( + env_id, overrides=sync + ) + native = _make_env(config, pool_type, spec_type) + obs1, _ = native.reset() + # Official relocate observations are reconstructed via robot + # sensor->sim propagation, which leaves an isolated ~2e-4 drift + # on object_z / pos_err_z after reset-sync. + np.testing.assert_allclose( + obs1[0], + obs0, + atol=_relocate_alignment_obs_atol(env_id), + rtol=1e-5, + ) + actions = _seeded_actions( + _batched_action_shape(native), _ALIGNMENT_STEPS, 8765 + ) + for action in actions: + obs0, reward0, terminated0, truncated0, _ = oracle.step( + action[0] + ) + obs1, reward1, terminated1, truncated1, _ = native.step( + action + ) + np.testing.assert_allclose( + obs1[0], + obs0, + atol=_relocate_alignment_obs_atol(env_id), + rtol=1e-5, + ) + np.testing.assert_allclose( + reward1[0], + float(reward0), + atol=_relocate_alignment_reward_atol(env_id), + rtol=1e-5, + ) + self.assertEqual(bool(terminated1[0]), bool(terminated0)) + self.assertEqual(bool(truncated1[0]), bool(truncated0)) + if terminated0 or truncated0: + break + native.close() + oracle.close() + + def test_baoding_alignment_with_oracle(self) -> None: + """Baoding should align stepwise with the official oracle.""" + for env_id in _BAODING_IDS: + with self.subTest(env_id=env_id): + _assert_alignment_with_oracle( + self, env_id, _baoding_config, action_seed=1123 + ) + + def test_bimanual_alignment_with_oracle(self) -> None: + """Bimanual should align stepwise with the official oracle.""" + for env_id in _BIMANUAL_IDS: + with self.subTest(env_id=env_id): + _assert_alignment_with_oracle( + self, env_id, _bimanual_config, action_seed=1223 + ) + + def test_runtrack_alignment_with_oracle(self) -> None: + """RunTrack should align stepwise with the official oracle.""" + for env_id in _RUNTRACK_IDS: + with self.subTest(env_id=env_id): + _assert_alignment_with_oracle( + self, + env_id, + _runtrack_config, + action_seed=1323, + ) + + def test_soccer_alignment_with_oracle(self) -> None: + """Soccer should align stepwise with the official oracle.""" + for env_id in _SOCCER_IDS: + with self.subTest(env_id=env_id): + _assert_alignment_with_oracle( + self, + env_id, + _soccer_config, + action_seed=1423, + ) + + def test_chasetag_alignment_with_oracle(self) -> None: + """ChaseTag should align stepwise with the official oracle.""" + for env_id in _CHASETAG_IDS: + with self.subTest(env_id=env_id): + _assert_alignment_with_oracle( + self, + env_id, + _chasetag_config, + action_seed=1523, + ) + + def test_tabletennis_alignment_with_oracle(self) -> None: + """TableTennis should align stepwise with the official oracle.""" + for env_id in _TABLETENNIS_IDS: + with self.subTest(env_id=env_id): + _assert_alignment_with_oracle( + self, + env_id, + _tabletennis_config, + action_seed=1623, + ) + + def test_reorient_pixel_observation_smoke(self) -> None: + """Reorient pixel wrappers should emit non-empty batched frames.""" + config, pool_type, spec_type = _reorient_pixel_config(_REORIENT_IDS[0]) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape[0], 1) + self.assertGreater(obs.size, 0) + env.close() + + def test_relocate_pixel_observation_smoke(self) -> None: + """Relocate pixel wrappers should emit non-empty batched frames.""" + config, pool_type, spec_type = _relocate_pixel_config(_RELOCATE_IDS[0]) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape[0], 1) + self.assertGreater(obs.size, 0) + env.close() + + def test_baoding_pixel_observation_smoke(self) -> None: + """Baoding pixel wrappers should emit non-empty batched frames.""" + config, pool_type, spec_type = _baoding_pixel_config(_BAODING_IDS[0]) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape[0], 1) + self.assertGreater(obs.size, 0) + env.close() + + def test_bimanual_pixel_observation_smoke(self) -> None: + """Bimanual pixel wrappers should emit non-empty batched frames.""" + config, pool_type, spec_type = _bimanual_pixel_config(_BIMANUAL_IDS[0]) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape[0], 1) + self.assertGreater(obs.size, 0) + env.close() + + def test_runtrack_pixel_observation_smoke(self) -> None: + """RunTrack pixel wrappers should emit non-empty batched frames.""" + config, pool_type, spec_type = _runtrack_pixel_config(_RUNTRACK_IDS[0]) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape[0], 1) + self.assertGreater(obs.size, 0) + env.close() + + def test_soccer_pixel_observation_smoke(self) -> None: + """Soccer pixel wrappers should emit non-empty batched frames.""" + config, pool_type, spec_type = _soccer_pixel_config(_SOCCER_IDS[0]) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape[0], 1) + self.assertGreater(obs.size, 0) + env.close() + + def test_chasetag_pixel_observation_smoke(self) -> None: + """ChaseTag pixel wrappers should emit non-empty batched frames.""" + config, pool_type, spec_type = _chasetag_pixel_config(_CHASETAG_IDS[0]) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape[0], 1) + self.assertGreater(obs.size, 0) + env.close() + + def test_tabletennis_pixel_observation_smoke(self) -> None: + """TableTennis pixel wrappers should emit non-empty batched frames.""" + config, pool_type, spec_type = _tabletennis_pixel_config( + _TABLETENNIS_IDS[0] + ) + env = _make_env(config, pool_type, spec_type) + obs, _ = env.reset() + self.assertEqual(obs.shape[0], 1) + self.assertGreater(obs.size, 0) + env.close() + + +if __name__ == "__main__": + absltest.main() diff --git a/envpool/mujoco/myosuite/myodm.h b/envpool/mujoco/myosuite/myodm.h new file mode 100644 index 000000000..cff945821 --- /dev/null +++ b/envpool/mujoco/myosuite/myodm.h @@ -0,0 +1,1215 @@ +// Copyright 2026 Garena Online Private 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 ENVPOOL_MUJOCO_MYOSUITE_MYODM_H_ +#define ENVPOOL_MUJOCO_MYOSUITE_MYODM_H_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifdef _WIN32 +#include +#else +#include +#endif + +#include "envpool/mujoco/myosuite/myobase.h" + +namespace myosuite_envpool { + +namespace detail { + +inline void ReplaceAll(std::string* text, std::string_view from, + std::string_view to) { + std::size_t pos = 0; + while ((pos = text->find(from, pos)) != std::string::npos) { + text->replace(pos, from.size(), to); + pos += to.size(); + } +} + +inline int ProcessId() { +#ifdef _WIN32 + return _getpid(); +#else + return getpid(); +#endif +} + +inline std::string TemporaryDirectory() { + for (const char* env_key : {"TMPDIR", "TEMP", "TMP"}) { + const char* env_value = std::getenv(env_key); + if (env_value != nullptr && env_value[0] != '\0') { + return env_value; + } + } +#ifdef _WIN32 + return "C:\\Windows\\Temp"; +#else + return "/tmp"; +#endif +} + +inline std::string MakeTempPath(const std::string& stem, + std::string_view suffix) { + static std::atomic counter{0}; + std::string path = TemporaryDirectory(); + if (!path.empty() && path.back() != '/' && path.back() != '\\') { +#ifdef _WIN32 + path.push_back('\\'); +#else + path.push_back('/'); +#endif + } + path += stem + "_" + std::to_string(ProcessId()) + "_" + + std::to_string(counter.fetch_add(1)) + std::string(suffix); + return path; +} + +inline std::string ReadTextFile(const std::string& path) { + std::ifstream input(path); + if (!input) { + throw std::runtime_error("Failed to open file: " + path); + } + return {std::istreambuf_iterator(input), + std::istreambuf_iterator()}; +} + +inline void WriteTextFile(const std::string& path, const std::string& text) { + std::ofstream output(path); + if (!output) { + throw std::runtime_error("Failed to create file: " + path); + } + output << text; +} + +inline std::string MyoSuiteAbsolutePath(const std::string& base_path, + std::string_view relative_path) { + return myosuite::MyoSuiteModelPath(base_path, relative_path); +} + +inline std::string BuildTrackModelPath(const std::string& base_path, + std::string_view relative_model_path, + std::string_view object_name) { + std::string asset_root = MyoSuiteAbsolutePath(base_path, ""); + if (!asset_root.empty() && asset_root.back() == '/') { + asset_root.pop_back(); + } + std::string source_model = + asset_root + "/" + std::string(relative_model_path); + std::string object_xml = ReadTextFile(source_model); + std::string tabletop_xml = + ReadTextFile(asset_root + "/envs/myo/assets/hand/myohand_tabletop.xml"); + std::string hand_assets_xml = ReadTextFile( + asset_root + "/simhive/myo_sim/hand/assets/myohand_assets.xml"); + + std::string stem = "envpool_myodm_track"; + std::string prefix = MakeTempPath(stem, ""); + std::string hand_assets_tmp = prefix + "_assets.xml"; + std::string tabletop_tmp = prefix + "_tabletop.xml"; + std::string object_tmp = prefix + "_object.xml"; + + std::string myo_sim_root = asset_root + "/simhive/myo_sim"; + ReplaceAll( + &hand_assets_xml, R"(meshdir=".." texturedir="..")", + "meshdir=\"" + myo_sim_root + "\" texturedir=\"" + myo_sim_root + "\""); + WriteTextFile(hand_assets_tmp, hand_assets_xml); + + ReplaceAll(&tabletop_xml, + "../../../../simhive/myo_sim/hand/assets/myohand_assets.xml", + hand_assets_tmp); + ReplaceAll( + &tabletop_xml, + "../../../../simhive/furniture_sim/simpleTable/simpleTable_asset.xml", + asset_root + "/simhive/furniture_sim/simpleTable/simpleTable_asset.xml"); + ReplaceAll(&tabletop_xml, + "../../../../simhive/myo_sim/hand/assets/myohand_body.xml", + asset_root + "/simhive/myo_sim/hand/assets/myohand_body.xml"); + ReplaceAll( + &tabletop_xml, + "../../../../simhive/furniture_sim/simpleTable/" + "simpleGraniteTable_body.xml", + asset_root + + "/simhive/furniture_sim/simpleTable/simpleGraniteTable_body.xml"); + ReplaceAll( + &tabletop_xml, + "meshdir=\"../../../../simhive/myo_sim/\" " + "texturedir=\"../../../../simhive/myo_sim/\"", + "meshdir=\"" + myo_sim_root + "\" texturedir=\"" + myo_sim_root + "\""); + WriteTextFile(tabletop_tmp, tabletop_xml); + + ReplaceAll(&object_xml, "OBJECT_NAME", object_name); + ReplaceAll(&object_xml, "myohand_tabletop.xml", tabletop_tmp); + ReplaceAll(&object_xml, "../../../../simhive/object_sim/common.xml", + asset_root + "/simhive/object_sim/common.xml"); + ReplaceAll(&object_xml, + "../../../../simhive/object_sim/" + std::string(object_name) + + "/assets.xml", + asset_root + "/simhive/object_sim/" + std::string(object_name) + + "/assets.xml"); + ReplaceAll(&object_xml, + "../../../../simhive/object_sim/" + std::string(object_name) + + "/body.xml", + asset_root + "/simhive/object_sim/" + std::string(object_name) + + "/body.xml"); + WriteTextFile(object_tmp, object_xml); + return object_tmp; +} + +inline void RemoveTrackTemporaryFiles(const std::string& model_path) { + std::remove(model_path.c_str()); + std::size_t object_pos = model_path.rfind("_object.xml"); + if (object_pos == std::string::npos) { + return; + } + std::string prefix = model_path.substr(0, object_pos); + std::remove((prefix + "_tabletop.xml").c_str()); + std::remove((prefix + "_assets.xml").c_str()); +} + +inline std::uint16_t ReadLe16(std::istream* input) { + std::array data{}; + input->read(reinterpret_cast(data.data()), data.size()); + return static_cast(data[0]) | + (static_cast(data[1]) << 8); +} + +inline std::uint32_t ReadLe32(std::istream* input) { + std::array data{}; + input->read(reinterpret_cast(data.data()), data.size()); + return static_cast(data[0]) | + (static_cast(data[1]) << 8) | + (static_cast(data[2]) << 16) | + (static_cast(data[3]) << 24); +} + +struct NpyArray { + std::vector values; + std::vector shape; +}; + +inline NpyArray ParseNpyPayload(const std::vector& payload) { + if (payload.size() < 10 || std::memcmp(payload.data(), "\x93NUMPY", 6) != 0) { + throw std::runtime_error("Unsupported NPY payload."); + } + auto major = static_cast(payload[6]); + auto minor = static_cast(payload[7]); + (void)minor; + std::size_t offset = 8; + std::size_t header_len = 0; + if (major == 1) { + header_len = static_cast(payload[offset]) | + (static_cast( + static_cast(payload[offset + 1])) + << 8); + offset += 2; + } else if (major == 2) { + header_len = static_cast(payload[offset]) | + (static_cast( + static_cast(payload[offset + 1])) + << 8) | + (static_cast( + static_cast(payload[offset + 2])) + << 16) | + (static_cast( + static_cast(payload[offset + 3])) + << 24); + offset += 4; + } else { + throw std::runtime_error("Unsupported NPY version."); + } + if (offset + header_len > payload.size()) { + throw std::runtime_error("Malformed NPY header."); + } + std::string header(payload.data() + offset, header_len); + if (header.find("'descr': ' shape; + std::string shape_text = header.substr(open + 1, close - open - 1); + std::size_t start = 0; + while (start < shape_text.size()) { + std::size_t end = shape_text.find(',', start); + if (end == std::string::npos) { + end = shape_text.size(); + } + std::string token = shape_text.substr(start, end - start); + token.erase( + std::remove_if(token.begin(), token.end(), + [](unsigned char ch) { return std::isspace(ch) != 0; }), + token.end()); + if (!token.empty()) { + shape.push_back(std::stoi(token)); + } + start = end + 1; + } + if (shape.empty()) { + throw std::runtime_error("Empty NPY shape."); + } + std::size_t count = 1; + for (int dim : shape) { + count *= static_cast(dim); + } + offset += header_len; + std::size_t data_bytes = count * sizeof(double); + if (offset + data_bytes > payload.size()) { + throw std::runtime_error("Malformed NPY payload size."); + } + NpyArray array; + array.shape = std::move(shape); + array.values.resize(count); + std::memcpy(array.values.data(), payload.data() + offset, data_bytes); + return array; +} + +inline std::vector ReadZipStoredEntry(std::istream* input, + std::uint32_t size) { + std::vector payload(size); + input->read(payload.data(), static_cast(payload.size())); + if (!*input) { + throw std::runtime_error("Failed to read NPZ payload."); + } + return payload; +} + +inline std::vector> LoadStoredNpzArrays( + const std::string& path) { + std::ifstream input(path, std::ios::binary); + if (!input) { + throw std::runtime_error("Failed to open NPZ file: " + path); + } + std::vector> arrays; + while (true) { + std::uint32_t signature = ReadLe32(&input); + if (!input) { + break; + } + if (signature == 0x06054b50 || signature == 0x02014b50) { + break; + } + if (signature != 0x04034b50) { + throw std::runtime_error("Unsupported NPZ zip record."); + } + ReadLe16(&input); // version + std::uint16_t flags = ReadLe16(&input); + std::uint16_t compression = ReadLe16(&input); + ReadLe16(&input); // mod time + ReadLe16(&input); // mod date + ReadLe32(&input); // crc32 + std::uint32_t compressed_size = ReadLe32(&input); + std::uint32_t uncompressed_size = ReadLe32(&input); + std::uint16_t name_len = ReadLe16(&input); + std::uint16_t extra_len = ReadLe16(&input); + if (flags != 0 || compression != 0 || + compressed_size != uncompressed_size) { + throw std::runtime_error("Only stored NPZ entries are supported."); + } + std::string name(name_len, '\0'); + input.read(name.data(), static_cast(name.size())); + input.seekg(extra_len, std::ios::cur); + auto payload = ReadZipStoredEntry(&input, compressed_size); + if (name.size() >= 4 && name.substr(name.size() - 4) == ".npy") { + name.erase(name.size() - 4); + } + arrays.emplace_back(std::move(name), ParseNpyPayload(payload)); + } + return arrays; +} + +inline std::array Mat9ToQuat(const mjtNum* mat) { + mjtNum trace = mat[0] + mat[4] + mat[8]; + std::array quat{}; + if (trace > 0.0) { + mjtNum s = std::sqrt(trace + 1.0) * 2.0; + quat[0] = 0.25 * s; + quat[1] = (mat[7] - mat[5]) / s; + quat[2] = (mat[2] - mat[6]) / s; + quat[3] = (mat[3] - mat[1]) / s; + } else if (mat[0] > mat[4] && mat[0] > mat[8]) { + mjtNum s = std::sqrt(1.0 + mat[0] - mat[4] - mat[8]) * 2.0; + quat[0] = (mat[7] - mat[5]) / s; + quat[1] = 0.25 * s; + quat[2] = (mat[1] + mat[3]) / s; + quat[3] = (mat[2] + mat[6]) / s; + } else if (mat[4] > mat[8]) { + mjtNum s = std::sqrt(1.0 + mat[4] - mat[0] - mat[8]) * 2.0; + quat[0] = (mat[2] - mat[6]) / s; + quat[1] = (mat[1] + mat[3]) / s; + quat[2] = 0.25 * s; + quat[3] = (mat[5] + mat[7]) / s; + } else { + mjtNum s = std::sqrt(1.0 + mat[8] - mat[0] - mat[4]) * 2.0; + quat[0] = (mat[3] - mat[1]) / s; + quat[1] = (mat[2] + mat[6]) / s; + quat[2] = (mat[5] + mat[7]) / s; + quat[3] = 0.25 * s; + } + if (quat[0] < 0.0) { + for (mjtNum& value : quat) { + value = -value; + } + } + return quat; +} + +inline std::array QuatToMat(const std::array& quat) { + mjtNum w = quat[0]; + mjtNum x = quat[1]; + mjtNum y = quat[2]; + mjtNum z = quat[3]; + mjtNum nq = w * w + x * x + y * y + z * z; + mjtNum s = nq > 0.0 ? 2.0 / nq : 0.0; + mjtNum x_scaled = x * s; + mjtNum y_scaled = y * s; + mjtNum z_scaled = z * s; + mjtNum w_x = w * x_scaled; + mjtNum w_y = w * y_scaled; + mjtNum w_z = w * z_scaled; + mjtNum x_x = x * x_scaled; + mjtNum x_y = x * y_scaled; + mjtNum x_z = x * z_scaled; + mjtNum y_y = y * y_scaled; + mjtNum y_z = y * z_scaled; + mjtNum z_z = z * z_scaled; + return { + 1.0 - (y_y + z_z), x_y - w_z, x_z + w_y, + x_y + w_z, 1.0 - (x_x + z_z), y_z - w_x, + x_z - w_y, y_z + w_x, 1.0 - (x_x + y_y), + }; +} + +inline std::array Mat9ToEuler(const std::array& mat) { + constexpr mjtNum eps4 = + std::numeric_limits::epsilon() * static_cast(4.0); + mjtNum cy = std::sqrt(mat[8] * mat[8] + mat[5] * mat[5]); + bool condition = cy > eps4; + mjtNum rz = + condition ? -std::atan2(mat[1], mat[0]) : -std::atan2(-mat[3], mat[4]); + mjtNum ry = -std::atan2(-mat[2], cy); + mjtNum rx = condition ? -std::atan2(mat[5], mat[8]) : 0.0; + return {rx, ry, rz}; +} + +inline std::array QuatToEuler(const std::array& quat) { + return Mat9ToEuler(QuatToMat(quat)); +} + +inline std::array QuatConjugate(const std::array& quat) { + return {quat[0], -quat[1], -quat[2], -quat[3]}; +} + +inline std::array QuatMul(const std::array& lhs, + const std::array& rhs) { + return { + lhs[0] * rhs[0] - lhs[1] * rhs[1] - lhs[2] * rhs[2] - lhs[3] * rhs[3], + lhs[0] * rhs[1] + lhs[1] * rhs[0] + lhs[2] * rhs[3] - lhs[3] * rhs[2], + lhs[0] * rhs[2] - lhs[1] * rhs[3] + lhs[2] * rhs[0] + lhs[3] * rhs[1], + lhs[0] * rhs[3] + lhs[1] * rhs[2] - lhs[2] * rhs[1] + lhs[3] * rhs[0], + }; +} + +inline mjtNum QuatAngle(const std::array& quat) { + mjtNum sin_half = + std::sqrt(quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3]); + return static_cast(2.0) * std::atan2(sin_half, quat[0]); +} + +inline mjtNum QuatDistance(const std::array& lhs, + const std::array& rhs) { + return std::abs(QuatAngle(QuatMul(rhs, QuatConjugate(lhs)))); +} + +inline mjtNum RoundReferenceTime(mjtNum time) { + return std::round(time * static_cast(10000.0)) / + static_cast(10000.0); +} + +} // namespace detail + +class MyoDMTrackEnvFns { + public: + static decltype(auto) DefaultConfig() { + return MakeDict( + "reward_threshold"_.Bind(0.0), "frame_skip"_.Bind(10), + "frame_stack"_.Bind(1), "model_path"_.Bind(std::string()), + "object_name"_.Bind(std::string()), + "reference_path"_.Bind(std::string()), + "reference_time"_.Bind(std::vector{}), + "reference_robot"_.Bind(std::vector{}), + "reference_robot_vel"_.Bind(std::vector{}), + "reference_object"_.Bind(std::vector{}), + "reference_robot_init"_.Bind(std::vector{}), + "reference_object_init"_.Bind(std::vector{}), + "normalize_act"_.Bind(true), "muscle_condition"_.Bind(std::string()), + "fatigue_reset_vec"_.Bind(std::vector{}), + "fatigue_reset_random"_.Bind(false), "obs_dim"_.Bind(0), + "qpos_dim"_.Bind(0), "qvel_dim"_.Bind(0), "act_dim"_.Bind(0), + "action_dim"_.Bind(0), "robot_dim"_.Bind(0), "object_dim"_.Bind(7), + "robot_horizon"_.Bind(0), "object_horizon"_.Bind(0), + "reference_has_robot_vel"_.Bind(false), "motion_start_time"_.Bind(0.0), + "motion_extrapolation"_.Bind(true), "reward_pose_w"_.Bind(0.0), + "reward_object_w"_.Bind(1.0), "reward_bonus_w"_.Bind(1.0), + "reward_penalty_w"_.Bind(-2.0), "terminate_obj_fail"_.Bind(true), + "terminate_pose_fail"_.Bind(false), + "test_playback_reference"_.Bind(false), + "test_reset_qpos"_.Bind(std::vector{}), + "test_reset_qvel"_.Bind(std::vector{}), + "test_reset_ctrl"_.Bind(std::vector{}), + "test_reset_act"_.Bind(std::vector{}), + "test_reset_act_dot"_.Bind(std::vector{}), + "test_reset_integration_state"_.Bind(std::vector{}), + "test_reset_qacc_warmstart"_.Bind(std::vector{}), + "test_reference_time"_.Bind(std::vector{}), + "test_reference_robot"_.Bind(std::vector{}), + "test_reference_robot_vel"_.Bind(std::vector{}), + "test_reference_object"_.Bind(std::vector{})); + } + + template + static decltype(auto) StateSpec(const Config& conf) { + mjtNum inf = std::numeric_limits::infinity(); + return MakeDict( + "obs"_.Bind(StackSpec(Spec({conf["obs_dim"_]}, {-inf, inf}), + conf["frame_stack"_])), + "info:pose"_.Bind(Spec({-1}, {-inf, inf})), + "info:object"_.Bind(Spec({-1}, {-inf, inf})), + "info:bonus"_.Bind(Spec({-1}, {-inf, inf})), + "info:penalty"_.Bind(Spec({-1}, {-inf, inf})), + "info:success"_.Bind(Spec({-1}, {0.0, 1.0})), +#ifdef ENVPOOL_TEST + "info:qpos0"_.Bind(Spec({conf["qpos_dim"_]})), + "info:qvel0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc0"_.Bind(Spec({conf["qvel_dim"_]})), + "info:qacc_warmstart0"_.Bind(Spec({conf["qvel_dim"_]})), +#endif + "info:target_pos"_.Bind(Spec({3}))); + } + + template + static decltype(auto) ActionSpec(const Config& conf) { + return MakeDict( + "action"_.Bind(Spec({-1, conf["action_dim"_]}, {-1.0, 1.0}))); + } +}; + +using MyoDMTrackEnvSpec = EnvSpec; +using MyoDMTrackPixelEnvFns = PixelObservationEnvFns; +using MyoDMTrackPixelEnvSpec = EnvSpec; + +template +class MyoDMTrackEnvBase : public Env, + public gymnasium_robotics::MujocoRobotEnv { + protected: + using Base = Env; + using Base::Allocate; + using Base::spec_; + + struct ReferenceState { + std::vector robot; + std::vector robot_vel; + std::vector object; + }; + + struct RewardInfo { + mjtNum dense_reward{0.0}; + mjtNum pose{0.0}; + mjtNum object{0.0}; + mjtNum bonus{0.0}; + mjtNum penalty{0.0}; + bool success{false}; + bool done{false}; + }; + + enum class ReferenceType : std::uint8_t { kFixed, kRandom, kTrack }; + + bool normalize_act_; + bool motion_extrapolation_; + bool reference_has_robot_vel_; + bool terminate_obj_fail_; + bool terminate_pose_fail_; + mjtNum motion_start_time_; + mjtNum reward_pose_w_; + mjtNum reward_object_w_; + mjtNum reward_bonus_w_; + mjtNum reward_penalty_w_; + mjtNum obj_err_scale_{50.0}; + mjtNum base_err_scale_{40.0}; + mjtNum lift_bonus_thresh_{0.02}; + mjtNum lift_bonus_mag_{1.0}; + mjtNum qpos_reward_weight_{0.35}; + mjtNum qpos_err_scale_{5.0}; + mjtNum qvel_reward_weight_{0.05}; + mjtNum qvel_err_scale_{0.1}; + mjtNum obj_fail_thresh_{0.25}; + mjtNum base_fail_thresh_{0.25}; + mjtNum qpos_fail_thresh_{0.75}; + std::string object_name_; + int target_sid_{-1}; + int object_bid_{-1}; + int wrist_bid_{-1}; + int body_geom_id_{-1}; + mjtNum lift_z_{0.0}; + std::vector muscle_actuator_; + detail::MyoConditionState muscle_condition_state_; + std::vector reference_time_; + std::vector reference_robot_; + std::vector reference_robot_vel_; + std::vector reference_object_; + std::vector reference_robot_init_; + std::vector reference_object_init_; + int robot_dim_{0}; + int object_dim_{0}; + int robot_horizon_{0}; + int object_horizon_{0}; + int horizon_{0}; + int reference_index_cache_{0}; + int playback_reference_index_{0}; + ReferenceType reference_type_{ReferenceType::kFixed}; + detail::NumpyPcg64 reference_rng_{0}; + ReferenceState current_reference_; + std::vector test_reset_qpos_; + std::vector test_reset_qvel_; + std::vector test_reset_ctrl_; + std::vector test_reset_act_; + std::vector test_reset_act_dot_; + std::vector test_reset_integration_state_; + std::vector test_reset_qacc_warmstart_; + std::vector test_reference_time_; + std::vector test_reference_robot_; + std::vector test_reference_robot_vel_; + std::vector test_reference_object_; + bool test_playback_reference_; + + public: + using Spec = EnvSpecT; + using Action = typename Base::Action; + + MyoDMTrackEnvBase(const Spec& spec, int env_id) + : Env(spec, env_id), + gymnasium_robotics::MujocoRobotEnv( + spec.config["base_path"_], + detail::BuildTrackModelPath(spec.config["base_path"_], + spec.config["model_path"_], + spec.config["object_name"_]), + spec.config["frame_skip"_], spec.config["max_episode_steps"_], + spec.config["frame_stack"_], + RenderWidthOrDefault(spec.config), + RenderHeightOrDefault(spec.config), + RenderCameraIdOrDefault(spec.config)), + normalize_act_(spec.config["normalize_act"_]), + motion_extrapolation_(spec.config["motion_extrapolation"_]), + reference_has_robot_vel_(spec.config["reference_has_robot_vel"_]), + terminate_obj_fail_(spec.config["terminate_obj_fail"_]), + terminate_pose_fail_(spec.config["terminate_pose_fail"_]), + motion_start_time_(spec.config["motion_start_time"_]), + reward_pose_w_(spec.config["reward_pose_w"_]), + reward_object_w_(spec.config["reward_object_w"_]), + reward_bonus_w_(spec.config["reward_bonus_w"_]), + reward_penalty_w_(spec.config["reward_penalty_w"_]), + object_name_(spec.config["object_name"_]), + robot_dim_(spec.config["robot_dim"_]), + object_dim_(spec.config["object_dim"_]), + robot_horizon_(spec.config["robot_horizon"_]), + object_horizon_(spec.config["object_horizon"_]), + reference_rng_(static_cast(this->seed_)), + test_reset_qpos_(detail::ToMjtVector(spec.config["test_reset_qpos"_])), + test_reset_qvel_(detail::ToMjtVector(spec.config["test_reset_qvel"_])), + test_reset_ctrl_(detail::ToMjtVector(spec.config["test_reset_ctrl"_])), + test_reset_act_(detail::ToMjtVector(spec.config["test_reset_act"_])), + test_reset_act_dot_( + detail::ToMjtVector(spec.config["test_reset_act_dot"_])), + test_reset_integration_state_( + detail::ToMjtVector(spec.config["test_reset_integration_state"_])), + test_reset_qacc_warmstart_( + detail::ToMjtVector(spec.config["test_reset_qacc_warmstart"_])), + test_reference_time_( + detail::ToMjtVector(spec.config["test_reference_time"_])), + test_reference_robot_( + detail::ToMjtVector(spec.config["test_reference_robot"_])), + test_reference_robot_vel_( + detail::ToMjtVector(spec.config["test_reference_robot_vel"_])), + test_reference_object_( + detail::ToMjtVector(spec.config["test_reference_object"_])), + test_playback_reference_(spec.config["test_playback_reference"_]) { + LoadReference(spec); + ValidateConfig(); + mj_forward(model_, data_); + CacheObjects(); + detail::BuildMuscleMask(model_, &muscle_actuator_); + detail::InitializeMyoConditionState( + model_, spec.config["muscle_condition"_], + spec.config["fatigue_reset_vec"_], spec.config["fatigue_reset_random"_], + spec.config["frame_skip"_], this->seed_, &muscle_condition_state_); + detail::AdjustInitialQposForNormalizedActions(model_, data_, + normalize_act_); + InitializeReferencePose(); + InitializeRobotEnv(); + PrimeReferenceRngForOracleResetBehavior(); + detail::RemoveTrackTemporaryFiles(model_path_); + } + + ~MyoDMTrackEnvBase() override = default; + + envpool::mujoco::CameraPolicy RenderCameraPolicy() const override { + return detail::MyoSuiteRenderCameraPolicy(); + } + + void ConfigureRenderOption(mjvOption* option) const override { + detail::ConfigureMyoSuiteRenderOptions(option); + } + + bool IsDone() override { return done_; } + + void Reset() override { + done_ = false; + elapsed_step_ = 0; + reference_index_cache_ = 0; + playback_reference_index_ = 0; + detail::ResetMyoConditionState(&muscle_condition_state_); + ResetToInitialState(); + ApplyResetState(); + ReferenceState reference = + test_playback_reference_ + ? PlaybackReferenceAtCurrentIndex() + : ReferenceAt(data_->time + motion_start_time_); + UpdateTargetSite(reference); + CaptureResetState(); + RewardInfo reward = ComputeReward(reference); + WriteState(reward, reference, true, 0.0); + } + + void Step(const Action& action) override { + if (test_playback_reference_) { + InvalidateRenderCache(); + if (playback_reference_index_ + 1 < horizon_) { + ++playback_reference_index_; + } + ReferenceState reference = PlaybackReferenceAtCurrentIndex(); + SetQposFromReference(reference); + mj_forward(model_, data_); + data_->time += Dt(); + ++elapsed_step_; + RewardInfo reward = ComputeReward(reference); + done_ = false; + WriteState(reward, reference, false, reward.dense_reward); + return; + } + const auto* raw = static_cast(action["action"_].Data()); + detail::ApplyMyoSuiteAction(model_, data_, muscle_actuator_, normalize_act_, + raw); + detail::ApplyMyoConditionAdjustments(model_, data_, muscle_actuator_, + &muscle_condition_state_); + InvalidateRenderCache(); + detail::DoMyoSuiteSimulation(model_, data_, frame_skip_); + detail::RefreshObservedMyoSuiteState(model_, data_); + ++elapsed_step_; + ReferenceState reference = ReferenceAt(data_->time + motion_start_time_); + UpdateTargetSite(reference); + RewardInfo reward = ComputeReward(reference); + done_ = reward.done || elapsed_step_ >= max_episode_steps_; + WriteState(reward, reference, false, reward.dense_reward); + } + + private: + void LoadReference(const Spec& spec) { + std::string reference_path = spec.config["reference_path"_]; + if (!reference_path.empty()) { + auto arrays = detail::LoadStoredNpzArrays( + myosuite::MyoSuiteAssetRoot(spec.config["base_path"_]) + "/" + + reference_path); + for (auto& [name, array] : arrays) { + if (name == "time") { + reference_time_.assign(array.values.begin(), array.values.end()); + } else if (name == "robot") { + robot_horizon_ = array.shape[0]; + robot_dim_ = array.shape[1]; + reference_robot_.assign(array.values.begin(), array.values.end()); + } else if (name == "robot_vel") { + reference_has_robot_vel_ = true; + reference_robot_vel_.assign(array.values.begin(), array.values.end()); + } else if (name == "object") { + object_horizon_ = array.shape[0]; + object_dim_ = array.shape[1]; + reference_object_.assign(array.values.begin(), array.values.end()); + } else if (name == "robot_init") { + reference_robot_init_.assign(array.values.begin(), + array.values.end()); + } else if (name == "object_init") { + reference_object_init_.assign(array.values.begin(), + array.values.end()); + } + } + } else { + reference_time_ = detail::ToMjtVector(spec.config["reference_time"_]); + reference_robot_ = detail::ToMjtVector(spec.config["reference_robot"_]); + reference_robot_vel_ = + detail::ToMjtVector(spec.config["reference_robot_vel"_]); + reference_object_ = detail::ToMjtVector(spec.config["reference_object"_]); + reference_robot_init_ = + detail::ToMjtVector(spec.config["reference_robot_init"_]); + reference_object_init_ = + detail::ToMjtVector(spec.config["reference_object_init"_]); + } + if (!test_reference_time_.empty()) { + int horizon = static_cast(test_reference_time_.size()); + if (robot_dim_ <= 0 || object_dim_ <= 0) { + throw std::runtime_error( + "TrackEnv test reference dims are not initialized."); + } + if (static_cast(test_reference_robot_.size()) != + horizon * robot_dim_ || + static_cast(test_reference_object_.size()) != + horizon * object_dim_) { + throw std::runtime_error( + "TrackEnv test reference arrays have wrong size."); + } + reference_time_ = test_reference_time_; + reference_robot_ = test_reference_robot_; + reference_object_ = test_reference_object_; + reference_robot_init_.assign(reference_robot_.begin(), + reference_robot_.begin() + robot_dim_); + reference_object_init_.assign(reference_object_.begin(), + reference_object_.begin() + object_dim_); + robot_horizon_ = horizon; + object_horizon_ = horizon; + if (reference_has_robot_vel_) { + if (static_cast(test_reference_robot_vel_.size()) != + horizon * robot_dim_) { + throw std::runtime_error( + "TrackEnv test reference robot_vel has wrong size."); + } + reference_robot_vel_ = test_reference_robot_vel_; + } else { + reference_robot_vel_.clear(); + } + } + if (reference_has_robot_vel_ && reference_robot_vel_.empty()) { + reference_has_robot_vel_ = false; + } + horizon_ = std::max(robot_horizon_, object_horizon_); + if (robot_horizon_ > 2 || object_horizon_ > 2) { + reference_type_ = ReferenceType::kTrack; + } else if (robot_horizon_ == 2 || object_horizon_ == 2) { + reference_type_ = ReferenceType::kRandom; + } else { + reference_type_ = ReferenceType::kFixed; + } + if (reference_robot_init_.empty() && !reference_robot_.empty()) { + reference_robot_init_.assign(reference_robot_.begin(), + reference_robot_.begin() + robot_dim_); + } + if (reference_object_init_.empty() && !reference_object_.empty()) { + reference_object_init_.assign(reference_object_.begin(), + reference_object_.begin() + object_dim_); + } + } + + void ValidateConfig() { + if (model_->nq != spec_.config["qpos_dim"_] || + model_->nv != spec_.config["qvel_dim"_] || + model_->nu != spec_.config["action_dim"_] || + model_->na != spec_.config["act_dim"_]) { + throw std::runtime_error("TrackEnv config dims do not match model."); + } + if (robot_dim_ <= 0 || object_dim_ != 7 || reference_time_.empty()) { + throw std::runtime_error("TrackEnv reference metadata is incomplete."); + } + if (static_cast(reference_robot_.size()) != + robot_horizon_ * robot_dim_ || + static_cast(reference_object_.size()) != + object_horizon_ * object_dim_) { + throw std::runtime_error("TrackEnv reference arrays have wrong size."); + } + if (reference_has_robot_vel_ && + static_cast(reference_robot_vel_.size()) != + robot_horizon_ * robot_dim_) { + throw std::runtime_error("TrackEnv robot_vel has wrong size."); + } + int expected_obs = model_->nq + model_->nv + robot_dim_ + + (reference_has_robot_vel_ ? robot_dim_ : 1) + 3 + + model_->na; + if (expected_obs != spec_.config["obs_dim"_]) { + throw std::runtime_error("TrackEnv obs_dim does not match reference."); + } + } + + void CacheObjects() { + target_sid_ = mj_name2id(model_, mjOBJ_SITE, "target"); + object_bid_ = mj_name2id(model_, mjOBJ_BODY, object_name_.c_str()); + wrist_bid_ = mj_name2id(model_, mjOBJ_BODY, "lunate"); + body_geom_id_ = mj_name2id(model_, mjOBJ_GEOM, "body"); + if (target_sid_ == -1 || object_bid_ == -1 || wrist_bid_ == -1) { + throw std::runtime_error("TrackEnv object ids missing."); + } + if (body_geom_id_ >= 0) { + model_->geom_rgba[body_geom_id_ * 4 + 3] = 0.0; + } + lift_z_ = data_->xipos[object_bid_ * 3 + 2] + lift_bonus_thresh_; + } + + void InitializeReferencePose() { + if (static_cast(reference_robot_init_.size()) != robot_dim_ || + static_cast(reference_object_init_.size()) != object_dim_) { + throw std::runtime_error("TrackEnv init reference has wrong size."); + } + ReferenceState reference; + reference.robot = reference_robot_init_; + reference.object = reference_object_init_; + SetQposFromReference(reference); + mju_zero(data_->qvel, model_->nv); + mj_forward(model_, data_); + } + + void ApplyResetState() { + if (!test_reset_integration_state_.empty()) { + mj_setState(model_, data_, test_reset_integration_state_.data(), + mjSTATE_INTEGRATION); + if (!test_reset_ctrl_.empty()) { + detail::RestoreVector(test_reset_ctrl_, data_->ctrl); + } + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, + data_->qacc_warmstart); + } + mj_forward(model_, data_); + if (!test_reset_act_dot_.empty()) { + detail::RestoreVector(test_reset_act_dot_, data_->act_dot); + } + mj_step1(model_, data_); + if (!test_reset_act_dot_.empty()) { + detail::RestoreVector(test_reset_act_dot_, data_->act_dot); + } + return; + } + bool has_test_reset_override = + !test_reset_qpos_.empty() || !test_reset_qvel_.empty() || + !test_reset_ctrl_.empty() || !test_reset_act_.empty() || + !test_reset_act_dot_.empty() || !test_reset_qacc_warmstart_.empty(); + if (!test_reset_qpos_.empty()) { + detail::RestoreVector(test_reset_qpos_, data_->qpos); + detail::RestoreVector(test_reset_qvel_, data_->qvel); + } + mj_forward(model_, data_); + if (!test_reset_ctrl_.empty()) { + detail::RestoreVector(test_reset_ctrl_, data_->ctrl); + } + bool rerun_forward = false; + if (!test_reset_act_.empty()) { + detail::RestoreVector(test_reset_act_, data_->act); + rerun_forward = true; + } + if (!test_reset_act_dot_.empty()) { + detail::RestoreVector(test_reset_act_dot_, data_->act_dot); + } + if (!test_reset_qacc_warmstart_.empty()) { + detail::RestoreVector(test_reset_qacc_warmstart_, data_->qacc_warmstart); + } + if (rerun_forward) { + mj_forward(model_, data_); + } + if (has_test_reset_override) { + // Official BaseV0 reset leaves dm_control Physics ready for the next + // legacy-step transition, including actuator-derivative fields such as + // act_dot. TrackEnv needs the same reset-sync treatment as Challenge + // tasks; otherwise the first mj_step2 can drift on some seeds/actions. + mj_step1(model_, data_); + } + } + + const mjtNum* RobotRow(int index) const { + return reference_robot_.data() + index * robot_dim_; + } + + void SetQposFromReference(const ReferenceState& reference) { + for (int i = 0; i < robot_dim_; ++i) { + data_->qpos[i] = reference.robot[i]; + } + for (int axis = 0; axis < 3; ++axis) { + data_->qpos[robot_dim_ + axis] = reference.object[axis]; + } + std::array quat{reference.object[3], reference.object[4], + reference.object[5], reference.object[6]}; + auto euler = detail::QuatToEuler(quat); + for (int axis = 0; axis < 3; ++axis) { + data_->qpos[robot_dim_ + 3 + axis] = euler[axis]; + } + } + + const mjtNum* RobotVelRow(int index) const { + return reference_has_robot_vel_ + ? reference_robot_vel_.data() + index * robot_dim_ + : nullptr; + } + + const mjtNum* ObjectRow(int index) const { + return reference_object_.data() + index * object_dim_; + } + + ReferenceState PlaybackReferenceAtCurrentIndex() { + current_reference_.robot.resize(robot_dim_); + current_reference_.object.resize(object_dim_); + if (reference_has_robot_vel_) { + current_reference_.robot_vel.resize(robot_dim_); + } else { + current_reference_.robot_vel.assign(1, 0.0); + } + int index = std::clamp(playback_reference_index_, 0, horizon_ - 1); + std::copy_n(RobotRow(std::min(index, robot_horizon_ - 1)), robot_dim_, + current_reference_.robot.data()); + std::copy_n(ObjectRow(std::min(index, object_horizon_ - 1)), object_dim_, + current_reference_.object.data()); + if (reference_has_robot_vel_) { + std::copy_n(RobotVelRow(std::min(index, robot_horizon_ - 1)), robot_dim_, + current_reference_.robot_vel.data()); + } + return current_reference_; + } + + void PrimeReferenceRngForOracleResetBehavior() { + if (reference_type_ != ReferenceType::kRandom) { + return; + } + // Upstream TrackEnv construction goes through env_base.MujocoEnv._setup(), + // which performs one zero-action step to build observation_space. For + // random references that eagerly consumes one ref.get_reference() sample + // before the user's first reset(). Mirror that here so the first native + // reset observes the same sampled target under the same seed. + (void)ReferenceAt(motion_start_time_); + } + + ReferenceState ReferenceAt(mjtNum raw_time) { + mjtNum time = detail::RoundReferenceTime(raw_time); + current_reference_.robot.resize(robot_dim_); + current_reference_.object.resize(object_dim_); + if (reference_has_robot_vel_) { + current_reference_.robot_vel.resize(robot_dim_); + } else { + current_reference_.robot_vel.assign(1, 0.0); + } + if (reference_type_ == ReferenceType::kFixed) { + std::copy_n(RobotRow(0), robot_dim_, current_reference_.robot.data()); + std::copy_n(ObjectRow(0), object_dim_, current_reference_.object.data()); + if (reference_has_robot_vel_) { + std::copy_n(RobotVelRow(0), robot_dim_, + current_reference_.robot_vel.data()); + } + return current_reference_; + } + if (reference_type_ == ReferenceType::kRandom) { + for (int i = 0; i < robot_dim_; ++i) { + current_reference_.robot[i] = + reference_rng_.UniformMjt(RobotRow(0)[i], RobotRow(1)[i]); + } + if (reference_has_robot_vel_) { + for (int i = 0; i < robot_dim_; ++i) { + current_reference_.robot_vel[i] = + reference_rng_.UniformMjt(RobotVelRow(0)[i], RobotVelRow(1)[i]); + } + } + for (int i = 0; i < object_dim_; ++i) { + current_reference_.object[i] = + reference_rng_.UniformMjt(ObjectRow(0)[i], ObjectRow(1)[i]); + } + return current_reference_; + } + int index = reference_index_cache_; + if (motion_extrapolation_ && time >= reference_time_.back()) { + index = horizon_ - 1; + } else { + if (time == reference_time_[reference_index_cache_]) { + index = reference_index_cache_; + } else if (reference_index_cache_ + 1 < horizon_ && + time == reference_time_[reference_index_cache_ + 1]) { + index = reference_index_cache_ + 1; + } else if (reference_index_cache_ + 1 < horizon_ && + time > reference_time_[reference_index_cache_] && + time < reference_time_[reference_index_cache_ + 1]) { + index = reference_index_cache_; + } else { + auto upper = std::upper_bound(reference_time_.begin(), + reference_time_.end(), time); + if (upper == reference_time_.begin()) { + index = 0; + } else { + index = static_cast(upper - reference_time_.begin() - 1); + } + if (index < 0) { + index = 0; + } else if (index >= horizon_) { + index = horizon_ - 1; + } + } + } + reference_index_cache_ = index; + std::copy_n(RobotRow(std::min(index, robot_horizon_ - 1)), robot_dim_, + current_reference_.robot.data()); + std::copy_n(ObjectRow(std::min(index, object_horizon_ - 1)), object_dim_, + current_reference_.object.data()); + if (reference_has_robot_vel_) { + std::copy_n(RobotVelRow(std::min(index, robot_horizon_ - 1)), robot_dim_, + current_reference_.robot_vel.data()); + } + return current_reference_; + } + + void UpdateTargetSite(const ReferenceState& reference) { + for (int axis = 0; axis < 3; ++axis) { + model_->site_pos[target_sid_ * 3 + axis] = reference.object[axis]; + } + mj_forward(model_, data_); + } + + RewardInfo ComputeReward(const ReferenceState& reference) const { + RewardInfo reward; + mjtNum qpos_sq = 0.0; + mjtNum qvel_sq = 0.0; + for (int i = 0; i < robot_dim_; ++i) { + mjtNum delta = data_->qpos[i] - reference.robot[i]; + qpos_sq += delta * delta; + if (reference_has_robot_vel_) { + mjtNum dv = data_->qvel[i] - reference.robot_vel[i]; + qvel_sq += dv * dv; + } + } + std::array obj_com{}; + std::array wrist{}; + for (int axis = 0; axis < 3; ++axis) { + obj_com[axis] = data_->xipos[object_bid_ * 3 + axis]; + wrist[axis] = data_->xipos[wrist_bid_ * 3 + axis]; + } + mjtNum obj_com_sq = 0.0; + mjtNum base_sq = 0.0; + for (int axis = 0; axis < 3; ++axis) { + mjtNum delta = obj_com[axis] - reference.object[axis]; + obj_com_sq += delta * delta; + mjtNum base_delta = obj_com[axis] - wrist[axis]; + base_sq += base_delta * base_delta; + } + auto obj_quat = detail::Mat9ToQuat(data_->ximat + object_bid_ * 9); + std::array target_quat{reference.object[3], reference.object[4], + reference.object[5], reference.object[6]}; + mjtNum obj_rot_err = detail::QuatDistance(obj_quat, target_quat) / + static_cast(3.14159265358979323846); + mjtNum obj_reward = + std::exp(-obj_err_scale_ * (std::sqrt(obj_com_sq) + 0.1 * obj_rot_err)); + bool lift_bonus = reference.object[2] >= lift_z_ && obj_com[2] >= lift_z_; + mjtNum pose_reward = + qpos_reward_weight_ * std::exp(-qpos_err_scale_ * qpos_sq); + mjtNum qvel_reward = + qvel_reward_weight_ * std::exp(-qvel_err_scale_ * qvel_sq); + mjtNum base_reward = std::exp(-base_err_scale_ * std::sqrt(base_sq)); + bool obj_term = terminate_obj_fail_ && + (obj_com_sq >= obj_fail_thresh_ * obj_fail_thresh_ || + base_sq >= base_fail_thresh_ * base_fail_thresh_); + bool pose_term = terminate_pose_fail_ && qpos_sq >= qpos_fail_thresh_; + reward.pose = pose_reward + qvel_reward; + reward.object = obj_reward + base_reward; + reward.bonus = lift_bonus ? lift_bonus_mag_ : 0.0; + reward.penalty = static_cast(obj_term || pose_term); + reward.done = obj_term || pose_term; + reward.dense_reward = + reward_pose_w_ * reward.pose + reward_object_w_ * reward.object + + reward_bonus_w_ * reward.bonus + reward_penalty_w_ * reward.penalty; + return reward; + } + + void WriteState(const RewardInfo& reward, const ReferenceState& reference, + bool reset, mjtNum reward_value) { + auto state = Allocate(); + state["reward"_] = reward_value; + if constexpr (kFromPixels) { + auto obs_pixels = state["obs:pixels"_]; + AssignPixelObservation("obs:pixels", &obs_pixels, reset); + } else { + auto obs = state["obs"_]; + mjtNum* buffer = PrepareObservation("obs", &obs); + for (int i = 0; i < model_->nq; ++i) { + *(buffer++) = data_->qpos[i]; + } + for (int i = 0; i < model_->nv; ++i) { + *(buffer++) = data_->qvel[i]; + } + for (int i = 0; i < robot_dim_; ++i) { + *(buffer++) = data_->qpos[i] - reference.robot[i]; + } + if (reference_has_robot_vel_) { + for (int i = 0; i < robot_dim_; ++i) { + *(buffer++) = data_->qvel[i] - reference.robot_vel[i]; + } + } else { + *(buffer++) = 0.0; + } + for (int axis = 0; axis < 3; ++axis) { + *(buffer++) = + data_->xipos[object_bid_ * 3 + axis] - reference.object[axis]; + } + for (int i = 0; i < model_->na; ++i) { + *(buffer++) = data_->act[i]; + } + CommitObservation("obs", &obs, reset); + } + state["info:pose"_] = reward.pose; + state["info:object"_] = reward.object; + state["info:bonus"_] = reward.bonus; + state["info:penalty"_] = reward.penalty; + state["info:success"_] = reward.success; +#ifdef ENVPOOL_TEST + state["info:qpos0"_].Assign(qpos0_.data(), qpos0_.size()); + state["info:qvel0"_].Assign(qvel0_.data(), qvel0_.size()); + state["info:qacc0"_].Assign(qacc0_.data(), qacc0_.size()); + state["info:qacc_warmstart0"_].Assign(qacc_warmstart0_.data(), + qacc_warmstart0_.size()); +#endif + state["info:target_pos"_].Assign(model_->site_pos + target_sid_ * 3, 3); + } +}; + +using MyoDMTrackEnv = MyoDMTrackEnvBase; +using MyoDMTrackPixelEnv = MyoDMTrackEnvBase; +using MyoDMTrackEnvPool = AsyncEnvPool; +using MyoDMTrackPixelEnvPool = AsyncEnvPool; + +} // namespace myosuite_envpool + +#endif // ENVPOOL_MUJOCO_MYOSUITE_MYODM_H_ diff --git a/envpool/mujoco/myosuite/myodm_test.py b/envpool/mujoco/myosuite/myodm_test.py new file mode 100644 index 000000000..d73011cc7 --- /dev/null +++ b/envpool/mujoco/myosuite/myodm_test.py @@ -0,0 +1,679 @@ +# Copyright 2026 Garena Online Private 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. +"""Internal MyoSuite MyoDM TrackEnv native env tests.""" + +from __future__ import annotations + +import tempfile +from contextlib import contextmanager +from dataclasses import dataclass +from functools import cache +from pathlib import Path +from typing import Any, Iterator + +import gymnasium +import mujoco +import numpy as np +from absl.testing import absltest + +from envpool.mujoco.myosuite.metadata import MYOSUITE_DIRECT_ENTRIES +from envpool.mujoco.myosuite.native import ( + MyoDMTrackEnvSpec, + MyoDMTrackGymnasiumEnvPool, + MyoDMTrackPixelEnvSpec, + MyoDMTrackPixelGymnasiumEnvPool, +) +from envpool.mujoco.myosuite.oracle_utils import ( + load_oracle_class, + prepared_track_oracle_model_path, +) +from envpool.mujoco.myosuite.paths import myosuite_asset_root +from envpool.python.glfw_context import preload_windows_gl_dlls + +preload_windows_gl_dlls(strict=True) + +_TRACK_IDS = tuple( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["class_name"] == "TrackEnv" +) +_TRACK_ALIGN_IDS = ( + "MyoHandAlarmclockFixed-v0", + "MyoHandAirplaneFixed-v0", + "MyoHandAirplaneRandom-v0", + "MyoHandAirplaneFly-v0", + "MyoHandPyramidmediumRandom-v0", +) +_TRACK_REPRESENTATIVE_IDS = ( + "MyoHandAirplaneFixed-v0", + "MyoHandAirplaneRandom-v0", + "MyoHandAirplaneFly-v0", +) +_ALIGNMENT_STEPS = 32 +_TRACK_ALIGNMENT_OBS_TOLERANCE = { + # The random airplane track reaches a mesh contact transition around step 8. + # The XML, reset integration state, ctrl, act, and a pure Python mujoco + # replay are bitwise with the official oracle; only EnvPool's Bazel-built + # MuJoCo binary diverges from the pip MuJoCo binary in contact qacc by + # ~2.4e-3, which accumulates to ~1.1e-5 in qvel. Keep this tolerance scoped + # to that contact-heavy oracle check instead of widening all MyoDM tracks. + "MyoHandAirplaneRandom-v0": 2e-5, +} + + +@dataclass(frozen=True) +class _TrackReferenceSample: + """One official TrackEnv reference sample consumed during a rollout.""" + + time: float + robot: np.ndarray + robot_vel: np.ndarray | None + object: np.ndarray + + +def _entry(env_id: str) -> dict[str, Any]: + return next( + entry for entry in MYOSUITE_DIRECT_ENTRIES if entry["id"] == env_id + ) + + +def _make_env(config: tuple[Any, ...], pool_type: type, spec_type: type) -> Any: + return pool_type(spec_type(config)) + + +def _seeded_actions( + shape: tuple[int, ...], steps: int, seed: int +) -> list[np.ndarray]: + rng = np.random.default_rng(seed) + return [ + rng.uniform(-0.9, 0.9, size=shape).astype(np.float32) + for _ in range(steps) + ] + + +def _track_obs_tolerance(env_id: str) -> float: + return _TRACK_ALIGNMENT_OBS_TOLERANCE.get(env_id, 1e-6) + + +def _assert_rollouts_match( + case: absltest.TestCase, + env0: Any, + env1: Any, + actions: list[np.ndarray], + *, + atol: float = 1e-9, + rtol: float = 1e-9, +) -> None: + obs0, info0 = env0.reset() + obs1, info1 = env1.reset() + np.testing.assert_allclose(obs0, obs1, atol=atol, rtol=rtol) + case.assertEqual( + info0["elapsed_step"].tolist(), info1["elapsed_step"].tolist() + ) + for action in actions: + obs0, reward0, terminated0, truncated0, info0 = env0.step(action) + obs1, reward1, terminated1, truncated1, info1 = env1.step(action) + np.testing.assert_allclose(obs0, obs1, atol=atol, rtol=rtol) + np.testing.assert_allclose(reward0, reward1, atol=atol, rtol=rtol) + np.testing.assert_array_equal(terminated0, terminated1) + np.testing.assert_array_equal(truncated0, truncated1) + case.assertEqual( + info0["elapsed_step"].tolist(), info1["elapsed_step"].tolist() + ) + if terminated0[0] or truncated0[0]: + break + + +def _replace_all(text: str, old: str, new: str) -> str: + return text.replace(old, new) + + +@contextmanager +def _edited_track_model(model_path: str, object_name: str) -> Iterator[Path]: + asset_root = myosuite_asset_root() + source_model = asset_root / model_path + object_xml = source_model.read_text() + tabletop_xml = ( + asset_root / "envs/myo/assets/hand/myohand_tabletop.xml" + ).read_text() + hand_assets_xml = ( + asset_root / "simhive/myo_sim/hand/assets/myohand_assets.xml" + ).read_text() + myo_sim_root = asset_root / "simhive/myo_sim" + myo_sim_root_str = str(myo_sim_root) + + with tempfile.TemporaryDirectory() as td: + tmp_dir = Path(td) + hand_assets_tmp = tmp_dir / "myohand_assets.xml" + tabletop_tmp = tmp_dir / "myohand_tabletop.xml" + object_tmp = tmp_dir / "myohand_object.xml" + + hand_assets_xml = _replace_all( + hand_assets_xml, + 'meshdir=".." texturedir=".."', + f'meshdir="{myo_sim_root_str}" texturedir="{myo_sim_root_str}"', + ) + hand_assets_tmp.write_text(hand_assets_xml) + + tabletop_xml = _replace_all( + tabletop_xml, + "../../../../simhive/myo_sim/hand/assets/myohand_assets.xml", + str(hand_assets_tmp), + ) + tabletop_xml = _replace_all( + tabletop_xml, + "../../../../simhive/furniture_sim/simpleTable/simpleTable_asset.xml", + str( + asset_root + / "simhive/furniture_sim/simpleTable/simpleTable_asset.xml" + ), + ) + tabletop_xml = _replace_all( + tabletop_xml, + "../../../../simhive/myo_sim/hand/assets/myohand_body.xml", + str(asset_root / "simhive/myo_sim/hand/assets/myohand_body.xml"), + ) + tabletop_xml = _replace_all( + tabletop_xml, + "../../../../simhive/furniture_sim/simpleTable/simpleGraniteTable_body.xml", + str( + asset_root + / "simhive/furniture_sim/simpleTable/simpleGraniteTable_body.xml" + ), + ) + tabletop_xml = _replace_all( + tabletop_xml, + 'meshdir="../../../../simhive/myo_sim/" texturedir="../../../../simhive/myo_sim/"', + f'meshdir="{myo_sim_root_str}" texturedir="{myo_sim_root_str}"', + ) + tabletop_tmp.write_text(tabletop_xml) + + object_xml = _replace_all(object_xml, "OBJECT_NAME", object_name) + object_xml = _replace_all( + object_xml, "myohand_tabletop.xml", str(tabletop_tmp) + ) + object_xml = _replace_all( + object_xml, + "../../../../simhive/object_sim/common.xml", + str(asset_root / "simhive/object_sim/common.xml"), + ) + object_xml = _replace_all( + object_xml, + f"../../../../simhive/object_sim/{object_name}/assets.xml", + str(asset_root / f"simhive/object_sim/{object_name}/assets.xml"), + ) + object_xml = _replace_all( + object_xml, + f"../../../../simhive/object_sim/{object_name}/body.xml", + str(asset_root / f"simhive/object_sim/{object_name}/body.xml"), + ) + object_tmp.write_text(object_xml) + yield object_tmp + + +@cache +def _track_model(model_path: str, object_name: str) -> mujoco.MjModel: + with _edited_track_model(model_path, object_name) as edited_model: + return mujoco.MjModel.from_xml_path(str(edited_model)) + + +def _dict_reference_init( + reference: dict[str, Any], key: str, array_key: str +) -> np.ndarray: + value = reference.get(key) + if value is not None: + return np.asarray(value, dtype=np.float64) + base = np.asarray(reference[array_key], dtype=np.float64) + if base.shape[0] == 2: + return np.mean(base, axis=0) + return base[0] + + +def _reference_config(reference: Any) -> dict[str, Any]: + if isinstance(reference, str): + reference_path = myosuite_asset_root() / reference + with np.load(reference_path) as data: + robot = np.asarray(data["robot"], dtype=np.float64) + obj = np.asarray(data["object"], dtype=np.float64) + robot_vel = ( + np.asarray(data["robot_vel"], dtype=np.float64) + if "robot_vel" in data.files + else None + ) + robot_init = ( + np.asarray(data["robot_init"], dtype=np.float64) + if "robot_init" in data.files + else robot[0] + ) + object_init = ( + np.asarray(data["object_init"], dtype=np.float64) + if "object_init" in data.files + else obj[0] + ) + return { + "reference_path": reference, + "reference_time": [], + "reference_robot": [], + "reference_robot_vel": [], + "reference_object": [], + "reference_robot_init": robot_init.tolist(), + "reference_object_init": object_init.tolist(), + "robot_dim": int(robot.shape[1]), + "object_dim": int(obj.shape[1]), + "robot_horizon": int(robot.shape[0]), + "object_horizon": int(obj.shape[0]), + "reference_has_robot_vel": robot_vel is not None, + } + + ref = dict(reference) + time = np.asarray(ref["time"], dtype=np.float64) + robot = np.asarray(ref["robot"], dtype=np.float64) + obj = np.asarray(ref["object"], dtype=np.float64) + robot_vel = ( + np.asarray(ref["robot_vel"], dtype=np.float64) + if ref.get("robot_vel") is not None + else None + ) + robot_init = _dict_reference_init(ref, "robot_init", "robot") + object_init = _dict_reference_init(ref, "object_init", "object") + return { + "reference_path": "", + "reference_time": time.tolist(), + "reference_robot": robot.reshape(-1).tolist(), + "reference_robot_vel": [] + if robot_vel is None + else robot_vel.reshape(-1).tolist(), + "reference_object": obj.reshape(-1).tolist(), + "reference_robot_init": robot_init.tolist(), + "reference_object_init": object_init.tolist(), + "robot_dim": int(robot.shape[1]), + "object_dim": int(obj.shape[1]), + "robot_horizon": int(robot.shape[0]), + "object_horizon": int(obj.shape[0]), + "reference_has_robot_vel": robot_vel is not None, + } + + +def _track_config( + env_id: str, + *, + overrides: dict[str, Any] | None = None, + seed: int | None = None, +) -> tuple[tuple[Any, ...], type, type]: + entry = _entry(env_id) + kwargs = dict(entry["kwargs"]) + model = _track_model(kwargs["model_path"], kwargs["object_name"]) + reference = _reference_config(kwargs["reference"]) + obs_dim = ( + model.nq + + model.nv + + reference["robot_dim"] + + ( + reference["robot_dim"] + if reference["reference_has_robot_vel"] + else 1 + ) + + 3 + + model.na + ) + config = MyoDMTrackEnvSpec.gen_config( + num_envs=1, + batch_size=1, + max_num_players=1, + **({} if seed is None else {"seed": seed}), + frame_skip=int(kwargs.get("frame_skip", 10)), + model_path=str(kwargs["model_path"]), + object_name=str(kwargs["object_name"]), + reference_path=str(reference["reference_path"]), + reference_time=list(reference["reference_time"]), + reference_robot=list(reference["reference_robot"]), + reference_robot_vel=list(reference["reference_robot_vel"]), + reference_object=list(reference["reference_object"]), + reference_robot_init=list(reference["reference_robot_init"]), + reference_object_init=list(reference["reference_object_init"]), + normalize_act=bool(kwargs.get("normalize_act", True)), + obs_dim=obs_dim, + qpos_dim=model.nq, + qvel_dim=model.nv, + act_dim=model.na, + action_dim=model.nu, + robot_dim=reference["robot_dim"], + object_dim=reference["object_dim"], + robot_horizon=reference["robot_horizon"], + object_horizon=reference["object_horizon"], + reference_has_robot_vel=bool(reference["reference_has_robot_vel"]), + motion_start_time=float(kwargs.get("motion_start_time", 0.0)), + motion_extrapolation=bool(kwargs.get("motion_extrapolation", True)), + reward_pose_w=float( + kwargs.get("weighted_reward_keys", {}).get("pose", 0.0) + ), + reward_object_w=float( + kwargs.get("weighted_reward_keys", {}).get("object", 1.0) + ), + reward_bonus_w=float( + kwargs.get("weighted_reward_keys", {}).get("bonus", 1.0) + ), + reward_penalty_w=float( + kwargs.get("weighted_reward_keys", {}).get("penalty", -2.0) + ), + terminate_obj_fail=bool( + kwargs.get( + "Termimate_obj_fail", kwargs.get("terminate_obj_fail", True) + ) + ), + terminate_pose_fail=bool( + kwargs.get( + "Termimate_pose_fail", kwargs.get("terminate_pose_fail", False) + ) + ), + ) + if overrides: + config = MyoDMTrackEnvSpec.gen_config( + **dict( + zip(MyoDMTrackEnvSpec._config_keys, config, strict=False), + **overrides, + ) + ) + return config, MyoDMTrackGymnasiumEnvPool, MyoDMTrackEnvSpec + + +def _track_pixel_config(env_id: str) -> tuple[tuple[Any, ...], type, type]: + config, _, _ = _track_config(env_id) + values = dict(zip(MyoDMTrackEnvSpec._config_keys, config, strict=False)) + pixel = MyoDMTrackPixelEnvSpec.gen_config( + **values, + render_width=64, + render_height=64, + render_camera_id=-1, + ) + return pixel, MyoDMTrackPixelGymnasiumEnvPool, MyoDMTrackPixelEnvSpec + + +def _oracle_reference(reference: Any) -> Any: + if isinstance(reference, str): + return str(myosuite_asset_root() / reference) + return { + key: np.asarray(value, dtype=np.float64) + for key, value in reference.items() + } + + +@contextmanager +def _oracle_track_kwargs(env_id: str) -> Iterator[dict[str, Any]]: + kwargs = dict(_entry(env_id)["kwargs"]) + with prepared_track_oracle_model_path() as model_path: + yield { + "object_name": kwargs["object_name"], + "model_path": model_path, + "reference": _oracle_reference(kwargs["reference"]), + } + + +def _oracle_reset_sync(env: Any) -> tuple[np.ndarray, dict[str, Any]]: + obs, _ = env.reset() + unwrapped = env.unwrapped + integration_state = unwrapped.sim.sim.get_state( + int(mujoco.mjtState.mjSTATE_INTEGRATION) + ) + return obs, { + "test_reset_qpos": unwrapped.sim.data.qpos.copy().tolist(), + "test_reset_qvel": unwrapped.sim.data.qvel.copy().tolist(), + "test_reset_ctrl": unwrapped.sim.data.ctrl.copy().tolist(), + "test_reset_act": ( + unwrapped.sim.data.act.copy().tolist() + if unwrapped.sim.model.na > 0 + else [] + ), + "test_reset_act_dot": ( + unwrapped.sim.data.act_dot.copy().tolist() + if unwrapped.sim.model.na > 0 + else [] + ), + "test_reset_qacc_warmstart": ( + unwrapped.sim.data.qacc_warmstart.copy().tolist() + ), + "test_reset_integration_state": integration_state.tolist(), + } + + +@contextmanager +def _record_track_reference_samples( + env: Any, +) -> Iterator[list[_TrackReferenceSample]]: + unwrapped = env.unwrapped + ref = getattr(unwrapped, "ref", None) + if ref is None or not hasattr(ref, "get_reference"): + yield [] + return + original_get_reference = ref.get_reference + samples: list[_TrackReferenceSample] = [] + + def wrapped_get_reference(time: Any) -> Any: + reference = original_get_reference(time) + samples.append( + _TrackReferenceSample( + time=float(time), + robot=np.asarray(reference.robot, dtype=np.float64).copy(), + robot_vel=( + None + if reference.robot_vel is None + else np.asarray( + reference.robot_vel, dtype=np.float64 + ).copy() + ), + object=np.asarray(reference.object, dtype=np.float64).copy(), + ) + ) + return reference + + ref.get_reference = wrapped_get_reference + try: + yield samples + finally: + ref.get_reference = original_get_reference + + +def _track_reference_sync( + samples: list[_TrackReferenceSample], +) -> dict[str, Any]: + if not samples: + return {} + has_robot_vel = samples[0].robot_vel is not None + return { + "test_reference_time": [round(sample.time, 4) for sample in samples], + "test_reference_robot": np.concatenate([ + sample.robot for sample in samples + ]).tolist(), + "test_reference_robot_vel": ( + [] + if not has_robot_vel + else np.concatenate([ + sample.robot_vel for sample in samples + ]).tolist() + ), + "test_reference_object": np.concatenate([ + sample.object for sample in samples + ]).tolist(), + } + + +class MyoDMTrackNativeTest(absltest.TestCase): + """Verifies the native MyoDM TrackEnv surface.""" + + def test_track_surface_count(self) -> None: + """TrackEnv metadata should expose the full generated MyoDM surface.""" + self.assertLen(_TRACK_IDS, 190) + + def test_track_construction_covers_all_registered_ids(self) -> None: + """Every TrackEnv id should construct and reset natively.""" + for env_id in _TRACK_IDS: + with self.subTest(env_id=env_id): + config, pool_type, spec_type = _track_config(env_id) + env = _make_env(config, pool_type, spec_type) + try: + obs, info = env.reset() + self.assertEqual(obs.shape[0], 1) + self.assertEqual(info["elapsed_step"].tolist(), [0]) + finally: + env.close() + + def test_track_native_determinism_representative_ids(self) -> None: + """Fixed, random, and tracked references should be deterministic.""" + for env_id in _TRACK_REPRESENTATIVE_IDS: + config, pool_type, spec_type = _track_config(env_id) + env0 = _make_env(config, pool_type, spec_type) + env1 = _make_env(config, pool_type, spec_type) + model = _track_model( + _entry(env_id)["kwargs"]["model_path"], + _entry(env_id)["kwargs"]["object_name"], + ) + actions = _seeded_actions((1, model.nu), steps=8, seed=173) + with self.subTest(env_id=env_id): + try: + _assert_rollouts_match(self, env0, env1, actions) + finally: + env0.close() + env1.close() + + def test_track_alignment_representative_ids(self) -> None: + """Fixed, random, and tracked references align with the official oracle.""" + cls = load_oracle_class("myosuite.envs.myo.myodm.myodm_v0", "TrackEnv") + for env_id in _TRACK_ALIGN_IDS: + with self.subTest(env_id=env_id): + entry = _entry(env_id) + with _oracle_track_kwargs(env_id) as oracle_kwargs: + oracle: Any | None = None + native: Any | None = None + try: + oracle = gymnasium.wrappers.TimeLimit( + cls(seed=123, **oracle_kwargs), + max_episode_steps=entry["max_episode_steps"], + ) + obs0, sync = _oracle_reset_sync(oracle) + config, pool_type, spec_type = _track_config( + env_id, overrides=sync, seed=123 + ) + model = _track_model( + entry["kwargs"]["model_path"], + entry["kwargs"]["object_name"], + ) + native = _make_env(config, pool_type, spec_type) + obs1, _ = native.reset() + np.testing.assert_allclose( + obs1, obs0[None, :], atol=1e-6, rtol=1e-6 + ) + actions = _seeded_actions( + (1, model.nu), steps=_ALIGNMENT_STEPS, seed=191 + ) + for step, action in enumerate(actions, start=1): + obs0, reward0, terminated0, truncated0, info0 = ( + oracle.step(action[0]) + ) + obs1, reward1, terminated1, truncated1, info1 = ( + native.step(action) + ) + obs_tolerance = _track_obs_tolerance(env_id) + np.testing.assert_allclose( + obs1, + obs0[None, :], + atol=obs_tolerance, + rtol=obs_tolerance, + err_msg=f"{env_id} step {step} observation", + ) + np.testing.assert_allclose( + reward1, + np.array([reward0]), + atol=1e-6, + rtol=1e-6, + err_msg=f"{env_id} step {step} reward", + ) + np.testing.assert_array_equal( + terminated1, np.array([terminated0]) + ) + np.testing.assert_array_equal( + truncated1, np.array([truncated0]) + ) + if terminated0 or truncated0: + break + finally: + if native is not None: + native.close() + if oracle is not None: + oracle.close() + + def test_track_playback_reference_alignment(self) -> None: + """Playback sync should consume repeated-time samples in oracle order.""" + env_id = "MyoHandPyramidmediumRandom-v0" + entry = _entry(env_id) + cls = load_oracle_class("myosuite.envs.myo.myodm.myodm_v0", "TrackEnv") + with _oracle_track_kwargs(env_id) as oracle_kwargs: + oracle: Any = None + native: Any = None + try: + oracle = gymnasium.wrappers.TimeLimit( + cls(seed=123, **oracle_kwargs), + max_episode_steps=entry["max_episode_steps"], + ) + with _record_track_reference_samples(oracle) as samples: + _, sync = _oracle_reset_sync(oracle) + _ = oracle.unwrapped.playback() + oracle_qpos = oracle.unwrapped.sim.data.qpos.copy() + oracle_qvel = oracle.unwrapped.sim.data.qvel.copy() + self.assertEqual( + [round(sample.time, 4) for sample in samples], [0.0, 0.0] + ) + sync["test_playback_reference"] = True + sync.update(_track_reference_sync(samples)) + config, pool_type, spec_type = _track_config( + env_id, overrides=sync, seed=123 + ) + native = _make_env(config, pool_type, spec_type) + _, _ = native.reset() + model = _track_model( + entry["kwargs"]["model_path"], + entry["kwargs"]["object_name"], + ) + zero = np.zeros((1, model.nu), dtype=np.float32) + obs, _, _, _, _ = native.step(zero) + qpos_dim = int(model.nq) + qvel_dim = int(model.nv) + native_qpos = np.asarray(obs)[0, :qpos_dim] + native_qvel = np.asarray(obs)[0, qpos_dim : qpos_dim + qvel_dim] + np.testing.assert_allclose( + native_qpos, oracle_qpos, atol=1e-9, rtol=1e-9 + ) + np.testing.assert_allclose( + native_qvel, oracle_qvel, atol=1e-9, rtol=1e-9 + ) + finally: + if native is not None: + native.close() + if oracle is not None: + oracle.close() + + def test_track_pixel_observation_smoke(self) -> None: + """Track pixel wrappers should emit batched RGB observations.""" + config, pool_type, spec_type = _track_pixel_config( + "MyoHandAirplaneFixed-v0" + ) + env = _make_env(config, pool_type, spec_type) + try: + obs, _ = env.reset() + self.assertEqual(obs.shape, (1, 3, 64, 64)) + finally: + env.close() + + +if __name__ == "__main__": + absltest.main() diff --git a/envpool/mujoco/myosuite/myosuite_asset_smoke_test.py b/envpool/mujoco/myosuite/myosuite_asset_smoke_test.py new file mode 100644 index 000000000..1137d37cf --- /dev/null +++ b/envpool/mujoco/myosuite/myosuite_asset_smoke_test.py @@ -0,0 +1,85 @@ +# Copyright 2026 Garena Online Private 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. +"""Smoke tests for staged MyoSuite MuJoCo asset loadability.""" + +from __future__ import annotations + +from pathlib import Path + +import mujoco +from absl.testing import absltest + +from envpool.mujoco.myosuite.metadata import MYOSUITE_DIRECT_ENTRIES +from envpool.mujoco.myosuite.paths import myosuite_asset_root + + +def _unique_model_paths() -> tuple[str, ...]: + return tuple( + sorted({ + entry["kwargs"]["model_path"] for entry in MYOSUITE_DIRECT_ENTRIES + }) + ) + + +def _unique_reference_paths() -> tuple[str, ...]: + return tuple( + sorted({ + entry["kwargs"]["reference"] + for entry in MYOSUITE_DIRECT_ENTRIES + if isinstance(entry["kwargs"].get("reference"), str) + }) + ) + + +def _loadable_model_paths() -> tuple[str, ...]: + return tuple( + sorted({ + entry["kwargs"]["model_path"] + for entry in MYOSUITE_DIRECT_ENTRIES + if not entry["model_placeholders"] + }) + ) + + +class MyoSuiteAssetSmokeTest(absltest.TestCase): + """Verifies the staged MyoSuite model tree is usable from runfiles.""" + + def test_all_registered_model_paths_exist(self) -> None: + """Checks that every generated direct task model path is staged.""" + root = myosuite_asset_root() + for relative_path in _unique_model_paths(): + with self.subTest(relative_path=relative_path): + self.assertTrue((root / relative_path).exists()) + + def test_reference_motion_files_exist(self) -> None: + """Checks that generated MyoDM motion references are staged.""" + root = myosuite_asset_root() + for relative_path in _unique_reference_paths(): + with self.subTest(relative_path=relative_path): + self.assertTrue((root / relative_path).exists()) + + def test_loadable_registered_models_open_in_mujoco(self) -> None: + """Checks that standalone staged models load through MuJoCo.""" + root = myosuite_asset_root() + for relative_path in _loadable_model_paths(): + with self.subTest(relative_path=relative_path): + xml_path = root / relative_path + self.assertIsInstance(xml_path, Path) + model = mujoco.MjModel.from_xml_path(str(xml_path)) + self.assertGreater(model.nq, 0, relative_path) + self.assertGreater(model.nv, 0, relative_path) + + +if __name__ == "__main__": + absltest.main() diff --git a/envpool/mujoco/myosuite/myosuite_metadata_test.py b/envpool/mujoco/myosuite/myosuite_metadata_test.py new file mode 100644 index 000000000..89d33d361 --- /dev/null +++ b/envpool/mujoco/myosuite/myosuite_metadata_test.py @@ -0,0 +1,364 @@ +# Copyright 2026 Garena Online Private 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. +"""Regression tests for generated MyoSuite upstream surface metadata.""" + +from __future__ import annotations + +import json +import os +import subprocess +import sys +import tempfile +from collections import Counter +from pathlib import Path + +from absl.testing import absltest + +from envpool.mujoco.myosuite.config import resolve_myosuite_task_config +from envpool.mujoco.myosuite.metadata import ( + MYOSUITE_COUNTS, + MYOSUITE_DIRECT_ENTRIES, + MYOSUITE_DIRECT_ENTRY_BY_ID, + MYOSUITE_DIRECT_IDS, + MYOSUITE_EXPANDED_IDS, + MYOSUITE_NOTES, + MYOSUITE_PINS, + MYOSUITE_SUITES, + load_myosuite_metadata, +) +from envpool.mujoco.myosuite.paths import ( + myosuite_asset_root, + resolve_workspace_path, +) + + +def _find_vendored_myosuite_upstream_root() -> Path: + """Locate the vendored upstream source tree in Bazel runfiles.""" + roots = [Path.cwd()] + for env_name in ("RUNFILES_DIR", "TEST_SRCDIR"): + value = os.environ.get(env_name) + if value: + roots.append(Path(value)) + for root in roots: + direct = root / "myosuite_src" + if (direct / "myosuite/envs/myo/myobase/__init__.py").exists(): + return direct + for init_path in root.rglob("myosuite/envs/myo/myobase/__init__.py"): + candidate = init_path.parents[4] + if (candidate / "pyproject.toml").exists(): + return candidate + raise FileNotFoundError("Unable to locate vendored myosuite upstream root") + + +class MyoSuiteMetadataTest(absltest.TestCase): + """Checks the vendored upstream registry snapshot stays stable.""" + + def test_upstream_pin_is_frozen(self) -> None: + """Checks the checked-in metadata stays anchored to the chosen release.""" + self.assertEqual( + MYOSUITE_PINS["myosuite"], + { + "version": "v2.11.6", + "commit": "05cb84678373f91271004f99602ebbf01e57d1a1", + }, + ) + + def test_surface_counts_match_generated_snapshot(self) -> None: + """Checks the generated direct and expanded ID counts remain stable.""" + self.assertEqual( + MYOSUITE_COUNTS, + { + "direct_total": 254, + "expanded_total": 398, + "myobase_direct": 45, + "myochallenge_direct": 19, + "myodm_direct": 190, + }, + ) + self.assertLen(MYOSUITE_DIRECT_IDS, MYOSUITE_COUNTS["direct_total"]) + self.assertLen(MYOSUITE_EXPANDED_IDS, MYOSUITE_COUNTS["expanded_total"]) + + def test_surface_lists_are_sorted_and_unique(self) -> None: + """Checks the generated ID lists are already canonicalized.""" + self.assertEqual(list(MYOSUITE_DIRECT_IDS), sorted(MYOSUITE_DIRECT_IDS)) + self.assertEqual( + list(MYOSUITE_EXPANDED_IDS), sorted(MYOSUITE_EXPANDED_IDS) + ) + self.assertLen(set(MYOSUITE_DIRECT_IDS), len(MYOSUITE_DIRECT_IDS)) + self.assertLen(set(MYOSUITE_EXPANDED_IDS), len(MYOSUITE_EXPANDED_IDS)) + + def test_expected_representative_ids_are_present(self) -> None: + """Checks representative direct and expanded IDs are preserved.""" + expected_direct = { + "myoElbowPose1D6MRandom-v0", + "myoHandPose7Fixed-v0", + "myoArmReachRandom-v0", + "myoChallengeBaodingP2-v1", + "myoChallengeTableTennisP2-v0", + "MyoHandBananaPass-v0", + "MyoHandWaterbottleShake-v0", + } + expected_expanded_only = { + "myoSarcElbowPose1D6MRandom-v0", + "myoFatiElbowPose1D6MRandom-v0", + "myoReafHandPoseRandom-v0", + } + self.assertTrue(expected_direct.issubset(MYOSUITE_DIRECT_IDS)) + self.assertTrue(expected_direct.issubset(MYOSUITE_EXPANDED_IDS)) + self.assertTrue(expected_expanded_only.issubset(MYOSUITE_EXPANDED_IDS)) + + def test_direct_entries_cover_direct_and_expanded_ids(self) -> None: + """Checks direct-entry metadata stays aligned with the ID surfaces.""" + self.assertLen(MYOSUITE_DIRECT_ENTRIES, MYOSUITE_COUNTS["direct_total"]) + self.assertEqual( + [entry["id"] for entry in MYOSUITE_DIRECT_ENTRIES], + list(MYOSUITE_DIRECT_IDS), + ) + expanded = sorted( + {entry["id"] for entry in MYOSUITE_DIRECT_ENTRIES} + | { + variant["variant_id"] + for entry in MYOSUITE_DIRECT_ENTRIES + for variant in entry["variant_defs"] + } + ) + self.assertEqual(expanded, list(MYOSUITE_EXPANDED_IDS)) + + def test_representative_direct_entries_are_stable(self) -> None: + """Checks representative task definitions remain pinned.""" + myobase_reach = MYOSUITE_DIRECT_ENTRY_BY_ID["myoArmReachRandom-v0"] + self.assertEqual(myobase_reach["suite"], "myobase") + self.assertEqual(myobase_reach["registration_suite"], "myoedits") + self.assertEqual(myobase_reach["class_name"], "ReachEnvV0") + self.assertEqual( + myobase_reach["kwargs"]["model_path"], + "simhive/myo_sim/arm/myoarm.xml", + ) + self.assertEqual(myobase_reach["kwargs"]["far_th"], 1.0) + self.assertEqual( + myobase_reach["kwargs"]["target_reach_range"]["IFtip"], + [[-0.35, -0.42, 0.98], [0.0, -0.07, 1.83]], + ) + self.assertEqual( + myobase_reach["kwargs"]["edit_fn"], "edit_fn_arm_reaching" + ) + self.assertEqual( + myobase_reach["default_config"]["obs_dim"], + 80, + ) + self.assertEqual( + myobase_reach["default_config"]["model_path"], + "simhive/myo_sim/arm/myoarm_reach.xml", + ) + self.assertEqual( + myobase_reach["default_config"]["action_dim"], + 34, + ) + self.assertEqual( + [ + variant["variant_id"] + for variant in myobase_reach["variant_defs"] + ], + [ + "myoFatiArmReachRandom-v0", + "myoSarcArmReachRandom-v0", + ], + ) + + myodm_track = MYOSUITE_DIRECT_ENTRY_BY_ID["MyoHandAirplaneFly-v0"] + self.assertEqual(myodm_track["suite"], "myodm") + self.assertEqual(myodm_track["class_name"], "TrackEnv") + self.assertEqual( + myodm_track["kwargs"]["model_path"], + "envs/myo/assets/hand/myohand_object.xml", + ) + self.assertEqual( + myodm_track["kwargs"]["reference"], + "envs/myo/myodm/data/MyoHand_airplane_fly1.npz", + ) + self.assertEqual(myodm_track["model_placeholders"], ["OBJECT_NAME"]) + self.assertEqual(myodm_track["default_config"]["robot_dim"], 29) + self.assertEqual(myodm_track["default_config"]["object_dim"], 7) + self.assertEqual(myodm_track["default_config"]["obs_dim"], 142) + self.assertEmpty(myodm_track["variant_defs"]) + + def test_reach_default_configs_capture_derived_render_flags(self) -> None: + """Checks baked reach configs keep walk-specific derived flags.""" + walk_reach = MYOSUITE_DIRECT_ENTRY_BY_ID["myoLegStandRandom-v0"] + self.assertEqual( + walk_reach["entry_module"], + "myosuite.envs.myo.myobase.walk_v0", + ) + self.assertEqual(walk_reach["class_name"], "ReachEnvV0") + self.assertTrue( + walk_reach["default_config"]["target_pos_relative_to_tip"] + ) + self.assertTrue(walk_reach["default_config"]["hide_skin_geom_group_1"]) + self.assertTrue(walk_reach["default_config"]["hide_terrain"]) + self.assertEqual( + walk_reach["default_config"]["joint_random_range"], + [-0.2, 0.2], + ) + + arm_reach = MYOSUITE_DIRECT_ENTRY_BY_ID["myoArmReachRandom-v0"] + self.assertEqual( + arm_reach["entry_module"], + "myosuite.envs.myo.myobase.reach_v0", + ) + self.assertEqual(arm_reach["class_name"], "ReachEnvV0") + self.assertFalse( + arm_reach["default_config"]["target_pos_relative_to_tip"] + ) + self.assertFalse(arm_reach["default_config"]["hide_skin_geom_group_1"]) + self.assertFalse(arm_reach["default_config"]["hide_terrain"]) + + def test_suite_breakdowns_match_expected_entrypoint_counts(self) -> None: + """Checks the pinned suite mix still matches the native port plan.""" + suite_counts = Counter( + (entry["suite"], entry["class_name"]) + for entry in MYOSUITE_DIRECT_ENTRIES + ) + self.assertEqual( + suite_counts, + Counter({ + ("myobase", "PoseEnvV0"): 20, + ("myobase", "ReachEnvV0"): 9, + ("myobase", "TerrainEnvV0"): 3, + ("myobase", "KeyTurnEnvV0"): 2, + ("myobase", "TorsoEnvV0"): 2, + ("myobase", "WalkEnvV0"): 1, + ("myobase", "ObjHoldFixedEnvV0"): 1, + ("myobase", "ObjHoldRandomEnvV0"): 1, + ("myobase", "PenTwirlFixedEnvV0"): 1, + ("myobase", "PenTwirlRandomEnvV0"): 1, + ("myobase", "Geometries8EnvV0"): 1, + ("myobase", "Geometries100EnvV0"): 1, + ("myobase", "InDistribution"): 1, + ("myobase", "OutofDistribution"): 1, + ("myochallenge", "TableTennisEnvV0"): 3, + ("myochallenge", "RelocateEnvV0"): 3, + ("myochallenge", "ChaseTagEnvV0"): 3, + ("myochallenge", "ReorientEnvV0"): 3, + ("myochallenge", "SoccerEnvV0"): 2, + ("myochallenge", "RunTrack"): 2, + ("myochallenge", "BaodingEnvV1"): 2, + ("myochallenge", "BimanualEnvV1"): 1, + ("myodm", "TrackEnv"): 190, + }), + ) + + def test_suite_lists_match_direct_entry_groups(self) -> None: + """Checks suite helper lists stay consistent with direct entries.""" + self.assertEqual( + MYOSUITE_SUITES["myobase_direct_ids"], + sorted( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["suite"] == "myobase" + ), + ) + self.assertEqual( + MYOSUITE_SUITES["myochallenge_direct_ids"], + sorted( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["suite"] == "myochallenge" + ), + ) + self.assertEqual( + MYOSUITE_SUITES["myodm_track_ids"], + sorted( + entry["id"] + for entry in MYOSUITE_DIRECT_ENTRIES + if entry["suite"] == "myodm" + and isinstance(entry["kwargs"].get("reference"), str) + ), + ) + + def test_duplicate_note_matches_upstream_myoedits_overlap(self) -> None: + """Checks duplicate-note metadata tracks the upstream myoedits overlap.""" + self.assertEqual( + MYOSUITE_NOTES["myoedits_duplicate_ids"], + [ + "myoArmReachFixed-v0", + "myoArmReachRandom-v0", + ], + ) + + def test_generated_paths_exist_in_staged_assets(self) -> None: + """Checks generated model and reference paths resolve under staged assets.""" + root = myosuite_asset_root() + for entry in MYOSUITE_DIRECT_ENTRIES: + with self.subTest(task_id=entry["id"], kind="model"): + self.assertTrue((root / entry["kwargs"]["model_path"]).exists()) + reference = entry["kwargs"].get("reference") + if isinstance(reference, str): + with self.subTest(task_id=entry["id"], kind="reference"): + self.assertTrue((root / reference).exists()) + + def test_preview_test_overrides_are_filtered_by_supported_spec( + self, + ) -> None: + """Checks pose-only preview state does not leak into torso configs.""" + base_path = str(myosuite_asset_root().parent.parent) + + pose_config = resolve_myosuite_task_config( + "myoElbowPose1D6MFixed-v0", + { + "base_path": base_path, + "test_target_qpos": [0.123], + }, + ) + self.assertEqual(pose_config["test_target_qpos"], [0.123]) + + torso_config = resolve_myosuite_task_config( + "myoTorsoPoseFixed-v0", + { + "base_path": base_path, + "test_target_qpos": [0.123], + }, + ) + self.assertNotIn("test_target_qpos", torso_config) + + def test_checked_in_metadata_matches_vendored_upstream_sources( + self, + ) -> None: + """Checks the checked-in snapshot still matches the pinned upstream.""" + upstream_root = _find_vendored_myosuite_upstream_root() + generator = resolve_workspace_path( + "third_party/myosuite/generate_metadata.py" + ) + with tempfile.TemporaryDirectory() as tmpdir: + output_path = Path(tmpdir) / "generated_env_ids.json" + subprocess.run( + [ + sys.executable, + str(generator), + "--upstream-root", + str(upstream_root), + "--version", + "v2.11.6", + "--commit", + "05cb84678373f91271004f99602ebbf01e57d1a1", + "--output", + str(output_path), + ], + check=True, + ) + regenerated = json.loads(output_path.read_text()) + self.assertEqual(regenerated, load_myosuite_metadata()) + + +if __name__ == "__main__": + absltest.main() diff --git a/envpool/mujoco/myosuite/myosuite_render_surface_test.py b/envpool/mujoco/myosuite/myosuite_render_surface_test.py new file mode 100644 index 000000000..a050dee4f --- /dev/null +++ b/envpool/mujoco/myosuite/myosuite_render_surface_test.py @@ -0,0 +1,217 @@ +# Copyright 2026 Garena Online Private 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. +"""Full-surface MyoSuite render oracle runner. + +Bazel sharding and absltest both consume TEST_TOTAL_SHARDS/TEST_SHARD_INDEX. +Keep the exhaustive render sweep out of absltest so every Bazel shard executes +the env-ID slice selected here instead of sharding unittest methods. +""" + +from __future__ import annotations + +import contextlib +import io +import os +from collections.abc import Sequence +from pathlib import Path + +import numpy as np +from absl import app, flags + +from envpool.mujoco.myosuite.render_utils import ( + MYOSUITE_RENDER_COMPARE_STEPS, + MYOSUITE_RENDER_RETRY_SEEDS, + MYOSUITE_RENDER_VALIDATE_TASK_IDS, + capture_render_sequence, + official_render_thresholds, +) + +_RENDER_WIDTH = 96 +_RENDER_HEIGHT = 72 +_CAPTURED_OUTPUT_TAIL_CHARS = 4000 + +FLAGS = flags.FLAGS +flags.DEFINE_integer( + "myosuite_render_shard_index", + 0, + "Zero-based shard index used only when Bazel sharding is disabled.", +) +flags.DEFINE_integer( + "myosuite_render_shard_count", + 1, + "Shard count used only when Bazel sharding is disabled.", +) + + +def _assert_frames_close( + actual: np.ndarray, + expected: np.ndarray, + *, + label: str, + max_mean_abs_diff: float, + max_mismatch_ratio: float, +) -> None: + if actual.shape != expected.shape: + raise AssertionError( + f"{label} shapes differ: {actual.shape} != {expected.shape}" + ) + diff = np.abs(actual.astype(np.int16) - expected.astype(np.int16)) + if diff.size == 0: + return + mismatch_count = int(np.count_nonzero(diff)) + mismatch_ratio = float(mismatch_count) / float(diff.size) + mean_abs_diff = float(diff.mean()) + max_abs_diff = int(diff.max()) + mismatch_detail = "" + if mismatch_count: + coords = np.argwhere(diff)[:5] + examples = [] + for y, x, channel in coords: + examples.append( + f"({int(y)},{int(x)},{int(channel)}): " + f"{int(actual[y, x, channel])}!={int(expected[y, x, channel])}" + ) + mismatch_detail = "; first mismatches " + ", ".join(examples) + if mean_abs_diff > max_mean_abs_diff: + raise AssertionError( + f"{label} mean render delta " + f"{mean_abs_diff:.6f} exceeded {max_mean_abs_diff:.6f}; " + f"max delta {max_abs_diff}, mismatched values " + f"{mismatch_count}/{diff.size} ({mismatch_ratio:.6%})" + f"{mismatch_detail}" + ) + if mismatch_ratio > max_mismatch_ratio: + raise AssertionError( + f"{label} render mismatch ratio " + f"{mismatch_ratio:.6%} exceeded {max_mismatch_ratio:.6%}; " + f"mean delta {mean_abs_diff:.6f}, max delta {max_abs_diff}, " + f"mismatched values {mismatch_count}/{diff.size}" + f"{mismatch_detail}" + ) + + +def _shard_params() -> tuple[int, int]: + bazel_shard_count = int(os.environ.get("TEST_TOTAL_SHARDS", "0")) + if bazel_shard_count > 1: + return int(os.environ.get("TEST_SHARD_INDEX", "0")), bazel_shard_count + return ( + int(FLAGS.myosuite_render_shard_index), + int(FLAGS.myosuite_render_shard_count), + ) + + +def _selected_task_ids() -> tuple[str, ...]: + shard_index, shard_count = _shard_params() + if shard_count <= 0: + raise ValueError("TEST_TOTAL_SHARDS must be positive") + if shard_index < 0 or shard_index >= shard_count: + raise ValueError( + "TEST_SHARD_INDEX must satisfy 0 <= index < TEST_TOTAL_SHARDS" + ) + # The public IDs are generated in sorted upstream order. Keep contiguous + # model variants together so macOS AGX does not compile shaders for many + # unrelated MyoDM object models in one process. + total = len(MYOSUITE_RENDER_VALIDATE_TASK_IDS) + start = total * shard_index // shard_count + end = total * (shard_index + 1) // shard_count + return tuple(MYOSUITE_RENDER_VALIDATE_TASK_IDS[start:end]) + + +def _captured_output_tail(captured_output: str) -> str: + if not captured_output: + return "" + if len(captured_output) > _CAPTURED_OUTPUT_TAIL_CHARS: + captured_output = captured_output[-_CAPTURED_OUTPUT_TAIL_CHARS:] + return f"\nCaptured oracle output tail:\n{captured_output}" + + +def _check_task(task_id: str) -> None: + max_mean_abs_diff, max_mismatch_ratio = official_render_thresholds(task_id) + sequence = capture_render_sequence( + task_id, + steps=MYOSUITE_RENDER_COMPARE_STEPS, + seed=7, + render_width=_RENDER_WIDTH, + render_height=_RENDER_HEIGHT, + action_mode="random", + retry_seeds=MYOSUITE_RENDER_RETRY_SEEDS, + ) + _assert_frames_close( + sequence.reset_envpool, + sequence.reset_official, + label=f"{task_id} reset", + max_mean_abs_diff=max_mean_abs_diff, + max_mismatch_ratio=max_mismatch_ratio, + ) + if len(sequence.envpool_frames) != MYOSUITE_RENDER_COMPARE_STEPS: + raise AssertionError( + f"{task_id} captured {len(sequence.envpool_frames)} EnvPool " + f"frames, expected {MYOSUITE_RENDER_COMPARE_STEPS}" + ) + if len(sequence.official_frames) != MYOSUITE_RENDER_COMPARE_STEPS: + raise AssertionError( + f"{task_id} captured {len(sequence.official_frames)} official " + f"frames, expected {MYOSUITE_RENDER_COMPARE_STEPS}" + ) + for index, (envpool_frame, official_frame) in enumerate( + zip(sequence.envpool_frames, sequence.official_frames, strict=True), + start=1, + ): + _assert_frames_close( + envpool_frame, + official_frame, + label=f"{task_id} step {index}", + max_mean_abs_diff=max_mean_abs_diff, + max_mismatch_ratio=max_mismatch_ratio, + ) + + +def main(argv: Sequence[str]) -> None: + """Run this Bazel shard's public MyoSuite render comparisons.""" + if len(argv) > 1: + raise app.UsageError("unexpected positional arguments") + shard_status_file = os.environ.get("TEST_SHARD_STATUS_FILE") + if shard_status_file: + Path(shard_status_file).touch() + task_ids = _selected_task_ids() + shard_index, shard_count = _shard_params() + if not task_ids: + raise AssertionError("render surface shard selected no MyoSuite tasks") + print( + "Checking MyoSuite render shard " + f"{shard_index}/{shard_count}: {len(task_ids)} task IDs", + flush=True, + ) + failures: list[str] = [] + for task_id in task_ids: + print(f"Checking {task_id}", flush=True) + captured_output = io.StringIO() + try: + with contextlib.redirect_stdout(captured_output): + with contextlib.redirect_stderr(captured_output): + _check_task(task_id) + except Exception as exc: + output_tail = _captured_output_tail(captured_output.getvalue()) + failures.append( + f"{task_id}: {type(exc).__name__}: {exc}{output_tail}" + ) + if failures: + joined = "\n\n".join(failures) + raise AssertionError( + f"{len(failures)} MyoSuite render comparison(s) failed:\n\n{joined}" + ) + + +if __name__ == "__main__": + app.run(main) diff --git a/envpool/mujoco/myosuite/myosuite_render_test.py b/envpool/mujoco/myosuite/myosuite_render_test.py new file mode 100644 index 000000000..be75de49b --- /dev/null +++ b/envpool/mujoco/myosuite/myosuite_render_test.py @@ -0,0 +1,149 @@ +# Copyright 2026 Garena Online Private 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. +"""Render tests for representative MyoSuite tasks.""" + +from __future__ import annotations + +from typing import Any, cast + +import numpy as np +from absl.testing import absltest + +from envpool.mujoco.myosuite.render_utils import ( + MYOSUITE_RENDER_COMPARE_CASES, + MYOSUITE_RENDER_COMPARE_STEPS, + MYOSUITE_RENDER_RETRY_SEEDS, + MYOSUITE_RENDER_VALIDATE_TASK_IDS, + capture_render_sequence, +) +from envpool.registration import make_gymnasium + +_RENDER_WIDTH = 96 +_RENDER_HEIGHT = 72 + + +def _render_array(env: Any, env_ids: Any = None) -> np.ndarray: + frame = env.render(env_ids=env_ids) + assert frame is not None + return cast(np.ndarray, frame) + + +def _zero_action(space: Any, num_envs: int) -> np.ndarray: + sample = np.asarray(space.sample()) + zero = np.zeros_like(sample) + if sample.ndim == 0: + return np.full((num_envs,), zero.item(), dtype=sample.dtype) + return np.repeat(zero[np.newaxis, ...], num_envs, axis=0) + + +def _assert_frames_close( + actual: np.ndarray, + expected: np.ndarray, + *, + label: str = "frame", + max_mean_abs_diff: float = 1.0, + max_mismatch_ratio: float = 0.1, +) -> None: + if actual.shape != expected.shape: + raise AssertionError( + f"{label} shapes differ: {actual.shape} != {expected.shape}" + ) + diff = np.abs(actual.astype(np.int16) - expected.astype(np.int16)) + if diff.size == 0: + return + mismatch_count = int(np.count_nonzero(diff)) + mismatch_ratio = float(mismatch_count) / float(diff.size) + mean_abs_diff = float(diff.mean()) + max_abs_diff = int(diff.max()) + if mean_abs_diff > max_mean_abs_diff: + raise AssertionError( + f"{label} mean render delta " + f"{mean_abs_diff:.6f} exceeded {max_mean_abs_diff:.6f}; " + f"max delta {max_abs_diff}, mismatched values " + f"{mismatch_count}/{diff.size} ({mismatch_ratio:.6%})" + ) + if mismatch_ratio > max_mismatch_ratio: + raise AssertionError( + f"{label} render mismatch ratio " + f"{mismatch_ratio:.6%} exceeded {max_mismatch_ratio:.6%}; " + f"mean delta {mean_abs_diff:.6f}, max delta {max_abs_diff}, " + f"mismatched values {mismatch_count}/{diff.size}" + ) + + +class MyoSuiteRenderTest(absltest.TestCase): + """Checks public render semantics and representative oracle frames.""" + + def test_render_is_batch_consistent_and_state_invariant(self) -> None: + """Repeated renders should be batch-consistent and side-effect free.""" + env = make_gymnasium( + "myoHandReorientID-v0", + num_envs=2, + seed=0, + render_mode="rgb_array", + render_width=_RENDER_WIDTH, + render_height=_RENDER_HEIGHT, + ) + try: + env.reset() + for step in range(MYOSUITE_RENDER_COMPARE_STEPS): + frame0 = _render_array(env) + frame1 = _render_array(env, env_ids=1) + frames = _render_array(env, env_ids=[0, 1]) + frame0_again = _render_array(env) + + self.assertEqual( + frame0.shape, (1, _RENDER_HEIGHT, _RENDER_WIDTH, 3) + ) + self.assertEqual( + frame1.shape, (1, _RENDER_HEIGHT, _RENDER_WIDTH, 3) + ) + self.assertEqual( + frames.shape, (2, _RENDER_HEIGHT, _RENDER_WIDTH, 3) + ) + self.assertEqual(frame0.dtype, np.uint8) + _assert_frames_close(frame0[0], frames[0]) + _assert_frames_close(frame1[0], frames[1]) + _assert_frames_close(frame0, frame0_again) + self.assertGreater(int(frame0.max()) - int(frame0.min()), 0) + + if step + 1 < MYOSUITE_RENDER_COMPARE_STEPS: + env.step(_zero_action(env.action_space, 2)) + finally: + env.close() + + def test_representative_render_cases_stay_documented(self) -> None: + """Representative doc cases remain part of the public render sweep.""" + covered = set(MYOSUITE_RENDER_VALIDATE_TASK_IDS) + for render_case in MYOSUITE_RENDER_COMPARE_CASES: + with self.subTest(task_id=render_case.task_id): + self.assertIn(render_case.task_id, covered) + + def test_render_retry_skips_early_terminal_seed(self) -> None: + """Render capture should retry when a candidate seed ends too early.""" + sequence = capture_render_sequence( + "MyoHandAlarmclockFixed-v0", + steps=MYOSUITE_RENDER_COMPARE_STEPS, + seed=7, + render_width=_RENDER_WIDTH, + render_height=_RENDER_HEIGHT, + action_mode="random", + retry_seeds=MYOSUITE_RENDER_RETRY_SEEDS, + ) + self.assertLen(sequence.envpool_frames, MYOSUITE_RENDER_COMPARE_STEPS) + self.assertLen(sequence.official_frames, MYOSUITE_RENDER_COMPARE_STEPS) + + +if __name__ == "__main__": + absltest.main() diff --git a/envpool/mujoco/myosuite/native.py b/envpool/mujoco/myosuite/native.py new file mode 100644 index 000000000..0c46f00b4 --- /dev/null +++ b/envpool/mujoco/myosuite/native.py @@ -0,0 +1,386 @@ +# Copyright 2026 Garena Online Private 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. +"""Internal native MyoSuite wrappers used before public registration.""" + +from envpool.mujoco.myosuite_envpool import ( + _MyoChallengeBaodingEnvPool, + _MyoChallengeBaodingEnvSpec, + _MyoChallengeBaodingPixelEnvPool, + _MyoChallengeBaodingPixelEnvSpec, + _MyoChallengeBimanualEnvPool, + _MyoChallengeBimanualEnvSpec, + _MyoChallengeBimanualPixelEnvPool, + _MyoChallengeBimanualPixelEnvSpec, + _MyoChallengeChaseTagEnvPool, + _MyoChallengeChaseTagEnvSpec, + _MyoChallengeChaseTagPixelEnvPool, + _MyoChallengeChaseTagPixelEnvSpec, + _MyoChallengeRelocateEnvPool, + _MyoChallengeRelocateEnvSpec, + _MyoChallengeRelocatePixelEnvPool, + _MyoChallengeRelocatePixelEnvSpec, + _MyoChallengeReorientEnvPool, + _MyoChallengeReorientEnvSpec, + _MyoChallengeReorientPixelEnvPool, + _MyoChallengeReorientPixelEnvSpec, + _MyoChallengeRunTrackEnvPool, + _MyoChallengeRunTrackEnvSpec, + _MyoChallengeRunTrackPixelEnvPool, + _MyoChallengeRunTrackPixelEnvSpec, + _MyoChallengeSoccerEnvPool, + _MyoChallengeSoccerEnvSpec, + _MyoChallengeSoccerPixelEnvPool, + _MyoChallengeSoccerPixelEnvSpec, + _MyoChallengeTableTennisEnvPool, + _MyoChallengeTableTennisEnvSpec, + _MyoChallengeTableTennisPixelEnvPool, + _MyoChallengeTableTennisPixelEnvSpec, + _MyoDMTrackEnvPool, + _MyoDMTrackEnvSpec, + _MyoDMTrackPixelEnvPool, + _MyoDMTrackPixelEnvSpec, + _MyoSuiteKeyTurnEnvPool, + _MyoSuiteKeyTurnEnvSpec, + _MyoSuiteKeyTurnPixelEnvPool, + _MyoSuiteKeyTurnPixelEnvSpec, + _MyoSuiteObjHoldEnvPool, + _MyoSuiteObjHoldEnvSpec, + _MyoSuiteObjHoldPixelEnvPool, + _MyoSuiteObjHoldPixelEnvSpec, + _MyoSuitePenTwirlEnvPool, + _MyoSuitePenTwirlEnvSpec, + _MyoSuitePenTwirlPixelEnvPool, + _MyoSuitePenTwirlPixelEnvSpec, + _MyoSuitePoseEnvPool, + _MyoSuitePoseEnvSpec, + _MyoSuitePosePixelEnvPool, + _MyoSuitePosePixelEnvSpec, + _MyoSuiteReachEnvPool, + _MyoSuiteReachEnvSpec, + _MyoSuiteReachPixelEnvPool, + _MyoSuiteReachPixelEnvSpec, + _MyoSuiteReorientEnvPool, + _MyoSuiteReorientEnvSpec, + _MyoSuiteReorientPixelEnvPool, + _MyoSuiteReorientPixelEnvSpec, + _MyoSuiteTerrainEnvPool, + _MyoSuiteTerrainEnvSpec, + _MyoSuiteTerrainPixelEnvPool, + _MyoSuiteTerrainPixelEnvSpec, + _MyoSuiteTorsoEnvPool, + _MyoSuiteTorsoEnvSpec, + _MyoSuiteTorsoPixelEnvPool, + _MyoSuiteTorsoPixelEnvSpec, + _MyoSuiteWalkEnvPool, + _MyoSuiteWalkEnvSpec, + _MyoSuiteWalkPixelEnvPool, + _MyoSuiteWalkPixelEnvSpec, +) + +from envpool.python.api import py_env + +( + MyoSuitePoseEnvSpec, + MyoSuitePoseDMEnvPool, + MyoSuitePoseGymnasiumEnvPool, +) = py_env(_MyoSuitePoseEnvSpec, _MyoSuitePoseEnvPool) +( + MyoSuitePosePixelEnvSpec, + MyoSuitePosePixelDMEnvPool, + MyoSuitePosePixelGymnasiumEnvPool, +) = py_env(_MyoSuitePosePixelEnvSpec, _MyoSuitePosePixelEnvPool) +( + MyoSuiteReachEnvSpec, + MyoSuiteReachDMEnvPool, + MyoSuiteReachGymnasiumEnvPool, +) = py_env(_MyoSuiteReachEnvSpec, _MyoSuiteReachEnvPool) +( + MyoSuiteReachPixelEnvSpec, + MyoSuiteReachPixelDMEnvPool, + MyoSuiteReachPixelGymnasiumEnvPool, +) = py_env(_MyoSuiteReachPixelEnvSpec, _MyoSuiteReachPixelEnvPool) +( + MyoSuiteReorientEnvSpec, + MyoSuiteReorientDMEnvPool, + MyoSuiteReorientGymnasiumEnvPool, +) = py_env(_MyoSuiteReorientEnvSpec, _MyoSuiteReorientEnvPool) +( + MyoSuiteReorientPixelEnvSpec, + MyoSuiteReorientPixelDMEnvPool, + MyoSuiteReorientPixelGymnasiumEnvPool, +) = py_env(_MyoSuiteReorientPixelEnvSpec, _MyoSuiteReorientPixelEnvPool) +( + MyoSuiteWalkEnvSpec, + MyoSuiteWalkDMEnvPool, + MyoSuiteWalkGymnasiumEnvPool, +) = py_env(_MyoSuiteWalkEnvSpec, _MyoSuiteWalkEnvPool) +( + MyoSuiteWalkPixelEnvSpec, + MyoSuiteWalkPixelDMEnvPool, + MyoSuiteWalkPixelGymnasiumEnvPool, +) = py_env(_MyoSuiteWalkPixelEnvSpec, _MyoSuiteWalkPixelEnvPool) +( + MyoSuiteTerrainEnvSpec, + MyoSuiteTerrainDMEnvPool, + MyoSuiteTerrainGymnasiumEnvPool, +) = py_env(_MyoSuiteTerrainEnvSpec, _MyoSuiteTerrainEnvPool) +( + MyoSuiteTerrainPixelEnvSpec, + MyoSuiteTerrainPixelDMEnvPool, + MyoSuiteTerrainPixelGymnasiumEnvPool, +) = py_env(_MyoSuiteTerrainPixelEnvSpec, _MyoSuiteTerrainPixelEnvPool) +( + MyoSuiteKeyTurnEnvSpec, + MyoSuiteKeyTurnDMEnvPool, + MyoSuiteKeyTurnGymnasiumEnvPool, +) = py_env(_MyoSuiteKeyTurnEnvSpec, _MyoSuiteKeyTurnEnvPool) +( + MyoSuiteKeyTurnPixelEnvSpec, + MyoSuiteKeyTurnPixelDMEnvPool, + MyoSuiteKeyTurnPixelGymnasiumEnvPool, +) = py_env(_MyoSuiteKeyTurnPixelEnvSpec, _MyoSuiteKeyTurnPixelEnvPool) +( + MyoSuiteObjHoldEnvSpec, + MyoSuiteObjHoldDMEnvPool, + MyoSuiteObjHoldGymnasiumEnvPool, +) = py_env(_MyoSuiteObjHoldEnvSpec, _MyoSuiteObjHoldEnvPool) +( + MyoSuiteObjHoldPixelEnvSpec, + MyoSuiteObjHoldPixelDMEnvPool, + MyoSuiteObjHoldPixelGymnasiumEnvPool, +) = py_env(_MyoSuiteObjHoldPixelEnvSpec, _MyoSuiteObjHoldPixelEnvPool) +( + MyoSuiteTorsoEnvSpec, + MyoSuiteTorsoDMEnvPool, + MyoSuiteTorsoGymnasiumEnvPool, +) = py_env(_MyoSuiteTorsoEnvSpec, _MyoSuiteTorsoEnvPool) +( + MyoSuiteTorsoPixelEnvSpec, + MyoSuiteTorsoPixelDMEnvPool, + MyoSuiteTorsoPixelGymnasiumEnvPool, +) = py_env(_MyoSuiteTorsoPixelEnvSpec, _MyoSuiteTorsoPixelEnvPool) +( + MyoSuitePenTwirlEnvSpec, + MyoSuitePenTwirlDMEnvPool, + MyoSuitePenTwirlGymnasiumEnvPool, +) = py_env(_MyoSuitePenTwirlEnvSpec, _MyoSuitePenTwirlEnvPool) +( + MyoSuitePenTwirlPixelEnvSpec, + MyoSuitePenTwirlPixelDMEnvPool, + MyoSuitePenTwirlPixelGymnasiumEnvPool, +) = py_env(_MyoSuitePenTwirlPixelEnvSpec, _MyoSuitePenTwirlPixelEnvPool) +( + MyoChallengeReorientEnvSpec, + MyoChallengeReorientDMEnvPool, + MyoChallengeReorientGymnasiumEnvPool, +) = py_env(_MyoChallengeReorientEnvSpec, _MyoChallengeReorientEnvPool) +( + MyoChallengeReorientPixelEnvSpec, + MyoChallengeReorientPixelDMEnvPool, + MyoChallengeReorientPixelGymnasiumEnvPool, +) = py_env(_MyoChallengeReorientPixelEnvSpec, _MyoChallengeReorientPixelEnvPool) +( + MyoChallengeRelocateEnvSpec, + MyoChallengeRelocateDMEnvPool, + MyoChallengeRelocateGymnasiumEnvPool, +) = py_env(_MyoChallengeRelocateEnvSpec, _MyoChallengeRelocateEnvPool) +( + MyoChallengeRelocatePixelEnvSpec, + MyoChallengeRelocatePixelDMEnvPool, + MyoChallengeRelocatePixelGymnasiumEnvPool, +) = py_env(_MyoChallengeRelocatePixelEnvSpec, _MyoChallengeRelocatePixelEnvPool) +( + MyoChallengeBaodingEnvSpec, + MyoChallengeBaodingDMEnvPool, + MyoChallengeBaodingGymnasiumEnvPool, +) = py_env(_MyoChallengeBaodingEnvSpec, _MyoChallengeBaodingEnvPool) +( + MyoChallengeBaodingPixelEnvSpec, + MyoChallengeBaodingPixelDMEnvPool, + MyoChallengeBaodingPixelGymnasiumEnvPool, +) = py_env(_MyoChallengeBaodingPixelEnvSpec, _MyoChallengeBaodingPixelEnvPool) +( + MyoChallengeBimanualEnvSpec, + MyoChallengeBimanualDMEnvPool, + MyoChallengeBimanualGymnasiumEnvPool, +) = py_env(_MyoChallengeBimanualEnvSpec, _MyoChallengeBimanualEnvPool) +( + MyoChallengeBimanualPixelEnvSpec, + MyoChallengeBimanualPixelDMEnvPool, + MyoChallengeBimanualPixelGymnasiumEnvPool, +) = py_env(_MyoChallengeBimanualPixelEnvSpec, _MyoChallengeBimanualPixelEnvPool) +( + MyoChallengeRunTrackEnvSpec, + MyoChallengeRunTrackDMEnvPool, + MyoChallengeRunTrackGymnasiumEnvPool, +) = py_env(_MyoChallengeRunTrackEnvSpec, _MyoChallengeRunTrackEnvPool) +( + MyoChallengeRunTrackPixelEnvSpec, + MyoChallengeRunTrackPixelDMEnvPool, + MyoChallengeRunTrackPixelGymnasiumEnvPool, +) = py_env(_MyoChallengeRunTrackPixelEnvSpec, _MyoChallengeRunTrackPixelEnvPool) +( + MyoChallengeSoccerEnvSpec, + MyoChallengeSoccerDMEnvPool, + MyoChallengeSoccerGymnasiumEnvPool, +) = py_env(_MyoChallengeSoccerEnvSpec, _MyoChallengeSoccerEnvPool) +( + MyoChallengeSoccerPixelEnvSpec, + MyoChallengeSoccerPixelDMEnvPool, + MyoChallengeSoccerPixelGymnasiumEnvPool, +) = py_env(_MyoChallengeSoccerPixelEnvSpec, _MyoChallengeSoccerPixelEnvPool) +( + MyoChallengeChaseTagEnvSpec, + MyoChallengeChaseTagDMEnvPool, + MyoChallengeChaseTagGymnasiumEnvPool, +) = py_env(_MyoChallengeChaseTagEnvSpec, _MyoChallengeChaseTagEnvPool) +( + MyoChallengeChaseTagPixelEnvSpec, + MyoChallengeChaseTagPixelDMEnvPool, + MyoChallengeChaseTagPixelGymnasiumEnvPool, +) = py_env(_MyoChallengeChaseTagPixelEnvSpec, _MyoChallengeChaseTagPixelEnvPool) +( + MyoChallengeTableTennisEnvSpec, + MyoChallengeTableTennisDMEnvPool, + MyoChallengeTableTennisGymnasiumEnvPool, +) = py_env(_MyoChallengeTableTennisEnvSpec, _MyoChallengeTableTennisEnvPool) +( + MyoChallengeTableTennisPixelEnvSpec, + MyoChallengeTableTennisPixelDMEnvPool, + MyoChallengeTableTennisPixelGymnasiumEnvPool, +) = py_env( + _MyoChallengeTableTennisPixelEnvSpec, + _MyoChallengeTableTennisPixelEnvPool, +) +( + MyoDMTrackEnvSpec, + MyoDMTrackDMEnvPool, + MyoDMTrackGymnasiumEnvPool, +) = py_env(_MyoDMTrackEnvSpec, _MyoDMTrackEnvPool) +( + MyoDMTrackPixelEnvSpec, + MyoDMTrackPixelDMEnvPool, + MyoDMTrackPixelGymnasiumEnvPool, +) = py_env(_MyoDMTrackPixelEnvSpec, _MyoDMTrackPixelEnvPool) + +__all__ = [ + "MyoSuitePoseEnvSpec", + "MyoSuitePoseDMEnvPool", + "MyoSuitePoseGymnasiumEnvPool", + "MyoSuitePosePixelEnvSpec", + "MyoSuitePosePixelDMEnvPool", + "MyoSuitePosePixelGymnasiumEnvPool", + "MyoSuiteReachEnvSpec", + "MyoSuiteReachDMEnvPool", + "MyoSuiteReachGymnasiumEnvPool", + "MyoSuiteReachPixelEnvSpec", + "MyoSuiteReachPixelDMEnvPool", + "MyoSuiteReachPixelGymnasiumEnvPool", + "MyoSuiteReorientEnvSpec", + "MyoSuiteReorientDMEnvPool", + "MyoSuiteReorientGymnasiumEnvPool", + "MyoSuiteReorientPixelEnvSpec", + "MyoSuiteReorientPixelDMEnvPool", + "MyoSuiteReorientPixelGymnasiumEnvPool", + "MyoSuiteWalkEnvSpec", + "MyoSuiteWalkDMEnvPool", + "MyoSuiteWalkGymnasiumEnvPool", + "MyoSuiteWalkPixelEnvSpec", + "MyoSuiteWalkPixelDMEnvPool", + "MyoSuiteWalkPixelGymnasiumEnvPool", + "MyoSuiteTerrainEnvSpec", + "MyoSuiteTerrainDMEnvPool", + "MyoSuiteTerrainGymnasiumEnvPool", + "MyoSuiteTerrainPixelEnvSpec", + "MyoSuiteTerrainPixelDMEnvPool", + "MyoSuiteTerrainPixelGymnasiumEnvPool", + "MyoSuiteKeyTurnEnvSpec", + "MyoSuiteKeyTurnDMEnvPool", + "MyoSuiteKeyTurnGymnasiumEnvPool", + "MyoSuiteKeyTurnPixelEnvSpec", + "MyoSuiteKeyTurnPixelDMEnvPool", + "MyoSuiteKeyTurnPixelGymnasiumEnvPool", + "MyoSuiteObjHoldEnvSpec", + "MyoSuiteObjHoldDMEnvPool", + "MyoSuiteObjHoldGymnasiumEnvPool", + "MyoSuiteObjHoldPixelEnvSpec", + "MyoSuiteObjHoldPixelDMEnvPool", + "MyoSuiteObjHoldPixelGymnasiumEnvPool", + "MyoSuiteTorsoEnvSpec", + "MyoSuiteTorsoDMEnvPool", + "MyoSuiteTorsoGymnasiumEnvPool", + "MyoSuiteTorsoPixelEnvSpec", + "MyoSuiteTorsoPixelDMEnvPool", + "MyoSuiteTorsoPixelGymnasiumEnvPool", + "MyoSuitePenTwirlEnvSpec", + "MyoSuitePenTwirlDMEnvPool", + "MyoSuitePenTwirlGymnasiumEnvPool", + "MyoSuitePenTwirlPixelEnvSpec", + "MyoSuitePenTwirlPixelDMEnvPool", + "MyoSuitePenTwirlPixelGymnasiumEnvPool", + "MyoChallengeReorientEnvSpec", + "MyoChallengeReorientDMEnvPool", + "MyoChallengeReorientGymnasiumEnvPool", + "MyoChallengeReorientPixelEnvSpec", + "MyoChallengeReorientPixelDMEnvPool", + "MyoChallengeReorientPixelGymnasiumEnvPool", + "MyoChallengeRelocateEnvSpec", + "MyoChallengeRelocateDMEnvPool", + "MyoChallengeRelocateGymnasiumEnvPool", + "MyoChallengeRelocatePixelEnvSpec", + "MyoChallengeRelocatePixelDMEnvPool", + "MyoChallengeRelocatePixelGymnasiumEnvPool", + "MyoChallengeBaodingEnvSpec", + "MyoChallengeBaodingDMEnvPool", + "MyoChallengeBaodingGymnasiumEnvPool", + "MyoChallengeBaodingPixelEnvSpec", + "MyoChallengeBaodingPixelDMEnvPool", + "MyoChallengeBaodingPixelGymnasiumEnvPool", + "MyoChallengeBimanualEnvSpec", + "MyoChallengeBimanualDMEnvPool", + "MyoChallengeBimanualGymnasiumEnvPool", + "MyoChallengeBimanualPixelEnvSpec", + "MyoChallengeBimanualPixelDMEnvPool", + "MyoChallengeBimanualPixelGymnasiumEnvPool", + "MyoChallengeRunTrackEnvSpec", + "MyoChallengeRunTrackDMEnvPool", + "MyoChallengeRunTrackGymnasiumEnvPool", + "MyoChallengeRunTrackPixelEnvSpec", + "MyoChallengeRunTrackPixelDMEnvPool", + "MyoChallengeRunTrackPixelGymnasiumEnvPool", + "MyoChallengeSoccerEnvSpec", + "MyoChallengeSoccerDMEnvPool", + "MyoChallengeSoccerGymnasiumEnvPool", + "MyoChallengeSoccerPixelEnvSpec", + "MyoChallengeSoccerPixelDMEnvPool", + "MyoChallengeSoccerPixelGymnasiumEnvPool", + "MyoChallengeChaseTagEnvSpec", + "MyoChallengeChaseTagDMEnvPool", + "MyoChallengeChaseTagGymnasiumEnvPool", + "MyoChallengeChaseTagPixelEnvSpec", + "MyoChallengeChaseTagPixelDMEnvPool", + "MyoChallengeChaseTagPixelGymnasiumEnvPool", + "MyoChallengeTableTennisEnvSpec", + "MyoChallengeTableTennisDMEnvPool", + "MyoChallengeTableTennisGymnasiumEnvPool", + "MyoChallengeTableTennisPixelEnvSpec", + "MyoChallengeTableTennisPixelDMEnvPool", + "MyoChallengeTableTennisPixelGymnasiumEnvPool", + "MyoDMTrackEnvSpec", + "MyoDMTrackDMEnvPool", + "MyoDMTrackGymnasiumEnvPool", + "MyoDMTrackPixelEnvSpec", + "MyoDMTrackPixelDMEnvPool", + "MyoDMTrackPixelGymnasiumEnvPool", +] diff --git a/envpool/mujoco/myosuite/oracle_utils.py b/envpool/mujoco/myosuite/oracle_utils.py new file mode 100644 index 000000000..427edc015 --- /dev/null +++ b/envpool/mujoco/myosuite/oracle_utils.py @@ -0,0 +1,405 @@ +# Copyright 2026 Garena Online Private 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 MyoSuite oracle helpers for tests and docs generation.""" + +from __future__ import annotations + +import ctypes +import importlib +import os +import platform +import subprocess +import sys +import tempfile +from contextlib import contextmanager +from functools import cache +from pathlib import Path +from typing import Any, Iterator + +from envpool.mujoco.myosuite.paths import ( + myosuite_asset_root, + resolve_workspace_path, +) + + +def _configure_linux_mujoco_gl() -> None: + if platform.system() != "Linux": + return + if os.environ.get("MUJOCO_GL"): + if os.environ["MUJOCO_GL"] == "egl": + os.environ.setdefault("EGL_PLATFORM", "surfaceless") + return + for backend in ("egl", "osmesa"): + env = dict(os.environ) + env["MUJOCO_GL"] = backend + if backend == "egl": + env.setdefault("EGL_PLATFORM", "surfaceless") + result = subprocess.run( + [ + sys.executable, + "-c", + ( + "import mujoco; " + "ctx = mujoco.GLContext(1, 1); " + "ctx.make_current(); " + "ctx.free()" + ), + ], + env=env, + check=False, + capture_output=True, + ) + if result.returncode == 0: + os.environ["MUJOCO_GL"] = backend + if backend == "egl": + os.environ.setdefault("EGL_PLATFORM", "surfaceless") + return + + +def _configure_macos_dm_control_imports() -> None: + if platform.system() != "Darwin": + return + # dm_control eagerly imports the GLFW backend when MUJOCO_GL is unset. + # Force the no-render backend at import time so non-render oracle tests do + # not hang inside glfw.init() on macOS. + os.environ.setdefault("MUJOCO_GL", "off") + + +def _configure_windows_dm_control_imports() -> None: + if platform.system() != "Windows": + return + # MyoSuite's official reset path can instantiate dm_control rendering + # helpers even when an oracle test only compares numeric state. Windows CI + # supplies Mesa for explicit render tests, but this non-render path should + # not depend on GLFW picking up a usable WGL context. + os.environ.setdefault("MUJOCO_GL", "off") + + +def _configure_macos_dm_control_renderer() -> None: + if platform.system() != "Darwin": + return + + from dm_control import _render + from dm_control._render import base as dm_control_render_base + from dm_control._render import executor as dm_control_render_executor + + class _CglContext(dm_control_render_base.ContextBase): + def __init__(self, max_width: int, max_height: int): + super().__init__( + max_width, + max_height, + dm_control_render_executor.PassthroughRenderExecutor, + ) + + def _platform_init(self, max_width: int, max_height: int) -> None: + del max_width, max_height + from mujoco.cgl import cgl + + self._pixel_format = None + self._context = None + self._locked = False + attrib = cgl.CGLPixelFormatAttribute + profile = cgl.CGLOpenGLProfile + attrib_values = ( + attrib.CGLPFAOpenGLProfile, + profile.CGLOGLPVersion_Legacy, + attrib.CGLPFAColorSize, + 24, + attrib.CGLPFAAlphaSize, + 8, + attrib.CGLPFADepthSize, + 24, + attrib.CGLPFAStencilSize, + 8, + attrib.CGLPFAAllowOfflineRenderers, + 0, + ) + attribs = (ctypes.c_int * len(attrib_values))(*attrib_values) + self._pixel_format = cgl.CGLPixelFormatObj() + num_pixel_formats = cgl.GLint() + err = cgl.CGLChoosePixelFormat( + attribs, + ctypes.byref(self._pixel_format), + ctypes.byref(num_pixel_formats), + ) + if err or not self._pixel_format or num_pixel_formats.value == 0: + raise RuntimeError("failed to create CGL pixel format") + + self._context = cgl.CGLContextObj() + err = cgl.CGLCreateContext( + self._pixel_format, + 0, + ctypes.byref(self._context), + ) + if err or not self._context: + cgl.CGLReleasePixelFormat(self._pixel_format) + self._pixel_format = None + raise RuntimeError("failed to create CGL context") + + def _platform_make_current(self) -> None: + from mujoco.cgl import cgl + + cgl.CGLSetCurrentContext(self._context) + # Mirror mujoco.cgl.GLContext so the official renderer uses the + # same CGL lifecycle as EnvPool's native renderer. + if not self._locked: + cgl.CGLLockContext(self._context) + self._locked = True + + def _platform_free(self) -> None: + from mujoco.cgl import cgl + + context = getattr(self, "_context", None) + if context: + if getattr(self, "_locked", False): + cgl.CGLUnlockContext(self._context) + self._locked = False + cgl.CGLSetCurrentContext(None) + cgl.CGLReleaseContext(self._context) + self._context = None + pixel_format = getattr(self, "_pixel_format", None) + if pixel_format: + cgl.CGLReleasePixelFormat(self._pixel_format) + self._pixel_format = None + + _render.Renderer = _CglContext + _render.BACKEND = "cgl" + _render.USING_GPU = True + + +def _configure_macos_mujoco_renderer() -> None: + if platform.system() != "Darwin": + return + + import mujoco.rendering.classic.gl_context as classic_gl_context + import mujoco.rendering.classic.renderer as classic_renderer + + class _ClassicCglContext: + def __init__(self, width: int, height: int): + del width, height + from mujoco.cgl import cgl + + self._pixel_format = None + self._context = None + self._locked = False + attrib = cgl.CGLPixelFormatAttribute + profile = cgl.CGLOpenGLProfile + attrib_values = ( + attrib.CGLPFAOpenGLProfile, + profile.CGLOGLPVersion_Legacy, + attrib.CGLPFAColorSize, + 24, + attrib.CGLPFAAlphaSize, + 8, + attrib.CGLPFADepthSize, + 24, + attrib.CGLPFAStencilSize, + 8, + attrib.CGLPFAAllowOfflineRenderers, + 0, + ) + attribs = (ctypes.c_int * len(attrib_values))(*attrib_values) + self._pixel_format = cgl.CGLPixelFormatObj() + num_pixel_formats = cgl.GLint() + err = cgl.CGLChoosePixelFormat( + attribs, + ctypes.byref(self._pixel_format), + ctypes.byref(num_pixel_formats), + ) + if err or not self._pixel_format or num_pixel_formats.value == 0: + raise RuntimeError("failed to create CGL pixel format") + + self._context = cgl.CGLContextObj() + err = cgl.CGLCreateContext( + self._pixel_format, + 0, + ctypes.byref(self._context), + ) + if err or not self._context: + cgl.CGLReleasePixelFormat(self._pixel_format) + self._pixel_format = None + raise RuntimeError("failed to create CGL context") + + def make_current(self) -> None: + from mujoco.cgl import cgl + + cgl.CGLSetCurrentContext(self._context) + if not self._locked: + cgl.CGLLockContext(self._context) + self._locked = True + + def free(self) -> None: + from mujoco.cgl import cgl + + context = getattr(self, "_context", None) + if context: + if getattr(self, "_locked", False): + cgl.CGLUnlockContext(self._context) + self._locked = False + cgl.CGLSetCurrentContext(None) + cgl.CGLReleaseContext(self._context) + self._context = None + pixel_format = getattr(self, "_pixel_format", None) + if pixel_format: + cgl.CGLReleasePixelFormat(self._pixel_format) + self._pixel_format = None + + def __del__(self) -> None: + self.free() + + os.environ["MUJOCO_GL"] = "cgl" + classic_gl_context.GLContext = _ClassicCglContext + classic_renderer.GLContext = _ClassicCglContext + + +def _replace_all(text: str, old: str, new: str) -> str: + return text.replace(old, new) + + +def _relative_model_path(path: Path, *, oracle_dir: Path) -> str: + prefix = "\\" if os.name == "nt" else "/" + return prefix + os.path.relpath(path, oracle_dir) + + +def find_vendored_myosuite_root() -> Path: + """Locate the vendored upstream MyoSuite Python source tree.""" + root = resolve_workspace_path(".") + for candidate in (root, *root.parents): + direct = candidate / "myosuite_src" + if (direct / "myosuite/envs/myo/myobase/pose_v0.py").exists(): + return direct + for pose_path in candidate.rglob( + "myosuite/envs/myo/myobase/pose_v0.py" + ): + return pose_path.parents[4] + raise FileNotFoundError("Unable to locate vendored myosuite source root") + + +@cache +def prepare_oracle_imports(*, render: bool = False) -> None: + """Expose vendored MyoSuite Python modules on sys.path.""" + _configure_linux_mujoco_gl() + if not render: + _configure_macos_dm_control_imports() + _configure_windows_dm_control_imports() + source_root = str(find_vendored_myosuite_root()) + if source_root not in sys.path: + sys.path.insert(0, source_root) + if render: + _configure_macos_dm_control_renderer() + _configure_macos_mujoco_renderer() + + +@cache +def load_oracle_class(entry_module: str, class_name: str) -> Any: + """Import one official MyoSuite environment class from vendored source.""" + prepare_oracle_imports() + module = importlib.import_module(entry_module) + return getattr(module, class_name) + + +@cache +def load_oracle_attr(module_path: str, attr_name: str) -> Any: + """Import one vendored upstream attribute by module path and name.""" + prepare_oracle_imports() + module = importlib.import_module(module_path) + return getattr(module, attr_name) + + +@contextmanager +def prepared_track_oracle_model_path() -> Iterator[str]: + """Yield a TrackEnv model_path that resolves to writable staged assets.""" + asset_root = myosuite_asset_root() + source_model = asset_root / "envs/myo/assets/hand/myohand_object.xml" + object_xml = source_model.read_text() + tabletop_xml = ( + asset_root / "envs/myo/assets/hand/myohand_tabletop.xml" + ).read_text() + hand_assets_xml = ( + asset_root / "simhive/myo_sim/hand/assets/myohand_assets.xml" + ).read_text() + myo_sim_root = asset_root / "simhive/myo_sim" + myo_sim_root_str = str(myo_sim_root) + object_sim_root = asset_root / "simhive/object_sim" + object_sim_root_str = str(object_sim_root) + oracle_dir = find_vendored_myosuite_root() / "myosuite/envs/myo/myodm" + + with tempfile.TemporaryDirectory(prefix="envpool_myodm_oracle_") as td: + tmp_dir = Path(td) + hand_assets_tmp = tmp_dir / "myohand_assets.xml" + tabletop_tmp = tmp_dir / "myohand_tabletop.xml" + object_tmp = tmp_dir / "myohand_object.xml" + + hand_assets_xml = _replace_all( + hand_assets_xml, + 'meshdir=".." texturedir=".."', + f'meshdir="{myo_sim_root_str}" texturedir="{myo_sim_root_str}"', + ) + hand_assets_tmp.write_text(hand_assets_xml) + + tabletop_xml = _replace_all( + tabletop_xml, + "../../../../simhive/myo_sim/hand/assets/myohand_assets.xml", + str(hand_assets_tmp), + ) + tabletop_xml = _replace_all( + tabletop_xml, + "../../../../simhive/furniture_sim/simpleTable/simpleTable_asset.xml", + str( + asset_root + / "simhive/furniture_sim/simpleTable/simpleTable_asset.xml" + ), + ) + tabletop_xml = _replace_all( + tabletop_xml, + "../../../../simhive/myo_sim/hand/assets/myohand_body.xml", + str(asset_root / "simhive/myo_sim/hand/assets/myohand_body.xml"), + ) + tabletop_xml = _replace_all( + tabletop_xml, + "../../../../simhive/furniture_sim/simpleTable/" + "simpleGraniteTable_body.xml", + str( + asset_root + / "simhive/furniture_sim/simpleTable/simpleGraniteTable_body.xml" + ), + ) + tabletop_xml = _replace_all( + tabletop_xml, + 'meshdir="../../../../simhive/myo_sim/" texturedir="../../../../simhive/myo_sim/"', + f'meshdir="{myo_sim_root_str}" texturedir="{myo_sim_root_str}"', + ) + tabletop_tmp.write_text(tabletop_xml) + + object_xml = _replace_all( + object_xml, "myohand_tabletop.xml", str(tabletop_tmp) + ) + object_xml = _replace_all( + object_xml, + "../../../../simhive/object_sim/common.xml", + str(object_sim_root / "common.xml"), + ) + object_xml = _replace_all( + object_xml, + "../../../../simhive/object_sim/OBJECT_NAME/assets.xml", + object_sim_root_str + "/OBJECT_NAME/assets.xml", + ) + object_xml = _replace_all( + object_xml, + "../../../../simhive/object_sim/OBJECT_NAME/body.xml", + object_sim_root_str + "/OBJECT_NAME/body.xml", + ) + object_tmp.write_text(object_xml) + + yield _relative_model_path(object_tmp, oracle_dir=oracle_dir) diff --git a/envpool/mujoco/myosuite/paths.h b/envpool/mujoco/myosuite/paths.h new file mode 100644 index 000000000..30888aabd --- /dev/null +++ b/envpool/mujoco/myosuite/paths.h @@ -0,0 +1,87 @@ +// Copyright 2026 Garena Online Private 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 ENVPOOL_MUJOCO_MYOSUITE_PATHS_H_ +#define ENVPOOL_MUJOCO_MYOSUITE_PATHS_H_ + +#include +#include +#include +#include + +#ifdef _WIN32 +#include +#else +#include +#endif + +namespace myosuite { + +inline std::string MyoSuiteAssetRoot(const std::string& base_path) { + return base_path + "/mujoco/myosuite_assets"; +} + +inline bool IsAbsolutePath(std::string_view path) { + if (path.empty()) { + return false; + } + if (path[0] == '/') { + return true; + } +#ifdef _WIN32 + if (path[0] == '\\') { + return true; + } + return path.size() >= 3 && + std::isalpha(static_cast(path[0])) && path[1] == ':' && + (path[2] == '/' || path[2] == '\\'); +#else + return false; +#endif +} + +inline std::string CurrentWorkingDirectory() { + std::array buffer{}; +#ifdef _WIN32 + if (_getcwd(buffer.data(), static_cast(buffer.size())) == nullptr) { + return {}; + } +#else + if (getcwd(buffer.data(), buffer.size()) == nullptr) { + return {}; + } +#endif + return {buffer.data()}; +} + +inline std::string MyoSuiteModelPath(const std::string& base_path, + std::string_view relative_model_path) { + if (IsAbsolutePath(relative_model_path)) { + return std::string(relative_model_path); + } + std::string model_path = + MyoSuiteAssetRoot(base_path) + "/" + std::string(relative_model_path); + if (IsAbsolutePath(model_path)) { + return model_path; + } + std::string cwd = CurrentWorkingDirectory(); + if (cwd.empty()) { + return model_path; + } + return cwd + "/" + model_path; +} + +} // namespace myosuite + +#endif // ENVPOOL_MUJOCO_MYOSUITE_PATHS_H_ diff --git a/envpool/mujoco/myosuite/paths.py b/envpool/mujoco/myosuite/paths.py new file mode 100644 index 000000000..07577f488 --- /dev/null +++ b/envpool/mujoco/myosuite/paths.py @@ -0,0 +1,69 @@ +# Copyright 2026 Garena Online Private 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. +"""Runfile helpers for vendored MyoSuite assets and metadata.""" + +from __future__ import annotations + +import os +from pathlib import Path + +_WORKSPACE_NAME = "envpool" + + +def _candidate_roots() -> list[Path]: + roots: list[Path] = [Path.cwd()] + for env_name in ("RUNFILES_DIR", "TEST_SRCDIR"): + value = os.environ.get(env_name) + if value: + roots.append(Path(value)) + here = Path(__file__).resolve() + roots.extend(here.parents) + return roots + + +def resolve_workspace_path(relative_path: str) -> Path: + """Resolve a workspace-relative path in repo or Bazel runfiles layouts.""" + rel = Path(relative_path) + seen: set[Path] = set() + for root in _candidate_roots(): + for candidate in (root / rel, root / _WORKSPACE_NAME / rel): + if candidate in seen: + continue + seen.add(candidate) + if candidate.exists(): + return candidate + raise FileNotFoundError( + f"Unable to resolve workspace path {relative_path!r}; " + f"checked {len(seen)} candidate locations." + ) + + +def myosuite_asset_root() -> Path: + """Return the staged MyoSuite asset tree root.""" + return resolve_workspace_path("envpool/mujoco/myosuite_assets") + + +def myosuite_metadata_path() -> Path: + """Return the vendored generated MyoSuite metadata JSON path.""" + try: + return resolve_workspace_path( + "third_party/myosuite/metadata/env_ids.json" + ) + except FileNotFoundError as workspace_error: + # Release wheels do not carry the source workspace, but Bazel packages + # this generated metadata next to the Python modules. + packaged_metadata = Path(__file__).resolve().with_name("env_ids.json") + if packaged_metadata.exists(): + return packaged_metadata + raise workspace_error diff --git a/envpool/mujoco/myosuite/registration.py b/envpool/mujoco/myosuite/registration.py new file mode 100644 index 000000000..e7f5ffcf3 --- /dev/null +++ b/envpool/mujoco/myosuite/registration.py @@ -0,0 +1,81 @@ +# Copyright 2026 Garena Online Private 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. +"""Public MyoSuite task registration.""" + +from envpool.mujoco.myosuite.config import ( + myosuite_expanded_entry, + resolve_myosuite_task_config, +) +from envpool.mujoco.myosuite.metadata import MYOSUITE_EXPANDED_IDS +from envpool.registration import register, registry + +_IMPORT_PATH = "envpool.mujoco.myosuite.native" +MYOSUITE_PUBLIC_TASK_IDS = tuple(MYOSUITE_EXPANDED_IDS) + +_CLASS_PREFIX = { + ("myobase", "PoseEnvV0"): "MyoSuitePose", + ("myobase", "ReachEnvV0"): "MyoSuiteReach", + ("myobase", "Geometries100EnvV0"): "MyoSuiteReorient", + ("myobase", "Geometries8EnvV0"): "MyoSuiteReorient", + ("myobase", "InDistribution"): "MyoSuiteReorient", + ("myobase", "OutofDistribution"): "MyoSuiteReorient", + ("myobase", "KeyTurnEnvV0"): "MyoSuiteKeyTurn", + ("myobase", "ObjHoldFixedEnvV0"): "MyoSuiteObjHold", + ("myobase", "ObjHoldRandomEnvV0"): "MyoSuiteObjHold", + ("myobase", "TorsoEnvV0"): "MyoSuiteTorso", + ("myobase", "PenTwirlFixedEnvV0"): "MyoSuitePenTwirl", + ("myobase", "PenTwirlRandomEnvV0"): "MyoSuitePenTwirl", + ("myobase", "WalkEnvV0"): "MyoSuiteWalk", + ("myobase", "TerrainEnvV0"): "MyoSuiteTerrain", + ("myochallenge", "ReorientEnvV0"): "MyoChallengeReorient", + ("myochallenge", "RelocateEnvV0"): "MyoChallengeRelocate", + ("myochallenge", "BaodingEnvV1"): "MyoChallengeBaoding", + ("myochallenge", "BimanualEnvV1"): "MyoChallengeBimanual", + ("myochallenge", "RunTrack"): "MyoChallengeRunTrack", + ("myochallenge", "SoccerEnvV0"): "MyoChallengeSoccer", + ("myochallenge", "ChaseTagEnvV0"): "MyoChallengeChaseTag", + ("myochallenge", "TableTennisEnvV0"): "MyoChallengeTableTennis", + ("myodm", "TrackEnv"): "MyoDMTrack", +} + + +def _public_env_names(task_id: str) -> tuple[str, str, str]: + entry, _ = myosuite_expanded_entry(task_id) + prefix = _CLASS_PREFIX[(entry["suite"], entry["class_name"])] + return ( + f"{prefix}EnvSpec", + f"{prefix}DMEnvPool", + f"{prefix}GymnasiumEnvPool", + ) + + +def register_myosuite_tasks() -> None: + """Register every public MyoSuite task ID.""" + for task_id in MYOSUITE_PUBLIC_TASK_IDS: + if task_id in registry.specs: + continue + entry, _ = myosuite_expanded_entry(task_id) + spec_cls, dm_cls, gym_cls = _public_env_names(task_id) + register( + task_id=task_id, + import_path=_IMPORT_PATH, + spec_cls=spec_cls, + dm_cls=dm_cls, + gymnasium_cls=gym_cls, + max_episode_steps=int(entry["max_episode_steps"]), + _config_resolver=resolve_myosuite_task_config, + ) + + +register_myosuite_tasks() diff --git a/envpool/mujoco/myosuite/render_utils.py b/envpool/mujoco/myosuite/render_utils.py new file mode 100644 index 000000000..61a58efb2 --- /dev/null +++ b/envpool/mujoco/myosuite/render_utils.py @@ -0,0 +1,1203 @@ +# Copyright 2026 Garena Online Private 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 MyoSuite official-render helpers for tests and docs generation.""" + +from __future__ import annotations + +import gc +import inspect +import platform +from contextlib import contextmanager +from dataclasses import dataclass +from pathlib import Path +from typing import Any, Iterator, cast + +import gymnasium +import numpy as np + +from envpool.mujoco.myosuite.config import ( + myosuite_expanded_entry, + resolve_myosuite_model_path, +) +from envpool.mujoco.myosuite.metadata import MYOSUITE_SUITES +from envpool.mujoco.myosuite.oracle_utils import ( + load_oracle_class, + prepare_oracle_imports, + prepared_track_oracle_model_path, +) +from envpool.mujoco.myosuite.paths import myosuite_asset_root +from envpool.mujoco.myosuite.registration import MYOSUITE_PUBLIC_TASK_IDS +from envpool.python.glfw_context import preload_windows_gl_dlls +from envpool.registration import make_gymnasium + +preload_windows_gl_dlls(strict=True) +prepare_oracle_imports(render=True) + + +_REORIENT_CLASS_NAMES = { + "Geometries100EnvV0", + "Geometries8EnvV0", + "InDistribution", + "OutofDistribution", +} + + +@dataclass(frozen=True) +class RenderCase: + """One representative MyoSuite task rendered in docs and tests.""" + + task_id: str + label: str + + +@dataclass(frozen=True) +class RenderSequence: + """Reset frame plus a short stepped render rollout for one task.""" + + reset_envpool: np.ndarray + reset_official: np.ndarray + envpool_frames: tuple[np.ndarray, ...] + official_frames: tuple[np.ndarray, ...] + + +class _RenderEarlyTerminationError(RuntimeError): + """A candidate render seed terminated before enough frames were captured.""" + + def __init__(self, task_id: str, seed: int, step_index: int): + super().__init__( + f"{task_id} terminated at step {step_index} before render capture " + f"finished for seed {seed}" + ) + self.task_id = task_id + self.seed = seed + self.step_index = step_index + + +@dataclass(frozen=True) +class _TrackReferenceSample: + """One oracle TrackEnv reference sample consumed during a rollout.""" + + time: float + robot: np.ndarray + robot_vel: np.ndarray | None + object: np.ndarray + + +MYOSUITE_RENDER_COMPARE_CASES = ( + RenderCase("myoHandReorientID-v0", "HandReorientID"), + RenderCase("myoLegWalk-v0", "LegWalk"), + RenderCase("myoChallengeBimanual-v0", "ChallengeBimanual"), + RenderCase("MyoHandAirplaneFly-v0", "TrackAirplaneFly"), + RenderCase("myoLegHillyTerrainWalk-v0", "HillyTerrainWalk"), +) +MYOSUITE_RENDER_VALIDATE_TASK_IDS = tuple(MYOSUITE_PUBLIC_TASK_IDS) +MYOSUITE_RENDER_COMPARE_STEPS = 3 +MYOSUITE_RENDER_RETRY_SEEDS = ( + 11, + 23, + 37, + 53, + 71, + 89, + 107, + 131, + 151, + 173, + 197, + 223, + 257, +) +_MYODM_FIXED_TASK_IDS = frozenset(MYOSUITE_SUITES["myodm_fixed_ids"]) + + +def official_render_thresholds(task_id: str) -> tuple[float, float]: + """Return the render oracle thresholds for MyoSuite compare frames.""" + del task_id + return (0.0, 0.0) + + +def _entry(task_id: str) -> dict[str, Any]: + entry, _ = myosuite_expanded_entry(task_id) + return entry + + +def _task_kwargs(task_id: str) -> dict[str, Any]: + entry, variant_kwargs = myosuite_expanded_entry(task_id) + return {**entry["kwargs"], **variant_kwargs} + + +def _tolist_array(value: Any) -> list[Any]: + return np.asarray(value).tolist() + + +def _soccer_policy_id(policy: str) -> int: + return {"block_ball": 0, "random": 1, "stationary": 2}[policy] + + +def _chasetag_policy_id(policy: str) -> int: + return { + "static_stationary": 0, + "stationary": 1, + "random": 2, + "chase_player": 3, + "repeller": 4, + }[policy] + + +def _runtrack_osl_state_id(state: str) -> int: + return { + "e_stance": 0, + "l_stance": 1, + "e_swing": 2, + "l_swing": 3, + }[state] + + +def _asset_model_path(model_path: str) -> Path: + path = Path(model_path) + return path if path.is_absolute() else myosuite_asset_root() / model_path + + +def _action_shape(env: Any) -> tuple[int, ...]: + sample = np.asarray(env.action_space.sample()) + if sample.ndim == 0: + return (1,) + return (1, *sample.shape) + + +def _seeded_actions( + shape: tuple[int, ...], steps: int, seed: int +) -> list[np.ndarray]: + rng = np.random.default_rng(seed) + return [ + rng.uniform(-0.9, 0.9, size=shape).astype(np.float32) + for _ in range(steps) + ] + + +def _zero_actions(shape: tuple[int, ...], steps: int) -> list[np.ndarray]: + return [np.zeros(shape, dtype=np.float32) for _ in range(steps)] + + +def _hold_actions( + env: Any, sync: dict[str, Any], steps: int +) -> list[np.ndarray]: + import mujoco + + ctrl = np.asarray(sync["test_reset_ctrl"], dtype=np.float64) + unwrapped = _unwrapped_env(env) + model = unwrapped.sim.model + raw = ctrl.copy() + for index in range(model.nu): + if ( + model.na != 0 + and model.actuator_dyntype[index] == mujoco.mjtDyn.mjDYN_MUSCLE + ): + activation = float(np.clip(ctrl[index], 1e-6, 1.0 - 1e-6)) + raw[index] = 0.5 - np.log(1.0 / activation - 1.0) / 5.0 + raw = np.clip(raw, -1.0, 1.0).astype(np.float32, copy=False) + action = raw.reshape(_action_shape(env)) + return [action.copy() for _ in range(steps)] + + +def _render_envpool_array(env: Any) -> np.ndarray: + frame = env.render() + if frame is None: + raise RuntimeError("EnvPool render() returned None") + return cast(np.ndarray, frame) + + +def _unwrapped_env(env: Any) -> Any: + return env.unwrapped if hasattr(env, "unwrapped") else env + + +def _close_oracle_render_resources(env: Any) -> None: + sim = getattr(_unwrapped_env(env), "sim", None) + renderer = getattr(sim, "renderer", None) + if renderer is None: + return + # MyoSuite's MJRenderer.close() only handles the onscreen window. Release + # its offscreen mujoco.Renderer explicitly so macOS AGX shader variants do + # not accumulate across the full-surface render oracle sweep. + offscreen_renderer = getattr(renderer, "_renderer", None) + if offscreen_renderer is not None and hasattr(offscreen_renderer, "close"): + offscreen_renderer.close() + renderer._renderer = None + window = getattr(renderer, "_window", None) + if window is not None and hasattr(window, "close"): + window.close() + renderer._window = None + + +def _to_uint8_frame(frame: Any) -> np.ndarray: + array = np.asarray(frame) + if array.dtype == np.uint8: + return array + if np.issubdtype(array.dtype, np.floating): + max_value = float(np.max(array)) if array.size else 0.0 + if max_value <= 1.0: + array = np.clip(array, 0.0, 1.0) * 255.0 + else: + array = np.clip(array, 0.0, 255.0) + return np.rint(array).astype(np.uint8) + return np.clip(array, 0, 255).astype(np.uint8) + + +def _render_official_array( + env: Any, + *, + width: int, + height: int, + camera_id: int, + prefer_physics_render: bool = False, +) -> np.ndarray: + import mujoco + + sim = getattr(_unwrapped_env(env), "sim", None) + if sim is None: + raise RuntimeError("official env does not expose sim") + physics = getattr(sim, "sim", None) + if ( + prefer_physics_render + and physics is not None + and hasattr(physics, "render") + ): + if hasattr(sim, "upload_height_field") and int(sim.model.nhfield) > 0: + for hfield_id in range(int(sim.model.nhfield)): + sim.upload_height_field(hfield_id) + frame = physics.render( + height=height, + width=width, + camera_id=camera_id, + ) + if frame is None: + raise RuntimeError("official physics.render() returned None") + return _to_uint8_frame(frame) + if hasattr(sim, "renderer"): + needs_macos_warmup = ( + platform.system() == "Darwin" + and getattr(sim.renderer, "_renderer", None) is None + ) + if hasattr(sim, "upload_height_field") and int(sim.model.nhfield) > 0: + if getattr(sim.renderer, "_renderer", None) is None: + sim.renderer.setup_renderer( + sim.model.ptr, height=height, width=width + ) + native_renderer = getattr(sim.renderer, "_renderer", None) + mjr_context = getattr(native_renderer, "_mjr_context", None) + gl_context = getattr(native_renderer, "_gl_context", None) + if mjr_context is not None: + if gl_context is not None: + gl_context.make_current() + for hfield_id in range(int(sim.model.nhfield)): + mujoco.mjr_uploadHField( + sim.model.ptr, + mjr_context, + hfield_id, + ) + for hfield_id in range(int(sim.model.nhfield)): + sim.upload_height_field(hfield_id) + if needs_macos_warmup: + # The official mujoco.Renderer CGL path can produce one-LSB edge + # pixels on its first draw after creating a fresh mjrContext. Match + # EnvPool's native renderer initialization by discarding that + # context warm-up frame before returning the oracle image. + sim.renderer.render_offscreen( + width=width, + height=height, + camera_id=camera_id, + ) + frame = sim.renderer.render_offscreen( + width=width, + height=height, + camera_id=camera_id, + ) + if frame is None: + raise RuntimeError("official render_offscreen() returned None") + return _to_uint8_frame(frame) + if physics is not None and hasattr(physics, "render"): + frame = physics.render( + height=height, + width=width, + camera_id=camera_id, + ) + if frame is None: + raise RuntimeError("official physics.render() returned None") + return _to_uint8_frame(frame) + raise RuntimeError("official env does not expose a renderable sim") + + +def _ctor_accepts_kwarg(cls: Any, name: str) -> bool: + sig = inspect.signature(cls.__init__) + if name in sig.parameters: + return True + return any( + param.kind == inspect.Parameter.VAR_KEYWORD + for param in sig.parameters.values() + ) + + +def _dict_reference_init( + reference: dict[str, Any], key: str, array_key: str +) -> np.ndarray: + value = reference.get(key) + if value is not None: + return np.asarray(value, dtype=np.float64) + base = np.asarray(reference[array_key], dtype=np.float64) + if base.shape[0] == 2: + return np.mean(base, axis=0) + return base[0] + + +def _oracle_reference(reference: Any) -> Any: + if isinstance(reference, str): + return str(myosuite_asset_root() / reference) + ref = dict(reference) + time = np.asarray(ref["time"], dtype=np.float64) + robot = np.asarray(ref["robot"], dtype=np.float64) + obj = np.asarray(ref["object"], dtype=np.float64) + robot_vel = ( + np.asarray(ref["robot_vel"], dtype=np.float64) + if ref.get("robot_vel") is not None + else None + ) + robot_init = _dict_reference_init(ref, "robot_init", "robot") + object_init = _dict_reference_init(ref, "object_init", "object") + return { + "time": time, + "robot": robot, + "robot_vel": robot_vel, + "object": obj, + "robot_init": robot_init, + "object_init": object_init, + } + + +@contextmanager +def _oracle_kwargs(task_id: str) -> Iterator[dict[str, Any]]: + entry = _entry(task_id) + kwargs = _task_kwargs(task_id) + if entry["suite"] == "myodm": + with prepared_track_oracle_model_path() as model_path: + yield { + "object_name": kwargs["object_name"], + "model_path": model_path, + "reference": _oracle_reference(kwargs["reference"]), + } + return + if entry["suite"] == "myobase" and kwargs.get("edit_fn") is not None: + edit_fn_name = str(kwargs.pop("edit_fn")) + runtime_model_path = resolve_myosuite_model_path( + str(kwargs["model_path"]), edit_fn_name + ) + kwargs["model_path"] = str(_asset_model_path(runtime_model_path)) + for path_key in ("model_path", "init_pose_path"): + if path_key in kwargs: + kwargs[path_key] = str(_asset_model_path(kwargs[path_key])) + yield kwargs + + +@contextmanager +def _make_oracle( + task_id: str, + *, + seed: int, + render_width: int, + render_height: int, + camera_id: int, +) -> Iterator[Any]: + entry = _entry(task_id) + oracle_cls = load_oracle_class(entry["entry_module"], entry["class_name"]) + with _oracle_kwargs(task_id) as kwargs: + if _ctor_accepts_kwarg(oracle_cls, "render_mode"): + kwargs["render_mode"] = "rgb_array" + if camera_id != -1 and _ctor_accepts_kwarg(oracle_cls, "camera_id"): + kwargs["camera_id"] = camera_id + oracle: Any = gymnasium.wrappers.TimeLimit( + oracle_cls(seed=seed, **kwargs), + max_episode_steps=int(entry["max_episode_steps"]), + ) + try: + yield oracle + finally: + _close_oracle_render_resources(oracle) + oracle.close() + gc.collect() + + +def _sync_reset_myobase( + env: Any, task_id: str +) -> tuple[np.ndarray, dict[str, Any]]: + obs, _ = env.reset() + unwrapped = env.unwrapped + entry = _entry(task_id) + sync: dict[str, Any] = { + "test_reset_qpos": unwrapped.sim.data.qpos.copy().tolist(), + "test_reset_qvel": unwrapped.sim.data.qvel.copy().tolist(), + "test_reset_act": ( + unwrapped.sim.data.act.copy().tolist() + if unwrapped.sim.model.na > 0 + else [] + ), + "test_reset_qacc_warmstart": ( + unwrapped.sim.data.qacc_warmstart.copy().tolist() + ), + } + if entry["class_name"] in {"PoseEnvV0", "ReachEnvV0"}: + sync["test_reset_ctrl"] = unwrapped.sim.data.ctrl.copy().tolist() + if entry["class_name"] == "KeyTurnEnvV0": + obs_sim = getattr(unwrapped, "sim_obsd", unwrapped.sim) + keyhead_sid = obs_sim.model.site_name2id("keyhead") + key_body_id = int(obs_sim.model.site_bodyid[keyhead_sid]) + sync["test_key_body_pos"] = ( + obs_sim.model.body_pos[key_body_id].copy().tolist() + ) + elif entry["class_name"] in {"ObjHoldFixedEnvV0", "ObjHoldRandomEnvV0"}: + goal_sid = unwrapped.sim.model.site_name2id("goal") + geom_id = unwrapped.sim.model.geom_name2id("object") + sync["test_goal_pos"] = ( + unwrapped.sim.model.site_pos[goal_sid].copy().tolist() + ) + sync["test_object_geom_size"] = ( + unwrapped.sim.model.geom_size[geom_id].copy().tolist() + ) + elif entry["class_name"] in _REORIENT_CLASS_NAMES: + model = unwrapped.sim.model + target_body_id = model.body_name2id("target") + obj_geom_id = model.geom_name2id("obj") + target_geom_id = model.geom_name2id("target") + top_geom_id = model.geom_name2id("top") + bot_geom_id = model.geom_name2id("bot") + target_top_geom_id = model.geom_name2id("t_top") + target_bot_geom_id = model.geom_name2id("t_bot") + object_body_id = model.body_name2id("Object") + sync["test_target_body_quat"] = ( + model.body_quat[target_body_id].copy().tolist() + ) + sync["test_object_geom_size"] = ( + model.geom_size[obj_geom_id].copy().tolist() + ) + sync["test_target_geom_size"] = ( + model.geom_size[target_geom_id].copy().tolist() + ) + sync["test_object_geom_rgba"] = ( + model.geom_rgba[obj_geom_id].copy().tolist() + ) + sync["test_target_geom_rgba"] = ( + model.geom_rgba[target_geom_id].copy().tolist() + ) + sync["test_object_geom_top_pos"] = ( + model.geom_pos[top_geom_id].copy().tolist() + ) + sync["test_object_geom_bottom_pos"] = ( + model.geom_pos[bot_geom_id].copy().tolist() + ) + sync["test_target_geom_top_pos"] = ( + model.geom_pos[target_top_geom_id].copy().tolist() + ) + sync["test_target_geom_bottom_pos"] = ( + model.geom_pos[target_bot_geom_id].copy().tolist() + ) + sync["test_object_body_mass"] = [float(model.body_mass[object_body_id])] + sync["test_object_geom_type"] = int(model.geom_type[obj_geom_id]) + sync["test_target_geom_type"] = int(model.geom_type[target_geom_id]) + sync["test_object_geom_condim"] = int(model.geom_condim[obj_geom_id]) + sync["test_success_site_rgba"] = ( + model.site_rgba[unwrapped.success_indicator_sid].copy().tolist() + ) + elif entry["class_name"] in {"PenTwirlFixedEnvV0", "PenTwirlRandomEnvV0"}: + target_body_id = unwrapped.sim.model.body_name2id("target") + sync["test_target_body_quat"] = ( + unwrapped.sim.model.body_quat[target_body_id].copy().tolist() + ) + elif entry["class_name"] == "TerrainEnvV0": + terrain_geom_id = unwrapped.sim.model.geom_name2id("terrain") + hfield_id = int(unwrapped.sim.model.geom_dataid[terrain_geom_id]) + nrow = int(unwrapped.sim.model.hfield_nrow[hfield_id]) + ncol = int(unwrapped.sim.model.hfield_ncol[hfield_id]) + adr = int(unwrapped.sim.model.hfield_adr[hfield_id]) + sync["test_hfield_data"] = ( + unwrapped.sim.model + .hfield_data[adr : adr + nrow * ncol] + .copy() + .tolist() + ) + sync["test_terrain_geom_rgba"] = ( + unwrapped.sim.model.geom_rgba[terrain_geom_id].copy().tolist() + ) + sync["test_terrain_geom_pos"] = ( + unwrapped.sim.model.geom_pos[terrain_geom_id].copy().tolist() + ) + sync["test_terrain_geom_contype"] = int( + unwrapped.sim.model.geom_contype[terrain_geom_id] + ) + sync["test_terrain_geom_conaffinity"] = int( + unwrapped.sim.model.geom_conaffinity[terrain_geom_id] + ) + elif entry["class_name"] == "WalkEnvV0": + terrain_geom_id = unwrapped.sim.model.geom_name2id("terrain") + sync["test_terrain_geom_rgba"] = ( + unwrapped.sim.model.geom_rgba[terrain_geom_id].copy().tolist() + ) + sync["test_terrain_geom_pos"] = ( + unwrapped.sim.model.geom_pos[terrain_geom_id].copy().tolist() + ) + sync["test_terrain_geom_contype"] = int( + unwrapped.sim.model.geom_contype[terrain_geom_id] + ) + sync["test_terrain_geom_conaffinity"] = int( + unwrapped.sim.model.geom_conaffinity[terrain_geom_id] + ) + elif entry["class_name"] == "PoseEnvV0": + sync["test_target_qpos"] = _tolist_array(unwrapped.target_jnt_value) + target_site_pos: list[float] = [] + for site_name in entry["kwargs"].get("viz_site_targets", []): + site_id = unwrapped.sim.model.site_name2id(site_name + "_target") + target_site_pos.extend( + unwrapped.sim.model.site_pos[site_id].copy().tolist() + ) + if target_site_pos: + sync["test_target_site_pos"] = target_site_pos + if getattr(unwrapped, "weight_bodyname", None): + body_id = unwrapped.sim.model.body_name2id( + unwrapped.weight_bodyname + ) + geom_id = unwrapped.sim.model.body_geomadr[body_id] + sync["test_body_mass"] = [ + float(unwrapped.sim.model.body_mass[body_id]) + ] + sync["test_geom_size0"] = [ + float(unwrapped.sim.model.geom_size[geom_id][0]) + ] + elif entry["class_name"] == "ReachEnvV0": + target_pos: list[float] = [] + for site_name in entry["kwargs"]["target_reach_range"]: + site_id = unwrapped.sim.model.site_name2id(site_name + "_target") + target_pos.extend( + unwrapped.sim.model.site_pos[site_id].copy().tolist() + ) + sync["test_target_pos"] = target_pos + return np.asarray(obs, dtype=np.float64), sync + + +def _sync_reset_myochallenge( + env: Any, task_id: str +) -> tuple[np.ndarray, dict[str, Any]]: + obs, _ = env.reset() + unwrapped = env.unwrapped + sim = unwrapped.sim + sync: dict[str, Any] = { + "test_reset_qpos": sim.data.qpos.copy().tolist(), + "test_reset_qvel": sim.data.qvel.copy().tolist(), + "test_reset_act": ( + sim.data.act.copy().tolist() if sim.model.na > 0 else [] + ), + "test_reset_qacc_warmstart": sim.data.qacc_warmstart.copy().tolist(), + } + entry = _entry(task_id) + if entry["class_name"] == "ReorientEnvV0": + sync["test_reset_act_dot"] = ( + sim.data.act_dot.copy().tolist() if sim.model.na > 0 else [] + ) + goal_bid = sim.model.body_name2id("target") + target_gid = sim.model.geom_name2id("target_dice") + object_bid = sim.model.body_name2id("Object") + start_geom = sim.model.body_geomadr[object_bid] + geom_count = sim.model.body_geomnum[object_bid] + sync["test_goal_body_pos"] = ( + sim.model.body_pos[goal_bid].copy().tolist() + ) + sync["test_goal_body_quat"] = ( + sim.model.body_quat[goal_bid].copy().tolist() + ) + sync["test_target_geom_size"] = ( + sim.model.geom_size[target_gid].copy().tolist() + ) + sync["test_object_geom_size"] = ( + sim.model + .geom_size[start_geom : start_geom + geom_count] + .reshape(-1) + .tolist() + ) + sync["test_object_geom_pos"] = ( + sim.model + .geom_pos[start_geom : start_geom + geom_count] + .reshape(-1) + .tolist() + ) + sync["test_object_geom_friction"] = ( + sim.model + .geom_friction[start_geom : start_geom + geom_count] + .reshape(-1) + .tolist() + ) + sync["test_object_body_mass"] = [float(sim.model.body_mass[object_bid])] + elif entry["class_name"] == "RelocateEnvV0": + sync["test_reset_act_dot"] = ( + sim.data.act_dot.copy().tolist() if sim.model.na > 0 else [] + ) + sync["test_reset_ctrl"] = sim.data.ctrl.copy().tolist() + goal_bid = sim.model.body_name2id("target") + goal_mocap_id = int(sim.model.body_mocapid[goal_bid]) + object_bid = sim.model.body_name2id("Object") + start_geom = sim.model.body_geomadr[object_bid] + geom_count = sim.model.body_geomnum[object_bid] + sync["test_goal_body_pos"] = ( + sim.model.body_pos[goal_bid].copy().tolist() + ) + sync["test_goal_body_quat"] = ( + sim.model.body_quat[goal_bid].copy().tolist() + ) + if goal_mocap_id >= 0: + sync["test_goal_mocap_pos"] = ( + sim.data.mocap_pos[goal_mocap_id].copy().tolist() + ) + sync["test_goal_mocap_quat"] = ( + sim.data.mocap_quat[goal_mocap_id].copy().tolist() + ) + sync["test_object_body_pos"] = ( + sim.model.body_pos[object_bid].copy().tolist() + ) + sync["test_object_body_mass"] = [float(sim.model.body_mass[object_bid])] + sync["test_object_geom_type"] = ( + sim.model + .geom_type[start_geom : start_geom + geom_count] + .astype(np.float64) + .tolist() + ) + sync["test_object_geom_size"] = ( + sim.model + .geom_size[start_geom : start_geom + geom_count] + .reshape(-1) + .tolist() + ) + sync["test_object_geom_pos"] = ( + sim.model + .geom_pos[start_geom : start_geom + geom_count] + .reshape(-1) + .tolist() + ) + sync["test_object_geom_quat"] = ( + sim.model + .geom_quat[start_geom : start_geom + geom_count] + .reshape(-1) + .tolist() + ) + sync["test_object_geom_rgba"] = ( + sim.model + .geom_rgba[start_geom : start_geom + geom_count] + .reshape(-1) + .tolist() + ) + sync["test_object_geom_friction"] = ( + sim.model + .geom_friction[start_geom : start_geom + geom_count] + .reshape(-1) + .tolist() + ) + elif entry["class_name"] == "BaodingEnvV1": + sync["test_task"] = int(unwrapped.which_task.value) + sync["test_ball1_starting_angle"] = float( + unwrapped.ball_1_starting_angle + ) + sync["test_ball2_starting_angle"] = float( + unwrapped.ball_2_starting_angle + ) + sync["test_x_radius"] = float(unwrapped.x_radius) + sync["test_y_radius"] = float(unwrapped.y_radius) + sync["test_goal_trajectory"] = ( + np.asarray(unwrapped.goal, dtype=np.float64).reshape(-1).tolist() + ) + sync["test_object1_body_mass"] = [ + float(sim.model.body_mass[unwrapped.object1_bid]) + ] + sync["test_object2_body_mass"] = [ + float(sim.model.body_mass[unwrapped.object2_bid]) + ] + sync["test_object1_geom_size"] = ( + sim.model.geom_size[unwrapped.object1_gid].copy().tolist() + ) + sync["test_object2_geom_size"] = ( + sim.model.geom_size[unwrapped.object2_gid].copy().tolist() + ) + sync["test_object1_geom_friction"] = ( + sim.model.geom_friction[unwrapped.object1_gid].copy().tolist() + ) + sync["test_object2_geom_friction"] = ( + sim.model.geom_friction[unwrapped.object2_gid].copy().tolist() + ) + elif entry["class_name"] == "BimanualEnvV1": + sync["test_start_pos"] = np.asarray( + unwrapped.start_pos, dtype=np.float64 + ).tolist() + sync["test_goal_pos"] = np.asarray( + unwrapped.goal_pos, dtype=np.float64 + ).tolist() + sync["test_object_body_mass"] = [ + float(sim.model.body_mass[unwrapped.obj_bid]) + ] + sync["test_object_geom_size"] = ( + sim.model.geom_size[unwrapped.obj_gid].copy().tolist() + ) + sync["test_object_geom_friction"] = ( + sim.model.geom_friction[unwrapped.obj_gid].copy().tolist() + ) + obs = np.asarray(unwrapped.get_obs(), dtype=np.float64) + elif entry["class_name"] == "RunTrack": + terrain_geom_id = sim.model.geom_name2id("terrain") + hfield_id = int(sim.model.geom_dataid[terrain_geom_id]) + nrow = int(sim.model.hfield_nrow[hfield_id]) + ncol = int(sim.model.hfield_ncol[hfield_id]) + adr = int(sim.model.hfield_adr[hfield_id]) + sync["test_hfield_data"] = ( + sim.model.hfield_data[adr : adr + nrow * ncol].copy().tolist() + ) + sync["test_terrain_geom_rgba"] = ( + sim.model.geom_rgba[terrain_geom_id].copy().tolist() + ) + sync["test_terrain_geom_pos"] = ( + sim.model.geom_pos[terrain_geom_id].copy().tolist() + ) + sync["test_terrain_type"] = int( + np.asarray(unwrapped.terrain_type).reshape(-1)[0] + ) + sync["test_osl_state"] = _runtrack_osl_state_id( + unwrapped.OSL_CTRL.STATE_MACHINE.current_state.get_name() + ) + obs = np.asarray(unwrapped.get_obs(), dtype=np.float64) + elif entry["class_name"] == "SoccerEnvV0": + goalkeeper = unwrapped.goalkeeper + sync["test_goalkeeper_pose"] = np.asarray( + goalkeeper.get_goalkeeper_pose(), dtype=np.float64 + ).tolist() + sync["test_goalkeeper_velocity"] = np.asarray( + goalkeeper.goalkeeper_vel, dtype=np.float64 + ).tolist() + sync["test_goalkeeper_noise_buffer"] = ( + np + .asarray(goalkeeper.noise_process.buffer, dtype=np.float64) + .reshape(-1) + .tolist() + ) + sync["test_goalkeeper_noise_idx"] = int(goalkeeper.noise_process.idx) + sync["test_goalkeeper_block_velocity"] = float( + goalkeeper.block_velocity + ) + sync["test_goalkeeper_policy"] = _soccer_policy_id( + goalkeeper.goalkeeper_policy + ) + # Soccer mutates the live goalkeeper after `reset()` has already built + # the returned observation, so align against the post-reset live state. + obs = np.asarray(unwrapped.get_obs(), dtype=np.float64) + elif entry["class_name"] == "ChaseTagEnvV0": + opponent = unwrapped.opponent + terrain_geom_id = sim.model.geom_name2id("terrain") + hfield_id = int(sim.model.geom_dataid[terrain_geom_id]) + sync["test_task"] = int(unwrapped.current_task.value) + sync["test_hfield_data"] = ( + sim.model + .hfield_data[ + int(sim.model.hfield_adr[hfield_id]) : int( + sim.model.hfield_adr[hfield_id] + ) + + int(sim.model.hfield_nrow[hfield_id]) + * int(sim.model.hfield_ncol[hfield_id]) + ] + .copy() + .tolist() + ) + sync["test_terrain_geom_rgba"] = ( + sim.model.geom_rgba[terrain_geom_id].copy().tolist() + ) + sync["test_terrain_geom_pos"] = ( + sim.model.geom_pos[terrain_geom_id].copy().tolist() + ) + sync["test_opponent_pose"] = np.asarray( + opponent.get_opponent_pose(), dtype=np.float64 + ).tolist() + sync["test_opponent_velocity"] = np.asarray( + opponent.opponent_vel, dtype=np.float64 + ).tolist() + sync["test_opponent_noise_buffer"] = ( + np + .asarray(opponent.noise_process.buffer, dtype=np.float64) + .reshape(-1) + .tolist() + ) + sync["test_opponent_noise_idx"] = int(opponent.noise_process.idx) + sync["test_chase_velocity"] = float(opponent.chase_velocity) + sync["test_opponent_policy"] = _chasetag_policy_id( + opponent.opponent_policy + ) + elif entry["class_name"] not in { + "RunTrack", + "SoccerEnvV0", + "ChaseTagEnvV0", + "TableTennisEnvV0", + }: + raise ValueError( + f"Unsupported MyoChallenge render sync task: {task_id}" + ) + elif entry["class_name"] == "TableTennisEnvV0": + sync["test_ball_body_pos"] = ( + sim.model.body_pos[unwrapped.id_info.ball_bid].copy().tolist() + ) + sync["test_ball_geom_friction"] = ( + sim.model.geom_friction[unwrapped.id_info.ball_gid].copy().tolist() + ) + sync["test_paddle_body_mass"] = [ + float(sim.model.body_mass[unwrapped.id_info.paddle_bid]) + ] + sync["test_init_qpos"] = np.asarray( + unwrapped.init_qpos, dtype=np.float64 + ).tolist() + sync["test_init_qvel"] = np.asarray( + unwrapped.init_qvel, dtype=np.float64 + ).tolist() + sync["test_reset_act_dot"] = ( + sim.data.act_dot.copy().tolist() if sim.model.na > 0 else [] + ) + return np.asarray(obs, dtype=np.float64), sync + + +def _sync_reset_myodm(env: Any) -> tuple[np.ndarray, dict[str, Any]]: + obs, _ = env.reset() + unwrapped = env.unwrapped + import mujoco + + integration_state = unwrapped.sim.sim.get_state( + int(mujoco.mjtState.mjSTATE_INTEGRATION) + ) + return np.asarray(obs, dtype=np.float64), { + "test_reset_qpos": unwrapped.sim.data.qpos.copy().tolist(), + "test_reset_qvel": unwrapped.sim.data.qvel.copy().tolist(), + "test_reset_ctrl": unwrapped.sim.data.ctrl.copy().tolist(), + "test_reset_act": ( + unwrapped.sim.data.act.copy().tolist() + if unwrapped.sim.model.na > 0 + else [] + ), + "test_reset_act_dot": ( + unwrapped.sim.data.act_dot.copy().tolist() + if unwrapped.sim.model.na > 0 + else [] + ), + "test_reset_qacc_warmstart": ( + unwrapped.sim.data.qacc_warmstart.copy().tolist() + ), + "test_reset_integration_state": integration_state.tolist(), + } + + +def _oracle_reset_sync( + env: Any, task_id: str +) -> tuple[np.ndarray, dict[str, Any]]: + suite = _entry(task_id)["suite"] + if suite == "myobase": + return _sync_reset_myobase(env, task_id) + if suite == "myochallenge": + return _sync_reset_myochallenge(env, task_id) + if suite == "myodm": + return _sync_reset_myodm(env) + raise ValueError(f"Unsupported MyoSuite suite for render sync: {suite}") + + +@contextmanager +def _record_track_reference_samples( + env: Any, +) -> Iterator[list[_TrackReferenceSample]]: + unwrapped = _unwrapped_env(env) + ref = getattr(unwrapped, "ref", None) + if ref is None or not hasattr(ref, "get_reference"): + yield [] + return + original_get_reference = ref.get_reference + samples: list[_TrackReferenceSample] = [] + + def wrapped_get_reference(time: Any) -> Any: + reference = original_get_reference(time) + samples.append( + _TrackReferenceSample( + time=float(time), + robot=np.asarray(reference.robot, dtype=np.float64).copy(), + robot_vel=( + None + if reference.robot_vel is None + else np.asarray( + reference.robot_vel, dtype=np.float64 + ).copy() + ), + object=np.asarray(reference.object, dtype=np.float64).copy(), + ) + ) + return reference + + ref.get_reference = wrapped_get_reference + try: + yield samples + finally: + ref.get_reference = original_get_reference + + +def _track_reference_sync( + samples: list[_TrackReferenceSample], +) -> dict[str, Any]: + if not samples: + return {} + has_robot_vel = samples[0].robot_vel is not None + return { + "test_reference_time": [round(sample.time, 4) for sample in samples], + "test_reference_robot": np.concatenate([ + sample.robot for sample in samples + ]).tolist(), + "test_reference_robot_vel": ( + [] + if not has_robot_vel + else np.concatenate([ + cast(np.ndarray, sample.robot_vel) for sample in samples + ]).tolist() + ), + "test_reference_object": np.concatenate([ + sample.object for sample in samples + ]).tolist(), + } + + +def _make_render_envpool_env( + task_id: str, + *, + seed: int, + render_width: int, + render_height: int, + camera_id: int, + sync: dict[str, Any], +) -> Any: + return make_gymnasium( + task_id, + num_envs=1, + seed=seed, + render_mode="rgb_array", + render_width=render_width, + render_height=render_height, + render_camera_id=camera_id, + **sync, + ) + + +def _capture_render_sequence_once( + task_id: str, + *, + steps: int, + seed: int, + render_width: int, + render_height: int, + camera_id: int, + action_mode: str, +) -> RenderSequence: + prefer_physics_render = False + with _make_oracle( + task_id, + seed=seed, + render_width=render_width, + render_height=render_height, + camera_id=camera_id, + ) as oracle: + suite = _entry(task_id)["suite"] + with _record_track_reference_samples(oracle) as track_samples: + _, sync = _oracle_reset_sync(oracle, task_id) + reset_official = _render_official_array( + oracle, + width=render_width, + height=render_height, + camera_id=camera_id, + prefer_physics_render=prefer_physics_render, + ).copy() + action_shape = _action_shape(oracle) + if action_mode == "random": + actions = _seeded_actions(action_shape, steps, seed + 97) + elif action_mode == "hold": + actions = _hold_actions(oracle, sync, steps) + elif action_mode == "playback": + actions = _zero_actions(action_shape, steps) + elif action_mode == "zero": + actions = _zero_actions(action_shape, steps) + else: + raise ValueError( + f"Unsupported render action mode: {action_mode}" + ) + official_frames: list[np.ndarray] = [] + step_outcomes: list[tuple[bool, bool]] = [] + if suite == "myodm": + if action_mode == "playback": + sync["test_playback_reference"] = True + for _ in range(steps): + _unwrapped_env(oracle).playback() + step_outcomes.append((False, False)) + official_frames.append( + _render_official_array( + oracle, + width=render_width, + height=render_height, + camera_id=camera_id, + prefer_physics_render=prefer_physics_render, + ).copy() + ) + else: + for action in actions: + _, _, terminated0, truncated0, _ = oracle.step( + action[0] + ) + step_outcomes.append(( + bool(terminated0), + bool(truncated0), + )) + official_frames.append( + _render_official_array( + oracle, + width=render_width, + height=render_height, + camera_id=camera_id, + prefer_physics_render=prefer_physics_render, + ).copy() + ) + if task_id not in _MYODM_FIXED_TASK_IDS: + sync.update(_track_reference_sync(track_samples)) + + env = _make_render_envpool_env( + task_id, + seed=seed, + render_width=render_width, + render_height=render_height, + camera_id=camera_id, + sync=sync, + ) + try: + env.reset() + reset_envpool = _render_envpool_array(env)[0].copy() + envpool_frames: list[np.ndarray] = [] + if suite == "myodm": + for step_index, ( + action, + (terminated0, truncated0), + ) in enumerate( + zip(actions, step_outcomes, strict=True), + start=1, + ): + _, _, terminated1, truncated1, _ = env.step(action) + if terminated0 != bool(terminated1[0]): + raise AssertionError( + f"{task_id} terminated mismatch at render step " + f"{step_index}" + ) + if truncated0 != bool(truncated1[0]): + raise AssertionError( + f"{task_id} truncated mismatch at render step " + f"{step_index}" + ) + if action_mode != "playback" and ( + step_index < steps and (terminated0 or truncated0) + ): + raise _RenderEarlyTerminationError( + task_id, seed, step_index + ) + envpool_frames.append(_render_envpool_array(env)[0].copy()) + return RenderSequence( + reset_envpool=reset_envpool, + reset_official=reset_official, + envpool_frames=tuple(envpool_frames), + official_frames=tuple(official_frames), + ) + + official_frames = [] + for step_index, action in enumerate(actions, start=1): + _, _, terminated0, truncated0, _ = oracle.step(action[0]) + _, _, terminated1, truncated1, _ = env.step(action) + if bool(terminated0) != bool(terminated1[0]): + raise AssertionError( + f"{task_id} terminated mismatch at render step " + f"{step_index}" + ) + if bool(truncated0) != bool(truncated1[0]): + raise AssertionError( + f"{task_id} truncated mismatch at render step " + f"{step_index}" + ) + if step_index < steps and (terminated0 or truncated0): + raise _RenderEarlyTerminationError( + task_id, seed, step_index + ) + envpool_frames.append(_render_envpool_array(env)[0].copy()) + official_frames.append( + _render_official_array( + oracle, + width=render_width, + height=render_height, + camera_id=camera_id, + prefer_physics_render=prefer_physics_render, + ).copy() + ) + return RenderSequence( + reset_envpool=reset_envpool, + reset_official=reset_official, + envpool_frames=tuple(envpool_frames), + official_frames=tuple(official_frames), + ) + finally: + env.close() + gc.collect() + + +def capture_render_sequence( + task_id: str, + *, + steps: int = MYOSUITE_RENDER_COMPARE_STEPS, + seed: int = 7, + render_width: int = 192, + render_height: int = 192, + camera_id: int = -1, + action_mode: str = "random", + retry_seeds: tuple[int, ...] = (), +) -> RenderSequence: + """Capture reset plus a short EnvPool-oracle render rollout.""" + last_early_termination: _RenderEarlyTerminationError | None = None + action_modes: tuple[str, ...] = (action_mode,) + if action_mode == "random": + if _entry(task_id)["suite"] == "myodm": + action_modes += ("hold", "zero", "playback") + else: + action_modes += ("zero",) + for candidate_action_mode in action_modes: + for candidate_seed in (seed, *retry_seeds): + try: + return _capture_render_sequence_once( + task_id, + steps=steps, + seed=candidate_seed, + render_width=render_width, + render_height=render_height, + camera_id=camera_id, + action_mode=candidate_action_mode, + ) + except _RenderEarlyTerminationError as exc: + last_early_termination = exc + continue + if last_early_termination is not None: + raise RuntimeError( + f"{task_id} terminated before {steps} render steps for all " + f"candidate action modes {action_modes} and seeds " + f"{(seed, *retry_seeds)}" + ) from last_early_termination + raise RuntimeError( + "capture_render_sequence exhausted without attempting a seed" + ) diff --git a/envpool/mujoco/myosuite_envpool.cc b/envpool/mujoco/myosuite_envpool.cc new file mode 100644 index 000000000..8d6a95443 --- /dev/null +++ b/envpool/mujoco/myosuite_envpool.cc @@ -0,0 +1,29 @@ +// Copyright 2026 Garena Online Private 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 "envpool/core/py_envpool.h" + +namespace myosuite_envpool { + +void RegisterMyoSuiteBaseBindings(py::module_& module); +void RegisterMyoSuiteChallengeBindings(py::module_& module); +void RegisterMyoSuiteDmBindings(py::module_& module); + +} // namespace myosuite_envpool + +PYBIND11_MODULE(myosuite_envpool, m) { + myosuite_envpool::RegisterMyoSuiteBaseBindings(m); + myosuite_envpool::RegisterMyoSuiteChallengeBindings(m); + myosuite_envpool::RegisterMyoSuiteDmBindings(m); +} diff --git a/envpool/mujoco/myosuite_envpool_base.cc b/envpool/mujoco/myosuite_envpool_base.cc new file mode 100644 index 000000000..308560322 --- /dev/null +++ b/envpool/mujoco/myosuite_envpool_base.cc @@ -0,0 +1,123 @@ +// Copyright 2026 Garena Online Private 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 "envpool/core/py_envpool.h" +#include "envpool/mujoco/myosuite/myobase.h" +#include "envpool/mujoco/myosuite/myobase_extended.h" + +namespace myosuite_envpool { + +void RegisterMyoSuiteBaseBindings(py::module_& module) { + using MyoSuitePoseEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuitePoseEnvSpec>; + using MyoSuitePoseEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuitePoseEnvPool>; + using MyoSuitePosePixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuitePosePixelEnvSpec>; + using MyoSuitePosePixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuitePosePixelEnvPool>; + + using MyoSuiteReachEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuiteReachEnvSpec>; + using MyoSuiteReachEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuiteReachEnvPool>; + using MyoSuiteReachPixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuiteReachPixelEnvSpec>; + using MyoSuiteReachPixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuiteReachPixelEnvPool>; + + using MyoSuiteKeyTurnEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuiteKeyTurnEnvSpec>; + using MyoSuiteKeyTurnEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuiteKeyTurnEnvPool>; + using MyoSuiteKeyTurnPixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuiteKeyTurnPixelEnvSpec>; + using MyoSuiteKeyTurnPixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuiteKeyTurnPixelEnvPool>; + + using MyoSuiteObjHoldEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuiteObjHoldEnvSpec>; + using MyoSuiteObjHoldEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuiteObjHoldEnvPool>; + using MyoSuiteObjHoldPixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuiteObjHoldPixelEnvSpec>; + using MyoSuiteObjHoldPixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuiteObjHoldPixelEnvPool>; + + using MyoSuiteTorsoEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuiteTorsoEnvSpec>; + using MyoSuiteTorsoEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuiteTorsoEnvPool>; + using MyoSuiteTorsoPixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuiteTorsoPixelEnvSpec>; + using MyoSuiteTorsoPixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuiteTorsoPixelEnvPool>; + + using MyoSuitePenTwirlEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuitePenTwirlEnvSpec>; + using MyoSuitePenTwirlEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuitePenTwirlEnvPool>; + using MyoSuitePenTwirlPixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuitePenTwirlPixelEnvSpec>; + using MyoSuitePenTwirlPixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuitePenTwirlPixelEnvPool>; + + using MyoSuiteReorientEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuiteReorientEnvSpec>; + using MyoSuiteReorientEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuiteReorientEnvPool>; + using MyoSuiteReorientPixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuiteReorientPixelEnvSpec>; + using MyoSuiteReorientPixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuiteReorientPixelEnvPool>; + + using MyoSuiteWalkEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuiteWalkEnvSpec>; + using MyoSuiteWalkEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuiteWalkEnvPool>; + using MyoSuiteWalkPixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuiteWalkPixelEnvSpec>; + using MyoSuiteWalkPixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuiteWalkPixelEnvPool>; + + using MyoSuiteTerrainEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuiteTerrainEnvSpec>; + using MyoSuiteTerrainEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuiteTerrainEnvPool>; + using MyoSuiteTerrainPixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoSuiteTerrainPixelEnvSpec>; + using MyoSuiteTerrainPixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoSuiteTerrainPixelEnvPool>; + + REGISTER(module, MyoSuitePoseEnvSpec, MyoSuitePoseEnvPool) + REGISTER(module, MyoSuitePosePixelEnvSpec, MyoSuitePosePixelEnvPool) + REGISTER(module, MyoSuiteReachEnvSpec, MyoSuiteReachEnvPool) + REGISTER(module, MyoSuiteReachPixelEnvSpec, MyoSuiteReachPixelEnvPool) + REGISTER(module, MyoSuiteKeyTurnEnvSpec, MyoSuiteKeyTurnEnvPool) + REGISTER(module, MyoSuiteKeyTurnPixelEnvSpec, MyoSuiteKeyTurnPixelEnvPool) + REGISTER(module, MyoSuiteObjHoldEnvSpec, MyoSuiteObjHoldEnvPool) + REGISTER(module, MyoSuiteObjHoldPixelEnvSpec, MyoSuiteObjHoldPixelEnvPool) + REGISTER(module, MyoSuiteTorsoEnvSpec, MyoSuiteTorsoEnvPool) + REGISTER(module, MyoSuiteTorsoPixelEnvSpec, MyoSuiteTorsoPixelEnvPool) + REGISTER(module, MyoSuitePenTwirlEnvSpec, MyoSuitePenTwirlEnvPool) + REGISTER(module, MyoSuitePenTwirlPixelEnvSpec, MyoSuitePenTwirlPixelEnvPool) + REGISTER(module, MyoSuiteReorientEnvSpec, MyoSuiteReorientEnvPool) + REGISTER(module, MyoSuiteReorientPixelEnvSpec, MyoSuiteReorientPixelEnvPool) + REGISTER(module, MyoSuiteWalkEnvSpec, MyoSuiteWalkEnvPool) + REGISTER(module, MyoSuiteWalkPixelEnvSpec, MyoSuiteWalkPixelEnvPool) + REGISTER(module, MyoSuiteTerrainEnvSpec, MyoSuiteTerrainEnvPool) + REGISTER(module, MyoSuiteTerrainPixelEnvSpec, MyoSuiteTerrainPixelEnvPool) +} + +} // namespace myosuite_envpool diff --git a/envpool/mujoco/myosuite_envpool_challenge.cc b/envpool/mujoco/myosuite_envpool_challenge.cc new file mode 100644 index 000000000..838e5d6d4 --- /dev/null +++ b/envpool/mujoco/myosuite_envpool_challenge.cc @@ -0,0 +1,121 @@ +// Copyright 2026 Garena Online Private 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 "envpool/core/py_envpool.h" +#include "envpool/mujoco/myosuite/myochallenge.h" +#include "envpool/mujoco/myosuite/myochallenge_extended.h" + +namespace myosuite_envpool { + +void RegisterMyoSuiteChallengeBindings(py::module_& module) { + using MyoChallengeReorientEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoChallengeReorientEnvSpec>; + using MyoChallengeReorientEnvPool = + PyEnvPool<::myosuite_envpool::MyoChallengeReorientEnvPool>; + using MyoChallengeReorientPixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoChallengeReorientPixelEnvSpec>; + using MyoChallengeReorientPixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoChallengeReorientPixelEnvPool>; + + using MyoChallengeRelocateEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoChallengeRelocateEnvSpec>; + using MyoChallengeRelocateEnvPool = + PyEnvPool<::myosuite_envpool::MyoChallengeRelocateEnvPool>; + using MyoChallengeRelocatePixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoChallengeRelocatePixelEnvSpec>; + using MyoChallengeRelocatePixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoChallengeRelocatePixelEnvPool>; + + using MyoChallengeBaodingEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoChallengeBaodingEnvSpec>; + using MyoChallengeBaodingEnvPool = + PyEnvPool<::myosuite_envpool::MyoChallengeBaodingEnvPool>; + using MyoChallengeBaodingPixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoChallengeBaodingPixelEnvSpec>; + using MyoChallengeBaodingPixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoChallengeBaodingPixelEnvPool>; + + using MyoChallengeBimanualEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoChallengeBimanualEnvSpec>; + using MyoChallengeBimanualEnvPool = + PyEnvPool<::myosuite_envpool::MyoChallengeBimanualEnvPool>; + using MyoChallengeBimanualPixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoChallengeBimanualPixelEnvSpec>; + using MyoChallengeBimanualPixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoChallengeBimanualPixelEnvPool>; + + using MyoChallengeRunTrackEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoChallengeRunTrackEnvSpec>; + using MyoChallengeRunTrackEnvPool = + PyEnvPool<::myosuite_envpool::MyoChallengeRunTrackEnvPool>; + using MyoChallengeRunTrackPixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoChallengeRunTrackPixelEnvSpec>; + using MyoChallengeRunTrackPixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoChallengeRunTrackPixelEnvPool>; + + using MyoChallengeSoccerEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoChallengeSoccerEnvSpec>; + using MyoChallengeSoccerEnvPool = + PyEnvPool<::myosuite_envpool::MyoChallengeSoccerEnvPool>; + using MyoChallengeSoccerPixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoChallengeSoccerPixelEnvSpec>; + using MyoChallengeSoccerPixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoChallengeSoccerPixelEnvPool>; + + using MyoChallengeChaseTagEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoChallengeChaseTagEnvSpec>; + using MyoChallengeChaseTagEnvPool = + PyEnvPool<::myosuite_envpool::MyoChallengeChaseTagEnvPool>; + using MyoChallengeChaseTagPixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoChallengeChaseTagPixelEnvSpec>; + using MyoChallengeChaseTagPixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoChallengeChaseTagPixelEnvPool>; + + using MyoChallengeTableTennisEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoChallengeTableTennisEnvSpec>; + using MyoChallengeTableTennisEnvPool = + PyEnvPool<::myosuite_envpool::MyoChallengeTableTennisEnvPool>; + using MyoChallengeTableTennisPixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoChallengeTableTennisPixelEnvSpec>; + using MyoChallengeTableTennisPixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoChallengeTableTennisPixelEnvPool>; + + REGISTER(module, MyoChallengeReorientEnvSpec, MyoChallengeReorientEnvPool) + REGISTER(module, MyoChallengeReorientPixelEnvSpec, + MyoChallengeReorientPixelEnvPool) + REGISTER(module, MyoChallengeRelocateEnvSpec, MyoChallengeRelocateEnvPool) + REGISTER(module, MyoChallengeRelocatePixelEnvSpec, + MyoChallengeRelocatePixelEnvPool) + REGISTER(module, MyoChallengeBaodingEnvSpec, MyoChallengeBaodingEnvPool) + REGISTER(module, MyoChallengeBaodingPixelEnvSpec, + MyoChallengeBaodingPixelEnvPool) + REGISTER(module, MyoChallengeBimanualEnvSpec, MyoChallengeBimanualEnvPool) + REGISTER(module, MyoChallengeBimanualPixelEnvSpec, + MyoChallengeBimanualPixelEnvPool) + REGISTER(module, MyoChallengeRunTrackEnvSpec, MyoChallengeRunTrackEnvPool) + REGISTER(module, MyoChallengeRunTrackPixelEnvSpec, + MyoChallengeRunTrackPixelEnvPool) + REGISTER(module, MyoChallengeSoccerEnvSpec, MyoChallengeSoccerEnvPool) + REGISTER(module, MyoChallengeSoccerPixelEnvSpec, + MyoChallengeSoccerPixelEnvPool) + REGISTER(module, MyoChallengeChaseTagEnvSpec, MyoChallengeChaseTagEnvPool) + REGISTER(module, MyoChallengeChaseTagPixelEnvSpec, + MyoChallengeChaseTagPixelEnvPool) + REGISTER(module, MyoChallengeTableTennisEnvSpec, + MyoChallengeTableTennisEnvPool) + REGISTER(module, MyoChallengeTableTennisPixelEnvSpec, + MyoChallengeTableTennisPixelEnvPool) +} + +} // namespace myosuite_envpool diff --git a/envpool/mujoco/myosuite_envpool_dm.cc b/envpool/mujoco/myosuite_envpool_dm.cc new file mode 100644 index 000000000..1d02e7d78 --- /dev/null +++ b/envpool/mujoco/myosuite_envpool_dm.cc @@ -0,0 +1,32 @@ +// Copyright 2026 Garena Online Private 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 "envpool/core/py_envpool.h" +#include "envpool/mujoco/myosuite/myodm.h" + +namespace myosuite_envpool { + +void RegisterMyoSuiteDmBindings(py::module_& module) { + using MyoDMTrackEnvSpec = PyEnvSpec<::myosuite_envpool::MyoDMTrackEnvSpec>; + using MyoDMTrackEnvPool = PyEnvPool<::myosuite_envpool::MyoDMTrackEnvPool>; + using MyoDMTrackPixelEnvSpec = + PyEnvSpec<::myosuite_envpool::MyoDMTrackPixelEnvSpec>; + using MyoDMTrackPixelEnvPool = + PyEnvPool<::myosuite_envpool::MyoDMTrackPixelEnvPool>; + + REGISTER(module, MyoDMTrackEnvSpec, MyoDMTrackEnvPool) + REGISTER(module, MyoDMTrackPixelEnvSpec, MyoDMTrackPixelEnvPool) +} + +} // namespace myosuite_envpool diff --git a/envpool/mujoco/offscreen_renderer.cc b/envpool/mujoco/offscreen_renderer.cc index 7a6cecec1..cb1b48a5d 100644 --- a/envpool/mujoco/offscreen_renderer.cc +++ b/envpool/mujoco/offscreen_renderer.cc @@ -27,6 +27,7 @@ #include #if defined(__APPLE__) && __has_include() +#define GL_SILENCE_DEPRECATION #include #define ENVPOOL_HAS_CGL 1 #elif defined(_WIN32) && __has_include() @@ -68,7 +69,7 @@ mjtNum MedianGeomPosition(const mjData* data, int ngeom, int axis) { class CglContext final : public GlContext { public: CglContext() { - const std::array attribs = { + const std::array attribs = { kCGLPFAOpenGLProfile, static_cast(kCGLOGLPVersion_Legacy), kCGLPFAColorSize, @@ -80,7 +81,6 @@ class CglContext final : public GlContext { kCGLPFAStencilSize, static_cast(8), kCGLPFAAllowOfflineRenderers, - static_cast(0), // value static_cast(0), // terminator }; GLint npix = 0; @@ -298,9 +298,6 @@ class EglContext final : public GlContext { EGL_OPENGL_BIT, EGL_NONE, }; - const std::array pbuffer_attribs = { - EGL_WIDTH, 1, EGL_HEIGHT, 1, EGL_NONE, - }; display_ = CreateDisplay(); if (display_ == EGL_NO_DISPLAY) { @@ -321,17 +318,8 @@ class EglContext final : public GlContext { display_ = EGL_NO_DISPLAY; throw std::runtime_error("failed to bind EGL OpenGL API"); } - surface_ = - eglCreatePbufferSurface(display_, config, pbuffer_attribs.data()); - if (surface_ == EGL_NO_SURFACE) { - eglTerminate(display_); - display_ = EGL_NO_DISPLAY; - throw std::runtime_error("failed to create EGL pbuffer surface"); - } context_ = eglCreateContext(display_, config, EGL_NO_CONTEXT, nullptr); if (context_ == EGL_NO_CONTEXT) { - eglDestroySurface(display_, surface_); - surface_ = EGL_NO_SURFACE; eglTerminate(display_); display_ = EGL_NO_DISPLAY; throw std::runtime_error("failed to create EGL context"); @@ -344,16 +332,14 @@ class EglContext final : public GlContext { if (context_ != EGL_NO_CONTEXT) { eglDestroyContext(display_, context_); } - if (surface_ != EGL_NO_SURFACE) { - eglDestroySurface(display_, surface_); - } eglTerminate(display_); eglReleaseThread(); } } void MakeCurrent() override { - if (eglMakeCurrent(display_, surface_, surface_, context_) != EGL_TRUE) { + if (eglMakeCurrent(display_, EGL_NO_SURFACE, EGL_NO_SURFACE, context_) != + EGL_TRUE) { throw std::runtime_error("failed to make EGL context current"); } } @@ -363,7 +349,6 @@ class EglContext final : public GlContext { EGL_NO_CONTEXT) != EGL_TRUE) { throw std::runtime_error("failed to clear EGL context"); } - eglReleaseThread(); } private: @@ -452,16 +437,14 @@ class EglContext final : public GlContext { EGLDisplay display_{EGL_NO_DISPLAY}; EGLContext context_{EGL_NO_CONTEXT}; - EGLSurface surface_{EGL_NO_SURFACE}; }; #endif std::shared_ptr CreateGlContext() { #if defined(ENVPOOL_HAS_CGL) - // Match Gymnasium's CGL lifecycle: create a context per renderer/viewer. - // Reusing one CGL context across different MuJoCo models can leave renderer - // state behind on macOS software/offline renderers. + // Use a dedicated offline CGL context per renderer so macOS CI can render + // without a hardware-accelerated pixel format. return std::make_shared(); #elif defined(ENVPOOL_HAS_WGL) if (wglGetCurrentContext() != nullptr && wglGetCurrentDC() != nullptr) { @@ -508,6 +491,12 @@ void OffscreenRenderer::Initialize(const mjModel* model) { mjv_makeScene(model, &scene_, 10000); mjr_makeContext(model, &context_, mjFONTSCALE_150); mjr_setBuffer(mjFB_OFFSCREEN, &context_); + if (camera_policy_ == CameraPolicy::kDmControl) { + context_.readDepthMap = mjDEPTH_ZEROFAR; +#if defined(__APPLE__) + needs_render_warmup_ = true; +#endif + } initialized_ = true; } @@ -517,6 +506,10 @@ void OffscreenRenderer::UpdateCamera(const mjModel* model, const mjData* data, if (camera_id < -1 || camera_id >= model->ncam) { throw std::out_of_range("camera_id is out of range"); } + if (camera_policy_ == CameraPolicy::kDmControl) { + camera_ = mjvCamera{}; + mjv_defaultCamera(&camera_); + } if (camera_id == -1 && camera_override != nullptr) { camera_ = *camera_override; camera_.type = mjCAMERA_FREE; @@ -540,34 +533,56 @@ void OffscreenRenderer::UpdateCamera(const mjModel* model, const mjData* data, free_camera_initialized_ = true; } } else if (camera_id == -1) { - camera_.type = mjCAMERA_FREE; camera_.fixedcamid = -1; - if (!free_camera_initialized_) { - mjv_defaultFreeCamera(model, &camera_); - free_camera_initialized_ = true; - } + camera_.type = mjCAMERA_FREE; + mjv_defaultFreeCamera(model, &camera_); + free_camera_initialized_ = true; } else { - camera_.type = mjCAMERA_FIXED; camera_.fixedcamid = camera_id; + camera_.type = mjCAMERA_FIXED; } } void OffscreenRenderer::Render(const mjModel* model, mjData* data, int width, int height, int camera_id, unsigned char* rgb, - const mjvCamera* camera_override) { + const mjvCamera* camera_override, + const mjvOption* option_override) { if (!initialized_) { Initialize(model); } gl_context_->MakeCurrent(); - if (context_.offWidth != width || context_.offHeight != height) { + if (camera_policy_ == CameraPolicy::kDmControl && + (width > context_.offWidth || height > context_.offHeight)) { + // Match mujoco.Renderer: render into the model-configured offscreen + // framebuffer and only grow it when callers exceed the existing capacity. + mjr_resizeOffscreen(std::max(width, context_.offWidth), + std::max(height, context_.offHeight), &context_); + } else if (camera_policy_ == CameraPolicy::kGymLike && + (context_.offWidth != width || context_.offHeight != height)) { mjr_resizeOffscreen(width, height, &context_); } mjr_setBuffer(mjFB_OFFSCREEN, &context_); + if (camera_policy_ == CameraPolicy::kDmControl) { + for (int hfield_id = 0; hfield_id < model->nhfield; ++hfield_id) { + mjr_uploadHField(model, &context_, hfield_id); + } + } + if (option_override != nullptr) { + option_ = *option_override; + } UpdateCamera(model, data, camera_id, camera_override); mjrRect viewport = {0, 0, width, height}; - mjv_updateScene(model, data, &option_, &perturb_, &camera_, mjCAT_ALL, - &scene_); + const mjvPerturb* perturb = + camera_policy_ == CameraPolicy::kDmControl ? nullptr : &perturb_; + mjv_updateScene(model, data, &option_, perturb, &camera_, mjCAT_ALL, &scene_); + if (needs_render_warmup_) { + // macOS CGL can return a one-time, one-LSB difference on the first + // offscreen render after a fresh mjrContext. Draw once before the public + // readback so reset-frame comparisons exercise the stable renderer state. + mjr_render(viewport, &scene_, &context_); + needs_render_warmup_ = false; + } mjr_render(viewport, &scene_, &context_); std::size_t frame_bytes = @@ -584,7 +599,9 @@ void OffscreenRenderer::Render(const mjModel* model, mjData* data, int width, scratch_.data() + static_cast(height - 1 - y) * row_bytes; std::memcpy(rgb + static_cast(y) * row_bytes, src, row_bytes); } - gl_context_->ClearCurrent(); + if (camera_policy_ == CameraPolicy::kGymLike) { + gl_context_->ClearCurrent(); + } } } // namespace envpool::mujoco diff --git a/envpool/mujoco/offscreen_renderer.h b/envpool/mujoco/offscreen_renderer.h index 7b35d409e..a2691e86f 100644 --- a/envpool/mujoco/offscreen_renderer.h +++ b/envpool/mujoco/offscreen_renderer.h @@ -48,7 +48,8 @@ class OffscreenRenderer { void Render(const mjModel* model, mjData* data, int width, int height, int camera_id, unsigned char* rgb, - const mjvCamera* camera_override = nullptr); + const mjvCamera* camera_override = nullptr, + const mjvOption* option_override = nullptr); private: void Initialize(const mjModel* model); @@ -65,6 +66,7 @@ class OffscreenRenderer { CameraPolicy camera_policy_; bool initialized_{false}; bool free_camera_initialized_{false}; + bool needs_render_warmup_{false}; }; } // namespace envpool::mujoco diff --git a/envpool/mujoco/robotics/mujoco_env.h b/envpool/mujoco/robotics/mujoco_env.h index d2d46121c..09970a6b5 100644 --- a/envpool/mujoco/robotics/mujoco_env.h +++ b/envpool/mujoco/robotics/mujoco_env.h @@ -113,29 +113,32 @@ class MujocoRobotEnv : public RenderableEnv { // Native pixel observations are rendered on worker threads, while env // teardown happens on the Python thread. Recreating the renderer on // Windows avoids cross-thread WGL resource lifetime issues. - envpool::mujoco::OffscreenRenderer renderer( - envpool::mujoco::CameraPolicy::kGymLike); + envpool::mujoco::OffscreenRenderer renderer(RenderCameraPolicy()); #else if (renderer_ == nullptr) { renderer_ = std::make_unique( - envpool::mujoco::CameraPolicy::kGymLike); + RenderCameraPolicy()); } #endif mjvCamera camera_override; InitializeRenderCamera(&camera_override); + mjvOption option_override; + InitializeRenderOption(&option_override); if (RenderCamera(&camera_override)) { #ifdef _WIN32 renderer.Render(model_, data_, width, height, camera_id, rgb, - &camera_override); + &camera_override, &option_override); #else renderer_->Render(model_, data_, width, height, camera_id, rgb, - &camera_override); + &camera_override, &option_override); #endif } else { #ifdef _WIN32 - renderer.Render(model_, data_, width, height, camera_id, rgb); + renderer.Render(model_, data_, width, height, camera_id, rgb, nullptr, + &option_override); #else - renderer_->Render(model_, data_, width, height, camera_id, rgb); + renderer_->Render(model_, data_, width, height, camera_id, rgb, nullptr, + &option_override); #endif } } @@ -230,8 +233,13 @@ class MujocoRobotEnv : public RenderableEnv { (void)camera; return false; } + virtual envpool::mujoco::CameraPolicy RenderCameraPolicy() const { + return envpool::mujoco::CameraPolicy::kGymLike; + } + virtual void ConfigureRenderOption(mjvOption* option) const { (void)option; } void InitializeRenderCamera(mjvCamera* camera) const { + *camera = mjvCamera{}; mjv_defaultCamera(camera); camera->type = mjCAMERA_FREE; camera->fixedcamid = -1; @@ -244,6 +252,12 @@ class MujocoRobotEnv : public RenderableEnv { } } + void InitializeRenderOption(mjvOption* option) const { + *option = mjvOption{}; + mjv_defaultOption(option); + ConfigureRenderOption(option); + } + void InitializeRobotEnv() { initial_time_ = data_->time; std::memcpy(initial_qpos_.data(), data_->qpos, sizeof(mjtNum) * model_->nq); @@ -256,6 +270,9 @@ class MujocoRobotEnv : public RenderableEnv { data_->time = initial_time_; std::memcpy(data_->qpos, initial_qpos_.data(), sizeof(mjtNum) * model_->nq); std::memcpy(data_->qvel, initial_qvel_.data(), sizeof(mjtNum) * model_->nv); + if (model_->nu != 0) { + mju_zero(data_->ctrl, model_->nu); + } if (model_->na != 0) { mju_zero(data_->act, model_->na); } @@ -271,6 +288,8 @@ class MujocoRobotEnv : public RenderableEnv { } } + void InvalidateRenderCache() { has_cached_render_ = false; } + void CaptureResetState() { #ifdef ENVPOOL_TEST std::memcpy(qpos0_.data(), data_->qpos, sizeof(mjtNum) * model_->nq); diff --git a/envpool/python/envpool.py b/envpool/python/envpool.py index 1ea972d7b..67d5cadfc 100644 --- a/envpool/python/envpool.py +++ b/envpool/python/envpool.py @@ -69,6 +69,13 @@ def _ensure_platform_render_context(self, width: int, height: int) -> None: if self._requires_windows_glfw_context(): try_ensure_mujoco_glfw_context(width or 640, height or 480) + def _requires_pixel_render_context(self) -> bool: + """Return whether _recv may render pixels before render() is called.""" + return ( + self._requires_windows_glfw_context() + and "obs:pixels" in getattr(self, "_state_keys", ()) + ) + def _player_action_count( self: EnvPool, adict: dict[str, Any] ) -> int | None: @@ -300,6 +307,11 @@ def recv( return_info: bool = True, ) -> TimeStep | tuple: """Recv a batch state from EnvPool.""" + if self._requires_pixel_render_context(): + self._ensure_platform_render_context( + int(getattr(self, "_render_width", 0)), + int(getattr(self, "_render_height", 0)), + ) state_list = self._recv() if not hasattr(self, "_state_names"): self._state_names = self._state_keys diff --git a/envpool/python/glfw_context.py b/envpool/python/glfw_context.py index 8de2948c6..de431268c 100644 --- a/envpool/python/glfw_context.py +++ b/envpool/python/glfw_context.py @@ -11,12 +11,13 @@ # 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. -"""GLFW-backed MuJoCo render context helpers for Windows.""" +"""GLFW-backed MuJoCo render context helpers.""" from __future__ import annotations import ctypes import os +import platform import threading import warnings from pathlib import Path @@ -82,16 +83,17 @@ def _glfw_error_details(glfw: object) -> str: def _warn_glfw_fallback(reason: str) -> None: + system = platform.system() warnings.warn( "Failed to initialize the GLFW-backed MuJoCo render context on " - f"Windows; falling back to EnvPool's native WGL path. {reason}", + f"{system}; falling back to EnvPool's native OpenGL path. {reason}", RuntimeWarning, stacklevel=2, ) class _GlfwContext: - """Hidden GLFW window aligned with MuJoCo's upstream Windows backend.""" + """Hidden GLFW window aligned with MuJoCo's upstream backend.""" def __init__(self, width: int, height: int) -> None: preload_windows_gl_dlls() @@ -99,7 +101,7 @@ def __init__(self, width: int, height: int) -> None: import glfw except ImportError as exc: raise RuntimeError( - "MuJoCo rendering on Windows requires the `glfw` package." + "MuJoCo rendering requires the `glfw` package." ) from exc if not glfw.init(): raise RuntimeError( @@ -138,7 +140,7 @@ def close(self) -> None: def try_ensure_mujoco_glfw_context(width: int, height: int) -> bool: - """Best-effort GLFW context setup that falls back to native WGL.""" + """Best-effort GLFW context setup that falls back to native GL paths.""" global _GLFW_CONTEXT, _GLFW_FAILURE_REASON with _CONTEXT_LOCK: if _GLFW_FAILURE_REASON is not None: diff --git a/envpool/python/protocol.py b/envpool/python/protocol.py index 9c3472737..6a90f74b1 100644 --- a/envpool/python/protocol.py +++ b/envpool/python/protocol.py @@ -207,6 +207,9 @@ def _requires_windows_glfw_context(self) -> bool: def _ensure_platform_render_context(self, width: int, height: int) -> None: """Prepare any platform-specific GL context required before render.""" + def _requires_pixel_render_context(self) -> bool: + """Whether recv may render pixels before render bootstraps GL.""" + def _show_human_frame(self, frame: np.ndarray) -> None: """Display a rendered frame in a Python viewer.""" diff --git a/envpool/registration.py b/envpool/registration.py index 9ececb0f6..ab24bb56f 100644 --- a/envpool/registration.py +++ b/envpool/registration.py @@ -17,7 +17,7 @@ import importlib.util import os from collections.abc import Sequence -from typing import Any, Literal, overload +from typing import Any, Callable, Literal, overload import numpy as np @@ -151,6 +151,7 @@ def _pixel_variant_supported(import_path: str) -> bool: "envpool.mujoco.dmc", "envpool.mujoco.gym", "envpool.mujoco.metaworld", + "envpool.mujoco.myosuite.native", "envpool.mujoco.robotics", } @@ -194,6 +195,15 @@ def _make_env_spec( import_path, spec_cls, kwargs = self._resolve_spec_entry( task_id, from_pixels ) + config_resolver: ( + Callable[[str, dict[str, Any]], dict[str, Any]] | None + ) = kwargs.pop("_config_resolver", None) + if config_resolver is not None: + preview_kwargs = {**kwargs, **make_kwargs} + kwargs = { + **kwargs, + **config_resolver(task_id, preview_kwargs), + } kwargs = {**kwargs, **make_kwargs} if needs_render: for key in ("render", "enable_render"): diff --git a/envpool/workspace0.bzl b/envpool/workspace0.bzl index 11bb74497..243bfdea0 100644 --- a/envpool/workspace0.bzl +++ b/envpool/workspace0.bzl @@ -438,6 +438,7 @@ perl -Iperllib -I. macros/macros.pl version.mac 'macros/*.mac' 'output/*.mac' patches = [ "//third_party/vizdoom:sdl_thread.patch", "//third_party/vizdoom:windows_msvc_compat.patch", + "//third_party/vizdoom:re2c_safe_substr.patch", ], ) @@ -599,6 +600,82 @@ perl -Iperllib -I. macros/macros.pl version.mac 'macros/*.mac' 'output/*.mac' build_file = "//third_party/metaworld_assets:metaworld_assets.BUILD", ) + maybe( + http_archive, + name = "myosuite_src", + patch_args = ["-p1"], + patches = [ + "//third_party/myosuite:upstream_compat.patch", + ], + sha256 = "d2c5d3348b505e886de466ae4fca2589149048c463ad34188322ddfa4a1161d6", + strip_prefix = "myosuite-05cb84678373f91271004f99602ebbf01e57d1a1", + type = "tar.gz", + urls = [ + "https://codeload.github.com/MyoHub/myosuite/tar.gz/05cb84678373f91271004f99602ebbf01e57d1a1", + ], + build_file = "//third_party/myosuite:myosuite.BUILD", + ) + + maybe( + http_archive, + name = "myo_sim_src", + sha256 = "bd8fdf313b46dbefcd25bf42cf8ddcc45066798164bb3551a990690cad514ebd", + strip_prefix = "myo_sim-33f3ded946f55adbdcf963c99999587aadaf975f", + type = "tar.gz", + urls = [ + "https://codeload.github.com/MyoHub/myo_sim/tar.gz/33f3ded946f55adbdcf963c99999587aadaf975f", + ], + build_file = "//third_party/myosuite:myo_sim.BUILD", + ) + + maybe( + http_archive, + name = "furniture_sim_src", + sha256 = "5fb42ed8c932f7c820a72fbb86ea736957476020bdf008e17277380c3693ce9e", + strip_prefix = "furniture_sim-c97995afb81c9e2d7325b0069f9abc9a2c74a2f0", + type = "tar.gz", + urls = [ + "https://codeload.github.com/vikashplus/furniture_sim/tar.gz/c97995afb81c9e2d7325b0069f9abc9a2c74a2f0", + ], + build_file = "//third_party/myosuite:furniture_sim.BUILD", + ) + + maybe( + http_archive, + name = "object_sim_src", + sha256 = "782ea1c051d9afebbbd4d164fdc35035c4bd617981f3e1cb9204c7a2e15e4a0e", + strip_prefix = "object_sim-87cd8dd5a11518b94fca16bc22bb04f6836c6aa7", + type = "tar.gz", + urls = [ + "https://codeload.github.com/vikashplus/object_sim/tar.gz/87cd8dd5a11518b94fca16bc22bb04f6836c6aa7", + ], + build_file = "//third_party/myosuite:object_sim.BUILD", + ) + + maybe( + http_archive, + name = "mpl_sim_src", + sha256 = "591fce117832c789e227499ea45c601a9ca142c7dd636492f8bbcd825d54ea0a", + strip_prefix = "MPL_sim-58dd1abc6058e0dc06e62f13a61c36adb4916815", + type = "tar.gz", + urls = [ + "https://codeload.github.com/vikashplus/MPL_sim/tar.gz/58dd1abc6058e0dc06e62f13a61c36adb4916815", + ], + build_file = "//third_party/myosuite:mpl_sim.BUILD", + ) + + maybe( + http_archive, + name = "ycb_sim_src", + sha256 = "81caf29e5b5c01b4af56991731b3f731a95d486addccafaaaedc7600a9f2437e", + strip_prefix = "YCB_sim-46edd9c361061c5d81a82f2511d4fbf76fead569", + type = "tar.gz", + urls = [ + "https://codeload.github.com/vikashplus/YCB_sim/tar.gz/46edd9c361061c5d81a82f2511d4fbf76fead569", + ], + build_file = "//third_party/myosuite:ycb_sim.BUILD", + ) + maybe( http_archive, name = "box2d", diff --git a/scripts/BUILD b/scripts/BUILD index 9788bd26e..7307d9674 100644 --- a/scripts/BUILD +++ b/scripts/BUILD @@ -21,12 +21,13 @@ py_binary( imports = [".."], visibility = ["//visibility:public"], deps = [ + "//envpool/mujoco:myosuite_render_utils", "//envpool/mujoco:metaworld", "//envpool/mujoco:metaworld_registration", "//envpool/python:glfw_context", "@metaworld_assets//:metaworld_oracle", + requirement("dm-control"), requirement("gymnasium"), - requirement("imageio"), requirement("mujoco"), requirement("numpy"), requirement("pillow"), diff --git a/scripts/render_compare.py b/scripts/render_compare.py index 9991e17c3..00349beaa 100644 --- a/scripts/render_compare.py +++ b/scripts/render_compare.py @@ -62,6 +62,9 @@ class RenderItem: key: str label: str + max_mean_abs_diff: float | None = None + max_mismatch_ratio: float | None = None + require_match: bool = True @dataclass(frozen=True) @@ -210,8 +213,78 @@ def render_pair( ) +def _make_myosuite_family() -> RenderFamily: + """Build the MyoSuite render comparison adapter.""" + from envpool.mujoco.myosuite.render_utils import ( + MYOSUITE_RENDER_COMPARE_CASES, + MYOSUITE_RENDER_COMPARE_STEPS, + MYOSUITE_RENDER_RETRY_SEEDS, + capture_render_sequence, + official_render_thresholds, + ) + + cases = tuple(MYOSUITE_RENDER_COMPARE_CASES) + sequence_cache: dict[str, Any] = {} + case_thresholds = { + case.task_id: official_render_thresholds(case.task_id) for case in cases + } + + def _split_case(key: str) -> tuple[str, int]: + task_id, step_index = key.split(":", maxsplit=1) + return task_id, int(step_index) + + def render_pair( + key: str, + cfg: RenderCompareConfig, + ) -> tuple[np.ndarray, np.ndarray]: + task_id, step_index = _split_case(key) + sequence = sequence_cache.get(task_id) + if sequence is None: + sequence = capture_render_sequence( + task_id, + steps=MYOSUITE_RENDER_COMPARE_STEPS, + seed=cfg.seed, + render_width=cfg.source_width, + render_height=cfg.source_height, + camera_id=cfg.camera_id, + retry_seeds=MYOSUITE_RENDER_RETRY_SEEDS, + ) + sequence_cache[task_id] = sequence + return ( + sequence.envpool_frames[step_index], + sequence.official_frames[step_index], + ) + + return RenderFamily( + items=tuple( + RenderItem( + key=f"{case.task_id}:{step_index}", + label=f"{case.label} step {step_index + 1}", + max_mean_abs_diff=( + case_thresholds[case.task_id][0] + if case_thresholds[case.task_id] is not None + else None + ), + max_mismatch_ratio=( + case_thresholds[case.task_id][1] + if case_thresholds[case.task_id] is not None + else None + ), + require_match=True, + ) + for case in cases + for step_index in range(MYOSUITE_RENDER_COMPARE_STEPS) + ), + default_output=Path( + "docs/_static/render_samples/myosuite_official_compare.png" + ), + render_pair=render_pair, + ) + + _FAMILY_BUILDERS: dict[str, Callable[[], RenderFamily]] = { "metaworld": _make_metaworld_family, + "myosuite": _make_myosuite_family, } @@ -338,7 +411,35 @@ def generate( for index, item in enumerate(family.items): envpool_frame, official_frame = family.render_pair(item.key, cfg) - _assert_frames_match(item.label, envpool_frame, official_frame, cfg) + if item.require_match: + item_cfg = RenderCompareConfig( + family=cfg.family, + tile_width=cfg.tile_width, + tile_height=cfg.tile_height, + source_width=cfg.source_width, + source_height=cfg.source_height, + columns=cfg.columns, + seed=cfg.seed, + camera_id=cfg.camera_id, + max_mean_abs_diff=( + cfg.max_mean_abs_diff + if item.max_mean_abs_diff is None + else item.max_mean_abs_diff + ), + max_mismatch_ratio=( + cfg.max_mismatch_ratio + if item.max_mismatch_ratio is None + else item.max_mismatch_ratio + ), + require_bitwise=cfg.require_bitwise, + flip_vertical=cfg.flip_vertical, + ) + _assert_frames_match( + item.label, + envpool_frame, + official_frame, + item_cfg, + ) envpool_image = _make_display_image(envpool_frame, cfg) official_image = _make_display_image(official_frame, cfg) _draw_panel( diff --git a/setup.cfg b/setup.cfg index e13e840b3..6daa3ad83 100644 --- a/setup.cfg +++ b/setup.cfg @@ -45,6 +45,7 @@ envpool = **/*_envpool.pyd **/*_envpool.pyd.dll mujoco/*.so.* + mujoco/myosuite/*.json vizdoom/bin/vizdoom vizdoom/bin/vizdoom.pk3 diff --git a/third_party/myosuite/BUILD b/third_party/myosuite/BUILD new file mode 100644 index 000000000..301af17ee --- /dev/null +++ b/third_party/myosuite/BUILD @@ -0,0 +1,33 @@ +# Copyright 2026 Garena Online Private 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. + +package(default_visibility = ["//visibility:public"]) + +exports_files([ + "defs.bzl", + "generate_arm_reach_asset.py", + "generate_tabletennis_asset.py", + "furniture_sim.BUILD", + "generate_metadata.py", + "metadata/env_ids.json", + "metadata/README.md", + "mpl_sim.BUILD", + "myosuite.BUILD", + "myo_sim.BUILD", + "object_sim.BUILD", + "simhive/myo_sim/arm/myoarm_reach.xml", + "simhive/myo_sim/arm/myoarm_tabletennis.xml", + "upstream_compat.patch", + "ycb_sim.BUILD", +]) diff --git a/third_party/myosuite/defs.bzl b/third_party/myosuite/defs.bzl new file mode 100644 index 000000000..2fe45e1a2 --- /dev/null +++ b/third_party/myosuite/defs.bzl @@ -0,0 +1,120 @@ +# Copyright 2026 Garena Online Private 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. + +"""MyoSuite runtime asset staging rule.""" + +def _myosuite_runtime_assets_impl(ctx): + out = ctx.actions.declare_directory(ctx.attr.out) + manifest = ctx.actions.declare_file(ctx.label.name + "_srcs.txt") + lines = [] + + def add_entries(srcs, strip_prefix, dest_prefix): + for src in sorted(srcs, key = lambda f: f.path): + lines.append("\t".join([src.path, strip_prefix, dest_prefix])) + + add_entries(ctx.files.myosuite_env_assets, "myosuite/", "") + add_entries(ctx.files.myo_sim_assets, "myo_sim_src/", "simhive/myo_sim/") + add_entries( + ctx.files.furniture_sim_assets, + "furniture_sim_src/", + "simhive/furniture_sim/", + ) + add_entries( + ctx.files.object_sim_assets, + "object_sim_src/", + "simhive/object_sim/", + ) + add_entries(ctx.files.mpl_sim_assets, "mpl_sim_src/", "simhive/MPL_sim/") + add_entries(ctx.files.ycb_sim_assets, "ycb_sim_src/", "simhive/YCB_sim/") + add_entries(ctx.files.patched_assets, "third_party/myosuite/", "") + + ctx.actions.write(output = manifest, content = "\n".join(lines) + "\n") + + args = ctx.actions.args() + args.add(out.path) + args.add(manifest.path) + + ctx.actions.run_shell( + inputs = depset( + ctx.files.myosuite_env_assets + + ctx.files.myo_sim_assets + + ctx.files.furniture_sim_assets + + ctx.files.object_sim_assets + + ctx.files.mpl_sim_assets + + ctx.files.ycb_sim_assets + + ctx.files.patched_assets + + [manifest], + ), + outputs = [out], + arguments = [args], + command = """ +set -eu + +out="$1" +manifest="$2" + +mkdir -p "$out" +while IFS=$'\\t' read -r src strip_prefix dest_prefix; do + [ -n "$src" ] || continue + rel="${src#*${strip_prefix}}" + if [ "$rel" = "$src" ]; then + rel="$(basename "$src")" + fi + dst="$out/$dest_prefix$rel" + mkdir -p "$(dirname "$dst")" + cp -R "$src" "$dst" +done < "$manifest" +""", + ) + + return [ + DefaultInfo( + files = depset([out]), + runfiles = ctx.runfiles(files = [out]), + ), + ] + +myosuite_runtime_assets = rule( + implementation = _myosuite_runtime_assets_impl, + attrs = { + "myosuite_env_assets": attr.label_list( + allow_files = True, + mandatory = True, + ), + "myo_sim_assets": attr.label_list( + allow_files = True, + mandatory = True, + ), + "furniture_sim_assets": attr.label_list( + allow_files = True, + mandatory = True, + ), + "object_sim_assets": attr.label_list( + allow_files = True, + mandatory = True, + ), + "mpl_sim_assets": attr.label_list( + allow_files = True, + mandatory = True, + ), + "ycb_sim_assets": attr.label_list( + allow_files = True, + mandatory = True, + ), + "patched_assets": attr.label_list( + allow_files = True, + ), + "out": attr.string(mandatory = True), + }, +) diff --git a/third_party/myosuite/furniture_sim.BUILD b/third_party/myosuite/furniture_sim.BUILD new file mode 100644 index 000000000..bca2024d3 --- /dev/null +++ b/third_party/myosuite/furniture_sim.BUILD @@ -0,0 +1,30 @@ +# Copyright 2026 Garena Online Private 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. + +filegroup( + name = "runtime_assets", + srcs = glob( + ["**"], + exclude = [ + "**/.git/**", + "**/.github/**", + "**/__pycache__/**", + "**/*.md", + "**/*.py", + "**/LICENSE", + "**/LICENSE.*", + ], + ), + visibility = ["//visibility:public"], +) diff --git a/third_party/myosuite/generate_arm_reach_asset.py b/third_party/myosuite/generate_arm_reach_asset.py new file mode 100644 index 000000000..142358742 --- /dev/null +++ b/third_party/myosuite/generate_arm_reach_asset.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 +# Copyright 2026 Garena Online Private 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 the vendored MyoSuite arm-reaching runtime XML asset.""" + +from __future__ import annotations + +import argparse +import tempfile +from pathlib import Path +from xml.etree import ElementTree as ET + +import mujoco + + +def _first_child_body(body: mujoco.MjsBody) -> mujoco.MjsBody | None: + return body.first_body() + + +def _arm_reaching_spec_to_xml(model_path: str) -> str: + spec = mujoco.MjSpec.from_file(model_path) + root_names = ("firstmc", "secondmc", "thirdmc", "fourthmc", "fifthmc") + tip_site = spec.site("IFtip") + tip_site_name = str(tip_site.name) + tip_site_size = tip_site.size.copy() + tip_site_pos = tip_site.pos.copy() + tip_site_rgba = tip_site.rgba.copy() + body_chains: dict[str, list[tuple[str, object, list[str]]]] = {} + + for root_name in root_names: + body_chains[root_name] = [] + root_body = spec.body(root_name) + child = _first_child_body(root_body) + while child is not None: + mesh_names = [ + str(geom.name) + for geom in child.geoms + if geom.type == mujoco.mjtGeom.mjGEOM_MESH + ] + body_chains[root_name].append(( + str(child.name), + child.pos.copy(), + mesh_names, + )) + child = _first_child_body(child) + + for root_name in root_names: + root_body = spec.body(root_name) + child = _first_child_body(root_body) + if child is not None: + spec.delete(child) + + for root_name in root_names: + parent = spec.body(root_name) + for body_name, pos, mesh_names in body_chains[root_name]: + parent.add_body(name=body_name, pos=pos) + current = spec.body(body_name) + for mesh_name in mesh_names: + current.add_geom( + meshname=mesh_name, + name=body_name, + type=mujoco.mjtGeom.mjGEOM_MESH, + ) + if body_name == "distph2": + current.add_site( + name=tip_site_name, + size=tip_site_size * 2.0, + pos=tip_site_pos, + rgba=tip_site_rgba, + ) + parent = current + + spec.body("world").add_site( + name="IFtip_target", + type=mujoco.mjtGeom.mjGEOM_SPHERE, + size=[0.02, 0.02, 0.02], + pos=[-0.2, -0.2, 1.2], + rgba=[0.0, 0.0, 1.0, 0.3], + ) + return spec.to_xml() + + +def _parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser() + parser.add_argument("--input", type=Path, required=True) + parser.add_argument("--output", type=Path, required=True) + return parser.parse_args() + + +def main() -> None: + """Generate the checked-in arm-reaching XML next to the vendored assets.""" + args = _parse_args() + with tempfile.TemporaryDirectory() as td: + staged_root = Path(td) / "simhive" + staged_root.mkdir(parents=True, exist_ok=True) + (staged_root / "myo_sim").symlink_to( + args.input.resolve().parent.parent, + target_is_directory=True, + ) + staged_model = staged_root / "myo_sim" / "arm" / args.input.name + xml_tree = ET.ElementTree( + ET.fromstring(_arm_reaching_spec_to_xml(str(staged_model))) + ) + root = xml_tree.getroot() + if root is not None: + compiler = root.find("compiler") + if compiler is not None: + compiler.set("meshdir", ".") + compiler.set("texturedir", ".") + for elem in root.iter(): + file_attr = elem.get("file") + if file_attr is not None and file_attr.startswith("../myo_sim/"): + elem.set("file", "../" + file_attr[len("../myo_sim/") :]) + args.output.parent.mkdir(parents=True, exist_ok=True) + xml_tree.write(args.output, encoding="utf-8") + + +if __name__ == "__main__": + main() diff --git a/third_party/myosuite/generate_metadata.py b/third_party/myosuite/generate_metadata.py new file mode 100644 index 000000000..eb0d2ef78 --- /dev/null +++ b/third_party/myosuite/generate_metadata.py @@ -0,0 +1,619 @@ +#!/usr/bin/env python3 +# Copyright 2026 Garena Online Private 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 canonical MyoSuite registry metadata from upstream source.""" + +from __future__ import annotations + +import argparse +import ast +import importlib.util +import json +import os +import shutil +import sys +import tempfile +import types +from contextlib import contextmanager +from pathlib import Path +from typing import Any, Iterable + +import numpy as np + + +def _constant_string(node: ast.AST) -> str | None: + if isinstance(node, ast.Constant) and isinstance(node.value, str): + return node.value + return None + + +def _load_module(path: Path) -> ast.Module: + return ast.parse(path.read_text(), filename=str(path)) + + +def _call_name(node: ast.Call) -> str | None: + if isinstance(node.func, ast.Name): + return node.func.id + if isinstance(node.func, ast.Attribute): + return node.func.attr + return None + + +def _keyword_string(node: ast.Call, arg_name: str) -> str | None: + for keyword in node.keywords: + if keyword.arg == arg_name: + return _constant_string(keyword.value) + return None + + +def _direct_ids_from_register_env_with_variants(path: Path) -> list[str]: + ids: list[str] = [] + for node in ast.walk(_load_module(path)): + if ( + isinstance(node, ast.Call) + and _call_name(node) == "register_env_with_variants" + ): + env_id = _keyword_string(node, "id") + if env_id is not None: + ids.append(env_id) + return ids + + +def _expand_upstream_variants(env_ids: Iterable[str]) -> list[str]: + expanded: list[str] = [] + for env_id in env_ids: + expanded.append(env_id) + if env_id.startswith("myo"): + expanded.append(env_id[:3] + "Sarc" + env_id[3:]) + expanded.append(env_id[:3] + "Fati" + env_id[3:]) + if env_id.startswith("myoHand"): + expanded.append(env_id[:3] + "Reaf" + env_id[3:]) + return expanded + + +def _dedupe_sorted(values: Iterable[str]) -> list[str]: + return sorted(set(values)) + + +def _normalize_json_value(value: Any) -> Any: + if isinstance(value, dict): + return { + str(key): _normalize_json_value(value[key]) for key in sorted(value) + } + if isinstance(value, tuple): + return [_normalize_json_value(item) for item in value] + if isinstance(value, list): + return [_normalize_json_value(item) for item in value] + if isinstance(value, np.ndarray): + return _normalize_json_value(value.tolist()) + if isinstance(value, np.generic): + return value.item() + if callable(value): + return getattr(value, "__name__", repr(value)) + return value + + +def _normalize_upstream_path( + value: str, module_dir: Path, myosuite_root: Path +) -> str: + candidate = Path(value) + if candidate.is_absolute() and str(candidate).startswith( + str(myosuite_root) + ): + resolved = candidate.resolve() + else: + relative = value[1:] if value.startswith("/") else value + resolved = (module_dir / relative).resolve() + if "myosuite" in resolved.parts: + index = max( + i for i, part in enumerate(resolved.parts) if part == "myosuite" + ) + return Path(*resolved.parts[index + 1 :]).as_posix() + return resolved.relative_to(myosuite_root).as_posix() + + +def _normalize_captured_kwargs( + kwargs: dict[str, Any], module_dir: Path, myosuite_root: Path +) -> dict[str, Any]: + normalized: dict[str, Any] = {} + for key in sorted(kwargs): + value = _normalize_json_value(kwargs[key]) + if isinstance(value, str) and ( + key.endswith("_path") or key == "reference" + ): + value = _normalize_upstream_path(value, module_dir, myosuite_root) + normalized[key] = value + return normalized + + +def _model_placeholders(model_path: str, myosuite_root: Path) -> list[str]: + placeholders: list[str] = [] + xml_path = myosuite_root / model_path + if not xml_path.exists(): + return placeholders + xml_text = xml_path.read_text() + if "OBJECT_NAME" in xml_text: + placeholders.append("OBJECT_NAME") + return placeholders + + +def _capture_suite_entries( + upstream_root: Path, suite_name: str +) -> tuple[list[dict[str, Any]], list[dict[str, Any]]]: + myosuite_root = upstream_root / "myosuite" + init_path = myosuite_root / "envs" / "myo" / suite_name / "__init__.py" + records: list[dict[str, Any]] = [] + variant_records: list[dict[str, Any]] = [] + + def fake_register( + *, id: str, entry_point: str, max_episode_steps: int, kwargs: Any + ) -> None: + records.append({ + "id": id, + "entry_point": entry_point, + "max_episode_steps": max_episode_steps, + "kwargs": dict(kwargs), + }) + + def fake_register_env_variant( + *, + env_id: str, + variants: dict[str, Any], + variant_id: str, + silent: bool = True, + ) -> None: + variant_records.append({ + "env_id": env_id, + "variant_id": variant_id, + "variants": _normalize_json_value(variants), + "silent": silent, + }) + + class FakeBackend: + @staticmethod + def get_sim_backend() -> str: + return "mujoco" + + module_names = ( + "mujoco", + "myosuite", + "myosuite.utils", + "myosuite.envs", + "myosuite.envs.myo", + "myosuite.envs.myo.myobase", + "myosuite.envs.env_variants", + "myosuite.physics", + "myosuite.physics.sim_scene", + ) + originals = {name: sys.modules.get(name) for name in module_names} + modules = {name: types.ModuleType(name) for name in module_names} + modules["mujoco"].MjSpec = type("MjSpec", (), {}) + modules["mujoco"].mjtGeom = types.SimpleNamespace( + mjGEOM_MESH=0, + mjGEOM_SPHERE=1, + ) + modules["myosuite.utils"].gym = types.SimpleNamespace( + register=fake_register + ) + modules[ + "myosuite.envs.env_variants" + ].register_env_variant = fake_register_env_variant + + def register_env_with_variants( + id: str, entry_point: str, max_episode_steps: int, kwargs: Any + ) -> None: + fake_register( + id=id, + entry_point=entry_point, + max_episode_steps=max_episode_steps, + kwargs=kwargs, + ) + if id.startswith("myo"): + fake_register_env_variant( + env_id=id, + variants={"muscle_condition": "sarcopenia"}, + variant_id=id[:3] + "Sarc" + id[3:], + silent=True, + ) + fake_register_env_variant( + env_id=id, + variants={"muscle_condition": "fatigue"}, + variant_id=id[:3] + "Fati" + id[3:], + silent=True, + ) + if id.startswith("myoHand"): + fake_register_env_variant( + env_id=id, + variants={"muscle_condition": "reafferentation"}, + variant_id=id[:3] + "Reaf" + id[3:], + silent=True, + ) + + modules[ + "myosuite.envs.myo.myobase" + ].register_env_with_variants = register_env_with_variants + modules["myosuite.physics.sim_scene"].SimBackend = FakeBackend + modules["myosuite"].utils = modules["myosuite.utils"] + modules["myosuite"].envs = modules["myosuite.envs"] + modules["myosuite"].physics = modules["myosuite.physics"] + modules["myosuite.envs"].myo = modules["myosuite.envs.myo"] + modules["myosuite.envs.myo"].myobase = modules["myosuite.envs.myo.myobase"] + modules["myosuite.envs"].env_variants = modules[ + "myosuite.envs.env_variants" + ] + modules["myosuite.physics"].sim_scene = modules[ + "myosuite.physics.sim_scene" + ] + + try: + for name, module in modules.items(): + sys.modules[name] = module + namespace = { + "__file__": str(init_path), + "__name__": f"_capture_{suite_name}", + } + exec(compile(init_path.read_text(), str(init_path), "exec"), namespace) + finally: + for name, module in originals.items(): + if module is None: + sys.modules.pop(name, None) + else: + sys.modules[name] = module + return records, variant_records + + +def _captured_entry_to_metadata( + record: dict[str, Any], + logical_suite_name: str, + registration_suite_name: str, + upstream_root: Path, + variant_map: dict[str, list[dict[str, Any]]], +) -> dict[str, Any]: + myosuite_root = upstream_root / "myosuite" + entry_module, class_name = record["entry_point"].split(":") + module_path = upstream_root / Path(*entry_module.split(".")) + module_path = module_path.with_suffix(".py") + kwargs = _normalize_captured_kwargs( + record["kwargs"], module_path.parent, myosuite_root + ) + model_path = kwargs["model_path"] + return { + "class_name": class_name, + "entry_module": entry_module, + "id": record["id"], + "kwargs": kwargs, + "max_episode_steps": record["max_episode_steps"], + "model_placeholders": _model_placeholders(model_path, myosuite_root), + "registration_suite": registration_suite_name, + "suite": logical_suite_name, + "variant_defs": sorted( + variant_map.get(record["id"], []), + key=lambda item: item["variant_id"], + ), + } + + +def _parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser() + parser.add_argument("--upstream-root", type=Path, required=True) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--version", required=True) + parser.add_argument("--commit", required=True) + return parser.parse_args() + + +def _repo_root() -> Path: + return Path(__file__).resolve().parents[2] + + +def _copy_or_symlink_tree(src: Path, dst: Path) -> None: + try: + os.symlink(src, dst, target_is_directory=True) + except OSError: + shutil.copytree(src, dst) + + +def _find_staged_runtime_asset_root() -> Path | None: + candidates: list[Path] = [] + for root in ( + Path.cwd(), + _repo_root(), + Path(os.environ["RUNFILES_DIR"]) + if os.environ.get("RUNFILES_DIR") + else None, + Path(os.environ["TEST_SRCDIR"]) + if os.environ.get("TEST_SRCDIR") + else None, + ): + if root is None: + continue + candidates.extend([ + root / "envpool/mujoco/myosuite_assets", + root / "envpool/envpool/mujoco/myosuite_assets", + root / "mujoco/myosuite_assets", + ]) + required_runtime_asset = ( + Path("envs") / "myo" / "assets" / "hand" / "myohand_object.xml" + ) + for candidate in candidates: + if (candidate / required_runtime_asset).exists(): + return candidate + return None + + +@contextmanager +def _staged_runtime_asset_base(upstream_root: Path) -> Iterable[Path]: + external_root = upstream_root.parent + patched_myo_sim_root = ( + _repo_root() / "third_party" / "myosuite" / "simhive" / "myo_sim" + ) + sibling_roots = { + "myo_sim": external_root / "myo_sim_src", + "furniture_sim": external_root / "furniture_sim_src", + "object_sim": external_root / "object_sim_src", + "mpl_sim": external_root / "mpl_sim_src", + "ycb_sim": external_root / "ycb_sim_src", + } + with tempfile.TemporaryDirectory() as tmpdir: + base_path = Path(tmpdir) + asset_root = base_path / "mujoco" / "myosuite_assets" + if all(path.exists() for path in sibling_roots.values()): + mappings = { + upstream_root / "myosuite" / "envs": asset_root / "envs", + sibling_roots["myo_sim"]: asset_root / "simhive/myo_sim", + sibling_roots["furniture_sim"]: asset_root + / "simhive/furniture_sim", + sibling_roots["object_sim"]: asset_root / "simhive/object_sim", + sibling_roots["mpl_sim"]: asset_root / "simhive/MPL_sim", + sibling_roots["ycb_sim"]: asset_root / "simhive/YCB_sim", + } + for src, dst in mappings.items(): + dst.parent.mkdir(parents=True, exist_ok=True) + if ( + src == sibling_roots["myo_sim"] + and patched_myo_sim_root.exists() + ): + shutil.copytree(src, dst) + shutil.copytree( + patched_myo_sim_root, dst, dirs_exist_ok=True + ) + else: + _copy_or_symlink_tree(src, dst) + else: + staged_root = _find_staged_runtime_asset_root() + if staged_root is None: + raise FileNotFoundError( + "Unable to locate staged MyoSuite runtime assets or " + "vendored asset repositories." + ) + asset_root.parent.mkdir(parents=True, exist_ok=True) + _copy_or_symlink_tree(staged_root, asset_root) + yield base_path + + +def _attach_default_configs( + direct_entry_by_id: dict[str, dict[str, Any]], upstream_root: Path +) -> None: + package_names = ( + "envpool", + "envpool.mujoco", + "envpool.mujoco.myosuite", + ) + originals = { + name: sys.modules.get(name) + for name in ( + *package_names, + "envpool.mujoco.myosuite.metadata", + "envpool.mujoco.myosuite.paths", + "_envpool_myosuite_config_codegen", + ) + } + for name in package_names: + if name not in sys.modules: + module = types.ModuleType(name) + module.__path__ = [] # type: ignore[attr-defined] + sys.modules[name] = module + + metadata_module = types.ModuleType("envpool.mujoco.myosuite.metadata") + metadata_module.MYOSUITE_DIRECT_ENTRIES = tuple(direct_entry_by_id.values()) + sys.modules["envpool.mujoco.myosuite.metadata"] = metadata_module + + paths_module = types.ModuleType("envpool.mujoco.myosuite.paths") + paths_module.myosuite_asset_root = lambda: Path("__unused__") + sys.modules["envpool.mujoco.myosuite.paths"] = paths_module + + config_path = _repo_root() / "envpool/mujoco/myosuite/config.py" + spec = importlib.util.spec_from_file_location( + "_envpool_myosuite_config_codegen", + config_path, + ) + if spec is None or spec.loader is None: + raise RuntimeError( + f"Unable to load MyoSuite config module: {config_path}" + ) + config_module = importlib.util.module_from_spec(spec) + sys.modules[spec.name] = config_module + spec.loader.exec_module(config_module) + generate_myosuite_task_config = config_module.generate_myosuite_task_config + + try: + with _staged_runtime_asset_base(upstream_root) as base_path: + for entry in direct_entry_by_id.values(): + entry["default_config"] = generate_myosuite_task_config( + entry, + {}, + base_path=str(base_path), + ) + finally: + for name, module in originals.items(): + if module is None: + sys.modules.pop(name, None) + else: + sys.modules[name] = module + + +def main() -> None: + """Capture upstream suite registrations and emit canonical metadata.""" + args = _parse_args() + upstream_root = args.upstream_root.resolve() + myo_root = upstream_root / "myosuite" / "envs" / "myo" + + direct_entry_by_id: dict[str, dict[str, Any]] = {} + for suite_name in ("myobase", "myochallenge", "myodm"): + records, variant_records = _capture_suite_entries( + upstream_root, suite_name + ) + variant_map: dict[str, list[dict[str, Any]]] = {} + for record in variant_records: + variant_map.setdefault(record["env_id"], []).append({ + "variant_id": record["variant_id"], + "variants": record["variants"], + }) + for record in records: + direct_entry_by_id[record["id"]] = _captured_entry_to_metadata( + record, + suite_name, + suite_name, + upstream_root, + variant_map, + ) + + myoedits_records, myoedits_variant_records = _capture_suite_entries( + upstream_root, "myoedits" + ) + myoedits_variant_map: dict[str, list[dict[str, Any]]] = {} + for record in myoedits_variant_records: + myoedits_variant_map.setdefault(record["env_id"], []).append({ + "variant_id": record["variant_id"], + "variants": record["variants"], + }) + myoedits_direct = _direct_ids_from_register_env_with_variants( + myo_root / "myoedits" / "__init__.py" + ) + for record in myoedits_records: + if record["id"] not in myoedits_direct: + continue + if record["id"] not in direct_entry_by_id: + continue + direct_entry_by_id[record["id"]] = _captured_entry_to_metadata( + record, + "myobase", + "myoedits", + upstream_root, + myoedits_variant_map, + ) + + direct_entries = sorted( + direct_entry_by_id.values(), key=lambda entry: entry["id"] + ) + _attach_default_configs(direct_entry_by_id, upstream_root) + direct_entries.sort(key=lambda entry: entry["id"]) + direct_ids = [entry["id"] for entry in direct_entries] + expanded_ids = _dedupe_sorted( + [entry["id"] for entry in direct_entries for _ in [None]] + + [ + variant["variant_id"] + for entry in direct_entries + for variant in entry["variant_defs"] + ] + ) + + myobase_direct = [ + entry["id"] for entry in direct_entries if entry["suite"] == "myobase" + ] + myochallenge_direct = [ + entry["id"] + for entry in direct_entries + if entry["suite"] == "myochallenge" + ] + myodm_direct = [ + entry["id"] for entry in direct_entries if entry["suite"] == "myodm" + ] + myodm_track_ids = [ + entry["id"] + for entry in direct_entries + if entry["suite"] == "myodm" + and isinstance(entry["kwargs"].get("reference"), str) + ] + myodm_fixed_ids = [ + entry["id"] + for entry in direct_entries + if entry["id"].endswith("Fixed-v0") + ] + myodm_random_ids = [ + entry["id"] + for entry in direct_entries + if entry["id"].endswith("Random-v0") + ] + myodm_object_names = sorted({ + entry["kwargs"]["object_name"] + for entry in direct_entries + if entry["suite"] == "myodm" + }) + data = { + "pins": { + "myosuite": { + "version": args.version, + "commit": args.commit, + }, + }, + "suites": { + "myobase_direct_ids": sorted(myobase_direct), + "myochallenge_direct_ids": sorted(myochallenge_direct), + "myodm_track_ids": sorted(myodm_track_ids), + "myodm_object_names": myodm_object_names, + "myodm_fixed_ids": sorted( + entry_id + for entry_id in myodm_fixed_ids + if entry_id in myodm_direct + ), + "myodm_random_ids": sorted( + entry_id + for entry_id in myodm_random_ids + if entry_id in myodm_direct + ), + "myoedits_direct_ids": sorted(myoedits_direct), + }, + "counts": { + "myobase_direct": len(myobase_direct), + "myochallenge_direct": len(myochallenge_direct), + "myodm_direct": len(myodm_direct), + "direct_total": len(direct_ids), + "expanded_total": len(expanded_ids), + }, + "notes": { + "myoedits_duplicate_ids": sorted(myoedits_direct), + "variant_rules": [ + "Every myo* task also creates Sarc and Fati variants.", + "Every myoHand* task also creates a Reaf variant.", + ], + }, + "direct_entries": direct_entries, + "direct_ids": direct_ids, + "expanded_ids": expanded_ids, + } + + expected_expanded_ids = _dedupe_sorted( + _expand_upstream_variants(direct_ids) + ) + if expected_expanded_ids != expanded_ids: + raise RuntimeError("Expanded IDs no longer match the expected rules") + + args.output.parent.mkdir(parents=True, exist_ok=True) + args.output.write_text(json.dumps(data, indent=2, sort_keys=True) + "\n") + + +if __name__ == "__main__": + main() diff --git a/third_party/myosuite/generate_tabletennis_asset.py b/third_party/myosuite/generate_tabletennis_asset.py new file mode 100644 index 000000000..6aeea1c32 --- /dev/null +++ b/third_party/myosuite/generate_tabletennis_asset.py @@ -0,0 +1,202 @@ +#!/usr/bin/env python3 +# Copyright 2026 Garena Online Private 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 the vendored MyoSuite table-tennis runtime XML asset.""" + +from __future__ import annotations + +import argparse +from collections.abc import Callable +from pathlib import Path +from xml.etree import ElementTree as ET + +import mujoco + + +def _recursive_immobilize( + spec: mujoco.MjSpec, + temp_model: mujoco.MjModel, + parent: mujoco.MjsBody, + *, + remove_eqs: bool = False, + remove_actuators: bool = False, +) -> list[int]: + removed_joint_ids: list[int] = [] + for site in list(parent.sites): + spec.delete(site) + for joint in list(parent.joints): + removed_joint_ids.extend(list(temp_model.joint(joint.name).qposadr)) + if remove_eqs: + for equality in list(spec.equalities): + if equality.type == mujoco.mjtEq.mjEQ_JOINT and ( + equality.name1 == joint.name or equality.name2 == joint.name + ): + spec.delete(equality) + if remove_actuators: + for actuator in list(spec.actuators): + if ( + actuator.trntype == mujoco.mjtTrn.mjTRN_JOINT + and actuator.target == joint.name + ): + spec.delete(actuator) + spec.delete(joint) + for child in list(parent.bodies): + removed_joint_ids.extend( + _recursive_immobilize( + spec, + temp_model, + child, + remove_eqs=remove_eqs, + remove_actuators=remove_actuators, + ) + ) + return removed_joint_ids + + +def _recursive_remove_contacts( + parent: mujoco.MjsBody, + *, + return_condition: Callable[[mujoco.MjsBody], bool] | None = None, +) -> None: + if return_condition is not None and return_condition(parent): + return + for geom in list(parent.geoms): + geom.contype = 0 + geom.conaffinity = 0 + for child in list(parent.bodies): + _recursive_remove_contacts(child, return_condition=return_condition) + + +def _recursive_mirror( + meshes_to_mirror: set[str], + spec_copy: mujoco.MjSpec, + parent: mujoco.MjsBody, +) -> None: + parent.pos[1] *= -1 + parent.quat[[1, 3]] *= -1 + parent.name += "_mirrored" + for geom in list(parent.geoms): + if geom.type != mujoco.mjtGeom.mjGEOM_MESH: + spec_copy.delete(geom) + continue + geom.pos[1] *= -1 + geom.quat[[1, 3]] *= -1 + geom.name += "_mirrored" + geom.group = 1 + meshes_to_mirror.add(geom.meshname) + geom.meshname += "_mirrored" + for child in list(parent.bodies): + if "ping_pong" in child.name: + spec_copy.detach_body(child) + continue + _recursive_mirror(meshes_to_mirror, spec_copy, child) + + +def _tabletennis_spec_to_xml(model_path: str) -> str: + spec = mujoco.MjSpec.from_file(model_path) + for sensor in list(spec.sensors): + if "pingpong" not in sensor.name and "paddle" not in sensor.name: + spec.delete(sensor) + + temp_model = spec.compile() + removed_ids = _recursive_immobilize( + spec, temp_model, spec.body("femur_l"), remove_eqs=True + ) + removed_ids.extend( + _recursive_immobilize( + spec, temp_model, spec.body("femur_r"), remove_eqs=True + ) + ) + for key in list(spec.keys): + key.qpos = [ + qpos for idx, qpos in enumerate(key.qpos) if idx not in removed_ids + ] + + _recursive_remove_contacts( + spec.body("full_body"), + return_condition=lambda body: "radius" in body.name, + ) + + spec_copy = spec.copy() + torso = spec.body("torso") + attachment_frame = torso.add_frame( + quat=[0.5, 0.5, -0.5, 0.5], + pos=[0.05, 0.373, -0.04], + ) + for collection in ( + list(spec_copy.keys), + list(spec_copy.textures), + list(spec_copy.materials), + list(spec_copy.tendons), + list(spec_copy.actuators), + list(spec_copy.equalities), + list(spec_copy.sensors), + list(spec_copy.cameras), + ): + for item in collection: + spec_copy.delete(item) + _recursive_immobilize(spec_copy, temp_model, spec_copy.worldbody) + _recursive_remove_contacts(spec_copy.worldbody) + meshes_to_mirror: set[str] = set() + _recursive_mirror(meshes_to_mirror, spec_copy, spec_copy.body("clavicle")) + for mesh in list(spec_copy.meshes): + if mesh.name in meshes_to_mirror: + mesh.name += "_mirrored" + mesh.scale[1] *= -1 + else: + spec_copy.delete(mesh) + attachment_frame.attach_body(spec_copy.body("clavicle_mirrored")) + spec.body("ulna_mirrored").quat = [0.546, 0, 0, -0.838] + spec.body("humerus_mirrored").quat = [0.924, 0.383, 0, 0] + + root = ET.fromstring(spec.to_xml()) + compiler = root.find("compiler") + if compiler is not None: + compiler.set("meshdir", ".") + compiler.set("texturedir", ".") + defaults = root.find("default") + if defaults is not None: + defaults.set("class", "main") + for child in list(defaults): + if child.tag == "default" and "class" not in child.attrib: + defaults.remove(child) + for elem in root.iter(): + file_attr = elem.get("file") + if file_attr is None: + continue + if file_attr.startswith("../myo_sim/"): + elem.set("file", "../" + file_attr[len("../myo_sim/") :]) + elif file_attr.startswith("../../envs/"): + elem.set("file", "../" + file_attr) + return ET.tostring(root, encoding="unicode") + + +def _parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser() + parser.add_argument("--input", type=Path, required=True) + parser.add_argument("--output", type=Path, required=True) + return parser.parse_args() + + +def main() -> None: + """Generate the normalized runtime XML asset from the upstream model.""" + args = _parse_args() + xml = _tabletennis_spec_to_xml(str(args.input)) + args.output.parent.mkdir(parents=True, exist_ok=True) + args.output.write_text(xml, encoding="utf-8") + + +if __name__ == "__main__": + main() diff --git a/third_party/myosuite/metadata/README.md b/third_party/myosuite/metadata/README.md new file mode 100644 index 000000000..2873d0a26 --- /dev/null +++ b/third_party/myosuite/metadata/README.md @@ -0,0 +1,20 @@ +# MyoSuite Metadata + +This directory documents the generated metadata derived from the pinned +upstream `MyoHub/myosuite` source tree. + +The source of truth is `../generate_metadata.py`, and the checked-in JSON +snapshot lives at `env_ids.json`. Regenerate the JSON after changing the +upstream pin or when validating that the local metadata still matches the +pinned official registry surface. + +The vendored arm reach runtime asset lives at +`../simhive/myo_sim/arm/myoarm_reach.xml`. Regenerate it with +`../generate_arm_reach_asset.py` if the upstream arm-edit XML changes. + +The checked-in snapshot includes: + +- the direct and expanded upstream Env ID surfaces +- per-task entrypoint, runtime kwargs, and variant definitions +- normalized model and reference asset paths +- suite helper lists used by registration and validation code diff --git a/third_party/myosuite/metadata/env_ids.json b/third_party/myosuite/metadata/env_ids.json new file mode 100644 index 000000000..2f22cc8b5 --- /dev/null +++ b/third_party/myosuite/metadata/env_ids.json @@ -0,0 +1,46238 @@ +{ + "counts": { + "direct_total": 254, + "expanded_total": 398, + "myobase_direct": 45, + "myochallenge_direct": 19, + "myodm_direct": 190 + }, + "direct_entries": [ + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "airplane", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandAirplaneFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "airplane", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "airplane", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.0034753341295926136, + -0.0019032628303226819, + -0.009783215444212377, + 0.7603706132919767, + -0.004684188776560037, + 0.05456284296147362, + 0.6471765485440971 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_airplane_fly1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.017162043710335088, + -0.05210350451908287, + -0.100130872431015, + -0.1733957909697339, + -0.21108645069918056, + 0.03741697080626347, + 1.2090459179194122, + 0.2171727184343208, + 0.23253759037557747, + -0.10312540538902144, + 0.42026829612160005, + -0.17809912061587843, + -0.7935004123963937, + 0.6954749485098286, + 0.15611643114990845, + 0.42169815057566135, + 1.397347527704904, + 1.0125900641883885, + -0.14029338073296835, + 0.2004823095936883, + 1.4844325469815105, + 0.6593297088826191, + -0.15762960064885972, + 0.08936664970614303, + -0.036377132208008964, + 0.45100920489300017, + -0.30617360583644077, + 0.18028094655686863, + -0.2188913041136361 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandAirplaneFly-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "airplane", + "reference": "envs/myo/myodm/data/MyoHand_airplane_fly1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 68, + "object_name": "airplane", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.0030592201617744437, + -0.0009997399498468326, + -0.009873622491503156, + 0.8752146329089348, + 0.006722668313022363, + 0.05448343110121021, + 0.4806097250454587 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_airplane_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.0303941027111385, + -0.054567972771794704, + -0.10048009102989335, + -0.21935543652893888, + -0.2113272263302687, + 0.04339383206736948, + 1.0686030485737936, + 0.19595820775705636, + 0.003720839746134345, + 0.029731089561182686, + 0.34905487397164736, + -0.20104119910825036, + -0.8218187099438243, + 1.1267088513948051, + 0.008286992208484532, + 0.19063147539157393, + 1.5819935814757935, + 1.152110543097404, + -0.22066379364975244, + 0.5093639408354078, + 1.3158717417052375, + 0.09558693069913715, + -0.1254599706830711, + 1.5355618549726089, + -0.01611329148444456, + -0.7544514137275405, + -0.1658973334405127, + 1.083281110695559, + -0.012816579754299173 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 68, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandAirplaneLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "airplane", + "reference": "envs/myo/myodm/data/MyoHand_airplane_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "airplane", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.002923188048780966, + -0.0006155150421420551, + -0.009759644534137486, + 0.914183300908831, + 0.012047076729493397, + 0.05214608670771184, + 0.40175184619710114 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_airplane_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.0015673577921481475, + -0.09578193541769561, + -0.10040860296957, + -0.22071258298117108, + -0.18870249195824315, + 0.03867530041993899, + 1.0931949406982095, + 0.16101933732376045, + -0.0050674828898660115, + -0.0748172577638845, + 0.23447265259378913, + -0.14108731842882768, + -0.821001781837445, + 1.1555155679375968, + 0.007961438816038882, + 0.2558538747392803, + 1.497698493001484, + 1.309676553462196, + -0.23521508344131345, + 0.5396529456596417, + 0.617613143190366, + 0.5267438582557804, + -0.26560934900102023, + 1.8025076057172968, + 0.13556202210156929, + 0.29896727200646384, + -0.976639738812443, + 1.7467497605182574, + 1.5782761463988335 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandAirplanePass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "airplane", + "reference": "envs/myo/myodm/data/MyoHand_airplane_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "airplane", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandAirplaneRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "airplane", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "alarmclock", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandAlarmclockFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "alarmclock", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 57, + "object_name": "alarmclock", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.0006016799382397318, + 0.0005142000838029913, + 0.018219027303374505, + 0.8382342798119508, + -0.0013218351698278718, + 0.0003512353341550211, + -0.5453085562630247 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_alarmclock_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.04612111614103849, + -0.08764241264342766, + -0.1004618563441028, + -0.14748395729870373, + -0.219324393694761, + 0.06063207577373252, + 0.9534976202805107, + -0.07599318411142228, + -0.14675318406298987, + 0.32964421485324036, + 0.5263517972379548, + -0.39017627458652354, + -1.0967694372150865, + 1.002024405945442, + -0.23297074939728185, + 0.15999478513989082, + 0.4838037425626626, + 1.0959720937432171, + -0.4623511576268443, + 0.39864596511265665, + 1.1844290652306602, + 1.1627431791759024, + -0.548609135612602, + 0.42254235548696545, + -0.008879880473953874, + 2.3229132271729855, + -0.5839308443253649, + -0.8814382463066064, + -0.11162446958250248 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 57, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandAlarmclockLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "alarmclock", + "reference": "envs/myo/myodm/data/MyoHand_alarmclock_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "alarmclock", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0006927776769237222, + -0.0012297215356738608, + 0.01826338644061439, + 0.941436194765722, + -0.0006848588429806159, + -0.0005802140906996755, + 0.3371899842892901 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_alarmclock_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.1204089600480685, + -0.1302542081354586, + -0.10277126606994652, + -0.30355033675139015, + -0.13961508706270942, + -0.0947988559361662, + 0.5207020585848335, + 0.2119336232052866, + -0.15179205020507358, + 0.2389072860912915, + 0.6167913023928236, + -0.40022681145763783, + -0.8960851645626243, + -0.08532327242546964, + -0.16198996691647347, + 0.32434201874356494, + -0.027690533133611236, + -0.03888772007566631, + -0.3560110036714495, + 0.8526426537176208, + -0.0026641383032670126, + -0.5184934399366592, + -0.08090644275379642, + 1.0214682396490167, + -0.016427175521681575, + -0.8885162977987726, + -0.11162255630498086, + 0.9126557873084061, + 0.21867375948449697 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandAlarmclockPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "alarmclock", + "reference": "envs/myo/myodm/data/MyoHand_alarmclock_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "alarmclock", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandAlarmclockRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "alarmclock", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 90, + "object_name": "alarmclock", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 8.082348946190961e-05, + -0.0004119155453639437, + 0.01827590409327985, + 0.9999787619898971, + -0.002513267997270691, + 0.0011757810650728723, + 0.005897168135094582 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_alarmclock_see1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.021493537536659068, + -0.07621551198218506, + -0.10297265918906791, + -0.17779567462129148, + -0.2029482550240516, + 0.03947654473974176, + 0.9936965306081635, + 0.3245610650547589, + 0.055181649072252405, + 0.1509850529226119, + 0.6108522482251759, + -0.14233669913058342, + -0.900879177999881, + 0.5877921287767128, + 0.19169644883626663, + 0.2550008475121356, + 0.9462615443309019, + 0.7537504114644014, + -0.12596816229191354, + 0.12724831978459766, + 0.43560474067380434, + 0.3641986996904529, + -0.15167261570130952, + 0.5296529699853666, + -0.01991950852217558, + 0.18278367852277785, + -0.5824827350265953, + 0.7344400764506755, + -0.0946601734754417 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 90, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandAlarmclockSee-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "alarmclock", + "reference": "envs/myo/myodm/data/MyoHand_alarmclock_see1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "apple", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandAppleFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "apple", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 58, + "object_name": "apple", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0006747519285893681, + -0.00039405819377766246, + 0.01409377866767035, + 0.7027837076343542, + -0.039281469182192545, + -0.025649786164253403, + 0.709854995708509 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_apple_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.009412945889494323, + -0.07425652299610216, + -0.10106952953904644, + -0.18195141870627726, + -0.20145527875647892, + 0.050156472268738386, + 1.0680688017595563, + 0.33614468092807415, + 0.09123390152904713, + 0.12595877669362374, + 0.6656473531706332, + -0.2204732776912725, + -0.8612561031075537, + 0.3449552330790505, + 0.21532239048457355, + 0.29404333956957585, + 0.7796919513929775, + 0.2018949553503495, + -0.12126354421709679, + 0.5666690586620652, + 0.45922292748297383, + 0.17486964306370834, + -0.262865446233478, + 0.6636006559019835, + -0.0009855142136620866, + 0.3487191159946804, + -0.6698707705107109, + 0.7023631754762569, + -0.10323201749726073 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 58, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandAppleLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "apple", + "reference": "envs/myo/myodm/data/MyoHand_apple_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "apple", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0005318587637855342, + -0.0004683922617296893, + 0.014043155500679416, + 0.8443043456033467, + -0.04464457866320054, + -0.015370055792016543, + 0.5337797251453451 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_apple_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.011368775442904265, + -0.05934540512247612, + -0.10048999882251691, + -0.1553722542701375, + -0.19719877288664564, + 0.03531080098652263, + 0.8466651865735932, + 0.0738459843850125, + 0.368371995385256, + 0.1361837020796348, + 0.5428537869948001, + -0.35896898175993214, + -0.8778315715366997, + 0.55939101919487, + 0.13294013527714277, + 0.007119585871975362, + 1.0774590791809402, + 0.5151138600943249, + -0.1882915665058793, + 0.056670764188888265, + 1.2655346182887341, + 0.18663442750282958, + -0.21053397413795372, + 0.5497750853860822, + -0.004695500222860626, + 0.09055453037005312, + -0.640645103844322, + 0.7180167748875371, + -0.05013594253808404 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandApplePass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "apple", + "reference": "envs/myo/myodm/data/MyoHand_apple_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "apple", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandAppleRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "apple", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "banana", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandBananaFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "banana", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 96, + "object_name": "banana", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.018259967063944702, + -0.011369954003678203, + -0.014196834018058043, + -0.012412962201306423, + -0.5480576462555131, + -0.8343859588118361, + -0.05726086349251534 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_banana_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.0006576955328547488, + -0.07920261074571795, + -0.10125216687823746, + -0.2337049250090724, + -0.2041444995460631, + 0.06500704624865644, + 0.8799312183780618, + 0.006452993461394409, + -0.22151848494720455, + 0.1945004665192283, + 0.5535348284633401, + -0.4227060765632186, + -0.8352384349174163, + 0.8386001268372529, + -0.2634558287309238, + 0.4265272499169592, + 0.5559990785614597, + 1.2826071756244912, + -0.5611398435760658, + 0.2634029386741423, + 1.3825824621109788, + 1.7528127317436553, + -0.637296159750171, + -0.3735612470820046, + -0.03039334463032818, + 2.4597962305064094, + -0.44154610703819774, + -0.9060411622637561, + -0.1325348607985839 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 96, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandBananaPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "banana", + "reference": "envs/myo/myodm/data/MyoHand_banana_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "banana", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandBananaRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "banana", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "binoculars", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandBinocularsFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "binoculars", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "binoculars", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.00024119622480940725, + -0.005217818530968735, + 0.020400860468333004, + 0.0008638732076157364, + -0.000357846948366676, + -0.0005275479209850206, + -0.9999994236807508 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_binoculars_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.02848214105888229, + -0.07366651406806549, + -0.10094192958118745, + -0.2483297800934432, + -0.20109177857410013, + 0.06013919259293981, + 0.5363679795135274, + 0.15724990223849575, + -0.03249148702295234, + -0.16189857001236135, + 0.8897849359664806, + -0.07264265154639103, + -0.7998577775787107, + 0.47165214831911034, + 0.20225582186423685, + 0.5525051019069468, + 1.1017761728136994, + 0.6473826182868045, + -0.11889176403236348, + 0.6496731343783194, + 0.9429924381721045, + 0.584066236378993, + -0.25586296799015734, + 0.48964325532280034, + -0.037113829234699004, + 0.30383898613186844, + -0.5665696306837574, + 0.9569917151095253, + -0.07490409541272681 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandBinocularsPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "binoculars", + "reference": "envs/myo/myodm/data/MyoHand_binoculars_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "binoculars", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandBinocularsRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "binoculars", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "bowl", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0011179162240226724, + -0.00027335160121092714, + 0.005237876012703417, + 0.2595826412957461, + 0.0009391204856199893, + -0.002052199442462705, + -0.9657182606061075 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_bowl_drink2.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.09955556343484662, + -0.1470440372183246, + -0.10145998844525658, + -0.28168216271422764, + -0.09793519172923641, + -0.17599007570800376, + 0.7646740539212051, + 0.23838798743242018, + 0.014093925860045308, + 0.29396117845499437, + 0.5879975972033321, + -0.040079439502322164, + -1.0057148424357243, + -0.1647206260697659, + 0.33118445390352536, + 0.44373712349915817, + 0.10144618085245602, + -0.1860292247287983, + -0.018568534380491466, + 0.8490785725124347, + -0.002157228238595626, + -0.06018493982530457, + 0.04137102227271232, + 0.7890410213498223, + -0.008589079106315761, + -0.047862756978071505, + -0.15180075338017918, + 1.010114759228784, + -0.044071815209124586 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandBowlDrink2-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "bowl", + "reference": "envs/myo/myodm/data/MyoHand_bowl_drink2.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "bowl", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandBowlFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "bowl", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 90, + "object_name": "bowl", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.00013767191516264571, + 4.3096650299070786e-05, + 0.004878940810830477, + 0.9963022606847955, + -0.00012330873489186644, + 0.00015169355340389755, + 0.08591721095559049 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_bowl_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.14338275093702404, + -0.14786650786879474, + -0.1019897159367426, + -0.23267266974024264, + -0.08583644398161921, + -0.18353049011153688, + 0.383256412704742, + -0.1677714707989096, + 0.27417629538420474, + 0.41893155575332963, + -0.30315883776055336, + -0.38168547414133874, + -0.8923218526709613, + 0.09060548217539022, + -0.2815612862251216, + 0.2875404824159707, + -0.03270097309646322, + 1.620728304690622, + -0.36277847736522645, + 0.3091457531665934, + 1.4698580858932302, + 1.8045885660447087, + -0.3551086676822248, + 0.20491407292774608, + -0.03385988882810962, + 2.358442679773066, + -0.3697496685202636, + -0.7163232155325268, + -0.16110307539968502 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 90, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandBowlPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "bowl", + "reference": "envs/myo/myodm/data/MyoHand_bowl_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "bowl", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandBowlRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "bowl", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "camera", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCameraFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "camera", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "camera", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.006145000420691532, + -0.010786466817163743, + -0.0023685834141908904, + 0.05318626359602407, + -0.0009264342129032154, + 0.00017303023744573524, + 0.9985841642770431 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_camera_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.06324568234358353, + -0.1345035907379745, + -0.10320420887839202, + -0.1215859161285557, + -0.27566278096081687, + 0.18581310681663943, + 0.8265812337706332, + -0.10138545171207564, + 0.08738830231847931, + -0.04845965505093979, + 0.13648387969601466, + -0.14161645783407142, + -0.8149073366277644, + 0.1727377004072739, + 0.23640572935945536, + 0.38020502001470263, + 0.14468032944300918, + -0.01426901756826788, + -0.03500495712328702, + 0.611419206474186, + 0.13967687095377315, + 0.050529981094164025, + -0.053486585288265824, + 0.5266518112178774, + -0.0010599073259592304, + 0.34442713360317856, + -0.4141587630014997, + 0.6958649421093942, + -0.1376764794369314 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCameraPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "camera", + "reference": "envs/myo/myodm/data/MyoHand_camera_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "camera", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCameraRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "camera", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "coffeemug", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCoffeemugFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "coffeemug", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "coffeemug", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCoffeemugRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "coffeemug", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "cubelarge", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCubelargeFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cubelarge", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "cubelarge", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 3.299977733803399e-05, + 0.0002937624708048972, + 0.021428371642837882, + -0.03430674032425902, + -0.0346489820466298, + 0.7063943318599982, + 0.7061370571833224 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_cubelarge_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.047364031013914044, + -0.0796051913687108, + -0.10106456523095829, + -0.2468003850706707, + -0.17033230371201036, + -0.007848127426352115, + 0.7032824760829828, + 0.5398388389435763, + 0.2601159766885313, + 0.3991354439834127, + 0.9000014503400078, + -0.110472007436108, + -1.1213481930690046, + -0.07408571477999575, + 0.6510684517880762, + 0.08256141349612435, + 1.0058833846705126, + -0.24550265817916633, + 0.0015579594946806177, + 0.5985083517014619, + -0.004946676228805584, + 0.007717874670503737, + -0.04037861005303641, + 0.19380196492691087, + 0.4179970742416615, + -0.13688378085191866, + -0.3905860563494772, + 0.7131755845007619, + -0.027652329980262916 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCubelargePass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cubelarge", + "reference": "envs/myo/myodm/data/MyoHand_cubelarge_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "cubelarge", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCubelargeRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cubelarge", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "cubemedium", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCubemediumFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cubemedium", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "cubemedium", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -7.661955272063348e-05, + -5.5839457274238455e-06, + 0.004188050320713774, + 0.46781296865783323, + 0.46714697509833525, + -0.5306533690385558, + -0.5304071379045073 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_cubemedium_inspect1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.00014272051721257462, + -0.09049290162013383, + -0.08127607179520198, + -0.12103544474953659, + -0.17347211392037235, + 0.09267438487280236, + 1.575944421982324, + 0.6334920916547929, + -0.15496980564119345, + 0.04992036604464358, + 0.5511256627996864, + -0.2037101636966384, + -0.8321556013162849, + 0.9858289257736279, + 0.17638503829875224, + -0.16289803445822568, + 1.5001931992158082, + 0.6618463483565824, + -0.32958958211731665, + 0.10654716158019528, + 0.2681998614632581, + 0.4465373014360697, + -0.5708850717904893, + 0.022284358280926987, + 0.09392766736496151, + -0.2141566102206119, + -1.2628365694065882, + 0.5566924491308762, + 1.6111996144217864 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCubemediumLInspect-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cubemedium", + "reference": "envs/myo/myodm/data/MyoHand_cubemedium_inspect1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "cubemedium", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCubemediumRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cubemedium", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "cubesmall", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCubesmallFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cubesmall", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 57, + "object_name": "cubesmall", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.000115145254942153, + -0.00010184185049416476, + -0.01470754142087408, + -0.033090549194725684, + -0.033565946849088354, + -0.7062281255831413, + -0.7064136022200015 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_cubesmall_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.06424840126420002, + -0.03519845039666138, + -0.1004056766243438, + -0.15735051679514356, + -0.27706137082282467, + 0.18406182190470838, + 1.4584189954895768, + 0.5087108792291192, + -0.19270957504437025, + 0.45223807889363626, + 0.564559749836207, + -0.4841569951654123, + -1.3149306878561986, + 0.9807556443876607, + -0.14955963552584775, + 0.31689409329191415, + 0.8086351678340569, + 0.4831035968072485, + -0.304610402424413, + 0.5625679144821621, + 0.805429981371772, + -0.349286098311239, + -0.13955501520210595, + 1.0277327660115465, + -0.017383567332930703, + -0.8840688746318025, + -0.2951652039220058, + 0.6225606246330272, + 0.8491406046713108 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 57, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCubesmallLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cubesmall", + "reference": "envs/myo/myodm/data/MyoHand_cubesmall_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "cubesmall", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.00010596321281110473, + -1.80804076732686e-05, + -0.014663560070063676, + 0.0037754944573748237, + 0.00027272958791703904, + -0.7087548154615104, + -0.7054447411529088 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_cubesmall_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.021961349855919944, + -0.06318932421345429, + -0.10044639305273366, + -0.16519027177494677, + -0.22962635938146844, + 0.14210012130643798, + 1.5876096877904216, + 0.5226471269423028, + -0.25587056792612134, + -0.013841085223846916, + 0.35793823849342893, + -0.2204644316760947, + -0.7902566776764388, + 0.9668678356461574, + -0.0417009009585847, + -0.07265971814767591, + 1.4789744491104841, + 0.9804599940732178, + -0.323989030587043, + 0.34856144736658445, + 1.196182285780996, + -0.09350156222467164, + -0.10978108732032525, + 1.167964226120047, + -0.02487450257860345, + -0.686753385019311, + -0.2072537422198139, + 0.8507334308149014, + 0.28461074519307095 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCubesmallPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cubesmall", + "reference": "envs/myo/myodm/data/MyoHand_cubesmall_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "cubesmall", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCubesmallRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cubesmall", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "cup", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0009643087298893574, + 0.00031177539859810773, + 0.01614537560538757, + 0.43411876340016003, + 0.002203394999148667, + 0.002377847605590592, + -0.9008498155381718 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_cup_drink1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.010589832026642007, + -0.08982901867278466, + -0.10085587961287117, + -0.3152102306159233, + -0.22174884490878433, + 0.08811149697293266, + 0.22219487819952713, + 0.22736290521757743, + -0.1095194895077276, + 0.09580584875189321, + 0.714062762055289, + -0.18204351128471036, + -0.8527013107223866, + -0.09620583336296555, + 0.09521760243287428, + 0.42072446688583093, + -0.022524104355094613, + 0.4010739916267628, + -0.05605655704691138, + -0.011323344010731705, + 1.4469596704850902, + 0.22894154795314697, + 0.04323136765441826, + 0.3479541298088417, + -0.02655723623023825, + 0.11321812505013501, + -0.03607124266983322, + 0.6776644099298986, + -0.1895045040901985 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCupDrink-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cup", + "reference": "envs/myo/myodm/data/MyoHand_cup_drink1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "cup", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCupFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cup", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "cup", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.00023433418488454658, + 0.00011804439136277416, + 0.016173894760496937, + 0.014077671470076391, + 0.00010414500475144022, + -0.0003292402491392732, + 0.9999008450444759 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_cup_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.10727778251119652, + -0.12722349936298227, + -0.10216149517675539, + -0.24350386493425993, + -0.14666914267298675, + -0.10548492722764022, + 0.43293666919912316, + 0.053845082238178875, + 0.12214326346562779, + 0.047822591476754446, + 0.5547790429274194, + -0.1415456400968491, + -0.852723556000257, + -0.10876142393674441, + 0.2586033064077457, + 0.35791768036382704, + -0.006050594303123081, + 0.1941187369058787, + 0.046056581789794826, + 0.1500197473522709, + 0.17506618611403962, + 0.01753685689156946, + 0.14905884071202985, + 0.6742292024371822, + -0.011135787309417463, + 0.009347081863154812, + -0.07774801730592457, + 0.8377054562842365, + -0.0923448584643816 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCupPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cup", + "reference": "envs/myo/myodm/data/MyoHand_cup_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 82, + "object_name": "cup", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0009047632229811623, + 0.00042885398370042893, + 0.01612508727652187, + 0.6280752832308919, + -4.2553222371813316e-05, + -0.0012706658624698258, + 0.778151541919647 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_cup_pour1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.0018695979799811348, + -0.0959884941986213, + -0.10379779420690374, + -0.30506249085207293, + -0.2305109173847771, + 0.11036518587690526, + 0.23767362203493206, + 0.10856300476712827, + -0.027143306985434244, + 0.12470038330808936, + 0.4137722530104803, + -0.18999791833854324, + -0.9182065480119409, + -0.36903763510805715, + 0.1636256355900811, + 0.08200841342358303, + -0.032823334853653165, + -0.20575692728393813, + 0.019555378064023937, + 0.4169044830806235, + -0.00844994577598804, + -0.28372106878715353, + 0.13905163795472905, + 0.6218214583162669, + -0.0029709487052623665, + -0.2126258661783111, + -0.009159061006192957, + 0.5463132189285749, + -0.03595601870108867 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 82, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCupPour-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cup", + "reference": "envs/myo/myodm/data/MyoHand_cup_pour1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "cup", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCupRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cup", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "cylinderlarge", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCylinderlargeFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cylinderlarge", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "cylinderlarge", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.002001089794205369, + 0.0004731390839994185, + 0.02632510594718468, + 0.003932036960066385, + 0.9968367636057913, + -0.07922533530049193, + -0.004934780218096034 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_cylinderlarge_inspect1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.004123455140433093, + -0.1400929206951431, + -0.10035002361229196, + -0.25262906316879324, + -0.23104133493700768, + 0.13065324043157703, + 0.18415641209222025, + -0.03476627419474529, + -0.327024031043646, + 0.5310030438422558, + 1.0063935241412276, + -0.43011411190382803, + -0.9813315174143407, + -0.17705518417284738, + 0.17245271757644445, + 0.12765544696751321, + -0.017187727715890062, + -0.30638723925681566, + -0.09416476364643131, + 0.4829373185498728, + -0.009639324470605708, + -0.24736228743980593, + -0.09667683169479215, + 0.5112368767355185, + 0.16996422618784135, + -0.4454419586280001, + -0.17106255673422224, + 0.8837982970023734, + 0.04880948684448142 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCylinderlargeInspect-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cylinderlarge", + "reference": "envs/myo/myodm/data/MyoHand_cylinderlarge_inspect1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "cylinderlarge", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCylinderlargeRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cylinderlarge", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "cylindermedium", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCylindermediumFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cylindermedium", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 54, + "object_name": "cylindermedium", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0003341562400193878, + 0.000574366744739929, + 0.004492692602962714, + 0.0002431233154671786, + 0.9999906986966633, + 0.004299239869455341, + 0.00024484231111174276 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_cylindermedium_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.027807502552836233, + -0.06304008303043494, + -0.10038914538329514, + -0.09417670095118012, + -0.2172274509448453, + 0.07236053353801312, + 1.2310417251525734, + 0.24234994022503437, + 0.2547481284716228, + 0.4153808862418216, + 0.6649694234460051, + -0.46588750930535777, + -1.0345578340596424, + 0.4161886247024945, + -0.17828822364429953, + 0.30960040490826957, + -0.010818408035928822, + -0.0332672541496216, + -0.42825699082187474, + 1.1124821719164313, + -0.006693548431117681, + -0.12073330045969764, + -0.551055258269723, + 1.0472747680287073, + 0.006286937395585984, + 2.2203937414524524, + -0.8120433609791099, + -0.533662540647611, + -0.10101509650939992 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 54, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCylindermediumLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cylindermedium", + "reference": "envs/myo/myodm/data/MyoHand_cylindermedium_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "cylindermedium", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 7.751658041989988e-05, + 0.0003211940804916197, + 0.004548303166708963, + 0.0007184917477226266, + 0.9942462879832027, + -0.10711550222589744, + 0.0002679255952096757 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_cylindermedium_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.039121684330962844, + -0.09055595621867285, + -0.1017489892141217, + -0.1424609425332945, + -0.20873356865420256, + 0.07747357505209136, + 0.9047441292461207, + -0.1213281914342276, + 0.08035018044702948, + 0.30775472107661805, + 0.5854408127261456, + -0.42951496063910427, + -0.9501922356736685, + 0.21853428174969636, + -0.043284533816057645, + 0.2526876976240094, + -0.012428857571496957, + -0.18814289059072978, + -0.32513244458061025, + 0.9957702581390692, + -0.010225788353548987, + -0.2507674386958774, + -0.40131404556718436, + 1.0367753565703088, + -0.010149789898071193, + 1.7055057887828478, + -0.8309536294896409, + -0.47011661523831155, + -0.16614176606886571 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCylindermediumPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cylindermedium", + "reference": "envs/myo/myodm/data/MyoHand_cylindermedium_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "cylindermedium", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCylindermediumRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cylindermedium", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "cylindersmall", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCylindersmallFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cylindersmall", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "cylindersmall", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0002046913269512813, + 0.0005051939285872619, + -0.014394474983988628, + 7.465330963368224e-05, + 0.44636686478166426, + 0.8948497962565752, + -0.0006771937954888924 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_cylindersmall_inspect1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.046093229909840404, + -0.06427920261574334, + -0.10046813522128027, + -0.24764049373175723, + -0.22054745074379667, + 0.00877516988685637, + 1.0335326772424314, + 0.3188772633935616, + -0.09377237358693107, + 0.22681783200765251, + 0.5411701968343885, + -0.4690242236522507, + -0.8734985347942059, + 0.8615695711505419, + -0.1416270564447536, + 0.19136808263535104, + 0.6324603225021516, + 0.8468544873912844, + -0.4022142223183665, + 0.8549316932620163, + 0.31455666083228934, + 0.1630264724836797, + -0.43233984739134756, + 1.519170806875472, + -0.017211242930691432, + -0.5711248569648576, + -0.8787725518522531, + 1.2679886432021743, + -0.15125242732462152 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCylindersmallInspect-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cylindersmall", + "reference": "envs/myo/myodm/data/MyoHand_cylindersmall_inspect1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 87, + "object_name": "cylindersmall", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0002488260008807393, + -7.289968053446386e-05, + -0.014553152403176759, + 0.00043053353573322084, + -0.9977582129990146, + -0.06691923539246762, + -0.00042304653994363 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_cylindersmall_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.007186481932529064, + -0.12584658304856208, + -0.10263207501421179, + -0.23658512346573818, + -0.21655107943042584, + 0.10062537321492637, + 0.740015007990705, + 0.07064167127837065, + -0.22226657486731843, + 0.0992626225108839, + 0.3952925782469232, + -0.3680717027469361, + -0.8373792530631455, + 0.5559419395352986, + -0.10341099577058487, + 0.40396597693838515, + 1.1212941368035676, + 0.4156757191798695, + -0.2694074755629115, + 1.0251051633496955, + 0.846068951990407, + -0.4152146467447744, + -0.05096030292081675, + 1.4767229149907593, + -0.029518806884839616, + -1.191254580631719, + 0.04192511812582208, + 1.2032770321487338, + -0.036146355280236186 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 87, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCylindersmallPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cylindersmall", + "reference": "envs/myo/myodm/data/MyoHand_cylindersmall_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "cylindersmall", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandCylindersmallRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "cylindersmall", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "duck", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandDuckFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "duck", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "duck", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0010302098380222079, + -0.0017233039106355269, + 0.013128748687378632, + 0.9342650434981536, + 0.0022686648753782964, + -0.0015798193335693279, + -0.35656862709435017 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_duck_inspect1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.00831315949260508, + -0.11177430777474512, + -0.1005199778794928, + -0.3027268026343859, + -0.2094488430857874, + 0.07730351168784703, + 0.7123533656572966, + 0.03785000221736924, + -0.2842381691276841, + 0.30572978770838793, + 0.5494079388941675, + -0.27373544241759357, + -1.0020709812997552, + 0.5083079573999367, + 0.002334746092745533, + 0.5111953203107119, + 0.004081601464152351, + 0.42575285696925463, + -0.16365395387380158, + 0.934654544534165, + 0.019911823320803313, + -0.405739674181957, + 0.011535801817204783, + 1.3234527409471133, + -0.022294518505757234, + -1.192882105887219, + 0.14587406187494906, + 0.9464013413514407, + 0.22038048800402962 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandDuckInspect-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "duck", + "reference": "envs/myo/myodm/data/MyoHand_duck_inspect1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 68, + "object_name": "duck", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.0077031638509692454, + 0.003585668205448139, + 0.013242783334620267, + 0.5047530078624177, + -0.0011270481238537685, + 0.0008519772363137797, + 0.8632626511967012 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_duck_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.06122489757529728, + -0.07201098996128155, + -0.10046588294982206, + -0.29691115436543164, + -0.22480250933467263, + 0.056127003253949787, + 0.7489905832631718, + -0.010676032865479149, + -0.3331207101700665, + 0.26446770386206375, + 0.7450273418334401, + -0.512129010563898, + -0.885857250324907, + 0.6485290486053819, + -0.3797636064413776, + 0.403833274174082, + -0.02084266098435553, + 0.27156636045908583, + -0.534319690469227, + 1.2784754505690072, + -0.001846848152234387, + -0.10923988700265058, + -0.48511240223350843, + 1.6390944685201865, + -0.031911574354737376, + -1.3627170822692174, + -0.008443971061811419, + 1.5648617210339788, + -0.15992275004993275 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 68, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandDuckLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "duck", + "reference": "envs/myo/myodm/data/MyoHand_duck_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "duck", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0011555620082811151, + -0.0013347597088447603, + 0.01284487425442028, + 0.9570496164133637, + -0.007459416957738767, + -0.006441573749490361, + -0.28975661329694224 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_duck_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.008030953292270215, + -0.07671392248338532, + -0.10041076787750104, + -0.32317899095264635, + -0.21000783498999637, + 0.04358222079933078, + 0.6700376164297305, + 0.16428880137849752, + -0.15874414508816248, + 0.1731658386886065, + 0.6351298217992289, + -0.22048969626558435, + -0.8784488286784821, + 0.3301138671483863, + 0.08744004941588165, + 0.3169057990695373, + 0.03759944735759533, + 0.9161590209597826, + -0.14999130593847085, + 0.09269219463270466, + 0.7922725697487636, + 0.20164410164288082, + -0.018428961401234848, + 0.9786129805133246, + -0.01717582461077154, + -0.1668369831459174, + -0.21028374682700607, + 1.1102593779155308, + -0.020226655088654164 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandDuckPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "duck", + "reference": "envs/myo/myodm/data/MyoHand_duck_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "duck", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandDuckRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "duck", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "elephant", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandElephantFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "elephant", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 58, + "object_name": "elephant", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -2.402876283912796e-05, + 0.0003060289216984053, + 0.015973990910398674, + 0.9998044442035848, + 0.00023627812979788495, + -0.00013360878515206664, + 0.019773711641943827 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_elephant_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.006652416416556285, + -0.05107184214501926, + -0.10061251253413846, + -0.11309768606623627, + -0.20600459747088595, + 0.04497144969696453, + 1.2302620036296246, + 0.5305484265001162, + 0.14529886651540028, + 0.20992320566330902, + 0.5948723114259443, + -0.20601853566543932, + -0.9043484244025147, + 0.795315340967816, + -0.05873666782307246, + 0.061810062447604903, + 0.5062356866989366, + 0.9436129567326188, + -0.3408815452687306, + -0.024429071320150476, + 1.1572427553651812, + 0.5506139972486801, + -0.36862748589686517, + 0.7112384957289617, + -0.015092902428916751, + 0.38382756396302947, + -0.8217828498372969, + 0.21232268502893042, + -0.09530053641000662 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 58, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandElephantLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "elephant", + "reference": "envs/myo/myodm/data/MyoHand_elephant_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "elephant", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.00026429675840388116, + -0.0005663735733839775, + 0.015969992328201912, + 0.9773929471132393, + -0.00012254603499976162, + -0.00020833014423930928, + -0.21143076529757288 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_elephant_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.016508187791067712, + -0.1147578312341724, + -0.10226479584464088, + -0.12222002468816058, + -0.192997548930877, + 0.045486028158587245, + 1.0190903262733082, + 0.15012855030554095, + 0.05464673503984843, + 0.1296507562763375, + 0.2861307848190853, + -0.27524088945844744, + -0.845526543106507, + 0.5351661158414525, + -0.08002940861887485, + 0.10661464209276268, + 0.6414510799161341, + 0.5793267281233515, + -0.3234035156474008, + 0.23351966901632562, + 1.3346572924105795, + 0.24791227197777357, + -0.2429230513552861, + 0.6592091980633074, + -0.014563634789730887, + 0.00024058828398970233, + -0.5907428523039896, + 0.4012571486370226, + -0.0542700271245097 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandElephantPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "elephant", + "reference": "envs/myo/myodm/data/MyoHand_elephant_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "elephant", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandElephantRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "elephant", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "eyeglasses", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandEyeglassesFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "eyeglasses", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "eyeglasses", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.015419172546344865, + -0.009498674427104942, + -0.016870730479621475, + 0.8736021532937873, + 0.046993122441713095, + -0.026835100461710282, + -0.4836225817585923 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_eyeglasses_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.09951467448575689, + -0.11370315694323328, + -0.10272304156942372, + -0.1221812876565767, + -0.29456725740681555, + 0.17863621294233728, + 1.7157801283083485, + 0.42913188756462867, + -0.4183437213745014, + -0.29301473269871686, + 0.19576830360453373, + 0.012412440799071897, + -0.8032744420167744, + 1.2328240957387733, + 0.11340570859470361, + 0.09226430431570845, + 1.5778359839357718, + 1.2417705250012538, + -0.1676068406590321, + 0.31231511854745536, + 0.9744471970334773, + 0.5487777161569815, + -0.21242954004805523, + 1.6727142221596523, + 0.19837284284400802, + 0.5004229265516021, + -0.8812484091601598, + 1.7188312532069085, + 1.5844539258050314 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandEyeglassesPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "eyeglasses", + "reference": "envs/myo/myodm/data/MyoHand_eyeglasses_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "eyeglasses", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandEyeglassesRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "eyeglasses", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "flashlight", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.011385998656012326, + -0.006837180140800319, + -0.006493312506152506, + 0.22150148769462358, + -0.7076513683782307, + -0.1602604127229309, + -0.6515237769225017 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_flashlight_on1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.0037511835374580906, + -0.05450226532006764, + -0.10451051102180693, + -0.22604193018074473, + -0.1834710272167852, + 0.08079835424155199, + 1.924274858447442, + 0.6710584300233134, + -0.47707034257971426, + -0.28188165702606677, + 0.2091726535619601, + 0.03279620627511848, + -0.7937871056599197, + 0.4464372073409983, + 0.18410323086300187, + 0.4614610718986794, + 1.191193648321357, + 0.5881320359897146, + 0.0014569071274487823, + 0.4889488965695785, + 1.428088475088276, + 0.2785200237548868, + 0.01043987434897736, + 0.7618511317100836, + -0.03244250832457071, + 0.09047425583495683, + -0.13582969365139896, + 1.0923878106417908, + -0.12094422937368066 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandFlashlight1On-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "flashlight", + "reference": "envs/myo/myodm/data/MyoHand_flashlight_on1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "flashlight", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.009163634477426163, + 0.009916720987292703, + -0.005806566430423803, + 0.42166136436850143, + 0.7142932788366148, + -0.12481890627354306, + 0.5444327747705939 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_flashlight_on2.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.02483395383935681, + -0.04092801793077819, + -0.10292982949704463, + -0.2425139071357342, + -0.21208461958363684, + 0.07089415525228125, + 2.2401570699145323, + 0.5350555686921018, + -1.0013612841822248, + -0.4278438130455592, + 0.16004673026350452, + 0.07744899588510408, + -0.7938989504396294, + 0.15258884608203818, + 0.5521633329467902, + 0.8550587372290352, + 0.017901463264697492, + -0.034578995860749626, + 0.22182451389806626, + 1.4174027947897074, + -0.0030353750872601326, + -0.1083848919261985, + 0.09964384545474253, + 1.4328987258462855, + -0.00501762791580299, + 0.060777050625548265, + -0.3516221350874927, + 1.2759486601652725, + -0.05231944603331302 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandFlashlight2On-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "flashlight", + "reference": "envs/myo/myodm/data/MyoHand_flashlight_on2.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "flashlight", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandFlashlightFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "flashlight", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 54, + "object_name": "flashlight", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.010831684448643603, + 0.008222808380614172, + -0.005791090268993819, + 0.5833683033146287, + 0.054701894540526486, + 0.7217459041923324, + -0.3684724890722252 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_flashlight_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.03300653278502228, + -0.03295585485399945, + -0.10046740667288585, + -0.22154519879158574, + -0.22325713656243057, + 0.08488418651501763, + 1.4866864936998327, + 0.39140631233229006, + -0.28278342329931067, + 0.3313703542545019, + 0.38100110777681095, + -0.4309578177277167, + -1.0024477841031458, + 1.2580851855949475, + -0.07713516772060908, + 0.2114967836765718, + 1.5417028742172694, + 1.6565545318181172, + -0.38273467496647784, + 0.06122145140909162, + 1.4960737896598304, + 1.8702475344059297, + -0.4625172130939911, + 0.07303802744365606, + -0.03391487891248534, + 2.6805313390124677, + -0.6603714577553773, + -0.7197431095840876, + -0.09839566677066235 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 54, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandFlashlightLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "flashlight", + "reference": "envs/myo/myodm/data/MyoHand_flashlight_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 89, + "object_name": "flashlight", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.012960049695931676, + -0.00018212014600111988, + -0.006588781424859769, + 0.2670170735763201, + -0.6717912994115117, + 0.27436706112607506, + -0.6341301508545159 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_flashlight_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.03495323682778943, + -0.10393793432845262, + -0.10059959547832303, + -0.22789224867326993, + -0.18579688824494864, + 0.04933691397211212, + 1.5032154596505989, + 0.4307410307704764, + -0.3745188409847609, + 0.01138943646640552, + 0.3098678185912981, + -0.1499008290476446, + -0.814630789394761, + 0.9907675996002029, + 0.08886826823582619, + 0.4028961631560149, + 1.4982958257444345, + 1.2068669511828884, + -0.24121091606501158, + 0.37986964812443397, + 0.46528291271441435, + 0.8205831477690956, + -0.3622344764943468, + -0.09032790161675729, + -0.04778136605987516, + -0.05524987405654615, + -0.7969993077230337, + -0.09231731686481344, + 0.07627588454694648 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 89, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandFlashlightPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "flashlight", + "reference": "envs/myo/myodm/data/MyoHand_flashlight_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "flashlight", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandFlashlightRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "flashlight", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "flute", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandFluteFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "flute", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 90, + "object_name": "flute", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.00019235124944967305, + -0.0009153035686558827, + -0.026047337698716408, + 0.8863907895003859, + -0.46040867054858825, + -0.018874918497089152, + 0.04448552376102448 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_flute_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.012292203705359755, + -0.08261470784461938, + -0.10029213109196403, + -0.22658926558510922, + -0.19910295431889655, + 0.10426317021619222, + 1.4280842475629782, + 0.38558852776356634, + -0.2006171208013037, + -0.10342211485840373, + 0.09255817800845302, + -0.34215661213776655, + -0.7873348686564784, + 0.8219354317854197, + 0.03213610718919998, + 0.07834894292250678, + 1.4834488382066078, + 0.6091266574918229, + -0.17435329896433371, + 0.6290076845359728, + 0.5770972303433641, + 0.053938880133296084, + -0.14355895791030843, + 1.0437277641630232, + -0.013421673384486429, + -0.20860698800748348, + -0.4744392552695583, + 0.6191925463200729, + 0.17925351747933482 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 90, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandFlutePass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "flute", + "reference": "envs/myo/myodm/data/MyoHand_flute_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "flute", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandFluteRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "flute", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "gamecontroller", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandGamecontrollerFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "gamecontroller", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 72, + "object_name": "gamecontroller", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.00024462004822198107, + 0.00012209906193912361, + -0.009029907670136124, + 0.9999030168924432, + 3.346118757100065e-05, + -0.0019434137970538118, + 0.013790534164886886 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_gamecontroller_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.01790095513098223, + -0.037986399199346786, + -0.10213079491022717, + -0.22246172094123196, + -0.1812357314682611, + 0.048604190037484486, + 1.5479095676174173, + 0.4452515779149564, + -0.12000759578833994, + 0.4692182082713236, + 0.662339302750692, + -0.23308000792002465, + -1.1550354823257287, + 0.30796457724465204, + 0.30318444507087794, + 0.15366681929887901, + 0.7827206822209876, + 0.15954749834178708, + -0.21350278042470675, + 0.596374480613087, + 0.22306557159005733, + 0.8336058400380869, + -0.4475935900924783, + 1.2691175849212888, + 0.011876551829407575, + 0.7813933805956063, + -0.9018938898485257, + 1.4505160529085435, + 0.7812415693301431 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 72, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandGamecontrollerPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "gamecontroller", + "reference": "envs/myo/myodm/data/MyoHand_gamecontroller_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "gamecontroller", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandGamecontrollerRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "gamecontroller", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "hammer", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandHammerFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "hammer", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "hammer", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.006921485573832344, + 0.009422182726275135, + -0.0231612721153598, + 0.9789334702048226, + -0.0005581382539746084, + -0.0012769929522725297, + -0.2041747258683923 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_hammer_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.01416321580450705, + -0.06408893091116158, + -0.10222185232906396, + -0.1995813728346816, + -0.2007862615623465, + 0.06080767252816511, + 1.2980592659604309, + 0.288186169167499, + -0.08038300050407816, + -0.1605128565115831, + 0.0925251007990752, + -0.016988742716120258, + -0.808842362416356, + 0.8594110753766189, + 0.003919249937894159, + 0.0951427020937262, + 1.285187879423097, + 0.8027231974530473, + -0.22215572500560293, + 0.42162163516326717, + 0.7961719479513799, + 0.7425233294512574, + -0.23230315413274846, + 0.43249670898900294, + -0.021732921156247108, + 1.249872857669053, + -0.4337117999533187, + -0.7421767372621607, + -0.18373680418065763 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandHammerPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "hammer", + "reference": "envs/myo/myodm/data/MyoHand_hammer_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "hammer", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandHammerRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "hammer", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "hammer", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.015961587884511345, + 0.015455824612053231, + -0.023073843696006617, + 0.9206642028754594, + -5.371119162460818e-05, + -0.0005576249075280106, + -0.39035510975682836 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_hammer_use1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.02731469434632699, + -0.06312997357712385, + -0.10045625998560674, + -0.2651386338981795, + -0.2148715303890316, + 0.05888162808030879, + 1.0910069687614634, + 0.09509004968035689, + -0.21948713514229065, + -0.15146985841550756, + 0.21251934909666248, + -0.16658421891438244, + -0.7827731012896453, + 0.912188126477188, + -0.07257525085149992, + 0.03722282256977514, + 1.5143452732637388, + 0.6052192771197332, + -0.3136803345702107, + 0.9128249076352282, + 1.3527240700435197, + 0.18779448463641235, + -0.32823023677612545, + 1.378353153094377, + -0.014244162128000771, + -0.2807526672296616, + -0.7120604333836672, + 1.2451302151394286, + -0.008881076860691672 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandHammerUse-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "hammer", + "reference": "envs/myo/myodm/data/MyoHand_hammer_use1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "hand", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandHandFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "hand", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "hand", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.002955205361812864, + 0.002556149651187057, + 0.034727530106396266, + 0.9863180778658139, + 0.0026236455540953577, + 7.947595153463718e-05, + -0.16483251937244087 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_hand_inspect1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.05939601841883351, + -0.0034923992010815873, + -0.10142445016561455, + -0.2586531433812461, + -0.2251863300258339, + 0.15597672268361346, + 0.7296613761279978, + 0.6820808020392234, + -0.24053632660315286, + -0.16782574306591236, + 0.6060289518992804, + -0.029135348199299773, + -0.8034068881592963, + 0.8123872639917805, + 0.17736464996002083, + 0.19167330950449843, + 0.25942054300904127, + 0.9902077409620351, + -0.1383086606322552, + 0.1057370000536497, + 0.03782469732097117, + 0.3857874065014335, + -0.27440052898731226, + 0.5447582490238688, + -0.012959035542446682, + 0.20085549227577848, + -0.759532274151673, + 0.5107384964804227, + 0.1631052072293028 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandHandInspect-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "hand", + "reference": "envs/myo/myodm/data/MyoHand_hand_inspect1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "hand", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandHandRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "hand", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "headphones", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandHeadphonesFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "headphones", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "headphones", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.000425895220903304, + 0.0017307551063302333, + -0.005340916072484762, + 0.010248885305285067, + 0.006839607446838613, + 0.9877320470425074, + -0.15567139546233355 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_headphones_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.05666211435853644, + -0.10140407389305262, + -0.10046218606162845, + -0.19465284261647314, + -0.18429464231199055, + -0.025858324747634748, + 0.7418151271384349, + -0.412502380430316, + 0.15205642684937215, + 0.2482421886023031, + 0.17095367539475592, + -0.42540862866517476, + -0.861650563309133, + 0.9703892971394564, + -0.28167524523573206, + 0.194514129874187, + 0.04154638716819561, + 0.7750528765473897, + -0.45232669732988945, + 0.9175335286726958, + 0.09728702676095599, + 0.7977538171784296, + -0.5276305510339572, + 0.8755452481651883, + -0.0017802545158828746, + 2.2282448594669457, + -0.6333014231684589, + -0.8462163526417014, + -0.12849245259949224 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandHeadphonesPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "headphones", + "reference": "envs/myo/myodm/data/MyoHand_headphones_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "headphones", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandHeadphonesRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "headphones", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "knife", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.003054852591932278, + -0.0002686307668266427, + -0.027512811031496616, + 0.05792833451354023, + 0.08045045979479562, + 0.9947416323229978, + -0.025712185878897294 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_knife_chop1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.18494731435034925, + -0.17700028334969714, + -0.10052034076338162, + -0.2576173750037992, + -0.1233138735817367, + -0.16392085446135682, + 1.3308905561719775, + 0.12765505893619533, + -0.052175982641409505, + 0.34614478581450125, + 0.1311166040866976, + -0.6114865939167631, + -0.9062065815518937, + 1.0042890523476637, + -0.2667324946812746, + 0.5801759391287196, + -0.0018588678982585673, + 1.3709800831854753, + -0.49656655777746345, + 1.015601174608633, + 0.26906647247899346, + 0.6937269325729988, + -0.6052491553146219, + 1.8603991933228192, + 0.06810881981169963, + -0.7913584842754179, + -0.8655207880894458, + 1.1541464410123359, + -0.30701624449026915 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandKnifeChop-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "knife", + "reference": "envs/myo/myodm/data/MyoHand_knife_chop1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "knife", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandKnifeFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "knife", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "knife", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandKnifeRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "knife", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "lightbulb", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandLightbulbFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "lightbulb", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 95, + "object_name": "lightbulb", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.013064899397179777, + -0.012261870828781392, + -0.005560044979203052, + -0.2304624061442339, + 0.657899610979591, + -0.5998408082028313, + 0.39274188220978806 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_lightbulb_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.06171144314989671, + -0.12821825794667477, + -0.1000004265692596, + -0.12296405298539702, + -0.3016934705150445, + 0.303418925782863, + 0.7656583742530008, + -0.2223282965802109, + -0.3231405964827677, + 0.22755429582632766, + 0.4851129051713877, + -0.5898245720559401, + -0.8555938460413518, + 0.41569437381179836, + -0.17823413850707492, + 0.2654129027418988, + -0.018560969246206205, + 0.12479195066421701, + -0.4616338729072975, + 0.9827926842804376, + -0.01111817742723203, + -0.12651179515178618, + -0.5093205126961238, + 1.3718227355712371, + -0.02576100743137423, + -0.8327268347520642, + -0.6666993697014051, + 0.9517500898677275, + -0.2016969185947932 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 95, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandLightbulbPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "lightbulb", + "reference": "envs/myo/myodm/data/MyoHand_lightbulb_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "lightbulb", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandLightbulbRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "lightbulb", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "mouse", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandMouseFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "mouse", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 68, + "object_name": "mouse", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.0010315985557547089, + -0.00217521225486023, + -0.01751603311869382, + 0.9469591250435837, + -0.004207142966555491, + 0.00460229528775879, + -0.321293532961424 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_mouse_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.023779884720601416, + -0.0761494311832228, + -0.10043276149074615, + -0.23499594988037925, + -0.21282904001008204, + 0.06535021024230116, + 1.007691502589389, + 0.15569456204601362, + -0.07821901630337744, + 0.1679345492906187, + 0.6834241453188677, + -0.4402704987384868, + -0.8447808390685357, + 0.2840726076409966, + -0.2723120689859014, + 0.1291699083259659, + 0.4054003455873921, + 0.4119206612163371, + -0.42368043277708967, + 0.9871396055714313, + 0.7329649988469691, + -0.05357926305347315, + -0.3099709503074097, + 1.410262328303948, + -0.026451557099371522, + -0.9489134690696536, + -0.09775805592712941, + 1.090506709210135, + 0.15024101269318124 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 68, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandMouseLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "mouse", + "reference": "envs/myo/myodm/data/MyoHand_mouse_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "mouse", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.0008090519678723681, + -0.002074506688626811, + -0.017454481447859847, + 0.9362929270184491, + -0.0020118859236675736, + 0.0006405157299066804, + -0.3512137481219306 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_mouse_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.06396650394657777, + -0.11525026227337891, + -0.1004264133353941, + -0.2508679851508682, + -0.28771454445974687, + 0.2089950692931208, + 0.76312886318014, + -0.08044911260069566, + -0.1802690086392494, + 0.07934538662646308, + 0.3523645056240964, + -0.24059008435045146, + -0.8373236744967956, + 0.14898475928656765, + 0.10773986907157285, + 0.2809909901941645, + -0.0037529559373852476, + 0.13159810996269736, + -0.07222114096825866, + 0.37270796900503744, + 0.11661506556025562, + -0.19843996864756167, + 0.14811847986696333, + 0.885200889940046, + -0.009981615804933569, + -0.7211426467089868, + 0.16002842703323886, + 0.9006073904673303, + 0.2328044266370653 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandMousePass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "mouse", + "reference": "envs/myo/myodm/data/MyoHand_mouse_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "mouse", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandMouseRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "mouse", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "mouse", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.0029002671399761975, + -0.003092935096980162, + -0.017616580004840073, + 0.6916446214723458, + 0.0001691650480762548, + -0.0001563759548247013, + -0.7222379556061304 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_mouse_use1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.0004911982816306077, + -0.11718847351860263, + -0.10225750199351626, + -0.2400443163223774, + -0.25232985143616465, + 0.163552319730492, + 0.9069614959346444, + -0.028481296580236393, + -0.3687285058264655, + 0.4032105922596677, + 0.3711863225116322, + -0.5707493546741108, + -0.9773294452212359, + 0.3179224038438467, + -0.17204799944157564, + 0.11297958957680451, + -0.019873039260031325, + 0.23041686465497058, + -0.5132903279025851, + 0.9553344442442528, + 0.19524275181672626, + -0.24493040795429283, + -0.4311382998909839, + 1.418713432518296, + -0.03577279638949718, + -1.0582802208872155, + -0.41830905366294213, + 1.0077079940017502, + 0.705365716756027 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandMouseUse-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "mouse", + "reference": "envs/myo/myodm/data/MyoHand_mouse_use1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "mug", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0006835262161112386, + 0.0023920440942197378, + 0.01900903409418283, + 0.9970521526498817, + -0.0006398458863834413, + -0.0025893303485282444, + -0.07668044641122519 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_mug_drink3.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.03695053488992741, + -0.07804305908862479, + -0.10241268060785239, + -0.29255534137816286, + -0.24371988308424755, + 0.1120415720359343, + 0.38857871238101305, + 0.06445695757974536, + -0.2646659180762475, + -0.40856316474086496, + 0.18810255111248336, + 0.42844939482512107, + -0.8270859618434122, + 0.8875051095931308, + 0.20733418886816005, + 0.426607186182124, + 1.5723617391007705, + 1.039304841602222, + 0.06179326825161477, + 0.2947881903798712, + 1.3498702144555232, + 0.9810896660193321, + -0.01717602007092364, + 0.20944394225325444, + -0.04006317484171634, + 1.1976750476689637, + -0.19417561589661742, + -0.5071480357469069, + -0.27484064739122255 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandMugDrink3-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "mug", + "reference": "envs/myo/myodm/data/MyoHand_mug_drink3.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "mug", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandMugFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "mug", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "mug", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0023441655403126788, + 0.008360856421285635, + 0.018835419653047483, + 0.9449831694705939, + -0.0002321713878494762, + -0.00045764378393285633, + -0.3271185504919041 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_mug_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.0038879972917569296, + -0.099209201679538, + -0.10044137483325581, + -0.20783766869221276, + -0.19249599661211436, + 0.006683827387311, + 0.4609612366648723, + -0.0830561450773409, + 0.24906392576760833, + -0.13161581285270596, + -0.07093285048038801, + 0.05994560824968333, + -0.8173775261408326, + 0.6358995636244336, + 0.308328211711796, + 0.6475364567220356, + 1.3815999478224579, + 0.8522467525632906, + 0.023073121407381873, + 0.43709725854801246, + 1.4717714506679893, + 0.57748106237601, + -0.014361790941091263, + 0.7328652150839113, + -0.03944138221471988, + 0.1953123159071805, + -0.48372693667980954, + 0.8874016963129625, + -0.0841854273238029 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandMugLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "mug", + "reference": "envs/myo/myodm/data/MyoHand_mug_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "mug", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.022513013867048737, + 0.009591221538834816, + 0.01862869320356323, + 0.3788769405376968, + 0.00019416100340868, + 0.0020640546334274494, + -0.9254447395219065 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_mug_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.061581127957475335, + 0.020330661986097565, + -0.1004514082401464, + -0.283305491427056, + -0.19436953406490332, + 0.03157012909876128, + 0.7019513648841196, + 0.5831871521403679, + 0.47187305637068644, + -0.10738255638202554, + 0.6230479122184496, + 0.027915946545281196, + -0.8230779350474668, + 0.14401036511685136, + 0.4068461252287523, + 0.28911267420792597, + 1.290955469611988, + 0.6130541510303089, + 0.1082492556461588, + -0.11789500686561025, + 1.2624759545318716, + 0.3115079854657653, + 0.15419294182523371, + 0.5059295349785243, + -0.036654458645678935, + -0.03177477078282164, + -0.08738657534292325, + 0.7808365471140939, + -0.09312289549989093 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandMugPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "mug", + "reference": "envs/myo/myodm/data/MyoHand_mug_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "mug", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandMugRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "mug", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "phone", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandPhoneFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "phone", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 47, + "object_name": "phone", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.013579718690417618, + -0.003265651506348214, + -0.022695950243191806, + 0.19458998813506537, + -0.18032865470506487, + -0.6853927541849878, + 0.6781246827247726 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_phone_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.023805223820143724, + -0.0579146127959179, + -0.10048883681048183, + -0.22083759037127046, + -0.21499237360157686, + 0.052845962082818165, + 1.1186298932283754, + 0.31636592004191205, + 0.0877782622459586, + 0.22489818954472351, + 0.653115683227331, + -0.438586885490454, + -0.8937571480146328, + 0.7384313323486202, + -0.1379051679604579, + 0.08529551550009833, + 0.14043287797292608, + 0.5151675452328103, + -0.39205511204047605, + 0.7824146944738888, + 0.003441235671060098, + 0.17903121598916677, + -0.4614768095238789, + 1.1033063759312718, + -0.01771192535306135, + 0.09815553667500153, + -1.1951075255650916, + 0.835900192668504, + 0.41317624649756496 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 47, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandPhoneLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "phone", + "reference": "envs/myo/myodm/data/MyoHand_phone_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "phone", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandPhoneRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "phone", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "piggybank", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandPiggybankFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "piggybank", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 85, + "object_name": "wineglass", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.004513004065405322, + 0.0005637480487716788, + 0.03390057287847571, + 0.022146137526410044, + 0.0014028104372088152, + 0.00019946878057891607, + -0.9997537401419133 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_piggybank_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.08555508735420694, + -0.048111757039514466, + -0.07954715045751172, + -0.13674838165349376, + -0.22661189465859735, + 0.16497713064544434, + 1.4138455263751026, + 0.6470707190377182, + -0.3883121159446101, + 0.5551224833402112, + 0.9050650392583001, + -0.06743942251309427, + -0.9868764069440713, + 0.43246338561391634, + -0.09214970083150299, + 0.033412664906305845, + -0.007900280115000045, + 0.6173756515243877, + -0.47751969520750476, + 0.07834297917999149, + -0.001757591725708845, + 0.2697697821093787, + -0.543695234635621, + 0.15366879095037164, + 0.16023471587064375, + -0.022458614210991042, + -1.2471432062266745, + 0.3938476531944963, + 1.6001746611434364 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 85, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandPiggybankPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "wineglass", + "reference": "envs/myo/myodm/data/MyoHand_piggybank_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "piggybank", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandPiggybankRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "piggybank", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "piggybank", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.004176202152597305, + 0.0001784863319131711, + 0.03381589814249965, + 0.08884925948636502, + -0.0006067856696685528, + 0.0003565290306419019, + 0.9960448352292812 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_piggybank_use1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.043298207736371476, + -0.06459810962624114, + -0.09186494743158162, + -0.11953032857885791, + -0.234935345407477, + 0.07321824783734428, + 1.1848240735253714, + 0.4402269795460398, + 0.0007759846513943364, + 0.5968738824366372, + 0.7019297549223394, + -0.21599252440119418, + -1.1745745496575477, + 0.42283658088014775, + 0.10650414733899753, + 0.16257383533409445, + 0.858223390584967, + 0.3672462366858215, + -0.23076611223889099, + 0.5145479755627239, + 0.5295931841397555, + 0.020355801648408688, + -0.23694673010802567, + 0.7360496748675613, + -0.006735866093315167, + -0.3256089717653902, + -0.7504438322457562, + 0.5901898394036073, + 0.5800304375598125 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandPiggybankUse-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "piggybank", + "reference": "envs/myo/myodm/data/MyoHand_piggybank_use1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "pyramidlarge", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandPyramidlargeFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "pyramidlarge", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "pyramidlarge", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0001015487712569026, + 0.0006805704902598074, + 0.010822096354739495, + 0.6409769578981174, + 0.0003907223063358625, + -0.002085842676739873, + -0.7675571874721009 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_pyramidlarge_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.002813578227551444, + -0.06258106731924487, + -0.1006452433782258, + -0.31438884236985587, + -0.19249206468791644, + 0.04350322463184956, + 0.9734719869574595, + 0.43818823663282375, + -0.08198940058944504, + 0.13702243918708776, + 0.5841131224434697, + -0.10134661232806315, + -0.873446282161563, + 0.3982145653586093, + 0.034756512972819995, + 0.21894155737014862, + 0.023721145500480524, + 0.7236538134357241, + -0.1745188771517196, + -0.18566126958930151, + 1.260307658847699, + 0.40440497173323314, + -0.05392008079641795, + 0.22856826729815358, + -0.0242845561176228, + 0.2690839430391631, + -0.298270008234787, + -0.041072335281148675, + -0.16500220907247162 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandPyramidlargePass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "pyramidlarge", + "reference": "envs/myo/myodm/data/MyoHand_pyramidlarge_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "pyramidlarge", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandPyramidlargeRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "pyramidlarge", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "pyramidmedium", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandPyramidmediumFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "pyramidmedium", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "pyramidmedium", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 1.1125087604872513e-05, + -0.00011929178526668293, + 0.0027818145343582318, + 0.9980677510330914, + -0.00036992698561599157, + 0.00010780469522581705, + -0.06213385453937379 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_pyramidmedium_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.0012060485333680391, + -0.05882146315227052, + -0.10047405865333417, + -0.3122850832041524, + -0.2067857114513115, + 0.05145026056348094, + 0.9500452738817207, + 0.3840064522512405, + -0.09985934419211932, + 0.05541224370896715, + 0.4289259252638245, + -0.13802437469505802, + -0.8509483047473837, + 0.5663302438619303, + 0.1773050762663905, + 0.3226081412318374, + 0.8781165430497059, + 0.6658087457849456, + -0.08525569416095093, + 0.2835220993306521, + 1.1903812100043938, + 0.35773095115225556, + -0.0421458676326805, + 0.7152375567888924, + -0.02462358196309813, + -0.09335056158239236, + -0.24766040186903804, + 0.7176887520811865, + -0.03957251497565437 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandPyramidmediumPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "pyramidmedium", + "reference": "envs/myo/myodm/data/MyoHand_pyramidmedium_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "pyramidmedium", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandPyramidmediumRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "pyramidmedium", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "pyramidsmall", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandPyramidsmallFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "pyramidsmall", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "pyramidsmall", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.00012187479011021153, + 6.640113299035272e-05, + -0.014467124813066255, + 0.7998135562286507, + -0.0008921537369457147, + -0.005099083082458848, + -0.6002261896038086 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_pyramidsmall_inspect1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.018432616512293692, + -0.04508848045760018, + -0.10046865870124833, + -0.2671091891371959, + -0.2094339957843693, + 0.06441195112831936, + 1.043133296922799, + 0.38143766760270487, + -0.06617950959052836, + -0.23523719002477653, + 0.13899006832514257, + 0.008653312689162897, + -0.7963884313357341, + 1.0597815774066552, + 0.24410527277171462, + 0.051142665392889516, + 1.5760347713648355, + 0.6535540460686254, + -0.06387045257249795, + 0.6310301111109484, + 1.22900833069749, + -0.01842429072800498, + 0.018167543876588414, + 1.2004311294113292, + -0.010642326594007913, + -0.39652935654382326, + -0.2684583933599205, + 0.9805291465432159, + 0.07839335670094483 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandPyramidsmallInspect-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "pyramidsmall", + "reference": "envs/myo/myodm/data/MyoHand_pyramidsmall_inspect1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "pyramidsmall", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandPyramidsmallRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "pyramidsmall", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "scissors", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandScissorsFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "scissors", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "scissors", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandScissorsRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "scissors", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "scissors", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.01916209464177447, + -0.021846197691150097, + -0.027827519617659863, + 0.6561696943736625, + -0.01911502041656964, + 0.0037225575545646395, + -0.7543620422219702 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_scissors_use1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.002427766862719339, + -0.06084085789193261, + -0.10122817853050167, + -0.19243797030706966, + -0.20412050508982033, + 0.03778587723101705, + 1.6944505609796268, + 0.4526089088695952, + -0.1442693995904196, + -0.09175074427997305, + 0.3091743465449626, + -0.1555831280474206, + -0.8022660382313015, + 0.9815286684708889, + 0.008794679251345184, + 0.3751658358236712, + 1.213856444705498, + 1.2898391517821968, + -0.20896021393719766, + 0.2361364256001514, + 1.470286551274087, + 0.8150428073093853, + -0.26064516317982606, + 0.6031143454057502, + -0.038769278285186504, + 0.8537199776165901, + -0.47870811643105327, + -0.15534531672500826, + -0.20222408121389063 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandScissorsUse-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "scissors", + "reference": "envs/myo/myodm/data/MyoHand_scissors_use1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "spherelarge", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandSpherelargeFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "spherelarge", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "spherelarge", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -1.6295172595546876e-05, + -0.00047801429255591804, + 0.017730568314058913, + 0.9691993998867691, + 0.000390454983476311, + 0.00190821345460774, + -0.24626962769583274 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_spherelarge_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.0019542290841518526, + -0.10921397409353836, + -0.09720767649683366, + -0.16725708458362998, + -0.20075599103217318, + 0.06492508346778575, + 1.2293886543744652, + 0.36871304035670366, + -0.03829413332751104, + 0.07483187416222965, + 0.6763475271498428, + -0.1303877339045103, + -0.8296858506884784, + 0.2961243566797948, + 0.4500995302872774, + 0.06903442872398986, + 1.2420957332249591, + 0.0500031173007955, + -0.028267905747338357, + 0.302936861728608, + 0.6759514048197116, + -0.16538375251998708, + -0.0966321082177844, + 0.4964027285470925, + 0.17210548731183736, + 0.07218443183397262, + -0.9226014334794544, + 0.5589446218498232, + -0.03619998166905767 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandSpherelargePass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "spherelarge", + "reference": "envs/myo/myodm/data/MyoHand_spherelarge_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "spherelarge", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandSpherelargeRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "spherelarge", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "spheremedium", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandSpheremediumFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "spheremedium", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "spheremedium", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -2.9935423596172913e-05, + 8.682456158550176e-05, + 0.001713007001431996, + 0.8911381058876625, + 4.749775068160596e-05, + -6.0949521259761316e-05, + -0.4537321569649531 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_spheremedium_inspect1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.020866264814376225, + -0.0849624473267855, + -0.10046057980540121, + -0.22797515342774274, + -0.1997949278028482, + 0.037850404085455924, + 1.12599212279341, + 0.3607525740411928, + -0.12239857594657859, + 0.28164550520936693, + 0.6009830055750721, + -0.39878909803769685, + -0.8798621298405912, + 0.43301824008319234, + 0.013968421085789533, + 0.2538673908936656, + 0.01360159851309329, + 0.2776701646194012, + -0.2763387128188157, + 0.7329922916782682, + 0.0004501191362910103, + 0.023480602735116273, + -0.30010051872994375, + 1.0863272535336665, + -0.006115178973204801, + -0.5978504033448196, + -0.5271091221360132, + 0.8119134530892633, + 0.41589165081692747 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandSpheremediumInspect-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "spheremedium", + "reference": "envs/myo/myodm/data/MyoHand_spheremedium_inspect1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 56, + "object_name": "spheremedium", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -2.1781405946077738e-05, + -2.279104577830773e-06, + 0.0016648227453280248, + 0.3415704302600261, + 0.0004415657075324854, + -0.0005578821966765944, + -0.9398559118073161 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_spheremedium_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.02226770488991782, + -0.052012171635508385, + -0.10037382274535538, + -0.11753184599657529, + -0.21993030541970998, + 0.11698015148523842, + 1.381119922898137, + 0.6000227796220975, + 0.13155932814384078, + 0.0991332708582096, + 0.803749991133791, + -0.2178951116502533, + -0.85727491147025, + 0.6145928324119309, + 0.27183417031875545, + 0.24532642773051472, + 1.3004199031361658, + 0.18240424982619377, + -0.20934375565953234, + 0.9153081866333895, + 0.022620665297864502, + 0.007386271869294684, + -0.36445899171619267, + 0.9205061966395175, + 0.032960171253301106, + -0.0619328734370334, + -1.2817616402494805, + 0.9739093311230907, + 1.5268394563107703 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 56, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandSpheremediumLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "spheremedium", + "reference": "envs/myo/myodm/data/MyoHand_spheremedium_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "spheremedium", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandSpheremediumRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "spheremedium", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "spheresmall", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandSpheresmallFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "spheresmall", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "spheresmall", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -3.8868635162959495e-05, + 8.364038204590368e-05, + -0.016133413718092682, + 0.9016038516452923, + 0.002094832850813151, + 0.0003445038620101886, + -0.4325574963987906 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_spheresmall_inspect1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.013120170740607305, + -0.0905555711921552, + -0.1004637839952073, + -0.2467876336448838, + -0.20885297885944726, + 0.05537169151924059, + 1.023146689191564, + 0.1998379549804853, + -0.12975391015576893, + 0.09139137834851395, + 0.3639911414111133, + -0.29469590450565125, + -0.8406597435367327, + 0.7619967907946354, + 0.1003658772853554, + 0.21752969002488884, + 1.5714036442080777, + 0.6289533124489232, + -0.1318694306249703, + 0.8347626060398688, + 0.7848299523631663, + -0.08255860047757584, + -0.00761397225273885, + 1.3620806792908555, + -0.014874324505285195, + -0.5455014672493375, + -0.17714623977524477, + 1.1080352883064661, + 0.00557547628415929 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandSpheresmallInspect-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "spheresmall", + "reference": "envs/myo/myodm/data/MyoHand_spheresmall_inspect1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 54, + "object_name": "spheresmall", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 9.349291916916697e-05, + 2.8150552934563434e-05, + -0.016163363405830276, + 0.9668805612697523, + 0.0019214866214554405, + -0.003317589540962929, + -0.2552004736035771 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_spheresmall_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.009417348619751421, + -0.049755304719122456, + -0.10046879922114141, + -0.21633140382101482, + -0.19426502916481692, + 0.04712507219994485, + 1.395860622257545, + 0.5676841335300488, + -0.14057116625424462, + 0.11981756257033337, + 0.4995282174659851, + -0.34271363433924185, + -0.8153311769707349, + 0.9372605620979115, + 0.08604220825056144, + 0.17982434526007562, + 1.57309522743903, + 0.9197537759705223, + -0.2573711505251045, + 0.8179240090051161, + 0.6225944378060356, + -0.15140599278962258, + -0.10291333548991477, + 1.4345761969924662, + -0.019358502630506463, + -1.0349898094428425, + -0.011931612750152122, + 0.6650874834553205, + 0.9492761771203658 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 54, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandSpheresmallLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "spheresmall", + "reference": "envs/myo/myodm/data/MyoHand_spheresmall_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "spheresmall", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -1.496354683026409e-05, + 8.690803760718247e-05, + -0.016229673726570755, + 0.8541703468565058, + 0.0019240981672286476, + 0.0013734660391618064, + -0.5199879133098372 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_spheresmall_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.021837385502766014, + -0.10279627292973799, + -0.10045214295842242, + -0.23939509534411046, + -0.20217304948852, + 0.062183112962188855, + 1.0698337019001336, + 0.03567942749565693, + -0.13491696264827105, + 0.10758135540518729, + 0.2948848610301087, + -0.2962334706295891, + -0.840689306380935, + 0.5344414209280598, + 0.13208412419961882, + 0.49453423733802315, + 1.2297825456327902, + 0.39689749975510824, + -0.07425719488046575, + 1.1185062839810587, + 0.4499665572142623, + -0.270975560939819, + 0.07077889566559789, + 1.556895842580783, + -0.01848533761200036, + -0.8101344098343848, + -0.004078006724364985, + 1.3070119618248222, + -0.05068355683530752 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandSpheresmallPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "spheresmall", + "reference": "envs/myo/myodm/data/MyoHand_spheresmall_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "spheresmall", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandSpheresmallRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "spheresmall", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "stamp", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandStampFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "stamp", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "stamp", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.00016985528387503012, + -0.00013087460541180853, + 0.0006236328005879803, + 0.08341085351638591, + -8.605275683189626e-05, + 0.00012798222805173766, + 0.9965152310582817 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_stamp_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.03478266544249285, + -0.062051504495707364, + -0.10034204809515465, + -0.1075978386653371, + -0.21495826941303603, + 0.07497435076119827, + 1.4255914603864634, + 0.38827303283544595, + 0.15599707528054918, + 0.051175466379736245, + 0.49434819654030887, + -0.18769298201476867, + -0.8176582972748393, + 0.8459626487835253, + 0.0970135245135694, + 0.061959183376937725, + 1.4797131203196487, + 0.534387338288159, + -0.24444379760121765, + 0.5510481280824485, + 0.8183313725877162, + 0.403666659434528, + -0.3770413103674624, + 0.621895202111175, + 0.008992107876818225, + 1.6140386173385732, + -0.7318424995935197, + -0.510073894234996, + -0.16094481544151948 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandStampLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "stamp", + "reference": "envs/myo/myodm/data/MyoHand_stamp_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "stamp", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandStampRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "stamp", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "stamp", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -7.626633690045837e-05, + 0.00018338319733421604, + 0.0006805985104503809, + 0.9850220244072847, + -0.0004786805086713876, + -0.00030161761700620476, + 0.17242764083626064 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_stamp_stamp1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.01727838846789478, + -0.07605946114704826, + -0.10262283256235417, + -0.13387405061091107, + -0.1968783272758783, + 0.05931221145731231, + 1.586706000916809, + 0.5703991413218781, + 0.03422559678691749, + 0.336949351003847, + 0.0001645949320477587, + -0.25362677152952495, + -0.9071210546574981, + 0.2059152709282257, + 0.35780962234487146, + 0.18262721273247223, + 0.6886722714584377, + -0.1932978987849945, + 0.02110569425858024, + 0.99353526883985, + -0.00649598756779961, + -0.2569764721145062, + -0.04304548591376633, + 0.9122991789830475, + -0.007821610205334678, + -0.2778316813105117, + -0.44880522816055046, + 1.0034760774084548, + 0.07741576579301009 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandStampStamp-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "stamp", + "reference": "envs/myo/myodm/data/MyoHand_stamp_stamp1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "stanfordbunny", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandStanfordbunnyFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "stanfordbunny", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "stanfordbunny", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.004058396910855691, + -0.0007330125219279834, + 0.028018555097592362, + 0.9932455183436674, + 0.0008117025786619505, + 0.0007743024154142417, + 0.11602621205965845 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_stanfordbunny_inspect1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.07579971051928289, + -0.11096414597415112, + -0.09279610750122416, + -0.30822193370379936, + -0.2239884563523312, + 0.12851018250452353, + 0.8361529691673846, + 0.532527589926851, + -0.4794096169547712, + 0.05493239780594077, + 0.6186751983210581, + -0.3192327553479647, + -0.7347620502508434, + 0.8009117972220844, + -0.022003931372184225, + 0.08405937020282017, + 0.23255193572013208, + 0.7496286902574257, + -0.287474749809911, + 0.13492533327149508, + 0.39207429824802864, + 0.16062819164957087, + -0.29675305486714526, + 0.6073597578828486, + -0.019011710892052582, + -0.32252545703257796, + -0.6971973179687225, + 0.44903231422213347, + 0.5182344918653395 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandStanfordbunnyInspect-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "stanfordbunny", + "reference": "envs/myo/myodm/data/MyoHand_stanfordbunny_inspect1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "stanfordbunny", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.005164976053229208, + 0.0055415880581201294, + 0.028205998341642603, + 0.937756629532933, + 1.5689033467898095e-05, + -0.0005677982818231792, + -0.3472926447911028 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_stanfordbunny_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.08765875502555444, + -0.10915050682684305, + -0.10062610490105263, + -0.2347158994763852, + -0.2846148067304571, + 0.28613600659569494, + 0.6873585860405439, + 0.0173117282719652, + -0.3688553216746605, + 0.1505517500887901, + 0.5062676737786372, + -0.2370514999105267, + -0.8770703153130035, + 0.410460910463658, + 0.013140561284287009, + 0.18589334241081462, + 0.39156618300750234, + 0.3227302754232589, + -0.2807157796945492, + 0.5393632803170579, + 0.9759721422173945, + 0.31590673417513804, + -0.3670035281088761, + 0.5186372075997013, + -0.008196269840208707, + 0.7226818984326361, + -0.7037492597428907, + 0.041380145680053146, + -0.17886499122555763 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandStanfordbunnyPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "stanfordbunny", + "reference": "envs/myo/myodm/data/MyoHand_stanfordbunny_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "stanfordbunny", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandStanfordbunnyRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "stanfordbunny", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "stapler", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandStaplerFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "stapler", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 56, + "object_name": "stapler", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0011386250208157325, + -0.0008076967495650852, + -0.016087664002346184, + 0.3476130264270983, + -0.0008192304694788243, + -0.0008879003247625791, + 0.9376373095993165 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_stapler_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.013051939452986931, + -0.08766875051236989, + -0.10046831878047889, + -0.22674453069660522, + -0.2033479324294085, + 0.06025014881367031, + 1.1339436867351873, + 0.20107456482939223, + -0.20159135533529482, + 0.14771165575722156, + 0.5184793774990497, + -0.46730813072732563, + -0.8322055705485343, + 1.1064521342104396, + -0.07222772863457476, + 0.0632646873504769, + 1.2973168183628123, + 1.113965129020368, + -0.39713426308856503, + 0.7957921116206333, + 0.4489808383186859, + -0.0068401615396608524, + -0.36285310426955975, + 1.6302741344679257, + -0.030785130549155985, + -1.0189283340461754, + -0.37598412210612847, + 1.144773554729879, + -0.10145382306700021 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 56, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandStaplerLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "stapler", + "reference": "envs/myo/myodm/data/MyoHand_stapler_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "stapler", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandStaplerRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "stapler", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "stapler", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0012800460875920518, + -0.0008998755724180681, + -0.01614875710890846, + 0.2705028454724595, + 0.00019696891317448213, + -0.0008278336057849122, + 0.9627187992794526 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_stapler_staple1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.024460176977708643, + -0.09132619145495023, + -0.1012426442938669, + -0.22046018284132163, + -0.21271828651492025, + 0.08481436191105281, + 1.120104840843357, + 0.163395240302607, + -0.27887689591599985, + 0.09416271106775811, + 0.20011473010255826, + -0.26248740941691157, + -0.8262623197166573, + 1.182746518350605, + -0.13602876862937824, + -0.0027682010893641516, + 1.5496847709215564, + 0.9667512938463733, + -0.34778239752847423, + 0.6429762026938868, + 1.4633702591330873, + 0.6114913303569424, + -0.44071609806026696, + 1.0976609540939244, + -0.013962396444133836, + 1.5469877171338449, + -0.8785983703721778, + -0.49657437648848185, + -0.11500865015673403 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandStaplerStaple1-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "stapler", + "reference": "envs/myo/myodm/data/MyoHand_stapler_staple1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "stapler", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0010132541285744134, + -0.000698664023320387, + -0.01616371933269646, + 0.3761429479819702, + -0.0014902479100307384, + -0.0014084184091056105, + 0.9265593765119343 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_stapler_staple2.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.03779789486161409, + -0.06630091087746696, + -0.10048742412906056, + -0.22528571585142396, + -0.2247833070142469, + 0.10303129158725924, + 1.2882497163269875, + 0.10558107251091935, + -0.24215835561089763, + 0.32628893530377906, + 0.07057396763196844, + -0.41074127417969264, + -0.8649762443733477, + 1.0616468123308058, + -0.2363771242037911, + 0.023225672247826026, + 1.366487639877499, + 0.8344392816276681, + -0.4241164324822266, + 0.6713474340042269, + 1.4324074753906326, + 0.47689221552947697, + -0.49144913557767167, + 1.1853967610054874, + -0.014933803409965937, + 1.6672904282344048, + -0.9517963321553136, + -0.3104203327073775, + -0.11094692667269118 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandStaplerStaple2-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "stapler", + "reference": "envs/myo/myodm/data/MyoHand_stapler_staple2.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "teapot", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandTeapotFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "teapot", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "teapot", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.01340987519480371, + -0.002679559085275999, + 0.022443829684294633, + 0.24664546023039718, + 0.0011581758536836, + 8.131598917702525e-05, + 0.969105086646509 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_teapot_pour2.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.003660118463354644, + -0.08845995685600067, + -0.10076012663027163, + -0.22862582951204724, + -0.2095380134495371, + 0.03221187059631137, + 0.7655512262785655, + 0.367517779041104, + -0.15762457593138396, + 0.6451949341354587, + 0.9564294145008092, + -0.31313536959644295, + -1.11573428223989, + 0.25028989582249855, + 0.019840211210157162, + 0.18435095052780132, + 0.28844301867634964, + 0.03022435855855725, + -0.3247104173925423, + 0.6029919227126527, + 1.3281075881839193, + -0.43149876117117525, + -0.32593547171478005, + 0.7855049356215439, + -0.005313541486900566, + -1.1035118055345041, + -0.5812980781677942, + 0.5294685379072318, + 0.3427076155614659 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandTeapotPour2-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "teapot", + "reference": "envs/myo/myodm/data/MyoHand_teapot_pour2.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "teapot", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandTeapotRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "teapot", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 74, + "object_name": "toothbrush", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -4.074878246533922e-05, + 0.0005820297058556914, + -0.028610495144876843, + 0.9998054977423189, + -0.0011977298824595725, + 0.0003651105668621567, + -0.019682449584249997 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_toothbrush_brush1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.0035341668845441073, + -0.06880557730994476, + -0.10255959752565487, + -0.24141674087926446, + -0.21295484950859375, + 0.09558055998385129, + 1.163381360375057, + 0.19810248695254853, + -0.019460702505904388, + -0.023269822383645476, + 0.08428010995403701, + -0.27971793631935443, + -0.8633140433515264, + 0.6268981038097756, + -0.10285905702438727, + 0.00994824874949565, + 1.4569797267020135, + 0.22851462557200605, + -0.16213840478985234, + 0.7765859823316767, + 1.4378785376023977, + -0.07880000762220617, + -0.12223474386098702, + 1.213111289317357, + -0.026753385540105125, + -0.30722174440008754, + -0.32717448835281626, + 1.0667067185104964, + -0.0648870164328109 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 74, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandToothbrushBrush1-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "toothbrush", + "reference": "envs/myo/myodm/data/MyoHand_toothbrush_brush1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "toothbrush", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandToothbrushFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "toothbrush", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 67, + "object_name": "toothbrush", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.00242304222328462, + 0.007419183676285887, + -0.02844288527814156, + 0.005263372880606716, + -0.9451124594672168, + 0.3266237994175369, + 0.007185368452687012 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_toothbrush_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.02815033834735119, + -0.058544143419867925, + -0.10044207372304673, + -0.19452911838300113, + -0.21785496561105425, + 0.0897377991379444, + 1.0764313517516968, + 0.17443491054481636, + 0.06152591073318219, + -0.1102707034556621, + 0.28179251864154903, + -0.14085860635789563, + -0.7959546139152275, + 1.166985230248856, + -0.05124654096941809, + 0.20969337539711158, + 1.5653018757522201, + 1.2592646628120112, + -0.31727065597200016, + 0.2959503274304011, + 1.3622660918270646, + 1.3940366328479756, + -0.3784329282547236, + 0.2586196883366241, + -0.030263859523474438, + 1.986657594801478, + -0.6084039999539783, + -0.748113549280345, + -0.1452717678653565 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 67, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandToothbrushLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "toothbrush", + "reference": "envs/myo/myodm/data/MyoHand_toothbrush_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "toothbrush", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandToothbrushRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "toothbrush", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "toothpaste", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandToothpasteFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "toothpaste", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 65, + "object_name": "toothpaste", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.0025996601932575954, + 0.002041985720130922, + -0.021819349887694073, + 0.6792662922237109, + -0.22775900081540285, + -0.6559494038237628, + -0.2375994979360677 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_toothpaste_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.021353787873991097, + -0.08096735308021233, + -0.10046121374981351, + -0.22627799184674746, + -0.21336010093150404, + 0.06297405998925483, + 1.235605600115579, + 0.2922749032994262, + -0.18824579089352209, + 0.10534602132380173, + 0.53836157161632, + -0.36390624516935144, + -0.8072151783219281, + 0.9937605246478342, + 0.0019317857961842016, + -0.07374139816648992, + 1.3596557057447267, + 0.8331706363492692, + -0.38278028826002936, + 0.5488796888308426, + 0.3484942509502439, + 0.7615998762912766, + -0.5470328848510464, + 0.6436837490763053, + 0.039632313527500046, + 2.068691493127075, + -0.8163513711835393, + -0.5538102460298482, + -0.10568147531098002 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 65, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandToothpasteLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "toothpaste", + "reference": "envs/myo/myodm/data/MyoHand_toothpaste_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "toothpaste", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandToothpasteRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "toothpaste", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 85, + "object_name": "toothpaste", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.0017759504508455416, + 0.0033387425160165468, + -0.021978139453367455, + 0.6347609032292555, + -0.32898143528657553, + -0.6106088048690008, + -0.3405975607440763 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_toothpaste_squeeze1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.03508652387009667, + -0.07702279978226818, + -0.10117802664790114, + -0.2586734158698154, + -0.217121219841365, + 0.08189545721750567, + 0.8398330030123042, + -0.16338649632659477, + 0.005722512625754378, + 0.25210644069966465, + 0.021863496121504542, + -0.3943546213881218, + -0.952679565080279, + 0.34610107905669274, + -0.4491255042156274, + -0.07721417549878792, + -0.03596162079897206, + -0.04301542920457129, + -0.33924523174692167, + 0.683643626062007, + -0.016176556053997756, + -0.4578864869262291, + -0.0973680204390138, + 1.1231186167385336, + -0.033353327788368636, + -0.818257995072908, + -0.11431420524610386, + 1.1132982544134005, + -0.03059549591130429 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 85, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandToothpasteSqueeze1-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "toothpaste", + "reference": "envs/myo/myodm/data/MyoHand_toothpaste_squeeze1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "toruslarge", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandToruslargeFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "toruslarge", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "toruslarge", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0001964111567570974, + 0.00010727229837293358, + -0.014287430707399344, + 0.8325798942690033, + -0.00022356884866526453, + 0.000898883391739336, + -0.5539041990133614 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_toruslarge_inspect1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.05052005194991011, + -0.08298816905709659, + -0.10054083866622308, + -0.26872669023908957, + -0.2295067469061609, + 0.15441677850899954, + 1.227003011802036, + 0.6430033985059771, + -0.4699012942942271, + -0.042841854359363, + 0.4402725074302614, + -0.19635333913079445, + -0.8034996294224773, + 0.9804695450825113, + -0.07749057269170383, + 0.07895833662241701, + 1.5289180292213749, + 1.0093170619883232, + -0.23984651858384276, + 0.1772948092135528, + 0.9495233292056103, + 0.23962030588171884, + -0.16459385533575865, + 0.6077456407763544, + -0.011722868673738427, + -0.4192219093815371, + -0.2164021061117543, + 0.7142031317710098, + 0.42525247239415903 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandToruslargeInspect-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "toruslarge", + "reference": "envs/myo/myodm/data/MyoHand_toruslarge_inspect1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 62, + "object_name": "toruslarge", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 8.918085700506161e-05, + 0.00020118300153511694, + -0.014226323435499424, + 0.8386315615791234, + 0.0005411112885627782, + -0.00022771114021888716, + -0.5446987784726259 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_toruslarge_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.020736279896853553, + -0.07680096029591992, + -0.10079721900383001, + -0.17226799343080046, + -0.201827772171826, + 0.07234454021255877, + 1.1611416602362237, + 0.146439598618393, + 0.17261838702911286, + 0.5534946430195311, + 0.835515525331792, + -0.45631382691868194, + -1.3293563242251245, + 0.35889327436595125, + -0.09674193457754153, + 0.10062839844028319, + -0.009011096372108705, + -0.2545837927690489, + -0.40655095081802683, + 0.9343701228425062, + -0.011669132453903825, + -0.39377552784325975, + -0.5465031052534975, + 0.7814000141858968, + -0.007584274810778918, + -0.1228643406389604, + -1.6028057585026172, + 0.1521213916722908, + 1.5916245049313757 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 62, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandToruslargeLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "toruslarge", + "reference": "envs/myo/myodm/data/MyoHand_toruslarge_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "toruslarge", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandToruslargeRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "toruslarge", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "torusmedium", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandTorusmediumFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "torusmedium", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 54, + "object_name": "torusmedium", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 1.719479455432824e-05, + -0.00019912571530033966, + -0.0211314620790994, + 0.8455745030169202, + -0.0016654152752610123, + 0.0008939291324940727, + -0.5338540878651724 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_torusmedium_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.007102192721766167, + -0.043574450266361656, + -0.10042592214368812, + -0.24624230767000668, + -0.18742055351021808, + 0.07232696334252207, + 1.1291618989254188, + 0.5150959938390977, + -0.08874541093400082, + 0.2852387951508639, + 0.7532129371498655, + -0.4389644379397035, + -0.9361639205042496, + 0.6084086864118826, + -0.017166131329087037, + 0.020710298342764485, + 0.7272380199924198, + 0.745571687734405, + -0.40508219815320595, + 0.42686199935558367, + 1.009548979757792, + 0.10229235732714777, + -0.4340369036955557, + 1.1140230638067392, + -0.0076030835154582655, + -0.7321687744405578, + -0.7002631392444656, + 0.8792308110211892, + -0.10976564320116874 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 54, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandTorusmediumLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "torusmedium", + "reference": "envs/myo/myodm/data/MyoHand_torusmedium_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "torusmedium", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 6.374708907131709e-05, + 1.1763989327971314e-05, + -0.021095138245582033, + 0.9345812612089014, + -0.00013692971361020114, + -0.0001533169677799892, + -0.3557496647100883 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_torusmedium_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.006634059297294468, + -0.06109289661091792, + -0.10053081790129624, + -0.25409422394452846, + -0.21542734119550871, + 0.06698750685935467, + 0.9954999528762223, + 0.050186485461048334, + -0.17790213339259, + 0.3579863558571012, + 0.348884244928864, + -0.5625894394482496, + -0.9309317537665402, + 1.2004094299470274, + -0.21344141719442908, + 0.1579680475109865, + 1.4087272924577239, + 1.518231624009529, + -0.403536014853745, + 0.6078862535599012, + 0.5625017355259146, + 0.6895384455068567, + -0.5916380649875873, + 1.8143690348613881, + 0.024235667653164625, + -1.2616907796764976, + -0.5435723636848518, + 2.117672786250385, + 1.656885547864263 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandTorusmediumPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "torusmedium", + "reference": "envs/myo/myodm/data/MyoHand_torusmedium_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "torusmedium", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandTorusmediumRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "torusmedium", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "torussmall", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandTorussmallFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "torussmall", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 55, + "object_name": "torussmall", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -6.207279344048103e-05, + 0.00015167840299192933, + -0.027615087240406237, + 0.8491334281062901, + 0.0009118534263294297, + -0.0021816686564516737, + -0.5281731062044547 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_torussmall_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.015040540323201103, + -0.03697911173711332, + -0.10047595479349945, + -0.2621987641480563, + -0.2063689201730048, + 0.09557738475749027, + 1.180575427469, + 0.578423752537596, + -0.2110074175982277, + 0.07138505903533379, + 0.5571298661062886, + -0.4132925687518983, + -0.8037166500531155, + 0.6797446822671454, + -0.19801351915789175, + 0.18221099895573595, + -0.002931561691005634, + 1.2584834594790018, + -0.36112856666301174, + 0.40139167473260073, + 0.9961186742563192, + -0.03635962296272914, + -0.21381991816845475, + 1.527232046035032, + -0.029891070147639382, + -1.1495900305807605, + 0.02644649040888287, + 1.3049172925370962, + -0.06466679343772024 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 55, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandTorussmallLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "torussmall", + "reference": "envs/myo/myodm/data/MyoHand_torussmall_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "torussmall", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -7.21179440516671e-05, + 5.4657040763663226e-05, + -0.02764310422635248, + 0.9084338269302836, + 0.0015728090004188153, + -0.002414117569539756, + -0.4180187560349512 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_torussmall_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.04559325872762768, + -0.058416097197564125, + -0.10049780756492718, + -0.28111855717910483, + -0.2236032400050197, + 0.04596051650021442, + 1.0121607392629668, + -0.06010616435727556, + -0.36175023076946083, + 0.2652593423164648, + 0.3858421447297805, + -0.48827220422073353, + -0.9097985350029163, + 1.5861647901394564, + -0.12443868799989954, + 0.22069791938691374, + 1.5735017386948107, + 1.68582774219033, + -0.33877368431799687, + 0.5070538519553172, + 1.040883592639438, + 1.3395478916946049, + -0.43031981559036087, + 1.2699952350757657, + -0.001629085456490419, + 2.53265423670504, + -0.47037478055124254, + -0.4547373546628904, + -0.1507663216743447 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandTorussmallPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "torussmall", + "reference": "envs/myo/myodm/data/MyoHand_torussmall_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "torussmall", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandTorussmallRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "torussmall", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "train", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandTrainFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "train", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "train", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.00015197298275881812, + 0.0007803131711911787, + 0.006498749258928991, + 0.9992426611684521, + 0.0003049429928163213, + -0.0001429946014477355, + 0.038910032938885855 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_train_play1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.03193725018385605, + -0.07201725229479797, + -0.10196220615564758, + -0.1867707240258522, + -0.17481694558731115, + 0.020862449585615256, + 1.1091074743357743, + 0.5315733969858631, + -0.01451521506350613, + 0.07349514711631491, + 0.506548514816098, + -0.1339303869930135, + -0.8272270295467691, + 0.5756898599155787, + -0.08658088146866887, + 0.26011316038132615, + 0.09408557953059296, + 0.4453724558087503, + -0.2953256835374207, + 0.7793596399405904, + 0.483986215321422, + 0.7330833745395134, + -0.3452536736493708, + 0.5652842485578711, + -0.015375960790855195, + 1.370885922563554, + -0.49857146387497636, + -0.5768238697728125, + -0.20915424192313353 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandTrainPlay-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "train", + "reference": "envs/myo/myodm/data/MyoHand_train_play1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "train", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandTrainRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "train", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "watch", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandWatchFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "watch", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 67, + "object_name": "watch", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.008672819093490809, + 0.006376922482956241, + -0.01782547116700869, + -0.10796356418166514, + -0.23492167144699946, + -0.6322629587729441, + -0.7303418569802923 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_watch_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.022727795080800497, + -0.09217857887805214, + -0.10043008120444037, + -0.23342933835415217, + -0.20079749029952212, + 0.04648990365131427, + 1.3203331996264398, + 0.481338622198504, + -0.20622305995162993, + -0.22940978869296436, + 0.1922348539492645, + -0.0809102832398086, + -0.7845308636529926, + 1.0064651861464808, + 0.029996948132055074, + 0.397425429864065, + 1.5710720432025171, + 0.8106921528290342, + -0.1545116008817661, + 0.9913514901449328, + 0.4348236601665966, + -0.04431585367972585, + 0.011171510828326719, + 2.0302123198547912, + -0.005793471426694157, + -0.5185628342622406, + -0.2937085615022097, + 2.098237995528949, + -0.07104413902812055 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 67, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandWatchLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "watch", + "reference": "envs/myo/myodm/data/MyoHand_watch_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 62, + "object_name": "watch", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.0028814865276363486, + 0.0063937914738883385, + -0.017708460124922828, + 0.38381064177251256, + 0.47000954404984713, + 0.48932117419983456, + 0.6263746548543627 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_watch_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.117480832725511, + -0.1194197100173859, + -0.0838343324917746, + -0.25921639881982056, + -0.3185568473442397, + 0.2840104900164634, + 1.2545693595127139, + 0.28710513588661307, + -0.5849682496763289, + -0.26179663461191255, + 0.08696365557065264, + -0.23363568304873092, + -0.789669592866559, + 0.8081056893545047, + -0.015629408326241594, + 0.2739994401807589, + 1.4466455142583683, + 0.8000898877733226, + -0.15467273177142404, + 0.3885987415772586, + 1.1993972272001863, + 0.29758586155110467, + -0.1856562123290027, + 1.5656434739379015, + -0.013260940165738946, + 0.3806894259735908, + -0.6438862803279711, + 1.2656236520854354, + -0.014803652407427216 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 62, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandWatchPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "watch", + "reference": "envs/myo/myodm/data/MyoHand_watch_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "watch", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandWatchRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "watch", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "waterbottle", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandWaterbottleFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "waterbottle", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 60, + "object_name": "waterbottle", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.000916544683207255, + -0.0013246907897116107, + 0.03306837001164271, + 0.7508253824045563, + -5.833857637931538e-05, + -0.0008113344009639493, + -0.6605002524376141 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_waterbottle_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.10883761490084445, + -0.14326725089503314, + -0.1020939094698553, + -0.2898558081307079, + -0.13666469103813902, + -0.1349739705055821, + 0.5020406288635604, + 0.3992086422360312, + -0.03235626115708138, + 0.08126379948187687, + 0.39773001352082626, + -0.19225763297628606, + -0.8400002983337849, + 0.061793723565826776, + 0.0904515937106938, + 0.7169799542341815, + -0.007112013071334301, + -0.07669492028531334, + -0.11844143129589502, + 1.1694724202319557, + 0.02148421836443574, + -0.042764032845244195, + -0.07969101442769147, + 1.0210775911080106, + -0.009544581566047348, + 0.9212780676982494, + -0.514241687600681, + -0.3115983702903236, + -0.20732986341092355 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 60, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandWaterbottleLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "waterbottle", + "reference": "envs/myo/myodm/data/MyoHand_waterbottle_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "waterbottle", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.001451546763940883, + -0.0022202007296026746, + 0.03304265802803421, + 0.7993731282198044, + -0.001369597156251266, + -0.0001287686665888602, + -0.6008333458641791 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_waterbottle_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.05204300023605827, + -0.11913116606960149, + -0.10104788645443238, + -0.2197219548421261, + -0.2815754393961654, + 0.1762424019774812, + 0.36880217420459194, + 0.09481704090104696, + -0.2401391183350259, + -0.1077746614946889, + 0.3942144487042121, + -0.04498594880707262, + -0.821465172514095, + 0.32360118965053114, + 0.18202117566322668, + 0.33293724902786426, + 0.3154064987869354, + 0.498024228663403, + -0.0730018059562737, + 0.18716928806641583, + 0.945681221203678, + 0.32837764807944264, + -0.04224109108765551, + 0.5586349644400492, + -0.020951925457867584, + 0.3972594576905477, + -0.3985605386980794, + 0.7410674700519365, + -0.15776210391623025 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandWaterbottlePass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "waterbottle", + "reference": "envs/myo/myodm/data/MyoHand_waterbottle_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "waterbottle", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandWaterbottleRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "waterbottle", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 89, + "object_name": "waterbottle", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.00029314398325013755, + -0.0006832569196881621, + 0.03314052875063803, + 0.28427297793940165, + 0.0007959625199331851, + -0.0018950940358787099, + -0.9587411793991777 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_waterbottle_shake1.npz", + "reference_robot": [], + "reference_robot_init": [ + 0.008206281648328543, + -0.11484511599086494, + -0.08775495051256273, + -0.3889080849130954, + -0.21793492082111948, + 0.15825683075934804, + 0.3069120913189448, + 0.7732206812845618, + -0.4368110760974084, + -0.07911769206365947, + 0.26319543106770316, + -0.48256517549017924, + -0.7970425981255627, + -0.22872545039806552, + -0.3417456154368117, + 0.6026382161366086, + -0.04187269527214146, + -0.5607116324253316, + -0.3600897811470791, + 1.0359784789678979, + -0.03038133989243371, + -0.8261038685518569, + -0.1791348244159566, + 1.168222121823083, + -0.04993915493969782, + -1.3543126424242342, + -0.11212499525498994, + 1.017060631334786, + -0.0022514155691580502 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 89, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandWaterbottleShake-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "waterbottle", + "reference": "envs/myo/myodm/data/MyoHand_waterbottle_shake1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "wineglass", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + -0.004013278730223179, + 0.025207939331173504, + -0.005933072647553404, + 0.7989419749072223, + 2.689888782803594e-06, + 0.00312847500377991, + 0.6013999778585485 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_wineglass_drink2.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.017237649118573558, + -0.08396725728735645, + -0.10095834985829898, + -0.35061879565369963, + -0.2461062140576467, + 0.13867951453809982, + 0.00985082199721964, + -0.023693720504537046, + -0.31401347130562196, + -0.16369774003006657, + 0.29577768594365367, + -0.20678729433933382, + -0.747314923025905, + 0.2754289015839667, + 0.08066058849803413, + 0.8850366609687502, + -4.7453221569945294e-05, + 0.2628262359388038, + -0.23727381129144748, + 0.9966419949361867, + 0.00499808193023388, + 0.8684622530039667, + -0.291433166351236, + 1.0189775392812128, + -0.008108135982381783, + 1.4627410691425144, + -0.36731738430771466, + 0.7725823610315443, + -0.23486728304942317 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandWineglassDrink2-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "wineglass", + "reference": "envs/myo/myodm/data/MyoHand_wineglass_drink2.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 1, + "object_name": "wineglass", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ], + "reference_object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 1, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandWineglassFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "wineglass", + "reference": { + "object": [ + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.1 + ] + ], + "object_init": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "wineglass", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.0018494310796690289, + 0.021254808501827625, + -0.005954273350469705, + 0.5645188397644522, + -0.00578323051836058, + -0.006773056931629895, + 0.8253721339466029 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_wineglass_lift.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.008824937459484733, + -0.1071939700878336, + -0.10079905081226445, + -0.23242732208418926, + -0.2094138481670267, + 0.06871823515953988, + 0.3110133810928935, + 0.09740710578111128, + -0.12957216845579816, + 0.05177413879252305, + 0.42580494748751635, + -0.18136539213915157, + -0.8356269919394368, + 0.08308249444277252, + 0.166379420100519, + 0.6223597682528833, + -0.006721293131720534, + -0.046818692230290136, + -0.06992377555626311, + 0.9120335420038949, + 0.11370313313803306, + -0.009976599444548004, + -0.08795390950724426, + 0.8869842171013195, + -0.004990875454479186, + 0.8468056054757468, + -0.5427354383712377, + -0.10736768383785243, + -0.22039520712306668 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandWineglassLift-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "wineglass", + "reference": "envs/myo/myodm/data/MyoHand_wineglass_lift.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 100, + "object_name": "wineglass", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.002678756007630811, + 0.02054396215583545, + -0.006034944902181996, + 0.4354973181971035, + 0.0004364709972812762, + -0.00040922508755400114, + 0.900189828797808 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_wineglass_pass1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.007420940642402633, + -0.10592274598822289, + -0.10235390113169095, + -0.21090486043499393, + -0.2077740638502344, + 0.06598527675375952, + 0.3669017269486438, + 0.025007428310777766, + -0.04063713814692774, + 0.0037969295579685985, + 0.4648650170667309, + -0.07342036332361497, + -0.8376739790658969, + 0.1289166096734235, + 0.21493651958289414, + 0.3039164827650085, + 0.5880581828127638, + 0.2405229200027628, + -0.06750553380906768, + 0.282435777836839, + 1.2875972830152307, + 0.14771973187835405, + -0.014480646973201982, + 0.5743204490548781, + -0.01408567264063169, + 0.28536605799832093, + -0.3889808934948272, + 0.7531115495874953, + -0.14395522373064013 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 100, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandWineglassPass-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "wineglass", + "reference": "envs/myo/myodm/data/MyoHand_wineglass_pass1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 2, + "object_name": "wineglass", + "obs_dim": 170, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": true, + "reference_object": [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0, + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ], + "reference_object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "reference_path": "", + "reference_robot": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_init": [ + 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.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_robot_vel": [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "reference_time": [ + 0.0, + 4.0 + ], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 2, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandWineglassRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "wineglass", + "reference": { + "object": [ + [ + -0.2, + -0.2, + 0.1, + 1.0, + 0.0, + 0.0, + -1.0 + ], + [ + 0.2, + 0.2, + 0.1, + 1.0, + 0.0, + 0.0, + 1.0 + ] + ], + "object_init": [ + 0.0, + 0.0, + 0.1, + 1.0, + 0.0, + 0.0, + 0.0 + ], + "robot": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "robot_vel": [ + [ + 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.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, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + ] + ], + "time": [ + 0.0, + 4.0 + ] + } + }, + "max_episode_steps": 50, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "TrackEnv", + "default_config": { + "act_dim": 39, + "action_dim": 45, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "motion_extrapolation": true, + "motion_start_time": 0.0, + "normalize_act": true, + "object_dim": 7, + "object_horizon": 94, + "object_name": "wineglass", + "obs_dim": 142, + "qpos_dim": 35, + "qvel_dim": 35, + "reference_has_robot_vel": false, + "reference_object": [], + "reference_object_init": [ + 0.00900009749329732, + 0.01629838937783397, + -0.005850765754151485, + 0.5940185888221269, + 0.00462240118324486, + 0.0001163605875697726, + -0.8044380249598373 + ], + "reference_path": "envs/myo/myodm/data/MyoHand_wineglass_toast1.npz", + "reference_robot": [], + "reference_robot_init": [ + -0.002098742482187126, + -0.03578940030462243, + -0.10082196884531312, + -0.4183930610946663, + -0.2103617528287915, + 0.06283828864989957, + 0.2577853818990201, + 0.40112846771406646, + -0.07397977284888951, + -0.3141263234426211, + 0.156975800747895, + -0.15381572142431807, + -0.7891438626430866, + 0.6529545321649703, + -0.014593488128844454, + 0.4630530782554164, + 1.3586751678125732, + 0.6212017090664411, + -0.03564470697040548, + 0.6445147593166255, + 1.4172421676821227, + 0.35960978675771477, + -0.05305549717446137, + 0.6767925242969381, + -0.01525345103812967, + 0.6754212682315615, + -0.1632978126360035, + -0.21952505428685776, + -0.27804085161403813 + ], + "reference_robot_vel": [], + "reference_time": [], + "reward_bonus_w": 1.0, + "reward_object_w": 1.0, + "reward_penalty_w": -2.0, + "reward_pose_w": 0.0, + "robot_dim": 29, + "robot_horizon": 94, + "terminate_obj_fail": true, + "terminate_pose_fail": false + }, + "entry_module": "myosuite.envs.myo.myodm.myodm_v0", + "id": "MyoHandWineglassToast1-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_object.xml", + "object_name": "wineglass", + "reference": "envs/myo/myodm/data/MyoHand_wineglass_toast1.npz" + }, + "max_episode_steps": 75, + "model_placeholders": [ + "OBJECT_NAME" + ], + "registration_suite": "myodm", + "suite": "myodm", + "variant_defs": [] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 0, + "action_dim": 5, + "frame_skip": 5, + "model_path": "simhive/myo_sim/finger/motorfinger_v0.xml", + "normalize_act": true, + "obs_dim": 12, + "pose_thd": 0.35, + "qpos_dim": 4, + "qvel_dim": 4, + "reset_type": "init", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [ + 0, + 0.75, + 0, + 0.75 + ], + "target_qpos_min": [ + 0, + 0.75, + 0, + 0.75 + ], + "target_qpos_value": [ + 0.0, + 0.75, + 0.0, + 0.75 + ], + "target_type": "fixed", + "viz_site_targets": [ + "IFtip" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "motorFingerPoseFixed-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "simhive/myo_sim/finger/motorfinger_v0.xml", + "normalize_act": true, + "target_jnt_range": { + "IFadb": [ + 0, + 0 + ], + "IFdip": [ + 0.75, + 0.75 + ], + "IFmcp": [ + 0, + 0 + ], + "IFpip": [ + 0.75, + 0.75 + ] + }, + "viz_site_targets": [ + "IFtip" + ] + }, + "max_episode_steps": 200, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 0, + "action_dim": 5, + "frame_skip": 5, + "model_path": "simhive/myo_sim/finger/motorfinger_v0.xml", + "normalize_act": true, + "obs_dim": 12, + "pose_thd": 0.35, + "qpos_dim": 4, + "qvel_dim": 4, + "reset_type": "init", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [ + 0.2, + 1, + 1, + 1 + ], + "target_qpos_min": [ + -0.2, + 0.1, + -0.4, + 0.1 + ], + "target_qpos_value": [ + 0.0, + 0.55, + 0.3, + 0.55 + ], + "target_type": "fixed", + "viz_site_targets": [ + "IFtip" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "motorFingerPoseRandom-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "simhive/myo_sim/finger/motorfinger_v0.xml", + "normalize_act": true, + "target_jnt_range": { + "IFadb": [ + -0.2, + 0.2 + ], + "IFdip": [ + 0.1, + 1 + ], + "IFmcp": [ + -0.4, + 1 + ], + "IFpip": [ + 0.1, + 1 + ] + }, + "viz_site_targets": [ + "IFtip" + ] + }, + "max_episode_steps": 200, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [] + }, + { + "class_name": "ReachEnvV0", + "default_config": { + "act_dim": 0, + "action_dim": 5, + "far_th": 0.35, + "frame_skip": 5, + "hide_skin_geom_group_1": false, + "hide_terrain": false, + "joint_random_range": [], + "model_path": "simhive/myo_sim/finger/motorfinger_v0.xml", + "normalize_act": true, + "obs_dim": 14, + "qpos_dim": 4, + "qvel_dim": 4, + "reward_act_reg_w": 0.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_reach_w": 1.0, + "target_pos_max": [ + 0.2, + 0.05, + 0.2 + ], + "target_pos_min": [ + 0.2, + 0.05, + 0.2 + ], + "target_pos_relative_to_tip": false, + "target_site_count": 1, + "target_site_names": [ + "IFtip" + ] + }, + "entry_module": "myosuite.envs.myo.myobase.reach_v0", + "id": "motorFingerReachFixed-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "simhive/myo_sim/finger/motorfinger_v0.xml", + "normalize_act": true, + "target_reach_range": { + "IFtip": [ + [ + 0.2, + 0.05, + 0.2 + ], + [ + 0.2, + 0.05, + 0.2 + ] + ] + } + }, + "max_episode_steps": 200, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [] + }, + { + "class_name": "ReachEnvV0", + "default_config": { + "act_dim": 0, + "action_dim": 5, + "far_th": 0.35, + "frame_skip": 5, + "hide_skin_geom_group_1": false, + "hide_terrain": false, + "joint_random_range": [], + "model_path": "simhive/myo_sim/finger/motorfinger_v0.xml", + "normalize_act": true, + "obs_dim": 14, + "qpos_dim": 4, + "qvel_dim": 4, + "reward_act_reg_w": 0.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_reach_w": 1.0, + "target_pos_max": [ + 0.27, + 0.1, + 0.3 + ], + "target_pos_min": [ + 0.1, + -0.1, + 0.1 + ], + "target_pos_relative_to_tip": false, + "target_site_count": 1, + "target_site_names": [ + "IFtip" + ] + }, + "entry_module": "myosuite.envs.myo.myobase.reach_v0", + "id": "motorFingerReachRandom-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "simhive/myo_sim/finger/motorfinger_v0.xml", + "normalize_act": true, + "target_reach_range": { + "IFtip": [ + [ + 0.1, + -0.1, + 0.1 + ], + [ + 0.27, + 0.1, + 0.3 + ] + ] + } + }, + "max_episode_steps": 200, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [] + }, + { + "class_name": "ReachEnvV0", + "default_config": { + "act_dim": 34, + "action_dim": 34, + "far_th": 1.0, + "frame_skip": 10, + "hide_skin_geom_group_1": false, + "hide_terrain": false, + "joint_random_range": [], + "model_path": "simhive/myo_sim/arm/myoarm_reach.xml", + "normalize_act": true, + "obs_dim": 80, + "qpos_dim": 20, + "qvel_dim": 20, + "reward_act_reg_w": 0.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_reach_w": 1.0, + "target_pos_max": [ + -0.175, + -0.245, + 1.405 + ], + "target_pos_min": [ + -0.175, + -0.245, + 1.405 + ], + "target_pos_relative_to_tip": false, + "target_site_count": 1, + "target_site_names": [ + "IFtip" + ] + }, + "entry_module": "myosuite.envs.myo.myobase.reach_v0", + "id": "myoArmReachFixed-v0", + "kwargs": { + "edit_fn": "edit_fn_arm_reaching", + "far_th": 1.0, + "model_path": "simhive/myo_sim/arm/myoarm.xml", + "normalize_act": true, + "target_reach_range": { + "IFtip": [ + [ + -0.175, + -0.245, + 1.405 + ], + [ + -0.175, + -0.245, + 1.405 + ] + ] + } + }, + "max_episode_steps": 150, + "model_placeholders": [], + "registration_suite": "myoedits", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiArmReachFixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcArmReachFixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "ReachEnvV0", + "default_config": { + "act_dim": 34, + "action_dim": 34, + "far_th": 1.0, + "frame_skip": 10, + "hide_skin_geom_group_1": false, + "hide_terrain": false, + "joint_random_range": [], + "model_path": "simhive/myo_sim/arm/myoarm_reach.xml", + "normalize_act": true, + "obs_dim": 80, + "qpos_dim": 20, + "qvel_dim": 20, + "reward_act_reg_w": 0.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_reach_w": 1.0, + "target_pos_max": [ + 0.0, + -0.07, + 1.83 + ], + "target_pos_min": [ + -0.35, + -0.42, + 0.98 + ], + "target_pos_relative_to_tip": false, + "target_site_count": 1, + "target_site_names": [ + "IFtip" + ] + }, + "entry_module": "myosuite.envs.myo.myobase.reach_v0", + "id": "myoArmReachRandom-v0", + "kwargs": { + "edit_fn": "edit_fn_arm_reaching", + "far_th": 1.0, + "model_path": "simhive/myo_sim/arm/myoarm.xml", + "normalize_act": true, + "target_reach_range": { + "IFtip": [ + [ + -0.35, + -0.42, + 0.98 + ], + [ + 0.0, + -0.07, + 1.83 + ] + ] + } + }, + "max_episode_steps": 150, + "model_placeholders": [], + "registration_suite": "myoedits", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiArmReachRandom-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcArmReachRandom-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "BaodingEnvV1", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "drop_th": 1.25, + "fixed_task": 2, + "frame_skip": 10, + "goal_time_period_high": 5.0, + "goal_time_period_low": 5.0, + "goal_xrange_high": 0.025, + "goal_xrange_low": 0.025, + "goal_yrange_high": 0.028, + "goal_yrange_low": 0.028, + "model_path": "envs/myo/assets/hand/myohand_baoding.xml", + "normalize_act": true, + "obj_friction_high": [], + "obj_friction_low": [], + "obj_mass_high": 0.0, + "obj_mass_low": 0.0, + "obj_size_high": 0.0, + "obj_size_low": 0.0, + "obs_dim": 86, + "proximity_th": 0.015, + "qpos_dim": 37, + "qvel_dim": 35, + "reward_pos_dist_1_w": 5.0, + "reward_pos_dist_2_w": 5.0, + "task_choice": "fixed" + }, + "entry_module": "myosuite.envs.myo.myochallenge.baoding_v1", + "id": "myoChallengeBaodingP1-v1", + "kwargs": { + "goal_time_period": [ + 5, + 5 + ], + "goal_xrange": [ + 0.025, + 0.025 + ], + "goal_yrange": [ + 0.028, + 0.028 + ], + "model_path": "envs/myo/assets/hand/myohand_baoding.xml", + "normalize_act": true + }, + "max_episode_steps": 200, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeBaodingP1-v1", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeBaodingP1-v1", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "BaodingEnvV1", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "drop_th": 1.25, + "fixed_task": 2, + "frame_skip": 10, + "goal_time_period_high": 6.0, + "goal_time_period_low": 4.0, + "goal_xrange_high": 0.03, + "goal_xrange_low": 0.02, + "goal_yrange_high": 0.032, + "goal_yrange_low": 0.022, + "model_path": "envs/myo/assets/hand/myohand_baoding.xml", + "normalize_act": true, + "obj_friction_high": [ + 1.2, + 0.006, + 0.00012 + ], + "obj_friction_low": [ + 0.8, + 0.004, + 8e-05 + ], + "obj_mass_high": 0.3, + "obj_mass_low": 0.03, + "obj_size_high": 0.024, + "obj_size_low": 0.018, + "obs_dim": 86, + "proximity_th": 0.015, + "qpos_dim": 37, + "qvel_dim": 35, + "reward_pos_dist_1_w": 5.0, + "reward_pos_dist_2_w": 5.0, + "task_choice": "random" + }, + "entry_module": "myosuite.envs.myo.myochallenge.baoding_v1", + "id": "myoChallengeBaodingP2-v1", + "kwargs": { + "goal_time_period": [ + 4, + 6 + ], + "goal_xrange": [ + 0.02, + 0.03 + ], + "goal_yrange": [ + 0.022, + 0.032 + ], + "model_path": "envs/myo/assets/hand/myohand_baoding.xml", + "normalize_act": true, + "obj_friction_change": [ + 0.2, + 0.001, + 2e-05 + ], + "obj_mass_range": [ + 0.03, + 0.3 + ], + "obj_size_range": [ + 0.018, + 0.024 + ], + "task_choice": "random" + }, + "max_episode_steps": 200, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeBaodingP2-v1", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeBaodingP2-v1", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "BimanualEnvV1", + "default_config": { + "act_dim": 63, + "action_dim": 80, + "frame_skip": 5, + "goal_center": [ + 0.4, + -0.25, + 1.05 + ], + "goal_shifts": [ + 0.098, + 0.098, + 0.0 + ], + "model_path": "envs/myo/assets/arm/myoarm_bionic_bimanual.xml", + "normalize_act": true, + "obj_friction_high": [ + 1.1, + 0.006, + 0.00012 + ], + "obj_friction_low": [ + 0.9, + 0.004, + 8e-05 + ], + "obj_mass_high": 0.32006089604646315, + "obj_mass_low": 0.22006089604646317, + "obj_scale_change": [ + 0.1, + 0.05, + 0.1 + ], + "obs_dim": 210, + "proximity_th": 0.17, + "qpos_dim": 71, + "qvel_dim": 70, + "reward_act_w": 0.0, + "reward_fin_dis_w": -0.5, + "reward_pass_err_w": -1.0, + "reward_reach_dist_w": -0.1, + "start_center": [ + -0.4, + -0.25, + 1.05 + ], + "start_shifts": [ + 0.055, + 0.055, + 0.0 + ] + }, + "entry_module": "myosuite.envs.myo.myochallenge.bimanual_v0", + "id": "myoChallengeBimanual-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "envs/myo/assets/arm/myoarm_bionic_bimanual.xml", + "normalize_act": true, + "obj_friction_change": [ + 0.1, + 0.001, + 2e-05 + ], + "obj_mass_change": [ + -0.05, + 0.05 + ], + "obj_scale_change": [ + 0.1, + 0.05, + 0.1 + ] + }, + "max_episode_steps": 1000, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeBimanual-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeBimanual-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "ChaseTagEnvV0", + "default_config": { + "act_dim": 80, + "action_dim": 80, + "chase_vel_high": 1.0, + "chase_vel_low": 1.0, + "frame_skip": 10, + "hills_range": [ + 0.0, + 0.0 + ], + "min_spawn_distance": 2.0, + "model_path": "envs/myo/assets/leg/myolegs_chasetag.xml", + "normalize_act": true, + "obs_dim": 393, + "opponent_probabilities": [ + 0.1, + 0.45, + 0.45 + ], + "qpos_dim": 35, + "qvel_dim": 34, + "random_vel_high": 1.0, + "random_vel_low": 1.0, + "relief_range": [ + 0.0, + 0.0 + ], + "repeller_opponent": false, + "repeller_vel_high": 1.0, + "repeller_vel_low": 1.0, + "reset_type": "init", + "reward_distance_w": -0.1, + "reward_lose_w": -1000.0, + "rough_range": [ + 0.0, + 0.0 + ], + "task_choice": "CHASE", + "terrain": "FLAT", + "win_distance": 0.5 + }, + "entry_module": "myosuite.envs.myo.myochallenge.chasetag_v0", + "id": "myoChallengeChaseTagP1-v0", + "kwargs": { + "hills_range": [ + 0.0, + 0.0 + ], + "min_spawn_distance": 2, + "model_path": "envs/myo/assets/leg/myolegs_chasetag.xml", + "normalize_act": true, + "opponent_probabilities": [ + 0.1, + 0.45, + 0.45 + ], + "relief_range": [ + 0.0, + 0.0 + ], + "reset_type": "init", + "rough_range": [ + 0.0, + 0.0 + ], + "task_choice": "CHASE", + "terrain": "FLAT", + "win_distance": 0.5 + }, + "max_episode_steps": 2000, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeChaseTagP1-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeChaseTagP1-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "ChaseTagEnvV0", + "default_config": { + "act_dim": 80, + "action_dim": 80, + "chase_vel_high": 1.0, + "chase_vel_low": 1.0, + "frame_skip": 10, + "hills_range": [ + 0.03, + 0.23 + ], + "min_spawn_distance": 2.0, + "model_path": "envs/myo/assets/leg/myolegs_chasetag.xml", + "normalize_act": true, + "obs_dim": 393, + "opponent_probabilities": [ + 0.1, + 0.45, + 0.45 + ], + "qpos_dim": 35, + "qvel_dim": 34, + "random_vel_high": 2.0, + "random_vel_low": -2.0, + "relief_range": [ + 0.1, + 0.3 + ], + "repeller_opponent": false, + "repeller_vel_high": 1.0, + "repeller_vel_low": 1.0, + "reset_type": "random", + "reward_distance_w": -0.1, + "reward_lose_w": -1000.0, + "rough_range": [ + 0.05, + 0.1 + ], + "task_choice": "random", + "terrain": "random", + "win_distance": 0.5 + }, + "entry_module": "myosuite.envs.myo.myochallenge.chasetag_v0", + "id": "myoChallengeChaseTagP2-v0", + "kwargs": { + "chase_vel_range": [ + 1.0, + 1.0 + ], + "hills_range": [ + 0.03, + 0.23 + ], + "min_spawn_distance": 2, + "model_path": "envs/myo/assets/leg/myolegs_chasetag.xml", + "normalize_act": true, + "opponent_probabilities": [ + 0.1, + 0.45, + 0.45 + ], + "random_vel_range": [ + -2, + 2 + ], + "relief_range": [ + 0.1, + 0.3 + ], + "repeller_opponent": false, + "reset_type": "random", + "rough_range": [ + 0.05, + 0.1 + ], + "task_choice": "random", + "terrain": "random", + "win_distance": 0.5 + }, + "max_episode_steps": 2000, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeChaseTagP2-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeChaseTagP2-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "ChaseTagEnvV0", + "default_config": { + "act_dim": 80, + "action_dim": 80, + "chase_vel_high": 5.0, + "chase_vel_low": 1.0, + "frame_skip": 10, + "hills_range": [ + 0.03, + 0.23 + ], + "min_spawn_distance": 2.0, + "model_path": "envs/myo/assets/leg/myolegs_chasetag.xml", + "normalize_act": true, + "obs_dim": 393, + "opponent_probabilities": [ + 0.1, + 0.35, + 0.35, + 0.2 + ], + "qpos_dim": 35, + "qvel_dim": 34, + "random_vel_high": 2.0, + "random_vel_low": -2.0, + "relief_range": [ + 0.1, + 0.3 + ], + "repeller_opponent": true, + "repeller_vel_high": 1.0, + "repeller_vel_low": 0.3, + "reset_type": "random", + "reward_distance_w": -0.1, + "reward_lose_w": -1000.0, + "rough_range": [ + 0.05, + 0.1 + ], + "task_choice": "random", + "terrain": "random", + "win_distance": 0.5 + }, + "entry_module": "myosuite.envs.myo.myochallenge.chasetag_v0", + "id": "myoChallengeChaseTagP2eval-v0", + "kwargs": { + "chase_vel_range": [ + 1, + 5 + ], + "hills_range": [ + 0.03, + 0.23 + ], + "min_spawn_distance": 2, + "model_path": "envs/myo/assets/leg/myolegs_chasetag.xml", + "normalize_act": true, + "opponent_probabilities": [ + 0.1, + 0.35, + 0.35, + 0.2 + ], + "random_vel_range": [ + -2, + 2 + ], + "relief_range": [ + 0.1, + 0.3 + ], + "repeller_opponent": true, + "repeller_vel_range": [ + 0.3, + 1 + ], + "reset_type": "random", + "rough_range": [ + 0.05, + 0.1 + ], + "task_choice": "random", + "terrain": "random", + "win_distance": 0.5 + }, + "max_episode_steps": 2000, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeChaseTagP2eval-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeChaseTagP2eval-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "ReorientEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "drop_th": 0.2, + "frame_skip": 5, + "goal_pos_high": 0.0, + "goal_pos_low": 0.0, + "goal_rot_high": 0.785, + "goal_rot_low": -0.785, + "model_path": "envs/myo/assets/hand/myohand_die.xml", + "normalize_act": true, + "obj_friction_change": [ + 0.0, + 0.0, + 0.0 + ], + "obj_mass_high": 0.108, + "obj_mass_low": 0.108, + "obj_size_change": 0.0, + "obs_dim": 102, + "pos_th": Infinity, + "qpos_dim": 29, + "qvel_dim": 29, + "reward_act_reg_w": 0.0, + "reward_bonus_w": 0.0, + "reward_penalty_w": 0.0, + "reward_pos_dist_w": 100.0, + "reward_rot_dist_w": 1.0, + "rot_th": 0.262 + }, + "entry_module": "myosuite.envs.myo.myochallenge.reorient_v0", + "id": "myoChallengeDieReorientDemo-v0", + "kwargs": { + "frame_skip": 5, + "goal_pos": [ + 0, + 0 + ], + "goal_rot": [ + -0.785, + 0.785 + ], + "model_path": "envs/myo/assets/hand/myohand_die.xml", + "normalize_act": true, + "pos_th": Infinity + }, + "max_episode_steps": 150, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeDieReorientDemo-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeDieReorientDemo-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "ReorientEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "drop_th": 0.2, + "frame_skip": 5, + "goal_pos_high": 0.01, + "goal_pos_low": -0.01, + "goal_rot_high": 1.57, + "goal_rot_low": -1.57, + "model_path": "envs/myo/assets/hand/myohand_die.xml", + "normalize_act": true, + "obj_friction_change": [ + 0.0, + 0.0, + 0.0 + ], + "obj_mass_high": 0.108, + "obj_mass_low": 0.108, + "obj_size_change": 0.0, + "obs_dim": 102, + "pos_th": 0.025, + "qpos_dim": 29, + "qvel_dim": 29, + "reward_act_reg_w": 0.0, + "reward_bonus_w": 0.0, + "reward_penalty_w": 0.0, + "reward_pos_dist_w": 100.0, + "reward_rot_dist_w": 1.0, + "rot_th": 0.262 + }, + "entry_module": "myosuite.envs.myo.myochallenge.reorient_v0", + "id": "myoChallengeDieReorientP1-v0", + "kwargs": { + "frame_skip": 5, + "goal_pos": [ + -0.01, + 0.01 + ], + "goal_rot": [ + -1.57, + 1.57 + ], + "model_path": "envs/myo/assets/hand/myohand_die.xml", + "normalize_act": true + }, + "max_episode_steps": 150, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeDieReorientP1-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeDieReorientP1-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "ReorientEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "drop_th": 0.2, + "frame_skip": 5, + "goal_pos_high": 0.02, + "goal_pos_low": -0.02, + "goal_rot_high": 3.14, + "goal_rot_low": -3.14, + "model_path": "envs/myo/assets/hand/myohand_die.xml", + "normalize_act": true, + "obj_friction_change": [ + 0.2, + 0.001, + 2e-05 + ], + "obj_mass_high": 0.25, + "obj_mass_low": 0.05, + "obj_size_change": 0.007, + "obs_dim": 102, + "pos_th": 0.025, + "qpos_dim": 29, + "qvel_dim": 29, + "reward_act_reg_w": 0.0, + "reward_bonus_w": 0.0, + "reward_penalty_w": 0.0, + "reward_pos_dist_w": 100.0, + "reward_rot_dist_w": 1.0, + "rot_th": 0.262 + }, + "entry_module": "myosuite.envs.myo.myochallenge.reorient_v0", + "id": "myoChallengeDieReorientP2-v0", + "kwargs": { + "frame_skip": 5, + "goal_pos": [ + -0.02, + 0.02 + ], + "goal_rot": [ + -3.14, + 3.14 + ], + "model_path": "envs/myo/assets/hand/myohand_die.xml", + "normalize_act": true, + "obj_friction_change": [ + 0.2, + 0.001, + 2e-05 + ], + "obj_mass_range": [ + 0.05, + 0.25 + ], + "obj_size_change": 0.007 + }, + "max_episode_steps": 150, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeDieReorientP2-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeDieReorientP2-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "RunTrack", + "default_config": { + "act_dim": 54, + "action_dim": 54, + "ctrl_dim": 56, + "end_pos": -15.0, + "frame_skip": 5, + "hills_difficulties": [ + 0.0, + 0.1, + 0.0, + 0.5, + 0.0, + 0.8, + 0.0, + 1.0 + ], + "model_path": "envs/myo/assets/leg/myoosl_runtrack.xml", + "normalize_act": true, + "obs_dim": 260, + "qpos_dim": 30, + "qvel_dim": 29, + "real_width": 1.0, + "reset_type": "random", + "reward_solved_w": 10.0, + "reward_sparse_w": 1.0, + "rough_difficulties": [ + 0.0, + 0.1, + 0.0, + 0.15, + 0.0, + 0.2, + 0.0, + 0.3 + ], + "stairs_difficulties": [ + 0.0, + 0.05, + 0.0, + 0.1, + 0.0, + 0.2, + 0.0, + 0.3 + ], + "start_pos": 14.0, + "terrain": "flat" + }, + "entry_module": "myosuite.envs.myo.myochallenge.run_track_v0", + "id": "myoChallengeOslRunFixed-v0", + "kwargs": { + "end_pos": -15, + "frame_skip": 5, + "hills_difficulties": [ + 0.0, + 0.1, + 0.0, + 0.5, + 0.0, + 0.8, + 0.0, + 1.0 + ], + "init_pose_path": "envs/myo/assets/leg/sample_gait_cycle.csv", + "max_episode_steps": 1000, + "model_path": "envs/myo/assets/leg/myoosl_runtrack.xml", + "normalize_act": true, + "reset_type": "random", + "rough_difficulties": [ + 0.0, + 0.1, + 0.0, + 0.15, + 0.0, + 0.2, + 0.0, + 0.3 + ], + "stairs_difficulties": [ + 0.0, + 0.05, + 0.0, + 0.1, + 0.0, + 0.2, + 0.0, + 0.3 + ], + "start_pos": 14, + "terrain": "flat" + }, + "max_episode_steps": 1000, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeOslRunFixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeOslRunFixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "RunTrack", + "default_config": { + "act_dim": 54, + "action_dim": 54, + "ctrl_dim": 56, + "end_pos": -45.0, + "frame_skip": 5, + "hills_difficulties": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.03, + 0.0, + 0.06, + 0.0, + 0.09, + 0.0, + 0.12, + 0.0, + 0.15, + 0.0, + 0.18, + 0.0, + 0.21, + 0.0, + 0.24, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "model_path": "envs/myo/assets/leg/myoosl_runtrack.xml", + "normalize_act": true, + "obs_dim": 260, + "qpos_dim": 30, + "qvel_dim": 29, + "real_width": 1.0, + "reset_type": "random", + "reward_solved_w": 10.0, + "reward_sparse_w": 1.0, + "rough_difficulties": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.03, + 0.0, + 0.06, + 0.0, + 0.09, + 0.0, + 0.12, + 0.0, + 0.15, + 0.0, + 0.18, + 0.0, + 0.21, + 0.0, + 0.24, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "stairs_difficulties": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.03, + 0.0, + 0.06, + 0.0, + 0.09, + 0.0, + 0.12, + 0.0, + 0.15, + 0.0, + 0.18, + 0.0, + 0.21, + 0.0, + 0.24, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "start_pos": 58.0, + "terrain": "random" + }, + "entry_module": "myosuite.envs.myo.myochallenge.run_track_v0", + "id": "myoChallengeOslRunRandom-v0", + "kwargs": { + "end_pos": -45, + "frame_skip": 5, + "hills_difficulties": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.03, + 0.0, + 0.06, + 0.0, + 0.09, + 0.0, + 0.12, + 0.0, + 0.15, + 0.0, + 0.18, + 0.0, + 0.21, + 0.0, + 0.24, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "init_pose_path": "envs/myo/assets/leg/sample_gait_cycle.csv", + "max_episode_steps": 60000, + "model_path": "envs/myo/assets/leg/myoosl_runtrack.xml", + "normalize_act": true, + "reset_type": "random", + "rough_difficulties": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.03, + 0.0, + 0.06, + 0.0, + 0.09, + 0.0, + 0.12, + 0.0, + 0.15, + 0.0, + 0.18, + 0.0, + 0.21, + 0.0, + 0.24, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "stairs_difficulties": [ + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.03, + 0.0, + 0.06, + 0.0, + 0.09, + 0.0, + 0.12, + 0.0, + 0.15, + 0.0, + 0.18, + 0.0, + 0.21, + 0.0, + 0.24, + 0.0, + 0.0, + 0.0, + 0.0 + ], + "start_pos": 58, + "terrain": "random" + }, + "max_episode_steps": 60000, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeOslRunRandom-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeOslRunRandom-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "RelocateEnvV0", + "default_config": { + "act_dim": 63, + "action_dim": 63, + "drop_th": 0.5, + "frame_skip": 5, + "model_path": "envs/myo/assets/arm/myoarm_relocate.xml", + "normalize_act": true, + "obj_friction_high": [], + "obj_friction_low": [], + "obj_geom_high": [], + "obj_geom_low": [], + "obj_mass_high": 0.0, + "obj_mass_low": 0.0, + "obj_xyz_high": [], + "obj_xyz_low": [], + "obs_dim": 156, + "pos_th": 0.1, + "qpos_dim": 44, + "qpos_noise_range": 0.0, + "qvel_dim": 44, + "reward_act_reg_w": 0.0, + "reward_pos_dist_w": 100.0, + "reward_rot_dist_w": 1.0, + "rot_th": Infinity, + "target_rxryrz_high": [ + 0.0, + 0.0, + 0.0 + ], + "target_rxryrz_low": [ + 0.0, + 0.0, + 0.0 + ], + "target_xyz_high": [ + 0.2, + -0.1, + 0.9 + ], + "target_xyz_low": [ + 0.0, + -0.35, + 0.9 + ] + }, + "entry_module": "myosuite.envs.myo.myochallenge.relocate_v0", + "id": "myoChallengeRelocateP1-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "envs/myo/assets/arm/myoarm_relocate.xml", + "normalize_act": true, + "pos_th": 0.1, + "rot_th": Infinity, + "target_rxryrz_range": { + "high": [ + 0.0, + 0.0, + 0.0 + ], + "low": [ + 0.0, + 0.0, + 0.0 + ] + }, + "target_xyz_range": { + "high": [ + 0.2, + -0.1, + 0.9 + ], + "low": [ + 0.0, + -0.35, + 0.9 + ] + } + }, + "max_episode_steps": 150, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeRelocateP1-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeRelocateP1-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "RelocateEnvV0", + "default_config": { + "act_dim": 63, + "action_dim": 63, + "drop_th": 0.5, + "frame_skip": 5, + "model_path": "envs/myo/assets/arm/myoarm_relocate.xml", + "normalize_act": true, + "obj_friction_high": [ + 1.2, + 0.006, + 0.00012 + ], + "obj_friction_low": [ + 0.8, + 0.004, + 8e-05 + ], + "obj_geom_high": [ + 0.025, + 0.025, + 0.025 + ], + "obj_geom_low": [ + 0.015, + 0.015, + 0.015 + ], + "obj_mass_high": 0.2, + "obj_mass_low": 0.05, + "obj_xyz_high": [ + 0.1, + -0.15, + 1.0 + ], + "obj_xyz_low": [ + -0.1, + -0.35, + 1.0 + ], + "obs_dim": 156, + "pos_th": 0.1, + "qpos_dim": 44, + "qpos_noise_range": 0.01, + "qvel_dim": 44, + "reward_act_reg_w": 0.0, + "reward_pos_dist_w": 100.0, + "reward_rot_dist_w": 1.0, + "rot_th": Infinity, + "target_rxryrz_high": [ + 0.2, + 0.2, + 0.2 + ], + "target_rxryrz_low": [ + -0.2, + -0.2, + -0.2 + ], + "target_xyz_high": [ + 0.3, + -0.1, + 1.05 + ], + "target_xyz_low": [ + 0.0, + -0.45, + 0.9 + ] + }, + "entry_module": "myosuite.envs.myo.myochallenge.relocate_v0", + "id": "myoChallengeRelocateP2-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "envs/myo/assets/arm/myoarm_relocate.xml", + "normalize_act": true, + "obj_friction_range": { + "high": [ + 1.2, + 0.006, + 0.00012 + ], + "low": [ + 0.8, + 0.004, + 8e-05 + ] + }, + "obj_geom_range": { + "high": [ + 0.025, + 0.025, + 0.025 + ], + "low": [ + 0.015, + 0.015, + 0.015 + ] + }, + "obj_mass_range": { + "high": 0.2, + "low": 0.05 + }, + "obj_xyz_range": { + "high": [ + 0.1, + -0.15, + 1.0 + ], + "low": [ + -0.1, + -0.35, + 1.0 + ] + }, + "pos_th": 0.1, + "qpos_noise_range": 0.01, + "rot_th": Infinity, + "target_rxryrz_range": { + "high": [ + 0.2, + 0.2, + 0.2 + ], + "low": [ + -0.2, + -0.2, + -0.2 + ] + }, + "target_xyz_range": { + "high": [ + 0.3, + -0.1, + 1.05 + ], + "low": [ + 0.0, + -0.45, + 0.9 + ] + } + }, + "max_episode_steps": 150, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeRelocateP2-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeRelocateP2-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "RelocateEnvV0", + "default_config": { + "act_dim": 63, + "action_dim": 63, + "drop_th": 0.5, + "frame_skip": 5, + "model_path": "envs/myo/assets/arm/myoarm_relocate.xml", + "normalize_act": true, + "obj_friction_high": [ + 1.2, + 0.006, + 0.00012 + ], + "obj_friction_low": [ + 0.8, + 0.004, + 8e-05 + ], + "obj_geom_high": [ + 0.025, + 0.025, + 0.035 + ], + "obj_geom_low": [ + 0.015, + 0.015, + 0.015 + ], + "obj_mass_high": 0.3, + "obj_mass_low": 0.05, + "obj_xyz_high": [ + 0.15, + -0.1, + 1.0 + ], + "obj_xyz_low": [ + -0.2, + -0.4, + 1.0 + ], + "obs_dim": 156, + "pos_th": 0.1, + "qpos_dim": 44, + "qpos_noise_range": 0.015, + "qvel_dim": 44, + "reward_act_reg_w": 0.0, + "reward_pos_dist_w": 100.0, + "reward_rot_dist_w": 1.0, + "rot_th": Infinity, + "target_rxryrz_high": [ + 0.3, + 0.3, + 0.3 + ], + "target_rxryrz_low": [ + -0.3, + -0.3, + -0.3 + ], + "target_xyz_high": [ + 0.4, + -0.1, + 1.1 + ], + "target_xyz_low": [ + -0.5, + -0.5, + 0.9 + ] + }, + "entry_module": "myosuite.envs.myo.myochallenge.relocate_v0", + "id": "myoChallengeRelocateP2eval-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "envs/myo/assets/arm/myoarm_relocate.xml", + "normalize_act": true, + "obj_friction_range": { + "high": [ + 1.2, + 0.006, + 0.00012 + ], + "low": [ + 0.8, + 0.004, + 8e-05 + ] + }, + "obj_geom_range": { + "high": [ + 0.025, + 0.025, + 0.035 + ], + "low": [ + 0.015, + 0.015, + 0.015 + ] + }, + "obj_mass_range": { + "high": 0.3, + "low": 0.05 + }, + "obj_xyz_range": { + "high": [ + 0.15, + -0.1, + 1.0 + ], + "low": [ + -0.2, + -0.4, + 1.0 + ] + }, + "pos_th": 0.1, + "qpos_noise_range": 0.015, + "rot_th": Infinity, + "target_rxryrz_range": { + "high": [ + 0.3, + 0.3, + 0.3 + ], + "low": [ + -0.3, + -0.3, + -0.3 + ] + }, + "target_xyz_range": { + "high": [ + 0.4, + -0.1, + 1.1 + ], + "low": [ + -0.5, + -0.5, + 0.9 + ] + } + }, + "max_episode_steps": 150, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeRelocateP2eval-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeRelocateP2eval-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "SoccerEnvV0", + "default_config": { + "act_dim": 290, + "action_dim": 290, + "frame_skip": 10, + "goalkeeper_probabilities": [ + 0, + 0, + 1 + ], + "max_time_sec": 10.0, + "min_agent_spawn_distance": 1.0, + "model_path": "envs/myo/assets/leg_soccer/myolegs_soccer.xml", + "normalize_act": true, + "obs_dim": 1276, + "qpos_dim": 60, + "qvel_dim": 58, + "random_vel_high": 5.0, + "random_vel_low": 1.0, + "reset_type": "none", + "reward_act_reg_w": -100.0, + "reward_goal_scored_w": 1000.0, + "reward_pain_w": -10.0, + "reward_time_cost_w": -0.01, + "rnd_joint_noise": 0.02, + "rnd_pos_noise": 1.0 + }, + "entry_module": "myosuite.envs.myo.myochallenge.soccer_v0", + "id": "myoChallengeSoccerP1-v0", + "kwargs": { + "goalkeeper_probabilities": [ + 0, + 0, + 1 + ], + "min_agent_spawn_distance": 1, + "model_path": "envs/myo/assets/leg_soccer/myolegs_soccer.xml", + "normalize_act": true, + "reset_type": "none" + }, + "max_episode_steps": 2000, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeSoccerP1-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeSoccerP1-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "SoccerEnvV0", + "default_config": { + "act_dim": 290, + "action_dim": 290, + "frame_skip": 10, + "goalkeeper_probabilities": [ + 0.1, + 0.3, + 0.6 + ], + "max_time_sec": 10.0, + "min_agent_spawn_distance": 1.0, + "model_path": "envs/myo/assets/leg_soccer/myolegs_soccer.xml", + "normalize_act": true, + "obs_dim": 1276, + "qpos_dim": 60, + "qvel_dim": 58, + "random_vel_high": 5.0, + "random_vel_low": 1.0, + "reset_type": "random", + "reward_act_reg_w": -100.0, + "reward_goal_scored_w": 1000.0, + "reward_pain_w": -10.0, + "reward_time_cost_w": -0.01, + "rnd_joint_noise": 0.02, + "rnd_pos_noise": 1.0 + }, + "entry_module": "myosuite.envs.myo.myochallenge.soccer_v0", + "id": "myoChallengeSoccerP2-v0", + "kwargs": { + "goalkeeper_probabilities": [ + 0.1, + 0.3, + 0.6 + ], + "max_time_sec": 10, + "min_agent_spawn_distance": 1, + "model_path": "envs/myo/assets/leg_soccer/myolegs_soccer.xml", + "normalize_act": true, + "reset_type": "random" + }, + "max_episode_steps": 2000, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeSoccerP2-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeSoccerP2-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "TableTennisEnvV0", + "default_config": { + "act_dim": 273, + "action_dim": 275, + "ball_friction_high": [], + "ball_friction_low": [], + "ball_qvel": false, + "ball_xyz_high": [], + "ball_xyz_low": [], + "frame_skip": 5, + "model_path": "simhive/myo_sim/arm/myoarm_tabletennis.xml", + "normalize_act": true, + "obs_dim": 417, + "paddle_mass_high": 0.0, + "paddle_mass_low": 0.0, + "qpos_dim": 72, + "qpos_noise_high": NaN, + "qpos_noise_low": NaN, + "qvel_dim": 70, + "rally_count": 1, + "reward_act_reg_w": 0.5, + "reward_done_w": -10.0, + "reward_paddle_quat_w": 2.0, + "reward_palm_dist_w": 1.0, + "reward_reach_dist_w": 1.0, + "reward_solved_w": 1000.0, + "reward_sparse_w": 100.0, + "reward_torso_up_w": 2.0 + }, + "entry_module": "myosuite.envs.myo.myochallenge.tabletennis_v0", + "id": "myoChallengeTableTennisP0-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "envs/myo/assets/arm/myoarm_tabletennis.xml", + "normalize_act": true + }, + "max_episode_steps": 300, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeTableTennisP0-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeTableTennisP0-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "TableTennisEnvV0", + "default_config": { + "act_dim": 273, + "action_dim": 275, + "ball_friction_high": [], + "ball_friction_low": [], + "ball_qvel": false, + "ball_xyz_high": [ + -1.2, + -0.45, + 1.5 + ], + "ball_xyz_low": [ + -1.25, + -0.5, + 1.4 + ], + "frame_skip": 5, + "model_path": "simhive/myo_sim/arm/myoarm_tabletennis.xml", + "normalize_act": true, + "obs_dim": 417, + "paddle_mass_high": 0.0, + "paddle_mass_low": 0.0, + "qpos_dim": 72, + "qpos_noise_high": NaN, + "qpos_noise_low": NaN, + "qvel_dim": 70, + "rally_count": 1, + "reward_act_reg_w": 0.5, + "reward_done_w": -10.0, + "reward_paddle_quat_w": 2.0, + "reward_palm_dist_w": 1.0, + "reward_reach_dist_w": 1.0, + "reward_solved_w": 1000.0, + "reward_sparse_w": 100.0, + "reward_torso_up_w": 2.0 + }, + "entry_module": "myosuite.envs.myo.myochallenge.tabletennis_v0", + "id": "myoChallengeTableTennisP1-v0", + "kwargs": { + "ball_xyz_range": { + "high": [ + -1.2, + -0.45, + 1.5 + ], + "low": [ + -1.25, + -0.5, + 1.4 + ] + }, + "frame_skip": 5, + "model_path": "envs/myo/assets/arm/myoarm_tabletennis.xml", + "normalize_act": true + }, + "max_episode_steps": 300, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeTableTennisP1-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeTableTennisP1-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "TableTennisEnvV0", + "default_config": { + "act_dim": 273, + "action_dim": 275, + "ball_friction_high": [ + 1.1, + 0.006, + 3e-05 + ], + "ball_friction_low": [ + 0.9, + 0.004, + 1e-05 + ], + "ball_qvel": true, + "ball_xyz_high": [ + -0.8, + 0.5, + 1.5 + ], + "ball_xyz_low": [ + -1.25, + -0.5, + 1.4 + ], + "frame_skip": 5, + "model_path": "simhive/myo_sim/arm/myoarm_tabletennis.xml", + "normalize_act": true, + "obs_dim": 417, + "paddle_mass_high": 0.15, + "paddle_mass_low": 0.1, + "qpos_dim": 72, + "qpos_noise_high": NaN, + "qpos_noise_low": NaN, + "qvel_dim": 70, + "rally_count": 1, + "reward_act_reg_w": 0.5, + "reward_done_w": -10.0, + "reward_paddle_quat_w": 2.0, + "reward_palm_dist_w": 1.0, + "reward_reach_dist_w": 1.0, + "reward_solved_w": 1000.0, + "reward_sparse_w": 100.0, + "reward_torso_up_w": 2.0 + }, + "entry_module": "myosuite.envs.myo.myochallenge.tabletennis_v0", + "id": "myoChallengeTableTennisP2-v0", + "kwargs": { + "ball_friction_range": { + "high": [ + 1.1, + 0.006, + 3e-05 + ], + "low": [ + 0.9, + 0.004, + 1e-05 + ] + }, + "ball_qvel": true, + "ball_xyz_range": { + "high": [ + -0.8, + 0.5, + 1.5 + ], + "low": [ + -1.25, + -0.5, + 1.4 + ] + }, + "frame_skip": 5, + "model_path": "envs/myo/assets/arm/myoarm_tabletennis.xml", + "normalize_act": true, + "paddle_mass_range": [ + 0.1, + 0.15 + ], + "qpos_noise_range": null + }, + "max_episode_steps": 300, + "model_placeholders": [], + "registration_suite": "myochallenge", + "suite": "myochallenge", + "variant_defs": [ + { + "variant_id": "myoFatiChallengeTableTennisP2-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcChallengeTableTennisP2-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 6, + "action_dim": 7, + "frame_skip": 10, + "model_path": "envs/myo/assets/elbow/myoelbow_1dof6muscles_1dofexo.xml", + "normalize_act": true, + "obs_dim": 9, + "pose_thd": 0.175, + "qpos_dim": 1, + "qvel_dim": 1, + "reset_type": "random", + "reward_act_reg_w": 5.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [ + 2 + ], + "target_qpos_min": [ + 2 + ], + "target_qpos_value": [ + 2.0 + ], + "target_type": "fixed", + "viz_site_targets": [ + "wrist" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoElbowPose1D6MExoFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/elbow/myoelbow_1dof6muscles_1dofexo.xml", + "normalize_act": true, + "pose_thd": 0.175, + "reset_type": "random", + "target_jnt_range": { + "r_elbow_flex": [ + 2, + 2 + ] + }, + "viz_site_targets": [ + "wrist" + ], + "weighted_reward_keys": { + "act_reg": 5.0, + "bonus": 4.0, + "penalty": 50, + "pose": 1.0 + } + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiElbowPose1D6MExoFixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcElbowPose1D6MExoFixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 6, + "action_dim": 7, + "frame_skip": 10, + "model_path": "envs/myo/assets/elbow/myoelbow_1dof6muscles_1dofexo.xml", + "normalize_act": true, + "obs_dim": 9, + "pose_thd": 0.175, + "qpos_dim": 1, + "qvel_dim": 1, + "reset_type": "random", + "reward_act_reg_w": 5.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [ + 2.27 + ], + "target_qpos_min": [ + 0 + ], + "target_qpos_value": [ + 1.135 + ], + "target_type": "fixed", + "viz_site_targets": [ + "wrist" + ], + "weight_bodyname": "carry_weight", + "weight_range": [ + 0.1, + 2 + ] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoElbowPose1D6MExoRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/elbow/myoelbow_1dof6muscles_1dofexo.xml", + "normalize_act": true, + "pose_thd": 0.175, + "reset_type": "random", + "target_jnt_range": { + "r_elbow_flex": [ + 0, + 2.27 + ] + }, + "viz_site_targets": [ + "wrist" + ], + "weight_bodyname": "carry_weight", + "weight_range": [ + 0.1, + 2 + ], + "weighted_reward_keys": { + "act_reg": 5.0, + "bonus": 4.0, + "penalty": 50, + "pose": 1.0 + } + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiElbowPose1D6MExoRandom-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcElbowPose1D6MExoRandom-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 6, + "action_dim": 6, + "frame_skip": 10, + "model_path": "envs/myo/assets/elbow/myoelbow_1dof6muscles.xml", + "normalize_act": true, + "obs_dim": 9, + "pose_thd": 0.175, + "qpos_dim": 1, + "qvel_dim": 1, + "reset_type": "random", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [ + 2 + ], + "target_qpos_min": [ + 2 + ], + "target_qpos_value": [ + 2.0 + ], + "target_type": "fixed", + "viz_site_targets": [ + "wrist" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoElbowPose1D6MFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/elbow/myoelbow_1dof6muscles.xml", + "normalize_act": true, + "pose_thd": 0.175, + "reset_type": "random", + "target_jnt_range": { + "r_elbow_flex": [ + 2, + 2 + ] + }, + "viz_site_targets": [ + "wrist" + ] + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiElbowPose1D6MFixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcElbowPose1D6MFixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 6, + "action_dim": 6, + "frame_skip": 10, + "model_path": "envs/myo/assets/elbow/myoelbow_1dof6muscles.xml", + "normalize_act": true, + "obs_dim": 9, + "pose_thd": 0.175, + "qpos_dim": 1, + "qvel_dim": 1, + "reset_type": "random", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [ + 2.27 + ], + "target_qpos_min": [ + 0 + ], + "target_qpos_value": [ + 1.135 + ], + "target_type": "fixed", + "viz_site_targets": [ + "wrist" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoElbowPose1D6MRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/elbow/myoelbow_1dof6muscles.xml", + "normalize_act": true, + "pose_thd": 0.175, + "reset_type": "random", + "target_jnt_range": { + "r_elbow_flex": [ + 0, + 2.27 + ] + }, + "viz_site_targets": [ + "wrist" + ] + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiElbowPose1D6MRandom-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcElbowPose1D6MRandom-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 5, + "action_dim": 5, + "frame_skip": 10, + "model_path": "simhive/myo_sim/finger/myofinger_v0.xml", + "normalize_act": true, + "obs_dim": 17, + "pose_thd": 0.35, + "qpos_dim": 4, + "qvel_dim": 4, + "reset_type": "init", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [ + 0, + 0.75, + 0, + 0.75 + ], + "target_qpos_min": [ + 0, + 0.75, + 0, + 0.75 + ], + "target_qpos_value": [ + 0.0, + 0.75, + 0.0, + 0.75 + ], + "target_type": "fixed", + "viz_site_targets": [ + "IFtip" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoFingerPoseFixed-v0", + "kwargs": { + "model_path": "simhive/myo_sim/finger/myofinger_v0.xml", + "normalize_act": true, + "target_jnt_range": { + "IFadb": [ + 0, + 0 + ], + "IFdip": [ + 0.75, + 0.75 + ], + "IFmcp": [ + 0, + 0 + ], + "IFpip": [ + 0.75, + 0.75 + ] + }, + "viz_site_targets": [ + "IFtip" + ] + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiFingerPoseFixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcFingerPoseFixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 5, + "action_dim": 5, + "frame_skip": 10, + "model_path": "simhive/myo_sim/finger/myofinger_v0.xml", + "normalize_act": true, + "obs_dim": 17, + "pose_thd": 0.35, + "qpos_dim": 4, + "qvel_dim": 4, + "reset_type": "init", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [ + 0.2, + 1, + 1, + 1 + ], + "target_qpos_min": [ + -0.2, + 0.1, + -0.4, + 0.1 + ], + "target_qpos_value": [ + 0.0, + 0.55, + 0.3, + 0.55 + ], + "target_type": "fixed", + "viz_site_targets": [ + "IFtip" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoFingerPoseRandom-v0", + "kwargs": { + "model_path": "simhive/myo_sim/finger/myofinger_v0.xml", + "normalize_act": true, + "target_jnt_range": { + "IFadb": [ + -0.2, + 0.2 + ], + "IFdip": [ + 0.1, + 1 + ], + "IFmcp": [ + -0.4, + 1 + ], + "IFpip": [ + 0.1, + 1 + ] + }, + "viz_site_targets": [ + "IFtip" + ] + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiFingerPoseRandom-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcFingerPoseRandom-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "ReachEnvV0", + "default_config": { + "act_dim": 5, + "action_dim": 5, + "far_th": 0.35, + "frame_skip": 10, + "hide_skin_geom_group_1": false, + "hide_terrain": false, + "joint_random_range": [], + "model_path": "simhive/myo_sim/finger/myofinger_v0.xml", + "normalize_act": true, + "obs_dim": 19, + "qpos_dim": 4, + "qvel_dim": 4, + "reward_act_reg_w": 0.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_reach_w": 1.0, + "target_pos_max": [ + 0.2, + 0.05, + 0.2 + ], + "target_pos_min": [ + 0.2, + 0.05, + 0.2 + ], + "target_pos_relative_to_tip": false, + "target_site_count": 1, + "target_site_names": [ + "IFtip" + ] + }, + "entry_module": "myosuite.envs.myo.myobase.reach_v0", + "id": "myoFingerReachFixed-v0", + "kwargs": { + "model_path": "simhive/myo_sim/finger/myofinger_v0.xml", + "normalize_act": true, + "target_reach_range": { + "IFtip": [ + [ + 0.2, + 0.05, + 0.2 + ], + [ + 0.2, + 0.05, + 0.2 + ] + ] + } + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiFingerReachFixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcFingerReachFixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "ReachEnvV0", + "default_config": { + "act_dim": 5, + "action_dim": 5, + "far_th": 0.35, + "frame_skip": 10, + "hide_skin_geom_group_1": false, + "hide_terrain": false, + "joint_random_range": [], + "model_path": "simhive/myo_sim/finger/myofinger_v0.xml", + "normalize_act": true, + "obs_dim": 19, + "qpos_dim": 4, + "qvel_dim": 4, + "reward_act_reg_w": 0.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_reach_w": 1.0, + "target_pos_max": [ + 0.27, + 0.1, + 0.3 + ], + "target_pos_min": [ + 0.1, + -0.1, + 0.1 + ], + "target_pos_relative_to_tip": false, + "target_site_count": 1, + "target_site_names": [ + "IFtip" + ] + }, + "entry_module": "myosuite.envs.myo.myobase.reach_v0", + "id": "myoFingerReachRandom-v0", + "kwargs": { + "model_path": "simhive/myo_sim/finger/myofinger_v0.xml", + "normalize_act": true, + "target_reach_range": { + "IFtip": [ + [ + 0.1, + -0.1, + 0.1 + ], + [ + 0.27, + 0.1, + 0.3 + ] + ] + } + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiFingerReachRandom-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcFingerReachRandom-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "KeyTurnEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 10, + "goal_th": 3.141592653589793, + "key_init_range": [ + 0.0, + 0.0 + ], + "model_path": "envs/myo/assets/hand/myohand_keyturn.xml", + "normalize_act": true, + "obs_dim": 93, + "qpos_dim": 24, + "qvel_dim": 24, + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_iftip_approach_w": 10.0, + "reward_key_turn_w": 1.0, + "reward_penalty_w": 25.0, + "reward_thtip_approach_w": 10.0 + }, + "entry_module": "myosuite.envs.myo.myobase.key_turn_v0", + "id": "myoHandKeyTurnFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_keyturn.xml", + "normalize_act": true + }, + "max_episode_steps": 200, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandKeyTurnFixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandKeyTurnFixed-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandKeyTurnFixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "KeyTurnEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 10, + "goal_th": 6.283185307179586, + "key_init_range": [ + -1.5707963267948966, + 1.5707963267948966 + ], + "model_path": "envs/myo/assets/hand/myohand_keyturn.xml", + "normalize_act": true, + "obs_dim": 93, + "qpos_dim": 24, + "qvel_dim": 24, + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_iftip_approach_w": 10.0, + "reward_key_turn_w": 1.0, + "reward_penalty_w": 25.0, + "reward_thtip_approach_w": 10.0 + }, + "entry_module": "myosuite.envs.myo.myobase.key_turn_v0", + "id": "myoHandKeyTurnRandom-v0", + "kwargs": { + "goal_th": 6.283185307179586, + "key_init_range": [ + -1.5707963267948966, + 1.5707963267948966 + ], + "model_path": "envs/myo/assets/hand/myohand_keyturn.xml", + "normalize_act": true + }, + "max_episode_steps": 200, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandKeyTurnRandom-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandKeyTurnRandom-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandKeyTurnRandom-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "ObjHoldFixedEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_hold.xml", + "normalize_act": true, + "obs_dim": 91, + "qpos_dim": 30, + "qvel_dim": 29, + "randomize_on_reset": false, + "reward_bonus_w": 4.0, + "reward_goal_dist_w": 100.0, + "reward_penalty_w": 10.0 + }, + "entry_module": "myosuite.envs.myo.myobase.obj_hold_v0", + "id": "myoHandObjHoldFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_hold.xml", + "normalize_act": true + }, + "max_episode_steps": 75, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandObjHoldFixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandObjHoldFixed-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandObjHoldFixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "ObjHoldRandomEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_hold.xml", + "normalize_act": true, + "obs_dim": 91, + "qpos_dim": 30, + "qvel_dim": 29, + "randomize_on_reset": true, + "reward_bonus_w": 4.0, + "reward_goal_dist_w": 100.0, + "reward_penalty_w": 10.0 + }, + "entry_module": "myosuite.envs.myo.myobase.obj_hold_v0", + "id": "myoHandObjHoldRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_hold.xml", + "normalize_act": true + }, + "max_episode_steps": 75, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandObjHoldRandom-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandObjHoldRandom-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandObjHoldRandom-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PenTwirlFixedEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 5, + "model_path": "envs/myo/assets/hand/myohand_pen.xml", + "normalize_act": true, + "obs_dim": 83, + "qpos_dim": 29, + "qvel_dim": 29, + "randomize_target": false, + "reward_act_reg_w": 5.0, + "reward_bonus_w": 10.0, + "reward_drop_w": 5.0, + "reward_pos_align_w": 1.0, + "reward_rot_align_w": 1.0 + }, + "entry_module": "myosuite.envs.myo.myobase.pen_v0", + "id": "myoHandPenTwirlFixed-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "envs/myo/assets/hand/myohand_pen.xml", + "normalize_act": true + }, + "max_episode_steps": 50, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandPenTwirlFixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandPenTwirlFixed-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandPenTwirlFixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PenTwirlRandomEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 5, + "model_path": "envs/myo/assets/hand/myohand_pen.xml", + "normalize_act": true, + "obs_dim": 83, + "qpos_dim": 29, + "qvel_dim": 29, + "randomize_target": true, + "reward_act_reg_w": 5.0, + "reward_bonus_w": 10.0, + "reward_drop_w": 5.0, + "reward_pos_align_w": 1.0, + "reward_rot_align_w": 1.0 + }, + "entry_module": "myosuite.envs.myo.myobase.pen_v0", + "id": "myoHandPenTwirlRandom-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "envs/myo/assets/hand/myohand_pen.xml", + "normalize_act": true + }, + "max_episode_steps": 50, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandPenTwirlRandom-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandPenTwirlRandom-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandPenTwirlRandom-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "obs_dim": 108, + "pose_thd": 0.7, + "qpos_dim": 23, + "qvel_dim": 23, + "reset_type": "init", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [], + "target_qpos_min": [], + "target_qpos_value": [ + 0.0, + 0.0, + 0.0, + 0.5624, + 0.28272, + -0.75573, + -1.309, + 1.30045, + -0.006982, + 1.45492, + 0.998897, + 1.26466, + 0.0, + 1.40604, + 0.227795, + 1.07614, + -0.020944, + 1.46103, + 0.06284, + 0.83263, + -0.14399, + 1.571, + 1.38248 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoHandPose0Fixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "pose_thd": 0.7, + "reset_type": "init", + "target_jnt_value": [ + 0.0, + 0.0, + 0.0, + 0.5624, + 0.28272, + -0.75573, + -1.309, + 1.30045, + -0.006982, + 1.45492, + 0.998897, + 1.26466, + 0.0, + 1.40604, + 0.227795, + 1.07614, + -0.020944, + 1.46103, + 0.06284, + 0.83263, + -0.14399, + 1.571, + 1.38248 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ] + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandPose0Fixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandPose0Fixed-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandPose0Fixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "obs_dim": 108, + "pose_thd": 0.7, + "qpos_dim": 23, + "qvel_dim": 23, + "reset_type": "init", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [], + "target_qpos_min": [], + "target_qpos_value": [ + 0.0, + 0.0, + 0.0, + 0.0248, + 0.04536, + -0.7854, + -1.309, + 0.366605, + 0.010473, + 0.269258, + 0.111722, + 1.48459, + 0.0, + 1.45318, + 1.44532, + 1.44532, + -0.204204, + 1.46103, + 1.44532, + 1.48459, + -0.2618, + 1.47674, + 1.48459 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoHandPose1Fixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "pose_thd": 0.7, + "reset_type": "init", + "target_jnt_value": [ + 0.0, + 0.0, + 0.0, + 0.0248, + 0.04536, + -0.7854, + -1.309, + 0.366605, + 0.010473, + 0.269258, + 0.111722, + 1.48459, + 0.0, + 1.45318, + 1.44532, + 1.44532, + -0.204204, + 1.46103, + 1.44532, + 1.48459, + -0.2618, + 1.47674, + 1.48459 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ] + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandPose1Fixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandPose1Fixed-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandPose1Fixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "obs_dim": 108, + "pose_thd": 0.7, + "qpos_dim": 23, + "qvel_dim": 23, + "reset_type": "init", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [], + "target_qpos_min": [], + "target_qpos_value": [ + 0.0, + 0.0, + 0.0, + 0.0248, + 0.04536, + -0.7854, + -1.13447, + 0.514973, + 0.010473, + 0.128305, + 0.111722, + 0.510575, + 0.0, + 0.37704, + 0.117825, + 1.44532, + -0.204204, + 1.46103, + 1.44532, + 1.48459, + -0.2618, + 1.47674, + 1.48459 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoHandPose2Fixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "pose_thd": 0.7, + "reset_type": "init", + "target_jnt_value": [ + 0.0, + 0.0, + 0.0, + 0.0248, + 0.04536, + -0.7854, + -1.13447, + 0.514973, + 0.010473, + 0.128305, + 0.111722, + 0.510575, + 0.0, + 0.37704, + 0.117825, + 1.44532, + -0.204204, + 1.46103, + 1.44532, + 1.48459, + -0.2618, + 1.47674, + 1.48459 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ] + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandPose2Fixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandPose2Fixed-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandPose2Fixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "obs_dim": 108, + "pose_thd": 0.7, + "qpos_dim": 23, + "qvel_dim": 23, + "reset_type": "init", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [], + "target_qpos_min": [], + "target_qpos_value": [ + 0.0, + 0.0, + 0.0, + 0.3384, + 0.25305, + 0.01569, + -0.0262045, + 0.645885, + 0.010473, + 0.128305, + 0.111722, + 0.510575, + 0.0, + 0.37704, + 0.117825, + 1.571, + -0.036652, + 1.52387, + 1.45318, + 1.40604, + -0.068068, + 1.39033, + 1.571 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoHandPose3Fixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "pose_thd": 0.7, + "reset_type": "init", + "target_jnt_value": [ + 0.0, + 0.0, + 0.0, + 0.3384, + 0.25305, + 0.01569, + -0.0262045, + 0.645885, + 0.010473, + 0.128305, + 0.111722, + 0.510575, + 0.0, + 0.37704, + 0.117825, + 1.571, + -0.036652, + 1.52387, + 1.45318, + 1.40604, + -0.068068, + 1.39033, + 1.571 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ] + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandPose3Fixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandPose3Fixed-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandPose3Fixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "obs_dim": 108, + "pose_thd": 0.7, + "qpos_dim": 23, + "qvel_dim": 23, + "reset_type": "init", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [], + "target_qpos_min": [], + "target_qpos_value": [ + 0.0, + 0.0, + 0.0, + 0.6392, + -0.147495, + -0.7854, + -1.309, + 0.637158, + 0.010473, + 0.128305, + 0.111722, + 0.510575, + 0.0, + 0.37704, + 0.117825, + 0.306345, + -0.010472, + 0.400605, + 0.133535, + 0.21994, + -0.068068, + 0.274925, + 0.01571 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoHandPose4Fixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "pose_thd": 0.7, + "reset_type": "init", + "target_jnt_value": [ + 0.0, + 0.0, + 0.0, + 0.6392, + -0.147495, + -0.7854, + -1.309, + 0.637158, + 0.010473, + 0.128305, + 0.111722, + 0.510575, + 0.0, + 0.37704, + 0.117825, + 0.306345, + -0.010472, + 0.400605, + 0.133535, + 0.21994, + -0.068068, + 0.274925, + 0.01571 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ] + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandPose4Fixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandPose4Fixed-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandPose4Fixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "obs_dim": 108, + "pose_thd": 0.7, + "qpos_dim": 23, + "qvel_dim": 23, + "reset_type": "init", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [], + "target_qpos_min": [], + "target_qpos_value": [ + 0.0, + 0.0, + 0.0, + 0.3384, + 0.25305, + 0.01569, + -0.0262045, + 0.645885, + 0.010473, + 0.128305, + 0.111722, + 0.510575, + 0.0, + 0.37704, + 0.117825, + 0.306345, + -0.010472, + 0.400605, + 0.133535, + 0.21994, + -0.068068, + 0.274925, + 0.01571 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoHandPose5Fixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "pose_thd": 0.7, + "reset_type": "init", + "target_jnt_value": [ + 0.0, + 0.0, + 0.0, + 0.3384, + 0.25305, + 0.01569, + -0.0262045, + 0.645885, + 0.010473, + 0.128305, + 0.111722, + 0.510575, + 0.0, + 0.37704, + 0.117825, + 0.306345, + -0.010472, + 0.400605, + 0.133535, + 0.21994, + -0.068068, + 0.274925, + 0.01571 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ] + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandPose5Fixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandPose5Fixed-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandPose5Fixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "obs_dim": 108, + "pose_thd": 0.7, + "qpos_dim": 23, + "qvel_dim": 23, + "reset_type": "init", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [], + "target_qpos_min": [], + "target_qpos_value": [ + 0.0, + 0.0, + 0.0, + 0.6392, + -0.147495, + -0.7854, + -1.309, + 0.637158, + 0.010473, + 0.128305, + 0.111722, + 0.510575, + 0.0, + 0.37704, + 0.117825, + 0.306345, + -0.010472, + 0.400605, + 0.133535, + 1.1861, + -0.2618, + 1.35891, + 1.48459 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoHandPose6Fixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "pose_thd": 0.7, + "reset_type": "init", + "target_jnt_value": [ + 0.0, + 0.0, + 0.0, + 0.6392, + -0.147495, + -0.7854, + -1.309, + 0.637158, + 0.010473, + 0.128305, + 0.111722, + 0.510575, + 0.0, + 0.37704, + 0.117825, + 0.306345, + -0.010472, + 0.400605, + 0.133535, + 1.1861, + -0.2618, + 1.35891, + 1.48459 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ] + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandPose6Fixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandPose6Fixed-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandPose6Fixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "obs_dim": 108, + "pose_thd": 0.7, + "qpos_dim": 23, + "qvel_dim": 23, + "reset_type": "init", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [], + "target_qpos_min": [], + "target_qpos_value": [ + 0.0, + 0.0, + 0.0, + 0.524, + 0.01569, + -0.7854, + -1.309, + 0.645885, + -0.006982, + 0.128305, + 0.111722, + 0.510575, + 0.0, + 0.37704, + 0.117825, + 1.28036, + -0.115192, + 1.52387, + 1.45318, + 0.432025, + -0.068068, + 0.18852, + 0.149245 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoHandPose7Fixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "pose_thd": 0.7, + "reset_type": "init", + "target_jnt_value": [ + 0.0, + 0.0, + 0.0, + 0.524, + 0.01569, + -0.7854, + -1.309, + 0.645885, + -0.006982, + 0.128305, + 0.111722, + 0.510575, + 0.0, + 0.37704, + 0.117825, + 1.28036, + -0.115192, + 1.52387, + 1.45318, + 0.432025, + -0.068068, + 0.18852, + 0.149245 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ] + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandPose7Fixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandPose7Fixed-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandPose7Fixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "obs_dim": 108, + "pose_thd": 0.7, + "qpos_dim": 23, + "qvel_dim": 23, + "reset_type": "init", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [], + "target_qpos_min": [], + "target_qpos_value": [ + 0.0, + 0.0, + 0.0, + 0.428, + 0.22338, + -0.7854, + -1.309, + 0.645885, + -0.006982, + 0.128305, + 0.194636, + 1.39033, + 0.0, + 1.08399, + 0.573415, + 0.667675, + -0.020944, + 0.0, + 0.06284, + 0.432025, + -0.068068, + 0.18852, + 0.149245 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoHandPose8Fixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "pose_thd": 0.7, + "reset_type": "init", + "target_jnt_value": [ + 0.0, + 0.0, + 0.0, + 0.428, + 0.22338, + -0.7854, + -1.309, + 0.645885, + -0.006982, + 0.128305, + 0.194636, + 1.39033, + 0.0, + 1.08399, + 0.573415, + 0.667675, + -0.020944, + 0.0, + 0.06284, + 0.432025, + -0.068068, + 0.18852, + 0.149245 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ] + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandPose8Fixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandPose8Fixed-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandPose8Fixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "obs_dim": 108, + "pose_thd": 0.7, + "qpos_dim": 23, + "qvel_dim": 23, + "reset_type": "init", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [], + "target_qpos_min": [], + "target_qpos_value": [ + 0.0, + 0.0, + 0.0, + 0.5624, + 0.28272, + -0.75573, + -1.309, + 1.30045, + -0.006982, + 1.45492, + 0.998897, + 0.39275, + 0.0, + 0.18852, + 0.227795, + 0.667675, + -0.020944, + 0.0, + 0.06284, + 0.432025, + -0.068068, + 0.18852, + 0.149245 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoHandPose9Fixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "pose_thd": 0.7, + "reset_type": "init", + "target_jnt_value": [ + 0.0, + 0.0, + 0.0, + 0.5624, + 0.28272, + -0.75573, + -1.309, + 1.30045, + -0.006982, + 1.45492, + 0.998897, + 0.39275, + 0.0, + 0.18852, + 0.227795, + 0.667675, + -0.020944, + 0.0, + 0.06284, + 0.432025, + -0.068068, + 0.18852, + 0.149245 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ] + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandPose9Fixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandPose9Fixed-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandPose9Fixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "obs_dim": 108, + "pose_thd": 0.7, + "qpos_dim": 23, + "qvel_dim": 23, + "reset_type": "init", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [], + "target_qpos_min": [], + "target_qpos_value": [ + 0.0, + 0.0, + 0.0, + -0.0904, + 0.0824475, + -0.681555, + -0.514888, + 0.0, + -0.013964, + -0.0458132, + 0.0, + 0.67553, + -0.020944, + 0.76979, + 0.65982, + 0.0, + 0.0, + 0.0, + 0.0, + 0.479155, + -0.099484, + 0.95831, + 0.0 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoHandPoseFixed-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "pose_thd": 0.7, + "reset_type": "init", + "target_jnt_value": [ + 0.0, + 0.0, + 0.0, + -0.0904, + 0.0824475, + -0.681555, + -0.514888, + 0.0, + -0.013964, + -0.0458132, + 0.0, + 0.67553, + -0.020944, + 0.76979, + 0.65982, + 0.0, + 0.0, + 0.0, + 0.0, + 0.479155, + -0.099484, + 0.95831, + 0.0 + ], + "target_type": "fixed", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ] + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandPoseFixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandPoseFixed-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandPoseFixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "PoseEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 10, + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "obs_dim": 108, + "pose_thd": 0.7, + "qpos_dim": 23, + "qvel_dim": 23, + "reset_type": "random", + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_max": [ + 0.6392, + 0.28272, + 0.0, + 0.0, + -0.0262045, + 0.010473, + 1.30045, + 0.0, + 1.48459, + -0.010472, + 1.571, + -0.068068, + 1.48459, + 0.998897, + 1.44532, + 1.45318, + 1.571, + 0.01569, + 1.45492, + 1.45318, + 1.52387, + 1.571, + 0.0 + ], + "target_qpos_min": [ + 0.0248, + -0.147495, + 0.0, + 0.0, + -1.309, + -0.006982, + 0.366605, + 0.0, + 0.39275, + -0.204204, + 0.306345, + -0.2618, + 0.21994, + 0.111722, + 0.117825, + 0.06284, + 0.01571, + -0.7854, + 0.128305, + 0.18852, + 0.0, + 0.18852, + 0.0 + ], + "target_qpos_value": [ + 0.332, + 0.06761250000000002, + 0.0, + 0.0, + -0.66760225, + 0.0017454999999999997, + 0.8335275000000001, + 0.0, + 0.93867, + -0.107338, + 0.9386725, + -0.164934, + 0.852265, + 0.5553095, + 0.7815725, + 0.75801, + 0.7933549999999999, + -0.384855, + 0.7916125, + 0.82085, + 0.761935, + 0.87976, + 0.0 + ], + "target_type": "generate", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ], + "weight_bodyname": "", + "weight_range": [] + }, + "entry_module": "myosuite.envs.myo.myobase.pose_v0", + "id": "myoHandPoseRandom-v0", + "kwargs": { + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "pose_thd": 0.7, + "reset_type": "random", + "target_jnt_range": { + "cmc_abduction": [ + 0.0248, + 0.6392 + ], + "cmc_flexion": [ + -0.147495, + 0.28272 + ], + "deviation": [ + 0.0, + 0.0 + ], + "flexion": [ + 0.0, + 0.0 + ], + "ip_flexion": [ + -1.309, + -0.0262045 + ], + "mcp2_abduction": [ + -0.006982, + 0.010473 + ], + "mcp2_flexion": [ + 0.366605, + 1.30045 + ], + "mcp3_abduction": [ + 0.0, + 0.0 + ], + "mcp3_flexion": [ + 0.39275, + 1.48459 + ], + "mcp4_abduction": [ + -0.204204, + -0.010472 + ], + "mcp4_flexion": [ + 0.306345, + 1.571 + ], + "mcp5_abduction": [ + -0.2618, + -0.068068 + ], + "mcp5_flexion": [ + 0.21994, + 1.48459 + ], + "md2_flexion": [ + 0.111722, + 0.998897 + ], + "md3_flexion": [ + 0.117825, + 1.44532 + ], + "md4_flexion": [ + 0.06284, + 1.45318 + ], + "md5_flexion": [ + 0.01571, + 1.571 + ], + "mp_flexion": [ + -0.7854, + 0.01569 + ], + "pm2_flexion": [ + 0.128305, + 1.45492 + ], + "pm3_flexion": [ + 0.18852, + 1.45318 + ], + "pm4_flexion": [ + 0.0, + 1.52387 + ], + "pm5_flexion": [ + 0.18852, + 1.571 + ], + "pro_sup": [ + 0.0, + 0.0 + ] + }, + "target_type": "generate", + "viz_site_targets": [ + "THtip", + "IFtip", + "MFtip", + "RFtip", + "LFtip" + ] + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandPoseRandom-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandPoseRandom-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandPoseRandom-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "ReachEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "far_th": 0.044, + "frame_skip": 10, + "hide_skin_geom_group_1": false, + "hide_terrain": false, + "joint_random_range": [], + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "obs_dim": 115, + "qpos_dim": 23, + "qvel_dim": 23, + "reward_act_reg_w": 0.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_reach_w": 1.0, + "target_pos_max": [ + -0.151, + -0.547, + 1.455, + -0.148, + -0.528, + 1.434, + -0.146, + -0.547, + 1.447, + -0.148, + -0.543, + 1.445, + -0.165, + -0.537, + 1.495 + ], + "target_pos_min": [ + -0.151, + -0.547, + 1.455, + -0.148, + -0.528, + 1.434, + -0.146, + -0.547, + 1.447, + -0.148, + -0.543, + 1.445, + -0.165, + -0.537, + 1.495 + ], + "target_pos_relative_to_tip": false, + "target_site_count": 5, + "target_site_names": [ + "IFtip", + "LFtip", + "MFtip", + "RFtip", + "THtip" + ] + }, + "entry_module": "myosuite.envs.myo.myobase.reach_v0", + "id": "myoHandReachFixed-v0", + "kwargs": { + "far_th": 0.044, + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "target_reach_range": { + "IFtip": [ + [ + -0.151, + -0.547, + 1.455 + ], + [ + -0.151, + -0.547, + 1.455 + ] + ], + "LFtip": [ + [ + -0.148, + -0.528, + 1.434 + ], + [ + -0.148, + -0.528, + 1.434 + ] + ], + "MFtip": [ + [ + -0.146, + -0.547, + 1.447 + ], + [ + -0.146, + -0.547, + 1.447 + ] + ], + "RFtip": [ + [ + -0.148, + -0.543, + 1.445 + ], + [ + -0.148, + -0.543, + 1.445 + ] + ], + "THtip": [ + [ + -0.165, + -0.537, + 1.495 + ], + [ + -0.165, + -0.537, + 1.495 + ] + ] + } + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandReachFixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandReachFixed-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandReachFixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "ReachEnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "far_th": 0.034, + "frame_skip": 10, + "hide_skin_geom_group_1": false, + "hide_terrain": false, + "joint_random_range": [], + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "obs_dim": 115, + "qpos_dim": 23, + "qvel_dim": 23, + "reward_act_reg_w": 0.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_reach_w": 1.0, + "target_pos_max": [ + -0.11099999999999999, + -0.527, + 1.465, + -0.10799999999999998, + -0.508, + 1.444, + -0.10599999999999998, + -0.527, + 1.457, + -0.10799999999999998, + -0.523, + 1.455, + -0.125, + -0.517, + 1.5350000000000001 + ], + "target_pos_min": [ + -0.191, + -0.5670000000000001, + 1.445, + -0.188, + -0.548, + 1.424, + -0.186, + -0.5670000000000001, + 1.437, + -0.188, + -0.5630000000000001, + 1.435, + -0.185, + -0.5770000000000001, + 1.455 + ], + "target_pos_relative_to_tip": false, + "target_site_count": 5, + "target_site_names": [ + "IFtip", + "LFtip", + "MFtip", + "RFtip", + "THtip" + ] + }, + "entry_module": "myosuite.envs.myo.myobase.reach_v0", + "id": "myoHandReachRandom-v0", + "kwargs": { + "far_th": 0.034, + "model_path": "envs/myo/assets/hand/myohand_pose.xml", + "normalize_act": true, + "target_reach_range": { + "IFtip": [ + [ + -0.191, + -0.5670000000000001, + 1.445 + ], + [ + -0.11099999999999999, + -0.527, + 1.465 + ] + ], + "LFtip": [ + [ + -0.188, + -0.548, + 1.424 + ], + [ + -0.10799999999999998, + -0.508, + 1.444 + ] + ], + "MFtip": [ + [ + -0.186, + -0.5670000000000001, + 1.437 + ], + [ + -0.10599999999999998, + -0.527, + 1.457 + ] + ], + "RFtip": [ + [ + -0.188, + -0.5630000000000001, + 1.435 + ], + [ + -0.10799999999999998, + -0.523, + 1.455 + ] + ], + "THtip": [ + [ + -0.185, + -0.5770000000000001, + 1.455 + ], + [ + -0.125, + -0.517, + 1.5350000000000001 + ] + ] + } + }, + "max_episode_steps": 100, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandReachRandom-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandReachRandom-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandReachRandom-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "Geometries100EnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 5, + "model_path": "envs/myo/assets/hand/myohand_sar.xml", + "normalize_act": true, + "obs_dim": 200, + "qpos_dim": 29, + "qvel_dim": 29, + "randomization_mode": "100", + "reward_act_reg_w": 5.0, + "reward_bonus_w": 10.0, + "reward_drop_w": 5.0, + "reward_pos_align_w": 1.0, + "reward_rot_align_w": 1.0 + }, + "entry_module": "myosuite.envs.myo.myobase.reorient_sar_v0", + "id": "myoHandReorient100-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "envs/myo/assets/hand/myohand_sar.xml", + "normalize_act": true + }, + "max_episode_steps": 50, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandReorient100-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandReorient100-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandReorient100-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "Geometries8EnvV0", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 5, + "model_path": "envs/myo/assets/hand/myohand_sar.xml", + "normalize_act": true, + "obs_dim": 200, + "qpos_dim": 29, + "qvel_dim": 29, + "randomization_mode": "8", + "reward_act_reg_w": 5.0, + "reward_bonus_w": 10.0, + "reward_drop_w": 5.0, + "reward_pos_align_w": 1.0, + "reward_rot_align_w": 1.0 + }, + "entry_module": "myosuite.envs.myo.myobase.reorient_sar_v0", + "id": "myoHandReorient8-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "envs/myo/assets/hand/myohand_sar.xml", + "normalize_act": true + }, + "max_episode_steps": 50, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandReorient8-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandReorient8-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandReorient8-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "InDistribution", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 5, + "model_path": "envs/myo/assets/hand/myohand_sar.xml", + "normalize_act": true, + "obs_dim": 200, + "qpos_dim": 29, + "qvel_dim": 29, + "randomization_mode": "id", + "reward_act_reg_w": 5.0, + "reward_bonus_w": 10.0, + "reward_drop_w": 5.0, + "reward_pos_align_w": 1.0, + "reward_rot_align_w": 1.0 + }, + "entry_module": "myosuite.envs.myo.myobase.reorient_sar_v0", + "id": "myoHandReorientID-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "envs/myo/assets/hand/myohand_sar.xml", + "normalize_act": true + }, + "max_episode_steps": 50, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandReorientID-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandReorientID-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandReorientID-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "OutofDistribution", + "default_config": { + "act_dim": 39, + "action_dim": 39, + "frame_skip": 5, + "model_path": "envs/myo/assets/hand/myohand_sar.xml", + "normalize_act": true, + "obs_dim": 200, + "qpos_dim": 29, + "qvel_dim": 29, + "randomization_mode": "ood", + "reward_act_reg_w": 5.0, + "reward_bonus_w": 10.0, + "reward_drop_w": 5.0, + "reward_pos_align_w": 1.0, + "reward_rot_align_w": 1.0 + }, + "entry_module": "myosuite.envs.myo.myobase.reorient_sar_v0", + "id": "myoHandReorientOOD-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "envs/myo/assets/hand/myohand_sar.xml", + "normalize_act": true + }, + "max_episode_steps": 50, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiHandReorientOOD-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoReafHandReorientOOD-v0", + "variants": { + "muscle_condition": "reafferentation" + } + }, + { + "variant_id": "myoSarcHandReorientOOD-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "TerrainEnvV0", + "default_config": { + "act_dim": 80, + "action_dim": 80, + "frame_skip": 10, + "hip_period": 100, + "max_rot": 0.8, + "min_height": 0.8, + "model_path": "simhive/myo_sim/leg/myolegs.xml", + "normalize_act": true, + "obs_dim": 403, + "qpos_dim": 35, + "qvel_dim": 34, + "reset_type": "init", + "reward_cyclic_hip_w": -10.0, + "reward_done_w": -100.0, + "reward_joint_angle_w": 5.0, + "reward_ref_rot_w": 10.0, + "reward_vel_w": 5.0, + "target_rot": [], + "target_x_vel": 0.0, + "target_y_vel": 1.2, + "terrain": "hilly", + "terrain_variant": "fixed", + "use_knee_condition": true + }, + "entry_module": "myosuite.envs.myo.myobase.walk_v0", + "id": "myoLegHillyTerrainWalk-v0", + "kwargs": { + "hip_period": 100, + "max_rot": 0.8, + "min_height": 0.8, + "model_path": "simhive/myo_sim/leg/myolegs.xml", + "normalize_act": true, + "reset_type": "init", + "target_rot": null, + "target_x_vel": 0.0, + "target_y_vel": 1.2, + "terrain": "hilly", + "variant": "fixed" + }, + "max_episode_steps": 1000, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiLegHillyTerrainWalk-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcLegHillyTerrainWalk-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "TerrainEnvV0", + "default_config": { + "act_dim": 80, + "action_dim": 80, + "frame_skip": 10, + "hip_period": 100, + "max_rot": 0.8, + "min_height": 0.8, + "model_path": "simhive/myo_sim/leg/myolegs.xml", + "normalize_act": true, + "obs_dim": 403, + "qpos_dim": 35, + "qvel_dim": 34, + "reset_type": "init", + "reward_cyclic_hip_w": -10.0, + "reward_done_w": -100.0, + "reward_joint_angle_w": 5.0, + "reward_ref_rot_w": 10.0, + "reward_vel_w": 5.0, + "target_rot": [], + "target_x_vel": 0.0, + "target_y_vel": 1.2, + "terrain": "rough", + "terrain_variant": "", + "use_knee_condition": true + }, + "entry_module": "myosuite.envs.myo.myobase.walk_v0", + "id": "myoLegRoughTerrainWalk-v0", + "kwargs": { + "hip_period": 100, + "max_rot": 0.8, + "min_height": 0.8, + "model_path": "simhive/myo_sim/leg/myolegs.xml", + "normalize_act": true, + "reset_type": "init", + "target_rot": null, + "target_x_vel": 0.0, + "target_y_vel": 1.2, + "terrain": "rough", + "variant": null + }, + "max_episode_steps": 1000, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiLegRoughTerrainWalk-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcLegRoughTerrainWalk-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "TerrainEnvV0", + "default_config": { + "act_dim": 80, + "action_dim": 80, + "frame_skip": 10, + "hip_period": 100, + "max_rot": 0.8, + "min_height": 0.8, + "model_path": "simhive/myo_sim/leg/myolegs.xml", + "normalize_act": true, + "obs_dim": 403, + "qpos_dim": 35, + "qvel_dim": 34, + "reset_type": "init", + "reward_cyclic_hip_w": -10.0, + "reward_done_w": -100.0, + "reward_joint_angle_w": 5.0, + "reward_ref_rot_w": 10.0, + "reward_vel_w": 5.0, + "target_rot": [], + "target_x_vel": 0.0, + "target_y_vel": 1.2, + "terrain": "stairs", + "terrain_variant": "fixed", + "use_knee_condition": true + }, + "entry_module": "myosuite.envs.myo.myobase.walk_v0", + "id": "myoLegStairTerrainWalk-v0", + "kwargs": { + "hip_period": 100, + "max_rot": 0.8, + "min_height": 0.8, + "model_path": "simhive/myo_sim/leg/myolegs.xml", + "normalize_act": true, + "reset_type": "init", + "target_rot": null, + "target_x_vel": 0.0, + "target_y_vel": 1.2, + "terrain": "stairs", + "variant": "fixed" + }, + "max_episode_steps": 1000, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiLegStairTerrainWalk-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcLegStairTerrainWalk-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "ReachEnvV0", + "default_config": { + "act_dim": 80, + "action_dim": 80, + "far_th": 0.44, + "frame_skip": 10, + "hide_skin_geom_group_1": true, + "hide_terrain": true, + "joint_random_range": [ + -0.2, + 0.2 + ], + "model_path": "simhive/myo_sim/leg/myolegs.xml", + "normalize_act": true, + "obs_dim": 155, + "qpos_dim": 35, + "qvel_dim": 34, + "reward_act_reg_w": 0.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_reach_w": 1.0, + "target_pos_max": [ + 0.05, + 0.05, + 0 + ], + "target_pos_min": [ + -0.05, + -0.05, + 0 + ], + "target_pos_relative_to_tip": true, + "target_site_count": 1, + "target_site_names": [ + "pelvis" + ] + }, + "entry_module": "myosuite.envs.myo.myobase.walk_v0", + "id": "myoLegStandRandom-v0", + "kwargs": { + "far_th": 0.44, + "joint_random_range": [ + -0.2, + 0.2 + ], + "model_path": "simhive/myo_sim/leg/myolegs.xml", + "normalize_act": true, + "target_reach_range": { + "pelvis": [ + [ + -0.05, + -0.05, + 0 + ], + [ + 0.05, + 0.05, + 0 + ] + ] + } + }, + "max_episode_steps": 150, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiLegStandRandom-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcLegStandRandom-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "WalkEnvV0", + "default_config": { + "act_dim": 80, + "action_dim": 80, + "frame_skip": 10, + "hip_period": 100, + "max_rot": 0.8, + "min_height": 0.8, + "model_path": "simhive/myo_sim/leg/myolegs.xml", + "normalize_act": true, + "obs_dim": 403, + "qpos_dim": 35, + "qvel_dim": 34, + "reset_type": "init", + "reward_cyclic_hip_w": -10.0, + "reward_done_w": -100.0, + "reward_joint_angle_w": 5.0, + "reward_ref_rot_w": 10.0, + "reward_vel_w": 5.0, + "target_rot": [], + "target_x_vel": 0.0, + "target_y_vel": 1.2, + "terrain": "", + "terrain_variant": "", + "use_knee_condition": false + }, + "entry_module": "myosuite.envs.myo.myobase.walk_v0", + "id": "myoLegWalk-v0", + "kwargs": { + "hip_period": 100, + "max_rot": 0.8, + "min_height": 0.8, + "model_path": "simhive/myo_sim/leg/myolegs.xml", + "normalize_act": true, + "reset_type": "init", + "target_rot": null, + "target_x_vel": 0.0, + "target_y_vel": 1.2 + }, + "max_episode_steps": 1000, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiLegWalk-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcLegWalk-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "TorsoEnvV0", + "default_config": { + "act_dim": 210, + "action_dim": 210, + "frame_skip": 5, + "model_path": "simhive/myo_sim/torso/myotorso_exosuit.xml", + "normalize_act": true, + "obs_dim": 290, + "pose_dim": 18, + "pose_thd": 0.25, + "qpos_dim": 32, + "qvel_dim": 30, + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_value": [ + 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 + ] + }, + "entry_module": "myosuite.envs.myo.myobase.torso_v0", + "id": "myoTorsoExoPoseFixed-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "simhive/myo_sim/torso/myotorso_exosuit.xml", + "normalize_act": true, + "target_jnt_range": { + "Abs_r3": [ + 0, + 0 + ], + "Abs_t1": [ + 0, + 0 + ], + "Abs_t2": [ + 0, + 0 + ], + "L1_L2_AR": [ + 0, + 0 + ], + "L1_L2_FE": [ + 0, + 0 + ], + "L1_L2_LB": [ + 0, + 0 + ], + "L2_L3_AR": [ + 0, + 0 + ], + "L2_L3_FE": [ + 0, + 0 + ], + "L2_L3_LB": [ + 0, + 0 + ], + "L3_L4_AR": [ + 0, + 0 + ], + "L3_L4_FE": [ + 0, + 0 + ], + "L3_L4_LB": [ + 0, + 0 + ], + "L4_L5_AR": [ + 0, + 0 + ], + "L4_L5_FE": [ + 0, + 0 + ], + "L4_L5_LB": [ + 0, + 0 + ], + "axial_rotation": [ + 0, + 0 + ], + "flex_extension": [ + 0, + 0 + ], + "lat_bending": [ + -0.1, + 0.1 + ] + } + }, + "max_episode_steps": 200, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiTorsoExoPoseFixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcTorsoExoPoseFixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + }, + { + "class_name": "TorsoEnvV0", + "default_config": { + "act_dim": 210, + "action_dim": 210, + "frame_skip": 5, + "model_path": "simhive/myo_sim/torso/myotorso.xml", + "normalize_act": true, + "obs_dim": 264, + "pose_dim": 18, + "pose_thd": 0.25, + "qpos_dim": 18, + "qvel_dim": 18, + "reward_act_reg_w": 1.0, + "reward_bonus_w": 4.0, + "reward_penalty_w": 50.0, + "reward_pose_w": 1.0, + "target_qpos_value": [ + 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 + ] + }, + "entry_module": "myosuite.envs.myo.myobase.torso_v0", + "id": "myoTorsoPoseFixed-v0", + "kwargs": { + "frame_skip": 5, + "model_path": "simhive/myo_sim/torso/myotorso.xml", + "normalize_act": true, + "target_jnt_range": { + "Abs_r3": [ + 0, + 0 + ], + "Abs_t1": [ + 0, + 0 + ], + "Abs_t2": [ + 0, + 0 + ], + "L1_L2_AR": [ + 0, + 0 + ], + "L1_L2_FE": [ + 0, + 0 + ], + "L1_L2_LB": [ + 0, + 0 + ], + "L2_L3_AR": [ + 0, + 0 + ], + "L2_L3_FE": [ + 0, + 0 + ], + "L2_L3_LB": [ + 0, + 0 + ], + "L3_L4_AR": [ + 0, + 0 + ], + "L3_L4_FE": [ + 0, + 0 + ], + "L3_L4_LB": [ + 0, + 0 + ], + "L4_L5_AR": [ + 0, + 0 + ], + "L4_L5_FE": [ + 0, + 0 + ], + "L4_L5_LB": [ + 0, + 0 + ], + "axial_rotation": [ + 0, + 0 + ], + "flex_extension": [ + 0.0, + 0.0 + ], + "lat_bending": [ + -0.1, + 0.1 + ] + } + }, + "max_episode_steps": 200, + "model_placeholders": [], + "registration_suite": "myobase", + "suite": "myobase", + "variant_defs": [ + { + "variant_id": "myoFatiTorsoPoseFixed-v0", + "variants": { + "muscle_condition": "fatigue" + } + }, + { + "variant_id": "myoSarcTorsoPoseFixed-v0", + "variants": { + "muscle_condition": "sarcopenia" + } + } + ] + } + ], + "direct_ids": [ + "MyoHandAirplaneFixed-v0", + "MyoHandAirplaneFly-v0", + "MyoHandAirplaneLift-v0", + "MyoHandAirplanePass-v0", + "MyoHandAirplaneRandom-v0", + "MyoHandAlarmclockFixed-v0", + "MyoHandAlarmclockLift-v0", + "MyoHandAlarmclockPass-v0", + "MyoHandAlarmclockRandom-v0", + "MyoHandAlarmclockSee-v0", + "MyoHandAppleFixed-v0", + "MyoHandAppleLift-v0", + "MyoHandApplePass-v0", + "MyoHandAppleRandom-v0", + "MyoHandBananaFixed-v0", + "MyoHandBananaPass-v0", + "MyoHandBananaRandom-v0", + "MyoHandBinocularsFixed-v0", + "MyoHandBinocularsPass-v0", + "MyoHandBinocularsRandom-v0", + "MyoHandBowlDrink2-v0", + "MyoHandBowlFixed-v0", + "MyoHandBowlPass-v0", + "MyoHandBowlRandom-v0", + "MyoHandCameraFixed-v0", + "MyoHandCameraPass-v0", + "MyoHandCameraRandom-v0", + "MyoHandCoffeemugFixed-v0", + "MyoHandCoffeemugRandom-v0", + "MyoHandCubelargeFixed-v0", + "MyoHandCubelargePass-v0", + "MyoHandCubelargeRandom-v0", + "MyoHandCubemediumFixed-v0", + "MyoHandCubemediumLInspect-v0", + "MyoHandCubemediumRandom-v0", + "MyoHandCubesmallFixed-v0", + "MyoHandCubesmallLift-v0", + "MyoHandCubesmallPass-v0", + "MyoHandCubesmallRandom-v0", + "MyoHandCupDrink-v0", + "MyoHandCupFixed-v0", + "MyoHandCupPass-v0", + "MyoHandCupPour-v0", + "MyoHandCupRandom-v0", + "MyoHandCylinderlargeFixed-v0", + "MyoHandCylinderlargeInspect-v0", + "MyoHandCylinderlargeRandom-v0", + "MyoHandCylindermediumFixed-v0", + "MyoHandCylindermediumLift-v0", + "MyoHandCylindermediumPass-v0", + "MyoHandCylindermediumRandom-v0", + "MyoHandCylindersmallFixed-v0", + "MyoHandCylindersmallInspect-v0", + "MyoHandCylindersmallPass-v0", + "MyoHandCylindersmallRandom-v0", + "MyoHandDuckFixed-v0", + "MyoHandDuckInspect-v0", + "MyoHandDuckLift-v0", + "MyoHandDuckPass-v0", + "MyoHandDuckRandom-v0", + "MyoHandElephantFixed-v0", + "MyoHandElephantLift-v0", + "MyoHandElephantPass-v0", + "MyoHandElephantRandom-v0", + "MyoHandEyeglassesFixed-v0", + "MyoHandEyeglassesPass-v0", + "MyoHandEyeglassesRandom-v0", + "MyoHandFlashlight1On-v0", + "MyoHandFlashlight2On-v0", + "MyoHandFlashlightFixed-v0", + "MyoHandFlashlightLift-v0", + "MyoHandFlashlightPass-v0", + "MyoHandFlashlightRandom-v0", + "MyoHandFluteFixed-v0", + "MyoHandFlutePass-v0", + "MyoHandFluteRandom-v0", + "MyoHandGamecontrollerFixed-v0", + "MyoHandGamecontrollerPass-v0", + "MyoHandGamecontrollerRandom-v0", + "MyoHandHammerFixed-v0", + "MyoHandHammerPass-v0", + "MyoHandHammerRandom-v0", + "MyoHandHammerUse-v0", + "MyoHandHandFixed-v0", + "MyoHandHandInspect-v0", + "MyoHandHandRandom-v0", + "MyoHandHeadphonesFixed-v0", + "MyoHandHeadphonesPass-v0", + "MyoHandHeadphonesRandom-v0", + "MyoHandKnifeChop-v0", + "MyoHandKnifeFixed-v0", + "MyoHandKnifeRandom-v0", + "MyoHandLightbulbFixed-v0", + "MyoHandLightbulbPass-v0", + "MyoHandLightbulbRandom-v0", + "MyoHandMouseFixed-v0", + "MyoHandMouseLift-v0", + "MyoHandMousePass-v0", + "MyoHandMouseRandom-v0", + "MyoHandMouseUse-v0", + "MyoHandMugDrink3-v0", + "MyoHandMugFixed-v0", + "MyoHandMugLift-v0", + "MyoHandMugPass-v0", + "MyoHandMugRandom-v0", + "MyoHandPhoneFixed-v0", + "MyoHandPhoneLift-v0", + "MyoHandPhoneRandom-v0", + "MyoHandPiggybankFixed-v0", + "MyoHandPiggybankPass-v0", + "MyoHandPiggybankRandom-v0", + "MyoHandPiggybankUse-v0", + "MyoHandPyramidlargeFixed-v0", + "MyoHandPyramidlargePass-v0", + "MyoHandPyramidlargeRandom-v0", + "MyoHandPyramidmediumFixed-v0", + "MyoHandPyramidmediumPass-v0", + "MyoHandPyramidmediumRandom-v0", + "MyoHandPyramidsmallFixed-v0", + "MyoHandPyramidsmallInspect-v0", + "MyoHandPyramidsmallRandom-v0", + "MyoHandScissorsFixed-v0", + "MyoHandScissorsRandom-v0", + "MyoHandScissorsUse-v0", + "MyoHandSpherelargeFixed-v0", + "MyoHandSpherelargePass-v0", + "MyoHandSpherelargeRandom-v0", + "MyoHandSpheremediumFixed-v0", + "MyoHandSpheremediumInspect-v0", + "MyoHandSpheremediumLift-v0", + "MyoHandSpheremediumRandom-v0", + "MyoHandSpheresmallFixed-v0", + "MyoHandSpheresmallInspect-v0", + "MyoHandSpheresmallLift-v0", + "MyoHandSpheresmallPass-v0", + "MyoHandSpheresmallRandom-v0", + "MyoHandStampFixed-v0", + "MyoHandStampLift-v0", + "MyoHandStampRandom-v0", + "MyoHandStampStamp-v0", + "MyoHandStanfordbunnyFixed-v0", + "MyoHandStanfordbunnyInspect-v0", + "MyoHandStanfordbunnyPass-v0", + "MyoHandStanfordbunnyRandom-v0", + "MyoHandStaplerFixed-v0", + "MyoHandStaplerLift-v0", + "MyoHandStaplerRandom-v0", + "MyoHandStaplerStaple1-v0", + "MyoHandStaplerStaple2-v0", + "MyoHandTeapotFixed-v0", + "MyoHandTeapotPour2-v0", + "MyoHandTeapotRandom-v0", + "MyoHandToothbrushBrush1-v0", + "MyoHandToothbrushFixed-v0", + "MyoHandToothbrushLift-v0", + "MyoHandToothbrushRandom-v0", + "MyoHandToothpasteFixed-v0", + "MyoHandToothpasteLift-v0", + "MyoHandToothpasteRandom-v0", + "MyoHandToothpasteSqueeze1-v0", + "MyoHandToruslargeFixed-v0", + "MyoHandToruslargeInspect-v0", + "MyoHandToruslargeLift-v0", + "MyoHandToruslargeRandom-v0", + "MyoHandTorusmediumFixed-v0", + "MyoHandTorusmediumLift-v0", + "MyoHandTorusmediumPass-v0", + "MyoHandTorusmediumRandom-v0", + "MyoHandTorussmallFixed-v0", + "MyoHandTorussmallLift-v0", + "MyoHandTorussmallPass-v0", + "MyoHandTorussmallRandom-v0", + "MyoHandTrainFixed-v0", + "MyoHandTrainPlay-v0", + "MyoHandTrainRandom-v0", + "MyoHandWatchFixed-v0", + "MyoHandWatchLift-v0", + "MyoHandWatchPass-v0", + "MyoHandWatchRandom-v0", + "MyoHandWaterbottleFixed-v0", + "MyoHandWaterbottleLift-v0", + "MyoHandWaterbottlePass-v0", + "MyoHandWaterbottleRandom-v0", + "MyoHandWaterbottleShake-v0", + "MyoHandWineglassDrink2-v0", + "MyoHandWineglassFixed-v0", + "MyoHandWineglassLift-v0", + "MyoHandWineglassPass-v0", + "MyoHandWineglassRandom-v0", + "MyoHandWineglassToast1-v0", + "motorFingerPoseFixed-v0", + "motorFingerPoseRandom-v0", + "motorFingerReachFixed-v0", + "motorFingerReachRandom-v0", + "myoArmReachFixed-v0", + "myoArmReachRandom-v0", + "myoChallengeBaodingP1-v1", + "myoChallengeBaodingP2-v1", + "myoChallengeBimanual-v0", + "myoChallengeChaseTagP1-v0", + "myoChallengeChaseTagP2-v0", + "myoChallengeChaseTagP2eval-v0", + "myoChallengeDieReorientDemo-v0", + "myoChallengeDieReorientP1-v0", + "myoChallengeDieReorientP2-v0", + "myoChallengeOslRunFixed-v0", + "myoChallengeOslRunRandom-v0", + "myoChallengeRelocateP1-v0", + "myoChallengeRelocateP2-v0", + "myoChallengeRelocateP2eval-v0", + "myoChallengeSoccerP1-v0", + "myoChallengeSoccerP2-v0", + "myoChallengeTableTennisP0-v0", + "myoChallengeTableTennisP1-v0", + "myoChallengeTableTennisP2-v0", + "myoElbowPose1D6MExoFixed-v0", + "myoElbowPose1D6MExoRandom-v0", + "myoElbowPose1D6MFixed-v0", + "myoElbowPose1D6MRandom-v0", + "myoFingerPoseFixed-v0", + "myoFingerPoseRandom-v0", + "myoFingerReachFixed-v0", + "myoFingerReachRandom-v0", + "myoHandKeyTurnFixed-v0", + "myoHandKeyTurnRandom-v0", + "myoHandObjHoldFixed-v0", + "myoHandObjHoldRandom-v0", + "myoHandPenTwirlFixed-v0", + "myoHandPenTwirlRandom-v0", + "myoHandPose0Fixed-v0", + "myoHandPose1Fixed-v0", + "myoHandPose2Fixed-v0", + "myoHandPose3Fixed-v0", + "myoHandPose4Fixed-v0", + "myoHandPose5Fixed-v0", + "myoHandPose6Fixed-v0", + "myoHandPose7Fixed-v0", + "myoHandPose8Fixed-v0", + "myoHandPose9Fixed-v0", + "myoHandPoseFixed-v0", + "myoHandPoseRandom-v0", + "myoHandReachFixed-v0", + "myoHandReachRandom-v0", + "myoHandReorient100-v0", + "myoHandReorient8-v0", + "myoHandReorientID-v0", + "myoHandReorientOOD-v0", + "myoLegHillyTerrainWalk-v0", + "myoLegRoughTerrainWalk-v0", + "myoLegStairTerrainWalk-v0", + "myoLegStandRandom-v0", + "myoLegWalk-v0", + "myoTorsoExoPoseFixed-v0", + "myoTorsoPoseFixed-v0" + ], + "expanded_ids": [ + "MyoHandAirplaneFixed-v0", + "MyoHandAirplaneFly-v0", + "MyoHandAirplaneLift-v0", + "MyoHandAirplanePass-v0", + "MyoHandAirplaneRandom-v0", + "MyoHandAlarmclockFixed-v0", + "MyoHandAlarmclockLift-v0", + "MyoHandAlarmclockPass-v0", + "MyoHandAlarmclockRandom-v0", + "MyoHandAlarmclockSee-v0", + "MyoHandAppleFixed-v0", + "MyoHandAppleLift-v0", + "MyoHandApplePass-v0", + "MyoHandAppleRandom-v0", + "MyoHandBananaFixed-v0", + "MyoHandBananaPass-v0", + "MyoHandBananaRandom-v0", + "MyoHandBinocularsFixed-v0", + "MyoHandBinocularsPass-v0", + "MyoHandBinocularsRandom-v0", + "MyoHandBowlDrink2-v0", + "MyoHandBowlFixed-v0", + "MyoHandBowlPass-v0", + "MyoHandBowlRandom-v0", + "MyoHandCameraFixed-v0", + "MyoHandCameraPass-v0", + "MyoHandCameraRandom-v0", + "MyoHandCoffeemugFixed-v0", + "MyoHandCoffeemugRandom-v0", + "MyoHandCubelargeFixed-v0", + "MyoHandCubelargePass-v0", + "MyoHandCubelargeRandom-v0", + "MyoHandCubemediumFixed-v0", + "MyoHandCubemediumLInspect-v0", + "MyoHandCubemediumRandom-v0", + "MyoHandCubesmallFixed-v0", + "MyoHandCubesmallLift-v0", + "MyoHandCubesmallPass-v0", + "MyoHandCubesmallRandom-v0", + "MyoHandCupDrink-v0", + "MyoHandCupFixed-v0", + "MyoHandCupPass-v0", + "MyoHandCupPour-v0", + "MyoHandCupRandom-v0", + "MyoHandCylinderlargeFixed-v0", + "MyoHandCylinderlargeInspect-v0", + "MyoHandCylinderlargeRandom-v0", + "MyoHandCylindermediumFixed-v0", + "MyoHandCylindermediumLift-v0", + "MyoHandCylindermediumPass-v0", + "MyoHandCylindermediumRandom-v0", + "MyoHandCylindersmallFixed-v0", + "MyoHandCylindersmallInspect-v0", + "MyoHandCylindersmallPass-v0", + "MyoHandCylindersmallRandom-v0", + "MyoHandDuckFixed-v0", + "MyoHandDuckInspect-v0", + "MyoHandDuckLift-v0", + "MyoHandDuckPass-v0", + "MyoHandDuckRandom-v0", + "MyoHandElephantFixed-v0", + "MyoHandElephantLift-v0", + "MyoHandElephantPass-v0", + "MyoHandElephantRandom-v0", + "MyoHandEyeglassesFixed-v0", + "MyoHandEyeglassesPass-v0", + "MyoHandEyeglassesRandom-v0", + "MyoHandFlashlight1On-v0", + "MyoHandFlashlight2On-v0", + "MyoHandFlashlightFixed-v0", + "MyoHandFlashlightLift-v0", + "MyoHandFlashlightPass-v0", + "MyoHandFlashlightRandom-v0", + "MyoHandFluteFixed-v0", + "MyoHandFlutePass-v0", + "MyoHandFluteRandom-v0", + "MyoHandGamecontrollerFixed-v0", + "MyoHandGamecontrollerPass-v0", + "MyoHandGamecontrollerRandom-v0", + "MyoHandHammerFixed-v0", + "MyoHandHammerPass-v0", + "MyoHandHammerRandom-v0", + "MyoHandHammerUse-v0", + "MyoHandHandFixed-v0", + "MyoHandHandInspect-v0", + "MyoHandHandRandom-v0", + "MyoHandHeadphonesFixed-v0", + "MyoHandHeadphonesPass-v0", + "MyoHandHeadphonesRandom-v0", + "MyoHandKnifeChop-v0", + "MyoHandKnifeFixed-v0", + "MyoHandKnifeRandom-v0", + "MyoHandLightbulbFixed-v0", + "MyoHandLightbulbPass-v0", + "MyoHandLightbulbRandom-v0", + "MyoHandMouseFixed-v0", + "MyoHandMouseLift-v0", + "MyoHandMousePass-v0", + "MyoHandMouseRandom-v0", + "MyoHandMouseUse-v0", + "MyoHandMugDrink3-v0", + "MyoHandMugFixed-v0", + "MyoHandMugLift-v0", + "MyoHandMugPass-v0", + "MyoHandMugRandom-v0", + "MyoHandPhoneFixed-v0", + "MyoHandPhoneLift-v0", + "MyoHandPhoneRandom-v0", + "MyoHandPiggybankFixed-v0", + "MyoHandPiggybankPass-v0", + "MyoHandPiggybankRandom-v0", + "MyoHandPiggybankUse-v0", + "MyoHandPyramidlargeFixed-v0", + "MyoHandPyramidlargePass-v0", + "MyoHandPyramidlargeRandom-v0", + "MyoHandPyramidmediumFixed-v0", + "MyoHandPyramidmediumPass-v0", + "MyoHandPyramidmediumRandom-v0", + "MyoHandPyramidsmallFixed-v0", + "MyoHandPyramidsmallInspect-v0", + "MyoHandPyramidsmallRandom-v0", + "MyoHandScissorsFixed-v0", + "MyoHandScissorsRandom-v0", + "MyoHandScissorsUse-v0", + "MyoHandSpherelargeFixed-v0", + "MyoHandSpherelargePass-v0", + "MyoHandSpherelargeRandom-v0", + "MyoHandSpheremediumFixed-v0", + "MyoHandSpheremediumInspect-v0", + "MyoHandSpheremediumLift-v0", + "MyoHandSpheremediumRandom-v0", + "MyoHandSpheresmallFixed-v0", + "MyoHandSpheresmallInspect-v0", + "MyoHandSpheresmallLift-v0", + "MyoHandSpheresmallPass-v0", + "MyoHandSpheresmallRandom-v0", + "MyoHandStampFixed-v0", + "MyoHandStampLift-v0", + "MyoHandStampRandom-v0", + "MyoHandStampStamp-v0", + "MyoHandStanfordbunnyFixed-v0", + "MyoHandStanfordbunnyInspect-v0", + "MyoHandStanfordbunnyPass-v0", + "MyoHandStanfordbunnyRandom-v0", + "MyoHandStaplerFixed-v0", + "MyoHandStaplerLift-v0", + "MyoHandStaplerRandom-v0", + "MyoHandStaplerStaple1-v0", + "MyoHandStaplerStaple2-v0", + "MyoHandTeapotFixed-v0", + "MyoHandTeapotPour2-v0", + "MyoHandTeapotRandom-v0", + "MyoHandToothbrushBrush1-v0", + "MyoHandToothbrushFixed-v0", + "MyoHandToothbrushLift-v0", + "MyoHandToothbrushRandom-v0", + "MyoHandToothpasteFixed-v0", + "MyoHandToothpasteLift-v0", + "MyoHandToothpasteRandom-v0", + "MyoHandToothpasteSqueeze1-v0", + "MyoHandToruslargeFixed-v0", + "MyoHandToruslargeInspect-v0", + "MyoHandToruslargeLift-v0", + "MyoHandToruslargeRandom-v0", + "MyoHandTorusmediumFixed-v0", + "MyoHandTorusmediumLift-v0", + "MyoHandTorusmediumPass-v0", + "MyoHandTorusmediumRandom-v0", + "MyoHandTorussmallFixed-v0", + "MyoHandTorussmallLift-v0", + "MyoHandTorussmallPass-v0", + "MyoHandTorussmallRandom-v0", + "MyoHandTrainFixed-v0", + "MyoHandTrainPlay-v0", + "MyoHandTrainRandom-v0", + "MyoHandWatchFixed-v0", + "MyoHandWatchLift-v0", + "MyoHandWatchPass-v0", + "MyoHandWatchRandom-v0", + "MyoHandWaterbottleFixed-v0", + "MyoHandWaterbottleLift-v0", + "MyoHandWaterbottlePass-v0", + "MyoHandWaterbottleRandom-v0", + "MyoHandWaterbottleShake-v0", + "MyoHandWineglassDrink2-v0", + "MyoHandWineglassFixed-v0", + "MyoHandWineglassLift-v0", + "MyoHandWineglassPass-v0", + "MyoHandWineglassRandom-v0", + "MyoHandWineglassToast1-v0", + "motorFingerPoseFixed-v0", + "motorFingerPoseRandom-v0", + "motorFingerReachFixed-v0", + "motorFingerReachRandom-v0", + "myoArmReachFixed-v0", + "myoArmReachRandom-v0", + "myoChallengeBaodingP1-v1", + "myoChallengeBaodingP2-v1", + "myoChallengeBimanual-v0", + "myoChallengeChaseTagP1-v0", + "myoChallengeChaseTagP2-v0", + "myoChallengeChaseTagP2eval-v0", + "myoChallengeDieReorientDemo-v0", + "myoChallengeDieReorientP1-v0", + "myoChallengeDieReorientP2-v0", + "myoChallengeOslRunFixed-v0", + "myoChallengeOslRunRandom-v0", + "myoChallengeRelocateP1-v0", + "myoChallengeRelocateP2-v0", + "myoChallengeRelocateP2eval-v0", + "myoChallengeSoccerP1-v0", + "myoChallengeSoccerP2-v0", + "myoChallengeTableTennisP0-v0", + "myoChallengeTableTennisP1-v0", + "myoChallengeTableTennisP2-v0", + "myoElbowPose1D6MExoFixed-v0", + "myoElbowPose1D6MExoRandom-v0", + "myoElbowPose1D6MFixed-v0", + "myoElbowPose1D6MRandom-v0", + "myoFatiArmReachFixed-v0", + "myoFatiArmReachRandom-v0", + "myoFatiChallengeBaodingP1-v1", + "myoFatiChallengeBaodingP2-v1", + "myoFatiChallengeBimanual-v0", + "myoFatiChallengeChaseTagP1-v0", + "myoFatiChallengeChaseTagP2-v0", + "myoFatiChallengeChaseTagP2eval-v0", + "myoFatiChallengeDieReorientDemo-v0", + "myoFatiChallengeDieReorientP1-v0", + "myoFatiChallengeDieReorientP2-v0", + "myoFatiChallengeOslRunFixed-v0", + "myoFatiChallengeOslRunRandom-v0", + "myoFatiChallengeRelocateP1-v0", + "myoFatiChallengeRelocateP2-v0", + "myoFatiChallengeRelocateP2eval-v0", + "myoFatiChallengeSoccerP1-v0", + "myoFatiChallengeSoccerP2-v0", + "myoFatiChallengeTableTennisP0-v0", + "myoFatiChallengeTableTennisP1-v0", + "myoFatiChallengeTableTennisP2-v0", + "myoFatiElbowPose1D6MExoFixed-v0", + "myoFatiElbowPose1D6MExoRandom-v0", + "myoFatiElbowPose1D6MFixed-v0", + "myoFatiElbowPose1D6MRandom-v0", + "myoFatiFingerPoseFixed-v0", + "myoFatiFingerPoseRandom-v0", + "myoFatiFingerReachFixed-v0", + "myoFatiFingerReachRandom-v0", + "myoFatiHandKeyTurnFixed-v0", + "myoFatiHandKeyTurnRandom-v0", + "myoFatiHandObjHoldFixed-v0", + "myoFatiHandObjHoldRandom-v0", + "myoFatiHandPenTwirlFixed-v0", + "myoFatiHandPenTwirlRandom-v0", + "myoFatiHandPose0Fixed-v0", + "myoFatiHandPose1Fixed-v0", + "myoFatiHandPose2Fixed-v0", + "myoFatiHandPose3Fixed-v0", + "myoFatiHandPose4Fixed-v0", + "myoFatiHandPose5Fixed-v0", + "myoFatiHandPose6Fixed-v0", + "myoFatiHandPose7Fixed-v0", + "myoFatiHandPose8Fixed-v0", + "myoFatiHandPose9Fixed-v0", + "myoFatiHandPoseFixed-v0", + "myoFatiHandPoseRandom-v0", + "myoFatiHandReachFixed-v0", + "myoFatiHandReachRandom-v0", + "myoFatiHandReorient100-v0", + "myoFatiHandReorient8-v0", + "myoFatiHandReorientID-v0", + "myoFatiHandReorientOOD-v0", + "myoFatiLegHillyTerrainWalk-v0", + "myoFatiLegRoughTerrainWalk-v0", + "myoFatiLegStairTerrainWalk-v0", + "myoFatiLegStandRandom-v0", + "myoFatiLegWalk-v0", + "myoFatiTorsoExoPoseFixed-v0", + "myoFatiTorsoPoseFixed-v0", + "myoFingerPoseFixed-v0", + "myoFingerPoseRandom-v0", + "myoFingerReachFixed-v0", + "myoFingerReachRandom-v0", + "myoHandKeyTurnFixed-v0", + "myoHandKeyTurnRandom-v0", + "myoHandObjHoldFixed-v0", + "myoHandObjHoldRandom-v0", + "myoHandPenTwirlFixed-v0", + "myoHandPenTwirlRandom-v0", + "myoHandPose0Fixed-v0", + "myoHandPose1Fixed-v0", + "myoHandPose2Fixed-v0", + "myoHandPose3Fixed-v0", + "myoHandPose4Fixed-v0", + "myoHandPose5Fixed-v0", + "myoHandPose6Fixed-v0", + "myoHandPose7Fixed-v0", + "myoHandPose8Fixed-v0", + "myoHandPose9Fixed-v0", + "myoHandPoseFixed-v0", + "myoHandPoseRandom-v0", + "myoHandReachFixed-v0", + "myoHandReachRandom-v0", + "myoHandReorient100-v0", + "myoHandReorient8-v0", + "myoHandReorientID-v0", + "myoHandReorientOOD-v0", + "myoLegHillyTerrainWalk-v0", + "myoLegRoughTerrainWalk-v0", + "myoLegStairTerrainWalk-v0", + "myoLegStandRandom-v0", + "myoLegWalk-v0", + "myoReafHandKeyTurnFixed-v0", + "myoReafHandKeyTurnRandom-v0", + "myoReafHandObjHoldFixed-v0", + "myoReafHandObjHoldRandom-v0", + "myoReafHandPenTwirlFixed-v0", + "myoReafHandPenTwirlRandom-v0", + "myoReafHandPose0Fixed-v0", + "myoReafHandPose1Fixed-v0", + "myoReafHandPose2Fixed-v0", + "myoReafHandPose3Fixed-v0", + "myoReafHandPose4Fixed-v0", + "myoReafHandPose5Fixed-v0", + "myoReafHandPose6Fixed-v0", + "myoReafHandPose7Fixed-v0", + "myoReafHandPose8Fixed-v0", + "myoReafHandPose9Fixed-v0", + "myoReafHandPoseFixed-v0", + "myoReafHandPoseRandom-v0", + "myoReafHandReachFixed-v0", + "myoReafHandReachRandom-v0", + "myoReafHandReorient100-v0", + "myoReafHandReorient8-v0", + "myoReafHandReorientID-v0", + "myoReafHandReorientOOD-v0", + "myoSarcArmReachFixed-v0", + "myoSarcArmReachRandom-v0", + "myoSarcChallengeBaodingP1-v1", + "myoSarcChallengeBaodingP2-v1", + "myoSarcChallengeBimanual-v0", + "myoSarcChallengeChaseTagP1-v0", + "myoSarcChallengeChaseTagP2-v0", + "myoSarcChallengeChaseTagP2eval-v0", + "myoSarcChallengeDieReorientDemo-v0", + "myoSarcChallengeDieReorientP1-v0", + "myoSarcChallengeDieReorientP2-v0", + "myoSarcChallengeOslRunFixed-v0", + "myoSarcChallengeOslRunRandom-v0", + "myoSarcChallengeRelocateP1-v0", + "myoSarcChallengeRelocateP2-v0", + "myoSarcChallengeRelocateP2eval-v0", + "myoSarcChallengeSoccerP1-v0", + "myoSarcChallengeSoccerP2-v0", + "myoSarcChallengeTableTennisP0-v0", + "myoSarcChallengeTableTennisP1-v0", + "myoSarcChallengeTableTennisP2-v0", + "myoSarcElbowPose1D6MExoFixed-v0", + "myoSarcElbowPose1D6MExoRandom-v0", + "myoSarcElbowPose1D6MFixed-v0", + "myoSarcElbowPose1D6MRandom-v0", + "myoSarcFingerPoseFixed-v0", + "myoSarcFingerPoseRandom-v0", + "myoSarcFingerReachFixed-v0", + "myoSarcFingerReachRandom-v0", + "myoSarcHandKeyTurnFixed-v0", + "myoSarcHandKeyTurnRandom-v0", + "myoSarcHandObjHoldFixed-v0", + "myoSarcHandObjHoldRandom-v0", + "myoSarcHandPenTwirlFixed-v0", + "myoSarcHandPenTwirlRandom-v0", + "myoSarcHandPose0Fixed-v0", + "myoSarcHandPose1Fixed-v0", + "myoSarcHandPose2Fixed-v0", + "myoSarcHandPose3Fixed-v0", + "myoSarcHandPose4Fixed-v0", + "myoSarcHandPose5Fixed-v0", + "myoSarcHandPose6Fixed-v0", + "myoSarcHandPose7Fixed-v0", + "myoSarcHandPose8Fixed-v0", + "myoSarcHandPose9Fixed-v0", + "myoSarcHandPoseFixed-v0", + "myoSarcHandPoseRandom-v0", + "myoSarcHandReachFixed-v0", + "myoSarcHandReachRandom-v0", + "myoSarcHandReorient100-v0", + "myoSarcHandReorient8-v0", + "myoSarcHandReorientID-v0", + "myoSarcHandReorientOOD-v0", + "myoSarcLegHillyTerrainWalk-v0", + "myoSarcLegRoughTerrainWalk-v0", + "myoSarcLegStairTerrainWalk-v0", + "myoSarcLegStandRandom-v0", + "myoSarcLegWalk-v0", + "myoSarcTorsoExoPoseFixed-v0", + "myoSarcTorsoPoseFixed-v0", + "myoTorsoExoPoseFixed-v0", + "myoTorsoPoseFixed-v0" + ], + "notes": { + "myoedits_duplicate_ids": [ + "myoArmReachFixed-v0", + "myoArmReachRandom-v0" + ], + "variant_rules": [ + "Every myo* task also creates Sarc and Fati variants.", + "Every myoHand* task also creates a Reaf variant." + ] + }, + "pins": { + "myosuite": { + "commit": "05cb84678373f91271004f99602ebbf01e57d1a1", + "version": "v2.11.6" + } + }, + "suites": { + "myobase_direct_ids": [ + "motorFingerPoseFixed-v0", + "motorFingerPoseRandom-v0", + "motorFingerReachFixed-v0", + "motorFingerReachRandom-v0", + "myoArmReachFixed-v0", + "myoArmReachRandom-v0", + "myoElbowPose1D6MExoFixed-v0", + "myoElbowPose1D6MExoRandom-v0", + "myoElbowPose1D6MFixed-v0", + "myoElbowPose1D6MRandom-v0", + "myoFingerPoseFixed-v0", + "myoFingerPoseRandom-v0", + "myoFingerReachFixed-v0", + "myoFingerReachRandom-v0", + "myoHandKeyTurnFixed-v0", + "myoHandKeyTurnRandom-v0", + "myoHandObjHoldFixed-v0", + "myoHandObjHoldRandom-v0", + "myoHandPenTwirlFixed-v0", + "myoHandPenTwirlRandom-v0", + "myoHandPose0Fixed-v0", + "myoHandPose1Fixed-v0", + "myoHandPose2Fixed-v0", + "myoHandPose3Fixed-v0", + "myoHandPose4Fixed-v0", + "myoHandPose5Fixed-v0", + "myoHandPose6Fixed-v0", + "myoHandPose7Fixed-v0", + "myoHandPose8Fixed-v0", + "myoHandPose9Fixed-v0", + "myoHandPoseFixed-v0", + "myoHandPoseRandom-v0", + "myoHandReachFixed-v0", + "myoHandReachRandom-v0", + "myoHandReorient100-v0", + "myoHandReorient8-v0", + "myoHandReorientID-v0", + "myoHandReorientOOD-v0", + "myoLegHillyTerrainWalk-v0", + "myoLegRoughTerrainWalk-v0", + "myoLegStairTerrainWalk-v0", + "myoLegStandRandom-v0", + "myoLegWalk-v0", + "myoTorsoExoPoseFixed-v0", + "myoTorsoPoseFixed-v0" + ], + "myochallenge_direct_ids": [ + "myoChallengeBaodingP1-v1", + "myoChallengeBaodingP2-v1", + "myoChallengeBimanual-v0", + "myoChallengeChaseTagP1-v0", + "myoChallengeChaseTagP2-v0", + "myoChallengeChaseTagP2eval-v0", + "myoChallengeDieReorientDemo-v0", + "myoChallengeDieReorientP1-v0", + "myoChallengeDieReorientP2-v0", + "myoChallengeOslRunFixed-v0", + "myoChallengeOslRunRandom-v0", + "myoChallengeRelocateP1-v0", + "myoChallengeRelocateP2-v0", + "myoChallengeRelocateP2eval-v0", + "myoChallengeSoccerP1-v0", + "myoChallengeSoccerP2-v0", + "myoChallengeTableTennisP0-v0", + "myoChallengeTableTennisP1-v0", + "myoChallengeTableTennisP2-v0" + ], + "myodm_fixed_ids": [ + "MyoHandAirplaneFixed-v0", + "MyoHandAlarmclockFixed-v0", + "MyoHandAppleFixed-v0", + "MyoHandBananaFixed-v0", + "MyoHandBinocularsFixed-v0", + "MyoHandBowlFixed-v0", + "MyoHandCameraFixed-v0", + "MyoHandCoffeemugFixed-v0", + "MyoHandCubelargeFixed-v0", + "MyoHandCubemediumFixed-v0", + "MyoHandCubesmallFixed-v0", + "MyoHandCupFixed-v0", + "MyoHandCylinderlargeFixed-v0", + "MyoHandCylindermediumFixed-v0", + "MyoHandCylindersmallFixed-v0", + "MyoHandDuckFixed-v0", + "MyoHandElephantFixed-v0", + "MyoHandEyeglassesFixed-v0", + "MyoHandFlashlightFixed-v0", + "MyoHandFluteFixed-v0", + "MyoHandGamecontrollerFixed-v0", + "MyoHandHammerFixed-v0", + "MyoHandHandFixed-v0", + "MyoHandHeadphonesFixed-v0", + "MyoHandKnifeFixed-v0", + "MyoHandLightbulbFixed-v0", + "MyoHandMouseFixed-v0", + "MyoHandMugFixed-v0", + "MyoHandPhoneFixed-v0", + "MyoHandPiggybankFixed-v0", + "MyoHandPyramidlargeFixed-v0", + "MyoHandPyramidmediumFixed-v0", + "MyoHandPyramidsmallFixed-v0", + "MyoHandScissorsFixed-v0", + "MyoHandSpherelargeFixed-v0", + "MyoHandSpheremediumFixed-v0", + "MyoHandSpheresmallFixed-v0", + "MyoHandStampFixed-v0", + "MyoHandStanfordbunnyFixed-v0", + "MyoHandStaplerFixed-v0", + "MyoHandTeapotFixed-v0", + "MyoHandToothbrushFixed-v0", + "MyoHandToothpasteFixed-v0", + "MyoHandToruslargeFixed-v0", + "MyoHandTorusmediumFixed-v0", + "MyoHandTorussmallFixed-v0", + "MyoHandTrainFixed-v0", + "MyoHandWatchFixed-v0", + "MyoHandWaterbottleFixed-v0", + "MyoHandWineglassFixed-v0" + ], + "myodm_object_names": [ + "airplane", + "alarmclock", + "apple", + "banana", + "binoculars", + "bowl", + "camera", + "coffeemug", + "cubelarge", + "cubemedium", + "cubesmall", + "cup", + "cylinderlarge", + "cylindermedium", + "cylindersmall", + "duck", + "elephant", + "eyeglasses", + "flashlight", + "flute", + "gamecontroller", + "hammer", + "hand", + "headphones", + "knife", + "lightbulb", + "mouse", + "mug", + "phone", + "piggybank", + "pyramidlarge", + "pyramidmedium", + "pyramidsmall", + "scissors", + "spherelarge", + "spheremedium", + "spheresmall", + "stamp", + "stanfordbunny", + "stapler", + "teapot", + "toothbrush", + "toothpaste", + "toruslarge", + "torusmedium", + "torussmall", + "train", + "watch", + "waterbottle", + "wineglass" + ], + "myodm_random_ids": [ + "MyoHandAirplaneRandom-v0", + "MyoHandAlarmclockRandom-v0", + "MyoHandAppleRandom-v0", + "MyoHandBananaRandom-v0", + "MyoHandBinocularsRandom-v0", + "MyoHandBowlRandom-v0", + "MyoHandCameraRandom-v0", + "MyoHandCoffeemugRandom-v0", + "MyoHandCubelargeRandom-v0", + "MyoHandCubemediumRandom-v0", + "MyoHandCubesmallRandom-v0", + "MyoHandCupRandom-v0", + "MyoHandCylinderlargeRandom-v0", + "MyoHandCylindermediumRandom-v0", + "MyoHandCylindersmallRandom-v0", + "MyoHandDuckRandom-v0", + "MyoHandElephantRandom-v0", + "MyoHandEyeglassesRandom-v0", + "MyoHandFlashlightRandom-v0", + "MyoHandFluteRandom-v0", + "MyoHandGamecontrollerRandom-v0", + "MyoHandHammerRandom-v0", + "MyoHandHandRandom-v0", + "MyoHandHeadphonesRandom-v0", + "MyoHandKnifeRandom-v0", + "MyoHandLightbulbRandom-v0", + "MyoHandMouseRandom-v0", + "MyoHandMugRandom-v0", + "MyoHandPhoneRandom-v0", + "MyoHandPiggybankRandom-v0", + "MyoHandPyramidlargeRandom-v0", + "MyoHandPyramidmediumRandom-v0", + "MyoHandPyramidsmallRandom-v0", + "MyoHandScissorsRandom-v0", + "MyoHandSpherelargeRandom-v0", + "MyoHandSpheremediumRandom-v0", + "MyoHandSpheresmallRandom-v0", + "MyoHandStampRandom-v0", + "MyoHandStanfordbunnyRandom-v0", + "MyoHandStaplerRandom-v0", + "MyoHandTeapotRandom-v0", + "MyoHandToothbrushRandom-v0", + "MyoHandToothpasteRandom-v0", + "MyoHandToruslargeRandom-v0", + "MyoHandTorusmediumRandom-v0", + "MyoHandTorussmallRandom-v0", + "MyoHandTrainRandom-v0", + "MyoHandWatchRandom-v0", + "MyoHandWaterbottleRandom-v0", + "MyoHandWineglassRandom-v0" + ], + "myodm_track_ids": [ + "MyoHandAirplaneFly-v0", + "MyoHandAirplaneLift-v0", + "MyoHandAirplanePass-v0", + "MyoHandAlarmclockLift-v0", + "MyoHandAlarmclockPass-v0", + "MyoHandAlarmclockSee-v0", + "MyoHandAppleLift-v0", + "MyoHandApplePass-v0", + "MyoHandBananaPass-v0", + "MyoHandBinocularsPass-v0", + "MyoHandBowlDrink2-v0", + "MyoHandBowlPass-v0", + "MyoHandCameraPass-v0", + "MyoHandCubelargePass-v0", + "MyoHandCubemediumLInspect-v0", + "MyoHandCubesmallLift-v0", + "MyoHandCubesmallPass-v0", + "MyoHandCupDrink-v0", + "MyoHandCupPass-v0", + "MyoHandCupPour-v0", + "MyoHandCylinderlargeInspect-v0", + "MyoHandCylindermediumLift-v0", + "MyoHandCylindermediumPass-v0", + "MyoHandCylindersmallInspect-v0", + "MyoHandCylindersmallPass-v0", + "MyoHandDuckInspect-v0", + "MyoHandDuckLift-v0", + "MyoHandDuckPass-v0", + "MyoHandElephantLift-v0", + "MyoHandElephantPass-v0", + "MyoHandEyeglassesPass-v0", + "MyoHandFlashlight1On-v0", + "MyoHandFlashlight2On-v0", + "MyoHandFlashlightLift-v0", + "MyoHandFlashlightPass-v0", + "MyoHandFlutePass-v0", + "MyoHandGamecontrollerPass-v0", + "MyoHandHammerPass-v0", + "MyoHandHammerUse-v0", + "MyoHandHandInspect-v0", + "MyoHandHeadphonesPass-v0", + "MyoHandKnifeChop-v0", + "MyoHandLightbulbPass-v0", + "MyoHandMouseLift-v0", + "MyoHandMousePass-v0", + "MyoHandMouseUse-v0", + "MyoHandMugDrink3-v0", + "MyoHandMugLift-v0", + "MyoHandMugPass-v0", + "MyoHandPhoneLift-v0", + "MyoHandPiggybankPass-v0", + "MyoHandPiggybankUse-v0", + "MyoHandPyramidlargePass-v0", + "MyoHandPyramidmediumPass-v0", + "MyoHandPyramidsmallInspect-v0", + "MyoHandScissorsUse-v0", + "MyoHandSpherelargePass-v0", + "MyoHandSpheremediumInspect-v0", + "MyoHandSpheremediumLift-v0", + "MyoHandSpheresmallInspect-v0", + "MyoHandSpheresmallLift-v0", + "MyoHandSpheresmallPass-v0", + "MyoHandStampLift-v0", + "MyoHandStampStamp-v0", + "MyoHandStanfordbunnyInspect-v0", + "MyoHandStanfordbunnyPass-v0", + "MyoHandStaplerLift-v0", + "MyoHandStaplerStaple1-v0", + "MyoHandStaplerStaple2-v0", + "MyoHandTeapotPour2-v0", + "MyoHandToothbrushBrush1-v0", + "MyoHandToothbrushLift-v0", + "MyoHandToothpasteLift-v0", + "MyoHandToothpasteSqueeze1-v0", + "MyoHandToruslargeInspect-v0", + "MyoHandToruslargeLift-v0", + "MyoHandTorusmediumLift-v0", + "MyoHandTorusmediumPass-v0", + "MyoHandTorussmallLift-v0", + "MyoHandTorussmallPass-v0", + "MyoHandTrainPlay-v0", + "MyoHandWatchLift-v0", + "MyoHandWatchPass-v0", + "MyoHandWaterbottleLift-v0", + "MyoHandWaterbottlePass-v0", + "MyoHandWaterbottleShake-v0", + "MyoHandWineglassDrink2-v0", + "MyoHandWineglassLift-v0", + "MyoHandWineglassPass-v0", + "MyoHandWineglassToast1-v0" + ], + "myoedits_direct_ids": [ + "myoArmReachFixed-v0", + "myoArmReachRandom-v0" + ] + } +} diff --git a/third_party/myosuite/mpl_sim.BUILD b/third_party/myosuite/mpl_sim.BUILD new file mode 100644 index 000000000..6ab72a846 --- /dev/null +++ b/third_party/myosuite/mpl_sim.BUILD @@ -0,0 +1,31 @@ +# Copyright 2026 Garena Online Private 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. + +filegroup( + name = "runtime_assets", + srcs = glob( + ["**"], + exclude = [ + "**/.git/**", + "**/.github/**", + "**/.idea/**", + "**/__pycache__/**", + "**/*.md", + "**/*.py", + "**/LICENSE", + "**/LICENSE.*", + ], + ), + visibility = ["//visibility:public"], +) diff --git a/third_party/myosuite/myo_sim.BUILD b/third_party/myosuite/myo_sim.BUILD new file mode 100644 index 000000000..bca2024d3 --- /dev/null +++ b/third_party/myosuite/myo_sim.BUILD @@ -0,0 +1,30 @@ +# Copyright 2026 Garena Online Private 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. + +filegroup( + name = "runtime_assets", + srcs = glob( + ["**"], + exclude = [ + "**/.git/**", + "**/.github/**", + "**/__pycache__/**", + "**/*.md", + "**/*.py", + "**/LICENSE", + "**/LICENSE.*", + ], + ), + visibility = ["//visibility:public"], +) diff --git a/third_party/myosuite/myosuite.BUILD b/third_party/myosuite/myosuite.BUILD new file mode 100644 index 000000000..a4d64db5c --- /dev/null +++ b/third_party/myosuite/myosuite.BUILD @@ -0,0 +1,55 @@ +# Copyright 2026 Garena Online Private 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. + +filegroup( + name = "myosuite_runtime_assets", + srcs = glob( + [ + "myosuite/envs/myo/assets/**", + "myosuite/envs/myo/myodm/data/**", + ], + exclude = [ + "myosuite/envs/myo/assets/**/*.py", + "myosuite/envs/myo/assets/**/__pycache__/**", + "myosuite/envs/myo/myodm/data/**/*.py", + "myosuite/envs/myo/myodm/data/**/__pycache__/**", + ], + ), + visibility = ["//visibility:public"], +) + +filegroup( + name = "myosuite_registry_sources", + srcs = [ + ".gitmodules", + "myosuite/__init__.py", + "myosuite/envs/myo/assets/hand/myohand_object.xml", + "myosuite/envs/myo/myobase/__init__.py", + "myosuite/envs/myo/myochallenge/__init__.py", + "myosuite/envs/myo/myodm/__init__.py", + "myosuite/envs/myo/myoedits/__init__.py", + "myosuite_init.py", + "pyproject.toml", + ], + visibility = ["//visibility:public"], +) + +filegroup( + name = "myosuite_python_sources", + srcs = glob( + ["myosuite/**/*.py"], + exclude = ["myosuite/**/__pycache__/**"], + ), + visibility = ["//visibility:public"], +) diff --git a/third_party/myosuite/object_sim.BUILD b/third_party/myosuite/object_sim.BUILD new file mode 100644 index 000000000..bca2024d3 --- /dev/null +++ b/third_party/myosuite/object_sim.BUILD @@ -0,0 +1,30 @@ +# Copyright 2026 Garena Online Private 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. + +filegroup( + name = "runtime_assets", + srcs = glob( + ["**"], + exclude = [ + "**/.git/**", + "**/.github/**", + "**/__pycache__/**", + "**/*.md", + "**/*.py", + "**/LICENSE", + "**/LICENSE.*", + ], + ), + visibility = ["//visibility:public"], +) diff --git a/third_party/myosuite/simhive/myo_sim/arm/myoarm_reach.xml b/third_party/myosuite/simhive/myo_sim/arm/myoarm_reach.xml new file mode 100644 index 000000000..40eda0ca3 --- /dev/null +++ b/third_party/myosuite/simhive/myo_sim/arm/myoarm_reach.xml @@ -0,0 +1,1140 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/third_party/myosuite/simhive/myo_sim/arm/myoarm_tabletennis.xml b/third_party/myosuite/simhive/myo_sim/arm/myoarm_tabletennis.xml new file mode 100644 index 000000000..fdeb1e6ed --- /dev/null +++ b/third_party/myosuite/simhive/myo_sim/arm/myoarm_tabletennis.xml @@ -0,0 +1,4209 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/third_party/myosuite/upstream_compat.patch b/third_party/myosuite/upstream_compat.patch new file mode 100644 index 000000000..4f520a5bd --- /dev/null +++ b/third_party/myosuite/upstream_compat.patch @@ -0,0 +1,201 @@ +diff --git a/myosuite/envs/myo/myochallenge/bimanual_v0.py b/myosuite/envs/myo/myochallenge/bimanual_v0.py +index 31ab7cf..7e45aa7 100644 +--- a/myosuite/envs/myo/myochallenge/bimanual_v0.py ++++ b/myosuite/envs/myo/myochallenge/bimanual_v0.py +@@ -87,9 +87,9 @@ class BimanualEnvV1(BaseV0): + self.start_bid = self.id_info.start_id + self.goal_bid = self.id_info.goal_id + +- self.obj_bid = self.id_info.manip_body_id ++ self.obj_bid = int(np.asarray(self.id_info.manip_body_id).reshape(-1)[0]) + self.obj_sid = self.sim.model.site_name2id('touch_site') +- self.obj_gid = self.sim.model.body(self.obj_bid).geomadr + 1 ++ self.obj_gid = int(np.asarray(self.sim.model.body(self.obj_bid).geomadr).reshape(-1)[0]) + 1 + self.obj_mid = next(i + for i in range(self.sim.model.nmesh) + if "box" in self.sim.model.mesh(i).name) +@@ -169,8 +169,10 @@ class BimanualEnvV1(BaseV0): + to call this method within setup after relevant ids have been identified. + """ + self.obj_size0 = self.sim.model.geom_size[self.obj_gid].copy() +- self.obj_vert_addr = np.arange(self.sim.model.mesh(self.obj_mid).vertadr, +- self.sim.model.mesh(self.obj_mid).vertadr + self.sim.model.mesh(0).vertnum) ++ obj_mesh = self.sim.model.mesh(self.obj_mid) ++ obj_vertadr = int(np.asarray(obj_mesh.vertadr).reshape(-1)[0]) ++ obj_vertnum = int(np.asarray(obj_mesh.vertnum).reshape(-1)[0]) ++ self.obj_vert_addr = np.arange(obj_vertadr, obj_vertadr + obj_vertnum) + q = self.sim.model.geom(self.obj_gid - 1).quat + r = R.from_quat([q[1], q[2], q[3], q[0]]) + self.sim.model.mesh_vert[self.obj_vert_addr] = r.apply(self.sim.model.mesh_vert[self.obj_vert_addr]) +@@ -407,7 +409,7 @@ class ContactTrajIssue(enum.Enum): + + class IdInfo: + def __init__(self, model: mujoco.MjModel): +- self.manip_body_id = model.body("manip_object").id ++ self.manip_body_id = int(np.asarray(model.body("manip_object").id).reshape(-1)[0]) + + myo_bodies = [model.body(i).id for i in range(model.nbody) + if not model.body(i).name.startswith("prosthesis") +@@ -431,14 +433,16 @@ class IdInfo: + self.prosth_dof_range = np.concatenate([model.joint(i).dofadr for i in range(model.njnt) + if model.joint(i).name.startswith("prosthesis")]) + +- self.manip_joint_range = np.arange(model.joint("manip_object/freejoint").qposadr, +- model.joint("manip_object/freejoint").qposadr + 7) ++ manip_joint = model.joint("manip_object/freejoint") ++ manip_qposadr = int(np.asarray(manip_joint.qposadr).reshape(-1)[0]) ++ manip_dofadr = int(np.asarray(manip_joint.dofadr).reshape(-1)[0]) ++ self.manip_joint_range = np.arange(manip_qposadr, manip_qposadr + 7) + +- self.manip_dof_range = np.arange(model.joint("manip_object/freejoint").dofadr, +- model.joint("manip_object/freejoint").dofadr + 6) ++ self.manip_dof_range = np.arange(manip_dofadr, ++ manip_dofadr + 6) + +- self.start_id = model.body("start").id +- self.goal_id = model.body("goal").id ++ self.start_id = int(np.asarray(model.body("start").id).reshape(-1)[0]) ++ self.goal_id = int(np.asarray(model.body("goal").id).reshape(-1)[0]) + + + def get_touching_objects(model: mujoco.MjModel, data: mujoco.MjData, id_info: IdInfo): +diff --git a/myosuite/envs/myo/myochallenge/chasetag_v0.py b/myosuite/envs/myo/myochallenge/chasetag_v0.py +index 7eaea4f..92f4eba 100644 +--- a/myosuite/envs/myo/myochallenge/chasetag_v0.py ++++ b/myosuite/envs/myo/myochallenge/chasetag_v0.py +@@ -631,12 +631,13 @@ class ChaseTagEnvV0(WalkEnvV0): + # The task is entirely defined by these 3 lines + win_cdt = self._win_condition() + lose_cdt = self._lose_condition() ++ time_value = float(np.asarray(self.obs_dict["time"]).reshape(-1)[0]) + if self.current_task.name == "CHASE": +- score = self._get_score(float(self.obs_dict["time"])) if win_cdt else 0 ++ score = self._get_score(time_value) if win_cdt else 0 + self.obs_dict["time"] = self.maxTime if lose_cdt else self.obs_dict["time"] + elif self.current_task.name == "EVADE": + score = ( +- self._get_score(float(self.obs_dict["time"])) ++ self._get_score(time_value) + if (win_cdt or lose_cdt) + else 0 + ) +diff --git a/myosuite/envs/myo/myochallenge/soccer_v0.py b/myosuite/envs/myo/myochallenge/soccer_v0.py +index c3bab63..ef41956 100644 +--- a/myosuite/envs/myo/myochallenge/soccer_v0.py ++++ b/myosuite/envs/myo/myochallenge/soccer_v0.py +@@ -344,6 +344,7 @@ class SoccerEnvV0(WalkEnvV0): + + pain = self.get_jnt_limit_violation() # Joint limit violation torque as pain score + done = bool(goal_scored or time_limit_exceeded or fallen) ++ time_cost = float(np.asarray(self.obs_dict['time']).reshape(-1)[0]) + # ---------------------- + + # Example reward, you should change this! +@@ -358,7 +359,7 @@ class SoccerEnvV0(WalkEnvV0): + + # Optional Keys + ('goal_scored', float(goal_scored)), +- ('time_cost', float(self.obs_dict['time'])), ++ ('time_cost', time_cost), + ('act_reg', act_mag), + ('pain', pain), + # Must keys +@@ -456,8 +457,8 @@ class SoccerEnvV0(WalkEnvV0): + # Add some noise to the joints + for jnt in self.myo_joints: +- self.sim.data.joint(jnt).qpos[0] += self.np_random.uniform(-np.abs(self.rnd_joint_noise), np.abs(self.rnd_joint_noise), size=(1,)) ++ self.sim.data.joint(jnt).qpos[0] += self.np_random.uniform(-np.abs(self.rnd_joint_noise), np.abs(self.rnd_joint_noise)) + + # Body start position randomizations + # Treatment for root joint is different +- self.sim.data.joint('root').qpos[0] += self.np_random.uniform(-np.abs(self.rnd_pos_noise), 0, size=(1,)) # Only allow body to move behind the ball, not in front +- self.sim.data.joint('root').qpos[1] += self.np_random.uniform(-np.abs(self.rnd_pos_noise), np.abs(self.rnd_pos_noise),size=(1,)) # Y-direction movement allowable ++ self.sim.data.joint('root').qpos[0] += self.np_random.uniform(-np.abs(self.rnd_pos_noise), 0) # Only allow body to move behind the ball, not in front ++ self.sim.data.joint('root').qpos[1] += self.np_random.uniform(-np.abs(self.rnd_pos_noise), np.abs(self.rnd_pos_noise)) # Y-direction movement allowable +diff --git a/myosuite/envs/myo/myochallenge/tabletennis_v0.py b/myosuite/envs/myo/myochallenge/tabletennis_v0.py +index 1bbbfe5..c840519 100644 +--- a/myosuite/envs/myo/myochallenge/tabletennis_v0.py ++++ b/myosuite/envs/myo/myochallenge/tabletennis_v0.py +@@ -401,7 +401,7 @@ class TableTennisEnvV0(BaseV0): + warnings.warn("A paddle was found that was not a free body. Confirm this is intended.") + for s in spec.sensors: + if "pingpong" not in s.name and "paddle" not in s.name: +- s.delete() ++ spec.delete(s) + temp_model = spec.compile() + + removed_ids = recursive_immobilize(spec, temp_model, spec.body("femur_l"), remove_eqs=True) +@@ -419,15 +419,15 @@ class TableTennisEnvV0(BaseV0): + spec_copy: mujoco.MjSpec = spec.copy() + attachment_frame = torso.add_frame(quat=[0.5, 0.5, -0.5, 0.5], + pos=[0.05, 0.373, -0.04]) +- [k.delete() for k in spec_copy.keys] +- [t.delete() for t in spec_copy.textures] +- [m.delete() for m in spec_copy.materials] +- [t.delete() for t in spec_copy.tendons] +- [a.delete() for a in spec_copy.actuators] +- [e.delete() for e in spec_copy.equalities] +- [s.delete() for s in spec_copy.sensors] +- [c.delete() for c in spec_copy.cameras] ++ [spec_copy.delete(k) for k in list(spec_copy.keys)] ++ [spec_copy.delete(t) for t in list(spec_copy.textures)] ++ [spec_copy.delete(m) for m in list(spec_copy.materials)] ++ [spec_copy.delete(t) for t in list(spec_copy.tendons)] ++ [spec_copy.delete(a) for a in list(spec_copy.actuators)] ++ [spec_copy.delete(e) for e in list(spec_copy.equalities)] ++ [spec_copy.delete(s) for s in list(spec_copy.sensors)] ++ [spec_copy.delete(c) for c in list(spec_copy.cameras)] +- recursive_immobilize(spec, temp_model, spec_copy.worldbody) ++ recursive_immobilize(spec_copy, temp_model, spec_copy.worldbody) + recursive_remove_contacts(spec_copy.worldbody, return_condition=None) + +@@ -438,7 +438,7 @@ class TableTennisEnvV0(BaseV0): + mesh.name += "_mirrored" + mesh.scale[1] *= -1 + else: +- mesh.delete() ++ spec_copy.delete(mesh) + + attachment_frame.attach_body(spec_copy.body("clavicle_mirrored")) + spec.body("ulna_mirrored").quat =[0.546, 0, 0, -0.838] +diff --git a/myosuite/utils/spec_processing.py b/myosuite/utils/spec_processing.py +index 924ccb6..0cfa789 100644 +--- a/myosuite/utils/spec_processing.py ++++ b/myosuite/utils/spec_processing.py +@@ -2,19 +2,19 @@ import mujoco + + def recursive_immobilize(spec, temp_model, parent, remove_eqs=False, remove_actuators=False): + removed_joint_ids = [] +- for s in parent.sites: +- s.delete() +- for j in parent.joints: ++ for s in list(parent.sites): ++ spec.delete(s) ++ for j in list(parent.joints): + removed_joint_ids.extend(temp_model.joint(j.name).qposadr) + if remove_eqs: +- for e in spec.equalities: ++ for e in list(spec.equalities): + if e.type == mujoco.mjtEq.mjEQ_JOINT and (e.name1 == j.name or e.name2 == j.name): +- e.delete() ++ spec.delete(e) + if remove_actuators: +- for a in spec.actuators: ++ for a in list(spec.actuators): + if a.trntype == mujoco.mjtTrn.mjTRN_JOINT and a.target == j.name: +- a.delete() +- j.delete() ++ spec.delete(a) ++ spec.delete(j) + for child in parent.bodies: + removed_joint_ids.extend( + recursive_immobilize(spec, temp_model, child, remove_eqs, remove_actuators) +@@ -38,7 +38,7 @@ def recursive_mirror(meshes_to_mirror, spec_copy, parent): + parent.name += "_mirrored" + for g in parent.geoms: + if g.type != mujoco.mjtGeom.mjGEOM_MESH: +- g.delete() ++ spec_copy.delete(g) + continue + g.pos[1] *= -1 + g.quat[[1, 3]] *= -1 diff --git a/third_party/myosuite/ycb_sim.BUILD b/third_party/myosuite/ycb_sim.BUILD new file mode 100644 index 000000000..bca2024d3 --- /dev/null +++ b/third_party/myosuite/ycb_sim.BUILD @@ -0,0 +1,30 @@ +# Copyright 2026 Garena Online Private 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. + +filegroup( + name = "runtime_assets", + srcs = glob( + ["**"], + exclude = [ + "**/.git/**", + "**/.github/**", + "**/__pycache__/**", + "**/*.md", + "**/*.py", + "**/LICENSE", + "**/LICENSE.*", + ], + ), + visibility = ["//visibility:public"], +) diff --git a/third_party/pip_requirements/requirements-dev-lock.txt b/third_party/pip_requirements/requirements-dev-lock.txt index 07df5e31e..7f9a24b32 100644 --- a/third_party/pip_requirements/requirements-dev-lock.txt +++ b/third_party/pip_requirements/requirements-dev-lock.txt @@ -4,6 +4,7 @@ box2d==2.3.10 ; sys_platform != "linux" or (platform_machine != "aarch64" and pl box2d==2.3.2 ; sys_platform == "linux" and (platform_machine == "aarch64" or platform_machine == "arm64") certifi==2026.2.25 charset-normalizer==3.4.6 +click==8.2.1 cloudpickle==3.1.2 colorama==0.4.6 contourpy==1.3.3 @@ -16,8 +17,11 @@ dm-tree==0.1.9 etils==1.14.0 farama-notifications==0.0.4 filelock==3.25.2 +flatten-dict==0.4.2 fonttools==4.61.1 fsspec==2026.2.0 +gitdb==4.0.12 +gitpython==3.1.44 glfw==2.10.0 grpcio==1.78.0 gymnasium==1.2.3 @@ -64,6 +68,7 @@ optree==0.19.0 packaging==26.0 pandas==2.3.1 pettingzoo==1.24.3 +pink-noise-rl==2.0.1 pillow==12.1.1 protobuf==7.34.1 pygame==2.6.1 @@ -74,10 +79,13 @@ pytz==2025.2 requests==2.32.5 scipy==1.17.1 six==1.17.0 +sk-video==1.1.10 +smmap==5.0.2 swig==4.4.1 sympy==1.14.0 tensorboard==2.20.0 tensorboard-data-server==0.7.2 +termcolor==3.1.0 tianshou==0.5.1 torch==2.10.0 tqdm==4.67.3 diff --git a/third_party/pip_requirements/requirements-dev.txt b/third_party/pip_requirements/requirements-dev.txt index c770b38c1..c7230c0d8 100644 --- a/third_party/pip_requirements/requirements-dev.txt +++ b/third_party/pip_requirements/requirements-dev.txt @@ -1,7 +1,10 @@ setuptools==70.3.0 wheel numpy>=2,<3 +click dm-env>=1.6 +flatten-dict +gitpython gymnasium>=1.2.3 gymnasium-robotics>=1.4.2 highway-env>=1.10.2 @@ -19,6 +22,9 @@ opencv-python-headless>=4.13.0.92 dm-control>=1.0.38 mujoco==3.6.0 imageio +pink-noise-rl pygame>=2.1.3 +sk-video swig==4.* +termcolor minigrid>=3.0.0 diff --git a/third_party/vizdoom/re2c_safe_substr.patch b/third_party/vizdoom/re2c_safe_substr.patch new file mode 100644 index 000000000..46b610b92 --- /dev/null +++ b/third_party/vizdoom/re2c_safe_substr.patch @@ -0,0 +1,43 @@ +--- tools/re2c/substr.cc ++++ tools/re2c/substr.cc +@@ -4,36 +4,32 @@ + #include "substr.h" + #include "globals.h" + +-#ifndef HAVE_STRNDUP +- +-char *strndup(const char *str, size_t len) ++static char *re2c_strndup(const char *str, size_t len) + { + char * ret = (char*)malloc(len + 1); ++ if (ret == NULL) ++ return NULL; + + memcpy(ret, str, len); + ret[len] = '\0'; + return ret; + } + +-#endif +- + namespace re2c + { + + void SubStr::out(std::ostream& o) const + { + o.write(str, len); + } + + bool operator==(const SubStr &s1, const SubStr &s2) + { + return (bool) (s1.len == s2.len && memcmp(s1.str, s2.str, s1.len) == 0); + } + + Str::Str(const SubStr& s) +- //VIZDOOM_CODE +- //: SubStr(strndup(s.str, s.len), s.len) +- : SubStr(strdup(s.str), s.len) ++ : SubStr(re2c_strndup(s.str, s.len), s.len) + { + ; + } diff --git a/third_party/vizdoom/vizdoom.BUILD b/third_party/vizdoom/vizdoom.BUILD index 5c8a42f48..7334777d7 100644 --- a/third_party/vizdoom/vizdoom.BUILD +++ b/third_party/vizdoom/vizdoom.BUILD @@ -38,6 +38,7 @@ genrule( srcs = [], outs = ["arith.h"], cmd = "$(execpath :arithchk) > $@", + cmd_bat = "$(execpath :arithchk) > $@", tools = [":arithchk"], ) @@ -54,6 +55,7 @@ genrule( srcs = [], outs = ["gd_qnan.h"], cmd = "$(execpath :qnan) > $@", + cmd_bat = "$(execpath :qnan) > $@", tools = [":qnan"], ) @@ -164,7 +166,10 @@ cc_binary( "tools/re2c/substr.cc", "tools/re2c/translate.cc", ] + glob(["tools/re2c/*.h"]), - copts = ["-DHAVE_CONFIG_H"], + copts = select({ + "@envpool//:windows": [], + "//conditions:default": ["-DHAVE_CONFIG_H"], + }), ) cc_binary( @@ -177,6 +182,7 @@ genrule( srcs = ["src/sc_man_scanner.re"], outs = ["src/sc_man_scanner.h"], cmd = "$(execpath :re2c) --no-generation-date -s -o $@ $<", + cmd_bat = "$(execpath :re2c) --no-generation-date -s -o $@ $<", tools = [":re2c"], ) @@ -202,6 +208,7 @@ genrule( "xlat_parser.h", ], cmd = "$(execpath :lemon) $<", + cmd_bat = "$(execpath :lemon) $<", tools = [ ":lemon", ":lemon_deps",