From d49546c3bf0092335b3ff7f8833048e4df9a5ecf Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Wed, 23 Aug 2023 17:04:27 -0700 Subject: [PATCH 1/5] add trunk control for tiago --- igibson/controllers/controller_base.py | 2 +- igibson/robots/robot_base.py | 1 - igibson/robots/tiago.py | 36 ++++++++++---------------- 3 files changed, 15 insertions(+), 24 deletions(-) diff --git a/igibson/controllers/controller_base.py b/igibson/controllers/controller_base.py index aa9ef9894..0a98114be 100644 --- a/igibson/controllers/controller_base.py +++ b/igibson/controllers/controller_base.py @@ -1,4 +1,4 @@ -from collections import Iterable +from collections.abc import Iterable import numpy as np diff --git a/igibson/robots/robot_base.py b/igibson/robots/robot_base.py index 32a15375a..08321d261 100644 --- a/igibson/robots/robot_base.py +++ b/igibson/robots/robot_base.py @@ -416,7 +416,6 @@ def _create_continuous_action_space(self): limits = controller.command_input_limits low.append(np.array([-np.inf] * controller.command_dim) if limits is None else limits[0]) high.append(np.array([np.inf] * controller.command_dim) if limits is None else limits[1]) - return gym.spaces.Box( shape=(self.action_dim,), low=np.concatenate(low), high=np.concatenate(high), dtype=np.float32 ) diff --git a/igibson/robots/tiago.py b/igibson/robots/tiago.py index 64a302678..cca786ff6 100644 --- a/igibson/robots/tiago.py +++ b/igibson/robots/tiago.py @@ -362,28 +362,20 @@ def _default_controller_config(self): cfg["base"] = {"JointController": self._default_base_controller_configs} for arm in self.arm_names: - # Use default IK controller -- also need to override joint idx being controlled to include trunk in default - # IK arm controller - cfg["arm_{}".format(arm)]["InverseKinematicsController"]["joint_idx"] = np.concatenate( - [self.trunk_control_idx, self.arm_control_idx[arm]] - ) - # TODO: is this not assigning the trunk to both arms? - - # If using rigid trunk, we also clamp its limits - if self.rigid_trunk: - cfg["arm_{}".format(arm)]["InverseKinematicsController"]["control_limits"]["position"][0][ - self.trunk_control_idx - ] = self.untucked_default_joint_pos[self.trunk_control_idx] - cfg["arm_{}".format(arm)]["InverseKinematicsController"]["control_limits"]["position"][1][ - self.trunk_control_idx - ] = self.untucked_default_joint_pos[self.trunk_control_idx] - - cfg["arm_{}".format(arm)]["InverseKinematicsController"]["ik_joint_idx"] = np.array( - [ - self.joint_idx_to_ik_joint_idx[x] - for x in cfg["arm_{}".format(arm)]["InverseKinematicsController"]["joint_idx"] - ] - ) + for arm_cfg in cfg["arm_{}".format(arm)].values(): + + if arm == "left": + # Need to override joint idx being controlled to include trunk in default arm controller configs + arm_cfg["joint_idx"] = np.concatenate([self.trunk_control_idx, self.arm_control_idx[arm]]) + + # If using rigid trunk, we also clamp its limits + # TODO: How to handle for right arm which has a fixed trunk internally even though the trunk is moving + # via the left arm?? + if self.rigid_trunk: + arm_cfg["control_limits"]["position"][0][self.trunk_control_idx] = \ + self.untucked_default_joint_pos[self.trunk_control_idx] + arm_cfg["control_limits"]["position"][1][self.trunk_control_idx] = \ + self.untucked_default_joint_pos[self.trunk_control_idx] return cfg From dfd8fdd9318596e9087837a30f8aa52fa4a7df74 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Wed, 23 Aug 2023 17:11:43 -0700 Subject: [PATCH 2/5] Update controller_base.py --- igibson/controllers/controller_base.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/igibson/controllers/controller_base.py b/igibson/controllers/controller_base.py index 0a98114be..aa9ef9894 100644 --- a/igibson/controllers/controller_base.py +++ b/igibson/controllers/controller_base.py @@ -1,4 +1,4 @@ -from collections.abc import Iterable +from collections import Iterable import numpy as np From 0b320b1fcf195cc9d574d9e19261b162184cc21b Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Wed, 23 Aug 2023 17:12:10 -0700 Subject: [PATCH 3/5] Update robot_base.py --- igibson/robots/robot_base.py | 1 + 1 file changed, 1 insertion(+) diff --git a/igibson/robots/robot_base.py b/igibson/robots/robot_base.py index 08321d261..859a67245 100644 --- a/igibson/robots/robot_base.py +++ b/igibson/robots/robot_base.py @@ -416,6 +416,7 @@ def _create_continuous_action_space(self): limits = controller.command_input_limits low.append(np.array([-np.inf] * controller.command_dim) if limits is None else limits[0]) high.append(np.array([np.inf] * controller.command_dim) if limits is None else limits[1]) + return gym.spaces.Box( shape=(self.action_dim,), low=np.concatenate(low), high=np.concatenate(high), dtype=np.float32 ) From a58f42007674bd89021389f82626ce7cfc8dce02 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Wed, 23 Aug 2023 17:12:28 -0700 Subject: [PATCH 4/5] Update robot_base.py --- igibson/robots/robot_base.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/igibson/robots/robot_base.py b/igibson/robots/robot_base.py index 859a67245..2cfa3f40b 100644 --- a/igibson/robots/robot_base.py +++ b/igibson/robots/robot_base.py @@ -416,7 +416,7 @@ def _create_continuous_action_space(self): limits = controller.command_input_limits low.append(np.array([-np.inf] * controller.command_dim) if limits is None else limits[0]) high.append(np.array([np.inf] * controller.command_dim) if limits is None else limits[1]) - + return gym.spaces.Box( shape=(self.action_dim,), low=np.concatenate(low), high=np.concatenate(high), dtype=np.float32 ) From 2e93c5d95e7783cf27adc0bacfa6914100210c61 Mon Sep 17 00:00:00 2001 From: Sujay Garlanka Date: Wed, 23 Aug 2023 17:12:55 -0700 Subject: [PATCH 5/5] Update robot_base.py --- igibson/robots/robot_base.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/igibson/robots/robot_base.py b/igibson/robots/robot_base.py index 2cfa3f40b..32a15375a 100644 --- a/igibson/robots/robot_base.py +++ b/igibson/robots/robot_base.py @@ -416,7 +416,7 @@ def _create_continuous_action_space(self): limits = controller.command_input_limits low.append(np.array([-np.inf] * controller.command_dim) if limits is None else limits[0]) high.append(np.array([np.inf] * controller.command_dim) if limits is None else limits[1]) - + return gym.spaces.Box( shape=(self.action_dim,), low=np.concatenate(low), high=np.concatenate(high), dtype=np.float32 )