AI/WalkTheseWays
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

initial commit

Browse Source
This commit is contained in:
gmargo
2022-11-21 09:42:09 -05:00
commit c7060d13d3
115 changed files with 307582 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
Executable
+70
View File
@@ -0,0 +1,70 @@
runs
# Jaynes secret files
.secret*
# ml_logger cache files
.cache
# These are some examples of commonly ignored file patterns.
# You should customize this list as applicable to your project.
# Learn more about .gitignore:
# https://www.atlassian.com/git/tutorials/saving-changes/gitignore
# Node artifact files
node_modules/
dist/
# Compiled Java class files
*.class
# Compiled Python bytecode
*.py[cod]
# Log files
*.log
# Package files
*.jar
# Maven
target/
dist/
# JetBrains IDE
.idea/
# Unit test reports
TEST*.xml
# Generated by MacOS
.DS_Store
# Generated by Windows
Thumbs.db
# Applications
*.app
*.exe
*.war
# Large media files
*.mp4
*.tiff
*.avi
*.flv
*.mov
*.wmv
# VS Code
.vscode
# logs
logs
runs
# other
*.egg-info
__pycache__
docker/rsc
*.swp
*.jit
tmp
*.tar
LICENSE
+25
View File
@@ -0,0 +1,25 @@
MIT License
Copyright (c) 2022 MIT Improbable AI Lab
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.
See licenses/legged_gym and licenses/rsl_rl for additional license information for some files in this package.
Files associated with these additional licenses indicate so in the header.
LICENSES/legged_gym/LICENSE
Executable
+31
View File
@@ -0,0 +1,31 @@
Copyright (c) 2021, ETH Zurich, Nikita Rudin
Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its contributors
may be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
See licenses/assets for license information for assets included in this repository.
See licenses/dependencies for license information of dependencies of this package.
LICENSES/rsl_rl/LICENSE
+30
View File
@@ -0,0 +1,30 @@
Copyright (c) 2021, ETH Zurich, Nikita Rudin
Copyright (c) 2021, NVIDIA CORPORATION & AFFILIATES
All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its contributors
may be used to endorse or promote products derived from this software without
specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
See licenses/dependencies for license information of dependencies of this package.
README.md
Executable
+232
View File
@@ -0,0 +1,232 @@
# Go1 Sim-to-Real Locomotion Starter Kit
# Table of contents
1. [Overview](#overview)
2. [System Requirements](#requirements)
3. [Training a Model](#simulation)
1. [Installation](#installation)
2. [Environment and Model Configuration](#configuration)
3. [Training and Logging](#training)
4. [Analyzing the Policy](#analysis)
4. [Deploying a Model](#realworld)
1. [Installing the Deployment Utility](#robotconfig)
2. [Running the Controller](#runcontroller)
3. [RC Configuration](#rcconfig)
2. [Deploying a Custom Model](#configuration)
4. [Deployment and Logging](#deployment)
5. [Analyzing Real-world Performance](#plotting)
5. [Acknowledgements](#acknowledgements)
## Overview <a name="introduction"></a>
This repository provides an implementation of the paper:
<td style="padding:20px;width:75%;vertical-align:middle">
<a href="">
<b> Walk these Ways: Learning with Parametric Auxiliary Rewards for an Uncertain Future </b>
</a>
<br>
<a href="https://gmargo11.github.io/" target="_blank">Gabriel B. Margolis</a> and <a href="https://people.csail.mit.edu/pulkitag" target="_blank">Pulkit Agrawal</a>
<br>
<em>Conference on Robot Learning</em>, 2022
<br>
<a href="">paper</a> /
<a href="">bibtex</a> /
<a href="https://sites.google.com/view/gait-conditioned-rl/" target="_blank">project page</a>
<br>
</td>
<br>
This environment builds on the [legged gym environment](https://leggedrobotics.github.io/legged_gym/) by Nikita
Rudin, Robotic Systems Lab, ETH Zurich (Paper: https://arxiv.org/abs/2109.11978) and the Isaac Gym simulator from
NVIDIA (Paper: https://arxiv.org/abs/2108.10470). Training code builds on the
[rsl_rl](https://github.com/leggedrobotics/rsl_rl) repository, also by Nikita
Rudin, Robotic Systems Lab, ETH Zurich. All redistributed code retains its
original [license](LICENSES/legged_gym/LICENSE).
Our initial release provides the following features:
* Train reinforcement learning policies for the Go1 robot using PPO, IsaacGym, Domain Randomization and Parametric Auxiliary Rewards.
* Evaluate a pretrained parametric auxiliary reward policy in simulation.
* Deploy learned policies on the Go1 using the `unitree_legged_sdk`.
## System Requirements <a name="requirements"></a>
**Simulated Training and Evaluation**: Isaac Gym requires an NVIDIA GPU. To train in the default configuration, we recommend a GPU with at least 10GB of VRAM. The code can run on a smaller GPU if you decrease the number of parallel environments (`Cfg.env.num_envs`). However, training will be slower with fewer environments.
**Hardware Deployment**: We provide deployment code for the Unitree Go1 Edu robot. This relatively low-cost, commercially available quadruped can be purchased here: https://shop.unitree.com/. You will need the Edu version of the robot to run and customize your locomotion controller.
<b>Users are advised to follow Unitree's recommendations for safety while using the Go1 in low-level control mode. This means hanging up the robot and keeping it away from people and obstacles. Use our code at your own risk; we do not take responsibility for any damage.</b>
## Training a Model <a name="simulation"></a>
### Installation <a name="installation"></a>
#### Install pytorch 1.10 with cuda-11.3:
```bash
pip3 install torch==1.10.0+cu113 torchvision==0.11.1+cu113 torchaudio==0.10.0+cu113 -f https://download.pytorch.org/whl/cu113/torch_stable.html
```
#### Install Isaac Gym
1. Download and install Isaac Gym Preview 4 from https://developer.nvidia.com/isaac-gym
2. unzip the file via:
```bash
tar -xf IsaacGym_Preview_4_Package.tar.gz
```
3. now install the python package
```bash
cd isaacgym/python && pip install -e .
```
4. Verify the installation by try running an example
```bash
python examples/1080_balls_of_solitude.py
```
5. For troubleshooting check docs `isaacgym/docs/index.html`
#### Install the `go1_gym` package
In this repository, run `pip install -e .`
### Verifying the Installation
If everything is installed correctly, you should be able to run the test script with:
```bash
python scripts/test.py
```
The script should print `Simulating step {i}`.
The GUI is off by default. To turn it on, set `headless=False` in `test.py`'s main function call.
### Environment and Model Configuration <a name="configuration"></a>
**CODE STRUCTURE** The main environment for simulating a legged robot is
in [legged_robot.py](go1_gym/envs/base/legged_robot.py). The default configuration parameters including reward
weightings are defined in [legged_robot_config.py::Cfg](go1_gym/envs/base/legged_robot_config.py).
There are three scripts in the [scripts](scripts/) directory:
```bash
scripts
├── __init__.py
├── play.py
├── test.py
└── train.py
```
You can run the `test.py` script to verify your environment setup. If it runs then you have installed the gym
environments correctly. To train an agent, run `train.py`. To evaluate a pretrained agent, run `play.py`. We provie a
pretrained agent checkpoint in the [./runs/pretrain-v0](runs/pretrain-v0) directory.
### Training and Logging <a name="training"></a>
To train the Go1 controller from [Walk these Ways](https://sites.google.com/view/gait-conditioned-rl/), run:
```bash
python scripts/train.py
```
After initializing the simulator, the script will print out a list of metrics every ten training iterations.
Training with the default configuration requires about 12GB of GPU memory. If you have less memory available, you can
still train by reducing the number of parallel environments used in simulation (the default is `Cfg.env.num_envs = 4000`).
To visualize training progress, first start the ml_dash frontend app:
```bash
python -m ml_dash.app
```
then start the ml_dash backend server by running this command in the parent directory of the `runs` folder:
```bash
python -m ml_dash.server .
```
Finally, use a web browser to go to the app IP (defaults to `localhost:3001`)
and create a new profile with the credentials:
Username: `runs`
API: [server IP] (defaults to `localhost:8081`)
Access Token: [blank]
Now, clicking on the profile should yield a
### Analyzing the Policy <a name="analysis"></a>
To evaluate the most recently trained model, run:
```bash
python scripts/play.py
```
The robot is commanded to run forward at 3m/s for 5 seconds. After completing the simulation,
the script plots the robot's velocity and joint angles.
The GUI is on by default.
If it does not appear, and you're working in docker, make sure you haven't forgotten to run `bash docker/visualize_access.bash`.
## Deploying a Model <a name="realworld"></a>
### Installing the Deployment Utility <a name="robotconfig"></a>
The first step is to connect your development machine to the robot using ethernet. You should ping the robot to verify the connection: `ping 192.168.123.15` should return `x packets transmitted, x received, 0% packet loss`.
Once you have confirmed the robot is connected, run the following command on your computer to transfer files to the robot. The first time you run it, the script will download and transfer the zipped docker image for development on the robot (`deployment_image.tar`). This file is quite large (3.5GB), but it only needs to be downloaded and transferred once.
```
cd go1_gym_deploy/scripts && ./send_to_unitree.sh
```
Next, you will log onto the robot's onboard computer and install the docker environment. To enter the onboard computer, the command is:
```
ssh unitree@192.168.123.15
```
Now, run the following commands on the robot's onboard computer:
```
cd ~/go1_gym/go1_gym_deploy/installer
./install_deployment_code.sh
```
The installer will automatically unzip and install the docker image containing the deployment environment.
### Running the Controller <a name="runcontroller"></a>
Place the robot into damping mode. The control sequence is: [L1+B], [L1+A], [L1+L2+START]. After this, the robot should sit on the ground and the joints should move freely.
Now, ssh to `unitree@192.168.123.15` and run the following two commands to start the controller. <b>This will operate the robot in low-level control mode. Make sure your Go1 is hung up.</b>
First:
```
cd ~/go1_gym/go1_gym_deploy/autostart
./start_unitree_sdk.sh
```
Second:
```
cd ~/go1_gym/go1_gym_deploy/docker
sudo make autostart
```
The robot will wait for you to press [R2], then calibrate, then wait for a second press of [R2] before running the control loop.
### The RC Mapping <a name="rcconfig"></a>
![RC Mapping](media/rc_map.png?raw=true)
The RC mapping is depicted above.
### Deploying a Custom Model <a name="configuration"></a>
<i>Coming soon</i>
### Logging and Debugging <a name="deployment"></a>
<i>Coming soon</i>
### Analyzing Real-world Performance <a name="plotting"></a>
<i>Coming soon</i>
go1_gym/__init__.py
Executable
+6
View File
@@ -0,0 +1,6 @@
import os
MINI_GYM_ROOT_DIR = os.path.dirname(os.path.dirname(os.path.realpath(__file__)))
MINI_GYM_ENVS_DIR = os.path.join(MINI_GYM_ROOT_DIR, 'go1_gym', 'envs')
go1_gym/envs/__init__.py
Executable
View File
go1_gym/envs/base/__init__.py
View File
go1_gym/envs/base/base_task.py
Executable
+137
View File
@@ -0,0 +1,137 @@
# License: see [LICENSE, LICENSES/legged_gym/LICENSE]
import sys
import gym
import torch
from isaacgym import gymapi, gymutil
from gym import spaces
import numpy as np
# Base class for RL tasks
class BaseTask(gym.Env):
def __init__(self, cfg, sim_params, physics_engine, sim_device, headless, eval_cfg=None):
self.gym = gymapi.acquire_gym()
if isinstance(physics_engine, str) and physics_engine == "SIM_PHYSX":
physics_engine = gymapi.SIM_PHYSX
self.sim_params = sim_params
self.physics_engine = physics_engine
self.sim_device = sim_device
sim_device_type, self.sim_device_id = gymutil.parse_device_str(self.sim_device)
self.headless = headless
# env device is GPU only if sim is on GPU and use_gpu_pipeline=True, otherwise returned tensors are copied to CPU by physX.
if sim_device_type == 'cuda' and sim_params.use_gpu_pipeline:
self.device = self.sim_device
else:
self.device = 'cpu'
# graphics device for rendering, -1 for no rendering
self.graphics_device_id = self.sim_device_id
if self.headless == True:
self.graphics_device_id = self.sim_device_id
self.num_obs = cfg.env.num_observations
self.num_privileged_obs = cfg.env.num_privileged_obs
self.num_actions = cfg.env.num_actions
if eval_cfg is not None:
self.num_eval_envs = eval_cfg.env.num_envs
self.num_train_envs = cfg.env.num_envs
self.num_envs = self.num_eval_envs + self.num_train_envs
else:
self.num_eval_envs = 0
self.num_train_envs = cfg.env.num_envs
self.num_envs = cfg.env.num_envs
# optimization flags for pytorch JIT
torch._C._jit_set_profiling_mode(False)
torch._C._jit_set_profiling_executor(False)
# allocate buffers
self.obs_buf = torch.zeros(self.num_envs, self.num_obs, device=self.device, dtype=torch.float)
self.rew_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.float)
self.rew_buf_pos = torch.zeros(self.num_envs, device=self.device, dtype=torch.float)
self.rew_buf_neg = torch.zeros(self.num_envs, device=self.device, dtype=torch.float)
self.reset_buf = torch.ones(self.num_envs, device=self.device, dtype=torch.long)
self.episode_length_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.long)
self.time_out_buf = torch.zeros(self.num_envs, device=self.device, dtype=torch.bool)
self.privileged_obs_buf = torch.zeros(self.num_envs, self.num_privileged_obs, device=self.device,
dtype=torch.float)
# self.num_privileged_obs = self.num_obs
self.extras = {}
# create envs, sim and viewer
self.create_sim()
self.gym.prepare_sim(self.sim)
# todo: read from config
self.enable_viewer_sync = True
self.viewer = None
# if running with a viewer, set up keyboard shortcuts and camera
if self.headless == False:
# subscribe to keyboard shortcuts
self.viewer = self.gym.create_viewer(
self.sim, gymapi.CameraProperties())
self.gym.subscribe_viewer_keyboard_event(
self.viewer, gymapi.KEY_ESCAPE, "QUIT")
self.gym.subscribe_viewer_keyboard_event(
self.viewer, gymapi.KEY_V, "toggle_viewer_sync")
def get_observations(self):
return self.obs_buf
def get_privileged_observations(self):
return self.privileged_obs_buf
def reset_idx(self, env_ids):
"""Reset selected robots"""
raise NotImplementedError
def reset(self):
""" Reset all robots"""
self.reset_idx(torch.arange(self.num_envs, device=self.device))
obs, privileged_obs, _, _, _ = self.step(
torch.zeros(self.num_envs, self.num_actions, device=self.device, requires_grad=False))
return obs, privileged_obs
def step(self, actions):
raise NotImplementedError
def render_gui(self, sync_frame_time=True):
if self.viewer:
# check for window closed
if self.gym.query_viewer_has_closed(self.viewer):
sys.exit()
# check for keyboard events
for evt in self.gym.query_viewer_action_events(self.viewer):
if evt.action == "QUIT" and evt.value > 0:
sys.exit()
elif evt.action == "toggle_viewer_sync" and evt.value > 0:
self.enable_viewer_sync = not self.enable_viewer_sync
# fetch results
if self.device != 'cpu':
self.gym.fetch_results(self.sim, True)
# step graphics
if self.enable_viewer_sync:
self.gym.step_graphics(self.sim)
self.gym.draw_viewer(self.viewer, self.sim, True)
if sync_frame_time:
self.gym.sync_frame_time(self.sim)
else:
self.gym.poll_viewer_events(self.viewer)
def close(self):
if self.headless == False:
self.gym.destroy_viewer(self.viewer)
self.gym.destroy_sim(self.sim)
go1_gym/envs/base/curriculum.py
+181
View File
@@ -0,0 +1,181 @@
import numpy as np
import torch
from matplotlib import pyplot as plt
def is_met(scale, l2_err, threshold):
return (l2_err / scale) < threshold
def key_is_met(metric_cache, config, ep_len, target_key, env_id, threshold):
# metric_cache[target_key][env_id] / ep_len
scale = 1
l2_err = 0
return is_met(scale, l2_err, threshold)
class Curriculum:
def set_to(self, low, high, value=1.0):
inds = np.logical_and(
self.grid >= low[:, None],
self.grid <= high[:, None]
).all(axis=0)
assert len(inds) != 0, "You are intializing your distribution with an empty domain!"
self.weights[inds] = value
def __init__(self, seed, **key_ranges):
self.rng = np.random.RandomState(seed)
self.cfg = cfg = {}
self.indices = indices = {}
for key, v_range in key_ranges.items():
bin_size = (v_range[1] - v_range[0]) / v_range[2]
cfg[key] = np.linspace(v_range[0] + bin_size / 2, v_range[1] - bin_size / 2, v_range[2])
indices[key] = np.linspace(0, v_range[2]-1, v_range[2])
self.lows = np.array([range[0] for range in key_ranges.values()])
self.highs = np.array([range[1] for range in key_ranges.values()])
# self.bin_sizes = {key: arr[1] - arr[0] for key, arr in cfg.items()}
self.bin_sizes = {key: (v_range[1] - v_range[0]) / v_range[2] for key, v_range in key_ranges.items()}
self._raw_grid = np.stack(np.meshgrid(*cfg.values(), indexing='ij'))
self._idx_grid = np.stack(np.meshgrid(*indices.values(), indexing='ij'))
self.keys = [*key_ranges.keys()]
self.grid = self._raw_grid.reshape([len(self.keys), -1])
self.idx_grid = self._idx_grid.reshape([len(self.keys), -1])
# self.grid = np.stack([params.flatten() for params in raw_grid])
self._l = l = len(self.grid[0])
self.ls = {key: len(self.cfg[key]) for key in self.cfg.keys()}
self.weights = np.zeros(l)
self.indices = np.arange(l)
def __len__(self):
return self._l
def __getitem__(self, *keys):
pass
def update(self, **kwargs):
# bump the envelop if
pass
def sample_bins(self, batch_size, low=None, high=None):
"""default to uniform"""
if low is not None and high is not None: # if bounds given
valid_inds = np.logical_and(
self.grid >= low[:, None],
self.grid <= high[:, None]
).all(axis=0)
temp_weights = np.zeros_like(self.weights)
temp_weights[valid_inds] = self.weights[valid_inds]
inds = self.rng.choice(self.indices, batch_size, p=temp_weights / temp_weights.sum())
else: # if no bounds given
inds = self.rng.choice(self.indices, batch_size, p=self.weights / self.weights.sum())
return self.grid.T[inds], inds
def sample_uniform_from_cell(self, centroids):
bin_sizes = np.array([*self.bin_sizes.values()])
low, high = centroids + bin_sizes / 2, centroids - bin_sizes / 2
return self.rng.uniform(low, high)#.clip(self.lows, self.highs)
def sample(self, batch_size, low=None, high=None):
cgf_centroid, inds = self.sample_bins(batch_size, low=low, high=high)
return np.stack([self.sample_uniform_from_cell(v_range) for v_range in cgf_centroid]), inds
class SumCurriculum(Curriculum):
def __init__(self, seed, **kwargs):
super().__init__(seed, **kwargs)
self.success = np.zeros(len(self))
self.trials = np.zeros(len(self))
def update(self, bin_inds, l1_error, threshold):
is_success = l1_error < threshold
self.success[bin_inds[is_success]] += 1
self.trials[bin_inds] += 1
def success_rates(self, *keys):
s_rate = self.success / (self.trials + 1e-6)
s_rate = s_rate.reshape(list(self.ls.values()))
marginals = tuple(i for i, key in enumerate(self.keys) if key not in keys)
if marginals:
return s_rate.mean(axis=marginals)
return s_rate
class RewardThresholdCurriculum(Curriculum):
def __init__(self, seed, **kwargs):
super().__init__(seed, **kwargs)
self.episode_reward_lin = np.zeros(len(self))
self.episode_reward_ang = np.zeros(len(self))
self.episode_lin_vel_raw = np.zeros(len(self))
self.episode_ang_vel_raw = np.zeros(len(self))
self.episode_duration = np.zeros(len(self))
def get_local_bins(self, bin_inds, ranges=0.1):
if isinstance(ranges, float):
ranges = np.ones(self.grid.shape[0]) * ranges
bin_inds = bin_inds.reshape(-1)
adjacent_inds = np.logical_and(
self.grid[:, None, :].repeat(bin_inds.shape[0], axis=1) >= self.grid[:, bin_inds, None] - ranges.reshape(-1, 1, 1),
self.grid[:, None, :].repeat(bin_inds.shape[0], axis=1) <= self.grid[:, bin_inds, None] + ranges.reshape(-1, 1, 1)
).all(axis=0)
return adjacent_inds
def update(self, bin_inds, task_rewards, success_thresholds, local_range=0.5):
is_success = 1.
for task_reward, success_threshold in zip(task_rewards, success_thresholds):
is_success = is_success * (task_reward > success_threshold).cpu()
if len(success_thresholds) == 0:
is_success = np.array([False] * len(bin_inds))
else:
is_success = np.array(is_success.bool())
# if len(is_success) > 0 and is_success.any():
# print("successes")
self.weights[bin_inds[is_success]] = np.clip(self.weights[bin_inds[is_success]] + 0.2, 0, 1)
adjacents = self.get_local_bins(bin_inds[is_success], ranges=local_range)
for adjacent in adjacents:
#print(adjacent)
#print(self.grid[:, adjacent])
adjacent_inds = np.array(adjacent.nonzero()[0])
self.weights[adjacent_inds] = np.clip(self.weights[adjacent_inds] + 0.2, 0, 1)
def log(self, bin_inds, lin_vel_raw=None, ang_vel_raw=None, episode_duration=None):
self.episode_lin_vel_raw[bin_inds] = lin_vel_raw.cpu().numpy()
self.episode_ang_vel_raw[bin_inds] = ang_vel_raw.cpu().numpy()
self.episode_duration[bin_inds] = episode_duration.cpu().numpy()
if __name__ == '__main__':
r = RewardThresholdCurriculum(100, x=(-1, 1, 5), y=(-1, 1, 2), z=(-1, 1, 11))
assert r._raw_grid.shape == (3, 5, 2, 11), "grid shape is wrong: {}".format(r.grid.shape)
low, high = np.array([-1.0, -0.6, -1.0]), np.array([1.0, 0.6, 1.0])
# r.set_to(low, high, value=1.0)
adjacents = r.get_local_bins(np.array([10, ]), range=0.5)
for adjacent in adjacents:
adjacent_inds = np.array(adjacent.nonzero()[0])
print(adjacent_inds)
r.update(bin_inds=adjacent_inds, lin_vel_rewards=np.ones_like(adjacent_inds),
ang_vel_rewards=np.ones_like(adjacent_inds), lin_vel_threshold=0.0, ang_vel_threshold=0.0,
local_range=0.5)
samples, bins = r.sample(10_000)
plt.scatter(*samples.T[:2])
plt.show()
go1_gym/envs/base/legged_robot.py
Executable
+1806
View File
File diff suppressed because it is too large Load Diff
go1_gym/envs/base/legged_robot_config.py
Executable
+421
View File
@@ -0,0 +1,421 @@
# License: see [LICENSE, LICENSES/legged_gym/LICENSE]
from params_proto import PrefixProto, ParamsProto
class Cfg(PrefixProto, cli=False):
class env(PrefixProto, cli=False):
num_envs = 4096
num_observations = 235
num_scalar_observations = 42
# if not None a privilige_obs_buf will be returned by step() (critic obs for assymetric training). None is returned otherwise
num_privileged_obs = 18
privileged_future_horizon = 1
num_actions = 12
num_observation_history = 15
env_spacing = 3. # not used with heightfields/trimeshes
send_timeouts = True # send time out information to the algorithm
episode_length_s = 20 # episode length in seconds
observe_vel = True
observe_only_ang_vel = False
observe_only_lin_vel = False
observe_yaw = False
observe_contact_states = False
observe_command = True
observe_height_command = False
observe_gait_commands = False
observe_timing_parameter = False
observe_clock_inputs = False
observe_two_prev_actions = False
observe_imu = False
record_video = True
recording_width_px = 360
recording_height_px = 240
recording_mode = "COLOR"
num_recording_envs = 1
debug_viz = False
all_agents_share = False
priv_observe_friction = True
priv_observe_friction_indep = True
priv_observe_ground_friction = False
priv_observe_ground_friction_per_foot = False
priv_observe_restitution = True
priv_observe_base_mass = True
priv_observe_com_displacement = True
priv_observe_motor_strength = False
priv_observe_motor_offset = False
priv_observe_joint_friction = True
priv_observe_Kp_factor = True
priv_observe_Kd_factor = True
priv_observe_contact_forces = False
priv_observe_contact_states = False
priv_observe_body_velocity = False
priv_observe_foot_height = False
priv_observe_body_height = False
priv_observe_gravity = False
priv_observe_terrain_type = False
priv_observe_clock_inputs = False
priv_observe_doubletime_clock_inputs = False
priv_observe_halftime_clock_inputs = False
priv_observe_desired_contact_states = False
priv_observe_dummy_variable = False
class terrain(PrefixProto, cli=False):
mesh_type = 'trimesh' # "heightfield" # none, plane, heightfield or trimesh
horizontal_scale = 0.1 # [m]
vertical_scale = 0.005 # [m]
border_size = 0 # 25 # [m]
curriculum = True
static_friction = 1.0
dynamic_friction = 1.0
restitution = 0.0
terrain_noise_magnitude = 0.1
# rough terrain only:
terrain_smoothness = 0.005
measure_heights = True
# 1mx1.6m rectangle (without center line)
measured_points_x = [-0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0., 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8]
measured_points_y = [-0.5, -0.4, -0.3, -0.2, -0.1, 0., 0.1, 0.2, 0.3, 0.4, 0.5]
selected = False # select a unique terrain type and pass all arguments
terrain_kwargs = None # Dict of arguments for selected terrain
min_init_terrain_level = 0
max_init_terrain_level = 5 # starting curriculum state
terrain_length = 8.
terrain_width = 8.
num_rows = 10 # number of terrain rows (levels)
num_cols = 20 # number of terrain cols (types)
# terrain types: [smooth slope, rough slope, stairs up, stairs down, discrete]
terrain_proportions = [0.1, 0.1, 0.35, 0.25, 0.2]
# trimesh only:
slope_treshold = 0.75 # slopes above this threshold will be corrected to vertical surfaces
difficulty_scale = 1.
x_init_range = 1.
y_init_range = 1.
yaw_init_range = 0.
x_init_offset = 0.
y_init_offset = 0.
teleport_robots = True
teleport_thresh = 2.0
max_platform_height = 0.2
center_robots = False
center_span = 5
class commands(PrefixProto, cli=False):
command_curriculum = False
max_reverse_curriculum = 1.
max_forward_curriculum = 1.
yaw_command_curriculum = False
max_yaw_curriculum = 1.
exclusive_command_sampling = False
num_commands = 3
resampling_time = 10. # time before command are changed[s]
subsample_gait = False
gait_interval_s = 10. # time between resampling gait params
vel_interval_s = 10.
jump_interval_s = 20. # time between jumps
jump_duration_s = 0.1 # duration of jump
jump_height = 0.3
heading_command = True # if true: compute ang vel command from heading error
global_reference = False
observe_accel = False
distributional_commands = False
curriculum_type = "RewardThresholdCurriculum"
lipschitz_threshold = 0.9
num_lin_vel_bins = 20
lin_vel_step = 0.3
num_ang_vel_bins = 20
ang_vel_step = 0.3
distribution_update_extension_distance = 1
curriculum_seed = 100
lin_vel_x = [-1.0, 1.0] # min max [m/s]
lin_vel_y = [-1.0, 1.0] # min max [m/s]
ang_vel_yaw = [-1, 1] # min max [rad/s]
body_height_cmd = [-0.05, 0.05]
impulse_height_commands = False
limit_vel_x = [-10.0, 10.0]
limit_vel_y = [-0.6, 0.6]
limit_vel_yaw = [-10.0, 10.0]
limit_body_height = [-0.05, 0.05]
limit_gait_phase = [0, 0.01]
limit_gait_offset = [0, 0.01]
limit_gait_bound = [0, 0.01]
limit_gait_frequency = [2.0, 2.01]
limit_gait_duration = [0.49, 0.5]
limit_footswing_height = [0.06, 0.061]
limit_body_pitch = [0.0, 0.01]
limit_body_roll = [0.0, 0.01]
limit_aux_reward_coef = [0.0, 0.01]
limit_compliance = [0.0, 0.01]
limit_stance_width = [0.0, 0.01]
limit_stance_length = [0.0, 0.01]
num_bins_vel_x = 25
num_bins_vel_y = 3
num_bins_vel_yaw = 25
num_bins_body_height = 1
num_bins_gait_frequency = 11
num_bins_gait_phase = 11
num_bins_gait_offset = 2
num_bins_gait_bound = 2
num_bins_gait_duration = 3
num_bins_footswing_height = 1
num_bins_body_pitch = 1
num_bins_body_roll = 1
num_bins_aux_reward_coef = 1
num_bins_compliance = 1
num_bins_compliance = 1
num_bins_stance_width = 1
num_bins_stance_length = 1
heading = [-3.14, 3.14]
gait_phase_cmd_range = [0.0, 0.01]
gait_offset_cmd_range = [0.0, 0.01]
gait_bound_cmd_range = [0.0, 0.01]
gait_frequency_cmd_range = [2.0, 2.01]
gait_duration_cmd_range = [0.49, 0.5]
footswing_height_range = [0.06, 0.061]
body_pitch_range = [0.0, 0.01]
body_roll_range = [0.0, 0.01]
aux_reward_coef_range = [0.0, 0.01]
compliance_range = [0.0, 0.01]
stance_width_range = [0.0, 0.01]
stance_length_range = [0.0, 0.01]
exclusive_phase_offset = True
binary_phases = False
pacing_offset = False
balance_gait_distribution = True
gaitwise_curricula = True
class curriculum_thresholds(PrefixProto, cli=False):
tracking_lin_vel = 0.8 # closer to 1 is tighter
tracking_ang_vel = 0.5
tracking_contacts_shaped_force = 0.8 # closer to 1 is tighter
tracking_contacts_shaped_vel = 0.8
class init_state(PrefixProto, cli=False):
pos = [0.0, 0.0, 1.] # x,y,z [m]
rot = [0.0, 0.0, 0.0, 1.0] # x,y,z,w [quat]
lin_vel = [0.0, 0.0, 0.0] # x,y,z [m/s]
ang_vel = [0.0, 0.0, 0.0] # x,y,z [rad/s]
# target angles when action = 0.0
default_joint_angles = {"joint_a": 0., "joint_b": 0.}
class control(PrefixProto, cli=False):
control_type = 'actuator_net' #'P' # P: position, V: velocity, T: torques
# PD Drive parameters:
stiffness = {'joint_a': 10.0, 'joint_b': 15.} # [N*m/rad]
damping = {'joint_a': 1.0, 'joint_b': 1.5} # [N*m*s/rad]
# action scale: target angle = actionScale * action + defaultAngle
action_scale = 0.5
hip_scale_reduction = 1.0
# decimation: Number of control action updates @ sim DT per policy DT
decimation = 4
class asset(PrefixProto, cli=False):
file = ""
foot_name = "None" # name of the feet bodies, used to index body state and contact force tensors
penalize_contacts_on = []
terminate_after_contacts_on = []
disable_gravity = False
# merge bodies connected by fixed joints. Specific fixed joints can be kept by adding " <... dont_collapse="true">
collapse_fixed_joints = True
fix_base_link = False # fixe the base of the robot
default_dof_drive_mode = 3 # see GymDofDriveModeFlags (0 is none, 1 is pos tgt, 2 is vel tgt, 3 effort)
self_collisions = 0 # 1 to disable, 0 to enable...bitwise filter
# replace collision cylinders with capsules, leads to faster/more stable simulation
replace_cylinder_with_capsule = True
flip_visual_attachments = True # Some .obj meshes must be flipped from y-up to z-up
density = 0.001
angular_damping = 0.
linear_damping = 0.
max_angular_velocity = 1000.
max_linear_velocity = 1000.
armature = 0.
thickness = 0.01
class domain_rand(PrefixProto, cli=False):
rand_interval_s = 10
randomize_rigids_after_start = True
randomize_friction = True
friction_range = [0.5, 1.25] # increase range
randomize_restitution = False
restitution_range = [0, 1.0]
randomize_base_mass = False
# add link masses, increase range, randomize inertia, randomize joint properties
added_mass_range = [-1., 1.]
randomize_com_displacement = False
# add link masses, increase range, randomize inertia, randomize joint properties
com_displacement_range = [-0.15, 0.15]
randomize_motor_strength = False
motor_strength_range = [0.9, 1.1]
randomize_Kp_factor = False
Kp_factor_range = [0.8, 1.3]
randomize_Kd_factor = False
Kd_factor_range = [0.5, 1.5]
gravity_rand_interval_s = 7
gravity_impulse_duration = 1.0
randomize_gravity = False
gravity_range = [-1.0, 1.0]
push_robots = True
push_interval_s = 15
max_push_vel_xy = 1.
randomize_lag_timesteps = True
lag_timesteps = 6
class rewards(PrefixProto, cli=False):
only_positive_rewards = True # if true negative total rewards are clipped at zero (avoids early termination problems)
only_positive_rewards_ji22_style = False
sigma_rew_neg = 5
reward_container_name = "CoRLRewards"
tracking_sigma = 0.25 # tracking reward = exp(-error^2/sigma)
tracking_sigma_lat = 0.25 # tracking reward = exp(-error^2/sigma)
tracking_sigma_long = 0.25 # tracking reward = exp(-error^2/sigma)
tracking_sigma_yaw = 0.25 # tracking reward = exp(-error^2/sigma)
soft_dof_pos_limit = 1. # percentage of urdf limits, values above this limit are penalized
soft_dof_vel_limit = 1.
soft_torque_limit = 1.
base_height_target = 1.
max_contact_force = 100. # forces above this value are penalized
use_terminal_body_height = False
terminal_body_height = 0.20
use_terminal_foot_height = False
terminal_foot_height = -0.005
use_terminal_roll_pitch = False
terminal_body_ori = 0.5
kappa_gait_probs = 0.07
gait_force_sigma = 50.
gait_vel_sigma = 0.5
footswing_height = 0.09
class reward_scales(ParamsProto, cli=False):
termination = -0.0
tracking_lin_vel = 1.0
tracking_ang_vel = 0.5
lin_vel_z = -2.0
ang_vel_xy = -0.05
orientation = -0.
torques = -0.00001
dof_vel = -0.
dof_acc = -2.5e-7
base_height = -0.
feet_air_time = 1.0
collision = -1.
feet_stumble = -0.0
action_rate = -0.01
stand_still = -0.
tracking_lin_vel_lat = 0.
tracking_lin_vel_long = 0.
tracking_contacts = 0.
tracking_contacts_shaped = 0.
tracking_contacts_shaped_force = 0.
tracking_contacts_shaped_vel = 0.
jump = 0.0
energy = 0.0
energy_expenditure = 0.0
survival = 0.0
dof_pos_limits = 0.0
feet_contact_forces = 0.
feet_slip = 0.
feet_clearance_cmd_linear = 0.
dof_pos = 0.
action_smoothness_1 = 0.
action_smoothness_2 = 0.
base_motion = 0.
feet_impact_vel = 0.0
raibert_heuristic = 0.0
class normalization(PrefixProto, cli=False):
clip_observations = 100.
clip_actions = 100.
friction_range = [0.05, 4.5]
ground_friction_range = [0.05, 4.5]
restitution_range = [0, 1.0]
added_mass_range = [-1., 3.]
com_displacement_range = [-0.1, 0.1]
motor_strength_range = [0.9, 1.1]
motor_offset_range = [-0.05, 0.05]
Kp_factor_range = [0.8, 1.3]
Kd_factor_range = [0.5, 1.5]
joint_friction_range = [0.0, 0.7]
contact_force_range = [0.0, 50.0]
contact_state_range = [0.0, 1.0]
body_velocity_range = [-6.0, 6.0]
foot_height_range = [0.0, 0.15]
body_height_range = [0.0, 0.60]
gravity_range = [-1.0, 1.0]
motion = [-0.01, 0.01]
class obs_scales(PrefixProto, cli=False):
lin_vel = 2.0
ang_vel = 0.25
dof_pos = 1.0
dof_vel = 0.05
imu = 0.1
height_measurements = 5.0
friction_measurements = 1.0
body_height_cmd = 2.0
gait_phase_cmd = 1.0
gait_freq_cmd = 1.0
footswing_height_cmd = 0.15
body_pitch_cmd = 0.3
body_roll_cmd = 0.3
aux_reward_cmd = 1.0
compliance_cmd = 1.0
stance_width_cmd = 1.0
stance_length_cmd = 1.0
segmentation_image = 1.0
rgb_image = 1.0
depth_image = 1.0
class noise(PrefixProto, cli=False):
add_noise = True
noise_level = 1.0 # scales other values
class noise_scales(PrefixProto, cli=False):
dof_pos = 0.01
dof_vel = 1.5
lin_vel = 0.1
ang_vel = 0.2
imu = 0.1
gravity = 0.05
contact_states = 0.05
height_measurements = 0.1
friction_measurements = 0.0
segmentation_image = 0.0
rgb_image = 0.0
depth_image = 0.0
# viewer camera:
class viewer(PrefixProto, cli=False):
ref_env = 0
pos = [10, 0, 6] # [m]
lookat = [11., 5, 3.] # [m]
class sim(PrefixProto, cli=False):
dt = 0.005
substeps = 1
gravity = [0., 0., -9.81] # [m/s^2]
up_axis = 1 # 0 is y, 1 is z
use_gpu_pipeline = True
class physx(PrefixProto, cli=False):
num_threads = 10
solver_type = 1 # 0: pgs, 1: tgs
num_position_iterations = 4
num_velocity_iterations = 0
contact_offset = 0.01 # [m]
rest_offset = 0.0 # [m]
bounce_threshold_velocity = 0.5 # 0.5 [m/s]
max_depenetration_velocity = 1.0
max_gpu_contact_pairs = 2 ** 23 # 2**24 -> needed for 8000 envs and more
default_buffer_size_multiplier = 5
contact_collection = 2 # 0: never, 1: last sub-step, 2: all sub-steps (default=2)
go1_gym/envs/go1/__init__.py
View File
go1_gym/envs/go1/go1_config.py
Executable
+106
View File
@@ -0,0 +1,106 @@
from typing import Union
from params_proto import Meta
from go1_gym.envs.base.legged_robot_config import Cfg
def config_go1(Cnfg: Union[Cfg, Meta]):
_ = Cnfg.init_state
_.pos = [0.0, 0.0, 0.34] # x,y,z [m]
_.default_joint_angles = { # = target angles [rad] when action = 0.0
'FL_hip_joint': 0.1, # [rad]
'RL_hip_joint': 0.1, # [rad]
'FR_hip_joint': -0.1, # [rad]
'RR_hip_joint': -0.1, # [rad]
'FL_thigh_joint': 0.8, # [rad]
'RL_thigh_joint': 1., # [rad]
'FR_thigh_joint': 0.8, # [rad]
'RR_thigh_joint': 1., # [rad]
'FL_calf_joint': -1.5, # [rad]
'RL_calf_joint': -1.5, # [rad]
'FR_calf_joint': -1.5, # [rad]
'RR_calf_joint': -1.5 # [rad]
}
_ = Cnfg.control
_.control_type = 'P'
_.stiffness = {'joint': 20.} # [N*m/rad]
_.damping = {'joint': 0.5} # [N*m*s/rad]
# action scale: target angle = actionScale * action + defaultAngle
_.action_scale = 0.25
_.hip_scale_reduction = 0.5
# decimation: Number of control action updates @ sim DT per policy DT
_.decimation = 4
_ = Cnfg.asset
_.file = '{MINI_GYM_ROOT_DIR}/resources/robots/go1/urdf/go1.urdf'
_.foot_name = "foot"
_.penalize_contacts_on = ["thigh", "calf"]
_.terminate_after_contacts_on = ["base"]
_.self_collisions = 0 # 1 to disable, 0 to enable...bitwise filter
_.flip_visual_attachments = False
_.fix_base_link = False
_ = Cnfg.rewards
_.soft_dof_pos_limit = 0.9
_.base_height_target = 0.34
_ = Cnfg.reward_scales
_.torques = -0.0001
_.action_rate = -0.01
_.dof_pos_limits = -10.0
_.orientation = -5.
_.base_height = -30.
_ = Cnfg.terrain
_.mesh_type = 'trimesh'
_.measure_heights = False
_.terrain_noise_magnitude = 0.0
_.teleport_robots = True
_.border_size = 50
_.terrain_proportions = [0, 0, 0, 0, 0, 0, 0, 0, 1.0]
_.curriculum = False
_ = Cnfg.env
_.num_observations = 42
_.observe_vel = False
_.num_envs = 4000
_ = Cnfg.commands
_.lin_vel_x = [-1.0, 1.0]
_.lin_vel_y = [-1.0, 1.0]
_ = Cnfg.commands
_.heading_command = False
_.resampling_time = 10.0
_.command_curriculum = True
_.num_lin_vel_bins = 30
_.num_ang_vel_bins = 30
_.lin_vel_x = [-0.6, 0.6]
_.lin_vel_y = [-0.6, 0.6]
_.ang_vel_yaw = [-1, 1]
_ = Cnfg.domain_rand
_.randomize_base_mass = True
_.added_mass_range = [-1, 3]
_.push_robots = False
_.max_push_vel_xy = 0.5
_.randomize_friction = True
_.friction_range = [0.05, 4.5]
_.randomize_restitution = True
_.restitution_range = [0.0, 1.0]
_.restitution = 0.5 # default terrain restitution
_.randomize_com_displacement = True
_.com_displacement_range = [-0.1, 0.1]
_.randomize_motor_strength = True
_.motor_strength_range = [0.9, 1.1]
_.randomize_Kp_factor = False
_.Kp_factor_range = [0.8, 1.3]
_.randomize_Kd_factor = False
_.Kd_factor_range = [0.5, 1.5]
_.rand_interval_s = 6
go1_gym/envs/go1/velocity_tracking/__init__.py
+50
View File
@@ -0,0 +1,50 @@
from isaacgym import gymutil, gymapi
import torch
from params_proto import Meta
from typing import Union
from go1_gym.envs.base.legged_robot import LeggedRobot
from go1_gym.envs.base.legged_robot_config import Cfg
class VelocityTrackingEasyEnv(LeggedRobot):
def __init__(self, sim_device, headless, num_envs=None, prone=False, deploy=False,
cfg: Cfg = None, eval_cfg: Cfg = None, initial_dynamics_dict=None, physics_engine="SIM_PHYSX"):
if num_envs is not None:
cfg.env.num_envs = num_envs
sim_params = gymapi.SimParams()
gymutil.parse_sim_config(vars(cfg.sim), sim_params)
super().__init__(cfg, sim_params, physics_engine, sim_device, headless, eval_cfg, initial_dynamics_dict)
def step(self, actions):
self.obs_buf, self.privileged_obs_buf, self.rew_buf, self.reset_buf, self.extras = super().step(actions)
self.foot_positions = self.rigid_body_state.view(self.num_envs, self.num_bodies, 13)[:, self.feet_indices,
0:3]
self.extras.update({
"privileged_obs": self.privileged_obs_buf,
"joint_pos": self.dof_pos.cpu().numpy(),
"joint_vel": self.dof_vel.cpu().numpy(),
"joint_pos_target": self.joint_pos_target.cpu().detach().numpy(),
"joint_vel_target": torch.zeros(12),
"body_linear_vel": self.base_lin_vel.cpu().detach().numpy(),
"body_angular_vel": self.base_ang_vel.cpu().detach().numpy(),
"body_linear_vel_cmd": self.commands.cpu().numpy()[:, 0:2],
"body_angular_vel_cmd": self.commands.cpu().numpy()[:, 2:],
"contact_states": (self.contact_forces[:, self.feet_indices, 2] > 1.).detach().cpu().numpy().copy(),
"foot_positions": (self.foot_positions).detach().cpu().numpy().copy(),
"body_pos": self.root_states[:, 0:3].detach().cpu().numpy(),
"torques": self.torques.detach().cpu().numpy()
})
return self.obs_buf, self.rew_buf, self.reset_buf, self.extras
def reset(self):
self.reset_idx(torch.arange(self.num_envs, device=self.device))
obs, _, _, _ = self.step(torch.zeros(self.num_envs, self.num_actions, device=self.device, requires_grad=False))
return obs
go1_gym/envs/rewards/corl_rewards.py
+202
View File
@@ -0,0 +1,202 @@
import torch
import numpy as np
from go1_gym.utils.math_utils import quat_apply_yaw, wrap_to_pi, get_scale_shift
from isaacgym.torch_utils import *
from isaacgym import gymapi
class CoRLRewards:
def __init__(self, env):
self.env = env
def load_env(self, env):
self.env = env
# ------------ reward functions----------------
def _reward_tracking_lin_vel(self):
# Tracking of linear velocity commands (xy axes)
lin_vel_error = torch.sum(torch.square(self.env.commands[:, :2] - self.env.base_lin_vel[:, :2]), dim=1)
return torch.exp(-lin_vel_error / self.env.cfg.rewards.tracking_sigma)
def _reward_tracking_ang_vel(self):
# Tracking of angular velocity commands (yaw)
ang_vel_error = torch.square(self.env.commands[:, 2] - self.env.base_ang_vel[:, 2])
return torch.exp(-ang_vel_error / self.env.cfg.rewards.tracking_sigma_yaw)
def _reward_lin_vel_z(self):
# Penalize z axis base linear velocity
return torch.square(self.env.base_lin_vel[:, 2])
def _reward_ang_vel_xy(self):
# Penalize xy axes base angular velocity
return torch.sum(torch.square(self.env.base_ang_vel[:, :2]), dim=1)
def _reward_orientation(self):
# Penalize non flat base orientation
return torch.sum(torch.square(self.env.projected_gravity[:, :2]), dim=1)
def _reward_torques(self):
# Penalize torques
return torch.sum(torch.square(self.env.torques), dim=1)
def _reward_dof_acc(self):
# Penalize dof accelerations
return torch.sum(torch.square((self.env.last_dof_vel - self.env.dof_vel) / self.env.dt), dim=1)
def _reward_action_rate(self):
# Penalize changes in actions
return torch.sum(torch.square(self.env.last_actions - self.env.actions), dim=1)
def _reward_collision(self):
# Penalize collisions on selected bodies
return torch.sum(1. * (torch.norm(self.env.contact_forces[:, self.env.penalised_contact_indices, :], dim=-1) > 0.1),
dim=1)
def _reward_dof_pos_limits(self):
# Penalize dof positions too close to the limit
out_of_limits = -(self.env.dof_pos - self.env.dof_pos_limits[:, 0]).clip(max=0.) # lower limit
out_of_limits += (self.env.dof_pos - self.env.dof_pos_limits[:, 1]).clip(min=0.)
return torch.sum(out_of_limits, dim=1)
def _reward_jump(self):
reference_heights = 0
body_height = self.env.base_pos[:, 2] - reference_heights
jump_height_target = self.env.commands[:, 3] + self.env.cfg.rewards.base_height_target
reward = - torch.square(body_height - jump_height_target)
return reward
def _reward_tracking_contacts_shaped_force(self):
foot_forces = torch.norm(self.env.contact_forces[:, self.env.feet_indices, :], dim=-1)
desired_contact = self.env.desired_contact_states
reward = 0
for i in range(4):
reward += - (1 - desired_contact[:, i]) * (
1 - torch.exp(-1 * foot_forces[:, i] ** 2 / self.env.cfg.rewards.gait_force_sigma))
return reward / 4
def _reward_tracking_contacts_shaped_vel(self):
foot_velocities = torch.norm(self.env.foot_velocities, dim=2).view(self.env.num_envs, -1)
desired_contact = self.env.desired_contact_states
reward = 0
for i in range(4):
reward += - (desired_contact[:, i] * (
1 - torch.exp(-1 * foot_velocities[:, i] ** 2 / self.env.cfg.rewards.gait_vel_sigma)))
return reward / 4
def _reward_dof_pos(self):
# Penalize dof positions
return torch.sum(torch.square(self.env.dof_pos - self.env.default_dof_pos), dim=1)
def _reward_dof_vel(self):
# Penalize dof velocities
return torch.sum(torch.square(self.env.dof_vel), dim=1)
def _reward_action_smoothness_1(self):
# Penalize changes in actions
diff = torch.square(self.env.joint_pos_target[:, :self.env.num_actuated_dof] - self.env.last_joint_pos_target[:, :self.env.num_actuated_dof])
diff = diff * (self.env.last_actions[:, :self.env.num_dof] != 0) # ignore first step
return torch.sum(diff, dim=1)
def _reward_action_smoothness_2(self):
# Penalize changes in actions
diff = torch.square(self.env.joint_pos_target[:, :self.env.num_actuated_dof] - 2 * self.env.last_joint_pos_target[:, :self.env.num_actuated_dof] + self.env.last_last_joint_pos_target[:, :self.env.num_actuated_dof])
diff = diff * (self.env.last_actions[:, :self.env.num_dof] != 0) # ignore first step
diff = diff * (self.env.last_last_actions[:, :self.env.num_dof] != 0) # ignore second step
return torch.sum(diff, dim=1)
def _reward_feet_slip(self):
contact = self.env.contact_forces[:, self.env.feet_indices, 2] > 1.
contact_filt = torch.logical_or(contact, self.env.last_contacts)
self.env.last_contacts = contact
foot_velocities = torch.square(torch.norm(self.env.foot_velocities[:, :, 0:2], dim=2).view(self.env.num_envs, -1))
rew_slip = torch.sum(contact_filt * foot_velocities, dim=1)
return rew_slip
def _reward_feet_contact_vel(self):
reference_heights = 0
near_ground = self.env.foot_positions[:, :, 2] - reference_heights < 0.03
foot_velocities = torch.square(torch.norm(self.env.foot_velocities[:, :, 0:3], dim=2).view(self.env.num_envs, -1))
rew_contact_vel = torch.sum(near_ground * foot_velocities, dim=1)
return rew_contact_vel
def _reward_feet_contact_forces(self):
# penalize high contact forces
return torch.sum((torch.norm(self.env.contact_forces[:, self.env.feet_indices, :],
dim=-1) - self.env.cfg.rewards.max_contact_force).clip(min=0.), dim=1)
def _reward_feet_clearance_cmd_linear(self):
phases = 1 - torch.abs(1.0 - torch.clip((self.env.foot_indices * 2.0) - 1.0, 0.0, 1.0) * 2.0)
foot_height = (self.env.foot_positions[:, :, 2]).view(self.env.num_envs, -1)# - reference_heights
target_height = self.env.commands[:, 9].unsqueeze(1) * phases + 0.02 # offset for foot radius 2cm
rew_foot_clearance = torch.square(target_height - foot_height) * (1 - self.env.desired_contact_states)
return torch.sum(rew_foot_clearance, dim=1)
def _reward_feet_impact_vel(self):
prev_foot_velocities = self.env.prev_foot_velocities[:, :, 2].view(self.env.num_envs, -1)
contact_states = torch.norm(self.env.contact_forces[:, self.env.feet_indices, :], dim=-1) > 1.0
rew_foot_impact_vel = contact_states * torch.square(torch.clip(prev_foot_velocities, -100, 0))
return torch.sum(rew_foot_impact_vel, dim=1)
def _reward_collision(self):
# Penalize collisions on selected bodies
return torch.sum(1. * (torch.norm(self.env.contact_forces[:, self.env.penalised_contact_indices, :], dim=-1) > 0.1),
dim=1)
def _reward_orientation_control(self):
# Penalize non flat base orientation
roll_pitch_commands = self.env.commands[:, 10:12]
quat_roll = quat_from_angle_axis(-roll_pitch_commands[:, 1],
torch.tensor([1, 0, 0], device=self.env.device, dtype=torch.float))
quat_pitch = quat_from_angle_axis(-roll_pitch_commands[:, 0],
torch.tensor([0, 1, 0], device=self.env.device, dtype=torch.float))
desired_base_quat = quat_mul(quat_roll, quat_pitch)
desired_projected_gravity = quat_rotate_inverse(desired_base_quat, self.env.gravity_vec)
return torch.sum(torch.square(self.env.projected_gravity[:, :2] - desired_projected_gravity[:, :2]), dim=1)
def _reward_raibert_heuristic(self):
cur_footsteps_translated = self.env.foot_positions - self.env.base_pos.unsqueeze(1)
footsteps_in_body_frame = torch.zeros(self.env.num_envs, 4, 3, device=self.env.device)
for i in range(4):
footsteps_in_body_frame[:, i, :] = quat_apply_yaw(quat_conjugate(self.env.base_quat),
cur_footsteps_translated[:, i, :])
# nominal positions: [FR, FL, RR, RL]
if self.env.cfg.commands.num_commands >= 13:
desired_stance_width = self.env.commands[:, 12:13]
desired_ys_nom = torch.cat([desired_stance_width / 2, -desired_stance_width / 2, desired_stance_width / 2, -desired_stance_width / 2], dim=1)
else:
desired_stance_width = 0.3
desired_ys_nom = torch.tensor([desired_stance_width / 2, -desired_stance_width / 2, desired_stance_width / 2, -desired_stance_width / 2], device=self.env.device).unsqueeze(0)
if self.env.cfg.commands.num_commands >= 14:
desired_stance_length = self.env.commands[:, 13:14]
desired_xs_nom = torch.cat([desired_stance_length / 2, desired_stance_length / 2, -desired_stance_length / 2, -desired_stance_length / 2], dim=1)
else:
desired_stance_length = 0.45
desired_xs_nom = torch.tensor([desired_stance_length / 2, desired_stance_length / 2, -desired_stance_length / 2, -desired_stance_length / 2], device=self.env.device).unsqueeze(0)
# raibert offsets
phases = torch.abs(1.0 - (self.env.foot_indices * 2.0)) * 1.0 - 0.5
frequencies = self.env.commands[:, 4]
x_vel_des = self.env.commands[:, 0:1]
yaw_vel_des = self.env.commands[:, 2:3]
y_vel_des = yaw_vel_des * desired_stance_length / 2
desired_ys_offset = phases * y_vel_des * (0.5 / frequencies.unsqueeze(1))
desired_ys_offset[:, 2:4] *= -1
desired_xs_offset = phases * x_vel_des * (0.5 / frequencies.unsqueeze(1))
desired_ys_nom = desired_ys_nom + desired_ys_offset
desired_xs_nom = desired_xs_nom + desired_xs_offset
desired_footsteps_body_frame = torch.cat((desired_xs_nom.unsqueeze(2), desired_ys_nom.unsqueeze(2)), dim=2)
err_raibert_heuristic = torch.abs(desired_footsteps_body_frame - footsteps_in_body_frame[:, :, 0:2])
reward = torch.sum(torch.square(err_raibert_heuristic), dim=(1, 2))
return reward
go1_gym/envs/wrappers/history_wrapper.py
Executable
+72
View File
@@ -0,0 +1,72 @@
import isaacgym
assert isaacgym
import torch
import gym
class HistoryWrapper(gym.Wrapper):
def __init__(self, env):
super().__init__(env)
self.env = env
self.obs_history_length = self.env.cfg.env.num_observation_history
self.num_obs_history = self.obs_history_length * self.num_obs
self.obs_history = torch.zeros(self.env.num_envs, self.num_obs_history, dtype=torch.float,
device=self.env.device, requires_grad=False)
self.num_privileged_obs = self.num_privileged_obs
def step(self, action):
# privileged information and observation history are stored in info
obs, rew, done, info = self.env.step(action)
privileged_obs = info["privileged_obs"]
self.obs_history = torch.cat((self.obs_history[:, self.env.num_obs:], obs), dim=-1)
return {'obs': obs, 'privileged_obs': privileged_obs, 'obs_history': self.obs_history}, rew, done, info
def get_observations(self):
obs = self.env.get_observations()
privileged_obs = self.env.get_privileged_observations()
self.obs_history = torch.cat((self.obs_history[:, self.env.num_obs:], obs), dim=-1)
return {'obs': obs, 'privileged_obs': privileged_obs, 'obs_history': self.obs_history}
def reset_idx(self, env_ids): # it might be a problem that this isn't getting called!!
ret = super().reset_idx(env_ids)
self.obs_history[env_ids, :] = 0
return ret
def reset(self):
ret = super().reset()
privileged_obs = self.env.get_privileged_observations()
self.obs_history[:, :] = 0
return {"obs": ret, "privileged_obs": privileged_obs, "obs_history": self.obs_history}
if __name__ == "__main__":
from tqdm import trange
import matplotlib.pyplot as plt
import ml_logger as logger
from go1_gym_learn.ppo import Runner
from go1_gym.envs.wrappers.history_wrapper import HistoryWrapper
from go1_gym_learn.ppo.actor_critic import AC_Args
from go1_gym.envs.base.legged_robot_config import Cfg
from go1_gym.envs.mini_cheetah.mini_cheetah_config import config_mini_cheetah
config_mini_cheetah(Cfg)
test_env = gym.make("VelocityTrackingEasyEnv-v0", cfg=Cfg)
env = HistoryWrapper(test_env)
env.reset()
action = torch.zeros(test_env.num_envs, 12)
for i in trange(3):
obs, rew, done, info = env.step(action)
print(obs.keys())
print(f"obs: {obs['obs']}")
print(f"privileged obs: {obs['privileged_obs']}")
print(f"obs_history: {obs['obs_history']}")
img = env.render('rgb_array')
plt.imshow(img)
plt.show()
go1_gym/utils/__init__.py
Executable
+2
View File
@@ -0,0 +1,2 @@
from .math_utils import *
from .terrain import Terrain
go1_gym/utils/math_utils.py
Executable
+38
View File
@@ -0,0 +1,38 @@
# License: see [LICENSE, LICENSES/legged_gym/LICENSE]
from typing import Tuple
import numpy as np
import torch
from isaacgym.torch_utils import quat_apply, normalize
from torch import Tensor
# @ torch.jit.script
def quat_apply_yaw(quat, vec):
quat_yaw = quat.clone().view(-1, 4)
quat_yaw[:, :2] = 0.
quat_yaw = normalize(quat_yaw)
return quat_apply(quat_yaw, vec)
# @ torch.jit.script
def wrap_to_pi(angles):
angles %= 2 * np.pi
angles -= 2 * np.pi * (angles > np.pi)
return angles
# @ torch.jit.script
def torch_rand_sqrt_float(lower, upper, shape, device):
# type: (float, float, Tuple[int, int], str) -> Tensor
r = 2 * torch.rand(*shape, device=device) - 1
r = torch.where(r < 0., -torch.sqrt(-r), torch.sqrt(r))
r = (r + 1.) / 2.
return (upper - lower) * r + lower
def get_scale_shift(range):
scale = 2. / (range[1] - range[0])
shift = (range[1] + range[0]) / 2.
return scale, shift
go1_gym/utils/terrain.py
Executable
+180
View File
@@ -0,0 +1,180 @@
# License: see [LICENSE, LICENSES/legged_gym/LICENSE]
import math
import numpy as np
from isaacgym import terrain_utils
from numpy.random import choice
from go1_gym.envs.base.legged_robot_config import Cfg
class Terrain:
def __init__(self, cfg: Cfg.terrain, num_robots, eval_cfg=None, num_eval_robots=0) -> None:
self.cfg = cfg
self.eval_cfg = eval_cfg
self.num_robots = num_robots
self.type = cfg.mesh_type
if self.type in ["none", 'plane']:
return
self.train_rows, self.train_cols, self.eval_rows, self.eval_cols = self.load_cfgs()
self.tot_rows = len(self.train_rows) + len(self.eval_rows)
self.tot_cols = max(len(self.train_cols), len(self.eval_cols))
self.cfg.env_length = cfg.terrain_length
self.cfg.env_width = cfg.terrain_width
self.height_field_raw = np.zeros((self.tot_rows, self.tot_cols), dtype=np.int16)
self.initialize_terrains()
self.heightsamples = self.height_field_raw
if self.type == "trimesh":
self.vertices, self.triangles = terrain_utils.convert_heightfield_to_trimesh(self.height_field_raw,
self.cfg.horizontal_scale,
self.cfg.vertical_scale,
self.cfg.slope_treshold)
def load_cfgs(self):
self._load_cfg(self.cfg)
self.cfg.row_indices = np.arange(0, self.cfg.tot_rows)
self.cfg.col_indices = np.arange(0, self.cfg.tot_cols)
self.cfg.x_offset = 0
self.cfg.rows_offset = 0
if self.eval_cfg is None:
return self.cfg.row_indices, self.cfg.col_indices, [], []
else:
self._load_cfg(self.eval_cfg)
self.eval_cfg.row_indices = np.arange(self.cfg.tot_rows, self.cfg.tot_rows + self.eval_cfg.tot_rows)
self.eval_cfg.col_indices = np.arange(0, self.eval_cfg.tot_cols)
self.eval_cfg.x_offset = self.cfg.tot_rows
self.eval_cfg.rows_offset = self.cfg.num_rows
return self.cfg.row_indices, self.cfg.col_indices, self.eval_cfg.row_indices, self.eval_cfg.col_indices
def _load_cfg(self, cfg):
cfg.proportions = [np.sum(cfg.terrain_proportions[:i + 1]) for i in range(len(cfg.terrain_proportions))]
cfg.num_sub_terrains = cfg.num_rows * cfg.num_cols
cfg.env_origins = np.zeros((cfg.num_rows, cfg.num_cols, 3))
cfg.width_per_env_pixels = int(cfg.terrain_length / cfg.horizontal_scale)
cfg.length_per_env_pixels = int(cfg.terrain_width / cfg.horizontal_scale)
cfg.border = int(cfg.border_size / cfg.horizontal_scale)
cfg.tot_cols = int(cfg.num_cols * cfg.width_per_env_pixels) + 2 * cfg.border
cfg.tot_rows = int(cfg.num_rows * cfg.length_per_env_pixels) + 2 * cfg.border
def initialize_terrains(self):
self._initialize_terrain(self.cfg)
if self.eval_cfg is not None:
self._initialize_terrain(self.eval_cfg)
def _initialize_terrain(self, cfg):
if cfg.curriculum:
self.curriculum(cfg)
elif cfg.selected:
self.selected_terrain(cfg)
else:
self.randomized_terrain(cfg)
def randomized_terrain(self, cfg):
for k in range(cfg.num_sub_terrains):
# Env coordinates in the world
(i, j) = np.unravel_index(k, (cfg.num_rows, cfg.num_cols))
choice = np.random.uniform(0, 1)
difficulty = np.random.choice([0.5, 0.75, 0.9])
terrain = self.make_terrain(cfg, choice, difficulty, cfg.proportions)
self.add_terrain_to_map(cfg, terrain, i, j)
def curriculum(self, cfg):
for j in range(cfg.num_cols):
for i in range(cfg.num_rows):
difficulty = i / cfg.num_rows * cfg.difficulty_scale
choice = j / cfg.num_cols + 0.001
terrain = self.make_terrain(cfg, choice, difficulty, cfg.proportions)
self.add_terrain_to_map(cfg, terrain, i, j)
def selected_terrain(self, cfg):
terrain_type = cfg.terrain_kwargs.pop('type')
for k in range(cfg.num_sub_terrains):
# Env coordinates in the world
(i, j) = np.unravel_index(k, (cfg.num_rows, cfg.num_cols))
terrain = terrain_utils.SubTerrain("terrain",
width=cfg.width_per_env_pixels,
length=cfg.width_per_env_pixels,
vertical_scale=cfg.vertical_scale,
horizontal_scale=cfg.horizontal_scale)
eval(terrain_type)(terrain, **cfg.terrain_kwargs.terrain_kwargs)
self.add_terrain_to_map(cfg, terrain, i, j)
def make_terrain(self, cfg, choice, difficulty, proportions):
terrain = terrain_utils.SubTerrain("terrain",
width=cfg.width_per_env_pixels,
length=cfg.width_per_env_pixels,
vertical_scale=cfg.vertical_scale,
horizontal_scale=cfg.horizontal_scale)
slope = difficulty * 0.4
step_height = 0.05 + 0.18 * difficulty
discrete_obstacles_height = 0.05 + difficulty * (cfg.max_platform_height - 0.05)
stepping_stones_size = 1.5 * (1.05 - difficulty)
stone_distance = 0.05 if difficulty == 0 else 0.1
if choice < proportions[0]:
if choice < proportions[0] / 2:
slope *= -1
terrain_utils.pyramid_sloped_terrain(terrain, slope=slope, platform_size=3.)
elif choice < proportions[1]:
terrain_utils.pyramid_sloped_terrain(terrain, slope=slope, platform_size=3.)
terrain_utils.random_uniform_terrain(terrain, min_height=-0.05, max_height=0.05,
step=self.cfg.terrain_smoothness, downsampled_scale=0.2)
elif choice < proportions[3]:
if choice < proportions[2]:
step_height *= -1
terrain_utils.pyramid_stairs_terrain(terrain, step_width=0.31, step_height=step_height, platform_size=3.)
elif choice < proportions[4]:
num_rectangles = 20
rectangle_min_size = 1.
rectangle_max_size = 2.
terrain_utils.discrete_obstacles_terrain(terrain, discrete_obstacles_height, rectangle_min_size,
rectangle_max_size, num_rectangles, platform_size=3.)
elif choice < proportions[5]:
terrain_utils.stepping_stones_terrain(terrain, stone_size=stepping_stones_size,
stone_distance=stone_distance, max_height=0., platform_size=4.)
elif choice < proportions[6]:
pass
elif choice < proportions[7]:
pass
elif choice < proportions[8]:
terrain_utils.random_uniform_terrain(terrain, min_height=-cfg.terrain_noise_magnitude,
max_height=cfg.terrain_noise_magnitude, step=0.005,
downsampled_scale=0.2)
elif choice < proportions[9]:
terrain_utils.random_uniform_terrain(terrain, min_height=-0.05, max_height=0.05,
step=self.cfg.terrain_smoothness, downsampled_scale=0.2)
terrain.height_field_raw[0:terrain.length // 2, :] = 0
return terrain
def add_terrain_to_map(self, cfg, terrain, row, col):
i = row
j = col
# map coordinate system
start_x = cfg.border + i * cfg.length_per_env_pixels + cfg.x_offset
end_x = cfg.border + (i + 1) * cfg.length_per_env_pixels + cfg.x_offset
start_y = cfg.border + j * cfg.width_per_env_pixels
end_y = cfg.border + (j + 1) * cfg.width_per_env_pixels
self.height_field_raw[start_x: end_x, start_y:end_y] = terrain.height_field_raw
env_origin_x = (i + 0.5) * cfg.terrain_length + cfg.x_offset * terrain.horizontal_scale
env_origin_y = (j + 0.5) * cfg.terrain_width
x1 = int((cfg.terrain_length / 2. - 1) / terrain.horizontal_scale) + cfg.x_offset
x2 = int((cfg.terrain_length / 2. + 1) / terrain.horizontal_scale) + cfg.x_offset
y1 = int((cfg.terrain_width / 2. - 1) / terrain.horizontal_scale)
y2 = int((cfg.terrain_width / 2. + 1) / terrain.horizontal_scale)
env_origin_z = np.max(self.height_field_raw[start_x: end_x, start_y:end_y]) * terrain.vertical_scale
cfg.env_origins[i, j] = [env_origin_x, env_origin_y, env_origin_z]
go1_gym_deploy/__init__.py
View File
go1_gym_deploy/autostart/start_controller.sh
+5
View File
@@ -0,0 +1,5 @@
#!/bin/bash
sudo docker stop foxy_controller || true
sudo docker rm foxy_controller || true
cd ~/go1_gym/go1_gym_deploy/docker/
sudo make autostart
go1_gym_deploy/autostart/start_unitree_sdk.sh
+6
View File
@@ -0,0 +1,6 @@
#!/bin/bash
sudo docker stop foxy_controller || true
sudo docker rm foxy_controller || true
sudo kill $(ps aux |grep lcm_position | awk '{print $2}')
cd ~/go1_gym/go1_gym_deploy/unitree_legged_sdk_bin/
yes "" | sudo ./lcm_position &
go1_gym_deploy/docker/Dockerfile
+190
View File
@@ -0,0 +1,190 @@
# syntax=docker/dockerfile:experimental
FROM nvcr.io/nvidia/l4t-pytorch:r32.6.1-pth1.9-py3
#ENV NVIDIA_VISIBLE_DEVICES ${NVIDIA_VISIBLE_DEVICES:-all}
#ENV NVIDIA_DRIVER_CAPABILITIES ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics
# add new sudo user
ENV USERNAME improbable
ENV HOME /home/$USERNAME
RUN useradd -m $USERNAME && \
echo "$USERNAME:$USERNAME" | chpasswd && \
usermod --shell /bin/bash $USERNAME && \
usermod -aG sudo $USERNAME && \
mkdir /etc/sudoers.d && \
echo "$USERNAME ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers.d/$USERNAME && \
chmod 0440 /etc/sudoers.d/$USERNAME && \
# Replace 1000 with your user/group id
usermod --uid 1000 $USERNAME && \
groupmod --gid 1000 $USERNAME
# install package
ENV DEBIAN_FRONTEND=noninteractive
RUN apt-get update && apt-get install -y --no-install-recommends \
build-essential \
curl \
sudo \
less \
emacs \
apt-utils \
tzdata \
git \
tmux \
bash-completion \
command-not-found \
libglib2.0-0 \
gstreamer1.0-plugins-* \
libgstreamer1.0-* \
libgstreamer-plugins-*1.0-* \
&& \
apt-get clean && \
rm -rf /var/lib/apt/lists/*
RUN echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections
#COPY config/nvidia_icd.json /usr/share/vulkan/icd.d/
USER root
#RUN apt-get update && apt-get install -y python3-pip && pip3 install torch==1.9.0+cu111 torchvision==0.10.0+cu111 torchaudio==0.9.0 -f https://download.pytorch.org/whl/torch_stable.html
# ==================================================================
# Useful Libraries for Development
# ------------------------------------------------------------------
#RUN apt update && apt install -y apt-transport-https ca-certificates curl software-properties-common
#RUN curl -fsSL https://download.sublimetext.com/sublimehq-pub.gpg | apt-key add - && add-apt-repository "deb https://download.sublimetext.com/ apt/stable/" && apt update && apt install sublime-text
# ==================================================================
# Python dependencies defined in requirements.txt
# ------------------------------------------------------------------
#RUN pip3 install --upgrade pip
# copy local requirements file for pip install python deps
#COPY ./requirements.txt /home/$USERNAME
#WORKDIR /home/$USERNAME
#RUN pip3 install -r requirements.txt
# LCM
RUN apt-get -y update && apt-get install -y make gcc-8 g++-8
RUN cd /home/$USERNAME && git clone https://github.com/lcm-proj/lcm.git && cd lcm && mkdir build && cd build && cmake .. && make -j && make install
RUN cd /home/$USERNAME/lcm/lcm-python && pip3 install -e .
RUN apt-get install -y vim
#RUN pip3 install pandas
# ROS
ENV ROS_DISTRO melodic
RUN apt-get install -y gnupg
COPY install_scripts/install_ros.sh /tmp/install_ros.sh
RUN chmod +x /tmp/install_ros.sh
RUN /tmp/install_ros.sh
# bootstrap rosdep
RUN rosdep init \
&& rosdep update
# create catkin workspace
ENV CATKIN_WS=/root/catkin_ws
RUN bash /opt/ros/melodic/setup.bash
RUN mkdir -p $CATKIN_WS/src
WORKDIR ${CATKIN_WS}
RUN catkin init
RUN catkin config --extend /opt/ros/$ROS_DISTRO \
--cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False
WORKDIR $CATKIN_WS/src
RUN apt-get update && apt-get install -y freeglut3-dev libudev-dev
#COPY ./install_scripts/install_vision_opencv.sh /tmp/install_vision_opencv.sh
#RUN chmod +x /tmp/install_vision_opencv.sh
#RUN /tmp/install_vision_opencv.sh
RUN apt-get install -y libgl1-mesa-dev libudev1 libudev-dev
#RUN apt-get install unzip
#
#RUN cd ~ && \
# wget -O opencv.zip https://github.com/opencv/opencv/archive/4.5.1.zip && \
# wget -O opencv_contrib.zip https://github.com/opencv/opencv_contrib/archive/4.5.1.zip && \
# unzip opencv.zip && \
# unzip opencv_contrib.zip && \
# mv opencv-4.5.1 opencv && \
# mv opencv_contrib-4.5.1 opencv_contrib && \
# rm opencv.zip && \
# rm opencv_contrib.zip
#
#RUN cd ~/opencv && \
# mkdir build && \
# cd build && \
# cmake -D CMAKE_BUILD_TYPE=RELEASE \
# -D CMAKE_INSTALL_PREFIX=/usr \
# -D OPENCV_EXTRA_MODULES_PATH=~/opencv_contrib/modules \
# -D EIGEN_INCLUDE_PATH=/usr/include/eigen3 \
# -D WITH_OPENCL=OFF \
# -D WITH_CUDA=OFF \
# -D CUDA_ARCH_BIN=5.3 \
# -D CUDA_ARCH_PTX="" \
# -D WITH_CUDNN=OFF \
# -D WITH_CUBLAS=OFF \
# -D ENABLE_FAST_MATH=ON \
# -D CUDA_FAST_MATH=OFF \
# -D OPENCV_DNN_CUDA=OFF \
# -D ENABLE_NEON=ON \
# -D WITH_QT=OFF \
# -D WITH_OPENMP=ON \
# -D WITH_OPENGL=ON \
# -D BUILD_TIFF=ON \
# -D WITH_FFMPEG=ON \
# -D WITH_GSTREAMER=ON \
# -D WITH_TBB=ON \
# -D BUILD_TBB=ON \
# -D BUILD_TESTS=OFF \
# -D WITH_EIGEN=ON \
# -D WITH_V4L=ON \
# -D WITH_LIBV4L=ON \
# -D OPENCV_ENABLE_NONFREE=ON \
# -D INSTALL_C_EXAMPLES=OFF \
# -D INSTALL_PYTHON_EXAMPLES=OFF \
# -D BUILD_NEW_PYTHON_SUPPORT=ON \
# -D BUILD_opencv_python3=TRUE \
# -D OPENCV_GENERATE_PKGCONFIG=ON \
# -D BUILD_EXAMPLES=OFF .. && \
# make -j4 && cd ~ && \
# # sudo rm -r /usr/include/opencv4/opencv2 && \
# cd ~/opencv/build && \
# sudo make install && \
# sudo ldconfig && \
# make clean && \
# sudo apt-get update
RUN apt-get install -y libgtk2.0-dev pkg-config
RUN pip3 install opencv-python opencv-contrib-python
####################################################################################
###### START HERE -- Install whatever dependencies you need specific to this project!
####################################################################################
#COPY ./rsc/IsaacGym_Preview_2_Package.tar.gz /home/$USERNAME/
#RUN cd /home/$USERNAME && tar -xvzf IsaacGym_Preview_2_Package.tar.gz
#COPY ./rsc/learning_to_walk_in_minutes.zip /home/$USERNAME/
#RUN apt-get install unzip && cd /home/$USERNAME/ && unzip learning_to_walk_in_minutes.zip && cd ./code/rl-pytorch && pip3 install -e .
#RUN cd /home/$USERNAME/isaacgym/python && pip3 install -e .
#RUN cd /home/$USERNAME/code/isaacgym_anymal && pip3 install -e .
#COPY ./src/isaacgym_anymal/ /home/$USERNAME/code/isaacgym_anymal/
# setup entrypoint
COPY entrypoint.sh /
ENTRYPOINT ["/entrypoint.sh"]
CMD ["bash"]
go1_gym_deploy/docker/Makefile
+37
View File
@@ -0,0 +1,37 @@
default: build
build:
docker build -t jetson-model-deployment .
clean-build:
docker build -t jetson-model-deployment . --no-cache=true
run:
docker run -it \
--env="DISPLAY" \
--env="QT_X11_NO_MITSHM=1" \
--volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
--env="XAUTHORITY=${XAUTH}" \
--volume="${XAUTH}:${XAUTH}" \
--volume="/home/unitree/go1_gym:/home/isaac/go1_gym" \
--privileged \
--runtime=nvidia \
--net=host \
--workdir="/home/isaac/go1_gym" \
--name="foxy_controller" \
jetson-model-deployment bash
autostart:
docker stop foxy_controller || true
docker rm foxy_controller || true
docker run -d\
--env="DISPLAY" \
--env="QT_X11_NO_MITSHM=1" \
--volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
--env="XAUTHORITY=${XAUTH}" \
--volume="${XAUTH}:${XAUTH}" \
--volume="/home/unitree/go1_gym:/home/isaac/go1_gym" \
--privileged \
--runtime=nvidia \
--net=host \
--workdir="/home/isaac/go1_gym" \
--name="foxy_controller" \
jetson-model-deployment tail -f /dev/null
docker start foxy_controller
docker exec foxy_controller bash -c 'cd /home/isaac/go1_gym/ && python3 setup.py install && cd go1_gym_deploy/scripts && ls && python3 deploy_policy.py'
go1_gym_deploy/docker/unzip_image.sh
Executable
+2
View File
@@ -0,0 +1,2 @@
#!/bin/bash
docker load -i deployment_image.tar
go1_gym_deploy/docker/zip_image.sh
Executable
+2
View File
@@ -0,0 +1,2 @@
#!/bin/bash
docker save -o deployment_image.tar jetson-model-deployment:latest
go1_gym_deploy/envs/__init__.py
View File
go1_gym_deploy/envs/history_wrapper.py
Executable
+51
View File
@@ -0,0 +1,51 @@
# import isaacgym
# assert isaacgym, "import isaacgym before pytorch"
import torch
class HistoryWrapper:
def __init__(self, env):
self.env = env
if isinstance(self.env.cfg, dict):
self.obs_history_length = self.env.cfg["env"]["num_observation_history"]
else:
self.obs_history_length = self.env.cfg.env.num_observation_history
self.num_obs_history = self.obs_history_length * self.env.num_obs
self.obs_history = torch.zeros(self.env.num_envs, self.num_obs_history, dtype=torch.float,
device=self.env.device, requires_grad=False)
self.num_privileged_obs = self.env.num_privileged_obs
def step(self, action):
obs, rew, done, info = self.env.step(action)
privileged_obs = info["privileged_obs"]
self.obs_history = torch.cat((self.obs_history[:, self.env.num_obs:], obs), dim=-1)
return {'obs': obs, 'privileged_obs': privileged_obs, 'obs_history': self.obs_history}, rew, done, info
def get_observations(self):
obs = self.env.get_observations()
privileged_obs = self.env.get_privileged_observations()
self.obs_history = torch.cat((self.obs_history[:, self.env.num_obs:], obs), dim=-1)
return {'obs': obs, 'privileged_obs': privileged_obs, 'obs_history': self.obs_history}
def get_obs(self):
obs = self.env.get_obs()
privileged_obs = self.env.get_privileged_observations()
self.obs_history = torch.cat((self.obs_history[:, self.env.num_obs:], obs), dim=-1)
return {'obs': obs, 'privileged_obs': privileged_obs, 'obs_history': self.obs_history}
def reset_idx(self, env_ids): # it might be a problem that this isn't getting called!!
ret = self.env.reset_idx(env_ids)
self.obs_history[env_ids, :] = 0
return ret
def reset(self):
ret = self.env.reset()
privileged_obs = self.env.get_privileged_observations()
self.obs_history[:, :] = 0
return {"obs": ret, "privileged_obs": privileged_obs, "obs_history": self.obs_history}
def __getattr__(self, name):
return getattr(self.env, name)
go1_gym_deploy/envs/lcm_agent.py
Executable
+301
View File
@@ -0,0 +1,301 @@
import time
import lcm
import numpy as np
import torch
import cv2
from go1_gym_deploy.lcm_types.pd_tau_targets_lcmt import pd_tau_targets_lcmt
lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=255")
def class_to_dict(obj) -> dict:
if not hasattr(obj, "__dict__"):
return obj
result = {}
for key in dir(obj):
if key.startswith("_") or key == "terrain":
continue
element = []
val = getattr(obj, key)
if isinstance(val, list):
for item in val:
element.append(class_to_dict(item))
else:
element = class_to_dict(val)
result[key] = element
return result
class LCMAgent():
def __init__(self, cfg, se, command_profile):
if not isinstance(cfg, dict):
cfg = class_to_dict(cfg)
self.cfg = cfg
self.se = se
self.command_profile = command_profile
self.dt = self.cfg["control"]["decimation"] * self.cfg["sim"]["dt"]
self.timestep = 0
self.num_obs = self.cfg["env"]["num_observations"]
self.num_envs = 1
self.num_privileged_obs = self.cfg["env"]["num_privileged_obs"]
self.num_actions = self.cfg["env"]["num_actions"]
self.num_commands = self.cfg["commands"]["num_commands"]
self.device = 'cpu'
if "obs_scales" in self.cfg.keys():
self.obs_scales = self.cfg["obs_scales"]
else:
self.obs_scales = self.cfg["normalization"]["obs_scales"]
self.commands_scale = np.array(
[self.obs_scales["lin_vel"], self.obs_scales["lin_vel"],
self.obs_scales["ang_vel"], self.obs_scales["body_height_cmd"], 1, 1, 1, 1, 1,
self.obs_scales["footswing_height_cmd"], self.obs_scales["body_pitch_cmd"],
# 0, self.obs_scales["body_pitch_cmd"],
self.obs_scales["body_roll_cmd"], self.obs_scales["stance_width_cmd"],
self.obs_scales["stance_length_cmd"], self.obs_scales["aux_reward_cmd"], 1, 1, 1, 1, 1, 1
])[:self.num_commands]
joint_names = [
"FL_hip_joint", "FL_thigh_joint", "FL_calf_joint",
"FR_hip_joint", "FR_thigh_joint", "FR_calf_joint",
"RL_hip_joint", "RL_thigh_joint", "RL_calf_joint",
"RR_hip_joint", "RR_thigh_joint", "RR_calf_joint", ]
self.default_dof_pos = np.array([self.cfg["init_state"]["default_joint_angles"][name] for name in joint_names])
try:
self.default_dof_pos_scale = np.array([self.cfg["init_state"]["default_hip_scales"], self.cfg["init_state"]["default_thigh_scales"], self.cfg["init_state"]["default_calf_scales"],
self.cfg["init_state"]["default_hip_scales"], self.cfg["init_state"]["default_thigh_scales"], self.cfg["init_state"]["default_calf_scales"],
self.cfg["init_state"]["default_hip_scales"], self.cfg["init_state"]["default_thigh_scales"], self.cfg["init_state"]["default_calf_scales"],
self.cfg["init_state"]["default_hip_scales"], self.cfg["init_state"]["default_thigh_scales"], self.cfg["init_state"]["default_calf_scales"]])
except KeyError:
self.default_dof_pos_scale = np.ones(12)
self.default_dof_pos = self.default_dof_pos * self.default_dof_pos_scale
self.p_gains = np.zeros(12)
self.d_gains = np.zeros(12)
for i in range(12):
joint_name = joint_names[i]
found = False
for dof_name in self.cfg["control"]["stiffness"].keys():
if dof_name in joint_name:
self.p_gains[i] = self.cfg["control"]["stiffness"][dof_name]
self.d_gains[i] = self.cfg["control"]["damping"][dof_name]
found = True
if not found:
self.p_gains[i] = 0.
self.d_gains[i] = 0.
if self.cfg["control"]["control_type"] in ["P", "V"]:
print(f"PD gain of joint {joint_name} were not defined, setting them to zero")
print(f"p_gains: {self.p_gains}")
self.commands = np.zeros((1, self.num_commands))
self.actions = torch.zeros(12)
self.last_actions = torch.zeros(12)
self.gravity_vector = np.zeros(3)
self.dof_pos = np.zeros(12)
self.dof_vel = np.zeros(12)
self.body_linear_vel = np.zeros(3)
self.body_angular_vel = np.zeros(3)
self.joint_pos_target = np.zeros(12)
self.joint_vel_target = np.zeros(12)
self.torques = np.zeros(12)
self.contact_state = np.ones(4)
self.joint_idxs = self.se.joint_idxs
self.gait_indices = torch.zeros(self.num_envs, dtype=torch.float)
self.clock_inputs = torch.zeros(self.num_envs, 4, dtype=torch.float)
if "obs_scales" in self.cfg.keys():
self.obs_scales = self.cfg["obs_scales"]
else:
self.obs_scales = self.cfg["normalization"]["obs_scales"]
self.is_currently_probing = False
def set_probing(self, is_currently_probing):
self.is_currently_probing = is_currently_probing
def get_obs(self):
self.gravity_vector = self.se.get_gravity_vector()
cmds, reset_timer = self.command_profile.get_command(self.timestep * self.dt, probe=self.is_currently_probing)
self.commands[:, :] = cmds[:self.num_commands]
if reset_timer:
self.reset_gait_indices()
#else:
# self.commands[:, 0:3] = self.command_profile.get_command(self.timestep * self.dt)[0:3]
self.dof_pos = self.se.get_dof_pos()
self.dof_vel = self.se.get_dof_vel()
self.body_linear_vel = self.se.get_body_linear_vel()
self.body_angular_vel = self.se.get_body_angular_vel()
ob = np.concatenate((self.gravity_vector.reshape(1, -1),
self.commands * self.commands_scale,
(self.dof_pos - self.default_dof_pos).reshape(1, -1) * self.obs_scales["dof_pos"],
self.dof_vel.reshape(1, -1) * self.obs_scales["dof_vel"],
torch.clip(self.actions, -self.cfg["normalization"]["clip_actions"],
self.cfg["normalization"]["clip_actions"]).cpu().detach().numpy().reshape(1, -1)
), axis=1)
if self.cfg["env"]["observe_two_prev_actions"]:
ob = np.concatenate((ob,
self.last_actions.cpu().detach().numpy().reshape(1, -1)), axis=1)
if self.cfg["env"]["observe_clock_inputs"]:
ob = np.concatenate((ob,
self.clock_inputs), axis=1)
# print(self.clock_inputs)
if self.cfg["env"]["observe_vel"]:
ob = np.concatenate(
(self.body_linear_vel.reshape(1, -1) * self.obs_scales["lin_vel"],
self.body_angular_vel.reshape(1, -1) * self.obs_scales["ang_vel"],
ob), axis=1)
if self.cfg["env"]["observe_only_lin_vel"]:
ob = np.concatenate(
(self.body_linear_vel.reshape(1, -1) * self.obs_scales["lin_vel"],
ob), axis=1)
if self.cfg["env"]["observe_yaw"]:
heading = self.se.get_yaw()
ob = np.concatenate((ob, heading.reshape(1, -1)), axis=-1)
self.contact_state = self.se.get_contact_state()
if "observe_contact_states" in self.cfg["env"].keys() and self.cfg["env"]["observe_contact_states"]:
ob = np.concatenate((ob, self.contact_state.reshape(1, -1)), axis=-1)
if "terrain" in self.cfg.keys() and self.cfg["terrain"]["measure_heights"]:
robot_height = 0.25
self.measured_heights = np.zeros(
(len(self.cfg["terrain"]["measured_points_x"]), len(self.cfg["terrain"]["measured_points_y"]))).reshape(
1, -1)
heights = np.clip(robot_height - 0.5 - self.measured_heights, -1, 1.) * self.obs_scales["height_measurements"]
ob = np.concatenate((ob, heights), axis=1)
return torch.tensor(ob, device=self.device).float()
def get_privileged_observations(self):
return None
def publish_action(self, action, hard_reset=False):
command_for_robot = pd_tau_targets_lcmt()
self.joint_pos_target = \
(action[0, :12].detach().cpu().numpy() * self.cfg["control"]["action_scale"]).flatten()
self.joint_pos_target[[0, 3, 6, 9]] *= self.cfg["control"]["hip_scale_reduction"]
# self.joint_pos_target[[0, 3, 6, 9]] *= -1
self.joint_pos_target = self.joint_pos_target
self.joint_pos_target += self.default_dof_pos
joint_pos_target = self.joint_pos_target[self.joint_idxs]
self.joint_vel_target = np.zeros(12)
# print(f'cjp {self.joint_pos_target}')
command_for_robot.q_des = joint_pos_target
command_for_robot.qd_des = self.joint_vel_target
command_for_robot.kp = self.p_gains
command_for_robot.kd = self.d_gains
command_for_robot.tau_ff = np.zeros(12)
command_for_robot.se_contactState = np.zeros(4)
command_for_robot.timestamp_us = int(time.time() * 10 ** 6)
command_for_robot.id = 0
if hard_reset:
command_for_robot.id = -1
self.torques = (self.joint_pos_target - self.dof_pos) * self.p_gains + (self.joint_vel_target - self.dof_vel) * self.d_gains
lc.publish("pd_plustau_targets", command_for_robot.encode())
def reset(self):
self.actions = torch.zeros(12)
self.time = time.time()
self.timestep = 0
return self.get_obs()
def reset_gait_indices(self):
self.gait_indices = torch.zeros(self.num_envs, dtype=torch.float)
def step(self, actions, hard_reset=False):
clip_actions = self.cfg["normalization"]["clip_actions"]
self.last_actions = self.actions[:]
self.actions = torch.clip(actions[0:1, :], -clip_actions, clip_actions)
self.publish_action(self.actions, hard_reset=hard_reset)
time.sleep(max(self.dt - (time.time() - self.time), 0))
if self.timestep % 100 == 0: print(f'frq: {1 / (time.time() - self.time)} Hz');
self.time = time.time()
obs = self.get_obs()
# clock accounting
frequencies = self.commands[:, 4]
phases = self.commands[:, 5]
offsets = self.commands[:, 6]
if self.num_commands == 8:
bounds = 0
durations = self.commands[:, 7]
else:
bounds = self.commands[:, 7]
durations = self.commands[:, 8]
self.gait_indices = torch.remainder(self.gait_indices + self.dt * frequencies, 1.0)
if "pacing_offset" in self.cfg["commands"] and self.cfg["commands"]["pacing_offset"]:
self.foot_indices = [self.gait_indices + phases + offsets + bounds,
self.gait_indices + bounds,
self.gait_indices + offsets,
self.gait_indices + phases]
else:
self.foot_indices = [self.gait_indices + phases + offsets + bounds,
self.gait_indices + offsets,
self.gait_indices + bounds,
self.gait_indices + phases]
self.clock_inputs[:, 0] = torch.sin(2 * np.pi * self.foot_indices[0])
self.clock_inputs[:, 1] = torch.sin(2 * np.pi * self.foot_indices[1])
self.clock_inputs[:, 2] = torch.sin(2 * np.pi * self.foot_indices[2])
self.clock_inputs[:, 3] = torch.sin(2 * np.pi * self.foot_indices[3])
images = {'front': self.se.get_camera_front(),
'bottom': self.se.get_camera_bottom(),
'rear': self.se.get_camera_rear(),
'left': self.se.get_camera_left(),
'right': self.se.get_camera_right()
}
downscale_factor = 2
temporal_downscale = 3
for k, v in images.items():
if images[k] is not None:
images[k] = cv2.resize(images[k], dsize=(images[k].shape[0]//downscale_factor, images[k].shape[1]//downscale_factor), interpolation=cv2.INTER_CUBIC)
if self.timestep % temporal_downscale != 0:
images[k] = None
#print(self.commands)
infos = {"joint_pos": self.dof_pos[np.newaxis, :],
"joint_vel": self.dof_vel[np.newaxis, :],
"joint_pos_target": self.joint_pos_target[np.newaxis, :],
"joint_vel_target": self.joint_vel_target[np.newaxis, :],
"body_linear_vel": self.body_linear_vel[np.newaxis, :],
"body_angular_vel": self.body_angular_vel[np.newaxis, :],
"contact_state": self.contact_state[np.newaxis, :],
"clock_inputs": self.clock_inputs[np.newaxis, :],
"body_linear_vel_cmd": self.commands[:, 0:2],
"body_angular_vel_cmd": self.commands[:, 2:],
"privileged_obs": None,
"camera_image_front": images['front'],
"camera_image_bottom": images['bottom'],
"camera_image_rear": images['rear'],
"camera_image_left": images['left'],
"camera_image_right": images['right'],
}
self.timestep += 1
return obs, None, None, infos
go1_gym_deploy/installer/install_deployment_code.sh
+22
View File
@@ -0,0 +1,22 @@
#!/bin/bash
echo "======================================"
echo "== Go1 Sim-to-Real Installation Kit =="
echo "======================================"
echo ""
echo "Author: Gabriel Margolis, Improbable AI Lab, MIT"
echo "This software is intended to support controls research. It includes safety features but may still damage your Go1. The user assumes all risk."
echo ""
read -r -p "Extract docker container? [y/N] " response
if [[ "$response" =~ ^([yY][eE][sS]|[yY])$ ]]
then
# load docker image
echo "[Step 1] Extracting docker image..."
docker load -i deployment_image.tar
printf "\nDone!\n"
else
echo "Quitting"
fi
go1_gym_deploy/lcm_types/__init__.py
View File
go1_gym_deploy/lcm_types/camera_message_lcmt.py
+60
View File
@@ -0,0 +1,60 @@
"""LCM type definitions
This file automatically generated by lcm.
DO NOT MODIFY BY HAND!!!!
"""
try:
import cStringIO.StringIO as BytesIO
except ImportError:
from io import BytesIO
import struct
class camera_message_lcmt(object):
__slots__ = ["data"]
__typenames__ = ["byte"]
__dimensions__ = [[278400]]
def __init__(self):
self.data = ""
def encode(self):
buf = BytesIO()
buf.write(camera_message_lcmt._get_packed_fingerprint())
self._encode_one(buf)
return buf.getvalue()
def _encode_one(self, buf):
buf.write(bytearray(self.data[:278400]))
def decode(data):
if hasattr(data, 'read'):
buf = data
else:
buf = BytesIO(data)
if buf.read(8) != camera_message_lcmt._get_packed_fingerprint():
raise ValueError("Decode error")
return camera_message_lcmt._decode_one(buf)
decode = staticmethod(decode)
def _decode_one(buf):
self = camera_message_lcmt()
self.data = buf.read(278400)
return self
_decode_one = staticmethod(_decode_one)
_hash = None
def _get_hash_recursive(parents):
if camera_message_lcmt in parents: return 0
tmphash = (0x1610a8a9f4d174b7) & 0xffffffffffffffff
tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff
return tmphash
_get_hash_recursive = staticmethod(_get_hash_recursive)
_packed_fingerprint = None
def _get_packed_fingerprint():
if camera_message_lcmt._packed_fingerprint is None:
camera_message_lcmt._packed_fingerprint = struct.pack(">Q", camera_message_lcmt._get_hash_recursive([]))
return camera_message_lcmt._packed_fingerprint
_get_packed_fingerprint = staticmethod(_get_packed_fingerprint)
go1_gym_deploy/lcm_types/camera_message_rect_wide.py
+57
View File
@@ -0,0 +1,57 @@
"""LCM type definitions
This file automatically generated by lcm.
DO NOT MODIFY BY HAND!!!!
"""
try:
import cStringIO.StringIO as BytesIO
except ImportError:
from io import BytesIO
import struct
class camera_message_rect_wide(object):
__slots__ = ["data"]
def __init__(self):
self.data = ""
def encode(self):
buf = BytesIO()
buf.write(camera_message_rect_wide._get_packed_fingerprint())
self._encode_one(buf)
return buf.getvalue()
def _encode_one(self, buf):
buf.write(bytearray(self.data[:34800]))
def decode(data):
if hasattr(data, 'read'):
buf = data
else:
buf = BytesIO(data)
if buf.read(8) != camera_message_rect_wide._get_packed_fingerprint():
raise ValueError("Decode error")
return camera_message_rect_wide._decode_one(buf)
decode = staticmethod(decode)
def _decode_one(buf):
self = camera_message_rect_wide()
self.data = buf.read(34800)
return self
_decode_one = staticmethod(_decode_one)
_hash = None
def _get_hash_recursive(parents):
if camera_message_rect_wide in parents: return 0
tmphash = (0xc3e9f058530b2a8b) & 0xffffffffffffffff
tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff
return tmphash
_get_hash_recursive = staticmethod(_get_hash_recursive)
_packed_fingerprint = None
def _get_packed_fingerprint():
if camera_message_rect_wide._packed_fingerprint is None:
camera_message_rect_wide._packed_fingerprint = struct.pack(">Q", camera_message_rect_wide._get_hash_recursive([]))
return camera_message_rect_wide._packed_fingerprint
_get_packed_fingerprint = staticmethod(_get_packed_fingerprint)
go1_gym_deploy/lcm_types/leg_control_data_lcmt.lcm
Executable
+11
View File
@@ -0,0 +1,11 @@
struct leg_control_data_lcmt
{
float q[12];
float qd[12];
float p[12];
float v[12];
float tau_est[12];
int64_t timestamp_us;
int64_t id;
int64_t robot_id;
}
go1_gym_deploy/lcm_types/leg_control_data_lcmt.py
Executable
+85
View File
@@ -0,0 +1,85 @@
"""LCM type definitions
This file automatically generated by lcm.
DO NOT MODIFY BY HAND!!!!
"""
try:
import cStringIO.StringIO as BytesIO
except ImportError:
from io import BytesIO
import struct
class leg_control_data_lcmt(object):
__slots__ = ["q", "qd", "p", "v", "tau_est", "timestamp_us", "id", "robot_id"]
__typenames__ = ["float", "float", "float", "float", "float", "int64_t", "int64_t", "int64_t"]
__dimensions__ = [[12], [12], [12], [12], [12], None, None, None]
def __init__(self):
self.q = [0.0 for dim0 in range(12)]
self.qd = [0.0 for dim0 in range(12)]
self.p = [0.0 for dim0 in range(12)]
self.v = [0.0 for dim0 in range(12)]
self.tau_est = [0.0 for dim0 in range(12)]
self.timestamp_us = 0
self.id = 0
self.robot_id = 0
def encode(self):
buf = BytesIO()
buf.write(leg_control_data_lcmt._get_packed_fingerprint())
self._encode_one(buf)
return buf.getvalue()
def _encode_one(self, buf):
buf.write(struct.pack('>12f', *self.q[:12]))
buf.write(struct.pack('>12f', *self.qd[:12]))
buf.write(struct.pack('>12f', *self.p[:12]))
buf.write(struct.pack('>12f', *self.v[:12]))
buf.write(struct.pack('>12f', *self.tau_est[:12]))
buf.write(struct.pack(">qqq", self.timestamp_us, self.id, self.robot_id))
def decode(data):
if hasattr(data, 'read'):
buf = data
else:
buf = BytesIO(data)
if buf.read(8) != leg_control_data_lcmt._get_packed_fingerprint():
raise ValueError("Decode error")
return leg_control_data_lcmt._decode_one(buf)
decode = staticmethod(decode)
def _decode_one(buf):
self = leg_control_data_lcmt()
self.q = struct.unpack('>12f', buf.read(48))
self.qd = struct.unpack('>12f', buf.read(48))
self.p = struct.unpack('>12f', buf.read(48))
self.v = struct.unpack('>12f', buf.read(48))
self.tau_est = struct.unpack('>12f', buf.read(48))
self.timestamp_us, self.id, self.robot_id = struct.unpack(">qqq", buf.read(24))
return self
_decode_one = staticmethod(_decode_one)
def _get_hash_recursive(parents):
if leg_control_data_lcmt in parents: return 0
tmphash = (0xa9a928b534bfc487) & 0xffffffffffffffff
tmphash = (((tmphash << 1) & 0xffffffffffffffff) + (tmphash >> 63)) & 0xffffffffffffffff
return tmphash
_get_hash_recursive = staticmethod(_get_hash_recursive)
_packed_fingerprint = None
def _get_packed_fingerprint():
if leg_control_data_lcmt._packed_fingerprint is None:
leg_control_data_lcmt._packed_fingerprint = struct.pack(">Q", leg_control_data_lcmt._get_hash_recursive([]))
return leg_control_data_lcmt._packed_fingerprint
_get_packed_fingerprint = staticmethod(_get_packed_fingerprint)
def get_hash(self):
"""Get the LCM hash of the struct"""
return struct.unpack(">Q", leg_control_data_lcmt._get_packed_fingerprint())[0]
go1_gym_deploy/lcm_types/pd_tau_targets_lcmt.lcm
Executable
+12
View File
@@ -0,0 +1,12 @@
struct pd_tau_targets_lcmt
{
double q_des[12];
double qd_des[12];
double tau_ff[12];
double kp[12];
double kd[12];
int64_t timestamp_us;
int64_t id;
int64_t robot_id;
double se_contactState[4];
}
go1_gym_deploy/lcm_types/pd_tau_targets_lcmt.py
Executable
+88
View File
@@ -0,0 +1,88 @@
"""LCM type definitions
This file automatically generated by lcm.
DO NOT MODIFY BY HAND!!!!
"""
try:
import cStringIO.StringIO as BytesIO
except ImportError:
from io import BytesIO
import struct
class pd_tau_targets_lcmt(object):
__slots__ = ["q_des", "qd_des", "tau_ff", "kp", "kd", "timestamp_us", "id", "robot_id", "se_contactState"]
__typenames__ = ["double", "double", "double", "double", "double", "int64_t", "int64_t", "int64_t", "double"]
__dimensions__ = [[12], [12], [12], [12], [12], None, None, None, [4]]
def __init__(self):
self.q_des = [0.0 for dim0 in range(12)]
self.qd_des = [0.0 for dim0 in range(12)]
self.tau_ff = [0.0 for dim0 in range(12)]
self.kp = [0.0 for dim0 in range(12)]
self.kd = [0.0 for dim0 in range(12)]
self.timestamp_us = 0
self.id = 0
self.robot_id = 0
self.se_contactState = [0.0 for dim0 in range(4)]
def encode(self):
buf = BytesIO()
buf.write(pd_tau_targets_lcmt._get_packed_fingerprint())
self._encode_one(buf)
return buf.getvalue()
def _encode_one(self, buf):
buf.write(struct.pack('>12d', *self.q_des[:12]))
buf.write(struct.pack('>12d', *self.qd_des[:12]))
buf.write(struct.pack('>12d', *self.tau_ff[:12]))
buf.write(struct.pack('>12d', *self.kp[:12]))
buf.write(struct.pack('>12d', *self.kd[:12]))
buf.write(struct.pack(">qqq", self.timestamp_us, self.id, self.robot_id))
buf.write(struct.pack('>4d', *self.se_contactState[:4]))
def decode(data):
if hasattr(data, 'read'):
buf = data
else:
buf = BytesIO(data)
if buf.read(8) != pd_tau_targets_lcmt._get_packed_fingerprint():
raise ValueError("Decode error")
return pd_tau_targets_lcmt._decode_one(buf)
decode = staticmethod(decode)
def _decode_one(buf):
self = pd_tau_targets_lcmt()
self.q_des = struct.unpack('>12d', buf.read(96))
self.qd_des = struct.unpack('>12d', buf.read(96))
self.tau_ff = struct.unpack('>12d', buf.read(96))
self.kp = struct.unpack('>12d', buf.read(96))
self.kd = struct.unpack('>12d', buf.read(96))
self.timestamp_us, self.id, self.robot_id = struct.unpack(">qqq", buf.read(24))
self.se_contactState = struct.unpack('>4d', buf.read(32))
return self
_decode_one = staticmethod(_decode_one)
def _get_hash_recursive(parents):
if pd_tau_targets_lcmt in parents: return 0
tmphash = (0x6d88128ef1291cc1) & 0xffffffffffffffff
tmphash = (((tmphash << 1) & 0xffffffffffffffff) + (tmphash >> 63)) & 0xffffffffffffffff
return tmphash
_get_hash_recursive = staticmethod(_get_hash_recursive)
_packed_fingerprint = None
def _get_packed_fingerprint():
if pd_tau_targets_lcmt._packed_fingerprint is None:
pd_tau_targets_lcmt._packed_fingerprint = struct.pack(">Q", pd_tau_targets_lcmt._get_hash_recursive([]))
return pd_tau_targets_lcmt._packed_fingerprint
_get_packed_fingerprint = staticmethod(_get_packed_fingerprint)
def get_hash(self):
"""Get the LCM hash of the struct"""
return struct.unpack(">Q", pd_tau_targets_lcmt._get_packed_fingerprint())[0]
go1_gym_deploy/lcm_types/rc_command_lcmt.lcm
Executable
+13
View File
@@ -0,0 +1,13 @@
struct rc_command_lcmt
{
int16_t mode;
float left_stick[2];
float right_stick[2];
float knobs[2];
int16_t left_upper_switch;
int16_t left_lower_left_switch;
int16_t left_lower_right_switch;
int16_t right_upper_switch;
int16_t right_lower_left_switch;
int16_t right_lower_right_switch;
}
go1_gym_deploy/lcm_types/rc_command_lcmt.py
Executable
+90
View File
@@ -0,0 +1,90 @@
"""LCM type definitions
This file automatically generated by lcm.
DO NOT MODIFY BY HAND!!!!
"""
try:
import cStringIO.StringIO as BytesIO
except ImportError:
from io import BytesIO
import struct
class rc_command_lcmt(object):
__slots__ = ["mode", "left_stick", "right_stick", "knobs", "left_upper_switch", "left_lower_left_switch",
"left_lower_right_switch", "right_upper_switch", "right_lower_left_switch", "right_lower_right_switch"]
__typenames__ = ["int16_t", "float", "float", "float", "int16_t", "int16_t", "int16_t", "int16_t", "int16_t",
"int16_t"]
__dimensions__ = [None, [2], [2], [2], None, None, None, None, None, None]
def __init__(self):
self.mode = 0
self.left_stick = [0.0 for dim0 in range(2)]
self.right_stick = [0.0 for dim0 in range(2)]
self.knobs = [0.0 for dim0 in range(2)]
self.left_upper_switch = 0
self.left_lower_left_switch = 0
self.left_lower_right_switch = 0
self.right_upper_switch = 0
self.right_lower_left_switch = 0
self.right_lower_right_switch = 0
def encode(self):
buf = BytesIO()
buf.write(rc_command_lcmt._get_packed_fingerprint())
self._encode_one(buf)
return buf.getvalue()
def _encode_one(self, buf):
buf.write(struct.pack(">h", self.mode))
buf.write(struct.pack('>2f', *self.left_stick[:2]))
buf.write(struct.pack('>2f', *self.right_stick[:2]))
buf.write(struct.pack('>2f', *self.knobs[:2]))
buf.write(
struct.pack(">hhhhhh", self.left_upper_switch, self.left_lower_left_switch, self.left_lower_right_switch,
self.right_upper_switch, self.right_lower_left_switch, self.right_lower_right_switch))
def decode(data):
if hasattr(data, 'read'):
buf = data
else:
buf = BytesIO(data)
if buf.read(8) != rc_command_lcmt._get_packed_fingerprint():
raise ValueError("Decode error")
return rc_command_lcmt._decode_one(buf)
decode = staticmethod(decode)
def _decode_one(buf):
self = rc_command_lcmt()
self.mode = struct.unpack(">h", buf.read(2))[0]
self.left_stick = struct.unpack('>2f', buf.read(8))
self.right_stick = struct.unpack('>2f', buf.read(8))
self.knobs = struct.unpack('>2f', buf.read(8))
self.left_upper_switch, self.left_lower_left_switch, self.left_lower_right_switch, self.right_upper_switch, self.right_lower_left_switch, self.right_lower_right_switch = struct.unpack(
">hhhhhh", buf.read(12))
return self
_decode_one = staticmethod(_decode_one)
def _get_hash_recursive(parents):
if rc_command_lcmt in parents: return 0
tmphash = (0xc7726b02ec3e7de2) & 0xffffffffffffffff
tmphash = (((tmphash << 1) & 0xffffffffffffffff) + (tmphash >> 63)) & 0xffffffffffffffff
return tmphash
_get_hash_recursive = staticmethod(_get_hash_recursive)
_packed_fingerprint = None
def _get_packed_fingerprint():
if rc_command_lcmt._packed_fingerprint is None:
rc_command_lcmt._packed_fingerprint = struct.pack(">Q", rc_command_lcmt._get_hash_recursive([]))
return rc_command_lcmt._packed_fingerprint
_get_packed_fingerprint = staticmethod(_get_packed_fingerprint)
def get_hash(self):
"""Get the LCM hash of the struct"""
return struct.unpack(">Q", rc_command_lcmt._get_packed_fingerprint())[0]
go1_gym_deploy/lcm_types/state_estimator_lcmt.lcm
Executable
+16
View File
@@ -0,0 +1,16 @@
struct state_estimator_lcmt
{
float p[3];
float vWorld[3];
float vBody[3];
float rpy[3];
float omegaBody[3];
float omegaWorld[3];
float quat[4];
float contact_estimate[4];
float aBody[3];
float aWorld[3];
int64_t timestamp_us;
int64_t id;
int64_t robot_id;
}
go1_gym_deploy/lcm_types/state_estimator_lcmt.py
Executable
+102
View File
@@ -0,0 +1,102 @@
"""LCM type definitions
This file automatically generated by lcm.
DO NOT MODIFY BY HAND!!!!
"""
try:
import cStringIO.StringIO as BytesIO
except ImportError:
from io import BytesIO
import struct
class state_estimator_lcmt(object):
__slots__ = ["p", "vWorld", "vBody", "rpy", "omegaBody", "omegaWorld", "quat", "contact_estimate", "aBody",
"aWorld", "timestamp_us", "id", "robot_id"]
__typenames__ = ["float", "float", "float", "float", "float", "float", "float", "float", "float", "float",
"int64_t", "int64_t", "int64_t"]
__dimensions__ = [[3], [3], [3], [3], [3], [3], [4], [4], [3], [3], None, None, None]
def __init__(self):
self.p = [0.0 for dim0 in range(3)]
self.vWorld = [0.0 for dim0 in range(3)]
self.vBody = [0.0 for dim0 in range(3)]
self.rpy = [0.0 for dim0 in range(3)]
self.omegaBody = [0.0 for dim0 in range(3)]
self.omegaWorld = [0.0 for dim0 in range(3)]
self.quat = [0.0 for dim0 in range(4)]
self.contact_estimate = [0.0 for dim0 in range(4)]
self.aBody = [0.0 for dim0 in range(3)]
self.aWorld = [0.0 for dim0 in range(3)]
self.timestamp_us = 0
self.id = 0
self.robot_id = 0
def encode(self):
buf = BytesIO()
buf.write(state_estimator_lcmt._get_packed_fingerprint())
self._encode_one(buf)
return buf.getvalue()
def _encode_one(self, buf):
buf.write(struct.pack('>3f', *self.p[:3]))
buf.write(struct.pack('>3f', *self.vWorld[:3]))
buf.write(struct.pack('>3f', *self.vBody[:3]))
buf.write(struct.pack('>3f', *self.rpy[:3]))
buf.write(struct.pack('>3f', *self.omegaBody[:3]))
buf.write(struct.pack('>3f', *self.omegaWorld[:3]))
buf.write(struct.pack('>4f', *self.quat[:4]))
buf.write(struct.pack('>4f', *self.contact_estimate[:4]))
buf.write(struct.pack('>3f', *self.aBody[:3]))
buf.write(struct.pack('>3f', *self.aWorld[:3]))
buf.write(struct.pack(">qqq", self.timestamp_us, self.id, self.robot_id))
def decode(data):
if hasattr(data, 'read'):
buf = data
else:
buf = BytesIO(data)
if buf.read(8) != state_estimator_lcmt._get_packed_fingerprint():
raise ValueError("Decode error")
return state_estimator_lcmt._decode_one(buf)
decode = staticmethod(decode)
def _decode_one(buf):
self = state_estimator_lcmt()
self.p = struct.unpack('>3f', buf.read(12))
self.vWorld = struct.unpack('>3f', buf.read(12))
self.vBody = struct.unpack('>3f', buf.read(12))
self.rpy = struct.unpack('>3f', buf.read(12))
self.omegaBody = struct.unpack('>3f', buf.read(12))
self.omegaWorld = struct.unpack('>3f', buf.read(12))
self.quat = struct.unpack('>4f', buf.read(16))
self.contact_estimate = struct.unpack('>4f', buf.read(16))
self.aBody = struct.unpack('>3f', buf.read(12))
self.aWorld = struct.unpack('>3f', buf.read(12))
self.timestamp_us, self.id, self.robot_id = struct.unpack(">qqq", buf.read(24))
return self
_decode_one = staticmethod(_decode_one)
def _get_hash_recursive(parents):
if state_estimator_lcmt in parents: return 0
tmphash = (0xea87c8282effe5b6) & 0xffffffffffffffff
tmphash = (((tmphash << 1) & 0xffffffffffffffff) + (tmphash >> 63)) & 0xffffffffffffffff
return tmphash
_get_hash_recursive = staticmethod(_get_hash_recursive)
_packed_fingerprint = None
def _get_packed_fingerprint():
if state_estimator_lcmt._packed_fingerprint is None:
state_estimator_lcmt._packed_fingerprint = struct.pack(">Q", state_estimator_lcmt._get_hash_recursive([]))
return state_estimator_lcmt._packed_fingerprint
_get_packed_fingerprint = staticmethod(_get_packed_fingerprint)
def get_hash(self):
"""Get the LCM hash of the struct"""
return struct.unpack(">Q", state_estimator_lcmt._get_packed_fingerprint())[0]
go1_gym_deploy/scripts/__init__.py
View File
go1_gym_deploy/scripts/deploy_policy.py
+90
View File
@@ -0,0 +1,90 @@
import glob
import pickle as pkl
import lcm
import sys
from go1_gym_deploy.utils.deployment_runner import DeploymentRunner
from go1_gym_deploy.envs.lcm_agent import LCMAgent
from go1_gym_deploy.utils.cheetah_state_estimator import StateEstimator
from go1_gym_deploy.utils.command_profile import *
import pathlib
lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=255")
def load_and_run_policy(label, experiment_name, probe_policy_label=None, max_vel=1.0, max_yaw_vel=1.0, max_vel_probe=1.0):
# load agent
dirs = glob.glob(f"../../runs/{label}/*")
logdir = sorted(dirs)[0]
with open(logdir+"/parameters.pkl", 'rb') as file:
pkl_cfg = pkl.load(file)
print(pkl_cfg.keys())
cfg = pkl_cfg["Cfg"]
print(cfg.keys())
se = StateEstimator(lc)
control_dt = 0.02
command_profile = RCControllerProfile(dt=control_dt, state_estimator=se, x_scale=max_vel, y_scale=0.6, yaw_scale=max_yaw_vel, probe_vel_multiplier=(max_vel_probe / max_vel))
hardware_agent = LCMAgent(cfg, se, command_profile)
se.spin()
from go1_gym_deploy.envs.history_wrapper import HistoryWrapper
hardware_agent = HistoryWrapper(hardware_agent)
policy = load_policy(logdir)
if probe_policy_label is not None:
# load agent
dirs = glob.glob(f"../runs/{probe_policy_label}_*")
probe_policy_logdir = sorted(dirs)[0]
with open(probe_policy_logdir + "/parameters.pkl", 'rb') as file:
probe_cfg = pkl.load(file)
probe_cfg = probe_cfg["Cfg"]
probe_policy = load_policy(probe_policy_logdir)
# load runner
root = f"{pathlib.Path(__file__).parent.resolve()}/../../logs/"
pathlib.Path(root).mkdir(parents=True, exist_ok=True)
deployment_runner = DeploymentRunner(experiment_name=experiment_name, se=None,
log_root=f"{root}/{experiment_name}")
deployment_runner.add_control_agent(hardware_agent, "hardware_closed_loop")
deployment_runner.add_policy(policy)
if probe_policy_label is not None:
deployment_runner.add_probe_policy(probe_policy, probe_cfg)
deployment_runner.add_command_profile(command_profile)
if len(sys.argv) >= 2:
max_steps = int(sys.argv[1])
else:
max_steps = 10000000
print(f'max steps {max_steps}')
deployment_runner.run(max_steps=max_steps, logging=True)
def load_policy(logdir):
body = torch.jit.load(logdir + '/checkpoints/body_latest.jit')
import os
adaptation_module = torch.jit.load(logdir + '/checkpoints/adaptation_module_latest.jit')
def policy(obs, info):
i = 0
latent = adaptation_module.forward(obs["obs_history"].to('cpu'))
action = body.forward(torch.cat((obs["obs_history"].to('cpu'), latent), dim=-1))
info['latent'] = latent
return action
return policy
if __name__ == '__main__':
label = "gait-conditioned-agility/pretrain-v0/train"
probe_policy_label = None
experiment_name = "example_experiment"
load_and_run_policy(label, experiment_name=experiment_name, probe_policy_label=probe_policy_label, max_vel=3.0, max_yaw_vel=5.0, max_vel_probe=1.0)
go1_gym_deploy/scripts/send_to_unitree.sh
Executable
+8
View File
@@ -0,0 +1,8 @@
#!/bin/bash
# download docker image if it doesn't exist yet
wget --directory-prefix=../docker -nc --load-cookies /tmp/cookies.txt "https://docs.google.com/uc?export=download&confirm=$(wget --quiet --save-cookies /tmp/cookies.txt --keep-session-cookies --no-check-certificate 'https://docs.google.com/uc?export=download&id=1XkVpyYyYqQQ4FcgLIDUxg-GR1WI89-XC' -O- | sed -rn 's/.*confirm=([0-9A-Za-z_]+).*/\1\n/p')&id=1XkVpyYyYqQQ4FcgLIDUxg-GR1WI89-XC" -O deployment_image.tar && rm -rf /tmp/cookies.txt
#rsync -av -e ssh --exclude=*.pt --exclude=*.mp4 $PWD/../../go1_gym_deploy $PWD/../../runs $PWD/../../setup.py pi@192.168.12.1:/home/pi/go1_gym
rsync -av -e ssh --exclude=*.pt --exclude=*.mp4 $PWD/../../go1_gym_deploy $PWD/../../runs $PWD/../../setup.py unitree@192.168.123.15:/home/unitree/go1_gym
#scp -r $PWD/../../runs pi@192.168.12.1:/home/pi/go1_gym
#scp -r $PWD/../../setup.py pi@192.168.12.1:/home/pi/go1_gym
go1_gym_deploy/tests/__init__.py
View File
go1_gym_deploy/tests/check_camera_msgs.py
+154
View File
@@ -0,0 +1,154 @@
import lcm
import threading
import time
import select
import numpy as np
from go1_gym_deploy.lcm_types.leg_control_data_lcmt import leg_control_data_lcmt
from go1_gym_deploy.lcm_types.rc_command_lcmt import rc_command_lcmt
from go1_gym_deploy.lcm_types.state_estimator_lcmt import state_estimator_lcmt
from go1_gym_deploy.lcm_types.vicon_pose_lcmt import vicon_pose_lcmt
from go1_gym_deploy.lcm_types.camera_message_lcmt import camera_message_lcmt
from go1_gym_deploy.lcm_types.camera_message_rect_wide import camera_message_rect_wide
from go1_gym_deploy.lcm_types.camera_message_rect_wide_mask import camera_message_rect_wide_mask
class UnitreeLCMInspector:
def __init__(self, lc):
self.lc = lc
self.camera_names = ["front", "bottom", "left", "right", "rear"]
for cam_name in self.camera_names:
self.camera_subscription = self.lc.subscribe(f"rect_image_{cam_name}", self._rect_camera_cb)
for cam_name in self.camera_names:
self.camera_subscription = self.lc.subscribe(f"rect_image_{cam_name}_mask", self._mask_camera_cb)
self.camera_image_left = None
self.camera_image_right = None
self.camera_image_front = None
self.camera_image_bottom = None
self.camera_image_rear = None
self.ts = [time.time(), time.time(), time.time(), time.time(), time.time(),]
self.num_low_states = 0
def _rect_camera_cb(self, channel, data):
# message_types = [camera_message_rect_front, camera_message_rect_front_chin, camera_message_rect_left,
# camera_message_rect_right, camera_message_rect_rear_down]
# image_shapes = [(200, 200, 3), (100, 100, 3), (100, 232, 3), (100, 232, 3), (200, 200, 3)]
message_types = [camera_message_rect_wide, camera_message_rect_wide, camera_message_rect_wide, camera_message_rect_wide, camera_message_rect_wide]
image_shapes = [(116, 100, 3), (116, 100, 3), (116, 100, 3), (116, 100, 3), (116, 100, 3)]
cam_name = channel.split("_")[-1]
cam_id = self.camera_names.index(cam_name) + 1
print(channel, message_types[cam_id - 1])
msg = message_types[cam_id - 1].decode(data)
img = np.fromstring(msg.data, dtype=np.uint8)
img = np.flip(np.flip(
img.reshape((image_shapes[cam_id - 1][2], image_shapes[cam_id - 1][1], image_shapes[cam_id - 1][0])),
axis=0), axis=1).transpose(1, 2, 0)
if cam_id == 1:
self.camera_image_front = img
elif cam_id == 2:
self.camera_image_bottom = img
elif cam_id == 3:
self.camera_image_left = img
elif cam_id == 4:
self.camera_image_right = img
elif cam_id == 5:
self.camera_image_rear = img
else:
print("Image received from camera with unknown ID#!")
print(f"f{1. / (time.time() - self.ts[cam_id - 1])}: received py from {cam_name}!")
self.ts[cam_id-1] = time.time()
from PIL import Image
im = Image.fromarray(img)
im.save(f"{cam_name}_image.jpeg")
def _mask_camera_cb(self, channel, data):
message_types = [camera_message_rect_wide_mask for i in range(5)]
image_shapes = [(116, 100, 1) for i in range(5)]
cam_name = channel.split("_")[-2]
cam_id = self.camera_names.index(cam_name) + 1
print(channel, message_types[cam_id - 1])
msg = message_types[cam_id - 1].decode(data)
img = np.array(list(msg.data)).astype(np.uint8)
img = np.flip(np.flip(
img.reshape((image_shapes[cam_id - 1][2], image_shapes[cam_id - 1][1], image_shapes[cam_id - 1][0])),
axis=0), axis=1)
if cam_id == 1:
self.camera_image_front = img
elif cam_id == 2:
self.camera_image_bottom = img
elif cam_id == 3:
self.camera_image_left = img
elif cam_id == 4:
self.camera_image_right = img
elif cam_id == 5:
self.camera_image_rear = img
else:
print("Image received from camera with unknown ID#!")
print(f"f{1. / (time.time() - self.ts[cam_id - 1])}: received py from {cam_name}!")
self.ts[cam_id-1] = time.time()
from PIL import Image
# print(img[0])
im = Image.fromarray(img[0])
im.save(f"{cam_name}_mask_image.jpeg")
def publish_30Hz(self):
msg = camera_message_rect_wide()
msg.data = [0] * 34800
self.lc.publish("rect_image_rear", msg.encode())
time.sleep(1./30.)
def poll(self, cb=None):
t = time.time()
try:
while True:
timeout = 0.01
rfds, wfds, efds = select.select([self.lc.fileno()], [], [], timeout)
if rfds:
# print("message received!")
self.lc.handle()
# print(f'Freq {1. / (time.time() - t)} Hz'); t = time.time()
else:
continue
# print(f'waiting for message... Freq {1. / (time.time() - t)} Hz'); t = time.time()
# if cb is not None:
# cb()
except KeyboardInterrupt:
pass
def spin(self):
self.run_thread = threading.Thread(target=self.poll, daemon=False)
self.run_thread.start()
if __name__ == "__main__":
import lcm
lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=255")
#check_lcm_msgs()
print("init")
insp = UnitreeLCMInspector(lc)
print("polling")
# insp.poll()
while True:
insp.publish_30Hz()
go1_gym_deploy/unitree_legged_sdk_bin/__init__.py
View File
go1_gym_deploy/unitree_legged_sdk_bin/lcm_position
Executable
BIN
View File
Binary file not shown.
go1_gym_deploy/utils/__init__.py
View File
go1_gym_deploy/utils/cheetah_state_estimator.py
Executable
+451
View File
@@ -0,0 +1,451 @@
import math
import select
import threading
import time
import numpy as np
from go1_gym_deploy.lcm_types.leg_control_data_lcmt import leg_control_data_lcmt
from go1_gym_deploy.lcm_types.rc_command_lcmt import rc_command_lcmt
from go1_gym_deploy.lcm_types.state_estimator_lcmt import state_estimator_lcmt
from go1_gym_deploy.lcm_types.camera_message_lcmt import camera_message_lcmt
from go1_gym_deploy.lcm_types.camera_message_rect_wide import camera_message_rect_wide
def get_rpy_from_quaternion(q):
w, x, y, z = q
r = np.arctan2(2 * (w * x + y * z), 1 - 2 * (x ** 2 + y ** 2))
p = np.arcsin(2 * (w * y - z * x))
y = np.arctan2(2 * (w * z + x * y), 1 - 2 * (y ** 2 + z ** 2))
return np.array([r, p, y])
def get_rotation_matrix_from_rpy(rpy):
"""
Get rotation matrix from the given quaternion.
Args:
q (np.array[float[4]]): quaternion [w,x,y,z]
Returns:
np.array[float[3,3]]: rotation matrix.
"""
r, p, y = rpy
R_x = np.array([[1, 0, 0],
[0, math.cos(r), -math.sin(r)],
[0, math.sin(r), math.cos(r)]
])
R_y = np.array([[math.cos(p), 0, math.sin(p)],
[0, 1, 0],
[-math.sin(p), 0, math.cos(p)]
])
R_z = np.array([[math.cos(y), -math.sin(y), 0],
[math.sin(y), math.cos(y), 0],
[0, 0, 1]
])
rot = np.dot(R_z, np.dot(R_y, R_x))
return rot
class StateEstimator:
def __init__(self, lc, use_cameras=True):
# reverse legs
self.joint_idxs = [3, 4, 5, 0, 1, 2, 9, 10, 11, 6, 7, 8]
self.contact_idxs = [1, 0, 3, 2]
# self.joint_idxs = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11]
self.lc = lc
self.joint_pos = np.zeros(12)
self.joint_vel = np.zeros(12)
self.tau_est = np.zeros(12)
self.world_lin_vel = np.zeros(3)
self.world_ang_vel = np.zeros(3)
self.euler = np.zeros(3)
self.R = np.eye(3)
self.buf_idx = 0
self.smoothing_length = 12
self.deuler_history = np.zeros((self.smoothing_length, 3))
self.dt_history = np.zeros((self.smoothing_length, 1))
self.euler_prev = np.zeros(3)
self.timuprev = time.time()
self.body_lin_vel = np.zeros(3)
self.body_ang_vel = np.zeros(3)
self.smoothing_ratio = 0.2
self.contact_state = np.ones(4)
self.mode = 0
self.ctrlmode = 0
self.left_stick = [0, 0]
self.right_stick = [0, 0]
self.left_upper_switch = 0
self.left_lower_left_switch = 0
self.left_lower_right_switch = 0
self.right_upper_switch = 0
self.right_lower_left_switch = 0
self.right_lower_right_switch = 0
self.left_upper_switch_pressed = 0
self.left_lower_left_switch_pressed = 0
self.left_lower_right_switch_pressed = 0
self.right_upper_switch_pressed = 0
self.right_lower_left_switch_pressed = 0
self.right_lower_right_switch_pressed = 0
# default trotting gait
self.cmd_freq = 3.0
self.cmd_phase = 0.5
self.cmd_offset = 0.0
self.cmd_duration = 0.5
self.init_time = time.time()
self.received_first_legdata = False
self.imu_subscription = self.lc.subscribe("state_estimator_data", self._imu_cb)
self.legdata_state_subscription = self.lc.subscribe("leg_control_data", self._legdata_cb)
self.rc_command_subscription = self.lc.subscribe("rc_command", self._rc_command_cb)
if use_cameras:
for cam_id in [1, 2, 3, 4, 5]:
self.camera_subscription = self.lc.subscribe(f"camera{cam_id}", self._camera_cb)
self.camera_names = ["front", "bottom", "left", "right", "rear"]
for cam_name in self.camera_names:
self.camera_subscription = self.lc.subscribe(f"rect_image_{cam_name}", self._rect_camera_cb)
self.camera_image_left = None
self.camera_image_right = None
self.camera_image_front = None
self.camera_image_bottom = None
self.camera_image_rear = None
self.body_loc = np.array([0, 0, 0])
self.body_quat = np.array([0, 0, 0, 1])
def get_body_linear_vel(self):
self.body_lin_vel = np.dot(self.R.T, self.world_lin_vel)
return self.body_lin_vel
def get_body_angular_vel(self):
self.body_ang_vel = self.smoothing_ratio * np.mean(self.deuler_history / self.dt_history, axis=0) + (
1 - self.smoothing_ratio) * self.body_ang_vel
return self.body_ang_vel
def get_gravity_vector(self):
grav = np.dot(self.R.T, np.array([0, 0, -1]))
return grav
def get_contact_state(self):
return self.contact_state[self.contact_idxs]
def get_rpy(self):
return self.euler
def get_command(self):
MODES = ["mobility", "body_control", "footswing"]
if self.left_upper_switch_pressed:
self.ctrlmode = (self.ctrlmode + 1) % 3
self.left_upper_switch_pressed = False
print(f"PRESSED{self.ctrlmode}")
# elif self.left_lower_left_switch_pressed:
# self.ctrlmode = 0
# self.left_lower_left_switch_pressed = False
# print("PRESSED0")
MODE = MODES[self.ctrlmode]
if MODE == "mobility":
cmd_x = 1 * self.left_stick[1]
cmd_y = 0 # -1 * self.left_stick[0]
cmd_yaw = -1 * self.right_stick[0]
# cmd_height = 0.3 * self.right_stick[1]
cmd_height = 0.3 * self.left_stick[0]
cmd_footswing = 0.08 # max(0, self.left_stick[0]) * 0.27 + 0.03
cmd_stance_width = 0.33
cmd_stance_length = 0.45
cmd_ori_pitch = 0.15
cmd_ori_roll = 0
min_freq = 2.0
max_freq = 4.0
cmd_freq = (1 + self.right_stick[1]) / 2 * (max_freq - min_freq) + min_freq
elif MODE == "body_control":
cmd_x = 1 * self.left_stick[1]
cmd_y = 0 # -1 * self.left_stick[0]
cmd_yaw = -1 * self.right_stick[0]
cmd_height = 0.0
cmd_footswing = 0.10 # max(0, self.left_stick[0]) * 0.27 + 0.03
cmd_stance_width = 0.275 + 0.175 * self.left_stick[0]
cmd_stance_length = 0.45 # 0.45 + 0.25 * self.left_stick[1]
cmd_ori_pitch = -0.4 * self.right_stick[1]
cmd_ori_roll = 0.0 # 0.5 * self.right_stick[1]
cmd_freq = 3.2
else:
cmd_x = 1 * self.left_stick[1]
cmd_y = 0 # -1 * self.left_stick[0]
cmd_yaw = -1 * self.right_stick[0]
cmd_height = 0.0
cmd_footswing = max(0, self.left_stick[0]) * 0.32 + 0.03
# cmd_footswing = max(0, self.left_stick[0]) * 0.29 + 0.03
cmd_stance_width = 0.3 # + 0.175 * self.left_stick[0]
cmd_stance_length = 0.40 # + 0.15 * self.left_stick[1]
cmd_ori_pitch = 0.0 # -0.3 * self.left_stick[0]
cmd_ori_roll = 0.0 # 0.5 * self.right_stick[1]
min_freq = 2.0
max_freq = 4.0
cmd_freq = (1 + self.right_stick[1]) / 2 * (max_freq - min_freq) + min_freq
# cmd_freq = (1 + self.left_stick[0]) / 2 * (max_freq-min_freq) + min_freq
# if self.left_upper_switch_pressed:
# if self.cmd_phase == 0.5:
# self.cmd_phase = 0.0
# elif self.cmd_phase == 0.0:
# self.cmd_phase = 0.25
# else:
# self.cmd_phase = 0.5
# self.left_upper_switch_pressed = False
if self.mode == 0:
self.cmd_phase = 0.5
self.cmd_offset = 0.0
self.cmd_bound = 0.0
self.cmd_duration = 0.5
elif self.mode == 1:
self.cmd_phase = 0.0
self.cmd_offset = 0.0
self.cmd_bound = 0.0
self.cmd_duration = 0.5
elif self.mode == 2:
self.cmd_phase = 0.0
self.cmd_offset = 0.5
self.cmd_bound = 0.0
self.cmd_duration = 0.5
elif self.mode == 3:
self.cmd_phase = 0.0
self.cmd_offset = 0.0
self.cmd_bound = 0.5
self.cmd_duration = 0.5
elif self.mode == 4:
self.cmd_phase = 0.7
self.cmd_offset = 0.0
self.cmd_bound = 0.0
self.cmd_duration = 0.5
elif self.mode == 5:
self.cmd_phase = 0.3
self.cmd_offset = 0.0
self.cmd_bound = 0.0
self.cmd_duration = 0.5
elif self.mode == 6:
self.cmd_phase = 0.0
self.cmd_offset = 0.7
self.cmd_bound = 0.0
self.cmd_duration = 0.5
elif self.mode == 7:
self.cmd_phase = 0.0
self.cmd_offset = 0.3
self.cmd_bound = 0.0
self.cmd_duration = 0.5
# cmd_freq = (3.0 if not self.left_upper_switch else 2.0)
# cmd_phase = (0.5 if not self.left_lower_left_switch else 0.0)
# cmd_offset = 0.0
# cmd_duration = 0.5
# print(cmd_x, cmd_y)
return np.array([cmd_x, cmd_y, cmd_yaw, cmd_height, cmd_freq, self.cmd_phase, self.cmd_offset, self.cmd_bound,
self.cmd_duration, cmd_footswing, cmd_ori_pitch, cmd_ori_roll, cmd_stance_width,
cmd_stance_length, 0, 0, 0, 0, 0])
def get_buttons(self):
return np.array([self.left_lower_left_switch, self.left_upper_switch, self.right_lower_right_switch, self.right_upper_switch])
def get_dof_pos(self):
# print("dofposquery", self.joint_pos[self.joint_idxs])
return self.joint_pos[self.joint_idxs]
def get_dof_vel(self):
return self.joint_vel[self.joint_idxs]
def get_tau_est(self):
return self.tau_est[self.joint_idxs]
def get_yaw(self):
return self.euler[2]
def get_body_loc(self):
return np.array(self.body_loc)
def get_body_quat(self):
return np.array(self.body_quat)
def get_camera_front(self):
return self.camera_image_front
def get_camera_bottom(self):
return self.camera_image_bottom
def get_camera_rear(self):
return self.camera_image_rear
def get_camera_left(self):
return self.camera_image_left
def get_camera_right(self):
return self.camera_image_right
def _legdata_cb(self, channel, data):
# print("update legdata")
if not self.received_first_legdata:
self.received_first_legdata = True
print(f"First legdata: {time.time() - self.init_time}")
msg = leg_control_data_lcmt.decode(data)
# print(msg.q)
self.joint_pos = np.array(msg.q)
self.joint_vel = np.array(msg.qd)
self.tau_est = np.array(msg.tau_est)
# print(f"update legdata {msg.id}")
def _imu_cb(self, channel, data):
# print("update imu")
msg = state_estimator_lcmt.decode(data)
self.euler = np.array(msg.rpy)
self.R = get_rotation_matrix_from_rpy(self.euler)
self.contact_state = 1.0 * (np.array(msg.contact_estimate) > 200)
self.deuler_history[self.buf_idx % self.smoothing_length, :] = msg.rpy - self.euler_prev
self.dt_history[self.buf_idx % self.smoothing_length] = time.time() - self.timuprev
self.timuprev = time.time()
self.buf_idx += 1
self.euler_prev = np.array(msg.rpy)
def _sensor_cb(self, channel, data):
pass
def _rc_command_cb(self, channel, data):
msg = rc_command_lcmt.decode(data)
self.left_upper_switch_pressed = ((msg.left_upper_switch and not self.left_upper_switch) or self.left_upper_switch_pressed)
self.left_lower_left_switch_pressed = ((msg.left_lower_left_switch and not self.left_lower_left_switch) or self.left_lower_left_switch_pressed)
self.left_lower_right_switch_pressed = ((msg.left_lower_right_switch and not self.left_lower_right_switch) or self.left_lower_right_switch_pressed)
self.right_upper_switch_pressed = ((msg.right_upper_switch and not self.right_upper_switch) or self.right_upper_switch_pressed)
self.right_lower_left_switch_pressed = ((msg.right_lower_left_switch and not self.right_lower_left_switch) or self.right_lower_left_switch_pressed)
self.right_lower_right_switch_pressed = ((msg.right_lower_right_switch and not self.right_lower_right_switch) or self.right_lower_right_switch_pressed)
self.mode = msg.mode
self.right_stick = msg.right_stick
self.left_stick = msg.left_stick
self.left_upper_switch = msg.left_upper_switch
self.left_lower_left_switch = msg.left_lower_left_switch
self.left_lower_right_switch = msg.left_lower_right_switch
self.right_upper_switch = msg.right_upper_switch
self.right_lower_left_switch = msg.right_lower_left_switch
self.right_lower_right_switch = msg.right_lower_right_switch
# print(self.right_stick, self.left_stick)
def _camera_cb(self, channel, data):
msg = camera_message_lcmt.decode(data)
img = np.fromstring(msg.data, dtype=np.uint8)
img = img.reshape((3, 200, 464)).transpose(1, 2, 0)
cam_id = int(channel[-1])
if cam_id == 1:
self.camera_image_front = img
elif cam_id == 2:
self.camera_image_bottom = img
elif cam_id == 3:
self.camera_image_left = img
elif cam_id == 4:
self.camera_image_right = img
elif cam_id == 5:
self.camera_image_rear = img
else:
print("Image received from camera with unknown ID#!")
#im = Image.fromarray(img).convert('RGB')
#im.save("test_image_" + channel + ".jpg")
#print(channel)
def _rect_camera_cb(self, channel, data):
message_types = [camera_message_rect_wide, camera_message_rect_wide, camera_message_rect_wide,
camera_message_rect_wide, camera_message_rect_wide]
image_shapes = [(116, 100, 3), (116, 100, 3), (116, 100, 3), (116, 100, 3), (116, 100, 3)]
cam_name = channel.split("_")[-1]
# print(f"received py from {cam_name}")
cam_id = self.camera_names.index(cam_name) + 1
msg = message_types[cam_id - 1].decode(data)
img = np.fromstring(msg.data, dtype=np.uint8)
img = np.flip(np.flip(
img.reshape((image_shapes[cam_id - 1][2], image_shapes[cam_id - 1][1], image_shapes[cam_id - 1][0])),
axis=0), axis=1).transpose(1, 2, 0)
# print(img.shape)
# img = np.flip(np.flip(img.reshape(image_shapes[cam_id - 1]), axis=0), axis=1)[:, :,
# [2, 1, 0]] # .transpose(1, 2, 0)
if cam_id == 1:
self.camera_image_front = img
elif cam_id == 2:
self.camera_image_bottom = img
elif cam_id == 3:
self.camera_image_left = img
elif cam_id == 4:
self.camera_image_right = img
elif cam_id == 5:
self.camera_image_rear = img
else:
print("Image received from camera with unknown ID#!")
def poll(self, cb=None):
t = time.time()
try:
while True:
timeout = 0.01
rfds, wfds, efds = select.select([self.lc.fileno()], [], [], timeout)
if rfds:
# print("message received!")
self.lc.handle()
# print(f'Freq {1. / (time.time() - t)} Hz'); t = time.time()
else:
continue
# print(f'waiting for message... Freq {1. / (time.time() - t)} Hz'); t = time.time()
# if cb is not None:
# cb()
except KeyboardInterrupt:
pass
def spin(self):
self.run_thread = threading.Thread(target=self.poll, daemon=False)
self.run_thread.start()
def close(self):
self.lc.unsubscribe(self.legdata_state_subscription)
if __name__ == "__main__":
import lcm
lc = lcm.LCM("udpm://239.255.76.67:7667?ttl=255")
se = StateEstimator(lc)
se.poll()
go1_gym_deploy/utils/command_profile.py
+231
View File
@@ -0,0 +1,231 @@
import torch
class CommandProfile:
def __init__(self, dt, max_time_s=10.):
self.dt = dt
self.max_timestep = int(max_time_s / self.dt)
self.commands = torch.zeros((self.max_timestep, 9))
self.start_time = 0
def get_command(self, t):
timestep = int((t - self.start_time) / self.dt)
timestep = min(timestep, self.max_timestep - 1)
return self.commands[timestep, :]
def get_buttons(self):
return [0, 0, 0, 0]
def reset(self, reset_time):
self.start_time = reset_time
class ConstantAccelerationProfile(CommandProfile):
def __init__(self, dt, max_speed, accel_time, zero_buf_time=0):
super().__init__(dt)
zero_buf_timesteps = int(zero_buf_time / self.dt)
accel_timesteps = int(accel_time / self.dt)
self.commands[:zero_buf_timesteps] = 0
self.commands[zero_buf_timesteps:zero_buf_timesteps + accel_timesteps, 0] = torch.arange(0, max_speed,
step=max_speed / accel_timesteps)
self.commands[zero_buf_timesteps + accel_timesteps:, 0] = max_speed
class ElegantForwardProfile(CommandProfile):
def __init__(self, dt, max_speed, accel_time, duration, deaccel_time, zero_buf_time=0):
import numpy as np
zero_buf_timesteps = int(zero_buf_time / dt)
accel_timesteps = int(accel_time / dt)
duration_timesteps = int(duration / dt)
deaccel_timesteps = int(deaccel_time / dt)
total_time_s = zero_buf_time + accel_time + duration + deaccel_time
super().__init__(dt, total_time_s)
x_vel_cmds = [0] * zero_buf_timesteps + [*np.linspace(0, max_speed, accel_timesteps)] + \
[max_speed] * duration_timesteps + [*np.linspace(max_speed, 0, deaccel_timesteps)]
self.commands[:len(x_vel_cmds), 0] = torch.Tensor(x_vel_cmds)
class ElegantYawProfile(CommandProfile):
def __init__(self, dt, max_speed, zero_buf_time, accel_time, duration, deaccel_time, yaw_rate):
import numpy as np
zero_buf_timesteps = int(zero_buf_time / dt)
accel_timesteps = int(accel_time / dt)
duration_timesteps = int(duration / dt)
deaccel_timesteps = int(deaccel_time / dt)
total_time_s = zero_buf_time + accel_time + duration + deaccel_time
super().__init__(dt, total_time_s)
x_vel_cmds = [0] * zero_buf_timesteps + [*np.linspace(0, max_speed, accel_timesteps)] + \
[max_speed] * duration_timesteps + [*np.linspace(max_speed, 0, deaccel_timesteps)]
yaw_vel_cmds = [0] * zero_buf_timesteps + [0] * accel_timesteps + \
[yaw_rate] * duration_timesteps + [0] * deaccel_timesteps
self.commands[:len(x_vel_cmds), 0] = torch.Tensor(x_vel_cmds)
self.commands[:len(yaw_vel_cmds), 2] = torch.Tensor(yaw_vel_cmds)
class ElegantGaitProfile(CommandProfile):
def __init__(self, dt, filename):
import numpy as np
import json
with open(f'../command_profiles/{filename}', 'r') as file:
command_sequence = json.load(file)
len_command_sequence = len(command_sequence["x_vel_cmd"])
total_time_s = int(len_command_sequence / dt)
super().__init__(dt, total_time_s)
self.commands[:len_command_sequence, 0] = torch.Tensor(command_sequence["x_vel_cmd"])
self.commands[:len_command_sequence, 2] = torch.Tensor(command_sequence["yaw_vel_cmd"])
self.commands[:len_command_sequence, 3] = torch.Tensor(command_sequence["height_cmd"])
self.commands[:len_command_sequence, 4] = torch.Tensor(command_sequence["frequency_cmd"])
self.commands[:len_command_sequence, 5] = torch.Tensor(command_sequence["offset_cmd"])
self.commands[:len_command_sequence, 6] = torch.Tensor(command_sequence["phase_cmd"])
self.commands[:len_command_sequence, 7] = torch.Tensor(command_sequence["bound_cmd"])
self.commands[:len_command_sequence, 8] = torch.Tensor(command_sequence["duration_cmd"])
class RCControllerProfile(CommandProfile):
def __init__(self, dt, state_estimator, x_scale=1.0, y_scale=1.0, yaw_scale=1.0, probe_vel_multiplier=1.0):
super().__init__(dt)
self.state_estimator = state_estimator
self.x_scale = x_scale
self.y_scale = y_scale
self.yaw_scale = yaw_scale
self.probe_vel_multiplier = probe_vel_multiplier
self.triggered_commands = {i: None for i in range(4)} # command profiles for each action button on the controller
self.currently_triggered = [0, 0, 0, 0]
self.button_states = [0, 0, 0, 0]
def get_command(self, t, probe=False):
command = self.state_estimator.get_command()
command[0] = command[0] * self.x_scale
command[1] = command[1] * self.y_scale
command[2] = command[2] * self.yaw_scale
reset_timer = False
if probe:
command[0] = command[0] * self.probe_vel_multiplier
command[2] = command[2] * self.probe_vel_multiplier
# check for action buttons
prev_button_states = self.button_states[:]
self.button_states = self.state_estimator.get_buttons()
for button in range(4):
if self.triggered_commands[button] is not None:
if self.button_states[button] == 1 and prev_button_states[button] == 0:
if not self.currently_triggered[button]:
# reset the triggered action
self.triggered_commands[button].reset(t)
# reset the internal timing variable
reset_timer = True
self.currently_triggered[button] = True
else:
self.currently_triggered[button] = False
# execute the triggered action
if self.currently_triggered[button] and t < self.triggered_commands[button].max_timestep:
command = self.triggered_commands[button].get_command(t)
return command, reset_timer
def add_triggered_command(self, button_idx, command_profile):
self.triggered_commands[button_idx] = command_profile
def get_buttons(self):
return self.state_estimator.get_buttons()
class RCControllerProfileAccel(RCControllerProfile):
def __init__(self, dt, state_estimator, x_scale=1.0, y_scale=1.0, yaw_scale=1.0):
super().__init__(dt, state_estimator, x_scale=x_scale, y_scale=y_scale, yaw_scale=yaw_scale)
self.x_scale, self.y_scale, self.yaw_scale = self.x_scale / 100., self.y_scale / 100., self.yaw_scale / 100.
self.velocity_command = torch.zeros(3)
def get_command(self, t):
accel_command = self.state_estimator.get_command()
self.velocity_command[0] = self.velocity_command[0] + accel_command[0] * self.x_scale
self.velocity_command[1] = self.velocity_command[1] + accel_command[1] * self.y_scale
self.velocity_command[2] = self.velocity_command[2] + accel_command[2] * self.yaw_scale
# check for action buttons
prev_button_states = self.button_states[:]
self.button_states = self.state_estimator.get_buttons()
for button in range(4):
if self.button_states[button] == 1 and self.triggered_commands[button] is not None:
if prev_button_states[button] == 0:
# reset the triggered action
self.triggered_commands[button].reset(t)
# execute the triggered action
return self.triggered_commands[button].get_command(t)
return self.velocity_command[:]
def add_triggered_command(self, button_idx, command_profile):
self.triggered_commands[button_idx] = command_profile
def get_buttons(self):
return self.state_estimator.get_buttons()
class KeyboardProfile(CommandProfile):
# for control via keyboard inputs to isaac gym visualizer
def __init__(self, dt, isaac_env, x_scale=1.0, y_scale=1.0, yaw_scale=1.0):
super().__init__(dt)
from isaacgym.gymapi import KeyboardInput
self.gym = isaac_env.gym
self.viewer = isaac_env.viewer
self.x_scale = x_scale
self.y_scale = y_scale
self.yaw_scale = yaw_scale
self.gym.subscribe_viewer_keyboard_event(self.viewer, KeyboardInput.KEY_UP, "FORWARD")
self.gym.subscribe_viewer_keyboard_event(self.viewer, KeyboardInput.KEY_DOWN, "REVERSE")
self.gym.subscribe_viewer_keyboard_event(self.viewer, KeyboardInput.KEY_LEFT, "LEFT")
self.gym.subscribe_viewer_keyboard_event(self.viewer, KeyboardInput.KEY_RIGHT, "RIGHT")
self.keyb_command = [0, 0, 0]
self.command = [0, 0, 0]
def get_command(self, t):
events = self.gym.query_viewer_action_events(self.viewer)
events_dict = {event.action: event.value for event in events}
print(events_dict)
if "FORWARD" in events_dict and events_dict["FORWARD"] == 1.0: self.keyb_command[0] = 1.0
if "FORWARD" in events_dict and events_dict["FORWARD"] == 0.0: self.keyb_command[0] = 0.0
if "REVERSE" in events_dict and events_dict["REVERSE"] == 1.0: self.keyb_command[0] = -1.0
if "REVERSE" in events_dict and events_dict["REVERSE"] == 0.0: self.keyb_command[0] = 0.0
if "LEFT" in events_dict and events_dict["LEFT"] == 1.0: self.keyb_command[1] = 1.0
if "LEFT" in events_dict and events_dict["LEFT"] == 0.0: self.keyb_command[1] = 0.0
if "RIGHT" in events_dict and events_dict["RIGHT"] == 1.0: self.keyb_command[1] = -1.0
if "RIGHT" in events_dict and events_dict["RIGHT"] == 0.0: self.keyb_command[1] = 0.0
self.command[0] = self.keyb_command[0] * self.x_scale
self.command[1] = self.keyb_command[2] * self.y_scale
self.command[2] = self.keyb_command[1] * self.yaw_scale
print(self.command)
return self.command
if __name__ == "__main__":
cmdprof = ConstantAccelerationProfile(dt=0.2, max_speed=4, accel_time=3)
print(cmdprof.commands)
print(cmdprof.get_command(2))
go1_gym_deploy/utils/deployment_runner.py
Executable
+229
View File
@@ -0,0 +1,229 @@
import copy
import time
import os
import numpy as np
import torch
from go1_gym_deploy.utils.logger import MultiLogger
class DeploymentRunner:
def __init__(self, experiment_name="unnamed", se=None, log_root="."):
self.agents = {}
self.policy = None
self.command_profile = None
self.logger = MultiLogger()
self.se = se
self.vision_server = None
self.log_root = log_root
self.init_log_filename()
self.control_agent_name = None
self.command_agent_name = None
self.triggered_commands = {i: None for i in range(4)} # command profiles for each action button on the controller
self.button_states = np.zeros(4)
self.is_currently_probing = False
self.is_currently_logging = [False, False, False, False]
def init_log_filename(self):
datetime = time.strftime("%Y/%m_%d/%H_%M_%S")
for i in range(100):
try:
os.makedirs(f"{self.log_root}/{datetime}_{i}")
self.log_filename = f"{self.log_root}/{datetime}_{i}/log.pkl"
return
except FileExistsError:
continue
def add_open_loop_agent(self, agent, name):
self.agents[name] = agent
self.logger.add_robot(name, agent.env.cfg)
def add_control_agent(self, agent, name):
self.control_agent_name = name
self.agents[name] = agent
self.logger.add_robot(name, agent.env.cfg)
def add_vision_server(self, vision_server):
self.vision_server = vision_server
def set_command_agents(self, name):
self.command_agent = name
def add_policy(self, policy):
self.policy = policy
def add_probe_policy(self, probe_policy, probe_cfg):
self.probe_policy = probe_policy
self.probe_cfg = probe_cfg
def add_command_profile(self, command_profile):
self.command_profile = command_profile
def calibrate(self, wait=True, low=False):
# first, if the robot is not in nominal pose, move slowly to the nominal pose
for agent_name in self.agents.keys():
if hasattr(self.agents[agent_name], "get_obs"):
agent = self.agents[agent_name]
agent.get_obs()
joint_pos = agent.dof_pos
if low:
final_goal = np.array([0., 0.3, -0.7,
0., 0.3, -0.7,
0., 0.3, -0.7,
0., 0.3, -0.7,])
else:
final_goal = np.zeros(12)
nominal_joint_pos = agent.default_dof_pos
print(f"About to calibrate; the robot will stand [Press R2 to calibrate]")
while wait:
self.button_states = self.command_profile.get_buttons()
if self.command_profile.state_estimator.right_lower_right_switch_pressed:
self.command_profile.state_estimator.right_lower_right_switch_pressed = False
break
cal_action = np.zeros((agent.num_envs, agent.num_actions))
target_sequence = []
target = joint_pos - nominal_joint_pos
while np.max(np.abs(target - final_goal)) > 0.01:
target -= np.clip((target - final_goal), -0.05, 0.05)
target_sequence += [copy.deepcopy(target)]
for target in target_sequence:
next_target = target
if isinstance(agent.cfg, dict):
hip_reduction = agent.cfg["control"]["hip_scale_reduction"]
action_scale = agent.cfg["control"]["action_scale"]
else:
hip_reduction = agent.cfg.control.hip_scale_reduction
action_scale = agent.cfg.control.action_scale
next_target[[0, 3, 6, 9]] /= hip_reduction
next_target = next_target / action_scale
cal_action[:, 0:12] = next_target
agent.step(torch.from_numpy(cal_action))
agent.get_obs()
time.sleep(0.05)
print("Starting pose calibrated [Press R2 to start controller]")
while True:
self.button_states = self.command_profile.get_buttons()
if self.command_profile.state_estimator.right_lower_right_switch_pressed:
self.command_profile.state_estimator.right_lower_right_switch_pressed = False
break
for agent_name in self.agents.keys():
obs = self.agents[agent_name].reset()
if agent_name == self.control_agent_name:
control_obs = obs
return control_obs
def run(self, num_log_steps=1000000000, max_steps=100000000, logging=True):
assert self.control_agent_name is not None, "cannot deploy, runner has no control agent!"
assert self.policy is not None, "cannot deploy, runner has no policy!"
assert self.command_profile is not None, "cannot deploy, runner has no command profile!"
# TODO: add basic test for comms
for agent_name in self.agents.keys():
obs = self.agents[agent_name].reset()
if agent_name == self.control_agent_name:
control_obs = obs
control_obs = self.calibrate(wait=True)
# now, run control loop
try:
for i in range(max_steps):
policy_info = {}
if self.is_currently_probing:
action = self.probe_policy(control_obs, policy_info)
else:
action = self.policy(control_obs, policy_info)
for agent_name in self.agents.keys():
obs, ret, done, info = self.agents[agent_name].step(action)
info.update(policy_info)
info.update({"observation": obs, "reward": ret, "done": done, "timestep": i,
"time": i * self.agents[self.control_agent_name].dt, "action": action, "rpy": self.agents[self.control_agent_name].se.get_rpy(), "torques": self.agents[self.control_agent_name].torques})
if logging: self.logger.log(agent_name, info)
if agent_name == self.control_agent_name:
control_obs, control_ret, control_done, control_info = obs, ret, done, info
# bad orientation emergency stop
rpy = self.agents[self.control_agent_name].se.get_rpy()
if abs(rpy[0]) > 1.6 or abs(rpy[1]) > 1.6:
self.calibrate(wait=False, low=True)
# check for logging command
prev_button_states = self.button_states[:]
self.button_states = self.command_profile.get_buttons()
if self.command_profile.state_estimator.right_upper_switch_pressed:
if not self.is_currently_probing:
print("START LOGGING")
self.is_currently_probing = True
self.agents[self.control_agent_name].set_probing(True)
self.init_log_filename()
self.logger.reset()
else:
print("SAVE LOG")
self.is_currently_probing = False
self.agents[self.control_agent_name].set_probing(False)
# calibrate, log, and then resume control
control_obs = self.calibrate(wait=False)
self.logger.save(self.log_filename)
self.init_log_filename()
self.logger.reset()
time.sleep(1)
control_obs = self.agents[self.control_agent_name].reset()
self.command_profile.state_estimator.right_upper_switch_pressed = False
for button in range(4):
if self.command_profile.currently_triggered[button]:
if not self.is_currently_logging[button]:
print("START LOGGING")
self.is_currently_logging[button] = True
self.init_log_filename()
self.logger.reset()
else:
if self.is_currently_logging[button]:
print("SAVE LOG")
self.is_currently_logging[button] = False
# calibrate, log, and then resume control
control_obs = self.calibrate(wait=False)
self.logger.save(self.log_filename)
self.init_log_filename()
self.logger.reset()
time.sleep(1)
control_obs = self.agents[self.control_agent_name].reset()
if self.command_profile.state_estimator.right_lower_right_switch_pressed:
control_obs = self.calibrate(wait=False)
time.sleep(1)
self.command_profile.state_estimator.right_lower_right_switch_pressed = False
# self.button_states = self.command_profile.get_buttons()
while not self.command_profile.state_estimator.right_lower_right_switch_pressed:
time.sleep(0.01)
# self.button_states = self.command_profile.get_buttons()
self.command_profile.state_estimator.right_lower_right_switch_pressed = False
# finally, return to the nominal pose
control_obs = self.calibrate(wait=False)
self.logger.save(self.log_filename)
except KeyboardInterrupt:
self.logger.save(self.log_filename)
go1_gym_deploy/utils/logger.py
Executable
+79
View File
@@ -0,0 +1,79 @@
import copy
import pickle as pkl
import numpy as np
import torch
def class_to_dict(obj) -> dict:
if not hasattr(obj, "__dict__"):
return obj
result = {}
for key in dir(obj):
if key.startswith("_") or key == "terrain":
continue
element = []
val = getattr(obj, key)
if isinstance(val, list):
for item in val:
element.append(class_to_dict(item))
else:
print(key)
element = class_to_dict(val)
result[key] = element
return result
class MultiLogger:
def __init__(self):
self.loggers = {}
def add_robot(self, name, cfg):
print(name, cfg)
self.loggers[name] = EpisodeLogger(cfg)
def log(self, name, info):
self.loggers[name].log(info)
def save(self, filename):
with open(filename, 'wb') as file:
logdict = {}
for key in self.loggers.keys():
logdict[key] = [class_to_dict(self.loggers[key].cfg), self.loggers[key].infos]
pkl.dump(logdict, file)
print(f"Saved log! Number of timesteps: {[len(self.loggers[key].infos) for key in self.loggers.keys()]}; Path: {filename}")
def read_metric(self, metric, robot_name=None):
if robot_name is None:
robot_name = list(self.loggers.keys())[0]
logger = self.loggers[robot_name]
metric_arr = []
for info in logger.infos:
metric_arr += [info[metric]]
return np.array(metric_arr)
def reset(self):
for key, log in self.loggers.items():
log.reset()
class EpisodeLogger:
def __init__(self, cfg):
self.infos = []
self.cfg = cfg
def log(self, info):
for key in info.keys():
if isinstance(info[key], torch.Tensor):
info[key] = info[key].detach().cpu().numpy()
if isinstance(info[key], dict):
continue
elif "image" not in key:
info[key] = copy.deepcopy(info[key])
self.infos += [dict(info)]
def reset(self):
self.infos = []
go1_gym_deploy/utils/network_config_unitree.py
+69
View File
@@ -0,0 +1,69 @@
#!/usr/bin/env python3
from os.path import expanduser
import netifaces
import sys
import subprocess
def get_saved_interface_name():
home = expanduser("~")
name = ""
try:
with open(home + "/.cheetah_network.txt"):
name = f.read().split()[0]
except:
name = ""
return name
def get_likely_iface():
ifs = netifaces.interfaces()
print("Found {} interfaces:".format(len(ifs)))
if_to_addrs = {}
for i in ifs:
if_to_addrs[i] = []
if netifaces.AF_INET in netifaces.ifaddresses(i).keys():
for ad in netifaces.ifaddresses(i)[netifaces.AF_INET]:
if_to_addrs[i].append(ad['addr'])
for i in range(len(ifs)):
print(" [{}] : {} : {}".format(i, ifs[i], if_to_addrs[ifs[i]]))
found_10_ip = 0
selected_if = ""
for i in ifs:
match_string = "192.168.123."
if len(if_to_addrs[i]) > 0 and if_to_addrs[i][0][:len(match_string)] == match_string:
found_10_ip = found_10_ip + 1
selected_if = i
if found_10_ip == 0:
print("None of the network adapters look correct. Make sure you have set a 10.0.0.x static ip!")
return ""
elif found_10_ip == 1:
print("The adapter {} seems correct".format(selected_if))
return selected_if
else:
print("Found {} possible adapters, giving up".format(found_10_ip))
return ""
def main():
name = get_saved_interface_name()
if not name:
print("Didn't find saved interface, searching...")
name = get_likely_iface()
if not name:
sys.exit("Failed to find network adapter name")
else:
print("Found saved interface {}".format(name))
print("Setup for interface {}".format(name))
subprocess.call(['sudo', 'ifconfig', name, 'multicast'])
subprocess.call(['sudo', 'route', 'add', '-net', '224.0.0.0', 'netmask', '240.0.0.0', 'dev', name])
if __name__ == "__main__":
main()
go1_gym_learn/__init__.py
Executable
View File
go1_gym_learn/env/__init__.py
Vendored Executable
+3
View File
@@ -0,0 +1,3 @@
from .vec_env import VecEnv
go1_gym_learn/env/vec_env.py
Vendored Executable
+39
View File
@@ -0,0 +1,39 @@
# License: see [LICENSE, LICENSES/rsl_rl/LICENSE]
from abc import ABC, abstractmethod
from typing import Tuple, Union
import torch
# minimal interface of the environment
class VecEnv(ABC):
num_envs: int
num_obs: int
num_privileged_obs: int
num_actions: int
max_episode_length: int
privileged_obs_buf: torch.Tensor
obs_buf: torch.Tensor
rew_buf: torch.Tensor
reset_buf: torch.Tensor
episode_length_buf: torch.Tensor # current episode duration
extras: dict
device: torch.device
@abstractmethod
def step(self, actions: torch.Tensor) -> Tuple[
torch.Tensor, Union[torch.Tensor, None], torch.Tensor, torch.Tensor, dict]:
pass
@abstractmethod
def reset(self, env_ids: Union[list, torch.Tensor]):
pass
@abstractmethod
def get_observations(self) -> torch.Tensor:
pass
@abstractmethod
def get_privileged_observations(self) -> Union[torch.Tensor, None]:
pass
go1_gym_learn/eval_metrics/__init__.py
View File
go1_gym_learn/eval_metrics/domain_randomization.py
+148
View File
@@ -0,0 +1,148 @@
from go1_gym.envs.base.legged_robot_config import Cfg
def base_set():
# set basics
Cfg.terrain.teleport_robots = True
Cfg.terrain.border_size = 50
Cfg.terrain.num_rows = 10
Cfg.terrain.num_cols = 10
Cfg.commands.resampling_time = 1e9
Cfg.env.episode_length_s = 500
Cfg.rewards.terminal_body_height = 0.0
Cfg.rewards.use_terminal_body_height = True
def rand_regular():
Cfg.domain_rand.randomize_friction = True
Cfg.domain_rand.friction_range = [0.05, 4.5]
Cfg.domain_rand.randomize_restitution = True
Cfg.domain_rand.restitution_range = [0, 1.0]
Cfg.domain_rand.restitution = 0.5
Cfg.domain_rand.randomize_base_mass = True
Cfg.domain_rand.added_mass_range = [-1., 3.]
Cfg.domain_rand.randomize_com_displacement = True
Cfg.domain_rand.com_displacement_range = [-0.1, 0.1]
Cfg.domain_rand.randomize_motor_strength = True
Cfg.domain_rand.motor_strength_range = [0.9, 1.1]
Cfg.domain_rand.randomize_Kp_factor = False
Cfg.domain_rand.Kp_factor_range = [0.8, 1.3]
Cfg.domain_rand.randomize_Kd_factor = False
Cfg.domain_rand.Kd_factor_range = [0.5, 1.5]
Cfg.domain_rand.push_robots = False
Cfg.domain_rand.push_interval_s = 15
Cfg.domain_rand.max_push_vel_xy = 1.
def rand_large():
Cfg.domain_rand.randomize_friction = True
Cfg.domain_rand.friction_range = [0.04, 6.0]
Cfg.domain_rand.randomize_restitution = True
Cfg.domain_rand.restitution_range = [0, 1.0]
Cfg.domain_rand.restitution = 0.5
Cfg.domain_rand.randomize_base_mass = True
Cfg.domain_rand.added_mass_range = [-1.5, 4.]
Cfg.domain_rand.randomize_com_displacement = True
Cfg.domain_rand.com_displacement_range = [-0.13, 0.13]
Cfg.domain_rand.randomize_motor_strength = True
Cfg.domain_rand.motor_strength_range = [0.88, 1.12] # table 1 in RMA may have a typo
Cfg.domain_rand.randomize_Kp_factor = False
Cfg.domain_rand.Kp_factor_range = [0.8, 1.3]
Cfg.domain_rand.randomize_Kd_factor = False
Cfg.domain_rand.Kd_factor_range = [0.5, 1.5]
Cfg.domain_rand.push_robots = False
Cfg.domain_rand.push_interval_s = 15
Cfg.domain_rand.max_push_vel_xy = 1.
def static_low():
Cfg.domain_rand.randomize_friction = True
Cfg.domain_rand.friction_range = [0.05, 0.06]
Cfg.domain_rand.randomize_restitution = True
Cfg.domain_rand.restitution_range = [0, 0.01]
Cfg.domain_rand.restitution = 0.5
Cfg.domain_rand.randomize_base_mass = True
Cfg.domain_rand.added_mass_range = [-1., -0.99]
Cfg.domain_rand.randomize_com_displacement = True
Cfg.domain_rand.com_displacement_range = [-0.1, -0.09]
Cfg.domain_rand.randomize_motor_strength = True
Cfg.domain_rand.motor_strength_range = [0.9, -0.99]
Cfg.domain_rand.randomize_Kp_factor = False
Cfg.domain_rand.Kp_factor_range = [0.8, 1.3]
Cfg.domain_rand.randomize_Kd_factor = False
Cfg.domain_rand.Kd_factor_range = [0.5, 1.5]
Cfg.domain_rand.push_robots = False
Cfg.domain_rand.push_interval_s = 15
Cfg.domain_rand.max_push_vel_xy = 1.
def static_medium():
Cfg.domain_rand.randomize_friction = True
Cfg.domain_rand.friction_range = [1.0, 1.01]
Cfg.domain_rand.randomize_restitution = True
Cfg.domain_rand.restitution_range = [0.5, 0.51]
Cfg.domain_rand.restitution = 0.5
Cfg.domain_rand.randomize_base_mass = True
Cfg.domain_rand.added_mass_range = [0.0, 0.01]
Cfg.domain_rand.randomize_com_displacement = True
Cfg.domain_rand.com_displacement_range = [0.0, 0.01]
Cfg.domain_rand.randomize_motor_strength = True
Cfg.domain_rand.motor_strength_range = [1.0, 1.01]
Cfg.domain_rand.randomize_Kp_factor = False
Cfg.domain_rand.Kp_factor_range = [0.8, 1.3]
Cfg.domain_rand.randomize_Kd_factor = False
Cfg.domain_rand.Kd_factor_range = [0.5, 1.5]
Cfg.domain_rand.push_robots = False
Cfg.domain_rand.push_interval_s = 15
Cfg.domain_rand.max_push_vel_xy = 1.
def static_high():
Cfg.domain_rand.randomize_friction = True
Cfg.domain_rand.friction_range = [4.49, 4.5]
Cfg.domain_rand.randomize_restitution = True
Cfg.domain_rand.restitution_range = [0.99, 1.0]
Cfg.domain_rand.restitution = 0.5
Cfg.domain_rand.randomize_base_mass = True
Cfg.domain_rand.added_mass_range = [2.99, 3.]
Cfg.domain_rand.randomize_com_displacement = True
Cfg.domain_rand.com_displacement_range = [0.09, 0.1]
Cfg.domain_rand.randomize_motor_strength = True
Cfg.domain_rand.motor_strength_range = [1.09, 1.1]
Cfg.domain_rand.randomize_Kp_factor = False
Cfg.domain_rand.Kp_factor_range = [0.8, 1.3]
Cfg.domain_rand.randomize_Kd_factor = False
Cfg.domain_rand.Kd_factor_range = [0.5, 1.5]
Cfg.domain_rand.push_robots = False
Cfg.domain_rand.push_interval_s = 15
Cfg.domain_rand.max_push_vel_xy = 1.
def only_base_mass():
Cfg.domain_rand.randomize_friction = True
Cfg.domain_rand.friction_range = [1.0, 1.01]
Cfg.domain_rand.randomize_restitution = True
Cfg.domain_rand.restitution_range = [0.5, 0.51]
Cfg.domain_rand.restitution = 0.5
Cfg.domain_rand.randomize_base_mass = True
Cfg.domain_rand.added_mass_range = [-1, 3]
Cfg.domain_rand.randomize_com_displacement = True
Cfg.domain_rand.com_displacement_range = [0.0, 0.01]
Cfg.domain_rand.randomize_motor_strength = True
Cfg.domain_rand.motor_strength_range = [1.0, 1.01]
Cfg.domain_rand.randomize_Kp_factor = False
Cfg.domain_rand.Kp_factor_range = [0.8, 1.3]
Cfg.domain_rand.randomize_Kd_factor = False
Cfg.domain_rand.Kd_factor_range = [0.5, 1.5]
Cfg.domain_rand.push_robots = False
Cfg.domain_rand.push_interval_s = 15
Cfg.domain_rand.max_push_vel_xy = 1.
DR_SETTINGS = dict(
rand_regular=rand_regular,
rand_large=rand_large,
static_low=static_low,
static_medium=static_medium,
static_high=static_high,
only_base_mass=only_base_mass,
)
go1_gym_learn/eval_metrics/metrics.py
+99
View File
@@ -0,0 +1,99 @@
def to_numpy(fn):
def thunk(*args, **kwargs):
return fn(*args, **kwargs).cpu().numpy()
return thunk
def lin_vel_rmsd(env, actor_critic, obs):
return ((env.base_lin_vel[:, 0] - env.commands[:, 0]) ** 2).cpu() ** 0.5
def ang_vel_rmsd(env, actor_critic, obs):
return ((env.base_ang_vel[:, 2] - env.commands[:, 2]) ** 2).cpu() ** 0.5
def lin_vel_x(env, actor_critic, obs):
return env.base_lin_vel[:, 0].cpu()
def ang_vel_yaw(env, actor_critic, obs):
return env.base_ang_vel[:, 2].cpu()
def base_height(env, actor_critic, obs):
import torch
return torch.mean(env.root_states[:, 2].unsqueeze(1) - env.measured_heights, dim=1).cpu()
def max_torques(env, actor_critic, obs):
import torch
max_torque, max_torque_indices = torch.max(torch.abs(env.torques), dim=1)
return max_torque.cpu()
def power_consumption(env, actor_critic, obs):
import torch
return torch.sum(torch.multiply(env.torques, env.dof_vel), dim=1).cpu()
def CoT(env, actor_critic, obs):
# P / (mgv)
import torch
P = power_consumption(env, actor_critic, obs)
m = (env.default_body_mass + env.payloads).cpu()
g = 9.8 # m/s^2
v = torch.norm(env.base_lin_vel[:, 0:2], dim=1).cpu()
return P / (m * g * v)
def froude_number(env, actor_critic, obs):
# v^2 / (gh)
v = lin_vel_x(env, actor_critic, obs)
g = 9.8
h = 0.30
return v ** 2 / (g * h)
def adaptation_loss(env, actor_critic, obs):
import torch
if hasattr(actor_critic, "adaptation_module"):
pred = actor_critic.adaptation_module(obs["obs_history"])
target = actor_critic.env_factor_encoder(obs["privileged_obs"])
return torch.mean((pred.cpu().detach() - target.cpu().detach()) ** 2, dim=1)
def auxiliary_rewards(env, actor_critic, obs):
rewards = {}
for i in range(len(env.reward_functions)):
name = env.reward_names[i]
rew = env.reward_functions[i]() * env.reward_scales[name]
rewards[name] = rew.cpu().detach()
return rewards
def termination(env, actor_critic, obs):
return env.reset_buf.cpu().detach()
def privileged_obs(env, actor_critic, obs):
return obs["privileged_obs"].cpu().numpy()
def latents(env, actor_critic, obs):
return actor_critic.env_factor_encoder(obs["privileged_obs"]).cpu().numpy()
METRICS_FNS = {name: fn for name, fn in locals().items() if name not in ['to_numpy'] and "__" not in name}
if __name__ == '__main__':
print(*METRICS_FNS.items(), sep="\n")
import torch
env = lambda: None
env.base_lin_vel = torch.rand(10, 3)
env.commands = torch.rand(10, 3)
metric = lin_vel_rmsd(env, None, None)
print(metric)
go1_gym_learn/ppo/__init__.py
Executable
+298
View File
@@ -0,0 +1,298 @@
# License: see [LICENSE, LICENSES/rsl_rl/LICENSE]
import time
from collections import deque
import torch
from ml_logger import logger
from params_proto import PrefixProto
import os
import copy
from .actor_critic import ActorCritic
from .rollout_storage import RolloutStorage
from go1_gym import MINI_GYM_ROOT_DIR
def class_to_dict(obj) -> dict:
if not hasattr(obj, "__dict__"):
return obj
result = {}
for key in dir(obj):
if key.startswith("_") or key == "terrain":
continue
element = []
val = getattr(obj, key)
if isinstance(val, list):
for item in val:
element.append(class_to_dict(item))
else:
element = class_to_dict(val)
result[key] = element
return result
class DataCaches:
def __init__(self, curriculum_bins):
from go1_gym_learn.ppo.metrics_caches import DistCache, SlotCache
self.slot_cache = SlotCache(curriculum_bins)
self.dist_cache = DistCache()
caches = DataCaches(1)
class RunnerArgs(PrefixProto, cli=False):
# runner
algorithm_class_name = 'PPO'
num_steps_per_env = 24 # per iteration
max_iterations = 1500 # number of policy updates
# logging
save_interval = 400 # check for potential saves every this many iterations
save_video_interval = 100
log_freq = 10
# load and resume
resume = False
load_run = -1 # -1 = last run
checkpoint = -1 # -1 = last saved model
resume_path = None # updated from load_run and chkpt
class Runner:
def __init__(self, env, device='cpu'):
from .ppo import PPO
self.device = device
self.env = env
actor_critic = ActorCritic(self.env.num_obs,
self.env.num_privileged_obs,
self.env.num_obs_history,
self.env.num_actions,
).to(self.device)
self.alg = PPO(actor_critic, device=self.device)
self.num_steps_per_env = RunnerArgs.num_steps_per_env
# init storage and model
self.alg.init_storage(self.env.num_train_envs, self.num_steps_per_env, [self.env.num_obs],
[self.env.num_privileged_obs], [self.env.num_obs_history], [self.env.num_actions])
self.tot_timesteps = 0
self.tot_time = 0
self.current_learning_iteration = 0
self.last_recording_it = 0
self.env.reset()
def learn(self, num_learning_iterations, init_at_random_ep_len=False, eval_freq=100, eval_expert=False):
from ml_logger import logger
# initialize writer
assert logger.prefix, "you will overwrite the entire instrument server"
logger.start('start', 'epoch', 'episode', 'run', 'step')
if init_at_random_ep_len:
self.env.episode_length_buf = torch.randint_like(self.env.episode_length_buf,
high=int(self.env.max_episode_length))
# split train and test envs
num_train_envs = self.env.num_train_envs
obs_dict = self.env.get_observations()
obs, privileged_obs, obs_history = obs_dict["obs"], obs_dict["privileged_obs"], obs_dict["obs_history"]
obs, privileged_obs, obs_history = obs.to(self.device), privileged_obs.to(self.device), obs_history.to(
self.device)
self.alg.actor_critic.train()
rewbuffer = deque(maxlen=100)
lenbuffer = deque(maxlen=100)
rewbuffer_eval = deque(maxlen=100)
lenbuffer_eval = deque(maxlen=100)
cur_reward_sum = torch.zeros(self.env.num_envs, dtype=torch.float, device=self.device)
cur_episode_length = torch.zeros(self.env.num_envs, dtype=torch.float, device=self.device)
if hasattr(self.env, "curriculum"):
caches.__init__(curriculum_bins=len(self.env.curriculum))
tot_iter = self.current_learning_iteration + num_learning_iterations
for it in range(self.current_learning_iteration, tot_iter):
start = time.time()
# Rollout
with torch.inference_mode():
for i in range(self.num_steps_per_env):
actions_train = self.alg.act(obs[:num_train_envs], privileged_obs[:num_train_envs],
obs_history[:num_train_envs])
if eval_expert:
actions_eval = self.alg.actor_critic.act_teacher(obs[num_train_envs:],
privileged_obs[num_train_envs:])
else:
actions_eval = self.alg.actor_critic.act_student(obs[num_train_envs:],
obs_history[num_train_envs:])
ret = self.env.step(torch.cat((actions_train, actions_eval), dim=0))
obs_dict, rewards, dones, infos = ret
obs, privileged_obs, obs_history = obs_dict["obs"], obs_dict["privileged_obs"], obs_dict[
"obs_history"]
obs, privileged_obs, obs_history, rewards, dones = obs.to(self.device), privileged_obs.to(
self.device), obs_history.to(self.device), rewards.to(self.device), dones.to(self.device)
self.alg.process_env_step(rewards[:num_train_envs], dones[:num_train_envs], infos)
if 'train/episode' in infos:
with logger.Prefix(metrics="train/episode"):
logger.store_metrics(**infos['train/episode'])
if 'eval/episode' in infos:
with logger.Prefix(metrics="eval/episode"):
logger.store_metrics(**infos['eval/episode'])
if 'curriculum' in infos:
curr_bins_train = infos['curriculum']['reset_train_env_bins']
curr_bins_eval = infos['curriculum']['reset_eval_env_bins']
caches.slot_cache.log(curr_bins_train, **{
k.split("/", 1)[-1]: v for k, v in infos['curriculum'].items()
if k.startswith('slot/train')
})
caches.slot_cache.log(curr_bins_eval, **{
k.split("/", 1)[-1]: v for k, v in infos['curriculum'].items()
if k.startswith('slot/eval')
})
caches.dist_cache.log(**{
k.split("/", 1)[-1]: v for k, v in infos['curriculum'].items()
if k.startswith('dist/train')
})
caches.dist_cache.log(**{
k.split("/", 1)[-1]: v for k, v in infos['curriculum'].items()
if k.startswith('dist/eval')
})
cur_reward_sum += rewards
cur_episode_length += 1
new_ids = (dones > 0).nonzero(as_tuple=False)
new_ids_train = new_ids[new_ids < num_train_envs]
rewbuffer.extend(cur_reward_sum[new_ids_train].cpu().numpy().tolist())
lenbuffer.extend(cur_episode_length[new_ids_train].cpu().numpy().tolist())
cur_reward_sum[new_ids_train] = 0
cur_episode_length[new_ids_train] = 0
new_ids_eval = new_ids[new_ids >= num_train_envs]
rewbuffer_eval.extend(cur_reward_sum[new_ids_eval].cpu().numpy().tolist())
lenbuffer_eval.extend(cur_episode_length[new_ids_eval].cpu().numpy().tolist())
cur_reward_sum[new_ids_eval] = 0
cur_episode_length[new_ids_eval] = 0
# Learning step
self.alg.compute_returns(obs[:num_train_envs], privileged_obs[:num_train_envs])
if it % eval_freq == 0:
self.env.reset_evaluation_envs()
if it % eval_freq == 0:
logger.save_pkl({"iteration": it,
**caches.slot_cache.get_summary(),
**caches.dist_cache.get_summary()},
path=f"curriculum/info.pkl", append=True)
mean_value_loss, mean_surrogate_loss, mean_adaptation_module_loss = self.alg.update()
logger.store_metrics(
time_elapsed=logger.since('start'),
time_iter=logger.split('epoch'),
adaptation_loss=mean_adaptation_module_loss,
mean_value_loss=mean_value_loss,
mean_surrogate_loss=mean_surrogate_loss
)
if RunnerArgs.save_video_interval:
self.log_video(it)
self.tot_timesteps += self.num_steps_per_env * self.env.num_envs
if logger.every(RunnerArgs.log_freq, "iteration", start_on=1):
# if it % Config.log_freq == 0:
logger.log_metrics_summary(key_values={"timesteps": self.tot_timesteps, "iterations": it})
logger.job_running()
if it % RunnerArgs.save_interval == 0:
with logger.Sync():
logger.torch_save(self.alg.actor_critic.state_dict(), f"checkpoints/ac_weights_{it:06d}.pt")
logger.duplicate(f"checkpoints/ac_weights_{it:06d}.pt", f"checkpoints/ac_weights_last.pt")
path = f'{MINI_GYM_ROOT_DIR}/tmp/legged_data'
os.makedirs(path, exist_ok=True)
adaptation_module_path = f'{path}/adaptation_module_latest.jit'
adaptation_module = copy.deepcopy(self.alg.actor_critic.adaptation_module).to('cpu')
traced_script_adaptation_module = torch.jit.script(adaptation_module)
traced_script_adaptation_module.save(adaptation_module_path)
body_path = f'{path}/body_latest.jit'
body_model = copy.deepcopy(self.alg.actor_critic.actor_body).to('cpu')
traced_script_body_module = torch.jit.script(body_model)
traced_script_body_module.save(body_path)
logger.upload_file(file_path=adaptation_module_path, target_path=f"checkpoints/", once=False)
logger.upload_file(file_path=body_path, target_path=f"checkpoints/", once=False)
self.current_learning_iteration += num_learning_iterations
with logger.Sync():
logger.torch_save(self.alg.actor_critic.state_dict(), f"checkpoints/ac_weights_{it:06d}.pt")
logger.duplicate(f"checkpoints/ac_weights_{it:06d}.pt", f"checkpoints/ac_weights_last.pt")
path = f'{MINI_GYM_ROOT_DIR}/tmp/legged_data'
os.makedirs(path, exist_ok=True)
adaptation_module_path = f'{path}/adaptation_module_latest.jit'
adaptation_module = copy.deepcopy(self.alg.actor_critic.adaptation_module).to('cpu')
traced_script_adaptation_module = torch.jit.script(adaptation_module)
traced_script_adaptation_module.save(adaptation_module_path)
body_path = f'{path}/body_latest.jit'
body_model = copy.deepcopy(self.alg.actor_critic.actor_body).to('cpu')
traced_script_body_module = torch.jit.script(body_model)
traced_script_body_module.save(body_path)
logger.upload_file(file_path=adaptation_module_path, target_path=f"checkpoints/", once=False)
logger.upload_file(file_path=body_path, target_path=f"checkpoints/", once=False)
def log_video(self, it):
if it - self.last_recording_it >= RunnerArgs.save_video_interval:
self.env.start_recording()
if self.env.num_eval_envs > 0:
self.env.start_recording_eval()
print("START RECORDING")
self.last_recording_it = it
frames = self.env.get_complete_frames()
if len(frames) > 0:
self.env.pause_recording()
print("LOGGING VIDEO")
logger.save_video(frames, f"videos/{it:05d}.mp4", fps=1 / self.env.dt)
if self.env.num_eval_envs > 0:
frames = self.env.get_complete_frames_eval()
if len(frames) > 0:
self.env.pause_recording_eval()
print("LOGGING EVAL VIDEO")
logger.save_video(frames, f"videos/{it:05d}_eval.mp4", fps=1 / self.env.dt)
def get_inference_policy(self, device=None):
self.alg.actor_critic.eval()
if device is not None:
self.alg.actor_critic.to(device)
return self.alg.actor_critic.act_inference
def get_expert_policy(self, device=None):
self.alg.actor_critic.eval()
if device is not None:
self.alg.actor_critic.to(device)
return self.alg.actor_critic.act_expert
go1_gym_learn/ppo/actor_critic.py
Executable
+193
View File
@@ -0,0 +1,193 @@
# License: see [LICENSE, LICENSES/rsl_rl/LICENSE]
import torch
import torch.nn as nn
from params_proto import PrefixProto
from torch.distributions import Normal
class AC_Args(PrefixProto, cli=False):
# policy
init_noise_std = 1.0
actor_hidden_dims = [512, 256, 128]
critic_hidden_dims = [512, 256, 128]
activation = 'elu' # can be elu, relu, selu, crelu, lrelu, tanh, sigmoid
adaptation_module_branch_hidden_dims = [[256, 32]]
env_factor_encoder_branch_input_dims = [18]
env_factor_encoder_branch_latent_dims = [18]
env_factor_encoder_branch_hidden_dims = [[256, 128]]
class ActorCritic(nn.Module):
is_recurrent = False
def __init__(self, num_obs,
num_privileged_obs,
num_obs_history,
num_actions,
**kwargs):
if kwargs:
print("ActorCritic.__init__ got unexpected arguments, which will be ignored: " + str(
[key for key in kwargs.keys()]))
super().__init__()
activation = get_activation(AC_Args.activation)
for i, (branch_input_dim, branch_hidden_dims, branch_latent_dim) in enumerate(
zip(AC_Args.env_factor_encoder_branch_input_dims,
AC_Args.env_factor_encoder_branch_hidden_dims,
AC_Args.env_factor_encoder_branch_latent_dims)):
# Env factor encoder
env_factor_encoder_layers = []
env_factor_encoder_layers.append(nn.Linear(branch_input_dim, branch_hidden_dims[0]))
env_factor_encoder_layers.append(activation)
for l in range(len(branch_hidden_dims)):
if l == len(branch_hidden_dims) - 1:
env_factor_encoder_layers.append(
nn.Linear(branch_hidden_dims[l], branch_latent_dim))
else:
env_factor_encoder_layers.append(
nn.Linear(branch_hidden_dims[l],
branch_hidden_dims[l + 1]))
env_factor_encoder_layers.append(activation)
self.env_factor_encoder = nn.Sequential(*env_factor_encoder_layers)
self.add_module(f"encoder", self.env_factor_encoder)
# Adaptation module
for i, (branch_hidden_dims, branch_latent_dim) in enumerate(zip(AC_Args.adaptation_module_branch_hidden_dims,
AC_Args.env_factor_encoder_branch_latent_dims)):
adaptation_module_layers = []
adaptation_module_layers.append(nn.Linear(num_obs_history, branch_hidden_dims[0]))
adaptation_module_layers.append(activation)
for l in range(len(branch_hidden_dims)):
if l == len(branch_hidden_dims) - 1:
adaptation_module_layers.append(
nn.Linear(branch_hidden_dims[l], branch_latent_dim))
else:
adaptation_module_layers.append(
nn.Linear(branch_hidden_dims[l],
branch_hidden_dims[l + 1]))
adaptation_module_layers.append(activation)
self.adaptation_module = nn.Sequential(*adaptation_module_layers)
self.add_module(f"adaptation_module", self.adaptation_module)
total_latent_dim = int(torch.sum(torch.Tensor(AC_Args.env_factor_encoder_branch_latent_dims)))
# Policy
actor_layers = []
actor_layers.append(nn.Linear(total_latent_dim + num_obs, AC_Args.actor_hidden_dims[0]))
actor_layers.append(activation)
for l in range(len(AC_Args.actor_hidden_dims)):
if l == len(AC_Args.actor_hidden_dims) - 1:
actor_layers.append(nn.Linear(AC_Args.actor_hidden_dims[l], num_actions))
else:
actor_layers.append(nn.Linear(AC_Args.actor_hidden_dims[l], AC_Args.actor_hidden_dims[l + 1]))
actor_layers.append(activation)
self.actor_body = nn.Sequential(*actor_layers)
# Value function
critic_layers = []
critic_layers.append(nn.Linear(total_latent_dim + num_obs, AC_Args.critic_hidden_dims[0]))
critic_layers.append(activation)
for l in range(len(AC_Args.critic_hidden_dims)):
if l == len(AC_Args.critic_hidden_dims) - 1:
critic_layers.append(nn.Linear(AC_Args.critic_hidden_dims[l], 1))
else:
critic_layers.append(nn.Linear(AC_Args.critic_hidden_dims[l], AC_Args.critic_hidden_dims[l + 1]))
critic_layers.append(activation)
self.critic_body = nn.Sequential(*critic_layers)
print(f"Environment Factor Encoder: {self.env_factor_encoder}")
print(f"Adaptation Module: {self.adaptation_module}")
print(f"Actor MLP: {self.actor_body}")
print(f"Critic MLP: {self.critic_body}")
# Action noise
self.std = nn.Parameter(AC_Args.init_noise_std * torch.ones(num_actions))
self.distribution = None
# disable args validation for speedup
Normal.set_default_validate_args = False
@staticmethod
# not used at the moment
def init_weights(sequential, scales):
[torch.nn.init.orthogonal_(module.weight, gain=scales[idx]) for idx, module in
enumerate(mod for mod in sequential if isinstance(mod, nn.Linear))]
def reset(self, dones=None):
pass
def forward(self):
raise NotImplementedError
@property
def action_mean(self):
return self.distribution.mean
@property
def action_std(self):
return self.distribution.stddev
@property
def entropy(self):
return self.distribution.entropy().sum(dim=-1)
def update_distribution(self, observations, privileged_observations):
latent = self.env_factor_encoder(privileged_observations)
mean = self.actor_body(torch.cat((observations, latent), dim=-1))
self.distribution = Normal(mean, mean * 0. + self.std)
def act(self, observations, privileged_observations, **kwargs):
self.update_distribution(observations, privileged_observations)
return self.distribution.sample()
def get_actions_log_prob(self, actions):
return self.distribution.log_prob(actions).sum(dim=-1)
def act_expert(self, ob, policy_info={}):
return self.act_teacher(ob["obs"], ob["privileged_obs"])
def act_inference(self, ob, policy_info={}):
if ob["privileged_obs"] is not None:
gt_latent = self.env_factor_encoder(ob["privileged_obs"])
policy_info["gt_latents"] = gt_latent.detach().cpu().numpy()
return self.act_student(ob["obs"], ob["obs_history"])
def act_student(self, observations, observation_history, policy_info={}):
latent = self.adaptation_module(observation_history)
actions_mean = self.actor_body(torch.cat((observations, latent), dim=-1))
policy_info["latents"] = latent.detach().cpu().numpy()
return actions_mean
def act_teacher(self, observations, privileged_info, policy_info={}):
latent = self.env_factor_encoder(privileged_info)
actions_mean = self.actor_body(torch.cat((observations, latent), dim=-1))
policy_info["latents"] = latent.detach().cpu().numpy()
return actions_mean
def evaluate(self, critic_observations, privileged_observations, **kwargs):
latent = self.env_factor_encoder(privileged_observations)
value = self.critic_body(torch.cat((critic_observations, latent), dim=-1))
return value
def get_activation(act_name):
if act_name == "elu":
return nn.ELU()
elif act_name == "selu":
return nn.SELU()
elif act_name == "relu":
return nn.ReLU()
elif act_name == "crelu":
return nn.ReLU()
elif act_name == "lrelu":
return nn.LeakyReLU()
elif act_name == "tanh":
return nn.Tanh()
elif act_name == "sigmoid":
return nn.Sigmoid()
else:
print("invalid activation function!")
return None
go1_gym_learn/ppo/metrics_caches.py
+88
View File
@@ -0,0 +1,88 @@
from collections import defaultdict
import numpy as np
class DistCache:
def __init__(self):
"""
Args:
n: Number of slots for the cache
"""
self.cache = defaultdict(lambda: 0)
def log(self, **key_vals):
"""
Args:
slots: ids for the array
**key_vals:
"""
for k, v in key_vals.items():
count = self.cache[k + '@counts'] + 1
self.cache[k + '@counts'] = count
self.cache[k] = v + (count - 1) * self.cache[k]
self.cache[k] /= count
def get_summary(self):
ret = {
k: v
for k, v in self.cache.items()
if not k.endswith("@counts")
}
self.cache.clear()
return ret
if __name__ == '__main__':
cl = DistCache()
lin_vel = np.ones((11, 11))
ang_vel = np.zeros((5, 5))
cl.log(lin_vel=lin_vel, ang_vel=ang_vel)
lin_vel = np.zeros((11, 11))
ang_vel = np.zeros((5, 5))
cl.log(lin_vel=lin_vel, ang_vel=ang_vel)
print(cl.get_summary())
class SlotCache:
def __init__(self, n):
"""
Args:
n: Number of slots for the cache
"""
self.n = n
self.cache = defaultdict(lambda: np.zeros([n]))
def log(self, slots=None, **key_vals):
"""
Args:
slots: ids for the array
**key_vals:
"""
if slots is None:
slots = range(self.n)
for k, v in key_vals.items():
counts = self.cache[k + '@counts'][slots] + 1
self.cache[k + '@counts'][slots] = counts
self.cache[k][slots] = v + (counts - 1) * self.cache[k][slots]
self.cache[k][slots] /= counts
def get_summary(self):
ret = {
k: v
for k, v in self.cache.items()
if not k.endswith("@counts")
}
self.cache.clear()
return ret
if __name__ == '__main__':
cl = SlotCache(100)
reset_env_ids = [2, 5, 6]
lin_vel = [0.1, 0.5, 0.8]
ang_vel = [0.4, -0.4, 0.2]
cl.log(reset_env_ids, lin_vel=lin_vel, ang_vel=ang_vel)
cl.log(lin_vel=np.ones(100))
go1_gym_learn/ppo/ppo.py
Executable
+178
View File
@@ -0,0 +1,178 @@
# License: see [LICENSE, LICENSES/rsl_rl/LICENSE]
import torch
import torch.nn as nn
import torch.optim as optim
import torch.nn.functional as F
import numpy as np
from params_proto import PrefixProto
from go1_gym_learn.ppo import ActorCritic
from go1_gym_learn.ppo import RolloutStorage
from go1_gym_learn.ppo import caches
class PPO_Args(PrefixProto):
# algorithm
value_loss_coef = 1.0
use_clipped_value_loss = True
clip_param = 0.2
entropy_coef = 0.01
num_learning_epochs = 5
num_mini_batches = 4 # mini batch size = num_envs*nsteps / nminibatches
learning_rate = 1.e-3 # 5.e-4
adaptation_module_learning_rate = 1.e-3
num_adaptation_module_substeps = 1
schedule = 'adaptive' # could be adaptive, fixed
gamma = 0.99
lam = 0.95
desired_kl = 0.01
max_grad_norm = 1.
class PPO:
actor_critic: ActorCritic
def __init__(self, actor_critic, device='cpu'):
self.device = device
# PPO components
self.actor_critic = actor_critic
self.actor_critic.to(device)
self.storage = None # initialized later
self.optimizer = optim.Adam(self.actor_critic.parameters(), lr=PPO_Args.learning_rate)
self.adaptation_module_optimizer = optim.Adam(self.actor_critic.parameters(),
lr=PPO_Args.adaptation_module_learning_rate)
self.transition = RolloutStorage.Transition()
self.learning_rate = PPO_Args.learning_rate
def init_storage(self, num_envs, num_transitions_per_env, actor_obs_shape, privileged_obs_shape, obs_history_shape,
action_shape):
self.storage = RolloutStorage(num_envs, num_transitions_per_env, actor_obs_shape, privileged_obs_shape,
obs_history_shape, action_shape, self.device)
def test_mode(self):
self.actor_critic.test()
def train_mode(self):
self.actor_critic.train()
def act(self, obs, privileged_obs, obs_history):
# Compute the actions and values
self.transition.actions = self.actor_critic.act(obs, privileged_obs).detach()
self.transition.values = self.actor_critic.evaluate(obs, privileged_obs).detach()
self.transition.actions_log_prob = self.actor_critic.get_actions_log_prob(self.transition.actions).detach()
self.transition.action_mean = self.actor_critic.action_mean.detach()
self.transition.action_sigma = self.actor_critic.action_std.detach()
# need to record obs and critic_obs before env.step()
self.transition.observations = obs
self.transition.critic_observations = obs
self.transition.privileged_observations = privileged_obs
self.transition.observation_histories = obs_history
return self.transition.actions
def process_env_step(self, rewards, dones, infos):
self.transition.rewards = rewards.clone()
self.transition.dones = dones
self.transition.env_bins = infos["env_bins"]
# Bootstrapping on time outs
if 'time_outs' in infos:
self.transition.rewards += PPO_Args.gamma * torch.squeeze(
self.transition.values * infos['time_outs'].unsqueeze(1).to(self.device), 1)
# Record the transition
self.storage.add_transitions(self.transition)
self.transition.clear()
self.actor_critic.reset(dones)
def compute_returns(self, last_critic_obs, last_critic_privileged_obs):
last_values = self.actor_critic.evaluate(last_critic_obs, last_critic_privileged_obs).detach()
self.storage.compute_returns(last_values, PPO_Args.gamma, PPO_Args.lam)
def update(self):
mean_value_loss = 0
mean_surrogate_loss = 0
mean_adaptation_module_loss = 0
generator = self.storage.mini_batch_generator(PPO_Args.num_mini_batches, PPO_Args.num_learning_epochs)
for obs_batch, critic_obs_batch, privileged_obs_batch, obs_history_batch, actions_batch, target_values_batch, advantages_batch, returns_batch, old_actions_log_prob_batch, \
old_mu_batch, old_sigma_batch, masks_batch, env_bins_batch in generator:
self.actor_critic.act(obs_batch, privileged_obs_batch, masks=masks_batch)
actions_log_prob_batch = self.actor_critic.get_actions_log_prob(actions_batch)
value_batch = self.actor_critic.evaluate(critic_obs_batch, privileged_obs_batch, masks=masks_batch)
mu_batch = self.actor_critic.action_mean
sigma_batch = self.actor_critic.action_std
entropy_batch = self.actor_critic.entropy
# KL
if PPO_Args.desired_kl != None and PPO_Args.schedule == 'adaptive':
with torch.inference_mode():
kl = torch.sum(
torch.log(sigma_batch / old_sigma_batch + 1.e-5) + (
torch.square(old_sigma_batch) + torch.square(old_mu_batch - mu_batch)) / (
2.0 * torch.square(sigma_batch)) - 0.5, axis=-1)
kl_mean = torch.mean(kl)
if kl_mean > PPO_Args.desired_kl * 2.0:
self.learning_rate = max(1e-5, self.learning_rate / 1.5)
elif kl_mean < PPO_Args.desired_kl / 2.0 and kl_mean > 0.0:
self.learning_rate = min(1e-2, self.learning_rate * 1.5)
for param_group in self.optimizer.param_groups:
param_group['lr'] = self.learning_rate
# Surrogate loss
ratio = torch.exp(actions_log_prob_batch - torch.squeeze(old_actions_log_prob_batch))
surrogate = -torch.squeeze(advantages_batch) * ratio
surrogate_clipped = -torch.squeeze(advantages_batch) * torch.clamp(ratio, 1.0 - PPO_Args.clip_param,
1.0 + PPO_Args.clip_param)
surrogate_loss = torch.max(surrogate, surrogate_clipped).mean()
# Value function loss
if PPO_Args.use_clipped_value_loss:
value_clipped = target_values_batch + \
(value_batch - target_values_batch).clamp(-PPO_Args.clip_param,
PPO_Args.clip_param)
value_losses = (value_batch - returns_batch).pow(2)
value_losses_clipped = (value_clipped - returns_batch).pow(2)
value_loss = torch.max(value_losses, value_losses_clipped).mean()
else:
value_loss = (returns_batch - value_batch).pow(2).mean()
loss = surrogate_loss + PPO_Args.value_loss_coef * value_loss - PPO_Args.entropy_coef * entropy_batch.mean()
# Gradient step
self.optimizer.zero_grad()
loss.backward()
nn.utils.clip_grad_norm_(self.actor_critic.parameters(), PPO_Args.max_grad_norm)
self.optimizer.step()
mean_value_loss += value_loss.item()
mean_surrogate_loss += surrogate_loss.item()
# Adaptation module gradient step
for epoch in range(PPO_Args.num_adaptation_module_substeps):
adaptation_pred = self.actor_critic.adaptation_module(obs_history_batch)
with torch.no_grad():
adaptation_target = self.actor_critic.env_factor_encoder(privileged_obs_batch)
residual = (adaptation_target - adaptation_pred).norm(dim=1)
caches.slot_cache.log(env_bins_batch[:, 0].cpu().numpy().astype(np.uint8),
sysid_residual=residual.cpu().numpy())
adaptation_loss = F.mse_loss(adaptation_pred, adaptation_target)
self.adaptation_module_optimizer.zero_grad()
adaptation_loss.backward()
self.adaptation_module_optimizer.step()
mean_adaptation_module_loss += adaptation_loss.item()
num_updates = PPO_Args.num_learning_epochs * PPO_Args.num_mini_batches
mean_value_loss /= num_updates
mean_surrogate_loss /= num_updates
mean_adaptation_module_loss /= (num_updates * PPO_Args.num_adaptation_module_substeps)
self.storage.clear()
return mean_value_loss, mean_surrogate_loss, mean_adaptation_module_loss
go1_gym_learn/ppo/rollout_storage.py
Executable
+180
View File
@@ -0,0 +1,180 @@
# License: see [LICENSE, LICENSES/rsl_rl/LICENSE]
import torch
from go1_gym_learn.utils import split_and_pad_trajectories
class RolloutStorage:
class Transition:
def __init__(self):
self.observations = None
self.privileged_observations = None
self.observation_histories = None
self.critic_observations = None
self.actions = None
self.rewards = None
self.dones = None
self.values = None
self.actions_log_prob = None
self.action_mean = None
self.action_sigma = None
self.env_bins = None
def clear(self):
self.__init__()
def __init__(self, num_envs, num_transitions_per_env, obs_shape, privileged_obs_shape, obs_history_shape, actions_shape, device='cpu'):
self.device = device
self.obs_shape = obs_shape
self.privileged_obs_shape = privileged_obs_shape
self.obs_history_shape = obs_history_shape
self.actions_shape = actions_shape
# Core
self.observations = torch.zeros(num_transitions_per_env, num_envs, *obs_shape, device=self.device)
self.privileged_observations = torch.zeros(num_transitions_per_env, num_envs, *privileged_obs_shape, device=self.device)
self.observation_histories = torch.zeros(num_transitions_per_env, num_envs, *obs_history_shape, device=self.device)
self.rewards = torch.zeros(num_transitions_per_env, num_envs, 1, device=self.device)
self.actions = torch.zeros(num_transitions_per_env, num_envs, *actions_shape, device=self.device)
self.dones = torch.zeros(num_transitions_per_env, num_envs, 1, device=self.device).byte()
# For PPO
self.actions_log_prob = torch.zeros(num_transitions_per_env, num_envs, 1, device=self.device)
self.values = torch.zeros(num_transitions_per_env, num_envs, 1, device=self.device)
self.returns = torch.zeros(num_transitions_per_env, num_envs, 1, device=self.device)
self.advantages = torch.zeros(num_transitions_per_env, num_envs, 1, device=self.device)
self.mu = torch.zeros(num_transitions_per_env, num_envs, *actions_shape, device=self.device)
self.sigma = torch.zeros(num_transitions_per_env, num_envs, *actions_shape, device=self.device)
self.env_bins = torch.zeros(num_transitions_per_env, num_envs, 1, device=self.device)
self.num_transitions_per_env = num_transitions_per_env
self.num_envs = num_envs
self.step = 0
def add_transitions(self, transition: Transition):
if self.step >= self.num_transitions_per_env:
raise AssertionError("Rollout buffer overflow")
self.observations[self.step].copy_(transition.observations)
self.privileged_observations[self.step].copy_(transition.privileged_observations)
self.observation_histories[self.step].copy_(transition.observation_histories)
self.actions[self.step].copy_(transition.actions)
self.rewards[self.step].copy_(transition.rewards.view(-1, 1))
self.dones[self.step].copy_(transition.dones.view(-1, 1))
self.values[self.step].copy_(transition.values)
self.actions_log_prob[self.step].copy_(transition.actions_log_prob.view(-1, 1))
self.mu[self.step].copy_(transition.action_mean)
self.sigma[self.step].copy_(transition.action_sigma)
self.env_bins[self.step].copy_(transition.env_bins.view(-1, 1))
self.step += 1
def clear(self):
self.step = 0
def compute_returns(self, last_values, gamma, lam):
advantage = 0
for step in reversed(range(self.num_transitions_per_env)):
if step == self.num_transitions_per_env - 1:
next_values = last_values
else:
next_values = self.values[step + 1]
next_is_not_terminal = 1.0 - self.dones[step].float()
delta = self.rewards[step] + next_is_not_terminal * gamma * next_values - self.values[step]
advantage = delta + next_is_not_terminal * gamma * lam * advantage
self.returns[step] = advantage + self.values[step]
# Compute and normalize the advantages
self.advantages = self.returns - self.values
self.advantages = (self.advantages - self.advantages.mean()) / (self.advantages.std() + 1e-8)
def get_statistics(self):
done = self.dones
done[-1] = 1
flat_dones = done.permute(1, 0, 2).reshape(-1, 1)
done_indices = torch.cat((flat_dones.new_tensor([-1], dtype=torch.int64), flat_dones.nonzero(as_tuple=False)[:, 0]))
trajectory_lengths = (done_indices[1:] - done_indices[:-1])
return trajectory_lengths.float().mean(), self.rewards.mean()
def mini_batch_generator(self, num_mini_batches, num_epochs=8):
batch_size = self.num_envs * self.num_transitions_per_env
mini_batch_size = batch_size // num_mini_batches
indices = torch.randperm(num_mini_batches*mini_batch_size, requires_grad=False, device=self.device)
observations = self.observations.flatten(0, 1)
privileged_obs = self.privileged_observations.flatten(0, 1)
obs_history = self.observation_histories.flatten(0, 1)
critic_observations = observations
actions = self.actions.flatten(0, 1)
values = self.values.flatten(0, 1)
returns = self.returns.flatten(0, 1)
old_actions_log_prob = self.actions_log_prob.flatten(0, 1)
advantages = self.advantages.flatten(0, 1)
old_mu = self.mu.flatten(0, 1)
old_sigma = self.sigma.flatten(0, 1)
old_env_bins = self.env_bins.flatten(0, 1)
for epoch in range(num_epochs):
for i in range(num_mini_batches):
start = i*mini_batch_size
end = (i+1)*mini_batch_size
batch_idx = indices[start:end]
obs_batch = observations[batch_idx]
critic_observations_batch = critic_observations[batch_idx]
privileged_obs_batch = privileged_obs[batch_idx]
obs_history_batch = obs_history[batch_idx]
actions_batch = actions[batch_idx]
target_values_batch = values[batch_idx]
returns_batch = returns[batch_idx]
old_actions_log_prob_batch = old_actions_log_prob[batch_idx]
advantages_batch = advantages[batch_idx]
old_mu_batch = old_mu[batch_idx]
old_sigma_batch = old_sigma[batch_idx]
env_bins_batch = old_env_bins[batch_idx]
yield obs_batch, critic_observations_batch, privileged_obs_batch, obs_history_batch, actions_batch, target_values_batch, advantages_batch, returns_batch, \
old_actions_log_prob_batch, old_mu_batch, old_sigma_batch, None, env_bins_batch
# for RNNs only
def reccurent_mini_batch_generator(self, num_mini_batches, num_epochs=8):
padded_obs_trajectories, trajectory_masks = split_and_pad_trajectories(self.observations, self.dones)
padded_privileged_obs_trajectories, trajectory_masks = split_and_pad_trajectories(self.privileged_observations, self.dones)
padded_obs_history_trajectories, trajectory_masks = split_and_pad_trajectories(self.observation_histories, self.dones)
padded_critic_obs_trajectories = padded_obs_trajectories
mini_batch_size = self.num_envs // num_mini_batches
for ep in range(num_epochs):
first_traj = 0
for i in range(num_mini_batches):
start = i*mini_batch_size
stop = (i+1)*mini_batch_size
dones = self.dones.squeeze(-1)
last_was_done = torch.zeros_like(dones, dtype=torch.bool)
last_was_done[1:] = dones[:-1]
last_was_done[0] = True
trajectories_batch_size = torch.sum(last_was_done[:, start:stop])
last_traj = first_traj + trajectories_batch_size
masks_batch = trajectory_masks[:, first_traj:last_traj]
obs_batch = padded_obs_trajectories[:, first_traj:last_traj]
critic_obs_batch = padded_critic_obs_trajectories[:, first_traj:last_traj]
privileged_obs_batch = padded_privileged_obs_trajectories[:, first_traj:last_traj]
obs_history_batch = padded_obs_history_trajectories[:, first_traj:last_traj]
actions_batch = self.actions[:, start:stop]
old_mu_batch = self.mu[:, start:stop]
old_sigma_batch = self.sigma[:, start:stop]
returns_batch = self.returns[:, start:stop]
advantages_batch = self.advantages[:, start:stop]
values_batch = self.values[:, start:stop]
old_actions_log_prob_batch = self.actions_log_prob[:, start:stop]
yield obs_batch, critic_obs_batch, privileged_obs_batch, obs_history_batch, actions_batch, values_batch, advantages_batch, returns_batch, \
old_actions_log_prob_batch, old_mu_batch, old_sigma_batch, masks_batch
first_traj = last_traj
go1_gym_learn/ppo_cse/__init__.py
Executable
+308
View File
@@ -0,0 +1,308 @@
import time
from collections import deque
import copy
import os
import torch
from ml_logger import logger
from params_proto import PrefixProto
from .actor_critic import ActorCritic
from .rollout_storage import RolloutStorage
def class_to_dict(obj) -> dict:
if not hasattr(obj, "__dict__"):
return obj
result = {}
for key in dir(obj):
if key.startswith("_") or key == "terrain":
continue
element = []
val = getattr(obj, key)
if isinstance(val, list):
for item in val:
element.append(class_to_dict(item))
else:
element = class_to_dict(val)
result[key] = element
return result
class DataCaches:
def __init__(self, curriculum_bins):
from go1_gym_learn.ppo.metrics_caches import SlotCache, DistCache
self.slot_cache = SlotCache(curriculum_bins)
self.dist_cache = DistCache()
caches = DataCaches(1)
class RunnerArgs(PrefixProto, cli=False):
# runner
algorithm_class_name = 'RMA'
num_steps_per_env = 24 # per iteration
max_iterations = 1500 # number of policy updates
# logging
save_interval = 400 # check for potential saves every this many iterations
save_video_interval = 100
log_freq = 10
# load and resume
resume = False
load_run = -1 # -1 = last run
checkpoint = -1 # -1 = last saved model
resume_path = None # updated from load_run and chkpt
resume_curriculum = True
class Runner:
def __init__(self, env, device='cpu'):
from .ppo import PPO
self.device = device
self.env = env
actor_critic = ActorCritic(self.env.num_obs,
self.env.num_privileged_obs,
self.env.num_obs_history,
self.env.num_actions,
).to(self.device)
if RunnerArgs.resume:
# load pretrained weights from resume_path
from ml_logger import ML_Logger
loader = ML_Logger(root="http://escher.csail.mit.edu:8080",
prefix=RunnerArgs.resume_path)
weights = loader.load_torch("checkpoints/ac_weights_last.pt")
actor_critic.load_state_dict(state_dict=weights)
if hasattr(self.env, "curricula") and RunnerArgs.resume_curriculum:
# load curriculum state
distributions = loader.load_pkl("curriculum/distribution.pkl")
distribution_last = distributions[-1]["distribution"]
gait_names = [key[8:] if key.startswith("weights_") else None for key in distribution_last.keys()]
for gait_id, gait_name in enumerate(self.env.category_names):
self.env.curricula[gait_id].weights = distribution_last[f"weights_{gait_name}"]
print(gait_name)
self.alg = PPO(actor_critic, device=self.device)
self.num_steps_per_env = RunnerArgs.num_steps_per_env
# init storage and model
self.alg.init_storage(self.env.num_train_envs, self.num_steps_per_env, [self.env.num_obs],
[self.env.num_privileged_obs], [self.env.num_obs_history], [self.env.num_actions])
self.tot_timesteps = 0
self.tot_time = 0
self.current_learning_iteration = 0
self.last_recording_it = 0
self.env.reset()
def learn(self, num_learning_iterations, init_at_random_ep_len=False, eval_freq=100, curriculum_dump_freq=500, eval_expert=False):
from ml_logger import logger
# initialize writer
assert logger.prefix, "you will overwrite the entire instrument server"
logger.start('start', 'epoch', 'episode', 'run', 'step')
if init_at_random_ep_len:
self.env.episode_length_buf = torch.randint_like(self.env.episode_length_buf,
high=int(self.env.max_episode_length))
# split train and test envs
num_train_envs = self.env.num_train_envs
obs_dict = self.env.get_observations() # TODO: check, is this correct on the first step?
obs, privileged_obs, obs_history = obs_dict["obs"], obs_dict["privileged_obs"], obs_dict["obs_history"]
obs, privileged_obs, obs_history = obs.to(self.device), privileged_obs.to(self.device), obs_history.to(
self.device)
self.alg.actor_critic.train() # switch to train mode (for dropout for example)
rewbuffer = deque(maxlen=100)
lenbuffer = deque(maxlen=100)
rewbuffer_eval = deque(maxlen=100)
lenbuffer_eval = deque(maxlen=100)
cur_reward_sum = torch.zeros(self.env.num_envs, dtype=torch.float, device=self.device)
cur_episode_length = torch.zeros(self.env.num_envs, dtype=torch.float, device=self.device)
tot_iter = self.current_learning_iteration + num_learning_iterations
for it in range(self.current_learning_iteration, tot_iter):
start = time.time()
# Rollout
with torch.inference_mode():
for i in range(self.num_steps_per_env):
actions_train = self.alg.act(obs[:num_train_envs], privileged_obs[:num_train_envs],
obs_history[:num_train_envs])
if eval_expert:
actions_eval = self.alg.actor_critic.act_teacher(obs_history[num_train_envs:],
privileged_obs[num_train_envs:])
else:
actions_eval = self.alg.actor_critic.act_student(obs_history[num_train_envs:])
ret = self.env.step(torch.cat((actions_train, actions_eval), dim=0))
obs_dict, rewards, dones, infos = ret
obs, privileged_obs, obs_history = obs_dict["obs"], obs_dict["privileged_obs"], obs_dict[
"obs_history"]
obs, privileged_obs, obs_history, rewards, dones = obs.to(self.device), privileged_obs.to(
self.device), obs_history.to(self.device), rewards.to(self.device), dones.to(self.device)
self.alg.process_env_step(rewards[:num_train_envs], dones[:num_train_envs], infos)
if 'train/episode' in infos:
with logger.Prefix(metrics="train/episode"):
logger.store_metrics(**infos['train/episode'])
if 'eval/episode' in infos:
with logger.Prefix(metrics="eval/episode"):
logger.store_metrics(**infos['eval/episode'])
if 'curriculum' in infos:
cur_reward_sum += rewards
cur_episode_length += 1
new_ids = (dones > 0).nonzero(as_tuple=False)
new_ids_train = new_ids[new_ids < num_train_envs]
rewbuffer.extend(cur_reward_sum[new_ids_train].cpu().numpy().tolist())
lenbuffer.extend(cur_episode_length[new_ids_train].cpu().numpy().tolist())
cur_reward_sum[new_ids_train] = 0
cur_episode_length[new_ids_train] = 0
new_ids_eval = new_ids[new_ids >= num_train_envs]
rewbuffer_eval.extend(cur_reward_sum[new_ids_eval].cpu().numpy().tolist())
lenbuffer_eval.extend(cur_episode_length[new_ids_eval].cpu().numpy().tolist())
cur_reward_sum[new_ids_eval] = 0
cur_episode_length[new_ids_eval] = 0
if 'curriculum/distribution' in infos:
distribution = infos['curriculum/distribution']
stop = time.time()
collection_time = stop - start
# Learning step
start = stop
self.alg.compute_returns(obs_history[:num_train_envs], privileged_obs[:num_train_envs])
if it % curriculum_dump_freq == 0:
logger.save_pkl({"iteration": it,
**caches.slot_cache.get_summary(),
**caches.dist_cache.get_summary()},
path=f"curriculum/info.pkl", append=True)
if 'curriculum/distribution' in infos:
logger.save_pkl({"iteration": it,
"distribution": distribution},
path=f"curriculum/distribution.pkl", append=True)
mean_value_loss, mean_surrogate_loss, mean_adaptation_module_loss, mean_decoder_loss, mean_decoder_loss_student, mean_adaptation_module_test_loss, mean_decoder_test_loss, mean_decoder_test_loss_student = self.alg.update()
stop = time.time()
learn_time = stop - start
logger.store_metrics(
# total_time=learn_time - collection_time,
time_elapsed=logger.since('start'),
time_iter=logger.split('epoch'),
adaptation_loss=mean_adaptation_module_loss,
mean_value_loss=mean_value_loss,
mean_surrogate_loss=mean_surrogate_loss,
mean_decoder_loss=mean_decoder_loss,
mean_decoder_loss_student=mean_decoder_loss_student,
mean_decoder_test_loss=mean_decoder_test_loss,
mean_decoder_test_loss_student=mean_decoder_test_loss_student,
mean_adaptation_module_test_loss=mean_adaptation_module_test_loss
)
if RunnerArgs.save_video_interval:
self.log_video(it)
self.tot_timesteps += self.num_steps_per_env * self.env.num_envs
if logger.every(RunnerArgs.log_freq, "iteration", start_on=1):
# if it % Config.log_freq == 0:
logger.log_metrics_summary(key_values={"timesteps": self.tot_timesteps, "iterations": it})
logger.job_running()
if it % RunnerArgs.save_interval == 0:
with logger.Sync():
logger.torch_save(self.alg.actor_critic.state_dict(), f"checkpoints/ac_weights_{it:06d}.pt")
logger.duplicate(f"checkpoints/ac_weights_{it:06d}.pt", f"checkpoints/ac_weights_last.pt")
path = './tmp/legged_data'
os.makedirs(path, exist_ok=True)
adaptation_module_path = f'{path}/adaptation_module_latest.jit'
adaptation_module = copy.deepcopy(self.alg.actor_critic.adaptation_module).to('cpu')
traced_script_adaptation_module = torch.jit.script(adaptation_module)
traced_script_adaptation_module.save(adaptation_module_path)
body_path = f'{path}/body_latest.jit'
body_model = copy.deepcopy(self.alg.actor_critic.actor_body).to('cpu')
traced_script_body_module = torch.jit.script(body_model)
traced_script_body_module.save(body_path)
logger.upload_file(file_path=adaptation_module_path, target_path=f"checkpoints/", once=False)
logger.upload_file(file_path=body_path, target_path=f"checkpoints/", once=False)
self.current_learning_iteration += num_learning_iterations
with logger.Sync():
logger.torch_save(self.alg.actor_critic.state_dict(), f"checkpoints/ac_weights_{it:06d}.pt")
logger.duplicate(f"checkpoints/ac_weights_{it:06d}.pt", f"checkpoints/ac_weights_last.pt")
path = './tmp/legged_data'
os.makedirs(path, exist_ok=True)
adaptation_module_path = f'{path}/adaptation_module_latest.jit'
adaptation_module = copy.deepcopy(self.alg.actor_critic.adaptation_module).to('cpu')
traced_script_adaptation_module = torch.jit.script(adaptation_module)
traced_script_adaptation_module.save(adaptation_module_path)
body_path = f'{path}/body_latest.jit'
body_model = copy.deepcopy(self.alg.actor_critic.actor_body).to('cpu')
traced_script_body_module = torch.jit.script(body_model)
traced_script_body_module.save(body_path)
logger.upload_file(file_path=adaptation_module_path, target_path=f"checkpoints/", once=False)
logger.upload_file(file_path=body_path, target_path=f"checkpoints/", once=False)
def log_video(self, it):
if it - self.last_recording_it >= RunnerArgs.save_video_interval:
self.env.start_recording()
if self.env.num_eval_envs > 0:
self.env.start_recording_eval()
print("START RECORDING")
self.last_recording_it = it
frames = self.env.get_complete_frames()
if len(frames) > 0:
self.env.pause_recording()
print("LOGGING VIDEO")
logger.save_video(frames, f"videos/{it:05d}.mp4", fps=1 / self.env.dt)
if self.env.num_eval_envs > 0:
frames = self.env.get_complete_frames_eval()
if len(frames) > 0:
self.env.pause_recording_eval()
print("LOGGING EVAL VIDEO")
logger.save_video(frames, f"videos/{it:05d}_eval.mp4", fps=1 / self.env.dt)
def get_inference_policy(self, device=None):
self.alg.actor_critic.eval() # switch to evaluation mode (dropout for example)
if device is not None:
self.alg.actor_critic.to(device)
return self.alg.actor_critic.act_inference
def get_expert_policy(self, device=None):
self.alg.actor_critic.eval() # switch to evaluation mode (dropout for example)
if device is not None:
self.alg.actor_critic.to(device)
return self.alg.actor_critic.act_expert
go1_gym_learn/ppo_cse/actor_critic.py
Executable
+166
View File
@@ -0,0 +1,166 @@
import torch
import torch.nn as nn
from params_proto import PrefixProto
from torch.distributions import Normal
class AC_Args(PrefixProto, cli=False):
# policy
init_noise_std = 1.0
actor_hidden_dims = [512, 256, 128]
critic_hidden_dims = [512, 256, 128]
activation = 'elu' # can be elu, relu, selu, crelu, lrelu, tanh, sigmoid
adaptation_module_branch_hidden_dims = [256, 128]
use_decoder = False
class ActorCritic(nn.Module):
is_recurrent = False
def __init__(self, num_obs,
num_privileged_obs,
num_obs_history,
num_actions,
**kwargs):
if kwargs:
print("ActorCritic.__init__ got unexpected arguments, which will be ignored: " + str(
[key for key in kwargs.keys()]))
self.decoder = AC_Args.use_decoder
super().__init__()
self.num_obs_history = num_obs_history
self.num_privileged_obs = num_privileged_obs
activation = get_activation(AC_Args.activation)
# Adaptation module
adaptation_module_layers = []
adaptation_module_layers.append(nn.Linear(self.num_obs_history, AC_Args.adaptation_module_branch_hidden_dims[0]))
adaptation_module_layers.append(activation)
for l in range(len(AC_Args.adaptation_module_branch_hidden_dims)):
if l == len(AC_Args.adaptation_module_branch_hidden_dims) - 1:
adaptation_module_layers.append(
nn.Linear(AC_Args.adaptation_module_branch_hidden_dims[l], self.num_privileged_obs))
else:
adaptation_module_layers.append(
nn.Linear(AC_Args.adaptation_module_branch_hidden_dims[l],
AC_Args.adaptation_module_branch_hidden_dims[l + 1]))
adaptation_module_layers.append(activation)
self.adaptation_module = nn.Sequential(*adaptation_module_layers)
# Policy
actor_layers = []
actor_layers.append(nn.Linear(self.num_privileged_obs + self.num_obs_history, AC_Args.actor_hidden_dims[0]))
actor_layers.append(activation)
for l in range(len(AC_Args.actor_hidden_dims)):
if l == len(AC_Args.actor_hidden_dims) - 1:
actor_layers.append(nn.Linear(AC_Args.actor_hidden_dims[l], num_actions))
else:
actor_layers.append(nn.Linear(AC_Args.actor_hidden_dims[l], AC_Args.actor_hidden_dims[l + 1]))
actor_layers.append(activation)
self.actor_body = nn.Sequential(*actor_layers)
# Value function
critic_layers = []
critic_layers.append(nn.Linear(self.num_privileged_obs + self.num_obs_history, AC_Args.critic_hidden_dims[0]))
critic_layers.append(activation)
for l in range(len(AC_Args.critic_hidden_dims)):
if l == len(AC_Args.critic_hidden_dims) - 1:
critic_layers.append(nn.Linear(AC_Args.critic_hidden_dims[l], 1))
else:
critic_layers.append(nn.Linear(AC_Args.critic_hidden_dims[l], AC_Args.critic_hidden_dims[l + 1]))
critic_layers.append(activation)
self.critic_body = nn.Sequential(*critic_layers)
print(f"Adaptation Module: {self.adaptation_module}")
print(f"Actor MLP: {self.actor_body}")
print(f"Critic MLP: {self.critic_body}")
# Action noise
self.std = nn.Parameter(AC_Args.init_noise_std * torch.ones(num_actions))
self.distribution = None
# disable args validation for speedup
Normal.set_default_validate_args = False
@staticmethod
# not used at the moment
def init_weights(sequential, scales):
[torch.nn.init.orthogonal_(module.weight, gain=scales[idx]) for idx, module in
enumerate(mod for mod in sequential if isinstance(mod, nn.Linear))]
def reset(self, dones=None):
pass
def forward(self):
raise NotImplementedError
@property
def action_mean(self):
return self.distribution.mean
@property
def action_std(self):
return self.distribution.stddev
@property
def entropy(self):
return self.distribution.entropy().sum(dim=-1)
def update_distribution(self, observation_history):
latent = self.adaptation_module(observation_history)
mean = self.actor_body(torch.cat((observation_history, latent), dim=-1))
self.distribution = Normal(mean, mean * 0. + self.std)
def act(self, observation_history, **kwargs):
self.update_distribution(observation_history)
return self.distribution.sample()
def get_actions_log_prob(self, actions):
return self.distribution.log_prob(actions).sum(dim=-1)
def act_expert(self, ob, policy_info={}):
return self.act_teacher(ob["obs_history"], ob["privileged_obs"])
def act_inference(self, ob, policy_info={}):
return self.act_student(ob["obs_history"], policy_info=policy_info)
def act_student(self, observation_history, policy_info={}):
latent = self.adaptation_module(observation_history)
actions_mean = self.actor_body(torch.cat((observation_history, latent), dim=-1))
policy_info["latents"] = latent.detach().cpu().numpy()
return actions_mean
def act_teacher(self, observation_history, privileged_info, policy_info={}):
actions_mean = self.actor_body(torch.cat((observation_history, privileged_info), dim=-1))
policy_info["latents"] = privileged_info
return actions_mean
def evaluate(self, observation_history, privileged_observations, **kwargs):
value = self.critic_body(torch.cat((observation_history, privileged_observations), dim=-1))
return value
def get_student_latent(self, observation_history):
return self.adaptation_module(observation_history)
def get_activation(act_name):
if act_name == "elu":
return nn.ELU()
elif act_name == "selu":
return nn.SELU()
elif act_name == "relu":
return nn.ReLU()
elif act_name == "crelu":
return nn.ReLU()
elif act_name == "lrelu":
return nn.LeakyReLU()
elif act_name == "tanh":
return nn.Tanh()
elif act_name == "sigmoid":
return nn.Sigmoid()
else:
print("invalid activation function!")
return None
go1_gym_learn/ppo_cse/metrics_caches.py
+90
View File
@@ -0,0 +1,90 @@
from collections import defaultdict
from ml_logger import logger
import numpy as np
import torch
class DistCache:
def __init__(self):
"""
Args:
n: Number of slots for the cache
"""
self.cache = defaultdict(lambda: 0)
def log(self, **key_vals):
"""
Args:
slots: ids for the array
**key_vals:
"""
for k, v in key_vals.items():
count = self.cache[k + '@counts'] + 1
self.cache[k + '@counts'] = count
self.cache[k] = v + (count - 1) * self.cache[k]
self.cache[k] /= count
def get_summary(self):
ret = {
k: v
for k, v in self.cache.items()
if not k.endswith("@counts")
}
self.cache.clear()
return ret
if __name__ == '__main__':
cl = DistCache()
lin_vel = np.ones((11, 11))
ang_vel = np.zeros((5, 5))
cl.log(lin_vel=lin_vel, ang_vel=ang_vel)
lin_vel = np.zeros((11, 11))
ang_vel = np.zeros((5, 5))
cl.log(lin_vel=lin_vel, ang_vel=ang_vel)
print(cl.get_summary())
class SlotCache:
def __init__(self, n):
"""
Args:
n: Number of slots for the cache
"""
self.n = n
self.cache = defaultdict(lambda: np.zeros([n]))
def log(self, slots=None, **key_vals):
"""
Args:
slots: ids for the array
**key_vals:
"""
if slots is None:
slots = range(self.n)
for k, v in key_vals.items():
counts = self.cache[k + '@counts'][slots] + 1
self.cache[k + '@counts'][slots] = counts
self.cache[k][slots] = v + (counts - 1) * self.cache[k][slots]
self.cache[k][slots] /= counts
def get_summary(self):
ret = {
k: v
for k, v in self.cache.items()
if not k.endswith("@counts")
}
self.cache.clear()
return ret
if __name__ == '__main__':
cl = SlotCache(100)
reset_env_ids = [2, 5, 6]
lin_vel = [0.1, 0.5, 0.8]
ang_vel = [0.4, -0.4, 0.2]
cl.log(reset_env_ids, lin_vel=lin_vel, ang_vel=ang_vel)
cl.log(lin_vel=np.ones(100))
go1_gym_learn/ppo_cse/ppo.py
Executable
+205
View File
@@ -0,0 +1,205 @@
import torch
import torch.nn as nn
import torch.optim as optim
import torch.nn.functional as F
import numpy as np
from params_proto import PrefixProto
from go1_gym_learn.ppo_cse import ActorCritic
from go1_gym_learn.ppo_cse import RolloutStorage
from go1_gym_learn.ppo_cse import caches
class PPO_Args(PrefixProto):
# algorithm
value_loss_coef = 1.0
use_clipped_value_loss = True
clip_param = 0.2
entropy_coef = 0.01
num_learning_epochs = 5
num_mini_batches = 4 # mini batch size = num_envs*nsteps / nminibatches
learning_rate = 1.e-3 # 5.e-4
adaptation_module_learning_rate = 1.e-3
num_adaptation_module_substeps = 1
schedule = 'adaptive' # could be adaptive, fixed
gamma = 0.99
lam = 0.95
desired_kl = 0.01
max_grad_norm = 1.
selective_adaptation_module_loss = False
class PPO:
actor_critic: ActorCritic
def __init__(self, actor_critic, device='cpu'):
self.device = device
# PPO components
self.actor_critic = actor_critic
self.actor_critic.to(device)
self.storage = None # initialized later
self.optimizer = optim.Adam(self.actor_critic.parameters(), lr=PPO_Args.learning_rate)
self.adaptation_module_optimizer = optim.Adam(self.actor_critic.parameters(),
lr=PPO_Args.adaptation_module_learning_rate)
if self.actor_critic.decoder:
self.decoder_optimizer = optim.Adam(self.actor_critic.parameters(),
lr=PPO_Args.adaptation_module_learning_rate)
self.transition = RolloutStorage.Transition()
self.learning_rate = PPO_Args.learning_rate
def init_storage(self, num_envs, num_transitions_per_env, actor_obs_shape, privileged_obs_shape, obs_history_shape,
action_shape):
self.storage = RolloutStorage(num_envs, num_transitions_per_env, actor_obs_shape, privileged_obs_shape,
obs_history_shape, action_shape, self.device)
def test_mode(self):
self.actor_critic.test()
def train_mode(self):
self.actor_critic.train()
def act(self, obs, privileged_obs, obs_history):
# Compute the actions and values
self.transition.actions = self.actor_critic.act(obs_history).detach()
self.transition.values = self.actor_critic.evaluate(obs_history, privileged_obs).detach()
self.transition.actions_log_prob = self.actor_critic.get_actions_log_prob(self.transition.actions).detach()
self.transition.action_mean = self.actor_critic.action_mean.detach()
self.transition.action_sigma = self.actor_critic.action_std.detach()
# need to record obs and critic_obs before env.step()
self.transition.observations = obs
self.transition.critic_observations = obs
self.transition.privileged_observations = privileged_obs
self.transition.observation_histories = obs_history
return self.transition.actions
def process_env_step(self, rewards, dones, infos):
self.transition.rewards = rewards.clone()
self.transition.dones = dones
self.transition.env_bins = infos["env_bins"]
# Bootstrapping on time outs
if 'time_outs' in infos:
self.transition.rewards += PPO_Args.gamma * torch.squeeze(
self.transition.values * infos['time_outs'].unsqueeze(1).to(self.device), 1)
# Record the transition
self.storage.add_transitions(self.transition)
self.transition.clear()
self.actor_critic.reset(dones)
def compute_returns(self, last_critic_obs, last_critic_privileged_obs):
last_values = self.actor_critic.evaluate(last_critic_obs, last_critic_privileged_obs).detach()
self.storage.compute_returns(last_values, PPO_Args.gamma, PPO_Args.lam)
def update(self):
mean_value_loss = 0
mean_surrogate_loss = 0
mean_adaptation_module_loss = 0
mean_decoder_loss = 0
mean_decoder_loss_student = 0
mean_adaptation_module_test_loss = 0
mean_decoder_test_loss = 0
mean_decoder_test_loss_student = 0
generator = self.storage.mini_batch_generator(PPO_Args.num_mini_batches, PPO_Args.num_learning_epochs)
for obs_batch, critic_obs_batch, privileged_obs_batch, obs_history_batch, actions_batch, target_values_batch, advantages_batch, returns_batch, old_actions_log_prob_batch, \
old_mu_batch, old_sigma_batch, masks_batch, env_bins_batch in generator:
self.actor_critic.act(obs_history_batch, masks=masks_batch)
actions_log_prob_batch = self.actor_critic.get_actions_log_prob(actions_batch)
value_batch = self.actor_critic.evaluate(obs_history_batch, privileged_obs_batch, masks=masks_batch)
mu_batch = self.actor_critic.action_mean
sigma_batch = self.actor_critic.action_std
entropy_batch = self.actor_critic.entropy
# KL
if PPO_Args.desired_kl != None and PPO_Args.schedule == 'adaptive':
with torch.inference_mode():
kl = torch.sum(
torch.log(sigma_batch / old_sigma_batch + 1.e-5) + (
torch.square(old_sigma_batch) + torch.square(old_mu_batch - mu_batch)) / (
2.0 * torch.square(sigma_batch)) - 0.5, axis=-1)
kl_mean = torch.mean(kl)
if kl_mean > PPO_Args.desired_kl * 2.0:
self.learning_rate = max(1e-5, self.learning_rate / 1.5)
elif kl_mean < PPO_Args.desired_kl / 2.0 and kl_mean > 0.0:
self.learning_rate = min(1e-2, self.learning_rate * 1.5)
for param_group in self.optimizer.param_groups:
param_group['lr'] = self.learning_rate
# Surrogate loss
ratio = torch.exp(actions_log_prob_batch - torch.squeeze(old_actions_log_prob_batch))
surrogate = -torch.squeeze(advantages_batch) * ratio
surrogate_clipped = -torch.squeeze(advantages_batch) * torch.clamp(ratio, 1.0 - PPO_Args.clip_param,
1.0 + PPO_Args.clip_param)
surrogate_loss = torch.max(surrogate, surrogate_clipped).mean()
# Value function loss
if PPO_Args.use_clipped_value_loss:
value_clipped = target_values_batch + \
(value_batch - target_values_batch).clamp(-PPO_Args.clip_param,
PPO_Args.clip_param)
value_losses = (value_batch - returns_batch).pow(2)
value_losses_clipped = (value_clipped - returns_batch).pow(2)
value_loss = torch.max(value_losses, value_losses_clipped).mean()
else:
value_loss = (returns_batch - value_batch).pow(2).mean()
loss = surrogate_loss + PPO_Args.value_loss_coef * value_loss - PPO_Args.entropy_coef * entropy_batch.mean()
# Gradient step
self.optimizer.zero_grad()
loss.backward()
nn.utils.clip_grad_norm_(self.actor_critic.parameters(), PPO_Args.max_grad_norm)
self.optimizer.step()
mean_value_loss += value_loss.item()
mean_surrogate_loss += surrogate_loss.item()
data_size = privileged_obs_batch.shape[0]
num_train = int(data_size // 5 * 4)
# Adaptation module gradient step
for epoch in range(PPO_Args.num_adaptation_module_substeps):
adaptation_pred = self.actor_critic.adaptation_module(obs_history_batch)
with torch.no_grad():
adaptation_target = privileged_obs_batch
# residual = (adaptation_target - adaptation_pred).norm(dim=1)
# caches.slot_cache.log(env_bins_batch[:, 0].cpu().numpy().astype(np.uint8),
# sysid_residual=residual.cpu().numpy())
selection_indices = torch.linspace(0, adaptation_pred.shape[1]-1, steps=adaptation_pred.shape[1], dtype=torch.long)
if PPO_Args.selective_adaptation_module_loss:
# mask out indices corresponding to swing feet
selection_indices = 0
adaptation_loss = F.mse_loss(adaptation_pred[:num_train, selection_indices], adaptation_target[:num_train, selection_indices])
adaptation_test_loss = F.mse_loss(adaptation_pred[num_train:, selection_indices], adaptation_target[num_train:, selection_indices])
self.adaptation_module_optimizer.zero_grad()
adaptation_loss.backward()
self.adaptation_module_optimizer.step()
mean_adaptation_module_loss += adaptation_loss.item()
mean_adaptation_module_test_loss += adaptation_test_loss.item()
num_updates = PPO_Args.num_learning_epochs * PPO_Args.num_mini_batches
mean_value_loss /= num_updates
mean_surrogate_loss /= num_updates
mean_adaptation_module_loss /= (num_updates * PPO_Args.num_adaptation_module_substeps)
mean_decoder_loss /= (num_updates * PPO_Args.num_adaptation_module_substeps)
mean_decoder_loss_student /= (num_updates * PPO_Args.num_adaptation_module_substeps)
mean_adaptation_module_test_loss /= (num_updates * PPO_Args.num_adaptation_module_substeps)
mean_decoder_test_loss /= (num_updates * PPO_Args.num_adaptation_module_substeps)
mean_decoder_test_loss_student /= (num_updates * PPO_Args.num_adaptation_module_substeps)
self.storage.clear()
return mean_value_loss, mean_surrogate_loss, mean_adaptation_module_loss, mean_decoder_loss, mean_decoder_loss_student, mean_adaptation_module_test_loss, mean_decoder_test_loss, mean_decoder_test_loss_student
go1_gym_learn/ppo_cse/rollout_storage.py
Executable
+180
View File
@@ -0,0 +1,180 @@
import torch
from go1_gym_learn.utils import split_and_pad_trajectories
class RolloutStorage:
class Transition:
def __init__(self):
self.observations = None
self.privileged_observations = None
self.observation_histories = None
self.critic_observations = None
self.actions = None
self.rewards = None
self.dones = None
self.values = None
self.actions_log_prob = None
self.action_mean = None
self.action_sigma = None
self.env_bins = None
def clear(self):
self.__init__()
def __init__(self, num_envs, num_transitions_per_env, obs_shape, privileged_obs_shape, obs_history_shape, actions_shape, device='cpu'):
self.device = device
self.obs_shape = obs_shape
self.privileged_obs_shape = privileged_obs_shape
self.obs_history_shape = obs_history_shape
self.actions_shape = actions_shape
# Core
self.observations = torch.zeros(num_transitions_per_env, num_envs, *obs_shape, device=self.device)
self.privileged_observations = torch.zeros(num_transitions_per_env, num_envs, *privileged_obs_shape, device=self.device)
self.observation_histories = torch.zeros(num_transitions_per_env, num_envs, *obs_history_shape, device=self.device)
self.rewards = torch.zeros(num_transitions_per_env, num_envs, 1, device=self.device)
self.actions = torch.zeros(num_transitions_per_env, num_envs, *actions_shape, device=self.device)
self.dones = torch.zeros(num_transitions_per_env, num_envs, 1, device=self.device).byte()
# For PPO
self.actions_log_prob = torch.zeros(num_transitions_per_env, num_envs, 1, device=self.device)
self.values = torch.zeros(num_transitions_per_env, num_envs, 1, device=self.device)
self.returns = torch.zeros(num_transitions_per_env, num_envs, 1, device=self.device)
self.advantages = torch.zeros(num_transitions_per_env, num_envs, 1, device=self.device)
self.mu = torch.zeros(num_transitions_per_env, num_envs, *actions_shape, device=self.device)
self.sigma = torch.zeros(num_transitions_per_env, num_envs, *actions_shape, device=self.device)
self.env_bins = torch.zeros(num_transitions_per_env, num_envs, 1, device=self.device)
self.num_transitions_per_env = num_transitions_per_env
self.num_envs = num_envs
self.step = 0
def add_transitions(self, transition: Transition):
if self.step >= self.num_transitions_per_env:
raise AssertionError("Rollout buffer overflow")
self.observations[self.step].copy_(transition.observations)
self.privileged_observations[self.step].copy_(transition.privileged_observations)
self.observation_histories[self.step].copy_(transition.observation_histories)
self.actions[self.step].copy_(transition.actions)
self.rewards[self.step].copy_(transition.rewards.view(-1, 1))
self.dones[self.step].copy_(transition.dones.view(-1, 1))
self.values[self.step].copy_(transition.values)
self.actions_log_prob[self.step].copy_(transition.actions_log_prob.view(-1, 1))
self.mu[self.step].copy_(transition.action_mean)
self.sigma[self.step].copy_(transition.action_sigma)
self.env_bins[self.step].copy_(transition.env_bins.view(-1, 1))
self.step += 1
def clear(self):
self.step = 0
def compute_returns(self, last_values, gamma, lam):
advantage = 0
for step in reversed(range(self.num_transitions_per_env)):
if step == self.num_transitions_per_env - 1:
next_values = last_values
else:
next_values = self.values[step + 1]
next_is_not_terminal = 1.0 - self.dones[step].float()
delta = self.rewards[step] + next_is_not_terminal * gamma * next_values - self.values[step]
advantage = delta + next_is_not_terminal * gamma * lam * advantage
self.returns[step] = advantage + self.values[step]
# Compute and normalize the advantages
self.advantages = self.returns - self.values
self.advantages = (self.advantages - self.advantages.mean()) / (self.advantages.std() + 1e-8)
def get_statistics(self):
done = self.dones
done[-1] = 1
flat_dones = done.permute(1, 0, 2).reshape(-1, 1)
done_indices = torch.cat((flat_dones.new_tensor([-1], dtype=torch.int64), flat_dones.nonzero(as_tuple=False)[:, 0]))
trajectory_lengths = (done_indices[1:] - done_indices[:-1])
return trajectory_lengths.float().mean(), self.rewards.mean()
def mini_batch_generator(self, num_mini_batches, num_epochs=8):
batch_size = self.num_envs * self.num_transitions_per_env
mini_batch_size = batch_size // num_mini_batches
indices = torch.randperm(num_mini_batches*mini_batch_size, requires_grad=False, device=self.device)
observations = self.observations.flatten(0, 1)
privileged_obs = self.privileged_observations.flatten(0, 1)
obs_history = self.observation_histories.flatten(0, 1)
critic_observations = observations
actions = self.actions.flatten(0, 1)
values = self.values.flatten(0, 1)
returns = self.returns.flatten(0, 1)
old_actions_log_prob = self.actions_log_prob.flatten(0, 1)
advantages = self.advantages.flatten(0, 1)
old_mu = self.mu.flatten(0, 1)
old_sigma = self.sigma.flatten(0, 1)
old_env_bins = self.env_bins.flatten(0, 1)
for epoch in range(num_epochs):
for i in range(num_mini_batches):
start = i*mini_batch_size
end = (i+1)*mini_batch_size
batch_idx = indices[start:end]
obs_batch = observations[batch_idx]
critic_observations_batch = critic_observations[batch_idx]
privileged_obs_batch = privileged_obs[batch_idx]
obs_history_batch = obs_history[batch_idx]
actions_batch = actions[batch_idx]
target_values_batch = values[batch_idx]
returns_batch = returns[batch_idx]
old_actions_log_prob_batch = old_actions_log_prob[batch_idx]
advantages_batch = advantages[batch_idx]
old_mu_batch = old_mu[batch_idx]
old_sigma_batch = old_sigma[batch_idx]
env_bins_batch = old_env_bins[batch_idx]
yield obs_batch, critic_observations_batch, privileged_obs_batch, obs_history_batch, actions_batch, target_values_batch, advantages_batch, returns_batch, \
old_actions_log_prob_batch, old_mu_batch, old_sigma_batch, None, env_bins_batch
# for RNNs only
def reccurent_mini_batch_generator(self, num_mini_batches, num_epochs=8):
padded_obs_trajectories, trajectory_masks = split_and_pad_trajectories(self.observations, self.dones)
padded_privileged_obs_trajectories, trajectory_masks = split_and_pad_trajectories(self.privileged_observations, self.dones)
padded_obs_history_trajectories, trajectory_masks = split_and_pad_trajectories(self.observation_histories, self.dones)
padded_critic_obs_trajectories = padded_obs_trajectories
mini_batch_size = self.num_envs // num_mini_batches
for ep in range(num_epochs):
first_traj = 0
for i in range(num_mini_batches):
start = i*mini_batch_size
stop = (i+1)*mini_batch_size
dones = self.dones.squeeze(-1)
last_was_done = torch.zeros_like(dones, dtype=torch.bool)
last_was_done[1:] = dones[:-1]
last_was_done[0] = True
trajectories_batch_size = torch.sum(last_was_done[:, start:stop])
last_traj = first_traj + trajectories_batch_size
masks_batch = trajectory_masks[:, first_traj:last_traj]
obs_batch = padded_obs_trajectories[:, first_traj:last_traj]
critic_obs_batch = padded_critic_obs_trajectories[:, first_traj:last_traj]
privileged_obs_batch = padded_privileged_obs_trajectories[:, first_traj:last_traj]
obs_history_batch = padded_obs_history_trajectories[:, first_traj:last_traj]
actions_batch = self.actions[:, start:stop]
old_mu_batch = self.mu[:, start:stop]
old_sigma_batch = self.sigma[:, start:stop]
returns_batch = self.returns[:, start:stop]
advantages_batch = self.advantages[:, start:stop]
values_batch = self.values[:, start:stop]
old_actions_log_prob_batch = self.actions_log_prob[:, start:stop]
yield obs_batch, critic_obs_batch, privileged_obs_batch, obs_history_batch, actions_batch, values_batch, advantages_batch, returns_batch, \
old_actions_log_prob_batch, old_mu_batch, old_sigma_batch, masks_batch
first_traj = last_traj
go1_gym_learn/utils/__init__.py
Executable
+3
View File
@@ -0,0 +1,3 @@
# License: see [LICENSE, LICENSES/rsl_rl/LICENSE]
from .utils import split_and_pad_trajectories, unpad_trajectories
go1_gym_learn/utils/utils.py
Executable
+43
View File
@@ -0,0 +1,43 @@
# License: see [LICENSE, LICENSES/rsl_rl/LICENSE]
import torch
def split_and_pad_trajectories(tensor, dones):
""" Splits trajectories at done indices. Then concatenates them and padds with zeros up to the length og the longest trajectory.
Returns masks corresponding to valid parts of the trajectories
Example:
Input: [ [a1, a2, a3, a4 | a5, a6],
[b1, b2 | b3, b4, b5 | b6]
f]
Output:[ [a1, a2, a3, a4], | [ [True, True, True, True],
[a5, a6, 0, 0], | [True, True, False, False],
[b1, b2, 0, 0], | [True, True, False, False],
[b3, b4, b5, 0], | [True, True, True, False],
[b6, 0, 0, 0] | [True, False, False, False],
] | ]
Assumes that the inputy has the following dimension order: [time, number of envs, aditional dimensions]
"""
dones = dones.clone()
dones[-1] = 1
# Permute the buffers to have order (num_envs, num_transitions_per_env, ...), for correct reshaping
flat_dones = dones.transpose(1, 0).reshape(-1, 1)
# Get length of trajectory by counting the number of successive not done elements
done_indices = torch.cat((flat_dones.new_tensor([-1], dtype=torch.int64), flat_dones.nonzero()[:, 0]))
trajectory_lengths = done_indices[1:] - done_indices[:-1]
trajectory_lengths_list = trajectory_lengths.tolist()
# Extract the individual trajectories
trajectories = torch.split(tensor.transpose(1, 0).flatten(0, 1),trajectory_lengths_list)
padded_trajectories = torch.nn.utils.rnn.pad_sequence(trajectories)
trajectory_masks = trajectory_lengths > torch.arange(0, tensor.shape[0], device=tensor.device).unsqueeze(1)
return padded_trajectories, trajectory_masks
def unpad_trajectories(trajectories, masks):
""" Does the inverse operation of split_and_pad_trajectories()
"""
# Need to transpose before and after the masking to have proper reshaping
return trajectories.transpose(1, 0)[masks.transpose(1, 0)].view(-1, trajectories.shape[0], trajectories.shape[-1]).transpose(1, 0)
media/rc_map.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 6.0 MiB

resources/actuator_nets/unitree_go1.pt
BIN
View File
Binary file not shown.
resources/objects/cube.urdf
+22
View File
@@ -0,0 +1,22 @@
<?xml version="1.0"?>
<robot name="object">
<link name="object">+
<visual>
<origin xyz="0 0 -0.075" rpy="1.57 0 0"/>
<geometry>
<mesh filename="concrete_block.obj" scale="1.0 1.0 1.0"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0"/>
<geometry>
<!-- <mesh filename="concrete_block.obj" scale="1.0 1.0 1.0"/> -->
<box size="1.0 1.0 0.05"/>
</geometry>
</collision>
<inertial>
<mass value="0.5"/>
<inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.5"/>
</inertial>
</link>
</robot>
resources/robots/go1/meshes/calf.stl
BIN
View File
Binary file not shown.
resources/robots/go1/meshes/hip.stl
BIN
View File
Binary file not shown.
resources/robots/go1/meshes/thigh.stl
BIN
View File
Binary file not shown.
resources/robots/go1/meshes/thigh_mirror.stl
BIN
View File
Binary file not shown.
resources/robots/go1/meshes/trunk.stl
BIN
View File
Binary file not shown.
resources/robots/go1/urdf/go1.urdf
+582
View File
@@ -0,0 +1,582 @@
<?xml version="1.0" ?>
<robot name="go1_description" xmlns:xacro="http://www.ros.org/wiki/xacro">
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="silver">
<color rgba="0.913725490196 0.913725490196 0.847058823529 1.0"/>
</material>
<material name="orange">
<color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
</material>
<material name="brown">
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/trunk.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.3762 0.0935 0.114"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.011611 0.004437 0.000108"/>
<mass value="4.8"/>
<inertia ixx="0.016130741919" ixy="0.000593180607" ixz="7.324662e-06" iyy="0.036507810812" iyz="2.0969537e-05" izz="0.044693872053"/>
</inertial>
</link>
<joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="-0.01592 -0.06659 -0.00617"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<material name="red"/>
</visual>
<!-- <collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision> -->
</link>
<joint name="FR_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="0.1881 -0.04675 0"/>
<parent link="trunk"/>
<child link="FR_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="50"/>
</joint>
<link name="FR_hip">
<visual>
<origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<!-- <collision>
<origin rpy="1.57079632679 0 0" xyz="0 -0.045 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision> -->
<inertial>
<origin rpy="0 0 0" xyz="-0.00541 0.00074 6e-06"/>
<mass value="0.510299"/>
<inertia ixx="0.00030528937" ixy="7.788013e-06" ixz="2.2016e-07" iyy="0.000590894859" iyz="1.7175e-08" izz="0.000396594572"/>
</inertial>
</link>
<joint name="FR_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 -0.08 0"/>
<parent link="FR_hip"/>
<child link="FR_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="FR_thigh_shoulder">
<!-- <collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.032" radius="0.041"/>
</geometry>
</collision> -->
</link>
<joint name="FR_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.08 0"/>
<parent link="FR_hip"/>
<child link="FR_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="28"/>
</joint>
<link name="FR_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/thigh_mirror.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.0245 0.034"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003468 0.018947 -0.032736"/>
<mass value="0.898919"/>
<inertia ixx="0.005395867678" ixy="-1.02809e-07" ixz="0.000337529085" iyy="0.005142451046" iyz="5.816563e-06" izz="0.00102478732"/>
</inertial>
</link>
<joint name="FR_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="FR_thigh"/>
<child link="FR_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="28"/>
</joint>
<link name="FR_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.016 0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.006286 0.001307 -0.122269"/>
<mass value="0.158015"/>
<inertia ixx="0.003607648222" ixy="1.494971e-06" ixz="-0.000132778525" iyy="0.003626771492" iyz="-2.8638535e-05" izz="3.5148003e-05"/>
</inertial>
</link>
<joint name="FR_foot_fixed" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="FR_calf"/>
<child link="FR_foot"/>
</joint>
<link name="FR_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
</inertial>
</link>
<joint name="FL_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="0.1881 0.04675 0"/>
<parent link="trunk"/>
<child link="FL_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="50"/>
</joint>
<link name="FL_hip">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<!-- <collision>
<origin rpy="1.57079632679 0 0" xyz="0 0.045 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision> -->
<inertial>
<origin rpy="0 0 0" xyz="-0.00541 -0.00074 6e-06"/>
<mass value="0.510299"/>
<inertia ixx="0.00030528937" ixy="-7.788013e-06" ixz="2.2016e-07" iyy="0.000590894859" iyz="-1.7175e-08" izz="0.000396594572"/>
</inertial>
</link>
<joint name="FL_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0.08 0"/>
<parent link="FL_hip"/>
<child link="FL_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="FL_thigh_shoulder">
<!-- <collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.032" radius="0.041"/>
</geometry>
</collision> -->
</link>
<joint name="FL_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0.08 0"/>
<parent link="FL_hip"/>
<child link="FL_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="28"/>
</joint>
<link name="FL_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/thigh.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.0245 0.034"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003468 -0.018947 -0.032736"/>
<mass value="0.898919"/>
<inertia ixx="0.005395867678" ixy="1.02809e-07" ixz="0.000337529085" iyy="0.005142451046" iyz="-5.816563e-06" izz="0.00102478732"/>
</inertial>
</link>
<joint name="FL_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="FL_thigh"/>
<child link="FL_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="28"/>
</joint>
<link name="FL_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.016 0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.006286 0.001307 -0.122269"/>
<mass value="0.158015"/>
<inertia ixx="0.003607648222" ixy="1.494971e-06" ixz="-0.000132778525" iyy="0.003626771492" iyz="-2.8638535e-05" izz="3.5148003e-05"/>
</inertial>
</link>
<joint name="FL_foot_fixed" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="FL_calf"/>
<child link="FL_foot"/>
</joint>
<link name="FL_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
</inertial>
</link>
<joint name="RR_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.1881 -0.04675 0"/>
<parent link="trunk"/>
<child link="RR_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="50"/>
</joint>
<link name="RR_hip">
<visual>
<origin rpy="3.14159265359 3.14159265359 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="1.57079632679 0 0" xyz="0 -0.045 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.00541 0.00074 6e-06"/>
<mass value="0.510299"/>
<inertia ixx="0.00030528937" ixy="-7.788013e-06" ixz="-2.2016e-07" iyy="0.000590894859" iyz="1.7175e-08" izz="0.000396594572"/>
</inertial>
</link>
<joint name="RR_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 -0.08 0"/>
<parent link="RR_hip"/>
<child link="RR_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="RR_thigh_shoulder">
<!-- <collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.032" radius="0.041"/>
</geometry>
</collision> -->
</link>
<joint name="RR_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 -0.08 0"/>
<parent link="RR_hip"/>
<child link="RR_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="28"/>
</joint>
<link name="RR_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/thigh_mirror.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.0245 0.034"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003468 0.018947 -0.032736"/>
<mass value="0.898919"/>
<inertia ixx="0.005395867678" ixy="-1.02809e-07" ixz="0.000337529085" iyy="0.005142451046" iyz="5.816563e-06" izz="0.00102478732"/>
</inertial>
</link>
<joint name="RR_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="RR_thigh"/>
<child link="RR_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="28"/>
</joint>
<link name="RR_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.016 0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.006286 0.001307 -0.122269"/>
<mass value="0.158015"/>
<inertia ixx="0.003607648222" ixy="1.494971e-06" ixz="-0.000132778525" iyy="0.003626771492" iyz="-2.8638535e-05" izz="3.5148003e-05"/>
</inertial>
</link>
<joint name="RR_foot_fixed" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="RR_calf"/>
<child link="RR_foot"/>
</joint>
<link name="RR_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
</inertial>
</link>
<joint name="RL_hip_joint" type="revolute">
<origin rpy="0 0 0" xyz="-0.1881 0.04675 0"/>
<parent link="trunk"/>
<child link="RL_hip"/>
<axis xyz="1 0 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="33.5" lower="-0.802851455917" upper="0.802851455917" velocity="50"/>
</joint>
<link name="RL_hip">
<visual>
<origin rpy="0 3.14159265359 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/hip.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<!-- <collision>
<origin rpy="1.57079632679 0 0" xyz="0 0.045 0"/>
<geometry>
<cylinder length="0.04" radius="0.046"/>
</geometry>
</collision> -->
<inertial>
<origin rpy="0 0 0" xyz="0.00541 -0.00074 6e-06"/>
<mass value="0.510299"/>
<inertia ixx="0.00030528937" ixy="7.788013e-06" ixz="-2.2016e-07" iyy="0.000590894859" iyz="-1.7175e-08" izz="0.000396594572"/>
</inertial>
</link>
<joint name="RL_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0.08 0"/>
<parent link="RL_hip"/>
<child link="RL_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="RL_thigh_shoulder">
<!-- <collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.032" radius="0.041"/>
</geometry>
</collision> -->
</link>
<joint name="RL_thigh_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0.08 0"/>
<parent link="RL_hip"/>
<child link="RL_thigh"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="33.5" lower="-1.0471975512" upper="4.18879020479" velocity="28"/>
</joint>
<link name="RL_thigh">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/thigh.stl" scale="1 1 1"/>
</geometry>
<material name="silver"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.0245 0.034"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003468 -0.018947 -0.032736"/>
<mass value="0.898919"/>
<inertia ixx="0.005395867678" ixy="1.02809e-07" ixz="0.000337529085" iyy="0.005142451046" iyz="-5.816563e-06" izz="0.00102478732"/>
</inertial>
</link>
<joint name="RL_calf_joint" type="revolute">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="RL_thigh"/>
<child link="RL_calf"/>
<axis xyz="0 1 0"/>
<dynamics damping="0" friction="0"/>
<limit effort="33.5" lower="-2.69653369433" upper="-0.916297857297" velocity="28"/>
</joint>
<link name="RL_calf">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="../meshes/calf.stl" scale="1 1 1"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.1065"/>
<geometry>
<box size="0.213 0.016 0.016"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.006286 0.001307 -0.122269"/>
<mass value="0.158015"/>
<inertia ixx="0.003607648222" ixy="1.494971e-06" ixz="-0.000132778525" iyy="0.003626771492" iyz="-2.8638535e-05" izz="3.5148003e-05"/>
</inertial>
</link>
<joint name="RL_foot_fixed" type="fixed" dont_collapse="true">
<origin rpy="0 0 0" xyz="0 0 -0.213"/>
<parent link="RL_calf"/>
<child link="RL_foot"/>
</joint>
<link name="RL_foot">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.01"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<sphere radius="0.02"/>
</geometry>
</collision>
<inertial>
<mass value="0.06"/>
<inertia ixx="9.6e-06" ixy="0.0" ixz="0.0" iyy="9.6e-06" iyz="0.0" izz="9.6e-06"/>
</inertial>
</link>
</robot>
resources/robots/go1/xml/go1.xml
+195
View File
@@ -0,0 +1,195 @@
<mujoco model="go1_description">
<compiler angle="radian" meshdir="../meshes/" />
<size njmax="500" nconmax="100" />
<option gravity='0 0 -9.806' iterations='50' solver='Newton' timestep='0.002'/>
<default>
<geom contype="1" conaffinity="1" friction="0.6 0.3 0.3" rgba="0.5 0.6 0.7 1" margin="0.001" group="0"/>
<light castshadow="false" diffuse="1 1 1"/>
<motor ctrlrange="-33.5 33.5" ctrllimited="true"/>
<camera fovy="60"/>
<joint damping="0.01" armature="0.01" frictionloss="0.2" />
</default>
=
<asset>
<mesh name="trunk" file="trunk.stl" />
<mesh name="hip" file="hip.stl" />
<mesh name="thigh_mirror" file="thigh_mirror.stl" />
<mesh name="calf" file="calf.stl" />
<mesh name="thigh" file="thigh.stl" />
</asset>
<asset>
<texture type="skybox" builtin="gradient" rgb1="1.0 1.0 1.0" rgb2="1.0 1.0 1.0" width="512" height="512"/>
<texture name="plane" type="2d" builtin="flat" rgb1="1 1 1" rgb2="1 1 1" width="512" height="512" mark="cross" markrgb="0 0 0"/>
<material name="plane" reflectance="0.0" texture="plane" texrepeat="3 3" texuniform="true"/>
</asset>
<visual>
<rgba com="0.502 1.0 0 0.5" contactforce="0.98 0.4 0.4 0.7" contactpoint="1.0 1.0 0.6 0.4"/>
<scale com="0.2" forcewidth="0.035" contactwidth="0.10" contactheight="0.04"/>
</visual>
<worldbody>
<light directional="true" diffuse=".8 .8 .8" pos="0 0 10" dir="0 0 -10"/>
<camera name="track" mode="trackcom" pos="0 -1.3 1.6" xyaxes="1 0 0 0 0.707 0.707"/>
<geom name='floor' type='plane' conaffinity='1' condim='3' contype='1' rgba="0.5 0.9 0.9 0.1" material='plane' pos='0 0 0' size='0 0 1'/>
<body name="trunk" pos="0 0 0.35">
<inertial pos="0.0116053 0.00442221 0.000106692" quat="0.0111438 0.707126 -0.00935374 0.706938" mass="4.801" diaginertia="0.0447997 0.0366257 0.0162187" />
<joint type="free" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="trunk" />
<geom size="0.13 0.04675 0.057" type="box" rgba="0.913725 0.913725 0.847059 1" />
<geom size="0.0005 0.0005 0.0005" pos="-0.01592 -0.06659 -0.00617" type="box" contype="0" conaffinity="0" group="1" rgba="0.8 0 0 0" />
<geom size="0.0005 0.0005 0.0005" pos="-0.01592 -0.06659 -0.00617" type="box" rgba="0.8 0 0 0" />
<site name="imu" pos="0 0 0"/>
<body name="FR_hip" pos="0.1881 -0.04675 0">
<inertial pos="-0.00406411 -0.0193463 4.50733e-06" quat="0.467526 0.531662 -0.466259 0.530431" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
<joint name="FR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
<geom quat="0 1 0 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
<geom size="0.046 0.02" pos="0 -0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
<geom size="0.031 0.02" pos="0 -0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
<body name="FR_thigh" pos="0 -0.08 0">
<inertial pos="-0.003468 0.018947 -0.032736" quat="0.999266 0.00067676 -0.0382978 0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
<joint name="FR_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh_mirror" />
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
<body name="FR_calf" pos="0 0 -0.213">
<inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
<joint name="FR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
<geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
<geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
<geom size="0.02" pos="0 0 -0.213" rgba="0 0 0 1" />
</body>
</body>
</body>
<body name="FL_hip" pos="0.1881 0.04675 0">
<inertial pos="-0.00406411 0.0193463 4.50733e-06" quat="0.531662 0.467526 -0.530431 0.466259" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
<joint name="FL_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
<geom size="0.046 0.02" pos="0 0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
<geom size="0.031 0.02" pos="0 0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
<body name="FL_thigh" pos="0 0.08 0">
<inertial pos="-0.003468 -0.018947 -0.032736" quat="0.999266 -0.00067676 -0.0382978 -0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
<joint name="FL_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh" />
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
<body name="FL_calf" pos="0 0 -0.213">
<inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
<joint name="FL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
<geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
<geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
<geom size="0.02" pos="0 0 -0.213" rgba="0 0 0 1" />
</body>
</body>
</body>
<body name="RR_hip" pos="-0.1881 -0.04675 0">
<inertial pos="0.00406411 -0.0193463 4.50733e-06" quat="0.530431 0.466259 -0.531662 0.467526" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
<joint name="RR_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
<geom quat="0 0 0 -1" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
<geom size="0.046 0.02" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 1" />
<geom size="0.046 0.02" pos="0 -0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
<geom size="0.031 0.02" pos="0 -0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
<body name="RR_thigh" pos="0 -0.08 0">
<inertial pos="-0.003468 0.018947 -0.032736" quat="0.999266 0.00067676 -0.0382978 0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
<joint name="RR_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh_mirror" />
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
<body name="RR_calf" pos="0 0 -0.213">
<inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
<joint name="RR_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
<geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
<geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
<geom size="0.02" pos="0 0 -0.213" rgba="0 0 0 1" />
</body>
</body>
</body>
<body name="RL_hip" pos="-0.1881 0.04675 0">
<inertial pos="0.00406411 0.0193463 4.50733e-06" quat="0.466259 0.530431 -0.467526 0.531662" mass="0.679292" diaginertia="0.00131334 0.00122648 0.000728484" />
<joint name="RL_hip_joint" pos="0 0 0" axis="1 0 0" limited="true" range="-0.802851 0.802851" />
<geom quat="0 0 1 0" type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="hip" />
<geom size="0.046 0.02" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 1" />
<geom size="0.046 0.02" pos="0 0.045 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
<geom size="0.031 0.02" pos="0 0.07 0" quat="0.707107 0.707107 0 0" type="cylinder" rgba="0.913725 0.913725 0.847059 0" />
<body name="RL_thigh" pos="0 0.08 0">
<inertial pos="-0.003468 -0.018947 -0.032736" quat="0.999266 -0.00067676 -0.0382978 -0.000639813" mass="0.898919" diaginertia="0.00542178 0.00514246 0.000998869" />
<joint name="RL_thigh_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-1.0472 4.18879" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0.913725 0.913725 0.847059 1" mesh="thigh" />
<geom size="0.1065 0.01225 0.017" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0.913725 0.913725 0.847059 0" />
<body name="RL_calf" pos="0 0 -0.213">
<inertial pos="0.00455603 0.0009473 -0.147239" quat="0.762045 0.00970173 0.0180098 0.647201" mass="0.218015" diaginertia="0.00399678 0.00398122 3.99428e-05" />
<joint name="RL_calf_joint" pos="0 0 0" axis="0 1 0" limited="true" range="-2.69653 -0.916298" />
<geom type="mesh" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" mesh="calf" />
<geom size="0.1065 0.008 0.008" pos="0 0 -0.1065" quat="0.707107 0 0.707107 0" type="box" rgba="0 0 0 0" />
<geom size="0.01" pos="0 0 -0.213" contype="0" conaffinity="0" group="1" rgba="0 0 0 1" />
<geom size="0.02" pos="0 0 -0.213" rgba="0 0 0 1" />
</body>
</body>
</body>
</body>
</worldbody>
<actuator>
<motor name="FR_hip" gear="1" joint="FR_hip_joint"/>
<motor name="FR_thigh" gear="1" joint="FR_thigh_joint"/>
<motor name="FR_calf" gear="1" joint="FR_calf_joint"/>
<motor name="FL_hip" gear="1" joint="FL_hip_joint"/>
<motor name="FL_thigh" gear="1" joint="FL_thigh_joint"/>
<motor name="FL_calf" gear="1" joint="FL_calf_joint"/>
<motor name="RR_hip" gear="1" joint="RR_hip_joint"/>
<motor name="RR_thigh" gear="1" joint="RR_thigh_joint"/>
<motor name="RR_calf" gear="1" joint="RR_calf_joint" />
<motor name="RL_hip" gear="1" joint="RL_hip_joint"/>
<motor name="RL_thigh" gear="1" joint="RL_thigh_joint"/>
<motor name="RL_calf" gear="1" joint="RL_calf_joint"/>
</actuator>
<sensor>
<jointpos name="FR_hip_pos" joint="FR_hip_joint"/>
<jointpos name="FR_thigh_pos" joint="FR_thigh_joint"/>
<jointpos name="FR_calf_pos" joint="FR_calf_joint"/>
<jointpos name="FL_hip_pos" joint="FL_hip_joint"/>
<jointpos name="FL_thigh_pos" joint="FL_thigh_joint"/>
<jointpos name="FL_calf_pos" joint="FL_calf_joint"/>
<jointpos name="RR_hip_pos" joint="RR_hip_joint"/>
<jointpos name="RR_thigh_pos" joint="RR_thigh_joint"/>
<jointpos name="RR_calf_pos" joint="RR_calf_joint" />
<jointpos name="RL_hip_pos" joint="RL_hip_joint"/>
<jointpos name="RL_thigh_pos" joint="RL_thigh_joint"/>
<jointpos name="RL_calf_pos" joint="RL_calf_joint"/>
<jointvel name="FR_hip_vel" joint="FR_hip_joint"/>
<jointvel name="FR_thigh_vel" joint="FR_thigh_joint"/>
<jointvel name="FR_calf_vel" joint="FR_calf_joint"/>
<jointvel name="FL_hip_vel" joint="FL_hip_joint"/>
<jointvel name="FL_thigh_vel" joint="FL_thigh_joint"/>
<jointvel name="FL_calf_vel" joint="FL_calf_joint"/>
<jointvel name="RR_hip_vel" joint="RR_hip_joint"/>
<jointvel name="RR_thigh_vel" joint="RR_thigh_joint"/>
<jointvel name="RR_calf_vel" joint="RR_calf_joint" />
<jointvel name="RL_hip_vel" joint="RL_hip_joint"/>
<jointvel name="RL_thigh_vel" joint="RL_thigh_joint"/>
<jointvel name="RL_calf_vel" joint="RL_calf_joint"/>
<accelerometer name="Body_Acc" site="imu"/>
<gyro name="Body_Gyro" site="imu"/>
<framepos name="Body_Pos" objtype=site objname="imu"/>
<framequat name="Body_Quat" objtype=site objname="imu"/>
</sensor>
</mujoco>
resources/robots/mini_cheetah/meshes/lower_leg.dae
Executable
+45396
View File
File diff suppressed because one or more lines are too long
resources/robots/mini_cheetah/meshes/lower_leg.dae.meta
Executable
+101
View File
@@ -0,0 +1,101 @@
fileFormatVersion: 2
guid: 1097cf594ed244591b250b735a5eb255
ModelImporter:
serializedVersion: 20101
internalIDToNameTable: []
externalObjects: {}
materials:
materialImportMode: 1
materialName: 0
materialSearch: 1
materialLocation: 1
animations:
legacyGenerateAnimations: 4
bakeSimulation: 0
resampleCurves: 1
optimizeGameObjects: 0
motionNodeName:
rigImportErrors:
rigImportWarnings:
animationImportErrors:
animationImportWarnings:
animationRetargetingWarnings:
animationDoRetargetingWarnings: 0
importAnimatedCustomProperties: 0
importConstraints: 0
animationCompression: 1
animationRotationError: 0.5
animationPositionError: 0.5
animationScaleError: 0.5
animationWrapMode: 0
extraExposedTransformPaths: []
extraUserProperties: []
clipAnimations: []
isReadable: 0
meshes:
lODScreenPercentages: []
globalScale: 1
meshCompression: 0
addColliders: 0
useSRGBMaterialColor: 1
sortHierarchyByName: 1
importVisibility: 1
importBlendShapes: 1
importCameras: 1
importLights: 1
fileIdsGeneration: 2
swapUVChannels: 0
generateSecondaryUV: 0
useFileUnits: 1
keepQuads: 0
weldVertices: 1
bakeAxisConversion: 0
preserveHierarchy: 0
skinWeightsMode: 0
maxBonesPerVertex: 4
minBoneWeight: 0.001
meshOptimizationFlags: -1
indexFormat: 0
secondaryUVAngleDistortion: 8
secondaryUVAreaDistortion: 15.000001
secondaryUVHardAngle: 88
secondaryUVMarginMethod: 1
secondaryUVMinLightmapResolution: 40
secondaryUVMinObjectScale: 1
secondaryUVPackMargin: 4
useFileScale: 1
tangentSpace:
normalSmoothAngle: 60
normalImportMode: 0
tangentImportMode: 3
normalCalculationMode: 4
legacyComputeAllNormalsFromSmoothingGroupsWhenMeshHasBlendShapes: 0
blendShapeNormalImportMode: 1
normalSmoothingSource: 0
referencedClips: []
importAnimation: 1
humanDescription:
serializedVersion: 3
human: []
skeleton: []
armTwist: 0.5
foreArmTwist: 0.5
upperLegTwist: 0.5
legTwist: 0.5
armStretch: 0.05
legStretch: 0.05
feetSpacing: 0
globalScale: 1
rootMotionBoneName:
hasTranslationDoF: 0
hasExtraRoot: 0
skeletonHasParents: 1
lastHumanDescriptionAvatarSource: {instanceID: 0}
autoGenerateAvatarMappingIfUnspecified: 1
animationType: 2
humanoidOversampling: 1
avatarSetup: 0
additionalBone: 0
userData:
assetBundleName:
assetBundleVariant:
resources/robots/mini_cheetah/meshes/mini_abad.obj
Executable
+10259
View File
File diff suppressed because it is too large Load Diff
resources/robots/mini_cheetah/meshes/mini_abad.obj.meta
Executable
+97
View File
@@ -0,0 +1,97 @@
fileFormatVersion: 2
guid: 4ee26848c5aa74acea976c348931fd00
ModelImporter:
serializedVersion: 19301
internalIDToNameTable: []
externalObjects: {}
materials:
materialImportMode: 1
materialName: 0
materialSearch: 1
materialLocation: 1
animations:
legacyGenerateAnimations: 4
bakeSimulation: 0
resampleCurves: 1
optimizeGameObjects: 0
motionNodeName:
rigImportErrors:
rigImportWarnings:
animationImportErrors:
animationImportWarnings:
animationRetargetingWarnings:
animationDoRetargetingWarnings: 0
importAnimatedCustomProperties: 0
importConstraints: 0
animationCompression: 1
animationRotationError: 0.5
animationPositionError: 0.5
animationScaleError: 0.5
animationWrapMode: 0
extraExposedTransformPaths: []
extraUserProperties: []
clipAnimations: []
isReadable: 0
meshes:
lODScreenPercentages: []
globalScale: 1
meshCompression: 0
addColliders: 0
useSRGBMaterialColor: 1
sortHierarchyByName: 1
importVisibility: 1
importBlendShapes: 1
importCameras: 1
importLights: 1
fileIdsGeneration: 2
swapUVChannels: 0
generateSecondaryUV: 0
useFileUnits: 1
keepQuads: 0
weldVertices: 1
preserveHierarchy: 0
skinWeightsMode: 0
maxBonesPerVertex: 4
minBoneWeight: 0.001
meshOptimizationFlags: -1
indexFormat: 0
secondaryUVAngleDistortion: 8
secondaryUVAreaDistortion: 15.000001
secondaryUVHardAngle: 88
secondaryUVPackMargin: 4
useFileScale: 1
tangentSpace:
normalSmoothAngle: 60
normalImportMode: 0
tangentImportMode: 3
normalCalculationMode: 4
legacyComputeAllNormalsFromSmoothingGroupsWhenMeshHasBlendShapes: 0
blendShapeNormalImportMode: 1
normalSmoothingSource: 0
referencedClips: []
importAnimation: 1
humanDescription:
serializedVersion: 3
human: []
skeleton: []
armTwist: 0.5
foreArmTwist: 0.5
upperLegTwist: 0.5
legTwist: 0.5
armStretch: 0.05
legStretch: 0.05
feetSpacing: 0
globalScale: 1
rootMotionBoneName:
hasTranslationDoF: 0
hasExtraRoot: 0
skeletonHasParents: 1
lastHumanDescriptionAvatarSource: {instanceID: 0}
autoGenerateAvatarMappingIfUnspecified: 1
animationType: 2
humanoidOversampling: 1
avatarSetup: 0
additionalBone: 0
userData:
assetBundleName:
assetBundleVariant:
resources/robots/mini_cheetah/meshes/mini_body.obj
Executable
+37168
View File
File diff suppressed because it is too large Load Diff
resources/robots/mini_cheetah/meshes/mini_body.obj.meta
Executable
+97
View File
@@ -0,0 +1,97 @@
fileFormatVersion: 2
guid: 664a786199a0d4e88a04f46cb19e0c08
ModelImporter:
serializedVersion: 19301
internalIDToNameTable: []
externalObjects: {}
materials:
materialImportMode: 1
materialName: 0
materialSearch: 1
materialLocation: 1
animations:
legacyGenerateAnimations: 4
bakeSimulation: 0
resampleCurves: 1
optimizeGameObjects: 0
motionNodeName:
rigImportErrors:
rigImportWarnings:
animationImportErrors:
animationImportWarnings:
animationRetargetingWarnings:
animationDoRetargetingWarnings: 0
importAnimatedCustomProperties: 0
importConstraints: 0
animationCompression: 1
animationRotationError: 0.5
animationPositionError: 0.5
animationScaleError: 0.5
animationWrapMode: 0
extraExposedTransformPaths: []
extraUserProperties: []
clipAnimations: []
isReadable: 0
meshes:
lODScreenPercentages: []
globalScale: 1
meshCompression: 0
addColliders: 0
useSRGBMaterialColor: 1
sortHierarchyByName: 1
importVisibility: 1
importBlendShapes: 1
importCameras: 1
importLights: 1
fileIdsGeneration: 2
swapUVChannels: 0
generateSecondaryUV: 0
useFileUnits: 1
keepQuads: 0
weldVertices: 1
preserveHierarchy: 0
skinWeightsMode: 0
maxBonesPerVertex: 4
minBoneWeight: 0.001
meshOptimizationFlags: -1
indexFormat: 0
secondaryUVAngleDistortion: 8
secondaryUVAreaDistortion: 15.000001
secondaryUVHardAngle: 88
secondaryUVPackMargin: 4
useFileScale: 1
tangentSpace:
normalSmoothAngle: 60
normalImportMode: 0
tangentImportMode: 3
normalCalculationMode: 4
legacyComputeAllNormalsFromSmoothingGroupsWhenMeshHasBlendShapes: 0
blendShapeNormalImportMode: 1
normalSmoothingSource: 0
referencedClips: []
importAnimation: 1
humanDescription:
serializedVersion: 3
human: []
skeleton: []
armTwist: 0.5
foreArmTwist: 0.5
upperLegTwist: 0.5
legTwist: 0.5
armStretch: 0.05
legStretch: 0.05
feetSpacing: 0
globalScale: 1
rootMotionBoneName:
hasTranslationDoF: 0
hasExtraRoot: 0
skeletonHasParents: 1
lastHumanDescriptionAvatarSource: {instanceID: 0}
autoGenerateAvatarMappingIfUnspecified: 1
animationType: 2
humanoidOversampling: 1
avatarSetup: 0
additionalBone: 0
userData:
assetBundleName:
assetBundleVariant:
resources/robots/mini_cheetah/meshes/mini_lower_link.obj
Executable
+32069
View File
File diff suppressed because it is too large Load Diff
resources/robots/mini_cheetah/meshes/mini_lower_link.obj.meta
Executable
+97
View File
@@ -0,0 +1,97 @@
fileFormatVersion: 2
guid: dd79403848d3a4b4888e9c9bbf98610a
ModelImporter:
serializedVersion: 19301
internalIDToNameTable: []
externalObjects: {}
materials:
materialImportMode: 1
materialName: 0
materialSearch: 1
materialLocation: 1
animations:
legacyGenerateAnimations: 4
bakeSimulation: 0
resampleCurves: 1
optimizeGameObjects: 0
motionNodeName:
rigImportErrors:
rigImportWarnings:
animationImportErrors:
animationImportWarnings:
animationRetargetingWarnings:
animationDoRetargetingWarnings: 0
importAnimatedCustomProperties: 0
importConstraints: 0
animationCompression: 1
animationRotationError: 0.5
animationPositionError: 0.5
animationScaleError: 0.5
animationWrapMode: 0
extraExposedTransformPaths: []
extraUserProperties: []
clipAnimations: []
isReadable: 0
meshes:
lODScreenPercentages: []
globalScale: 1
meshCompression: 0
addColliders: 0
useSRGBMaterialColor: 1
sortHierarchyByName: 1
importVisibility: 1
importBlendShapes: 1
importCameras: 1
importLights: 1
fileIdsGeneration: 2
swapUVChannels: 0
generateSecondaryUV: 0
useFileUnits: 1
keepQuads: 0
weldVertices: 1
preserveHierarchy: 0
skinWeightsMode: 0
maxBonesPerVertex: 4
minBoneWeight: 0.001
meshOptimizationFlags: -1
indexFormat: 0
secondaryUVAngleDistortion: 8
secondaryUVAreaDistortion: 15.000001
secondaryUVHardAngle: 88
secondaryUVPackMargin: 4
useFileScale: 1
tangentSpace:
normalSmoothAngle: 60
normalImportMode: 0
tangentImportMode: 3
normalCalculationMode: 4
legacyComputeAllNormalsFromSmoothingGroupsWhenMeshHasBlendShapes: 0
blendShapeNormalImportMode: 1
normalSmoothingSource: 0
referencedClips: []
importAnimation: 1
humanDescription:
serializedVersion: 3
human: []
skeleton: []
armTwist: 0.5
foreArmTwist: 0.5
upperLegTwist: 0.5
legTwist: 0.5
armStretch: 0.05
legStretch: 0.05
feetSpacing: 0
globalScale: 1
rootMotionBoneName:
hasTranslationDoF: 0
hasExtraRoot: 0
skeletonHasParents: 1
lastHumanDescriptionAvatarSource: {instanceID: 0}
autoGenerateAvatarMappingIfUnspecified: 1
animationType: 2
humanoidOversampling: 1
avatarSetup: 0
additionalBone: 0
userData:
assetBundleName:
assetBundleVariant:
resources/robots/mini_cheetah/meshes/mini_upper_link.obj
Executable
+45910
View File
File diff suppressed because it is too large Load Diff
resources/robots/mini_cheetah/meshes/mini_upper_link.obj.meta
Executable
+97
View File
@@ -0,0 +1,97 @@
fileFormatVersion: 2
guid: 35947fc8881f9458a8a9cd250ca07d76
ModelImporter:
serializedVersion: 19301
internalIDToNameTable: []
externalObjects: {}
materials:
materialImportMode: 1
materialName: 0
materialSearch: 1
materialLocation: 1
animations:
legacyGenerateAnimations: 4
bakeSimulation: 0
resampleCurves: 1
optimizeGameObjects: 0
motionNodeName:
rigImportErrors:
rigImportWarnings:
animationImportErrors:
animationImportWarnings:
animationRetargetingWarnings:
animationDoRetargetingWarnings: 0
importAnimatedCustomProperties: 0
importConstraints: 0
animationCompression: 1
animationRotationError: 0.5
animationPositionError: 0.5
animationScaleError: 0.5
animationWrapMode: 0
extraExposedTransformPaths: []
extraUserProperties: []
clipAnimations: []
isReadable: 0
meshes:
lODScreenPercentages: []
globalScale: 1
meshCompression: 0
addColliders: 0
useSRGBMaterialColor: 1
sortHierarchyByName: 1
importVisibility: 1
importBlendShapes: 1
importCameras: 1
importLights: 1
fileIdsGeneration: 2
swapUVChannels: 0
generateSecondaryUV: 0
useFileUnits: 1
keepQuads: 0
weldVertices: 1
preserveHierarchy: 0
skinWeightsMode: 0
maxBonesPerVertex: 4
minBoneWeight: 0.001
meshOptimizationFlags: -1
indexFormat: 0
secondaryUVAngleDistortion: 8
secondaryUVAreaDistortion: 15.000001
secondaryUVHardAngle: 88
secondaryUVPackMargin: 4
useFileScale: 1
tangentSpace:
normalSmoothAngle: 60
normalImportMode: 0
tangentImportMode: 3
normalCalculationMode: 4
legacyComputeAllNormalsFromSmoothingGroupsWhenMeshHasBlendShapes: 0
blendShapeNormalImportMode: 1
normalSmoothingSource: 0
referencedClips: []
importAnimation: 1
humanDescription:
serializedVersion: 3
human: []
skeleton: []
armTwist: 0.5
foreArmTwist: 0.5
upperLegTwist: 0.5
legTwist: 0.5
armStretch: 0.05
legStretch: 0.05
feetSpacing: 0
globalScale: 1
rootMotionBoneName:
hasTranslationDoF: 0
hasExtraRoot: 0
skeletonHasParents: 1
lastHumanDescriptionAvatarSource: {instanceID: 0}
autoGenerateAvatarMappingIfUnspecified: 1
animationType: 2
humanoidOversampling: 1
avatarSetup: 0
additionalBone: 0
userData:
assetBundleName:
assetBundleVariant:
resources/robots/mini_cheetah/urdf/meshes/mini_abad.obj
Executable
+10259
View File
File diff suppressed because it is too large Load Diff
resources/robots/mini_cheetah/urdf/meshes/mini_body.obj
Executable
+37168
View File
File diff suppressed because it is too large Load Diff
resources/robots/mini_cheetah/urdf/meshes/mini_lower_link.obj
Executable
+32069
View File
File diff suppressed because it is too large Load Diff
resources/robots/mini_cheetah/urdf/meshes/mini_upper_link.obj
Executable
+45910
View File
File diff suppressed because it is too large Load Diff
resources/robots/mini_cheetah/urdf/mini_cheetah.urdf
Executable
+457
View File
@@ -0,0 +1,457 @@
<?xml version="1.0" ?>
<robot name="mini_cheetah" xmlns:xacro="http://ros.org/wiki/xacro">
<material name="black">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
<material name="blue">
<color rgba="0.0 0.0 0.8 1.0"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.2 0.2 0.2 1.0"/>
</material>
<material name="silver">
<color rgba="0.913725490196 0.913725490196 0.847058823529 1.0"/>
</material>
<material name="orange">
<!-- <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/> -->
<color rgba="0.12 0.15 0.2 1.0"/>
</material>
<material name="brown">
<color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
</material>
<material name="red">
<color rgba="0.8 0.0 0.0 1.0"/>
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
<inertial>
<mass value="3.3"/>
<origin xyz="0.0 0.0 0.0"/>
<inertia ixx="0.011253" ixy="0" ixz="0" iyy="0.362030" iyz="0" izz="0.042673"/>
</inertial>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<geometry>
<mesh filename="meshes/mini_body.obj"/>
</geometry>
<origin rpy="0 0.0 0" xyz="0.0 0.0 0.0"/>
</visual>
<!-- <collision>
<geometry>
<mesh filename="meshes/mini_body.obj"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision> -->
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.23 0.18 0.1"/>
</geometry>
</collision>
</link>
<joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<material name="red"/>
</visual>
</link>
<!--!!!!!!!!!!!! Front Right Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="FR_hip_joint" type="revolute">
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="0.19 -0.049 0.0"/>
<parent link="trunk"/>
<child link="FR_hip"/>
<!-- <actuator gear_ratio="6" voltage="24">
<friction damping="0.01" dry_friction="0.2"/>
<motor Kt="0.05" R="0.173" TauMax="3"/>
<rotor_inertia ixx="33.0e-6" iyy="0.000033" izz="63e-6" iyz="0.0" ixy="0" ixz="0" />
</actuator> -->
<dynamics damping="0" friction="0"/>
<limit effort="18" lower="-1.6" upper="1.6" velocity="40"/>
</joint>
<link name="FR_hip">
<inertial>
<mass value="0.54"/>
<origin xyz="0.0 0.036 0."/>
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="3.141592 0.0 1.5708" xyz="-0.055 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="3.141592 0 1.5708" xyz="-0.055 0 0"/>
</collision>
</link>
<joint name="FR_thigh_joint" type="revolute">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.062 0.00"/>
<parent link="FR_hip"/>
<child link="FR_thigh"/>
<dynamics damping="0" friction="0"/>
<limit effort="18" lower="-2.6" upper="2.6" velocity="40"/>
</joint>
<link name="FR_thigh">
<inertial>
<mass value="0.634"/>
<origin xyz="0.0 0.016 -0.02"/>
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0.0 -1.5708 0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.105"/>
<geometry>
<box size="0.17 0.015 0.030"/>
</geometry>
</collision>
</link>
<joint name="FR_calf_joint" type="revolute">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
<parent link="FR_thigh"/>
<child link="FR_calf"/>
<dynamics damping="0" friction="0"/>
<limit effort="26" lower="-2.6" upper="2.6" velocity="26"/>
</joint>
<link name="FR_calf">
<inertial>
<mass value="0.064"/>
<origin xyz="0.0 0.0 -0.209"/>
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0 3.141592 0.0" xyz="0 0 0"/>
</collision>
</link>
<!--!!!!!!!!!!!! Front Left Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="FL_hip_joint" type="revolute">
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="0.19 0.049 0.0"/>
<parent link="trunk"/>
<child link="FL_hip"/>
<dynamics damping="0" friction="0"/>
<limit effort="18" lower="-1.6" upper="1.6" velocity="40"/>
</joint>
<link name="FL_hip">
<inertial>
<mass value="0.54"/>
<origin xyz="0.0 0.036 0."/>
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="0. 0. -1.5708" xyz="-0.055 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="0 0 -1.5708" xyz="-0.055 0 0"/>
</collision>
</link>
<joint name="FL_thigh_joint" type="revolute">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.062 0.00"/>
<parent link="FL_hip"/>
<child link="FL_thigh"/>
<dynamics damping="0" friction="0"/>
<limit effort="18" lower="-2.6" upper="2.6" velocity="40"/>
</joint>
<link name="FL_thigh">
<inertial>
<mass value="0.634"/>
<origin xyz="0.0 0.016 -0.02"/>
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.105"/>
<geometry>
<box size="0.17 0.015 0.030"/>
</geometry>
</collision>
</link>
<joint name="FL_calf_joint" type="revolute">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
<parent link="FL_thigh"/>
<child link="FL_calf"/>
<dynamics damping="0" friction="0"/>
<limit effort="26" lower="-2.6" upper="2.6" velocity="26"/>
</joint>
<link name="FL_calf">
<inertial>
<mass value="0.064"/>
<origin xyz="0.0 0.0 -0.209"/>
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0 3.141592 0.0" xyz="0 0 0"/>
</collision>
</link>
<!--!!!!!!!!!!!! Hind Right Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="RR_hip_joint" type="revolute">
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="-0.19 -0.049 0.0"/>
<parent link="trunk"/>
<child link="RR_hip"/>
<dynamics damping="0" friction="0"/>
<limit effort="18" lower="-1.6" upper="1.6" velocity="40"/>
</joint>
<link name="RR_hip">
<inertial>
<mass value="0.54"/>
<origin xyz="0.0 0.036 0."/>
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="0.0 0.0 1.5708" xyz="0.055 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="0 0 1.5708" xyz="0.055 0 0"/>
</collision>
</link>
<joint name="RR_thigh_joint" type="revolute">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.062 0.00"/>
<parent link="RR_hip"/>
<child link="RR_thigh"/>
<dynamics damping="0" friction="0"/>
<limit effort="18" lower="-2.6" upper="2.6" velocity="40"/>
</joint>
<link name="RR_thigh">
<inertial>
<mass value="0.634"/>
<origin xyz="0.0 0.016 -0.02"/>
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<!-- <collision>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0 -1.5708 0.0" xyz="0 0 0"/>
</collision> -->
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.105"/>
<geometry>
<box size="0.17 0.015 0.030"/>
</geometry>
</collision>
</link>
<joint name="RR_calf_joint" type="revolute">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
<parent link="RR_thigh"/>
<child link="RR_calf"/>
<dynamics damping="0" friction="0"/>
<limit effort="26" lower="-2.6" upper="2.6" velocity="26"/>
</joint>
<link name="RR_calf">
<inertial>
<mass value="0.064"/>
<origin xyz="0.0 0.0 -0.209"/>
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0 3.141592 0.0" xyz="0 0 0"/>
</collision>
</link>
<!--!!!!!!!!!!!! Hind Left Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="RL_hip_joint" type="revolute">
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="-0.19 0.049 0.0"/>
<parent link="trunk"/>
<child link="RL_hip"/>
<dynamics damping="0" friction="0"/>
<limit effort="18" lower="-1.6" upper="1.6" velocity="40"/>
</joint>
<link name="RL_hip">
<inertial>
<mass value="0.54"/>
<origin xyz="0.0 0.036 0."/>
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="3.141592 0.0 -1.5708" xyz="0.055 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_abad.obj"/>
</geometry>
<origin rpy="3.141592 0 -1.5708" xyz="0.055 0 0"/>
</collision>
</link>
<joint name="RL_thigh_joint" type="revolute">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.062 0.00"/>
<parent link="RL_hip"/>
<child link="RL_thigh"/>
<dynamics damping="0" friction="0"/>
<limit effort="18" lower="-2.6" upper="2.6" velocity="40"/>
</joint>
<link name="RL_thigh">
<inertial>
<mass value="0.634"/>
<origin xyz="0.0 0.016 -0.02"/>
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0.0 -1.5708 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<!-- <collision>
<geometry>
<mesh filename="meshes/mini_upper_link.obj"/>
</geometry>
<origin rpy="0 -1.5708 0.0" xyz="0 0 0"/>
</collision> -->
<collision>
<origin rpy="0 1.57079632679 0" xyz="0 0 -0.105"/>
<geometry>
<box size="0.17 0.015 0.030"/>
</geometry>
</collision>
</link>
<joint name="RL_calf_joint" type="revolute">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
<parent link="RL_thigh"/>
<child link="RL_calf"/>
<dynamics damping="0" friction="0"/>
<limit effort="26" lower="-2.6" upper="2.6" velocity="26"/>
</joint>
<link name="RL_calf">
<inertial>
<mass value="0.064"/>
<origin xyz="0.0 0.0 -0.209"/>
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
</inertial>
<visual>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<mesh filename="meshes/mini_lower_link.obj"/>
</geometry>
<origin rpy="0 3.141592 0.0" xyz="0 0 0"/>
</collision>
</link>
</robot>
resources/robots/mini_cheetah/urdf/mini_cheetah_simple.urdf
Executable
+714
View File
@@ -0,0 +1,714 @@
<?xml version="1.0" ?>
<robot name="mini_cheetah" xmlns:xacro="http://ros.org/wiki/xacro">
<material name="cheetah_material">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
<!--!!!!!!!!!!!!!!!!!!BODY!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<!-- <link name="trunk">
<inertial>
<mass value="3.3"/>
<origin xyz="0.0 0.0 0.0"/>
<inertia ixx="0.011253" ixy="0" ixz="0" iyy="0.036203" iyz="0" izz="0.042673"/>
</inertial>
<visual>
<geometry>
mesh filename="meshes/mini_body.obj"/!!!
<box size="0.23 0.18 0.1"/>
</geometry>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
mesh filename="meshes/mini_body.obj"/
<box size="0.23 0.18 0.1"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
<material name="cheetah_material"/>
</link>
-->
<link name="base">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0.012731 0.002186 0.000515"/>
<mass value="3.3"/>
<inertia ixx="0.01683993" ixy="8.3902e-05" ixz="0.000597679" iyy="0.056579028" iyz="2.5134e-05" izz="0.064713601"/>
</inertial>
</link>
<joint name="floating_base" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="base"/>
<child link="trunk"/>
</joint>
<link name="trunk">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.23 0.18 0.1"/>
</geometry>
<!-- <geometry>
<mesh filename="../meshes/trunk.dae" scale="1 1 1"/>
</geometry> -->
<material name="cheetah_material"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.23 0.18 0.1"/>
</geometry>
</collision>
<material name="cheetah_material"/>
</link>
<!-- <joint name="imu_joint" type="fixed">
<parent link="trunk"/>
<child link="imu_link"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
<link name="imu_link">
<inertial>
<mass value="0.001"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="0.001 0.001 0.001"/>
</geometry>
<material name="red"/>
</visual> -->
<!-- <collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size=".001 .001 .001"/>
</geometry>
</collision>
</link> -->
<!--!!!!!!!!!!!! Front Left Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="FL_hip_joint" type="revolute">
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="0.19 0.049 0.0"/>
<parent link="trunk"/>
<child link="FL_hip"/>
<limit effort="18" lower="-1.6" upper="1.6" velocity="40"/>
</joint>
<link name="FL_hip">
<material name="cheetah_material"/>
<inertial>
<mass value="0.54"/>
<origin xyz="0.0 0.036 0."/>
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
</inertial>
<visual>
<geometry>
<!--mesh filename="meshes/mini_abad.obj"/-->
<cylinder length ="0.01" radius = "0.0475"/>
</geometry>
<origin rpy="0. 1.5708 -1.5708" xyz="-0.055 0.0 0.0"/>
</visual>
<visual>
<geometry>
<!--mesh filename="meshes/mini_abad.obj"/-->
<cylinder length ="0.06" radius = "0.0425"/>
</geometry>
<!--origin rpy="3.141592 0.0 1.5708" xyz="-0.055 0.0 0.0"/-->
<origin rpy="1.5708 0.0 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<!--mesh filename="meshes/mini_abad.obj"/-->
<cylinder length ="0.01" radius = "0.0475"/>
</geometry>
<!--origin rpy="0 0 -1.5708" xyz="-0.055 0 0"/-->
<origin rpy="0 1.5708 -1.5708" xyz="-0.055 0.0 0.0"/>
</collision>
<collision>
<geometry>
<!--mesh filename="meshes/mini_abad.obj"/-->
<cylinder length ="0.06" radius = "0.0425"/>
</geometry>
<!--origin rpy="0 0 -1.5708" xyz="-0.055 0 0"/-->
<origin rpy="1.5708 0.0 0.0" xyz="0.0 0.0 0.0"/>
</collision>
</link>
<joint name="FL_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0.081 0"/>
<parent link="FL_hip"/>
<child link="FL_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="FL_thigh_shoulder">
<!-- <collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.032" radius="0.041"/>
</geometry>
</collision> -->
</link>
<joint name="FL_thigh_joint" type="revolute">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.062 0.00"/>
<parent link="FL_hip"/>
<child link="FL_thigh"/>
<limit effort="18" lower="-2.6" upper="2.6" velocity="40"/>
</joint>
<link name="FL_thigh">
<material name="cheetah_material"/>
<inertial>
<mass value="0.634"/>
<origin xyz="0.0 0.016 -0.02"/>
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
</inertial>
<visual>
<geometry>
<!--mesh filename="meshes/mini_upper_link.obj"/-->
<cylinder length ="0.17" radius = "0.015"/>
</geometry>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.105"/>
</visual>
<collision>
<geometry>
<!--mesh filename="meshes/mini_upper_link.obj"/-->
<cylinder length ="0.17" radius = "0.015"/>
</geometry>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.105"/>
</collision>
</link>
<joint name="FL_calf_joint" type="revolute">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
<parent link="FL_thigh"/>
<child link="FL_calf"/>
<limit effort="26" lower="-2.6" upper="2.6" velocity="26"/>
</joint>
<link name="FL_calf">
<material name="cheetah_material"/>
<inertial>
<mass value="0.064"/>
<origin xyz="0.0 0.0 -0.209"/>
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
</inertial>
<visual>
<geometry>
<!--mesh filename="meshes/mini_lower_link.obj"/-->
<cylinder length ="0.15" radius = "0.01"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 -0.095"/>
</visual>
<collision>
<geometry>
<!--mesh filename="meshes/mini_lower_link.obj"/-->
<cylinder length ="0.15" radius = "0.01"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0 0 -0.095"/>
</collision>
</link>
<!-- Adapter to Foot joint -->
<joint name="FL_foot_fixed" type="fixed">
<parent link="FL_calf"/>
<child link="FL_foot"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.19"/>
</joint>
<link name="FL_foot">
<material name="cheetah_material"/>
<inertial>
<mass value="0.0"/>
<inertia ixx="0.000025" ixy="0" ixz="0.0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
<visual>
<geometry>
<sphere radius = "0.0175"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<!--mesh filename="meshes/mini_lower_link.obj"/-->
<sphere radius = "0.0175"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0 0 0"/>
</collision>
</link>
<!--!!!!!!!!!!!! Front Right Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<!--!!!!Joint!!!!!!!!!!!!-->
<joint name="FR_hip_joint" type="revolute">
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="0.19 -0.049 0.0"/>
<parent link="trunk"/>
<child link="FR_hip"/>
<limit effort="18" lower="-1.6" upper="1.6" velocity="40"/>
</joint>
<link name="FR_hip">
<material name="cheetah_material"/>
<inertial>
<mass value="0.54"/>
<origin xyz="0.0 0.036 0."/>
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
</inertial>
<visual>
<geometry>
<!--mesh filename="meshes/mini_abad.obj"/-->
<cylinder length ="0.01" radius = "0.0475"/>
</geometry>
<!--origin rpy="3.141592 0.0 1.5708" xyz="-0.055 0.0 0.0"/-->
<origin rpy="3.141592 1.5708 1.5708" xyz="-0.055 0.0 0.0"/>
</visual>
<visual>
<geometry>
<!--mesh filename="meshes/mini_abad.obj"/-->
<cylinder length ="0.06" radius = "0.0425"/>
</geometry>
<!--origin rpy="3.141592 0.0 1.5708" xyz="-0.055 0.0 0.0"/-->
<origin rpy="1.5708 0.0 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<!--mesh filename="meshes/mini_abad.obj"/-->
<cylinder length ="0.01" radius = "0.0475"/>
</geometry>
<origin rpy="3.141592 1.5708 1.5708" xyz="-0.055 0 0"/>
</collision>
<collision>
<geometry>
<!--mesh filename="meshes/mini_abad.obj"/-->
<cylinder length ="0.06" radius = "0.0425"/>
</geometry>
<origin rpy="1.5708 0.0 0.0" xyz="-0.055 0 0"/>
</collision>
</link>
<joint name="FR_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0.081 0"/>
<parent link="FR_hip"/>
<child link="FR_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="FR_thigh_shoulder">
<!-- <collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.032" radius="0.041"/>
</geometry>
</collision> -->
</link>
<joint name="FR_thigh_joint" type="revolute">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.062 0.00"/>
<parent link="FR_hip"/>
<child link="FR_thigh"/>
<limit effort="18" lower="-2.6" upper="2.6" velocity="40"/>
</joint>
<link name="FR_thigh">
<material name="cheetah_material"/>
<inertial>
<mass value="0.634"/>
<origin xyz="0.0 0.016 -0.02"/>
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
</inertial>
<visual>
<geometry>
<!--mesh filename="meshes/mini_upper_link.obj"/-->
<cylinder length ="0.17" radius = "0.015"/>
</geometry>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.105"/>
</visual>
<collision>
<geometry>
<!--mesh filename="meshes/mini_upper_link.obj"/-->
<cylinder length ="0.17" radius = "0.015"/>
</geometry>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.105"/>
</collision>
</link>
<joint name="FR_calf_joint" type="revolute">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
<parent link="FR_thigh"/>
<child link="FR_calf"/>
<limit effort="26" lower="-2.6" upper="2.6" velocity="26"/>
</joint>
<link name="FR_calf">
<material name="cheetah_material"/>
<inertial>
<mass value="0.064"/>
<origin xyz="0.0 0.0 -0.209"/>
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
</inertial>
<visual>
<geometry>
<!--mesh filename="meshes/mini_lower_link.obj"/-->
<cylinder length ="0.15" radius = "0.01"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 -0.095"/>
</visual>
<collision>
<geometry>
<!--mesh filename="meshes/mini_lower_link.obj"/-->
<cylinder length ="0.15" radius = "0.01"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 -0.095"/>
</collision>
</link>
<!-- Adapter to Foot joint -->
<joint name="FR_foot_fixed" type="fixed">
<parent link="FR_calf"/>
<child link="FR_foot"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.19"/>
</joint>
<link name="FR_foot">
<material name="cheetah_material"/>
<inertial>
<mass value="0.0"/>
<inertia ixx="0.000025" ixy="0" ixz="0.0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
<visual>
<geometry>
<sphere radius = "0.0175"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<!--mesh filename="meshes/mini_lower_link.obj"/-->
<sphere radius = "0.0175"/>
</geometry>
<origin rpy="0.0 3.141592 0" xyz="0.0 0.0 0"/>
</collision>
</link>
<!--!!!!!!!!!!!! Hind Left Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="RL_hip_joint" type="revolute">
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="-0.19 0.049 0.0"/>
<parent link="trunk"/>
<child link="RL_hip"/>
<limit effort="18" lower="-1.6" upper="1.6" velocity="40"/>
</joint>
<link name="RL_hip">
<material name="cheetah_material"/>
<inertial>
<mass value="0.54"/>
<origin xyz="0.0 0.036 0."/>
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
</inertial>
<visual>
<geometry>
<!--mesh filename="meshes/mini_abad.obj"/-->
<cylinder length ="0.01" radius = "0.0475"/>
</geometry>
<!--origin rpy="3.141592 0.0 -1.5708" xyz="0.055 0.0 0.0"/-->
<origin rpy="3.141592 1.5708 -1.5708" xyz="0.055 0.0 0.0"/>
</visual>
<visual>
<geometry>
<!--mesh filename="meshes/mini_abad.obj"/-->
<cylinder length ="0.06" radius = "0.0425"/>
</geometry>
<!--origin rpy="3.141592 0.0 1.5708" xyz="-0.055 0.0 0.0"/-->
<origin rpy="1.5708 0.0 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<!--mesh filename="meshes/mini_abad.obj"/-->
<cylinder length ="0.01" radius = "0.0475"/>
</geometry>
<origin rpy="3.141592 1.5708 -1.5708" xyz="0.055 0 0"/>
</collision>
<collision>
<geometry>
<!--mesh filename="meshes/mini_abad.obj"/-->
<cylinder length ="0.06" radius = "0.0425"/>
</geometry>
<origin rpy="1.5708 0.0 0.0" xyz="0.0 0.0 0.0"/>
</collision>
</link>
<joint name="RL_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0.081 0"/>
<parent link="RL_hip"/>
<child link="RL_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="RL_thigh_shoulder">
<!-- <collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.032" radius="0.041"/>
</geometry>
</collision> -->
</link>
<joint name="RL_thigh_joint" type="revolute">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.062 0.00"/>
<parent link="RL_hip"/>
<child link="RL_thigh"/>
<limit effort="18" lower="-2.6" upper="2.6" velocity="40"/>
</joint>
<link name="RL_thigh">
<material name="cheetah_material"/>
<inertial>
<mass value="0.634"/>
<origin xyz="0.0 0.016 -0.02"/>
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
</inertial>
<visual>
<geometry>
<!--mesh filename="meshes/mini_upper_link.obj"/-->
<cylinder length ="0.17" radius = "0.015"/>
</geometry>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.105"/>
</visual>
<collision>
<geometry>
<!--mesh filename="meshes/mini_upper_link.obj"/-->
<cylinder length ="0.17" radius = "0.015"/>
</geometry>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.105"/>
</collision>
</link>
<joint name="RL_calf_joint" type="revolute">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
<parent link="RL_thigh"/>
<child link="RL_calf"/>
<limit effort="26" lower="-2.6" upper="2.6" velocity="26"/>
</joint>
<link name="RL_calf">
<material name="cheetah_material"/>
<inertial>
<mass value="0.064"/>
<origin xyz="0.0 0.0 -0.209"/>
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
</inertial>
<visual>
<geometry>
<!--mesh filename="meshes/mini_lower_link.obj"/-->
<cylinder length ="0.15" radius = "0.01"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 -0.095"/>
</visual>
<collision>
<geometry>
<!--mesh filename="meshes/mini_lower_link.obj"/-->
<cylinder length ="0.15" radius = "0.01"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 -0.095"/>
</collision>
</link>
<!-- Adapter to Foot joint -->
<joint name="RL_foot_fixed" type="fixed">
<parent link="RL_calf"/>
<child link="RL_foot"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.19"/>
</joint>
<link name="RL_foot">
<material name="cheetah_material"/>
<inertial>
<mass value="0.0"/>
<inertia ixx="0.000025" ixy="0" ixz="0.0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
<visual>
<geometry>
<sphere radius = "0.0175"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<!--mesh filename="meshes/mini_lower_link.obj"/-->
<sphere radius = "0.0175"/>
</geometry>
<origin rpy="0.0 3.141592 0" xyz="0.0 0.0 0"/>
</collision>
</link>
<!--!!!!!!!!!!!! Hind Right Leg !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!-->
<joint name="RR_hip_joint" type="revolute">
<axis xyz="1 0 0"/>
<origin rpy="0 0 0" xyz="-0.19 -0.049 0.0"/>
<parent link="trunk"/>
<child link="RR_hip"/>
<limit effort="18" lower="-1.6" upper="1.6" velocity="40"/>
</joint>
<link name="RR_hip">
<material name="cheetah_material"/>
<inertial>
<mass value="0.54"/>
<origin xyz="0.0 0.036 0."/>
<inertia ixx="0.000381" ixy="0.000058" ixz="0.00000045"
iyy="0.000560" iyz="0.00000095" izz="0.000444"/>
</inertial>
<visual>
<geometry>
<!--mesh filename="meshes/mini_abad.obj"/-->
<cylinder length ="0.01" radius = "0.0475"/>
</geometry>
<!--origin rpy="0.0 0.0 1.5708" xyz="0.055 0.0 0.0"/-->
<origin rpy="0.0 1.5708 1.5708" xyz="0.055 0.0 0.0"/>
</visual>
<visual>
<geometry>
<!--mesh filename="meshes/mini_abad.obj"/-->
<cylinder length ="0.06" radius = "0.0425"/>
</geometry>
<!--origin rpy="3.141592 0.0 1.5708" xyz="-0.055 0.0 0.0"/-->
<origin rpy="1.5708 0.0 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<!--mesh filename="meshes/mini_abad.obj"/-->
<cylinder length ="0.01" radius = "0.0475"/>
</geometry>
<origin rpy="0.0 1.5708 1.5708" xyz="0.055 0.0 0.0"/>
</collision>
<collision>
<geometry>
<!--mesh filename="meshes/mini_abad.obj"/-->
<cylinder length ="0.06" radius = "0.0425"/>
</geometry>
<origin rpy="1.5708 0.0 0.0" xyz="0.0 0.0 0.0"/>
</collision>
</link>
<joint name="RR_hip_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0.081 0"/>
<parent link="RR_hip"/>
<child link="RR_thigh_shoulder"/>
</joint>
<!-- this link is only for collision -->
<link name="RR_thigh_shoulder">
<!-- <collision>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.032" radius="0.041"/>
</geometry>
</collision> -->
</link>
<joint name="RR_thigh_joint" type="revolute">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 -0.062 0.00"/>
<parent link="RR_hip"/>
<child link="RR_thigh"/>
<limit effort="18" lower="-2.6" upper="2.6" velocity="40"/>
</joint>
<link name="RR_thigh">
<material name="cheetah_material"/>
<inertial>
<mass value="0.634"/>
<origin xyz="0.0 0.016 -0.02"/>
<inertia ixx="0.001983" ixy="0.000245" ixz="0.000013"
iyy="0.002103" iyz="0.0000015" izz="0.000408"/>
</inertial>
<visual>
<geometry>
<!--mesh filename="meshes/mini_upper_link.obj"/-->
<cylinder length ="0.17" radius = "0.015"/>
</geometry>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.105"/>
</visual>
<collision>
<geometry>
<!--mesh filename="meshes/mini_upper_link.obj"/-->
<cylinder length ="0.17" radius = "0.015"/>
</geometry>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.105"/>
</collision>
</link>
<joint name="RR_calf_joint" type="revolute">
<axis xyz="0 -1 0"/>
<origin rpy="0.0 0 0.0" xyz="0.0 0.0 -0.209"/>
<parent link="RR_thigh"/>
<child link="RR_calf"/>
<limit effort="26" lower="-2.6" upper="2.6" velocity="26"/>
</joint>
<link name="RR_calf">
<material name="cheetah_material"/>
<inertial>
<mass value="0.064"/>
<origin xyz="0.0 0.0 -0.209"/>
<inertia ixx="0.000245" ixy="0" ixz="0.0" iyy="0.000248" iyz="0" izz="0.000006"/>
</inertial>
<visual>
<geometry>
<!--mesh filename="meshes/mini_lower_link.obj"/-->
<cylinder length ="0.15" radius = "0.01"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 -0.095"/>
</visual>
<collision>
<geometry>
<!--mesh filename="meshes/mini_lower_link.obj"/-->
<cylinder length ="0.15" radius = "0.01"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0 0 -0.095"/>
</collision>
</link>
<!-- Adapter to Foot joint -->
<joint name="RR_foot_fixed" type="fixed">
<parent link="RR_calf"/>
<child link="RR_foot"/>
<origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.19"/>
</joint>
<link name="RR_foot">
<material name="cheetah_material"/>
<inertial>
<mass value="0.0"/>
<inertia ixx="0.000025" ixy="0" ixz="0.0" iyy="0.000025" iyz="0" izz="0.000025"/>
</inertial>
<visual>
<geometry>
<sphere radius = "0.0175"/>
</geometry>
<origin rpy="0.0 3.141592 0.0" xyz="0.0 0.0 0.0"/>
</visual>
<collision>
<geometry>
<!--mesh filename="meshes/mini_lower_link.obj"/-->
<sphere radius = "0.0175"/>
</geometry>
<origin rpy="0.0 3.141592 0" xyz="0.0 0.0 0"/>
</collision>
</link>
</robot>
resources/textures/background_texture_metal_rust.jpg
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 546 KiB

Some files were not shown because too many files have changed in this diff Show More