From 1dc4a383a6f539dd95ce07d4eb062cd2604a07ac Mon Sep 17 00:00:00 2001 From: You Liang Tan Date: Sun, 3 Nov 2024 00:06:25 -0700 Subject: [PATCH 1/7] fix part controller demo Signed-off-by: You Liang Tan --- .../composite/composite_controller_factory.py | 6 ++-- .../controllers/parts/controller_factory.py | 2 +- robosuite/demos/demo_control.py | 16 ++++++++-- robosuite/robots/fixed_base_robot.py | 1 + robosuite/robots/robot.py | 9 ++++-- robosuite/utils/input_utils.py | 29 +++++++++++++++++++ .../test_variable_impedance.py | 2 +- 7 files changed, 56 insertions(+), 9 deletions(-) diff --git a/robosuite/controllers/composite/composite_controller_factory.py b/robosuite/controllers/composite/composite_controller_factory.py index 3a67b227c5..815e4e6e18 100644 --- a/robosuite/controllers/composite/composite_controller_factory.py +++ b/robosuite/controllers/composite/composite_controller_factory.py @@ -75,13 +75,13 @@ def load_composite_controller_config(controller: Optional[str] = None, robot: Op validate_composite_controller_config(composite_controller_config) body_parts_controller_configs = composite_controller_config.pop("body_parts_controller_configs", {}) - composite_controller_config["body_parts"] = {} + composite_controller_config["body_parts_controller_configs"] = {} for part_name, part_config in body_parts_controller_configs.items(): if part_name == "arms": for arm_name, arm_config in part_config.items(): - composite_controller_config["body_parts"][arm_name] = arm_config + composite_controller_config["body_parts_controller_configs"][arm_name] = arm_config else: - composite_controller_config["body_parts"][part_name] = part_config + composite_controller_config["body_parts_controller_configs"][part_name] = part_config return composite_controller_config diff --git a/robosuite/controllers/parts/controller_factory.py b/robosuite/controllers/parts/controller_factory.py index b5c6331852..88cf9ee52d 100644 --- a/robosuite/controllers/parts/controller_factory.py +++ b/robosuite/controllers/parts/controller_factory.py @@ -52,7 +52,7 @@ def load_part_controller_config(custom_fpath=None, default_controller=None): # Store the default controller config fpath associated with the requested controller custom_fpath = os.path.join( - os.path.dirname(__file__), "..", "controllers/config/parts/{}.json".format(default_controller.lower()) + os.path.dirname(__file__), "..", "config/default/parts/{}.json".format(default_controller.lower()) ) # Assert that the fpath to load the controller is not empty diff --git a/robosuite/demos/demo_control.py b/robosuite/demos/demo_control.py index 32d0dafd01..7066b8c57b 100644 --- a/robosuite/demos/demo_control.py +++ b/robosuite/demos/demo_control.py @@ -89,10 +89,22 @@ joint_dim = 6 if options["robots"] == "UR5e" else (16 if options["robots"] == "GR1" else 7) # Choose controller - controller_name = choose_controller() + controller_name = choose_part_controller() + part_controller_config = suite.load_part_controller_config(default_controller=controller_name) + # explicitly set the gripper type + part_controller_config["gripper"] = dict( + type="GRIP", + ) # Load the desired controller - options["controller_configs"] = suite.load_part_controller_config(default_controller=controller_name) + controller_config = dict( + type="BASIC", + body_parts_controller_configs=dict( + left=part_controller_config, # refer to arm + right=part_controller_config, # refer to arm + ), + ) + options["controller_configs"] = controller_config # Define the pre-defined controller actions to use (action_dim, num_test_steps, test_value) controller_settings = { diff --git a/robosuite/robots/fixed_base_robot.py b/robosuite/robots/fixed_base_robot.py index 27f088c761..7ec1576172 100644 --- a/robosuite/robots/fixed_base_robot.py +++ b/robosuite/robots/fixed_base_robot.py @@ -49,6 +49,7 @@ def _load_controller(self): robot_model=self.robot_model, grippers={self.get_gripper_name(arm): self.gripper[arm] for arm in self.arms}, ) + print("Loaded composite controller: {}".format(self.composite_controller)) self._load_arm_controllers() diff --git a/robosuite/robots/robot.py b/robosuite/robots/robot.py index 9a2bade2a5..ed082ec052 100644 --- a/robosuite/robots/robot.py +++ b/robosuite/robots/robot.py @@ -70,7 +70,10 @@ def __init__( self.composite_controller_config = composite_controller_config else: self.composite_controller_config = load_composite_controller_config(robot=robot_type) - self.part_controller_config = copy.deepcopy(self.composite_controller_config.get("body_parts", {})) + + self.part_controller_config = copy.deepcopy( + self.composite_controller_config.get("body_parts_controller_configs", {}) + ) self.gripper = self._input2dict(None) self.gripper_type = self._input2dict(gripper_type) @@ -145,7 +148,9 @@ def _postprocess_part_controller_config(self): Remove unused parts that are not in the controller. Called by _load_controller() function """ - for part_name, controller_config in self.composite_controller_config.get("body_parts", {}).items(): + for part_name, controller_config in self.composite_controller_config.get( + "body_parts_controller_configs", {} + ).items(): if not self.has_part(part_name): ROBOSUITE_DEFAULT_LOGGER.warn( f'The config has defined for the controller "{part_name}", ' diff --git a/robosuite/utils/input_utils.py b/robosuite/utils/input_utils.py index 1931b7539b..04431ad635 100644 --- a/robosuite/utils/input_utils.py +++ b/robosuite/utils/input_utils.py @@ -67,6 +67,35 @@ def choose_controller(): return controllers[k] +def choose_part_controller(): + """ + Prints out part controller options, and returns the requested part controller name + + Returns: + str: Chosen part controller name + """ + controllers = list(suite.ALL_PART_CONTROLLERS) + + # Select controller to use + print("Here is a list of part controllers in the suite:\n") + + for k, controller in enumerate(controllers): + print("[{}] {}".format(k, controller)) + print() + try: + s = input( + "Choose a part controller for the robot " + "(enter a number from 0 to {}): ".format(len(controllers) - 1) + ) + # parse input into a number within range + k = min(max(int(s), 0), len(controllers) - 1) + except: + k = 0 + print("Input is not valid. Use {} by default.".format(controllers)[k]) + + # Return chosen controller + return controllers[k] + + def choose_multi_arm_config(): """ Prints out multi-arm environment configuration options, and returns the requested config name diff --git a/tests/test_controllers/test_variable_impedance.py b/tests/test_controllers/test_variable_impedance.py index 36fbdbd8d2..109124af87 100644 --- a/tests/test_controllers/test_variable_impedance.py +++ b/tests/test_controllers/test_variable_impedance.py @@ -56,7 +56,7 @@ def test_variable_impedance(): np.random.seed(3) composite_controller_config = load_composite_controller_config(controller=None, robot="Sawyer") - controller_config = composite_controller_config["body_parts"]["right"] + controller_config = composite_controller_config["body_parts_controller_configs"]["right"] controller_config["type"] = controller_name # Manually edit impedance settings controller_config["impedance_mode"] = "variable" From 457d9e72ca20e9d446f600986a3f7a8f97e1c71f Mon Sep 17 00:00:00 2001 From: You Liang Tan Date: Sun, 3 Nov 2024 00:07:43 -0700 Subject: [PATCH 2/7] readd part controller config from v1.4 Signed-off-by: You Liang Tan --- .gitignore | 2 +- .../config/parts/default_baxter.json | 11 +++++++++++ .../controllers/config/parts/default_iiwa.json | 11 +++++++++++ .../config/parts/default_jaco (copy).json | 11 +++++++++++ .../controllers/config/parts/default_jaco.json | 11 +++++++++++ .../config/parts/default_kinova3.json | 11 +++++++++++ .../config/parts/default_panda.json | 11 +++++++++++ .../config/parts/default_sawyer.json | 11 +++++++++++ .../controllers/config/parts/default_ur5e.json | 11 +++++++++++ .../controllers/config/parts/ik_pose.json | 7 +++++++ .../config/parts/joint_position.json | 15 +++++++++++++++ .../controllers/config/parts/joint_torque.json | 10 ++++++++++ .../config/parts/joint_velocity.json | 11 +++++++++++ .../controllers/config/parts/osc_pose.json | 18 ++++++++++++++++++ .../controllers/config/parts/osc_position.json | 16 ++++++++++++++++ 15 files changed, 166 insertions(+), 1 deletion(-) create mode 100644 robosuite/controllers/config/parts/default_baxter.json create mode 100644 robosuite/controllers/config/parts/default_iiwa.json create mode 100644 robosuite/controllers/config/parts/default_jaco (copy).json create mode 100644 robosuite/controllers/config/parts/default_jaco.json create mode 100644 robosuite/controllers/config/parts/default_kinova3.json create mode 100644 robosuite/controllers/config/parts/default_panda.json create mode 100644 robosuite/controllers/config/parts/default_sawyer.json create mode 100644 robosuite/controllers/config/parts/default_ur5e.json create mode 100644 robosuite/controllers/config/parts/ik_pose.json create mode 100644 robosuite/controllers/config/parts/joint_position.json create mode 100644 robosuite/controllers/config/parts/joint_torque.json create mode 100644 robosuite/controllers/config/parts/joint_velocity.json create mode 100644 robosuite/controllers/config/parts/osc_pose.json create mode 100644 robosuite/controllers/config/parts/osc_position.json diff --git a/.gitignore b/.gitignore index a919c684d4..951e0ca2df 100644 --- a/.gitignore +++ b/.gitignore @@ -17,7 +17,7 @@ eggs/ .eggs/ lib/ lib64/ -parts/ + !robosuite/controllers/parts sdist/ var/ diff --git a/robosuite/controllers/config/parts/default_baxter.json b/robosuite/controllers/config/parts/default_baxter.json new file mode 100644 index 0000000000..960d52fcd3 --- /dev/null +++ b/robosuite/controllers/config/parts/default_baxter.json @@ -0,0 +1,11 @@ +{ + "type": "JOINT_VELOCITY", + "input_max": 1, + "input_min": -1, + "output_max": 0.5, + "output_min": -0.5, + "kp": 0.03, + "velocity_limits": [-1, 1], + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/default_iiwa.json b/robosuite/controllers/config/parts/default_iiwa.json new file mode 100644 index 0000000000..73d9018da0 --- /dev/null +++ b/robosuite/controllers/config/parts/default_iiwa.json @@ -0,0 +1,11 @@ +{ + "type" : "JOINT_VELOCITY", + "input_max": 1, + "input_min": -1, + "output_max": 0.5, + "output_min": -0.5, + "kp": 0.03, + "velocity_limits": [-1,1], + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/default_jaco (copy).json b/robosuite/controllers/config/parts/default_jaco (copy).json new file mode 100644 index 0000000000..73d9018da0 --- /dev/null +++ b/robosuite/controllers/config/parts/default_jaco (copy).json @@ -0,0 +1,11 @@ +{ + "type" : "JOINT_VELOCITY", + "input_max": 1, + "input_min": -1, + "output_max": 0.5, + "output_min": -0.5, + "kp": 0.03, + "velocity_limits": [-1,1], + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/default_jaco.json b/robosuite/controllers/config/parts/default_jaco.json new file mode 100644 index 0000000000..73d9018da0 --- /dev/null +++ b/robosuite/controllers/config/parts/default_jaco.json @@ -0,0 +1,11 @@ +{ + "type" : "JOINT_VELOCITY", + "input_max": 1, + "input_min": -1, + "output_max": 0.5, + "output_min": -0.5, + "kp": 0.03, + "velocity_limits": [-1,1], + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/default_kinova3.json b/robosuite/controllers/config/parts/default_kinova3.json new file mode 100644 index 0000000000..73d9018da0 --- /dev/null +++ b/robosuite/controllers/config/parts/default_kinova3.json @@ -0,0 +1,11 @@ +{ + "type" : "JOINT_VELOCITY", + "input_max": 1, + "input_min": -1, + "output_max": 0.5, + "output_min": -0.5, + "kp": 0.03, + "velocity_limits": [-1,1], + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/default_panda.json b/robosuite/controllers/config/parts/default_panda.json new file mode 100644 index 0000000000..73d9018da0 --- /dev/null +++ b/robosuite/controllers/config/parts/default_panda.json @@ -0,0 +1,11 @@ +{ + "type" : "JOINT_VELOCITY", + "input_max": 1, + "input_min": -1, + "output_max": 0.5, + "output_min": -0.5, + "kp": 0.03, + "velocity_limits": [-1,1], + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/default_sawyer.json b/robosuite/controllers/config/parts/default_sawyer.json new file mode 100644 index 0000000000..73d9018da0 --- /dev/null +++ b/robosuite/controllers/config/parts/default_sawyer.json @@ -0,0 +1,11 @@ +{ + "type" : "JOINT_VELOCITY", + "input_max": 1, + "input_min": -1, + "output_max": 0.5, + "output_min": -0.5, + "kp": 0.03, + "velocity_limits": [-1,1], + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/default_ur5e.json b/robosuite/controllers/config/parts/default_ur5e.json new file mode 100644 index 0000000000..73d9018da0 --- /dev/null +++ b/robosuite/controllers/config/parts/default_ur5e.json @@ -0,0 +1,11 @@ +{ + "type" : "JOINT_VELOCITY", + "input_max": 1, + "input_min": -1, + "output_max": 0.5, + "output_min": -0.5, + "kp": 0.03, + "velocity_limits": [-1,1], + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/ik_pose.json b/robosuite/controllers/config/parts/ik_pose.json new file mode 100644 index 0000000000..45a0223f20 --- /dev/null +++ b/robosuite/controllers/config/parts/ik_pose.json @@ -0,0 +1,7 @@ +{ + "type" : "IK_POSE", + "ik_pos_limit": 0.02, + "ik_ori_limit": 0.05, + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/joint_position.json b/robosuite/controllers/config/parts/joint_position.json new file mode 100644 index 0000000000..86cb4f576f --- /dev/null +++ b/robosuite/controllers/config/parts/joint_position.json @@ -0,0 +1,15 @@ +{ + "type": "JOINT_POSITION", + "input_max": 1, + "input_min": -1, + "output_max": 0.05, + "output_min": -0.05, + "kp": 50, + "damping_ratio": 1, + "impedance_mode": "fixed", + "kp_limits": [0, 300], + "damping_ratio_limits": [0, 10], + "qpos_limits": null, + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/joint_torque.json b/robosuite/controllers/config/parts/joint_torque.json new file mode 100644 index 0000000000..eab76b8b38 --- /dev/null +++ b/robosuite/controllers/config/parts/joint_torque.json @@ -0,0 +1,10 @@ +{ + "type": "JOINT_TORQUE", + "input_max": 1, + "input_min": -1, + "output_max": 0.1, + "output_min": -0.1, + "torque_limits": null, + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/joint_velocity.json b/robosuite/controllers/config/parts/joint_velocity.json new file mode 100644 index 0000000000..4d8752a3a2 --- /dev/null +++ b/robosuite/controllers/config/parts/joint_velocity.json @@ -0,0 +1,11 @@ +{ + "type" : "JOINT_VELOCITY", + "input_max": 1, + "input_min": -1, + "output_max": 0.5, + "output_min": -0.5, + "kp": 3.0, + "velocity_limits": [-1,1], + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/osc_pose.json b/robosuite/controllers/config/parts/osc_pose.json new file mode 100644 index 0000000000..8dc645e44b --- /dev/null +++ b/robosuite/controllers/config/parts/osc_pose.json @@ -0,0 +1,18 @@ +{ + "type": "OSC_POSE", + "input_max": 1, + "input_min": -1, + "output_max": [0.05, 0.05, 0.05, 0.5, 0.5, 0.5], + "output_min": [-0.05, -0.05, -0.05, -0.5, -0.5, -0.5], + "kp": 150, + "damping_ratio": 1, + "impedance_mode": "fixed", + "kp_limits": [0, 300], + "damping_ratio_limits": [0, 10], + "position_limits": null, + "orientation_limits": null, + "uncouple_pos_ori": true, + "control_delta": true, + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/osc_position.json b/robosuite/controllers/config/parts/osc_position.json new file mode 100644 index 0000000000..8e1fd3b164 --- /dev/null +++ b/robosuite/controllers/config/parts/osc_position.json @@ -0,0 +1,16 @@ +{ + "type": "OSC_POSITION", + "input_max": 1, + "input_min": -1, + "output_max": [0.05, 0.05, 0.05], + "output_min": [-0.05, -0.05, -0.05], + "kp": 150, + "damping_ratio": 1, + "impedance_mode": "fixed", + "kp_limits": [0, 300], + "damping_ratio_limits": [0, 10], + "position_limits": null, + "control_delta": true, + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file From 6d2acf6d8b8f433d01cbcc533890b5ea8d7c5aa4 Mon Sep 17 00:00:00 2001 From: You Liang Tan Date: Sun, 3 Nov 2024 23:26:27 -0800 Subject: [PATCH 3/7] add part configs Signed-off-by: You Liang Tan --- .gitignore | 1 - .../config/default/parts/ik_pose.json | 7 +++++++ .../config/default/parts/joint_position.json | 15 +++++++++++++++ .../config/default/parts/joint_torque.json | 10 ++++++++++ .../config/default/parts/joint_velocity.json | 11 +++++++++++ .../config/default/parts/osc_pose.json | 18 ++++++++++++++++++ .../config/default/parts/osc_position.json | 16 ++++++++++++++++ 7 files changed, 77 insertions(+), 1 deletion(-) create mode 100644 robosuite/controllers/config/default/parts/ik_pose.json create mode 100644 robosuite/controllers/config/default/parts/joint_position.json create mode 100644 robosuite/controllers/config/default/parts/joint_torque.json create mode 100644 robosuite/controllers/config/default/parts/joint_velocity.json create mode 100644 robosuite/controllers/config/default/parts/osc_pose.json create mode 100644 robosuite/controllers/config/default/parts/osc_position.json diff --git a/.gitignore b/.gitignore index 951e0ca2df..21b4da1e98 100644 --- a/.gitignore +++ b/.gitignore @@ -17,7 +17,6 @@ eggs/ .eggs/ lib/ lib64/ - !robosuite/controllers/parts sdist/ var/ diff --git a/robosuite/controllers/config/default/parts/ik_pose.json b/robosuite/controllers/config/default/parts/ik_pose.json new file mode 100644 index 0000000000..45a0223f20 --- /dev/null +++ b/robosuite/controllers/config/default/parts/ik_pose.json @@ -0,0 +1,7 @@ +{ + "type" : "IK_POSE", + "ik_pos_limit": 0.02, + "ik_ori_limit": 0.05, + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/default/parts/joint_position.json b/robosuite/controllers/config/default/parts/joint_position.json new file mode 100644 index 0000000000..86cb4f576f --- /dev/null +++ b/robosuite/controllers/config/default/parts/joint_position.json @@ -0,0 +1,15 @@ +{ + "type": "JOINT_POSITION", + "input_max": 1, + "input_min": -1, + "output_max": 0.05, + "output_min": -0.05, + "kp": 50, + "damping_ratio": 1, + "impedance_mode": "fixed", + "kp_limits": [0, 300], + "damping_ratio_limits": [0, 10], + "qpos_limits": null, + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/default/parts/joint_torque.json b/robosuite/controllers/config/default/parts/joint_torque.json new file mode 100644 index 0000000000..eab76b8b38 --- /dev/null +++ b/robosuite/controllers/config/default/parts/joint_torque.json @@ -0,0 +1,10 @@ +{ + "type": "JOINT_TORQUE", + "input_max": 1, + "input_min": -1, + "output_max": 0.1, + "output_min": -0.1, + "torque_limits": null, + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/default/parts/joint_velocity.json b/robosuite/controllers/config/default/parts/joint_velocity.json new file mode 100644 index 0000000000..4d8752a3a2 --- /dev/null +++ b/robosuite/controllers/config/default/parts/joint_velocity.json @@ -0,0 +1,11 @@ +{ + "type" : "JOINT_VELOCITY", + "input_max": 1, + "input_min": -1, + "output_max": 0.5, + "output_min": -0.5, + "kp": 3.0, + "velocity_limits": [-1,1], + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/default/parts/osc_pose.json b/robosuite/controllers/config/default/parts/osc_pose.json new file mode 100644 index 0000000000..8dc645e44b --- /dev/null +++ b/robosuite/controllers/config/default/parts/osc_pose.json @@ -0,0 +1,18 @@ +{ + "type": "OSC_POSE", + "input_max": 1, + "input_min": -1, + "output_max": [0.05, 0.05, 0.05, 0.5, 0.5, 0.5], + "output_min": [-0.05, -0.05, -0.05, -0.5, -0.5, -0.5], + "kp": 150, + "damping_ratio": 1, + "impedance_mode": "fixed", + "kp_limits": [0, 300], + "damping_ratio_limits": [0, 10], + "position_limits": null, + "orientation_limits": null, + "uncouple_pos_ori": true, + "control_delta": true, + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file diff --git a/robosuite/controllers/config/default/parts/osc_position.json b/robosuite/controllers/config/default/parts/osc_position.json new file mode 100644 index 0000000000..8e1fd3b164 --- /dev/null +++ b/robosuite/controllers/config/default/parts/osc_position.json @@ -0,0 +1,16 @@ +{ + "type": "OSC_POSITION", + "input_max": 1, + "input_min": -1, + "output_max": [0.05, 0.05, 0.05], + "output_min": [-0.05, -0.05, -0.05], + "kp": 150, + "damping_ratio": 1, + "impedance_mode": "fixed", + "kp_limits": [0, 300], + "damping_ratio_limits": [0, 10], + "position_limits": null, + "control_delta": true, + "interpolation": null, + "ramp_ratio": 0.2 +} \ No newline at end of file From c28abb4443e7ff4cee5dfc3a44aafc79cff5e627 Mon Sep 17 00:00:00 2001 From: You Liang Tan Date: Sun, 3 Nov 2024 23:40:54 -0800 Subject: [PATCH 4/7] fix tune joints Signed-off-by: You Liang Tan --- docs/modules/controllers.rst | 14 +++++++------- robosuite/scripts/tune_joints.py | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/docs/modules/controllers.rst b/docs/modules/controllers.rst index ab0594440f..f3c7cf7743 100644 --- a/docs/modules/controllers.rst +++ b/docs/modules/controllers.rst @@ -222,19 +222,19 @@ Controller Settings Loading a Controller --------------------- -By default, if no controller configuration is specified during environment creation, then ``JOINT_VELOCITY`` controllers with robot-specific configurations will be used. +By default, user will use the `load_composite_controller_config()` method to create a controller configuration. Using a Default Controller Configuration ***************************************** -Any controller can be used with its default configuration, and can be easily loaded into a given environment by calling its name as shown below (where ``controller_name`` is one of acceptable controller ``type`` strings): +Any controller can be used with its default configuration, and can be easily loaded into a given environment by calling its name as shown below (where ``controller`` is one of acceptable controller ``type`` strings): .. code-block:: python import robosuite as suite - from robosuite import load_controller_config + from robosuite import load_composite_controller_config - # Load the desired controller's default config as a dict - config = load_controller_config(default_controller=controller_name) + # Load the desired controller config with default Basic controller + config = load_composite_controller_config(controller="BASIC") # Create environment env = suite.make("Lift", robots="Panda", controller_configs=config, ... ) @@ -248,13 +248,13 @@ A custom controller configuration can also be used by simply creating a new conf .. code-block:: python import robosuite as suite - from robosuite import load_controller_config + from robosuite import load_composite_controller_config # Path to config file controller_fpath = "/your/custom/config/filepath/here/filename.json" # Import the file as a dict - config = load_controller_config(custom_fpath=controller_fpath) + config = load_composite_controller_config(controller=controller_fpath) # Create environment env = suite.make("Lift", robots="Panda", controller_configs=config, ... ) diff --git a/robosuite/scripts/tune_joints.py b/robosuite/scripts/tune_joints.py index a77d7ef06d..ea9881c0ad 100644 --- a/robosuite/scripts/tune_joints.py +++ b/robosuite/scripts/tune_joints.py @@ -282,7 +282,7 @@ def print_command(char, info): np.set_printoptions(formatter={"float": lambda x: "{0:0.3f}".format(x)}) # Define the controller - controller_config = robosuite.load_controller_config(default_controller="JOINT_POSITION") + controller_config = robosuite.load_composite_controller_config(controller="BASIC") # make the environment env = robosuite.make( From edffa09d97fdcd47b64b37f844ccf61650baea42 Mon Sep 17 00:00:00 2001 From: Kevin Lin Date: Wed, 6 Nov 2024 15:14:53 -0600 Subject: [PATCH 5/7] Remove redundant part jsons --- .../config/parts/default_baxter.json | 11 ----------- .../controllers/config/parts/default_iiwa.json | 11 ----------- .../config/parts/default_jaco (copy).json | 11 ----------- .../controllers/config/parts/default_jaco.json | 11 ----------- .../config/parts/default_kinova3.json | 11 ----------- .../config/parts/default_panda.json | 11 ----------- .../config/parts/default_sawyer.json | 11 ----------- .../controllers/config/parts/default_ur5e.json | 11 ----------- .../controllers/config/parts/ik_pose.json | 7 ------- .../config/parts/joint_position.json | 15 --------------- .../controllers/config/parts/joint_torque.json | 10 ---------- .../config/parts/joint_velocity.json | 11 ----------- .../controllers/config/parts/osc_pose.json | 18 ------------------ .../controllers/config/parts/osc_position.json | 16 ---------------- 14 files changed, 165 deletions(-) delete mode 100644 robosuite/controllers/config/parts/default_baxter.json delete mode 100644 robosuite/controllers/config/parts/default_iiwa.json delete mode 100644 robosuite/controllers/config/parts/default_jaco (copy).json delete mode 100644 robosuite/controllers/config/parts/default_jaco.json delete mode 100644 robosuite/controllers/config/parts/default_kinova3.json delete mode 100644 robosuite/controllers/config/parts/default_panda.json delete mode 100644 robosuite/controllers/config/parts/default_sawyer.json delete mode 100644 robosuite/controllers/config/parts/default_ur5e.json delete mode 100644 robosuite/controllers/config/parts/ik_pose.json delete mode 100644 robosuite/controllers/config/parts/joint_position.json delete mode 100644 robosuite/controllers/config/parts/joint_torque.json delete mode 100644 robosuite/controllers/config/parts/joint_velocity.json delete mode 100644 robosuite/controllers/config/parts/osc_pose.json delete mode 100644 robosuite/controllers/config/parts/osc_position.json diff --git a/robosuite/controllers/config/parts/default_baxter.json b/robosuite/controllers/config/parts/default_baxter.json deleted file mode 100644 index 960d52fcd3..0000000000 --- a/robosuite/controllers/config/parts/default_baxter.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "type": "JOINT_VELOCITY", - "input_max": 1, - "input_min": -1, - "output_max": 0.5, - "output_min": -0.5, - "kp": 0.03, - "velocity_limits": [-1, 1], - "interpolation": null, - "ramp_ratio": 0.2 -} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/default_iiwa.json b/robosuite/controllers/config/parts/default_iiwa.json deleted file mode 100644 index 73d9018da0..0000000000 --- a/robosuite/controllers/config/parts/default_iiwa.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "type" : "JOINT_VELOCITY", - "input_max": 1, - "input_min": -1, - "output_max": 0.5, - "output_min": -0.5, - "kp": 0.03, - "velocity_limits": [-1,1], - "interpolation": null, - "ramp_ratio": 0.2 -} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/default_jaco (copy).json b/robosuite/controllers/config/parts/default_jaco (copy).json deleted file mode 100644 index 73d9018da0..0000000000 --- a/robosuite/controllers/config/parts/default_jaco (copy).json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "type" : "JOINT_VELOCITY", - "input_max": 1, - "input_min": -1, - "output_max": 0.5, - "output_min": -0.5, - "kp": 0.03, - "velocity_limits": [-1,1], - "interpolation": null, - "ramp_ratio": 0.2 -} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/default_jaco.json b/robosuite/controllers/config/parts/default_jaco.json deleted file mode 100644 index 73d9018da0..0000000000 --- a/robosuite/controllers/config/parts/default_jaco.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "type" : "JOINT_VELOCITY", - "input_max": 1, - "input_min": -1, - "output_max": 0.5, - "output_min": -0.5, - "kp": 0.03, - "velocity_limits": [-1,1], - "interpolation": null, - "ramp_ratio": 0.2 -} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/default_kinova3.json b/robosuite/controllers/config/parts/default_kinova3.json deleted file mode 100644 index 73d9018da0..0000000000 --- a/robosuite/controllers/config/parts/default_kinova3.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "type" : "JOINT_VELOCITY", - "input_max": 1, - "input_min": -1, - "output_max": 0.5, - "output_min": -0.5, - "kp": 0.03, - "velocity_limits": [-1,1], - "interpolation": null, - "ramp_ratio": 0.2 -} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/default_panda.json b/robosuite/controllers/config/parts/default_panda.json deleted file mode 100644 index 73d9018da0..0000000000 --- a/robosuite/controllers/config/parts/default_panda.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "type" : "JOINT_VELOCITY", - "input_max": 1, - "input_min": -1, - "output_max": 0.5, - "output_min": -0.5, - "kp": 0.03, - "velocity_limits": [-1,1], - "interpolation": null, - "ramp_ratio": 0.2 -} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/default_sawyer.json b/robosuite/controllers/config/parts/default_sawyer.json deleted file mode 100644 index 73d9018da0..0000000000 --- a/robosuite/controllers/config/parts/default_sawyer.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "type" : "JOINT_VELOCITY", - "input_max": 1, - "input_min": -1, - "output_max": 0.5, - "output_min": -0.5, - "kp": 0.03, - "velocity_limits": [-1,1], - "interpolation": null, - "ramp_ratio": 0.2 -} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/default_ur5e.json b/robosuite/controllers/config/parts/default_ur5e.json deleted file mode 100644 index 73d9018da0..0000000000 --- a/robosuite/controllers/config/parts/default_ur5e.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "type" : "JOINT_VELOCITY", - "input_max": 1, - "input_min": -1, - "output_max": 0.5, - "output_min": -0.5, - "kp": 0.03, - "velocity_limits": [-1,1], - "interpolation": null, - "ramp_ratio": 0.2 -} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/ik_pose.json b/robosuite/controllers/config/parts/ik_pose.json deleted file mode 100644 index 45a0223f20..0000000000 --- a/robosuite/controllers/config/parts/ik_pose.json +++ /dev/null @@ -1,7 +0,0 @@ -{ - "type" : "IK_POSE", - "ik_pos_limit": 0.02, - "ik_ori_limit": 0.05, - "interpolation": null, - "ramp_ratio": 0.2 -} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/joint_position.json b/robosuite/controllers/config/parts/joint_position.json deleted file mode 100644 index 86cb4f576f..0000000000 --- a/robosuite/controllers/config/parts/joint_position.json +++ /dev/null @@ -1,15 +0,0 @@ -{ - "type": "JOINT_POSITION", - "input_max": 1, - "input_min": -1, - "output_max": 0.05, - "output_min": -0.05, - "kp": 50, - "damping_ratio": 1, - "impedance_mode": "fixed", - "kp_limits": [0, 300], - "damping_ratio_limits": [0, 10], - "qpos_limits": null, - "interpolation": null, - "ramp_ratio": 0.2 -} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/joint_torque.json b/robosuite/controllers/config/parts/joint_torque.json deleted file mode 100644 index eab76b8b38..0000000000 --- a/robosuite/controllers/config/parts/joint_torque.json +++ /dev/null @@ -1,10 +0,0 @@ -{ - "type": "JOINT_TORQUE", - "input_max": 1, - "input_min": -1, - "output_max": 0.1, - "output_min": -0.1, - "torque_limits": null, - "interpolation": null, - "ramp_ratio": 0.2 -} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/joint_velocity.json b/robosuite/controllers/config/parts/joint_velocity.json deleted file mode 100644 index 4d8752a3a2..0000000000 --- a/robosuite/controllers/config/parts/joint_velocity.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "type" : "JOINT_VELOCITY", - "input_max": 1, - "input_min": -1, - "output_max": 0.5, - "output_min": -0.5, - "kp": 3.0, - "velocity_limits": [-1,1], - "interpolation": null, - "ramp_ratio": 0.2 -} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/osc_pose.json b/robosuite/controllers/config/parts/osc_pose.json deleted file mode 100644 index 8dc645e44b..0000000000 --- a/robosuite/controllers/config/parts/osc_pose.json +++ /dev/null @@ -1,18 +0,0 @@ -{ - "type": "OSC_POSE", - "input_max": 1, - "input_min": -1, - "output_max": [0.05, 0.05, 0.05, 0.5, 0.5, 0.5], - "output_min": [-0.05, -0.05, -0.05, -0.5, -0.5, -0.5], - "kp": 150, - "damping_ratio": 1, - "impedance_mode": "fixed", - "kp_limits": [0, 300], - "damping_ratio_limits": [0, 10], - "position_limits": null, - "orientation_limits": null, - "uncouple_pos_ori": true, - "control_delta": true, - "interpolation": null, - "ramp_ratio": 0.2 -} \ No newline at end of file diff --git a/robosuite/controllers/config/parts/osc_position.json b/robosuite/controllers/config/parts/osc_position.json deleted file mode 100644 index 8e1fd3b164..0000000000 --- a/robosuite/controllers/config/parts/osc_position.json +++ /dev/null @@ -1,16 +0,0 @@ -{ - "type": "OSC_POSITION", - "input_max": 1, - "input_min": -1, - "output_max": [0.05, 0.05, 0.05], - "output_min": [-0.05, -0.05, -0.05], - "kp": 150, - "damping_ratio": 1, - "impedance_mode": "fixed", - "kp_limits": [0, 300], - "damping_ratio_limits": [0, 10], - "position_limits": null, - "control_delta": true, - "interpolation": null, - "ramp_ratio": 0.2 -} \ No newline at end of file From 2920183e2ccdc1ff8d86781107d25befdde61830 Mon Sep 17 00:00:00 2001 From: kevin-thankyou-lin <33344633+kevin-thankyou-lin@users.noreply.github.com> Date: Wed, 6 Nov 2024 19:21:29 -0600 Subject: [PATCH 6/7] Remove choose_part_controller from input_utils.py --- robosuite/utils/input_utils.py | 29 ----------------------------- 1 file changed, 29 deletions(-) diff --git a/robosuite/utils/input_utils.py b/robosuite/utils/input_utils.py index 612be8d2dc..a7f6860323 100644 --- a/robosuite/utils/input_utils.py +++ b/robosuite/utils/input_utils.py @@ -67,35 +67,6 @@ def choose_controller(part_controllers=False): return controllers[k] -def choose_part_controller(): - """ - Prints out part controller options, and returns the requested part controller name - - Returns: - str: Chosen part controller name - """ - controllers = list(suite.ALL_PART_CONTROLLERS) - - # Select controller to use - print("Here is a list of part controllers in the suite:\n") - - for k, controller in enumerate(controllers): - print("[{}] {}".format(k, controller)) - print() - try: - s = input( - "Choose a part controller for the robot " + "(enter a number from 0 to {}): ".format(len(controllers) - 1) - ) - # parse input into a number within range - k = min(max(int(s), 0), len(controllers) - 1) - except: - k = 0 - print("Input is not valid. Use {} by default.".format(controllers)[k]) - - # Return chosen controller - return controllers[k] - - def choose_multi_arm_config(): """ Prints out multi-arm environment configuration options, and returns the requested config name From 46c34b2e16bd4a7bca2da8fd6912446b3f863614 Mon Sep 17 00:00:00 2001 From: Abhiram824 Date: Wed, 6 Nov 2024 19:34:45 -0600 Subject: [PATCH 7/7] update refactor code --- .../controllers/composite/composite_controller_factory.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/robosuite/controllers/composite/composite_controller_factory.py b/robosuite/controllers/composite/composite_controller_factory.py index 5885eee0ad..22d0442de4 100644 --- a/robosuite/controllers/composite/composite_controller_factory.py +++ b/robosuite/controllers/composite/composite_controller_factory.py @@ -62,11 +62,11 @@ def refactor_composite_controller_config(controller_config, robot_type, arms): else: new_controller_config = {} new_controller_config["type"] = "BASIC" - new_controller_config["body_parts"] = {} + new_controller_config["body_parts_controller_configs"] = {} for arm in arms: - new_controller_config["body_parts"][arm] = copy.deepcopy(controller_config) - new_controller_config["body_parts"][arm]["gripper"] = {"type": "GRIP"} + new_controller_config["body_parts_controller_configs"][arm] = copy.deepcopy(controller_config) + new_controller_config["body_parts_controller_configs"][arm]["gripper"] = {"type": "GRIP"} return new_controller_config