From 0abf3a7d539b81e231f7b72a7900c9a23c86a086 Mon Sep 17 00:00:00 2001 From: yuqix Date: Tue, 19 Nov 2024 16:03:10 -0600 Subject: [PATCH 1/6] add latest actions in composite controller --- .../composite/composite_controller.py | 2 + .../default/composite/joint_position.json | 87 +++++++++++++++++++ 2 files changed, 89 insertions(+) create mode 100644 robosuite/controllers/config/default/composite/joint_position.json diff --git a/robosuite/controllers/composite/composite_controller.py b/robosuite/controllers/composite/composite_controller.py index b832827c38..8cf832e54a 100644 --- a/robosuite/controllers/composite/composite_controller.py +++ b/robosuite/controllers/composite/composite_controller.py @@ -228,6 +228,7 @@ def __init__(self, sim: MjSim, robot_model: RobotModel, grippers: Dict[str, Grip # task space actions (such as end effector poses) to joint actions (such as joint angles or joint torques) self._whole_body_controller_action_split_indexes: OrderedDict = OrderedDict() + self._latest_all_joint_angle_action: Optional[np.ndarray] = None def _init_controllers(self): for part_name in self.part_controller_config.keys(): @@ -309,6 +310,7 @@ def set_goal(self, all_action): target_qpos = self.joint_action_policy.solve(all_action[: self.joint_action_policy.control_dim]) # create new all_action vector with the IK solver's actions first all_action = np.concatenate([target_qpos, all_action[self.joint_action_policy.control_dim :]]) + self._latest_all_joint_angle_action = all_action for part_name, controller in self.part_controllers.items(): start_idx, end_idx = self._action_split_indexes[part_name] action = all_action[start_idx:end_idx] diff --git a/robosuite/controllers/config/default/composite/joint_position.json b/robosuite/controllers/config/default/composite/joint_position.json new file mode 100644 index 0000000000..da14139aac --- /dev/null +++ b/robosuite/controllers/config/default/composite/joint_position.json @@ -0,0 +1,87 @@ +{ + "type": "BASIC", + "body_parts": { + "arms": { + "right": { + "type": "JOINT_POSITION", + "input_max": 1, + "input_min": -1, + "output_max": 0.5, + "output_min": -0.5, + "kd": 200, + "kv": 200, + "kp": 1000, + "velocity_limits": [-1,1], + "kp_limits": [0, 1000], + "interpolation": null, + "ramp_ratio": 0.2, + "gripper": { + "type": "GRIP" + } + }, + "left": { + "type": "JOINT_POSITION", + "input_max": 1, + "input_min": -1, + "output_max": 0.5, + "output_min": -0.5, + "kd": 200, + "kv": 200, + "kp": 1000, + "velocity_limits": [-1,1], + "kp_limits": [0, 1000], + "interpolation": null, + "ramp_ratio": 0.2, + "gripper": { + "type": "GRIP" + } + } + }, + "torso": { + "type" : "JOINT_POSITION", + "input_max": 1, + "input_min": -1, + "output_max": 0.5, + "output_min": -0.5, + "kd": 200, + "kv": 200, + "kp": 1000, + "velocity_limits": [-1,1], + "kp_limits": [0, 1000], + "interpolation": null, + "ramp_ratio": 0.2 + }, + "head": { + "type" : "JOINT_POSITION", + "input_max": 1, + "input_min": -1, + "output_max": 0.5, + "output_min": -0.5, + "kd": 200, + "kv": 200, + "kp": 1000, + "velocity_limits": [-1,1], + "kp_limits": [0, 1000], + "interpolation": null, + "ramp_ratio": 0.2 + }, + "base": { + "type": "JOINT_VELOCITY", + "interpolation": "null" + }, + "legs": { + "type": "JOINT_POSITION", + "input_max": 1, + "input_min": -1, + "output_max": 0.5, + "output_min": -0.5, + "kd": 200, + "kv": 200, + "kp": 1000, + "velocity_limits": [-1,1], + "kp_limits": [0, 1000], + "interpolation": null, + "ramp_ratio": 0.2 + } + } +} \ No newline at end of file From bf018a001cbda5f81499096a60cdc5ee277451dc Mon Sep 17 00:00:00 2001 From: yuqix Date: Wed, 20 Nov 2024 18:54:25 -0600 Subject: [PATCH 2/6] add joint bypassing flag --- .../composite/composite_controller.py | 16 ++++- .../default_mink_ik_gr1_arms_only.json | 68 +++++++++++++++++++ .../third_party_controller/mink_controller.py | 5 ++ 3 files changed, 86 insertions(+), 3 deletions(-) create mode 100644 robosuite/examples/third_party_controller/default_mink_ik_gr1_arms_only.json diff --git a/robosuite/controllers/composite/composite_controller.py b/robosuite/controllers/composite/composite_controller.py index 8cf832e54a..a7d6e16274 100644 --- a/robosuite/controllers/composite/composite_controller.py +++ b/robosuite/controllers/composite/composite_controller.py @@ -229,6 +229,7 @@ def __init__(self, sim: MjSim, robot_model: RobotModel, grippers: Dict[str, Grip self._whole_body_controller_action_split_indexes: OrderedDict = OrderedDict() self._latest_all_joint_angle_action: Optional[np.ndarray] = None + self._use_joint_angle_action_input: bool = False def _init_controllers(self): for part_name in self.part_controller_config.keys(): @@ -307,9 +308,10 @@ def setup_whole_body_controller_action_split_idx(self): previous_idx = last_idx def set_goal(self, all_action): - target_qpos = self.joint_action_policy.solve(all_action[: self.joint_action_policy.control_dim]) - # create new all_action vector with the IK solver's actions first - all_action = np.concatenate([target_qpos, all_action[self.joint_action_policy.control_dim :]]) + if not self._use_joint_angle_action_input: + target_qpos = self.joint_action_policy.solve(all_action[: self.joint_action_policy.control_dim]) + # create new all_action vector with the IK solver's actions first + all_action = np.concatenate([target_qpos, all_action[self.joint_action_policy.control_dim :]]) self._latest_all_joint_angle_action = all_action for part_name, controller in self.part_controllers.items(): start_idx, end_idx = self._action_split_indexes[part_name] @@ -328,6 +330,9 @@ def action_limits(self): Returns the action limits for the whole body controller. Corresponds to each term in the action vector passed to env.step(). """ + if self._use_joint_angle_action_input: + return super().action_limits + low, high = [], [] # assumption: IK solver's actions come first low_c, high_c = self.joint_action_policy.control_limits @@ -430,6 +435,11 @@ def _validate_composite_controller_specific_config(self) -> None: # Update the configuration with only the valid reference names self.composite_controller_specific_config["ref_name"] = valid_ref_names + # Set the use joint angle action input flag + self._use_joint_angle_action_input = self.composite_controller_specific_config.get( + "use_joint_angle_action_input", False + ) + def _init_joint_action_policy(self): joint_names: str = [] for part_name in self.composite_controller_specific_config["actuation_part_names"]: diff --git a/robosuite/examples/third_party_controller/default_mink_ik_gr1_arms_only.json b/robosuite/examples/third_party_controller/default_mink_ik_gr1_arms_only.json new file mode 100644 index 0000000000..8e22f725ad --- /dev/null +++ b/robosuite/examples/third_party_controller/default_mink_ik_gr1_arms_only.json @@ -0,0 +1,68 @@ +{ + "type": "WHOLE_BODY_MINK_IK", + "composite_controller_specific_configs": { + "ref_name": ["gripper0_right_grip_site", "gripper0_left_grip_site"], + "interpolation": null, + "actuation_part_names": ["right", "left"], + "max_dq": 4, + "ik_pseudo_inverse_damping": 5e-2, + "ik_integration_dt": 1e-1, + "ik_input_type": "absolute", + "ik_input_ref_frame": "world", + "ik_input_rotation_repr": "axis_angle", + "verbose": false, + "ik_posture_weights": { + "robot0_l_shoulder_pitch": 4.0, + "robot0_r_shoulder_pitch": 4.0, + "robot0_l_shoulder_roll": 3.0, + "robot0_r_shoulder_roll": 3.0, + "robot0_l_shoulder_yaw": 2.0, + "robot0_r_shoulder_yaw": 2.0 + }, + "ik_hand_pos_cost": 1.0, + "ik_hand_ori_cost": 0.5, + "use_joint_angle_action_input": false + }, + "body_parts": { + "arms": { + "right": { + "type" : "JOINT_POSITION", + "input_max": 1, + "input_min": -1, + "input_type": "absolute", + "output_max": 0.5, + "output_min": -0.5, + "kd": 200, + "kv": 200, + "kp": 1000, + "velocity_limits": [-1,1], + "kp_limits": [0, 1000], + "interpolation": null, + "ramp_ratio": 0.2, + "gripper": { + "type": "GRIP", + "use_action_scaling": false + } + }, + "left": { + "type" : "JOINT_POSITION", + "input_max": 1, + "input_min": -1, + "input_type": "absolute", + "output_max": 0.5, + "output_min": -0.5, + "kd": 200, + "kv": 200, + "kp": 1000, + "velocity_limits": [-1,1], + "kp_limits": [0, 1000], + "interpolation": null, + "ramp_ratio": 0.2, + "gripper": { + "type": "GRIP", + "use_action_scaling": false + } + } + } + } +} diff --git a/robosuite/examples/third_party_controller/mink_controller.py b/robosuite/examples/third_party_controller/mink_controller.py index 6d796ea6ee..2e2781f875 100644 --- a/robosuite/examples/third_party_controller/mink_controller.py +++ b/robosuite/examples/third_party_controller/mink_controller.py @@ -475,6 +475,11 @@ def _validate_composite_controller_specific_config(self) -> None: # Update the configuration with only the valid posture weights self.composite_controller_specific_config["ik_posture_weights"] = valid_posture_weights + # Set the use joint angle action input flag + self._use_joint_angle_action_input = self.composite_controller_specific_config.get( + "use_joint_angle_action_input", False + ) + def _init_joint_action_policy(self): joint_names: str = [] for part_name in self.composite_controller_specific_config["actuation_part_names"]: From 4a5b805dc10abd0b58f87e1e29ce3ea725605571 Mon Sep 17 00:00:00 2001 From: xieleo5 Date: Wed, 20 Nov 2024 20:50:32 -0800 Subject: [PATCH 3/6] pre commit fix --- robosuite/controllers/composite/composite_controller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robosuite/controllers/composite/composite_controller.py b/robosuite/controllers/composite/composite_controller.py index a7d6e16274..e4fed8ae24 100644 --- a/robosuite/controllers/composite/composite_controller.py +++ b/robosuite/controllers/composite/composite_controller.py @@ -332,7 +332,7 @@ def action_limits(self): """ if self._use_joint_angle_action_input: return super().action_limits - + low, high = [], [] # assumption: IK solver's actions come first low_c, high_c = self.joint_action_policy.control_limits From e486350f2511264a084157411d062430da00008c Mon Sep 17 00:00:00 2001 From: xieleo5 Date: Thu, 21 Nov 2024 21:26:17 -0800 Subject: [PATCH 4/6] remove wrong joint position config --- .../default/composite/joint_position.json | 87 ------------------- 1 file changed, 87 deletions(-) delete mode 100644 robosuite/controllers/config/default/composite/joint_position.json diff --git a/robosuite/controllers/config/default/composite/joint_position.json b/robosuite/controllers/config/default/composite/joint_position.json deleted file mode 100644 index da14139aac..0000000000 --- a/robosuite/controllers/config/default/composite/joint_position.json +++ /dev/null @@ -1,87 +0,0 @@ -{ - "type": "BASIC", - "body_parts": { - "arms": { - "right": { - "type": "JOINT_POSITION", - "input_max": 1, - "input_min": -1, - "output_max": 0.5, - "output_min": -0.5, - "kd": 200, - "kv": 200, - "kp": 1000, - "velocity_limits": [-1,1], - "kp_limits": [0, 1000], - "interpolation": null, - "ramp_ratio": 0.2, - "gripper": { - "type": "GRIP" - } - }, - "left": { - "type": "JOINT_POSITION", - "input_max": 1, - "input_min": -1, - "output_max": 0.5, - "output_min": -0.5, - "kd": 200, - "kv": 200, - "kp": 1000, - "velocity_limits": [-1,1], - "kp_limits": [0, 1000], - "interpolation": null, - "ramp_ratio": 0.2, - "gripper": { - "type": "GRIP" - } - } - }, - "torso": { - "type" : "JOINT_POSITION", - "input_max": 1, - "input_min": -1, - "output_max": 0.5, - "output_min": -0.5, - "kd": 200, - "kv": 200, - "kp": 1000, - "velocity_limits": [-1,1], - "kp_limits": [0, 1000], - "interpolation": null, - "ramp_ratio": 0.2 - }, - "head": { - "type" : "JOINT_POSITION", - "input_max": 1, - "input_min": -1, - "output_max": 0.5, - "output_min": -0.5, - "kd": 200, - "kv": 200, - "kp": 1000, - "velocity_limits": [-1,1], - "kp_limits": [0, 1000], - "interpolation": null, - "ramp_ratio": 0.2 - }, - "base": { - "type": "JOINT_VELOCITY", - "interpolation": "null" - }, - "legs": { - "type": "JOINT_POSITION", - "input_max": 1, - "input_min": -1, - "output_max": 0.5, - "output_min": -0.5, - "kd": 200, - "kv": 200, - "kp": 1000, - "velocity_limits": [-1,1], - "kp_limits": [0, 1000], - "interpolation": null, - "ramp_ratio": 0.2 - } - } -} \ No newline at end of file From d65e9dab62bfe6ebd46a2e9b41c85f5831051580 Mon Sep 17 00:00:00 2001 From: xieleo5 Date: Thu, 21 Nov 2024 21:37:56 -0800 Subject: [PATCH 5/6] add teleop view in table arena --- robosuite/models/assets/arenas/table_arena.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/robosuite/models/assets/arenas/table_arena.xml b/robosuite/models/assets/arenas/table_arena.xml index 294e0ca3c3..a6f06c98c3 100644 --- a/robosuite/models/assets/arenas/table_arena.xml +++ b/robosuite/models/assets/arenas/table_arena.xml @@ -47,5 +47,7 @@ + + From dbfefb281b48646b3a18f30e6480d6a0de2eb1db Mon Sep 17 00:00:00 2001 From: xieleo5 Date: Tue, 26 Nov 2024 13:49:59 -0800 Subject: [PATCH 6/6] fix teleop view --- robosuite/models/assets/arenas/table_arena.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robosuite/models/assets/arenas/table_arena.xml b/robosuite/models/assets/arenas/table_arena.xml index a6f06c98c3..202a98d189 100644 --- a/robosuite/models/assets/arenas/table_arena.xml +++ b/robosuite/models/assets/arenas/table_arena.xml @@ -48,6 +48,6 @@ - +