From 1c8c448ed86aced2e160e322e7067c2d6a3dfac5 Mon Sep 17 00:00:00 2001 From: Wenxuan Zhou Date: Thu, 20 Apr 2023 00:03:16 -0400 Subject: [PATCH 01/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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/49] 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 From 1b06dea5f2c43534ddd68bf67d7c5f3cb864eb25 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Wed, 15 Jul 2026 17:34:40 -0400 Subject: [PATCH 37/49] FEATURE: We not check for time key in hardware classes directly in the base class. So there is no need to deviate away from the get_sensor() signature --- robohive/robot/hardware_base.py | 34 ++++++++++++++++++++-------- robohive/robot/hardware_dynamixel.py | 2 +- robohive/robot/hardware_franka.py | 4 ++-- robohive/robot/hardware_optitrack.py | 2 +- robohive/robot/hardware_realsense.py | 2 +- robohive/robot/hardware_robotiq.py | 4 ++-- 6 files changed, 31 insertions(+), 17 deletions(-) diff --git a/robohive/robot/hardware_base.py b/robohive/robot/hardware_base.py index 6b003d69..4b611c85 100644 --- a/robohive/robot/hardware_base.py +++ b/robohive/robot/hardware_base.py @@ -11,6 +11,29 @@ class hardwareBase(abc.ABC): + + # add tests to all defined subclasses to ensure that get_sensors() returns a dict with a 'time' key + def __init_subclass__(cls, **kwargs): + super().__init_subclass__(**kwargs) + if 'connect' in cls.__dict__: + user_connect = cls.__dict__['connect'] + + def connect_then_check(self, *args, **kw): + result = user_connect(self, *args, **kw) + try: + data = self.get_sensors() + if not (isinstance(data, dict) and 'time' in data): + warnings.warn( + f"{self.name}: get_sensors() should return a dict containing a 'time' key, got {type(data)}" + ) + except Exception as e: + warnings.warn( + f"{self.name}: could not verify get_sensors() 'time'-key contract after connect: {e}" + ) + return result + + cls.connect = connect_then_check + def __init__(self, name, *args, **kwargs) -> None: self.name = name @@ -31,17 +54,8 @@ def reset(self) -> None: """Reset hardware""" @abc.abstractmethod - 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() - 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 + """Get hardware sensors — should return a dict containing a 'time' key""" @abc.abstractmethod def apply_commands(self) -> None: diff --git a/robohive/robot/hardware_dynamixel.py b/robohive/robot/hardware_dynamixel.py index 16c99c55..6c769f18 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 7a2b8f07..cdba1a10 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) -> dict: + def get_sensors(self) -> dict: """Get hardware sensors""" try: joint_pos = self.robot.get_joint_positions() @@ -193,7 +193,7 @@ def _get_sensors(self) -> dict: except: print("Failed to get current sensors: ", end="") self.reconnect() - return self._get_sensors() + return self.get_sensors() return {'time': time.time(), 'joint_pos': joint_pos, 'joint_vel': joint_vel} diff --git a/robohive/robot/hardware_optitrack.py b/robohive/robot/hardware_optitrack.py index 5aab551b..31106f3a 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) -> dict: + 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 faa68e5f..17c0541c 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) -> dict: + 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 76182c8d..689568e6 100644 --- a/robohive/robot/hardware_robotiq.py +++ b/robohive/robot/hardware_robotiq.py @@ -88,14 +88,14 @@ def reset(self, width=None, **kwargs): self.apply_commands(width=width, **kwargs) - def _get_sensors(self) -> dict: + 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 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): From 4866bb4748e885265a0f2dcdd47360266743c95d Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Wed, 15 Jul 2026 17:39:50 -0400 Subject: [PATCH 38/49] FEATURE: Improved path_utils. Better scanning of directories. Better handling of outputs. Better plots --- robohive/logger/examine_logs.py | 5 +- robohive/utils/paths_utils.py | 299 ++++++++++++++++++++++++-------- 2 files changed, 231 insertions(+), 73 deletions(-) diff --git a/robohive/logger/examine_logs.py b/robohive/logger/examine_logs.py index 99d9eb00..4782a8c1 100644 --- a/robohive/logger/examine_logs.py +++ b/robohive/logger/examine_logs.py @@ -211,6 +211,9 @@ def examine_logs(env_name, rollout_path, rollout_format, mode, horizon, seed, nu elif i_step < trace_horizon: # incase last step actions (nans) can cause issues in step obs, rwd, done, *_, env_info = env.step(act, update_exteroception=include_exteroception) ep_rwd += rwd + sleep_time = 1*env.dt if render=='onscreen' else 0.0 + if sleep_time>0: + time.sleep(sleep_time) # save offscreen buffers as video and clear the dataset if render == 'offscreen': @@ -230,7 +233,7 @@ def examine_logs(env_name, rollout_path, rollout_format, mode, horizon, seed, nu # plot paths if plot_paths: file_name = os.path.join(output_dir, output_name + '{}'.format(time_stamp)) - plotnsave_paths(trace.trace, env=env, fileName_prefix=file_name) + plotnsave_paths(trace.trace, env_handle=env, output_name=file_name) # Close and save paths trace.close() diff --git a/robohive/utils/paths_utils.py b/robohive/utils/paths_utils.py index 32c44e7d..d65c915c 100644 --- a/robohive/utils/paths_utils.py +++ b/robohive/utils/paths_utils.py @@ -5,33 +5,158 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================= """ -import numpy as np -import os import glob +import json +import os import pickle + +import anthrohive +import click import h5py +import numpy as np import skvideo.io from PIL import Image -import click -from robohive.utils.dict_utils import flatten_dict, dict_numpify -import json + +from robohive.utils import gym +from robohive.utils.dict_utils import dict_numpify, flatten_dict #TODO: Harmonize names, remove rollout_paths, use path for one and paths for multiple +ROLLOUT_EXTENSIONS = ('.h5', '.pickle') + + +# Normalize a loaded paths object (list or dict-like) into a name->path dict +def _normalize_paths(paths): + if isinstance(paths, (list, tuple)): + return {'Trial{}'.format(i): path for i, path in enumerate(paths)} + return paths + + +# Load a single rollout file (.h5 / .pickle) into a normalized path object +def load_rollout_file(rollout_path:str): + """ + Load a single rollout file (.h5 or .pickle) from disk. + + Args: + rollout_path (str): absolute path to a rollout file + Returns: + dict-like object mapping path/trial name to its data + """ + ext = os.path.splitext(rollout_path)[-1] + if ext == '.h5': + paths = h5py.File(rollout_path, 'r') + elif ext == '.pickle': + paths = pickle.load(open(rollout_path, 'rb')) + else: + raise TypeError("Unknown rollout format:{}. Supported formats: {}".format(ext, ROLLOUT_EXTENSIONS)) + return _normalize_paths(paths) + + +# Harmonized preprocessor: resolve a path_handle into a loaded path object +def resolve_path_handle(path_handle, extensions=ROLLOUT_EXTENSIONS, return_sources=False): + """ + Resolve a path_handle into a normalized, loaded path object. + + path_handle can be- + - an already loaded path object (dict / list / h5py.File / Trace.trace, ...) + - a str path to a single rollout file (.h5 or .pickle) + - a str path to a directory containing rollout files (.h5 / .pickle), + searched recursively through subdirectories, and all matching files + are loaded and merged + + Args: + path_handle: handle to resolve (see above) + extensions: rollout file extensions to scan for when path_handle is a directory + return_sources (bool): if True, also return a dict mapping each path/trial name to a + (source_dir, source_file_stem) tuple describing the file it was loaded from + ((None, None) if path_handle was already a loaded object with no known file location) + Returns: + dict-like object mapping path/trial name to its data + (dict mapping path/trial name to its (source_dir, source_file_stem), if return_sources) + """ + if isinstance(path_handle, str): + if os.path.isdir(path_handle): + rollout_files = sorted( + f for ext in extensions + for f in glob.glob(os.path.join(path_handle, '**', '*'+ext), recursive=True)) + assert len(rollout_files) > 0, \ + "No rollout files (formats:{}) found in directory:{} (searched recursively)".format(extensions, path_handle) + paths, sources = {}, {} + for rollout_file in rollout_files: + prefix = os.path.splitext(os.path.relpath(rollout_file, path_handle))[0] + file_dir = os.path.dirname(rollout_file) + file_stem = os.path.splitext(os.path.basename(rollout_file))[0] + for name, data in load_rollout_file(rollout_file).items(): + key = '{}/{}'.format(prefix, name) + paths[key] = data + sources[key] = (file_dir, file_stem) + return (paths, sources) if return_sources else paths + elif os.path.isfile(path_handle): + paths = load_rollout_file(path_handle) + file_dir = os.path.dirname(os.path.abspath(path_handle)) + file_stem = os.path.splitext(os.path.basename(path_handle))[0] + return (paths, {key: (file_dir, file_stem) for key in paths.keys()}) if return_sources else paths + else: + raise FileNotFoundError("Path not found:{}".format(path_handle)) + else: + # already a loaded path object; source location is unknown + paths = _normalize_paths(path_handle) + return (paths, {key: None for key in paths.keys()}) if return_sources else paths + + +# Harmonized preprocessor: resolve an env_handle into a loaded, unwrapped env +def resolve_env_handle(env_handle, env_args=None): + """ + Resolve an env_handle into a loaded, unwrapped env object. + + env_handle can be- + - an already loaded env object (wrapped or unwrapped) + - a str env_name to be created via gym.make + + Args: + env_handle: handle to resolve (see above) + env_args (str, optional): kwargs (as a str-eval'able dict) to pass to gym.make, when env_handle is a str + Returns: + unwrapped env object, or None if env_handle is None + """ + if env_handle is None: + return None + elif isinstance(env_handle, str): + env = gym.make(env_handle) if env_args is None else gym.make(env_handle, **(eval(env_args))) + return env.unwrapped + else: + # already a loaded env object + return env_handle.unwrapped if hasattr(env_handle, 'unwrapped') else env_handle + + # Check the horizon for teleOp / Hardware experiments -def plot_horizon(paths, env, fileName_prefix=None): +def plot_horizon(path_handle, env_handle, output_dir=None, output_name='', env_args=None): """ Check the horizon for teleOp / Hardware experiments Args: - paths: paths to examine - env: unwrapped env - fileName_prefix (str): prefix to use in the filename + path_handle: loaded path object, or path to a rollout file/ directory of rollout files + env_handle: loaded (unwrapped) env object, or an env_name to be created via gym.make + output_dir (str, optional): Directory to save the outputs. Defaults to path_handle's + directory (or cwd if path_handle is not a str path). + output_name (str, optional): prefix to use in the output filename + env_args (str, optional): kwargs (as a str-eval'able dict) to pass to gym.make, when env_handle is a str Saves: - fileName_prefix + '_horizon.pdf' + output_dir/output_name + '_horizon.pdf' """ + paths = list(resolve_path_handle(path_handle).values()) + env = resolve_env_handle(env_handle, env_args=env_args) + + if output_dir is None: + if isinstance(path_handle, str): + output_dir = path_handle if os.path.isdir(path_handle) else (os.path.dirname(os.path.abspath(path_handle))) + else: + output_dir = '.' + os.makedirs(output_dir, exist_ok=True) + fileName_prefix = os.path.join(output_dir, output_name or '') + import matplotlib as mpl - mpl.use('TkAgg') + mpl.use('Agg') import matplotlib.pyplot as plt plt.rcParams.update({'font.size': 5}) @@ -41,26 +166,28 @@ def plot_horizon(paths, env, fileName_prefix=None): # plot timesteps plt.clf() - rl_dt_ideal = env.frame_skip * env.model.opt.timestep + rl_dt_ideal = env.dt for i, path in enumerate(paths): dt = path['env_infos']['time'][1:] - path['env_infos']['time'][:-1] horizon[i] = path['env_infos']['time'][-1] - path['env_infos'][ 'time'][0] - h1 = plt.plot( + plt.plot( path['env_infos']['time'][1:], dt, '-', - label=('time=%1.2f' % horizon[i])) - h1 = plt.plot( + alpha=.8, + label=('hor=%1.2f' % horizon[i])) + plt.plot( np.array([0, max(horizon)]), rl_dt_ideal * np.ones(2), 'g', alpha=.5, - linewidth=2.0) + linewidth=2.0, + label='ideal dt') - plt.legend([h1[0]], ['ideal'], loc='upper right') + plt.legend(loc='upper right') plt.ylabel('time step (sec)') plt.xlabel('time (sec)') - plt.ylim(rl_dt_ideal - 0.005, rl_dt_ideal + .005) + plt.ylim(rl_dt_ideal - 2*rl_dt_ideal, rl_dt_ideal + 3*rl_dt_ideal) plt.suptitle('Timestep profile for %d rollouts' % len(paths)) file_name = fileName_prefix + '_timesteps.pdf' @@ -69,7 +196,7 @@ def plot_horizon(paths, env, fileName_prefix=None): # plot horizon plt.clf() - h1 = plt.plot( + plt.plot( np.array([0, len(paths)]), env.horizon * rl_dt_ideal * np.ones(2), 'g', @@ -87,31 +214,62 @@ def plot_horizon(paths, env, fileName_prefix=None): # 2D-plot of paths detailing obs, act, rwds across time -def plot(paths, env=None, fileName_prefix=''): +def plot(path_handle, env_handle=None, output_dir=None, output_name='', env_args=None): """ 2D-plot of paths detailing obs, act, rwds across time Args: - paths: paths to examine - env: unwrapped env - fileName_prefix: prefix to use in the filename + path_handle: loaded path object, or path to a rollout file/ directory of rollout files + env_handle: loaded (unwrapped) env object, or an env_name to be created via gym.make + output_dir (str, optional): Directory to save the outputs. If not provided, each + trial's plot is saved next to the rollout file it came from. If provided, the + source directory structure (for directory path_handles) is mirrored under it. + output_name (str, optional): prefix to use in the output filenames. If not provided, + defaults to the name of the rollout file the trial came from (when known). + env_args (str, optional): kwargs (as a str-eval'able dict) to pass to gym.make, when env_handle is a str Saves: - fileName_prefix + path_name + '.pdf' + one pdf per trial, named _ + '.pdf' """ import matplotlib as mpl mpl.use('Agg') import matplotlib.pyplot as plt plt.rcParams.update({'font.size': 5}) + paths, sources = resolve_path_handle(path_handle, return_sources=True) + env = resolve_env_handle(env_handle, env_args=env_args) + is_dir_handle = isinstance(path_handle, str) and os.path.isdir(path_handle) for path_name, path in paths.items(): + leaf_name = path_name.split('/')[-1] + source_dir, source_stem = sources.get(path_name) or (None, None) + if output_dir is not None: + if is_dir_handle and source_dir is not None: + rel_dir = os.path.relpath(source_dir, path_handle) + save_dir = os.path.join(output_dir, rel_dir) if rel_dir != '.' else output_dir + else: + save_dir = output_dir + else: + save_dir = source_dir or '.' + os.makedirs(save_dir, exist_ok=True) + name_prefix = output_name or source_stem or '' + file_stem = '_'.join(p for p in (name_prefix, leaf_name) if p) plt.clf() # observations - nplt1 = len(path['env_infos']['obs_dict'].keys()) - for iplt1, key in enumerate( - sorted(path['env_infos']['obs_dict'].keys())): + obs_keys = sorted( + key for key in path['env_infos']['obs_dict'].keys() + if path['env_infos']['obs_dict'][key].ndim < 3) + nplt1 = len(obs_keys) + time = np.asarray(path['env_infos']['time']) + print(obs_keys) + for iplt1, key in enumerate(obs_keys): ax = plt.subplot(nplt1, 2, iplt1 * 2 + 1) + if iplt1 == 0: + plt.ylabel('Observations') + ax.yaxis.tick_right() + if path['env_infos']['obs_dict'][key].size > 0: + plt.plot(time, np.asarray(path['env_infos']['obs_dict'][key]), label=key) + ax.set_xlim(time[0], time[-1]) if iplt1 != (nplt1 - 1): ax.axes.xaxis.set_ticklabels([]) if iplt1 == 0: @@ -123,7 +281,7 @@ def plot(paths, env=None, fileName_prefix=''): path['env_infos']['obs_dict'][key], label=key) # plt.ylabel(key) - plt.text(0.01, .01, key, transform=ax.transAxes) + plt.text(0.01, .01, f"{key}{path['env_infos']['obs_dict'][key].shape}", transform=ax.transAxes) plt.xlabel('time (sec)') # actions @@ -132,7 +290,7 @@ def plot(paths, env=None, fileName_prefix=''): ax.set_prop_cycle(None) # h4 = plt.plot(path['env_infos']['time'], env.act_mid + path['actions']*env.act_rng, '-', label='act') # plot scaled actions h4 = plt.plot( - path['env_infos']['time'], path['actions'], '-', + time, np.asarray(path['actions']), '-', label='act') # plot normalized actions plt.ylabel('actions') ax.axes.xaxis.set_ticklabels([]) @@ -142,8 +300,8 @@ def plot(paths, env=None, fileName_prefix=''): if "score" in path['env_infos']: ax = plt.subplot(nplt2, 2, 6) plt.plot( - path['env_infos']['time'], - path['env_infos']['score'], + time, + np.asarray(path['env_infos']['score']), label='score') plt.xlabel('time') plt.ylabel('score') @@ -154,8 +312,8 @@ def plot(paths, env=None, fileName_prefix=''): ax.set_prop_cycle(None) for key in sorted(path['env_infos']['rwd_dict'].keys()): plt.plot( - path['env_infos']['time'], - path['env_infos']['rwd_dict'][key], + time, + np.asarray(path['env_infos']['rwd_dict'][key]), label=key) plt.legend( loc='upper left', @@ -171,8 +329,8 @@ def plot(paths, env=None, fileName_prefix=''): ax.set_prop_cycle(None) for key in sorted(env.rwd_keys_wt.keys()): plt.plot( - path['env_infos']['time'], - path['env_infos']['rwd_dict'][key]*env.rwd_keys_wt[key], + time, + np.asarray(path['env_infos']['rwd_dict'][key])*env.rwd_keys_wt[key], label=key) plt.legend( loc='upper left', @@ -183,20 +341,22 @@ def plot(paths, env=None, fileName_prefix=''): plt.ylabel('wt*rewards') ax.yaxis.tick_right() - file_name = fileName_prefix + path_name + '.pdf' + file_name = os.path.join(save_dir, file_stem + '.pdf') plt.savefig(file_name) print("saved ", file_name) # Render frames/videos -def render(rollout_path, render_format:str="mp4", cam_names:list=["left"]): +def render(path_handle, render_format:str="mp4", cam_names:list=["left"], output_dir=None, output_name=None): """ Render the frames from a given rollout. Parameters: - rollout_path (str): Absolute path of the rollout (h5/pickle). + path_handle: loaded path object, or path to a rollout file/ directory of rollout files (h5/pickle). render_format (str, optional): Format to save the rendered frames. Default is "mp4". cam_names (list, optional): List of cameras to render. Default is ["left"]. Example ['left', 'right', 'top', 'Franka_wrist'] + output_dir (str, optional): Directory to save the outputs. Defaults to the rollout's directory (or cwd if path_handle is not a file path). + output_name (str, optional): Prefix to use for the output filenames. Defaults to the rollout's name (or "rollout" if path_handle is not a file path). Returns: None @@ -208,31 +368,27 @@ def render(rollout_path, render_format:str="mp4", cam_names:list=["left"]): - The frames are saved in the specified render format. - The rendered frames can be saved as an mp4 video or as individual RGB images. - The frames are rendered for each camera specified in the cam_names list. - - The frames are saved in the same directory as the rollout path. - The output file names are generated based on the rollout name and the camera names. Example: - render(rollout_path="/path/to/rollout.h5", render_format="mp4", cam_names=["left", "right"]) + render(path_handle="/path/to/rollout.h5", render_format="mp4", cam_names=["left", "right"]) """ - output_dir = os.path.dirname(rollout_path) - rollout_name = os.path.split(rollout_path)[-1] - output_name, output_type = os.path.splitext(rollout_name) + if isinstance(path_handle, str) and os.path.isfile(path_handle): + output_dir = output_dir or os.path.dirname(path_handle) or '.' + output_name = output_name or os.path.splitext(os.path.basename(path_handle))[0] + else: + output_dir = output_dir or '.' + output_name = output_name or 'rollout' file_name = os.path.join(output_dir, output_name+"_"+"-".join(cam_names)) - # resolve data format - if output_type=='.h5': - paths = h5py.File(rollout_path, 'r') - elif output_type=='.pickle': - paths = pickle.load(open(rollout_path, 'rb')) - else: - raise TypeError("Unknown path format. Check file") + paths = resolve_path_handle(path_handle) # Run through all trajs in the paths - for i_path, path in enumerate(paths): + for i_path, (path_name, path) in enumerate(paths.items()): - if output_type=='.h5': - data = paths[path]['data'] + if 'data' in path.keys(): + data = path['data'] path_horizon = data['time'].shape[0] else: data = path['env_infos']['obs_dict'] @@ -332,16 +488,17 @@ def path2dataset(path:dict, config_path=None)->dict: # Print h5 schema -def print_h5_schema(obj): - "Recursively find all keys in an h5py.Group." - keys = (obj.name,) - if isinstance(obj, h5py.Group): +def print_h5_schema(obj, name="/"): + "Recursively find all keys in an h5py.Group or a dict-like path object." + keys = (getattr(obj, 'name', name),) + if isinstance(obj, h5py.Group) or isinstance(obj, dict): for key, value in obj.items(): - if isinstance(value, h5py.Group): - keys = keys + print_h5_schema(value) + child_name = getattr(value, 'name', '{}/{}'.format(name, key)) + if isinstance(value, (h5py.Group, dict)): + keys = keys + print_h5_schema(value, name=child_name) else: - print("\t", "{0:35}".format(value.name), value) - keys = keys + (value.name,) + print("\t", "{0:35}".format(child_name), value) + keys = keys + (child_name,) return keys @@ -439,8 +596,9 @@ def pickle2h5(rollout_path, output_dir=None, verify_output=False, h5_format:str= """ @click.command(help=DESC) @click.option('-u', '--util', type=click.Choice(['plot_horizon', 'plot', 'render', 'pickle2h5', 'h5schema']), help='pick utility', required=True) -@click.option('-p', '--path', type=click.Path(exists=True), help='absolute path of the rollout (h5/pickle)', default=None) -@click.option('-e', '--env', type=str, help='Env name', default=None) +@click.option('-p', '--path', type=click.Path(exists=True), help='path_handle: absolute path of a rollout file (h5/pickle), or a directory containing rollout files', default=None) +@click.option('-e', '--env', type=str, help='env_handle: Env name to be created via gym.make (used by plot/plot_horizon)', default=None) +@click.option('-ea', '--env_args', type=str, default=None, help=('env args. E.g. --env_args "{\'is_hardware\':True}"')) @click.option('-on', '--output_name', type=str, default=None, help=('Output name')) @click.option('-od', '--output_dir', type=str, default=None, help=('Directory to save the outputs')) @click.option('-vo', '--verify_output', type=bool, default=False, help=('Verify the saved file')) @@ -450,22 +608,19 @@ def pickle2h5(rollout_path, output_dir=None, verify_output=False, h5_format:str= @click.option('-cn', '--cam_names', multiple=True, help='camera to render. Eg: left, right, top, Franka_wrist', default=["left", "top", "right", "wrist"]) @click.option('-ac', '--add_config', help='Add extra infos to config using as json', default=None) @click.option('-mp', '--max_paths', type=int, help='maximum number of paths to process', default=1e6) -def util_path_cli(util, path, env, output_name, output_dir, verify_output, render_format, cam_names, h5_format, compress_path, add_config, max_paths): +def util_path_cli(util, path, env, env_args, output_name, output_dir, verify_output, render_format, cam_names, h5_format, compress_path, add_config, max_paths): if util=='plot_horizon': - fileName_prefix = os.join(output_dir, output_name) - plot_horizon(path, env, fileName_prefix) + plot_horizon(path, env, output_dir=output_dir, output_name=output_name or '', env_args=env_args) elif util=='plot': - fileName_prefix = os.join(output_dir, output_name) - plot(path, env, fileName_prefix) + plot(path, env, output_dir=output_dir, output_name=output_name or '', env_args=env_args) elif util=='render': - render(rollout_path=path, render_format=render_format, cam_names=cam_names) + render(path_handle=path, render_format=render_format, cam_names=cam_names, output_dir=output_dir, output_name=output_name) elif util=='pickle2h5': pickle2h5(rollout_path=path, output_dir=output_dir, verify_output=verify_output, h5_format=h5_format, compress_path=compress_path, config_path=add_config, max_paths=max_paths) elif util=='h5schema': - with h5py.File(path, "r") as h5file: - print("Printing schema read from output: ", path) - keys = print_h5_schema(h5file) + print("Printing schema for: ", path) + keys = print_h5_schema(resolve_path_handle(path)) else: raise TypeError("Unknown utility requested") From dc2893871f44f1eefa9e0bacaf7bdd5ebfc00996 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Fri, 17 Jul 2026 12:08:16 -0400 Subject: [PATCH 39/49] DOCS: Improve the docs on hardware base class --- 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 4b611c85..4397b69f 100644 --- a/robohive/robot/hardware_base.py +++ b/robohive/robot/hardware_base.py @@ -43,7 +43,11 @@ def connect(self) -> bool: @abc.abstractmethod def okay(self) -> bool: - """Return hardware health""" + """Check if hardware is healthy and return the status""" + + @abc.abstractmethod + def recover(self) -> None: + """Recover hardware from any error, connection loss, failure, etc """ @abc.abstractmethod def close(self) -> bool: @@ -51,7 +55,7 @@ def close(self) -> bool: @abc.abstractmethod def reset(self) -> None: - """Reset hardware""" + """Reset hardware to a known state. Used for resetting the hardware to a known state""" @abc.abstractmethod def get_sensors(self) -> dict: From f8287aacdfc7478960f66c6433d086d97f7a4ef1 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Fri, 24 Jul 2026 18:29:40 -0400 Subject: [PATCH 40/49] Core hardware registry mechanism MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - robohive/robot/hardware_base.py — new HARDWARE_REGISTRY/SENSOR_POSTPROCESS dicts and @register_hardware(type_name, sensor_postprocess=None) decorator (works on classes or factory functions). - robohive/robot/hardware_franka.py, hardware_robotiq.py, hardware_optitrack.py, hardware_realsense.py, hardware_realsense_single.py, hardware_dynamixel.py — each now conforms to hardwareBase (adds recover(), fixes get_sensors() naming/keys to the neutral pos/vel/effort contract) and self-registers via @register_hardware(...). - hardware_optitrack.py also had a pre-existing broken import (darwin.darwin_robot...) fixed to robohive.robot.hardware_base. - hardware_dynamixel.py's Dynamixels was previously non-functional. Added a real adapter. It will need a consolidation effort to reconsile all dynamixel based robots --- robohive/robot/hardware_base.py | 25 ++++++ robohive/robot/hardware_dynamixel.py | 93 +++++++++++++++++---- robohive/robot/hardware_franka.py | 19 +++-- robohive/robot/hardware_optitrack.py | 26 +++++- robohive/robot/hardware_realsense.py | 18 +++- robohive/robot/hardware_realsense_single.py | 11 ++- robohive/robot/hardware_robotiq.py | 15 +++- 7 files changed, 174 insertions(+), 33 deletions(-) diff --git a/robohive/robot/hardware_base.py b/robohive/robot/hardware_base.py index 4397b69f..600b9a7b 100644 --- a/robohive/robot/hardware_base.py +++ b/robohive/robot/hardware_base.py @@ -9,6 +9,31 @@ import abc import warnings +# Registry mapping robot_config's interface['type'] string -> hardwareBase subclass. +# Populated by @register_hardware(type_name) decorators on concrete hardware classes. +HARDWARE_REGISTRY = {} +SENSOR_POSTPROCESS = {} + + +def register_hardware(type_name, sensor_postprocess=None): + """Registers a hardwareBase subclass (or a factory function returning one) against an + interface['type'] string. Usable as a class decorator, or called directly with a + factory function (e.g. one that tries a preferred implementation and falls back to + an alternate one). + + sensor_postprocess(raw: dict) -> np.ndarray, optional: transforms the dict returned by + get_sensors() into other formats. + """ + def _decorator(cls_or_factory): + if isinstance(cls_or_factory, type): + assert issubclass(cls_or_factory, hardwareBase), \ + f"{cls_or_factory} must subclass hardwareBase to be registered" + HARDWARE_REGISTRY[type_name] = cls_or_factory + if sensor_postprocess is not None: + SENSOR_POSTPROCESS[type_name] = sensor_postprocess + return cls_or_factory + return _decorator + class hardwareBase(abc.ABC): diff --git a/robohive/robot/hardware_dynamixel.py b/robohive/robot/hardware_dynamixel.py index 6c769f18..7915cb23 100644 --- a/robohive/robot/hardware_dynamixel.py +++ b/robohive/robot/hardware_dynamixel.py @@ -5,44 +5,101 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================= """ +import time + import numpy as np from dynamixel_py import dxl -from .hardware_base import hardwareBase +from .hardware_base import hardwareBase, register_hardware +@register_hardware('dynamixel') class Dynamixels(hardwareBase): - def __init__(self, name, motor_ids, motor_type, devicename, **kwargs): + """ + Adapter around dynamixel_py's dxl client. A single dynamixel bus is shared across all + of a device's sensors and actuators, and needs per-motor mode ('Position'/'PWM') at + connect time — that per-actuator detail lives in the robot_config device dict (not in + `interface`), so this class opts in to receiving it (see Robot.hardware_init()) rather + than requiring config authors to duplicate motor IDs/modes into `interface` too. This + is a deliberately scoped exception for dynamixel-bus hardware (which genuinely needs + per-motor mode bookkeeping); other hardware classes stay interface-only. + + interface config keys: + motor_type : dynamixel motor series (e.g. 'X') + port : serial port (e.g. '/dev/ttyUSB0') + """ + + def __init__(self, name, motor_type, port, device, **kwargs): self.name = name - # initialize dynamixels - self.dxls = dxl(motor_id=motor_ids, motor_type=motor_type, devicename=devicename) - self.motor_ids = motor_ids + self.motor_type = motor_type + self.port = port + self.device = device + self.motor_ids = np.unique(device['sensor_ids'] + device['actuator_ids']).tolist() + self.dxls = None - def connect(self): + def connect(self) -> bool: """Establish hardware connection""" + self.dxls = dxl(motor_id=self.motor_ids, motor_type=self.motor_type, devicename=self.port) self.dxls.open_port() # set actuator mode - for actuator in device['actuator']: - self.dxls.set_operation_mode(motor_id=[actuator['adr']], mode=actuator['mode']) + for actuator in self.device['actuator']: + self.dxls.set_operation_mode(motor_id=[actuator['hdr_adr']], mode=actuator['mode']) # engage motors - self.dxls.engage_motor(motor_id=self.dxls, enable=True) + self.dxls.engage_motor(motor_id=self.device['actuator_ids'], enable=True) + return True - def okay(self): + def okay(self) -> bool: """Return hardware health""" + return self.dxls is not None + + def recover(self) -> None: + """Recover hardware from any error, connection loss, failure, etc""" + self.close() + self.connect() - def close(self): + def close(self) -> bool: """Close hardware connection""" + if self.dxls: + status = self.dxls.close(self.motor_ids) + if status: + self.dxls = None + return status + return True - def reset(self): - """Reset hardware""" + def reset(self, hw_q=None) -> None: + """Reset hardware to a known state""" + if hw_q is not None: + self.apply_commands(hw_q) - def get_sensors(self): - """Get hardware sensors""" + def get_sensors(self) -> dict: + """Get hardware sensors. 'pos'/'vel' are positionally ordered to match + device['sensor_ids'] (i.e. device['sensor'] declaration order).""" + return { + 'time': time.time(), + 'pos': self.dxls.get_pos(self.device['sensor_ids']), + 'vel': self.dxls.get_vel(self.device['sensor_ids']), + } - def apply_commands(self): - """Apply hardware commands""" + def apply_commands(self, hw_q) -> None: + """Apply hardware commands. hw_q is positionally ordered to match device['actuator'].""" + pos_ids, pos_ctrl, pwm_ids, pwm_ctrl = [], [], [], [] + for i, actuator in enumerate(self.device['actuator']): + val = hw_q[i] + mode = actuator['mode'] + if mode == 'Position': + pos_ids.append(actuator['hdr_adr']) + pos_ctrl.append(val) + elif mode == 'PWM': + pwm_ids.append(actuator['hdr_adr']) + pwm_ctrl.append(val) + else: + raise NotImplementedError(f"Actuator mode {mode} not found") + if pos_ids: + self.dxls.set_des_pos(pos_ids, pos_ctrl) + if pwm_ids: + self.dxls.set_des_pwm(pwm_ids, pwm_ctrl) def __del__(self): - self.close() \ No newline at end of file + self.close() diff --git a/robohive/robot/hardware_franka.py b/robohive/robot/hardware_franka.py index cdba1a10..2a018a81 100644 --- a/robohive/robot/hardware_franka.py +++ b/robohive/robot/hardware_franka.py @@ -15,7 +15,7 @@ from polymetis import RobotInterface import torchcontrol as toco -from robohive.robot.hardware_base import hardwareBase +from robohive.robot.hardware_base import hardwareBase, register_hardware from robohive.utils.min_jerk import generate_joint_space_min_jerk import argparse @@ -56,6 +56,7 @@ def forward(self, state_dict: Dict[str, torch.Tensor]): +@register_hardware('franka') class FrankaArm(hardwareBase): def __init__(self, name, ip_address, gain_scale=1.0, reset_gain_scale=1.0, **kwargs): self.name = name @@ -90,7 +91,7 @@ def connect(self, policy=None): # Create policy instance s_initial = self.get_sensors() policy = JointPDPolicy( - desired_joint_pos=s_initial['joint_pos'], + desired_joint_pos=s_initial['pos'], kp=self.gain_scale * torch.Tensor(self.robot.metadata.default_Kq), kd=self.gain_scale * torch.Tensor(self.robot.metadata.default_Kqd), ) @@ -144,6 +145,12 @@ def reconnect(self): print("Re-connection success") + def recover(self) -> None: + """Recover hardware from any error, connection loss, failure, etc""" + self.reconnect() + self.reset() + + def reset(self, reset_pos=None, time_to_go=5): """Reset hardware""" @@ -157,7 +164,7 @@ def reset(self, reset_pos=None, time_to_go=5): reset_pos = torch.Tensor(reset_pos) # Use registered controller - q_current = self.get_sensors()['joint_pos'] + q_current = self.get_sensors()['pos'] # generate min jerk trajectory dt = 0.1 waypoints = generate_joint_space_min_jerk(start=q_current, goal=reset_pos, time_to_go=time_to_go, dt=dt) @@ -194,7 +201,7 @@ def get_sensors(self) -> dict: print("Failed to get current sensors: ", end="") self.reconnect() return self.get_sensors() - return {'time': time.time(), 'joint_pos': joint_pos, 'joint_vel': joint_vel} + return {'time': time.time(), 'pos': joint_pos, 'vel': joint_vel} def apply_commands(self, q_desired=None, kp=None, kd=None): @@ -252,8 +259,8 @@ def get_args(): # Update policy to execute a sine trajectory on joint 6 for 5 seconds print("Starting sine motion updates...") s_initial = franka.get_sensors() - q_initial = s_initial['joint_pos'].clone() - q_desired = s_initial['joint_pos'].clone() + q_initial = s_initial['pos'].clone() + q_desired = s_initial['pos'].clone() for i in range(int(time_to_go * hz)): q_desired[5] = q_initial[5] + m * np.sin(np.pi * i / (T * hz)) diff --git a/robohive/robot/hardware_optitrack.py b/robohive/robot/hardware_optitrack.py index 31106f3a..b7cf58fa 100644 --- a/robohive/robot/hardware_optitrack.py +++ b/robohive/robot/hardware_optitrack.py @@ -5,7 +5,8 @@ License :: Under Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. ================================================= """ -from darwin.darwin_robot.hardware_base import hardwareBase +from robohive.robot.hardware_base import hardwareBase, register_hardware +from robohive.utils.quat_math import quat2euler import numpy as np import socket import argparse @@ -15,6 +16,17 @@ _USE_UDP = True + +def _optitrack_sensor_postprocess(raw): + c, b, a = quat2euler(raw['quat']) + rx = np.pi - a + rx = (rx - 2*np.pi) if rx > np.pi else rx + ry = b + rz = -c + return np.concatenate([raw['pos'], np.array([rx, ry, rz])]) + + +@register_hardware('optitrack', sensor_postprocess=_optitrack_sensor_postprocess) class OptiTrack(hardwareBase): """ OptiTrack Client: Connects to the server and receives streaming data @@ -27,9 +39,11 @@ class OptiTrack(hardwareBase): # Cached client that is shared for the application lifetime. _OPTI_CLIENT = None - def __init__(self, ip: str, port:int=5000, packet_size:int=36, cache_maxsize:int=0): + def __init__(self, name='optitrack', ip: str = None, client_name: str = None, port:int=5000, packet_size:int=36, cache_maxsize:int=0, **kwargs): + self.name = name if self._OPTI_CLIENT is None: - self.ip = ip + # robot_config's interface dict historically names this key 'client_name' + self.ip = ip if ip is not None else client_name self.port = port self.packet_size = packet_size self._sensor_cache_maxsize = cache_maxsize @@ -93,6 +107,12 @@ def reset(self): self.connect() + def recover(self) -> None: + """Recover hardware from any error, connection loss, failure, etc""" + self.close() + self.connect() + + # [t, id, x, y, z, q0, q1, q2, q3, q4] def read_sensor(self): # receive response diff --git a/robohive/robot/hardware_realsense.py b/robohive/robot/hardware_realsense.py index 17c0541c..7616e735 100644 --- a/robohive/robot/hardware_realsense.py +++ b/robohive/robot/hardware_realsense.py @@ -1,6 +1,6 @@ import numpy as np # from hardware_base import hardwareBase -from robohive.robot.hardware_base import hardwareBase +from robohive.robot.hardware_base import hardwareBase, register_hardware import argparse import a0 @@ -19,6 +19,7 @@ class RealSense(hardwareBase): def __init__(self, name, rgb_topic=None, d_topic=None, **kwargs): assert rgb_topic or d_topic, "Atleast one of the topics is needed" + self.name = name self.rgb_topic = rgb_topic self.d_topic = d_topic self.last_image_pkt = None @@ -27,6 +28,10 @@ def __init__(self, name, rgb_topic=None, d_topic=None, **kwargs): self.most_recent_pkt_ts = None self.timeout = 1 # in seconds + def recover(self) -> None: + """Recover hardware from any error, connection loss, failure, etc""" + self.connect() + def connect(self): # sub to the topics if self.rgb_topic: @@ -96,6 +101,17 @@ def reset(self): return 0 +def _realsense_factory(name, **iface): + """Try the a0-based RealSense first, fall back to the direct pyrealsense2 wrapper.""" + try: + return RealSense(name=name, **iface) + except Exception: + from .hardware_realsense_single import RealsenseAPI + return RealsenseAPI(name=name, **iface) + + +register_hardware('realsense')(_realsense_factory) + # Get inputs from user def get_args(): diff --git a/robohive/robot/hardware_realsense_single.py b/robohive/robot/hardware_realsense_single.py index cb18472d..e64ec1a3 100644 --- a/robohive/robot/hardware_realsense_single.py +++ b/robohive/robot/hardware_realsense_single.py @@ -2,11 +2,14 @@ import pyrealsense2 as rs from collections import OrderedDict +from robohive.robot.hardware_base import hardwareBase -class RealsenseAPI: + +class RealsenseAPI(hardwareBase): """Wrapper that implements boilerplate code for RealSense cameras""" - def __init__(self, device_id=None, height=480, width=640, fps=30, warm_start=30, type=None,): + def __init__(self, name='realsense', device_id=None, height=480, width=640, fps=30, warm_start=30, type=None, **kwargs): + self.name = name self.height = height self.width = width self.fps = fps @@ -73,6 +76,10 @@ def get_sensors(self): def okay(self): return True + def recover(self) -> None: + """Recover hardware from any error, connection loss, failure, etc""" + self.connect() + def apply_commands(self): return 0 diff --git a/robohive/robot/hardware_robotiq.py b/robohive/robot/hardware_robotiq.py index 689568e6..df10a837 100644 --- a/robohive/robot/hardware_robotiq.py +++ b/robohive/robot/hardware_robotiq.py @@ -1,11 +1,12 @@ from enum import Flag from polymetis import GripperInterface -from robohive.robot.hardware_base import hardwareBase +from robohive.robot.hardware_base import hardwareBase, register_hardware import numpy as np import argparse import time +@register_hardware('robotiq') class Robotiq(hardwareBase): def __init__(self, name, ip_address, **kwargs): self.name = name @@ -81,6 +82,11 @@ def reconnect(self): print("RBQ:> Re-connection success") + def recover(self) -> None: + """Recover hardware from any error, connection loss, failure, etc""" + self.reconnect() + + def reset(self, width=None, **kwargs): """Reset hardware""" if not width: @@ -96,9 +102,12 @@ def get_sensors(self) -> dict: print("RBQ:> Failed to get current sensors: ", end="") self.reconnect() return self.get_sensors() - return {'time': time.time(), 'width': np.array([curr_state.width])} + return {'time': time.time(), 'pos': np.array([curr_state.width])} - def apply_commands(self, width:float, speed:float=0.1, force:float=0.1): + def apply_commands(self, width, speed:float=0.1, force:float=0.1): + # width may be a scalar or a length-1 array-like (Robot.hardware_apply_controls + # always passes an array positionally matching this device's single actuator). + width = float(np.asarray(width).reshape(-1)[0]) assert width>=0.0 and width<=self.max_width, "Gripper desired width ({}) is out of bound (0,{})".format(width, self.max_width) self.robot.goto(width=width, speed=speed, force=force) return 0 From ad258553f8a33e66875d513df1a0082ecd59c82d Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Fri, 24 Jul 2026 18:41:47 -0400 Subject: [PATCH 41/49] =?UTF-8?q?Abiding=20by=20hardware=20base=20class's?= =?UTF-8?q?=20contract:=20-=20hardware=5Finit,=20hardware=5Fget=5Fsensors,?= =?UTF-8?q?=20hardware=5Fapply=5Fcontrols,=20hardware=5Fclose=20collapsed?= =?UTF-8?q?=20from=20hardcoded=20if/elif=20chains=20on=20interface['type']?= =?UTF-8?q?=20into=20uniform=20registry-driven=20loops.=20-=20New=20Robot.?= =?UTF-8?q?hardware=5Freset(reset=5Fpos):=20a=20dedicated=20blocking/large?= =?UTF-8?q?-displacement=20reset=20path=20(each=20hardware=20class=20runs?= =?UTF-8?q?=20its=20own=20min-jerk=20trajectory),=20replacing=20the=20old?= =?UTF-8?q?=20hardware=5Fapply=5Fcontrols(...,=20is=5Freset=3DTrue)=20over?= =?UTF-8?q?load.=20-=20Fixes=20a=20real=20bug=20where=20reset=20indexed=20?= =?UTF-8?q?by=20hdr=5Fid=20(counts=20all=20actuators)=20while=20the=20ctrl?= =?UTF-8?q?=20vector=20it=20built=20only=20counted=20qpos-type=20actuators?= =?UTF-8?q?=20=E2=80=94=20silently=20misaligned=20on=20any=20device=20mixi?= =?UTF-8?q?ng=20qpos=20and=20tendon=20actuators=20(e.g.=20arm+gripper).=20?= =?UTF-8?q?-=20get=5Fvisual=5Fsensors()=20now=20duck-types=20hasattr(devic?= =?UTF-8?q?e['robot'],=20'get=5Fframe')=20instead=20of=20asserting=20inter?= =?UTF-8?q?face['type']=20=3D=3D=20'realsense',=20so=20UVC/webcam-style=20?= =?UTF-8?q?cameras=20(rgb-only,=20no=20depth)=20no=20longer=20crash=20it.?= =?UTF-8?q?=20-=20adr=20renamed=20to=20hdr=5Fadr=20throughout=20(configure?= =?UTF-8?q?=5Frobot(),=20camera=20calibration)=20=E2=80=94=20now=20consist?= =?UTF-8?q?ently=20documented=20as=20a=20physical=20hardware=20address=20(?= =?UTF-8?q?motor=20ID,=20RTDE=20join-=20adr=20renamed=20to=20hdr=5Fadr=20t?= =?UTF-8?q?hroughout=20(confe=20raise=20NotImplemented=20=E2=86=92=20raise?= =?UTF-8?q?=20NotImplementedError(...)=20with=20descriptive=20messages.?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robohive/robot/robot.py | 240 +++++++++++++--------------------------- 1 file changed, 79 insertions(+), 161 deletions(-) diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index 12e5338e..741e7feb 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -5,6 +5,7 @@ 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 inspect import os import time from collections import deque @@ -13,8 +14,8 @@ import numpy as np from robohive.physics.sim_scene import SimScene +from robohive.robot.hardware_base import HARDWARE_REGISTRY, SENSOR_POSTPROCESS from robohive.utils.prompt_utils import Prompt, prompt -from robohive.utils.quat_math import quat2euler np.set_printoptions(precision=4) @@ -35,9 +36,9 @@ # 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) + # sim_id: ID (defined by order in mujoco's xml) of the sensor/actuator in the sim + # hdr_id: ID (defined by the order in .config) of the sensor/actuator in the robot_config (hardware) space (robot_config unifies different hardware into a single unified hardware space) + # hdr_adr: Physical address of the sensor/actuator (defined by hardware specs) (e.g. dynamixel - This is the address/motor_id used to communicate with the individual motors) @@ -127,63 +128,26 @@ def hardware_init(self, robot_config): # initalize for name, device in robot_config.items(): prompt("Initializing device: %s"%(name), 'white', 'on_grey') - if device['interface']['type'] == 'dynamixel': - # initialize dynamixels - from dynamixel_py import dxl - ids = np.unique([device['sensor_ids'] + device['actuator_ids']]).tolist() - device['robot'] = dxl(motor_id=ids, motor_type=\ - device['interface']['motor_type'], devicename= device['interface']['name']) - - # from .hardware_dynamixel import Dynamixels - # motor_ids = np.unique([device['sensor_ids'] + device['actuator_ids']]).tolist() - # device['robot'] = Dynamixels(name=name, motor_ids=motor_ids, motor_type=device['interface']['motor_type'], devicename= device['interface']['name']) - - elif device['interface']['type'] == 'optitrack': - from .hardware_optitrack import OptiTrack - device['robot'] = OptiTrack(ip=device['interface']['client_name'], \ - port=device['interface']['port'], packet_size=device['interface']['packet_size']) - - elif device['interface']['type'] == 'franka': - from .hardware_franka import FrankaArm - device['robot'] = FrankaArm(name=name, **device['interface']) - - elif device['interface']['type'] == 'realsense': - try: - from .hardware_realsense import RealSense - device['robot'] = RealSense(name=name, **device['interface']) - except: - from .hardware_realsense_single import RealsenseAPI - device['robot'] = RealsenseAPI(**device['interface']) - - elif device['interface']['type'] == 'robotiq': - from .hardware_robotiq import Robotiq - device['robot'] = Robotiq(name=name, **device['interface']) - - else: - print("ERROR: interface ({}) not found".format(device['interface']['type'])) - raise NotImplemented + itype = device['interface']['type'] + cls = HARDWARE_REGISTRY.get(itype) + if cls is None: + raise NotImplementedError( + "No hardwareBase registered for interface.type={!r}. Registered types: {}. " + "Import the module that registers this type before constructing " + "Robot(is_hardware=True).".format(itype, sorted(HARDWARE_REGISTRY))) + iface = {k: v for k, v in device['interface'].items() if k != 'type'} + # Most hardware classes are self-contained (ip/port/etc. from `interface` is + # enough). A few (dynamixel-bus devices, which manage several motors sharing + # one connection and need per-motor address/mode bookkeeping) opt in to + # receiving the full robot_config device dict by naming `device` as a + # constructor parameter; everyone else never sees it. + if 'device' in inspect.signature(cls).parameters: + iface['device'] = device + device['robot'] = cls(name=name, **iface) # start all hardware for name, device in robot_config.items(): - - # Dynamixels - if device['interface']['type'] == 'dynamixel': - device['robot'].open_port() - - # set actuator mode - for actuator in device['actuator']: - device['robot'].set_operation_mode(motor_id=[actuator['adr']], mode=actuator['mode']) - - # engage motors - device['robot'].engage_motor(motor_id=device['actuator_ids'], enable=True) - - # Other devices - elif device['interface']['type'] in ['optitrack', 'franka', 'realsense', 'robotiq']: - device['robot'].connect() - - else: - print("ERROR: interface ({}) not found".format(device['interface']['type'])) - raise NotImplementedError + device['robot'].connect() return robot_config @@ -194,115 +158,67 @@ def hardware_get_sensors(self): current_sensor_value['time'] = time.time() - self.time_start for name, device in self.robot_config.items(): if 'sensor' in device.keys() and len(device['sensor'])>0: - # get sensors - if device['interface']['type'] == 'dynamixel': - # TODO: choose between pos, vel, or posvel - current_sensor_value[name] = device['robot'].get_pos(device['sensor_ids']) - current_sensor_value[name+'_vel'] = device['robot'].get_vel(device['sensor_ids']) - - elif device['interface']['type'] == 'optitrack': - data = device['robot'].get_sensors() - c, b, a = quat2euler(data['quat']) - rx = np.pi - a - rx = (rx - 2*np.pi) if rx > np.pi else rx - ry = b - rz = -c - # print("Pos:", x, y, z) - # print("Rotations:", rx, ry, rz) - current_sensor_value[name] = np.concatenate([data['pos'], np.array([rx, ry, rz])]) - # current_sensor_value[name] = np.array([x, y, z, 0, 0, 0]) - # current_sensor_value[name] = np.array([x, y, z, -(a+np.pi/2), -c, -b]) - - elif device['interface']['type'] == 'franka': - sensors = device['robot'].get_sensors() - current_sensor_value[name] = np.concatenate([sensors['joint_pos'], sensors['joint_vel']]) - - elif device['interface']['type'] == 'robotiq': - sensors = device['robot'].get_sensors() - current_sensor_value[name] = sensors - - else: - print("ERROR: interface ({}) not found".format(device['interface']['type'])) - raise NotImplementedError - - # calibrate sensors + itype = device['interface']['type'] + raw = device['robot'].get_sensors() + postprocess = SENSOR_POSTPROCESS.get(itype, lambda raw: raw['pos']) + vals = postprocess(raw) + + # calibrate sensors. vals is positionally ordered to match device['sensor'] + # (both built by iterating the same config-declared list, see configure_robot()). + arr = np.empty(len(device['sensor']), dtype=np.float64) for id, sensor in enumerate(device['sensor']): - current_sensor_value[name][id] = current_sensor_value[name][id]*sensor['scale'] + sensor['offset'] - device['sensor_data'] = current_sensor_value[name] + arr[id] = vals[id]*sensor['scale'] + sensor['offset'] + current_sensor_value[name] = arr + device['sensor_data'] = arr device['sensor_time'] = current_sensor_value['time'] return current_sensor_value # apply controls to hardware - def hardware_apply_controls(self, control, space='hdr', is_reset=False): + def hardware_apply_controls(self, control, space='hdr'): """ + Send one dt's worth of (already-clipped, locally-achievable) controls to hardware. control: control vector in hdr or sim space space: 'hdr' or 'sim' (defaults to 'hdr' as represented in robot_config) - is_reset: if True, reset the hardware to the control values """ for name, device in self.robot_config.items(): if 'actuator' in device.keys() and len(device['actuator'])>0: - if device['interface']['type'] == 'dynamixel': - # group as per mode - pos_ctrl = [] - pos_ids = [] - pwm_ctrl = [] - pwm_ids = [] - for actuator in device['actuator']: - 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['adr']) - pos_ctrl.append(calib_ctrl) - elif actuator['mode'] == 'PWM': - pwm_ids.append(actuator['adr']) - pwm_ctrl.append(calib_ctrl) - else: - print("ERROR: Mode not found") - 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'] in ['franka', 'robotiq']: - des_pos = [] - for actuator in device['actuator']: - 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: - device['robot'].reset(des_pos) - else: - device['robot'].apply_commands(des_pos) - else: - raise NotImplementedError("ERROR: interface not found") + # hw_q is positionally ordered to match device['actuator'] (both built by + # iterating the same config-declared list, see configure_robot()). + hw_q = [] + for actuator in device['actuator']: + ctrl = control[actuator['sim_id']] if space == 'sim' else control[actuator['hdr_id']] + hw_q.append(ctrl*actuator['scale'] + actuator['offset']) + device['robot'].apply_commands(hw_q) + # move actuated dofs to a target position (blocking, large-displacement — distinct from + # the per-dt hardware_apply_controls above; hardware classes implement this via their own + # min-jerk/via-point trajectories) + def hardware_reset(self, reset_pos): + for name, device in self.robot_config.items(): + if name == 'default_robot': + continue + qpos_actuators = [a for a in device.get('actuator', []) if a['data_type'] == 'qpos'] + if qpos_actuators: + hw_q = [np.clip(reset_pos[a['data_id']], a['pos_range'][0], a['pos_range'][1]) + for a in qpos_actuators] + device['robot'].reset(hw_q) + else: + # passive device (tendon-driven gripper, camera, etc.) — no qpos target to + # compute; let the device bring itself to its own known reset state + device['robot'].reset() + # close hardware def hardware_close(self): status = True for name, device in self.robot_config.items(): - if device['interface']['type'] == 'dynamixel': - if device['robot']: - print("Closing dynamixel connection") - ids = np.unique([device['sensor_ids'] + device['actuator_ids']]).tolist() - status = device['robot'].close(ids) - if status is True: - device['robot']= None - elif device['interface']['type'] in ['optitrack', 'franka', 'realsense', 'robotiq']: - if device['robot']: - print("Closing {} connection".format(device['interface']['type'])) - status = device['robot'].close() - if status is True: - device['robot']= None - else: - print("ERROR: interface not found") - raise NotImplemented - + if device.get('robot'): + print("Closing {} connection".format(device['interface']['type'])) + status = device['robot'].close() + if status is True: + device['robot'] = None return status @@ -333,7 +249,7 @@ def configure_robot(self, sim, config_path): 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 + device['sensor_ids'].append(sensor['hdr_adr']) # list of all ids sensor_type = sim.model.sensor_type[sensor['sim_id']] sensor_objid = sim.model.sensor_objid[sensor['sim_id']] # sensordata_id: address in sim.data.sensordata for this sensor. @@ -360,7 +276,7 @@ def configure_robot(self, sim, config_path): 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 + device['actuator_ids'].append(actuator['hdr_adr']) # list of all ids actuator_trntype = sim.model.actuator_trntype[actuator['sim_id']] actuator_trnid = sim.model.actuator_trnid[actuator['sim_id'], 0] if actuator_trntype == mujoco.mjtTrn.mjTRN_JOINT: # // force on joint @@ -456,8 +372,14 @@ def get_visual_sensors(self, height:int, width:int, cameras:list, device_id:int, for ind, cam_name in enumerate(cameras): assert cam_name in self.robot_config.keys(), "{} camera not found".format(cam_name) device = self.robot_config[cam_name] - assert device['interface']['type'] == 'realsense', "Check interface type for {}".format(cam) - data = device['robot'].get_sensors() + + if hasattr(device['robot'], 'get_frame'): + # RGB-only camera (e.g. a UVC/webcam) — no depth stream. + rgb = device['robot'].get_frame() + data = {'time': time.time() - self.time_start, 'rgb': rgb, 'd': None} + else: + # RealSense-style camera: get_sensors() itself returns {'rgb','d'}. + data = device['robot'].get_sensors() data_height = data['rgb'].shape[0] assert data_height == height, "Incorrect image height: required:{}, found:{}".format(height, data_height) data_width = data['rgb'].shape[1] @@ -466,11 +388,12 @@ def get_visual_sensors(self, height:int, width:int, cameras:list, device_id:int, # calibrate sensors for cam in device['cam']: - current_sensor_value[cam_name][cam['adr']] = current_sensor_value[cam_name][cam['adr']]*cam['scale'] + cam['offset'] + current_sensor_value[cam_name][cam['hdr_adr']] = current_sensor_value[cam_name][cam['hdr_adr']]*cam['scale'] + cam['offset'] device['sensor_data'] = current_sensor_value[cam_name] device['sensor_time'] = current_sensor_value['time'] imgs[ind, :, :, :] = current_sensor_value[cam_name]['rgb'] - depths[ind, :, :] = current_sensor_value[cam_name]['d'][:,:,0] # assumes single channel depth + if current_sensor_value[cam_name]['d'] is not None: + depths[ind, :, :] = current_sensor_value[cam_name]['d'][:,:,0] # assumes single channel depth else: imgs = np.zeros((len(cameras), height, width, 3), dtype=np.uint8) @@ -759,14 +682,12 @@ def reset(self, # for passive dofs => sensor specs feasibe_pos = reset_pos.copy() feasibe_vel = reset_vel.copy() - ctrl_feasible=[] for name, device in self.robot_config.items(): if name != "default_robot": if len(device['actuator'])>0: # actuated dofs for actuator in device['actuator']: if actuator['data_type'] == 'qpos': feasibe_pos[actuator['data_id']] = np.clip(reset_pos[actuator['data_id']], actuator['pos_range'][0], actuator['pos_range'][1]) - ctrl_feasible.append(feasibe_pos[actuator['data_id']]) else: # passive dofs for sensor in device['sensor']: if sensor['data_type'] == 'qpos': @@ -778,11 +699,8 @@ def reset(self, t_reset_start = time.time() prompt("\nRollout took:{}".format(t_reset_start- self.time_start)) prompt("\aResetting {}: ".format(self.name), 'white', 'on_grey', flush=True, end="") - # send request to the actuated dofs - self.hardware_apply_controls(ctrl_feasible, is_reset=True) - - # engage other reset mechanisms for passive dofs - # TODO raise NotImplementedError + # send request to all devices, actuated and passive alike + self.hardware_reset(feasibe_pos) if blocking: input("press a key to start rollout") From 34ba2dcd1adf5eae97f8815840fba93da9ae5dce Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Fri, 24 Jul 2026 18:52:57 -0400 Subject: [PATCH 42/49] MAJOR: Better Env initializationm especially when backed by hardware MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Added robot_cls = kwargs.pop('robot_cls', Robot) extension point so downstream repos never need to subclass/swap Robot. - Added optional init_qpos param — if provided, used directly as self.init_qpos; otherwise falls back to today's auto-computed mid-actuator-range behavior. Warns if hardware-backed with no explicit init_qpos (homing to an implicit pose is likely unintended on real hardware). - Swapped the startup call from self.step(np.zeros(...)) to self.reset() + self.forward(), so hardware now goes through Robot.reset()'s min-jerk hardware_reset() on env construction instead of jumping straight to a raw zero-ctrl command from wherever the robot currently is. --- robohive/envs/env_base.py | 44 ++++++++++++++++++++++++--------------- 1 file changed, 27 insertions(+), 17 deletions(-) diff --git a/robohive/envs/env_base.py b/robohive/envs/env_base.py index cd7edd19..6e9afac2 100644 --- a/robohive/envs/env_base.py +++ b/robohive/envs/env_base.py @@ -74,6 +74,7 @@ def _setup(self, obs_range:tuple = (-10, 10), # Permissible range of values in obs vector returned by get_obs() rwd_viz:bool = False, # Visualize rewards (WIP, needs vtils) device_id:int = 0, # Device id for rendering + init_qpos = None, # Explicit reset/home qpos. Auto-computed (mid actuator range) if not provided **kwargs, # Additional arguments ): @@ -87,7 +88,8 @@ def _setup(self, self.viewer_setup() # resolve robot config - self.robot = Robot(mj_sim=self.sim, + robot_cls = kwargs.pop('robot_cls', Robot) + self.robot = robot_cls(mj_sim=self.sim, random_generator=self.np_random, **kwargs) @@ -100,18 +102,25 @@ def _setup(self, # resolve initial state self.init_qvel = self.sim.data.qvel.ravel().copy() - self.init_qpos = self.sim.data.qpos.ravel().copy() # has issues with initial jump during reset - # self.init_qpos = np.mean(self.sim.model.actuator_ctrlrange, axis=1) if self.normalize_act else self.sim.data.qpos.ravel().copy() # has issues when nq!=nu - # self.init_qpos[self.sim.model.jnt_dofadr] = np.mean(self.sim.model.jnt_range, axis=1) if self.normalize_act else self.sim.data.qpos.ravel().copy() - if self.normalize_act: - # find all linear+actuated joints. Use mean(jnt_range) as init position - actuated_jnt_ids = self.sim.model.actuator_trnid[self.sim.model.actuator_trntype==self.sim.lib.mjtTrn.mjTRN_JOINT, 0] # dm - linear_jnt_ids = np.logical_or(self.sim.model.jnt_type==self.sim.lib.mjtJoint.mjJNT_SLIDE, self.sim.model.jnt_type==self.sim.lib.mjtJoint.mjJNT_HINGE) - linear_jnt_ids = np.where(linear_jnt_ids==True)[0] - linear_actuated_jnt_ids = np.intersect1d(actuated_jnt_ids, linear_jnt_ids) - # assert np.any(actuated_jnt_ids==linear_actuated_jnt_ids), "Wooho: Great evidence that it was important to check for actuated_jnt_ids as well as linear_actuated_jnt_ids" - linear_actuated_jnt_qposids = self.sim.model.jnt_qposadr[linear_actuated_jnt_ids] - self.init_qpos[linear_actuated_jnt_qposids] = np.mean(self.sim.model.jnt_range[linear_actuated_jnt_ids], axis=1) + if init_qpos is not None: + # use the provided init_pos + self.init_qpos = np.array(init_qpos, dtype=np.float64).ravel().copy() + else: + # create one if not provided + self.init_qpos = self.sim.data.qpos.ravel().copy() # has issues with initial jump during reset + if self.normalize_act: + # find all linear+actuated joints. Use mean(jnt_range) as init position + actuated_jnt_ids = self.sim.model.actuator_trnid[self.sim.model.actuator_trntype==self.sim.lib.mjtTrn.mjTRN_JOINT, 0] # dm + linear_jnt_ids = np.logical_or(self.sim.model.jnt_type==self.sim.lib.mjtJoint.mjJNT_SLIDE, self.sim.model.jnt_type==self.sim.lib.mjtJoint.mjJNT_HINGE) + linear_jnt_ids = np.where(linear_jnt_ids==True)[0] + linear_actuated_jnt_ids = np.intersect1d(actuated_jnt_ids, linear_jnt_ids) + linear_actuated_jnt_qposids = self.sim.model.jnt_qposadr[linear_actuated_jnt_ids] + self.init_qpos[linear_actuated_jnt_qposids] = np.mean(self.sim.model.jnt_range[linear_actuated_jnt_ids], axis=1) + if self.robot.is_hardware: + prompt(f"WARNING: {self.robot.name} is hardware-backed but no init_qpos was provided — " + "defaulting to the mid-range actuator pose. Pass init_qpos explicitly " + "to control where the robot homes to on reset.", + type=Prompt.WARN) # resolve rewards self.rwd_dict = {} @@ -131,10 +140,11 @@ def _setup(self, self.visual_keys = visual_keys if type(visual_keys)==list or visual_keys==None else [visual_keys] self._setup_rgb_encoders(self.visual_keys, device=None) - # reset to get the env ready - observation, _reward, done, *_, _info = self.step(np.zeros(self.sim.model.nu)) - # Question: Should we replace above with following? Its specially helpful for hardware as it forces a env reset before continuing, without which the hardware will make a big jump from its position to the position asked by step. - # observation = self.reset() + # reset to get the env ready. Using reset() rather than step(zeros) routes hardware + # through Robot.reset()'s min-jerk hardware_reset(), instead of jumping straight to + # a raw ctrl command from whatever pose the robot is currently in. + self.reset() + observation, _reward, done, *_, _info = self.forward() assert not done, "Check initialization. Simulation starts in a done state." self.observation_space = gym.spaces.Box(obs_range[0]*np.ones(observation.size), obs_range[1]*np.ones(observation.size), dtype=np.float32) From 99d0420eb18309c5f73ebdffe10102f845b1efc8 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Fri, 24 Jul 2026 18:54:56 -0400 Subject: [PATCH 43/49] UPDATE: Adjusting configs to comply with changes and renaming --- .../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 | 38 ++++---- robohive/envs/fm/assets/franka_dmanus.config | 70 +++++++-------- 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 | 86 +++++++++---------- .../quadrupeds/dkitty/dkitty_walk_v0.config | 86 +++++++++---------- 12 files changed, 361 insertions(+), 361 deletions(-) diff --git a/robohive/envs/arms/fetch/assets/fetch_reach_v0.config b/robohive/envs/arms/fetch/assets/fetch_reach_v0.config index b35c8ae3..aab9fd2f 100644 --- a/robohive/envs/arms/fetch/assets/fetch_reach_v0.config +++ b/robohive/envs/arms/fetch/assets/fetch_reach_v0.config @@ -3,27 +3,27 @@ 'franka':{ 'interface': {'type': 'fetch'}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, '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'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_pan_jp'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_lift_jp'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'upperarm_roll_jp'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'elbow_flex_jp'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'forearm_roll_jp'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'wrist_flex_jp'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'wrist_roll_jp'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_jp'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_jp'}, ], 'actuator':[ - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), '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'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_pan'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'shoulder_lift'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'upperarm_roll'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'elbow_flex'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'forearm_roll'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'wrist_flex'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'wrist_roll'}, + {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger'}, + {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger'}, ] } } \ No newline at end of file diff --git a/robohive/envs/arms/franka/assets/franka_busbin_v0.config b/robohive/envs/arms/franka/assets/franka_busbin_v0.config index 2904463b..bf2894e8 100644 --- a/robohive/envs/arms/franka/assets/franka_busbin_v0.config +++ b/robohive/envs/arms/franka/assets/franka_busbin_v0.config @@ -3,39 +3,39 @@ 'franka':{ 'interface': {'type': 'franka', 'ip_address':'169.254.163.91'}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, '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_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, - {'range':(-2.9, 2.9), 'noise':0.05, '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'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, ], 'actuator':[ - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), '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':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, ] }, # 'busbin':{ # 'interface': {}, # 'sensor':[ - # {'range':(-2.0, 0.0), 'noise':0.05, 'adr':7, 'scale':1, 'offset':0, 'name':'???'}, + # {'range':(-2.0, 0.0), 'noise':0.05, 'hdr_adr':7, 'scale':1, 'offset':0, 'name':'???'}, # ], # 'actuator':[] # } diff --git a/robohive/envs/arms/franka/assets/franka_reach_v0.config b/robohive/envs/arms/franka/assets/franka_reach_v0.config index 04f0819b..87b489cd 100644 --- a/robohive/envs/arms/franka/assets/franka_reach_v0.config +++ b/robohive/envs/arms/franka/assets/franka_reach_v0.config @@ -3,37 +3,37 @@ 'franka':{ 'interface': {'type': 'franka', 'ip_address':'172.16.0.1', 'gain_scale':0.5}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, '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.9, 2.9), 'noise':0.05, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, + # {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':7, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, + # {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':8, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, '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'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, ], 'actuator':[ - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), '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'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, + # {'pos_range':(-0.0000, 0.0400), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_joint'}, + # {'pos_range':(-0.0000, 0.0400), 'vel_range':(-1.0*np.pi/2, 1.0*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_joint'}, ] }, @@ -41,8 +41,8 @@ 'interface': {'type': 'realsense', 'topic':'realsense_815412070228/color/image_raw', 'data_type':'rgb240x320'}, 'sensor':[], 'cam': [ - {'range':(0, 255), 'noise':0.00, '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'}, + {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + # {'range':(0, 255), 'noise':0.00, 'hdr_adr':'d', 'scale':1, 'offset':0, 'name':'/depth_mono/image_raw'}, ], 'actuator':[] }, @@ -51,8 +51,8 @@ 'interface': {'type': 'realsense', 'topic':'realsense_815412070341/color/image_raw', 'data_type':'rgb'}, 'sensor':[], 'cam': [ - {'range':(0, 255), 'noise':0.00, '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'}, + {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + # {'range':(0, 255), 'noise':0.00, 'hdr_adr':'d', 'scale':1, 'offset':0, 'name':'/depth_mono/image_raw'}, ], 'actuator':[] }, @@ -61,8 +61,8 @@ 'interface': {'type': 'realsense', 'topic':'realsense_936322070233/color/image_raw', 'data_type':'rgb'}, 'sensor':[], 'cams': [ - {'range':(0, 255), 'noise':0.00, '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'}, + {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + # {'range':(0, 255), 'noise':0.00, 'hdr_adr':'d', 'scale':1, 'offset':0, 'name':'/depth_mono/image_raw'}, ], 'actuator':[] }, diff --git a/robohive/envs/arms/franka/assets/franka_ycb_v0.config b/robohive/envs/arms/franka/assets/franka_ycb_v0.config index 81b49b42..e9a9297c 100644 --- a/robohive/envs/arms/franka/assets/franka_ycb_v0.config +++ b/robohive/envs/arms/franka/assets/franka_ycb_v0.config @@ -3,48 +3,48 @@ 'franka':{ 'interface': {'type': 'franka', 'ip_address':'169.254.163.91'}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, '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.9, 2.9), 'noise':0.05, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':7, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':8, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, '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'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, ], 'actuator':[ - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), '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'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, + {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_joint'}, + {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_joint'}, ] }, 'object':{ 'interface':{}, 'sensor':[ - {'range':(-1.0, 1.0), 'noise':0.005, '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'}, + {'range':(-1.0, 1.0), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Tx'}, + {'range':(-0.5, 0.5), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Ty'}, + {'range':(-1.0, 1.0), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Tz'}, + {'range':(-3.1, 3.1), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Rx'}, + {'range':(-3.1, 3.1), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Ry'}, + {'range':(-3.1, 3.1), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'Rz'}, ], 'actuator':[] } diff --git a/robohive/envs/fm/assets/dmanus.config b/robohive/envs/fm/assets/dmanus.config index a0bf3062..7a307820 100644 --- a/robohive/envs/fm/assets/dmanus.config +++ b/robohive/envs/fm/assets/dmanus.config @@ -1,29 +1,29 @@ { # device1: sensors, actuators 'dmanus':{ - 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/dev/ttyUSB0"}, + 'interface': {'type': 'dynamixel', 'motor_type':"X", 'port':"/dev/ttyUSB0"}, 'sensor':[ - {'range':(-0.75, 0.57), 'noise':0.05, '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 }, + {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':10, 'name':'TFJ1', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':11, 'name':'TFJ2', 'scale':-1, 'offset':3*np.pi/2 }, + {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':12, 'name':'TFJ3', 'scale':-1, 'offset':np.pi }, + {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':20, 'name':'IFJ1', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':21, 'name':'IFJ2', 'scale':-1, 'offset':3*np.pi/2 }, + {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':22, 'name':'IFJ3', 'scale':+1, 'offset':-np.pi }, + {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':30, 'name':'LFJ1', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':31, 'name':'LFJ2', 'scale':+1, 'offset':-np.pi/2 }, + {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':32, 'name':'LFJ3', 'scale':+1, 'offset':-np.pi }, ], 'actuator':[ - {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), '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 }, + {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'name':'TFA1', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'name':'TFA2', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 }, + {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'name':'TFA3', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'name':'IFA1', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'name':'IFA2', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 }, + {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'name':'IFA3', 'mode':'Position', 'scale':+1, 'offset':np.pi }, + {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'name':'LFA1', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'name':'LFA2', 'mode':'Position', 'scale':+1, 'offset':np.pi/2 }, + {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'name':'LFA3', 'mode':'Position', 'scale':+1, 'offset':np.pi }, ] } } \ No newline at end of file diff --git a/robohive/envs/fm/assets/franka_dmanus.config b/robohive/envs/fm/assets/franka_dmanus.config index 2384ee5b..478e3d1e 100644 --- a/robohive/envs/fm/assets/franka_dmanus.config +++ b/robohive/envs/fm/assets/franka_dmanus.config @@ -5,53 +5,53 @@ 'interface': {'type': 'franka', 'ip_address':'172.16.0.1', 'gain_scale':0.2}, # 'interface': {'type': 'franka', 'ip_address':'169.254.163.91'}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, '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_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, ], 'actuator':[ - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), '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':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, ] }, # device1: sensors, actuators 'dmanus':{ - 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/dev/ttyUSB0"}, + 'interface': {'type': 'dynamixel', 'motor_type':"X", 'port':"/dev/ttyUSB0"}, 'sensor':[ - {'range':(-0.75, 0.57), 'noise':0.05, '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_adr':10, 'name':'TF_ADB_jp', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':11, 'name':'TF_MCP_jp', 'scale':-1, 'offset':3*np.pi/2 }, + {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':12, 'name':'TF_PIP_jp', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':13, 'name':'TF_DIP_jp', 'scale':-1, 'offset':np.pi }, - {'range':(-0.75, 0.57), 'noise':0.05, '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 }, + {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':20, 'name':'FF_ADB_jp', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':21, 'name':'FF_MCP_jp', 'scale':-1, 'offset':3*np.pi/2 }, + {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':22, 'name':'FF_PIP_jp', 'scale':+1, 'offset':-np.pi }, + {'range':(-0.75, 0.57), 'noise':0.05, 'hdr_adr':30, 'name':'PF_ADB_jp', 'scale':-1, 'offset':np.pi }, + {'range':(-0.00, 2.14), 'noise':0.05, 'hdr_adr':31, 'name':'PF_MCP_jp', 'scale':+1, 'offset':-np.pi/2 }, + {'range':(-0.00, 2.00), 'noise':0.05, 'hdr_adr':32, 'name':'PF_PIP_jp', 'scale':+1, 'offset':-np.pi }, ], 'actuator':[ - {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), '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 }, + {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'name':'TF_ADB', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'name':'TF_MCP', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 }, + {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'name':'TF_PIP', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':13, 'name':'TF_DIP', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'name':'FF_ADB', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'name':'FF_MCP', 'mode':'Position', 'scale':-1, 'offset':3*np.pi/2 }, + {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'name':'FF_PIP', 'mode':'Position', 'scale':+1, 'offset':np.pi }, + {'pos_range':(-0.75, 0.57), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'name':'PF_ADB', 'mode':'Position', 'scale':-1, 'offset':np.pi }, + {'pos_range':(-0.00, 2.14), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'name':'PF_MCP', 'mode':'Position', 'scale':+1, 'offset':np.pi/2 }, + {'pos_range':(-0.00, 2.00), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'name':'PF_PIP', 'mode':'Position', 'scale':+1, 'offset':np.pi }, ] } } \ No newline at end of file diff --git a/robohive/envs/fm/assets/franka_robotiq.config b/robohive/envs/fm/assets/franka_robotiq.config index 7ed26bb2..73d68220 100644 --- a/robohive/envs/fm/assets/franka_robotiq.config +++ b/robohive/envs/fm/assets/franka_robotiq.config @@ -4,23 +4,23 @@ 'franka':{ 'interface': {'type': 'franka', 'ip_address':'172.16.0.1', 'gain_scale':0.5}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, '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_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, ], 'actuator':[ - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), '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':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, ] }, @@ -28,10 +28,10 @@ 'robotiq':{ 'interface': {'type': 'robotiq', 'ip_address':'172.16.0.1'}, 'sensor':[ - {'range':(0, 0.834), 'noise':0.0, 'adr':0, 'name':'robotiq_2f_85', 'scale':-9.81, 'offset':0.834}, + {'range':(0, 0.834), 'noise':0.0, 'hdr_adr':0, 'name':'robotiq_2f_85', 'scale':-9.81, 'offset':0.834}, ], 'actuator':[ - {'pos_range':(0, 1), 'vel_range':(-20*np.pi/4, 20*np.pi/4), 'adr':0, 'name':'robotiq_2f_85', 'scale':-0.085, 'offset':0.085}, + {'pos_range':(0, 1), 'vel_range':(-20*np.pi/4, 20*np.pi/4), 'hdr_adr':0, 'name':'robotiq_2f_85', 'scale':-0.085, 'offset':0.085}, ] }, @@ -39,8 +39,8 @@ 'interface': {'type': 'realsense', 'rgb_topic':'realsense_815412070228/color/image_raw', 'd_topic':'realsense_815412070228/depth_uncolored/image_raw'}, 'sensor':[], 'cam': [ - {'range':(0, 255), 'noise':0.00, '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'}, + {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_adr':'d', 'scale':1, 'offset':0, 'name':'/depth_uncolored/image_raw'}, ], 'actuator':[] }, @@ -49,7 +49,7 @@ 'interface': {'type': 'realsense', 'rgb_topic':'realsense_815412070341/color/image_raw', 'd_topic':'realsense_815412070341/depth_uncolored/image_raw'}, 'sensor':[], 'cam': [ - {'range':(0, 255), 'noise':0.00, 'adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, ], 'actuator':[] }, @@ -58,7 +58,7 @@ 'interface': {'type': 'realsense', 'rgb_topic':'realsense_936322070233/color/image_raw', 'd_topic':'realsense_936322070233/depth_uncolored/image_raw'}, 'sensor':[], 'cam': [ - {'range':(0, 255), 'noise':0.00, 'adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, ], 'actuator':[] }, @@ -67,7 +67,7 @@ 'interface': {'type': 'realsense', 'rgb_topic':'realsense_814412070228/color/image_raw', 'd_topic':'realsense_814412070228/depth_uncolored/image_raw'}, 'sensor':[], 'cam': [ - {'range':(0, 255), 'noise':0.00, 'adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, + {'range':(0, 255), 'noise':0.00, 'hdr_adr':'rgb', 'scale':1, 'offset':0, 'name':'/color/image_raw'}, ], 'actuator':[] }, diff --git a/robohive/envs/multi_task/common/kitchen/franka_kitchen.config b/robohive/envs/multi_task/common/kitchen/franka_kitchen.config index 1fa983b7..c98ab660 100644 --- a/robohive/envs/multi_task/common/kitchen/franka_kitchen.config +++ b/robohive/envs/multi_task/common/kitchen/franka_kitchen.config @@ -3,38 +3,38 @@ 'franka':{ 'interface': {'type': 'franka'}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, '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.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jp7'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'}, - # {'range':(-2*np.pi, np.pi), 'noise':0.05, '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'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + # {'range':(-2*np.pi, np.pi), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, ], 'actuator':[ # TODO: ranges here exceed the corresponding sensor ranges - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), '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'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'panda0_joint7'}, + {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'r_gripper_finger_joint'}, + {'pos_range':(-0.0000, 0.0400), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'l_gripper_finger_joint'}, ] }, @@ -42,22 +42,22 @@ 'kitchen':{ 'interface': {'type': 'tbd'}, 'sensor':[ - {'range':(-1.57, 00), 'noise':0.05, '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.57, 00), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'knob1_joint'}, + {'range':(-1.57, 00), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'knob2_joint'}, + {'range':(-1.57, 00), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'knob3_joint'}, + {'range':(-1.57, 00), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'knob4_joint'}, + {'range':(-0.7, 0.0), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'lightswitch_joint'}, + {'range':(0.0, 1.57), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'slidedoor_joint'}, + {'range':(-1.57, .0), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'leftdoorhinge'}, + {'range':(0.0, 1.57), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'rightdoorhinge'}, + {'range':(-2.09, 00), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'micro0joint'}, - {'range':(-1.25, 1.75), 'noise':0.05, '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'}, + {'range':(-1.25, 1.75), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Tx'}, + {'range':(-1.50, 1.50), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Ty'}, + {'range':(-0.10, 2.90), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Tz'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Rx'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Ry'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'kettle0:Rz'}, ], 'actuator':[] diff --git a/robohive/envs/multi_task/common/microwave/franka_microwave.config b/robohive/envs/multi_task/common/microwave/franka_microwave.config index 0d58447b..233094c0 100644 --- a/robohive/envs/multi_task/common/microwave/franka_microwave.config +++ b/robohive/envs/multi_task/common/microwave/franka_microwave.config @@ -3,39 +3,39 @@ 'franka':{ 'interface': {'type': 'franka', 'ip_address':'169.254.163.91'}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, '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_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, - {'range':(-2.9, 2.9), 'noise':0.05, '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'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, ], 'actuator':[ - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), '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':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, ] }, 'microwave':{ 'interface': {}, 'sensor':[ - {'range':(-2.0, 0.0), 'noise':0.05, 'adr':7, 'scale':1, 'offset':0, 'name':'micro0joint'}, + {'range':(-2.0, 0.0), 'noise':0.05, 'hdr_adr':7, 'scale':1, 'offset':0, 'name':'micro0joint'}, ], 'actuator':[] } diff --git a/robohive/envs/multi_task/common/slidecabinet/franka_slidecabinet.config b/robohive/envs/multi_task/common/slidecabinet/franka_slidecabinet.config index d7023e1f..8c518054 100644 --- a/robohive/envs/multi_task/common/slidecabinet/franka_slidecabinet.config +++ b/robohive/envs/multi_task/common/slidecabinet/franka_slidecabinet.config @@ -3,41 +3,41 @@ 'franka':{ 'interface': {'type': 'franka', 'ip_address':'169.254.163.91'}, 'sensor':[ - {'range':(-2.9, 2.9), 'noise':0.05, '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_adr':0, 'scale':1, 'offset':0, 'name':'fr_arm_jp1'}, + {'range':(-1.8, 1.8), 'noise':0.05, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'fr_arm_jp2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'fr_arm_jp3'}, + {'range':(-3.1, 0.0), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'fr_arm_jp4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jp5'}, + {'range':(-1.7, 3.8), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':-np.pi/2, 'name':'fr_arm_jp6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':6, 'scale':1, 'offset':-np.pi/4, 'name':'fr_arm_jp7'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp1'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jp2'}, - {'range':(-2.9, 2.9), 'noise':0.05, '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'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv1'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv2'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv3'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv4'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv5'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv6'}, + {'range':(-2.9, 2.9), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'fr_arm_jv7'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv1'}, + {'range':(0.00, .04), 'noise':0.05, 'hdr_adr':-1, 'scale':1, 'offset':0, 'name':'fr_fin_jv2'}, ], 'actuator':[ - {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), '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':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'panda0_joint1'}, + {'pos_range':(-1.8326, 1.8326), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'panda0_joint2'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':2, 'scale':1, 'offset':0, 'name':'panda0_joint3'}, + {'pos_range':(-3.1416, 0.0000), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'panda0_joint4'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'panda0_joint5'}, + {'pos_range':(-1.6600, 2.1817), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':5, 'scale':1, 'offset':np.pi/2, 'name':'panda0_joint6'}, + {'pos_range':(-2.9671, 2.9671), 'vel_range':(-4*np.pi/2, 4*np.pi/2), 'hdr_adr':6, 'scale':1, 'offset':np.pi/4, 'name':'panda0_joint7'}, ] }, 'slidecabinet':{ 'interface': {}, 'sensor':[ - {'range':(-0.0, .44), 'noise':0.05, 'adr':7, 'scale':1, 'offset':0, 'name':'slidedoor_joint'}, + {'range':(-0.0, .44), 'noise':0.05, 'hdr_adr':7, 'scale':1, 'offset':0, 'name':'slidedoor_joint'}, ], 'actuator':[] } diff --git a/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config b/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config index 8dae22fd..73318883 100644 --- a/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config +++ b/robohive/envs/quadrupeds/dkitty/dkitty_stand_v0.config @@ -3,61 +3,61 @@ 'dkitty_root':{ 'interface': {'type': 'optitrack', 'server_name': '169.254.163.86', 'client_name': '169.254.163.96','port':5000, 'packet_size':36, 'id':'1'}, 'sensor':[ - {'range':(-5.00, 5.00), 'noise':0.005, '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'}, + {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'A:Tx'}, + {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'A:Ty'}, + {'range':(-2.00, 2.00), 'noise':0.005, 'hdr_adr':2, 'scale':1, 'offset':-.32, 'name':'A:Tz'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'A:Rx'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'A:Ry'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':0, 'name':'A:Rz'}, ], 'actuator':[] }, # device1: sensors, actuators 'dkitty':{ - 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/dev/DKitty"}, + 'interface': {'type': 'dynamixel', 'motor_type':"X", 'port':"/dev/DKitty"}, 'sensor':[ - {'range':(-3.419, 0.279), 'noise':0.05, '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'} + {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_adr':10, 'scale':+1, 'offset':-3*np.pi/2, 'name':'A:FRJ10_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':11, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ11_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':12, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ12_pos_sensor'}, + {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_adr':20, 'scale':+1, 'offset':-np.pi/2, 'name':'A:FLJ20_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':21, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ21_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':22, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ22_pos_sensor'}, + {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_adr':30, 'scale':-1, 'offset':3*np.pi/2, 'name':'A:BLJ30_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':31, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ31_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':32, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ32_pos_sensor'}, + {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_adr':40, 'scale':-1, 'offset':np.pi/2, 'name':'A:BRJ40_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':41, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ41_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':42, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ42_pos_sensor'} ], 'actuator':[ - {'pos_range':(-1.57 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), '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'} + {'pos_range':(-1.57 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10', 'mode':'Position'}, + {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11', 'mode':'Position'}, + {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12', 'mode':'Position'}, + {'pos_range':(-0.279, 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20', 'mode':'Position'}, + {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21', 'mode':'Position'}, + {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22', 'mode':'Position'}, + {'pos_range':(-0.279, 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30', 'mode':'Position'}, + {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31', 'mode':'Position'}, + {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32', 'mode':'Position'}, + {'pos_range':(-1.57 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40', 'mode':'Position'}, + {'pos_range':(-2.14 , 2.14 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41', 'mode':'Position'}, + {'pos_range':(-1.57 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42', 'mode':'Position'} ] # 'actuator':[ # restricted shoulder - # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), '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'} + # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10', 'mode':'Position'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11', 'mode':'Position'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12', 'mode':'Position'}, + # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20', 'mode':'Position'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21', 'mode':'Position'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22', 'mode':'Position'}, + # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30', 'mode':'Position'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31', 'mode':'Position'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32', 'mode':'Position'}, + # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40', 'mode':'Position'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41', 'mode':'Position'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42', 'mode':'Position'} # ] } } \ No newline at end of file diff --git a/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config b/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config index 3dad488f..e9fed7c1 100644 --- a/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config +++ b/robohive/envs/quadrupeds/dkitty/dkitty_walk_v0.config @@ -3,61 +3,61 @@ 'dkitty_root':{ 'interface': {'type': 'optitrack', 'server_name': '169.254.163.86', 'client_name': '169.254.163.96','port':5000, 'packet_size':36, 'id':'1'}, 'sensor':[ - {'range':(-5.00, 5.00), 'noise':0.005, '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'}, + {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_adr':0, 'scale':1, 'offset':0, 'name':'A:Tx'}, + {'range':(-5.00, 5.00), 'noise':0.005, 'hdr_adr':1, 'scale':1, 'offset':0, 'name':'A:Ty'}, + {'range':(-2.00, 2.00), 'noise':0.005, 'hdr_adr':2, 'scale':1, 'offset':-.32, 'name':'A:Tz'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':3, 'scale':1, 'offset':0, 'name':'A:Rx'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':4, 'scale':1, 'offset':0, 'name':'A:Ry'}, + {'range':(-3.14, 3.14), 'noise':0.05, 'hdr_adr':5, 'scale':1, 'offset':0, 'name':'A:Rz'}, ], 'actuator':[] }, # device1: sensors, actuators 'dkitty':{ - 'interface': {'type': 'dynamixel', 'motor_type':"X", 'name':"/dev/DKitty"}, + 'interface': {'type': 'dynamixel', 'motor_type':"X", 'port':"/dev/DKitty"}, 'sensor':[ - {'range':(-3.419, 0.279), 'noise':0.05, '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'} + {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_adr':10, 'scale':+1, 'offset':-3*np.pi/2, 'name':'A:FRJ10_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':11, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ11_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':12, 'scale':-1, 'offset':np.pi, 'name':'A:FRJ12_pos_sensor'}, + {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_adr':20, 'scale':+1, 'offset':-np.pi/2, 'name':'A:FLJ20_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':21, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ21_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':22, 'scale':+1, 'offset':-np.pi, 'name':'A:FLJ22_pos_sensor'}, + {'range':(-0.279, 3.419), 'noise':0.05, 'hdr_adr':30, 'scale':-1, 'offset':3*np.pi/2, 'name':'A:BLJ30_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':31, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ31_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':32, 'scale':+1, 'offset':-np.pi, 'name':'A:BLJ32_pos_sensor'}, + {'range':(-3.419, 0.279), 'noise':0.05, 'hdr_adr':40, 'scale':-1, 'offset':np.pi/2, 'name':'A:BRJ40_pos_sensor'}, + {'range':(-2.14 , 2.14 ), 'noise':0.05, 'hdr_adr':41, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ41_pos_sensor'}, + {'range':(-1.57 , 1.57 ), 'noise':0.05, 'hdr_adr':42, 'scale':-1, 'offset':np.pi, 'name':'A:BRJ42_pos_sensor'} ], 'actuator':[ - {'pos_range':(-0.75 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), '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'} + {'pos_range':(-0.75 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10', 'mode':'Position'}, + {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11', 'mode':'Position'}, + {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12', 'mode':'Position'}, + {'pos_range':(-0.279, 0.75 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20', 'mode':'Position'}, + {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21', 'mode':'Position'}, + {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22', 'mode':'Position'}, + {'pos_range':(-0.279, 0.75 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30', 'mode':'Position'}, + {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31', 'mode':'Position'}, + {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32', 'mode':'Position'}, + {'pos_range':(-0.75 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40', 'mode':'Position'}, + {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41', 'mode':'Position'}, + {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42', 'mode':'Position'} ] # 'actuator':[ # restricted shoulder - # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), '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'} + # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':10, 'scale':+1, 'offset':-1*(-3*np.pi/2), 'name':'A:FRJ10', 'mode':'Position'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':11, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ11', 'mode':'Position'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':12, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:FRJ12', 'mode':'Position'}, + # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':20, 'scale':+1, 'offset':-1*(-np.pi/2), 'name':'A:FLJ20', 'mode':'Position'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':21, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ21', 'mode':'Position'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':22, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:FLJ22', 'mode':'Position'}, + # {'pos_range':(-0.279, 0.279 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':30, 'scale':-1, 'offset':+1*(3*np.pi/2), 'name':'A:BLJ30', 'mode':'Position'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':31, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ31', 'mode':'Position'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':32, 'scale':+1, 'offset':-1*(-np.pi), 'name':'A:BLJ32', 'mode':'Position'}, + # {'pos_range':(-0.279 , 0.279), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':40, 'scale':-1, 'offset':+1*(np.pi/2), 'name':'A:BRJ40', 'mode':'Position'}, + # {'pos_range':(-0.00 , 1.57 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':41, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ41', 'mode':'Position'}, + # {'pos_range':(-1.57 , 0.00 ), 'vel_range':(-2*np.pi/4, 2*np.pi/4), 'hdr_adr':42, 'scale':-1, 'offset':+1*(np.pi), 'name':'A:BRJ42', 'mode':'Position'} # ] } } \ No newline at end of file From a6df89fdce42a9258633000b057fded6516f52ac Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Fri, 24 Jul 2026 18:55:17 -0400 Subject: [PATCH 44/49] MAJOR: Relaxing gymnasium version --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index bbfac246..88c5a0aa 100644 --- a/setup.py +++ b/setup.py @@ -60,7 +60,7 @@ def package_files(directory): install_requires=[ "click", # 'gym==0.13', # default to this stable point if caught in gym issues. - "gymnasium==0.29.1", + "gymnasium>=0.29.1", "mujoco==3.3.3", "numpy>=2", "dm-control==1.0.31", From cf40cf485f0236badbb85d19fcbdb4ff0a1af2f8 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Fri, 24 Jul 2026 23:27:32 -0400 Subject: [PATCH 45/49] BUGFIX: env_base.py's recent refactor made _setup() call self.reset() internally (to route hardware envs through min-jerk reset) instead of self.step(zeros). Since reset() is often overridden by subclasses, this exposed a latent ordering bug across ~20 env files: they computed a task-specific self.init_qpos after calling super()._setup(), but by then super()._setup() had already triggered the first reset() using a stale/default init_qpos. In relocate_v0.py this stale pose left the hand and object permanently in collision, and the env's collision-retry logic in reset() recursed forever (RecursionError). --- robohive/envs/hands/baoding_v1.py | 4 +--- 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/multi_task_base_v1.py | 15 ++++++++------- robohive/envs/myo/myobase/baoding_v1.py | 12 +++++++----- robohive/envs/myo/myobase/key_turn_v0.py | 5 ++++- robohive/envs/myo/myobase/obj_hold_v0.py | 7 +++++-- robohive/envs/myo/myobase/pen_v0.py | 7 +++++-- robohive/envs/myo/myobase/reorient_sar_v0.py | 8 ++++++-- robohive/envs/myo/myobase/walk_v0.py | 6 +++--- robohive/envs/myo/myochallenge/baoding_v1.py | 10 ++++++---- robohive/envs/myo/myochallenge/chasetag_v0.py | 1 - robohive/envs/myo/myochallenge/relocate_v0.py | 10 ++++++---- robohive/envs/myo/myochallenge/reorient_v0.py | 7 +++++-- robohive/envs/myo/myodm/myodm_v0.py | 18 ++++++++++-------- robohive/envs/myo/myomimic/myomimic_v0.py | 18 ++++++++++-------- robohive/envs/quadrupeds/orient_v0.py | 2 ++ robohive/envs/quadrupeds/walk_v0.py | 2 ++ robohive/envs/tcdm/track.py | 18 ++++++++++-------- 21 files changed, 94 insertions(+), 64 deletions(-) diff --git a/robohive/envs/hands/baoding_v1.py b/robohive/envs/hands/baoding_v1.py index 1b6d97da..fcf35517 100644 --- a/robohive/envs/hands/baoding_v1.py +++ b/robohive/envs/hands/baoding_v1.py @@ -131,11 +131,9 @@ def _setup(self, super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, frame_skip=frame_skip, + init_qpos=self.sim.model.key_qpos[0].copy(), **kwargs, ) - - # reset position - self.init_qpos = self.sim.model.key_qpos[0].copy() # self.init_qpos[:-14] *= 0 # Use fully open as init pos # V0: Centered the action space around key_qpos[0]. Not sure if it matter. diff --git a/robohive/envs/hands/door_v1.py b/robohive/envs/hands/door_v1.py index 4e756e9f..18dbfdc3 100644 --- a/robohive/envs/hands/door_v1.py +++ b/robohive/envs/hands/door_v1.py @@ -59,8 +59,8 @@ def _setup(self, weighted_reward_keys=weighted_reward_keys, reward_mode=reward_mode, frame_skip=frame_skip, + init_qpos=np.zeros(sim.data.qpos.shape), **kwargs) - self.init_qpos = np.zeros(self.init_qpos.shape) self.init_qvel = np.zeros(self.init_qpos.shape) diff --git a/robohive/envs/hands/hammer_v1.py b/robohive/envs/hands/hammer_v1.py index cbdccb92..dfd4ecb0 100644 --- a/robohive/envs/hands/hammer_v1.py +++ b/robohive/envs/hands/hammer_v1.py @@ -66,8 +66,8 @@ def _setup(self, weighted_reward_keys=weighted_reward_keys, reward_mode=reward_mode, frame_skip=frame_skip, + init_qpos=np.zeros(sim.data.qpos.shape), **kwargs) - self.init_qpos = np.zeros(self.init_qpos.shape) self.init_qvel = np.zeros(self.init_qpos.shape) diff --git a/robohive/envs/hands/pen_v1.py b/robohive/envs/hands/pen_v1.py index f38c1c1d..0ca38e9c 100644 --- a/robohive/envs/hands/pen_v1.py +++ b/robohive/envs/hands/pen_v1.py @@ -65,8 +65,8 @@ def _setup(self, weighted_reward_keys=weighted_reward_keys, reward_mode=reward_mode, frame_skip=frame_skip, + init_qpos=np.zeros(sim.data.qpos.shape), **kwargs) - self.init_qpos = np.zeros(self.init_qpos.shape) self.init_qvel = np.zeros(self.init_qpos.shape) diff --git a/robohive/envs/hands/relocate_v1.py b/robohive/envs/hands/relocate_v1.py index be2d8ea0..003d9df0 100644 --- a/robohive/envs/hands/relocate_v1.py +++ b/robohive/envs/hands/relocate_v1.py @@ -64,8 +64,8 @@ def _setup(self, weighted_reward_keys=weighted_reward_keys, reward_mode=reward_mode, frame_skip=frame_skip, + init_qpos=np.zeros(sim.data.qpos.shape), **kwargs) - self.init_qpos = np.zeros(self.init_qpos.shape) self.init_qvel = np.zeros(self.init_qpos.shape) diff --git a/robohive/envs/multi_task/multi_task_base_v1.py b/robohive/envs/multi_task/multi_task_base_v1.py index 4122593e..50cb58b3 100644 --- a/robohive/envs/multi_task/multi_task_base_v1.py +++ b/robohive/envs/multi_task/multi_task_base_v1.py @@ -112,6 +112,13 @@ def _setup(self, self.input_obj_init = obj_init self.set_obj_goal(obj_goal=self.input_obj_goal, interact_site=interact_site) + # Recover init from the saved qposes and input specs before super()._setup(), since it + # triggers the first reset() which relies on init_qpos already reflecting these values. + keyFrame_id = 0 + self.init_qpos = self.sim.model.key_qpos[keyFrame_id].copy() + if obj_init: + self.set_obj_init(self.input_obj_init) + super()._setup(obs_keys=obs_keys_wt, proprio_keys=proprio_keys_wt, weighted_reward_keys=weighted_reward_keys, @@ -119,15 +126,9 @@ def _setup(self, act_mode=act_mode, obs_range=obs_range, robot_name=robot_name, + init_qpos=self.init_qpos, **kwargs) - - - # Recover init from the saved qposes and input specs - keyFrame_id = 0 - self.init_qpos[:] = self.sim.model.key_qpos[keyFrame_id].copy() self.init_qvel[:] = self.sim.model.key_qvel[keyFrame_id].copy() - if obj_init: - self.set_obj_init(self.input_obj_init) def get_dof_proximity(self, obj_dof_ranges, obj_dof_type): diff --git a/robohive/envs/myo/myobase/baoding_v1.py b/robohive/envs/myo/myobase/baoding_v1.py index 44f63fad..c5614f96 100644 --- a/robohive/envs/myo/myobase/baoding_v1.py +++ b/robohive/envs/myo/myobase/baoding_v1.py @@ -113,17 +113,19 @@ def _setup(self, # update rewards to be move rewards weighted_reward_keys = self.MOVE_TO_LOCATION_RWD_KEYS_AND_WEIGHTS + # reset position + # init_qpos = self.sim.model.key_qpos[0].copy() + init_qpos = self.sim.data.qpos.ravel().copy() + init_qpos[:-14] *= 0 # Use fully open as init pos + init_qpos[0] = -1.57 # Palm up + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, frame_skip=frame_skip, + init_qpos=init_qpos, **kwargs, ) - # reset position - # self.init_qpos = self.sim.model.key_qpos[0].copy() - self.init_qpos[:-14] *= 0 # Use fully open as init pos - self.init_qpos[0] = -1.57 # Palm up - # V0: Centered the action space around key_qpos[0]. Not sure if it matter. # self.act_mid = self.init_qpos[:self.n_jnt].copy() # self.upper_rng = 0.9*(self.model.actuator_ctrlrange[:,1]-self.act_mid) diff --git a/robohive/envs/myo/myobase/key_turn_v0.py b/robohive/envs/myo/myobase/key_turn_v0.py index 9fc51e1d..e4f3657b 100644 --- a/robohive/envs/myo/myobase/key_turn_v0.py +++ b/robohive/envs/myo/myobase/key_turn_v0.py @@ -54,11 +54,14 @@ def _setup(self, self.key_init_range = key_init_range self.key_init_pos = self.sim.data.site_xpos[self.keyhead_sid].copy() + init_qpos = self.sim.data.qpos.ravel().copy() + init_qpos[:-1] *= 0 # Use fully open as init pos + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, + init_qpos=init_qpos, **kwargs, ) - self.init_qpos[:-1] *= 0 # Use fully open as init pos def get_obs_vec(self): self.obs_dict['time'] = np.array([self.sim.data.time]) diff --git a/robohive/envs/myo/myobase/obj_hold_v0.py b/robohive/envs/myo/myobase/obj_hold_v0.py index b0864933..bfc29c0d 100644 --- a/robohive/envs/myo/myobase/obj_hold_v0.py +++ b/robohive/envs/myo/myobase/obj_hold_v0.py @@ -47,12 +47,15 @@ def _setup(self, self.goal_sid = self.sim.model.site_name2id("goal") self.object_init_pos = self.sim.data.site_xpos[self.object_sid].copy() + init_qpos = self.sim.data.qpos.ravel().copy() + init_qpos[:-7] *= 0 # Use fully open as init pos + init_qpos[0] = -1.5 # place palm up + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, + init_qpos=init_qpos, **kwargs, ) - self.init_qpos[:-7] *= 0 # Use fully open as init pos - self.init_qpos[0] = -1.5 # place palm up def get_obs_vec(self): diff --git a/robohive/envs/myo/myobase/pen_v0.py b/robohive/envs/myo/myobase/pen_v0.py index 4684ed77..eb098524 100644 --- a/robohive/envs/myo/myobase/pen_v0.py +++ b/robohive/envs/myo/myobase/pen_v0.py @@ -57,12 +57,15 @@ def _setup(self, self.pen_length = np.linalg.norm(self.sim.model.site_pos[self.obj_t_sid] - self.sim.model.site_pos[self.obj_b_sid]) self.tar_length = np.linalg.norm(self.sim.model.site_pos[self.tar_t_sid] - self.sim.model.site_pos[self.tar_b_sid]) + init_qpos = self.sim.data.qpos.ravel().copy() + init_qpos[:-6] *= 0 # Use fully open as init pos + init_qpos[0] = -1.5 # place palm up + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, + init_qpos=init_qpos, **kwargs, ) - self.init_qpos[:-6] *= 0 # Use fully open as init pos - self.init_qpos[0] = -1.5 # place palm up def get_obs_vec(self): # qpos for hand, xpos for obj, xpos for target diff --git a/robohive/envs/myo/myobase/reorient_sar_v0.py b/robohive/envs/myo/myobase/reorient_sar_v0.py index ed99a603..246de692 100644 --- a/robohive/envs/myo/myobase/reorient_sar_v0.py +++ b/robohive/envs/myo/myobase/reorient_sar_v0.py @@ -69,13 +69,17 @@ def _setup(self, self.tar_length = np.linalg.norm(self.sim.model.geom_pos[self.tar_t_gid] - self.sim.model.geom_pos[self.tar_b_gid]) self.sim.model.body_mass[self.obj_bid] *= 1.25 + + init_qpos = self.sim.data.qpos.ravel().copy() + init_qpos[:-6] *= 0 # Use fully open as init pos + init_qpos[0] = -1.5 # place palm up + super()._setup(obs_keys=['hand_jnt','obj_pos','obj_vel','obj_rot','obj_des_rot', 'obj_err_pos','obj_err_rot','mlen','mvel','mforce'], weighted_reward_keys=weighted_reward_keys, + init_qpos=init_qpos, **kwargs, ) - self.init_qpos[:-6] *= 0 # Use fully open as init pos - self.init_qpos[0] = -1.5 # place palm up def get_obs_dict(self, sim): diff --git a/robohive/envs/myo/myobase/walk_v0.py b/robohive/envs/myo/myobase/walk_v0.py index a5bf3164..65025cbd 100644 --- a/robohive/envs/myo/myobase/walk_v0.py +++ b/robohive/envs/myo/myobase/walk_v0.py @@ -53,9 +53,9 @@ def _setup(self, super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, sites=self.target_reach_range.keys(), + init_qpos=self.sim.model.key_qpos[0].copy(), **kwargs, ) - self.init_qpos[:] = self.sim.model.key_qpos[0] self.init_qvel[:] = self.sim.model.key_qvel[0] # find geometries with ID == 1 which indicates the skins geom_1_indices = np.where(self.sim.model.geom_group == 1) @@ -206,9 +206,9 @@ def _setup(self, self.steps = 0 super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, + init_qpos=self.sim.model.key_qpos[0].copy(), **kwargs ) - self.init_qpos[:] = self.sim.model.key_qpos[0] self.init_qvel[:] = 0.0 # move heightfield down if not used @@ -473,9 +473,9 @@ def _setup(self, BaseV0._setup(self, obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, + init_qpos=self.sim.model.key_qpos[0].copy(), **kwargs ) - self.init_qpos[:] = self.sim.model.key_qpos[0] self.init_qvel[:] = 0.0 def reset(self, **kwargs): diff --git a/robohive/envs/myo/myochallenge/baoding_v1.py b/robohive/envs/myo/myochallenge/baoding_v1.py index 861a1f40..c226b80a 100644 --- a/robohive/envs/myo/myochallenge/baoding_v1.py +++ b/robohive/envs/myo/myochallenge/baoding_v1.py @@ -96,16 +96,18 @@ def _setup(self, self.obj_friction_range = {'low':self.sim.model.geom_friction[self.object1_gid] - obj_friction_change, 'high':self.sim.model.geom_friction[self.object1_gid] + obj_friction_change} if obj_friction_change else None + # reset position + init_qpos = self.sim.data.qpos.ravel().copy() + init_qpos[:-14] *= 0 # Use fully open as init pos + init_qpos[0] = -1.57 # Palm up + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, frame_skip=frame_skip, + init_qpos=init_qpos, **kwargs, ) - # reset position - self.init_qpos[:-14] *= 0 # Use fully open as init pos - self.init_qpos[0] = -1.57 # Palm up - def step(self, a, **kwargs): if self.which_task in [Task.HOLD, Task.BAODING_CW, Task.BAODING_CCW]: desired_angle_wrt_palm = self.goal[self.counter].copy() diff --git a/robohive/envs/myo/myochallenge/chasetag_v0.py b/robohive/envs/myo/myochallenge/chasetag_v0.py index 2c035765..09a6e475 100644 --- a/robohive/envs/myo/myochallenge/chasetag_v0.py +++ b/robohive/envs/myo/myochallenge/chasetag_v0.py @@ -679,7 +679,6 @@ def _setup(self, reset_type=reset_type, **kwargs ) - self.init_qpos[:] = self.sim.model.key_qpos[0] self.init_qvel[:] = 0.0 self.startFlag = True self.assert_settings() diff --git a/robohive/envs/myo/myochallenge/relocate_v0.py b/robohive/envs/myo/myochallenge/relocate_v0.py index 5547c169..36361e81 100644 --- a/robohive/envs/myo/myochallenge/relocate_v0.py +++ b/robohive/envs/myo/myochallenge/relocate_v0.py @@ -4,11 +4,13 @@ ================================================= """ import collections + import numpy as np -from robohive.utils import gym from robohive.envs.myo.base_v0 import BaseV0 -from robohive.utils.quat_math import mat2euler, euler2quat +from robohive.utils import gym +from robohive.utils.quat_math import euler2quat, mat2euler + class RelocateEnvV0(BaseV0): @@ -57,12 +59,12 @@ def _setup(self, self.rot_th = rot_th self.drop_th = drop_th + keyFrame_id = 0 if self.obj_xyz_range is None else 1 super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, + init_qpos=self.sim.model.key_qpos[keyFrame_id].copy(), **kwargs, ) - keyFrame_id = 0 if self.obj_xyz_range is None else 1 - self.init_qpos[:] = self.sim.model.key_qpos[keyFrame_id].copy() def get_obs_dict(self, sim): diff --git a/robohive/envs/myo/myochallenge/reorient_v0.py b/robohive/envs/myo/myochallenge/reorient_v0.py index fd919193..92937f64 100644 --- a/robohive/envs/myo/myochallenge/reorient_v0.py +++ b/robohive/envs/myo/myochallenge/reorient_v0.py @@ -67,12 +67,15 @@ def _setup(self, self.obj_friction_range = {'low':self.sim.model.geom_friction[self.object_gid0:self.object_gidn] - obj_friction_change, 'high':self.sim.model.geom_friction[self.object_gid0:self.object_gidn] + obj_friction_change} + init_qpos = self.sim.data.qpos.ravel().copy() + init_qpos[:-7] *= 0 # Use fully open as init pos + init_qpos[0] = -1.5 # Palm up + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, + init_qpos=init_qpos, **kwargs, ) - self.init_qpos[:-7] *= 0 # Use fully open as init pos - self.init_qpos[0] = -1.5 # Palm up def get_obs_dict(self, sim): obs_dict = {} diff --git a/robohive/envs/myo/myodm/myodm_v0.py b/robohive/envs/myo/myodm/myodm_v0.py index 4c9ebeeb..012daab7 100644 --- a/robohive/envs/myo/myodm/myodm_v0.py +++ b/robohive/envs/myo/myodm/myodm_v0.py @@ -135,23 +135,25 @@ def _setup(self, self._lift_z = self.sim.data.xipos[self.object_bid][2] + self.lift_bonus_thresh + # Adjust init as per the specified key + init_qpos = self.sim.data.qpos.ravel().copy() + robot_init, object_init = self.ref.get_init() + if robot_init is not None: + init_qpos[:self.ref.robot_dim] = robot_init + if object_init is not None: + init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3] + init_qpos[-3:] = quat2euler(object_init[3:]) + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, frame_skip=10, + init_qpos=init_qpos, **kwargs) # Adjust horizon if not motion_extrapolation if motion_extrapolation == False: self.spec.max_episode_steps = self.ref.horizon # doesn't work always. WIP - # Adjust init as per the specified key - robot_init, object_init = self.ref.get_init() - if robot_init is not None: - self.init_qpos[:self.ref.robot_dim] = robot_init - if object_init is not None: - self.init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3] - self.init_qpos[-3:] = quat2euler(object_init[3:]) - # hack because in the super()._setup the initial posture is set to the average qpos and when a step is called, it ends in a `done` state self.initialized_pos = True # if self.sim.model.nkey>0: diff --git a/robohive/envs/myo/myomimic/myomimic_v0.py b/robohive/envs/myo/myomimic/myomimic_v0.py index c634de95..212e3bd0 100644 --- a/robohive/envs/myo/myomimic/myomimic_v0.py +++ b/robohive/envs/myo/myomimic/myomimic_v0.py @@ -82,23 +82,25 @@ def _setup(self, self.TermPose = Termimate_pose_fail ########################################## + # Adjust init as per the specified key + init_qpos = self.sim.data.qpos.ravel().copy() + robot_init, object_init = self.ref.get_init() + if robot_init is not None: + init_qpos[:self.ref.robot_dim] = robot_init + if object_init is not None: + init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3] + init_qpos[-3:] = quat2euler(object_init[3:]) + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, frame_skip=10, + init_qpos=init_qpos, **kwargs) # Adjust horizon if not motion_extrapolation if motion_extrapolation == False: self.spec.max_episode_steps = self.ref.horizon # doesn't work always. WIP - # Adjust init as per the specified key - robot_init, object_init = self.ref.get_init() - if robot_init is not None: - self.init_qpos[:self.ref.robot_dim] = robot_init - if object_init is not None: - self.init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3] - self.init_qpos[-3:] = quat2euler(object_init[3:]) - # hack because in the super()._setup the initial posture is set to the average qpos and when a step is called, it ends in a `done` state self.initialized_pos = True # if self.sim.model.nkey>0: diff --git a/robohive/envs/quadrupeds/orient_v0.py b/robohive/envs/quadrupeds/orient_v0.py index 776e9978..e115cbf3 100644 --- a/robohive/envs/quadrupeds/orient_v0.py +++ b/robohive/envs/quadrupeds/orient_v0.py @@ -91,6 +91,8 @@ def _setup(self, **kwargs) # configure + # self.robot (and its robot_config) is only created inside super()._setup(), so this + # can't be computed earlier and passed via the init_qpos kwarg like other envs. for name, device in self.robot.robot_config.items(): for act_id, actuator in enumerate(device['actuator']): self.init_qpos[actuator['data_id']] = np.mean(actuator['pos_range']) diff --git a/robohive/envs/quadrupeds/walk_v0.py b/robohive/envs/quadrupeds/walk_v0.py index bf91307f..0d5d3c2d 100644 --- a/robohive/envs/quadrupeds/walk_v0.py +++ b/robohive/envs/quadrupeds/walk_v0.py @@ -92,6 +92,8 @@ def _setup(self, **kwargs) # configure + # self.robot (and its robot_config) is only created inside super()._setup(), so this + # can't be computed earlier and passed via the init_qpos kwarg like other envs. for name, device in self.robot.robot_config.items(): for act_id, actuator in enumerate(device['actuator']): self.init_qpos[actuator['data_id']] = np.mean(actuator['pos_range']) diff --git a/robohive/envs/tcdm/track.py b/robohive/envs/tcdm/track.py index 408b8c38..9101e044 100644 --- a/robohive/envs/tcdm/track.py +++ b/robohive/envs/tcdm/track.py @@ -128,23 +128,25 @@ def _setup(self, self._lift_z = self.sim.data.xipos[self.object_bid][2] + self.lift_bonus_thresh + # Adjust init as per the specified key + init_qpos = self.sim.data.qpos.ravel().copy() + robot_init, object_init = self.ref.get_init() + if robot_init is None: + init_qpos[:self.ref.robot_dim] = robot_init + if object_init is None: + init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3] + init_qpos[-3:] = quat2euler(object_init[3:]) + super()._setup(obs_keys=obs_keys, weighted_reward_keys=weighted_reward_keys, frame_skip=10, + init_qpos=init_qpos, **kwargs) # Adjust horizon if not motion_extrapolation if motion_extrapolation == False: self.spec.max_episode_steps = self.ref.horizon # doesn't work always. WIP - # Adjust init as per the specified key - robot_init, object_init = self.ref.get_init() - if robot_init is None: - self.init_qpos[:self.ref.robot_dim] = robot_init - if object_init is None: - self.init_qpos[self.ref.robot_dim:self.ref.robot_dim+3] = object_init[:3] - self.init_qpos[-3:] = quat2euler(object_init[3:]) - # hack because in the super()._setup the initial posture is set to the average qpos and when a step is called, it ends in a `done` state self.initialized_pos = True # if self.sim.model.nkey>0: From 5ac72cb755e6f836dbecbfa00a5e83b74b4da9de Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Tue, 28 Jul 2026 09:37:01 -0400 Subject: [PATCH 46/49] FEATURE: Also check visual_dict for visual keys --- robohive/utils/paths_utils.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/robohive/utils/paths_utils.py b/robohive/utils/paths_utils.py index d65c915c..ff3f6d69 100644 --- a/robohive/utils/paths_utils.py +++ b/robohive/utils/paths_utils.py @@ -10,7 +10,6 @@ import os import pickle -import anthrohive import click import h5py import numpy as np @@ -391,7 +390,11 @@ def render(path_handle, render_format:str="mp4", cam_names:list=["left"], output data = path['data'] path_horizon = data['time'].shape[0] else: - data = path['env_infos']['obs_dict'] + obs_dict = path['env_infos']['obs_dict'] + data = {key: obs_dict[key] for key in obs_dict.keys()} + if 'visual_dict' in path['env_infos'].keys(): + visual_dict = path['env_infos']['visual_dict'] + data.update({key: visual_dict[key] for key in visual_dict.keys()}) path_horizon = path['env_infos']['time'].shape[0] # find full key name From 2bab12fa086433da5804a0df79cc5f8daf9f06b0 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Tue, 28 Jul 2026 10:43:28 -0400 Subject: [PATCH 47/49] BUGFIX: Updating the signature of plotnsave_paths everywhere --- robohive/utils/examine_env.py | 4 ++-- robohive/utils/paths_utils.py | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/robohive/utils/examine_env.py b/robohive/utils/examine_env.py index f8370a64..513c0c11 100644 --- a/robohive/utils/examine_env.py +++ b/robohive/utils/examine_env.py @@ -113,8 +113,8 @@ def main(env_name, policy_path, mode, seed, num_episodes, render, camera_name, o # plot paths if plot_paths: - file_name = output_dir + '/' + output_name + '{}'.format(time_stamp) - plotnsave_paths(paths, env=env, fileName_prefix=file_name) + file_name = output_name + '{}'.format(time_stamp) + plotnsave_paths(paths, env_handle=env, output_dir=output_dir, output_name=file_name) # render visuals keys if env.visual_keys and render_visuals: diff --git a/robohive/utils/paths_utils.py b/robohive/utils/paths_utils.py index ff3f6d69..743518f6 100644 --- a/robohive/utils/paths_utils.py +++ b/robohive/utils/paths_utils.py @@ -18,6 +18,7 @@ from robohive.utils import gym from robohive.utils.dict_utils import dict_numpify, flatten_dict +from robohive.logger.grouped_datasets import Trace #TODO: Harmonize names, remove rollout_paths, use path for one and paths for multiple @@ -28,6 +29,8 @@ def _normalize_paths(paths): if isinstance(paths, (list, tuple)): return {'Trial{}'.format(i): path for i, path in enumerate(paths)} + if isinstance(paths, Trace): + return dict(paths.trace) return paths From 68c27831ea5b19b4e40bc71954ae47ad6e955f45 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Fri, 31 Jul 2026 10:36:28 -0400 Subject: [PATCH 48/49] BUGFIX: newly minted hardware_reset function was ignoring spaces as well as calibration hardware_reset now takes an explicit space='sim'|'hdr' parameter and indexes reset_pos via data_id (sim) or hdr_id (hdr), matching the pattern already used in hardware_apply_controls. It now applies actuator['scale']/actuator['offset'] before handing the position to device['robot'].reset(...), same conversion hardware_apply_controls does for per-step controls. --- robohive/robot/robot.py | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index 741e7feb..f4adee6c 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -196,14 +196,23 @@ def hardware_apply_controls(self, control, space='hdr'): # move actuated dofs to a target position (blocking, large-displacement — distinct from # the per-dt hardware_apply_controls above; hardware classes implement this via their own # min-jerk/via-point trajectories) - def hardware_reset(self, reset_pos): + def hardware_reset(self, reset_pos, space='sim'): + """ + reset_pos: target qpos vector + space: 'sim' (full sim qpos vector, addressed via data_id) or + 'hdr' (per-actuator vector ordered by hdr_id, as in hardware_apply_controls) + """ + assert space in ['sim', 'hdr'], "space must be 'sim' or 'hdr'" for name, device in self.robot_config.items(): if name == 'default_robot': continue qpos_actuators = [a for a in device.get('actuator', []) if a['data_type'] == 'qpos'] if qpos_actuators: - hw_q = [np.clip(reset_pos[a['data_id']], a['pos_range'][0], a['pos_range'][1]) - for a in qpos_actuators] + hw_q = [] + for a in qpos_actuators: + pos = reset_pos[a['data_id']] if space == 'sim' else reset_pos[a['hdr_id']] + pos = np.clip(pos, a['pos_range'][0], a['pos_range'][1]) + hw_q.append(pos*a['scale'] + a['offset']) device['robot'].reset(hw_q) else: # passive device (tendon-driven gripper, camera, etc.) — no qpos target to @@ -700,7 +709,8 @@ def reset(self, prompt("\nRollout took:{}".format(t_reset_start- self.time_start)) prompt("\aResetting {}: ".format(self.name), 'white', 'on_grey', flush=True, end="") # send request to all devices, actuated and passive alike - self.hardware_reset(feasibe_pos) + # feasibe_pos was built above via actuator['data_id'] indexing, i.e. sim space + self.hardware_reset(feasibe_pos, space='sim') if blocking: input("press a key to start rollout") From e8aad6a57924e7010c109d273bf5e9577b376c57 Mon Sep 17 00:00:00 2001 From: Vikash Kumar Date: Fri, 31 Jul 2026 10:38:15 -0400 Subject: [PATCH 49/49] BUGFIX: mixing vel_range and pos_range in the vel-mode branch, looks like a copy-paste typo. Fixed --- robohive/robot/robot.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robohive/robot/robot.py b/robohive/robot/robot.py index f4adee6c..26cf5e66 100644 --- a/robohive/robot/robot.py +++ b/robohive/robot/robot.py @@ -543,7 +543,7 @@ def normalize_actions(self, controls, out_space='sim', unnormalize=False): act_rng = (actuator['pos_range'][1]-actuator['pos_range'][0])/2.0 elif self._act_mode == "vel": act_mid = (actuator['vel_range'][1]+actuator['vel_range'][0])/2.0 - act_rng = (actuator['vel_range'][1]-actuator['pos_range'][0])/2.0 + act_rng = (actuator['vel_range'][1]-actuator['vel_range'][0])/2.0 else: raise TypeError("Unknown act mode: {}".format(self._act_mode))