89 changed files with 2837 additions and 3541 deletions
-32
View File
@@ -1,32 +0,0 @@
---
name: Bug report
about: Create a report to help us improve
title: ''
labels: ''
assignees: ''
---
**Describe the bug**
A clear and concise description of what the bug is.
**To Reproduce**
Steps to reproduce the behavior:
1. Go to '...'
2. Select variables '....'
3. Execute '....'
4. See error
**Expected behavior**
A clear and concise description of what you expected to happen.
**Screenshots**
If applicable, add screenshots to help explain your problem.
**Desktop (please complete the following information):**
- OS: [e.g. Ubunutu]
- Python version [e.g. 3.8]
- Version [e.g. 1.0]
**Additional context**
Add any other context about the problem here.
-20
View File
@@ -1,20 +0,0 @@
---
name: Feature request
about: Suggest an idea for this project
title: ''
labels: ''
assignees: ''
---
**Is your feature request related to a problem? Please describe.**
A clear and concise description of what the problem is. Ex. I'm always frustrated when [...]
**Describe the solution you'd like**
A clear and concise description of what you want to happen.
**Describe alternatives you've considered**
A clear and concise description of any alternative solutions or features you've considered.
**Additional context**
Add any other context or screenshots about the feature request here.
-128
View File
@@ -1,128 +0,0 @@
# Contributor Covenant Code of Conduct
## Our Pledge
We as members, contributors, and leaders pledge to make participation in our
community a harassment-free experience for everyone, regardless of age, body
size, visible or invisible disability, ethnicity, sex characteristics, gender
identity and expression, level of experience, education, socio-economic status,
nationality, personal appearance, race, religion, or sexual identity
and orientation.
We pledge to act and interact in ways that contribute to an open, welcoming,
diverse, inclusive, and healthy community.
## Our Standards
Examples of behavior that contributes to a positive environment for our
community include:
* Demonstrating empathy and kindness toward other people
* Being respectful of differing opinions, viewpoints, and experiences
* Giving and gracefully accepting constructive feedback
* Accepting responsibility and apologizing to those affected by our mistakes,
and learning from the experience
* Focusing on what is best not just for us as individuals, but for the
overall community
Examples of unacceptable behavior include:
* The use of sexualized language or imagery, and sexual attention or
advances of any kind
* Trolling, insulting or derogatory comments, and personal or political attacks
* Public or private harassment
* Publishing others' private information, such as a physical or email
address, without their explicit permission
* Other conduct which could reasonably be considered inappropriate in a
professional setting
## Enforcement Responsibilities
Community leaders are responsible for clarifying and enforcing our standards of
acceptable behavior and will take appropriate and fair corrective action in
response to any behavior that they deem inappropriate, threatening, offensive,
or harmful.
Community leaders have the right and responsibility to remove, edit, or reject
comments, commits, code, wiki edits, issues, and other contributions that are
not aligned to this Code of Conduct, and will communicate reasons for moderation
decisions when appropriate.
## Scope
This Code of Conduct applies within all community spaces, and also applies when
an individual is officially representing the community in public spaces.
Examples of representing our community include using an official e-mail address,
posting via an official social media account, or acting as an appointed
representative at an online or offline event.
## Enforcement
Instances of abusive, harassing, or otherwise unacceptable behavior may be
reported to the community leaders responsible for enforcement at
their respective email addresses.
All complaints will be reviewed and investigated promptly and fairly.
All community leaders are obligated to respect the privacy and security of the
reporter of any incident.
## Enforcement Guidelines
Community leaders will follow these Community Impact Guidelines in determining
the consequences for any action they deem in violation of this Code of Conduct:
### 1. Correction
**Community Impact**: Use of inappropriate language or other behavior deemed
unprofessional or unwelcome in the community.
**Consequence**: A private, written warning from community leaders, providing
clarity around the nature of the violation and an explanation of why the
behavior was inappropriate. A public apology may be requested.
### 2. Warning
**Community Impact**: A violation through a single incident or series
of actions.
**Consequence**: A warning with consequences for continued behavior. No
interaction with the people involved, including unsolicited interaction with
those enforcing the Code of Conduct, for a specified period of time. This
includes avoiding interactions in community spaces as well as external channels
like social media. Violating these terms may lead to a temporary or
permanent ban.
### 3. Temporary Ban
**Community Impact**: A serious violation of community standards, including
sustained inappropriate behavior.
**Consequence**: A temporary ban from any sort of interaction or public
communication with the community for a specified period of time. No public or
private interaction with the people involved, including unsolicited interaction
with those enforcing the Code of Conduct, is allowed during this period.
Violating these terms may lead to a permanent ban.
### 4. Permanent Ban
**Community Impact**: Demonstrating a pattern of violation of community
standards, including sustained inappropriate behavior, harassment of an
individual, or aggression toward or disparagement of classes of individuals.
**Consequence**: A permanent ban from any sort of public interaction within
the community.
## Attribution
This Code of Conduct is adapted from the [Contributor Covenant][homepage],
version 2.0, available at
https://www.contributor-covenant.org/version/2/0/code_of_conduct.html.
Community Impact Guidelines were inspired by [Mozilla's code of conduct
enforcement ladder](https://github.com/mozilla/diversity).
[homepage]: https://www.contributor-covenant.org
For answers to common questions about this code of conduct, see the FAQ at
https://www.contributor-covenant.org/faq. Translations are available at
https://www.contributor-covenant.org/translations.
-28
View File
@@ -1,28 +0,0 @@
# Contribution Guidelines
We welcome and appreciate contributions to this repository from the community. To ensure a positive and productive collaboration, we have established the following guidelines for contributing to this project:
**Code of Conduct**: Please abide by our Code of Conduct, which sets the standards for respectful and inclusive behavior within the community.
**Issues and Feature Requests**: Feel free to open new issues to report bugs or suggest new features. Before submitting, please check if a similar issue or request already exists.
**Pull Requests**: We encourage pull requests for bug fixes, new features, and improvements. Please follow these steps when submitting a pull request:
- Fork this repository and create a new branch for your changes.
- Ensure your code aligns with our coding standards.
- Provide a clear and concise description of your changes.
- Test your changes thoroughly.
- Make sure your code is well-documented.
- Code Style: Follow our coding style guidelines, which include formatting, naming conventions, and other coding standards. Adhering to these guidelines will help streamline the review process.
**Collaboration**: Be prepared for constructive feedback during the review process. We aim to maintain the quality and consistency of the codebase.
**License**: Ensure your contributions comply with the project's existing open-source license. By contributing, you grant us the right to distribute your code under the project's license.
**Ownership**: When you submit a contribution, you confirm that you have the right to license your code to us and that your work does not violate any existing patents, trademarks, or intellectual property rights.
**Maintainers**: The repository maintainers have the final authority on merging or rejecting contributions. They will work collaboratively with contributors to ensure a smooth process.
**Community Support**: As part of our community, please consider helping others, answering questions, and providing support to fellow contributors.
We appreciate your interest in contributing to our project. Your involvement helps make this community vibrant and successful.
Thank you for being part of this project!
-21
View File
@@ -1,21 +0,0 @@
MIT License
Copyright (c) 2023 Autonomous Learning Robots Lab @ KIT
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
+85 -166
View File
@@ -1,112 +1,103 @@
<h1 align="center">
<br>
<img src='./icon.svg' width="250px">
<br><br>
<b>Fancy Gym</b>
<br><br>
</h1>
# Fancy Gym
| :exclamation: Fancy Gym has recently received a major refactor, which also updated many of the used dependencies to current versions. The update has brought some breaking changes. If you want to access the old version, check out the [legacy branch](https://github.com/ALRhub/fancy_gym/tree/legacy). Find out more about what changed [here](https://github.com/ALRhub/fancy_gym/pull/75). |
| --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
Built upon the foundation of [Gymnasium](https://gymnasium.farama.org/) (a maintained fork of OpenAIs renowned Gym library) `fancy_gym` offers a comprehensive collection of reinforcement learning environments.
**Key Features**:
- **New Challenging Environments**: `fancy_gym` includes several new environments (Panda Box Pushing, Table Tennis, etc.) that present a higher degree of difficulty, pushing the boundaries of reinforcement learning research.
- **Support for Movement Primitives**: `fancy_gym` supports a range of movement primitives (MPs), including Dynamic Movement Primitives (DMPs), Probabilistic Movement Primitives (ProMP), and Probabilistic Dynamic Movement Primitives (ProDMP).
- **Upgrade to Movement Primitives**: With our framework, it's straightforward to transform standard Gymnasium environments into environments that support movement primitives.
- **Benchmark Suite Compatibility**: `fancy_gym` makes it easy to access renowned benchmark suites such as [DeepMind Control](https://deepmind.com/research/publications/2020/dm-control-Software-and-Tasks-for-Continuous-Control) and [Metaworld](https://meta-world.github.io/), whether you want to use them in the regular step-based setting or using MPs.
- **Contribute Your Own Environments**: If you're inspired to create custom gym environments, both step-based and with movement primitives, this [guide](https://gymnasium.farama.org/tutorials/gymnasium_basics/environment_creation/) will assist you. We encourage and highly appreciate submissions via PRs to integrate these environments into `fancy_gym`.
`fancy_gym` offers a large variety of reinforcement learning environments under the unifying interface
of [OpenAI gym](https://gymlibrary.dev/). We provide support (under the OpenAI gym interface) for the benchmark suites
[DeepMind Control](https://deepmind.com/research/publications/2020/dm-control-Software-and-Tasks-for-Continuous-Control)
(DMC) and [Metaworld](https://meta-world.github.io/). If those are not sufficient and you want to create your own custom
gym environments, use [this guide](https://www.gymlibrary.dev/content/environment_creation/). We highly appreciate it, if
you would then submit a PR for this environment to become part of `fancy_gym`.
In comparison to existing libraries, we additionally support to control agents with movement primitives, such as Dynamic
Movement Primitives (DMPs) and Probabilistic Movement Primitives (ProMP).
## Movement Primitive Environments (Episode-Based/Black-Box Environments)
<p align="justify">
Movement primitive (MP) environments differ from traditional step-based environments. They align more with concepts from stochastic search, black-box optimization, and methods commonly found in classical robotics and control. Instead of individual steps, MP environments operate on an episode basis, executing complete trajectories. These trajectories are produced by trajectory generators like Dynamic Movement Primitives (DMP), Probabilistic Movement Primitives (ProMP) or Probabilistic Dynamic Movement Primitives (ProDMP).
</p>
<p align="justify">
Once generated, these trajectories are converted into step-by-step actions using a trajectory tracking controller. The specific controller chosen depends on the environment's requirements. Currently, we support position, velocity, and PD-Controllers tailored for position, velocity, and torque control. Additionally, we have a specialized controller designed for the MetaWorld control suite.
</p>
<p align="justify">
While the overarching objective of MP environments remains the learning of an optimal policy, the actions here represent the parametrization of motion primitives to craft the right trajectory. Our framework further enhances this by accommodating a contextual setting. At the episode's onset, we present the context space—a subset of the observation space. This demands the prediction of a new action or MP parametrization for every unique context.
</p>
Unlike step-based environments, movement primitive (MP) environments are closer related to stochastic search, black-box
optimization, and methods that are often used in traditional robotics and control. MP environments are typically
episode-based and execute a full trajectory, which is generated by a trajectory generator, such as a Dynamic Movement
Primitive (DMP) or a Probabilistic Movement Primitive (ProMP). The generated trajectory is translated into individual
step-wise actions by a trajectory tracking controller. The exact choice of controller is, however, dependent on the type
of environment. We currently support position, velocity, and PD-Controllers for position, velocity, and torque control,
respectively as well as a special controller for the MetaWorld control suite.
The goal of all MP environments is still to learn an optimal policy. Yet, an action represents the parametrization of
the motion primitives to generate a suitable trajectory. Additionally, in this framework we support all of this also for
the contextual setting, i.e. we expose the context space - a subset of the observation space - in the beginning of the
episode. This requires to predict a new action/MP parametrization for each context.
## Installation
1. Clone the repository
```bash
```bash
git clone git@github.com:ALRhub/fancy_gym.git
```
2. Go to the folder
```bash
```bash
cd fancy_gym
```
3. Install with
```bash
```bash
pip install -e .
```
We have a few optional dependencies. If you also want to install those use
In case you want to use dm_control oder metaworld, you can install them by specifying extras
```bash
pip install -e '.[all]' # to install all optional dependencies
pip install -e '.[dmc,metaworld,box2d,mujoco,mujoco-legacy,jax,testing]' # or choose only those you want
```bash
pip install -e .[dmc,metaworld]
```
> **Note:**
> While our library already fully supports the new mujoco bindings, metaworld still relies on
> [mujoco_py](https://github.com/openai/mujoco-py), hence make sure to have mujoco 2.1 installed beforehand.
## How to use Fancy Gym
We will only show the basics here and prepared [multiple examples](fancy_gym/examples/) for a more detailed look.
### Step-Based Environments
Regular step based environments added by Fancy Gym are added into the `fancy/` namespace.
| :exclamation: Legacy versions of Fancy Gym used `fancy_gym.make(...)`. This is no longer supported and will raise an Exception on new versions. |
| ----------------------------------------------------------------------------------------------------------------------------------------------- |
### Step-wise Environments
```python
import gymnasium as gym
import fancy_gym
env = gym.make('fancy/Reacher5d-v0')
# or env = gym.make('metaworld/reach-v2') # fancy_gym allows access to all metaworld ML1 tasks via the metaworld/ NS
# or env = gym.make('dm_control/ball_in_cup-catch-v0')
# or env = gym.make('Reacher-v2')
observation = env.reset(seed=1)
env = fancy_gym.make('Reacher5d-v0', seed=1)
obs = env.reset()
for i in range(1000):
action = env.action_space.sample()
observation, reward, terminated, truncated, info = env.step(action)
obs, reward, done, info = env.step(action)
if i % 5 == 0:
env.render()
if terminated or truncated:
observation, info = env.reset()
```
if done:
obs = env.reset()
```
When using `dm_control` tasks we expect the `env_id` to be specified as `dmc:domain_name-task_name` or for manipulation
tasks as `dmc:manipulation-environment_name`. For `metaworld` tasks, we require the structure `metaworld:env_id-v2`, our
custom tasks and standard gym environments can be created without prefixes.
### Black-box Environments
All environments provide by default the cumulative episode reward, this can however be changed if necessary. Optionally, each environment returns all collected information from each step as part of the infos. This information is, however, mainly meant for debugging as well as logging and not for training.
All environments provide by default the cumulative episode reward, this can however be changed if necessary. Optionally,
each environment returns all collected information from each step as part of the infos. This information is, however,
mainly meant for debugging as well as logging and not for training.
| Key | Description | Type |
| ------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -------- |
| `positions` | Generated trajectory from MP | Optional |
| `velocities` | Generated trajectory from MP | Optional |
| `step_actions` | Step-wise executed action based on controller output | Optional |
| `step_observations` | Step-wise intermediate observations | Optional |
| `step_rewards` | Step-wise rewards | Optional |
| `trajectory_length` | Total number of environment interactions | Always |
| `other` | All other information from the underlying environment are returned as a list with length `trajectory_length` maintaining the original key. In case some information are not provided every time step, the missing values are filled with `None`. | Always |
|Key| Description|Type
|---|---|---|
`positions`| Generated trajectory from MP | Optional
`velocities`| Generated trajectory from MP | Optional
`step_actions`| Step-wise executed action based on controller output | Optional
`step_observations`| Step-wise intermediate observations | Optional
`step_rewards`| Step-wise rewards | Optional
`trajectory_length`| Total number of environment interactions | Always
`other`| All other information from the underlying environment are returned as a list with length `trajectory_length` maintaining the original key. In case some information are not provided every time step, the missing values are filled with `None`. | Always
Existing MP tasks can be created the same way as above. The namespace of a MP-variant of an environment is given by `<original namespace>_<MP name>/`.
Just keep in mind, calling `step()` executes a full trajectory.
Existing MP tasks can be created the same way as above. Just keep in mind, calling `step()` executes a full trajectory.
> **Note:**
> **Note:**
> Currently, we are also in the process of enabling replanning as well as learning of sub-trajectories.
> This allows to split the episode into multiple trajectories and is a hybrid setting between step-based and
> black-box leaning.
@@ -114,38 +105,30 @@ Just keep in mind, calling `step()` executes a full trajectory.
> Feel free to try it and open an issue with any problems that occur.
```python
import gymnasium as gym
import fancy_gym
env = gym.make('fancy_ProMP/Reacher5d-v0')
# or env = gym.make('metaworld_ProDMP/reach-v2')
# or env = gym.make('dm_control_DMP/ball_in_cup-catch-v0')
# or env = gym.make('gym_ProMP/Reacher-v2') # mp versions of envs added directly by gymnasium are in the gym_<MP-type> NS
env = fancy_gym.make('Reacher5dProMP-v0', seed=1)
# render() can be called once in the beginning with all necessary arguments.
# To turn it of again just call render() without any arguments.
# To turn it of again just call render() without any arguments.
env.render(mode='human')
# This returns the context information, not the full state observation
observation, info = env.reset(seed=1)
obs = env.reset()
for i in range(5):
action = env.action_space.sample()
observation, reward, terminated, truncated, info = env.step(action)
obs, reward, done, info = env.step(action)
# terminated or truncated is always True as we are working on the episode level, hence we always reset()
observation, info = env.reset()
# Done is always True as we are working on the episode level, hence we always reset()
obs = env.reset()
```
To show all available environments, we provide some additional convenience variables. All of them return a dictionary
with the keys `DMP`, `ProMP`, `ProDMP` and `all` that store a list of available environment ids.
with two keys `DMP` and `ProMP` that store a list of available environment ids.
```python
import fancy_gym
print("All Black-box tasks:")
print(fancy_gym.ALL_MOVEMENT_PRIMITIVE_ENVIRONMENTS)
print("Fancy Black-box tasks:")
print(fancy_gym.ALL_FANCY_MOVEMENT_PRIMITIVE_ENVIRONMENTS)
@@ -157,9 +140,6 @@ print(fancy_gym.ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS)
print("MetaWorld Black-box tasks:")
print(fancy_gym.ALL_METAWORLD_MOVEMENT_PRIMITIVE_ENVIRONMENTS)
print("If you add custom envs, their mp versions will be found in:")
print(fancy_gym.MOVEMENT_PRIMITIVE_ENVIRONMENTS_FOR_NS['<my_custom_namespace>'])
```
### How to create a new MP task
@@ -171,27 +151,23 @@ hand, the following [interface](fancy_gym/black_box/raw_interface_wrapper.py) ne
from abc import abstractmethod
from typing import Union, Tuple
import gymnasium as gym
import gym
import numpy as np
class RawInterfaceWrapper(gym.Wrapper):
mp_config = {
'ProMP': {},
'DMP': {},
'ProDMP': {},
}
@property
def context_mask(self) -> np.ndarray:
"""
Returns boolean mask of the same shape as the observation space.
It determines whether the observation is returned for the contextual case or not.
This effectively allows to filter unwanted or unnecessary observations from the full step-based case.
E.g. Velocities starting at 0 are only changing after the first action. Given we only receive the
context/part of the first observation, the velocities are not necessary in the observation for the task.
Returns:
bool array representing the indices of the observations
Returns boolean mask of the same shape as the observation space.
It determines whether the observation is returned for the contextual case or not.
This effectively allows to filter unwanted or unnecessary observations from the full step-based case.
E.g. Velocities starting at 0 are only changing after the first action. Given we only receive the
context/part of the first observation, the velocities are not necessary in the observation for the task.
Returns:
bool array representing the indices of the observations
"""
return np.ones(self.env.observation_space.shape[0], dtype=bool)
@@ -221,91 +197,34 @@ class RawInterfaceWrapper(gym.Wrapper):
```
Default configurations for MPs can be overitten by defining attributes in mp_config.
Available parameters are documented in the [MP_PyTorch Userguide](https://github.com/ALRhub/MP_PyTorch/blob/main/doc/README.md).
```python
class RawInterfaceWrapper(gym.Wrapper):
mp_config = {
'ProMP': {
'phase_generator_kwargs': {
'phase_generator_type': 'linear'
# When selecting another generator type, the default configuration will not be merged for the attribute.
},
'controller_kwargs': {
'p_gains': 0.5 * np.array([1.0, 4.0, 2.0, 4.0, 1.0, 4.0, 1.0]),
'd_gains': 0.5 * np.array([0.1, 0.4, 0.2, 0.4, 0.1, 0.4, 0.1]),
},
'basis_generator_kwargs': {
'num_basis': 3,
'num_basis_zero_start': 1,
'num_basis_zero_goal': 1,
},
},
'DMP': {},
'ProDMP': {}.
}
[...]
```
If you created a new task wrapper, feel free to open a PR, so we can integrate it for others to use as well. Without the
integration the task can still be used. A rough outline can be shown here, for more details we recommend having a look
at the [examples](fancy_gym/examples/).
If the step-based is already registered with gym, you can simply do the following:
```python
fancy_gym.upgrade(
id='custom/cool_new_env-v0',
mp_wrapper=my_custom_MPWrapper
)
```
import fancy_gym
If the step-based is not yet registered with gym we can add both the step-based and MP-versions via
# Base environment name, according to structure of above example
base_env_id = "dmc:ball_in_cup-catch"
```python
fancy_gym.register(
id='custom/cool_new_env-v0',
entry_point=my_custom_env,
mp_wrapper=my_custom_MPWrapper
)
```
From this point on, you can access MP-version of your environments via
```python
env = gym.make('custom_ProDMP/cool_new_env-v0')
# Replace this wrapper with the custom wrapper for your environment by inheriting from the RawInferfaceWrapper.
# You can also add other gym.Wrappers in case they are needed,
# e.g. gym.wrappers.FlattenObservation for dict observations
wrappers = [fancy_gym.dmc.suite.ball_in_cup.MPWrapper]
kwargs = {...}
env = fancy_gym.make_bb(base_env_id, wrappers=wrappers, seed=0, **kwargs)
rewards = 0
observation, info = env.reset()
obs = env.reset()
# number of samples/full trajectories (multiple environment steps)
for i in range(5):
ac = env.action_space.sample()
observation, reward, terminated, truncated, info = env.step(ac)
obs, reward, done, info = env.step(ac)
rewards += reward
if terminated or truncated:
print(rewards)
if done:
print(base_env_id, rewards)
rewards = 0
observation, info = env.reset()
obs = env.reset()
```
## Citing the Project
To cite this repository in publications:
```bibtex
@software{fancy_gym,
title = {Fancy Gym},
author = {Otto, Fabian and Celik, Onur and Roth, Dominik and Zhou, Hongyi},
abstract = {Fancy Gym: Unifying interface for various RL benchmarks with support for Black Box approaches.},
url = {https://github.com/ALRhub/fancy_gym},
organization = {Autonomous Learning Robots Lab (ALR) at KIT},
}
```
## Icon Attribution
The icon is based on the [Gymnasium](https://github.com/Farama-Foundation/Gymnasium) icon as can be found [here](https://gymnasium.farama.org/_static/img/gymnasium_black.svg).
+11 -15
View File
@@ -1,17 +1,13 @@
from fancy_gym import dmc, meta, open_ai
from fancy_gym import envs as fancy
from fancy_gym.utils.make_env_helpers import make_bb
from .envs.registry import register, upgrade
from .envs.registry import ALL_MOVEMENT_PRIMITIVE_ENVIRONMENTS, MOVEMENT_PRIMITIVE_ENVIRONMENTS_FOR_NS
from fancy_gym.utils.make_env_helpers import make, make_bb, make_rank
from .dmc import ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS
# Convenience function for all MP environments
from .envs import ALL_FANCY_MOVEMENT_PRIMITIVE_ENVIRONMENTS
from .meta import ALL_METAWORLD_MOVEMENT_PRIMITIVE_ENVIRONMENTS
from .open_ai import ALL_GYM_MOVEMENT_PRIMITIVE_ENVIRONMENTS
ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS = MOVEMENT_PRIMITIVE_ENVIRONMENTS_FOR_NS['dm_control']
ALL_FANCY_MOVEMENT_PRIMITIVE_ENVIRONMENTS = MOVEMENT_PRIMITIVE_ENVIRONMENTS_FOR_NS['fancy']
ALL_METAWORLD_MOVEMENT_PRIMITIVE_ENVIRONMENTS = MOVEMENT_PRIMITIVE_ENVIRONMENTS_FOR_NS['metaworld']
ALL_GYM_MOVEMENT_PRIMITIVE_ENVIRONMENTS = MOVEMENT_PRIMITIVE_ENVIRONMENTS_FOR_NS['gym']
def make(*args, **kwargs):
"""
As part of the refactor of Fancy Gym and upgrade to gymnasium the use of fancy_gym.make has been discontinued. Regular gym.make should be used instead. For more details check out the github README. If your codebase was build for older versions of Fancy Gym and relies on the old behavior and dependency versions, please check out the legacy branch.
"""
raise Exception('As part of the refactor of Fancy Gym and upgrade to gymnasium the use of fancy_gym.make has been discontinued. Regular gym.make should be used instead. For more details check out the github README. If your codebase was build for older versions of Fancy Gym and relies on the old behavior and dependency versions, please check out the legacy branch.')
ALL_MOVEMENT_PRIMITIVE_ENVIRONMENTS = {
key: value + ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS[key] +
ALL_GYM_MOVEMENT_PRIMITIVE_ENVIRONMENTS[key] +
ALL_METAWORLD_MOVEMENT_PRIMITIVE_ENVIRONMENTS[key]
for key, value in ALL_FANCY_MOVEMENT_PRIMITIVE_ENVIRONMENTS.items()}
+31 -37
View File
@@ -1,9 +1,8 @@
from typing import Tuple, Optional, Callable, Dict, Any
from typing import Tuple, Optional, Callable
import gymnasium as gym
import gym
import numpy as np
from gymnasium import spaces
from gymnasium.core import ObsType
from gym import spaces
from mp_pytorch.mp.mp_interfaces import MPInterface
from fancy_gym.black_box.controller.base_controller import BaseController
@@ -68,14 +67,13 @@ class BlackBoxWrapper(gym.ObservationWrapper):
self.reward_aggregation = reward_aggregation
# spaces
self.return_context_observation = not (
learn_sub_trajectories or self.do_replanning)
self.return_context_observation = not (learn_sub_trajectories or self.do_replanning)
self.traj_gen_action_space = self._get_traj_gen_action_space()
self.action_space = self._get_action_space()
self.observation_space = self._get_observation_space()
# rendering
self.do_render = False
self.render_kwargs = {}
self.verbose = verbose
# condition value
@@ -101,17 +99,14 @@ class BlackBoxWrapper(gym.ObservationWrapper):
# If we do not do this, the traj_gen assumes we are continuing the trajectory.
self.traj_gen.reset()
clipped_params = np.clip(
action, self.traj_gen_action_space.low, self.traj_gen_action_space.high)
clipped_params = np.clip(action, self.traj_gen_action_space.low, self.traj_gen_action_space.high)
self.traj_gen.set_params(clipped_params)
init_time = np.array(
0 if not self.do_replanning else self.current_traj_steps * self.dt)
init_time = np.array(0 if not self.do_replanning else self.current_traj_steps * self.dt)
condition_pos = self.condition_pos if self.condition_pos is not None else self.env.get_wrapper_attr('current_pos')
condition_vel = self.condition_vel if self.condition_vel is not None else self.env.get_wrapper_attr('current_vel')
condition_pos = self.condition_pos if self.condition_pos is not None else self.current_pos
condition_vel = self.condition_vel if self.condition_vel is not None else self.current_vel
self.traj_gen.set_initial_conditions(
init_time, condition_pos, condition_vel)
self.traj_gen.set_initial_conditions(init_time, condition_pos, condition_vel)
self.traj_gen.set_duration(duration, self.dt)
position = get_numpy(self.traj_gen.get_traj_pos())
@@ -158,27 +153,24 @@ class BlackBoxWrapper(gym.ObservationWrapper):
trajectory_length = len(position)
rewards = np.zeros(shape=(trajectory_length,))
if self.verbose >= 2:
actions = np.zeros(shape=(trajectory_length,) +
self.env.action_space.shape)
actions = np.zeros(shape=(trajectory_length,) + self.env.action_space.shape)
observations = np.zeros(shape=(trajectory_length,) + self.env.observation_space.shape,
dtype=self.env.observation_space.dtype)
infos = dict()
terminated, truncated = False, False
done = False
if not traj_is_valid:
obs, trajectory_return, terminated, truncated, infos = self.env.invalid_traj_callback(action, position, velocity,
self.return_context_observation, self.tau_bound, self.delay_bound)
return self.observation(obs), trajectory_return, terminated, truncated, infos
obs, trajectory_return, done, infos = self.env.invalid_traj_callback(action, position, velocity,
self.return_context_observation,
self.tau_bound, self.delay_bound)
return self.observation(obs), trajectory_return, done, infos
self.plan_steps += 1
for t, (pos, vel) in enumerate(zip(position, velocity)):
step_action = self.tracking_controller.get_action(
pos, vel, self.env.get_wrapper_attr('current_pos'), self.env.get_wrapper_attr('current_vel'))
c_action = np.clip(
step_action, self.env.action_space.low, self.env.action_space.high)
obs, c_reward, terminated, truncated, info = self.env.step(
c_action)
step_action = self.tracking_controller.get_action(pos, vel, self.current_pos, self.current_vel)
c_action = np.clip(step_action, self.env.action_space.low, self.env.action_space.high)
obs, c_reward, done, info = self.env.step(c_action)
rewards[t] = c_reward
if self.verbose >= 2:
@@ -190,11 +182,12 @@ class BlackBoxWrapper(gym.ObservationWrapper):
elems[t] = v
infos[k] = elems
if self.do_render:
self.env.render()
if self.render_kwargs:
self.env.render(**self.render_kwargs)
if terminated or truncated or (self.replanning_schedule(self.env.get_wrapper_attr('current_pos'), self.env.get_wrapper_attr('current_vel'), obs, c_action, t + 1 + self.current_traj_steps) and self.plan_steps < self.max_planning_times):
if done or (self.replanning_schedule(self.current_pos, self.current_vel, obs, c_action,
t + 1 + self.current_traj_steps)
and self.plan_steps < self.max_planning_times):
if self.condition_on_desired:
self.condition_pos = pos
@@ -214,16 +207,17 @@ class BlackBoxWrapper(gym.ObservationWrapper):
infos['trajectory_length'] = t + 1
trajectory_return = self.reward_aggregation(rewards[:t + 1])
return self.observation(obs), trajectory_return, terminated, truncated, infos
return self.observation(obs), trajectory_return, done, infos
def render(self):
self.do_render = True
def render(self, **kwargs):
"""Only set render options here, such that they can be used during the rollout.
This only needs to be called once"""
self.render_kwargs = kwargs
def reset(self, *, seed: Optional[int] = None, options: Optional[Dict[str, Any]] = None) \
-> Tuple[ObsType, Dict[str, Any]]:
def reset(self, *, seed: Optional[int] = None, return_info: bool = False, options: Optional[dict] = None):
self.current_traj_steps = 0
self.plan_steps = 0
self.traj_gen.reset()
self.condition_pos = None
self.condition_vel = None
return super(BlackBoxWrapper, self).reset(seed=seed, options=options)
return super(BlackBoxWrapper, self).reset()
@@ -11,11 +11,11 @@ def get_controller(controller_type: str, **kwargs):
if controller_type == "motor":
return PDController(**kwargs)
elif controller_type == "velocity":
return VelController(**kwargs)
return VelController()
elif controller_type == "position":
return PosController(**kwargs)
return PosController()
elif controller_type == "metaworld":
return MetaWorldController(**kwargs)
return MetaWorldController()
else:
raise ValueError(f"Specified controller type {controller_type} not supported, "
f"please choose one of {ALL_TYPES}.")
+3 -4
View File
@@ -1,6 +1,6 @@
from typing import Union, Tuple
import gymnasium as gym
import gym
import numpy as np
from mp_pytorch.mp.mp_interfaces import MPInterface
@@ -114,8 +114,7 @@ class RawInterfaceWrapper(gym.Wrapper):
Returns:
obs: artificial observation if the trajectory is invalid, by default a zero vector
reward: artificial reward if the trajectory is invalid, by default 0
terminated: artificial terminated if the trajectory is invalid, by default True
truncated: artificial truncated if the trajectory is invalid, by default False
done: artificial done if the trajectory is invalid, by default True
info: artificial info if the trajectory is invalid, by default empty dict
"""
return np.zeros(1), 0, True, False, {}
return np.zeros(1), 0, True, {}
+10 -10
View File
@@ -1,7 +1,7 @@
# DeepMind Control (DMC) Wrappers
These are the Environment Wrappers for selected
[DeepMind Control](https://deepmind.com/research/publications/2020/dm-control-Software-and-Tasks-for-Continuous-Control)
These are the Environment Wrappers for selected
[DeepMind Control](https://deepmind.com/research/publications/2020/dm-control-Software-and-Tasks-for-Continuous-Control)
environments in order to use our Motion Primitive gym interface with them.
## MP Environments
@@ -9,11 +9,11 @@ environments in order to use our Motion Primitive gym interface with them.
[//]: <> (These environments are wrapped-versions of their Deep Mind Control Suite &#40;DMC&#41; counterparts. Given most task can be)
[//]: <> (solved in shorter horizon lengths than the original 1000 steps, we often shorten the episodes for those task.)
| Name | Description | Trajectory Horizon | Action Dimension | Context Dimension |
| ---------------------------------------- | ------------------------------------------------------------------------------ | ------------------ | ---------------- | ----------------- |
| `dm_control_ProDMP/ball_in_cup-catch-v0` | A ProMP wrapped version of the "catch" task for the "ball_in_cup" environment. | 1000 | 10 | 2 |
| `dm_control_DMP/ball_in_cup-catch-v0` | A DMP wrapped version of the "catch" task for the "ball_in_cup" environment. | 1000 | 10 | 2 |
| `dm_control_ProDMP/reacher-easy-v0` | A ProMP wrapped version of the "easy" task for the "reacher" environment. | 1000 | 10 | 4 |
| `dm_control_DMP/reacher-easy-v0` | A DMP wrapped version of the "easy" task for the "reacher" environment. | 1000 | 10 | 4 |
| `dm_control_ProDMP/reacher-hard-v0` | A ProMP wrapped version of the "hard" task for the "reacher" environment. | 1000 | 10 | 4 |
| `dm_control_DMP/reacher-hard-v0` | A DMP wrapped version of the "hard" task for the "reacher" environment. | 1000 | 10 | 4 |
|Name| Description|Trajectory Horizon|Action Dimension|Context Dimension
|---|---|---|---|---|
|`dmc_ball_in_cup-catch_promp-v0`| A ProMP wrapped version of the "catch" task for the "ball_in_cup" environment. | 1000 | 10 | 2
|`dmc_ball_in_cup-catch_dmp-v0`| A DMP wrapped version of the "catch" task for the "ball_in_cup" environment. | 1000| 10 | 2
|`dmc_reacher-easy_promp-v0`| A ProMP wrapped version of the "easy" task for the "reacher" environment. | 1000 | 10 | 4
|`dmc_reacher-easy_dmp-v0`| A DMP wrapped version of the "easy" task for the "reacher" environment. | 1000| 10 | 4
|`dmc_reacher-hard_promp-v0`| A ProMP wrapped version of the "hard" task for the "reacher" environment.| 1000 | 10 | 4
|`dmc_reacher-hard_dmp-v0`| A DMP wrapped version of the "hard" task for the "reacher" environment. | 1000 | 10 | 4
+219 -35
View File
@@ -1,61 +1,245 @@
from gym.envs.registration import register
from copy import deepcopy
from gymnasium.wrappers import FlattenObservation
from gymnasium.envs.registration import register
from ..envs.registry import register
from . import manipulation, suite
ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS = {"DMP": [], "ProMP": [], "ProDMP": []}
DEFAULT_BB_DICT_ProMP = {
"name": 'EnvName',
"wrappers": [],
"trajectory_generator_kwargs": {
'trajectory_generator_type': 'promp'
},
"phase_generator_kwargs": {
'phase_generator_type': 'linear'
},
"controller_kwargs": {
'controller_type': 'motor',
"p_gains": 50.,
"d_gains": 1.,
},
"basis_generator_kwargs": {
'basis_generator_type': 'zero_rbf',
'num_basis': 5,
'num_basis_zero_start': 1
}
}
DEFAULT_BB_DICT_DMP = {
"name": 'EnvName',
"wrappers": [],
"trajectory_generator_kwargs": {
'trajectory_generator_type': 'dmp'
},
"phase_generator_kwargs": {
'phase_generator_type': 'exp'
},
"controller_kwargs": {
'controller_type': 'motor',
"p_gains": 50.,
"d_gains": 1.,
},
"basis_generator_kwargs": {
'basis_generator_type': 'rbf',
'num_basis': 5
}
}
# DeepMind Control Suite (DMC)
kwargs_dict_bic_dmp = deepcopy(DEFAULT_BB_DICT_DMP)
kwargs_dict_bic_dmp['name'] = f"dmc:ball_in_cup-catch"
kwargs_dict_bic_dmp['wrappers'].append(suite.ball_in_cup.MPWrapper)
# bandwidth_factor=2
kwargs_dict_bic_dmp['phase_generator_kwargs']['alpha_phase'] = 2
kwargs_dict_bic_dmp['trajectory_generator_kwargs']['weight_scale'] = 10 # TODO: weight scale 1, but goal scale 0.1
register(
id=f"dm_control/ball_in_cup-catch-v0",
register_step_based=False,
mp_wrapper=suite.ball_in_cup.MPWrapper,
add_mp_types=['DMP', 'ProMP'],
id=f'dmc_ball_in_cup-catch_dmp-v0',
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_bic_dmp
)
ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS["DMP"].append("dmc_ball_in_cup-catch_dmp-v0")
kwargs_dict_bic_promp = deepcopy(DEFAULT_BB_DICT_ProMP)
kwargs_dict_bic_promp['name'] = f"dmc:ball_in_cup-catch"
kwargs_dict_bic_promp['wrappers'].append(suite.ball_in_cup.MPWrapper)
register(
id=f"dm_control/reacher-easy-v0",
register_step_based=False,
mp_wrapper=suite.reacher.MPWrapper,
add_mp_types=['DMP', 'ProMP'],
id=f'dmc_ball_in_cup-catch_promp-v0',
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_bic_promp
)
ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append("dmc_ball_in_cup-catch_promp-v0")
kwargs_dict_reacher_easy_dmp = deepcopy(DEFAULT_BB_DICT_DMP)
kwargs_dict_reacher_easy_dmp['name'] = f"dmc:reacher-easy"
kwargs_dict_reacher_easy_dmp['wrappers'].append(suite.reacher.MPWrapper)
# bandwidth_factor=2
kwargs_dict_reacher_easy_dmp['phase_generator_kwargs']['alpha_phase'] = 2
# TODO: weight scale 50, but goal scale 0.1
kwargs_dict_reacher_easy_dmp['trajectory_generator_kwargs']['weight_scale'] = 500
register(
id=f"dm_control/reacher-hard-v0",
register_step_based=False,
mp_wrapper=suite.reacher.MPWrapper,
add_mp_types=['DMP', 'ProMP'],
id=f'dmc_reacher-easy_dmp-v0',
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_bic_dmp
)
ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS["DMP"].append("dmc_reacher-easy_dmp-v0")
kwargs_dict_reacher_easy_promp = deepcopy(DEFAULT_BB_DICT_ProMP)
kwargs_dict_reacher_easy_promp['name'] = f"dmc:reacher-easy"
kwargs_dict_reacher_easy_promp['wrappers'].append(suite.reacher.MPWrapper)
kwargs_dict_reacher_easy_promp['trajectory_generator_kwargs']['weight_scale'] = 0.2
register(
id=f'dmc_reacher-easy_promp-v0',
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_reacher_easy_promp
)
ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append("dmc_reacher-easy_promp-v0")
kwargs_dict_reacher_hard_dmp = deepcopy(DEFAULT_BB_DICT_DMP)
kwargs_dict_reacher_hard_dmp['name'] = f"dmc:reacher-hard"
kwargs_dict_reacher_hard_dmp['wrappers'].append(suite.reacher.MPWrapper)
# bandwidth_factor = 2
kwargs_dict_reacher_hard_dmp['phase_generator_kwargs']['alpha_phase'] = 2
# TODO: weight scale 50, but goal scale 0.1
kwargs_dict_reacher_hard_dmp['trajectory_generator_kwargs']['weight_scale'] = 500
register(
id=f'dmc_reacher-hard_dmp-v0',
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_reacher_hard_dmp
)
ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS["DMP"].append("dmc_reacher-hard_dmp-v0")
kwargs_dict_reacher_hard_promp = deepcopy(DEFAULT_BB_DICT_ProMP)
kwargs_dict_reacher_hard_promp['name'] = f"dmc:reacher-hard"
kwargs_dict_reacher_hard_promp['wrappers'].append(suite.reacher.MPWrapper)
kwargs_dict_reacher_hard_promp['trajectory_generator_kwargs']['weight_scale'] = 0.2
register(
id=f'dmc_reacher-hard_promp-v0',
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_reacher_hard_promp
)
ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append("dmc_reacher-hard_promp-v0")
_dmc_cartpole_tasks = ["balance", "balance_sparse", "swingup", "swingup_sparse"]
for _task in _dmc_cartpole_tasks:
_env_id = f'dmc_cartpole-{_task}_dmp-v0'
kwargs_dict_cartpole_dmp = deepcopy(DEFAULT_BB_DICT_DMP)
kwargs_dict_cartpole_dmp['name'] = f"dmc:cartpole-{_task}"
kwargs_dict_cartpole_dmp['wrappers'].append(suite.cartpole.MPWrapper)
# bandwidth_factor = 2
kwargs_dict_cartpole_dmp['phase_generator_kwargs']['alpha_phase'] = 2
# TODO: weight scale 50, but goal scale 0.1
kwargs_dict_cartpole_dmp['trajectory_generator_kwargs']['weight_scale'] = 500
kwargs_dict_cartpole_dmp['controller_kwargs']['p_gains'] = 10
kwargs_dict_cartpole_dmp['controller_kwargs']['d_gains'] = 10
register(
id=f'dm_control/cartpole-{_task}-v0',
register_step_based=False,
mp_wrapper=suite.cartpole.MPWrapper,
add_mp_types=['DMP', 'ProMP'],
id=_env_id,
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_cartpole_dmp
)
ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS["DMP"].append(_env_id)
register(
id=f"dm_control/cartpole-two_poles-v0",
register_step_based=False,
mp_wrapper=suite.cartpole.TwoPolesMPWrapper,
add_mp_types=['DMP', 'ProMP'],
)
_env_id = f'dmc_cartpole-{_task}_promp-v0'
kwargs_dict_cartpole_promp = deepcopy(DEFAULT_BB_DICT_ProMP)
kwargs_dict_cartpole_promp['name'] = f"dmc:cartpole-{_task}"
kwargs_dict_cartpole_promp['wrappers'].append(suite.cartpole.MPWrapper)
kwargs_dict_cartpole_promp['controller_kwargs']['p_gains'] = 10
kwargs_dict_cartpole_promp['controller_kwargs']['d_gains'] = 10
kwargs_dict_cartpole_promp['trajectory_generator_kwargs']['weight_scale'] = 0.2
register(
id=_env_id,
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_cartpole_promp
)
ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append(_env_id)
kwargs_dict_cartpole2poles_dmp = deepcopy(DEFAULT_BB_DICT_DMP)
kwargs_dict_cartpole2poles_dmp['name'] = f"dmc:cartpole-two_poles"
kwargs_dict_cartpole2poles_dmp['wrappers'].append(suite.cartpole.TwoPolesMPWrapper)
# bandwidth_factor = 2
kwargs_dict_cartpole2poles_dmp['phase_generator_kwargs']['alpha_phase'] = 2
# TODO: weight scale 50, but goal scale 0.1
kwargs_dict_cartpole2poles_dmp['trajectory_generator_kwargs']['weight_scale'] = 500
kwargs_dict_cartpole2poles_dmp['controller_kwargs']['p_gains'] = 10
kwargs_dict_cartpole2poles_dmp['controller_kwargs']['d_gains'] = 10
_env_id = f'dmc_cartpole-two_poles_dmp-v0'
register(
id=f"dm_control/cartpole-three_poles-v0",
register_step_based=False,
mp_wrapper=suite.cartpole.ThreePolesMPWrapper,
add_mp_types=['DMP', 'ProMP'],
id=_env_id,
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_cartpole2poles_dmp
)
ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS["DMP"].append(_env_id)
kwargs_dict_cartpole2poles_promp = deepcopy(DEFAULT_BB_DICT_ProMP)
kwargs_dict_cartpole2poles_promp['name'] = f"dmc:cartpole-two_poles"
kwargs_dict_cartpole2poles_promp['wrappers'].append(suite.cartpole.TwoPolesMPWrapper)
kwargs_dict_cartpole2poles_promp['controller_kwargs']['p_gains'] = 10
kwargs_dict_cartpole2poles_promp['controller_kwargs']['d_gains'] = 10
kwargs_dict_cartpole2poles_promp['trajectory_generator_kwargs']['weight_scale'] = 0.2
_env_id = f'dmc_cartpole-two_poles_promp-v0'
register(
id=_env_id,
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_cartpole2poles_promp
)
ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append(_env_id)
kwargs_dict_cartpole3poles_dmp = deepcopy(DEFAULT_BB_DICT_DMP)
kwargs_dict_cartpole3poles_dmp['name'] = f"dmc:cartpole-three_poles"
kwargs_dict_cartpole3poles_dmp['wrappers'].append(suite.cartpole.ThreePolesMPWrapper)
# bandwidth_factor = 2
kwargs_dict_cartpole3poles_dmp['phase_generator_kwargs']['alpha_phase'] = 2
# TODO: weight scale 50, but goal scale 0.1
kwargs_dict_cartpole3poles_dmp['trajectory_generator_kwargs']['weight_scale'] = 500
kwargs_dict_cartpole3poles_dmp['controller_kwargs']['p_gains'] = 10
kwargs_dict_cartpole3poles_dmp['controller_kwargs']['d_gains'] = 10
_env_id = f'dmc_cartpole-three_poles_dmp-v0'
register(
id=_env_id,
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_cartpole3poles_dmp
)
ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS["DMP"].append(_env_id)
kwargs_dict_cartpole3poles_promp = deepcopy(DEFAULT_BB_DICT_ProMP)
kwargs_dict_cartpole3poles_promp['name'] = f"dmc:cartpole-three_poles"
kwargs_dict_cartpole3poles_promp['wrappers'].append(suite.cartpole.ThreePolesMPWrapper)
kwargs_dict_cartpole3poles_promp['controller_kwargs']['p_gains'] = 10
kwargs_dict_cartpole3poles_promp['controller_kwargs']['d_gains'] = 10
kwargs_dict_cartpole3poles_promp['trajectory_generator_kwargs']['weight_scale'] = 0.2
_env_id = f'dmc_cartpole-three_poles_promp-v0'
register(
id=_env_id,
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_cartpole3poles_promp
)
ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append(_env_id)
# DeepMind Manipulation
kwargs_dict_mani_reach_site_features_dmp = deepcopy(DEFAULT_BB_DICT_DMP)
kwargs_dict_mani_reach_site_features_dmp['name'] = f"dmc:manipulation-reach_site_features"
kwargs_dict_mani_reach_site_features_dmp['wrappers'].append(manipulation.reach_site.MPWrapper)
kwargs_dict_mani_reach_site_features_dmp['phase_generator_kwargs']['alpha_phase'] = 2
# TODO: weight scale 50, but goal scale 0.1
kwargs_dict_mani_reach_site_features_dmp['trajectory_generator_kwargs']['weight_scale'] = 500
kwargs_dict_mani_reach_site_features_dmp['controller_kwargs']['controller_type'] = 'velocity'
register(
id=f"dm_control/reach_site_features-v0",
register_step_based=False,
mp_wrapper=manipulation.reach_site.MPWrapper,
add_mp_types=['DMP', 'ProMP'],
id=f'dmc_manipulation-reach_site_dmp-v0',
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_mani_reach_site_features_dmp
)
ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS["DMP"].append("dmc_manipulation-reach_site_dmp-v0")
kwargs_dict_mani_reach_site_features_promp = deepcopy(DEFAULT_BB_DICT_ProMP)
kwargs_dict_mani_reach_site_features_promp['name'] = f"dmc:manipulation-reach_site_features"
kwargs_dict_mani_reach_site_features_promp['wrappers'].append(manipulation.reach_site.MPWrapper)
kwargs_dict_mani_reach_site_features_promp['trajectory_generator_kwargs']['weight_scale'] = 0.2
kwargs_dict_mani_reach_site_features_promp['controller_kwargs']['controller_type'] = 'velocity'
register(
id=f'dmc_manipulation-reach_site_promp-v0',
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_mani_reach_site_features_promp
)
ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append("dmc_manipulation-reach_site_promp-v0")
+186
View File
@@ -0,0 +1,186 @@
# Adopted from: https://github.com/denisyarats/dmc2gym/blob/master/dmc2gym/wrappers.py
# License: MIT
# Copyright (c) 2020 Denis Yarats
import collections
from collections.abc import MutableMapping
from typing import Any, Dict, Tuple, Optional, Union, Callable
import gym
import numpy as np
from dm_control import composer
from dm_control.rl import control
from dm_env import specs
from gym import spaces
from gym.core import ObsType
def _spec_to_box(spec):
def extract_min_max(s):
assert s.dtype == np.float64 or s.dtype == np.float32, \
f"Only float64 and float32 types are allowed, instead {s.dtype} was found"
dim = int(np.prod(s.shape))
if type(s) == specs.Array:
bound = np.inf * np.ones(dim, dtype=s.dtype)
return -bound, bound
elif type(s) == specs.BoundedArray:
zeros = np.zeros(dim, dtype=s.dtype)
return s.minimum + zeros, s.maximum + zeros
mins, maxs = [], []
for s in spec:
mn, mx = extract_min_max(s)
mins.append(mn)
maxs.append(mx)
low = np.concatenate(mins, axis=0)
high = np.concatenate(maxs, axis=0)
assert low.shape == high.shape
return spaces.Box(low, high, dtype=s.dtype)
def _flatten_obs(obs: MutableMapping):
"""
Flattens an observation of type MutableMapping, e.g. a dict to a 1D array.
Args:
obs: observation to flatten
Returns: 1D array of observation
"""
if not isinstance(obs, MutableMapping):
raise ValueError(f'Requires dict-like observations structure. {type(obs)} found.')
# Keep key order consistent for non OrderedDicts
keys = obs.keys() if isinstance(obs, collections.OrderedDict) else sorted(obs.keys())
obs_vals = [np.array([obs[key]]) if np.isscalar(obs[key]) else obs[key].ravel() for key in keys]
return np.concatenate(obs_vals)
class DMCWrapper(gym.Env):
def __init__(self,
env: Callable[[], Union[composer.Environment, control.Environment]],
):
# TODO: Currently this is required to be a function because dmc does not allow to copy composers environments
self._env = env()
# action and observation space
self._action_space = _spec_to_box([self._env.action_spec()])
self._observation_space = _spec_to_box(self._env.observation_spec().values())
self._window = None
self.id = 'dmc'
def __getattr__(self, item):
"""Propagate only non-existent properties to wrapped env."""
if item.startswith('_'):
raise AttributeError("attempted to get missing private attribute '{}'".format(item))
if item in self.__dict__:
return getattr(self, item)
return getattr(self._env, item)
def _get_obs(self, time_step):
obs = _flatten_obs(time_step.observation).astype(self.observation_space.dtype)
return obs
@property
def observation_space(self):
return self._observation_space
@property
def action_space(self):
return self._action_space
@property
def dt(self):
return self._env.control_timestep()
def seed(self, seed=None):
self._action_space.seed(seed)
self._observation_space.seed(seed)
def step(self, action) -> Tuple[np.ndarray, float, bool, Dict[str, Any]]:
assert self._action_space.contains(action)
extra = {'internal_state': self._env.physics.get_state().copy()}
time_step = self._env.step(action)
reward = time_step.reward or 0.
done = time_step.last()
obs = self._get_obs(time_step)
extra['discount'] = time_step.discount
return obs, reward, done, extra
def reset(self, *, seed: Optional[int] = None, return_info: bool = False,
options: Optional[dict] = None, ) -> Union[ObsType, Tuple[ObsType, dict]]:
time_step = self._env.reset()
obs = self._get_obs(time_step)
return obs
def render(self, mode='rgb_array', height=240, width=320, camera_id=-1, overlays=(), depth=False,
segmentation=False, scene_option=None, render_flag_overrides=None):
# assert mode == 'rgb_array', 'only support rgb_array mode, given %s' % mode
if mode == "rgb_array":
return self._env.physics.render(height=height, width=width, camera_id=camera_id, overlays=overlays,
depth=depth, segmentation=segmentation, scene_option=scene_option,
render_flag_overrides=render_flag_overrides)
# Render max available buffer size. Larger is only possible by altering the XML.
img = self._env.physics.render(height=self._env.physics.model.vis.global_.offheight,
width=self._env.physics.model.vis.global_.offwidth,
camera_id=camera_id, overlays=overlays, depth=depth, segmentation=segmentation,
scene_option=scene_option, render_flag_overrides=render_flag_overrides)
if depth:
img = np.dstack([img.astype(np.uint8)] * 3)
if mode == 'human':
try:
import cv2
if self._window is None:
self._window = cv2.namedWindow(self.id, cv2.WINDOW_AUTOSIZE)
cv2.imshow(self.id, img[..., ::-1]) # Image in BGR
cv2.waitKey(1)
except ImportError:
raise gym.error.DependencyNotInstalled("Rendering requires opencv. Run `pip install opencv-python`")
# PYGAME seems to destroy some global rendering configs from the physics render
# except ImportError:
# import pygame
# img_copy = img.copy().transpose((1, 0, 2))
# if self._window is None:
# pygame.init()
# pygame.display.init()
# self._window = pygame.display.set_mode(img_copy.shape[:2])
# self.clock = pygame.time.Clock()
#
# surf = pygame.surfarray.make_surface(img_copy)
# self._window.blit(surf, (0, 0))
# pygame.event.pump()
# self.clock.tick(30)
# pygame.display.flip()
def close(self):
super().close()
if self._window is not None:
try:
import cv2
cv2.destroyWindow(self.id)
except ImportError:
import pygame
pygame.display.quit()
pygame.quit()
@property
def reward_range(self) -> Tuple[float, float]:
reward_spec = self._env.reward_spec()
if isinstance(reward_spec, specs.BoundedArray):
return reward_spec.minimum, reward_spec.maximum
return -float('inf'), float('inf')
@property
def metadata(self):
return {'render.modes': ['human', 'rgb_array'],
'video.frames_per_second': round(1.0 / self._env.control_timestep())}
@@ -6,28 +6,6 @@ from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
class MPWrapper(RawInterfaceWrapper):
mp_config = {
'ProMP': {
'controller_kwargs': {
'p_gains': 50.0,
},
'trajectory_generator_kwargs': {
'weights_scale': 0.2,
},
},
'DMP': {
'controller_kwargs': {
'p_gains': 50.0,
},
'phase_generator': {
'alpha_phase': 2,
},
'trajectory_generator_kwargs': {
'weights_scale': 500,
},
},
'ProDMP': {},
}
@property
def context_mask(self) -> np.ndarray:
@@ -57,4 +35,4 @@ class MPWrapper(RawInterfaceWrapper):
@property
def dt(self) -> Union[float, int]:
return self.env.control_timestep()
return self.env.dt
+1 -20
View File
@@ -6,25 +6,6 @@ from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
class MPWrapper(RawInterfaceWrapper):
mp_config = {
'ProMP': {
'controller_kwargs': {
'p_gains': 50.0,
},
},
'DMP': {
'controller_kwargs': {
'p_gains': 50.0,
},
'phase_generator': {
'alpha_phase': 2,
},
'trajectory_generator_kwargs': {
'weights_scale': 10
},
},
'ProDMP': {},
}
@property
def context_mask(self) -> np.ndarray:
@@ -50,4 +31,4 @@ class MPWrapper(RawInterfaceWrapper):
@property
def dt(self) -> Union[float, int]:
return self.env.control_timestep()
return self.env.dt
+1 -25
View File
@@ -6,30 +6,6 @@ from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
class MPWrapper(RawInterfaceWrapper):
mp_config = {
'ProMP': {
'controller_kwargs': {
'p_gains': 10,
'd_gains': 10,
},
'trajectory_generator_kwargs': {
'weights_scale': 0.2,
},
},
'DMP': {
'controller_kwargs': {
'p_gains': 10,
'd_gains': 10,
},
'phase_generator': {
'alpha_phase': 2,
},
'trajectory_generator_kwargs': {
'weights_scale': 500,
},
},
'ProDMP': {},
}
def __init__(self, env, n_poles: int = 1):
self.n_poles = n_poles
@@ -59,7 +35,7 @@ class MPWrapper(RawInterfaceWrapper):
@property
def dt(self) -> Union[float, int]:
return self.env.control_timestep()
return self.env.dt
class TwoPolesMPWrapper(MPWrapper):
+1 -25
View File
@@ -6,30 +6,6 @@ from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
class MPWrapper(RawInterfaceWrapper):
mp_config = {
'ProMP': {
'controller_kwargs': {
'p_gains': 50.0,
'd_gains': 1.0,
},
'trajectory_generator_kwargs': {
'weights_scale': 0.2,
},
},
'DMP': {
'controller_kwargs': {
'p_gains': 50.0,
'd_gains': 1.0,
},
'phase_generator': {
'alpha_phase': 2,
},
'trajectory_generator_kwargs': {
'weights_scale': 500,
},
},
'ProDMP': {},
}
@property
def context_mask(self) -> np.ndarray:
@@ -54,4 +30,4 @@ class MPWrapper(RawInterfaceWrapper):
@property
def dt(self) -> Union[float, int]:
return self.env.control_timestep()
return self.env.dt
+731 -132
View File
File diff suppressed because it is too large Load Diff
+12 -14
View File
@@ -1,20 +1,18 @@
### Classic Control
## Step-based Environments
| Name | Description | Horizon | Action Dimension | Observation Dimension |
| ---------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------- | ---------------- | --------------------- |
| `fancy/SimpleReacher-v0` | Simple reaching task (2 links) without any physics simulation. Provides no reward until 150 time steps. This allows the agent to explore the space, but requires precise actions towards the end of the trajectory. | 200 | 2 | 9 |
| `fancy/LongSimpleReacher-v0` | Simple reaching task (5 links) without any physics simulation. Provides no reward until 150 time steps. This allows the agent to explore the space, but requires precise actions towards the end of the trajectory. | 200 | 5 | 18 |
| `fancy/ViaPointReacher-v0` | Simple reaching task leveraging a via point, which supports self collision detection. Provides a reward only at 100 and 199 for reaching the viapoint and goal point, respectively. | 200 | 5 | 18 |
| `fancy/HoleReacher-v0` | 5 link reaching task where the end-effector needs to reach into a narrow hole without collding with itself or walls | 200 | 5 | 18 |
|Name| Description|Horizon|Action Dimension|Observation Dimension
|---|---|---|---|---|
|`SimpleReacher-v0`| Simple reaching task (2 links) without any physics simulation. Provides no reward until 150 time steps. This allows the agent to explore the space, but requires precise actions towards the end of the trajectory.| 200 | 2 | 9
|`LongSimpleReacher-v0`| Simple reaching task (5 links) without any physics simulation. Provides no reward until 150 time steps. This allows the agent to explore the space, but requires precise actions towards the end of the trajectory.| 200 | 5 | 18
|`ViaPointReacher-v0`| Simple reaching task leveraging a via point, which supports self collision detection. Provides a reward only at 100 and 199 for reaching the viapoint and goal point, respectively.| 200 | 5 | 18
|`HoleReacher-v0`| 5 link reaching task where the end-effector needs to reach into a narrow hole without collding with itself or walls | 200 | 5 | 18
## MP Environments
|Name| Description|Horizon|Action Dimension|Context Dimension
|---|---|---|---|---|
|`ViaPointReacherDMP-v0`| A DMP provides a trajectory for the `ViaPointReacher-v0` task. | 200 | 25
|`HoleReacherFixedGoalDMP-v0`| A DMP provides a trajectory for the `HoleReacher-v0` task with a fixed goal attractor. | 200 | 25
|`HoleReacherDMP-v0`| A DMP provides a trajectory for the `HoleReacher-v0` task. The goal attractor needs to be learned. | 200 | 30
| Name | Description | Horizon | Action Dimension | Context Dimension |
| ----------------------------------- | -------------------------------------------------------------------------------------------------------- | ------- | ---------------- | ----------------- |
| `fancy_DMP/ViaPointReacher-v0` | A DMP provides a trajectory for the `fancy/ViaPointReacher-v0` task. | 200 | 25 |
| `fancy_DMP/HoleReacherFixedGoal-v0` | A DMP provides a trajectory for the `fancy/HoleReacher-v0` task with a fixed goal attractor. | 200 | 25 |
| `fancy_DMP/HoleReacher-v0` | A DMP provides a trajectory for the `fancy/HoleReacher-v0` task. The goal attractor needs to be learned. | 200 | 30 |
[//]: |`fancy/HoleReacherProMPP-v0`|
[//]: |`HoleReacherProMPP-v0`|
@@ -1,10 +1,10 @@
from typing import Union, Tuple, Optional, Any, Dict
from typing import Union, Tuple, Optional
import gymnasium as gym
import gym
import numpy as np
from gymnasium import spaces
from gymnasium.core import ObsType
from gymnasium.utils import seeding
from gym import spaces
from gym.core import ObsType
from gym.utils import seeding
from fancy_gym.envs.classic_control.utils import intersect
@@ -14,14 +14,12 @@ class BaseReacherEnv(gym.Env):
Base class for all reaching environments.
"""
def __init__(self, n_links: int, random_start: bool = True, allow_self_collision: bool = False, render_mode: str = None):
def __init__(self, n_links: int, random_start: bool = True, allow_self_collision: bool = False):
super().__init__()
self.link_lengths = np.ones(n_links)
self.n_links = n_links
self._dt = 0.01
self.render_mode = render_mode
self.random_start = random_start
self.allow_self_collision = allow_self_collision
@@ -57,6 +55,7 @@ class BaseReacherEnv(gym.Env):
self.fig = None
self._steps = 0
self.seed()
@property
def dt(self) -> Union[float, int]:
@@ -70,15 +69,10 @@ class BaseReacherEnv(gym.Env):
def current_vel(self):
return self._angle_velocity.copy()
def reset(self, *, seed: Optional[int] = None, options: Optional[Dict[str, Any]] = None) \
-> Tuple[ObsType, Dict[str, Any]]:
def reset(self, *, seed: Optional[int] = None, return_info: bool = False,
options: Optional[dict] = None, ) -> Union[ObsType, Tuple[ObsType, dict]]:
# Sample only orientation of first link, i.e. the arm is always straight.
super(BaseReacherEnv, self).reset(seed=seed, options=options)
try:
random_start = options.get('random_start', self.random_start)
except AttributeError:
random_start = self.random_start
if random_start:
if self.random_start:
first_joint = self.np_random.uniform(np.pi / 4, 3 * np.pi / 4)
self._joint_angles = np.hstack([[first_joint], np.zeros(self.n_links - 1)])
self._start_pos = self._joint_angles.copy()
@@ -90,7 +84,7 @@ class BaseReacherEnv(gym.Env):
self._update_joints()
self._steps = 0
return self._get_obs().copy(), {}
return self._get_obs().copy()
def _update_joints(self):
"""
@@ -130,6 +124,10 @@ class BaseReacherEnv(gym.Env):
def _terminate(self, info) -> bool:
raise NotImplementedError
def seed(self, seed=None):
self.np_random, seed = seeding.np_random(seed)
return [seed]
def close(self):
super(BaseReacherEnv, self).close()
del self.fig
@@ -1,5 +1,5 @@
import numpy as np
from gymnasium import spaces
from gym import spaces
from fancy_gym.envs.classic_control.base_reacher.base_reacher import BaseReacherEnv
@@ -10,8 +10,8 @@ class BaseReacherDirectEnv(BaseReacherEnv):
"""
def __init__(self, n_links: int, random_start: bool = True,
allow_self_collision: bool = False, **kwargs):
super().__init__(n_links, random_start, allow_self_collision, **kwargs)
allow_self_collision: bool = False):
super().__init__(n_links, random_start, allow_self_collision)
self.max_vel = 2 * np.pi
action_bound = np.ones((self.n_links,)) * self.max_vel
@@ -32,7 +32,6 @@ class BaseReacherDirectEnv(BaseReacherEnv):
reward, info = self._get_reward(action)
self._steps += 1
terminated = self._terminate(info)
truncated = False
done = self._terminate(info)
return self._get_obs().copy(), reward, terminated, truncated, info
return self._get_obs().copy(), reward, done, info
@@ -1,5 +1,5 @@
import numpy as np
from gymnasium import spaces
from gym import spaces
from fancy_gym.envs.classic_control.base_reacher.base_reacher import BaseReacherEnv
@@ -10,8 +10,8 @@ class BaseReacherTorqueEnv(BaseReacherEnv):
"""
def __init__(self, n_links: int, random_start: bool = True,
allow_self_collision: bool = False, **kwargs):
super().__init__(n_links, random_start, allow_self_collision, **kwargs)
allow_self_collision: bool = False):
super().__init__(n_links, random_start, allow_self_collision)
self.max_torque = 1000
action_bound = np.ones((self.n_links,)) * self.max_torque
@@ -31,7 +31,6 @@ class BaseReacherTorqueEnv(BaseReacherEnv):
reward, info = self._get_reward(action)
self._steps += 1
terminated = False
truncated = False
done = False
return self._get_obs().copy(), reward, terminated, truncated, info
return self._get_obs().copy(), reward, done, info
@@ -1,25 +1,22 @@
from typing import Union, Optional, Tuple, Any, Dict
from typing import Union, Optional, Tuple
import gymnasium as gym
import gym
import matplotlib.pyplot as plt
import numpy as np
from gymnasium import spaces
from gymnasium.core import ObsType
from gym.core import ObsType
from matplotlib import patches
from fancy_gym.envs.classic_control.base_reacher.base_reacher_direct import BaseReacherDirectEnv
from . import MPWrapper
MAX_EPISODE_STEPS_HOLEREACHER = 200
class HoleReacherEnv(BaseReacherDirectEnv):
def __init__(self, n_links: int, hole_x: Union[None, float] = None, hole_depth: Union[None, float] = None,
hole_width: float = 1., random_start: bool = False, allow_self_collision: bool = False,
allow_wall_collision: bool = False, collision_penalty: float = 1000, rew_fct: str = "simple", **kwargs):
allow_wall_collision: bool = False, collision_penalty: float = 1000, rew_fct: str = "simple"):
super().__init__(n_links, random_start, allow_self_collision, **kwargs)
super().__init__(n_links, random_start, allow_self_collision)
# provided initial parameters
self.initial_x = hole_x # x-position of center of hole
@@ -43,7 +40,7 @@ class HoleReacherEnv(BaseReacherDirectEnv):
[np.inf] # env steps, because reward start after n steps TODO: Maybe
])
# self.action_space = gym.spaces.Box(low=-action_bound, high=action_bound, shape=action_bound.shape)
self.observation_space = spaces.Box(low=-state_bound, high=state_bound, shape=state_bound.shape)
self.observation_space = gym.spaces.Box(low=-state_bound, high=state_bound, shape=state_bound.shape)
if rew_fct == "simple":
from fancy_gym.envs.classic_control.hole_reacher.hr_simple_reward import HolereacherReward
@@ -57,18 +54,13 @@ class HoleReacherEnv(BaseReacherDirectEnv):
else:
raise ValueError("Unknown reward function {}".format(rew_fct))
def reset(self, *, seed: Optional[int] = None, options: Optional[Dict[str, Any]] = None) \
-> Tuple[ObsType, Dict[str, Any]]:
# initialize seed here as the random goal needs to be generated before the super reset()
gym.Env.reset(self, seed=seed, options=options)
def reset(self, *, seed: Optional[int] = None, return_info: bool = False,
options: Optional[dict] = None, ) -> Union[ObsType, Tuple[ObsType, dict]]:
self._generate_hole()
self._set_patches()
self.reward_function.reset()
# do not provide seed to avoid setting it twice
return super(HoleReacherEnv, self).reset(options=options)
return super().reset()
def _get_reward(self, action: np.ndarray) -> (float, dict):
return self.reward_function.get_reward(self)
@@ -168,7 +160,7 @@ class HoleReacherEnv(BaseReacherDirectEnv):
# all points that are above the hole
r, c = np.where((line_points[:, :, 0] > (self._tmp_x - self._tmp_width / 2)) & (
line_points[:, :, 0] < (self._tmp_x + self._tmp_width / 2)))
line_points[:, :, 0] < (self._tmp_x + self._tmp_width / 2)))
# check if any of those points are below surface
nr_line_points_below_surface_in_hole = np.sum(line_points[r, c, 1] < -self._tmp_depth)
@@ -178,9 +170,7 @@ class HoleReacherEnv(BaseReacherDirectEnv):
return False
def render(self, mode=None):
if mode==None:
mode = self.render_mode
def render(self, mode='human'):
if self.fig is None:
# Create base figure once on the beginning. Afterwards only update
plt.ion()
@@ -233,3 +223,16 @@ class HoleReacherEnv(BaseReacherDirectEnv):
self.fig.gca().add_patch(left_block)
self.fig.gca().add_patch(right_block)
self.fig.gca().add_patch(hole_floor)
if __name__ == "__main__":
env = HoleReacherEnv(5)
env.reset()
for i in range(10000):
ac = env.action_space.sample()
obs, rew, done, info = env.step(ac)
env.render()
if done:
env.reset()
@@ -7,30 +7,6 @@ from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
class MPWrapper(RawInterfaceWrapper):
mp_config = {
'ProMP': {
'controller_kwargs': {
'controller_type': 'velocity',
},
'trajectory_generator_kwargs': {
'weights_scale': 2,
},
},
'DMP': {
'controller_kwargs': {
'controller_type': 'velocity',
},
'trajectory_generator_kwargs': {
# TODO: Before it was weight scale 50 and goal scale 0.1. We now only have weight scale and thus set it to 500. Check
'weights_scale': 500,
},
'phase_generator_kwargs': {
'alpha_phase': 2.5,
},
},
'ProDMP': {},
}
@property
def context_mask(self):
return np.hstack([
@@ -7,28 +7,6 @@ from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
class MPWrapper(RawInterfaceWrapper):
mp_config = {
'ProMP': {
'controller_kwargs': {
'p_gains': 0.6,
'd_gains': 0.075,
},
},
'DMP': {
'controller_kwargs': {
'p_gains': 0.6,
'd_gains': 0.075,
},
'trajectory_generator_kwargs': {
'weights_scale': 50,
},
'phase_generator_kwargs': {
'alpha_phase': 2,
},
},
'ProDMP': {},
}
@property
def context_mask(self):
return np.hstack([
@@ -1,12 +1,11 @@
from typing import Iterable, Union, Optional, Tuple, Any, Dict
from typing import Iterable, Union, Optional, Tuple
import matplotlib.pyplot as plt
import numpy as np
from gymnasium import spaces
from gymnasium.core import ObsType
from gym import spaces
from gym.core import ObsType
from fancy_gym.envs.classic_control.base_reacher.base_reacher_torque import BaseReacherTorqueEnv
from . import MPWrapper
class SimpleReacherEnv(BaseReacherTorqueEnv):
@@ -17,8 +16,8 @@ class SimpleReacherEnv(BaseReacherTorqueEnv):
"""
def __init__(self, n_links: int, target: Union[None, Iterable] = None, random_start: bool = True,
allow_self_collision: bool = False, **kwargs):
super().__init__(n_links, random_start, allow_self_collision, **kwargs)
allow_self_collision: bool = False, ):
super().__init__(n_links, random_start, allow_self_collision)
# provided initial parameters
self.inital_target = target
@@ -43,15 +42,11 @@ class SimpleReacherEnv(BaseReacherTorqueEnv):
# def start_pos(self):
# return self._start_pos
def reset(self, *, seed: Optional[int] = None, options: Optional[Dict[str, Any]] = None) \
-> Tuple[ObsType, Dict[str, Any]]:
# Reset twice to ensure we return obs after generating goal and generating goal after executing seeded reset.
# (Env will not behave deterministic otherwise)
# Yes, there is probably a more elegant solution to this problem...
def reset(self, *, seed: Optional[int] = None, return_info: bool = False,
options: Optional[dict] = None, ) -> Union[ObsType, Tuple[ObsType, dict]]:
self._generate_goal()
super().reset(seed=seed, options=options)
self._generate_goal()
return super().reset(seed=seed, options=options)
return super().reset()
def _get_reward(self, action: np.ndarray):
diff = self.end_effector - self._goal
@@ -98,9 +93,7 @@ class SimpleReacherEnv(BaseReacherTorqueEnv):
def _check_collisions(self) -> bool:
return self._check_self_collision()
def render(self, mode=None): # pragma: no cover
if mode==None:
mode = self.render_mode
def render(self, mode='human'): # pragma: no cover
if self.fig is None:
# Create base figure once on the beginning. Afterwards only update
plt.ion()
@@ -134,3 +127,15 @@ class SimpleReacherEnv(BaseReacherTorqueEnv):
self.fig.canvas.draw()
self.fig.canvas.flush_events()
if __name__ == "__main__":
env = SimpleReacherEnv(5)
env.reset()
for i in range(200):
ac = env.action_space.sample()
obs, rew, done, info = env.step(ac)
env.render()
if done:
break
@@ -7,26 +7,6 @@ from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
class MPWrapper(RawInterfaceWrapper):
mp_config = {
'ProMP': {
'controller_kwargs': {
'controller_type': 'velocity',
},
},
'DMP': {
'controller_kwargs': {
'controller_type': 'velocity',
},
'trajectory_generator_kwargs': {
'weights_scale': 50,
},
'phase_generator_kwargs': {
'alpha_phase': 2,
},
},
'ProDMP': {},
}
@property
def context_mask(self):
return np.hstack([
@@ -1,21 +1,19 @@
from typing import Iterable, Union, Tuple, Optional, Any, Dict
from typing import Iterable, Union, Tuple, Optional
import gymnasium as gym
import gym
import matplotlib.pyplot as plt
import numpy as np
from gymnasium import spaces
from gymnasium.core import ObsType
from gym.core import ObsType
from fancy_gym.envs.classic_control.base_reacher.base_reacher_direct import BaseReacherDirectEnv
from . import MPWrapper
class ViaPointReacherEnv(BaseReacherDirectEnv):
def __init__(self, n_links, random_start: bool = False, via_target: Union[None, Iterable] = None,
target: Union[None, Iterable] = None, allow_self_collision=False, collision_penalty=1000, **kwargs):
target: Union[None, Iterable] = None, allow_self_collision=False, collision_penalty=1000):
super().__init__(n_links, random_start, allow_self_collision, **kwargs)
super().__init__(n_links, random_start, allow_self_collision)
# provided initial parameters
self.intitial_target = target # provided target value
@@ -36,21 +34,16 @@ class ViaPointReacherEnv(BaseReacherDirectEnv):
[np.inf] * 2, # x-y coordinates of target distance
[np.inf] # env steps, because reward start after n steps
])
self.observation_space = spaces.Box(low=-state_bound, high=state_bound, shape=state_bound.shape)
self.observation_space = gym.spaces.Box(low=-state_bound, high=state_bound, shape=state_bound.shape)
# @property
# def start_pos(self):
# return self._start_pos
def reset(self, *, seed: Optional[int] = None, options: Optional[Dict[str, Any]] = None) \
-> Tuple[ObsType, Dict[str, Any]]:
# Reset twice to ensure we return obs after generating goal and generating goal after executing seeded reset.
# (Env will not behave deterministic otherwise)
# Yes, there is probably a more elegant solution to this problem...
def reset(self, *, seed: Optional[int] = None, return_info: bool = False,
options: Optional[dict] = None, ) -> Union[ObsType, Tuple[ObsType, dict]]:
self._generate_goal()
super().reset(seed=seed, options=options)
self._generate_goal()
return super().reset(seed=seed, options=options)
return super().reset()
def _generate_goal(self):
# TODO: Maybe improve this later, this can yield quite a lot of invalid settings
@@ -123,9 +116,7 @@ class ViaPointReacherEnv(BaseReacherDirectEnv):
def _check_collisions(self) -> bool:
return self._check_self_collision()
def render(self, mode=None):
if mode==None:
mode = self.render_mode
def render(self, mode='human'):
goal_pos = self._goal.T
via_pos = self._via_point.T
@@ -192,3 +183,16 @@ class ViaPointReacherEnv(BaseReacherDirectEnv):
plt.plot(self._joints[:, 0], self._joints[:, 1], 'ro-', markerfacecolor='k')
plt.pause(0.01)
if __name__ == "__main__":
env = ViaPointReacherEnv(5)
env.reset()
for i in range(10000):
ac = env.action_space.sample()
obs, rew, done, info = env.step(ac)
env.render()
if done:
env.reset()
+12 -45
View File
@@ -1,48 +1,15 @@
# Custom Mujoco tasks
## Step-based Environments
| Name | Description | Horizon | Action Dimension | Observation Dimension |
| ------------------------------------------ | -------------------------------------------------------------------------------------------------- | ------- | ---------------- | --------------------- |
| `fancy/Reacher-v0` | Modified (5 links) gymnasiums's mujoco `Reacher-v2` (2 links) | 200 | 5 | 21 |
| `fancy/ReacherSparse-v0` | Same as `fancy/Reacher-v0`, but the distance penalty is only provided in the last time step. | 200 | 5 | 21 |
| `fancy/ReacherSparseBalanced-v0` | Same as `fancy/ReacherSparse-v0`, but the end-effector has to remain upright. | 200 | 5 | 21 |
| `fancy/LongReacher-v0` | Modified (7 links) gymnasiums's mujoco `Reacher-v2` (2 links) | 200 | 7 | 27 |
| `fancy/LongReacherSparse-v0` | Same as `fancy/LongReacher-v0`, but the distance penalty is only provided in the last time step. | 200 | 7 | 27 |
| `fancy/LongReacherSparseBalanced-v0` | Same as `fancy/LongReacherSparse-v0`, but the end-effector has to remain upright. | 200 | 7 | 27 |
| `fancy/Reacher5d-v0` | Reacher task with 5 links, based on Gymnasium's `gym.envs.mujoco.ReacherEnv` | 200 | 5 | 20 |
| `fancy/Reacher5dSparse-v0` | Sparse Reacher task with 5 links, based on Gymnasium's `gym.envs.mujoco.ReacherEnv` | 200 | 5 | 20 |
| `fancy/Reacher7d-v0` | Reacher task with 7 links, based on Gymnasium's `gym.envs.mujoco.ReacherEnv` | 200 | 7 | 22 |
| `fancy/Reacher7dSparse-v0` | Sparse Reacher task with 7 links, based on Gymnasium's `gym.envs.mujoco.ReacherEnv` | 200 | 7 | 22 |
| `fancy/HopperJumpSparse-v0` | Hopper Jump task with sparse rewards, based on Gymnasium's `gym.envs.mujoco.Hopper` | 250 | 3 | 15 / 16\* |
| `fancy/HopperJump-v0` | Hopper Jump task with continuous rewards, based on Gymnasium's `gym.envs.mujoco.Hopper` | 250 | 3 | 15 / 16\* |
| `fancy/AntJump-v0` | Ant Jump task, based on Gymnasium's `gym.envs.mujoco.Ant` | 200 | 8 | 119 |
| `fancy/HalfCheetahJump-v0` | HalfCheetah Jump task, based on Gymnasium's `gym.envs.mujoco.HalfCheetah` | 100 | 6 | 112 |
| `fancy/HopperJumpOnBox-v0` | Hopper Jump on Box task, based on Gymnasium's `gym.envs.mujoco.Hopper` | 250 | 4 | 16 / 100\* |
| `fancy/HopperThrow-v0` | Hopper Throw task, based on Gymnasium's `gym.envs.mujoco.Hopper` | 250 | 3 | 18 / 100\* |
| `fancy/HopperThrowInBasket-v0` | Hopper Throw in Basket task, based on Gymnasium's `gym.envs.mujoco.Hopper` | 250 | 3 | 18 / 100\* |
| `fancy/Walker2DJump-v0` | Walker 2D Jump task, based on Gymnasium's `gym.envs.mujoco.Walker2d` | 300 | 6 | 18 / 19\* |
| `fancy/BeerPong-v0` | Beer Pong task, based on a custom environment with multiple task variations | 300 | 3 | 29 |
| `fancy/BeerPongStepBased-v0` | Step-based Beer Pong task, based on a custom environment with episodic rewards | 300 | 3 | 29 |
| `fancy/BeerPongFixedRelease-v0` | Beer Pong with fixed release, based on a custom environment with episodic rewards | 300 | 3 | 29 |
| `fancy/BoxPushingDense-v0` | Custom Box-pushing task with dense rewards | 100 | 3 | 13 |
| `fancy/BoxPushingTemporalSparse-v0` | Custom Box-pushing task with temporally sparse rewards | 100 | 3 | 13 |
| `fancy/BoxPushingTemporalSpatialSparse-v0` | Custom Box-pushing task with temporally and spatially sparse rewards | 100 | 3 | 13 |
| `fancy/TableTennis2D-v0` | Table Tennis task with 2D context, based on a custom environment for table tennis | 350 | 7 | 19 |
| `fancy/TableTennis2DReplan-v0` | Table Tennis task with 2D context and replanning, based on a custom environment for table tennis | 350 | 7 | 19 |
| `fancy/TableTennis4D-v0` | Table Tennis task with 4D context, based on a custom environment for table tennis | 350 | 7 | 22 |
| `fancy/TableTennis4DReplan-v0` | Table Tennis task with 4D context and replanning, based on a custom environment for table tennis | 350 | 7 | 22 |
| `fancy/TableTennisWind-v0` | Table Tennis task with wind effects, based on a custom environment for table tennis | 350 | 7 | 19 |
| `fancy/TableTennisGoalSwitching-v0` | Table Tennis task with goal switching, based on a custom environment for table tennis | 350 | 7 | 19 |
| `fancy/TableTennisWindReplan-v0` | Table Tennis task with wind effects and replanning, based on a custom environment for table tennis | 350 | 7 | 19 |
\*Observation dimensions depend on configuration.
<!--
No longer used?
| Name | Description | Horizon | Action Dimension | Observation Dimension |
| --------------------------- | --------------------------------------------------------------------------------------------------- | ------- | ---------------- | --------------------- |
| `fancy/BallInACupSimple-v0` | Ball-in-a-cup task where a robot needs to catch a ball attached to a cup at its end-effector. | 4000 | 3 | wip |
| `fancy/BallInACup-v0` | Ball-in-a-cup task where a robot needs to catch a ball attached to a cup at its end-effector | 4000 | 7 | wip |
| `fancy/BallInACupGoal-v0` | Similar to `fancy/BallInACupSimple-v0` but the ball needs to be caught at a specified goal position | 4000 | 7 | wip |
-->
|Name| Description|Horizon|Action Dimension|Observation Dimension
|---|---|---|---|---|
|`ALRReacher-v0`|Modified (5 links) Mujoco gym's `Reacher-v2` (2 links)| 200 | 5 | 21
|`ALRReacherSparse-v0`|Same as `ALRReacher-v0`, but the distance penalty is only provided in the last time step.| 200 | 5 | 21
|`ALRReacherSparseBalanced-v0`|Same as `ALRReacherSparse-v0`, but the end-effector has to remain upright.| 200 | 5 | 21
|`ALRLongReacher-v0`|Modified (7 links) Mujoco gym's `Reacher-v2` (2 links)| 200 | 7 | 27
|`ALRLongReacherSparse-v0`|Same as `ALRLongReacher-v0`, but the distance penalty is only provided in the last time step.| 200 | 7 | 27
|`ALRLongReacherSparseBalanced-v0`|Same as `ALRLongReacherSparse-v0`, but the end-effector has to remain upright.| 200 | 7 | 27
|`ALRBallInACupSimple-v0`| Ball-in-a-cup task where a robot needs to catch a ball attached to a cup at its end-effector. | 4000 | 3 | wip
|`ALRBallInACup-v0`| Ball-in-a-cup task where a robot needs to catch a ball attached to a cup at its end-effector | 4000 | 7 | wip
|`ALRBallInACupGoal-v0`| Similar to `ALRBallInACupSimple-v0` but the ball needs to be caught at a specified goal position | 4000 | 7 | wip
+11 -84
View File
@@ -1,11 +1,8 @@
from typing import Tuple, Union, Optional, Any, Dict
from typing import Tuple, Union, Optional
import numpy as np
from gymnasium.core import ObsType
from gymnasium.envs.mujoco.ant_v4 import AntEnv, DEFAULT_CAMERA_CONFIG
from gymnasium import utils
from gymnasium.envs.mujoco import MujocoEnv
from gymnasium.spaces import Box
from gym.core import ObsType
from gym.envs.mujoco.ant_v4 import AntEnv
MAX_EPISODE_STEPS_ANTJUMP = 200
@@ -15,74 +12,8 @@ MAX_EPISODE_STEPS_ANTJUMP = 200
# to the same structure as the Hopper, where the angles are randomized (->contexts) and the agent should jump as heigh
# as possible, while landing at a specific target position
class AntEnvCustomXML(AntEnv):
def __init__(
self,
xml_file="ant.xml",
ctrl_cost_weight=0.5,
use_contact_forces=False,
contact_cost_weight=5e-4,
healthy_reward=1.0,
terminate_when_unhealthy=True,
healthy_z_range=(0.2, 1.0),
contact_force_range=(-1.0, 1.0),
reset_noise_scale=0.1,
exclude_current_positions_from_observation=True,
**kwargs,
):
utils.EzPickle.__init__(
self,
xml_file,
ctrl_cost_weight,
use_contact_forces,
contact_cost_weight,
healthy_reward,
terminate_when_unhealthy,
healthy_z_range,
contact_force_range,
reset_noise_scale,
exclude_current_positions_from_observation,
**kwargs,
)
self._ctrl_cost_weight = ctrl_cost_weight
self._contact_cost_weight = contact_cost_weight
self._healthy_reward = healthy_reward
self._terminate_when_unhealthy = terminate_when_unhealthy
self._healthy_z_range = healthy_z_range
self._contact_force_range = contact_force_range
self._reset_noise_scale = reset_noise_scale
self._use_contact_forces = use_contact_forces
self._exclude_current_positions_from_observation = (
exclude_current_positions_from_observation
)
obs_shape = 27 + 1
if not exclude_current_positions_from_observation:
obs_shape += 2
if use_contact_forces:
obs_shape += 84
observation_space = Box(
low=-np.inf, high=np.inf, shape=(obs_shape,), dtype=np.float64
)
MujocoEnv.__init__(
self,
xml_file,
5,
observation_space=observation_space,
default_camera_config=DEFAULT_CAMERA_CONFIG,
**kwargs,
)
class AntJumpEnv(AntEnvCustomXML):
class AntJumpEnv(AntEnv):
"""
Initialization changes to normal Ant:
- healthy_reward: 1.0 -> 0.01 -> 0.0 no healthy reward needed - Paul and Marc
@@ -101,7 +32,6 @@ class AntJumpEnv(AntEnvCustomXML):
contact_force_range=(-1.0, 1.0),
reset_noise_scale=0.1,
exclude_current_positions_from_observation=True,
**kwargs
):
self.current_step = 0
self.max_height = 0
@@ -114,7 +44,7 @@ class AntJumpEnv(AntEnvCustomXML):
healthy_z_range=healthy_z_range,
contact_force_range=contact_force_range,
reset_noise_scale=reset_noise_scale,
exclude_current_positions_from_observation=exclude_current_positions_from_observation, **kwargs)
exclude_current_positions_from_observation=exclude_current_positions_from_observation)
def step(self, action):
self.current_step += 1
@@ -131,10 +61,9 @@ class AntJumpEnv(AntEnvCustomXML):
costs = ctrl_cost + contact_cost
terminated = bool(
height < 0.3) # fall over -> is the 0.3 value from healthy_z_range? TODO change 0.3 to the value of healthy z angle
done = bool(height < 0.3) # fall over -> is the 0.3 value from healthy_z_range? TODO change 0.3 to the value of healthy z angle
if self.current_step == MAX_EPISODE_STEPS_ANTJUMP or terminated:
if self.current_step == MAX_EPISODE_STEPS_ANTJUMP or done:
# -10 for scaling the value of the distance between the max_height and the goal height; only used when context is enabled
# height_reward = -10 * (np.linalg.norm(self.max_height - self.goal))
height_reward = -10 * np.linalg.norm(self.max_height - self.goal)
@@ -151,21 +80,19 @@ class AntJumpEnv(AntEnvCustomXML):
'max_height': self.max_height,
'goal': self.goal
}
truncated = False
return obs, reward, terminated, truncated, info
return obs, reward, done, info
def _get_obs(self):
return np.append(super()._get_obs(), self.goal)
def reset(self, *, seed: Optional[int] = None, options: Optional[Dict[str, Any]] = None) \
-> Tuple[ObsType, Dict[str, Any]]:
def reset(self, *, seed: Optional[int] = None, return_info: bool = False,
options: Optional[dict] = None, ) -> Union[ObsType, Tuple[ObsType, dict]]:
self.current_step = 0
self.max_height = 0
# goal heights from 1.0 to 2.5; can be increased, but didnt work well with CMORE
ret = super().reset(seed=seed, options=options)
self.goal = self.np_random.uniform(1.0, 2.5, 1)
return ret
return super().reset()
# reset_model had to be implemented in every env to make it deterministic
def reset_model(self):
+18 -45
View File
@@ -1,13 +1,9 @@
import os
from typing import Optional, Any, Dict, Tuple
from typing import Optional
import numpy as np
from gymnasium import utils
from gymnasium.core import ObsType
from gymnasium.envs.mujoco import MujocoEnv
from gymnasium.spaces import Box
import mujoco
from gym import utils
from gym.envs.mujoco import MujocoEnv
MAX_EPISODE_STEPS_BEERPONG = 300
FIXED_RELEASE_STEP = 62 # empirically evaluated for frame_skip=2!
@@ -34,16 +30,7 @@ CUP_COLLISION_OBJ = ["cup_geom_table3", "cup_geom_table4", "cup_geom_table5", "c
class BeerPongEnv(MujocoEnv, utils.EzPickle):
metadata = {
"render_modes": [
"human",
"rgb_array",
"depth_array",
],
"render_fps": 100
}
def __init__(self, **kwargs):
def __init__(self):
self._steps = 0
# Small Context -> Easier. Todo: Should we do different versions?
# self.xml_path = os.path.join(os.path.dirname(os.path.abspath(__file__)), "assets", "beerpong_wo_cup.xml")
@@ -63,9 +50,9 @@ class BeerPongEnv(MujocoEnv, utils.EzPickle):
self.repeat_action = 2
# TODO: If accessing IDs is easier in the (new) official mujoco bindings, remove this
self.model = None
self.geom_id = lambda x: mujoco.mj_name2id(self.model,
mujoco.mjtObj.mjOBJ_GEOM,
x)
self.geom_id = lambda x: self._mujoco_bindings.mj_name2id(self.model,
self._mujoco_bindings.mjtObj.mjOBJ_GEOM,
x)
# for reward calculation
self.dists = []
@@ -78,17 +65,7 @@ class BeerPongEnv(MujocoEnv, utils.EzPickle):
self.ball_in_cup = False
self.dist_ground_cup = -1 # distance floor to cup if first floor contact
self.observation_space = Box(
low=-np.inf, high=np.inf, shape=(29,), dtype=np.float64
)
MujocoEnv.__init__(
self,
self.xml_path,
frame_skip=1,
observation_space=self.observation_space,
**kwargs
)
MujocoEnv.__init__(self, model_path=self.xml_path, frame_skip=1, mujoco_bindings="mujoco")
utils.EzPickle.__init__(self)
@property
@@ -99,8 +76,7 @@ class BeerPongEnv(MujocoEnv, utils.EzPickle):
def start_vel(self):
return self._start_vel
def reset(self, *, seed: Optional[int] = None, options: Optional[Dict[str, Any]] = None) \
-> Tuple[ObsType, Dict[str, Any]]:
def reset(self, *, seed: Optional[int] = None, return_info: bool = False, options: Optional[dict] = None):
self.dists = []
self.dists_final = []
self.action_costs = []
@@ -110,7 +86,7 @@ class BeerPongEnv(MujocoEnv, utils.EzPickle):
self.ball_cup_contact = False
self.ball_in_cup = False
self.dist_ground_cup = -1 # distance floor to cup if first floor contact
return super().reset(seed=seed, options=options)
return super().reset()
def reset_model(self):
init_pos_all = self.init_qpos.copy()
@@ -152,11 +128,11 @@ class BeerPongEnv(MujocoEnv, utils.EzPickle):
if not crash:
reward, reward_infos = self._get_reward(applied_action)
is_collided = reward_infos['is_collided'] # TODO: Remove if self collision does not make a difference
terminated = is_collided
done = is_collided
self._steps += 1
else:
reward = -30
terminated = True
done = True
reward_infos = {"success": False, "ball_pos": np.zeros(3), "ball_vel": np.zeros(3), "is_collided": False}
infos = dict(
@@ -166,10 +142,7 @@ class BeerPongEnv(MujocoEnv, utils.EzPickle):
q_vel=self.data.qvel[0:7].ravel().copy(), sim_crash=crash,
)
infos.update(reward_infos)
truncated = False
return ob, reward, terminated, truncated, infos
return ob, reward, done, infos
def _get_obs(self):
theta = self.data.qpos.flat[:7].copy()
@@ -224,13 +197,13 @@ class BeerPongEnv(MujocoEnv, utils.EzPickle):
min_dist_coeff, final_dist_coeff, ground_contact_dist_coeff, rew_offset = 0, 1, 0, 0
action_cost = 1e-4 * np.mean(action_cost)
reward = rew_offset - min_dist_coeff * min_dist ** 2 - final_dist_coeff * final_dist ** 2 - \
action_cost - ground_contact_dist_coeff * self.dist_ground_cup ** 2
action_cost - ground_contact_dist_coeff * self.dist_ground_cup ** 2
# release step punishment
min_time_bound = 0.1
max_time_bound = 1.0
release_time = self.release_step * self.dt
release_time_rew = int(release_time < min_time_bound) * (-30 - 10 * (release_time - min_time_bound) ** 2) + \
int(release_time > max_time_bound) * (-30 - 10 * (release_time - max_time_bound) ** 2)
int(release_time > max_time_bound) * (-30 - 10 * (release_time - max_time_bound) ** 2)
reward += release_time_rew
success = self.ball_in_cup
else:
@@ -285,9 +258,9 @@ class BeerPongEnvStepBasedEpisodicReward(BeerPongEnv):
return super(BeerPongEnvStepBasedEpisodicReward, self).step(a)
else:
reward = 0
terminated, truncated = True, False
done = True
while self._steps < MAX_EPISODE_STEPS_BEERPONG:
obs, sub_reward, terminated, truncated, infos = super(BeerPongEnvStepBasedEpisodicReward, self).step(
obs, sub_reward, done, infos = super(BeerPongEnvStepBasedEpisodicReward, self).step(
np.zeros(a.shape))
reward += sub_reward
return obs, reward, terminated, truncated, infos
return obs, reward, done, infos
@@ -1,8 +1,9 @@
import os
import mujoco_py.builder
import numpy as np
from gymnasium import utils
from gymnasium.envs.mujoco import MujocoEnv
from gym import utils
from gym.envs.mujoco import MujocoEnv
from fancy_gym.envs.mujoco.beerpong.deprecated.beerpong_reward_staged import BeerPongReward
@@ -73,24 +74,27 @@ class BeerPongEnv(MujocoEnv, utils.EzPickle):
crash = False
for _ in range(self.repeat_action):
applied_action = a + self.sim.data.qfrc_bias[:len(a)].copy() / self.model.actuator_gear[:, 0]
self.do_simulation(applied_action, self.frame_skip)
self.reward_function.initialize(self)
# self.reward_function.check_contacts(self.sim) # I assume this is not important?
if self._steps < self.release_step:
self.sim.data.qpos[7::] = self.sim.data.site_xpos[self.site_id("init_ball_pos"), :].copy()
self.sim.data.qvel[7::] = self.sim.data.site_xvelp[self.site_id("init_ball_pos"), :].copy()
crash = False
try:
self.do_simulation(applied_action, self.frame_skip)
self.reward_function.initialize(self)
# self.reward_function.check_contacts(self.sim) # I assume this is not important?
if self._steps < self.release_step:
self.sim.data.qpos[7::] = self.sim.data.site_xpos[self.site_id("init_ball_pos"), :].copy()
self.sim.data.qvel[7::] = self.sim.data.site_xvelp[self.site_id("init_ball_pos"), :].copy()
crash = False
except mujoco_py.builder.MujocoException:
crash = True
ob = self._get_obs()
if not crash:
reward, reward_infos = self.reward_function.compute_reward(self, applied_action)
is_collided = reward_infos['is_collided']
terminated = is_collided or self._steps == self.ep_length - 1
done = is_collided or self._steps == self.ep_length - 1
self._steps += 1
else:
reward = -30
terminated = True
done = True
reward_infos = {"success": False, "ball_pos": np.zeros(3), "ball_vel": np.zeros(3), "is_collided": False}
infos = dict(
@@ -100,7 +104,7 @@ class BeerPongEnv(MujocoEnv, utils.EzPickle):
q_vel=self.sim.data.qvel[0:7].ravel().copy(), sim_crash=crash,
)
infos.update(reward_infos)
return ob, reward, terminated, infos
return ob, reward, done, infos
def _get_obs(self):
theta = self.sim.data.qpos.flat[:7]
@@ -139,16 +143,16 @@ class BeerPongEnvStepBasedEpisodicReward(BeerPongEnv):
return super(BeerPongEnvStepBasedEpisodicReward, self).step(a)
else:
reward = 0
terminated, truncated = False, False
while not (terminated or truncated):
sub_ob, sub_reward, terminated, truncated, sub_infos = super(BeerPongEnvStepBasedEpisodicReward,
self).step(np.zeros(a.shape))
done = False
while not done:
sub_ob, sub_reward, done, sub_infos = super(BeerPongEnvStepBasedEpisodicReward, self).step(
np.zeros(a.shape))
reward += sub_reward
infos = sub_infos
ob = sub_ob
ob[-1] = self.release_step + 1 # Since we simulate until the end of the episode, PPO does not see the
# internal steps and thus, the observation also needs to be set correctly
return ob, reward, terminated, truncated, infos
return ob, reward, done, infos
# class BeerBongEnvStepBased(BeerBongEnv):
@@ -182,3 +186,27 @@ class BeerPongEnvStepBasedEpisodicReward(BeerPongEnv):
# ob[-1] = self.release_step + 1 # Since we simulate until the end of the episode, PPO does not see the
# # internal steps and thus, the observation also needs to be set correctly
# return ob, reward, done, infos
if __name__ == "__main__":
env = BeerPongEnv(frame_skip=2)
env.seed(0)
# env = BeerBongEnvStepBased(frame_skip=2)
# env = BeerBongEnvStepBasedEpisodicReward(frame_skip=2)
# env = BeerBongEnvFixedReleaseStep(frame_skip=2)
import time
env.reset()
env.render("human")
for i in range(600):
# ac = 10 * env.action_space.sample()
ac = 0.05 * np.ones(7)
obs, rew, d, info = env.step(ac)
env.render("human")
if d:
print('reward:', rew)
print('RESETTING')
env.reset()
time.sleep(1)
env.close()
@@ -6,23 +6,6 @@ from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
class MPWrapper(RawInterfaceWrapper):
mp_config = {
'ProMP': {
'phase_generator_kwargs': {
'learn_tau': True
},
'controller_kwargs': {
'p_gains': np.array([1.5, 5, 2.55, 3, 2., 2, 1.25]),
'd_gains': np.array([0.02333333, 0.1, 0.0625, 0.08, 0.03, 0.03, 0.0125]),
},
'basis_generator_kwargs': {
'num_basis': 2,
'num_basis_zero_start': 2,
},
},
'DMP': {},
'ProDMP': {},
}
@property
def context_mask(self) -> np.ndarray:
@@ -56,23 +39,3 @@ class MPWrapper(RawInterfaceWrapper):
xyz[-1] = 0.840
self.model.body_pos[self.cup_table_id] = xyz
return self.get_observation_from_step(self.get_obs())
class MPWrapper_FixedRelease(MPWrapper):
mp_config = {
'ProMP': {
'phase_generator_kwargs': {
'tau': 0.62,
},
'controller_kwargs': {
'p_gains': np.array([1.5, 5, 2.55, 3, 2., 2, 1.25]),
'd_gains': np.array([0.02333333, 0.1, 0.0625, 0.08, 0.03, 0.03, 0.0125]),
},
'basis_generator_kwargs': {
'num_basis': 2,
'num_basis_zero_start': 2,
},
},
'DMP': {},
'ProDMP': {},
}
@@ -1 +1 @@
from .mp_wrapper import MPWrapper, ReplanMPWrapper
from .mp_wrapper import MPWrapper
@@ -1,8 +1,8 @@
import os
import numpy as np
from gymnasium import utils, spaces
from gymnasium.envs.mujoco import MujocoEnv
from gym import utils, spaces
from gym.envs.mujoco import MujocoEnv
from fancy_gym.envs.mujoco.box_pushing.box_pushing_utils import rot_to_quat, get_quaternion_error, rotation_distance
from fancy_gym.envs.mujoco.box_pushing.box_pushing_utils import q_max, q_min, q_dot_max, q_torque_max
from fancy_gym.envs.mujoco.box_pushing.box_pushing_utils import desired_rod_quat
@@ -13,7 +13,6 @@ MAX_EPISODE_STEPS_BOX_PUSHING = 100
BOX_POS_BOUND = np.array([[0.3, -0.45, -0.01], [0.6, 0.45, -0.01]])
class BoxPushingEnvBase(MujocoEnv, utils.EzPickle):
"""
franka box pushing environment
@@ -27,16 +26,7 @@ class BoxPushingEnvBase(MujocoEnv, utils.EzPickle):
3. time-spatial-depend sparse reward
"""
metadata = {
"render_modes": [
"human",
"rgb_array",
"depth_array",
],
"render_fps": 50
}
def __init__(self, frame_skip: int = 10, random_init: bool = False, **kwargs):
def __init__(self, frame_skip: int = 10, random_init: bool = False):
utils.EzPickle.__init__(**locals())
self._steps = 0
self.init_qpos_box_pushing = np.array([0., 0., 0., -1.5, 0., 1.5, 0., 0., 0., 0.6, 0.45, 0.0, 1., 0., 0., 0.])
@@ -49,16 +39,11 @@ class BoxPushingEnvBase(MujocoEnv, utils.EzPickle):
self._desired_rod_quat = desired_rod_quat
self._episode_energy = 0.
self.observation_space = spaces.Box(
low=-np.inf, high=np.inf, shape=(28,), dtype=np.float64
)
self.random_init = random_init
MujocoEnv.__init__(self,
model_path=os.path.join(os.path.dirname(__file__), "assets", "box_pushing.xml"),
frame_skip=self.frame_skip,
observation_space=self.observation_space, **kwargs)
mujoco_bindings="mujoco")
self.action_space = spaces.Box(low=-1, high=1, shape=(7,))
def step(self, action):
@@ -104,11 +89,7 @@ class BoxPushingEnvBase(MujocoEnv, utils.EzPickle):
'is_success': True if episode_end and box_goal_pos_dist < 0.05 and box_goal_quat_dist < 0.5 else False,
'num_steps': self._steps
}
terminated = episode_end and infos['is_success']
truncated = episode_end and not infos['is_success']
return obs, reward, terminated, truncated, infos
return obs, reward, episode_end, infos
def reset_model(self):
# rest box to initial position
@@ -269,7 +250,7 @@ class BoxPushingEnvBase(MujocoEnv, utils.EzPickle):
old_err_norm = err_norm
# get Jacobian by mujoco
### get Jacobian by mujoco
self.data.qpos[:7] = q
mujoco.mj_forward(self.model, self.data)
@@ -303,10 +284,9 @@ class BoxPushingEnvBase(MujocoEnv, utils.EzPickle):
return q
class BoxPushingDense(BoxPushingEnvBase):
def __init__(self, **kwargs):
super(BoxPushingDense, self).__init__(**kwargs)
def __init__(self, frame_skip: int = 10, random_init: bool = False):
super(BoxPushingDense, self).__init__(frame_skip=frame_skip, random_init=random_init)
def _get_reward(self, episode_end, box_pos, box_quat, target_pos, target_quat,
rod_tip_pos, rod_quat, qpos, qvel, action):
joint_penalty = self._joint_limit_violate_penalty(qpos,
@@ -319,7 +299,7 @@ class BoxPushingDense(BoxPushingEnvBase):
energy_cost = -0.0005 * np.sum(np.square(action))
reward = joint_penalty + tcp_box_dist_reward + \
box_goal_pos_dist_reward + box_goal_rot_dist_reward + energy_cost
box_goal_pos_dist_reward + box_goal_rot_dist_reward + energy_cost
rod_inclined_angle = rotation_distance(rod_quat, self._desired_rod_quat)
if rod_inclined_angle > np.pi / 4:
@@ -327,10 +307,9 @@ class BoxPushingDense(BoxPushingEnvBase):
return reward
class BoxPushingTemporalSparse(BoxPushingEnvBase):
def __init__(self, **kwargs):
super(BoxPushingTemporalSparse, self).__init__(**kwargs)
def __init__(self, frame_skip: int = 10, random_init: bool = False):
super(BoxPushingTemporalSparse, self).__init__(frame_skip=frame_skip, random_init=random_init)
def _get_reward(self, episode_end, box_pos, box_quat, target_pos, target_quat,
rod_tip_pos, rod_quat, qpos, qvel, action):
@@ -361,8 +340,8 @@ class BoxPushingTemporalSparse(BoxPushingEnvBase):
class BoxPushingTemporalSpatialSparse(BoxPushingEnvBase):
def __init__(self, **kwargs):
super(BoxPushingTemporalSpatialSparse, self).__init__(**kwargs)
def __init__(self, frame_skip: int = 10, random_init: bool = False):
super(BoxPushingTemporalSpatialSparse, self).__init__(frame_skip=frame_skip, random_init=random_init)
def _get_reward(self, episode_end, box_pos, box_quat, target_pos, target_quat,
rod_tip_pos, rod_quat, qpos, qvel, action):
@@ -389,11 +368,10 @@ class BoxPushingTemporalSpatialSparse(BoxPushingEnvBase):
return reward
class BoxPushingTemporalSpatialSparse2(BoxPushingEnvBase):
def __init__(self, **kwargs):
super(BoxPushingTemporalSpatialSparse2, self).__init__(**kwargs)
def __init__(self, frame_skip: int = 10, random_init: bool = False):
super(BoxPushingTemporalSpatialSparse2, self).__init__(frame_skip=frame_skip, random_init=random_init)
def _get_reward(self, episode_end, box_pos, box_quat, target_pos, target_quat,
rod_tip_pos, rod_quat, qpos, qvel, action):
@@ -428,8 +406,8 @@ class BoxPushingTemporalSpatialSparse2(BoxPushingEnvBase):
class BoxPushingNoConstraintSparse(BoxPushingEnvBase):
def __init__(self, **kwargs):
super(BoxPushingNoConstraintSparse, self).__init__(**kwargs)
def __init__(self, frame_skip: int = 10, random_init: bool = False):
super(BoxPushingNoConstraintSparse, self).__init__(frame_skip=frame_skip, random_init=random_init)
def _get_reward(self, episode_end, box_pos, box_quat, target_pos, target_quat,
rod_tip_pos, rod_quat, qpos, qvel, action):
@@ -6,27 +6,6 @@ from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
class MPWrapper(RawInterfaceWrapper):
mp_config = {
'ProMP': {
'controller_kwargs': {
'p_gains': 0.01 * np.array([120., 120., 120., 120., 50., 30., 10.]),
'd_gains': 0.01 * np.array([10., 10., 10., 10., 6., 5., 3.]),
},
'basis_generator_kwargs': {
'basis_bandwidth_factor': 2 # 3.5, 4 to try
}
},
'DMP': {},
'ProDMP': {
'controller_kwargs': {
'p_gains': 0.01 * np.array([120., 120., 120., 120., 50., 30., 10.]),
'd_gains': 0.01 * np.array([10., 10., 10., 10., 6., 5., 3.]),
},
'basis_generator_kwargs': {
'basis_bandwidth_factor': 2 # 3.5, 4 to try
}
},
}
# Random x goal + random init pos
@property
@@ -59,35 +38,3 @@ class MPWrapper(RawInterfaceWrapper):
@property
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
return self.data.qvel[:7].copy()
class ReplanMPWrapper(MPWrapper):
mp_config = {
'ProMP': {},
'DMP': {},
'ProDMP': {
'controller_kwargs': {
'p_gains': 0.01 * np.array([120., 120., 120., 120., 50., 30., 10.]),
'd_gains': 0.01 * np.array([10., 10., 10., 10., 6., 5., 3.]),
},
'trajectory_generator_kwargs': {
'weights_scale': 0.3,
'goal_scale': 0.3,
'auto_scale_basis': True,
'goal_offset': 1.0,
'disable_goal': True,
},
'basis_generator_kwargs': {
'num_basis': 5,
'basis_bandwidth_factor': 3,
},
'phase_generator_kwargs': {
'alpha_phase': 3,
},
'black_box_kwargs': {
'max_planning_times': 4,
'replanning_schedule': lambda pos, vel, obs, action, t: t % 25 == 0,
'condition_on_desired': True,
}
}
}
@@ -1,68 +1,14 @@
import os
from typing import Tuple, Union, Optional, Any, Dict
from typing import Tuple, Union, Optional
import numpy as np
from gymnasium.core import ObsType
from gymnasium.envs.mujoco.half_cheetah_v4 import HalfCheetahEnv, DEFAULT_CAMERA_CONFIG
from gymnasium import utils
from gymnasium.envs.mujoco import MujocoEnv
from gymnasium.spaces import Box
from gym.core import ObsType
from gym.envs.mujoco.half_cheetah_v4 import HalfCheetahEnv
MAX_EPISODE_STEPS_HALFCHEETAHJUMP = 100
class HalfCheetahEnvCustomXML(HalfCheetahEnv):
def __init__(
self,
xml_file,
forward_reward_weight=1.0,
ctrl_cost_weight=0.1,
reset_noise_scale=0.1,
exclude_current_positions_from_observation=True,
**kwargs,
):
utils.EzPickle.__init__(
self,
xml_file,
forward_reward_weight,
ctrl_cost_weight,
reset_noise_scale,
exclude_current_positions_from_observation,
**kwargs,
)
self._forward_reward_weight = forward_reward_weight
self._ctrl_cost_weight = ctrl_cost_weight
self._reset_noise_scale = reset_noise_scale
self._exclude_current_positions_from_observation = (
exclude_current_positions_from_observation
)
if exclude_current_positions_from_observation:
observation_space = Box(
low=-np.inf, high=np.inf, shape=(18,), dtype=np.float64
)
else:
observation_space = Box(
low=-np.inf, high=np.inf, shape=(19,), dtype=np.float64
)
MujocoEnv.__init__(
self,
xml_file,
5,
observation_space=observation_space,
default_camera_config=DEFAULT_CAMERA_CONFIG,
**kwargs,
)
class HalfCheetahJumpEnv(HalfCheetahEnvCustomXML):
class HalfCheetahJumpEnv(HalfCheetahEnv):
"""
_ctrl_cost_weight 0.1 -> 0.0
"""
@@ -74,8 +20,7 @@ class HalfCheetahJumpEnv(HalfCheetahEnvCustomXML):
reset_noise_scale=0.1,
context=True,
exclude_current_positions_from_observation=True,
max_episode_steps=100,
**kwargs):
max_episode_steps=100):
self.current_step = 0
self.max_height = 0
# self.max_episode_steps = max_episode_steps
@@ -86,8 +31,7 @@ class HalfCheetahJumpEnv(HalfCheetahEnvCustomXML):
forward_reward_weight=forward_reward_weight,
ctrl_cost_weight=ctrl_cost_weight,
reset_noise_scale=reset_noise_scale,
exclude_current_positions_from_observation=exclude_current_positions_from_observation,
**kwargs)
exclude_current_positions_from_observation=exclude_current_positions_from_observation)
def step(self, action):
@@ -97,11 +41,10 @@ class HalfCheetahJumpEnv(HalfCheetahEnvCustomXML):
height_after = self.get_body_com("torso")[2]
self.max_height = max(height_after, self.max_height)
# Didnt use fell_over, because base env also has no done condition - Paul and Marc
## Didnt use fell_over, because base env also has no done condition - Paul and Marc
# fell_over = abs(self.sim.data.qpos[2]) > 2.5 # how to figure out if the cheetah fell over? -> 2.5 oke?
# TODO: Should a fall over be checked here?
terminated = False
truncated = False
done = False
ctrl_cost = self.control_cost(action)
costs = ctrl_cost
@@ -120,18 +63,17 @@ class HalfCheetahJumpEnv(HalfCheetahEnvCustomXML):
'max_height': self.max_height
}
return observation, reward, terminated, truncated, info
return observation, reward, done, info
def _get_obs(self):
return np.append(super()._get_obs(), self.goal)
def reset(self, *, seed: Optional[int] = None, options: Optional[Dict[str, Any]] = None) \
-> Tuple[ObsType, Dict[str, Any]]:
def reset(self, *, seed: Optional[int] = None, return_info: bool = False,
options: Optional[dict] = None, ) -> Union[ObsType, Tuple[ObsType, dict]]:
self.max_height = 0
self.current_step = 0
ret = super().reset(seed=seed, options=options)
self.goal = self.np_random.uniform(1.1, 1.6, 1) # 1.1 1.6
return ret
return super().reset()
# overwrite reset_model to make it deterministic
def reset_model(self):
@@ -6,12 +6,6 @@ from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
class MPWrapper(RawInterfaceWrapper):
mp_config = {
'ProMP': {},
'DMP': {},
'ProDMP': {},
}
@property
def context_mask(self) -> np.ndarray:
return np.hstack([
@@ -1,52 +0,0 @@
<mujoco model="hopper">
<compiler angle="degree" coordinate="global" inertiafromgeom="true"/>
<default>
<joint armature="1" damping="1" limited="true"/>
<geom conaffinity="1" condim="1" contype="1" margin="0.001" material="geom" rgba="0.8 0.6 .4 1" solimp=".8 .8 .01" solref=".02 1"/>
<motor ctrllimited="true" ctrlrange="-.4 .4"/>
</default>
<option integrator="RK4" timestep="0.002"/>
<visual>
<map znear="0.02"/>
</visual>
<worldbody>
<light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
<geom conaffinity="1" condim="3" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="20 20 .125" type="plane" material="MatPlane"/>
<body name="torso" pos="0 0 1.25">
<camera name="track" mode="trackcom" pos="0 -3 1" xyaxes="1 0 0 0 0 1"/>
<joint armature="0" axis="1 0 0" damping="0" limited="false" name="rootx" pos="0 0 0" stiffness="0" type="slide"/>
<joint armature="0" axis="0 0 1" damping="0" limited="false" name="rootz" pos="0 0 0" ref="1.25" stiffness="0" type="slide"/>
<joint armature="0" axis="0 1 0" damping="0" limited="false" name="rooty" pos="0 0 1.25" stiffness="0" type="hinge"/>
<geom friction="0.9" fromto="0 0 1.45 0 0 1.05" name="torso_geom" size="0.05" type="capsule"/>
<body name="thigh" pos="0 0 1.05">
<joint axis="0 -1 0" name="thigh_joint" pos="0 0 1.05" range="-150 0" type="hinge"/>
<geom friction="0.9" fromto="0 0 1.05 0 0 0.6" name="thigh_geom" size="0.05" type="capsule"/>
<body name="leg" pos="0 0 0.35">
<joint axis="0 -1 0" name="leg_joint" pos="0 0 0.6" range="-150 0" type="hinge"/>
<geom friction="0.9" fromto="0 0 0.6 0 0 0.1" name="leg_geom" size="0.04" type="capsule"/>
<body name="foot" pos="0.13/2 0 0.1">
<site name="foot_site" pos="0 0 0.04" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"/>
<joint axis="0 -1 0" name="foot_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
<geom friction="2.0" fromto="-0.13 0 0.1 0.26 0 0.1" name="foot_geom" size="0.06" type="capsule"/>
</body>
</body>
</body>
</body>
<body name="goal_site_body" pos = "0 0 0">
<site name="goal_site" pos="0 0 0.0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>
</body>
</worldbody>
<actuator>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="thigh_joint"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="leg_joint"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="foot_joint"/>
</actuator>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".4 .5 .6" rgb2="0 0 0"
width="100" height="100"/>
<texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
<texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
<material name="geom" texture="texgeom" texuniform="true"/>
</asset>
</mujoco>
@@ -1,51 +1,52 @@
<mujoco model="hopper">
<compiler angle="radian" autolimits="true"/>
<option integrator="RK4"/>
<compiler angle="degree" coordinate="global" inertiafromgeom="true"/>
<default>
<joint armature="1" damping="1" limited="true"/>
<geom conaffinity="1" condim="1" contype="1" margin="0.001" material="geom" rgba="0.8 0.6 .4 1" solimp=".8 .8 .01" solref=".02 1"/>
<motor ctrllimited="true" ctrlrange="-.4 .4"/>
</default>
<option integrator="RK4" timestep="0.002"/>
<visual>
<map znear="0.02"/>
</visual>
<default class="main">
<joint limited="true" armature="1" damping="1"/>
<geom condim="1" solimp="0.8 0.8 0.01 0.5 2" margin="0.001" material="geom" rgba="0.8 0.6 0.4 1"/>
<general ctrllimited="true" ctrlrange="-0.4 0.4"/>
</default>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.4 0.5 0.6" rgb2="0 0 0" width="100" height="600"/>
<texture type="cube" name="texgeom" builtin="flat" mark="cross" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" width="127" height="762"/>
<texture type="2d" name="texplane" builtin="checker" rgb1="0 0 0" rgb2="0.8 0.8 0.8" width="100" height="100"/>
<material name="MatPlane" texture="texplane" texrepeat="60 60" specular="1" shininess="1" reflectance="0.5"/>
<material name="geom" texture="texgeom" texuniform="true"/>
</asset>
<worldbody>
<geom name="floor" size="20 20 0.125" type="plane" condim="3" material="MatPlane" rgba="0.8 0.9 0.8 1"/>
<light pos="0 0 1.3" dir="0 0 -1" directional="true" cutoff="100" exponent="1" diffuse="1 1 1" specular="0.1 0.1 0.1"/>
<body name="torso" pos="0 0 1.25" gravcomp="0">
<joint name="rootx" pos="0 0 -1.25" axis="1 0 0" limited="false" type="slide" armature="0" damping="0"/>
<joint name="rootz" pos="0 0 -1.25" axis="0 0 1" limited="false" type="slide" ref="1.25" armature="0" damping="0"/>
<joint name="rooty" pos="0 0 0" axis="0 1 0" limited="false" armature="0" damping="0"/>
<geom name="torso_geom" size="0.05 0.2" type="capsule" friction="0.9 0.005 0.0001"/>
<camera name="track" pos="0 -3 -0.25" quat="0.707107 0.707107 0 0" mode="trackcom"/>
<body name="thigh" pos="0 0 -0.2" gravcomp="0">
<joint name="thigh_joint" pos="0 0 0" axis="0 -1 0" range="-2.61799 0"/>
<geom name="thigh_geom" size="0.05 0.225" pos="0 0 -0.225" type="capsule" friction="0.9 0.005 0.0001"/>
<body name="leg" pos="0 0 -0.7" gravcomp="0">
<joint name="leg_joint" pos="0 0 0.25" axis="0 -1 0" range="-2.61799 0"/>
<geom name="leg_geom" size="0.04 0.25" type="capsule" friction="0.9 0.005 0.0001"/>
<body name="foot" pos="0.065 0 -0.25" gravcomp="0">
<joint name="foot_joint" pos="-0.065 0 0" axis="0 -1 0" range="-0.785398 0.785398"/>
<geom name="foot_geom" size="0.06 0.195" quat="0.707107 0 -0.707107 0" type="capsule" friction="2 0.005 0.0001"/>
<site name="foot_site" pos="-0.065 0 -0.06" size="0.02" rgba="1 0 0 1"/>
<light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
<geom conaffinity="1" condim="3" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="20 20 .125" type="plane" material="MatPlane"/>
<body name="torso" pos="0 0 1.25">
<camera name="track" mode="trackcom" pos="0 -3 1" xyaxes="1 0 0 0 0 1"/>
<joint armature="0" axis="1 0 0" damping="0" limited="false" name="rootx" pos="0 0 0" stiffness="0" type="slide"/>
<joint armature="0" axis="0 0 1" damping="0" limited="false" name="rootz" pos="0 0 0" ref="1.25" stiffness="0" type="slide"/>
<joint armature="0" axis="0 1 0" damping="0" limited="false" name="rooty" pos="0 0 1.25" stiffness="0" type="hinge"/>
<geom friction="0.9" fromto="0 0 1.45 0 0 1.05" name="torso_geom" size="0.05" type="capsule"/>
<body name="thigh" pos="0 0 1.05">
<joint axis="0 -1 0" name="thigh_joint" pos="0 0 1.05" range="-150 0" type="hinge"/>
<geom friction="0.9" fromto="0 0 1.05 0 0 0.6" name="thigh_geom" size="0.05" type="capsule"/>
<body name="leg" pos="0 0 0.35">
<joint axis="0 -1 0" name="leg_joint" pos="0 0 0.6" range="-150 0" type="hinge"/>
<geom friction="0.9" fromto="0 0 0.6 0 0 0.1" name="leg_geom" size="0.04" type="capsule"/>
<body name="foot" pos="0.13/2 0 0.1">
<site name="foot_site" pos="0 0 0.04" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"/>
<joint axis="0 -1 0" name="foot_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
<geom friction="2.0" fromto="-0.13 0 0.1 0.26 0 0.1" name="foot_geom" size="0.06" type="capsule"/>
</body>
</body>
</body>
</body>
<body name="goal_site_body" pos="0 0 0" gravcomp="0">
<site name="goal_site" pos="0 0 0" size="0.02" rgba="0 1 0 1"/>
</body>
<body name="goal_site_body" pos = "0 0 0">
<site name="goal_site" pos="0 0 0.0" size="0.02 0.02 0.02" rgba="0 1 0 1" type="sphere"/>
</body>
</worldbody>
<actuator>
<general joint="thigh_joint" ctrlrange="-1 1" gear="200 0 0 0 0 0" actdim="0"/>
<general joint="leg_joint" ctrlrange="-1 1" gear="200 0 0 0 0 0" actdim="0"/>
<general joint="foot_joint" ctrlrange="-1 1" gear="200 0 0 0 0 0" actdim="0"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="thigh_joint"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="leg_joint"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="foot_joint"/>
</actuator>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".4 .5 .6" rgb2="0 0 0"
width="100" height="100"/>
<texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
<texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
<material name="geom" texture="texgeom" texuniform="true"/>
</asset>
</mujoco>
@@ -1,50 +1,51 @@
<mujoco model="hopper">
<compiler angle="radian" autolimits="true"/>
<option integrator="RK4"/>
<compiler angle="degree" coordinate="global" inertiafromgeom="true"/>
<default>
<joint armature="1" damping="1" limited="true"/>
<geom conaffinity="1" condim="1" contype="1" margin="0.001" material="geom" rgba="0.8 0.6 .4 1" solimp=".8 .8 .01" solref=".02 1"/>
<motor ctrllimited="true" ctrlrange="-.4 .4"/>
</default>
<option integrator="RK4" timestep="0.002"/>
<visual>
<map znear="0.02"/>
</visual>
<default class="main">
<joint limited="true" armature="1" damping="1"/>
<geom condim="1" solimp="0.8 0.8 0.01 0.5 2" margin="0.001" material="geom" rgba="0.8 0.6 0.4 1"/>
<general ctrllimited="true" ctrlrange="-0.4 0.4"/>
</default>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.4 0.5 0.6" rgb2="0 0 0" width="100" height="600"/>
<texture type="cube" name="texgeom" builtin="flat" mark="cross" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" width="127" height="762"/>
<texture type="2d" name="texplane" builtin="checker" rgb1="0 0 0" rgb2="0.8 0.8 0.8" width="100" height="100"/>
<material name="MatPlane" texture="texplane" texrepeat="60 60" specular="1" shininess="1" reflectance="0.5"/>
<material name="geom" texture="texgeom" texuniform="true"/>
</asset>
<worldbody>
<geom name="floor" size="20 20 0.125" type="plane" condim="3" material="MatPlane" rgba="0.8 0.9 0.8 1"/>
<light pos="0 0 1.3" dir="0 0 -1" directional="true" cutoff="100" exponent="1" diffuse="1 1 1" specular="0.1 0.1 0.1"/>
<body name="torso" pos="0 0 1.25" gravcomp="0">
<joint name="rootx" pos="0 0 -1.25" axis="1 0 0" limited="false" type="slide" armature="0" damping="0"/>
<joint name="rootz" pos="0 0 -1.25" axis="0 0 1" limited="false" type="slide" ref="1.25" armature="0" damping="0"/>
<joint name="rooty" pos="0 0 0" axis="0 1 0" limited="false" armature="0" damping="0"/>
<geom name="torso_geom" size="0.05 0.2" type="capsule" friction="0.9 0.005 0.0001"/>
<camera name="track" pos="0 -3 -0.25" quat="0.707107 0.707107 0 0" mode="trackcom"/>
<body name="thigh" pos="0 0 -0.2" gravcomp="0">
<joint name="thigh_joint" pos="0 0 0" axis="0 -1 0" range="-2.61799 0"/>
<geom name="thigh_geom" size="0.05 0.225" pos="0 0 -0.225" type="capsule" friction="0.9 0.005 0.0001"/>
<body name="leg" pos="0 0 -0.7" gravcomp="0">
<joint name="leg_joint" pos="0 0 0.25" axis="0 -1 0" range="-2.61799 0"/>
<geom name="leg_geom" size="0.04 0.25" type="capsule" friction="0.9 0.005 0.0001"/>
<body name="foot" pos="0.065 0 -0.25" gravcomp="0">
<joint name="foot_joint" pos="-0.065 0 0" axis="0 -1 0" range="-0.785398 0.785398"/>
<geom name="foot_geom" size="0.06 0.195" quat="0.707107 0 -0.707107 0" type="capsule" friction="2 0.005 0.0001"/>
<light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
<geom conaffinity="1" condim="3" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="20 20 .125" type="plane" material="MatPlane"/>
<body name="torso" pos="0 0 1.25">
<camera name="track" mode="trackcom" pos="0 -3 1" xyaxes="1 0 0 0 0 1"/>
<joint armature="0" axis="1 0 0" damping="0" limited="false" name="rootx" pos="0 0 0" stiffness="0" type="slide"/>
<joint armature="0" axis="0 0 1" damping="0" limited="false" name="rootz" pos="0 0 0" ref="1.25" stiffness="0" type="slide"/>
<joint armature="0" axis="0 1 0" damping="0" limited="false" name="rooty" pos="0 0 1.25" stiffness="0" type="hinge"/>
<geom friction="0.9" fromto="0 0 1.45 0 0 1.05" name="torso_geom" size="0.05" type="capsule"/>
<body name="thigh" pos="0 0 1.05">
<joint axis="0 -1 0" name="thigh_joint" pos="0 0 1.05" range="-150 0" type="hinge"/>
<geom friction="0.9" fromto="0 0 1.05 0 0 0.6" name="thigh_geom" size="0.05" type="capsule"/>
<body name="leg" pos="0 0 0.35">
<joint axis="0 -1 0" name="leg_joint" pos="0 0 0.6" range="-150 0" type="hinge"/>
<geom friction="0.9" fromto="0 0 0.6 0 0 0.1" name="leg_geom" size="0.04" type="capsule"/>
<body name="foot" pos="0.13/2 0 0.1">
<joint axis="0 -1 0" name="foot_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
<geom friction="2.0" fromto="-0.13 0 0.1 0.26 0 0.1" name="foot_geom" size="0.06" type="capsule"/>
</body>
</body>
</body>
</body>
<body name="box" pos="1 0 0" gravcomp="0">
<geom name="basket_ground_geom" size="0.3 0.3 0.26" pos="-0.26 0 0" quat="0.707107 0 -0.707107 0" type="box" rgba="1 0 0 1"/>
<body name="box" pos="1 0 0">
<geom friction="1.0" fromto="0.48 0 0 1 0 0" name="basket_ground_geom" size="0.3" type="box" rgba="1 0 0 1"/>
</body>
</worldbody>
<actuator>
<general joint="thigh_joint" ctrlrange="-1 1" gear="200 0 0 0 0 0" actdim="0"/>
<general joint="leg_joint" ctrlrange="-1 1" gear="200 0 0 0 0 0" actdim="0"/>
<general joint="foot_joint" ctrlrange="-1 1" gear="200 0 0 0 0 0" actdim="0"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="thigh_joint"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="leg_joint"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="foot_joint"/>
</actuator>
</mujoco>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".4 .5 .6" rgb2="0 0 0"
width="100" height="100"/>
<texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
<texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
<material name="geom" texture="texgeom" texuniform="true"/>
</asset>
</mujoco>
+14 -100
View File
@@ -1,95 +1,12 @@
import os
import numpy as np
from gymnasium.envs.mujoco.hopper_v4 import HopperEnv, DEFAULT_CAMERA_CONFIG
from gymnasium import utils
from gymnasium.envs.mujoco import MujocoEnv
from gymnasium.spaces import Box
import mujoco
from gym.envs.mujoco.hopper_v4 import HopperEnv
MAX_EPISODE_STEPS_HOPPERJUMP = 250
class HopperEnvCustomXML(HopperEnv):
"""
Initialization changes to normal Hopper:
- terminate_when_unhealthy: True -> False
- healthy_reward: 1.0 -> 2.0
- healthy_z_range: (0.7, float('inf')) -> (0.5, float('inf'))
- healthy_angle_range: (-0.2, 0.2) -> (-float('inf'), float('inf'))
- exclude_current_positions_from_observation: True -> False
"""
def __init__(
self,
xml_file,
forward_reward_weight=1.0,
ctrl_cost_weight=1e-3,
healthy_reward=1.0,
terminate_when_unhealthy=True,
healthy_state_range=(-100.0, 100.0),
healthy_z_range=(0.7, float("inf")),
healthy_angle_range=(-0.2, 0.2),
reset_noise_scale=5e-3,
exclude_current_positions_from_observation=True,
**kwargs,
):
xml_file = os.path.join(os.path.dirname(__file__), "assets", xml_file)
utils.EzPickle.__init__(
self,
xml_file,
forward_reward_weight,
ctrl_cost_weight,
healthy_reward,
terminate_when_unhealthy,
healthy_state_range,
healthy_z_range,
healthy_angle_range,
reset_noise_scale,
exclude_current_positions_from_observation,
**kwargs
)
self._forward_reward_weight = forward_reward_weight
self._ctrl_cost_weight = ctrl_cost_weight
self._healthy_reward = healthy_reward
self._terminate_when_unhealthy = terminate_when_unhealthy
self._healthy_state_range = healthy_state_range
self._healthy_z_range = healthy_z_range
self._healthy_angle_range = healthy_angle_range
self._reset_noise_scale = reset_noise_scale
self._exclude_current_positions_from_observation = (
exclude_current_positions_from_observation
)
if not hasattr(self, 'observation_space'):
if exclude_current_positions_from_observation:
self.observation_space = Box(
low=-np.inf, high=np.inf, shape=(15,), dtype=np.float64
)
else:
self.observation_space = Box(
low=-np.inf, high=np.inf, shape=(16,), dtype=np.float64
)
MujocoEnv.__init__(
self,
xml_file,
4,
observation_space=self.observation_space,
default_camera_config=DEFAULT_CAMERA_CONFIG,
**kwargs,
)
class HopperJumpEnv(HopperEnvCustomXML):
class HopperJumpEnv(HopperEnv):
"""
Initialization changes to normal Hopper:
- terminate_when_unhealthy: True -> False
@@ -115,7 +32,6 @@ class HopperJumpEnv(HopperEnvCustomXML):
reset_noise_scale=5e-3,
exclude_current_positions_from_observation=False,
sparse=False,
**kwargs
):
self.sparse = sparse
@@ -142,8 +58,7 @@ class HopperJumpEnv(HopperEnvCustomXML):
healthy_z_range=healthy_z_range,
healthy_angle_range=healthy_angle_range,
reset_noise_scale=reset_noise_scale,
exclude_current_positions_from_observation=exclude_current_positions_from_observation,
**kwargs)
exclude_current_positions_from_observation=exclude_current_positions_from_observation)
# increase initial height
self.init_qpos[1] = 1.5
@@ -158,7 +73,7 @@ class HopperJumpEnv(HopperEnvCustomXML):
self.do_simulation(action, self.frame_skip)
height_after = self.get_body_com("torso")[2]
# site_pos_after = self.data.get_site_xpos('foot_site')
#site_pos_after = self.data.get_site_xpos('foot_site')
site_pos_after = self.data.site('foot_site').xpos
self.max_height = max(height_after, self.max_height)
@@ -173,8 +88,7 @@ class HopperJumpEnv(HopperEnvCustomXML):
ctrl_cost = self.control_cost(action)
costs = ctrl_cost
terminated = False
truncated = False
done = False
goal_dist = np.linalg.norm(site_pos_after - self.goal)
if self.contact_dist is None and self.contact_with_floor:
@@ -201,7 +115,7 @@ class HopperJumpEnv(HopperEnvCustomXML):
healthy=self.is_healthy,
contact_dist=self.contact_dist or 0
)
return observation, reward, terminated, truncated, info
return observation, reward, done, info
def _get_obs(self):
# goal_dist = self.data.get_site_xpos('foot_site') - self.goal
@@ -226,8 +140,8 @@ class HopperJumpEnv(HopperEnvCustomXML):
noise_high[5] = 0.785
qpos = (
self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nq) +
self.init_qpos
self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nq) +
self.init_qpos
)
qvel = (
# self.np_random.uniform(low=noise_low, high=noise_high, size=self.model.nv) +
@@ -248,12 +162,12 @@ class HopperJumpEnv(HopperEnvCustomXML):
# floor_geom_id = self.model.geom_name2id('floor')
# foot_geom_id = self.model.geom_name2id('foot_geom')
# TODO: do this properly over a sensor in the xml file, see dmc hopper
floor_geom_id = mujoco.mj_name2id(self.model,
mujoco.mjtObj.mjOBJ_GEOM,
'floor')
foot_geom_id = mujoco.mj_name2id(self.model,
mujoco.mjtObj.mjOBJ_GEOM,
'foot_geom')
floor_geom_id = self._mujoco_bindings.mj_name2id(self.model,
self._mujoco_bindings.mjtObj.mjOBJ_GEOM,
'floor')
foot_geom_id = self._mujoco_bindings.mj_name2id(self.model,
self._mujoco_bindings.mjtObj.mjOBJ_GEOM,
'foot_geom')
for i in range(self.data.ncon):
contact = self.data.contact[i]
collision = contact.geom1 == floor_geom_id and contact.geom2 == foot_geom_id
@@ -1,16 +1,12 @@
import os
from typing import Optional, Dict, Any, Tuple
import numpy as np
from gymnasium.core import ObsType
from fancy_gym.envs.mujoco.hopper_jump.hopper_jump import HopperEnvCustomXML
from gymnasium import spaces
from gym.envs.mujoco.hopper_v4 import HopperEnv
MAX_EPISODE_STEPS_HOPPERJUMPONBOX = 250
class HopperJumpOnBoxEnv(HopperEnvCustomXML):
class HopperJumpOnBoxEnv(HopperEnv):
"""
Initialization changes to normal Hopper:
- healthy_reward: 1.0 -> 0.01 -> 0.001
@@ -29,8 +25,7 @@ class HopperJumpOnBoxEnv(HopperEnvCustomXML):
reset_noise_scale=5e-3,
context=True,
exclude_current_positions_from_observation=True,
max_episode_steps=250,
**kwargs):
max_episode_steps=250):
self.current_step = 0
self.max_height = 0
self.max_episode_steps = max_episode_steps
@@ -38,21 +33,10 @@ class HopperJumpOnBoxEnv(HopperEnvCustomXML):
self.hopper_on_box = False
self.context = context
self.box_x = 1
if exclude_current_positions_from_observation:
self.observation_space = spaces.Box(
low=-np.inf, high=np.inf, shape=(12,), dtype=np.float64
)
else:
self.observation_space = spaces.Box(
low=-np.inf, high=np.inf, shape=(13,), dtype=np.float64
)
xml_file = os.path.join(os.path.dirname(__file__), "assets", xml_file)
super().__init__(xml_file, forward_reward_weight, ctrl_cost_weight, healthy_reward, terminate_when_unhealthy,
healthy_state_range, healthy_z_range, healthy_angle_range, reset_noise_scale,
exclude_current_positions_from_observation,
**kwargs)
exclude_current_positions_from_observation)
def step(self, action):
@@ -90,10 +74,10 @@ class HopperJumpOnBoxEnv(HopperEnvCustomXML):
costs = ctrl_cost
terminated = fell_over or self.hopper_on_box
done = fell_over or self.hopper_on_box
if self.current_step >= self.max_episode_steps or terminated:
done = False # TODO why are we doing this???
if self.current_step >= self.max_episode_steps or done:
done = False
max_height = self.max_height.copy()
min_distance = self.min_distance.copy()
@@ -138,25 +122,21 @@ class HopperJumpOnBoxEnv(HopperEnvCustomXML):
'goal': self.box_x,
}
truncated = self.current_step >= self.max_episode_steps and not terminated
return observation, reward, terminated, truncated, info
return observation, reward, done, info
def _get_obs(self):
return np.append(super()._get_obs(), self.box_x)
def reset(self, *, seed: Optional[int] = None, options: Optional[Dict[str, Any]] = None) \
-> Tuple[ObsType, Dict[str, Any]]:
def reset(self):
self.max_height = 0
self.min_distance = 5000
self.current_step = 0
self.hopper_on_box = False
ret = super().reset(seed=seed, options=options)
if self.context:
self.box_x = self.np_random.uniform(1, 3, 1)
self.model.body("box").pos = [self.box_x[0], 0, 0]
return ret
return super().reset()
# overwrite reset_model to make it deterministic
def reset_model(self):
@@ -170,3 +150,21 @@ class HopperJumpOnBoxEnv(HopperEnvCustomXML):
observation = self._get_obs()
return observation
if __name__ == '__main__':
render_mode = "human" # "human" or "partial" or "final"
env = HopperJumpOnBoxEnv()
obs = env.reset()
for i in range(2000):
# objective.load_result("/tmp/cma")
# test with random actions
ac = env.action_space.sample()
obs, rew, d, info = env.step(ac)
if i % 10 == 0:
env.render(mode=render_mode)
if d:
print('After ', i, ' steps, done: ', d)
env.reset()
env.close()
@@ -6,11 +6,6 @@ from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
class MPWrapper(RawInterfaceWrapper):
mp_config = {
'ProMP': {},
'DMP': {},
'ProDMP': {},
}
# Random x goal + random init pos
@property
@@ -1,54 +1,56 @@
<mujoco model="hopper">
<compiler angle="radian" autolimits="true"/>
<option integrator="RK4"/>
<compiler angle="degree" coordinate="global" inertiafromgeom="true"/>
<default>
<joint armature="1" damping="1" limited="true"/>
<geom conaffinity="1" condim="1" contype="1" margin="0.001" material="geom" rgba="0.8 0.6 .4 1" solimp=".8 .8 .01" solref=".02 1"/>
<motor ctrllimited="true" ctrlrange="-.4 .4"/>
</default>
<option integrator="RK4" timestep="0.002"/>
<visual>
<map znear="0.02"/>
</visual>
<default class="main">
<joint limited="true" armature="1" damping="1"/>
<geom condim="1" solimp="0.8 0.8 0.01 0.5 2" margin="0.001" material="geom" rgba="0.8 0.6 0.4 1"/>
<general ctrllimited="true" ctrlrange="-0.4 0.4"/>
</default>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.4 0.5 0.6" rgb2="0 0 0" width="100" height="600"/>
<texture type="cube" name="texgeom" builtin="flat" mark="cross" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" width="127" height="762"/>
<texture type="2d" name="texplane" builtin="checker" rgb1="0 0 0" rgb2="0.8 0.8 0.8" width="100" height="100"/>
<material name="MatPlane" texture="texplane" texrepeat="60 60" specular="1" shininess="1" reflectance="0.5"/>
<material name="geom" texture="texgeom" texuniform="true"/>
</asset>
<worldbody>
<geom name="floor" size="20 20 0.125" type="plane" condim="3" material="MatPlane" rgba="0.8 0.9 0.8 1"/>
<light pos="0 0 1.3" dir="0 0 -1" directional="true" cutoff="100" exponent="1" diffuse="1 1 1" specular="0.1 0.1 0.1"/>
<body name="torso" pos="0 0 1.25" gravcomp="0">
<joint name="rootx" pos="0 0 -1.25" axis="1 0 0" limited="false" type="slide" armature="0" damping="0"/>
<joint name="rootz" pos="0 0 -1.25" axis="0 0 1" limited="false" type="slide" ref="1.25" armature="0" damping="0"/>
<joint name="rooty" pos="0 0 0" axis="0 1 0" limited="false" armature="0" damping="0"/>
<geom name="torso_geom" size="0.05 0.2" type="capsule" friction="0.9 0.005 0.0001"/>
<camera name="track" pos="0 -3 -0.25" quat="0.707107 0.707107 0 0" mode="trackcom"/>
<body name="thigh" pos="0 0 -0.2" gravcomp="0">
<joint name="thigh_joint" pos="0 0 0" axis="0 -1 0" range="-2.61799 0"/>
<geom name="thigh_geom" size="0.05 0.225" pos="0 0 -0.225" type="capsule" friction="0.9 0.005 0.0001"/>
<body name="leg" pos="0 0 -0.7" gravcomp="0">
<joint name="leg_joint" pos="0 0 0.25" axis="0 -1 0" range="-2.61799 0"/>
<geom name="leg_geom" size="0.04 0.25" type="capsule" friction="0.9 0.005 0.0001"/>
<body name="foot" pos="0.065 0 -0.25" gravcomp="0">
<joint name="foot_joint" pos="-0.065 0 0" axis="0 -1 0" range="-0.785398 0.785398"/>
<geom name="foot_geom" size="0.06 0.195" quat="0.707107 0 -0.707107 0" type="capsule" friction="2 0.005 0.0001"/>
<light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
<geom conaffinity="1" condim="3" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="20 20 .125" type="plane" material="MatPlane"/>
<body name="torso" pos="0 0 1.25">
<camera name="track" mode="trackcom" pos="0 -3 1" xyaxes="1 0 0 0 0 1"/>
<joint armature="0" axis="1 0 0" damping="0" limited="false" name="rootx" pos="0 0 0" stiffness="0" type="slide"/>
<joint armature="0" axis="0 0 1" damping="0" limited="false" name="rootz" pos="0 0 0" ref="1.25" stiffness="0" type="slide"/>
<joint armature="0" axis="0 1 0" damping="0" limited="false" name="rooty" pos="0 0 1.25" stiffness="0" type="hinge"/>
<geom friction="0.9" fromto="0 0 1.45 0 0 1.05" name="torso_geom" size="0.05" type="capsule"/>
<body name="thigh" pos="0 0 1.05">
<joint axis="0 -1 0" name="thigh_joint" pos="0 0 1.05" range="-150 0" type="hinge"/>
<geom friction="0.9" fromto="0 0 1.05 0 0 0.6" name="thigh_geom" size="0.05" type="capsule"/>
<body name="leg" pos="0 0 0.35">
<joint axis="0 -1 0" name="leg_joint" pos="0 0 0.6" range="-150 0" type="hinge"/>
<geom friction="0.9" fromto="0 0 0.6 0 0 0.1" name="leg_geom" size="0.04" type="capsule"/>
<body name="foot" pos="0.13/2 0 0.1">
<joint axis="0 -1 0" name="foot_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
<geom friction="2.0" fromto="-0.13 0 0.1 0.26 0 0.1" name="foot_geom" size="0.06" type="capsule"/>
</body>
</body>
</body>
</body>
<body name="ball" pos="0 0 1.53" gravcomp="0">
<joint name="tar:x" pos="0 0 0" axis="1 0 0" limited="false" type="slide" armature="0" damping="0"/>
<joint name="tar:y" pos="0 0 0" axis="0 1 0" limited="false" type="slide" armature="0" damping="0"/>
<joint name="tar:z" pos="0 0 0" axis="0 0 1" limited="false" type="slide" armature="0" damping="0"/>
<geom name="ball_geom" size="0.025" condim="4" priority="1" friction="0.1 0.1 0.1" solref="-10000 -10" solimp="0.9 0.95 0.001 0.5 2" mass="0.1" rgba="0.8 0.2 0.1 1"/>
<site name="target_ball" pos="0 0 0" size="0.04" rgba="1 0 0 1"/>
<body name="ball" pos="0 0 1.53">
<joint armature="0" axis="1 0 0" damping="0.0" name="tar:x" pos="0 0 1.53" stiffness="0" type="slide" frictionloss="0" limited="false"/>
<joint armature="0" axis="0 1 0" damping="0.0" name="tar:y" pos="0 0 1.53" stiffness="0" type="slide" frictionloss="0" limited="false"/>
<joint armature="0" axis="0 0 1" damping="0.0" name="tar:z" pos="0 0 1.53" stiffness="0" type="slide" frictionloss="0" limited="false"/>
<geom pos="0 0 1.53" priority= "1" size="0.025 0.025 0.025" type="sphere" condim="4" name="ball_geom" rgba="0.8 0.2 0.1 1" mass="0.1"
friction="0.1 0.1 0.1" solimp="0.9 0.95 0.001 0.5 2" solref="-10000 -10"/>
<site name="target_ball" pos="0 0 1.53" size="0.04 0.04 0.04" rgba="1 0 0 1" type="sphere"/>
</body>
</worldbody>
<actuator>
<general joint="thigh_joint" ctrlrange="-1 1" gear="200 0 0 0 0 0" actdim="0"/>
<general joint="leg_joint" ctrlrange="-1 1" gear="200 0 0 0 0 0" actdim="0"/>
<general joint="foot_joint" ctrlrange="-1 1" gear="200 0 0 0 0 0" actdim="0"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="thigh_joint"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="leg_joint"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="foot_joint"/>
</actuator>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".4 .5 .6" rgb2="0 0 0"
width="100" height="100"/>
<texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
<texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
<material name="geom" texture="texgeom" texuniform="true"/>
</asset>
</mujoco>
@@ -1,129 +1,132 @@
<mujoco model="hopper">
<compiler angle="radian" autolimits="true"/>
<option integrator="RK4"/>
<compiler angle="degree" coordinate="global" inertiafromgeom="true"/>
<default>
<joint armature="1" damping="1" limited="true"/>
<geom conaffinity="1" condim="1" contype="1" margin="0.001" material="geom" rgba="0.8 0.6 .4 1" solimp=".8 .8 .01" solref=".02 1"/>
<motor ctrllimited="true" ctrlrange="-.4 .4"/>
</default>
<option integrator="RK4" timestep="0.002"/>
<visual>
<map znear="0.02"/>
</visual>
<default class="main">
<joint limited="true" armature="1" damping="1"/>
<geom condim="1" solimp="0.8 0.8 0.01 0.5 2" margin="0.001" material="geom" rgba="0.8 0.6 0.4 1"/>
<general ctrllimited="true" ctrlrange="-0.4 0.4"/>
</default>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.4 0.5 0.6" rgb2="0 0 0" width="100" height="600"/>
<texture type="cube" name="texgeom" builtin="flat" mark="cross" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" width="127" height="762"/>
<texture type="2d" name="texplane" builtin="checker" rgb1="0 0 0" rgb2="0.8 0.8 0.8" width="100" height="100"/>
<material name="MatPlane" texture="texplane" texrepeat="60 60" specular="1" shininess="1" reflectance="0.5"/>
<material name="geom" texture="texgeom" texuniform="true"/>
</asset>
<worldbody>
<geom name="floor" size="20 20 0.125" type="plane" condim="3" material="MatPlane" rgba="0.8 0.9 0.8 1"/>
<light pos="0 0 1.3" dir="0 0 -1" directional="true" cutoff="100" exponent="1" diffuse="1 1 1" specular="0.1 0.1 0.1"/>
<body name="torso" pos="0 0 1.25" gravcomp="0">
<joint name="rootx" pos="0 0 -1.25" axis="1 0 0" limited="false" type="slide" armature="0" damping="0"/>
<joint name="rootz" pos="0 0 -1.25" axis="0 0 1" limited="false" type="slide" ref="1.25" armature="0" damping="0"/>
<joint name="rooty" pos="0 0 0" axis="0 1 0" limited="false" armature="0" damping="0"/>
<geom name="torso_geom" size="0.05 0.2" type="capsule" friction="0.9 0.005 0.0001"/>
<camera name="track" pos="0 -3 -0.25" quat="0.707107 0.707107 0 0" mode="trackcom"/>
<body name="thigh" pos="0 0 -0.2" gravcomp="0">
<joint name="thigh_joint" pos="0 0 0" axis="0 -1 0" range="-2.61799 0"/>
<geom name="thigh_geom" size="0.05 0.225" pos="0 0 -0.225" type="capsule" friction="0.9 0.005 0.0001"/>
<body name="leg" pos="0 0 -0.7" gravcomp="0">
<joint name="leg_joint" pos="0 0 0.25" axis="0 -1 0" range="-2.61799 0"/>
<geom name="leg_geom" size="0.04 0.25" type="capsule" friction="0.9 0.005 0.0001"/>
<body name="foot" pos="0.065 0 -0.25" gravcomp="0">
<joint name="foot_joint" pos="-0.065 0 0" axis="0 -1 0" range="-0.785398 0.785398"/>
<geom name="foot_geom" size="0.06 0.195" quat="0.707107 0 -0.707107 0" type="capsule" friction="2 0.005 0.0001"/>
<light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
<geom conaffinity="1" condim="3" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="20 20 .125" type="plane" material="MatPlane"/>
<body name="torso" pos="0 0 1.25">
<camera name="track" mode="trackcom" pos="0 -3 1" xyaxes="1 0 0 0 0 1"/>
<joint armature="0" axis="1 0 0" damping="0" limited="false" name="rootx" pos="0 0 0" stiffness="0" type="slide"/>
<joint armature="0" axis="0 0 1" damping="0" limited="false" name="rootz" pos="0 0 0" ref="1.25" stiffness="0" type="slide"/>
<joint armature="0" axis="0 1 0" damping="0" limited="false" name="rooty" pos="0 0 1.25" stiffness="0" type="hinge"/>
<geom friction="0.9" fromto="0 0 1.45 0 0 1.05" name="torso_geom" size="0.05" type="capsule"/>
<body name="thigh" pos="0 0 1.05">
<joint axis="0 -1 0" name="thigh_joint" pos="0 0 1.05" range="-150 0" type="hinge"/>
<geom friction="0.9" fromto="0 0 1.05 0 0 0.6" name="thigh_geom" size="0.05" type="capsule"/>
<body name="leg" pos="0 0 0.35">
<joint axis="0 -1 0" name="leg_joint" pos="0 0 0.6" range="-150 0" type="hinge"/>
<geom friction="0.9" fromto="0 0 0.6 0 0 0.1" name="leg_geom" size="0.04" type="capsule"/>
<body name="foot" pos="0.13/2 0 0.1">
<joint axis="0 -1 0" name="foot_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
<geom friction="2.0" fromto="-0.13 0 0.1 0.26 0 0.1" name="foot_geom" size="0.06" type="capsule"/>
</body>
</body>
</body>
</body>
<body name="ball" pos="0 0 1.53" gravcomp="0">
<joint name="tar:x" pos="0 0 0" axis="1 0 0" limited="false" type="slide" armature="0" damping="0"/>
<joint name="tar:y" pos="0 0 0" axis="0 1 0" limited="false" type="slide" armature="0" damping="0"/>
<joint name="tar:z" pos="0 0 0" axis="0 0 1" limited="false" type="slide" armature="0" damping="0"/>
<geom name="ball_geom" size="0.025" condim="4" priority="1" friction="0.1 0.1 0.1" solref="-10000 -10" solimp="0.9 0.95 0.001 0.5 2" mass="0.1" rgba="0.8 0.2 0.1 1"/>
<site name="target_ball" pos="0 0 0" size="0.04" rgba="1 0 0 1"/>
<body name="ball" pos="0 0 1.53">
<joint armature="0" axis="1 0 0" damping="0.0" name="tar:x" pos="0 0 1.53" stiffness="0" type="slide" frictionloss="0" limited="false"/>
<joint armature="0" axis="0 1 0" damping="0.0" name="tar:y" pos="0 0 1.53" stiffness="0" type="slide" frictionloss="0" limited="false"/>
<joint armature="0" axis="0 0 1" damping="0.0" name="tar:z" pos="0 0 1.53" stiffness="0" type="slide" frictionloss="0" limited="false"/>
<geom pos="0 0 1.53" priority= "1" size="0.025 0.025 0.025" type="sphere" condim="4" name="ball_geom" rgba="0.8 0.2 0.1 1" mass="0.1"
friction="0.1 0.1 0.1" solimp="0.9 0.95 0.001 0.5 2" solref="-10000 -10"/>
<site name="target_ball" pos="0 0 1.53" size="0.04 0.04 0.04" rgba="1 0 0 1" type="sphere"/>
</body>
<body name="basket_ground" pos="5 0 0" gravcomp="0">
<geom name="basket_ground_geom" size="0.1 0.1 0.15" pos="0.15 0 0" quat="0.707107 0 -0.707107 0" type="box" friction="0.9 0.005 0.0001"/>
<body name="edge1" pos="0 0 0" gravcomp="0">
<geom name="edge1_geom" size="0.04 0.1" pos="0 0 0.1" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge2" pos="0 0 0.05" gravcomp="0">
<geom name="edge2_geom" size="0.04 0.1" pos="0 0.05 0.05" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge3" pos="0 0 0.1" gravcomp="0">
<geom name="edge3_geom" size="0.04 0.1" pos="0 0.1 0" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge4" pos="0 0 0.15" gravcomp="0">
<geom name="edge4_geom" size="0.04 0.1" pos="0 0.15 -0.05" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge5" pos="0.05 0 0.15" gravcomp="0">
<geom name="edge5_geom" size="0.04 0.1" pos="0 0.15 -0.05" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge6" pos="0.1 0 0.15" gravcomp="0">
<geom name="edge6_geom" size="0.04 0.1" pos="0 0.15 -0.05" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge7" pos="0.15 0 0.15" gravcomp="0">
<geom name="edge7_geom" size="0.04 0.1" pos="0 0.15 -0.05" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge8" pos="0.2 0 0.15" gravcomp="0">
<geom name="edge8_geom" size="0.04 0.1" pos="0 0.15 -0.05" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge9" pos="0.25 0 0.15" gravcomp="0">
<geom name="edge9_geom" size="0.04 0.1" pos="0 0.15 -0.05" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge10" pos="0.3 0 0.15" gravcomp="0">
<geom name="edge10_geom" size="0.04 0.1" pos="0 0.15 -0.05" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge11" pos="0.3 0 0.1" gravcomp="0">
<geom name="edge11_geom" size="0.04 0.1" pos="0 0.1 0" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge12" pos="0.3 0 0.05" gravcomp="0">
<geom name="edge12_geom" size="0.04 0.1" pos="0 0.05 0.05" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge13" pos="0.3 0 0" gravcomp="0">
<geom name="edge13_geom" size="0.04 0.1" pos="0 0 0.1" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge14" pos="0.3 0 -0.05" gravcomp="0">
<geom name="edge14_geom" size="0.04 0.1" pos="0 -0.05 0.15" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge15" pos="0.3 0 -0.1" gravcomp="0">
<geom name="edge15_geom" size="0.04 0.1" pos="0 -0.1 0.2" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge16" pos="0.3 0 -0.15" gravcomp="0">
<geom name="edge16_geom" size="0.04 0.1" pos="0 -0.15 0.25" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge20" pos="0.25 0 -0.15" gravcomp="0">
<geom name="edge20_geom" size="0.04 0.1" pos="0 -0.15 0.25" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge21" pos="0.2 0 -0.15" gravcomp="0">
<geom name="edge21_geom" size="0.04 0.1" pos="0 -0.15 0.25" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge22" pos="0.15 0 -0.15" gravcomp="0">
<geom name="edge22_geom" size="0.04 0.1" pos="0 -0.15 0.25" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge23" pos="0.1 0 -0.15" gravcomp="0">
<geom name="edge23_geom" size="0.04 0.1" pos="0 -0.15 0.25" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge24" pos="0.05 0 -0.15" gravcomp="0">
<geom name="edge24_geom" size="0.04 0.1" pos="0 -0.15 0.25" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge25" pos="0 0 -0.15" gravcomp="0">
<geom name="edge25_geom" size="0.04 0.1" pos="0 -0.15 0.25" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge26" pos="0 0 -0.1" gravcomp="0">
<geom name="edge26_geom" size="0.04 0.1" pos="0 -0.1 0.2" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="edge27" pos="0 0 -0.05" gravcomp="0">
<geom name="edge27_geom" size="0.04 0.1" pos="0 -0.05 0.15" quat="0 1 0 0" type="capsule" friction="2 0.005 0.0001"/>
</body>
<body name="basket_ground" pos="5 0 0">
<geom friction="0.9" fromto="5 0 0 5.3 0 0" name="basket_ground_geom" size="0.1 0.4 0.3" type="box"/>
<body name="edge1" pos="5 0 0">
<geom friction="2.0" fromto="5 0 0 5 0 0.2" name="edge1_geom" size="0.04" type="capsule"/>
</body>
<body name="edge2" pos="5 0 0.05">
<geom friction="2.0" fromto="5 0.05 0 5 0.05 0.2" name="edge2_geom" size="0.04" type="capsule"/>
</body>
<body name="edge3" pos="5 0 0.1">
<geom friction="2.0" fromto="5 0.1 0 5 0.1 0.2" name="edge3_geom" size="0.04" type="capsule"/>
</body>
<body name="edge4" pos="5 0 0.15">
<geom friction="2.0" fromto="5 0.15 0 5 0.15 0.2" name="edge4_geom" size="0.04" type="capsule"/>
</body>
<body name="edge5" pos="5.05 0 0.15">
<geom friction="2.0" fromto="5.05 0.15 0 5.05 0.15 0.2" name="edge5_geom" size="0.04" type="capsule"/>
</body>
<body name="edge6" pos="5.1 0 0.15">
<geom friction="2.0" fromto="5.1 0.15 0 5.1 0.15 0.2" name="edge6_geom" size="0.04" type="capsule"/>
</body>
<body name="edge7" pos="5.15 0 0.15">
<geom friction="2.0" fromto="5.15 0.15 0 5.15 0.15 0.2" name="edge7_geom" size="0.04" type="capsule"/>
</body>
<body name="edge8" pos="5.2 0 0.15">
<geom friction="2.0" fromto="5.2 0.15 0 5.2 0.15 0.2" name="edge8_geom" size="0.04" type="capsule"/>
</body>
<body name="edge9" pos="5.25 0 0.15">
<geom friction="2.0" fromto="5.25 0.15 0 5.25 0.15 0.2" name="edge9_geom" size="0.04" type="capsule"/>
</body>
<body name="edge10" pos="5.3 0 0.15">
<geom friction="2.0" fromto="5.3 0.15 0 5.3 0.15 0.2" name="edge10_geom" size="0.04" type="capsule"/>
</body>
<body name="edge11" pos="5.3 0 0.1">
<geom friction="2.0" fromto="5.3 0.1 0 5.3 0.1 0.2" name="edge11_geom" size="0.04" type="capsule"/>
</body>
<body name="edge12" pos="5.3 0 0.05">
<geom friction="2.0" fromto="5.3 0.05 0 5.3 0.05 0.2" name="edge12_geom" size="0.04" type="capsule"/>
</body>
<body name="edge13" pos="5.3 0 0.0">
<geom friction="2.0" fromto="5.3 0 0 5.3 0 0.2" name="edge13_geom" size="0.04" type="capsule"/>
</body>
<body name="edge14" pos="5.3 0 -0.05">
<geom friction="2.0" fromto="5.3 -0.05 0 5.3 -0.05 0.2" name="edge14_geom" size="0.04" type="capsule"/>
</body>
<body name="edge15" pos="5.3 0 -0.1">
<geom friction="2.0" fromto="5.3 -0.1 0 5.3 -0.1 0.2" name="edge15_geom" size="0.04" type="capsule"/>
</body>
<body name="edge16" pos="5.3 0 -0.15">
<geom friction="2.0" fromto="5.3 -0.15 0 5.3 -0.15 0.2" name="edge16_geom" size="0.04" type="capsule"/>
</body>
<body name="edge20" pos="5.25 0 -0.15">
<geom friction="2.0" fromto="5.25 -0.15 0 5.25 -0.15 0.2" name="edge20_geom" size="0.04" type="capsule"/>
</body>
<body name="edge21" pos="5.2 0 -0.15">
<geom friction="2.0" fromto="5.2 -0.15 0 5.2 -0.15 0.2" name="edge21_geom" size="0.04" type="capsule"/>
</body>
<body name="edge22" pos="5.15 0 -0.15">
<geom friction="2.0" fromto="5.15 -0.15 0 5.15 -0.15 0.2" name="edge22_geom" size="0.04" type="capsule"/>
</body>
<body name="edge23" pos="5.1 0 -0.15">
<geom friction="2.0" fromto="5.1 -0.15 0 5.1 -0.15 0.2" name="edge23_geom" size="0.04" type="capsule"/>
</body>
<body name="edge24" pos="5.05 0 -0.15">
<geom friction="2.0" fromto="5.05 -0.15 0 5.05 -0.15 0.2" name="edge24_geom" size="0.04" type="capsule"/>
</body>
<body name="edge25" pos="5 0 -0.15">
<geom friction="2.0" fromto="5 -0.15 0 5 -0.15 0.2" name="edge25_geom" size="0.04" type="capsule"/>
</body>
<body name="edge26" pos="5 0 -0.1">
<geom friction="2.0" fromto="5 -0.1 0 5 -0.1 0.2" name="edge26_geom" size="0.04" type="capsule"/>
</body>
<body name="edge27" pos="5 0 -0.05">
<geom friction="2.0" fromto="5 -0.05 0 5 -0.05 0.2" name="edge27_geom" size="0.04" type="capsule"/>
</body>
</body>
</worldbody>
<actuator>
<general joint="thigh_joint" ctrlrange="-1 1" gear="200 0 0 0 0 0" actdim="0"/>
<general joint="leg_joint" ctrlrange="-1 1" gear="200 0 0 0 0 0" actdim="0"/>
<general joint="foot_joint" ctrlrange="-1 1" gear="200 0 0 0 0 0" actdim="0"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="thigh_joint"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="leg_joint"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="200.0" joint="foot_joint"/>
</actuator>
</mujoco>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".4 .5 .6" rgb2="0 0 0"
width="100" height="100"/>
<texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
<texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
<material name="geom" texture="texgeom" texuniform="true"/>
</asset>
</mujoco>
@@ -1,15 +1,13 @@
import os
from typing import Optional, Any, Dict, Tuple
from typing import Optional
import numpy as np
from gymnasium.core import ObsType
from fancy_gym.envs.mujoco.hopper_jump.hopper_jump import HopperEnvCustomXML
from gymnasium import spaces
from gym.envs.mujoco.hopper_v4 import HopperEnv
MAX_EPISODE_STEPS_HOPPERTHROW = 250
class HopperThrowEnv(HopperEnvCustomXML):
class HopperThrowEnv(HopperEnv):
"""
Initialization changes to normal Hopper:
- healthy_reward: 1.0 -> 0.0 -> 0.1
@@ -32,23 +30,12 @@ class HopperThrowEnv(HopperEnvCustomXML):
reset_noise_scale=5e-3,
context=True,
exclude_current_positions_from_observation=True,
max_episode_steps=250,
**kwargs):
max_episode_steps=250):
xml_file = os.path.join(os.path.dirname(__file__), "assets", xml_file)
self.current_step = 0
self.max_episode_steps = max_episode_steps
self.context = context
self.goal = 0
if not hasattr(self, 'observation_space'):
self.observation_space = spaces.Box(
low=-np.inf, high=np.inf, shape=(18,), dtype=np.float64
)
else:
self.observation_space = spaces.Box(
low=-np.inf, high=np.inf, shape=(19,), dtype=np.float64
)
super().__init__(xml_file=xml_file,
forward_reward_weight=forward_reward_weight,
ctrl_cost_weight=ctrl_cost_weight,
@@ -58,8 +45,7 @@ class HopperThrowEnv(HopperEnvCustomXML):
healthy_z_range=healthy_z_range,
healthy_state_range=healthy_angle_range,
reset_noise_scale=reset_noise_scale,
exclude_current_positions_from_observation=exclude_current_positions_from_observation,
**kwargs)
exclude_current_positions_from_observation=exclude_current_positions_from_observation)
def step(self, action):
self.current_step += 1
@@ -70,14 +56,14 @@ class HopperThrowEnv(HopperEnvCustomXML):
# done = self.done TODO We should use this, not sure why there is no other termination; ball_landed should be enough, because we only look at the throw itself? - Paul and Marc
ball_landed = bool(self.get_body_com("ball")[2] <= 0.05)
terminated = ball_landed
done = ball_landed
ctrl_cost = self.control_cost(action)
costs = ctrl_cost
rewards = 0
if self.current_step >= self.max_episode_steps or terminated:
if self.current_step >= self.max_episode_steps or done:
distance_reward = -np.linalg.norm(ball_pos_after - self.goal) if self.context else \
self._forward_reward_weight * ball_pos_after
healthy_reward = 0 if self.context else self.healthy_reward * self.current_step
@@ -92,19 +78,16 @@ class HopperThrowEnv(HopperEnvCustomXML):
'_steps': self.current_step,
'goal': self.goal,
}
truncated = False
return observation, reward, terminated, truncated, info
return observation, reward, done, info
def _get_obs(self):
return np.append(super()._get_obs(), self.goal)
def reset(self, *, seed: Optional[int] = None, options: Optional[Dict[str, Any]] = None) \
-> Tuple[ObsType, Dict[str, Any]]:
def reset(self, *, seed: Optional[int] = None, return_info: bool = False, options: Optional[dict] = None):
self.current_step = 0
ret = super().reset(seed=seed, options=options)
self.goal = self.goal = self.np_random.uniform(2.0, 6.0, 1) # 0.5 8.0
return ret
return super().reset()
# overwrite reset_model to make it deterministic
def reset_model(self):
@@ -118,3 +101,22 @@ class HopperThrowEnv(HopperEnvCustomXML):
observation = self._get_obs()
return observation
if __name__ == '__main__':
render_mode = "human" # "human" or "partial" or "final"
env = HopperThrowEnv()
obs = env.reset()
for i in range(2000):
# objective.load_result("/tmp/cma")
# test with random actions
ac = env.action_space.sample()
obs, rew, d, info = env.step(ac)
if i % 10 == 0:
env.render(mode=render_mode)
if d:
print('After ', i, ' steps, done: ', d)
env.reset()
env.close()
@@ -1,16 +1,13 @@
import os
from typing import Optional, Any, Dict, Tuple
from typing import Optional
import numpy as np
from fancy_gym.envs.mujoco.hopper_jump.hopper_jump import HopperEnvCustomXML
from gymnasium.core import ObsType
from gymnasium import spaces
from gym.envs.mujoco.hopper_v4 import HopperEnv
MAX_EPISODE_STEPS_HOPPERTHROWINBASKET = 250
class HopperThrowInBasketEnv(HopperEnvCustomXML):
class HopperThrowInBasketEnv(HopperEnv):
"""
Initialization changes to normal Hopper:
- healthy_reward: 1.0 -> 0.0
@@ -36,8 +33,7 @@ class HopperThrowInBasketEnv(HopperEnvCustomXML):
context=True,
penalty=0.0,
exclude_current_positions_from_observation=True,
max_episode_steps=250,
**kwargs):
max_episode_steps=250):
self.hit_basket_reward = hit_basket_reward
self.current_step = 0
self.max_episode_steps = max_episode_steps
@@ -46,16 +42,6 @@ class HopperThrowInBasketEnv(HopperEnvCustomXML):
self.context = context
self.penalty = penalty
self.basket_x = 5
if exclude_current_positions_from_observation:
self.observation_space = spaces.Box(
low=-np.inf, high=np.inf, shape=(18,), dtype=np.float64
)
else:
self.observation_space = spaces.Box(
low=-np.inf, high=np.inf, shape=(19,), dtype=np.float64
)
xml_file = os.path.join(os.path.dirname(__file__), "assets", xml_file)
super().__init__(xml_file=xml_file,
forward_reward_weight=forward_reward_weight,
@@ -66,8 +52,7 @@ class HopperThrowInBasketEnv(HopperEnvCustomXML):
healthy_z_range=healthy_z_range,
healthy_angle_range=healthy_angle_range,
reset_noise_scale=reset_noise_scale,
exclude_current_positions_from_observation=exclude_current_positions_from_observation,
**kwargs)
exclude_current_positions_from_observation=exclude_current_positions_from_observation)
def step(self, action):
@@ -80,14 +65,14 @@ class HopperThrowInBasketEnv(HopperEnvCustomXML):
is_in_basket_x = ball_pos[0] >= basket_pos[0] and ball_pos[0] <= basket_pos[0] + self.basket_size
is_in_basket_y = ball_pos[1] >= basket_pos[1] - (self.basket_size / 2) and ball_pos[1] <= basket_pos[1] + (
self.basket_size / 2)
self.basket_size / 2)
is_in_basket_z = ball_pos[2] < 0.1
is_in_basket = is_in_basket_x and is_in_basket_y and is_in_basket_z
if is_in_basket:
self.ball_in_basket = True
ball_landed = self.get_body_com("ball")[2] <= 0.05
terminated = bool(ball_landed or is_in_basket)
done = bool(ball_landed or is_in_basket)
rewards = 0
@@ -95,7 +80,7 @@ class HopperThrowInBasketEnv(HopperEnvCustomXML):
costs = ctrl_cost
if self.current_step >= self.max_episode_steps or terminated:
if self.current_step >= self.max_episode_steps or done:
if is_in_basket:
if not self.context:
@@ -116,27 +101,23 @@ class HopperThrowInBasketEnv(HopperEnvCustomXML):
info = {
'ball_pos': ball_pos[0],
}
truncated = False
return observation, reward, terminated, truncated, info
return observation, reward, done, info
def _get_obs(self):
return np.append(super()._get_obs(), self.basket_x)
def reset(self, *, seed: Optional[int] = None, options: Optional[Dict[str, Any]] = None) \
-> Tuple[ObsType, Dict[str, Any]]:
def reset(self, *, seed: Optional[int] = None, return_info: bool = False, options: Optional[dict] = None):
if self.max_episode_steps == 10:
# We have to initialize this here, because the spec is only added after creating the env.
self.max_episode_steps = self.spec.max_episode_steps
self.current_step = 0
self.ball_in_basket = False
ret = super().reset(seed=seed, options=options)
if self.context:
self.basket_x = self.np_random.uniform(low=3, high=7, size=1)
self.model.body("basket_ground").pos[:] = [self.basket_x[0], 0, 0]
return ret
return super().reset()
# overwrite reset_model to make it deterministic
def reset_model(self):
@@ -151,3 +132,22 @@ class HopperThrowInBasketEnv(HopperEnvCustomXML):
observation = self._get_obs()
return observation
if __name__ == '__main__':
render_mode = "human" # "human" or "partial" or "final"
env = HopperThrowInBasketEnv()
obs = env.reset()
for i in range(2000):
# objective.load_result("/tmp/cma")
# test with random actions
ac = env.action_space.sample()
obs, rew, d, info = env.step(ac)
if i % 10 == 0:
env.render(mode=render_mode)
if d:
print('After ', i, ' steps, done: ', d)
env.reset()
env.close()
@@ -6,11 +6,6 @@ from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
class MPWrapper(RawInterfaceWrapper):
mp_config = {
'ProMP': {},
'DMP': {},
'ProDMP': {},
}
@property
def context_mask(self):
@@ -7,16 +7,6 @@ from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
class MPWrapper(RawInterfaceWrapper):
mp_config = {
'ProMP': {},
'DMP': {
'phase_generator_kwargs': {
'alpha_phase': 2,
},
},
'ProDMP': {},
}
@property
def context_mask(self):
return np.concatenate([[False] * self.n_links, # cos
+7 -29
View File
@@ -1,9 +1,8 @@
import os
import numpy as np
from gymnasium import utils
from gymnasium.envs.mujoco import MujocoEnv
from gymnasium.spaces import Box
from gym import utils
from gym.envs.mujoco import MujocoEnv
MAX_EPISODE_STEPS_REACHER = 200
@@ -13,17 +12,7 @@ class ReacherEnv(MujocoEnv, utils.EzPickle):
More general version of the gym mujoco Reacher environment
"""
metadata = {
"render_modes": [
"human",
"rgb_array",
"depth_array",
],
"render_fps": 50,
}
def __init__(self, sparse: bool = False, n_links: int = 5, reward_weight: float = 1, ctrl_cost_weight: float = 1.,
**kwargs):
def __init__(self, sparse: bool = False, n_links: int = 5, reward_weight: float = 1, ctrl_cost_weight: float = 1):
utils.EzPickle.__init__(**locals())
self._steps = 0
@@ -36,16 +25,10 @@ class ReacherEnv(MujocoEnv, utils.EzPickle):
file_name = f'reacher_{n_links}links.xml'
# sin, cos, velocity * n_Links + goal position (2) and goal distance (3)
shape = (self.n_links * 3 + 5,)
observation_space = Box(low=-np.inf, high=np.inf, shape=shape, dtype=np.float64)
MujocoEnv.__init__(self,
model_path=os.path.join(os.path.dirname(__file__), "assets", file_name),
frame_skip=2,
observation_space=observation_space,
**kwargs
)
mujoco_bindings="mujoco")
def step(self, action):
self._steps += 1
@@ -62,14 +45,10 @@ class ReacherEnv(MujocoEnv, utils.EzPickle):
reward = reward_dist + reward_ctrl + angular_vel
self.do_simulation(action, self.frame_skip)
if self.render_mode == "human":
self.render()
ob = self._get_obs()
terminated = False
truncated = False
done = False
info = dict(
infos = dict(
reward_dist=reward_dist,
reward_ctrl=reward_ctrl,
velocity=angular_vel,
@@ -77,7 +56,7 @@ class ReacherEnv(MujocoEnv, utils.EzPickle):
goal=self.goal if hasattr(self, "goal") else None
)
return ob, reward, terminated, truncated, info
return ob, reward, done, infos
def distance_reward(self):
vec = self.get_body_com("fingertip") - self.get_body_com("target")
@@ -87,7 +66,6 @@ class ReacherEnv(MujocoEnv, utils.EzPickle):
return -10 * np.square(self.data.qvel.flat[:self.n_links]).sum() if self.sparse else 0.0
def viewer_setup(self):
assert self.viewer is not None
self.viewer.cam.trackbodyid = 0
def reset_model(self):
@@ -7,53 +7,6 @@ from fancy_gym.envs.mujoco.table_tennis.table_tennis_utils import jnt_pos_low, j
class TT_MPWrapper(RawInterfaceWrapper):
mp_config = {
'ProMP': {
'phase_generator_kwargs': {
'learn_tau': False,
'learn_delay': False,
'tau_bound': [0.8, 1.5],
'delay_bound': [0.05, 0.15],
},
'controller_kwargs': {
'p_gains': 0.5 * np.array([1.0, 4.0, 2.0, 4.0, 1.0, 4.0, 1.0]),
'd_gains': 0.5 * np.array([0.1, 0.4, 0.2, 0.4, 0.1, 0.4, 0.1]),
},
'basis_generator_kwargs': {
'num_basis': 3,
'num_basis_zero_start': 1,
'num_basis_zero_goal': 1,
},
'black_box_kwargs': {
'verbose': 2,
},
},
'DMP': {},
'ProDMP': {
'phase_generator_kwargs': {
'learn_tau': True,
'learn_delay': True,
'tau_bound': [0.8, 1.5],
'delay_bound': [0.05, 0.15],
'alpha_phase': 3,
},
'controller_kwargs': {
'p_gains': 0.5 * np.array([1.0, 4.0, 2.0, 4.0, 1.0, 4.0, 1.0]),
'd_gains': 0.5 * np.array([0.1, 0.4, 0.2, 0.4, 0.1, 0.4, 0.1]),
},
'basis_generator_kwargs': {
'num_basis': 3,
'alpha': 25,
'basis_bandwidth_factor': 3,
},
'trajectory_generator_kwargs': {
'weights_scale': 0.7,
'auto_scale_basis': True,
'relative_goal': True,
'disable_goal': True,
},
},
}
# Random x goal + random init pos
@property
@@ -63,7 +16,7 @@ class TT_MPWrapper(RawInterfaceWrapper):
[False] * 7, # joints velocity
[True] * 2, # position ball x, y
[False] * 1, # position ball z
# [True] * 3, # velocity ball x, y, z
#[True] * 3, # velocity ball x, y, z
[True] * 2, # target landing position
# [True] * 1, # time
])
@@ -87,42 +40,7 @@ class TT_MPWrapper(RawInterfaceWrapper):
return_contextual_obs: bool, tau_bound:list, delay_bound:list) -> Tuple[np.ndarray, float, bool, dict]:
return self.get_invalid_traj_step_return(action, pos_traj, return_contextual_obs, tau_bound, delay_bound)
class TT_MPWrapper_Replan(TT_MPWrapper):
mp_config = {
'ProMP': {},
'DMP': {},
'ProDMP': {
'phase_generator_kwargs': {
'learn_tau': True,
'learn_delay': True,
'tau_bound': [0.8, 1.5],
'delay_bound': [0.05, 0.15],
'alpha_phase': 3,
},
'controller_kwargs': {
'p_gains': 0.5 * np.array([1.0, 4.0, 2.0, 4.0, 1.0, 4.0, 1.0]),
'd_gains': 0.5 * np.array([0.1, 0.4, 0.2, 0.4, 0.1, 0.4, 0.1]),
},
'basis_generator_kwargs': {
'num_basis': 2,
'alpha': 25,
'basis_bandwidth_factor': 3,
},
'trajectory_generator_kwargs': {
'auto_scale_basis': True,
'goal_offset': 1.0,
},
'black_box_kwargs': {
'max_planning_times': 3,
'replanning_schedule': lambda pos, vel, obs, action, t: t % 50 == 0,
},
},
}
class TTVelObs_MPWrapper(TT_MPWrapper):
# Will inherit mp_config from TT_MPWrapper
@property
def context_mask(self):
@@ -134,20 +52,4 @@ class TTVelObs_MPWrapper(TT_MPWrapper):
[True] * 3, # velocity ball x, y, z
[True] * 2, # target landing position
# [True] * 1, # time
])
class TTVelObs_MPWrapper_Replan(TT_MPWrapper_Replan):
# Will inherit mp_config from TT_MPWrapper_Replan
@property
def context_mask(self):
return np.hstack([
[False] * 7, # joints position
[False] * 7, # joints velocity
[True] * 2, # position ball x, y
[False] * 1, # position ball z
[True] * 3, # velocity ball x, y, z
[True] * 2, # target landing position
# [True] * 1, # time
])
])
@@ -1,8 +1,8 @@
import os
import numpy as np
from gymnasium import utils, spaces
from gymnasium.envs.mujoco import MujocoEnv
from gym import utils, spaces
from gym.envs.mujoco import MujocoEnv
from fancy_gym.envs.mujoco.table_tennis.table_tennis_utils import is_init_state_valid, magnus_force
from fancy_gym.envs.mujoco.table_tennis.table_tennis_utils import jnt_pos_low, jnt_pos_high
@@ -22,19 +22,9 @@ class TableTennisEnv(MujocoEnv, utils.EzPickle):
"""
7 DoF table tennis environment
"""
metadata = {
"render_modes": [
"human",
"rgb_array",
"depth_array",
],
"render_fps": 125
}
def __init__(self, ctxt_dim: int = 4, frame_skip: int = 4,
goal_switching_step: int = None,
enable_artificial_wind: bool = False, **kwargs):
enable_artificial_wind: bool = False):
utils.EzPickle.__init__(**locals())
self._steps = 0
@@ -60,17 +50,11 @@ class TableTennisEnv(MujocoEnv, utils.EzPickle):
self._artificial_force = 0.
if not hasattr(self, 'observation_space'):
self.observation_space = spaces.Box(
low=-np.inf, high=np.inf, shape=(19,), dtype=np.float64
)
MujocoEnv.__init__(self,
model_path=os.path.join(os.path.dirname(__file__), "assets", "xml", "table_tennis_env.xml"),
frame_skip=frame_skip,
observation_space=self.observation_space,
**kwargs)
mujoco_bindings="mujoco")
if ctxt_dim == 2:
self.context_bounds = CONTEXT_BOUNDS_2DIMS
elif ctxt_dim == 4:
@@ -99,11 +83,11 @@ class TableTennisEnv(MujocoEnv, utils.EzPickle):
unstable_simulation = False
if self._steps == self._goal_switching_step and self.np_random.uniform() < 0.5:
new_goal_pos = self._generate_goal_pos(random=True)
new_goal_pos[1] = -new_goal_pos[1]
self._goal_pos = new_goal_pos
self.model.body_pos[5] = np.concatenate([self._goal_pos, [0.77]])
mujoco.mj_forward(self.model, self.data)
new_goal_pos = self._generate_goal_pos(random=True)
new_goal_pos[1] = -new_goal_pos[1]
self._goal_pos = new_goal_pos
self.model.body_pos[5] = np.concatenate([self._goal_pos, [0.77]])
mujoco.mj_forward(self.model, self.data)
for _ in range(self.frame_skip):
if self._enable_artificial_wind:
@@ -118,7 +102,7 @@ class TableTennisEnv(MujocoEnv, utils.EzPickle):
if not self._hit_ball:
self._hit_ball = self._contact_checker(self._ball_contact_id, self._bat_front_id) or \
self._contact_checker(self._ball_contact_id, self._bat_back_id)
self._contact_checker(self._ball_contact_id, self._bat_back_id)
if not self._hit_ball:
ball_land_on_floor_no_hit = self._contact_checker(self._ball_contact_id, self._floor_contact_id)
if ball_land_on_floor_no_hit:
@@ -146,9 +130,9 @@ class TableTennisEnv(MujocoEnv, utils.EzPickle):
reward = -25 if unstable_simulation else self._get_reward(self._terminated)
land_dist_err = np.linalg.norm(self._ball_landing_pos[:-1] - self._goal_pos) \
if self._ball_landing_pos is not None else 10.
if self._ball_landing_pos is not None else 10.
info = {
return self._get_obs(), reward, self._terminated, {
"hit_ball": self._hit_ball,
"ball_returned_success": self._ball_return_success,
"land_dist_error": land_dist_err,
@@ -156,10 +140,6 @@ class TableTennisEnv(MujocoEnv, utils.EzPickle):
"num_steps": self._steps,
}
terminated, truncated = self._terminated, False
return self._get_obs(), reward, terminated, truncated, info
def _contact_checker(self, id_1, id_2):
for coni in range(0, self.data.ncon):
con = self.data.contact[coni]
@@ -222,7 +202,7 @@ class TableTennisEnv(MujocoEnv, utils.EzPickle):
if not self._hit_ball:
return 0.2 * (1 - np.tanh(min_r_b_dist**2))
if self._ball_landing_pos is None:
min_b_des_b_dist = np.min(np.linalg.norm(np.array(self._ball_traj)[:, :2] - self._goal_pos[:2], axis=1))
min_b_des_b_dist = np.min(np.linalg.norm(np.array(self._ball_traj)[:,:2] - self._goal_pos[:2], axis=1))
return 2 * (1 - np.tanh(min_r_b_dist ** 2)) + (1 - np.tanh(min_b_des_b_dist**2))
min_b_des_b_land_dist = np.linalg.norm(self._goal_pos[:2] - self._ball_landing_pos[:2])
over_net_bonus = int(self._ball_landing_pos[0] < 0)
@@ -251,13 +231,13 @@ class TableTennisEnv(MujocoEnv, utils.EzPickle):
violate_high_bound_error = np.mean(np.maximum(pos_traj - jnt_pos_high, 0))
violate_low_bound_error = np.mean(np.maximum(jnt_pos_low - pos_traj, 0))
invalid_penalty = tau_invalid_penalty + delay_invalid_penalty + \
violate_high_bound_error + violate_low_bound_error
violate_high_bound_error + violate_low_bound_error
return -invalid_penalty
def get_invalid_traj_step_return(self, action, pos_traj, contextual_obs, tau_bound, delay_bound):
obs = self._get_obs() if contextual_obs else np.concatenate([self._get_obs(), np.array([0])]) # 0 for invalid traj
obs = self._get_obs() if contextual_obs else np.concatenate([self._get_obs(), np.array([0])]) # 0 for invalid traj
penalty = self._get_traj_invalid_penalty(action, pos_traj, tau_bound, delay_bound)
return obs, penalty, True, False, {
return obs, penalty, True, {
"hit_ball": [False],
"ball_returned_success": [False],
"land_dist_error": [10.],
@@ -269,18 +249,15 @@ class TableTennisEnv(MujocoEnv, utils.EzPickle):
@staticmethod
def check_traj_validity(action, pos_traj, vel_traj, tau_bound, delay_bound):
time_invalid = action[0] > tau_bound[1] or action[0] < tau_bound[0] \
or action[1] > delay_bound[1] or action[1] < delay_bound[0]
or action[1] > delay_bound[1] or action[1] < delay_bound[0]
if time_invalid or np.any(pos_traj > jnt_pos_high) or np.any(pos_traj < jnt_pos_low):
return False, pos_traj, vel_traj
return True, pos_traj, vel_traj
class TableTennisWind(TableTennisEnv):
def __init__(self, ctxt_dim: int = 4, frame_skip: int = 4, **kwargs):
self.observation_space = spaces.Box(
low=-np.inf, high=np.inf, shape=(22,), dtype=np.float64
)
super().__init__(ctxt_dim=ctxt_dim, frame_skip=frame_skip, enable_artificial_wind=True, **kwargs)
def __init__(self, ctxt_dim: int = 4, frame_skip: int = 4):
super().__init__(ctxt_dim=ctxt_dim, frame_skip=frame_skip, enable_artificial_wind=True)
def _get_obs(self):
obs = np.concatenate([
@@ -298,5 +275,5 @@ class TableTennisWind(TableTennisEnv):
class TableTennisGoalSwitching(TableTennisEnv):
def __init__(self, frame_skip: int = 4, goal_switching_step: int = 99, **kwargs):
super().__init__(frame_skip=frame_skip, goal_switching_step=goal_switching_step, **kwargs)
def __init__(self, frame_skip: int = 4, goal_switching_step: int = 99):
super().__init__(frame_skip=frame_skip, goal_switching_step=goal_switching_step)
@@ -1,60 +1,64 @@
<mujoco model="walker2d">
<compiler angle="radian" autolimits="true"/>
<option integrator="RK4"/>
<default class="main">
<joint limited="true" armature="0.01" damping="0.1"/>
<geom conaffinity="0" friction="0.7 0.1 0.1" rgba="0.8 0.6 0.4 1"/>
<compiler angle="degree" coordinate="global" inertiafromgeom="true"/>
<default>
<joint armature="0.01" damping=".1" limited="true"/>
<geom conaffinity="0" condim="3" contype="1" density="1000" friction=".7 .1 .1" rgba="0.8 0.6 .4 1"/>
</default>
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.4 0.5 0.6" rgb2="0 0 0" width="100" height="600"/>
<texture type="cube" name="texgeom" builtin="flat" mark="cross" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" width="127" height="762"/>
<texture type="2d" name="texplane" builtin="checker" rgb1="0 0 0" rgb2="0.8 0.8 0.8" width="100" height="100"/>
<material name="MatPlane" texture="texplane" texrepeat="60 60" specular="1" shininess="1" reflectance="0.5"/>
<material name="geom" texture="texgeom" texuniform="true"/>
</asset>
<option integrator="RK4" timestep="0.002"/>
<worldbody>
<geom name="floor" size="40 40 40" type="plane" conaffinity="1" material="MatPlane" rgba="0.8 0.9 0.8 1"/>
<light pos="0 0 1.3" dir="0 0 -1" directional="true" cutoff="100" exponent="1" diffuse="1 1 1" specular="0.1 0.1 0.1"/>
<body name="torso" pos="0 0 1.25" gravcomp="0">
<joint name="rootx" pos="0 0 -1.25" axis="1 0 0" limited="false" type="slide" armature="0" damping="0"/>
<joint name="rootz" pos="0 0 -1.25" axis="0 0 1" limited="false" type="slide" ref="1.25" armature="0" damping="0"/>
<joint name="rooty" pos="0 0 0" axis="0 1 0" limited="false" armature="0" damping="0"/>
<geom name="torso_geom" size="0.05 0.2" type="capsule" friction="0.9 0.1 0.1"/>
<camera name="track" pos="0 -3 -0.25" quat="0.707107 0.707107 0 0" mode="trackcom"/>
<body name="thigh" pos="0 0 -0.2" gravcomp="0">
<joint name="thigh_joint" pos="0 0 0" axis="0 -1 0" range="-2.61799 0"/>
<geom name="thigh_geom" size="0.05 0.225" pos="0 0 -0.225" type="capsule" friction="0.9 0.1 0.1"/>
<body name="leg" pos="0 0 -0.7" gravcomp="0">
<joint name="leg_joint" pos="0 0 0.25" axis="0 -1 0" range="-2.61799 0"/>
<geom name="leg_geom" size="0.04 0.25" type="capsule" friction="0.9 0.1 0.1"/>
<body name="foot" pos="0.1 0 -0.25" gravcomp="0">
<joint name="foot_joint" pos="-0.1 0 0" axis="0 -1 0" range="-0.785398 0.785398"/>
<geom name="foot_geom" size="0.06 0.1" quat="0.707107 0 -0.707107 0" type="capsule" friction="0.9 0.1 0.1"/>
<site name="foot_right_site" pos="-0.1 0 -0.06" size="0.02" rgba="0 0 1 1"/>
<light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
<geom conaffinity="1" condim="3" name="floor" pos="0 0 0" rgba="0.8 0.9 0.8 1" size="40 40 40" type="plane" material="MatPlane"/>
<body name="torso" pos="0 0 1.25">
<camera name="track" mode="trackcom" pos="0 -3 1" xyaxes="1 0 0 0 0 1"/>
<joint armature="0" axis="1 0 0" damping="0" limited="false" name="rootx" pos="0 0 0" stiffness="0" type="slide"/>
<joint armature="0" axis="0 0 1" damping="0" limited="false" name="rootz" pos="0 0 0" ref="1.25" stiffness="0" type="slide"/>
<joint armature="0" axis="0 1 0" damping="0" limited="false" name="rooty" pos="0 0 1.25" stiffness="0" type="hinge"/>
<geom friction="0.9" fromto="0 0 1.45 0 0 1.05" name="torso_geom" size="0.05" type="capsule"/>
<body name="thigh" pos="0 0 1.05">
<joint axis="0 -1 0" name="thigh_joint" pos="0 0 1.05" range="-150 0" type="hinge"/>
<geom friction="0.9" fromto="0 0 1.05 0 0 0.6" name="thigh_geom" size="0.05" type="capsule"/>
<body name="leg" pos="0 0 0.35">
<joint axis="0 -1 0" name="leg_joint" pos="0 0 0.6" range="-150 0" type="hinge"/>
<geom friction="0.9" fromto="0 0 0.6 0 0 0.1" name="leg_geom" size="0.04" type="capsule"/>
<body name="foot" pos="0.2/2 0 0.1">
<site name="foot_right_site" pos="0 0 0.04" size="0.02 0.02 0.02" rgba="0 0 1 1" type="sphere"/>
<joint axis="0 -1 0" name="foot_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
<geom friction="0.9" fromto="-0.0 0 0.1 0.2 0 0.1" name="foot_geom" size="0.06" type="capsule"/>
</body>
</body>
</body>
<body name="thigh_left" pos="0 0 -0.2" gravcomp="0">
<joint name="thigh_left_joint" pos="0 0 0" axis="0 -1 0" range="-2.61799 0"/>
<geom name="thigh_left_geom" size="0.05 0.225" pos="0 0 -0.225" type="capsule" friction="0.9 0.1 0.1" rgba="0.7 0.3 0.6 1"/>
<body name="leg_left" pos="0 0 -0.7" gravcomp="0">
<joint name="leg_left_joint" pos="0 0 0.25" axis="0 -1 0" range="-2.61799 0"/>
<geom name="leg_left_geom" size="0.04 0.25" type="capsule" friction="0.9 0.1 0.1" rgba="0.7 0.3 0.6 1"/>
<body name="foot_left" pos="0.1 0 -0.25" gravcomp="0">
<joint name="foot_left_joint" pos="-0.1 0 0" axis="0 -1 0" range="-0.785398 0.785398"/>
<geom name="foot_left_geom" size="0.06 0.1" quat="0.707107 0 -0.707107 0" type="capsule" friction="1.9 0.1 0.1" rgba="0.7 0.3 0.6 1"/>
<site name="foot_left_site" pos="-0.1 0 -0.06" size="0.02" rgba="1 0 0 1"/>
<!-- copied and then replace thigh->thigh_left, leg->leg_left, foot->foot_right -->
<body name="thigh_left" pos="0 0 1.05">
<joint axis="0 -1 0" name="thigh_left_joint" pos="0 0 1.05" range="-150 0" type="hinge"/>
<geom friction="0.9" fromto="0 0 1.05 0 0 0.6" name="thigh_left_geom" rgba=".7 .3 .6 1" size="0.05" type="capsule"/>
<body name="leg_left" pos="0 0 0.35">
<joint axis="0 -1 0" name="leg_left_joint" pos="0 0 0.6" range="-150 0" type="hinge"/>
<geom friction="0.9" fromto="0 0 0.6 0 0 0.1" name="leg_left_geom" rgba=".7 .3 .6 1" size="0.04" type="capsule"/>
<body name="foot_left" pos="0.2/2 0 0.1">
<site name="foot_left_site" pos="0 0 0.04" size="0.02 0.02 0.02" rgba="1 0 0 1" type="sphere"/>
<joint axis="0 -1 0" name="foot_left_joint" pos="0 0 0.1" range="-45 45" type="hinge"/>
<geom friction="1.9" fromto="-0.0 0 0.1 0.2 0 0.1" name="foot_left_geom" rgba=".7 .3 .6 1" size="0.06" type="capsule"/>
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<general joint="thigh_joint" ctrlrange="-1 1" gear="100 0 0 0 0 0" actdim="0"/>
<general joint="leg_joint" ctrlrange="-1 1" gear="100 0 0 0 0 0" actdim="0"/>
<general joint="foot_joint" ctrlrange="-1 1" gear="100 0 0 0 0 0" actdim="0"/>
<general joint="thigh_left_joint" ctrlrange="-1 1" gear="100 0 0 0 0 0" actdim="0"/>
<general joint="leg_left_joint" ctrlrange="-1 1" gear="100 0 0 0 0 0" actdim="0"/>
<general joint="foot_left_joint" ctrlrange="-1 1" gear="100 0 0 0 0 0" actdim="0"/>
<!-- <motor joint="torso_joint" ctrlrange="-100.0 100.0" isctrllimited="true"/>-->
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="100" joint="thigh_joint"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="100" joint="leg_joint"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="100" joint="foot_joint"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="100" joint="thigh_left_joint"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="100" joint="leg_left_joint"/>
<motor ctrllimited="true" ctrlrange="-1.0 1.0" gear="100" joint="foot_left_joint"/>
<!-- <motor joint="finger2_rot" ctrlrange="-20.0 20.0" isctrllimited="true"/>-->
</actuator>
<asset>
<texture type="skybox" builtin="gradient" rgb1=".4 .5 .6" rgb2="0 0 0"
width="100" height="100"/>
<texture builtin="flat" height="1278" mark="cross" markrgb="1 1 1" name="texgeom" random="0.01" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" type="cube" width="127"/>
<texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="60 60" texture="texplane"/>
<material name="geom" texture="texgeom" texuniform="true"/>
</asset>
</mujoco>
@@ -6,11 +6,6 @@ from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
class MPWrapper(RawInterfaceWrapper):
mp_config = {
'ProMP': {},
'DMP': {},
'ProDMP': {},
}
@property
def context_mask(self):
@@ -1,13 +1,8 @@
import os
from typing import Optional, Any, Dict, Tuple
from typing import Optional
import numpy as np
from gymnasium.envs.mujoco.walker2d_v4 import Walker2dEnv, DEFAULT_CAMERA_CONFIG
from gymnasium.core import ObsType
from gymnasium import utils
from gymnasium.envs.mujoco import MujocoEnv
from gymnasium.spaces import Box
from gym.envs.mujoco.walker2d_v4 import Walker2dEnv
MAX_EPISODE_STEPS_WALKERJUMP = 300
@@ -16,71 +11,8 @@ MAX_EPISODE_STEPS_WALKERJUMP = 300
# to the same structure as the Hopper, where the angles are randomized (->contexts) and the agent should jump as height
# as possible, while landing at a specific target position
class Walker2dEnvCustomXML(Walker2dEnv):
def __init__(
self,
xml_file,
forward_reward_weight=1.0,
ctrl_cost_weight=1e-3,
healthy_reward=1.0,
terminate_when_unhealthy=True,
healthy_z_range=(0.8, 2.0),
healthy_angle_range=(-1.0, 1.0),
reset_noise_scale=5e-3,
exclude_current_positions_from_observation=True,
**kwargs,
):
utils.EzPickle.__init__(
self,
xml_file,
forward_reward_weight,
ctrl_cost_weight,
healthy_reward,
terminate_when_unhealthy,
healthy_z_range,
healthy_angle_range,
reset_noise_scale,
exclude_current_positions_from_observation,
**kwargs,
)
self._forward_reward_weight = forward_reward_weight
self._ctrl_cost_weight = ctrl_cost_weight
self._healthy_reward = healthy_reward
self._terminate_when_unhealthy = terminate_when_unhealthy
self._healthy_z_range = healthy_z_range
self._healthy_angle_range = healthy_angle_range
self._reset_noise_scale = reset_noise_scale
self._exclude_current_positions_from_observation = (
exclude_current_positions_from_observation
)
if exclude_current_positions_from_observation:
observation_space = Box(
low=-np.inf, high=np.inf, shape=(18,), dtype=np.float64
)
else:
observation_space = Box(
low=-np.inf, high=np.inf, shape=(19,), dtype=np.float64
)
self.observation_space = observation_space
MujocoEnv.__init__(
self,
xml_file,
4,
observation_space=observation_space,
default_camera_config=DEFAULT_CAMERA_CONFIG,
**kwargs,
)
class Walker2dJumpEnv(Walker2dEnvCustomXML):
class Walker2dJumpEnv(Walker2dEnv):
"""
healthy reward 1.0 -> 0.005 -> 0.0025 not from alex
penalty 10 -> 0 not from alex
@@ -97,8 +29,7 @@ class Walker2dJumpEnv(Walker2dEnvCustomXML):
reset_noise_scale=5e-3,
penalty=0,
exclude_current_positions_from_observation=True,
max_episode_steps=300,
**kwargs):
max_episode_steps=300):
self.current_step = 0
self.max_episode_steps = max_episode_steps
self.max_height = 0
@@ -113,8 +44,7 @@ class Walker2dJumpEnv(Walker2dEnvCustomXML):
healthy_z_range=healthy_z_range,
healthy_angle_range=healthy_angle_range,
reset_noise_scale=reset_noise_scale,
exclude_current_positions_from_observation=exclude_current_positions_from_observation,
**kwargs)
exclude_current_positions_from_observation=exclude_current_positions_from_observation)
def step(self, action):
self.current_step += 1
@@ -124,13 +54,13 @@ class Walker2dJumpEnv(Walker2dEnvCustomXML):
self.max_height = max(height, self.max_height)
terminated = bool(height < 0.2)
done = bool(height < 0.2)
ctrl_cost = self.control_cost(action)
costs = ctrl_cost
rewards = 0
if self.current_step >= self.max_episode_steps or terminated:
terminated = True
if self.current_step >= self.max_episode_steps or done:
done = True
height_goal_distance = -10 * (np.linalg.norm(self.max_height - self.goal))
healthy_reward = self.healthy_reward * self.current_step
@@ -143,20 +73,17 @@ class Walker2dJumpEnv(Walker2dEnvCustomXML):
'max_height': self.max_height,
'goal': self.goal,
}
truncated = False
return observation, reward, terminated, truncated, info
return observation, reward, done, info
def _get_obs(self):
return np.append(super()._get_obs(), self.goal)
def reset(self, *, seed: Optional[int] = None, options: Optional[Dict[str, Any]] = None) \
-> Tuple[ObsType, Dict[str, Any]]:
def reset(self, *, seed: Optional[int] = None, return_info: bool = False, options: Optional[dict] = None):
self.current_step = 0
self.max_height = 0
ret = super().reset(seed=seed, options=options)
self.goal = self.np_random.uniform(1.5, 2.5, 1) # 1.5 3.0
return ret
return super().reset()
# overwrite reset_model to make it deterministic
def reset_model(self):
@@ -170,3 +97,21 @@ class Walker2dJumpEnv(Walker2dEnvCustomXML):
observation = self._get_obs()
return observation
if __name__ == '__main__':
render_mode = "human" # "human" or "partial" or "final"
env = Walker2dJumpEnv()
obs = env.reset()
for i in range(6000):
# test with random actions
ac = env.action_space.sample()
obs, rew, d, info = env.step(ac)
if i % 10 == 0:
env.render(mode=render_mode)
if d:
print('After ', i, ' steps, done: ', d)
env.reset()
env.close()
-309
View File
@@ -1,309 +0,0 @@
from typing import Tuple, Union, Callable, List, Dict, Any, Optional
import copy
import importlib
import numpy as np
from collections import defaultdict
from collections.abc import Mapping, MutableMapping
from fancy_gym.utils.make_env_helpers import make_bb
from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
from gymnasium import register as gym_register
from gymnasium import make as gym_make
from gymnasium.envs.registration import registry as gym_registry
class DefaultMPWrapper(RawInterfaceWrapper):
@property
def context_mask(self):
"""
Returns boolean mask of the same shape as the observation space.
It determines whether the observation is returned for the contextual case or not.
This effectively allows to filter unwanted or unnecessary observations from the full step-based case.
E.g. Velocities starting at 0 are only changing after the first action. Given we only receive the
context/part of the first observation, the velocities are not necessary in the observation for the task.
Returns:
bool array representing the indices of the observations
"""
# If the env already defines a context_mask, we will use that
if hasattr(self.env, 'context_mask'):
return self.env.context_mask
# Otherwise we will use the whole observation as the context. (Write a custom MPWrapper to change this behavior)
return np.full(self.env.observation_space.shape, True)
@property
def current_pos(self) -> Union[float, int, np.ndarray, Tuple]:
"""
Returns the current position of the action/control dimension.
The dimensionality has to match the action/control dimension.
This is not required when exclusively using velocity control,
it should, however, be implemented regardless.
E.g. The joint positions that are directly or indirectly controlled by the action.
"""
assert hasattr(self.env, 'current_pos'), 'DefaultMPWrapper was unable to access env.current_pos. Please write a custom MPWrapper (recommended) or expose this attribute directly.'
return self.env.current_pos
@property
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
"""
Returns the current velocity of the action/control dimension.
The dimensionality has to match the action/control dimension.
This is not required when exclusively using position control,
it should, however, be implemented regardless.
E.g. The joint velocities that are directly or indirectly controlled by the action.
"""
assert hasattr(self.env, 'current_vel'), 'DefaultMPWrapper was unable to access env.current_vel. Please write a custom MPWrapper (recommended) or expose this attribute directly.'
return self.env.current_vel
_BB_DEFAULTS = {
'ProMP': {
'wrappers': [],
'trajectory_generator_kwargs': {
'trajectory_generator_type': 'promp'
},
'phase_generator_kwargs': {
'phase_generator_type': 'linear'
},
'controller_kwargs': {
'controller_type': 'motor',
'p_gains': 1.0,
'd_gains': 0.1,
},
'basis_generator_kwargs': {
'basis_generator_type': 'zero_rbf',
'num_basis': 5,
'num_basis_zero_start': 1,
'basis_bandwidth_factor': 3.0,
},
'black_box_kwargs': {
}
},
'DMP': {
'wrappers': [],
'trajectory_generator_kwargs': {
'trajectory_generator_type': 'dmp'
},
'phase_generator_kwargs': {
'phase_generator_type': 'exp'
},
'controller_kwargs': {
'controller_type': 'motor',
'p_gains': 1.0,
'd_gains': 0.1,
},
'basis_generator_kwargs': {
'basis_generator_type': 'rbf',
'num_basis': 5
},
'black_box_kwargs': {
}
},
'ProDMP': {
'wrappers': [],
'trajectory_generator_kwargs': {
'trajectory_generator_type': 'prodmp',
'duration': 2.0,
'weights_scale': 1.0,
},
'phase_generator_kwargs': {
'phase_generator_type': 'exp',
'tau': 1.5,
},
'controller_kwargs': {
'controller_type': 'motor',
'p_gains': 1.0,
'd_gains': 0.1,
},
'basis_generator_kwargs': {
'basis_generator_type': 'prodmp',
'alpha': 10,
'num_basis': 5,
},
'black_box_kwargs': {
}
}
}
KNOWN_MPS = list(_BB_DEFAULTS.keys())
_KNOWN_MPS_PLUS_ALL = KNOWN_MPS + ['all']
ALL_MOVEMENT_PRIMITIVE_ENVIRONMENTS = {mp_type: [] for mp_type in _KNOWN_MPS_PLUS_ALL}
MOVEMENT_PRIMITIVE_ENVIRONMENTS_FOR_NS = {}
def register(
id: str,
entry_point: Optional[Union[Callable, str]] = None,
mp_wrapper: RawInterfaceWrapper = DefaultMPWrapper,
register_step_based: bool = True, # TODO: Detect
add_mp_types: List[str] = KNOWN_MPS,
mp_config_override: Dict[str, Any] = {},
**kwargs
):
"""
Registers a Gymnasium environment, including Movement Primitives (MP) versions.
If you only want to register MP versions for an already registered environment, use fancy_gym.upgrade instead.
Args:
id (str): The unique identifier for the environment.
entry_point (Optional[Union[Callable, str]]): The entry point for creating the environment.
mp_wrapper (RawInterfaceWrapper): The MP wrapper for the environment.
register_step_based (bool): Whether to also register the raw srtep-based version of the environment (default True).
add_mp_types (List[str]): List of additional MP types to register.
mp_config_override (Dict[str, Any]): Dictionary for overriding MP configuration.
**kwargs: Additional keyword arguments which are passed to the environment constructor.
Notes:
- When `register_step_based` is True, the raw environment will also be registered to gymnasium otherwise only mp-versions will be registered.
- `entry_point` can be given as a string, allowing the same notation as gymnasium.
- If `id` already exists in the Gymnasium registry and `register_step_based` is True,
a warning message will be printed, suggesting to set `register_step_based=False` or use `fancy_gym.upgrade`.
Example:
To register a step-based environment with Movement Primitive versions (will use default mp_wrapper):
>>> register("MyEnv-v0", MyEnvClass"my_module:MyEnvClass")
The entry point can also be provided as a string:
>>> register("MyEnv-v0", "my_module:MyEnvClass")
"""
if register_step_based and id in gym_registry:
print(f'[Info] Gymnasium env with id "{id}" already exists. You should supply register_step_based=False or use fancy_gym.upgrade if you only want to register mp versions of an existing env.')
if register_step_based:
assert entry_point != None, 'You need to provide an entry-point, when registering step-based.'
if not callable(mp_wrapper): # mp_wrapper can be given as a String (same notation as for entry_point)
mod_name, attr_name = mp_wrapper.split(':')
mod = importlib.import_module(mod_name)
mp_wrapper = getattr(mod, attr_name)
if register_step_based:
gym_register(id=id, entry_point=entry_point, **kwargs)
upgrade(id, mp_wrapper, add_mp_types, mp_config_override)
def upgrade(
id: str,
mp_wrapper: RawInterfaceWrapper = DefaultMPWrapper,
add_mp_types: List[str] = KNOWN_MPS,
base_id: Optional[str] = None,
mp_config_override: Dict[str, Any] = {},
):
"""
Upgrades an existing Gymnasium environment to include Movement Primitives (MP) versions.
We expect the raw step-based env to be already registered with gymnasium. Otherwise please use fancy_gym.register instead.
Args:
id (str): The unique identifier for the environment.
mp_wrapper (RawInterfaceWrapper): The MP wrapper for the environment (default is DefaultMPWrapper).
add_mp_types (List[str]): List of additional MP types to register (default is KNOWN_MPS).
base_id (Optional[str]): The unique identifier for the environment to upgrade. Will use id if non is provided. Can be defined to allow multiple registrations of different versions for the same step-based environment.
mp_config_override (Dict[str, Any]): Dictionary for overriding MP configuration.
Notes:
- The `id` parameter should match the ID of the existing Gymnasium environment you wish to upgrade. You can also pick a new one, but then `base_id` needs to be provided.
- The `mp_wrapper` parameter specifies the MP wrapper to use, allowing for customization.
- `add_mp_types` can be used to specify additional MP types to register alongside the base environment.
- The `base_id` parameter should match the ID of the existing Gymnasium environment you wish to upgrade.
- `mp_config_override` allows for customizing MP configuration if needed.
Example:
To upgrade an existing environment with MP versions:
>>> upgrade("MyEnv-v0", mp_wrapper=CustomMPWrapper)
To upgrade an existing environment with custom MP types and configuration:
>>> upgrade("MyEnv-v0", mp_wrapper=CustomMPWrapper, add_mp_types=["ProDMP", "DMP"], mp_config_override={"param": 42})
"""
if not base_id:
base_id = id
register_mps(id, base_id, mp_wrapper, add_mp_types, mp_config_override)
def register_mps(id: str, base_id: str, mp_wrapper: RawInterfaceWrapper, add_mp_types: List[str] = KNOWN_MPS, mp_config_override: Dict[str, Any] = {}):
for mp_type in add_mp_types:
register_mp(id, base_id, mp_wrapper, mp_type, mp_config_override.get(mp_type, {}))
def register_mp(id: str, base_id: str, mp_wrapper: RawInterfaceWrapper, mp_type: List[str], mp_config_override: Dict[str, Any] = {}):
assert mp_type in KNOWN_MPS, 'Unknown mp_type'
assert id not in ALL_MOVEMENT_PRIMITIVE_ENVIRONMENTS[mp_type], f'The environment {id} is already registered for {mp_type}.'
parts = id.split('/')
if len(parts) == 1:
ns, name = 'gym', parts[0]
elif len(parts) == 2:
ns, name = parts[0], parts[1]
else:
raise ValueError('env id can not contain multiple "/".')
parts = name.split('-')
assert len(parts) >= 2 and parts[-1].startswith('v'), 'Malformed env id, must end in -v{int}.'
fancy_id = f'{ns}_{mp_type}/{name}'
gym_register(
id=fancy_id,
entry_point=bb_env_constructor,
kwargs={
'underlying_id': base_id,
'mp_wrapper': mp_wrapper,
'mp_type': mp_type,
'_mp_config_override_register': mp_config_override
}
)
ALL_MOVEMENT_PRIMITIVE_ENVIRONMENTS[mp_type].append(fancy_id)
ALL_MOVEMENT_PRIMITIVE_ENVIRONMENTS['all'].append(fancy_id)
if ns not in MOVEMENT_PRIMITIVE_ENVIRONMENTS_FOR_NS:
MOVEMENT_PRIMITIVE_ENVIRONMENTS_FOR_NS[ns] = {mp_type: [] for mp_type in _KNOWN_MPS_PLUS_ALL}
MOVEMENT_PRIMITIVE_ENVIRONMENTS_FOR_NS[ns][mp_type].append(fancy_id)
MOVEMENT_PRIMITIVE_ENVIRONMENTS_FOR_NS[ns]['all'].append(fancy_id)
def nested_update(base: MutableMapping, update):
"""
Updated method for nested Mappings
Args:
base: main Mapping to be updated
update: updated values for base Mapping
"""
if any([item.endswith('_type') for item in update]):
base = update
return base
for k, v in update.items():
base[k] = nested_update(base.get(k, {}), v) if isinstance(v, Mapping) else v
return base
def bb_env_constructor(underlying_id, mp_wrapper, mp_type, mp_config_override={}, _mp_config_override_register={}, **kwargs):
raw_underlying_env = gym_make(underlying_id, **kwargs)
underlying_env = mp_wrapper(raw_underlying_env)
mp_config = getattr(underlying_env, 'mp_config') if hasattr(underlying_env, 'mp_config') else {}
active_mp_config = copy.deepcopy(mp_config.get(mp_type, {}))
global_inherit_defaults = mp_config.get('inherit_defaults', True)
inherit_defaults = active_mp_config.pop('inherit_defaults', global_inherit_defaults)
config = copy.deepcopy(_BB_DEFAULTS[mp_type]) if inherit_defaults else {}
nested_update(config, active_mp_config)
nested_update(config, _mp_config_override_register)
nested_update(config, mp_config_override)
wrappers = config.pop('wrappers')
traj_gen_kwargs = config.pop('trajectory_generator_kwargs', {})
black_box_kwargs = config.pop('black_box_kwargs', {})
contr_kwargs = config.pop('controller_kwargs', {})
phase_kwargs = config.pop('phase_generator_kwargs', {})
basis_kwargs = config.pop('basis_generator_kwargs', {})
return make_bb(underlying_env,
wrappers=wrappers,
black_box_kwargs=black_box_kwargs,
traj_gen_kwargs=traj_gen_kwargs,
controller_kwargs=contr_kwargs,
phase_kwargs=phase_kwargs,
basis_kwargs=basis_kwargs,
**config)
+10 -13
View File
@@ -1,23 +1,20 @@
import gymnasium as gym
import fancy_gym
def example_run_replanning_env(env_name="fancy_ProDMP/BoxPushingDenseReplan-v0", seed=1, iterations=1, render=False):
env = gym.make(env_name)
env.reset(seed=seed)
def example_run_replanning_env(env_name="BoxPushingDenseReplanProDMP-v0", seed=1, iterations=1, render=False):
env = fancy_gym.make(env_name, seed=seed)
env.reset()
for i in range(iterations):
done = False
while done is False:
ac = env.action_space.sample()
obs, reward, terminated, truncated, info = env.step(ac)
obs, reward, done, info = env.step(ac)
if render:
env.render(mode="human")
if terminated or truncated:
if done:
env.reset()
env.close()
del env
def example_custom_replanning_envs(seed=0, iteration=100, render=True):
# id for a step-based environment
base_env_id = "BoxPushingDense-v0"
@@ -25,7 +22,7 @@ def example_custom_replanning_envs(seed=0, iteration=100, render=True):
wrappers = [fancy_gym.envs.mujoco.box_pushing.mp_wrapper.MPWrapper]
trajectory_generator_kwargs = {'trajectory_generator_type': 'prodmp',
'weights_scale': 1}
'weight_scale': 1}
phase_generator_kwargs = {'phase_generator_type': 'exp'}
controller_kwargs = {'controller_type': 'velocity'}
basis_generator_kwargs = {'basis_generator_type': 'prodmp',
@@ -49,8 +46,8 @@ def example_custom_replanning_envs(seed=0, iteration=100, render=True):
for i in range(iteration):
ac = env.action_space.sample()
obs, reward, terminated, truncated, info = env.step(ac)
if terminated or truncated:
obs, reward, done, info = env.step(ac)
if done:
env.reset()
env.close()
@@ -59,7 +56,7 @@ def example_custom_replanning_envs(seed=0, iteration=100, render=True):
if __name__ == "__main__":
# run a registered replanning environment
example_run_replanning_env(env_name="fancy_ProDMP/BoxPushingDenseReplan-v0", seed=1, iterations=1, render=False)
example_run_replanning_env(env_name="BoxPushingDenseReplanProDMP-v0", seed=1, iterations=1, render=False)
# run a custom replanning environment
example_custom_replanning_envs(seed=0, iteration=8, render=True)
example_custom_replanning_envs(seed=0, iteration=8, render=True)
+13 -14
View File
@@ -1,8 +1,7 @@
import gymnasium as gym
import fancy_gym
def example_dmc(env_id="dm_control/fish-swim", seed=1, iterations=1000, render=True):
def example_dmc(env_id="dmc:fish-swim", seed=1, iterations=1000, render=True):
"""
Example for running a DMC based env in the step based setting.
The env_id has to be specified as `domain_name:task_name` or
@@ -17,9 +16,9 @@ def example_dmc(env_id="dm_control/fish-swim", seed=1, iterations=1000, render=T
Returns:
"""
env = gym.make(env_id)
env = fancy_gym.make(env_id, seed)
rewards = 0
obs = env.reset(seed=seed)
obs = env.reset()
print("observation shape:", env.observation_space.shape)
print("action shape:", env.action_space.shape)
@@ -27,10 +26,10 @@ def example_dmc(env_id="dm_control/fish-swim", seed=1, iterations=1000, render=T
ac = env.action_space.sample()
if render:
env.render(mode="human")
obs, reward, terminated, truncated, info = env.step(ac)
obs, reward, done, info = env.step(ac)
rewards += reward
if terminated or truncated:
if done:
print(env_id, rewards)
rewards = 0
obs = env.reset()
@@ -57,7 +56,7 @@ def example_custom_dmc_and_mp(seed=1, iterations=1, render=True):
"""
# Base DMC name, according to structure of above example
base_env_id = "dm_control/ball_in_cup-catch"
base_env_id = "dmc:ball_in_cup-catch"
# Replace this wrapper with the custom wrapper for your environment by inheriting from the RawInterfaceWrapper.
# You can also add other gym.Wrappers in case they are needed.
@@ -66,8 +65,8 @@ def example_custom_dmc_and_mp(seed=1, iterations=1, render=True):
trajectory_generator_kwargs = {'trajectory_generator_type': 'promp'}
phase_generator_kwargs = {'phase_generator_type': 'linear'}
controller_kwargs = {'controller_type': 'motor',
"p_gains": 1.0,
"d_gains": 0.1, }
"p_gains": 1.0,
"d_gains": 0.1,}
basis_generator_kwargs = {'basis_generator_type': 'zero_rbf',
'num_basis': 5,
'num_basis_zero_start': 1
@@ -103,10 +102,10 @@ def example_custom_dmc_and_mp(seed=1, iterations=1, render=True):
# number of samples/full trajectories (multiple environment steps)
for i in range(iterations):
ac = env.action_space.sample()
obs, reward, terminated, truncated, info = env.step(ac)
obs, reward, done, info = env.step(ac)
rewards += reward
if terminated or truncated:
if done:
print(base_env_id, rewards)
rewards = 0
obs = env.reset()
@@ -124,14 +123,14 @@ if __name__ == '__main__':
render = True
# # Standard DMC Suite tasks
example_dmc("dm_control/fish-swim", seed=10, iterations=1000, render=render)
example_dmc("dmc:fish-swim", seed=10, iterations=1000, render=render)
#
# # Manipulation tasks
# # Disclaimer: The vision versions are currently not integrated and yield an error
example_dmc("dm_control/manipulation-reach_site_features", seed=10, iterations=250, render=render)
example_dmc("dmc:manipulation-reach_site_features", seed=10, iterations=250, render=render)
#
# # Gym + DMC hybrid task provided in the MP framework
example_dmc("dm_control_ProMP/ball_in_cup-catch-v0", seed=10, iterations=1, render=render)
example_dmc("dmc_ball_in_cup-catch_promp-v0", seed=10, iterations=1, render=render)
# Custom DMC task # Different seed, because the episode is longer for this example and the name+seed combo is
# already registered above
+10 -12
View File
@@ -1,6 +1,6 @@
from collections import defaultdict
import gymnasium as gym
import gym
import numpy as np
import fancy_gym
@@ -21,27 +21,27 @@ def example_general(env_id="Pendulum-v1", seed=1, iterations=1000, render=True):
"""
env = gym.make(env_id)
env = fancy_gym.make(env_id, seed)
rewards = 0
obs = env.reset(seed=seed)
obs = env.reset()
print("Observation shape: ", env.observation_space.shape)
print("Action shape: ", env.action_space.shape)
# number of environment steps
for i in range(iterations):
obs, reward, terminated, truncated, info = env.step(env.action_space.sample())
obs, reward, done, info = env.step(env.action_space.sample())
rewards += reward
if render:
env.render()
if terminated or truncated:
if done:
print(rewards)
rewards = 0
obs = env.reset()
def example_async(env_id="fancy/HoleReacher-v0", n_cpu=4, seed=int('533D', 16), n_samples=800):
def example_async(env_id="HoleReacher-v0", n_cpu=4, seed=int('533D', 16), n_samples=800):
"""
Example for running any env in a vectorized multiprocessing setting to generate more samples faster.
This also includes DMC and DMP environments when leveraging our custom make_env function.
@@ -69,15 +69,12 @@ def example_async(env_id="fancy/HoleReacher-v0", n_cpu=4, seed=int('533D', 16),
# this would generate more samples than requested if n_samples % num_envs != 0
repeat = int(np.ceil(n_samples / env.num_envs))
for i in range(repeat):
obs, reward, terminated, truncated, info = env.step(env.action_space.sample())
obs, reward, done, info = env.step(env.action_space.sample())
buffer['obs'].append(obs)
buffer['reward'].append(reward)
buffer['terminated'].append(terminated)
buffer['truncated'].append(truncated)
buffer['done'].append(done)
buffer['info'].append(info)
rewards += reward
done = terminated or truncated
if np.any(done):
print(f"Reward at iteration {i}: {rewards[done]}")
rewards[done] = 0
@@ -93,10 +90,11 @@ if __name__ == '__main__':
example_general("Pendulum-v1", seed=10, iterations=200, render=render)
# Mujoco task from framework
example_general("fancy/Reacher5d-v0", seed=10, iterations=200, render=render)
example_general("Reacher5d-v0", seed=10, iterations=200, render=render)
# # OpenAI Mujoco task
example_general("HalfCheetah-v2", seed=10, render=render)
# Vectorized multiprocessing environments
# example_async(env_id="HoleReacher-v0", n_cpu=2, seed=int('533D', 16), n_samples=2 * 200)
+13 -13
View File
@@ -1,8 +1,7 @@
import gymnasium as gym
import fancy_gym
def example_meta(env_id="fish-swim", seed=1, iterations=1000, render=True):
def example_dmc(env_id="fish-swim", seed=1, iterations=1000, render=True):
"""
Example for running a MetaWorld based env in the step based setting.
The env_id has to be specified as `task_name-v2`. V1 versions are not supported and we always
@@ -18,9 +17,9 @@ def example_meta(env_id="fish-swim", seed=1, iterations=1000, render=True):
Returns:
"""
env = gym.make(env_id)
env = fancy_gym.make(env_id, seed)
rewards = 0
obs = env.reset(seed=seed)
obs = env.reset()
print("observation shape:", env.observation_space.shape)
print("action shape:", env.action_space.shape)
@@ -30,9 +29,9 @@ def example_meta(env_id="fish-swim", seed=1, iterations=1000, render=True):
# THIS NEEDS TO BE SET TO FALSE FOR NOW, BECAUSE THE INTERFACE FOR RENDERING IS DIFFERENT TO BASIC GYM
# TODO: Remove this, when Metaworld fixes its interface.
env.render(False)
obs, reward, terminated, truncated, info = env.step(ac)
obs, reward, done, info = env.step(ac)
rewards += reward
if terminated or truncated:
if done:
print(env_id, rewards)
rewards = 0
obs = env.reset()
@@ -41,7 +40,7 @@ def example_meta(env_id="fish-swim", seed=1, iterations=1000, render=True):
del env
def example_custom_meta_and_mp(seed=1, iterations=1, render=True):
def example_custom_dmc_and_mp(seed=1, iterations=1, render=True):
"""
Example for running a custom movement primitive based environments.
Our already registered environments follow the same structure.
@@ -59,7 +58,7 @@ def example_custom_meta_and_mp(seed=1, iterations=1, render=True):
"""
# Base MetaWorld name, according to structure of above example
base_env_id = "metaworld/button-press-v2"
base_env_id = "metaworld:button-press-v2"
# Replace this wrapper with the custom wrapper for your environment by inheriting from the RawInterfaceWrapper.
# You can also add other gym.Wrappers in case they are needed.
@@ -104,10 +103,10 @@ def example_custom_meta_and_mp(seed=1, iterations=1, render=True):
# number of samples/full trajectories (multiple environment steps)
for i in range(iterations):
ac = env.action_space.sample()
obs, reward, terminated, truncated, info = env.step(ac)
obs, reward, done, info = env.step(ac)
rewards += reward
if terminated or truncated:
if done:
print(base_env_id, rewards)
rewards = 0
obs = env.reset()
@@ -125,10 +124,11 @@ if __name__ == '__main__':
render = False
# # Standard Meta world tasks
example_meta("metaworld/button-press-v2", seed=10, iterations=500, render=render)
example_dmc("metaworld:button-press-v2", seed=10, iterations=500, render=render)
# # MP + MetaWorld hybrid task provided in the our framework
example_meta("metaworld_ProMP/ButtonPress-v2", seed=10, iterations=1, render=render)
example_dmc("ButtonPressProMP-v2", seed=10, iterations=1, render=render)
#
# # Custom MetaWorld task
example_custom_meta_and_mp(seed=10, iterations=1, render=render)
example_custom_dmc_and_mp(seed=10, iterations=1, render=render)
@@ -1,8 +1,7 @@
import gymnasium as gym
import fancy_gym
def example_mp(env_name="fancy_ProMP/HoleReacher-v0", seed=1, iterations=1, render=True):
def example_mp(env_name="HoleReacherProMP-v0", seed=1, iterations=1, render=True):
"""
Example for running a black box based environment, which is already registered
Args:
@@ -16,16 +15,24 @@ def example_mp(env_name="fancy_ProMP/HoleReacher-v0", seed=1, iterations=1, rend
"""
# Equivalent to gym, we have a make function which can be used to create environments.
# It takes care of seeding and enables the use of a variety of external environments using the gym interface.
env = gym.make(env_name, render_mode='human' if render else None)
env = fancy_gym.make(env_name, seed)
returns = 0
# env.render(mode=None)
obs = env.reset(seed=seed)
obs = env.reset()
# number of samples/full trajectories (multiple environment steps)
for i in range(iterations):
if render and i % 1 == 0:
# This renders the full MP trajectory
# It is only required to call render() once in the beginning, which renders every consecutive trajectory.
# Resetting to no rendering, can be achieved by render(mode=None).
# It is also possible to change the mode multiple times when
# e.g. only every second trajectory should be displayed, such as here
# Just make sure the correct mode is set before executing the step.
env.render(mode="human")
else:
env.render()
# Now the action space is not the raw action but the parametrization of the trajectory generator,
@@ -34,94 +41,53 @@ def example_mp(env_name="fancy_ProMP/HoleReacher-v0", seed=1, iterations=1, rend
# This executes a full trajectory and gives back the context (obs) of the last step in the trajectory, or the
# full observation space of the last step, if replanning/sub-trajectory learning is used. The 'reward' is equal
# to the return of a trajectory. Default is the sum over the step-wise rewards.
obs, reward, terminated, truncated, info = env.step(ac)
obs, reward, done, info = env.step(ac)
# Aggregated returns
returns += reward
if terminated or truncated:
if done:
print(reward)
obs = env.reset()
env.close()
def example_custom_mp(env_name="fancy_ProMP/Reacher5d-v0", seed=1, iterations=1, render=True):
def example_custom_mp(env_name="Reacher5dProMP-v0", seed=1, iterations=1, render=True):
"""
Example for running a custom movement primitive based environments.
Our already registered environments follow the same structure.
Hence, this also allows to adjust hyperparameters of the movement primitives.
Yet, we recommend the method above if you are just interested in changing those parameters for existing tasks.
We appreciate PRs for custom environments (especially MP wrappers of existing tasks)
for our repo: https://github.com/ALRhub/fancy_gym/
Example for running a movement primitive based environment, which is already registered
Args:
seed: seed
env_name: DMP env_id
seed: seed for deterministic behaviour
iterations: Number of rollout steps to run
render: Render the episode
Returns:
"""
# Changing the arguments of the black box env is possible by providing them to gym through mp_config_override.
# Changing the arguments of the black box env is possible by providing them to gym as with all kwargs.
# E.g. here for way to many basis functions
env = gym.make(env_name, seed, mp_config_override={'basis_generator_kwargs': {'num_basis': 1000}}, render_mode='human' if render else None)
env = fancy_gym.make(env_name, seed, basis_generator_kwargs={'num_basis': 1000})
# env = fancy_gym.make(env_name, seed)
# mp_dict.update({'black_box_kwargs': {'learn_sub_trajectories': True}})
# mp_dict.update({'black_box_kwargs': {'do_replanning': lambda pos, vel, t: lambda t: t % 100}})
returns = 0
obs = env.reset()
# This time rendering every trajectory
if render:
env.render()
env.render(mode="human")
# number of samples/full trajectories (multiple environment steps)
for i in range(iterations):
ac = env.action_space.sample()
obs, reward, terminated, truncated, info = env.step(ac)
obs, reward, done, info = env.step(ac)
returns += reward
if terminated or truncated:
if done:
print(i, reward)
obs = env.reset()
env.close()
return obs
class Custom_MPWrapper(fancy_gym.envs.mujoco.reacher.MPWrapper):
mp_config = {
'ProMP': {
'trajectory_generator_kwargs': {
'trajectory_generator_type': 'promp',
'weights_scale': 2
},
'phase_generator_kwargs': {
'phase_generator_type': 'linear'
},
'controller_kwargs': {
'controller_type': 'velocity'
},
'basis_generator_kwargs': {
'basis_generator_type': 'zero_rbf',
'num_basis': 5,
'num_basis_zero_start': 1
}
},
'DMP': {
'trajectory_generator_kwargs': {
'trajectory_generator_type': 'dmp',
'weights_scale': 500
},
'phase_generator_kwargs': {
'phase_generator_type': 'exp',
'alpha_phase': 2.5
},
'controller_kwargs': {
'controller_type': 'velocity'
},
'basis_generator_kwargs': {
'basis_generator_type': 'rbf',
'num_basis': 5
}
}
}
def example_fully_custom_mp(seed=1, iterations=1, render=True):
"""
@@ -140,93 +106,38 @@ def example_fully_custom_mp(seed=1, iterations=1, render=True):
"""
base_env_id = "fancy/Reacher5d-v0"
custom_env_id = "fancy/Reacher5d-Custom-v0"
custom_env_id_DMP = "fancy_DMP/Reacher5d-Custom-v0"
custom_env_id_ProMP = "fancy_ProMP/Reacher5d-Custom-v0"
base_env_id = "Reacher5d-v0"
fancy_gym.upgrade(custom_env_id, mp_wrapper=Custom_MPWrapper, add_mp_types=['ProMP', 'DMP'], base_id=base_env_id)
# Replace this wrapper with the custom wrapper for your environment by inheriting from the RawInterfaceWrapper.
# You can also add other gym.Wrappers in case they are needed.
wrappers = [fancy_gym.envs.mujoco.reacher.MPWrapper]
env = gym.make(custom_env_id_ProMP, render_mode='human' if render else None)
# For a ProMP
trajectory_generator_kwargs = {'trajectory_generator_type': 'promp',
'weight_scale': 2}
phase_generator_kwargs = {'phase_generator_type': 'linear'}
controller_kwargs = {'controller_type': 'velocity'}
basis_generator_kwargs = {'basis_generator_type': 'zero_rbf',
'num_basis': 5,
'num_basis_zero_start': 1
}
rewards = 0
obs = env.reset()
# # For a DMP
# trajectory_generator_kwargs = {'trajectory_generator_type': 'dmp',
# 'weight_scale': 500}
# phase_generator_kwargs = {'phase_generator_type': 'exp',
# 'alpha_phase': 2.5}
# controller_kwargs = {'controller_type': 'velocity'}
# basis_generator_kwargs = {'basis_generator_type': 'rbf',
# 'num_basis': 5
# }
env = fancy_gym.make_bb(env_id=base_env_id, wrappers=wrappers, black_box_kwargs={},
traj_gen_kwargs=trajectory_generator_kwargs, controller_kwargs=controller_kwargs,
phase_kwargs=phase_generator_kwargs, basis_kwargs=basis_generator_kwargs,
seed=seed)
if render:
env.render()
# number of samples/full trajectories (multiple environment steps)
for i in range(iterations):
ac = env.action_space.sample()
obs, reward, terminated, truncated, info = env.step(ac)
rewards += reward
if terminated or truncated:
print(rewards)
rewards = 0
obs = env.reset()
try: # Some mujoco-based envs don't correlcty implement .close
env.close()
except:
pass
def example_fully_custom_mp_alternative(seed=1, iterations=1, render=True):
"""
Instead of defining the mp_args in a new custom MP_Wrapper, they can also be provided during registration.
Args:
seed: seed
iterations: Number of rollout steps to run
render: Render the episode
Returns:
"""
base_env_id = "fancy/Reacher5d-v0"
custom_env_id = "fancy/Reacher5d-Custom-v0"
custom_env_id_ProMP = "fancy_ProMP/Reacher5d-Custom-v0"
fancy_gym.upgrade(custom_env_id, mp_wrapper=fancy_gym.envs.mujoco.reacher.MPWrapper, add_mp_types=['ProMP'], base_id=base_env_id, mp_config_override= {'ProMP': {
'trajectory_generator_kwargs': {
'trajectory_generator_type': 'promp',
'weights_scale': 2
},
'phase_generator_kwargs': {
'phase_generator_type': 'linear'
},
'controller_kwargs': {
'controller_type': 'velocity'
},
'basis_generator_kwargs': {
'basis_generator_type': 'zero_rbf',
'num_basis': 5,
'num_basis_zero_start': 1
}
}})
env = gym.make(custom_env_id_ProMP, render_mode='human' if render else None)
rewards = 0
obs = env.reset()
if render:
env.render()
# number of samples/full trajectories (multiple environment steps)
for i in range(iterations):
ac = env.action_space.sample()
obs, reward, terminated, truncated, info = env.step(ac)
rewards += reward
if terminated or truncated:
print(rewards)
rewards = 0
obs = env.reset()
if render:
env.render()
env.render(mode="human")
rewards = 0
obs = env.reset()
@@ -234,41 +145,32 @@ def example_fully_custom_mp_alternative(seed=1, iterations=1, render=True):
# number of samples/full trajectories (multiple environment steps)
for i in range(iterations):
ac = env.action_space.sample()
obs, reward, terminated, truncated, info = env.step(ac)
obs, reward, done, info = env.step(ac)
rewards += reward
if terminated or truncated:
if done:
print(rewards)
rewards = 0
obs = env.reset()
try: # Some mujoco-based envs don't correlcty implement .close
env.close()
except:
pass
def main():
if __name__ == '__main__':
render = False
# DMP
example_mp("fancy_DMP/HoleReacher-v0", seed=10, iterations=5, render=render)
example_mp("HoleReacherDMP-v0", seed=10, iterations=5, render=render)
# ProMP
example_mp("fancy_ProMP/HoleReacher-v0", seed=10, iterations=5, render=render)
example_mp("fancy_ProMP/BoxPushingTemporalSparse-v0", seed=10, iterations=1, render=render)
example_mp("fancy_ProMP/TableTennis4D-v0", seed=10, iterations=20, render=render)
example_mp("HoleReacherProMP-v0", seed=10, iterations=5, render=render)
example_mp("BoxPushingTemporalSparseProMP-v0", seed=10, iterations=1, render=render)
example_mp("TableTennis4DProMP-v0", seed=10, iterations=20, render=render)
# ProDMP with Replanning
example_mp("fancy_ProDMP/BoxPushingDenseReplan-v0", seed=10, iterations=4, render=render)
example_mp("fancy_ProDMP/TableTennis4DReplan-v0", seed=10, iterations=20, render=render)
example_mp("fancy_ProDMP/TableTennisWindReplan-v0", seed=10, iterations=20, render=render)
example_mp("BoxPushingDenseReplanProDMP-v0", seed=10, iterations=4, render=render)
example_mp("TableTennis4DReplanProDMP-v0", seed=10, iterations=20, render=render)
example_mp("TableTennisWindReplanProDMP-v0", seed=10, iterations=20, render=render)
# Altered basis functions
obs1 = example_custom_mp("fancy_ProMP/Reacher5d-v0", seed=10, iterations=1, render=render)
obs1 = example_custom_mp("Reacher5dProMP-v0", seed=10, iterations=1, render=render)
# Custom MP
example_fully_custom_mp(seed=10, iterations=1, render=render)
example_fully_custom_mp_alternative(seed=10, iterations=1, render=render)
if __name__=='__main__':
main()
+7 -6
View File
@@ -1,4 +1,3 @@
import gymnasium as gym
import fancy_gym
@@ -13,10 +12,11 @@ def example_mp(env_name, seed=1, render=True):
Returns:
"""
env = gym.make(env_name)
# While in this case gym.make() is possible to use as well, we recommend our custom make env function.
env = fancy_gym.make(env_name, seed)
returns = 0
obs = env.reset(seed=seed)
obs = env.reset()
# number of samples/full trajectories (multiple environment steps)
for i in range(10):
if render and i % 2 == 0:
@@ -24,13 +24,14 @@ def example_mp(env_name, seed=1, render=True):
else:
env.render()
ac = env.action_space.sample()
obs, reward, terminated, truncated, info = env.step(ac)
obs, reward, done, info = env.step(ac)
returns += reward
if terminated or truncated:
if done:
print(returns)
obs = env.reset()
if __name__ == '__main__':
example_mp("gym_ProMP/Reacher-v2")
example_mp("ReacherProMP-v2")
+3 -7
View File
@@ -1,14 +1,10 @@
import gymnasium as gym
import fancy_gym
def compare_bases_shape(env1_id, env2_id):
env1 = gym.make(env1_id)
env1 = fancy_gym.make(env1_id, seed=0)
env1.traj_gen.show_scaled_basis(plot=True)
env2 = gym.make(env2_id)
env2 = fancy_gym.make(env2_id, seed=0)
env2.traj_gen.show_scaled_basis(plot=True)
return
if __name__ == '__main__':
compare_bases_shape("fancy_ProDMP/TableTennis4D-v0", "fancy_ProMP/TableTennis4D-v0")
compare_bases_shape("TableTennis4DProDMP-v0", "TableTennis4DProMP-v0")
+4 -5
View File
@@ -3,20 +3,19 @@ from collections import OrderedDict
import numpy as np
from matplotlib import pyplot as plt
import gymnasium as gym
import fancy_gym
# This might work for some environments, however, please verify either way the correct trajectory information
# for your environment are extracted below
SEED = 1
env_id = "fancy_ProMP/Reacher5d-v0"
env_id = "Reacher5dProMP-v0"
env = fancy_gym.make(env_id, mp_config_override={'controller_kwargs': {'p_gains': 0.05, 'd_gains': 0.05}}).env
env = fancy_gym.make(env_id, seed=SEED, controller_kwargs={'p_gains': 0.05, 'd_gains': 0.05}).env
env.action_space.seed(SEED)
# Plot difference between real trajectory and target MP trajectory
env.reset(seed=SEED)
env.reset()
w = env.action_space.sample()
pos, vel = env.get_trajectory(w)
@@ -35,7 +34,7 @@ fig.show()
for t, (des_pos, des_vel) in enumerate(zip(pos, vel)):
actions = env.tracking_controller.get_action(des_pos, des_vel, env.current_pos, env.current_vel)
actions = np.clip(actions, env.env.action_space.low, env.env.action_space.high)
env.env.step(actions)
_, _, _, _ = env.env.step(actions)
if t % 15 == 0:
img.set_data(env.env.render(mode="rgb_array"))
fig.canvas.draw()
+25 -63
View File
@@ -1,64 +1,26 @@
# Metaworld
# MetaWorld Wrappers
[Metaworld](https://meta-world.github.io/) is an open-source simulated benchmark designed to advance meta-reinforcement learning and multi-task learning, comprising 50 diverse robotic manipulation tasks. The benchmark features a universal tabletop environment equipped with a simulated Sawyer arm and a variety of everyday objects. This shared environment is pivotal for reusing structured learning and efficiently acquiring related tasks.
## Step-Based Envs
`fancy_gym` makes all metaworld ML1 tasks avaible via the standard gym interface. To access metaworld environments using a different mode of operation (MT1 / ML100 / etc.) please use the functionality provided by metaworld directly.
| Name | Description | Horizon | Action Dimension | Observation Dimension | Context Dimension |
| ---------------------------------------- | ------------------------------------------------------------------------------------- | ------- | ---------------- | --------------------- | ----------------- |
| `metaworld/assembly-v2` | A task where the robot must assemble components. | 500 | 4 | 39 | 6 |
| `metaworld/basketball-v2` | A task where the robot must play a game of basketball. | 500 | 4 | 39 | 6 |
| `metaworld/bin-picking-v2` | A task involving the robot picking objects from a bin. | 500 | 4 | 39 | 6 |
| `metaworld/box-close-v2` | A task requiring the robot to close a box. | 500 | 4 | 39 | 6 |
| `metaworld/button-press-topdown-v2` | A task where the robot must press a button from a top-down perspective. | 500 | 4 | 39 | 6 |
| `metaworld/button-press-topdown-wall-v2` | A task involving the robot pressing a button with a wall from a top-down perspective. | 500 | 4 | 39 | 6 |
| `metaworld/button-press-v2` | A task where the robot must press a button. | 500 | 4 | 39 | 6 |
| `metaworld/button-press-wall-v2` | A task involving the robot pressing a button with a wall. | 500 | 4 | 39 | 6 |
| `metaworld/coffee-button-v2` | A task where the robot must press a button on a coffee machine. | 500 | 4 | 39 | 6 |
| `metaworld/coffee-pull-v2` | A task involving the robot pulling a lever on a coffee machine. | 500 | 4 | 39 | 6 |
| `metaworld/coffee-push-v2` | A task involving the robot pushing a component on a coffee machine. | 500 | 4 | 39 | 6 |
| `metaworld/dial-turn-v2` | A task where the robot must turn a dial. | 500 | 4 | 39 | 6 |
| `metaworld/disassemble-v2` | A task requiring the robot to disassemble an object. | 500 | 4 | 39 | 6 |
| `metaworld/door-close-v2` | A task where the robot must close a door. | 500 | 4 | 39 | 6 |
| `metaworld/door-lock-v2` | A task involving the robot locking a door. | 500 | 4 | 39 | 6 |
| `metaworld/door-open-v2` | A task where the robot must open a door. | 500 | 4 | 39 | 6 |
| `metaworld/door-unlock-v2` | A task involving the robot unlocking a door. | 500 | 4 | 39 | 6 |
| `metaworld/hand-insert-v2` | A task requiring the robot to insert a hand into an object. | 500 | 4 | 39 | 6 |
| `metaworld/drawer-close-v2` | A task where the robot must close a drawer. | 500 | 4 | 39 | 6 |
| `metaworld/drawer-open-v2` | A task involving the robot opening a drawer. | 500 | 4 | 39 | 6 |
| `metaworld/faucet-open-v2` | A task requiring the robot to open a faucet. | 500 | 4 | 39 | 6 |
| `metaworld/faucet-close-v2` | A task where the robot must close a faucet. | 500 | 4 | 39 | 6 |
| `metaworld/hammer-v2` | A task where the robot must use a hammer. | 500 | 4 | 39 | 6 |
| `metaworld/handle-press-side-v2` | A task involving the robot pressing a handle from the side. | 500 | 4 | 39 | 6 |
| `metaworld/handle-press-v2` | A task where the robot must press a handle. | 500 | 4 | 39 | 6 |
| `metaworld/handle-pull-side-v2` | A task requiring the robot to pull a handle from the side. | 500 | 4 | 39 | 6 |
| `metaworld/handle-pull-v2` | A task where the robot must pull a handle. | 500 | 4 | 39 | 6 |
| `metaworld/lever-pull-v2` | A task involving the robot pulling a lever. | 500 | 4 | 39 | 6 |
| `metaworld/peg-insert-side-v2` | A task requiring the robot to insert a peg from the side. | 500 | 4 | 39 | 6 |
| `metaworld/pick-place-wall-v2` | A task involving the robot picking and placing an object with a wall. | 500 | 4 | 39 | 6 |
| `metaworld/pick-out-of-hole-v2` | A task where the robot must pick an object out of a hole. | 500 | 4 | 39 | 6 |
| `metaworld/reach-v2` | A task where the robot must reach an object. | 500 | 4 | 39 | 6 |
| `metaworld/push-back-v2` | A task involving the robot pushing an object backward. | 500 | 4 | 39 | 6 |
| `metaworld/push-v2` | A task where the robot must push an object. | 500 | 4 | 39 | 6 |
| `metaworld/pick-place-v2` | A task involving the robot picking up and placing an object. | 500 | 4 | 39 | 6 |
| `metaworld/plate-slide-v2` | A task requiring the robot to slide a plate. | 500 | 4 | 39 | 6 |
| `metaworld/plate-slide-side-v2` | A task involving the robot sliding a plate from the side. | 500 | 4 | 39 | 6 |
| `metaworld/plate-slide-back-v2` | A task where the robot must slide a plate backward. | 500 | 4 | 39 | 6 |
| `metaworld/plate-slide-back-side-v2` | A task involving the robot sliding a plate backward from the side. | 500 | 4 | 39 | 6 |
| `metaworld/peg-unplug-side-v2` | A task where the robot must unplug a peg from the side. | 500 | 4 | 39 | 6 |
| `metaworld/soccer-v2` | A task where the robot must play soccer. | 500 | 4 | 39 | 6 |
| `metaworld/stick-push-v2` | A task involving the robot pushing a stick. | 500 | 4 | 39 | 6 |
| `metaworld/stick-pull-v2` | A task where the robot must pull a stick. | 500 | 4 | 39 | 6 |
| `metaworld/push-wall-v2` | A task involving the robot pushing against a wall. | 500 | 4 | 39 | 6 |
| `metaworld/reach-wall-v2` | A task where the robot must reach an object with a wall. | 500 | 4 | 39 | 6 |
| `metaworld/shelf-place-v2` | A task involving the robot placing an object on a shelf. | 500 | 4 | 39 | 6 |
| `metaworld/sweep-into-v2` | A task where the robot must sweep objects into a container. | 500 | 4 | 39 | 6 |
| `metaworld/sweep-v2` | A task requiring the robot to sweep. | 500 | 4 | 39 | 6 |
| `metaworld/window-open-v2` | A task where the robot must open a window. | 500 | 4 | 39 | 6 |
| `metaworld/window-close-v2` | A task involving the robot closing a window. | 500 | 4 | 39 | 6 |
## MP-Based Envs
All envs also exist in MP-variants. Refer to them using `metaworld_ProMP/<name-v2>` or `metaworld_ProDMP/<name-v2>` (DMP is currently not supported as of now).
These are the Environment Wrappers for selected [Metaworld](https://meta-world.github.io/) environments in order to use our Movement Primitive gym interface with them.
All Metaworld environments have a 39 dimensional observation space with the same structure. The tasks differ only in the objective and the initial observations that are randomized.
Unused observations are zeroed out. E.g. for `Button-Press-v2` the observation mask looks the following:
```python
return np.hstack([
# Current observation
[False] * 3, # end-effector position
[False] * 1, # normalized gripper open distance
[True] * 3, # main object position
[False] * 4, # main object quaternion
[False] * 3, # secondary object position
[False] * 4, # secondary object quaternion
# Previous observation
[False] * 3, # previous end-effector position
[False] * 1, # previous normalized gripper open distance
[False] * 3, # previous main object position
[False] * 4, # previous main object quaternion
[False] * 3, # previous second object position
[False] * 4, # previous second object quaternion
# Goal
[True] * 3, # goal position
])
```
For other tasks only the boolean values have to be adjusted accordingly.
+155 -23
View File
@@ -1,37 +1,125 @@
from typing import Iterable, Type, Union, Optional
from copy import deepcopy
from ..envs.registry import register
from gym import register
from . import goal_object_change_mp_wrapper, goal_change_mp_wrapper, goal_endeffector_change_mp_wrapper, \
object_change_mp_wrapper
from . import metaworld_adapter
metaworld_adapter.register_all_ML1()
ALL_METAWORLD_MOVEMENT_PRIMITIVE_ENVIRONMENTS = {"DMP": [], "ProMP": [], "ProDMP": []}
# MetaWorld
DEFAULT_BB_DICT_ProMP = {
"name": 'EnvName',
"wrappers": [],
"trajectory_generator_kwargs": {
'trajectory_generator_type': 'promp',
'weights_scale': 10,
},
"phase_generator_kwargs": {
'phase_generator_type': 'linear'
},
"controller_kwargs": {
'controller_type': 'metaworld',
},
"basis_generator_kwargs": {
'basis_generator_type': 'zero_rbf',
'num_basis': 5,
'num_basis_zero_start': 1
},
'black_box_kwargs': {
'condition_on_desired': False,
}
}
DEFAULT_BB_DICT_ProDMP = {
"name": 'EnvName',
"wrappers": [],
"trajectory_generator_kwargs": {
'trajectory_generator_type': 'prodmp',
'auto_scale_basis': True,
'weights_scale': 10,
# 'goal_scale': 0.,
'disable_goal': True,
},
"phase_generator_kwargs": {
'phase_generator_type': 'exp',
# 'alpha_phase' : 3,
},
"controller_kwargs": {
'controller_type': 'metaworld',
},
"basis_generator_kwargs": {
'basis_generator_type': 'prodmp',
'num_basis': 5,
'alpha': 10
},
'black_box_kwargs': {
'condition_on_desired': False,
}
}
_goal_change_envs = ["assembly-v2", "pick-out-of-hole-v2", "plate-slide-v2", "plate-slide-back-v2",
"plate-slide-side-v2", "plate-slide-back-side-v2"]
for _task in _goal_change_envs:
task_id_split = _task.split("-")
name = "".join([s.capitalize() for s in task_id_split[:-1]])
# ProMP
_env_id = f'{name}ProMP-{task_id_split[-1]}'
kwargs_dict_goal_change_promp = deepcopy(DEFAULT_BB_DICT_ProMP)
kwargs_dict_goal_change_promp['wrappers'].append(goal_change_mp_wrapper.MPWrapper)
kwargs_dict_goal_change_promp['name'] = f'metaworld:{_task}'
register(
id=f'metaworld/{_task}',
register_step_based=False,
mp_wrapper=goal_change_mp_wrapper.MPWrapper,
add_mp_types=['ProMP', 'ProDMP'],
id=_env_id,
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_goal_change_promp
)
ALL_METAWORLD_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append(_env_id)
# ProDMP
_env_id = f'{name}ProDMP-{task_id_split[-1]}'
kwargs_dict_goal_change_prodmp = deepcopy(DEFAULT_BB_DICT_ProDMP)
kwargs_dict_goal_change_prodmp['wrappers'].append(goal_change_mp_wrapper.MPWrapper)
kwargs_dict_goal_change_prodmp['name'] = f'metaworld:{_task}'
register(
id=_env_id,
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_goal_change_prodmp
)
ALL_METAWORLD_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProDMP"].append(_env_id)
_object_change_envs = ["bin-picking-v2", "hammer-v2", "sweep-into-v2"]
for _task in _object_change_envs:
task_id_split = _task.split("-")
name = "".join([s.capitalize() for s in task_id_split[:-1]])
# ProMP
_env_id = f'{name}ProMP-{task_id_split[-1]}'
kwargs_dict_object_change_promp = deepcopy(DEFAULT_BB_DICT_ProMP)
kwargs_dict_object_change_promp['wrappers'].append(object_change_mp_wrapper.MPWrapper)
kwargs_dict_object_change_promp['name'] = f'metaworld:{_task}'
register(
id=f'metaworld/{_task}',
register_step_based=False,
mp_wrapper=object_change_mp_wrapper.MPWrapper,
add_mp_types=['ProMP', 'ProDMP'],
id=_env_id,
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_object_change_promp
)
ALL_METAWORLD_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append(_env_id)
# ProDMP
_env_id = f'{name}ProDMP-{task_id_split[-1]}'
kwargs_dict_object_change_prodmp = deepcopy(DEFAULT_BB_DICT_ProDMP)
kwargs_dict_object_change_prodmp['wrappers'].append(object_change_mp_wrapper.MPWrapper)
kwargs_dict_object_change_prodmp['name'] = f'metaworld:{_task}'
register(
id=_env_id,
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_object_change_prodmp
)
ALL_METAWORLD_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProDMP"].append(_env_id)
_goal_and_object_change_envs = ["box-close-v2", "button-press-v2", "button-press-wall-v2", "button-press-topdown-v2",
"button-press-topdown-wall-v2", "coffee-button-v2", "coffee-pull-v2",
@@ -45,18 +133,62 @@ _goal_and_object_change_envs = ["box-close-v2", "button-press-v2", "button-press
"shelf-place-v2", "sweep-v2", "window-open-v2", "window-close-v2"
]
for _task in _goal_and_object_change_envs:
task_id_split = _task.split("-")
name = "".join([s.capitalize() for s in task_id_split[:-1]])
# ProMP
_env_id = f'{name}ProMP-{task_id_split[-1]}'
kwargs_dict_goal_and_object_change_promp = deepcopy(DEFAULT_BB_DICT_ProMP)
kwargs_dict_goal_and_object_change_promp['wrappers'].append(goal_object_change_mp_wrapper.MPWrapper)
kwargs_dict_goal_and_object_change_promp['name'] = f'metaworld:{_task}'
register(
id=f'metaworld/{_task}',
register_step_based=False,
mp_wrapper=goal_object_change_mp_wrapper.MPWrapper,
add_mp_types=['ProMP', 'ProDMP'],
id=_env_id,
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_goal_and_object_change_promp
)
ALL_METAWORLD_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append(_env_id)
# ProDMP
_env_id = f'{name}ProDMP-{task_id_split[-1]}'
kwargs_dict_goal_and_object_change_prodmp = deepcopy(DEFAULT_BB_DICT_ProDMP)
kwargs_dict_goal_and_object_change_prodmp['wrappers'].append(goal_object_change_mp_wrapper.MPWrapper)
kwargs_dict_goal_and_object_change_prodmp['name'] = f'metaworld:{_task}'
register(
id=_env_id,
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_goal_and_object_change_prodmp
)
ALL_METAWORLD_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProDMP"].append(_env_id)
_goal_and_endeffector_change_envs = ["basketball-v2"]
for _task in _goal_and_endeffector_change_envs:
task_id_split = _task.split("-")
name = "".join([s.capitalize() for s in task_id_split[:-1]])
# ProMP
_env_id = f'{name}ProMP-{task_id_split[-1]}'
kwargs_dict_goal_and_endeffector_change_promp = deepcopy(DEFAULT_BB_DICT_ProMP)
kwargs_dict_goal_and_endeffector_change_promp['wrappers'].append(goal_endeffector_change_mp_wrapper.MPWrapper)
kwargs_dict_goal_and_endeffector_change_promp['name'] = f'metaworld:{_task}'
register(
id=f'metaworld/{_task}',
register_step_based=False,
mp_wrapper=goal_endeffector_change_mp_wrapper.MPWrapper,
add_mp_types=['ProMP', 'ProDMP'],
id=_env_id,
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_goal_and_endeffector_change_promp
)
ALL_METAWORLD_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append(_env_id)
# ProDMP
_env_id = f'{name}ProDMP-{task_id_split[-1]}'
kwargs_dict_goal_and_endeffector_change_prodmp = deepcopy(DEFAULT_BB_DICT_ProDMP)
kwargs_dict_goal_and_endeffector_change_prodmp['wrappers'].append(goal_endeffector_change_mp_wrapper.MPWrapper)
kwargs_dict_goal_and_endeffector_change_prodmp['name'] = f'metaworld:{_task}'
register(
id=_env_id,
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_goal_and_endeffector_change_prodmp
)
ALL_METAWORLD_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProDMP"].append(_env_id)
+2 -53
View File
@@ -6,63 +6,12 @@ from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
class BaseMetaworldMPWrapper(RawInterfaceWrapper):
mp_config = {
'inherit_defaults': False,
'ProMP': {
'wrappers': [],
'trajectory_generator_kwargs': {
'trajectory_generator_type': 'promp',
'weights_scale': 10,
},
'phase_generator_kwargs': {
'phase_generator_type': 'linear'
},
'controller_kwargs': {
'controller_type': 'metaworld',
},
'basis_generator_kwargs': {
'basis_generator_type': 'zero_rbf',
'num_basis': 5,
'num_basis_zero_start': 1
},
'black_box_kwargs': {
'condition_on_desired': False,
},
},
'DMP': {},
'ProDMP': {
'wrappers': [],
'trajectory_generator_kwargs': {
'trajectory_generator_type': 'prodmp',
'auto_scale_basis': True,
'weights_scale': 10,
# 'goal_scale': 0.,
'disable_goal': True,
},
'phase_generator_kwargs': {
'phase_generator_type': 'exp',
# 'alpha_phase' : 3,
},
'controller_kwargs': {
'controller_type': 'metaworld',
},
'basis_generator_kwargs': {
'basis_generator_type': 'prodmp',
'num_basis': 5,
'alpha': 10
},
'black_box_kwargs': {
'condition_on_desired': False,
},
},
}
@property
def current_pos(self) -> Union[float, int, np.ndarray]:
r_close = self.env.data.joint('r_close').qpos
r_close = self.env.data.get_joint_qpos("r_close")
return np.hstack([self.env.data.mocap_pos.flatten() / self.env.action_scale, r_close])
@property
def current_vel(self) -> Union[float, int, np.ndarray, Tuple]:
return np.zeros(4, )
# raise NotImplementedError('Velocity cannot be retrieved.')
# raise NotImplementedError("Velocity cannot be retrieved.")
+13
View File
@@ -9,6 +9,19 @@ class MPWrapper(BaseMetaworldMPWrapper):
and no secondary objects or end effectors are altered at the start of an episode.
You can verify this by executing the code below for your environment id and check if the output is non-zero
at the same indices.
```python
import fancy_gym
env = fancy_gym.make(env_id, 1)
print(env.reset() - env.reset())
array([ 0. , 0. , 0. , 0. , 0,
0 , 0 , 0. , 0. , 0. ,
0. , 0. , 0. , 0. , 0. ,
0. , 0. , 0. , 0. , 0. ,
0. , 0. , 0 , 0 , 0 ,
0. , 0. , 0. , 0. , 0. ,
0. , 0. , 0. , 0. , 0. ,
0. , !=0 , !=0 , !=0])
```
"""
@property
@@ -9,6 +9,19 @@ class MPWrapper(BaseMetaworldMPWrapper):
and no secondary objects or end effectors are altered at the start of an episode.
You can verify this by executing the code below for your environment id and check if the output is non-zero
at the same indices.
```python
import fancy_gym
env = fancy_gym.make(env_id, 1)
print(env.reset() - env.reset())
array([ !=0 , !=0 , !=0 , 0. , 0.,
0. , 0. , 0. , 0. , 0. ,
0. , 0. , 0. , 0. , 0. ,
0. , 0. , 0. , !=0 , !=0 ,
!=0 , 0. , 0. , 0. , 0. ,
0. , 0. , 0. , 0. , 0. ,
0. , 0. , 0. , 0. , 0. ,
0. , !=0 , !=0 , !=0])
```
"""
@property
@@ -9,6 +9,19 @@ class MPWrapper(BaseMetaworldMPWrapper):
and no secondary objects or end effectors are altered at the start of an episode.
You can verify this by executing the code below for your environment id and check if the output is non-zero
at the same indices.
```python
import fancy_gym
env = fancy_gym.make(env_id, 1)
print(env.reset() - env.reset())
array([ 0. , 0. , 0. , 0. , !=0,
!=0 , !=0 , 0. , 0. , 0. ,
0. , 0. , 0. , 0. , 0. ,
0. , 0. , 0. , 0. , 0. ,
0. , 0. , !=0 , !=0 , !=0 ,
0. , 0. , 0. , 0. , 0. ,
0. , 0. , 0. , 0. , 0. ,
0. , !=0 , !=0 , !=0])
```
"""
@property
-97
View File
@@ -1,97 +0,0 @@
import random
from typing import Iterable, Type, Union, Optional
import numpy as np
from gymnasium import register as gym_register
import uuid
import gymnasium as gym
import numpy as np
from fancy_gym.utils.env_compatibility import EnvCompatibility
try:
import metaworld
except Exception:
print('[FANCY GYM] Metaworld not avaible')
class FixMetaworldHasIncorrectObsSpaceWrapper(gym.Wrapper, gym.utils.RecordConstructorArgs):
def __init__(self, env: gym.Env):
gym.utils.RecordConstructorArgs.__init__(self)
gym.Wrapper.__init__(self, env)
eos = env.observation_space
eas = env.action_space
Obs_Space_Class = getattr(gym.spaces, str(eos.__class__).split("'")[1].split('.')[-1])
Act_Space_Class = getattr(gym.spaces, str(eas.__class__).split("'")[1].split('.')[-1])
self.observation_space = Obs_Space_Class(low=eos.low-np.inf, high=eos.high+np.inf, dtype=eos.dtype)
self.action_space = Act_Space_Class(low=eas.low, high=eas.high, dtype=eas.dtype)
class FixMetaworldIncorrectResetPathLengthWrapper(gym.Wrapper, gym.utils.RecordConstructorArgs):
def __init__(self, env: gym.Env):
gym.utils.RecordConstructorArgs.__init__(self)
gym.Wrapper.__init__(self, env)
def reset(self, **kwargs):
ret = self.env.reset(**kwargs)
head = self.env
try:
for i in range(16):
head.curr_path_length = 0
head = head.env
except:
pass
return ret
class FixMetaworldIgnoresSeedOnResetWrapper(gym.Wrapper, gym.utils.RecordConstructorArgs):
def __init__(self, env: gym.Env):
gym.utils.RecordConstructorArgs.__init__(self)
gym.Wrapper.__init__(self, env)
def reset(self, **kwargs):
print('[!] You just called .reset on a Metaworld env and supplied a seed. Metaworld curretly does not correctly implement seeding. Do not rely on deterministic behavior.')
if 'seed' in kwargs:
self.env.seed(kwargs['seed'])
return self.env.reset(**kwargs)
def make_metaworld(underlying_id: str, seed: int = 1, render_mode: Optional[str] = None, **kwargs):
if underlying_id not in metaworld.ML1.ENV_NAMES:
raise ValueError(f'Specified environment "{underlying_id}" not present in metaworld ML1.')
env = metaworld.envs.ALL_V2_ENVIRONMENTS_GOAL_OBSERVABLE[underlying_id + "-goal-observable"](seed=seed, **kwargs)
# setting this avoids generating the same initialization after each reset
env._freeze_rand_vec = False
# New argument to use global seeding
env.seeded_rand_vec = True
# TODO remove, when this has been fixed upstream
env = FixMetaworldHasIncorrectObsSpaceWrapper(env)
# TODO remove, when this has been fixed upstream
# env = FixMetaworldIncorrectResetPathLengthWrapper(env)
# TODO remove, when this has been fixed upstream
env = FixMetaworldIgnoresSeedOnResetWrapper(env)
return env
def register_all_ML1(**kwargs):
for env_id in metaworld.ML1.ENV_NAMES:
_env = metaworld.envs.ALL_V2_ENVIRONMENTS_GOAL_OBSERVABLE[env_id + "-goal-observable"](seed=0)
max_episode_steps = _env.max_path_length
gym_register(
id='metaworld/'+env_id,
entry_point=make_metaworld,
max_episode_steps=max_episode_steps,
kwargs={
'underlying_id': env_id
},
**kwargs
)
+6 -7
View File
@@ -4,12 +4,11 @@ These are the Environment Wrappers for selected [OpenAI Gym](https://gym.openai.
the Motion Primitive gym interface for them.
## MP Environments
These environments are wrapped-versions of their OpenAI-gym counterparts.
| Name | Description | Trajectory Horizon | Action Dimension |
| ------------------------------------ | -------------------------------------------------------------------- | ------------------ | ---------------- |
| `gym_ProMP/ContinuousMountainCar-v0` | A ProMP wrapped version of the ContinuousMountainCar-v0 environment. | 100 | 1 |
| `gym_ProMP/Reacher-v2` | A ProMP wrapped version of the Reacher-v2 environment. | 50 | 2 |
| `gym_ProMP/FetchSlideDense-v1` | A ProMP wrapped version of the FetchSlideDense-v1 environment. | 50 | 4 |
| `gym_ProMP/FetchReachDense-v1` | A ProMP wrapped version of the FetchReachDense-v1 environment. | 50 | 4 |
|Name| Description|Trajectory Horizon|Action Dimension|Context Dimension
|---|---|---|---|---|
|`ContinuousMountainCarProMP-v0`| A ProMP wrapped version of the ContinuousMountainCar-v0 environment. | 100 | 1
|`ReacherProMP-v2`| A ProMP wrapped version of the Reacher-v2 environment. | 50 | 2
|`FetchSlideDenseProMP-v1`| A ProMP wrapped version of the FetchSlideDense-v1 environment. | 50 | 4
|`FetchReachDenseProMP-v1`| A ProMP wrapped version of the FetchReachDense-v1 environment. | 50 | 4
+35 -6
View File
@@ -1,16 +1,45 @@
from copy import deepcopy
from ..envs.registry import register, upgrade
from gym import register
from . import mujoco
from .deprecated_needs_gym_robotics import robotics
upgrade(
id='Reacher-v2',
mp_wrapper=mujoco.reacher_v2.MPWrapper,
add_mp_types=['ProMP'],
)
ALL_GYM_MOVEMENT_PRIMITIVE_ENVIRONMENTS = {"DMP": [], "ProMP": [], "ProDMP": []}
DEFAULT_BB_DICT_ProMP = {
"name": 'EnvName',
"wrappers": [],
"trajectory_generator_kwargs": {
'trajectory_generator_type': 'promp'
},
"phase_generator_kwargs": {
'phase_generator_type': 'linear'
},
"controller_kwargs": {
'controller_type': 'motor',
"p_gains": 1.0,
"d_gains": 0.1,
},
"basis_generator_kwargs": {
'basis_generator_type': 'zero_rbf',
'num_basis': 5,
'num_basis_zero_start': 1
}
}
kwargs_dict_reacher_promp = deepcopy(DEFAULT_BB_DICT_ProMP)
kwargs_dict_reacher_promp['controller_kwargs']['p_gains'] = 0.6
kwargs_dict_reacher_promp['controller_kwargs']['d_gains'] = 0.075
kwargs_dict_reacher_promp['basis_generator_kwargs']['num_basis'] = 6
kwargs_dict_reacher_promp['name'] = "Reacher-v2"
kwargs_dict_reacher_promp['wrappers'].append(mujoco.reacher_v2.MPWrapper)
register(
id='ReacherProMP-v2',
entry_point='fancy_gym.utils.make_env_helpers:make_bb_env_helper',
kwargs=kwargs_dict_reacher_promp
)
ALL_GYM_MOVEMENT_PRIMITIVE_ENVIRONMENTS["ProMP"].append("ReacherProMP-v2")
"""
The Fetch environments are not supported by gym anymore. A new repository (gym_robotics) is supporting the environments.
However, the usage and so on needs to be checked
@@ -6,28 +6,6 @@ from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
class MPWrapper(RawInterfaceWrapper):
mp_config = {
'ProMP': {
"trajectory_generator_kwargs": {
'trajectory_generator_type': 'promp'
},
"phase_generator_kwargs": {
'phase_generator_type': 'linear'
},
"controller_kwargs": {
'controller_type': 'motor',
"p_gains": 0.6,
"d_gains": 0.075,
},
"basis_generator_kwargs": {
'basis_generator_type': 'zero_rbf',
'num_basis': 6,
'num_basis_zero_start': 1
}
},
'DMP': {},
'ProDMP': {},
}
@property
def current_vel(self) -> Union[float, int, np.ndarray]:
-11
View File
@@ -1,11 +0,0 @@
import gymnasium as gym
class EnvCompatibility(gym.wrappers.EnvCompatibility):
def __getattr__(self, item):
"""Propagate only non-existent properties to wrapped env."""
if item.startswith('_'):
raise AttributeError("attempted to get missing private attribute '{}'".format(item))
if item in self.__dict__:
return getattr(self, item)
return getattr(self.env, item)
+239 -60
View File
@@ -1,27 +1,17 @@
from fancy_gym.utils.wrappers import TimeAwareObservation
from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
from fancy_gym.black_box.factory.trajectory_generator_factory import get_trajectory_generator
from fancy_gym.black_box.factory.phase_generator_factory import get_phase_generator
from fancy_gym.black_box.factory.controller_factory import get_controller
from fancy_gym.black_box.factory.basis_generator_factory import get_basis_generator
from fancy_gym.black_box.black_box_wrapper import BlackBoxWrapper
import logging
import re
import uuid
from collections.abc import MutableMapping
from copy import deepcopy
from math import ceil
from typing import Iterable, Type, Union, Optional
from typing import Iterable, Type, Union
import gymnasium as gym
from gymnasium import make
import gym
import numpy as np
from gymnasium.envs.registration import register, registry
from gymnasium.wrappers import TimeLimit
from fancy_gym.utils.env_compatibility import EnvCompatibility
from fancy_gym.utils.wrappers import FlattenObservation
from gym.envs.registration import register, registry
try:
import shimmy
from shimmy.dm_control_compatibility import EnvType
from dm_control import suite, manipulation
except ImportError:
pass
@@ -31,44 +21,111 @@ except Exception:
# catch Exception as Import error does not catch missing mujoco-py
pass
import fancy_gym
from fancy_gym.black_box.black_box_wrapper import BlackBoxWrapper
from fancy_gym.black_box.factory.basis_generator_factory import get_basis_generator
from fancy_gym.black_box.factory.controller_factory import get_controller
from fancy_gym.black_box.factory.phase_generator_factory import get_phase_generator
from fancy_gym.black_box.factory.trajectory_generator_factory import get_trajectory_generator
from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
from fancy_gym.utils.time_aware_observation import TimeAwareObservation
from fancy_gym.utils.utils import nested_update
def _make_wrapped_env(env: gym.Env, wrappers: Iterable[Type[gym.Wrapper]], seed=1, fallback_max_steps=None):
def make_rank(env_id: str, seed: int, rank: int = 0, return_callable=True, **kwargs):
"""
TODO: Do we need this?
Generate a callable to create a new gym environment with a given seed.
The rank is added to the seed and can be used for example when using vector environments.
E.g. [make_rank("my_env_name-v0", 123, i) for i in range(8)] creates a list of 8 environments
with seeds 123 through 130.
Hence, testing environments should be seeded with a value which is offset by the number of training environments.
Here e.g. [make_rank("my_env_name-v0", 123 + 8, i) for i in range(5)] for 5 testing environmetns
Args:
env_id: name of the environment
seed: seed for deterministic behaviour
rank: environment rank for deterministic over multiple seeds behaviour
return_callable: If True returns a callable to create the environment instead of the environment itself.
Returns:
"""
def f():
return make(env_id, seed + rank, **kwargs)
return f if return_callable else f()
def make(env_id: str, seed: int, **kwargs):
"""
Converts an env_id to an environment with the gym API.
This also works for DeepMind Control Suite environments that are wrapped using the DMCWrapper, they can be
specified with "dmc:domain_name-task_name"
Analogously, metaworld tasks can be created as "metaworld:env_id-v2".
Args:
env_id: spec or env_id for gym tasks, external environments require a domain specification
**kwargs: Additional kwargs for the constructor such as pixel observations, etc.
Returns: Gym environment
"""
if ':' in env_id:
split_id = env_id.split(':')
framework, env_id = split_id[-2:]
else:
framework = None
if framework == 'metaworld':
# MetaWorld environment
env = make_metaworld(env_id, seed, **kwargs)
elif framework == 'dmc':
# DeepMind Control environment
env = make_dmc(env_id, seed, **kwargs)
else:
env = make_gym(env_id, seed, **kwargs)
env.seed(seed)
env.action_space.seed(seed)
env.observation_space.seed(seed)
return env
def _make_wrapped_env(env_id: str, wrappers: Iterable[Type[gym.Wrapper]], seed=1, **kwargs):
"""
Helper function for creating a wrapped gym environment using MPs.
It adds all provided wrappers to the specified environment and verifies at least one RawInterfaceWrapper is
provided to expose the interface for MPs.
Args:
env: base environemnt to wrap
env_id: name of the environment
wrappers: list of wrappers (at least an RawInterfaceWrapper),
seed: seed of environment
Returns: gym environment with all specified wrappers applied
"""
if fallback_max_steps:
env = ensure_finite_time(env, fallback_max_steps)
# _env = gym.make(env_id)
_env = make(env_id, seed, **kwargs)
has_black_box_wrapper = False
head = env
while hasattr(head, 'env'):
if isinstance(head, RawInterfaceWrapper):
has_black_box_wrapper = True
break
head = head.env
for w in wrappers:
# only wrap the environment if not BlackBoxWrapper, e.g. for vision
if issubclass(w, RawInterfaceWrapper):
has_black_box_wrapper = True
env = w(env)
_env = w(_env)
if not has_black_box_wrapper:
raise ValueError("A RawInterfaceWrapper is required in order to leverage movement primitive environments.")
return env
return _env
def make_bb(
env: Union[gym.Env, str], wrappers: Iterable, black_box_kwargs: MutableMapping, traj_gen_kwargs: MutableMapping,
controller_kwargs: MutableMapping, phase_kwargs: MutableMapping, basis_kwargs: MutableMapping,
time_limit: int = None, fallback_max_steps: int = None, **kwargs):
env_id: str, wrappers: Iterable, black_box_kwargs: MutableMapping, traj_gen_kwargs: MutableMapping,
controller_kwargs: MutableMapping, phase_kwargs: MutableMapping, basis_kwargs: MutableMapping, seed: int = 1,
**kwargs):
"""
This can also be used standalone for manually building a custom DMP environment.
Args:
@@ -76,7 +133,7 @@ def make_bb(
basis_kwargs: kwargs for the basis generator
phase_kwargs: kwargs for the phase generator
controller_kwargs: kwargs for the tracking controller
env: step based environment (or environment id),
env_id: base_env_name,
wrappers: list of wrappers (at least an RawInterfaceWrapper),
seed: seed of environment
traj_gen_kwargs: dict of at least {num_dof: int, num_basis: int} for DMP
@@ -84,7 +141,7 @@ def make_bb(
Returns: DMP wrapped gym env
"""
_verify_time_limit(traj_gen_kwargs.get("duration"), time_limit)
_verify_time_limit(traj_gen_kwargs.get("duration"), kwargs.get("time_limit"))
learn_sub_trajs = black_box_kwargs.get('learn_sub_trajectories')
do_replanning = black_box_kwargs.get('replanning_schedule')
@@ -96,19 +153,12 @@ def make_bb(
# Add as first wrapper in order to alter observation
wrappers.insert(0, TimeAwareObservation)
if isinstance(env, str):
env = make(env, **kwargs)
env = _make_wrapped_env(env=env, wrappers=wrappers, fallback_max_steps=fallback_max_steps)
# BB expects a spaces.Box to be exposed, need to convert for dict-observations
if type(env.observation_space) == gym.spaces.dict.Dict:
env = FlattenObservation(env)
env = _make_wrapped_env(env_id=env_id, wrappers=wrappers, seed=seed, **kwargs)
traj_gen_kwargs['action_dim'] = traj_gen_kwargs.get('action_dim', np.prod(env.action_space.shape).item())
if black_box_kwargs.get('duration') is None:
black_box_kwargs['duration'] = get_env_duration(env)
black_box_kwargs['duration'] = env.spec.max_episode_steps * env.dt
if phase_kwargs.get('tau') is None:
phase_kwargs['tau'] = black_box_kwargs['duration']
@@ -136,27 +186,156 @@ def make_bb(
return bb_env
def ensure_finite_time(env: gym.Env, fallback_max_steps=500):
cur_limit = env.spec.max_episode_steps
if not cur_limit:
if hasattr(env.unwrapped, 'max_path_length'):
return TimeLimit(env, env.unwrapped.__getattribute__('max_path_length'))
return TimeLimit(env, fallback_max_steps)
def make_bb_env_helper(**kwargs):
"""
Helper function for registering a black box gym environment.
Args:
**kwargs: expects at least the following:
{
"name": base environment name.
"wrappers": list of wrappers (at least an BlackBoxWrapper is required),
"traj_gen_kwargs": {
"trajectory_generator_type": type_of_your_movement_primitive,
non default arguments for the movement primitive instance
...
}
"controller_kwargs": {
"controller_type": type_of_your_controller,
non default arguments for the tracking_controller instance
...
},
"basis_generator_kwargs": {
"basis_generator_type": type_of_your_basis_generator,
non default arguments for the basis generator instance
...
},
"phase_generator_kwargs": {
"phase_generator_type": type_of_your_phase_generator,
non default arguments for the phase generator instance
...
},
}
Returns: MP wrapped gym env
"""
seed = kwargs.pop("seed", None)
wrappers = kwargs.pop("wrappers")
traj_gen_kwargs = kwargs.pop("trajectory_generator_kwargs", {})
black_box_kwargs = kwargs.pop('black_box_kwargs', {})
contr_kwargs = kwargs.pop("controller_kwargs", {})
phase_kwargs = kwargs.pop("phase_generator_kwargs", {})
basis_kwargs = kwargs.pop("basis_generator_kwargs", {})
return make_bb(env_id=kwargs.pop("name"), wrappers=wrappers,
black_box_kwargs=black_box_kwargs,
traj_gen_kwargs=traj_gen_kwargs, controller_kwargs=contr_kwargs,
phase_kwargs=phase_kwargs,
basis_kwargs=basis_kwargs, **kwargs, seed=seed)
def make_dmc(
env_id: str,
seed: int = None,
visualize_reward: bool = True,
time_limit: Union[None, float] = None,
**kwargs
):
if not re.match(r"\w+-\w+", env_id):
raise ValueError("env_id does not have the following structure: 'domain_name-task_name'")
domain_name, task_name = env_id.split("-")
if task_name.endswith("_vision"):
# TODO
raise ValueError("The vision interface for manipulation tasks is currently not supported.")
if (domain_name, task_name) not in suite.ALL_TASKS and task_name not in manipulation.ALL:
raise ValueError(f'Specified domain "{domain_name}" and task "{task_name}" combination does not exist.')
# env_id = f'dmc_{domain_name}_{task_name}_{seed}-v1'
gym_id = uuid.uuid4().hex + '-v1'
task_kwargs = {'random': seed}
if time_limit is not None:
task_kwargs['time_limit'] = time_limit
# create task
# Accessing private attribute because DMC does not expose time_limit or step_limit.
# Only the current time_step/time as well as the control_timestep can be accessed.
if domain_name == "manipulation":
env = manipulation.load(environment_name=task_name, seed=seed)
max_episode_steps = ceil(env._time_limit / env.control_timestep())
else:
env = suite.load(domain_name=domain_name, task_name=task_name, task_kwargs=task_kwargs,
visualize_reward=visualize_reward, environment_kwargs=kwargs)
max_episode_steps = int(env._step_limit)
register(
id=gym_id,
entry_point='fancy_gym.dmc.dmc_wrapper:DMCWrapper',
kwargs={'env': lambda: env},
max_episode_steps=max_episode_steps,
)
env = gym.make(gym_id)
env.seed(seed)
return env
def get_env_duration(env: gym.Env):
def make_metaworld(env_id: str, seed: int, **kwargs):
if env_id not in metaworld.ML1.ENV_NAMES:
raise ValueError(f'Specified environment "{env_id}" not present in metaworld ML1.')
_env = metaworld.envs.ALL_V2_ENVIRONMENTS_GOAL_OBSERVABLE[env_id + "-goal-observable"](seed=seed, **kwargs)
# setting this avoids generating the same initialization after each reset
_env._freeze_rand_vec = False
# New argument to use global seeding
_env.seeded_rand_vec = True
gym_id = uuid.uuid4().hex + '-v1'
register(
id=gym_id,
entry_point=lambda: _env,
max_episode_steps=_env.max_path_length,
)
# TODO enable checker when the incorrect dtype of obs and observation space are fixed by metaworld
env = gym.make(gym_id, disable_env_checker=True)
return env
def make_gym(env_id, seed, **kwargs):
"""
Create
Args:
env_id:
seed:
**kwargs:
Returns:
"""
# Getting the existing keywords to allow for nested dict updates for BB envs
# gym only allows for non nested updates.
try:
duration = env.spec.max_episode_steps * env.dt
except (AttributeError, TypeError) as e:
if env.env_type is EnvType.COMPOSER:
max_episode_steps = ceil(env.unwrapped._time_limit / env.dt)
elif env.env_type is EnvType.RL_CONTROL:
max_episode_steps = int(env.unwrapped._step_limit)
else:
raise e
duration = max_episode_steps * env.control_timestep()
return duration
all_kwargs = deepcopy(registry.get(env_id).kwargs)
except AttributeError as e:
logging.error(f'The gym environment with id {env_id} could not been found.')
raise e
nested_update(all_kwargs, kwargs)
kwargs = all_kwargs
# Add seed to kwargs for bb environments to pass seed to step environments
all_bb_envs = sum(fancy_gym.ALL_MOVEMENT_PRIMITIVE_ENVIRONMENTS.values(), [])
if env_id in all_bb_envs:
kwargs.update({"seed": seed})
# Gym
env = gym.make(env_id, **kwargs)
return env
def _verify_time_limit(mp_time_limit: Union[None, float], env_time_limit: Union[None, float]):
+78
View File
@@ -0,0 +1,78 @@
"""
Adapted from: https://github.com/openai/gym/blob/907b1b20dd9ac0cba5803225059b9c6673702467/gym/wrappers/time_aware_observation.py
License: MIT
Copyright (c) 2016 OpenAI (https://openai.com)
Wrapper for adding time aware observations to environment observation.
"""
import gym
import numpy as np
from gym.spaces import Box
class TimeAwareObservation(gym.ObservationWrapper):
"""Augment the observation with the current time step in the episode.
The observation space of the wrapped environment is assumed to be a flat :class:`Box`.
In particular, pixel observations are not supported. This wrapper will append the current timestep
within the current episode to the observation.
Example:
>>> import gym
>>> env = gym.make('CartPole-v1')
>>> env = TimeAwareObservation(env)
>>> env.reset()
array([ 0.03810719, 0.03522411, 0.02231044, -0.01088205, 0. ])
>>> env.step(env.action_space.sample())[0]
array([ 0.03881167, -0.16021058, 0.0220928 , 0.28875574, 1. ])
"""
def __init__(self, env: gym.Env):
"""Initialize :class:`TimeAwareObservation` that requires an environment with a flat :class:`Box`
observation space.
Args:
env: The environment to apply the wrapper
"""
super().__init__(env)
assert isinstance(env.observation_space, Box)
low = np.append(self.observation_space.low, 0.0)
high = np.append(self.observation_space.high, 1.0)
self.observation_space = Box(low, high, dtype=self.observation_space.dtype)
self.t = 0
self._max_episode_steps = env.spec.max_episode_steps
def observation(self, observation):
"""Adds to the observation with the current time step normalized with max steps.
Args:
observation: The observation to add the time step to
Returns:
The observation with the time step appended to
"""
return np.append(observation, self.t / self._max_episode_steps)
def step(self, action):
"""Steps through the environment, incrementing the time step.
Args:
action: The action to take
Returns:
The environment's step using the action.
"""
self.t += 1
return super().step(action)
def reset(self, **kwargs):
"""Reset the environment setting the time to zero.
Args:
**kwargs: Kwargs to apply to env.reset()
Returns:
The reset environment
"""
self.t = 0
return super().reset(**kwargs)
-130
View File
@@ -1,130 +0,0 @@
from gymnasium.spaces import Box, Dict, flatten, flatten_space
try:
from gym.spaces import Box as OldBox
except ImportError:
OldBox = None
import gymnasium as gym
import numpy as np
import copy
class TimeAwareObservation(gym.ObservationWrapper, gym.utils.RecordConstructorArgs):
"""Augment the observation with the current time step in the episode.
The observation space of the wrapped environment is assumed to be a flat :class:`Box` or flattable :class:`Dict`.
In particular, pixel observations are not supported. This wrapper will append the current progress within the current episode to the observation.
The progress will be indicated as a number between 0 and 1.
"""
def __init__(self, env: gym.Env, enforce_dtype_float32=False):
"""Initialize :class:`TimeAwareObservation` that requires an environment with a flat :class:`Box` or flattable :class:`Dict` observation space.
Args:
env: The environment to apply the wrapper
"""
gym.utils.RecordConstructorArgs.__init__(self)
gym.ObservationWrapper.__init__(self, env)
allowed_classes = [Box, OldBox, Dict]
if enforce_dtype_float32:
assert env.observation_space.dtype == np.float32, 'TimeAwareObservation was given an environment with a dtype!=np.float32 ('+str(
env.observation_space.dtype)+'). This requirement can be removed by setting enforce_dtype_float32=False.'
assert env.observation_space.__class__ in allowed_classes, str(env.observation_space)+' is not supported. Only Box or Dict'
if env.observation_space.__class__ in [Box, OldBox]:
dtype = env.observation_space.dtype
low = np.append(env.observation_space.low, 0.0)
high = np.append(env.observation_space.high, 1.0)
self.observation_space = Box(low, high, dtype=dtype)
else:
spaces = copy.copy(env.observation_space.spaces)
dtype = np.float64
spaces['time_awareness'] = Box(0, 1, dtype=dtype)
self.observation_space = Dict(spaces)
self.is_vector_env = getattr(env, "is_vector_env", False)
def observation(self, observation):
"""Adds to the observation with the current time step.
Args:
observation: The observation to add the time step to
Returns:
The observation with the time step appended to (relative to total number of steps)
"""
if self.observation_space.__class__ in [Box, OldBox]:
return np.append(observation, self.t / self.env.spec.max_episode_steps)
else:
obs = copy.copy(observation)
obs['time_awareness'] = self.t / self.env.spec.max_episode_steps
return obs
def step(self, action):
"""Steps through the environment, incrementing the time step.
Args:
action: The action to take
Returns:
The environment's step using the action.
"""
self.t += 1
return super().step(action)
def reset(self, **kwargs):
"""Reset the environment setting the time to zero.
Args:
**kwargs: Kwargs to apply to env.reset()
Returns:
The reset environment
"""
self.t = 0
return super().reset(**kwargs)
class FlattenObservation(gym.ObservationWrapper, gym.utils.RecordConstructorArgs):
"""Observation wrapper that flattens the observation.
Example:
>>> import gymnasium as gym
>>> from gymnasium.wrappers import FlattenObservation
>>> env = gym.make("CarRacing-v2")
>>> env.observation_space.shape
(96, 96, 3)
>>> env = FlattenObservation(env)
>>> env.observation_space.shape
(27648,)
>>> obs, _ = env.reset()
>>> obs.shape
(27648,)
"""
def __init__(self, env: gym.Env):
"""Flattens the observations of an environment.
Args:
env: The environment to apply the wrapper
"""
gym.utils.RecordConstructorArgs.__init__(self)
gym.ObservationWrapper.__init__(self, env)
self.observation_space = flatten_space(env.observation_space)
def observation(self, observation):
"""Flattens an observation.
Args:
observation: The observation to flatten
Returns:
The flattened observation
"""
try:
return flatten(self.env.observation_space, observation)
except:
return np.array([flatten(self.env.observation_space, observation[i]) for i in range(len(observation))])
-101
View File
File diff suppressed because one or more lines are too long

Before

Width:  |  Height:  |  Size: 114 KiB

+12 -18
View File
@@ -6,38 +6,33 @@ from setuptools import setup, find_packages
# Environment-specific dependencies for dmc and metaworld
extras = {
'dmc': ['shimmy[dm-control]', 'Shimmy==1.0.0'],
'metaworld': ['metaworld @ git+https://github.com/Farama-Foundation/Metaworld.git@d155d0051630bb365ea6a824e02c66c068947439#egg=metaworld'],
'box2d': ['gymnasium[box2d]>=0.26.0'],
'mujoco': ['mujoco==2.3.3', 'gymnasium[mujoco]>0.26.0'],
'mujoco-legacy': ['mujoco-py >=2.1,<2.2', 'cython<3'],
'jax': ["jax >=0.4.0", "jaxlib >=0.4.0"],
"dmc": ["dm_control>=1.0.1"],
"metaworld": ["metaworld @ git+https://github.com/rlworkgroup/metaworld.git@master#egg=metaworld",
'mujoco-py<2.2,>=2.1',
'scipy'
],
}
# All dependencies
all_groups = set(extras.keys())
extras["all"] = list(set(itertools.chain.from_iterable(
map(lambda group: extras[group], all_groups))))
extras['testing'] = extras["all"] + ['pytest']
extras["all"] = list(set(itertools.chain.from_iterable(map(lambda group: extras[group], all_groups))))
def find_package_data(extensions_to_include: List[str]) -> List[str]:
envs_dir = Path("fancy_gym/envs/mujoco")
package_data_paths = []
for extension in extensions_to_include:
package_data_paths.extend([str(path)[10:]
for path in envs_dir.rglob(extension)])
package_data_paths.extend([str(path)[10:] for path in envs_dir.rglob(extension)])
return package_data_paths
setup(
author='Fabian Otto, Onur Celik, Dominik Roth, Hongyi Zhou',
author='Fabian Otto, Onur Celik',
name='fancy_gym',
version='1.0',
version='0.2',
classifiers=[
'Development Status :: 4 - Beta',
'Development Status :: 3 - Alpha',
'Intended Audience :: Science/Research',
'License :: OSI Approved :: MIT License',
'Natural Language :: English',
@@ -51,11 +46,10 @@ setup(
],
extras_require=extras,
install_requires=[
'gymnasium>=0.26.0',
'gym[mujoco]<0.25.0,>=0.24.1',
'mp_pytorch<=0.1.3'
],
packages=[package for package in find_packages(
) if package.startswith("fancy_gym")],
packages=[package for package in find_packages() if package.startswith("fancy_gym")],
package_data={
"fancy_gym": find_package_data(extensions_to_include=["*.stl", "*.xml"])
},
+38 -60
View File
@@ -1,23 +1,21 @@
from itertools import chain
from typing import Tuple, Type, Union, Optional, Callable
import gymnasium as gym
import gym
import numpy as np
import pytest
from gymnasium import register, make
from gymnasium.core import ActType, ObsType
from gym import register
from gym.core import ActType, ObsType
import fancy_gym
from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
from fancy_gym.utils.wrappers import TimeAwareObservation
from fancy_gym.utils.time_aware_observation import TimeAwareObservation
SEED = 1
ENV_IDS = ['fancy/Reacher5d-v0', 'dm_control/ball_in_cup-catch-v0', 'metaworld/reach-v2', 'Reacher-v2']
ENV_IDS = ['Reacher5d-v0', 'dmc:ball_in_cup-catch', 'metaworld:reach-v2', 'Reacher-v2']
WRAPPERS = [fancy_gym.envs.mujoco.reacher.MPWrapper, fancy_gym.dmc.suite.ball_in_cup.MPWrapper,
fancy_gym.meta.goal_object_change_mp_wrapper.MPWrapper, fancy_gym.open_ai.mujoco.reacher_v2.MPWrapper]
ALL_MP_ENVS = fancy_gym.ALL_MOVEMENT_PRIMITIVE_ENVIRONMENTS['all']
MAX_STEPS_FALLBACK = 100
ALL_MP_ENVS = chain(*fancy_gym.ALL_MOVEMENT_PRIMITIVE_ENVIRONMENTS.values())
class Object(object):
@@ -34,12 +32,10 @@ class ToyEnv(gym.Env):
def reset(self, *, seed: Optional[int] = None, return_info: bool = False,
options: Optional[dict] = None) -> Union[ObsType, Tuple[ObsType, dict]]:
obs, options = np.array([-1]), {}
return obs, options
return np.array([-1])
def step(self, action: ActType) -> Tuple[ObsType, float, bool, dict]:
obs, reward, terminated, truncated, info = np.array([-1]), 1, False, False, {}
return obs, reward, terminated, truncated, info
return np.array([-1]), 1, False, {}
def render(self, mode="human"):
pass
@@ -80,7 +76,7 @@ def test_missing_local_state(mp_type: str):
{'controller_type': 'motor'},
{'phase_generator_type': 'exp'},
{'basis_generator_type': basis_generator_type})
env.reset(seed=SEED)
env.reset()
with pytest.raises(NotImplementedError):
env.step(env.action_space.sample())
@@ -97,14 +93,12 @@ def test_verbosity(mp_type: str, env_wrap: Tuple[str, Type[RawInterfaceWrapper]]
{'controller_type': 'motor'},
{'phase_generator_type': 'exp'},
{'basis_generator_type': basis_generator_type})
env.reset(seed=SEED)
_obs, _reward, _terminated, _truncated, info = env.step(env.action_space.sample())
info_keys = list(info.keys())
env.reset()
info_keys = list(env.step(env.action_space.sample())[3].keys())
env_step = make(env_id)
env_step = fancy_gym.make(env_id, SEED)
env_step.reset()
_obs, _reward, _terminated, _truncated, info = env_step.step(env_step.action_space.sample())
info_keys_step = info.keys()
info_keys_step = env_step.step(env_step.action_space.sample())[3].keys()
assert all(e in info_keys for e in info_keys_step)
assert 'trajectory_length' in info_keys
@@ -124,15 +118,13 @@ def test_length(mp_type: str, env_wrap: Tuple[str, Type[RawInterfaceWrapper]]):
{'trajectory_generator_type': mp_type},
{'controller_type': 'motor'},
{'phase_generator_type': 'exp'},
{'basis_generator_type': basis_generator_type}, fallback_max_steps=MAX_STEPS_FALLBACK)
{'basis_generator_type': basis_generator_type})
for i in range(5):
env.reset(seed=SEED)
for _ in range(5):
env.reset()
length = env.step(env.action_space.sample())[3]['trajectory_length']
_obs, _reward, _terminated, _truncated, info = env.step(env.action_space.sample())
length = info['trajectory_length']
assert length == env.spec.max_episode_steps, f'Expcted total simulation length ({length}) to be equal to spec.max_episode_steps ({env.spec.max_episode_steps}), but was not during test nr. {i}'
assert length == env.spec.max_episode_steps
@pytest.mark.parametrize('mp_type', ['promp', 'dmp', 'prodmp'])
@@ -144,10 +136,9 @@ def test_aggregation(mp_type: str, reward_aggregation: Callable[[np.ndarray], fl
{'controller_type': 'motor'},
{'phase_generator_type': 'exp'},
{'basis_generator_type': basis_generator_type})
env.reset(seed=SEED)
env.reset()
# ToyEnv only returns 1 as reward
_obs, reward, _terminated, _truncated, _info = env.step(env.action_space.sample())
assert reward == reward_aggregation(np.ones(50, ))
assert env.step(env.action_space.sample())[1] == reward_aggregation(np.ones(50, ))
@pytest.mark.parametrize('mp_type', ['promp', 'dmp'])
@@ -160,16 +151,14 @@ def test_context_space(mp_type: str, env_wrap: Tuple[str, Type[RawInterfaceWrapp
{'phase_generator_type': 'exp'},
{'basis_generator_type': 'rbf'})
# check if observation space matches with the specified mask values which are true
env_step = make(env_id)
env_step = fancy_gym.make(env_id, SEED)
wrapper = wrapper_class(env_step)
assert env.observation_space.shape == wrapper.context_mask[wrapper.context_mask].shape
@pytest.mark.parametrize('mp_type', ['promp', 'dmp', 'prodmp'])
@pytest.mark.parametrize('num_dof', [0, 1, 2, 5])
@pytest.mark.parametrize('num_basis', [
pytest.param(0, marks=pytest.mark.xfail(reason="Basis Length 0 is not yet implemented.")),
1, 2, 5])
@pytest.mark.parametrize('num_basis', [0, 1, 2, 5])
@pytest.mark.parametrize('learn_tau', [True, False])
@pytest.mark.parametrize('learn_delay', [True, False])
def test_action_space(mp_type: str, num_dof: int, num_basis: int, learn_tau: bool, learn_delay: bool):
@@ -230,18 +219,16 @@ def test_learn_tau(mp_type: str, tau: float):
'learn_delay': False
},
{'basis_generator_type': basis_generator_type,
})
}, seed=SEED)
env.reset(seed=SEED)
done = True
d = True
for i in range(5):
if done:
env.reset(seed=SEED)
if d:
env.reset()
action = env.action_space.sample()
action[0] = tau
_obs, _reward, terminated, truncated, info = env.step(action)
done = terminated or truncated
obs, r, d, info = env.step(action)
length = info['trajectory_length']
assert length == env.spec.max_episode_steps
@@ -261,8 +248,6 @@ def test_learn_tau(mp_type: str, tau: float):
assert np.all(vel[:tau_time_steps - 2] != vel[-1])
#
#
@pytest.mark.parametrize('mp_type', ['promp', 'prodmp'])
@pytest.mark.parametrize('delay', [0, 0.25, 0.5, 0.75])
def test_learn_delay(mp_type: str, delay: float):
@@ -277,18 +262,16 @@ def test_learn_delay(mp_type: str, delay: float):
'learn_delay': True
},
{'basis_generator_type': basis_generator_type,
})
}, seed=SEED)
env.reset(seed=SEED)
done = True
d = True
for i in range(5):
if done:
env.reset(seed=SEED)
if d:
env.reset()
action = env.action_space.sample()
action[0] = delay
_obs, _reward, terminated, truncated, info = env.step(action)
done = terminated or truncated
obs, r, d, info = env.step(action)
length = info['trajectory_length']
assert length == env.spec.max_episode_steps
@@ -307,8 +290,6 @@ def test_learn_delay(mp_type: str, delay: float):
assert np.all(vel[max(1, delay_time_steps)] != vel[0])
#
#
@pytest.mark.parametrize('mp_type', ['promp', 'prodmp'])
@pytest.mark.parametrize('tau', [0.25, 0.5, 0.75, 1])
@pytest.mark.parametrize('delay', [0.25, 0.5, 0.75, 1])
@@ -324,23 +305,20 @@ def test_learn_tau_and_delay(mp_type: str, tau: float, delay: float):
'learn_delay': True
},
{'basis_generator_type': basis_generator_type,
})
env.reset(seed=SEED)
}, seed=SEED)
if env.spec.max_episode_steps * env.dt < delay + tau:
return
done = True
d = True
for i in range(5):
if done:
env.reset(seed=SEED)
if d:
env.reset()
action = env.action_space.sample()
action[0] = tau
action[1] = delay
_obs, _reward, terminated, truncated, info = env.step(action)
done = terminated or truncated
obs, r, d, info = env.step(action)
length = info['trajectory_length']
assert length == env.spec.max_episode_steps
@@ -365,4 +343,4 @@ def test_learn_tau_and_delay(mp_type: str, tau: float, delay: float):
active_pos = pos[delay_time_steps: joint_time_steps - 1]
active_vel = vel[delay_time_steps: joint_time_steps - 2]
assert np.all(active_pos != pos[-1]) and np.all(active_pos != pos[0])
assert np.all(active_vel != vel[-1]) and np.all(active_vel != vel[0])
assert np.all(active_vel != vel[-1]) and np.all(active_vel != vel[0])
+22 -13
View File
@@ -1,30 +1,39 @@
from itertools import chain
from typing import Callable
import gymnasium as gym
import pytest
from dm_control import suite, manipulation
import fancy_gym
from test.utils import run_env, run_env_determinism
DMC_IDS = [spec.id for spec in gym.envs.registry.values() if
spec.id.startswith('dm_control/')
and 'compatibility-env-v0' not in spec.id
and 'lqr-lqr' not in spec.id]
DMC_MP_IDS = fancy_gym.ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS['all']
SUITE_IDS = [f'dmc:{env}-{task}' for env, task in suite.ALL_TASKS if env != "lqr"]
MANIPULATION_IDS = [f'dmc:manipulation-{task}' for task in manipulation.ALL if task.endswith('_features')]
DMC_MP_IDS = chain(*fancy_gym.ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS.values())
SEED = 1
@pytest.mark.parametrize('env_id', DMC_IDS)
def test_step_dm_control_functionality(env_id: str):
@pytest.mark.parametrize('env_id', SUITE_IDS)
def test_step_suite_functionality(env_id: str):
"""Tests that suite step environments run without errors using random actions."""
run_env(env_id, 5000, wrappers=[gym.wrappers.FlattenObservation])
run_env(env_id)
@pytest.mark.parametrize('env_id', DMC_IDS)
def test_step_dm_control_determinism(env_id: str):
@pytest.mark.parametrize('env_id', SUITE_IDS)
def test_step_suite_determinism(env_id: str):
"""Tests that for step environments identical seeds produce identical trajectories."""
run_env_determinism(env_id, SEED, 5000, wrappers=[gym.wrappers.FlattenObservation])
run_env_determinism(env_id, SEED)
@pytest.mark.parametrize('env_id', MANIPULATION_IDS)
def test_step_manipulation_functionality(env_id: str):
"""Tests that manipulation step environments run without errors using random actions."""
run_env(env_id)
@pytest.mark.parametrize('env_id', MANIPULATION_IDS)
def test_step_manipulation_determinism(env_id: str):
"""Tests that for step environments identical seeds produce identical trajectories."""
run_env_determinism(env_id, SEED)
@pytest.mark.parametrize('env_id', DMC_MP_IDS)
+4 -6
View File
@@ -1,16 +1,14 @@
from itertools import chain
from typing import Callable
import itertools
import fancy_gym
import gymnasium as gym
import gym
import pytest
from test.utils import run_env, run_env_determinism
CUSTOM_IDS = [id for id, spec in gym.envs.registry.items() if
not isinstance(spec.entry_point, Callable) and
CUSTOM_IDS = [spec.id for spec in gym.envs.registry.all() if
"fancy_gym" in spec.entry_point and 'make_bb_env_helper' not in spec.entry_point]
CUSTOM_MP_IDS = fancy_gym.ALL_FANCY_MOVEMENT_PRIMITIVE_ENVIRONMENTS['all']
CUSTOM_MP_IDS = itertools.chain(*fancy_gym.ALL_FANCY_MOVEMENT_PRIMITIVE_ENVIRONMENTS.values())
SEED = 1
-78
View File
@@ -1,78 +0,0 @@
from typing import Tuple, Type, Union, Optional, Callable
import gymnasium as gym
import numpy as np
import pytest
from gymnasium import make
from gymnasium.core import ActType, ObsType
import fancy_gym
from fancy_gym import register
KNOWN_NS = ['dm_control', 'fancy', 'metaworld', 'gym']
class Object(object):
pass
class ToyEnv(gym.Env):
observation_space = gym.spaces.Box(low=-1, high=1, shape=(1,), dtype=np.float64)
action_space = gym.spaces.Box(low=-1, high=1, shape=(1,), dtype=np.float64)
dt = 0.02
def __init__(self, a: int = 0, b: float = 0.0, c: list = [], d: dict = {}, e: Object = Object()):
self.a, self.b, self.c, self.d, self.e = a, b, c, d, e
def reset(self, *, seed: Optional[int] = None, return_info: bool = False,
options: Optional[dict] = None) -> Union[ObsType, Tuple[ObsType, dict]]:
obs, options = np.array([-1]), {}
return obs, options
def step(self, action: ActType) -> Tuple[ObsType, float, bool, dict]:
obs, reward, terminated, truncated, info = np.array([-1]), 1, False, False, {}
return obs, reward, terminated, truncated, info
def render(self, mode="human"):
pass
@pytest.fixture(scope="session", autouse=True)
def setup():
register(
id=f'dummy/toy2-v0',
entry_point='test.test_black_box:ToyEnv',
max_episode_steps=50,
)
@pytest.mark.parametrize('env_id', ['dummy/toy2-v0'])
@pytest.mark.parametrize('mp_type', ['ProMP', 'DMP', 'ProDMP'])
def test_make_mp(env_id: str, mp_type: str):
parts = env_id.split('/')
if len(parts) == 1:
ns, name = 'gym', parts[0]
elif len(parts) == 2:
ns, name = parts[0], parts[1]
else:
raise ValueError('env id can not contain multiple "/".')
fancy_id = f'{ns}_{mp_type}/{name}'
make(fancy_id)
def test_make_raw_toy():
make('dummy/toy2-v0')
@pytest.mark.parametrize('mp_type', ['ProMP', 'DMP', 'ProDMP'])
def test_make_mp_toy(mp_type: str):
fancy_id = f'dummy_{mp_type}/toy2-v0'
make(fancy_id)
@pytest.mark.parametrize('ns', KNOWN_NS)
def test_ns_nonempty(ns):
assert len(fancy_gym.MOVEMENT_PRIMITIVE_ENVIRONMENTS_FOR_NS[ns]), f'The namespace {ns} is empty even though, it should not be...'
@@ -1,25 +1,17 @@
import re
from itertools import chain
from typing import Callable
import gymnasium as gym
import gym
import pytest
import fancy_gym
from test.utils import run_env, run_env_determinism
GYM_IDS = [spec.id for spec in gym.envs.registry.values() if
not isinstance(spec.entry_point, Callable) and
"fancy_gym" not in spec.entry_point and 'make_bb_env_helper' not in spec.entry_point
and 'jax' not in spec.id.lower()
and 'shimmy' not in spec.id.lower()
and 'ale_py' not in spec.id.lower()
and 'tabular' not in spec.id.lower()
and not re.match(r'GymV2.Environment', spec.id)
]
GYM_MP_IDS = fancy_gym.ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS['all']
GYM_IDS = [spec.id for spec in gym.envs.registry.all() if
"fancy_gym" not in spec.entry_point and 'make_bb_env_helper' not in spec.entry_point]
GYM_MP_IDS = chain(*fancy_gym.ALL_DMC_MOVEMENT_PRIMITIVE_ENVIRONMENTS.values())
SEED = 1
@pytest.mark.parametrize('env_id', GYM_IDS)
def test_step_gym_functionality(env_id: str):
"""Tests that step environments run without errors using random actions."""
+2 -4
View File
@@ -6,9 +6,9 @@ from metaworld.envs import ALL_V2_ENVIRONMENTS_GOAL_OBSERVABLE
import fancy_gym
from test.utils import run_env, run_env_determinism
METAWORLD_IDS = [f'metaworld/{env.split("-goal-observable")[0]}' for env, _ in
METAWORLD_IDS = [f'metaworld:{env.split("-goal-observable")[0]}' for env, _ in
ALL_V2_ENVIRONMENTS_GOAL_OBSERVABLE.items()]
METAWORLD_MP_IDS = fancy_gym.ALL_METAWORLD_MOVEMENT_PRIMITIVE_ENVIRONMENTS['all']
METAWORLD_MP_IDS = chain(*fancy_gym.ALL_METAWORLD_MOVEMENT_PRIMITIVE_ENVIRONMENTS.values())
SEED = 1
@@ -18,7 +18,6 @@ def test_step_metaworld_functionality(env_id: str):
run_env(env_id)
@pytest.mark.skip(reason="Seeding does not correctly work on current Metaworld.")
@pytest.mark.parametrize('env_id', METAWORLD_IDS)
def test_step_metaworld_determinism(env_id: str):
"""Tests that for step environments identical seeds produce identical trajectories."""
@@ -31,7 +30,6 @@ def test_bb_metaworld_functionality(env_id: str):
run_env(env_id)
@pytest.mark.skip(reason="Seeding does not correctly work on current Metaworld.")
@pytest.mark.parametrize('env_id', METAWORLD_MP_IDS)
def test_bb_metaworld_determinism(env_id: str):
"""Tests that for black box environment identical seeds produce identical trajectories."""
+74 -105
View File
@@ -2,25 +2,21 @@ from itertools import chain
from types import FunctionType
from typing import Tuple, Type, Union, Optional
import gymnasium as gym
import gym
import numpy as np
import pytest
from gymnasium import register, make
from gymnasium.core import ActType, ObsType
from gymnasium import spaces
from gym import register
from gym.core import ActType, ObsType
import fancy_gym
from fancy_gym.black_box.raw_interface_wrapper import RawInterfaceWrapper
from fancy_gym.utils.wrappers import TimeAwareObservation
from fancy_gym.utils.make_env_helpers import ensure_finite_time
from fancy_gym.utils.time_aware_observation import TimeAwareObservation
SEED = 1
ENV_IDS = ['fancy/Reacher5d-v0', 'dm_control/ball_in_cup-catch-v0', 'metaworld/reach-v2', 'Reacher-v2']
ENV_IDS = ['Reacher5d-v0', 'dmc:ball_in_cup-catch', 'metaworld:reach-v2', 'Reacher-v2']
WRAPPERS = [fancy_gym.envs.mujoco.reacher.MPWrapper, fancy_gym.dmc.suite.ball_in_cup.MPWrapper,
fancy_gym.meta.goal_object_change_mp_wrapper.MPWrapper, fancy_gym.open_ai.mujoco.reacher_v2.MPWrapper]
ALL_MP_ENVS = fancy_gym.ALL_MOVEMENT_PRIMITIVE_ENVIRONMENTS['all']
MAX_STEPS_FALLBACK = 50
ALL_MP_ENVS = chain(*fancy_gym.ALL_MOVEMENT_PRIMITIVE_ENVIRONMENTS.values())
class ToyEnv(gym.Env):
@@ -30,12 +26,10 @@ class ToyEnv(gym.Env):
def reset(self, *, seed: Optional[int] = None, return_info: bool = False,
options: Optional[dict] = None) -> Union[ObsType, Tuple[ObsType, dict]]:
obs, options = np.array([-1]), {}
return obs, options
return np.array([-1])
def step(self, action: ActType) -> Tuple[ObsType, float, bool, dict]:
obs, reward, terminated, truncated, info = np.array([-1]), 1, False, False, {}
return obs, reward, terminated, truncated, info
return np.array([-1]), 1, False, {}
def render(self, mode="human"):
pass
@@ -67,7 +61,7 @@ def setup():
def test_learn_sub_trajectories(mp_type: str, env_wrap: Tuple[str, Type[RawInterfaceWrapper]],
add_time_aware_wrapper_before: bool):
env_id, wrapper_class = env_wrap
env_step = TimeAwareObservation(ensure_finite_time(make(env_id, SEED), MAX_STEPS_FALLBACK))
env_step = TimeAwareObservation(fancy_gym.make(env_id, SEED))
wrappers = [wrapper_class]
# has time aware wrapper
@@ -78,29 +72,24 @@ def test_learn_sub_trajectories(mp_type: str, env_wrap: Tuple[str, Type[RawInter
{'trajectory_generator_type': mp_type},
{'controller_type': 'motor'},
{'phase_generator_type': 'exp'},
{'basis_generator_type': 'rbf'}, fallback_max_steps=MAX_STEPS_FALLBACK)
env.reset(seed=SEED)
{'basis_generator_type': 'rbf'}, seed=SEED)
assert env.learn_sub_trajectories
assert env.spec.max_episode_steps
assert env_step.spec.max_episode_steps
assert env.traj_gen.learn_tau
# This also verifies we are not adding the TimeAwareObservationWrapper twice
assert spaces.flatten_space(env_step.observation_space) == spaces.flatten_space(env.observation_space)
assert env.observation_space == env_step.observation_space
done = True
d = True
for i in range(25):
if done:
env.reset(seed=SEED)
if d:
env.reset()
action = env.action_space.sample()
_obs, _reward, terminated, truncated, info = env.step(action)
done = terminated or truncated
obs, r, d, info = env.step(action)
length = info['trajectory_length']
if not done:
if not d:
assert length == np.round(action[0] / env.dt)
assert length == np.round(env.traj_gen.tau.numpy() / env.dt)
else:
@@ -116,14 +105,14 @@ def test_learn_sub_trajectories(mp_type: str, env_wrap: Tuple[str, Type[RawInter
def test_replanning_time(mp_type: str, env_wrap: Tuple[str, Type[RawInterfaceWrapper]],
add_time_aware_wrapper_before: bool, replanning_time: int):
env_id, wrapper_class = env_wrap
env_step = TimeAwareObservation(ensure_finite_time(make(env_id, SEED), MAX_STEPS_FALLBACK))
env_step = TimeAwareObservation(fancy_gym.make(env_id, SEED))
wrappers = [wrapper_class]
# has time aware wrapper
if add_time_aware_wrapper_before:
wrappers += [TimeAwareObservation]
def replanning_schedule(c_pos, c_vel, obs, c_action, t): return t % replanning_time == 0
replanning_schedule = lambda c_pos, c_vel, obs, c_action, t: t % replanning_time == 0
basis_generator_type = 'prodmp' if mp_type == 'prodmp' else 'rbf'
phase_generator_type = 'exp' if 'dmp' in mp_type else 'linear'
@@ -132,36 +121,31 @@ def test_replanning_time(mp_type: str, env_wrap: Tuple[str, Type[RawInterfaceWra
{'trajectory_generator_type': mp_type},
{'controller_type': 'motor'},
{'phase_generator_type': phase_generator_type},
{'basis_generator_type': basis_generator_type}, fallback_max_steps=MAX_STEPS_FALLBACK)
env.reset(seed=SEED)
{'basis_generator_type': basis_generator_type}, seed=SEED)
assert env.do_replanning
assert env.spec.max_episode_steps
assert env_step.spec.max_episode_steps
assert callable(env.replanning_schedule)
# This also verifies we are not adding the TimeAwareObservationWrapper twice
assert spaces.flatten_space(env_step.observation_space) == spaces.flatten_space(env.observation_space)
assert env.observation_space == env_step.observation_space
env.reset(seed=SEED)
env.reset()
episode_steps = env_step.spec.max_episode_steps // replanning_time
# Make 3 episodes, total steps depend on the replanning steps
for i in range(3 * episode_steps):
action = env.action_space.sample()
_obs, _reward, terminated, truncated, info = env.step(action)
done = terminated or truncated
obs, r, d, info = env.step(action)
length = info['trajectory_length']
if done:
if d:
# Check if number of steps until termination match the replanning interval
print(done, (i + 1), episode_steps)
print(d, (i + 1), episode_steps)
assert (i + 1) % episode_steps == 0
env.reset(seed=SEED)
env.reset()
assert replanning_schedule(None, None, None, None, length)
@pytest.mark.parametrize('mp_type', ['promp', 'prodmp'])
@pytest.mark.parametrize('max_planning_times', [1, 2, 3, 4])
@pytest.mark.parametrize('sub_segment_steps', [5, 10])
@@ -181,19 +165,15 @@ def test_max_planning_times(mp_type: str, max_planning_times: int, sub_segment_s
},
{'basis_generator_type': basis_generator_type,
},
fallback_max_steps=MAX_STEPS_FALLBACK)
_ = env.reset(seed=SEED)
done = False
seed=SEED)
_ = env.reset()
d = False
planning_times = 0
while not done:
action = env.action_space.sample()
_obs, _reward, terminated, truncated, _info = env.step(action)
done = terminated or truncated
while not d:
_, _, d, _ = env.step(env.action_space.sample())
planning_times += 1
assert planning_times == max_planning_times
@pytest.mark.parametrize('mp_type', ['promp', 'prodmp'])
@pytest.mark.parametrize('max_planning_times', [1, 2, 3, 4])
@pytest.mark.parametrize('sub_segment_steps', [5, 10])
@@ -214,20 +194,17 @@ def test_replanning_with_learn_tau(mp_type: str, max_planning_times: int, sub_se
},
{'basis_generator_type': basis_generator_type,
},
fallback_max_steps=MAX_STEPS_FALLBACK)
_ = env.reset(seed=SEED)
done = False
seed=SEED)
_ = env.reset()
d = False
planning_times = 0
while not done:
while not d:
action = env.action_space.sample()
action[0] = tau
_obs, _reward, terminated, truncated, _info = env.step(action)
done = terminated or truncated
_, _, d, info = env.step(action)
planning_times += 1
assert planning_times == max_planning_times
@pytest.mark.parametrize('mp_type', ['promp', 'prodmp'])
@pytest.mark.parametrize('max_planning_times', [1, 2, 3, 4])
@pytest.mark.parametrize('sub_segment_steps', [5, 10])
@@ -236,28 +213,26 @@ def test_replanning_with_learn_delay(mp_type: str, max_planning_times: int, sub_
basis_generator_type = 'prodmp' if mp_type == 'prodmp' else 'rbf'
phase_generator_type = 'exp' if mp_type == 'prodmp' else 'linear'
env = fancy_gym.make_bb('toy-v0', [ToyWrapper],
{'replanning_schedule': lambda pos, vel, obs, action, t: t % sub_segment_steps == 0,
'max_planning_times': max_planning_times,
'verbose': 2},
{'trajectory_generator_type': mp_type,
},
{'controller_type': 'motor'},
{'phase_generator_type': phase_generator_type,
'learn_tau': False,
'learn_delay': True
},
{'basis_generator_type': basis_generator_type,
},
fallback_max_steps=MAX_STEPS_FALLBACK)
_ = env.reset(seed=SEED)
done = False
{'replanning_schedule': lambda pos, vel, obs, action, t: t % sub_segment_steps == 0,
'max_planning_times': max_planning_times,
'verbose': 2},
{'trajectory_generator_type': mp_type,
},
{'controller_type': 'motor'},
{'phase_generator_type': phase_generator_type,
'learn_tau': False,
'learn_delay': True
},
{'basis_generator_type': basis_generator_type,
},
seed=SEED)
_ = env.reset()
d = False
planning_times = 0
while not done:
while not d:
action = env.action_space.sample()
action[0] = delay
_obs, _reward, terminated, truncated, info = env.step(action)
done = terminated or truncated
_, _, d, info = env.step(action)
delay_time_steps = int(np.round(delay / env.dt))
pos = info['positions'].flatten()
@@ -281,7 +256,6 @@ def test_replanning_with_learn_delay(mp_type: str, max_planning_times: int, sub_
assert planning_times == max_planning_times
@pytest.mark.parametrize('mp_type', ['promp', 'prodmp'])
@pytest.mark.parametrize('max_planning_times', [1, 2, 3])
@pytest.mark.parametrize('sub_segment_steps', [5, 10, 15])
@@ -292,29 +266,27 @@ def test_replanning_with_learn_delay_and_tau(mp_type: str, max_planning_times: i
basis_generator_type = 'prodmp' if mp_type == 'prodmp' else 'rbf'
phase_generator_type = 'exp' if mp_type == 'prodmp' else 'linear'
env = fancy_gym.make_bb('toy-v0', [ToyWrapper],
{'replanning_schedule': lambda pos, vel, obs, action, t: t % sub_segment_steps == 0,
'max_planning_times': max_planning_times,
'verbose': 2},
{'trajectory_generator_type': mp_type,
},
{'controller_type': 'motor'},
{'phase_generator_type': phase_generator_type,
'learn_tau': True,
'learn_delay': True
},
{'basis_generator_type': basis_generator_type,
},
fallback_max_steps=MAX_STEPS_FALLBACK)
_ = env.reset(seed=SEED)
done = False
{'replanning_schedule': lambda pos, vel, obs, action, t: t % sub_segment_steps == 0,
'max_planning_times': max_planning_times,
'verbose': 2},
{'trajectory_generator_type': mp_type,
},
{'controller_type': 'motor'},
{'phase_generator_type': phase_generator_type,
'learn_tau': True,
'learn_delay': True
},
{'basis_generator_type': basis_generator_type,
},
seed=SEED)
_ = env.reset()
d = False
planning_times = 0
while not done:
while not d:
action = env.action_space.sample()
action[0] = tau
action[1] = delay
_obs, _reward, terminated, truncated, info = env.step(action)
done = terminated or truncated
_, _, d, info = env.step(action)
delay_time_steps = int(np.round(delay / env.dt))
@@ -334,7 +306,6 @@ def test_replanning_with_learn_delay_and_tau(mp_type: str, max_planning_times: i
assert planning_times == max_planning_times
@pytest.mark.parametrize('mp_type', ['promp', 'prodmp'])
@pytest.mark.parametrize('max_planning_times', [1, 2, 3, 4])
@pytest.mark.parametrize('sub_segment_steps', [5, 10])
@@ -354,11 +325,9 @@ def test_replanning_schedule(mp_type: str, max_planning_times: int, sub_segment_
},
{'basis_generator_type': basis_generator_type,
},
fallback_max_steps=MAX_STEPS_FALLBACK)
_ = env.reset(seed=SEED)
seed=SEED)
_ = env.reset()
d = False
for i in range(max_planning_times):
action = env.action_space.sample()
_obs, _reward, terminated, truncated, _info = env.step(action)
done = terminated or truncated
assert done
_, _, d, _ = env.step(env.action_space.sample())
assert d
+25 -46
View File
@@ -1,12 +1,9 @@
from typing import List, Type
import gymnasium as gym
import gym
import numpy as np
from gymnasium import make
from fancy_gym import make
def run_env(env_id: str, iterations: int = None, seed: int = 0, wrappers: List[Type[gym.Wrapper]] = [],
render: bool = False):
def run_env(env_id, iterations=None, seed=0, render=False):
"""
Example for running a DMC based env in the step based setting.
The env_id has to be specified as `dmc:domain_name-task_name` or
@@ -16,88 +13,70 @@ def run_env(env_id: str, iterations: int = None, seed: int = 0, wrappers: List[T
env_id: Either `dmc:domain_name-task_name` or `dmc:manipulation-environment_name`
iterations: Number of rollout steps to run
seed: random seeding
wrappers: List of Wrappers to apply to the environment
render: Render the episode
Returns: observations, rewards, terminations, truncations, actions
Returns: observations, rewards, dones, actions
"""
env: gym.Env = make(env_id)
for w in wrappers:
env = w(env)
env: gym.Env = make(env_id, seed=seed)
rewards = []
observations = []
actions = []
terminations = []
truncations = []
obs, _ = env.reset(seed=seed)
env.action_space.seed(seed)
dones = []
obs = env.reset()
verify_observations(obs, env.observation_space, "reset()")
iterations = iterations or (env.spec.max_episode_steps or 1)
# number of samples (multiple environment steps)
# number of samples(multiple environment steps)
for i in range(iterations):
observations.append(obs)
ac = env.action_space.sample()
actions.append(ac)
# ac = np.random.uniform(env.action_space.low, env.action_space.high, env.action_space.shape)
obs, reward, terminated, truncated, info = env.step(ac)
obs, reward, done, info = env.step(ac)
verify_observations(obs, env.observation_space, "step()")
verify_reward(reward)
verify_done(terminated)
verify_done(truncated)
verify_done(done)
rewards.append(reward)
terminations.append(terminated)
truncations.append(truncated)
dones.append(done)
if render:
env.render("human")
if terminated or truncated:
if done:
break
if not hasattr(env, "replanning_schedule"):
assert terminated or truncated, f"Termination or truncation flag is not True after {i + 1} iterations."
assert done, "Done flag is not True after end of episode."
observations.append(obs)
env.close()
del env
return np.array(observations), np.array(rewards), np.array(terminations), np.array(truncations), np.array(actions)
return np.array(observations), np.array(rewards), np.array(dones), np.array(actions)
def run_env_determinism(env_id: str, seed: int, iterations: int = None, wrappers: List[Type[gym.Wrapper]] = []):
traj1 = run_env(env_id, iterations=iterations,
seed=seed, wrappers=wrappers)
traj2 = run_env(env_id, iterations=iterations,
seed=seed, wrappers=wrappers)
def run_env_determinism(env_id: str, seed: int):
traj1 = run_env(env_id, seed=seed)
traj2 = run_env(env_id, seed=seed)
# Iterate over two trajectories, which should have the same state and action sequence
for i, time_step in enumerate(zip(*traj1, *traj2)):
obs1, rwd1, term1, trunc1, ac1, obs2, rwd2, term2, trunc2, ac2 = time_step
assert np.allclose(
obs1, obs2), f"Observations [{i}] {obs1} ({obs1.shape}) and {obs2} ({obs2.shape}) do not match: Biggest difference is {np.abs(obs1-obs2).max()} at index {np.abs(obs1-obs2).argmax()}."
assert np.array_equal(
ac1, ac2), f"Actions [{i}] {ac1} and {ac2} do not match."
assert np.array_equal(
rwd1, rwd2), f"Rewards [{i}] {rwd1} and {rwd2} do not match."
assert np.array_equal(
term1, term2), f"Terminateds [{i}] {term1} and {term2} do not match."
assert np.array_equal(
term1, term2), f"Truncateds [{i}] {trunc1} and {trunc2} do not match."
obs1, rwd1, done1, ac1, obs2, rwd2, done2, ac2 = time_step
assert np.array_equal(obs1, obs2), f"Observations [{i}] {obs1} and {obs2} do not match."
assert np.array_equal(ac1, ac2), f"Actions [{i}] {ac1} and {ac2} do not match."
assert np.array_equal(rwd1, rwd2), f"Rewards [{i}] {rwd1} and {rwd2} do not match."
assert np.array_equal(done1, done2), f"Dones [{i}] {done1} and {done2} do not match."
def verify_observations(obs, observation_space: gym.Space, obs_type="reset()"):
assert observation_space.contains(obs), \
f"Observation {obs} ({obs.shape}) received from {obs_type} not contained in observation space {observation_space}."
f"Observation {obs} received from {obs_type} not contained in observation space {observation_space}."
def verify_reward(reward):
assert isinstance(
reward, (float, int)), f"Returned type {type(reward)} as reward, expected float or int."
assert isinstance(reward, (float, int)), f"Returned type {type(reward)} as reward, expected float or int."
def verify_done(done):
assert isinstance(
done, bool), f"Returned {done} as done flag, expected bool."
assert isinstance(done, bool), f"Returned {done} as done flag, expected bool."