From d8f9f1aaa9427a5756c6932649f8f1b9f1a777d4 Mon Sep 17 00:00:00 2001 From: Kevin Lin Date: Tue, 29 Oct 2024 20:07:21 -0500 Subject: [PATCH 1/6] Fix typo --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 21ea151dda..71f5b66779 100644 --- a/README.md +++ b/README.md @@ -27,7 +27,7 @@ Data-driven algorithms, such as reinforcement learning and imitation learning, p * a modular design that offers great flexibility in designing new robot simulation environments; * a high-quality implementation of robot controllers and off-the-shelf learning algorithms to lower the barriers to entry. -This framework was originally developed in late 2017 by researchers in [Stanford Vision and Learning Lab](http://svl.stanford.edu) (SVL) as an internal tool for robot learning research. Now, it is actively maintained and used for robotics research projects in SVL, the [UT Robot Perception and Learning Lab](http://rpl.cs.utexas.edu) (RPL) and NVIDIA [Genearlist Embodied Agent Research Group](https://research.nvidia.com/labs/gear/) (GEAR). We welcome community contributions to this project. For details, please check out our [contributing guidelines](CONTRIBUTING.md). +This framework was originally developed in late 2017 by researchers in [Stanford Vision and Learning Lab](http://svl.stanford.edu) (SVL) as an internal tool for robot learning research. Now, it is actively maintained and used for robotics research projects in SVL, the [UT Robot Perception and Learning Lab](http://rpl.cs.utexas.edu) (RPL) and NVIDIA [Generalist Embodied Agent Research Group](https://research.nvidia.com/labs/gear/) (GEAR). We welcome community contributions to this project. For details, please check out our [contributing guidelines](CONTRIBUTING.md). **Robosuite** offers a modular design of APIs for building new environments, robot embodiments, and robot controllers with procedural generation. We highlight these primary features below: From bcb7c9fbe4ccbb5e9a4d44fbd57bd1d72306161f Mon Sep 17 00:00:00 2001 From: Kevin Lin Date: Tue, 29 Oct 2024 23:50:09 -0500 Subject: [PATCH 2/6] Fix test_linear_interpolator case --- robosuite/controllers/parts/controller.py | 4 +-- .../mobile_base/mobile_base_controller.py | 4 +-- .../test_linear_interpolator.py | 36 +++++++++---------- 3 files changed, 20 insertions(+), 24 deletions(-) diff --git a/robosuite/controllers/parts/controller.py b/robosuite/controllers/parts/controller.py index bf867da6f6..2b3b4f58b7 100644 --- a/robosuite/controllers/parts/controller.py +++ b/robosuite/controllers/parts/controller.py @@ -258,8 +258,8 @@ def update_initial_joints(self, initial_joints): self.update(force=True) if self.ref_name is not None: - self.initial_ee_pos = self.ee_pos - self.initial_ee_ori_mat = self.ee_ori_mat + self.initial_ref_pos = self.ref_pos + self.initial_ref_ori_mat = self.ref_ori_mat def clip_torques(self, torques): """ diff --git a/robosuite/controllers/parts/mobile_base/mobile_base_controller.py b/robosuite/controllers/parts/mobile_base/mobile_base_controller.py index c5e1850c54..d06c4b58ab 100644 --- a/robosuite/controllers/parts/mobile_base/mobile_base_controller.py +++ b/robosuite/controllers/parts/mobile_base/mobile_base_controller.py @@ -160,8 +160,8 @@ def update_initial_joints(self, initial_joints): """ self.initial_joint = np.array(initial_joints) self.update(force=True) - self.initial_ee_pos = self.ee_pos - self.initial_ee_ori_mat = self.ee_ori_mat + self.initial_ref_pos = self.ref_pos + self.initial_ref_ori_mat = self.ref_ori_mat def clip_torques(self, torques): """ diff --git a/tests/test_controllers/test_linear_interpolator.py b/tests/test_controllers/test_linear_interpolator.py index d12729562c..98dadc5b2f 100644 --- a/tests/test_controllers/test_linear_interpolator.py +++ b/tests/test_controllers/test_linear_interpolator.py @@ -28,6 +28,7 @@ import numpy as np import robosuite as suite +from robosuite.controllers.composite.composite_controller_factory import load_composite_controller_config import robosuite.utils.transform_utils as T # Define the threshold locations, delta values, and ratio # @@ -70,7 +71,7 @@ def step(env, action, current_torques): env.sim.forward() env._pre_action(action, policy_step) last_torques = current_torques - current_torques = env.robots[0].torques + current_torques = env.robots[0].composite_controller.part_controllers['right'].torques summed_abs_delta_torques += np.abs(current_torques - last_torques) env.sim.step() policy_step = False @@ -83,7 +84,7 @@ def step(env, action, current_torques): # Running the actual test # def test_linear_interpolator(): - for controller_name in ["IK_POSE", "OSC_POSE"]: + for controller_name in [None, "BASIC"]: # None corresponds to robot's default controller for traj in ["pos", "ori"]: @@ -95,16 +96,11 @@ def test_linear_interpolator(): # Define numpy seed so we guarantee consistent starting pos / ori for each trajectory np.random.seed(3) - # Define controller path to load - controller_path = os.path.join( - os.path.dirname(__file__), - "../../robosuite", - "controllers/config/{}.json".format(controller_name.lower()), + # load a composite controller + controller_config = load_composite_controller_config( + controller=controller_name, + robot="Sawyer", ) - with open(controller_path) as f: - controller_config = json.load(f) - controller_config["interpolation"] = interpolator - controller_config["ramp_ratio"] = 1.0 # Now, create a test env for testing the controller on env = suite.make( @@ -124,8 +120,8 @@ def test_linear_interpolator(): # Hardcode the starting position for sawyer init_qpos = [-0.5538, -0.8208, 0.4155, 1.8409, -0.4955, 0.6482, 1.9628] env.robots[0].set_robot_joint_positions(init_qpos) - env.robots[0].controller.update_initial_joints(init_qpos) - env.robots[0].controller.reset_goal() + env.robots[0].composite_controller.part_controllers["right"].update_initial_joints(init_qpos) + env.robots[0].composite_controller.part_controllers["right"].reset_goal() # Notify user a new trajectory is beginning print( @@ -140,19 +136,19 @@ def test_linear_interpolator(): # Keep track of state of robot eef (pos, ori (euler)) and torques current_torques = np.zeros(7) - initial_state = [env.robots[0]._hand_pos, T.mat2quat(env.robots[0]._hand_orn)] + initial_state = [env.robots[0]._hand_pos["right"], T.mat2quat(env.robots[0]._hand_orn["right"])] dstate = [ - env.robots[0]._hand_pos - initial_state[0], - T.mat2euler(T.quat2mat(T.quat_distance(T.mat2quat(env.robots[0]._hand_orn), initial_state[1]))), + env.robots[0]._hand_pos["right"] - initial_state[0], + T.mat2euler(T.quat2mat(T.quat_distance(T.mat2quat(env.robots[0]._hand_orn["right"]), initial_state[1]))), ] # Define the uniform trajectory action if traj == "pos": - pos_act = pos_action_ik if controller_name == "IK_POSE" else pos_action_osc + pos_act = pos_action_osc rot_act = np.zeros(3) else: pos_act = np.zeros(3) - rot_act = rot_action_ik if controller_name == "IK_POSE" else rot_action_osc + rot_act = rot_action_osc # Compose the action action = np.concatenate([pos_act, rot_act, [0]]) @@ -171,8 +167,8 @@ def test_linear_interpolator(): summed_abs_delta_torques[j] += summed_torques timesteps[j] += 1 dstate = [ - env.robots[0]._hand_pos - initial_state[0], - T.mat2euler(T.quat2mat(T.quat_distance(T.mat2quat(env.robots[0]._hand_orn), initial_state[1]))), + env.robots[0]._hand_pos["right"] - initial_state[0], + T.mat2euler(T.quat2mat(T.quat_distance(T.mat2quat(env.robots[0]._hand_orn["right"]), initial_state[1]))), ] # When finished, print out the timestep results From 9363a964f4cbcaa38f3fb36f1535b7649afd4a8f Mon Sep 17 00:00:00 2001 From: Kevin Lin Date: Wed, 30 Oct 2024 00:13:35 -0500 Subject: [PATCH 3/6] Fix jaco input dof --- robosuite/models/grippers/jaco_three_finger_gripper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robosuite/models/grippers/jaco_three_finger_gripper.py b/robosuite/models/grippers/jaco_three_finger_gripper.py index 12b3625e10..3a379a37dc 100644 --- a/robosuite/models/grippers/jaco_three_finger_gripper.py +++ b/robosuite/models/grippers/jaco_three_finger_gripper.py @@ -76,7 +76,7 @@ def speed(self): @property def dof(self): - return 3 + return 1 class JacoThreeFingerDexterousGripper(JacoThreeFingerGripperBase): From aeac5ec68b4fc7d6e7aa0a7e399f5a9be52b427d Mon Sep 17 00:00:00 2001 From: Kevin Lin Date: Wed, 30 Oct 2024 00:41:10 -0500 Subject: [PATCH 4/6] Fixes for var impedance test --- .../test_variable_impedance.py | 28 ++++++++++--------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/tests/test_controllers/test_variable_impedance.py b/tests/test_controllers/test_variable_impedance.py index 7dc80c41de..a31bb61721 100644 --- a/tests/test_controllers/test_variable_impedance.py +++ b/tests/test_controllers/test_variable_impedance.py @@ -30,6 +30,7 @@ import numpy as np import robosuite as suite +from robosuite.controllers.composite.composite_controller_factory import load_composite_controller_config # Define the rate of change when sweeping through kp / damping values num_timesteps_per_change = 10 @@ -56,20 +57,21 @@ def test_variable_impedance(): # Define numpy seed so we guarantee consistent starting pos / ori for each trajectory np.random.seed(3) - # Define controller path to load - controller_path = os.path.join( - os.path.dirname(__file__), "../../robosuite", "controllers/config/{}.json".format(controller_name.lower()) - ) - - # Load the controller - with open(controller_path) as f: - controller_config = json.load(f) - + composite_controller_config = load_composite_controller_config(controller=None, robot="Sawyer") + controller_config = composite_controller_config["body_parts"]["right"] + controller_config["type"] = controller_name # Manually edit impedance settings controller_config["impedance_mode"] = "variable" controller_config["kp_limits"] = [0, 300] controller_config["damping_limits"] = [0, 10] + if controller_name == "OSC_POSITION": + controller_config["output_min"] = [0.05, 0.05, 0.05] + controller_config["output_max"] = [-0.05, -0.05, -0.05] + elif controller_name == "JOINT_POSITION": + controller_config["output_min"] = -1 + controller_config["output_max"] = 1 + # Now, create a test env for testing the controller on env = suite.make( "Lift", @@ -79,7 +81,7 @@ def test_variable_impedance(): use_camera_obs=False, horizon=10000, control_freq=20, - controller_configs=controller_config, + controller_configs=composite_controller_config, ) # Setup printing options for numbers @@ -116,7 +118,7 @@ def test_variable_impedance(): # Hardcode the starting position for sawyer init_qpos = [-0.5538, -0.8208, 0.4155, 1.8409, -0.4955, 0.6482, 1.9628] env.robots[0].set_robot_joint_positions(init_qpos) - env.robots[0].controller.update_initial_joints(init_qpos) + env.robots[0].composite_controller.part_controllers["right"].update_initial_joints(init_qpos) # Notify user a new test is beginning print("\nTesting controller {} while sweeping {}...".format(controller_name, gain)) @@ -126,7 +128,7 @@ def test_variable_impedance(): env.viewer.set_camera(camera_id=0) # Keep track of relative changes in robot eef position - last_pos = env.robots[0]._hand_pos + last_pos = env.robots[0]._hand_pos["right"] # Initialize gains if gain == "kp": @@ -155,7 +157,7 @@ def test_variable_impedance(): env.render() # Update the current change in state - cur_pos = env.robots[0]._hand_pos + cur_pos = env.robots[0]._hand_pos["right"] # If we're at the end of the increase, switch direction of traj and gain changes if i == int(num_timesteps_per_change / percent_increase): From 125ddc5ebef7fe684a4994418c08a3c5c976cdda Mon Sep 17 00:00:00 2001 From: Kevin Lin Date: Wed, 30 Oct 2024 00:45:27 -0500 Subject: [PATCH 5/6] Fix formatting --- tests/test_controllers/test_linear_interpolator.py | 10 +++++++--- tests/test_grippers/test_all_grippers.py | 1 + tests/test_grippers/test_panda_gripper.py | 1 + tests/test_grippers/test_rethink_gripper.py | 1 + 4 files changed, 10 insertions(+), 3 deletions(-) diff --git a/tests/test_controllers/test_linear_interpolator.py b/tests/test_controllers/test_linear_interpolator.py index 98dadc5b2f..26626549d1 100644 --- a/tests/test_controllers/test_linear_interpolator.py +++ b/tests/test_controllers/test_linear_interpolator.py @@ -71,7 +71,7 @@ def step(env, action, current_torques): env.sim.forward() env._pre_action(action, policy_step) last_torques = current_torques - current_torques = env.robots[0].composite_controller.part_controllers['right'].torques + current_torques = env.robots[0].composite_controller.part_controllers["right"].torques summed_abs_delta_torques += np.abs(current_torques - last_torques) env.sim.step() policy_step = False @@ -139,7 +139,9 @@ def test_linear_interpolator(): initial_state = [env.robots[0]._hand_pos["right"], T.mat2quat(env.robots[0]._hand_orn["right"])] dstate = [ env.robots[0]._hand_pos["right"] - initial_state[0], - T.mat2euler(T.quat2mat(T.quat_distance(T.mat2quat(env.robots[0]._hand_orn["right"]), initial_state[1]))), + T.mat2euler( + T.quat2mat(T.quat_distance(T.mat2quat(env.robots[0]._hand_orn["right"]), initial_state[1])) + ), ] # Define the uniform trajectory action @@ -168,7 +170,9 @@ def test_linear_interpolator(): timesteps[j] += 1 dstate = [ env.robots[0]._hand_pos["right"] - initial_state[0], - T.mat2euler(T.quat2mat(T.quat_distance(T.mat2quat(env.robots[0]._hand_orn["right"]), initial_state[1]))), + T.mat2euler( + T.quat2mat(T.quat_distance(T.mat2quat(env.robots[0]._hand_orn["right"]), initial_state[1])) + ), ] # When finished, print out the timestep results diff --git a/tests/test_grippers/test_all_grippers.py b/tests/test_grippers/test_all_grippers.py index 3794ab1155..d0c589f7e3 100644 --- a/tests/test_grippers/test_all_grippers.py +++ b/tests/test_grippers/test_all_grippers.py @@ -6,6 +6,7 @@ Obviously, if an environment crashes during runtime, that is considered a failure as well. """ + from robosuite.models.grippers import GRIPPER_MAPPING diff --git a/tests/test_grippers/test_panda_gripper.py b/tests/test_grippers/test_panda_gripper.py index 3139ae2690..7b785a71b0 100644 --- a/tests/test_grippers/test_panda_gripper.py +++ b/tests/test_grippers/test_panda_gripper.py @@ -1,6 +1,7 @@ """ Tests panda gripper on grabbing task """ + from robosuite.models.grippers import GripperTester, PandaGripper diff --git a/tests/test_grippers/test_rethink_gripper.py b/tests/test_grippers/test_rethink_gripper.py index 7b937878bd..2ca4f5345f 100644 --- a/tests/test_grippers/test_rethink_gripper.py +++ b/tests/test_grippers/test_rethink_gripper.py @@ -1,6 +1,7 @@ """ Tests two finger gripper and left two finger gripper on grabbing task """ + from robosuite.models.grippers import GripperTester, RethinkGripper From 9a6c384eaeb13bd1622004946fde5d72bfee38a0 Mon Sep 17 00:00:00 2001 From: Kevin Lin Date: Wed, 30 Oct 2024 00:58:07 -0500 Subject: [PATCH 6/6] Remove imports --- tests/test_controllers/test_variable_impedance.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/tests/test_controllers/test_variable_impedance.py b/tests/test_controllers/test_variable_impedance.py index a31bb61721..36fbdbd8d2 100644 --- a/tests/test_controllers/test_variable_impedance.py +++ b/tests/test_controllers/test_variable_impedance.py @@ -24,8 +24,6 @@ """ import argparse -import json -import os import numpy as np