diff --git a/.github/workflows/python-app.yml b/.github/workflows/python-app.yml
index 16e061fb..8751200f 100644
--- a/.github/workflows/python-app.yml
+++ b/.github/workflows/python-app.yml
@@ -23,21 +23,25 @@ jobs:
steps:
- uses: actions/checkout@v3
+
- name: Set up Python 3.10
uses: actions/setup-python@v3
with:
python-version: "3.10"
+
- name: Install mesa and patchelf
run: |
+ sudo apt-get update -y -qq
sudo apt-get install -y \
libgl1-mesa-dev \
- libgl1-mesa-glx \
+ libgl1 \
+ libglx-mesa0 \
libglew-dev \
libosmesa6-dev \
- software-properties-common
- sudo apt-get install -y patchelf
- sudo apt-get update -y -qq
- sudo apt-get install -y xvfb x11-xserver-utils
+ software-properties-common \
+ patchelf \
+ xvfb \
+ x11-xserver-utils
sudo /usr/bin/Xvfb $DISPLAY -screen 0 1280x1024x24 &
- name: Install ffmpeg
@@ -46,6 +50,7 @@ jobs:
- name: Checkout submodules
run: git submodule update --init --recursive
+
- name: Install dependencies
run: |
python -m pip install --upgrade pip
@@ -56,10 +61,6 @@ jobs:
- name: Run MuJoCo Rendering test
run: python3 -m mujoco.render_test
- # - name: Test RoboHive (Fetching SimHive via git-submodules)
- # run: |
- # python robohive/tests/test_all.py
-
- name: Test RoboHive (Programmatically fetching SimHive)
run: |
rm -rf robohive/simhive
diff --git a/.gitmodules b/.gitmodules
index 4b60597e..a6d2cc96 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -38,4 +38,4 @@
url = https://github.com/vikashplus/robotiq_sim.git
[submodule "robohive/simhive/myo_sim"]
path = robohive/simhive/myo_sim
- url = https://github.com/MyoHub/myo_sim.git
\ No newline at end of file
+ url = https://github.com/MyoHub/myo_sim.git
diff --git a/README.md b/README.md
index aa3aae59..8c242647 100644
--- a/README.md
+++ b/README.md
@@ -73,7 +73,7 @@ MultiTask Suite
This suite contains a collection of environments centered around dexterous manipulation. Standard [TCDM benchmarks](https://pregrasps.github.io/) are a part of this suite
## - ROBEL Suite (Coming soon)
- This suite contains a collection of environments centered around real-world locomotion and manipulation. Standard [ROBEL benchmarks](http://roboticsbenchmarks.org/) are a part of this suite
+ This suite contains a collection of environments centered around real-world locomotion and manipulation. Standard [ROBEL benchmarks](https://sites.google.com/view/roboticsbenchmarks) are a part of this suite
# Citation
If you find `RoboHive` useful in your research,
diff --git a/robohive/envs/arms/__init__.py b/robohive/envs/arms/__init__.py
index d6a3220b..3fe5f663 100644
--- a/robohive/envs/arms/__init__.py
+++ b/robohive/envs/arms/__init__.py
@@ -8,11 +8,14 @@
# from gym.envs.registration import register
from robohive.utils import gym; register=gym.register
-from robohive.envs.env_variants import register_env_variant
import os
+
+from robohive.envs.env_variants import register_env_variant
+from robohive.utils.prompt_utils import Prompt, prompt
+
curr_dir = os.path.dirname(os.path.abspath(__file__))
-print("RoboHive:> Registering Arms Envs")
+prompt("RoboHive:> Registering Arms Envs", Prompt.ONCE)
# FRANKA REACH =======================================================================
from robohive.envs.arms.reach_base_v0 import ReachBaseV0
diff --git a/robohive/envs/arms/fetch/assets/fetch_reach_v0.config b/robohive/envs/arms/fetch/assets/fetch_reach_v0.config
index d0ac1c0a..aab9fd2f 100644
--- a/robohive/envs/arms/fetch/assets/fetch_reach_v0.config
+++ b/robohive/envs/arms/fetch/assets/fetch_reach_v0.config
@@ -3,27 +3,27 @@
'franka':{
'interface': {'type': 'fetch'},
'sensor':[
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'shoulder_pan_jp'},
- {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'shoulder_lift_jp'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'upperarm_roll_jp'},
- {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'elbow_flex_jp'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'forearm_roll_jp'},
- {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'wrist_flex_jp'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'wrist_roll_jp'},
- {'range':(0.00, .04), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_jp'},
- {'range':(0.00, .04), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_jp'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_pan_jp'},
+ {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_lift_jp'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'upperarm_roll_jp'},
+ {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'elbow_flex_jp'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'forearm_roll_jp'},
+ {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'wrist_flex_jp'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'wrist_roll_jp'},
+ {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_jp'},
+ {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_jp'},
],
'actuator':[
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'shoulder_pan'},
- {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'shoulder_lift'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'upperarm_roll'},
- {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'elbow_flex'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'forearm_roll'},
- {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'wrist_flex'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'wrist_roll'},
- {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger'},
- {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_pan'},
+ {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_lift'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'upperarm_roll'},
+ {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'elbow_flex'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'forearm_roll'},
+ {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'wrist_flex'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'wrist_roll'},
+ {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger'},
+ {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger'},
]
}
}
\ No newline at end of file
diff --git a/robohive/envs/arms/franka/assets/franka_busbin_v0.config b/robohive/envs/arms/franka/assets/franka_busbin_v0.config
index 859dad30..bf2894e8 100644
--- a/robohive/envs/arms/franka/assets/franka_busbin_v0.config
+++ b/robohive/envs/arms/franka/assets/franka_busbin_v0.config
@@ -3,39 +3,39 @@
'franka':{
'interface': {'type': 'franka', 'ip_address':'169.254.163.91'},
'sensor':[
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'},
- {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_id':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'},
- {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_id':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'},
- {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_id':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'},
+ {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'},
+ {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'},
+ {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'},
- {'range':(0.00, .04), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'},
- {'range':(0.00, .04), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'},
+ {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'},
+ {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'},
],
'actuator':[
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'},
- {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'},
- {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'},
- {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'},
+ {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'},
+ {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'},
+ {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'},
]
},
# 'busbin':{
# 'interface': {},
# 'sensor':[
- # {'range':(-2.0, 0.0), 'noise':0.05, 'hdr_id':7, 'scale':1, 'offset':0, 'name':'???'},
+ # {'range':(-2.0, 0.0), 'noise':0.05, 'hdr_adr':7, 'scale':1, 'offset':0, 'name':'???'},
# ],
# 'actuator':[]
# }
diff --git a/robohive/envs/arms/franka/assets/franka_reach_v0.config b/robohive/envs/arms/franka/assets/franka_reach_v0.config
index e61944bb..87b489cd 100644
--- a/robohive/envs/arms/franka/assets/franka_reach_v0.config
+++ b/robohive/envs/arms/franka/assets/franka_reach_v0.config
@@ -3,37 +3,37 @@
'franka':{
'interface': {'type': 'franka', 'ip_address':'172.16.0.1', 'gain_scale':0.5},
'sensor':[
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'},
- {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_id':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'},
- {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_id':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'},
- {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_id':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'},
- # {'range':(0.00, .04), 'noise':0.05, 'hdr_id':7, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'},
- # {'range':(0.00, .04), 'noise':0.05, 'hdr_id':8, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'},
+ {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'},
+ {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'},
+ {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'},
+ # {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':7, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'},
+ # {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':8, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'},
],
'actuator':[
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_id':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'},
- {'pos_range':(-1.8326, 1.8326), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_id':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_id':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'},
- {'pos_range':(-3.1416, 0.0000), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_id':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_id':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'},
- {'pos_range':(-1.6600, 2.1817), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_id':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_id':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'},
- # {'pos_range':(-0.0000, 0.0400), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_joint'},
- # {'pos_range':(-0.0000, 0.0400), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_joint'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'},
+ {'pos_range':(-1.8326, 1.8326), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'},
+ {'pos_range':(-3.1416, 0.0000), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'},
+ {'pos_range':(-1.6600, 2.1817), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'},
+ # {'pos_range':(-0.0000, 0.0400), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_joint'},
+ # {'pos_range':(-0.0000, 0.0400), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_joint'},
]
},
@@ -41,8 +41,8 @@
'interface': {'type': 'realsense', 'topic':'realsense_815412070228/color/image_raw', 'data_type':'rgb240x320'},
'sensor':[],
'cam': [
- {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'},
- # {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':1, 'offset':0, 'name':'/depth_mono/image_raw'},
+ {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'},
+ # {'range':(0, 255), 'noise':0.00, 'hdr_adr':'d', 'scale':1, 'offset':0, 'name':'/depth_mono/image_raw'},
],
'actuator':[]
},
@@ -51,8 +51,8 @@
'interface': {'type': 'realsense', 'topic':'realsense_815412070341/color/image_raw', 'data_type':'rgb'},
'sensor':[],
'cam': [
- {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'},
- # {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':1, 'offset':0, 'name':'/depth_mono/image_raw'},
+ {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'},
+ # {'range':(0, 255), 'noise':0.00, 'hdr_adr':'d', 'scale':1, 'offset':0, 'name':'/depth_mono/image_raw'},
],
'actuator':[]
},
@@ -61,8 +61,8 @@
'interface': {'type': 'realsense', 'topic':'realsense_936322070233/color/image_raw', 'data_type':'rgb'},
'sensor':[],
'cams': [
- {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'},
- # {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':1, 'offset':0, 'name':'/depth_mono/image_raw'},
+ {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'},
+ # {'range':(0, 255), 'noise':0.00, 'hdr_adr':'d', 'scale':1, 'offset':0, 'name':'/depth_mono/image_raw'},
],
'actuator':[]
},
diff --git a/robohive/envs/arms/franka/assets/franka_ycb_v0.config b/robohive/envs/arms/franka/assets/franka_ycb_v0.config
index 54b23efe..e9a9297c 100644
--- a/robohive/envs/arms/franka/assets/franka_ycb_v0.config
+++ b/robohive/envs/arms/franka/assets/franka_ycb_v0.config
@@ -3,48 +3,48 @@
'franka':{
'interface': {'type': 'franka', 'ip_address':'169.254.163.91'},
'sensor':[
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'},
- {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_id':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'},
- {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_id':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'},
- {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_id':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'},
- {'range':(0.00, .04), 'noise':0.05, 'hdr_id':7, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'},
- {'range':(0.00, .04), 'noise':0.05, 'hdr_id':8, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'},
+ {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'},
+ {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'},
+ {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'},
+ {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':7, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'},
+ {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':8, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'},
],
'actuator':[
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'},
- {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'},
- {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'},
- {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'},
- {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_joint'},
- {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_joint'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'},
+ {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'},
+ {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'},
+ {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'},
+ {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_joint'},
+ {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_joint'},
]
},
'object':{
'interface':{},
'sensor':[
- {'range':(-1.0, 1.0), 'noise':0.005, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'Tx'},
- {'range':(-0.5, 0.5), 'noise':0.005, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'Ty'},
- {'range':(-1.0, 1.0), 'noise':0.005, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'Tz'},
- {'range':(-3.1, 3.1), 'noise':0.005, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'Rx'},
- {'range':(-3.1, 3.1), 'noise':0.005, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'Ry'},
- {'range':(-3.1, 3.1), 'noise':0.005, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'Rz'},
+ {'range':(-1.0, 1.0), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Tx'},
+ {'range':(-0.5, 0.5), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Ty'},
+ {'range':(-1.0, 1.0), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Tz'},
+ {'range':(-3.1, 3.1), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Rx'},
+ {'range':(-3.1, 3.1), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Ry'},
+ {'range':(-3.1, 3.1), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Rz'},
],
'actuator':[]
}
diff --git a/robohive/envs/claws/__init__.py b/robohive/envs/claws/__init__.py
index ec019270..f17f850d 100644
--- a/robohive/envs/claws/__init__.py
+++ b/robohive/envs/claws/__init__.py
@@ -7,10 +7,13 @@
from robohive.utils import gym; register=gym.register
import os
+
curr_dir = os.path.dirname(os.path.abspath(__file__))
from robohive.envs.env_variants import register_env_variant
+from robohive.utils.prompt_utils import Prompt, prompt
+
+prompt("RoboHive:> Registering Claw Envs", Prompt.ONCE)
-print("RoboHive:> Registering Claw Envs")
# TRIFINGER REORIENT =======================================================================
from robohive.envs.claws.reorient_v0 import ReorientBaseV0
diff --git a/robohive/envs/env_base.py b/robohive/envs/env_base.py
index 1072b132..6e9afac2 100644
--- a/robohive/envs/env_base.py
+++ b/robohive/envs/env_base.py
@@ -5,21 +5,21 @@
License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
================================================= """
-from robohive.utils import gym
-import numpy as np
import os
import time as timer
+from sys import platform
-from robohive.envs.obs_vec_dict import ObsVecDict
-from robohive.utils import tensor_utils
-from robohive.robot.robot import Robot
-from robohive.utils.implement_for import implement_for
-from robohive.utils.prompt_utils import prompt, Prompt
+import numpy as np
import skvideo.io
-from sys import platform
-from robohive.physics.sim_scene import SimScene
+
import robohive.utils.import_utils as import_utils
from robohive.envs.env_variants import gym_registry_specs
+from robohive.envs.obs_vec_dict import ObsVecDict
+from robohive.physics.sim_scene import SimScene
+from robohive.robot.robot import Robot
+from robohive.utils import gym, tensor_utils
+from robohive.utils.implement_for import implement_for
+from robohive.utils.prompt_utils import Prompt, prompt
# TODO
# remove rwd_mode
@@ -74,6 +74,7 @@ def _setup(self,
obs_range:tuple = (-10, 10), # Permissible range of values in obs vector returned by get_obs()
rwd_viz:bool = False, # Visualize rewards (WIP, needs vtils)
device_id:int = 0, # Device id for rendering
+ init_qpos = None, # Explicit reset/home qpos. Auto-computed (mid actuator range) if not provided
**kwargs, # Additional arguments
):
@@ -87,7 +88,8 @@ def _setup(self,
self.viewer_setup()
# resolve robot config
- self.robot = Robot(mj_sim=self.sim,
+ robot_cls = kwargs.pop('robot_cls', Robot)
+ self.robot = robot_cls(mj_sim=self.sim,
random_generator=self.np_random,
**kwargs)
@@ -100,18 +102,25 @@ def _setup(self,
# resolve initial state
self.init_qvel = self.sim.data.qvel.ravel().copy()
- self.init_qpos = self.sim.data.qpos.ravel().copy() # has issues with initial jump during reset
- # self.init_qpos = np.mean(self.sim.model.actuator_ctrlrange, axis=1) if self.normalize_act else self.sim.data.qpos.ravel().copy() # has issues when nq!=nu
- # self.init_qpos[self.sim.model.jnt_dofadr] = np.mean(self.sim.model.jnt_range, axis=1) if self.normalize_act else self.sim.data.qpos.ravel().copy()
- if self.normalize_act:
- # find all linear+actuated joints. Use mean(jnt_range) as init position
- actuated_jnt_ids = self.sim.model.actuator_trnid[self.sim.model.actuator_trntype==self.sim.lib.mjtTrn.mjTRN_JOINT, 0] # dm
- linear_jnt_ids = np.logical_or(self.sim.model.jnt_type==self.sim.lib.mjtJoint.mjJNT_SLIDE, self.sim.model.jnt_type==self.sim.lib.mjtJoint.mjJNT_HINGE)
- linear_jnt_ids = np.where(linear_jnt_ids==True)[0]
- linear_actuated_jnt_ids = np.intersect1d(actuated_jnt_ids, linear_jnt_ids)
- # assert np.any(actuated_jnt_ids==linear_actuated_jnt_ids), "Wooho: Great evidence that it was important to check for actuated_jnt_ids as well as linear_actuated_jnt_ids"
- linear_actuated_jnt_qposids = self.sim.model.jnt_qposadr[linear_actuated_jnt_ids]
- self.init_qpos[linear_actuated_jnt_qposids] = np.mean(self.sim.model.jnt_range[linear_actuated_jnt_ids], axis=1)
+ if init_qpos is not None:
+ # use the provided init_pos
+ self.init_qpos = np.array(init_qpos, dtype=np.float64).ravel().copy()
+ else:
+ # create one if not provided
+ self.init_qpos = self.sim.data.qpos.ravel().copy() # has issues with initial jump during reset
+ if self.normalize_act:
+ # find all linear+actuated joints. Use mean(jnt_range) as init position
+ actuated_jnt_ids = self.sim.model.actuator_trnid[self.sim.model.actuator_trntype==self.sim.lib.mjtTrn.mjTRN_JOINT, 0] # dm
+ linear_jnt_ids = np.logical_or(self.sim.model.jnt_type==self.sim.lib.mjtJoint.mjJNT_SLIDE, self.sim.model.jnt_type==self.sim.lib.mjtJoint.mjJNT_HINGE)
+ linear_jnt_ids = np.where(linear_jnt_ids==True)[0]
+ linear_actuated_jnt_ids = np.intersect1d(actuated_jnt_ids, linear_jnt_ids)
+ linear_actuated_jnt_qposids = self.sim.model.jnt_qposadr[linear_actuated_jnt_ids]
+ self.init_qpos[linear_actuated_jnt_qposids] = np.mean(self.sim.model.jnt_range[linear_actuated_jnt_ids], axis=1)
+ if self.robot.is_hardware:
+ prompt(f"WARNING: {self.robot.name} is hardware-backed but no init_qpos was provided — "
+ "defaulting to the mid-range actuator pose. Pass init_qpos explicitly "
+ "to control where the robot homes to on reset.",
+ type=Prompt.WARN)
# resolve rewards
self.rwd_dict = {}
@@ -131,10 +140,11 @@ def _setup(self,
self.visual_keys = visual_keys if type(visual_keys)==list or visual_keys==None else [visual_keys]
self._setup_rgb_encoders(self.visual_keys, device=None)
- # reset to get the env ready
- observation, _reward, done, *_, _info = self.step(np.zeros(self.sim.model.nu))
- # Question: Should we replace above with following? Its specially helpful for hardware as it forces a env reset before continuing, without which the hardware will make a big jump from its position to the position asked by step.
- # observation = self.reset()
+ # reset to get the env ready. Using reset() rather than step(zeros) routes hardware
+ # through Robot.reset()'s min-jerk hardware_reset(), instead of jumping straight to
+ # a raw ctrl command from whatever pose the robot is currently in.
+ self.reset()
+ observation, _reward, done, *_, _info = self.forward()
assert not done, "Check initialization. Simulation starts in a done state."
self.observation_space = gym.spaces.Box(obs_range[0]*np.ones(observation.size), obs_range[1]*np.ones(observation.size), dtype=np.float32)
@@ -183,7 +193,14 @@ def forward(self, x):
if "rrl" in id_encoder or "resnet" in id_encoder:
import_utils.torchvision_isavailable()
import torchvision.transforms as T
- from torchvision.models import resnet50, ResNet50_Weights, resnet34, ResNet34_Weights, resnet18, ResNet18_Weights
+ from torchvision.models import (
+ ResNet18_Weights,
+ ResNet34_Weights,
+ ResNet50_Weights,
+ resnet18,
+ resnet34,
+ resnet50,
+ )
if "r3m" in id_encoder:
import_utils.torchvision_isavailable()
@@ -495,8 +512,8 @@ def get_input_seed(self):
def _reset(self, reset_qpos=None, reset_qvel=None, seed=None, **kwargs):
"""
- Reset the environment
- Default implemention provided. Override if env needs custom reset
+ Reset the environment (Default implemention provided).
+ Override if env needs custom reset. Carefully handle return type for gym/gymnasium compatibility
"""
qpos = self.init_qpos.copy() if reset_qpos is None else reset_qpos
qvel = self.init_qvel.copy() if reset_qvel is None else reset_qvel
@@ -517,6 +534,13 @@ def reset(self, reset_qpos=None, reset_qvel=None, seed=None, **kwargs):
# return self.step(a)
+ def close(self):
+ """
+ Clean up the environment
+ """
+ if self.sim:
+ self.sim.close()
+
@property
def dt(self):
return self.sim.model.opt.timestep * self.frame_skip
diff --git a/robohive/envs/fm/__init__.py b/robohive/envs/fm/__init__.py
index b9d71406..32aa666f 100644
--- a/robohive/envs/fm/__init__.py
+++ b/robohive/envs/fm/__init__.py
@@ -1,8 +1,13 @@
from robohive.utils import gym; register=gym.register
-import numpy as np
import os
+
+import numpy as np
+
curr_dir = os.path.dirname(os.path.abspath(__file__))
+from robohive.utils.prompt_utils import Prompt, prompt
+
+prompt("RoboHive:> Registering FM Envs", Prompt.ONCE)
# Reach to fixed target
# register(
diff --git a/robohive/envs/fm/assets/dmanus.config b/robohive/envs/fm/assets/dmanus.config
index 938e47d0..7a307820 100644
--- a/robohive/envs/fm/assets/dmanus.config
+++ b/robohive/envs/fm/assets/dmanus.config
@@ -1,29 +1,29 @@
{
# device1: sensors, actuators
'dmanus':{
- 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/dev/ttyUSB0"},
+ 'interface': {'type': 'dynamixel', 'motor_type':"X", 'port':"/dev/ttyUSB0"},
'sensor':[
- {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_id':10, 'name':'TFJ1', 'scale':-1, 'offset':np.pi },
- {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_id':11, 'name':'TFJ2', 'scale':-1, 'offset':3*np.pi/2 },
- {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_id':12, 'name':'TFJ3', 'scale':-1, 'offset':np.pi },
- {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_id':20, 'name':'IFJ1', 'scale':-1, 'offset':np.pi },
- {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_id':21, 'name':'IFJ2', 'scale':-1, 'offset':3*np.pi/2 },
- {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_id':22, 'name':'IFJ3', 'scale':+1, 'offset':-np.pi },
- {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_id':30, 'name':'LFJ1', 'scale':-1, 'offset':np.pi },
- {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_id':31, 'name':'LFJ2', 'scale':+1, 'offset':-np.pi/2 },
- {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_id':32, 'name':'LFJ3', 'scale':+1, 'offset':-np.pi },
+ {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':10, 'name':'TFJ1', 'scale':-1, 'offset':np.pi },
+ {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':11, 'name':'TFJ2', 'scale':-1, 'offset':3*np.pi/2 },
+ {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':12, 'name':'TFJ3', 'scale':-1, 'offset':np.pi },
+ {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':20, 'name':'IFJ1', 'scale':-1, 'offset':np.pi },
+ {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':21, 'name':'IFJ2', 'scale':-1, 'offset':3*np.pi/2 },
+ {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':22, 'name':'IFJ3', 'scale':+1, 'offset':-np.pi },
+ {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':30, 'name':'LFJ1', 'scale':-1, 'offset':np.pi },
+ {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':31, 'name':'LFJ2', 'scale':+1, 'offset':-np.pi/2 },
+ {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':32, 'name':'LFJ3', 'scale':+1, 'offset':-np.pi },
],
'actuator':[
- {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':10, 'name':'TFA1', 'mode':'Position', 'scale':-1, 'offset':np.pi },
- {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':11, 'name':'TFA2', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 },
- {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':12, 'name':'TFA3', 'mode':'Position', 'scale':-1, 'offset':np.pi },
- {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':20, 'name':'IFA1', 'mode':'Position', 'scale':-1, 'offset':np.pi },
- {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':21, 'name':'IFA2', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 },
- {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':22, 'name':'IFA3', 'mode':'Position', 'scale':+1, 'offset':np.pi },
- {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':30, 'name':'LFA1', 'mode':'Position', 'scale':-1, 'offset':np.pi },
- {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':31, 'name':'LFA2', 'mode':'Position', 'scale':+1, 'offset':np.pi/2 },
- {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':32, 'name':'LFA3', 'mode':'Position', 'scale':+1, 'offset':np.pi },
+ {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'name':'TFA1', 'mode':'Position', 'scale':-1, 'offset':np.pi },
+ {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'name':'TFA2', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 },
+ {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'name':'TFA3', 'mode':'Position', 'scale':-1, 'offset':np.pi },
+ {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'name':'IFA1', 'mode':'Position', 'scale':-1, 'offset':np.pi },
+ {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'name':'IFA2', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 },
+ {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'name':'IFA3', 'mode':'Position', 'scale':+1, 'offset':np.pi },
+ {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'name':'LFA1', 'mode':'Position', 'scale':-1, 'offset':np.pi },
+ {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'name':'LFA2', 'mode':'Position', 'scale':+1, 'offset':np.pi/2 },
+ {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'name':'LFA3', 'mode':'Position', 'scale':+1, 'offset':np.pi },
]
}
}
\ No newline at end of file
diff --git a/robohive/envs/fm/assets/franka_dmanus.config b/robohive/envs/fm/assets/franka_dmanus.config
index e43a868f..478e3d1e 100644
--- a/robohive/envs/fm/assets/franka_dmanus.config
+++ b/robohive/envs/fm/assets/franka_dmanus.config
@@ -5,53 +5,53 @@
'interface': {'type': 'franka', 'ip_address':'172.16.0.1', 'gain_scale':0.2},
# 'interface': {'type': 'franka', 'ip_address':'169.254.163.91'},
'sensor':[
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'},
- {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_id':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'},
- {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_id':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'},
- {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_id':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'},
+ {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'},
+ {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'},
+ {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'},
],
'actuator':[
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'},
- {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'},
- {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'},
- {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'},
+ {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'},
+ {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'},
+ {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'},
]
},
# device1: sensors, actuators
'dmanus':{
- 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/dev/ttyUSB0"},
+ 'interface': {'type': 'dynamixel', 'motor_type':"X", 'port':"/dev/ttyUSB0"},
'sensor':[
- {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_id':10, 'name':'TF_ADB_jp', 'scale':-1, 'offset':np.pi },
- {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_id':11, 'name':'TF_MCP_jp', 'scale':-1, 'offset':3*np.pi/2 },
- {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_id':12, 'name':'TF_PIP_jp', 'scale':-1, 'offset':np.pi },
- {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_id':13, 'name':'TF_DIP_jp', 'scale':-1, 'offset':np.pi },
+ {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':10, 'name':'TF_ADB_jp', 'scale':-1, 'offset':np.pi },
+ {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':11, 'name':'TF_MCP_jp', 'scale':-1, 'offset':3*np.pi/2 },
+ {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':12, 'name':'TF_PIP_jp', 'scale':-1, 'offset':np.pi },
+ {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':13, 'name':'TF_DIP_jp', 'scale':-1, 'offset':np.pi },
- {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_id':20, 'name':'FF_ADB_jp', 'scale':-1, 'offset':np.pi },
- {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_id':21, 'name':'FF_MCP_jp', 'scale':-1, 'offset':3*np.pi/2 },
- {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_id':22, 'name':'FF_PIP_jp', 'scale':+1, 'offset':-np.pi },
- {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_id':30, 'name':'PF_ADB_jp', 'scale':-1, 'offset':np.pi },
- {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_id':31, 'name':'PF_MCP_jp', 'scale':+1, 'offset':-np.pi/2 },
- {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_id':32, 'name':'PF_PIP_jp', 'scale':+1, 'offset':-np.pi },
+ {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':20, 'name':'FF_ADB_jp', 'scale':-1, 'offset':np.pi },
+ {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':21, 'name':'FF_MCP_jp', 'scale':-1, 'offset':3*np.pi/2 },
+ {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':22, 'name':'FF_PIP_jp', 'scale':+1, 'offset':-np.pi },
+ {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':30, 'name':'PF_ADB_jp', 'scale':-1, 'offset':np.pi },
+ {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':31, 'name':'PF_MCP_jp', 'scale':+1, 'offset':-np.pi/2 },
+ {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':32, 'name':'PF_PIP_jp', 'scale':+1, 'offset':-np.pi },
],
'actuator':[
- {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':10, 'name':'TF_ADB', 'mode':'Position', 'scale':-1, 'offset':np.pi },
- {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':11, 'name':'TF_MCP', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 },
- {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':12, 'name':'TF_PIP', 'mode':'Position', 'scale':-1, 'offset':np.pi },
- {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':13, 'name':'TF_DIP', 'mode':'Position', 'scale':-1, 'offset':np.pi },
- {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':20, 'name':'FF_ADB', 'mode':'Position', 'scale':-1, 'offset':np.pi },
- {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':21, 'name':'FF_MCP', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 },
- {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':22, 'name':'FF_PIP', 'mode':'Position', 'scale':+1, 'offset':np.pi },
- {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':30, 'name':'PF_ADB', 'mode':'Position', 'scale':-1, 'offset':np.pi },
- {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':31, 'name':'PF_MCP', 'mode':'Position', 'scale':+1, 'offset':np.pi/2 },
- {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':32, 'name':'PF_PIP', 'mode':'Position', 'scale':+1, 'offset':np.pi },
+ {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'name':'TF_ADB', 'mode':'Position', 'scale':-1, 'offset':np.pi },
+ {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'name':'TF_MCP', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 },
+ {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'name':'TF_PIP', 'mode':'Position', 'scale':-1, 'offset':np.pi },
+ {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':13, 'name':'TF_DIP', 'mode':'Position', 'scale':-1, 'offset':np.pi },
+ {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'name':'FF_ADB', 'mode':'Position', 'scale':-1, 'offset':np.pi },
+ {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'name':'FF_MCP', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 },
+ {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'name':'FF_PIP', 'mode':'Position', 'scale':+1, 'offset':np.pi },
+ {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'name':'PF_ADB', 'mode':'Position', 'scale':-1, 'offset':np.pi },
+ {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'name':'PF_MCP', 'mode':'Position', 'scale':+1, 'offset':np.pi/2 },
+ {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'name':'PF_PIP', 'mode':'Position', 'scale':+1, 'offset':np.pi },
]
}
}
\ No newline at end of file
diff --git a/robohive/envs/fm/assets/franka_robotiq.config b/robohive/envs/fm/assets/franka_robotiq.config
index c2dcc441..73d68220 100644
--- a/robohive/envs/fm/assets/franka_robotiq.config
+++ b/robohive/envs/fm/assets/franka_robotiq.config
@@ -4,23 +4,23 @@
'franka':{
'interface': {'type': 'franka', 'ip_address':'172.16.0.1', 'gain_scale':0.5},
'sensor':[
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'},
- {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_id':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'},
- {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_id':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'},
- {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_id':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'},
+ {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'},
+ {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'},
+ {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'},
],
'actuator':[
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'},
- {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'},
- {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'},
- {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'},
+ {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'},
+ {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'},
+ {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'},
]
},
@@ -28,10 +28,10 @@
'robotiq':{
'interface': {'type': 'robotiq', 'ip_address':'172.16.0.1'},
'sensor':[
- {'range':(0, 0.834), 'noise':0.0, 'hdr_id':0, 'name':'robotiq_2f_85', 'scale':-9.81, 'offset':0.834},
+ {'range':(0, 0.834), 'noise':0.0, 'hdr_adr':0, 'name':'robotiq_2f_85', 'scale':-9.81, 'offset':0.834},
],
'actuator':[
- {'pos_range':(0, 1), 'vel_range':(-20*np.pi/4, 20*np.pi/4), 'hdr_id':0, 'name':'robotiq_2f_85', 'scale':-0.085, 'offset':0.085},
+ {'pos_range':(0, 1), 'vel_range':(-20*np.pi/4, 20*np.pi/4), 'hdr_adr':0, 'name':'robotiq_2f_85', 'scale':-0.085, 'offset':0.085},
]
},
@@ -39,8 +39,8 @@
'interface': {'type': 'realsense', 'rgb_topic':'realsense_815412070228/color/image_raw', 'd_topic':'realsense_815412070228/depth_uncolored/image_raw'},
'sensor':[],
'cam': [
- {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'},
- {'range':(0, 255), 'noise':0.00, 'hdr_id':'d', 'scale':1, 'offset':0, 'name':'/depth_uncolored/image_raw'},
+ {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'},
+ {'range':(0, 255), 'noise':0.00, 'hdr_adr':'d', 'scale':1, 'offset':0, 'name':'/depth_uncolored/image_raw'},
],
'actuator':[]
},
@@ -49,7 +49,7 @@
'interface': {'type': 'realsense', 'rgb_topic':'realsense_815412070341/color/image_raw', 'd_topic':'realsense_815412070341/depth_uncolored/image_raw'},
'sensor':[],
'cam': [
- {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'},
+ {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'},
],
'actuator':[]
},
@@ -58,7 +58,7 @@
'interface': {'type': 'realsense', 'rgb_topic':'realsense_936322070233/color/image_raw', 'd_topic':'realsense_936322070233/depth_uncolored/image_raw'},
'sensor':[],
'cam': [
- {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'},
+ {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'},
],
'actuator':[]
},
@@ -67,7 +67,7 @@
'interface': {'type': 'realsense', 'rgb_topic':'realsense_814412070228/color/image_raw', 'd_topic':'realsense_814412070228/depth_uncolored/image_raw'},
'sensor':[],
'cam': [
- {'range':(0, 255), 'noise':0.00, 'hdr_id':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'},
+ {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'},
],
'actuator':[]
},
diff --git a/robohive/envs/hands/__init__.py b/robohive/envs/hands/__init__.py
index 9ffc7b7a..9c9bebb4 100644
--- a/robohive/envs/hands/__init__.py
+++ b/robohive/envs/hands/__init__.py
@@ -6,12 +6,14 @@
================================================= """
from robohive.utils import gym; register=gym.register
+import os
from robohive.envs.env_variants import register_env_variant
-import os
+from robohive.utils.prompt_utils import Prompt, prompt
+
curr_dir = os.path.dirname(os.path.abspath(__file__))
-print("RoboHive:> Registering Hand Envs")
+prompt("RoboHive:> Registering Hand Envs", Prompt.ONCE)
# ==================================================================================
# V1 envs:
@@ -64,6 +66,7 @@
)
from robohive.envs.hands.relocate_v1 import RelocateEnvV1
+
# Reach to random target using visual inputs
def register_visual_envs(env_name, encoder_type):
register_env_variant(
@@ -102,6 +105,7 @@ def register_visual_envs(env_name, encoder_type):
}
)
from robohive.envs.hands.baoding_v1 import BaodingFixedEnvV1
+
register(
id='baoding4th-v1',
entry_point='robohive.envs.hands:BaodingFixedEnvV1',
diff --git a/robohive/envs/hands/baoding_v1.py b/robohive/envs/hands/baoding_v1.py
index 1b7cafec..fcf35517 100644
--- a/robohive/envs/hands/baoding_v1.py
+++ b/robohive/envs/hands/baoding_v1.py
@@ -131,11 +131,9 @@ def _setup(self,
super()._setup(obs_keys=obs_keys,
weighted_reward_keys=weighted_reward_keys,
frame_skip=frame_skip,
+ init_qpos=self.sim.model.key_qpos[0].copy(),
**kwargs,
)
-
- # reset position
- self.init_qpos = self.sim.model.key_qpos[0].copy()
# self.init_qpos[:-14] *= 0 # Use fully open as init pos
# V0: Centered the action space around key_qpos[0]. Not sure if it matter.
@@ -264,8 +262,7 @@ def reset(self, reset_pose=None, reset_vel=None, reset_goal=None, time_period=6,
self.goal = self.create_goal_trajectory(time_period=time_period) if reset_goal is None else reset_goal.copy()
# reset scene
- obs = super().reset(reset_qpos=reset_pose, reset_qvel=reset_vel, **kwargs)
- return obs
+ return super().reset(reset_qpos=reset_pose, reset_qvel=reset_vel, **kwargs)
def create_goal_trajectory(self, time_step=.1, time_period=6):
len_of_goals = 1000 # assumes that its greator than env horizon
@@ -326,5 +323,4 @@ def create_goal_trajectory(self, time_step=.1, time_period=6):
class BaodingRandomEnvV1(BaodingFixedEnvV1):
def reset(self, **kwargs):
- obs = super().reset(time_period = self.np_random.uniform(high=5, low=7), **kwargs)
- return obs
+ return super().reset(time_period = self.np_random.uniform(high=5, low=7), **kwargs)
\ No newline at end of file
diff --git a/robohive/envs/hands/door_v1.py b/robohive/envs/hands/door_v1.py
index 8cd9f5e1..18dbfdc3 100644
--- a/robohive/envs/hands/door_v1.py
+++ b/robohive/envs/hands/door_v1.py
@@ -59,8 +59,8 @@ def _setup(self,
weighted_reward_keys=weighted_reward_keys,
reward_mode=reward_mode,
frame_skip=frame_skip,
+ init_qpos=np.zeros(sim.data.qpos.shape),
**kwargs)
- self.init_qpos = np.zeros(self.init_qpos.shape)
self.init_qvel = np.zeros(self.init_qpos.shape)
@@ -96,17 +96,13 @@ def get_reward_dict(self, obs_dict):
return rwd_dict
- def reset(self, reset_qpos=None, reset_qvel=None, **kwargs):
+ def reset(self, **kwargs):
self.sim.reset()
- qp = self.init_qpos.copy() if reset_qpos is None else reset_qpos
- qv = self.init_qvel.copy() if reset_qvel is None else reset_qvel
- self.robot.reset(reset_pos=qp, reset_vel=qv, **kwargs)
-
self.sim.model.body_pos[self.door_bid,0] = self.np_random.uniform(low=-0.3, high=-0.2)
self.sim.model.body_pos[self.door_bid, 1] = self.np_random.uniform(low=0.25, high=0.35)
self.sim.model.body_pos[self.door_bid,2] = self.np_random.uniform(low=0.252, high=0.35)
self.sim.forward()
- return self.get_obs()
+ return super().reset(**kwargs)
def get_env_state(self):
diff --git a/robohive/envs/hands/hammer_v1.py b/robohive/envs/hands/hammer_v1.py
index 3ef29033..dfd4ecb0 100644
--- a/robohive/envs/hands/hammer_v1.py
+++ b/robohive/envs/hands/hammer_v1.py
@@ -66,8 +66,8 @@ def _setup(self,
weighted_reward_keys=weighted_reward_keys,
reward_mode=reward_mode,
frame_skip=frame_skip,
+ init_qpos=np.zeros(sim.data.qpos.shape),
**kwargs)
- self.init_qpos = np.zeros(self.init_qpos.shape)
self.init_qvel = np.zeros(self.init_qpos.shape)
@@ -120,15 +120,12 @@ def get_obs_dict(self, sim):
return obs_dict
- def reset(self, reset_qpos=None, reset_qvel=None, **kwargs):
+ def reset(self, **kwargs):
self.sim.reset()
- qp = self.init_qpos.copy() if reset_qpos is None else reset_qpos
- qv = self.init_qvel.copy() if reset_qvel is None else reset_qvel
- self.robot.reset(reset_pos=qp, reset_vel=qv, **kwargs)
-
self.sim.model.body_pos[self.target_bid,2] = self.np_random.uniform(low=0.1, high=0.25)
self.sim.forward()
- return self.get_obs()
+ return super().reset(**kwargs)
+
def get_env_state(self):
"""
diff --git a/robohive/envs/hands/pen_v1.py b/robohive/envs/hands/pen_v1.py
index 54539d34..0ca38e9c 100644
--- a/robohive/envs/hands/pen_v1.py
+++ b/robohive/envs/hands/pen_v1.py
@@ -65,8 +65,8 @@ def _setup(self,
weighted_reward_keys=weighted_reward_keys,
reward_mode=reward_mode,
frame_skip=frame_skip,
+ init_qpos=np.zeros(sim.data.qpos.shape),
**kwargs)
- self.init_qpos = np.zeros(self.init_qpos.shape)
self.init_qvel = np.zeros(self.init_qpos.shape)
@@ -108,19 +108,14 @@ def get_reward_dict(self, obs_dict):
return rwd_dict
- def reset(self, reset_qpos=None, reset_qvel=None, **kwargs):
+ def reset(self, **kwargs):
self.sim.reset()
- qp = self.init_qpos.copy() if reset_qpos is None else reset_qpos
- qv = self.init_qvel.copy() if reset_qvel is None else reset_qvel
- self.robot.reset(reset_pos=qp, reset_vel=qv, **kwargs)
-
desired_orien = np.zeros(3)
desired_orien[0] = self.np_random.uniform(low=-1, high=1)
desired_orien[1] = self.np_random.uniform(low=-1, high=1)
self.sim.model.body_quat[self.target_obj_bid] = euler2quat(desired_orien)
self.sim.forward()
-
- return self.get_obs()
+ return super().reset(**kwargs)
def get_env_state(self):
diff --git a/robohive/envs/hands/relocate_v1.py b/robohive/envs/hands/relocate_v1.py
index 88e273e2..003d9df0 100644
--- a/robohive/envs/hands/relocate_v1.py
+++ b/robohive/envs/hands/relocate_v1.py
@@ -64,8 +64,8 @@ def _setup(self,
weighted_reward_keys=weighted_reward_keys,
reward_mode=reward_mode,
frame_skip=frame_skip,
+ init_qpos=np.zeros(sim.data.qpos.shape),
**kwargs)
- self.init_qpos = np.zeros(self.init_qpos.shape)
self.init_qvel = np.zeros(self.init_qpos.shape)
@@ -136,20 +136,16 @@ def get_obs_dict(self, sim):
return obs_dict
- def reset(self, reset_qpos=None, reset_qvel=None, **kwargs):
+ def reset(self, **kwargs):
self.sim.reset()
- qp = self.init_qpos.copy() if reset_qpos is None else reset_qpos
- qv = self.init_qvel.copy() if reset_qvel is None else reset_qvel
- self.robot.reset(reset_pos=qp, reset_vel=qv, **kwargs)
-
-
self.sim.model.body_pos[self.obj_bid,0] = self.np_random.uniform(low=-0.15, high=0.15)
self.sim.model.body_pos[self.obj_bid,1] = self.np_random.uniform(low=-0.15, high=0.3)
self.sim.model.site_pos[self.target_obj_sid, 0] = self.np_random.uniform(low=-0.2, high=0.2)
self.sim.model.site_pos[self.target_obj_sid,1] = self.np_random.uniform(low=-0.2, high=0.2)
self.sim.model.site_pos[self.target_obj_sid,2] = self.np_random.uniform(low=0.15, high=0.35)
self.sim.forward()
- return self.get_obs()
+ return super().reset(**kwargs)
+
def get_env_state(self):
"""
diff --git a/robohive/envs/multi_task/README.md b/robohive/envs/multi_task/README.md
index 4e365916..dc0ee157 100644
--- a/robohive/envs/multi_task/README.md
+++ b/robohive/envs/multi_task/README.md
@@ -2,7 +2,7 @@
This suite is designed to study generalization in multi-task settings. RoboHive's multi-task suite builds from `FrankaKitchen` environements originally studied in the [Relay Policy Learning](https://relay-policy-learning.github.io/) project.
# Franka Kitchen
-`FrankaKitchen` domain offers a challenging set of manipulation problems in an unstructured environment with many possible tasks to perform. The original set consisted of a franka robot in a kitchen domain. Overtime, Franka Kitchen has became a popular choice of environments for studying multi-task generalization. Its widespread use has led to a few different publically available variations. To help navigate these changes, we name these variations and document its evolution of across various versions below -
+`FrankaKitchen` domain offers a challenging set of manipulation problems in an unstructured environment with many possible tasks to perform. The original set consisted of a franka robot in a kitchen domain. Overtime, Franka Kitchen has became a popular choice of environments for studying multi-task generalization. Its widespread use has led to a few different publically available variations. To help navigate these changes, we name these variations and document its evolution across various versions below -
## Change log/ History
diff --git a/robohive/envs/multi_task/common/kitchen/franka_kitchen.config b/robohive/envs/multi_task/common/kitchen/franka_kitchen.config
index bb1816a2..c98ab660 100644
--- a/robohive/envs/multi_task/common/kitchen/franka_kitchen.config
+++ b/robohive/envs/multi_task/common/kitchen/franka_kitchen.config
@@ -3,38 +3,38 @@
'franka':{
'interface': {'type': 'franka'},
'sensor':[
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'},
- {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'},
- {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'},
- {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp6'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp7'},
- {'range':(0.00, .04), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'},
- {'range':(0.00, .04), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'},
+ {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'},
+ {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'},
+ {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp6'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp7'},
+ {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'},
+ {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'},
- # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'},
+ # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'},
],
'actuator':[
# TODO: ranges here exceed the corresponding sensor ranges
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'panda0_joint1'},
- {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'panda0_joint2'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'panda0_joint3'},
- {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'panda0_joint4'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'panda0_joint5'},
- {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'panda0_joint6'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'panda0_joint7'},
- {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_joint'},
- {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_joint'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint1'},
+ {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint2'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint3'},
+ {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint4'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint5'},
+ {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint6'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint7'},
+ {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_joint'},
+ {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_joint'},
]
},
@@ -42,22 +42,22 @@
'kitchen':{
'interface': {'type': 'tbd'},
'sensor':[
- {'range':(-1.57, 00), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'knob1_joint'},
- {'range':(-1.57, 00), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'knob2_joint'},
- {'range':(-1.57, 00), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'knob3_joint'},
- {'range':(-1.57, 00), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'knob4_joint'},
- {'range':(-0.7, 0.0), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'lightswitch_joint'},
- {'range':(0.0, 1.57), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'slidedoor_joint'},
- {'range':(-1.57, .0), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'leftdoorhinge'},
- {'range':(0.0, 1.57), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'rightdoorhinge'},
- {'range':(-2.09, 00), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'micro0joint'},
+ {'range':(-1.57, 00), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'knob1_joint'},
+ {'range':(-1.57, 00), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'knob2_joint'},
+ {'range':(-1.57, 00), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'knob3_joint'},
+ {'range':(-1.57, 00), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'knob4_joint'},
+ {'range':(-0.7, 0.0), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'lightswitch_joint'},
+ {'range':(0.0, 1.57), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'slidedoor_joint'},
+ {'range':(-1.57, .0), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'leftdoorhinge'},
+ {'range':(0.0, 1.57), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'rightdoorhinge'},
+ {'range':(-2.09, 00), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'micro0joint'},
- {'range':(-1.25, 1.75), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'kettle0:Tx'},
- {'range':(-1.50, 1.50), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'kettle0:Ty'},
- {'range':(-0.10, 2.90), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'kettle0:Tz'},
- {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'kettle0:Rx'},
- {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'kettle0:Ry'},
- {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'kettle0:Rz'},
+ {'range':(-1.25, 1.75), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Tx'},
+ {'range':(-1.50, 1.50), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Ty'},
+ {'range':(-0.10, 2.90), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Tz'},
+ {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Rx'},
+ {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Ry'},
+ {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Rz'},
],
'actuator':[]
diff --git a/robohive/envs/multi_task/common/microwave/franka_microwave.config b/robohive/envs/multi_task/common/microwave/franka_microwave.config
index 15aeedb8..233094c0 100644
--- a/robohive/envs/multi_task/common/microwave/franka_microwave.config
+++ b/robohive/envs/multi_task/common/microwave/franka_microwave.config
@@ -3,39 +3,39 @@
'franka':{
'interface': {'type': 'franka', 'ip_address':'169.254.163.91'},
'sensor':[
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'},
- {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_id':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'},
- {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_id':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'},
- {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_id':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'},
+ {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'},
+ {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'},
+ {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'},
- {'range':(0.00, .04), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'},
- {'range':(0.00, .04), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'},
+ {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'},
+ {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'},
],
'actuator':[
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'},
- {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'},
- {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'},
- {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'},
+ {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'},
+ {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'},
+ {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'},
]
},
'microwave':{
'interface': {},
'sensor':[
- {'range':(-2.0, 0.0), 'noise':0.05, 'hdr_id':7, 'scale':1, 'offset':0, 'name':'micro0joint'},
+ {'range':(-2.0, 0.0), 'noise':0.05, 'hdr_adr':7, 'scale':1, 'offset':0, 'name':'micro0joint'},
],
'actuator':[]
}
diff --git a/robohive/envs/multi_task/common/slidecabinet/franka_slidecabinet.config b/robohive/envs/multi_task/common/slidecabinet/franka_slidecabinet.config
index a2869a27..8c518054 100644
--- a/robohive/envs/multi_task/common/slidecabinet/franka_slidecabinet.config
+++ b/robohive/envs/multi_task/common/slidecabinet/franka_slidecabinet.config
@@ -3,41 +3,41 @@
'franka':{
'interface': {'type': 'franka', 'ip_address':'169.254.163.91'},
'sensor':[
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'},
- {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_id':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'},
- {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_id':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'},
- {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_id':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'},
- {'range':(0.00, .04), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'},
- {'range':(0.00, .04), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'},
+ {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'},
+ {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'},
+ {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'},
+ {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'},
+ {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'},
- {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'},
- {'range':(0.00, .04), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'},
- {'range':(0.00, .04), 'noise':0.05, 'hdr_id':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'},
+ {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'},
+ {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'},
+ {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'},
],
'actuator':[
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'},
- {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'},
- {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'},
- {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'},
- {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_id':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'},
+ {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'},
+ {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'},
+ {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'},
+ {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'},
]
},
'slidecabinet':{
'interface': {},
'sensor':[
- {'range':(-0.0, .44), 'noise':0.05, 'hdr_id':7, 'scale':1, 'offset':0, 'name':'slidedoor_joint'},
+ {'range':(-0.0, .44), 'noise':0.05, 'hdr_adr':7, 'scale':1, 'offset':0, 'name':'slidedoor_joint'},
],
'actuator':[]
}
diff --git a/robohive/envs/multi_task/multi_task_base_v1.py b/robohive/envs/multi_task/multi_task_base_v1.py
index a18adda4..50cb58b3 100644
--- a/robohive/envs/multi_task/multi_task_base_v1.py
+++ b/robohive/envs/multi_task/multi_task_base_v1.py
@@ -112,6 +112,13 @@ def _setup(self,
self.input_obj_init = obj_init
self.set_obj_goal(obj_goal=self.input_obj_goal, interact_site=interact_site)
+ # Recover init from the saved qposes and input specs before super()._setup(), since it
+ # triggers the first reset() which relies on init_qpos already reflecting these values.
+ keyFrame_id = 0
+ self.init_qpos = self.sim.model.key_qpos[keyFrame_id].copy()
+ if obj_init:
+ self.set_obj_init(self.input_obj_init)
+
super()._setup(obs_keys=obs_keys_wt,
proprio_keys=proprio_keys_wt,
weighted_reward_keys=weighted_reward_keys,
@@ -119,15 +126,9 @@ def _setup(self,
act_mode=act_mode,
obs_range=obs_range,
robot_name=robot_name,
+ init_qpos=self.init_qpos,
**kwargs)
-
-
- # Recover init from the saved qposes and input specs
- keyFrame_id = 0
- self.init_qpos[:] = self.sim.model.key_qpos[keyFrame_id].copy()
self.init_qvel[:] = self.sim.model.key_qvel[keyFrame_id].copy()
- if obj_init:
- self.set_obj_init(self.input_obj_init)
def get_dof_proximity(self, obj_dof_ranges, obj_dof_type):
@@ -200,9 +201,9 @@ def get_reward_dict(self, obs_dict):
# Optional Keys
("obj_goal", -np.sum(goal_dist, axis=-1)),
("bonus",
- 1.0*np.product(goal_dist < 5 * self.obj["dof_proximity"], axis=-1)
+ 1.0*np.prod(goal_dist < 5 * self.obj["dof_proximity"], axis=-1)
# np.product(goal_dist < 0.75 * self.obj["dof_ranges"], axis=-1)
- + 1.0*np.product(goal_dist < 1.67 * self.obj["dof_proximity"], axis=-1),
+ + 1.0*np.prod(goal_dist < 1.67 * self.obj["dof_proximity"], axis=-1),
# + np.product(goal_dist < 0.25 * self.obj["dof_ranges"], axis=-1),
),
("pose", -np.sum(np.abs(obs_dict["pose_err"]), axis=-1)),
@@ -272,4 +273,4 @@ def set_obj_goal(self, obj_goal=None, interact_site=None):
elif type(interact_site) is str: # overwrite using name
self.interact_sid = self.sim.model.site_name2id(interact_site)
elif type(interact_site) is int: # overwrite using id
- self.interact_sid = interact_site
\ No newline at end of file
+ self.interact_sid = interact_site
diff --git a/robohive/envs/multi_task/substeps1/__init__.py b/robohive/envs/multi_task/substeps1/__init__.py
index db479c9e..4a2c2486 100644
--- a/robohive/envs/multi_task/substeps1/__init__.py
+++ b/robohive/envs/multi_task/substeps1/__init__.py
@@ -7,14 +7,15 @@
import os
from robohive.utils import gym; register=gym.register
-
+from robohive.utils.prompt_utils import Prompt, prompt
CURR_DIR = os.path.dirname(os.path.abspath(__file__))
# Appliences ============================================================================
-print("RoboHive:> Registering Appliances Envs")
+prompt("RoboHive:> Registering Appliances Envs", Prompt.ONCE)
from robohive.envs.multi_task.common.franka_appliance_v1 import FrankaAppliance
+
# MICROWAVE
register(
id="franka_micro_open-v3",
diff --git a/robohive/envs/multi_task/substeps2/__init__.py b/robohive/envs/multi_task/substeps2/__init__.py
index 28c381da..e56e856f 100644
--- a/robohive/envs/multi_task/substeps2/__init__.py
+++ b/robohive/envs/multi_task/substeps2/__init__.py
@@ -4,8 +4,9 @@
Source :: https://github.com/vikashplus/robohive
License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
================================================= """
+from robohive.utils.prompt_utils import Prompt, prompt
-print("RoboHive:> Registering Multi-Task (2 subtasks) Envs")
+prompt("RoboHive:> Registering Multi-Task (2 subtasks) Envs", Prompt.ONCE)
from robohive.envs.multi_task.substeps1.franka_kitchen import register_all_env_variants
# ===================================================================
diff --git a/robohive/envs/multi_task/utils/parse_demos.py b/robohive/envs/multi_task/utils/parse_demos.py
index 65795e3a..44f98cc1 100644
--- a/robohive/envs/multi_task/utils/parse_demos.py
+++ b/robohive/envs/multi_task/utils/parse_demos.py
@@ -18,12 +18,16 @@
import glob
import pickle
import numpy as np
-from vive.source.parse_mjl import parse_mjl_logs, viz_parsed_mjl_logs
-from mjrl.utils.gym_env import GymEnv
+
+try:
+ from mjrl.utils.gym_env import GymEnv
+except ImportError:
+ GymEnv = None
import robohive
import time as timer
# import skvideo.io
from robohive.utils import gym
+from robohive.utils.parse_mjl import parse_mjl_logs, viz_parsed_mjl_logs
from tqdm import tqdm
@@ -52,6 +56,7 @@ def viewer(env,
render_buffer.append(curr_frame)
if mode == 'save':
+ import skvideo
skvideo.io.vwrite(filename, np.asarray(render_buffer))
print("\noffscreen buffer saved", filename)
@@ -84,7 +89,7 @@ def render_demos(env, data, filename='demo_rendering.mp4', render=None):
# playback demos and get data(physics respected)
def gather_training_data(env, data, filename='demo_playback.mp4', render=None):
- env = env.env
+ env = env.unwrapped
FPS = 30
render_skip = max(1, round(1. / \
(FPS * env.sim.model.opt.timestep * env.frame_skip)))
@@ -127,7 +132,7 @@ def gather_training_data(env, data, filename='demo_playback.mp4', render=None):
# normalization and env stepping
act = (ctrl - act_mid) / act_rng
act = np.clip(act, -0.999, 0.999)
- next_obs, reward, done, env_info = env.step(act)
+ next_obs, reward, done, *_, env_info = env.step(act)
path_reward += reward
# populate path
diff --git a/robohive/envs/myo/base_v0.py b/robohive/envs/myo/base_v0.py
index dd7ddca1..1bc837af 100644
--- a/robohive/envs/myo/base_v0.py
+++ b/robohive/envs/myo/base_v0.py
@@ -4,6 +4,7 @@
================================================= """
from robohive.envs import env_base
+import mujoco
import numpy as np
class BaseV0(env_base.MujocoEnv):
@@ -88,15 +89,24 @@ def step(self, a, **kwargs):
# implement abnormalities
if self.muscle_condition == 'fatigue':
+ actuator_moment = np.zeros((self.sim.model.nu, self.sim.model.nv))
+ mujoco.mju_sparse2dense(
+ actuator_moment,
+ self.sim.data.actuator_moment.reshape(-1),
+ self.sim.data.moment_rownnz,
+ self.sim.data.moment_rowadr,
+ self.sim.data.moment_colind.reshape(-1),
+ )
for mus_idx in range(self.sim.model.actuator_gainprm.shape[0]):
- if self.sim.data.actuator_moment.shape[1]==1:
- self.f_load[mus_idx].append(self.sim.data.actuator_moment[mus_idx].copy())
+ if actuator_moment.shape[1] == 1:
+ self.f_load[mus_idx].append(actuator_moment[mus_idx].copy())
else:
- self.f_load[mus_idx].append(self.sim.data.actuator_moment[mus_idx,1].copy())
+ self.f_load[mus_idx].append(actuator_moment[mus_idx, 1].copy())
if self.MVC_rest[mus_idx] != 0:
- f_int = np.sum(self.f_load[mus_idx]-np.max(self.f_load[mus_idx],0),0)/self.MVC_rest[mus_idx]
+ f_load = np.asarray(self.f_load[mus_idx])
+ f_int = np.sum(f_load - np.max(f_load, 0), 0)/self.MVC_rest[mus_idx]
f_cem = self.MVC_rest[mus_idx]*np.exp(self.k_fatigue*f_int)
else:
f_cem = 0
diff --git a/robohive/envs/myo/myobase/__init__.py b/robohive/envs/myo/myobase/__init__.py
index eac00fdb..48c7d43d 100644
--- a/robohive/envs/myo/myobase/__init__.py
+++ b/robohive/envs/myo/myobase/__init__.py
@@ -4,11 +4,13 @@
================================================= """
from robohive.utils import gym; register=gym.register
+import os
+
+import numpy as np
from robohive.envs.env_variants import register_env_variant
+from robohive.utils.prompt_utils import Prompt, prompt
-import os
-import numpy as np
# utility to register envs with all muscle conditions
def register_env_with_variants(id, entry_point, max_episode_steps, kwargs):
@@ -46,7 +48,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs):
curr_dir = os.path.dirname(os.path.abspath(__file__))
-print("RoboHive:> Registering Myo Envs")
+prompt("RoboHive:> Registering Myo Envs", Prompt.ONCE)
# Finger-tip reaching ==============================
register_env_with_variants(id='motorFingerReachFixed-v0',
@@ -287,6 +289,7 @@ def register_env_with_variants(id, entry_point, max_episode_steps, kwargs):
# Gait Torso Reaching ==============================
from robohive.physics.sim_scene import SimBackend
+
sim_backend = SimBackend.get_sim_backend()
leg_model='/../../../simhive/myo_sim/leg/myolegs.xml'
diff --git a/robohive/envs/myo/myobase/baoding_v1.py b/robohive/envs/myo/myobase/baoding_v1.py
index 44f63fad..c5614f96 100644
--- a/robohive/envs/myo/myobase/baoding_v1.py
+++ b/robohive/envs/myo/myobase/baoding_v1.py
@@ -113,17 +113,19 @@ def _setup(self,
# update rewards to be move rewards
weighted_reward_keys = self.MOVE_TO_LOCATION_RWD_KEYS_AND_WEIGHTS
+ # reset position
+ # init_qpos = self.sim.model.key_qpos[0].copy()
+ init_qpos = self.sim.data.qpos.ravel().copy()
+ init_qpos[:-14] *= 0 # Use fully open as init pos
+ init_qpos[0] = -1.57 # Palm up
+
super()._setup(obs_keys=obs_keys,
weighted_reward_keys=weighted_reward_keys,
frame_skip=frame_skip,
+ init_qpos=init_qpos,
**kwargs,
)
- # reset position
- # self.init_qpos = self.sim.model.key_qpos[0].copy()
- self.init_qpos[:-14] *= 0 # Use fully open as init pos
- self.init_qpos[0] = -1.57 # Palm up
-
# V0: Centered the action space around key_qpos[0]. Not sure if it matter.
# self.act_mid = self.init_qpos[:self.n_jnt].copy()
# self.upper_rng = 0.9*(self.model.actuator_ctrlrange[:,1]-self.act_mid)
diff --git a/robohive/envs/myo/myobase/key_turn_v0.py b/robohive/envs/myo/myobase/key_turn_v0.py
index 9fc51e1d..e4f3657b 100644
--- a/robohive/envs/myo/myobase/key_turn_v0.py
+++ b/robohive/envs/myo/myobase/key_turn_v0.py
@@ -54,11 +54,14 @@ def _setup(self,
self.key_init_range = key_init_range
self.key_init_pos = self.sim.data.site_xpos[self.keyhead_sid].copy()
+ init_qpos = self.sim.data.qpos.ravel().copy()
+ init_qpos[:-1] *= 0 # Use fully open as init pos
+
super()._setup(obs_keys=obs_keys,
weighted_reward_keys=weighted_reward_keys,
+ init_qpos=init_qpos,
**kwargs,
)
- self.init_qpos[:-1] *= 0 # Use fully open as init pos
def get_obs_vec(self):
self.obs_dict['time'] = np.array([self.sim.data.time])
diff --git a/robohive/envs/myo/myobase/obj_hold_v0.py b/robohive/envs/myo/myobase/obj_hold_v0.py
index b0864933..bfc29c0d 100644
--- a/robohive/envs/myo/myobase/obj_hold_v0.py
+++ b/robohive/envs/myo/myobase/obj_hold_v0.py
@@ -47,12 +47,15 @@ def _setup(self,
self.goal_sid = self.sim.model.site_name2id("goal")
self.object_init_pos = self.sim.data.site_xpos[self.object_sid].copy()
+ init_qpos = self.sim.data.qpos.ravel().copy()
+ init_qpos[:-7] *= 0 # Use fully open as init pos
+ init_qpos[0] = -1.5 # place palm up
+
super()._setup(obs_keys=obs_keys,
weighted_reward_keys=weighted_reward_keys,
+ init_qpos=init_qpos,
**kwargs,
)
- self.init_qpos[:-7] *= 0 # Use fully open as init pos
- self.init_qpos[0] = -1.5 # place palm up
def get_obs_vec(self):
diff --git a/robohive/envs/myo/myobase/pen_v0.py b/robohive/envs/myo/myobase/pen_v0.py
index 4684ed77..eb098524 100644
--- a/robohive/envs/myo/myobase/pen_v0.py
+++ b/robohive/envs/myo/myobase/pen_v0.py
@@ -57,12 +57,15 @@ def _setup(self,
self.pen_length = np.linalg.norm(self.sim.model.site_pos[self.obj_t_sid] - self.sim.model.site_pos[self.obj_b_sid])
self.tar_length = np.linalg.norm(self.sim.model.site_pos[self.tar_t_sid] - self.sim.model.site_pos[self.tar_b_sid])
+ init_qpos = self.sim.data.qpos.ravel().copy()
+ init_qpos[:-6] *= 0 # Use fully open as init pos
+ init_qpos[0] = -1.5 # place palm up
+
super()._setup(obs_keys=obs_keys,
weighted_reward_keys=weighted_reward_keys,
+ init_qpos=init_qpos,
**kwargs,
)
- self.init_qpos[:-6] *= 0 # Use fully open as init pos
- self.init_qpos[0] = -1.5 # place palm up
def get_obs_vec(self):
# qpos for hand, xpos for obj, xpos for target
diff --git a/robohive/envs/myo/myobase/reorient_sar_v0.py b/robohive/envs/myo/myobase/reorient_sar_v0.py
index ed99a603..246de692 100644
--- a/robohive/envs/myo/myobase/reorient_sar_v0.py
+++ b/robohive/envs/myo/myobase/reorient_sar_v0.py
@@ -69,13 +69,17 @@ def _setup(self,
self.tar_length = np.linalg.norm(self.sim.model.geom_pos[self.tar_t_gid] - self.sim.model.geom_pos[self.tar_b_gid])
self.sim.model.body_mass[self.obj_bid] *= 1.25
+
+ init_qpos = self.sim.data.qpos.ravel().copy()
+ init_qpos[:-6] *= 0 # Use fully open as init pos
+ init_qpos[0] = -1.5 # place palm up
+
super()._setup(obs_keys=['hand_jnt','obj_pos','obj_vel','obj_rot','obj_des_rot',
'obj_err_pos','obj_err_rot','mlen','mvel','mforce'],
weighted_reward_keys=weighted_reward_keys,
+ init_qpos=init_qpos,
**kwargs,
)
- self.init_qpos[:-6] *= 0 # Use fully open as init pos
- self.init_qpos[0] = -1.5 # place palm up
def get_obs_dict(self, sim):
diff --git a/robohive/envs/myo/myobase/walk_v0.py b/robohive/envs/myo/myobase/walk_v0.py
index a5bf3164..65025cbd 100644
--- a/robohive/envs/myo/myobase/walk_v0.py
+++ b/robohive/envs/myo/myobase/walk_v0.py
@@ -53,9 +53,9 @@ def _setup(self,
super()._setup(obs_keys=obs_keys,
weighted_reward_keys=weighted_reward_keys,
sites=self.target_reach_range.keys(),
+ init_qpos=self.sim.model.key_qpos[0].copy(),
**kwargs,
)
- self.init_qpos[:] = self.sim.model.key_qpos[0]
self.init_qvel[:] = self.sim.model.key_qvel[0]
# find geometries with ID == 1 which indicates the skins
geom_1_indices = np.where(self.sim.model.geom_group == 1)
@@ -206,9 +206,9 @@ def _setup(self,
self.steps = 0
super()._setup(obs_keys=obs_keys,
weighted_reward_keys=weighted_reward_keys,
+ init_qpos=self.sim.model.key_qpos[0].copy(),
**kwargs
)
- self.init_qpos[:] = self.sim.model.key_qpos[0]
self.init_qvel[:] = 0.0
# move heightfield down if not used
@@ -473,9 +473,9 @@ def _setup(self,
BaseV0._setup(self, obs_keys=obs_keys,
weighted_reward_keys=weighted_reward_keys,
+ init_qpos=self.sim.model.key_qpos[0].copy(),
**kwargs
)
- self.init_qpos[:] = self.sim.model.key_qpos[0]
self.init_qvel[:] = 0.0
def reset(self, **kwargs):
diff --git a/robohive/envs/myo/myochallenge/__init__.py b/robohive/envs/myo/myochallenge/__init__.py
index 26fc6f7c..44708719 100644
--- a/robohive/envs/myo/myochallenge/__init__.py
+++ b/robohive/envs/myo/myochallenge/__init__.py
@@ -1,8 +1,12 @@
from robohive.utils import gym; register=gym.register
import os
+
+from robohive.utils.prompt_utils import Prompt, prompt
+
curr_dir = os.path.dirname(os.path.abspath(__file__))
import numpy as np
+prompt("RoboHive:> Registering MyoChallenge Envs", Prompt.ONCE)
# MyoChallenge 2023 envs ==============================================
# MyoChallenge Manipulation P1
diff --git a/robohive/envs/myo/myochallenge/baoding_v1.py b/robohive/envs/myo/myochallenge/baoding_v1.py
index 861a1f40..c226b80a 100644
--- a/robohive/envs/myo/myochallenge/baoding_v1.py
+++ b/robohive/envs/myo/myochallenge/baoding_v1.py
@@ -96,16 +96,18 @@ def _setup(self,
self.obj_friction_range = {'low':self.sim.model.geom_friction[self.object1_gid] - obj_friction_change,
'high':self.sim.model.geom_friction[self.object1_gid] + obj_friction_change} if obj_friction_change else None
+ # reset position
+ init_qpos = self.sim.data.qpos.ravel().copy()
+ init_qpos[:-14] *= 0 # Use fully open as init pos
+ init_qpos[0] = -1.57 # Palm up
+
super()._setup(obs_keys=obs_keys,
weighted_reward_keys=weighted_reward_keys,
frame_skip=frame_skip,
+ init_qpos=init_qpos,
**kwargs,
)
- # reset position
- self.init_qpos[:-14] *= 0 # Use fully open as init pos
- self.init_qpos[0] = -1.57 # Palm up
-
def step(self, a, **kwargs):
if self.which_task in [Task.HOLD, Task.BAODING_CW, Task.BAODING_CCW]:
desired_angle_wrt_palm = self.goal[self.counter].copy()
diff --git a/robohive/envs/myo/myochallenge/chasetag_v0.py b/robohive/envs/myo/myochallenge/chasetag_v0.py
index 2c035765..09a6e475 100644
--- a/robohive/envs/myo/myochallenge/chasetag_v0.py
+++ b/robohive/envs/myo/myochallenge/chasetag_v0.py
@@ -679,7 +679,6 @@ def _setup(self,
reset_type=reset_type,
**kwargs
)
- self.init_qpos[:] = self.sim.model.key_qpos[0]
self.init_qvel[:] = 0.0
self.startFlag = True
self.assert_settings()
diff --git a/robohive/envs/myo/myochallenge/relocate_v0.py b/robohive/envs/myo/myochallenge/relocate_v0.py
index 5547c169..36361e81 100644
--- a/robohive/envs/myo/myochallenge/relocate_v0.py
+++ b/robohive/envs/myo/myochallenge/relocate_v0.py
@@ -4,11 +4,13 @@
================================================= """
import collections
+
import numpy as np
-from robohive.utils import gym
from robohive.envs.myo.base_v0 import BaseV0
-from robohive.utils.quat_math import mat2euler, euler2quat
+from robohive.utils import gym
+from robohive.utils.quat_math import euler2quat, mat2euler
+
class RelocateEnvV0(BaseV0):
@@ -57,12 +59,12 @@ def _setup(self,
self.rot_th = rot_th
self.drop_th = drop_th
+ keyFrame_id = 0 if self.obj_xyz_range is None else 1
super()._setup(obs_keys=obs_keys,
weighted_reward_keys=weighted_reward_keys,
+ init_qpos=self.sim.model.key_qpos[keyFrame_id].copy(),
**kwargs,
)
- keyFrame_id = 0 if self.obj_xyz_range is None else 1
- self.init_qpos[:] = self.sim.model.key_qpos[keyFrame_id].copy()
def get_obs_dict(self, sim):
diff --git a/robohive/envs/myo/myochallenge/reorient_v0.py b/robohive/envs/myo/myochallenge/reorient_v0.py
index fd919193..92937f64 100644
--- a/robohive/envs/myo/myochallenge/reorient_v0.py
+++ b/robohive/envs/myo/myochallenge/reorient_v0.py
@@ -67,12 +67,15 @@ def _setup(self,
self.obj_friction_range = {'low':self.sim.model.geom_friction[self.object_gid0:self.object_gidn] - obj_friction_change,
'high':self.sim.model.geom_friction[self.object_gid0:self.object_gidn] + obj_friction_change}
+ init_qpos = self.sim.data.qpos.ravel().copy()
+ init_qpos[:-7] *= 0 # Use fully open as init pos
+ init_qpos[0] = -1.5 # Palm up
+
super()._setup(obs_keys=obs_keys,
weighted_reward_keys=weighted_reward_keys,
+ init_qpos=init_qpos,
**kwargs,
)
- self.init_qpos[:-7] *= 0 # Use fully open as init pos
- self.init_qpos[0] = -1.5 # Palm up
def get_obs_dict(self, sim):
obs_dict = {}
diff --git a/robohive/envs/myo/myodm/myodm_v0.py b/robohive/envs/myo/myodm/myodm_v0.py
index abd47ea0..012daab7 100644
--- a/robohive/envs/myo/myodm/myodm_v0.py
+++ b/robohive/envs/myo/myodm/myodm_v0.py
@@ -135,23 +135,25 @@ def _setup(self,
self._lift_z = self.sim.data.xipos[self.object_bid][2] + self.lift_bonus_thresh
+ # Adjust init as per the specified key
+ init_qpos = self.sim.data.qpos.ravel().copy()
+ robot_init, object_init = self.ref.get_init()
+ if robot_init is not None:
+ init_qpos[:self.ref.robot_dim] = robot_init
+ if object_init is not None:
+ init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3]
+ init_qpos[-3:] = quat2euler(object_init[3:])
+
super()._setup(obs_keys=obs_keys,
weighted_reward_keys=weighted_reward_keys,
frame_skip=10,
+ init_qpos=init_qpos,
**kwargs)
# Adjust horizon if not motion_extrapolation
if motion_extrapolation == False:
self.spec.max_episode_steps = self.ref.horizon # doesn't work always. WIP
- # Adjust init as per the specified key
- robot_init, object_init = self.ref.get_init()
- if robot_init is not None:
- self.init_qpos[:self.ref.robot_dim] = robot_init
- if object_init is not None:
- self.init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3]
- self.init_qpos[-3:] = quat2euler(object_init[3:])
-
# hack because in the super()._setup the initial posture is set to the average qpos and when a step is called, it ends in a `done` state
self.initialized_pos = True
# if self.sim.model.nkey>0:
@@ -285,7 +287,7 @@ def playback(self):
ref_mot = self.ref.get_reference(self.time+self.motion_start_time)
self.qpos_from_robot_object(self.sim.data.qpos, ref_mot.robot, ref_mot.object )
self.sim.forward()
- self.sim.data.time = self.sim.data.time + 0.02#self.env.env.dt
+ self.sim.data.time = self.sim.data.time + 0.02#self.env.unwrapped.dt
return idxs[0] < self.ref.horizon-1
diff --git a/robohive/envs/myo/myomimic/myomimic_v0.py b/robohive/envs/myo/myomimic/myomimic_v0.py
index c634de95..212e3bd0 100644
--- a/robohive/envs/myo/myomimic/myomimic_v0.py
+++ b/robohive/envs/myo/myomimic/myomimic_v0.py
@@ -82,23 +82,25 @@ def _setup(self,
self.TermPose = Termimate_pose_fail
##########################################
+ # Adjust init as per the specified key
+ init_qpos = self.sim.data.qpos.ravel().copy()
+ robot_init, object_init = self.ref.get_init()
+ if robot_init is not None:
+ init_qpos[:self.ref.robot_dim] = robot_init
+ if object_init is not None:
+ init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3]
+ init_qpos[-3:] = quat2euler(object_init[3:])
+
super()._setup(obs_keys=obs_keys,
weighted_reward_keys=weighted_reward_keys,
frame_skip=10,
+ init_qpos=init_qpos,
**kwargs)
# Adjust horizon if not motion_extrapolation
if motion_extrapolation == False:
self.spec.max_episode_steps = self.ref.horizon # doesn't work always. WIP
- # Adjust init as per the specified key
- robot_init, object_init = self.ref.get_init()
- if robot_init is not None:
- self.init_qpos[:self.ref.robot_dim] = robot_init
- if object_init is not None:
- self.init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3]
- self.init_qpos[-3:] = quat2euler(object_init[3:])
-
# hack because in the super()._setup the initial posture is set to the average qpos and when a step is called, it ends in a `done` state
self.initialized_pos = True
# if self.sim.model.nkey>0:
diff --git a/robohive/envs/obs_vec_dict.py b/robohive/envs/obs_vec_dict.py
index fb79e5e6..36a771b0 100644
--- a/robohive/envs/obs_vec_dict.py
+++ b/robohive/envs/obs_vec_dict.py
@@ -5,9 +5,11 @@
License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
================================================= """
-import numpy as np
from collections import deque
+import numpy as np
+
+
class ObsVecDict():
"""
Class to help with conversion between obs_dict <> obs_vector
@@ -61,10 +63,29 @@ def initialize(self, obs_dict, ordered_obs_keys):
t, obsvec = self.obsdict2obsvec(obs_dict, ordered_obs_keys)
self.obsvec_cache_flush(t, obsvec) # populate the cache with initial obsvec values
- # Squeeze out singleton dimensions
+ # Squeeze out the (num_traj=1, horizon=1) dims added by expand_dims.
+ # Squeezing all singleton dims (np.squeeze(arr)) would also collapse
+ # single-value obs (e.g. shape (1,)) down to 0-d scalars, which then
+ # fail to concatenate with other obs vectors downstream.
def squeeze_dims(self, obs_dict):
for key in obs_dict.keys():
- obs_dict[key] = np.squeeze(obs_dict[key])
+ val = np.asarray(obs_dict[key])
+
+ # expand_dims() only ever adds two leading dims: (num_traj=1, horizon=1, ...).
+ # Some dict values (e.g. scalar entries in rwd_dict like 'done', 'solved')
+ # never went through expand_dims, so they may have 0 or 1 dims. Only squeeze
+ # axis 0 and/or axis 1 if they actually exist AND are singleton -- this way we
+ # never touch a real data dimension (e.g. a single-value obs of shape (1,)).
+ axes_to_squeeze = []
+ if val.ndim > 0 and val.shape[0] == 1:
+ axes_to_squeeze.append(0)
+ if val.ndim > 1 and val.shape[1] == 1:
+ axes_to_squeeze.append(1)
+
+ if axes_to_squeeze:
+ val = np.squeeze(val, axis=tuple(axes_to_squeeze))
+
+ obs_dict[key] = val
return obs_dict
# Exapand observation dimensions to (num_traj=1, horizon=1, obs_dim)
@@ -79,7 +100,7 @@ def obsdict2obsvec(self, obs_dict, ordered_obs_keys):
self.initialize(obs_dict, ordered_obs_keys)
# recover vec
- obsvec = np.zeros(0)
+ obsvec = np.zeros(0, dtype=np.float32)
for key in self.ordered_obs_keys:
obsvec = np.concatenate([obsvec, obs_dict[key].ravel()]) # ravel helps with images
diff --git a/robohive/envs/quadrupeds/__init__.py b/robohive/envs/quadrupeds/__init__.py
index d5cb8855..ed289d4c 100644
--- a/robohive/envs/quadrupeds/__init__.py
+++ b/robohive/envs/quadrupeds/__init__.py
@@ -1,11 +1,14 @@
from robohive.utils import gym; register=gym.register
-from robohive.envs.env_variants import register_env_variant
-import numpy as np
import os
-curr_dir = os.path.dirname(os.path.abspath(__file__))
-print("RoboHive:> Registering Quadruped Envs")
+import numpy as np
+
+from robohive.envs.env_variants import register_env_variant
+from robohive.utils.prompt_utils import Prompt, prompt
+
+curr_dir = os.path.dirname(os.path.abspath(__file__))
+prompt("RoboHive:> Registering Quadruped Envs", Prompt.ONCE)
WALK_HORIZON = 160
@@ -35,6 +38,7 @@
# Reach to random target using proprio and visual inputs
from robohive.envs.quadrupeds.walk_v0 import WalkBaseV0
+
register_env_variant(
variant_id='DKittyWalkRandom_v2d-v0',
env_id='DKittyWalkRandom-v0',
@@ -78,6 +82,7 @@
)
# Orient to random orientation using proprio and visual inputs
from robohive.envs.quadrupeds.orient_v0 import OrientBaseV0
+
register_env_variant(
variant_id='DKittyOrientRandom_v2d-v0',
env_id='DKittyOrientRandom-v0',
@@ -117,6 +122,7 @@
)
# Stand-Up from random pose using proprio and visual inputs
from robohive.envs.quadrupeds.orient_v0 import OrientBaseV0
+
register_env_variant(
variant_id='DKittyStandRandom_v2d-v0',
env_id='DKittyStandRandom-v0',
diff --git a/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config b/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config
index 7e1c3b58..73318883 100644
--- a/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config
+++ b/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config
@@ -3,61 +3,61 @@
'dkitty_root':{
'interface': {'type': 'optitrack', 'server_name': '169.254.163.86', 'client_name': '169.254.163.96','port':5000, 'packet_size':36, 'id':'1'},
'sensor':[
- {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'A:Tx'},
- {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_id':1, 'scale':1, 'offset':0, 'name':'A:Ty'},
- {'range':(-2.00, 2.00), 'noise':0.005, 'hdr_id':2, 'scale':1, 'offset':-.32, 'name':'A:Tz'},
- {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_id':3, 'scale':1, 'offset':0, 'name':'A:Rx'},
- {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'A:Ry'},
- {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_id':5, 'scale':1, 'offset':0, 'name':'A:Rz'},
+ {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'A:Tx'},
+ {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'A:Ty'},
+ {'range':(-2.00, 2.00), 'noise':0.005, 'hdr_adr':2, 'scale':1, 'offset':-.32, 'name':'A:Tz'},
+ {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'A:Rx'},
+ {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'A:Ry'},
+ {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':0, 'name':'A:Rz'},
],
'actuator':[]
},
# device1: sensors, actuators
'dkitty':{
- 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/dev/DKitty"},
+ 'interface': {'type': 'dynamixel', 'motor_type':"X", 'port':"/dev/DKitty"},
'sensor':[
- {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_id':10, 'scale':+1, 'offset':-3*np.pi/2, 'name':'A:FRJ10_pos_sensor'},
- {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_id':11, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ11_pos_sensor'},
- {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_id':12, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ12_pos_sensor'},
- {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_id':20, 'scale':+1, 'offset':-np.pi/2, 'name':'A:FLJ20_pos_sensor'},
- {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_id':21, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ21_pos_sensor'},
- {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_id':22, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ22_pos_sensor'},
- {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_id':30, 'scale':-1, 'offset':3*np.pi/2, 'name':'A:BLJ30_pos_sensor'},
- {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_id':31, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ31_pos_sensor'},
- {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_id':32, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ32_pos_sensor'},
- {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_id':40, 'scale':-1, 'offset':np.pi/2, 'name':'A:BRJ40_pos_sensor'},
- {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_id':41, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ41_pos_sensor'},
- {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_id':42, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ42_pos_sensor'}
+ {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_adr':10, 'scale':+1, 'offset':-3*np.pi/2, 'name':'A:FRJ10_pos_sensor'},
+ {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':11, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ11_pos_sensor'},
+ {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':12, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ12_pos_sensor'},
+ {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_adr':20, 'scale':+1, 'offset':-np.pi/2, 'name':'A:FLJ20_pos_sensor'},
+ {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':21, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ21_pos_sensor'},
+ {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':22, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ22_pos_sensor'},
+ {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_adr':30, 'scale':-1, 'offset':3*np.pi/2, 'name':'A:BLJ30_pos_sensor'},
+ {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':31, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ31_pos_sensor'},
+ {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':32, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ32_pos_sensor'},
+ {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_adr':40, 'scale':-1, 'offset':np.pi/2, 'name':'A:BRJ40_pos_sensor'},
+ {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':41, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ41_pos_sensor'},
+ {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':42, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ42_pos_sensor'}
],
'actuator':[
- {'pos_range':(-1.57 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10'},
- {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11'},
- {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12'},
- {'pos_range':(-0.279, 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20'},
- {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21'},
- {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22'},
- {'pos_range':(-0.279, 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30'},
- {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31'},
- {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32'},
- {'pos_range':(-1.57 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40'},
- {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41'},
- {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42'}
+ {'pos_range':(-1.57 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10', 'mode':'Position'},
+ {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11', 'mode':'Position'},
+ {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12', 'mode':'Position'},
+ {'pos_range':(-0.279, 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20', 'mode':'Position'},
+ {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21', 'mode':'Position'},
+ {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22', 'mode':'Position'},
+ {'pos_range':(-0.279, 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30', 'mode':'Position'},
+ {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31', 'mode':'Position'},
+ {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32', 'mode':'Position'},
+ {'pos_range':(-1.57 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40', 'mode':'Position'},
+ {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41', 'mode':'Position'},
+ {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42', 'mode':'Position'}
]
# 'actuator':[ # restricted shoulder
- # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10'},
- # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11'},
- # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12'},
- # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20'},
- # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21'},
- # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22'},
- # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30'},
- # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31'},
- # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32'},
- # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40'},
- # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41'},
- # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42'}
+ # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10', 'mode':'Position'},
+ # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11', 'mode':'Position'},
+ # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12', 'mode':'Position'},
+ # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20', 'mode':'Position'},
+ # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21', 'mode':'Position'},
+ # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22', 'mode':'Position'},
+ # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30', 'mode':'Position'},
+ # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31', 'mode':'Position'},
+ # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32', 'mode':'Position'},
+ # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40', 'mode':'Position'},
+ # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41', 'mode':'Position'},
+ # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42', 'mode':'Position'}
# ]
}
}
\ No newline at end of file
diff --git a/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config b/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config
index 33dfe430..e9fed7c1 100644
--- a/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config
+++ b/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config
@@ -3,61 +3,61 @@
'dkitty_root':{
'interface': {'type': 'optitrack', 'server_name': '169.254.163.86', 'client_name': '169.254.163.96','port':5000, 'packet_size':36, 'id':'1'},
'sensor':[
- {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_id':0, 'scale':1, 'offset':0, 'name':'A:Tx'},
- {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_id':1, 'scale':1, 'offset':0, 'name':'A:Ty'},
- {'range':(-2.00, 2.00), 'noise':0.005, 'hdr_id':2, 'scale':1, 'offset':-.32, 'name':'A:Tz'},
- {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_id':3, 'scale':1, 'offset':0, 'name':'A:Rx'},
- {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_id':4, 'scale':1, 'offset':0, 'name':'A:Ry'},
- {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_id':5, 'scale':1, 'offset':0, 'name':'A:Rz'},
+ {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'A:Tx'},
+ {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'A:Ty'},
+ {'range':(-2.00, 2.00), 'noise':0.005, 'hdr_adr':2, 'scale':1, 'offset':-.32, 'name':'A:Tz'},
+ {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'A:Rx'},
+ {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'A:Ry'},
+ {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':0, 'name':'A:Rz'},
],
'actuator':[]
},
# device1: sensors, actuators
'dkitty':{
- 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/dev/DKitty"},
+ 'interface': {'type': 'dynamixel', 'motor_type':"X", 'port':"/dev/DKitty"},
'sensor':[
- {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_id':10, 'scale':+1, 'offset':-3*np.pi/2, 'name':'A:FRJ10_pos_sensor'},
- {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_id':11, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ11_pos_sensor'},
- {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_id':12, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ12_pos_sensor'},
- {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_id':20, 'scale':+1, 'offset':-np.pi/2, 'name':'A:FLJ20_pos_sensor'},
- {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_id':21, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ21_pos_sensor'},
- {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_id':22, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ22_pos_sensor'},
- {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_id':30, 'scale':-1, 'offset':3*np.pi/2, 'name':'A:BLJ30_pos_sensor'},
- {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_id':31, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ31_pos_sensor'},
- {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_id':32, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ32_pos_sensor'},
- {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_id':40, 'scale':-1, 'offset':np.pi/2, 'name':'A:BRJ40_pos_sensor'},
- {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_id':41, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ41_pos_sensor'},
- {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_id':42, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ42_pos_sensor'}
+ {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_adr':10, 'scale':+1, 'offset':-3*np.pi/2, 'name':'A:FRJ10_pos_sensor'},
+ {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':11, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ11_pos_sensor'},
+ {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':12, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ12_pos_sensor'},
+ {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_adr':20, 'scale':+1, 'offset':-np.pi/2, 'name':'A:FLJ20_pos_sensor'},
+ {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':21, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ21_pos_sensor'},
+ {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':22, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ22_pos_sensor'},
+ {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_adr':30, 'scale':-1, 'offset':3*np.pi/2, 'name':'A:BLJ30_pos_sensor'},
+ {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':31, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ31_pos_sensor'},
+ {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':32, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ32_pos_sensor'},
+ {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_adr':40, 'scale':-1, 'offset':np.pi/2, 'name':'A:BRJ40_pos_sensor'},
+ {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':41, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ41_pos_sensor'},
+ {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':42, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ42_pos_sensor'}
],
'actuator':[
- {'pos_range':(-0.75 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10'},
- {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11'},
- {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12'},
- {'pos_range':(-0.279, 0.75 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20'},
- {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21'},
- {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22'},
- {'pos_range':(-0.279, 0.75 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30'},
- {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31'},
- {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32'},
- {'pos_range':(-0.75 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40'},
- {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41'},
- {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42'}
+ {'pos_range':(-0.75 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10', 'mode':'Position'},
+ {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11', 'mode':'Position'},
+ {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12', 'mode':'Position'},
+ {'pos_range':(-0.279, 0.75 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20', 'mode':'Position'},
+ {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21', 'mode':'Position'},
+ {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22', 'mode':'Position'},
+ {'pos_range':(-0.279, 0.75 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30', 'mode':'Position'},
+ {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31', 'mode':'Position'},
+ {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32', 'mode':'Position'},
+ {'pos_range':(-0.75 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40', 'mode':'Position'},
+ {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41', 'mode':'Position'},
+ {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42', 'mode':'Position'}
]
# 'actuator':[ # restricted shoulder
- # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10'},
- # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11'},
- # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12'},
- # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20'},
- # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21'},
- # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22'},
- # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30'},
- # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31'},
- # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32'},
- # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40'},
- # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41'},
- # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_id':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42'}
+ # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10', 'mode':'Position'},
+ # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11', 'mode':'Position'},
+ # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12', 'mode':'Position'},
+ # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20', 'mode':'Position'},
+ # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21', 'mode':'Position'},
+ # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22', 'mode':'Position'},
+ # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30', 'mode':'Position'},
+ # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31', 'mode':'Position'},
+ # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32', 'mode':'Position'},
+ # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40', 'mode':'Position'},
+ # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41', 'mode':'Position'},
+ # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42', 'mode':'Position'}
# ]
}
}
\ No newline at end of file
diff --git a/robohive/envs/quadrupeds/orient_v0.py b/robohive/envs/quadrupeds/orient_v0.py
index 776e9978..e115cbf3 100644
--- a/robohive/envs/quadrupeds/orient_v0.py
+++ b/robohive/envs/quadrupeds/orient_v0.py
@@ -91,6 +91,8 @@ def _setup(self,
**kwargs)
# configure
+ # self.robot (and its robot_config) is only created inside super()._setup(), so this
+ # can't be computed earlier and passed via the init_qpos kwarg like other envs.
for name, device in self.robot.robot_config.items():
for act_id, actuator in enumerate(device['actuator']):
self.init_qpos[actuator['data_id']] = np.mean(actuator['pos_range'])
diff --git a/robohive/envs/quadrupeds/walk_v0.py b/robohive/envs/quadrupeds/walk_v0.py
index bf91307f..0d5d3c2d 100644
--- a/robohive/envs/quadrupeds/walk_v0.py
+++ b/robohive/envs/quadrupeds/walk_v0.py
@@ -92,6 +92,8 @@ def _setup(self,
**kwargs)
# configure
+ # self.robot (and its robot_config) is only created inside super()._setup(), so this
+ # can't be computed earlier and passed via the init_qpos kwarg like other envs.
for name, device in self.robot.robot_config.items():
for act_id, actuator in enumerate(device['actuator']):
self.init_qpos[actuator['data_id']] = np.mean(actuator['pos_range'])
diff --git a/robohive/envs/tcdm/__init__.py b/robohive/envs/tcdm/__init__.py
index 055ac7ac..df1ec829 100644
--- a/robohive/envs/tcdm/__init__.py
+++ b/robohive/envs/tcdm/__init__.py
@@ -6,13 +6,16 @@
================================================= """
from robohive.utils import gym; register=gym.register
+import collections
+import os
import numpy as np
-import os
-import collections
+
+from robohive.utils.prompt_utils import Prompt, prompt
+
curr_dir = os.path.dirname(os.path.abspath(__file__))
-print("RoboHive:> Registering TCDM Envs")
+prompt("RoboHive:> Registering TCDM Envs", Prompt.ONCE)
# Task specification format
task_spec = collections.namedtuple('task_spec',
diff --git a/robohive/envs/tcdm/playback_mocap.py b/robohive/envs/tcdm/playback_mocap.py
index 44ce818a..0c69f1cb 100644
--- a/robohive/envs/tcdm/playback_mocap.py
+++ b/robohive/envs/tcdm/playback_mocap.py
@@ -23,9 +23,9 @@
args = parser.parse_args()
args.end = args.end if args.end is not None else args.start + args.length
- # envMyoSuit=gym.make('MyoHandAirplanePass-v0')
- envMyoSuit=gym.make(args.sim_name)
- physics_myo=envMyoSuit.env.sim
+ # envMyoSuite=gym.make('MyoHandAirplanePass-v0')
+ envMyoSuite=gym.make(args.sim_name)
+ physics_myo=envMyoSuite.unwrapped.sim
# jx_mocap coordinates
# for i,b in enumerate(BODIES): #print(b,i, physics_myo.sim.named.data.xipos[b])
# print(f"\" ")
diff --git a/robohive/envs/tcdm/track.py b/robohive/envs/tcdm/track.py
index af75b558..9101e044 100644
--- a/robohive/envs/tcdm/track.py
+++ b/robohive/envs/tcdm/track.py
@@ -128,23 +128,25 @@ def _setup(self,
self._lift_z = self.sim.data.xipos[self.object_bid][2] + self.lift_bonus_thresh
+ # Adjust init as per the specified key
+ init_qpos = self.sim.data.qpos.ravel().copy()
+ robot_init, object_init = self.ref.get_init()
+ if robot_init is None:
+ init_qpos[:self.ref.robot_dim] = robot_init
+ if object_init is None:
+ init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3]
+ init_qpos[-3:] = quat2euler(object_init[3:])
+
super()._setup(obs_keys=obs_keys,
weighted_reward_keys=weighted_reward_keys,
frame_skip=10,
+ init_qpos=init_qpos,
**kwargs)
# Adjust horizon if not motion_extrapolation
if motion_extrapolation == False:
self.spec.max_episode_steps = self.ref.horizon # doesn't work always. WIP
- # Adjust init as per the specified key
- robot_init, object_init = self.ref.get_init()
- if robot_init is None:
- self.init_qpos[:self.ref.robot_dim] = robot_init
- if object_init is None:
- self.init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3]
- self.init_qpos[-3:] = quat2euler(object_init[3:])
-
# hack because in the super()._setup the initial posture is set to the average qpos and when a step is called, it ends in a `done` state
self.initialized_pos = True
# if self.sim.model.nkey>0:
@@ -273,7 +275,7 @@ def playback(self):
ref_mot = self.ref.get_reference(self.time)
self.qpos_from_robot_object(self.sim.data.qpos, ref_mot.robot, ref_mot.object )
self.sim.forward()
- self.sim.data.time = self.sim.data.time + 0.02#self.env.env.dt
+ self.sim.data.time = self.sim.data.time + 0.02#self.env.unwrapped.dt
return idxs[0] < self.ref.horizon-1
diff --git a/robohive/logger/examine_logs.py b/robohive/logger/examine_logs.py
index 621c3711..4782a8c1 100644
--- a/robohive/logger/examine_logs.py
+++ b/robohive/logger/examine_logs.py
@@ -15,13 +15,14 @@
$ python logger/examine_logs.py --env_name rpFrankaRobotiqData-v0 --rollout_path teleOp_trace.h5 --rollout_format RoboSet --render offscreen --compress_paths False -c left_cam -c right_cam -c top_cam -c Franka_wrist_cam --plot_paths True
'''
-from robohive.utils.paths_utils import plot as plotnsave_paths
-from robohive.utils import tensor_utils
-from robohive.utils import gym
+import os
+import time
+
import click
import numpy as np
-import time
-import os
+
+from robohive.utils import gym, tensor_utils
+from robohive.utils.paths_utils import plot as plotnsave_paths
@click.command(help=DESC)
@@ -210,6 +211,9 @@ def examine_logs(env_name, rollout_path, rollout_format, mode, horizon, seed, nu
elif i_step < trace_horizon: # incase last step actions (nans) can cause issues in step
obs, rwd, done, *_, env_info = env.step(act, update_exteroception=include_exteroception)
ep_rwd += rwd
+ sleep_time = 1*env.dt if render=='onscreen' else 0.0
+ if sleep_time>0:
+ time.sleep(sleep_time)
# save offscreen buffers as video and clear the dataset
if render == 'offscreen':
@@ -229,7 +233,7 @@ def examine_logs(env_name, rollout_path, rollout_format, mode, horizon, seed, nu
# plot paths
if plot_paths:
file_name = os.path.join(output_dir, output_name + '{}'.format(time_stamp))
- plotnsave_paths(trace.trace, env=env, fileName_prefix=file_name)
+ plotnsave_paths(trace.trace, env_handle=env, output_name=file_name)
# Close and save paths
trace.close()
diff --git a/robohive/logger/grouped_datasets.py b/robohive/logger/grouped_datasets.py
index 3450af9e..0ba24e65 100644
--- a/robohive/logger/grouped_datasets.py
+++ b/robohive/logger/grouped_datasets.py
@@ -19,7 +19,6 @@
# access pattern for pickle and h5 backbone post load isn't the same
# - Should we get rid of pickle support and double down on h5?
# - other way would to make the default container (trace.trace) h5 container instead of a dict
-# Should we explicitely keep tract if the trace has been flattened/ stacked/ closed etc?
class TraceType(enum.Enum):
@@ -32,7 +31,7 @@ def get_type(input_type):
"""
A more robust way of getting trace type. Supports strings
"""
- if type(input_type) == str:
+ if isinstance(input_type, str):
if input_type.lower() == "robohive":
return TraceType.ROBOHIVE
elif input_type.lower() == "roboset":
@@ -49,22 +48,23 @@ def __init__(self, name):
self.trace = self.root[name]
self.index = 0
self.type = TraceType.ROBOHIVE
+ self.closed = False # False: Trace is open for edits. True: Trace can be analyzed but not edited.
# Create a group in your logs
def create_group(self, name):
self.trace[name] = {}
- # Directly add a full dataset to a given group
+ # Directly add a full dataset to a given group. If data appending is needed, use create_datum() instead
def create_dataset(self, group_key, dataset_key, dataset_val):
if group_key not in self.trace.keys():
self.create_group(name=group_key)
- self.trace[group_key][dataset_key] = [dataset_val]
+ self.trace[group_key][dataset_key] = dataset_val
# Remove dataset from an existing group(s)
def remove_dataset(self, group_keys:list, dataset_key:str):
- if type(group_keys)==str:
+ if isinstance(group_keys, str):
if group_keys==":":
group_keys = self.trace.keys()
else:
@@ -76,6 +76,13 @@ def remove_dataset(self, group_keys:list, dataset_key:str):
del self.trace[group_key][dataset_key]
+ # Create the first datum of an existing group. Use append_datum() to append more elements
+ def create_datum(self, group_key, dataset_key, dataset_val):
+ if group_key not in self.trace.keys():
+ self.create_group(name=group_key)
+ self.trace[group_key][dataset_key] = [dataset_val]
+
+
# Append dataset datum to an existing group
def append_datum(self, group_key, dataset_key, dataset_val):
assert group_key in self.trace.keys(), "Group:{} does not exist".format(group_key)
@@ -114,7 +121,21 @@ def set(self, group_key, dataset_key, dataset_ind=None, dataset_val=None):
# verify if a data can be a part of an existing datasets
def verify_type(self, dataset, data):
dataset_type = type(dataset[0])
- assert type(data) == dataset_type, TypeError("Type mismatch while appending. Datum should be {}".format(dataset_type))
+ assert isinstance(data, dataset_type), TypeError("Type mismatch while appending. Datum should be {}".format(dataset_type))
+
+ # check for array
+ if isinstance(data, np.ndarray):
+ assert data.shape == dataset[0].shape, ValueError(f"Data dimenstion({data.shape}) not compatible with dataset dimensions({dataset[0].shape})")
+ # check for list
+ if isinstance(data, list):
+ assert len(data) == len(dataset[0]), ValueError(f"Data dimenstion({len(data)}) not compatible with dataset dimensions({len(dataset[0])})")
+ # check for dictionary
+ if isinstance(data, dict):
+ flattened_data = flatten_dict(data)
+ flattened_dataset = flatten_dict(dataset[0])
+ assert flattened_data.keys() == flattened_dataset.keys(), ValueError(f"Data keys {flattened_data.keys()} not compatible with dataset keys {flattened_dataset.keys()}")
+ for key in flattened_data:
+ assert np.array(flattened_data[key]).shape == np.array(flattened_dataset[key]).shape, ValueError(f"Data dimension for key '{key}' ({np.array(flattened_data[key]).shape}) not compatible with dataset dimensions ({np.array(flattened_dataset[key]).shape})")
# Verify that all datasets in each groups are of same length. Helpful for time synced traces
@@ -126,11 +147,14 @@ def verify_len(self):
trace_len = len(self.trace[grp_k][key])
else:
key_len = len(self.trace[grp_k][key])
- assert trace_len == key_len, ValueError("len({}[{}]={}, should be {}".format(grp_k, key, key_len, trace_len))
+ assert trace_len == key_len, ValueError("Dataset length mismatch: len({}[{}]={}, should be {}".format(grp_k, key, key_len, trace_len))
# Very if trace is stacked and flattened. Useful for utilities like render, save etc
def verify_stacked_flattened(self):
+ if self.closed:
+ True
+
for grp_k, grp_v in self.trace.items():
for dst_k, dst_v in grp_v.items():
# Check if stacked
@@ -141,6 +165,120 @@ def verify_stacked_flattened(self):
return False
return True
+ # plot data
+ def plot(self, output_dir, output_format, groups:list, datasets:list, x_dataset:str='time'):
+ # Plot dataset traces using the groups and datasets keys list. T
+ # ARGUMENTS:
+ # output_dir: path for output
+ # output_format: pdf/png/None(for onscreen)
+ # groups: - list(Groups)_ng to plot:
+ # - ng = len(groups) == number of subplots
+ # - ":" to consider each group once
+ # - None entry in the list can will leave the subplot empty
+ # datasets: - list(list(datasets))_ng to plot([['left',], ['right', 'top']]),
+ # - ng = len(groups) == len(datasets) == number of subplots
+ # - ":" to plot each dataset once
+ # x_dataset: - dataset key to use as x-axis if available
+ # EXAMPLES
+ # 1. plot(..., groups=":", data=":")
+ # produces a plot with len(groups) subplots
+ # 2. plot(...,groups=['traj1', 'traj1', 'traj2'], data=[['qpos'], ['qpos','qvel'], ['qvel']])
+ # produces a plot with three subplots
+
+ if not self.closed:
+ prompt("Trace is still open for edits. Close the trace to enable plotting", type=Prompt.WARN)
+ return
+
+ import matplotlib as mpl
+ # mpl.use('Agg')
+ import matplotlib.pyplot as plt
+ plt.rcParams.update({'font.size': 5})
+ h_fig = plt.figure(self.name)
+ plt.clf()
+
+ # Resolve groups
+ if isinstance(groups, str) and groups==":":
+ groups = list(self.trace.keys())
+ elif isinstance(groups, str):
+ groups = [groups]
+ else:
+ assert isinstance(groups, list), TypeError(f"Expected a list of groups. Got {groups}")
+
+ # number of subplots
+ n_subplot = len(groups)
+
+ # Check for datasets
+ if isinstance(datasets, str) and datasets==":":
+ datasets = n_subplot*[":"]
+ elif isinstance(datasets, str):
+ datasets = [datasets]
+ else:
+ assert (isinstance(datasets, list)), TypeError(f"Dataset keys needs to be a list. Got {datasets}")
+
+ # Check for group and datasets sizes
+ assert len(datasets)==n_subplot, ValueError(f"len(groups):{n_subplot} has to match len(datasets):{len(datasets)}")
+ # print(groups)
+ # print(datasets)
+
+ # Run through all groups
+ for i_grp, grp_key in enumerate(groups):
+
+ # Leave empty if requested
+ if grp_key is None:
+ continue
+
+ # process group / subplot
+ assert isinstance(grp_key, str), TypeError(f"Dataset key needs to be a string. Got {grp_key}")
+ assert grp_key in self.trace.keys(), "Unknown group {}. Available groups {}".format(grp_key, self.trace.keys())
+ grp_val = self.trace[grp_key]
+
+ # print('selected group', grp_key)
+
+ # Resolve datasets within existing group
+ if isinstance(datasets, str) and datasets==":":
+ i_grp_datasets = list(grp_val.keys())
+ elif isinstance(datasets[i_grp], str) and datasets[i_grp]==":":
+ i_grp_datasets = list(grp_val.keys())
+ else:
+ i_grp_datasets = datasets[i_grp]
+
+ assert isinstance(i_grp_datasets, list) and isinstance(i_grp_datasets[0], str), TypeError(f"Unrecognized dataset input for group:{grp_key}. Expected ':', or a list from {grp_val.keys()}. Got: {i_grp_datasets}")
+
+ # Run through all dataset requests within the group
+ for ds_key in i_grp_datasets:
+ assert ds_key in grp_val.keys(), f"Group: {grp_key} :> Unknown dataset {ds_key}. Available datasets {grp_val.keys()}"
+ ds_val = grp_val[ds_key]
+
+ assert isinstance(ds_val, np.ndarray), ValueError(f"Dataset for plotting needs to be an array. Provided data:{ds_val}, type:{type(ds_val)}")
+ assert np.issubdtype(ds_val.dtype, np.number), ValueError(f"Dataset for plotting needs to of numerical dtype. Provided dtype: {ds_val.dtype}")
+ assert len(ds_val.shape)<3, ValueError(f"Plotting is only supported for 1D and 2D Dataset. Provided data dims: {ds_val.shape}")
+
+ # print(f"g:{grp_key}/ d:{ds_key}")
+ h_axis = plt.subplot(n_subplot, 1, i_grp+1)
+ # h_axis.set_prop_cycle(None)
+
+ if x_dataset in grp_val.keys():
+ plt.plot(grp_val[x_dataset][:], ds_val, label=f"{ds_key}", marker='')
+ h_axis.set_xlabel(x_dataset)
+ elif x_dataset in self.trace.keys():
+ plt.plot(self.trace[x_dataset], ds_val, label=f"{ds_key}", marker='')
+ h_axis.set_xlabel(x_dataset)
+ else:
+ plt.plot(ds_val, label=f"{grp_key}/{ds_key}", marker='*')
+ h_axis.set_title(grp_key)
+ h_axis.legend()
+
+
+ # show/save plot
+ if output_format is None:
+ plt.show()
+ return False
+ else:
+ file_name = os.path.join(output_dir, f"{self.name}_{grp_key}_{ds_key}_{output_format}".replace("/", "_"))
+ # plt.savefig(file_name)
+ print("saved ", file_name)
+ return h_fig
+
# Render frames/videos
def render(self, output_dir, output_format, groups:list, datasets:list, input_fps:int=25):
@@ -249,10 +387,10 @@ def items(self):
return zip(self.trace.keys(), self)
# return length
- """
- returns the number of groups in the trace
- """
def __len__(self) -> str:
+ """
+ returns the number of groups in the trace
+ """
return len(self.trace.keys())
@@ -337,6 +475,8 @@ def close(self,
if verify_length:
self.verify_len()
+ self.closed = True
+
# Save
def save(self,
@@ -391,34 +531,109 @@ def load(trace_path, trace_type=TraceType.UNSET):
trace.trace = file_data[trace.name] # load data
trace.root = file_data # build root
trace.trace_type=TraceType.get_type(trace_type)
+ trace.closed = True
return trace
+def test_trace_plot():
+ trace = Trace("root_name")
+
+ data1 = np.sin(np.arange(0,100))
+ data2 = np.cos(np.arange(0,100))
+ data3 = np.sin(np.arange(0,200))+np.cos(np.arange(0,200))
+ time = 0.01*np.arange(0,200)
+
+ trace.create_group("grp1")
+ trace.create_dataset(group_key="grp1", dataset_key="dst1", dataset_val=data1)
+ trace.create_dataset(group_key="grp1", dataset_key="dst2", dataset_val=data2)
+
+ trace.create_group("grp2")
+ trace.create_dataset(group_key="grp2", dataset_key="time", dataset_val=time)
+ trace.create_dataset(group_key="grp2", dataset_key="dst3", dataset_val=data3)
+ trace.close()
+
+ trace.plot(output_format='plot0.pdf', output_dir=".", groups=["grp1",], datasets=[["dst1",],], x_dataset="dst1")
+ trace.plot(output_format='plot1.pdf', output_dir=".", groups=":", datasets=":")
+ trace.plot(output_format='plot2.pdf', output_dir=".", groups=":", datasets=[":", ":"])
+ trace.plot(output_format='plot3.pdf', output_dir=".", groups=":", datasets=[["dst2",], ":"])
+
+ # Catch issues plotting string array
+ try:
+ trace = Trace("string")
+ trace.create_dataset(group_key="grp1", dataset_key="dst_k1", dataset_val=np.array(["v1", "v2", "v3"]))
+ trace.close()
+ trace.plot(output_format=None, output_dir=".", groups=["grp1",], datasets=[["dst_k1",],])
+ except Exception as e:
+ prompt(f"EXPECTED: Caught exception while trying to plot array of strings: {e}", type=Prompt.WARN)
+
+ # Catch issues plotting list(strings)
+ try:
+ trace = Trace("string")
+ trace.create_dataset(group_key="grp1", dataset_key="dst_k1", dataset_val=["v1", "v2", "v3"])
+ trace.close()
+ trace.plot(output_format='plot4.pdf', output_dir=".", groups=["grp1",], datasets=[["dst_k1",],])
+ except Exception as e:
+ prompt(f"EXPECTED: Caught exception while trying to plot list of strings: {e}", type=Prompt.WARN)
+
+
+ # plot complex dicts
+ trace = Trace("root_dict")
+ trace.create_datum(group_key="grp1", dataset_key="dst_k1", dataset_val={"one":1, "two":2.0, "three":"3"})
+ trace.append_datum(group_key="grp1", dataset_key="dst_k1", dataset_val={"one":11, "two":22.0, "three":"33"})
+ trace.append_datum(group_key="grp1", dataset_key="dst_k1", dataset_val={"one":111, "two":222.0, "three":"333"})
+ trace.close()
+ trace.plot(output_format='plot5.pdf', output_dir=".", groups=["grp1"], datasets=[["dst_k1/one","dst_k1/two"],])
+ trace.plot(output_format='plot6.pdf', output_dir=".", groups=["grp1","grp1"], datasets=[["dst_k1/one"],["dst_k1/two"]])
+ trace.plot(output_format='plot7.pdf', output_dir=".", groups=["grp1","grp1"], datasets=[["dst_k1/one"],["dst_k1/one","dst_k1/two",]])
+ trace.plot(output_format='plot8.pdf', output_dir=".", groups=[None, "grp1"], datasets=[None, ["dst_k1/one","dst_k1/two"]])
+ # catch trying to plot strings
+ try:
+ trace.plot(output_format=None, output_dir=".", groups=["grp1"], datasets=[":"])
+ except Exception as e:
+ prompt(f"EXPECTED: Caught exception while trying to plot dict with strings: {e}", type=Prompt.WARN)
+
+
+ # Catch trying to plot >2D array
+ trace = Trace("root_3darray")
+ trace.create_group("grp1")
+ trace.create_dataset(group_key="grp1", dataset_key="dst_k1", dataset_val=np.ones([4, 2, 4]))
+ trace.close()
+ try:
+ trace.plot(output_format='plot9.pdf', output_dir=".", groups=["grp1"], datasets=[["dst_k1",],])
+ except Exception as e:
+ prompt(f"EXPECTED: Caught expected exception during plotting >2D dataset: {e}", type=Prompt.WARN)
+
+
# Test trace
def test_trace():
trace = Trace("Root_name")
# Create a group: append and verify
trace.create_group("grp1")
- trace.create_dataset(group_key="grp1", dataset_key="dst_k1", dataset_val="dst_v1")
+ trace.create_datum(group_key="grp1", dataset_key="dst_k1", dataset_val="dst_v1")
trace.append_datum(group_key="grp1", dataset_key="dst_k1", dataset_val="dst_v11")
- trace.create_dataset(group_key="grp1", dataset_key="dst_k2", dataset_val="dst_v2")
+ trace.create_datum(group_key="grp1", dataset_key="dst_k2", dataset_val="dst_v2")
trace.append_datum(group_key="grp1", dataset_key="dst_k2", dataset_val="dst_v22")
trace.verify_len()
# Add another group
trace.create_group("grp2")
- trace.create_dataset(group_key="grp2", dataset_key="dst_k3", dataset_val={"dst_v3":[3]})
- trace.create_dataset(group_key="grp2", dataset_key="dst_k4", dataset_val={"dst_v4":[4]})
+ trace.create_datum(group_key="grp2", dataset_key="dst_k3", dataset_val={"dst_v3":[3]})
+ trace.create_datum(group_key="grp2", dataset_key="dst_k4", dataset_val={"dst_v4":[4]})
print(trace)
# get set methods
datum = "dst_v111"
trace.set('grp1','dst_k1', 0, datum)
assert datum == trace.get('grp1','dst_k1', 0), "Get-Set error"
- datum = {"dst_v33":[33]}
+ datum = {"dst_v4":[0]}
trace.set('grp2','dst_k4', 0, datum)
assert datum == trace.get('grp2','dst_k4', 0), "Get-Set error"
+ try:
+ datum = {"dst_diff_name":[33]}
+ trace.set('grp2','dst_k4', 0, datum)
+ except Exception as e:
+ prompt(f"Caught expected exception trying to insert an inconsistent datum: {e}", type=Prompt.WARN)
# save-load methods
trace.save(trace_name='test_trace.pickle', verify_length=True)
@@ -432,10 +647,77 @@ def test_trace():
print("PKL trace")
print(pkl_trace)
-if __name__ == '__main__':
- test_trace()
+def test_trace_append():
+ # Create a group: append str
+ trace = Trace("string")
+ trace.create_group("grp1")
+ trace.create_datum(group_key="grp1", dataset_key="dst_k1", dataset_val="dst_v1")
+ trace.append_datum(group_key="grp1", dataset_key="dst_k1", dataset_val="dst_v11")
+ trace.append_datum(group_key="grp1", dataset_key="dst_k1", dataset_val="dst_v111")
+ # print(trace)
+ trace.close()
+ print(trace)
+ # Create a group: append list(string)
+ trace = Trace("list(string)")
+ trace.create_group("grp1")
+ trace.create_datum(group_key="grp1", dataset_key="dst_k1", dataset_val=["dst_v1","dst_v2"])
+ trace.append_datum(group_key="grp1", dataset_key="dst_k1", dataset_val=["dst_v11","dst_v22"])
+ trace.append_datum(group_key="grp1", dataset_key="dst_k1", dataset_val=["dst_v111","dst_v222"])
+ # print(trace)
+ trace.close()
+ print(trace)
+ # Create a group: append list(float)
+ trace = Trace("list(float)")
+ trace.create_group("grp1")
+ trace.create_datum(group_key="grp1", dataset_key="dst_k1", dataset_val=[1, 2])
+ trace.append_datum(group_key="grp1", dataset_key="dst_k1", dataset_val=[11, 22])
+ trace.append_datum(group_key="grp1", dataset_key="dst_k1", dataset_val=[111, 222])
+ # print(trace)
+ trace.close(i_res=np.int16)
+ print(trace)
+ # Create a group: append dict
+ trace = Trace("dict")
+ trace.create_group("grp1")
+ trace.create_datum(group_key="grp1", dataset_key="dst_k1", dataset_val={"one":1, "two":2.0, "three":"3"})
+ trace.append_datum(group_key="grp1", dataset_key="dst_k1", dataset_val={"one":11, "two":22.0, "three":"33"})
+ trace.append_datum(group_key="grp1", dataset_key="dst_k1", dataset_val={"one":111, "two":222.0, "three":"333"})
+ # print(trace)
+ trace.close()
+ print(trace)
+ # Create a group: append ndarray
+ trace = Trace("ndarray")
+ trace.create_group("grp1")
+ trace.create_datum(group_key="grp1", dataset_key="dst_k1", dataset_val=np.array([1, 2]))
+ trace.append_datum(group_key="grp1", dataset_key="dst_k1", dataset_val=np.array([11, 22]))
+ trace.append_datum(group_key="grp1", dataset_key="dst_k1", dataset_val=np.array([111, 222]))
+ print(trace)
+ trace.close(i_res=np.int16)
+ print(trace)
+
+ # Create a group: append ndarray
+ trace = Trace("ndarray_stack")
+ trace.create_group("grp1")
+ trace.create_datum(group_key="grp1", dataset_key="dst_k1", dataset_val=np.ones([4, 2]))
+ trace.append_datum(group_key="grp1", dataset_key="dst_k1", dataset_val=np.zeros([4, 2]))
+ try:
+ trace.append_datum(group_key="grp1", dataset_key="dst_k1", dataset_val=np.array([11, 22]))
+ except Exception as e:
+ prompt(f"Caught expected exception during append_datum: {e}", type=Prompt.WARN)
+ trace.close(i_res=np.int16)
+ assert trace['grp1']['dst_k1'].shape==(2, 4 ,2), ValueError("Check ndarray concatenation")
+ try:
+ trace.plot(output_format=None, output_dir=".", groups=["grp1"], datasets=[["dst_k1",],])
+ except Exception as e:
+ prompt(f"Caught expected exception during plotting >2D dataset: {e}", type=Prompt.WARN)
+
+
+
+if __name__ == '__main__':
+ test_trace()
+ test_trace_append()
+ test_trace_plot()
diff --git a/robohive/physics/mj_sim_scene.py b/robohive/physics/mj_sim_scene.py
index 6705e8b8..4ddb1b98 100644
--- a/robohive/physics/mj_sim_scene.py
+++ b/robohive/physics/mj_sim_scene.py
@@ -9,21 +9,201 @@
import copy
import logging
+import os
+import tempfile
+import xml.etree.ElementTree as ET
from typing import Any
import robohive.utils.import_utils as import_utils
-from robohive.utils.prompt_utils import prompt, Prompt
+from robohive.utils.prompt_utils import Prompt, prompt
+
import_utils.dm_control_isavailable()
import_utils.mujoco_isavailable()
import dm_control.mujoco as dm_mujoco
-from robohive.renderer.mj_renderer import MJRenderer
from robohive.physics.sim_scene import SimScene
+from robohive.renderer.mj_renderer import MJRenderer
class DMSimScene(SimScene):
"""Encapsulates a MuJoCo robotics simulation using dm_control."""
+ def _normalize_xml_path(self, model_path: str) -> str:
+ """Creates a MuJoCo-3-safe temporary main XML without changing sources.
+
+ The compatibility issue here is limited to a subset of included XMLs.
+ Keep the original asset tree in place and only rewrite the main XML plus
+ any incompatible include files into a temporary location.
+ """
+ temp_dir = tempfile.mkdtemp(prefix='robohive_mjcf_')
+
+ def normalize_top_level_defaults(tree: ET.ElementTree) -> bool:
+ root = tree.getroot()
+ if root.tag != 'mujocoinclude':
+ return False
+
+ changed = False
+ for child in list(root):
+ if child.tag == 'default' and child.get('class') is not None:
+ if child.get('class') == 'main':
+ del child.attrib['class']
+ changed = True
+ continue
+ wrapper = ET.Element('default')
+ root.insert(list(root).index(child), wrapper)
+ root.remove(child)
+ wrapper.append(child)
+ changed = True
+ return changed
+
+ def collect_default_classes(root: ET.Element) -> dict[str, ET.Element]:
+ classes = {}
+ for elem in root.iter('default'):
+ default_class = elem.get('class')
+ if default_class:
+ classes[default_class] = elem
+ return classes
+
+ def merge_default_nodes(base: ET.Element, extra: ET.Element):
+ for child in list(extra):
+ if child.tag == 'default' and child.get('class') is not None:
+ match = None
+ for base_child in list(base):
+ if base_child.tag == 'default' and base_child.get('class') == child.get('class'):
+ match = base_child
+ break
+ if match is None:
+ base.append(copy.deepcopy(child))
+ else:
+ merge_default_nodes(match, child)
+ continue
+
+ match = None
+ for base_child in list(base):
+ if base_child.tag == child.tag:
+ match = base_child
+ break
+
+ if match is None:
+ base.append(copy.deepcopy(child))
+ else:
+ match.attrib.update(child.attrib)
+
+ def collapse_duplicate_defaults(root: ET.Element, parent_defaults: dict[str, ET.Element]) -> bool:
+ changed = False
+
+ def visit(parent: ET.Element):
+ nonlocal changed
+ for child in list(parent):
+ visit(child)
+ if child.tag != 'default':
+ continue
+
+ default_class = child.get('class')
+ if default_class is None or default_class not in parent_defaults:
+ continue
+
+ merge_default_nodes(parent_defaults[default_class], child)
+ parent.remove(child)
+ changed = True
+
+ visit(root)
+ return changed
+
+ def absolutize_resource_paths(root: ET.Element, src_dir: str, fallback_dirs: tuple[str, ...] = ()) -> bool:
+ changed = False
+ candidate_dirs = []
+ seen_dirs = set()
+
+ def add_candidate_dir(path: str):
+ path = os.path.abspath(path)
+ if path not in seen_dirs:
+ candidate_dirs.append(path)
+ seen_dirs.add(path)
+
+ for base_dir in (src_dir,) + fallback_dirs:
+ base_dir = os.path.abspath(base_dir)
+ current_dir = base_dir
+ while True:
+ add_candidate_dir(current_dir)
+ parent_dir = os.path.dirname(current_dir)
+ if parent_dir == current_dir:
+ break
+ current_dir = parent_dir
+
+ def resolve_existing_path(relative_path: str) -> str | None:
+ for base_dir in candidate_dirs:
+ candidate = os.path.abspath(os.path.join(base_dir, relative_path))
+ if os.path.exists(candidate):
+ return candidate
+ return None
+
+ for elem in root.iter():
+ if elem.tag == 'include' and 'file' in elem.attrib:
+ include_path = elem.get('file')
+ if include_path and not os.path.isabs(include_path):
+ resolved_include_path = resolve_existing_path(include_path)
+ if resolved_include_path is not None and resolved_include_path != include_path:
+ elem.set('file', resolved_include_path)
+ changed = True
+
+ if elem.tag != 'include' and 'file' in elem.attrib:
+ file_path = elem.get('file')
+ if file_path and not os.path.isabs(file_path):
+ resolved_file_path = resolve_existing_path(file_path)
+ if resolved_file_path is not None and resolved_file_path != file_path:
+ elem.set('file', resolved_file_path)
+ changed = True
+
+ if elem.tag == 'compiler':
+ for attr_name in ('assetdir', 'meshdir', 'texturedir'):
+ attr_value = elem.get(attr_name)
+ if attr_value and not os.path.isabs(attr_value):
+ resolved_dir = resolve_existing_path(attr_value)
+ if resolved_dir is not None and os.path.isdir(resolved_dir) and resolved_dir != attr_value:
+ elem.set(attr_name, resolved_dir)
+ changed = True
+
+ return changed
+
+ model_path = os.path.abspath(model_path)
+ model_dir = os.path.dirname(model_path)
+ model_tree = ET.parse(model_path)
+ model_root = model_tree.getroot()
+ known_defaults = collect_default_classes(model_root)
+ absolutize_resource_paths(model_root, model_dir)
+
+ include_index = 0
+ for include in model_root.iter('include'):
+ include_file = include.get('file')
+ if not include_file:
+ continue
+
+ include_src = os.path.abspath(os.path.join(model_dir, include_file))
+ include_tree = ET.parse(include_src)
+ include_changed = normalize_top_level_defaults(include_tree)
+ include_root = include_tree.getroot()
+ include_changed = collapse_duplicate_defaults(include_root, known_defaults) or include_changed
+ include_changed = absolutize_resource_paths(
+ include_root,
+ os.path.dirname(include_src),
+ fallback_dirs=(os.path.dirname(os.path.dirname(include_src)), model_dir),
+ ) or include_changed
+ for default_class, default_elem in collect_default_classes(include_root).items():
+ known_defaults.setdefault(default_class, default_elem)
+
+ if include_changed:
+ include_index += 1
+ include_dst = os.path.join(temp_dir, f'include_{include_index}.xml')
+ include_tree.write(include_dst, encoding='utf-8', xml_declaration=False)
+ include.set('file', include_dst)
+ else:
+ include.set('file', include_src)
+
+ normalized_root = os.path.join(temp_dir, os.path.basename(model_path))
+ model_tree.write(normalized_root, encoding='utf-8', xml_declaration=False)
+ return normalized_root
+
def _load_simulation(self, model_handle: Any) -> Any:
"""Loads the simulation from the given model handle.
@@ -36,7 +216,7 @@ def _load_simulation(self, model_handle: Any) -> Any:
"""
if isinstance(model_handle, str):
if model_handle.endswith('.xml'):
- sim = dm_mujoco.Physics.from_xml_path(model_handle)
+ sim = dm_mujoco.Physics.from_xml_path(self._normalize_xml_path(model_handle))
elif isinstance(model_handle, str) and " hardwareBase subclass.
+# Populated by @register_hardware(type_name) decorators on concrete hardware classes.
+HARDWARE_REGISTRY = {}
+SENSOR_POSTPROCESS = {}
+
+
+def register_hardware(type_name, sensor_postprocess=None):
+ """Registers a hardwareBase subclass (or a factory function returning one) against an
+ interface['type'] string. Usable as a class decorator, or called directly with a
+ factory function (e.g. one that tries a preferred implementation and falls back to
+ an alternate one).
+
+ sensor_postprocess(raw: dict) -> np.ndarray, optional: transforms the dict returned by
+ get_sensors() into other formats.
+ """
+ def _decorator(cls_or_factory):
+ if isinstance(cls_or_factory, type):
+ assert issubclass(cls_or_factory, hardwareBase), \
+ f"{cls_or_factory} must subclass hardwareBase to be registered"
+ HARDWARE_REGISTRY[type_name] = cls_or_factory
+ if sensor_postprocess is not None:
+ SENSOR_POSTPROCESS[type_name] = sensor_postprocess
+ return cls_or_factory
+ return _decorator
+
class hardwareBase(abc.ABC):
- def __init__(self, name, *args, **kwargs):
+
+ # add tests to all defined subclasses to ensure that get_sensors() returns a dict with a 'time' key
+ def __init_subclass__(cls, **kwargs):
+ super().__init_subclass__(**kwargs)
+ if 'connect' in cls.__dict__:
+ user_connect = cls.__dict__['connect']
+
+ def connect_then_check(self, *args, **kw):
+ result = user_connect(self, *args, **kw)
+ try:
+ data = self.get_sensors()
+ if not (isinstance(data, dict) and 'time' in data):
+ warnings.warn(
+ f"{self.name}: get_sensors() should return a dict containing a 'time' key, got {type(data)}"
+ )
+ except Exception as e:
+ warnings.warn(
+ f"{self.name}: could not verify get_sensors() 'time'-key contract after connect: {e}"
+ )
+ return result
+
+ cls.connect = connect_then_check
+
+ def __init__(self, name, *args, **kwargs) -> None:
self.name = name
@abc.abstractmethod
- def connect(self):
+ def connect(self) -> bool:
"""Establish hardware connection"""
@abc.abstractmethod
- def okay(self):
- """Return hardware health"""
+ def okay(self) -> bool:
+ """Check if hardware is healthy and return the status"""
+
+ @abc.abstractmethod
+ def recover(self) -> None:
+ """Recover hardware from any error, connection loss, failure, etc """
@abc.abstractmethod
- def close(self):
+ def close(self) -> bool:
"""Close hardware connection"""
@abc.abstractmethod
- def reset(self):
- """Reset hardware"""
+ def reset(self) -> None:
+ """Reset hardware to a known state. Used for resetting the hardware to a known state"""
@abc.abstractmethod
- def get_sensors(self):
- """Get hardware sensors"""
+ def get_sensors(self) -> dict:
+ """Get hardware sensors — should return a dict containing a 'time' key"""
@abc.abstractmethod
- def apply_commands(self):
+ def apply_commands(self) -> None:
"""Apply hardware commands"""
- def __del__(self):
+ def __del__(self) -> None:
self.close()
\ No newline at end of file
diff --git a/robohive/robot/hardware_dynamixel.py b/robohive/robot/hardware_dynamixel.py
index 5e94513c..7915cb23 100644
--- a/robohive/robot/hardware_dynamixel.py
+++ b/robohive/robot/hardware_dynamixel.py
@@ -5,42 +5,101 @@
License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
================================================= """
-from .hardware_base import hardwareBase
-from dynamixel_py import dxl
+import time
+
import numpy as np
+from dynamixel_py import dxl
+
+from .hardware_base import hardwareBase, register_hardware
+
+@register_hardware('dynamixel')
class Dynamixels(hardwareBase):
- def __init__(self, name, motor_ids, motor_type, devicename, **kwargs):
+ """
+ Adapter around dynamixel_py's dxl client. A single dynamixel bus is shared across all
+ of a device's sensors and actuators, and needs per-motor mode ('Position'/'PWM') at
+ connect time — that per-actuator detail lives in the robot_config device dict (not in
+ `interface`), so this class opts in to receiving it (see Robot.hardware_init()) rather
+ than requiring config authors to duplicate motor IDs/modes into `interface` too. This
+ is a deliberately scoped exception for dynamixel-bus hardware (which genuinely needs
+ per-motor mode bookkeeping); other hardware classes stay interface-only.
+
+ interface config keys:
+ motor_type : dynamixel motor series (e.g. 'X')
+ port : serial port (e.g. '/dev/ttyUSB0')
+ """
+
+ def __init__(self, name, motor_type, port, device, **kwargs):
self.name = name
- # initialize dynamixels
- self.dxls = dxl(motor_id=motor_ids, motor_type=motor_type, devicename=devicename)
- self.motor_ids = motor_ids
+ self.motor_type = motor_type
+ self.port = port
+ self.device = device
+ self.motor_ids = np.unique(device['sensor_ids'] + device['actuator_ids']).tolist()
+ self.dxls = None
- def connect(self):
+ def connect(self) -> bool:
"""Establish hardware connection"""
+ self.dxls = dxl(motor_id=self.motor_ids, motor_type=self.motor_type, devicename=self.port)
self.dxls.open_port()
# set actuator mode
- for actuator in device['actuator']:
- self.dxls.set_operation_mode(motor_id=[actuator['hdr_id']], mode=actuator['mode'])
+ for actuator in self.device['actuator']:
+ self.dxls.set_operation_mode(motor_id=[actuator['hdr_adr']], mode=actuator['mode'])
# engage motors
- self.dxls.engage_motor(motor_id=self.dxls, enable=True)
+ self.dxls.engage_motor(motor_id=self.device['actuator_ids'], enable=True)
+ return True
- def okay(self):
+ def okay(self) -> bool:
"""Return hardware health"""
+ return self.dxls is not None
+
+ def recover(self) -> None:
+ """Recover hardware from any error, connection loss, failure, etc"""
+ self.close()
+ self.connect()
- def close(self):
+ def close(self) -> bool:
"""Close hardware connection"""
+ if self.dxls:
+ status = self.dxls.close(self.motor_ids)
+ if status:
+ self.dxls = None
+ return status
+ return True
- def reset(self):
- """Reset hardware"""
+ def reset(self, hw_q=None) -> None:
+ """Reset hardware to a known state"""
+ if hw_q is not None:
+ self.apply_commands(hw_q)
- def get_sensors(self):
- """Get hardware sensors"""
+ def get_sensors(self) -> dict:
+ """Get hardware sensors. 'pos'/'vel' are positionally ordered to match
+ device['sensor_ids'] (i.e. device['sensor'] declaration order)."""
+ return {
+ 'time': time.time(),
+ 'pos': self.dxls.get_pos(self.device['sensor_ids']),
+ 'vel': self.dxls.get_vel(self.device['sensor_ids']),
+ }
- def apply_commands(self):
- """Apply hardware commands"""
+ def apply_commands(self, hw_q) -> None:
+ """Apply hardware commands. hw_q is positionally ordered to match device['actuator']."""
+ pos_ids, pos_ctrl, pwm_ids, pwm_ctrl = [], [], [], []
+ for i, actuator in enumerate(self.device['actuator']):
+ val = hw_q[i]
+ mode = actuator['mode']
+ if mode == 'Position':
+ pos_ids.append(actuator['hdr_adr'])
+ pos_ctrl.append(val)
+ elif mode == 'PWM':
+ pwm_ids.append(actuator['hdr_adr'])
+ pwm_ctrl.append(val)
+ else:
+ raise NotImplementedError(f"Actuator mode {mode} not found")
+ if pos_ids:
+ self.dxls.set_des_pos(pos_ids, pos_ctrl)
+ if pwm_ids:
+ self.dxls.set_des_pwm(pwm_ids, pwm_ctrl)
def __del__(self):
- self.close()
\ No newline at end of file
+ self.close()
diff --git a/robohive/robot/hardware_franka.py b/robohive/robot/hardware_franka.py
index bb2b89d9..2a018a81 100644
--- a/robohive/robot/hardware_franka.py
+++ b/robohive/robot/hardware_franka.py
@@ -15,7 +15,7 @@
from polymetis import RobotInterface
import torchcontrol as toco
-from robohive.robot.hardware_base import hardwareBase
+from robohive.robot.hardware_base import hardwareBase, register_hardware
from robohive.utils.min_jerk import generate_joint_space_min_jerk
import argparse
@@ -56,6 +56,7 @@ def forward(self, state_dict: Dict[str, torch.Tensor]):
+@register_hardware('franka')
class FrankaArm(hardwareBase):
def __init__(self, name, ip_address, gain_scale=1.0, reset_gain_scale=1.0, **kwargs):
self.name = name
@@ -90,7 +91,7 @@ def connect(self, policy=None):
# Create policy instance
s_initial = self.get_sensors()
policy = JointPDPolicy(
- desired_joint_pos=s_initial['joint_pos'],
+ desired_joint_pos=s_initial['pos'],
kp=self.gain_scale * torch.Tensor(self.robot.metadata.default_Kq),
kd=self.gain_scale * torch.Tensor(self.robot.metadata.default_Kqd),
)
@@ -144,6 +145,12 @@ def reconnect(self):
print("Re-connection success")
+ def recover(self) -> None:
+ """Recover hardware from any error, connection loss, failure, etc"""
+ self.reconnect()
+ self.reset()
+
+
def reset(self, reset_pos=None, time_to_go=5):
"""Reset hardware"""
@@ -157,7 +164,7 @@ def reset(self, reset_pos=None, time_to_go=5):
reset_pos = torch.Tensor(reset_pos)
# Use registered controller
- q_current = self.get_sensors()['joint_pos']
+ q_current = self.get_sensors()['pos']
# generate min jerk trajectory
dt = 0.1
waypoints = generate_joint_space_min_jerk(start=q_current, goal=reset_pos, time_to_go=time_to_go, dt=dt)
@@ -185,7 +192,7 @@ def reset(self, reset_pos=None, time_to_go=5):
self.reset(reset_pos, time_to_go)
- def get_sensors(self):
+ def get_sensors(self) -> dict:
"""Get hardware sensors"""
try:
joint_pos = self.robot.get_joint_positions()
@@ -194,7 +201,7 @@ def get_sensors(self):
print("Failed to get current sensors: ", end="")
self.reconnect()
return self.get_sensors()
- return {'joint_pos': joint_pos, 'joint_vel':joint_vel}
+ return {'time': time.time(), 'pos': joint_pos, 'vel': joint_vel}
def apply_commands(self, q_desired=None, kp=None, kd=None):
@@ -252,8 +259,8 @@ def get_args():
# Update policy to execute a sine trajectory on joint 6 for 5 seconds
print("Starting sine motion updates...")
s_initial = franka.get_sensors()
- q_initial = s_initial['joint_pos'].clone()
- q_desired = s_initial['joint_pos'].clone()
+ q_initial = s_initial['pos'].clone()
+ q_desired = s_initial['pos'].clone()
for i in range(int(time_to_go * hz)):
q_desired[5] = q_initial[5] + m * np.sin(np.pi * i / (T * hz))
diff --git a/robohive/robot/hardware_optitrack.py b/robohive/robot/hardware_optitrack.py
index 9a96e3b5..b7cf58fa 100644
--- a/robohive/robot/hardware_optitrack.py
+++ b/robohive/robot/hardware_optitrack.py
@@ -5,7 +5,8 @@
License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
================================================= """
-from darwin.darwin_robot.hardware_base import hardwareBase
+from robohive.robot.hardware_base import hardwareBase, register_hardware
+from robohive.utils.quat_math import quat2euler
import numpy as np
import socket
import argparse
@@ -15,6 +16,17 @@
_USE_UDP = True
+
+def _optitrack_sensor_postprocess(raw):
+ c, b, a = quat2euler(raw['quat'])
+ rx = np.pi - a
+ rx = (rx - 2*np.pi) if rx > np.pi else rx
+ ry = b
+ rz = -c
+ return np.concatenate([raw['pos'], np.array([rx, ry, rz])])
+
+
+@register_hardware('optitrack', sensor_postprocess=_optitrack_sensor_postprocess)
class OptiTrack(hardwareBase):
"""
OptiTrack Client: Connects to the server and receives streaming data
@@ -27,9 +39,11 @@ class OptiTrack(hardwareBase):
# Cached client that is shared for the application lifetime.
_OPTI_CLIENT = None
- def __init__(self, ip: str, port:int=5000, packet_size:int=36, cache_maxsize:int=0):
+ def __init__(self, name='optitrack', ip: str = None, client_name: str = None, port:int=5000, packet_size:int=36, cache_maxsize:int=0, **kwargs):
+ self.name = name
if self._OPTI_CLIENT is None:
- self.ip = ip
+ # robot_config's interface dict historically names this key 'client_name'
+ self.ip = ip if ip is not None else client_name
self.port = port
self.packet_size = packet_size
self._sensor_cache_maxsize = cache_maxsize
@@ -93,6 +107,12 @@ def reset(self):
self.connect()
+ def recover(self) -> None:
+ """Recover hardware from any error, connection loss, failure, etc"""
+ self.close()
+ self.connect()
+
+
# [t, id, x, y, z, q0, q1, q2, q3, q4]
def read_sensor(self):
# receive response
@@ -134,7 +154,7 @@ def __repr__(self):
self.data_float[2], self.data_float[3], self.data_float[4])
# get latest sensor value (helpful when there is a single sensors)
- def get_sensors(self):
+ def get_sensors(self) -> dict:
# sensor_data isn't updated in place ==> it can be easily passed around and cached
# repeated calls will return the same data_frame ==> no overhead for multiple queries to the same sensor reading
return self.sensor_data
diff --git a/robohive/robot/hardware_realsense.py b/robohive/robot/hardware_realsense.py
index 0da3ec03..7616e735 100644
--- a/robohive/robot/hardware_realsense.py
+++ b/robohive/robot/hardware_realsense.py
@@ -1,6 +1,6 @@
import numpy as np
# from hardware_base import hardwareBase
-from robohive.robot.hardware_base import hardwareBase
+from robohive.robot.hardware_base import hardwareBase, register_hardware
import argparse
import a0
@@ -19,6 +19,7 @@
class RealSense(hardwareBase):
def __init__(self, name, rgb_topic=None, d_topic=None, **kwargs):
assert rgb_topic or d_topic, "Atleast one of the topics is needed"
+ self.name = name
self.rgb_topic = rgb_topic
self.d_topic = d_topic
self.last_image_pkt = None
@@ -27,6 +28,10 @@ def __init__(self, name, rgb_topic=None, d_topic=None, **kwargs):
self.most_recent_pkt_ts = None
self.timeout = 1 # in seconds
+ def recover(self) -> None:
+ """Recover hardware from any error, connection loss, failure, etc"""
+ self.connect()
+
def connect(self):
# sub to the topics
if self.rgb_topic:
@@ -58,7 +63,7 @@ def callback(self, pkt):
timestamp_str_wo_nano = timestamp_str[:23] + timestamp_str[29:]
self.most_recent_pkt_ts = datetime.datetime.fromisoformat(timestamp_str_wo_nano)
- def get_sensors(self):
+ def get_sensors(self) -> dict:
# get all data from all topics
last_img = copy.deepcopy(self.last_image_pkt)
last_depth = copy.deepcopy(self.last_depth_pkt)
@@ -96,6 +101,17 @@ def reset(self):
return 0
+def _realsense_factory(name, **iface):
+ """Try the a0-based RealSense first, fall back to the direct pyrealsense2 wrapper."""
+ try:
+ return RealSense(name=name, **iface)
+ except Exception:
+ from .hardware_realsense_single import RealsenseAPI
+ return RealsenseAPI(name=name, **iface)
+
+
+register_hardware('realsense')(_realsense_factory)
+
# Get inputs from user
def get_args():
diff --git a/robohive/robot/hardware_realsense_single.py b/robohive/robot/hardware_realsense_single.py
index cb18472d..e64ec1a3 100644
--- a/robohive/robot/hardware_realsense_single.py
+++ b/robohive/robot/hardware_realsense_single.py
@@ -2,11 +2,14 @@
import pyrealsense2 as rs
from collections import OrderedDict
+from robohive.robot.hardware_base import hardwareBase
-class RealsenseAPI:
+
+class RealsenseAPI(hardwareBase):
"""Wrapper that implements boilerplate code for RealSense cameras"""
- def __init__(self, device_id=None, height=480, width=640, fps=30, warm_start=30, type=None,):
+ def __init__(self, name='realsense', device_id=None, height=480, width=640, fps=30, warm_start=30, type=None, **kwargs):
+ self.name = name
self.height = height
self.width = width
self.fps = fps
@@ -73,6 +76,10 @@ def get_sensors(self):
def okay(self):
return True
+ def recover(self) -> None:
+ """Recover hardware from any error, connection loss, failure, etc"""
+ self.connect()
+
def apply_commands(self):
return 0
diff --git a/robohive/robot/hardware_robotiq.py b/robohive/robot/hardware_robotiq.py
index 27b4e2f0..df10a837 100644
--- a/robohive/robot/hardware_robotiq.py
+++ b/robohive/robot/hardware_robotiq.py
@@ -1,11 +1,12 @@
from enum import Flag
from polymetis import GripperInterface
-from robohive.robot.hardware_base import hardwareBase
+from robohive.robot.hardware_base import hardwareBase, register_hardware
import numpy as np
import argparse
import time
+@register_hardware('robotiq')
class Robotiq(hardwareBase):
def __init__(self, name, ip_address, **kwargs):
self.name = name
@@ -81,6 +82,11 @@ def reconnect(self):
print("RBQ:> Re-connection success")
+ def recover(self) -> None:
+ """Recover hardware from any error, connection loss, failure, etc"""
+ self.reconnect()
+
+
def reset(self, width=None, **kwargs):
"""Reset hardware"""
if not width:
@@ -88,7 +94,7 @@ def reset(self, width=None, **kwargs):
self.apply_commands(width=width, **kwargs)
- def get_sensors(self):
+ def get_sensors(self) -> dict:
"""Get hardware sensors"""
try:
curr_state = self.robot.get_state()
@@ -96,9 +102,12 @@ def get_sensors(self):
print("RBQ:> Failed to get current sensors: ", end="")
self.reconnect()
return self.get_sensors()
- return np.array([curr_state.width])
+ return {'time': time.time(), 'pos': np.array([curr_state.width])}
- def apply_commands(self, width:float, speed:float=0.1, force:float=0.1):
+ def apply_commands(self, width, speed:float=0.1, force:float=0.1):
+ # width may be a scalar or a length-1 array-like (Robot.hardware_apply_controls
+ # always passes an array positionally matching this device's single actuator).
+ width = float(np.asarray(width).reshape(-1)[0])
assert width>=0.0 and width<=self.max_width, "Gripper desired width ({}) is out of bound (0,{})".format(width, self.max_width)
self.robot.goto(width=width, speed=speed, force=force)
return 0
diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py
index 4daa9a5d..26cf5e66 100644
--- a/robohive/robot/robot.py
+++ b/robohive/robot/robot.py
@@ -5,14 +5,18 @@
License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
================================================= """
-from robohive.physics.sim_scene import SimScene
-from robohive.utils.quat_math import quat2euler
-from robohive.utils.prompt_utils import prompt, Prompt
-import mujoco
+import inspect
+import os
import time
-import numpy as np
from collections import deque
-import os
+
+import mujoco
+import numpy as np
+
+from robohive.physics.sim_scene import SimScene
+from robohive.robot.hardware_base import HARDWARE_REGISTRY, SENSOR_POSTPROCESS
+from robohive.utils.prompt_utils import Prompt, prompt
+
np.set_printoptions(precision=4)
@@ -31,6 +35,11 @@
# NOTE/ GOOD PRACTICES ===========================
# nq should be nv
# Order of sensors and actuators in config should follow XML order
+# Space definitions
+ # sim_id: ID (defined by order in mujoco's xml) of the sensor/actuator in the sim
+ # hdr_id: ID (defined by the order in .config) of the sensor/actuator in the robot_config (hardware) space (robot_config unifies different hardware into a single unified hardware space)
+ # hdr_adr: Physical address of the sensor/actuator (defined by hardware specs) (e.g. dynamixel - This is the address/motor_id used to communicate with the individual motors)
+
class Robot():
@@ -119,63 +128,26 @@ def hardware_init(self, robot_config):
# initalize
for name, device in robot_config.items():
prompt("Initializing device: %s"%(name), 'white', 'on_grey')
- if device['interface']['type'] == 'dynamixel':
- # initialize dynamixels
- from dynamixel_py import dxl
- ids = np.unique([device['sensor_ids'] + device['actuator_ids']]).tolist()
- device['robot'] = dxl(motor_id=ids, motor_type=\
- device['interface']['motor_type'], devicename= device['interface']['name'])
-
- # from .hardware_dynamixel import Dynamixels
- # motor_ids = np.unique([device['sensor_ids'] + device['actuator_ids']]).tolist()
- # device['robot'] = Dynamixels(name=name, motor_ids=motor_ids, motor_type=device['interface']['motor_type'], devicename= device['interface']['name'])
-
- elif device['interface']['type'] == 'optitrack':
- from .hardware_optitrack import OptiTrack
- device['robot'] = OptiTrack(ip=device['interface']['client_name'], \
- port=device['interface']['port'], packet_size=device['interface']['packet_size'])
-
- elif device['interface']['type'] == 'franka':
- from .hardware_franka import FrankaArm
- device['robot'] = FrankaArm(name=name, **device['interface'])
-
- elif device['interface']['type'] == 'realsense':
- try:
- from .hardware_realsense import RealSense
- device['robot'] = RealSense(name=name, **device['interface'])
- except:
- from .hardware_realsense_single import RealsenseAPI
- device['robot'] = RealsenseAPI(**device['interface'])
-
- elif device['interface']['type'] == 'robotiq':
- from .hardware_robotiq import Robotiq
- device['robot'] = Robotiq(name=name, **device['interface'])
-
- else:
- print("ERROR: interface ({}) not found".format(device['interface']['type']))
- raise NotImplemented
+ itype = device['interface']['type']
+ cls = HARDWARE_REGISTRY.get(itype)
+ if cls is None:
+ raise NotImplementedError(
+ "No hardwareBase registered for interface.type={!r}. Registered types: {}. "
+ "Import the module that registers this type before constructing "
+ "Robot(is_hardware=True).".format(itype, sorted(HARDWARE_REGISTRY)))
+ iface = {k: v for k, v in device['interface'].items() if k != 'type'}
+ # Most hardware classes are self-contained (ip/port/etc. from `interface` is
+ # enough). A few (dynamixel-bus devices, which manage several motors sharing
+ # one connection and need per-motor address/mode bookkeeping) opt in to
+ # receiving the full robot_config device dict by naming `device` as a
+ # constructor parameter; everyone else never sees it.
+ if 'device' in inspect.signature(cls).parameters:
+ iface['device'] = device
+ device['robot'] = cls(name=name, **iface)
# start all hardware
for name, device in robot_config.items():
-
- # Dynamixels
- if device['interface']['type'] == 'dynamixel':
- device['robot'].open_port()
-
- # set actuator mode
- for actuator in device['actuator']:
- device['robot'].set_operation_mode(motor_id=[actuator['hdr_id']], mode=actuator['mode'])
-
- # engage motors
- device['robot'].engage_motor(motor_id=device['actuator_ids'], enable=True)
-
- # Other devices
- elif device['interface']['type'] in ['optitrack', 'franka', 'realsense', 'robotiq']:
- device['robot'].connect()
-
- else:
- print("ERROR: interface ({}) not found".format(device['interface']['type']))
- raise NotImplemented
+ device['robot'].connect()
return robot_config
@@ -186,117 +158,76 @@ def hardware_get_sensors(self):
current_sensor_value['time'] = time.time() - self.time_start
for name, device in self.robot_config.items():
if 'sensor' in device.keys() and len(device['sensor'])>0:
- # get sensors
- if device['interface']['type'] == 'dynamixel':
- # TODO: choose between pos, vel, or posvel
- current_sensor_value[name] = device['robot'].get_pos(device['sensor_ids'])
- current_sensor_value[name+'_vel'] = device['robot'].get_vel(device['sensor_ids'])
-
- elif device['interface']['type'] == 'optitrack':
- data = device['robot'].get_sensors()
- c, b, a = quat2euler(data['quat'])
- rx = np.pi - a
- rx = (rx - 2*np.pi) if rx > np.pi else rx
- ry = b
- rz = -c
- # print("Pos:", x, y, z)
- # print("Rotations:", rx, ry, rz)
- current_sensor_value[name] = np.concatenate([data['pos'], np.array([rx, ry, rz])])
- # current_sensor_value[name] = np.array([x, y, z, 0, 0, 0])
- # current_sensor_value[name] = np.array([x, y, z, -(a+np.pi/2), -c, -b])
-
- elif device['interface']['type'] == 'franka':
- sensors = device['robot'].get_sensors()
- current_sensor_value[name] = np.concatenate([sensors['joint_pos'], sensors['joint_vel']])
-
- elif device['interface']['type'] == 'robotiq':
- sensors = device['robot'].get_sensors()
- current_sensor_value[name] = sensors
-
- else:
- print("ERROR: interface ({}) not found".format(device['interface']['type']))
- raise NotImplemented
-
- # calibrate sensors
+ itype = device['interface']['type']
+ raw = device['robot'].get_sensors()
+ postprocess = SENSOR_POSTPROCESS.get(itype, lambda raw: raw['pos'])
+ vals = postprocess(raw)
+
+ # calibrate sensors. vals is positionally ordered to match device['sensor']
+ # (both built by iterating the same config-declared list, see configure_robot()).
+ arr = np.empty(len(device['sensor']), dtype=np.float64)
for id, sensor in enumerate(device['sensor']):
- current_sensor_value[name][id] = current_sensor_value[name][id]*sensor['scale'] + sensor['offset']
- device['sensor_data'] = current_sensor_value[name]
+ arr[id] = vals[id]*sensor['scale'] + sensor['offset']
+ current_sensor_value[name] = arr
+ device['sensor_data'] = arr
device['sensor_time'] = current_sensor_value['time']
return current_sensor_value
# apply controls to hardware
- def hardware_apply_controls(self, control, is_reset=False):
+ def hardware_apply_controls(self, control, space='hdr'):
+ """
+ Send one dt's worth of (already-clipped, locally-achievable) controls to hardware.
+ control: control vector in hdr or sim space
+ space: 'hdr' or 'sim' (defaults to 'hdr' as represented in robot_config)
+ """
+
for name, device in self.robot_config.items():
if 'actuator' in device.keys() and len(device['actuator'])>0:
- if device['interface']['type'] == 'dynamixel':
- # group as per mode
- pos_ctrl = []
- pos_ids = []
- pwm_ctrl = []
- pwm_ids = []
- for actuator in device['actuator']:
- # calibrate
- calib_ctrl = control[actuator['sim_id']]*actuator['scale']+ actuator['offset']
- if actuator['mode'] == 'Position':
- pos_ids.append(actuator['hdr_id'])
- pos_ctrl.append(calib_ctrl)
- elif actuator['mode'] == 'PWM':
- pwm_ids.append(actuator['hdr_id'])
- pwm_ctrl.append(calib_ctrl)
- else:
- print("ERROR: Mode not found")
- raise NotImplemented
- # send controls
- if pos_ids:
- device['robot'].set_des_pos(pos_ids, pos_ctrl)
- if pwm_ids:
- device['robot'].set_des_pwm(pwm_ids, pwm_ctrl)
-
- elif device['interface']['type'] == 'franka':
- franka_des_pos = []
- for actuator in device['actuator']:
- # calibrate
- franka_des_pos.append(control[actuator['sim_id']]*actuator['scale']+ actuator['offset'])
- if is_reset:
- device['robot'].reset(franka_des_pos)
- else:
- device['robot'].apply_commands(franka_des_pos)
+ # hw_q is positionally ordered to match device['actuator'] (both built by
+ # iterating the same config-declared list, see configure_robot()).
+ hw_q = []
+ for actuator in device['actuator']:
+ ctrl = control[actuator['sim_id']] if space == 'sim' else control[actuator['hdr_id']]
+ hw_q.append(ctrl*actuator['scale'] + actuator['offset'])
+ device['robot'].apply_commands(hw_q)
- elif device['interface']['type'] == 'robotiq':
- robotiq_des_pos = []
- for actuator in device['actuator']:
- # calibrate
- robotiq_des_pos.append(control[actuator['sim_id']]*actuator['scale']+ actuator['offset'])
- if is_reset:
- device['robot'].reset(robotiq_des_pos[0])
- else:
- device['robot'].apply_commands(robotiq_des_pos[0])
- else:
- raise NotImplemented("ERROR: interface not found")
+ # move actuated dofs to a target position (blocking, large-displacement — distinct from
+ # the per-dt hardware_apply_controls above; hardware classes implement this via their own
+ # min-jerk/via-point trajectories)
+ def hardware_reset(self, reset_pos, space='sim'):
+ """
+ reset_pos: target qpos vector
+ space: 'sim' (full sim qpos vector, addressed via data_id) or
+ 'hdr' (per-actuator vector ordered by hdr_id, as in hardware_apply_controls)
+ """
+ assert space in ['sim', 'hdr'], "space must be 'sim' or 'hdr'"
+ for name, device in self.robot_config.items():
+ if name == 'default_robot':
+ continue
+ qpos_actuators = [a for a in device.get('actuator', []) if a['data_type'] == 'qpos']
+ if qpos_actuators:
+ hw_q = []
+ for a in qpos_actuators:
+ pos = reset_pos[a['data_id']] if space == 'sim' else reset_pos[a['hdr_id']]
+ pos = np.clip(pos, a['pos_range'][0], a['pos_range'][1])
+ hw_q.append(pos*a['scale'] + a['offset'])
+ device['robot'].reset(hw_q)
+ else:
+ # passive device (tendon-driven gripper, camera, etc.) — no qpos target to
+ # compute; let the device bring itself to its own known reset state
+ device['robot'].reset()
# close hardware
def hardware_close(self):
status = True
for name, device in self.robot_config.items():
- if device['interface']['type'] == 'dynamixel':
- if device['robot']:
- print("Closing dynamixel connection")
- ids = np.unique([device['sensor_ids'] + device['actuator_ids']]).tolist()
- status = device['robot'].close(ids)
- if status is True:
- device['robot']= None
- elif device['interface']['type'] in ['optitrack', 'franka', 'realsense', 'robotiq']:
- if device['robot']:
- print("Closing {} connection".format(device['interface']['type']))
- status = device['robot'].close()
- if status is True:
- device['robot']= None
- else:
- print("ERROR: interface not found")
- raise NotImplemented
-
+ if device.get('robot'):
+ print("Closing {} connection".format(device['interface']['type']))
+ status = device['robot'].close()
+ if status is True:
+ device['robot'] = None
return status
@@ -314,6 +245,8 @@ def configure_robot(self, sim, config_path):
with open(config_path, 'r') as f:
robot_config = eval(f.read())
+ hdr_sensor_id = -1 # IDs as per the config order
+ hdr_actuator_id = -1 # IDs as per the config order
for name, device in robot_config.items():
prompt("Configuring component %s"% name)
@@ -321,32 +254,49 @@ def configure_robot(self, sim, config_path):
device['sensor_ids'] = []
device['sensor_names'] = []
for sensor in device['sensor']:
- device['sensor_names'].append(sensor['name']) # list of all ids
- device['sensor_ids'].append(sensor['hdr_id']) # list of all ids
+ hdr_sensor_id += 1
+ sensor['hdr_id'] = hdr_sensor_id
sensor['sim_id'] = sim.model.sensor_name2id(sensor['name'])
+ device['sensor_names'].append(sensor['name']) # list of all ids
+ device['sensor_ids'].append(sensor['hdr_adr']) # list of all ids
sensor_type = sim.model.sensor_type[sensor['sim_id']]
sensor_objid = sim.model.sensor_objid[sensor['sim_id']]
- if sensor_type == mujoco.mjtSensor.mjSENS_JOINTPOS: # mjSENS_JOINTPOS,// scalar joint position (hinge and slide only)
+ # sensordata_id: address in sim.data.sensordata for this sensor.
+ # Stored for all sensor types so Pass 2 of sensor2sim can write
+ # hardware readings into sensordata as the single authoritative source.
+ sensor['sensordata_id'] = int(sim.model.sensor_adr[sensor['sim_id']])
+ if sensor_type == mujoco.mjtSensor.mjSENS_JOINTPOS: # scalar joint position (hinge and slide only)
sensor['data_type'] = 'qpos'
sensor['data_id'] = sim.model.jnt_qposadr[sensor_objid]
- elif sensor_type == mujoco.mjtSensor.mjSENS_JOINTVEL: # mjSENS_JOINTVEL,// scalar joint position (hinge and slide only)
+ elif sensor_type == mujoco.mjtSensor.mjSENS_JOINTVEL: # scalar joint velocity (hinge and slide only)
sensor['data_type'] = 'qvel'
sensor['data_id'] = sim.model.jnt_dofadr[sensor_objid]
else:
- quit("ERROR: Sensor {} has unsupported sensor_type: {}".format(sensor['name'],sensor_type))
+ # Non-invertible sensor: sim.forward() recomputes this from state.
+ # Covers tendon position, joint actuator force, actuator force, etc.
+ sensor['data_type'] = 'sensordata'
+ sensor['data_id'] = sensor['sensordata_id']
# configure device actuators
device['actuator_ids'] = []
device['actuator_names'] = []
for actuator in device['actuator']:
- device['actuator_names'].append(actuator['name']) # list of all ids
- device['actuator_ids'].append(actuator['hdr_id']) # list of all ids
+ hdr_actuator_id += 1
+ actuator['hdr_id'] = hdr_actuator_id
actuator['sim_id'] = sim.model.actuator_name2id(actuator['name'])
+ device['actuator_names'].append(actuator['name']) # list of all ids
+ device['actuator_ids'].append(actuator['hdr_adr']) # list of all ids
actuator_trntype = sim.model.actuator_trntype[actuator['sim_id']]
actuator_trnid = sim.model.actuator_trnid[actuator['sim_id'], 0]
- if actuator_trntype == 0: # mjTRN_JOINT // force on joint
+ if actuator_trntype == mujoco.mjtTrn.mjTRN_JOINT: # // force on joint
actuator['data_type'] = 'qpos'
- actuator['data_id'] = sim.model.jnt_dofadr[actuator_trnid]
+ # qpos is indexed by jnt_qposadr, not jnt_dofadr; the two
+ # diverge when a free/ball joint precedes this joint
+ # (freejoint: 7 qpos vs 6 dofs), shifting every read.
+ actuator['data_id'] = sim.model.jnt_qposadr[actuator_trnid]
+ elif actuator_trntype == mujoco.mjtTrn.mjTRN_TENDON: # force on tendon
+ actuator['data_type'] = 'ten_length'
+ actuator['data_id'] = actuator_trnid
else:
quit("ERROR: actuator {} has unsupported transmission_type: {}".format(actuator['name'],actuator_trntype))
return robot_config
@@ -374,6 +324,9 @@ def get_sensors(self, noise_scale=None, random_generator=None):
current_sen={}
noise_scale = self._noise_scale if noise_scale is None else noise_scale
+ # Update time
+ self.time_wall = time.time()-self.time_start
+
if self.is_hardware:
# record sensor*device['scale']+device['offset']
current_sen = self.hardware_get_sensors()
@@ -397,6 +350,8 @@ def get_sensors(self, noise_scale=None, random_generator=None):
# add noise
if noise_scale!=0:
s += noise_scale*sensor['noise']*self.np_random.uniform(low=-1.0, high=1.0)
+ # ensure range
+ s = np.clip(s, sensor['range'][0], sensor['range'][1])
sen.append(s)
current_sen[name] = np.array(sen)
@@ -410,9 +365,6 @@ def get_sensors(self, noise_scale=None, random_generator=None):
# cache sensors
self._sensor_cache.append(current_sen)
- # Update time
- self.time_wall = time.time()-self.time_start
-
return current_sen
@@ -429,8 +381,14 @@ def get_visual_sensors(self, height:int, width:int, cameras:list, device_id:int,
for ind, cam_name in enumerate(cameras):
assert cam_name in self.robot_config.keys(), "{} camera not found".format(cam_name)
device = self.robot_config[cam_name]
- assert device['interface']['type'] == 'realsense', "Check interface type for {}".format(cam)
- data = device['robot'].get_sensors()
+
+ if hasattr(device['robot'], 'get_frame'):
+ # RGB-only camera (e.g. a UVC/webcam) — no depth stream.
+ rgb = device['robot'].get_frame()
+ data = {'time': time.time() - self.time_start, 'rgb': rgb, 'd': None}
+ else:
+ # RealSense-style camera: get_sensors() itself returns {'rgb','d'}.
+ data = device['robot'].get_sensors()
data_height = data['rgb'].shape[0]
assert data_height == height, "Incorrect image height: required:{}, found:{}".format(height, data_height)
data_width = data['rgb'].shape[1]
@@ -439,11 +397,12 @@ def get_visual_sensors(self, height:int, width:int, cameras:list, device_id:int,
# calibrate sensors
for cam in device['cam']:
- current_sensor_value[cam_name][cam['hdr_id']] = current_sensor_value[cam_name][cam['hdr_id']]*cam['scale'] + cam['offset']
+ current_sensor_value[cam_name][cam['hdr_adr']] = current_sensor_value[cam_name][cam['hdr_adr']]*cam['scale'] + cam['offset']
device['sensor_data'] = current_sensor_value[cam_name]
device['sensor_time'] = current_sensor_value['time']
imgs[ind, :, :, :] = current_sensor_value[cam_name]['rgb']
- depths[ind, :, :] = current_sensor_value[cam_name]['d'][:,:,0] # assumes single channel depth
+ if current_sensor_value[cam_name]['d'] is not None:
+ depths[ind, :, :] = current_sensor_value[cam_name]['d'][:,:,0] # assumes single channel depth
else:
imgs = np.zeros((len(cameras), height, width, 3), dtype=np.uint8)
@@ -471,19 +430,31 @@ def sensor2sim(self, sensor, sim):
print("WARNING: Propagating noisy sensors back to sim can destablize simulation")
sim.data.time = sensor['time']
+
+ # Pass 1: write invertible state — qpos and qvel survive sim.forward()
for name, device in self.robot_config.items():
if name == "default_robot":
sim.data.qpos[:] = device['sensor_data']['qpos']
sim.data.qvel[:] = device['sensor_data']['qvel']
- if self.sim.model.na >0:
+ if self.sim.model.na > 0:
sim.data.act[:] = device['sensor_data']['act']
else:
for s_id, s_val in enumerate(device['sensor']):
- # prompt(getattr(sim.data, s_val["data_type"])[s_val["data_id"]], sensor[name][s_id])
- data = getattr(sim.data, s_val["data_type"])
- data[s_val["data_id"]] = sensor[name][s_id]
+ if s_val['data_type'] in ('qpos', 'qvel'):
+ getattr(sim.data, s_val['data_type'])[s_val['data_id']] = sensor[name][s_id]
+
+ # Propagate qpos/qvel through kinematics; also recomputes sensordata from sim physics
sim.forward()
+ # Pass 2: write all hardware readings into sensordata, making it the single
+ # authoritative source regardless of sensor type. For qpos/qvel sensors this
+ # is redundant (forward() already propagated them) but keeps sensordata complete.
+ for name, device in self.robot_config.items():
+ if name == "default_robot":
+ continue
+ for s_id, s_val in enumerate(device['sensor']):
+ sim.data.sensordata[s_val['sensordata_id']] = sensor[name][s_id]
+
# synchronize states between two sims
def sync_sims(self, source_sim, destination_sim, model=True, data=True):
@@ -552,7 +523,6 @@ def normalize_actions(self, controls, out_space='sim', unnormalize=False):
Recover actions from unit space to absolute space; if unnormalize==True
in_space for controls has to be 'sim'
"""
- act_id = -1
controls_out = controls.copy()
for name, device in self.robot_config.items():
if name == "default_robot":
@@ -564,17 +534,16 @@ def normalize_actions(self, controls, out_space='sim', unnormalize=False):
raise TypeError("only pos act supported")
else:
for actuator in device['actuator']:
- act_id += 1
in_id = actuator['sim_id']
# output ordering is as per the config order for hdr
- out_id = actuator['sim_id'] if out_space == 'sim' else act_id
+ out_id = actuator['sim_id'] if out_space == 'sim' else actuator['hdr_id']
if self._act_mode == "pos":
act_mid = (actuator['pos_range'][1]+actuator['pos_range'][0])/2.0
act_rng = (actuator['pos_range'][1]-actuator['pos_range'][0])/2.0
elif self._act_mode == "vel":
act_mid = (actuator['vel_range'][1]+actuator['vel_range'][0])/2.0
- act_rng = (actuator['vel_range'][1]-actuator['pos_range'][0])/2.0
+ act_rng = (actuator['vel_range'][1]-actuator['vel_range'][0])/2.0
else:
raise TypeError("Unknown act mode: {}".format(self._act_mode))
@@ -605,7 +574,6 @@ def process_actuator(self, controls, step_duration,
"""
# last_obs = self.get_sensor_from_cache(-1)
processed_controls = controls.copy()
- act_id = -1
for name, device in self.robot_config.items():
if name == "default_robot":
if self._act_mode == "pos":
@@ -616,10 +584,9 @@ def process_actuator(self, controls, step_duration,
raise TypeError("only pos act supported")
else:
for actuator in device['actuator']:
- act_id += 1
in_id = actuator['sim_id']
# output ordering is as per the config order for hdr
- out_id = actuator['sim_id'] if out_space == 'sim' else act_id
+ out_id = actuator['sim_id'] if out_space == 'sim' else actuator['hdr_id']
control = controls[in_id]
if self._act_mode == "pos":
@@ -676,7 +643,7 @@ def step(self, ctrl_desired, step_duration, ctrl_normalized=True, realTimeSim=Fa
# Send controls to the robot
if self.is_hardware:
- self.hardware_apply_controls(ctrl_feasible)
+ self.hardware_apply_controls(ctrl_feasible, space=robot_type)
if render_cbk:
render_cbk()
else:
@@ -720,18 +687,16 @@ def reset(self,
prompt("Resetting {}".format(self.name), 'white', 'on_grey', flush=True)
# Enforce specs on the request
- # for actuated dofs => actoator specs
+ # for actuated dofs => actuator specs
# for passive dofs => sensor specs
feasibe_pos = reset_pos.copy()
feasibe_vel = reset_vel.copy()
- ctrl_feasible=[]
for name, device in self.robot_config.items():
if name != "default_robot":
if len(device['actuator'])>0: # actuated dofs
for actuator in device['actuator']:
if actuator['data_type'] == 'qpos':
feasibe_pos[actuator['data_id']] = np.clip(reset_pos[actuator['data_id']], actuator['pos_range'][0], actuator['pos_range'][1])
- ctrl_feasible.append(feasibe_pos[actuator['data_id']])
else: # passive dofs
for sensor in device['sensor']:
if sensor['data_type'] == 'qpos':
@@ -743,11 +708,9 @@ def reset(self,
t_reset_start = time.time()
prompt("\nRollout took:{}".format(t_reset_start- self.time_start))
prompt("\aResetting {}: ".format(self.name), 'white', 'on_grey', flush=True, end="")
- # send request to the actuated dofs
- self.hardware_apply_controls(ctrl_feasible, is_reset=True)
-
- # engage other reset mechanisms for passive dofs
- # TODO raise NotImplementedError
+ # send request to all devices, actuated and passive alike
+ # feasibe_pos was built above via actuator['data_id'] indexing, i.e. sim space
+ self.hardware_reset(feasibe_pos, space='sim')
if blocking:
input("press a key to start rollout")
@@ -802,8 +765,9 @@ def demo_robot():
from robohive.utils import gym
prompt("Starting Robot===================")
- env = gym.make('FrankaReachFixed-v0')
- rob = env.env.robot
+ envw = gym.make('FrankaReachFixed-v0')
+ env = envw.unwrapped
+ rob = env.robot
prompt("Getting sensor data==============")
sen = rob.get_sensors()
@@ -811,16 +775,16 @@ def demo_robot():
prompt(sen)
prompt("stepping forward=================")
- ctrl = env.env.np_random.uniform(size=env.env.sim.model.nu)
+ ctrl = env.np_random.uniform(size=env.sim.model.nu)
rob.step(ctrl, 1.0)
prompt("Resetting Robot==================")
- pos = env.env.np_random.uniform(size=env.env.sim.model.nq)
- vel = env.env.np_random.uniform(size=env.env.sim.model.nv)
+ pos = env.np_random.uniform(size=env.sim.model.nq)
+ vel = env.np_random.uniform(size=env.sim.model.nv)
rob.reset(pos, vel)
prompt("Closing Robot====================")
rob.close()
if __name__ == '__main__':
- demo_robot()
\ No newline at end of file
+ demo_robot()
diff --git a/robohive/simhive/franka_sim b/robohive/simhive/franka_sim
index 82aaf3be..2e6542c3 160000
--- a/robohive/simhive/franka_sim
+++ b/robohive/simhive/franka_sim
@@ -1 +1 @@
-Subproject commit 82aaf3bebfa29e00133a6eebc7684e793c668fc1
+Subproject commit 2e6542c36114e92803adfc3016f2c69cd9a8ee8c
diff --git a/robohive/tests/test_envs.py b/robohive/tests/test_envs.py
index 78137191..21bec889 100644
--- a/robohive/tests/test_envs.py
+++ b/robohive/tests/test_envs.py
@@ -5,15 +5,18 @@
License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
================================================= """
+import copy
+import os
+import pickle
import unittest
-from robohive.utils import gym
import numpy as np
-import pickle
-import copy
-import os
from flatten_dict import flatten
+from robohive.utils import gym
+from robohive.utils.implement_for import implement_for
+
+
def assert_close(prm1, prm2, atol=1e-05, rtol=1e-08):
if prm1 is None and prm2 is None:
return True
@@ -66,7 +69,8 @@ def check_env(self, environment_id, input_seed):
rwd_dict1 = env1.get_reward_dict(obs_dict1)
assert len(rwd_dict1) > 0
# reset env
- env1.reset()
+ reset_data = env1.reset()
+ self.check_reset(reset_data)
# serialize / deserialize env ------------
env2w = pickle.loads(pickle.dumps(env1w))
@@ -102,35 +106,50 @@ def check_env(self, environment_id, input_seed):
del(env1)
del(env2)
+
+ @implement_for("gym", None, "0.26")
+ def check_reset(self, reset_data):
+ assert isinstance(reset_data, np.ndarray), "Reset should return the observation vector"
+
+ @implement_for("gym", "0.26", None)
+ def check_reset(self, reset_data):
+ assert isinstance(reset_data, tuple) and len(reset_data) == 2, "Reset should return a tuple of length 2"
+ assert isinstance(reset_data[1], dict), "second element returned should be a dict"
+ @implement_for("gymnasium")
+ def check_reset(self, reset_data):
+ assert isinstance(reset_data, tuple) and len(reset_data) == 2, "Reset should return a tuple of length 2"
+ assert isinstance(reset_data[1], dict), "second element returned should be a dict"
+
def check_old_envs(self, module_name, env_names, lite=False, seed=1234):
print("\nTesting module:: ", module_name)
for env_name in env_names:
print("Testing env: ", env_name)
# test init
- env = gym.make(env_name)
+ envw = gym.make(env_name)
+ env = envw.unwrapped
env.seed(seed)
# test reset
- env.env.reset()
+ env.reset()
# test obs vec
- obs = env.env.get_obs()
+ obs = env.get_obs()
if not lite:
# test obs dict
- obs_dict = env.env.get_obs_dict(env.env.sim)
+ obs_dict = env.get_obs_dict(env.sim)
# test rewards
- rwd = env.env.get_reward_dict(obs_dict)
+ rwd = env.get_reward_dict(obs_dict)
# test vector => dict upgrade
- # print(env.env.get_obs() - env.env.get_obs_vec())
- # assert (env.env.get_obs() == env.env.get_obs_vec()).all(), "check vectorized computations"
+ # print(env.get_obs() - env.get_obs_vec())
+ # assert (env.get_obs() == env.get_obs_vec()).all(), "check vectorized computations"
# test env infos
- infos = env.env.get_env_infos()
+ infos = env.get_env_infos()
# test step (everything together)
- observation, _reward, done, _info = env.env.step(np.zeros(env.env.sim.model.nu))
- del(env)
+ observation, _reward, done, *_, _info = env.step(np.zeros(env.sim.model.nu))
+ del(envw)
if __name__ == '__main__':
diff --git a/robohive/tests/test_logger.py b/robohive/tests/test_logger.py
index d07e0010..7cbf5787 100644
--- a/robohive/tests/test_logger.py
+++ b/robohive/tests/test_logger.py
@@ -2,18 +2,42 @@
import click
import click.testing
-from robohive.logger.grouped_datasets import test_trace
+from robohive.logger.grouped_datasets import test_trace, test_trace_append, test_trace_plot
from robohive.logger.examine_logs import examine_logs
from robohive.utils.examine_env import main as examine_env
import os
import re
+import glob
class TestTrace(unittest.TestCase):
- def teast_trace(self):
+ def test_trace(self):
# Call your function and test its output/assertions
print("Testing Trace Basics")
test_trace()
+ def test_trace_append(self):
+ # Call your function and test its output/assertions
+ print("Testing Trace complex appends")
+ test_trace_append()
+
+ def test_trace_plot(self):
+ # Call your function and test its output/assertions
+ print("Testing Trace plotting")
+ test_trace_plot()
+ # Define the pattern for the files you want to delete
+ pattern = "./*plot*.pdf"
+
+ # Use glob to find all files matching the pattern
+ files_to_delete = glob.glob(pattern)
+
+ # Iterate over the list of files and delete each one
+ for file_path in files_to_delete:
+ try:
+ os.remove(file_path)
+ print(f"Deleted: {file_path}")
+ except Exception as e:
+ print(f"Error deleting file {file_path}: {e}")
+
class TestExamineTrace(unittest.TestCase):
def test_logs_playback(self):
diff --git a/robohive/tutorials/3_get_obs_proprio_extero.ipynb b/robohive/tutorials/3_get_obs_proprio_extero.ipynb
index d39e8436..166899a3 100644
--- a/robohive/tutorials/3_get_obs_proprio_extero.ipynb
+++ b/robohive/tutorials/3_get_obs_proprio_extero.ipynb
@@ -135,6 +135,60 @@
"print(f\"visual_dict = {env.visual_dict.keys()}\")"
]
},
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "### 5. convert depth into a point cloud\n",
+ "\n",
+ "If you need a point cloud observation, you can convert the depth image into the point cloud in the following way.\n",
+ "\n",
+ "First, use env.get_visuals to obtain the depth image in the obs."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "import matplotlib.pyplot as plt\n",
+ "obs = env.get_visuals(visual_keys=['rgb:vil_camera:224x224:2d', 'd:vil_camera:224x224:2d'])\n",
+ "color = obs['rgb:vil_camera:224x224:2d']\n",
+ "depth = obs['d:vil_camera:224x224:2d'][0]\n",
+ "plt.imshow(color)\n",
+ "plt.show()\n",
+ "plt.imshow(depth)\n",
+ "plt.show()"
+ ]
+ },
+ {
+ "cell_type": "markdown",
+ "metadata": {},
+ "source": [
+ "Then we convert the depth image into a point cloud based on camera intrinsics and extrinsics."
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from robohive.utils.pointcloud_utils import get_point_cloud, visualize_point_cloud_from_nparray\n",
+ "pcd = get_point_cloud(env, obs['d:vil_camera:224x224:2d'][0], 'vil_camera')\n",
+ "\n",
+ "# # Interactive point cloud visualization with open3d (requires open3d installation)\n",
+ "# visualize_point_cloud_from_nparray(pcd, obs['rgb:vil_camera:224x224:2d'], vis_coordinate=True)\n",
+ "\n",
+ "# Visualize the point cloud with matplotlib (if you are too lazy to install open3d)\n",
+ "import matplotlib.pyplot as plt\n",
+ "fig = plt.figure()\n",
+ "ax = fig.add_subplot(projection='3d')\n",
+ "ax.scatter(pcd[:,0], pcd[:,1], pcd[:,2], c=obs['rgb:vil_camera:224x224:2d'].reshape(-1, 3)/256)\n",
+ "plt.show()\n"
+ ]
+ },
{
"attachments": {},
"cell_type": "markdown",
@@ -159,7 +213,7 @@
"outputs": [],
"source": [
"# Recover all info at current timestep: obs(t), rwd(t), done(t), info(t)\n",
- "obs_t, rwd_t, done_t, info_t = env.env.forward(update_proprioception=True, update_exteroception=True)\n",
+ "obs_t, rwd_t, done_t, *_, info_t = env.unwrapped.forward(update_proprioception=True, update_exteroception=True)\n",
"print(f\"time = {env.obs_dict['time']}\")\n",
"print(f\"obs vector = {obs_t}\")\n",
"print(f\"obs_dict = {env.obs_dict.keys()}\")\n",
@@ -167,7 +221,7 @@
"print(f\"visual_dict = {env.visual_dict.keys()}\")\n",
"\n",
"# Recover info at the next timestep: obs(t+dt), rwd(t+dt), done(t+dt), info(t+dt)\n",
- "obs_tdt, rwd_tdt, done_tdt, info_tdt = env.env.step(env.action_space.sample(), update_proprioception=True, update_exteroception=True)\n",
+ "obs_tdt, rwd_tdt, done_tdt, *_, info_tdt = env.unwrapped.step(env.action_space.sample(), update_proprioception=True, update_exteroception=True)\n",
"print(f\"time = {env.obs_dict['time']}\")\n",
"print(f\"obs vector = {obs_tdt}\")\n",
"print(f\"obs_dict = {env.obs_dict.keys()}\")\n",
@@ -202,7 +256,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
- "version": "3.8.16"
+ "version": "3.10.13"
},
"orig_nbformat": 4
},
diff --git a/robohive/tutorials/ee_teleop.py b/robohive/tutorials/ee_teleop.py
index 2808e775..c0eab658 100644
--- a/robohive/tutorials/ee_teleop.py
+++ b/robohive/tutorials/ee_teleop.py
@@ -180,9 +180,10 @@ def main(env_name, env_args, reset_noise, action_noise, input_device, output, ho
# seed and load environments
np.random.seed(seed)
- env = gym.make(env_name) if env_args==None else gym.make(env_name, **(eval(env_args)))
+ envw = gym.make(env_name) if env_args==None else gym.make(env_name, **(eval(env_args)))
+ env = envw.unwrapped
env.seed(seed)
- env.env.mujoco_render_frames = True if 'onscreen'in render else False
+ env.mujoco_render_frames = True if 'onscreen'in render else False
goal_sid = env.sim.model.site_name2id(goal_site)
env.sim.model.site_rgba[goal_sid][3] = 0.2 # make visible
@@ -211,7 +212,7 @@ def main(env_name, env_args, reset_noise, action_noise, input_device, output, ho
env.reset(reset_qpos=env.init_qpos+reset_noise, blocking=True)
# recover init state
- obs, rwd, done, env_info = env.forward()
+ obs, rwd, done, *_, env_info = env.forward()
act = np.zeros(env.action_space.shape)
gripper_state = 0
@@ -254,9 +255,9 @@ def main(env_name, env_args, reset_noise, action_noise, input_device, output, ho
act[:7] = ik_result.qpos[:7]
act[7:] = gripper_state
if action_noise:
- act = act + env.env.np_random.uniform(high=action_noise, low=-action_noise, size=len(act)).astype(act.dtype)
+ act = act + env.np_random.uniform(high=action_noise, low=-action_noise, size=len(act)).astype(act.dtype)
if env.normalize_act:
- act = env.env.robot.normalize_actions(act)
+ act = env.robot.normalize_actions(act)
# nan actions for last log entry
act = np.nan*np.ones(env.action_space.shape) if i_step == horizon else act
@@ -275,7 +276,7 @@ def main(env_name, env_args, reset_noise, action_noise, input_device, output, ho
# step env using action from t=>t+1 ----------------------
if i_step < horizon: #incase last actions (nans) can cause issues in step
- obs, rwd, done, env_info = env.step(act)
+ obs, rwd, done, *_, env_info = env.step(act)
print("rollout {} end".format(i_rollout))
@@ -294,4 +295,4 @@ def main(env_name, env_args, reset_noise, action_noise, input_device, output, ho
if __name__ == '__main__':
- main()
\ No newline at end of file
+ main()
diff --git a/robohive/tutorials/ee_teleop_oculus.py b/robohive/tutorials/ee_teleop_oculus.py
index bd8f3165..b54715c4 100644
--- a/robohive/tutorials/ee_teleop_oculus.py
+++ b/robohive/tutorials/ee_teleop_oculus.py
@@ -83,9 +83,10 @@ def main(env_name, env_args, reset_noise, action_noise, output, horizon, num_rol
# seed and load environments
np.random.seed(seed)
- env = gym.make(env_name) if env_args==None else gym.make(env_name, **(eval(env_args)))
+ envw = gym.make(env_name) if env_args==None else gym.make(env_name, **(eval(env_args)))
+ env = envw.unwrapped
env.seed(seed)
- env.env.mujoco_render_frames = True if 'onscreen'in render else False
+ env.mujoco_render_frames = True if 'onscreen'in render else False
goal_sid = env.sim.model.site_name2id(goal_site)
teleop_sid = env.sim.model.site_name2id(teleop_site)
env.sim.model.site_rgba[goal_sid][3] = 0.2 # make visible
@@ -128,7 +129,7 @@ def main(env_name, env_args, reset_noise, action_noise, output, horizon, num_rol
env.sim.model.site_quat[goal_sid] = mat2quat(np.reshape(env.sim.data.site_xmat[teleop_sid], [3,-1]))
# recover init state
- obs, rwd, done, env_info = env.forward()
+ obs, rwd, done, *_, env_info = env.forward()
act = np.zeros(env.action_space.shape)
gripper_state = 0
@@ -201,9 +202,9 @@ def main(env_name, env_args, reset_noise, action_noise, output, horizon, num_rol
act[:7] = ik_result.qpos[:7]
act[7:] = gripper_state
if action_noise:
- act = act + env.env.np_random.uniform(high=action_noise, low=-action_noise, size=len(act)).astype(act.dtype)
+ act = act + env.np_random.uniform(high=action_noise, low=-action_noise, size=len(act)).astype(act.dtype)
if env.normalize_act:
- act = env.env.robot.normalize_actions(act)
+ act = env.robot.normalize_actions(act)
# nan actions for last log entry
act = np.nan*np.ones(env.action_space.shape) if i_step == horizon else act
@@ -222,7 +223,7 @@ def main(env_name, env_args, reset_noise, action_noise, output, horizon, num_rol
# step env using action from t=>t+1 ----------------------
if i_step < horizon: #incase last actions (nans) can cause issues in step
- obs, rwd, done, env_info = env.step(act)
+ obs, rwd, done, *_, env_info = env.step(act)
# Detect jumps
qpos_now = env_info['obs_dict']['qp_arm']
@@ -251,4 +252,4 @@ def main(env_name, env_args, reset_noise, action_noise, output, horizon, num_rol
if __name__ == '__main__':
- main()
\ No newline at end of file
+ main()
diff --git a/robohive/tutorials/render_cams.py b/robohive/tutorials/render_cams.py
index c5ef5df9..551978d9 100644
--- a/robohive/tutorials/render_cams.py
+++ b/robohive/tutorials/render_cams.py
@@ -9,29 +9,18 @@
DESC = '''
Helper script to render images offscreen and save using a mujoco model.\n
USAGE:\n
- $ python render_cams.py --model_path franka_sim.xml --cam_names top_cam --cam_names left_cam --cam_names right_cam \n
+ $ python render_cams.py --model_path <../model.xml> --cam_names --cam_names \n
EXAMPLE:\n
- $ python utils/render_cams.py -m envs/fm/assets/franka_microwave.xml -c top_cam -c left_cam -c right_cam
+ $ python robohive/tutorials/render_cams.py -m robohive/envs/arms/franka/assets/franka_reach_v0.xml -c left_cam -c top_cam
+ $ python robohive/tutorials/render_cams.py -m robohive/simhive/robel_sim/dkitty/kitty-v2.1.xml -c "A:trackingY" -c "A:trackingZ"
+
'''
-from mujoco_py import load_model_from_path, MjSim
+import mujoco
+
from PIL import Image
-import numpy as np
import click
-
-def render_camera_offscreen(cameras:list, width:int=640, height:int=480, device_id:int=0, sim=None):
- """
- Render images(widthxheight) from a list_of_cameras on the specified device_id.
- """
- imgs = np.zeros((len(cameras), height, width, 3), dtype=np.uint8)
- for ind, cam in enumerate(cameras) :
- img = sim.render(width=width, height=height, mode='offscreen', camera_name=cam, device_id=device_id)
- img = img[::-1, :, : ] # Image has to be flipped
- imgs[ind, :, :, :] = img
- return imgs
-
-
@click.command(help=DESC)
@click.option('-m', '--model_path', required=True, type=str, help='model file')
@click.option('-c', '--cam_names', required=True, multiple=True, help=('Camera names for rendering'))
@@ -40,14 +29,23 @@ def render_camera_offscreen(cameras:list, width:int=640, height:int=480, device_
@click.option('-d', '--device_id', type=int, default=0, help='device id for rendering')
def main(model_path, cam_names, width, height, device_id):
- # render images
- model = load_model_from_path(model_path)
- sim = MjSim(model)
- imgs = render_camera_offscreen(cameras=cam_names, width=width, height=height, device_id=device_id, sim=sim)
+
+ # prepare model, data, scene
+ mj_model = mujoco.MjModel.from_xml_path(model_path)
+ mj_data = mujoco.MjData(mj_model)
+ mujoco.mj_forward(mj_model, mj_data)
+
+ # prepare the renderer
+ renderer = mujoco.Renderer(mj_model, height=height, width=width)
# save images
for i, cam in enumerate(cam_names):
- image = Image.fromarray(imgs[i])
+ # update the scene
+ renderer.update_scene(mj_data, camera=cam)
+ # render the rgb_array
+ rgb_arr = renderer.render()
+ # save the image
+ image = Image.fromarray(rgb_arr)
image.save(cam+".jpeg")
print("saved "+cam+".jpeg")
diff --git a/robohive/utils/examine_env.py b/robohive/utils/examine_env.py
index f8370a64..513c0c11 100644
--- a/robohive/utils/examine_env.py
+++ b/robohive/utils/examine_env.py
@@ -113,8 +113,8 @@ def main(env_name, policy_path, mode, seed, num_episodes, render, camera_name, o
# plot paths
if plot_paths:
- file_name = output_dir + '/' + output_name + '{}'.format(time_stamp)
- plotnsave_paths(paths, env=env, fileName_prefix=file_name)
+ file_name = output_name + '{}'.format(time_stamp)
+ plotnsave_paths(paths, env_handle=env, output_dir=output_dir, output_name=file_name)
# render visuals keys
if env.visual_keys and render_visuals:
diff --git a/robohive/utils/parse_mjl.py b/robohive/utils/parse_mjl.py
new file mode 100644
index 00000000..d9abe466
--- /dev/null
+++ b/robohive/utils/parse_mjl.py
@@ -0,0 +1,93 @@
+DESC = '''
+Parse mujoco (.mjl) logs\n
+mjl format: http://www.mujoco.org/book/haptix.html#uiRecord
+'''
+import struct
+import numpy as np
+import matplotlib as mpl
+mpl.use('TkAgg')
+import matplotlib.pyplot as plt
+import click
+
+# parse mjl binary logs into python dictionary
+def parse_mjl_logs(read_filename, skipamount):
+ with open(read_filename, mode='rb') as file:
+ fileContent = file.read()
+ headers = struct.unpack('iiiiiii', fileContent[:28])
+ nq = headers[0]
+ nv = headers[1]
+ nu = headers[2]
+ nmocap = headers[3]
+ nsensordata = headers[4]
+ nuserdata = headers[5]
+ name_len = headers[6]
+ name = struct.unpack(str(name_len) + 's', fileContent[28:28+name_len])[0]
+ rem_size = len(fileContent[28 + name_len:])
+ num_floats = int(rem_size/4)
+ dat = np.asarray(struct.unpack(str(num_floats) + 'f', fileContent[28+name_len:]))
+ recsz = 1 + nq + nv + nu + 7*nmocap + nsensordata + nuserdata
+ if rem_size % recsz != 0:
+ print("ERROR")
+ else:
+ dat = np.reshape(dat, (int(len(dat)/recsz), recsz))
+ dat = dat.T
+
+ time = dat[0,:][::skipamount] - 0*dat[0, 0]
+ qpos = dat[1:nq + 1, :].T[::skipamount, :]
+ qvel = dat[nq+1:nq+nv+1,:].T[::skipamount, :]
+ ctrl = dat[nq+nv+1:nq+nv+nu+1,:].T[::skipamount,:]
+ mocap_pos = dat[nq+nv+nu+1:nq+nv+nu+3*nmocap+1,:].T[::skipamount, :]
+ mocap_quat = dat[nq+nv+nu+3*nmocap+1:nq+nv+nu+7*nmocap+1,:].T[::skipamount, :]
+ sensordata = dat[nq+nv+nu+7*nmocap+1:nq+nv+nu+7*nmocap+nsensordata+1,:].T[::skipamount,:]
+ userdata = dat[nq+nv+nu+7*nmocap+nsensordata+1:,:].T[::skipamount,:]
+
+ data = dict(nq=nq,
+ nv=nv,
+ nu=nu,
+ nmocap=nmocap,
+ nsensordata=nsensordata,
+ name=name,
+ time=time,
+ qpos=qpos,
+ qvel=qvel,
+ ctrl=ctrl,
+ mocap_pos=mocap_pos,
+ mocap_quat=mocap_quat,
+ sensordata=sensordata,
+ userdata=userdata,
+ logName = read_filename
+ )
+ return data
+
+# visualize parsed logs
+def viz_parsed_mjl_logs(data):
+ f, axarr = plt.subplots(2, sharex=True)
+ axarr[0].plot(data['time'], data['qpos'])
+ axarr[0].set_ylabel('qpos')
+ axarr[0].set_title(data['logName'])
+ axarr[1].plot(data['time'], data['ctrl'])
+ axarr[1].set_ylabel('ctrl')
+ axarr[1].set_xlabel('time')
+ plt.savefig(data['logName'][:-4]+".png")
+ print(data['logName'][:-4]+".png saved")
+
+
+
+# MAIN =========================================================
+@click.command(help=DESC)
+@click.option('--log', '-l', type=str, help='.mjl log to parse', required= True)
+@click.option('--skip', '-s', type=int, help='number of frames to skip (1:no skip)', default=1)
+@click.option('--plot', '-p', type=bool, help='plot parsed logs', default=False)
+def main(log, skip, plot):
+ print("Loading log file: %s" % log)
+ data = parse_mjl_logs(log, skip)
+ print("file successfully parsed")
+
+
+ if(plot):
+ print("plotting data")
+ viz_parsed_mjl_logs(data)
+
+
+if __name__ == '__main__':
+ main()
diff --git a/robohive/utils/paths_utils.py b/robohive/utils/paths_utils.py
index 32c44e7d..743518f6 100644
--- a/robohive/utils/paths_utils.py
+++ b/robohive/utils/paths_utils.py
@@ -5,33 +5,160 @@
License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
================================================= """
-import numpy as np
-import os
import glob
+import json
+import os
import pickle
+
+import click
import h5py
+import numpy as np
import skvideo.io
from PIL import Image
-import click
-from robohive.utils.dict_utils import flatten_dict, dict_numpify
-import json
+
+from robohive.utils import gym
+from robohive.utils.dict_utils import dict_numpify, flatten_dict
+from robohive.logger.grouped_datasets import Trace
#TODO: Harmonize names, remove rollout_paths, use path for one and paths for multiple
+ROLLOUT_EXTENSIONS = ('.h5', '.pickle')
+
+
+# Normalize a loaded paths object (list or dict-like) into a name->path dict
+def _normalize_paths(paths):
+ if isinstance(paths, (list, tuple)):
+ return {'Trial{}'.format(i): path for i, path in enumerate(paths)}
+ if isinstance(paths, Trace):
+ return dict(paths.trace)
+ return paths
+
+
+# Load a single rollout file (.h5 / .pickle) into a normalized path object
+def load_rollout_file(rollout_path:str):
+ """
+ Load a single rollout file (.h5 or .pickle) from disk.
+
+ Args:
+ rollout_path (str): absolute path to a rollout file
+ Returns:
+ dict-like object mapping path/trial name to its data
+ """
+ ext = os.path.splitext(rollout_path)[-1]
+ if ext == '.h5':
+ paths = h5py.File(rollout_path, 'r')
+ elif ext == '.pickle':
+ paths = pickle.load(open(rollout_path, 'rb'))
+ else:
+ raise TypeError("Unknown rollout format:{}. Supported formats: {}".format(ext, ROLLOUT_EXTENSIONS))
+ return _normalize_paths(paths)
+
+
+# Harmonized preprocessor: resolve a path_handle into a loaded path object
+def resolve_path_handle(path_handle, extensions=ROLLOUT_EXTENSIONS, return_sources=False):
+ """
+ Resolve a path_handle into a normalized, loaded path object.
+
+ path_handle can be-
+ - an already loaded path object (dict / list / h5py.File / Trace.trace, ...)
+ - a str path to a single rollout file (.h5 or .pickle)
+ - a str path to a directory containing rollout files (.h5 / .pickle),
+ searched recursively through subdirectories, and all matching files
+ are loaded and merged
+
+ Args:
+ path_handle: handle to resolve (see above)
+ extensions: rollout file extensions to scan for when path_handle is a directory
+ return_sources (bool): if True, also return a dict mapping each path/trial name to a
+ (source_dir, source_file_stem) tuple describing the file it was loaded from
+ ((None, None) if path_handle was already a loaded object with no known file location)
+ Returns:
+ dict-like object mapping path/trial name to its data
+ (dict mapping path/trial name to its (source_dir, source_file_stem), if return_sources)
+ """
+ if isinstance(path_handle, str):
+ if os.path.isdir(path_handle):
+ rollout_files = sorted(
+ f for ext in extensions
+ for f in glob.glob(os.path.join(path_handle, '**', '*'+ext), recursive=True))
+ assert len(rollout_files) > 0, \
+ "No rollout files (formats:{}) found in directory:{} (searched recursively)".format(extensions, path_handle)
+ paths, sources = {}, {}
+ for rollout_file in rollout_files:
+ prefix = os.path.splitext(os.path.relpath(rollout_file, path_handle))[0]
+ file_dir = os.path.dirname(rollout_file)
+ file_stem = os.path.splitext(os.path.basename(rollout_file))[0]
+ for name, data in load_rollout_file(rollout_file).items():
+ key = '{}/{}'.format(prefix, name)
+ paths[key] = data
+ sources[key] = (file_dir, file_stem)
+ return (paths, sources) if return_sources else paths
+ elif os.path.isfile(path_handle):
+ paths = load_rollout_file(path_handle)
+ file_dir = os.path.dirname(os.path.abspath(path_handle))
+ file_stem = os.path.splitext(os.path.basename(path_handle))[0]
+ return (paths, {key: (file_dir, file_stem) for key in paths.keys()}) if return_sources else paths
+ else:
+ raise FileNotFoundError("Path not found:{}".format(path_handle))
+ else:
+ # already a loaded path object; source location is unknown
+ paths = _normalize_paths(path_handle)
+ return (paths, {key: None for key in paths.keys()}) if return_sources else paths
+
+
+# Harmonized preprocessor: resolve an env_handle into a loaded, unwrapped env
+def resolve_env_handle(env_handle, env_args=None):
+ """
+ Resolve an env_handle into a loaded, unwrapped env object.
+
+ env_handle can be-
+ - an already loaded env object (wrapped or unwrapped)
+ - a str env_name to be created via gym.make
+
+ Args:
+ env_handle: handle to resolve (see above)
+ env_args (str, optional): kwargs (as a str-eval'able dict) to pass to gym.make, when env_handle is a str
+ Returns:
+ unwrapped env object, or None if env_handle is None
+ """
+ if env_handle is None:
+ return None
+ elif isinstance(env_handle, str):
+ env = gym.make(env_handle) if env_args is None else gym.make(env_handle, **(eval(env_args)))
+ return env.unwrapped
+ else:
+ # already a loaded env object
+ return env_handle.unwrapped if hasattr(env_handle, 'unwrapped') else env_handle
+
+
# Check the horizon for teleOp / Hardware experiments
-def plot_horizon(paths, env, fileName_prefix=None):
+def plot_horizon(path_handle, env_handle, output_dir=None, output_name='', env_args=None):
"""
Check the horizon for teleOp / Hardware experiments
Args:
- paths: paths to examine
- env: unwrapped env
- fileName_prefix (str): prefix to use in the filename
+ path_handle: loaded path object, or path to a rollout file/ directory of rollout files
+ env_handle: loaded (unwrapped) env object, or an env_name to be created via gym.make
+ output_dir (str, optional): Directory to save the outputs. Defaults to path_handle's
+ directory (or cwd if path_handle is not a str path).
+ output_name (str, optional): prefix to use in the output filename
+ env_args (str, optional): kwargs (as a str-eval'able dict) to pass to gym.make, when env_handle is a str
Saves:
- fileName_prefix + '_horizon.pdf'
+ output_dir/output_name + '_horizon.pdf'
"""
+ paths = list(resolve_path_handle(path_handle).values())
+ env = resolve_env_handle(env_handle, env_args=env_args)
+
+ if output_dir is None:
+ if isinstance(path_handle, str):
+ output_dir = path_handle if os.path.isdir(path_handle) else (os.path.dirname(os.path.abspath(path_handle)))
+ else:
+ output_dir = '.'
+ os.makedirs(output_dir, exist_ok=True)
+ fileName_prefix = os.path.join(output_dir, output_name or '')
+
import matplotlib as mpl
- mpl.use('TkAgg')
+ mpl.use('Agg')
import matplotlib.pyplot as plt
plt.rcParams.update({'font.size': 5})
@@ -41,26 +168,28 @@ def plot_horizon(paths, env, fileName_prefix=None):
# plot timesteps
plt.clf()
- rl_dt_ideal = env.frame_skip * env.model.opt.timestep
+ rl_dt_ideal = env.dt
for i, path in enumerate(paths):
dt = path['env_infos']['time'][1:] - path['env_infos']['time'][:-1]
horizon[i] = path['env_infos']['time'][-1] - path['env_infos'][
'time'][0]
- h1 = plt.plot(
+ plt.plot(
path['env_infos']['time'][1:],
dt,
'-',
- label=('time=%1.2f' % horizon[i]))
- h1 = plt.plot(
+ alpha=.8,
+ label=('hor=%1.2f' % horizon[i]))
+ plt.plot(
np.array([0, max(horizon)]),
rl_dt_ideal * np.ones(2),
'g', alpha=.5,
- linewidth=2.0)
+ linewidth=2.0,
+ label='ideal dt')
- plt.legend([h1[0]], ['ideal'], loc='upper right')
+ plt.legend(loc='upper right')
plt.ylabel('time step (sec)')
plt.xlabel('time (sec)')
- plt.ylim(rl_dt_ideal - 0.005, rl_dt_ideal + .005)
+ plt.ylim(rl_dt_ideal - 2*rl_dt_ideal, rl_dt_ideal + 3*rl_dt_ideal)
plt.suptitle('Timestep profile for %d rollouts' % len(paths))
file_name = fileName_prefix + '_timesteps.pdf'
@@ -69,7 +198,7 @@ def plot_horizon(paths, env, fileName_prefix=None):
# plot horizon
plt.clf()
- h1 = plt.plot(
+ plt.plot(
np.array([0, len(paths)]),
env.horizon * rl_dt_ideal * np.ones(2),
'g',
@@ -87,31 +216,62 @@ def plot_horizon(paths, env, fileName_prefix=None):
# 2D-plot of paths detailing obs, act, rwds across time
-def plot(paths, env=None, fileName_prefix=''):
+def plot(path_handle, env_handle=None, output_dir=None, output_name='', env_args=None):
"""
2D-plot of paths detailing obs, act, rwds across time
Args:
- paths: paths to examine
- env: unwrapped env
- fileName_prefix: prefix to use in the filename
+ path_handle: loaded path object, or path to a rollout file/ directory of rollout files
+ env_handle: loaded (unwrapped) env object, or an env_name to be created via gym.make
+ output_dir (str, optional): Directory to save the outputs. If not provided, each
+ trial's plot is saved next to the rollout file it came from. If provided, the
+ source directory structure (for directory path_handles) is mirrored under it.
+ output_name (str, optional): prefix to use in the output filenames. If not provided,
+ defaults to the name of the rollout file the trial came from (when known).
+ env_args (str, optional): kwargs (as a str-eval'able dict) to pass to gym.make, when env_handle is a str
Saves:
- fileName_prefix + path_name + '.pdf'
+ one pdf per trial, named _ + '.pdf'
"""
import matplotlib as mpl
mpl.use('Agg')
import matplotlib.pyplot as plt
plt.rcParams.update({'font.size': 5})
+ paths, sources = resolve_path_handle(path_handle, return_sources=True)
+ env = resolve_env_handle(env_handle, env_args=env_args)
+ is_dir_handle = isinstance(path_handle, str) and os.path.isdir(path_handle)
for path_name, path in paths.items():
+ leaf_name = path_name.split('/')[-1]
+ source_dir, source_stem = sources.get(path_name) or (None, None)
+ if output_dir is not None:
+ if is_dir_handle and source_dir is not None:
+ rel_dir = os.path.relpath(source_dir, path_handle)
+ save_dir = os.path.join(output_dir, rel_dir) if rel_dir != '.' else output_dir
+ else:
+ save_dir = output_dir
+ else:
+ save_dir = source_dir or '.'
+ os.makedirs(save_dir, exist_ok=True)
+ name_prefix = output_name or source_stem or ''
+ file_stem = '_'.join(p for p in (name_prefix, leaf_name) if p)
plt.clf()
# observations
- nplt1 = len(path['env_infos']['obs_dict'].keys())
- for iplt1, key in enumerate(
- sorted(path['env_infos']['obs_dict'].keys())):
+ obs_keys = sorted(
+ key for key in path['env_infos']['obs_dict'].keys()
+ if path['env_infos']['obs_dict'][key].ndim < 3)
+ nplt1 = len(obs_keys)
+ time = np.asarray(path['env_infos']['time'])
+ print(obs_keys)
+ for iplt1, key in enumerate(obs_keys):
ax = plt.subplot(nplt1, 2, iplt1 * 2 + 1)
+ if iplt1 == 0:
+ plt.ylabel('Observations')
+ ax.yaxis.tick_right()
+ if path['env_infos']['obs_dict'][key].size > 0:
+ plt.plot(time, np.asarray(path['env_infos']['obs_dict'][key]), label=key)
+ ax.set_xlim(time[0], time[-1])
if iplt1 != (nplt1 - 1):
ax.axes.xaxis.set_ticklabels([])
if iplt1 == 0:
@@ -123,7 +283,7 @@ def plot(paths, env=None, fileName_prefix=''):
path['env_infos']['obs_dict'][key],
label=key)
# plt.ylabel(key)
- plt.text(0.01, .01, key, transform=ax.transAxes)
+ plt.text(0.01, .01, f"{key}{path['env_infos']['obs_dict'][key].shape}", transform=ax.transAxes)
plt.xlabel('time (sec)')
# actions
@@ -132,7 +292,7 @@ def plot(paths, env=None, fileName_prefix=''):
ax.set_prop_cycle(None)
# h4 = plt.plot(path['env_infos']['time'], env.act_mid + path['actions']*env.act_rng, '-', label='act') # plot scaled actions
h4 = plt.plot(
- path['env_infos']['time'], path['actions'], '-',
+ time, np.asarray(path['actions']), '-',
label='act') # plot normalized actions
plt.ylabel('actions')
ax.axes.xaxis.set_ticklabels([])
@@ -142,8 +302,8 @@ def plot(paths, env=None, fileName_prefix=''):
if "score" in path['env_infos']:
ax = plt.subplot(nplt2, 2, 6)
plt.plot(
- path['env_infos']['time'],
- path['env_infos']['score'],
+ time,
+ np.asarray(path['env_infos']['score']),
label='score')
plt.xlabel('time')
plt.ylabel('score')
@@ -154,8 +314,8 @@ def plot(paths, env=None, fileName_prefix=''):
ax.set_prop_cycle(None)
for key in sorted(path['env_infos']['rwd_dict'].keys()):
plt.plot(
- path['env_infos']['time'],
- path['env_infos']['rwd_dict'][key],
+ time,
+ np.asarray(path['env_infos']['rwd_dict'][key]),
label=key)
plt.legend(
loc='upper left',
@@ -171,8 +331,8 @@ def plot(paths, env=None, fileName_prefix=''):
ax.set_prop_cycle(None)
for key in sorted(env.rwd_keys_wt.keys()):
plt.plot(
- path['env_infos']['time'],
- path['env_infos']['rwd_dict'][key]*env.rwd_keys_wt[key],
+ time,
+ np.asarray(path['env_infos']['rwd_dict'][key])*env.rwd_keys_wt[key],
label=key)
plt.legend(
loc='upper left',
@@ -183,20 +343,22 @@ def plot(paths, env=None, fileName_prefix=''):
plt.ylabel('wt*rewards')
ax.yaxis.tick_right()
- file_name = fileName_prefix + path_name + '.pdf'
+ file_name = os.path.join(save_dir, file_stem + '.pdf')
plt.savefig(file_name)
print("saved ", file_name)
# Render frames/videos
-def render(rollout_path, render_format:str="mp4", cam_names:list=["left"]):
+def render(path_handle, render_format:str="mp4", cam_names:list=["left"], output_dir=None, output_name=None):
"""
Render the frames from a given rollout.
Parameters:
- rollout_path (str): Absolute path of the rollout (h5/pickle).
+ path_handle: loaded path object, or path to a rollout file/ directory of rollout files (h5/pickle).
render_format (str, optional): Format to save the rendered frames. Default is "mp4".
cam_names (list, optional): List of cameras to render. Default is ["left"]. Example ['left', 'right', 'top', 'Franka_wrist']
+ output_dir (str, optional): Directory to save the outputs. Defaults to the rollout's directory (or cwd if path_handle is not a file path).
+ output_name (str, optional): Prefix to use for the output filenames. Defaults to the rollout's name (or "rollout" if path_handle is not a file path).
Returns:
None
@@ -208,34 +370,34 @@ def render(rollout_path, render_format:str="mp4", cam_names:list=["left"]):
- The frames are saved in the specified render format.
- The rendered frames can be saved as an mp4 video or as individual RGB images.
- The frames are rendered for each camera specified in the cam_names list.
- - The frames are saved in the same directory as the rollout path.
- The output file names are generated based on the rollout name and the camera names.
Example:
- render(rollout_path="/path/to/rollout.h5", render_format="mp4", cam_names=["left", "right"])
+ render(path_handle="/path/to/rollout.h5", render_format="mp4", cam_names=["left", "right"])
"""
- output_dir = os.path.dirname(rollout_path)
- rollout_name = os.path.split(rollout_path)[-1]
- output_name, output_type = os.path.splitext(rollout_name)
+ if isinstance(path_handle, str) and os.path.isfile(path_handle):
+ output_dir = output_dir or os.path.dirname(path_handle) or '.'
+ output_name = output_name or os.path.splitext(os.path.basename(path_handle))[0]
+ else:
+ output_dir = output_dir or '.'
+ output_name = output_name or 'rollout'
file_name = os.path.join(output_dir, output_name+"_"+"-".join(cam_names))
- # resolve data format
- if output_type=='.h5':
- paths = h5py.File(rollout_path, 'r')
- elif output_type=='.pickle':
- paths = pickle.load(open(rollout_path, 'rb'))
- else:
- raise TypeError("Unknown path format. Check file")
+ paths = resolve_path_handle(path_handle)
# Run through all trajs in the paths
- for i_path, path in enumerate(paths):
+ for i_path, (path_name, path) in enumerate(paths.items()):
- if output_type=='.h5':
- data = paths[path]['data']
+ if 'data' in path.keys():
+ data = path['data']
path_horizon = data['time'].shape[0]
else:
- data = path['env_infos']['obs_dict']
+ obs_dict = path['env_infos']['obs_dict']
+ data = {key: obs_dict[key] for key in obs_dict.keys()}
+ if 'visual_dict' in path['env_infos'].keys():
+ visual_dict = path['env_infos']['visual_dict']
+ data.update({key: visual_dict[key] for key in visual_dict.keys()})
path_horizon = path['env_infos']['time'].shape[0]
# find full key name
@@ -332,16 +494,17 @@ def path2dataset(path:dict, config_path=None)->dict:
# Print h5 schema
-def print_h5_schema(obj):
- "Recursively find all keys in an h5py.Group."
- keys = (obj.name,)
- if isinstance(obj, h5py.Group):
+def print_h5_schema(obj, name="/"):
+ "Recursively find all keys in an h5py.Group or a dict-like path object."
+ keys = (getattr(obj, 'name', name),)
+ if isinstance(obj, h5py.Group) or isinstance(obj, dict):
for key, value in obj.items():
- if isinstance(value, h5py.Group):
- keys = keys + print_h5_schema(value)
+ child_name = getattr(value, 'name', '{}/{}'.format(name, key))
+ if isinstance(value, (h5py.Group, dict)):
+ keys = keys + print_h5_schema(value, name=child_name)
else:
- print("\t", "{0:35}".format(value.name), value)
- keys = keys + (value.name,)
+ print("\t", "{0:35}".format(child_name), value)
+ keys = keys + (child_name,)
return keys
@@ -439,8 +602,9 @@ def pickle2h5(rollout_path, output_dir=None, verify_output=False, h5_format:str=
"""
@click.command(help=DESC)
@click.option('-u', '--util', type=click.Choice(['plot_horizon', 'plot', 'render', 'pickle2h5', 'h5schema']), help='pick utility', required=True)
-@click.option('-p', '--path', type=click.Path(exists=True), help='absolute path of the rollout (h5/pickle)', default=None)
-@click.option('-e', '--env', type=str, help='Env name', default=None)
+@click.option('-p', '--path', type=click.Path(exists=True), help='path_handle: absolute path of a rollout file (h5/pickle), or a directory containing rollout files', default=None)
+@click.option('-e', '--env', type=str, help='env_handle: Env name to be created via gym.make (used by plot/plot_horizon)', default=None)
+@click.option('-ea', '--env_args', type=str, default=None, help=('env args. E.g. --env_args "{\'is_hardware\':True}"'))
@click.option('-on', '--output_name', type=str, default=None, help=('Output name'))
@click.option('-od', '--output_dir', type=str, default=None, help=('Directory to save the outputs'))
@click.option('-vo', '--verify_output', type=bool, default=False, help=('Verify the saved file'))
@@ -450,22 +614,19 @@ def pickle2h5(rollout_path, output_dir=None, verify_output=False, h5_format:str=
@click.option('-cn', '--cam_names', multiple=True, help='camera to render. Eg: left, right, top, Franka_wrist', default=["left", "top", "right", "wrist"])
@click.option('-ac', '--add_config', help='Add extra infos to config using as json', default=None)
@click.option('-mp', '--max_paths', type=int, help='maximum number of paths to process', default=1e6)
-def util_path_cli(util, path, env, output_name, output_dir, verify_output, render_format, cam_names, h5_format, compress_path, add_config, max_paths):
+def util_path_cli(util, path, env, env_args, output_name, output_dir, verify_output, render_format, cam_names, h5_format, compress_path, add_config, max_paths):
if util=='plot_horizon':
- fileName_prefix = os.join(output_dir, output_name)
- plot_horizon(path, env, fileName_prefix)
+ plot_horizon(path, env, output_dir=output_dir, output_name=output_name or '', env_args=env_args)
elif util=='plot':
- fileName_prefix = os.join(output_dir, output_name)
- plot(path, env, fileName_prefix)
+ plot(path, env, output_dir=output_dir, output_name=output_name or '', env_args=env_args)
elif util=='render':
- render(rollout_path=path, render_format=render_format, cam_names=cam_names)
+ render(path_handle=path, render_format=render_format, cam_names=cam_names, output_dir=output_dir, output_name=output_name)
elif util=='pickle2h5':
pickle2h5(rollout_path=path, output_dir=output_dir, verify_output=verify_output, h5_format=h5_format, compress_path=compress_path, config_path=add_config, max_paths=max_paths)
elif util=='h5schema':
- with h5py.File(path, "r") as h5file:
- print("Printing schema read from output: ", path)
- keys = print_h5_schema(h5file)
+ print("Printing schema for: ", path)
+ keys = print_h5_schema(resolve_path_handle(path))
else:
raise TypeError("Unknown utility requested")
diff --git a/robohive/utils/pointcloud_utils.py b/robohive/utils/pointcloud_utils.py
new file mode 100644
index 00000000..7ada4c33
--- /dev/null
+++ b/robohive/utils/pointcloud_utils.py
@@ -0,0 +1,108 @@
+import numpy as np
+from robohive.utils.quat_math import mulQuat, euler2quat, quat2mat
+
+
+# ------ MuJoCo specific functions ------
+
+def get_point_cloud(env, depth, camera_name):
+ # Make sure the depth values are in meters. If the depth comes
+ # from robohive, it is already in meters. If it directly comes
+ # from mujoco, you need to use the convert_depth function below.
+ # Output is flattened. Each row is a point in 3D space.
+ fovy = env.sim.model.cam_fovy[env.sim.model.camera_name2id(camera_name)]
+ K = get_intrinsics(fovy, depth.shape[0], depth.shape[1])
+ pc = depth2xyz(depth, K)
+ pc = pc.reshape(-1, 3)
+
+ transform = get_extrinsics(env, camera_name=camera_name)
+ new_pc = np.ones((pc.shape[0], 4))
+ new_pc[:, :3] = pc
+ new_pc = (transform @ new_pc.transpose()).transpose()
+ return new_pc[:, :-1]
+
+
+def convert_depth(env, depth):
+ # Convert raw depth values into meters
+ # Check this as well: https://github.com/deepmind/dm_control/blob/master/dm_control/mujoco/engine.py#L734
+ extent = env.sim.model.stat.extent
+ near = env.sim.model.vis.map.znear * extent
+ far = env.sim.model.vis.map.zfar * extent
+ depth_m = depth * 2 - 1
+ depth_m = (2 * near * far) / (far + near - depth_m * (far - near))
+ return depth_m
+
+
+def get_extrinsics(env, camera_name):
+ # Transformation from camera frame to world frame
+ cam_id = env.sim.model.camera_name2id(camera_name)
+ cam_pos = env.sim.model.cam_pos[cam_id]
+ cam_quat = env.sim.model.cam_quat[cam_id]
+ cam_quat = mulQuat(cam_quat, euler2quat([np.pi, 0, 0]))
+ return get_transformation_matrix(cam_pos, cam_quat)
+
+
+def get_transformation_matrix(pos, quat):
+ # Convert the pose from MuJoCo format to a 4x4 transformation matrix
+ arr = np.identity(4)
+ arr[:3, :3] = quat2mat(quat)
+ arr[:3, 3] = pos
+ return arr
+
+
+# ------ General functions ------
+
+def get_intrinsics(fovy, img_width, img_height):
+ # Get the camera intrinsics matrix
+ aspect = float(img_width) / img_height
+ fovx = 2 * np.arctan(np.tan(np.deg2rad(fovy) * 0.5) * aspect)
+ fovx = np.rad2deg(fovx)
+ cx = img_width / 2.
+ cy = img_height / 2.
+ fx = cx / np.tan(np.deg2rad(fovx / 2.))
+ fy = cy / np.tan(np.deg2rad(fovy / 2.))
+ K = np.zeros((3,3), dtype=np.float64)
+ K[2][2] = 1
+ K[0][0] = fx
+ K[1][1] = fy
+ K[0][2] = cx
+ K[1][2] = cy
+ return K
+
+
+def depth2xyz(depth, cam_K):
+ # Convert depth image to point cloud
+ h, w = depth.shape
+ ymap, xmap = np.meshgrid(np.arange(w), np.arange(h))
+
+ x = ymap
+ y = xmap
+ z = depth
+
+ x = (x - cam_K[0,2]) * z / cam_K[0,0]
+ y = (y - cam_K[1,2]) * z / cam_K[1,1]
+
+ xyz = np.stack([x, y, z], axis=2)
+ return xyz
+
+
+def visualize_point_cloud_from_nparray(d, c=None, vis_coordinate=False):
+ # Visualize a point cloud using open3d
+ if c is not None:
+ if len(c.shape) == 3:
+ c = c.reshape(-1, 3)
+ if c.max() > 1:
+ c = c.astype(np.float64)/256
+
+ import open3d as o3d
+ pcd = o3d.geometry.PointCloud()
+ pcd.points = o3d.utility.Vector3dVector(d)
+ if c is not None:
+ pcd.colors = o3d.utility.Vector3dVector(c)
+
+ if vis_coordinate:
+ # Visualize coordinate frame
+ mesh = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5)
+ o3d.visualization.draw_geometries([mesh, pcd])
+ else:
+ o3d.visualization.draw_geometries([pcd])
+
diff --git a/robohive/utils/prompt_utils.py b/robohive/utils/prompt_utils.py
index 9d59d632..5d1752f7 100644
--- a/robohive/utils/prompt_utils.py
+++ b/robohive/utils/prompt_utils.py
@@ -7,7 +7,7 @@
"""
Utility script to help with information verbosity produced by RoboHive
-To control verbosity set env variable ROBOHIVE_VERBOSITY=ALL/INFO/(WARN)/ERROR/ONCE/ALWAYS
+To control verbosity set env variable ROBOHIVE_VERBOSITY=ALL/INFO/(WARN)/ERROR/ONCE/ALWAYS/SILENT
"""
from termcolor import cprint
@@ -19,9 +19,9 @@
class Prompt(enum.IntEnum):
"""Prompt verbosity types"""
ALL = 0 # print everything (lowest priority)
- INFO = 1
- WARN = 2
- ERROR = 3
+ INFO = 1 # useful info
+ WARN = 2 # warnings (default)
+ ERROR = 3 # errors
ONCE = 4 # print: once and higher
ALWAYS = 5 # print: only always (highest priority)
SILENT = 6 # Supress all prints
@@ -33,7 +33,7 @@ class Prompt(enum.IntEnum):
# Infer verbose mode to be used
VERBOSE_MODE = os.getenv('ROBOHIVE_VERBOSITY')
-if VERBOSE_MODE==None:
+if VERBOSE_MODE is None:
VERBOSE_MODE = Prompt.WARN
else:
VERBOSE_MODE = VERBOSE_MODE.upper()
diff --git a/robohive/utils/tensor_utils.py b/robohive/utils/tensor_utils.py
index 5f54d284..6de21549 100644
--- a/robohive/utils/tensor_utils.py
+++ b/robohive/utils/tensor_utils.py
@@ -1,6 +1,4 @@
-# Source: https://github.dev/aravindr93/mjrl/tree/master/mjrl
-import operator
-
+# Adapted from Source: https://github.com/aravindr93/mjrl/tree/master/mjrl
import numpy as np
diff --git a/setup.py b/setup.py
index b19980c0..88c5a0aa 100644
--- a/setup.py
+++ b/setup.py
@@ -19,74 +19,84 @@
(2) OSX: brew install ffmpeg"""
raise ModuleNotFoundError(help)
-if sys.version_info.major < 3 or (sys.version_info.major == 3 and sys.version_info.minor < 8):
- print("This library requires Python 3.8 or higher, but you are running "
- "Python {}.{}. The installation will likely fail.".format(sys.version_info.major, sys.version_info.minor))
+if sys.version_info.major < 3 or (
+ sys.version_info.major == 3 and sys.version_info.minor < 8
+):
+ print(
+ "This library requires Python 3.8 or higher, but you are running "
+ "Python {}.{}. The installation will likely fail.".format(
+ sys.version_info.major, sys.version_info.minor
+ )
+ )
+
def read(fname):
return open(os.path.join(os.path.dirname(__file__), fname)).read()
+
def package_files(directory):
paths = []
- for (path, directories, filenames) in os.walk(directory):
+ for path, directories, filenames in os.walk(directory):
for filename in filenames:
- paths.append(os.path.join('..', path, filename))
+ paths.append(os.path.join("..", path, filename))
return paths
-extra_files = package_files('robohive')
+
+extra_files = package_files("robohive")
setup(
- name='robohive',
- version='0.7.0',
- license='Apache 2.0',
+ name="robohive",
+ version="0.7.0",
+ license="Apache 2.0",
packages=find_packages(),
- package_data={"": extra_files+['../robohive_init.py']},
+ package_data={"": extra_files + ["../robohive_init.py"]},
include_package_data=True,
- description='A Unified Framework for Robot Learning',
- long_description=read('README.md'),
+ description="A Unified Framework for Robot Learning",
+ long_description=read("README.md"),
long_description_content_type="text/markdown",
- url='https://github.com/vikashplus/robohive.git',
- author='Vikash Kumar',
+ url="https://github.com/vikashplus/robohive.git",
+ author="Vikash Kumar",
author_email="vikahsplus@gmail.com",
install_requires=[
- 'click',
+ "click",
# 'gym==0.13', # default to this stable point if caught in gym issues.
- 'gymnasium==0.29.1',
- 'mujoco==3.1.3',
- 'dm-control==1.0.16',
- 'termcolor',
- 'sk-video',
- 'flatten_dict',
- 'matplotlib',
- 'ffmpeg',
- 'absl-py',
- 'torch',
- 'h5py==3.7.0',
- 'pink-noise-rl',
- 'gitpython'
+ "gymnasium>=0.29.1",
+ "mujoco==3.3.3",
+ "numpy>=2",
+ "dm-control==1.0.31",
+ "termcolor",
+ "sk-video",
+ "flatten_dict",
+ "matplotlib",
+ "ffmpeg",
+ "absl-py",
+ "torch",
+ "h5py>=3.11.0",
+ "pink-noise-rl",
+ "gitpython",
],
extras_require={
- # To use mujoco bindings, run (pip install -e ".[mujoco]") and set sim_backend=MUJOCO
- 'mujoco_py':[
- 'free-mujoco-py',
+ # To use mujoco bindings, run (pip install -e ".[mujoco]") and set sim_backend=MUJOCO
+ "mujoco_py": [
+ "free-mujoco-py",
+ ],
+ # To use hardware dependencies, run (pip install -e ".[a0]") and follow install instructions inside robot
+ "a0": [
+ "pycapnp>=1.1.1",
+ "alephzero", # real_sense subscribers dependency
],
- # To use hardware dependencies, run (pip install -e ".[a0]") and follow install instructions inside robot
- 'a0': [
- 'pycapnp>=1.1.1',
- 'alephzero', # real_sense subscribers dependency
+ "encoder": [
+ "torchvision",
+ # Unlike pypi, Git dependencies can be directly installed in editable mode.
+ # To use r3m/vc encoders, uncomment below and run (pip install -e ".[encoder]")
+ # 'r3m @ git+https://github.com/facebookresearch/r3m.git',
+ # 'vc_models @ git+https://github.com/facebookresearch/eai-vc.git@9958b278666bcbde193d665cc0df9ccddcdb8a5a#egg=vc_models&subdirectory=vc_models',
],
- 'encoder':[
- 'torchvision',
- # Unlike pypi, Git dependencies can be directly installed in editable mode.
- # To use r3m/vc encoders, uncomment below and run (pip install -e ".[encoder]")
- # 'r3m @ git+https://github.com/facebookresearch/r3m.git',
- # 'vc_models @ git+https://github.com/facebookresearch/eai-vc.git@9958b278666bcbde193d665cc0df9ccddcdb8a5a#egg=vc_models&subdirectory=vc_models',
- ]
},
entry_points={
- 'console_scripts': [
- 'robohive_init = robohive_init:fetch_simhive',
- 'robohive_clean = robohive_init:clean_simhive',
+ "console_scripts": [
+ "robohive_init = robohive_init:fetch_simhive",
+ "robohive_clean = robohive_init:clean_simhive",
],
},
)
diff --git a/setup/env.yaml b/setup/env.yaml
index 1bb1eefa..0e3bffe2 100644
--- a/setup/env.yaml
+++ b/setup/env.yaml
@@ -16,8 +16,9 @@ dependencies:
- pip
- pip:
- click
- - gym==0.13
- - mujoco==2.3.3
+ - gymnasium==0.29.1
+ - mujoco==3.3.3
+ - dm-control==1.0.31
- mujoco-py<2.2,>=2.1
- termcolor
- sk-video
@@ -27,5 +28,5 @@ dependencies:
- absl-py
- pycapnp==1.1.0
- r3m @ git+https://github.com/facebookresearch/r3m.git
- - h5py==3.7.0
+ - h5py>=3.11.0
- alephzero # real_sense subscribers dependency