From 1c8c448ed86aced2e160e322e7067c2d6a3dfac5 Mon Sep 17 00:00:00 2001 From: Wenxuan Zhou Date: Thu, 20 Apr 2023 00:03:16 -0400 Subject: [PATCH 01/36] Create pointcloud_utils.py --- robohive/utils/pointcloud_utils.py | 112 +++++++++++++++++++++++++++++ 1 file changed, 112 insertions(+) create mode 100644 robohive/utils/pointcloud_utils.py diff --git a/robohive/utils/pointcloud_utils.py b/robohive/utils/pointcloud_utils.py new file mode 100644 index 00000000..6d7d1d3f --- /dev/null +++ b/robohive/utils/pointcloud_utils.py @@ -0,0 +1,112 @@ +import numpy as np +from utils.rotations import * +import cv2 + + +# -------------------- Generic ---------------------------- +def get_intrinsics(fovy, img_width, img_height): + # fovy = self.sim.model.cam_fovy[cam_no] + 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): + 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): + 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: + mesh = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.5) + o3d.visualization.draw_geometries([mesh, pcd]) + else: + o3d.visualization.draw_geometries([pcd]) + + +# -------------------- MuJoCo Specific ---------------------------- +def get_transformation_matrix(pos, quat): + arr = np.identity(4) + arr[:3, :3] = quat2mat(quat) + arr[:3, 3] = pos + return arr + + +def get_transformation(env, camera_name=None): + if camera_name is None: + camera_name = env.camera_names[0] + 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 = quat_mul(cam_quat, euler2quat([np.pi, 0, 0])) + return get_transformation_matrix(cam_pos, cam_quat) + + +def convert_depth(env, depth): + # Convert depth into meter + 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)) + # Check this as well: https://github.com/deepmind/dm_control/blob/master/dm_control/mujoco/engine.py#L734 + return depth_m + + +def get_object_point_cloud(env, depth, img): + depth = convert_depth(env, depth) + full_pc = get_point_cloud(env, depth) + obj_mask = get_obj_mask(img) + pc = full_pc[obj_mask.reshape(-1),:] + return pc + + +def get_point_cloud(env, depth, camera_name=None): + # make sure to convert the raw depth image from MuJoCo using convert_depth + # output is flattened + if camera_name is None: + camera_name = env.camera_names[0] + 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_transformation(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] + From eb8faa5ce5e85de82d13702e6e595e3600f6b428 Mon Sep 17 00:00:00 2001 From: Wenxuan Zhou Date: Sun, 12 Nov 2023 16:23:42 -0800 Subject: [PATCH 02/36] point cloud utility functions with an example --- .../tutorials/3_get_obs_proprio_extero.ipynb | 50 ++++++++ robohive/utils/pointcloud_utils.py | 112 +++++++++--------- 2 files changed, 104 insertions(+), 58 deletions(-) diff --git a/robohive/tutorials/3_get_obs_proprio_extero.ipynb b/robohive/tutorials/3_get_obs_proprio_extero.ipynb index d39e8436..4d8d93c2 100644 --- a/robohive/tutorials/3_get_obs_proprio_extero.ipynb +++ b/robohive/tutorials/3_get_obs_proprio_extero.ipynb @@ -135,6 +135,56 @@ "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", + "depth = obs['d:vil_camera:224x224:2d'][0][-1::-1] # for some reason the depth image is upside down?\n", + "plt.imshow(depth)" + ] + }, + { + "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][-1::-1], '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", diff --git a/robohive/utils/pointcloud_utils.py b/robohive/utils/pointcloud_utils.py index 6d7d1d3f..7ada4c33 100644 --- a/robohive/utils/pointcloud_utils.py +++ b/robohive/utils/pointcloud_utils.py @@ -1,11 +1,58 @@ import numpy as np -from utils.rotations import * -import cv2 +from robohive.utils.quat_math import mulQuat, euler2quat, quat2mat -# -------------------- Generic ---------------------------- +# ------ 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): - # fovy = self.sim.model.cam_fovy[cam_no] + # 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) @@ -23,6 +70,7 @@ def get_intrinsics(fovy, img_width, img_height): 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)) @@ -38,6 +86,7 @@ def depth2xyz(depth, cam_K): 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) @@ -51,62 +100,9 @@ def visualize_point_cloud_from_nparray(d, c=None, vis_coordinate=False): 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]) - -# -------------------- MuJoCo Specific ---------------------------- -def get_transformation_matrix(pos, quat): - arr = np.identity(4) - arr[:3, :3] = quat2mat(quat) - arr[:3, 3] = pos - return arr - - -def get_transformation(env, camera_name=None): - if camera_name is None: - camera_name = env.camera_names[0] - 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 = quat_mul(cam_quat, euler2quat([np.pi, 0, 0])) - return get_transformation_matrix(cam_pos, cam_quat) - - -def convert_depth(env, depth): - # Convert depth into meter - 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)) - # Check this as well: https://github.com/deepmind/dm_control/blob/master/dm_control/mujoco/engine.py#L734 - return depth_m - - -def get_object_point_cloud(env, depth, img): - depth = convert_depth(env, depth) - full_pc = get_point_cloud(env, depth) - obj_mask = get_obj_mask(img) - pc = full_pc[obj_mask.reshape(-1),:] - return pc - - -def get_point_cloud(env, depth, camera_name=None): - # make sure to convert the raw depth image from MuJoCo using convert_depth - # output is flattened - if camera_name is None: - camera_name = env.camera_names[0] - 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_transformation(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] - From 424d2bedbccf55653a5c7f59f9a80c3c5b37c14f Mon Sep 17 00:00:00 2001 From: Wenxuan Zhou Date: Sat, 25 Nov 2023 23:04:38 -0800 Subject: [PATCH 03/36] flip back depth image --- robohive/renderer/mj_renderer.py | 2 +- robohive/tutorials/3_get_obs_proprio_extero.ipynb | 12 ++++++++---- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/robohive/renderer/mj_renderer.py b/robohive/renderer/mj_renderer.py index 74daf8b5..dae39e63 100644 --- a/robohive/renderer/mj_renderer.py +++ b/robohive/renderer/mj_renderer.py @@ -114,7 +114,7 @@ def render_offscreen(self, self._renderer.enable_depth_rendering() self._renderer.update_scene(self._sim.data.ptr, camera=camera_id, scene_option=self._scene_option) dpt_arr = self._renderer.render() - dpt_arr = dpt_arr[::-1, :] + # dpt_arr = dpt_arr[::-1, :] self._renderer.disable_depth_rendering() if segmentation: self._renderer.enable_segmentation_rendering() diff --git a/robohive/tutorials/3_get_obs_proprio_extero.ipynb b/robohive/tutorials/3_get_obs_proprio_extero.ipynb index 4d8d93c2..ad9614f1 100644 --- a/robohive/tutorials/3_get_obs_proprio_extero.ipynb +++ b/robohive/tutorials/3_get_obs_proprio_extero.ipynb @@ -154,8 +154,12 @@ "source": [ "import matplotlib.pyplot as plt\n", "obs = env.get_visuals(visual_keys=['rgb:vil_camera:224x224:2d', 'd:vil_camera:224x224:2d'])\n", - "depth = obs['d:vil_camera:224x224:2d'][0][-1::-1] # for some reason the depth image is upside down?\n", - "plt.imshow(depth)" + "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()" ] }, { @@ -172,7 +176,7 @@ "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][-1::-1], 'vil_camera')\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", @@ -252,7 +256,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.8.16" + "version": "3.10.13" }, "orig_nbformat": 4 }, From ae026821ce3483a5455cfdfe29a46b6719013e36 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Sun, 21 Apr 2024 15:11:16 +0530 Subject: [PATCH 04/36] DOCS: Improved information on different verbosity patterns --- robohive/utils/prompt_utils.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/robohive/utils/prompt_utils.py b/robohive/utils/prompt_utils.py index 9d59d632..0bc06f5f 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 From f039625d1cd63c0ef8cd3d654f51ca01993759c5 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Sun, 21 Apr 2024 15:12:33 +0530 Subject: [PATCH 05/36] DOCS: Udpating the docs to clearly outline the models edits --- robohive/simhive/franka_sim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 63b44ad3227bcfc4799ae608cefbe04976539773 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Sun, 21 Apr 2024 15:15:08 +0530 Subject: [PATCH 06/36] BUGFIX: Envs re-defininng their resets were missing the gym/gymnasium compliance. Envs now overwrite the correct _reset function. Gym compliance is now preserved across the defaulting envs --- robohive/envs/hands/baoding_v1.py | 2 +- robohive/envs/hands/door_v1.py | 2 +- robohive/envs/hands/hammer_v1.py | 2 +- robohive/envs/hands/pen_v1.py | 2 +- robohive/envs/hands/relocate_v1.py | 2 +- robohive/envs/multi_task/README.md | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/robohive/envs/hands/baoding_v1.py b/robohive/envs/hands/baoding_v1.py index 1b7cafec..ce68419e 100644 --- a/robohive/envs/hands/baoding_v1.py +++ b/robohive/envs/hands/baoding_v1.py @@ -256,7 +256,7 @@ def get_reward_dict(self, obs_dict): rwd_dict['dense'] = np.sum([wt*rwd_dict[key] for key, wt in self.rwd_keys_wt.items()], axis=0) return rwd_dict - def reset(self, reset_pose=None, reset_vel=None, reset_goal=None, time_period=6, **kwargs): + def _reset(self, reset_pose=None, reset_vel=None, reset_goal=None, time_period=6, **kwargs): # reset counters self.counter=0 diff --git a/robohive/envs/hands/door_v1.py b/robohive/envs/hands/door_v1.py index 8cd9f5e1..d0f80b9a 100644 --- a/robohive/envs/hands/door_v1.py +++ b/robohive/envs/hands/door_v1.py @@ -96,7 +96,7 @@ def get_reward_dict(self, obs_dict): return rwd_dict - def reset(self, reset_qpos=None, reset_qvel=None, **kwargs): + def _reset(self, reset_qpos=None, reset_qvel=None, **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 diff --git a/robohive/envs/hands/hammer_v1.py b/robohive/envs/hands/hammer_v1.py index 3ef29033..9ce25020 100644 --- a/robohive/envs/hands/hammer_v1.py +++ b/robohive/envs/hands/hammer_v1.py @@ -120,7 +120,7 @@ def get_obs_dict(self, sim): return obs_dict - def reset(self, reset_qpos=None, reset_qvel=None, **kwargs): + def _reset(self, reset_qpos=None, reset_qvel=None, **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 diff --git a/robohive/envs/hands/pen_v1.py b/robohive/envs/hands/pen_v1.py index 54539d34..98172bbc 100644 --- a/robohive/envs/hands/pen_v1.py +++ b/robohive/envs/hands/pen_v1.py @@ -108,7 +108,7 @@ def get_reward_dict(self, obs_dict): return rwd_dict - def reset(self, reset_qpos=None, reset_qvel=None, **kwargs): + def _reset(self, reset_qpos=None, reset_qvel=None, **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 diff --git a/robohive/envs/hands/relocate_v1.py b/robohive/envs/hands/relocate_v1.py index 88e273e2..7b8da68c 100644 --- a/robohive/envs/hands/relocate_v1.py +++ b/robohive/envs/hands/relocate_v1.py @@ -136,7 +136,7 @@ def get_obs_dict(self, sim): return obs_dict - def reset(self, reset_qpos=None, reset_qvel=None, **kwargs): + def _reset(self, reset_qpos=None, reset_qvel=None, **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 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 From a9d227eccfb3e8c6373ff69d76487b49a5aedc9f Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Wed, 1 May 2024 00:54:00 +0530 Subject: [PATCH 07/36] BUGFIX: Fixing return type issues of envs with custom reset - gym/gymnasium compatibility doesn't port over when custom reset is provided - we are now recommending a call to super().reset(**kwargs) to get the appropritae return type ensuring compatibility --- robohive/envs/env_base.py | 4 ++-- robohive/envs/hands/baoding_v1.py | 8 +++----- robohive/envs/hands/door_v1.py | 8 ++------ robohive/envs/hands/hammer_v1.py | 9 +++------ robohive/envs/hands/pen_v1.py | 9 ++------- robohive/envs/hands/relocate_v1.py | 10 +++------- robohive/tests/test_envs.py | 18 +++++++++++++++++- 7 files changed, 32 insertions(+), 34 deletions(-) diff --git a/robohive/envs/env_base.py b/robohive/envs/env_base.py index 1072b132..efc2d540 100644 --- a/robohive/envs/env_base.py +++ b/robohive/envs/env_base.py @@ -495,8 +495,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 diff --git a/robohive/envs/hands/baoding_v1.py b/robohive/envs/hands/baoding_v1.py index ce68419e..1b6d97da 100644 --- a/robohive/envs/hands/baoding_v1.py +++ b/robohive/envs/hands/baoding_v1.py @@ -256,7 +256,7 @@ def get_reward_dict(self, obs_dict): rwd_dict['dense'] = np.sum([wt*rwd_dict[key] for key, wt in self.rwd_keys_wt.items()], axis=0) return rwd_dict - def _reset(self, reset_pose=None, reset_vel=None, reset_goal=None, time_period=6, **kwargs): + def reset(self, reset_pose=None, reset_vel=None, reset_goal=None, time_period=6, **kwargs): # reset counters self.counter=0 @@ -264,8 +264,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 +325,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 d0f80b9a..4e756e9f 100644 --- a/robohive/envs/hands/door_v1.py +++ b/robohive/envs/hands/door_v1.py @@ -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 9ce25020..cbdccb92 100644 --- a/robohive/envs/hands/hammer_v1.py +++ b/robohive/envs/hands/hammer_v1.py @@ -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 98172bbc..f38c1c1d 100644 --- a/robohive/envs/hands/pen_v1.py +++ b/robohive/envs/hands/pen_v1.py @@ -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 7b8da68c..be2d8ea0 100644 --- a/robohive/envs/hands/relocate_v1.py +++ b/robohive/envs/hands/relocate_v1.py @@ -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/tests/test_envs.py b/robohive/tests/test_envs.py index 78137191..cb01fcae 100644 --- a/robohive/tests/test_envs.py +++ b/robohive/tests/test_envs.py @@ -8,6 +8,7 @@ import unittest from robohive.utils import gym +from robohive.utils.implement_for import implement_for import numpy as np import pickle import copy @@ -66,7 +67,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,6 +104,20 @@ 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: From 88110c95a0a67e1e99eccb9b756cc851676ad930 Mon Sep 17 00:00:00 2001 From: Vincent Moens Date: Mon, 24 Jun 2024 10:24:38 +0100 Subject: [PATCH 08/36] np.product -> np.prod for np 2.0 compatibility --- robohive/envs/multi_task/multi_task_base_v1.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robohive/envs/multi_task/multi_task_base_v1.py b/robohive/envs/multi_task/multi_task_base_v1.py index a18adda4..4122593e 100644 --- a/robohive/envs/multi_task/multi_task_base_v1.py +++ b/robohive/envs/multi_task/multi_task_base_v1.py @@ -200,9 +200,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 +272,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 From 4271114ccad44a99b90bdf7cb55b3e07c3778881 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Thu, 24 Oct 2024 15:36:24 -0400 Subject: [PATCH 09/36] UPDATE:Upgrading the render_cams tutorial to official mujoco binding --- robohive/tutorials/render_cams.py | 42 +++++++++++++++---------------- 1 file changed, 20 insertions(+), 22 deletions(-) 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") From 28d07775bbb1f4c6a30f8a8891084e7c98757fbc Mon Sep 17 00:00:00 2001 From: Vittorio-Caggiano Date: Thu, 24 Oct 2024 17:53:02 -0400 Subject: [PATCH 10/36] bump pyh5 and pin numpy --- setup.py | 100 ++++++++++++++++++++++++++++++------------------------- 1 file changed, 55 insertions(+), 45 deletions(-) diff --git a/setup.py b/setup.py index b19980c0..8e71326f 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.1.3", + "numpy>=2.*", + "dm-control==1.0.16", + "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", ], }, ) From 2d4a8657bc6577912ae0487d6e2c79191b29f830 Mon Sep 17 00:00:00 2001 From: Vittorio-Caggiano Date: Thu, 24 Oct 2024 17:55:42 -0400 Subject: [PATCH 11/36] pin numpy --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 8e71326f..7af1f1d7 100644 --- a/setup.py +++ b/setup.py @@ -62,7 +62,7 @@ def package_files(directory): # 'gym==0.13', # default to this stable point if caught in gym issues. "gymnasium==0.29.1", "mujoco==3.1.3", - "numpy>=2.*", + "numpy>=2", "dm-control==1.0.16", "termcolor", "sk-video", From 9a72ad23660d81d4558f789ff9a7cd9a2f56fc2d Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Sun, 17 Nov 2024 14:54:41 -0500 Subject: [PATCH 12/36] DataConsistency and 2D plotting support 1. FEATURE: Explicitely marks the closure of the trace 2. BUGFIX: Introduces create_datum. create_dataset is updated and is now reserved for adding full datasets (shouldn't be edited later). If appending is needed first create_datum() and then follow it with repeated calls to append_datum(). 3. BUGFIX: verify_type is stricter now. It goes beyond checking for root dtypes. It thoroughly checks for type, shapes, keys - even for nested structures. This implies that dict{datum} also needs to maintain its internal keys to support append leading to further consistency 4. FEATURE: Much like rendering of rgb datasets, now trace supports plotting of numerical datasets using a list(grp_keys)_ng and list(list(dataset_keys))_ng 5. Adds multiple tests to check for data consistency and plotting consistency --- robohive/logger/grouped_datasets.py | 309 ++++++++++++++++++++++++++-- robohive/tests/test_logger.py | 28 ++- robohive/utils/prompt_utils.py | 2 +- robohive/utils/tensor_utils.py | 4 +- 4 files changed, 320 insertions(+), 23 deletions(-) diff --git a/robohive/logger/grouped_datasets.py b/robohive/logger/grouped_datasets.py index 3450af9e..ac00cc63 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,114 @@ 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}) + 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"{grp_key}/{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() + 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) + # Render frames/videos def render(self, output_dir, output_format, groups:list, datasets:list, input_fps:int=25): @@ -249,10 +381,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 +469,8 @@ def close(self, if verify_length: self.verify_len() + self.closed = True + # Save def save(self, @@ -394,31 +528,105 @@ def load(trace_path, trace_type=TraceType.UNSET): 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 +640,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/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/utils/prompt_utils.py b/robohive/utils/prompt_utils.py index 0bc06f5f..5d1752f7 100644 --- a/robohive/utils/prompt_utils.py +++ b/robohive/utils/prompt_utils.py @@ -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 From 29f73461b461fb7e703d56bcadfb1fdf4decc995 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Mon, 25 Nov 2024 23:10:16 -0500 Subject: [PATCH 13/36] BUGFIX: Mark the trace as closed post load --- robohive/logger/grouped_datasets.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/robohive/logger/grouped_datasets.py b/robohive/logger/grouped_datasets.py index ac00cc63..0ba24e65 100644 --- a/robohive/logger/grouped_datasets.py +++ b/robohive/logger/grouped_datasets.py @@ -193,6 +193,7 @@ def plot(self, output_dir, output_format, groups:list, datasets:list, x_dataset: # mpl.use('Agg') import matplotlib.pyplot as plt plt.rcParams.update({'font.size': 5}) + h_fig = plt.figure(self.name) plt.clf() # Resolve groups @@ -257,7 +258,10 @@ def plot(self, output_dir, output_format, groups:list, datasets:list, x_dataset: # h_axis.set_prop_cycle(None) if x_dataset in grp_val.keys(): - plt.plot(grp_val[x_dataset], ds_val, label=f"{grp_key}/{ds_key}", marker='*') + 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='*') @@ -268,10 +272,12 @@ def plot(self, output_dir, output_format, groups:list, datasets:list, x_dataset: # 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) + # plt.savefig(file_name) print("saved ", file_name) + return h_fig # Render frames/videos @@ -525,6 +531,7 @@ 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 From 5625b2dac32449c0a074642ee9261fd7ef3b0607 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Thu, 7 May 2026 12:58:56 -0400 Subject: [PATCH 14/36] MINOR: Fix broken links --- README.md | 2 +- robohive/robot/README.md | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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/robot/README.md b/robohive/robot/README.md index 7dfa2036..18ac300c 100644 --- a/robohive/robot/README.md +++ b/robohive/robot/README.md @@ -21,7 +21,7 @@ RoboHive provides a unified [base class](hardware_base.py) for all hardware inte RoboHive uses [Polymetis](https://facebookresearch.github.io/fairo/polymetis/) for interfacing with Franka Arms. Please follow the install instructions provided by the Polymetis authors. 2. [Dynamixel](http://www.dynamixel.com/) -RoboHive supports all dynamixel based robots such as [ROBEL](http://roboticsbenchmarks.org/). Please follow driver install instructions [here](https://github.com/vikashplus/dynamixel). +RoboHive supports all dynamixel based robots such as [ROBEL](https://sites.google.com/view/roboticsbenchmarks). Please follow driver install instructions [here](https://github.com/vikashplus/dynamixel). 3. [OptiTrack](https://optitrack.com/) RoboHive supports optitrack motion tracking system. Please follow install instructions [here](https://github.com/vikashplus/OptiTrack) From 1d2989bf96ec0845210225082a80b827d88524c8 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Mon, 11 May 2026 14:05:12 -0400 Subject: [PATCH 15/36] MINOR: Make precision consistent between envs base and obs_dict. This can be further improved by having a global settings of rnumerical precision throughout the repo --- .gitmodules | 5 ++++- robohive/envs/obs_vec_dict.py | 6 ++++-- robohive/simhive/ur_sim | 1 + 3 files changed, 9 insertions(+), 3 deletions(-) create mode 160000 robohive/simhive/ur_sim diff --git a/.gitmodules b/.gitmodules index 4b60597e..544f6dec 100644 --- a/.gitmodules +++ b/.gitmodules @@ -38,4 +38,7 @@ 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 +[submodule "robohive/simhive/ur_sim"] + path = robohive/simhive/ur_sim + url = https://github.com/vikashplus/ur_sim.git diff --git a/robohive/envs/obs_vec_dict.py b/robohive/envs/obs_vec_dict.py index fb79e5e6..4dece6bb 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 @@ -79,7 +81,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/simhive/ur_sim b/robohive/simhive/ur_sim new file mode 160000 index 00000000..ec5f794d --- /dev/null +++ b/robohive/simhive/ur_sim @@ -0,0 +1 @@ +Subproject commit ec5f794d34939c8e9b1e81b9aeed9c2fd04d1fc2 From 66c4a0bed0040b82ee419cc2d2c049e981eea024 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Mon, 11 May 2026 14:06:50 -0400 Subject: [PATCH 16/36] MINOR: Formatting --- robohive/envs/env_base.py | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/robohive/envs/env_base.py b/robohive/envs/env_base.py index efc2d540..543eb96f 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 @@ -183,7 +183,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() From 7eaab0be1b42c9e32337f02f08ba997874316264 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Mon, 11 May 2026 14:25:10 -0400 Subject: [PATCH 17/36] MINOR:Unwarpping to read env_info as per gymnasium suggestions --- robohive/tests/test_envs.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robohive/tests/test_envs.py b/robohive/tests/test_envs.py index cb01fcae..5b6598fb 100644 --- a/robohive/tests/test_envs.py +++ b/robohive/tests/test_envs.py @@ -142,7 +142,7 @@ def check_old_envs(self, module_name, env_names, lite=False, seed=1234): # assert (env.env.get_obs() == env.env.get_obs_vec()).all(), "check vectorized computations" # test env infos - infos = env.env.get_env_infos() + infos = env.unwrapped.get_env_infos() # test step (everything together) observation, _reward, done, _info = env.env.step(np.zeros(env.env.sim.model.nu)) From 3d7b862cf74dbc2a18ad3ef862f0977f52c2fb56 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Tue, 12 May 2026 08:26:58 -0400 Subject: [PATCH 18/36] Removing unused files --- .gitmodules | 3 --- robohive/simhive/ur_sim | 1 - 2 files changed, 4 deletions(-) delete mode 160000 robohive/simhive/ur_sim diff --git a/.gitmodules b/.gitmodules index 544f6dec..a6d2cc96 100644 --- a/.gitmodules +++ b/.gitmodules @@ -39,6 +39,3 @@ [submodule "robohive/simhive/myo_sim"] path = robohive/simhive/myo_sim url = https://github.com/MyoHub/myo_sim.git -[submodule "robohive/simhive/ur_sim"] - path = robohive/simhive/ur_sim - url = https://github.com/vikashplus/ur_sim.git diff --git a/robohive/simhive/ur_sim b/robohive/simhive/ur_sim deleted file mode 160000 index ec5f794d..00000000 --- a/robohive/simhive/ur_sim +++ /dev/null @@ -1 +0,0 @@ -Subproject commit ec5f794d34939c8e9b1e81b9aeed9c2fd04d1fc2 From 4ad3247ef58ecf01d0e596d917ee1f9e64dd26eb Mon Sep 17 00:00:00 2001 From: Andrea Prestia <67275846+andreh1111@users.noreply.github.com> Date: Tue, 12 May 2026 14:13:49 -0400 Subject: [PATCH 19/36] dependency update and code alignment --- robohive/envs/multi_task/utils/parse_demos.py | 4 ++-- robohive/envs/myo/myodm/myodm_v0.py | 2 +- robohive/envs/tcdm/playback_mocap.py | 2 +- robohive/envs/tcdm/track.py | 2 +- robohive/robot/robot.py | 13 ++++++------ robohive/tests/test_envs.py | 21 ++++++++++--------- .../tutorials/3_get_obs_proprio_extero.ipynb | 4 ++-- robohive/tutorials/ee_teleop.py | 15 ++++++------- robohive/tutorials/ee_teleop_oculus.py | 15 ++++++------- setup.py | 4 ++-- setup/env.yaml | 7 ++++--- 11 files changed, 47 insertions(+), 42 deletions(-) diff --git a/robohive/envs/multi_task/utils/parse_demos.py b/robohive/envs/multi_task/utils/parse_demos.py index 65795e3a..6ad26b0f 100644 --- a/robohive/envs/multi_task/utils/parse_demos.py +++ b/robohive/envs/multi_task/utils/parse_demos.py @@ -84,7 +84,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 +127,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/myodm/myodm_v0.py b/robohive/envs/myo/myodm/myodm_v0.py index abd47ea0..4c9ebeeb 100644 --- a/robohive/envs/myo/myodm/myodm_v0.py +++ b/robohive/envs/myo/myodm/myodm_v0.py @@ -285,7 +285,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/tcdm/playback_mocap.py b/robohive/envs/tcdm/playback_mocap.py index 44ce818a..18aa72a5 100644 --- a/robohive/envs/tcdm/playback_mocap.py +++ b/robohive/envs/tcdm/playback_mocap.py @@ -25,7 +25,7 @@ # envMyoSuit=gym.make('MyoHandAirplanePass-v0') envMyoSuit=gym.make(args.sim_name) - physics_myo=envMyoSuit.env.sim + physics_myo=envMyoSuit.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..408b8c38 100644 --- a/robohive/envs/tcdm/track.py +++ b/robohive/envs/tcdm/track.py @@ -273,7 +273,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/robot/robot.py b/robohive/robot/robot.py index 4daa9a5d..aa559712 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -802,8 +802,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 +812,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/tests/test_envs.py b/robohive/tests/test_envs.py index 5b6598fb..998d446c 100644 --- a/robohive/tests/test_envs.py +++ b/robohive/tests/test_envs.py @@ -123,30 +123,31 @@ def check_old_envs(self, module_name, env_names, lite=False, seed=1234): 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.unwrapped.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/tutorials/3_get_obs_proprio_extero.ipynb b/robohive/tutorials/3_get_obs_proprio_extero.ipynb index ad9614f1..166899a3 100644 --- a/robohive/tutorials/3_get_obs_proprio_extero.ipynb +++ b/robohive/tutorials/3_get_obs_proprio_extero.ipynb @@ -213,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", @@ -221,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", 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/setup.py b/setup.py index 7af1f1d7..bbfac246 100644 --- a/setup.py +++ b/setup.py @@ -61,9 +61,9 @@ def package_files(directory): "click", # 'gym==0.13', # default to this stable point if caught in gym issues. "gymnasium==0.29.1", - "mujoco==3.1.3", + "mujoco==3.3.3", "numpy>=2", - "dm-control==1.0.16", + "dm-control==1.0.31", "termcolor", "sk-video", "flatten_dict", 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 From b17a67fb291ea4f65060bc6dbf20a999b9bd430b Mon Sep 17 00:00:00 2001 From: Andrea Prestia <67275846+andreh1111@users.noreply.github.com> Date: Tue, 12 May 2026 14:20:55 -0400 Subject: [PATCH 20/36] fix typo --- robohive/envs/tcdm/playback_mocap.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robohive/envs/tcdm/playback_mocap.py b/robohive/envs/tcdm/playback_mocap.py index 18aa72a5..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.unwrapped.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"\" ") From 43b231531bfc22c33200e73c46bc9d45159ef101 Mon Sep 17 00:00:00 2001 From: Andrea Prestia <67275846+andreh1111@users.noreply.github.com> Date: Tue, 12 May 2026 14:30:00 -0400 Subject: [PATCH 21/36] Update python-app.yml --- .github/workflows/python-app.yml | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) 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 From 1dd2a84ee02eb4325a956efd264b06cdce57455b Mon Sep 17 00:00:00 2001 From: Andrea Prestia <67275846+andreh1111@users.noreply.github.com> Date: Tue, 12 May 2026 14:53:30 -0400 Subject: [PATCH 22/36] update actuator_moment usage for mujoco 3.3.3 --- robohive/envs/myo/base_v0.py | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) 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 From 0abc98695b818c9753dcff384a3cd9def85b369f Mon Sep 17 00:00:00 2001 From: Andrea Prestia <67275846+andreh1111@users.noreply.github.com> Date: Tue, 12 May 2026 17:08:54 -0400 Subject: [PATCH 23/36] Update mj_sim_scene.py --- robohive/physics/mj_sim_scene.py | 181 ++++++++++++++++++++++++++++++- 1 file changed, 180 insertions(+), 1 deletion(-) diff --git a/robohive/physics/mj_sim_scene.py b/robohive/physics/mj_sim_scene.py index 6705e8b8..2cce0ad2 100644 --- a/robohive/physics/mj_sim_scene.py +++ b/robohive/physics/mj_sim_scene.py @@ -9,6 +9,9 @@ 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 @@ -24,6 +27,182 @@ 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 +215,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 " Date: Mon, 11 May 2026 17:09:52 +0530 Subject: [PATCH 24/36] fix: gc of render-window causes render-thread to hang --- robohive/envs/env_base.py | 7 +++++++ robohive/renderer/mj_renderer.py | 15 ++++++++++----- 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/robohive/envs/env_base.py b/robohive/envs/env_base.py index 543eb96f..cd7edd19 100644 --- a/robohive/envs/env_base.py +++ b/robohive/envs/env_base.py @@ -524,6 +524,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/renderer/mj_renderer.py b/robohive/renderer/mj_renderer.py index 4e5ce9e2..e979cdf7 100644 --- a/robohive/renderer/mj_renderer.py +++ b/robohive/renderer/mj_renderer.py @@ -32,6 +32,7 @@ def __init__(self, sim): self._renderer = None self._paused = False self._user_exit = False + self._closing = False # viewer callback @@ -68,7 +69,7 @@ def render_to_window(self): def refresh_window(self): """Refreshes the rendered window if one is present.""" - if self._window is None: + if self._window is None or self._closing: return self._window.sync() @@ -152,7 +153,11 @@ def _update_renderer_settings(self, renderer): def close(self): """Cleans up any resources being used by the renderer.""" - if self._window: - self._window.close() - self._window = None - quit() \ No newline at end of file + if self._window and not self._closing: + self._closing = True + try: + if self._window.is_running(): + self._window.close() + except Exception: + pass + self._window = None \ No newline at end of file From fe452dd16391f9cb4d72ae00e40057c632d59482 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ak=E1=B9=A3aya=20=C5=9Ar=C4=ABniv=C4=81san?= Date: Tue, 12 May 2026 12:20:08 +0530 Subject: [PATCH 25/36] parse_demos: add parse_mjl from vive + imports cleanup --- robohive/envs/multi_task/utils/parse_demos.py | 9 +- robohive/utils/parse_mjl.py | 93 +++++++++++++++++++ 2 files changed, 100 insertions(+), 2 deletions(-) create mode 100644 robohive/utils/parse_mjl.py diff --git a/robohive/envs/multi_task/utils/parse_demos.py b/robohive/envs/multi_task/utils/parse_demos.py index 6ad26b0f..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) 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() From b796c74ee9dcaa29ae7602bc3c2d8d1aa31ce46a Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Tue, 23 Jun 2026 23:35:18 -0400 Subject: [PATCH 26/36] FEATURE: Added tendon related sensors and actuators support to the robot class --- robohive/robot/robot.py | 29 +++++++++++++++++++---------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index aa559712..db8b7bd5 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -5,14 +5,17 @@ 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 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.utils.prompt_utils import Prompt, prompt +from robohive.utils.quat_math import quat2euler + np.set_printoptions(precision=4) @@ -175,7 +178,7 @@ def hardware_init(self, robot_config): else: print("ERROR: interface ({}) not found".format(device['interface']['type'])) - raise NotImplemented + raise NotImplementedError return robot_config @@ -215,7 +218,7 @@ def hardware_get_sensors(self): else: print("ERROR: interface ({}) not found".format(device['interface']['type'])) - raise NotImplemented + raise NotImplementedError # calibrate sensors for id, sensor in enumerate(device['sensor']): @@ -332,6 +335,9 @@ def configure_robot(self, sim, config_path): elif sensor_type == mujoco.mjtSensor.mjSENS_JOINTVEL: # mjSENS_JOINTVEL,// scalar joint position (hinge and slide only) sensor['data_type'] = 'qvel' sensor['data_id'] = sim.model.jnt_dofadr[sensor_objid] + elif sensor_type == mujoco.mjtSensor.mjSENS_TENDON: # mjSENS_TENDON // tendon force + sensor['data_type'] = 'ten_length' + sensor['data_id'] = sensor_objid else: quit("ERROR: Sensor {} has unsupported sensor_type: {}".format(sensor['name'],sensor_type)) @@ -344,9 +350,12 @@ def configure_robot(self, sim, config_path): actuator['sim_id'] = sim.model.actuator_name2id(actuator['name']) 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] + 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 @@ -720,7 +729,7 @@ 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() From 49a1e9837997d5b6fa9f8af9b2f28490d713166b Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Tue, 23 Jun 2026 23:45:11 -0400 Subject: [PATCH 27/36] REFACTOR: Registration info is only displayed once --- robohive/envs/arms/__init__.py | 7 +++++-- robohive/envs/claws/__init__.py | 5 ++++- robohive/envs/fm/__init__.py | 7 ++++++- robohive/envs/hands/__init__.py | 8 ++++++-- robohive/envs/multi_task/substeps1/__init__.py | 5 +++-- robohive/envs/multi_task/substeps2/__init__.py | 3 ++- robohive/envs/myo/myobase/__init__.py | 9 ++++++--- robohive/envs/myo/myochallenge/__init__.py | 4 ++++ robohive/envs/quadrupeds/__init__.py | 14 ++++++++++---- robohive/envs/tcdm/__init__.py | 9 ++++++--- robohive/logger/examine_logs.py | 11 ++++++----- robohive/physics/mj_sim_scene.py | 7 ++++--- robohive/tests/test_envs.py | 12 +++++++----- 13 files changed, 69 insertions(+), 32 deletions(-) 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/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/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/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/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/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/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/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/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/logger/examine_logs.py b/robohive/logger/examine_logs.py index 621c3711..99d9eb00 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) diff --git a/robohive/physics/mj_sim_scene.py b/robohive/physics/mj_sim_scene.py index 2cce0ad2..4ddb1b98 100644 --- a/robohive/physics/mj_sim_scene.py +++ b/robohive/physics/mj_sim_scene.py @@ -15,13 +15,14 @@ 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): @@ -298,8 +299,8 @@ def name2id(type_name, name): return obj_id def get_xml(): - from tempfile import TemporaryDirectory import os + from tempfile import TemporaryDirectory with TemporaryDirectory() as td: filename = os.path.join(td, 'model.xml') ret = mjlib.mj_saveLastXML(filename.encode(), model.ptr) diff --git a/robohive/tests/test_envs.py b/robohive/tests/test_envs.py index 998d446c..21bec889 100644 --- a/robohive/tests/test_envs.py +++ b/robohive/tests/test_envs.py @@ -5,16 +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 -from robohive.utils.implement_for import implement_for 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 From 64f23f2e9d048e9790f33d7243dec4904ce42393 Mon Sep 17 00:00:00 2001 From: Andrea Prestia <67275846+andreh1111@users.noreply.github.com> Date: Thu, 2 Jul 2026 18:04:52 -0400 Subject: [PATCH 28/36] fix time_wall update --- robohive/robot/robot.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index db8b7bd5..81d0666c 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -419,9 +419,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 @@ -710,6 +707,9 @@ def step(self, ctrl_desired, step_duration, ctrl_normalized=True, realTimeSim=Fa elif time_left_in_step < 0.0: prompt("Step duration %0.4fs, Step took %0.4fs, Time left %0.4f"% (step_duration, (time_now-self.time_wall), time_left_in_step), type=Prompt.WARN) + # Update time + self.time_wall = time.time()-self.time_start + if _ROBOT_VIZ: global timing_SRV_t0 timing_SRV_t = time.time() From e5cfe1704a3cabfa8f95929fa2f7e96103c3dfb7 Mon Sep 17 00:00:00 2001 From: Andrea Prestia <67275846+andreh1111@users.noreply.github.com> Date: Thu, 2 Jul 2026 18:33:40 -0400 Subject: [PATCH 29/36] fix prev commit --- robohive/robot/robot.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index 81d0666c..2636920a 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -383,6 +383,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() @@ -707,9 +710,6 @@ def step(self, ctrl_desired, step_duration, ctrl_normalized=True, realTimeSim=Fa elif time_left_in_step < 0.0: prompt("Step duration %0.4fs, Step took %0.4fs, Time left %0.4f"% (step_duration, (time_now-self.time_wall), time_left_in_step), type=Prompt.WARN) - # Update time - self.time_wall = time.time()-self.time_start - if _ROBOT_VIZ: global timing_SRV_t0 timing_SRV_t = time.time() From ef8e6073973eed48b8d5bc2d20f3b6c5cc2e7081 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Fri, 3 Jul 2026 13:56:55 -0400 Subject: [PATCH 30/36] BUGFIX: Operations shouldn't be influenced by the order of definitions in robot_config. Currently it was hardcoded to sim so when the definitions in config differend from sim, things didn't line up accurately. Now hardware_apply_controls() also accepts the space in which controls is provided --- robohive/robot/robot.py | 49 +++++++++++++++++++++++------------------ 1 file changed, 27 insertions(+), 22 deletions(-) diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index 2636920a..1d913b5b 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -30,6 +30,10 @@ # Support for non uniform noise in sensor readings # Support for noisy actions + separate noise_scale for sensor and actuator # rename pos/vel to act/delta_act +# Improve space definitions + # sim_id: ID of the sensor/actuator in the sim + # act_id: ID of the sensor/actuator in the robot_config (hardware) space ==> Rename to hdr_id (robot_config unified different hardware into a single unified hardware space) + # hdr_id: ID of the sensor/actuator in the hardware space (e.g. dynamixel) ==> Rename to adr (This is the address that is used to communicate with the hardware. It is not necessarily the same as the act_id or sim_id.) # NOTE/ GOOD PRACTICES =========================== # nq should be nv @@ -229,7 +233,14 @@ def hardware_get_sensors(self): # apply controls to hardware - def hardware_apply_controls(self, control, is_reset=False): + def hardware_apply_controls(self, control, space='hdr', is_reset=False): + """ + control: control vector in hdr or sim space + space: 'hdr' or 'sim' (defaults to 'hdr' as represented in robot_config) + is_reset: if True, reset the hardware to the control values + """ + + act_id = -1 # IDs as per the config order for name, device in self.robot_config.items(): if 'actuator' in device.keys() and len(device['actuator'])>0: if device['interface']['type'] == 'dynamixel': @@ -239,8 +250,10 @@ def hardware_apply_controls(self, control, is_reset=False): pwm_ctrl = [] pwm_ids = [] for actuator in device['actuator']: + act_id += 1 + ctrl = control[actuator['sim_id']] if space == 'sim' else control[act_id] # calibrate - calib_ctrl = control[actuator['sim_id']]*actuator['scale']+ actuator['offset'] + calib_ctrl = ctrl*actuator['scale']+ actuator['offset'] if actuator['mode'] == 'Position': pos_ids.append(actuator['hdr_id']) pos_ctrl.append(calib_ctrl) @@ -249,34 +262,26 @@ def hardware_apply_controls(self, control, is_reset=False): pwm_ctrl.append(calib_ctrl) else: print("ERROR: Mode not found") - raise NotImplemented + raise NotImplementedError(f"ERROR: Actuator mode {actuator['mode']} not found") # 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) - - elif device['interface']['type'] == 'robotiq': - robotiq_des_pos = [] + elif device['interface']['type'] in ['franka', 'robotiq']: + des_pos = [] for actuator in device['actuator']: + act_id += 1 + ctrl = control[actuator['sim_id']] if space == 'sim' else control[act_id] # calibrate - robotiq_des_pos.append(control[actuator['sim_id']]*actuator['scale']+ actuator['offset']) + des_pos.append(ctrl*actuator['scale']+ actuator['offset']) if is_reset: - device['robot'].reset(robotiq_des_pos[0]) + device['robot'].reset(des_pos) else: - device['robot'].apply_commands(robotiq_des_pos[0]) + device['robot'].apply_commands(des_pos) else: - raise NotImplemented("ERROR: interface not found") + raise NotImplementedError("ERROR: interface not found") # close hardware @@ -561,7 +566,7 @@ 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 + act_id = -1 # IDs as per the config order controls_out = controls.copy() for name, device in self.robot_config.items(): if name == "default_robot": @@ -614,7 +619,7 @@ def process_actuator(self, controls, step_duration, """ # last_obs = self.get_sensor_from_cache(-1) processed_controls = controls.copy() - act_id = -1 + act_id = -1 # IDs as per the config order for name, device in self.robot_config.items(): if name == "default_robot": if self._act_mode == "pos": @@ -685,7 +690,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: From 999bee67c882d98da8dc3b3fc627913c67f87801 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Fri, 3 Jul 2026 15:21:49 -0400 Subject: [PATCH 31/36] MAJOR: REFACTOR: Improving space definitions: # sim_id: ID of the sensor/actuator in the sim > # hdr_id: ID of the sensor/actuator in the robot_config (hardware) space (robot_config unifies different hardware into a single unified hardware space) # adr: Address of the sensor/actuator in the individual hardware space (e.g. dynamixel) (This is the address used during communicate with the individual hardware) --- .../arms/fetch/assets/fetch_reach_v0.config | 36 ++++---- .../franka/assets/franka_busbin_v0.config | 48 +++++------ .../arms/franka/assets/franka_reach_v0.config | 66 +++++++-------- .../arms/franka/assets/franka_ycb_v0.config | 66 +++++++-------- robohive/envs/fm/assets/dmanus.config | 36 ++++---- robohive/envs/fm/assets/franka_dmanus.config | 68 +++++++-------- robohive/envs/fm/assets/franka_robotiq.config | 42 +++++----- .../common/kitchen/franka_kitchen.config | 84 +++++++++---------- .../common/microwave/franka_microwave.config | 48 +++++------ .../slidecabinet/franka_slidecabinet.config | 52 ++++++------ .../quadrupeds/dkitty/dkitty_stand_v0.config | 84 +++++++++---------- .../quadrupeds/dkitty/dkitty_walk_v0.config | 84 +++++++++---------- robohive/robot/README.md | 24 +++--- robohive/robot/hardware_dynamixel.py | 8 +- robohive/robot/robot.py | 46 +++++----- 15 files changed, 397 insertions(+), 395 deletions(-) diff --git a/robohive/envs/arms/fetch/assets/fetch_reach_v0.config b/robohive/envs/arms/fetch/assets/fetch_reach_v0.config index d0ac1c0a..b35c8ae3 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, 'adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_pan_jp'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_lift_jp'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'upperarm_roll_jp'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'elbow_flex_jp'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'forearm_roll_jp'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'wrist_flex_jp'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'wrist_roll_jp'}, + {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_jp'}, + {'range':(0.00, .04), 'noise':0.05, '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), '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), '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), '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), '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), '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), '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), '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), '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), '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..2904463b 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, 'adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, '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, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + {'range':(0.00, .04), 'noise':0.05, '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), '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), '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), '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), '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), '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), '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), '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, '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..04f0819b 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, 'adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, + # {'range':(0.00, .04), 'noise':0.05, 'adr':7, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, + # {'range':(0.00, .04), 'noise':0.05, '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, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, '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), '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), '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), '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), '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), '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), '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), '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), '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), '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, 'adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + # {'range':(0, 255), 'noise':0.00, '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, 'adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + # {'range':(0, 255), 'noise':0.00, '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, 'adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + # {'range':(0, 255), 'noise':0.00, '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..81b49b42 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, 'adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, + {'range':(0.00, .04), 'noise':0.05, 'adr':7, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, + {'range':(0.00, .04), 'noise':0.05, '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, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, '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), '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), '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), '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), '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), '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), '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), '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), '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), '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, 'adr':0, 'scale':1, 'offset':0, 'name':'Tx'}, + {'range':(-0.5, 0.5), 'noise':0.005, 'adr':0, 'scale':1, 'offset':0, 'name':'Ty'}, + {'range':(-1.0, 1.0), 'noise':0.005, 'adr':0, 'scale':1, 'offset':0, 'name':'Tz'}, + {'range':(-3.1, 3.1), 'noise':0.005, 'adr':0, 'scale':1, 'offset':0, 'name':'Rx'}, + {'range':(-3.1, 3.1), 'noise':0.005, 'adr':0, 'scale':1, 'offset':0, 'name':'Ry'}, + {'range':(-3.1, 3.1), 'noise':0.005, 'adr':0, 'scale':1, 'offset':0, 'name':'Rz'}, ], 'actuator':[] } diff --git a/robohive/envs/fm/assets/dmanus.config b/robohive/envs/fm/assets/dmanus.config index 938e47d0..a0bf3062 100644 --- a/robohive/envs/fm/assets/dmanus.config +++ b/robohive/envs/fm/assets/dmanus.config @@ -3,27 +3,27 @@ 'dmanus':{ 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/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, 'adr':10, 'name':'TFJ1', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'adr':11, 'name':'TFJ2', 'scale':-1, 'offset':3*np.pi/2 }, + {'range':(-0.00, 2.00), 'noise':0.05, 'adr':12, 'name':'TFJ3', 'scale':-1, 'offset':np.pi }, + {'range':(-0.75, 0.57), 'noise':0.05, 'adr':20, 'name':'IFJ1', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'adr':21, 'name':'IFJ2', 'scale':-1, 'offset':3*np.pi/2 }, + {'range':(-0.00, 2.00), 'noise':0.05, 'adr':22, 'name':'IFJ3', 'scale':+1, 'offset':-np.pi }, + {'range':(-0.75, 0.57), 'noise':0.05, 'adr':30, 'name':'LFJ1', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'adr':31, 'name':'LFJ2', 'scale':+1, 'offset':-np.pi/2 }, + {'range':(-0.00, 2.00), 'noise':0.05, '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), '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), '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), '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), '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), '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), '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), '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), '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), '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..2384ee5b 100644 --- a/robohive/envs/fm/assets/franka_dmanus.config +++ b/robohive/envs/fm/assets/franka_dmanus.config @@ -5,23 +5,23 @@ '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, 'adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, '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), '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), '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), '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), '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), '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), '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), 'adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, ] }, @@ -29,29 +29,29 @@ 'dmanus':{ 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/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, 'adr':10, 'name':'TF_ADB_jp', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'adr':11, 'name':'TF_MCP_jp', 'scale':-1, 'offset':3*np.pi/2 }, + {'range':(-0.00, 2.14), 'noise':0.05, 'adr':12, 'name':'TF_PIP_jp', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.00), 'noise':0.05, '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, 'adr':20, 'name':'FF_ADB_jp', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'adr':21, 'name':'FF_MCP_jp', 'scale':-1, 'offset':3*np.pi/2 }, + {'range':(-0.00, 2.00), 'noise':0.05, 'adr':22, 'name':'FF_PIP_jp', 'scale':+1, 'offset':-np.pi }, + {'range':(-0.75, 0.57), 'noise':0.05, 'adr':30, 'name':'PF_ADB_jp', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'adr':31, 'name':'PF_MCP_jp', 'scale':+1, 'offset':-np.pi/2 }, + {'range':(-0.00, 2.00), 'noise':0.05, '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), '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), '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), '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), '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), '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), '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), '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), '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), '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), '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..7ed26bb2 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, 'adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, '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), '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), '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), '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), '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), '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), '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), '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, '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), '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, 'adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, '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, '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, '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, 'adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, ], 'actuator':[] }, diff --git a/robohive/envs/multi_task/common/kitchen/franka_kitchen.config b/robohive/envs/multi_task/common/kitchen/franka_kitchen.config index bb1816a2..1fa983b7 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, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp7'}, + {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, + {'range':(0.00, .04), 'noise':0.05, '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, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, '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), '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), '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), '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), '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), '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), '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), '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), '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), '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, 'adr':-1, 'scale':1, 'offset':0, 'name':'knob1_joint'}, + {'range':(-1.57, 00), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'knob2_joint'}, + {'range':(-1.57, 00), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'knob3_joint'}, + {'range':(-1.57, 00), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'knob4_joint'}, + {'range':(-0.7, 0.0), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'lightswitch_joint'}, + {'range':(0.0, 1.57), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'slidedoor_joint'}, + {'range':(-1.57, .0), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'leftdoorhinge'}, + {'range':(0.0, 1.57), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'rightdoorhinge'}, + {'range':(-2.09, 00), 'noise':0.05, '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, 'adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Tx'}, + {'range':(-1.50, 1.50), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Ty'}, + {'range':(-0.10, 2.90), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Tz'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Rx'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Ry'}, + {'range':(-3.14, 3.14), 'noise':0.05, '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..0d58447b 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, 'adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, '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, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + {'range':(0.00, .04), 'noise':0.05, '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), '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), '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), '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), '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), '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), '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), '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, '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..d7023e1f 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, 'adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, + {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, + {'range':(0.00, .04), 'noise':0.05, '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, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + {'range':(0.00, .04), 'noise':0.05, 'adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + {'range':(0.00, .04), 'noise':0.05, '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), '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), '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), '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), '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), '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), '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), '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, 'adr':7, 'scale':1, 'offset':0, 'name':'slidedoor_joint'}, ], 'actuator':[] } diff --git a/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config b/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config index 7e1c3b58..8dae22fd 100644 --- a/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config +++ b/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config @@ -3,12 +3,12 @@ '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, 'adr':0, 'scale':1, 'offset':0, 'name':'A:Tx'}, + {'range':(-5.00, 5.00), 'noise':0.005, 'adr':1, 'scale':1, 'offset':0, 'name':'A:Ty'}, + {'range':(-2.00, 2.00), 'noise':0.005, 'adr':2, 'scale':1, 'offset':-.32, 'name':'A:Tz'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'A:Rx'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'A:Ry'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'adr':5, 'scale':1, 'offset':0, 'name':'A:Rz'}, ], 'actuator':[] }, @@ -17,47 +17,47 @@ 'dkitty':{ 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/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, 'adr':10, 'scale':+1, 'offset':-3*np.pi/2, 'name':'A:FRJ10_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'adr':11, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ11_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'adr':12, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ12_pos_sensor'}, + {'range':(-0.279, 3.419), 'noise':0.05, 'adr':20, 'scale':+1, 'offset':-np.pi/2, 'name':'A:FLJ20_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'adr':21, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ21_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'adr':22, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ22_pos_sensor'}, + {'range':(-0.279, 3.419), 'noise':0.05, 'adr':30, 'scale':-1, 'offset':3*np.pi/2, 'name':'A:BLJ30_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'adr':31, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ31_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'adr':32, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ32_pos_sensor'}, + {'range':(-3.419, 0.279), 'noise':0.05, 'adr':40, 'scale':-1, 'offset':np.pi/2, 'name':'A:BRJ40_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'adr':41, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ41_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, '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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42'} ] # '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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42'} # ] } } \ 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..3dad488f 100644 --- a/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config +++ b/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config @@ -3,12 +3,12 @@ '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, 'adr':0, 'scale':1, 'offset':0, 'name':'A:Tx'}, + {'range':(-5.00, 5.00), 'noise':0.005, 'adr':1, 'scale':1, 'offset':0, 'name':'A:Ty'}, + {'range':(-2.00, 2.00), 'noise':0.005, 'adr':2, 'scale':1, 'offset':-.32, 'name':'A:Tz'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'adr':3, 'scale':1, 'offset':0, 'name':'A:Rx'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'adr':4, 'scale':1, 'offset':0, 'name':'A:Ry'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'adr':5, 'scale':1, 'offset':0, 'name':'A:Rz'}, ], 'actuator':[] }, @@ -17,47 +17,47 @@ 'dkitty':{ 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/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, 'adr':10, 'scale':+1, 'offset':-3*np.pi/2, 'name':'A:FRJ10_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'adr':11, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ11_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'adr':12, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ12_pos_sensor'}, + {'range':(-0.279, 3.419), 'noise':0.05, 'adr':20, 'scale':+1, 'offset':-np.pi/2, 'name':'A:FLJ20_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'adr':21, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ21_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'adr':22, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ22_pos_sensor'}, + {'range':(-0.279, 3.419), 'noise':0.05, 'adr':30, 'scale':-1, 'offset':3*np.pi/2, 'name':'A:BLJ30_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'adr':31, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ31_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'adr':32, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ32_pos_sensor'}, + {'range':(-3.419, 0.279), 'noise':0.05, 'adr':40, 'scale':-1, 'offset':np.pi/2, 'name':'A:BRJ40_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'adr':41, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ41_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, '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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42'} ] # '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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':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), 'adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42'} # ] } } \ No newline at end of file diff --git a/robohive/robot/README.md b/robohive/robot/README.md index 18ac300c..6d61cb6d 100644 --- a/robohive/robot/README.md +++ b/robohive/robot/README.md @@ -6,7 +6,7 @@ RoboHive uses an abstract class called `robot` to interface with all agents/devi ![Alt text](robohive_robot_overview.png "Optional title") RoboHive's goal is to make hardware as seamless to use as simulations. In order to achieve this, RoboHive projects hardware into simulation. Conceptually, this implies that all our computations happen as if we are always working in a simulated setting, hardware merely is a dynamic function that updates the sim. This has a few fundamental benefits -1. Interpretability of the simulation carried over to hardware. Users don't need to know all the hardware details. +1. Interpretability of the simulation carried over to hardware. Users doesn't need to know all the hardware details. 2. It's easy to prototype in simulation and later port results into hardware. This is very useful for paradigms like sim2real. 3. Simulation provides a general parameterization to the hardware details when it's important. 4. Simulation rendering can be used as a real time visualizer for the hardware updates. @@ -42,12 +42,12 @@ Robot configurations are specified using config files. The file is essentially a '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, 'adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, ], 'actuator':[ - {'pos_range':(-2.9, 2.9), 'vel_range':(-2, 2), 'hdr_id':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, - {'pos_range':(-1.8, 1.8), 'vel_range':(-2, 2), 'hdr_id':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9, 2.9), 'vel_range':(-2, 2), 'adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8, 1.8), 'vel_range':(-2, 2), 'adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, ], 'cam': [] }, @@ -56,10 +56,10 @@ Robot configurations are specified using config files. The file is essentially a '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, 'adr':0, 'name':'robotiq_2f_85', 'scale':-9.81, 'offset':0.834}, ], 'actuator':[ - {'pos_range':(0, 1), 'vel_range':(-2, 4), 'hdr_id':0, 'name':'robotiq_2f_85', 'scale':-0.08, 'offset':0.08}, + {'pos_range':(0, 1), 'vel_range':(-2, 4), 'adr':0, 'name':'robotiq_2f_85', 'scale':-0.08, 'offset':0.08}, ], 'cam': [] }, @@ -70,8 +70,8 @@ Robot configurations are specified using config files. The file is essentially a 'sensor':[], 'actuator':[] '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, 'adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'adr':'d', 'scale':1, 'offset':0, 'name':'/depth_uncolored/image_raw'}, ], }, ``` @@ -90,7 +90,7 @@ Robot class is configured using a config file. The config file is essentially a - `name`: Name of the sensor. Note: This has to be the same as the sensor name in the MuJoCo model. - `range`: Range of values expected from this sensor. Readings outside these ranges are clamped to the limits. - `noise`: expected amplitude of noise in the sensor readings. Use this parameter to add noise to the simulated sensor reading. Noise is sampled from a uniform distribution between `(-noise, +noise)` and added to the sensor reading. It has no effect if the robot is instantiated using hardware backend. ```sensor += noise_scale*sensor['noise']*self.np_random.uniform(low=-1.0, high=1.0)``` where `noise_scale` is a parameter (of the entire robot) to scale the noise of the whole robot. - - `hdr_id`: Hardware index of the sensor in the hardware's sensor array. + - `adr`: Hardware index of the sensor in the hardware's sensor array. - `scale`/`offset`: parameters to map hardware sensor values to simulated sensor values `sensor_sim = sensor_hdr*scale+offset` **Note**: Currently only scalar sensors are supported. Multi-valued sensor support is on the wishlist. @@ -99,14 +99,14 @@ Robot class is configured using a config file. The config file is essentially a - `name`: Name of the sensor. Since there are no explicit camera sensors in MuJoCo, there are no specific constraints on camera names. We internally use the pub-sub topics names as camera names. - `range`: see description in sensor - `noise`:see description in sensor - - `hdr_id`: see description in sensor + - `adr`: see description in sensor - `scale`/`offset`: see description in sensor - **`actuator`**: An ordered list of all available actuators. List ordering must follow the same order as specified in the MuJoCo model. List ordering is used to implicitly determine the actuator's index in the actuator array - - `name`: Name of the actuator. Note: This has to be the same as the actuator name in the MuJoCo model. - `range`: Range of values the actuator can accept in the sim space. Readings outside these ranges are clamped to the limits. - `noise`: [_Wishlist_] expected amplitude of noise in actuator's performance. Use this parameter to add noise to the simulated actuator demands. Noise is sampled from a uniform distribution between `(-noise, +noise)` and added to the actuator demands. It has no effect if the robot is instantiated using hardware backend. ```ctrl_sim += noise_scale*act['noise']*self.np_random.uniform(low=-1.0, high=1.0)``` where `noise_scale` is a parameter (of the entire robot) to scale the noise of the whole robot. - - `hdr_id`: Hardware index of the actuator in the hardware's sensor array. + - `adr`: Hardware index of the actuator in the hardware's sensor array. - `scale`/`offset`: parameters to map simulated desired values to hardware actuator demands `ctrl_hdr = ctrl_sim*scale+offset` diff --git a/robohive/robot/hardware_dynamixel.py b/robohive/robot/hardware_dynamixel.py index 5e94513c..6c769f18 100644 --- a/robohive/robot/hardware_dynamixel.py +++ b/robohive/robot/hardware_dynamixel.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. ================================================= """ -from .hardware_base import hardwareBase -from dynamixel_py import dxl import numpy as np +from dynamixel_py import dxl + +from .hardware_base import hardwareBase + class Dynamixels(hardwareBase): def __init__(self, name, motor_ids, motor_type, devicename, **kwargs): @@ -22,7 +24,7 @@ def connect(self): # set actuator mode for actuator in device['actuator']: - self.dxls.set_operation_mode(motor_id=[actuator['hdr_id']], mode=actuator['mode']) + self.dxls.set_operation_mode(motor_id=[actuator['adr']], mode=actuator['mode']) # engage motors self.dxls.engage_motor(motor_id=self.dxls, enable=True) diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index 1d913b5b..f95f48e5 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -30,14 +30,15 @@ # Support for non uniform noise in sensor readings # Support for noisy actions + separate noise_scale for sensor and actuator # rename pos/vel to act/delta_act -# Improve space definitions - # sim_id: ID of the sensor/actuator in the sim - # act_id: ID of the sensor/actuator in the robot_config (hardware) space ==> Rename to hdr_id (robot_config unified different hardware into a single unified hardware space) - # hdr_id: ID of the sensor/actuator in the hardware space (e.g. dynamixel) ==> Rename to adr (This is the address that is used to communicate with the hardware. It is not necessarily the same as the act_id or sim_id.) # NOTE/ GOOD PRACTICES =========================== # nq should be nv # Order of sensors and actuators in config should follow XML order +# Space definitions + # sim_id: ID of the sensor/actuator in the sim + # hdr_id: ID of the sensor/actuator in the robot_config (hardware) space (robot_config unifies different hardware into a single unified hardware space) + # adr: Address of the sensor/actuator in the individual hardware space (e.g. dynamixel) (This is the address used during communicate with the individual hardware) + class Robot(): @@ -171,7 +172,7 @@ def hardware_init(self, robot_config): # set actuator mode for actuator in device['actuator']: - device['robot'].set_operation_mode(motor_id=[actuator['hdr_id']], mode=actuator['mode']) + device['robot'].set_operation_mode(motor_id=[actuator['adr']], mode=actuator['mode']) # engage motors device['robot'].engage_motor(motor_id=device['actuator_ids'], enable=True) @@ -240,7 +241,6 @@ def hardware_apply_controls(self, control, space='hdr', is_reset=False): is_reset: if True, reset the hardware to the control values """ - act_id = -1 # IDs as per the config order for name, device in self.robot_config.items(): if 'actuator' in device.keys() and len(device['actuator'])>0: if device['interface']['type'] == 'dynamixel': @@ -250,15 +250,14 @@ def hardware_apply_controls(self, control, space='hdr', is_reset=False): pwm_ctrl = [] pwm_ids = [] for actuator in device['actuator']: - act_id += 1 - ctrl = control[actuator['sim_id']] if space == 'sim' else control[act_id] + ctrl = control[actuator['sim_id']] if space == 'sim' else control[actuator['hdr_id']] # calibrate calib_ctrl = ctrl*actuator['scale']+ actuator['offset'] if actuator['mode'] == 'Position': - pos_ids.append(actuator['hdr_id']) + pos_ids.append(actuator['adr']) pos_ctrl.append(calib_ctrl) elif actuator['mode'] == 'PWM': - pwm_ids.append(actuator['hdr_id']) + pwm_ids.append(actuator['adr']) pwm_ctrl.append(calib_ctrl) else: print("ERROR: Mode not found") @@ -272,8 +271,7 @@ def hardware_apply_controls(self, control, space='hdr', is_reset=False): elif device['interface']['type'] in ['franka', 'robotiq']: des_pos = [] for actuator in device['actuator']: - act_id += 1 - ctrl = control[actuator['sim_id']] if space == 'sim' else control[act_id] + ctrl = control[actuator['sim_id']] if space == 'sim' else control[actuator['hdr_id']] # calibrate des_pos.append(ctrl*actuator['scale']+ actuator['offset']) if is_reset: @@ -322,6 +320,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) @@ -329,9 +329,11 @@ 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['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) @@ -350,9 +352,11 @@ def configure_robot(self, sim, config_path): 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['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 == mujoco.mjtTrn.mjTRN_JOINT: # // force on joint @@ -453,7 +457,7 @@ 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['adr']] = current_sensor_value[cam_name][cam['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'] @@ -566,7 +570,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 # IDs as per the config order controls_out = controls.copy() for name, device in self.robot_config.items(): if name == "default_robot": @@ -578,10 +581,9 @@ 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 @@ -619,7 +621,6 @@ def process_actuator(self, controls, step_duration, """ # last_obs = self.get_sensor_from_cache(-1) processed_controls = controls.copy() - act_id = -1 # IDs as per the config order for name, device in self.robot_config.items(): if name == "default_robot": if self._act_mode == "pos": @@ -630,10 +631,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": From 3f4ab07e5c703fba43e662dceb5ef540a06e987f Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Fri, 3 Jul 2026 19:49:25 -0400 Subject: [PATCH 32/36] UPGRADE: Adding return types to the base class for consistency and check for existance of 'time' key for sensors --- robohive/robot/hardware_base.py | 25 ++++++++++++++++--------- robohive/robot/hardware_dynamixel.py | 2 +- robohive/robot/hardware_franka.py | 6 +++--- robohive/robot/hardware_optitrack.py | 2 +- robohive/robot/hardware_realsense.py | 2 +- robohive/robot/hardware_robotiq.py | 6 +++--- 6 files changed, 25 insertions(+), 18 deletions(-) diff --git a/robohive/robot/hardware_base.py b/robohive/robot/hardware_base.py index ef6944a0..19a0b565 100644 --- a/robohive/robot/hardware_base.py +++ b/robohive/robot/hardware_base.py @@ -9,32 +9,39 @@ import abc class hardwareBase(abc.ABC): - def __init__(self, name, *args, **kwargs): + 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): + def okay(self) -> bool: """Return hardware health""" @abc.abstractmethod - def close(self): + def close(self) -> bool: """Close hardware connection""" @abc.abstractmethod - def reset(self): + def reset(self) -> None: """Reset hardware""" @abc.abstractmethod - def get_sensors(self): - """Get hardware sensors""" + def _get_sensors(self) -> dict: + """Get hardware sensors — returned dict must include a 'time' key""" + + def get_sensors(self) -> dict: + """Get hardware sensors, enforcing 'time' key contract""" + data = self._get_sensors() + assert isinstance(data, dict) and 'time' in data, \ + f"{self.name}: get_sensors() must return a dict containing a 'time' key, got {type(data)}" + return data @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 6c769f18..16c99c55 100644 --- a/robohive/robot/hardware_dynamixel.py +++ b/robohive/robot/hardware_dynamixel.py @@ -38,7 +38,7 @@ def close(self): def reset(self): """Reset hardware""" - def get_sensors(self): + def _get_sensors(self): """Get hardware sensors""" def apply_commands(self): diff --git a/robohive/robot/hardware_franka.py b/robohive/robot/hardware_franka.py index bb2b89d9..7a2b8f07 100644 --- a/robohive/robot/hardware_franka.py +++ b/robohive/robot/hardware_franka.py @@ -185,7 +185,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() @@ -193,8 +193,8 @@ def get_sensors(self): except: print("Failed to get current sensors: ", end="") self.reconnect() - return self.get_sensors() - return {'joint_pos': joint_pos, 'joint_vel':joint_vel} + return self._get_sensors() + return {'time': time.time(), 'joint_pos': joint_pos, 'joint_vel': joint_vel} def apply_commands(self, q_desired=None, kp=None, kd=None): diff --git a/robohive/robot/hardware_optitrack.py b/robohive/robot/hardware_optitrack.py index 9a96e3b5..5aab551b 100644 --- a/robohive/robot/hardware_optitrack.py +++ b/robohive/robot/hardware_optitrack.py @@ -134,7 +134,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..faa68e5f 100644 --- a/robohive/robot/hardware_realsense.py +++ b/robohive/robot/hardware_realsense.py @@ -58,7 +58,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) diff --git a/robohive/robot/hardware_robotiq.py b/robohive/robot/hardware_robotiq.py index 27b4e2f0..76182c8d 100644 --- a/robohive/robot/hardware_robotiq.py +++ b/robohive/robot/hardware_robotiq.py @@ -88,15 +88,15 @@ 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() except: print("RBQ:> Failed to get current sensors: ", end="") self.reconnect() - return self.get_sensors() - return np.array([curr_state.width]) + return self._get_sensors() + return {'time': time.time(), 'width': np.array([curr_state.width])} def apply_commands(self, width:float, speed:float=0.1, force:float=0.1): assert width>=0.0 and width<=self.max_width, "Gripper desired width ({}) is out of bound (0,{})".format(width, self.max_width) From e09299ac35527c086b5088f363f1faa0a8c5d999 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Fri, 3 Jul 2026 20:13:30 -0400 Subject: [PATCH 33/36] FEATURE: Adding support for multiple kinds of sensors that can now carry complex data from the hardware into sensordata field of the model. Reply on sensordata field to create obs and obs_dicts --- robohive/robot/robot.py | 38 ++++++++++++++++++++++++++++---------- 1 file changed, 28 insertions(+), 10 deletions(-) diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index f95f48e5..34279954 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -336,17 +336,21 @@ def configure_robot(self, sim, config_path): device['sensor_ids'].append(sensor['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] - elif sensor_type == mujoco.mjtSensor.mjSENS_TENDON: # mjSENS_TENDON // tendon force - sensor['data_type'] = 'ten_length' - sensor['data_id'] = 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'] = [] @@ -418,6 +422,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) @@ -489,19 +495,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): From 70f43e77e6d9ed9c2c29ba0963559c9b6344a9c6 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Tue, 7 Jul 2026 16:53:10 -0400 Subject: [PATCH 34/36] BUGFIX: squeeze_dims collapsing single-value obs into unconcatenatable 0-d scalars squeeze_dims() previously did a blanket np.squeeze(), which removed every singleton dimension instead of just the (num_traj=1, horizon=1) wrapper added by expand_dims(). Single-value observations (e.g. shape (1,)) were squeezed all the way down to 0-d scalars, which then failed to concatenate with other proprio/obs vectors in get_proprioception() and obsdict2obsvec(). Now only squeeze the leading axes that expand_dims actually added, leaving real data dimensions (and pre-existing scalar rwd_dict entries) untouched. --- robohive/envs/obs_vec_dict.py | 23 +++++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/robohive/envs/obs_vec_dict.py b/robohive/envs/obs_vec_dict.py index 4dece6bb..36a771b0 100644 --- a/robohive/envs/obs_vec_dict.py +++ b/robohive/envs/obs_vec_dict.py @@ -63,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) From 1388789af8d2402265ccfc304b8c69e63b58a188 Mon Sep 17 00:00:00 2001 From: Ouassim Aouattah Date: Thu, 9 Jul 2026 13:43:53 +0200 Subject: [PATCH 35/36] fix(robot): index actuator qpos reads with jnt_qposadr, not jnt_dofadr MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Robot._resolve_data_ids caches each joint actuator's feedback address as data_id into sim.data.qpos, but computed it with jnt_dofadr (the qvel address). qpos and qvel addresses coincide only while all preceding joints are 1-DOF: any free/ball joint earlier in the model (e.g. a manipulable object defined before the arm) shifts every subsequent actuator's read by the qpos/qvel size difference (freejoint: 7 vs 6). The stale read feeds the velocity limiter in process_actuator(), which re-anchors each command to the wrong joint's position — in practice the arm freezes at whatever pose keeps the misread error small. Observed on anthrohive URDtriosPickPlace-v0 (box freejoint precedes the UR5e): the arm ignored all position targets until this fix. --- robohive/robot/robot.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index 34279954..12e5338e 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -365,7 +365,10 @@ def configure_robot(self, sim, config_path): actuator_trnid = sim.model.actuator_trnid[actuator['sim_id'], 0] 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 From 157aa4c24f46c04e1159cb9baedd2903c2cdb18f Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Thu, 9 Jul 2026 15:10:13 -0400 Subject: [PATCH 36/36] FEATURE: Adding a warning if time key is not found --- robohive/robot/hardware_base.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/robohive/robot/hardware_base.py b/robohive/robot/hardware_base.py index 19a0b565..6b003d69 100644 --- a/robohive/robot/hardware_base.py +++ b/robohive/robot/hardware_base.py @@ -7,6 +7,8 @@ # Base robot class for other hardware devices to inheret from import abc +import warnings + class hardwareBase(abc.ABC): def __init__(self, name, *args, **kwargs) -> None: @@ -35,8 +37,10 @@ def _get_sensors(self) -> dict: def get_sensors(self) -> dict: """Get hardware sensors, enforcing 'time' key contract""" data = self._get_sensors() - assert isinstance(data, dict) and 'time' in data, \ - f"{self.name}: get_sensors() must return a dict containing a 'time' key, got {type(data)}" + 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)}. " + "Please add 'time' details to your sensor data to suppress this warning.") return data @abc.abstractmethod