From ac937af91c923a3981471fb86be2622ba7c36715 Mon Sep 17 00:00:00 2001 From: Andrea Rosasco Date: Tue, 7 May 2024 10:35:18 +0200 Subject: [PATCH 1/5] Update multi_table_arena.py Some of the files in branch v1.4.1 already have the `import Iterable` modified to be compatible with `python 3.10` but this one does not. Updating it. --- robosuite/models/arenas/multi_table_arena.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robosuite/models/arenas/multi_table_arena.py b/robosuite/models/arenas/multi_table_arena.py index ce94ab7c75..f62ef357e2 100644 --- a/robosuite/models/arenas/multi_table_arena.py +++ b/robosuite/models/arenas/multi_table_arena.py @@ -1,4 +1,4 @@ -from collections import Iterable +from collections.abc import Iterable import numpy as np From 93193af178a87146eea7c34e727af596e78d986f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=8C=83=E7=91=BE?= Date: Fri, 17 Jan 2025 15:11:35 +0800 Subject: [PATCH 2/5] opt control --- robosuite/controllers/config/osc_pose.json | 2 +- robosuite/controllers/osc.py | 22 ++++++++++++++++++ robosuite/demos/demo_device_control.py | 26 ++++++++++++++++++++-- robosuite/devices/keyboard.py | 26 +++++++++++----------- robosuite/utils/input_utils.py | 11 ++++----- source_env.sh | 9 ++++++++ 6 files changed, 75 insertions(+), 21 deletions(-) create mode 100644 source_env.sh diff --git a/robosuite/controllers/config/osc_pose.json b/robosuite/controllers/config/osc_pose.json index 8dc645e44b..07f0b1b0b4 100644 --- a/robosuite/controllers/config/osc_pose.json +++ b/robosuite/controllers/config/osc_pose.json @@ -4,7 +4,7 @@ "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, + "kp": 15000, "damping_ratio": 1, "impedance_mode": "fixed", "kp_limits": [0, 300], diff --git a/robosuite/controllers/osc.py b/robosuite/controllers/osc.py index a45843d087..156db74c78 100644 --- a/robosuite/controllers/osc.py +++ b/robosuite/controllers/osc.py @@ -5,6 +5,7 @@ import robosuite.utils.transform_utils as T from robosuite.controllers.base_controller import Controller from robosuite.utils.control_utils import * +from scipy.spatial.transform import Rotation as R # Supported impedance modes IMPEDANCE_MODES = {"fixed", "variable", "variable_kp"} @@ -218,6 +219,27 @@ def set_goal(self, action, set_pos=None, set_ori=None): """ # Update state self.update() + + # + set_pos = self.ee_pos + action[:3] + T_local_global = np.array( + [ + [0, 1, 0], + [-1, 0, 0], + [0, 0, 1] + ],dtype=float + ) + cur_ee_rpy_global = R.from_matrix(self.ee_ori_mat@T_local_global.T).as_euler("xyz", degrees=False) + set_rpy_global = action[3:6] + cur_ee_rpy_global + set_mat_gloabl = R.from_euler("xyz", set_rpy_global, degrees=False).as_matrix() + set_mat_local = set_mat_gloabl@T_local_global + set_ori = set_mat_local + + + if np.any(action[:6]!=0): + print(f"set_pos:{set_pos}") + set_quat = R.from_matrix(set_mat_gloabl).as_quat() + print(f"set_quat(global):{set_quat}") # Parse action based on the impedance mode, and update kp / kd as necessary if self.impedance_mode == "variable": diff --git a/robosuite/demos/demo_device_control.py b/robosuite/demos/demo_device_control.py index 57c8cb1c81..c22696328f 100644 --- a/robosuite/demos/demo_device_control.py +++ b/robosuite/demos/demo_device_control.py @@ -99,9 +99,10 @@ import numpy as np import robosuite as suite -from robosuite import load_controller_config +from robosuite.controllers import load_controller_config from robosuite.utils.input_utils import input2action from robosuite.wrappers import VisualizationWrapper +from scipy.spatial.transform import Rotation as R if __name__ == "__main__": @@ -131,7 +132,8 @@ # Get controller config controller_config = load_controller_config(default_controller=controller_name) - + # controller_config["control_delta"] = False + # Create argument configuration config = { "env_name": args.environment, @@ -177,6 +179,7 @@ else: raise Exception("Invalid device choice: choose either 'keyboard' or 'spacemouse'.") + CNT = 0 while True: # Reset the environment obs = env.reset() @@ -235,7 +238,26 @@ elif rem_action_dim < 0: # We're in an environment with no gripper action space, so trim the action space to be the action dim action = action[: env.action_dim] + + if np.any(action[:6]!=0): + print(f"action:{action}") + pre_pos = obs['robot0_eef_pos'] + print(f"obs_before:{obs['robot0_eef_quat']}") + CNT = 3 # Step through the simulation and render obs, reward, done, info = env.step(action) + # print("----------------") + # print(obs["robot0_eef_pos"]) + # ee_mat = R.from_quat(obs["robot0_eef_quat"]).as_matrix() + # print(ee_mat) + # print("*********************") + env.render() + + if CNT>0: + CNT -= 1 + if CNT <= 3: + print(f"CNT:{CNT}") + print(f"obs:{obs['robot0_eef_pos']}") + print(f"obs:{obs['robot0_eef_quat']}") diff --git a/robosuite/devices/keyboard.py b/robosuite/devices/keyboard.py index fb37648d61..88be33df8b 100644 --- a/robosuite/devices/keyboard.py +++ b/robosuite/devices/keyboard.py @@ -24,7 +24,7 @@ def __init__(self, pos_sensitivity=1.0, rot_sensitivity=1.0): self._reset_state = 0 self._enabled = False - self._pos_step = 0.05 + self._pos_step = 0.001 self.pos_sensitivity = pos_sensitivity self.rot_sensitivity = rot_sensitivity @@ -121,29 +121,29 @@ def on_press(self, key): # controls for moving orientation elif key.char == "z": - drot = rotation_matrix(angle=0.1 * self.rot_sensitivity, direction=[1.0, 0.0, 0.0])[:3, :3] + drot = rotation_matrix(angle=0.001 * self.rot_sensitivity, direction=[1.0, 0.0, 0.0])[:3, :3] self.rotation = self.rotation.dot(drot) # rotates x - self.raw_drotation[1] -= 0.1 * self.rot_sensitivity + self.raw_drotation[1] -= 0.001 * self.rot_sensitivity elif key.char == "x": - drot = rotation_matrix(angle=-0.1 * self.rot_sensitivity, direction=[1.0, 0.0, 0.0])[:3, :3] + drot = rotation_matrix(angle=-0.001 * self.rot_sensitivity, direction=[1.0, 0.0, 0.0])[:3, :3] self.rotation = self.rotation.dot(drot) # rotates x - self.raw_drotation[1] += 0.1 * self.rot_sensitivity + self.raw_drotation[1] += 0.001 * self.rot_sensitivity elif key.char == "t": - drot = rotation_matrix(angle=0.1 * self.rot_sensitivity, direction=[0.0, 1.0, 0.0])[:3, :3] + drot = rotation_matrix(angle=0.001 * self.rot_sensitivity, direction=[0.0, 1.0, 0.0])[:3, :3] self.rotation = self.rotation.dot(drot) # rotates y - self.raw_drotation[0] += 0.1 * self.rot_sensitivity + self.raw_drotation[0] += 0.001 * self.rot_sensitivity elif key.char == "g": - drot = rotation_matrix(angle=-0.1 * self.rot_sensitivity, direction=[0.0, 1.0, 0.0])[:3, :3] + drot = rotation_matrix(angle=-0.001 * self.rot_sensitivity, direction=[0.0, 1.0, 0.0])[:3, :3] self.rotation = self.rotation.dot(drot) # rotates y - self.raw_drotation[0] -= 0.1 * self.rot_sensitivity + self.raw_drotation[0] -= 0.001 * self.rot_sensitivity elif key.char == "c": - drot = rotation_matrix(angle=0.1 * self.rot_sensitivity, direction=[0.0, 0.0, 1.0])[:3, :3] + drot = rotation_matrix(angle=0.001 * self.rot_sensitivity, direction=[0.0, 0.0, 1.0])[:3, :3] self.rotation = self.rotation.dot(drot) # rotates z - self.raw_drotation[2] += 0.1 * self.rot_sensitivity + self.raw_drotation[2] += 0.001 * self.rot_sensitivity elif key.char == "v": - drot = rotation_matrix(angle=-0.1 * self.rot_sensitivity, direction=[0.0, 0.0, 1.0])[:3, :3] + drot = rotation_matrix(angle=-0.001 * self.rot_sensitivity, direction=[0.0, 0.0, 1.0])[:3, :3] self.rotation = self.rotation.dot(drot) # rotates z - self.raw_drotation[2] -= 0.1 * self.rot_sensitivity + self.raw_drotation[2] -= 0.001 * self.rot_sensitivity except AttributeError as e: pass diff --git a/robosuite/utils/input_utils.py b/robosuite/utils/input_utils.py index 26dee46cce..3146db5e9a 100644 --- a/robosuite/utils/input_utils.py +++ b/robosuite/utils/input_utils.py @@ -208,8 +208,8 @@ def input2action(device, robot, active_arm="right", env_configuration=None): # Flip x drotation[0] = -drotation[0] # Scale rotation for teleoperation (tuned for IK) - drotation *= 10 - dpos *= 5 + drotation *= 1 + dpos *= 1 # relative rotation of desired from current eef orientation # map to quat drotation = T.mat2quat(T.euler2mat(drotation)) @@ -233,9 +233,10 @@ def input2action(device, robot, active_arm="right", env_configuration=None): elif controller.name == "OSC_POSE": # Flip z drotation[2] = -drotation[2] - # Scale rotation for teleoperation (tuned for OSC) -- gains tuned for each device - drotation = drotation * 1.5 if isinstance(device, Keyboard) else drotation * 50 - dpos = dpos * 75 if isinstance(device, Keyboard) else dpos * 125 + # # Scale rotation for teleoperation (tuned for OSC) -- gains tuned for each device + # drotation = drotation * 1.5 if isinstance(device, Keyboard) else drotation * 50 + # dpos = dpos * 75 if isinstance(device, Keyboard) else dpos * 125 + dpos = dpos * 1 if isinstance(device, Keyboard) else dpos * 125 elif controller.name == "OSC_POSITION": dpos = dpos * 75 if isinstance(device, Keyboard) else dpos * 125 else: diff --git a/source_env.sh b/source_env.sh new file mode 100644 index 0000000000..5cd6f47e55 --- /dev/null +++ b/source_env.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +# 获取当前脚本的绝对路径 +current_directory=$(cd "$(dirname "$0")"; pwd) + +# 将当前文件夹路径添加到PYTHONPATH +export PYTHONPATH="$PYTHONPATH:$current_directory" + +echo "Current directory added to PYTHONPATH: $current_directory" \ No newline at end of file From 2ff7a2961860b3536087550d07990a9119b2223d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=8C=83=E7=91=BE?= Date: Fri, 17 Jan 2025 15:11:35 +0800 Subject: [PATCH 3/5] opt control --- robosuite/controllers/config/osc_pose.json | 2 +- robosuite/controllers/osc.py | 30 ++++++++++++++++++++ robosuite/demos/demo_device_control.py | 33 ++++++++++++++++++++-- robosuite/devices/keyboard.py | 26 ++++++++--------- robosuite/utils/input_utils.py | 11 ++++---- source_env.sh | 9 ++++++ 6 files changed, 90 insertions(+), 21 deletions(-) create mode 100644 source_env.sh diff --git a/robosuite/controllers/config/osc_pose.json b/robosuite/controllers/config/osc_pose.json index 8dc645e44b..07f0b1b0b4 100644 --- a/robosuite/controllers/config/osc_pose.json +++ b/robosuite/controllers/config/osc_pose.json @@ -4,7 +4,7 @@ "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, + "kp": 15000, "damping_ratio": 1, "impedance_mode": "fixed", "kp_limits": [0, 300], diff --git a/robosuite/controllers/osc.py b/robosuite/controllers/osc.py index a45843d087..a34d6e011f 100644 --- a/robosuite/controllers/osc.py +++ b/robosuite/controllers/osc.py @@ -5,6 +5,7 @@ import robosuite.utils.transform_utils as T from robosuite.controllers.base_controller import Controller from robosuite.utils.control_utils import * +from scipy.spatial.transform import Rotation as R # Supported impedance modes IMPEDANCE_MODES = {"fixed", "variable", "variable_kp"} @@ -218,6 +219,35 @@ def set_goal(self, action, set_pos=None, set_ori=None): """ # Update state self.update() + + # + set_pos = self.ee_pos + action[:3] + T_local_global = np.array( + [ + [0, 1, 0], + [-1, 0, 0], + [0, 0, 1] + ],dtype=float + ) + cur_ee_rpy_global = R.from_matrix(self.ee_ori_mat@T_local_global.T).as_euler("xyz", degrees=False) + set_rpy_global = action[3:6] + cur_ee_rpy_global + set_mat_gloabl = R.from_euler("xyz", set_rpy_global, degrees=False).as_matrix() + set_mat_local = set_mat_gloabl@T_local_global + set_ori = set_mat_local + + + if np.any(action[:6]!=0): + print(f"ee_pos:{self.ee_pos}") + ee_quat = R.from_matrix(self.ee_ori_mat).as_quat() + print(f"ee_quat(local):{ee_quat}") + ee_quat_gloabl = R.from_matrix(self.ee_ori_mat@T_local_global.T).as_quat() + print(f"ee_quat(global):{ee_quat_gloabl}") + + print(f"set_pos:{set_pos}") + set_quat = R.from_matrix(set_mat_gloabl).as_quat() + print(f"set_quat(global):{set_quat}") + set_rpy = R.from_quat(set_quat).as_euler('xyz', degrees=False) + print(f"set_rpy(global):{set_rpy}") # Parse action based on the impedance mode, and update kp / kd as necessary if self.impedance_mode == "variable": diff --git a/robosuite/demos/demo_device_control.py b/robosuite/demos/demo_device_control.py index 57c8cb1c81..8db55fa9c3 100644 --- a/robosuite/demos/demo_device_control.py +++ b/robosuite/demos/demo_device_control.py @@ -99,9 +99,10 @@ import numpy as np import robosuite as suite -from robosuite import load_controller_config +from robosuite.controllers import load_controller_config from robosuite.utils.input_utils import input2action from robosuite.wrappers import VisualizationWrapper +from scipy.spatial.transform import Rotation as R if __name__ == "__main__": @@ -131,7 +132,8 @@ # Get controller config controller_config = load_controller_config(default_controller=controller_name) - + # controller_config["control_delta"] = False + # Create argument configuration config = { "env_name": args.environment, @@ -177,6 +179,7 @@ else: raise Exception("Invalid device choice: choose either 'keyboard' or 'spacemouse'.") + CNT = 0 while True: # Reset the environment obs = env.reset() @@ -235,7 +238,33 @@ elif rem_action_dim < 0: # We're in an environment with no gripper action space, so trim the action space to be the action dim action = action[: env.action_dim] + + if np.any(action[:6]!=0): + print(f"action:{action}") + pre_pos = obs['robot0_eef_pos'] + pre_rpy = R.from_quat(obs['robot0_eef_quat']).as_euler('xyz', degrees=False) + print(f"obs_before:{obs['robot0_eef_quat']}") + print(f"obs_before:{pre_rpy}") + CNT = 3 # Step through the simulation and render obs, reward, done, info = env.step(action) + # print("----------------") + # print(obs["robot0_eef_pos"]) + # ee_mat = R.from_quat(obs["robot0_eef_quat"]).as_matrix() + # print(ee_mat) + # print("*********************") + env.render() + + if CNT>0: + CNT -= 1 + if CNT <= 3: + print(f"CNT:{CNT}") + print(f"obs:{obs['robot0_eef_pos']}") + print(f"obs:{obs['robot0_eef_quat']}") + pre_rpy = R.from_quat(obs['robot0_eef_quat']).as_euler('xyz', degrees=False) + print(f"obs:{pre_rpy}") + if CNT==0: + print("----------------------------------") +5 \ No newline at end of file diff --git a/robosuite/devices/keyboard.py b/robosuite/devices/keyboard.py index fb37648d61..88be33df8b 100644 --- a/robosuite/devices/keyboard.py +++ b/robosuite/devices/keyboard.py @@ -24,7 +24,7 @@ def __init__(self, pos_sensitivity=1.0, rot_sensitivity=1.0): self._reset_state = 0 self._enabled = False - self._pos_step = 0.05 + self._pos_step = 0.001 self.pos_sensitivity = pos_sensitivity self.rot_sensitivity = rot_sensitivity @@ -121,29 +121,29 @@ def on_press(self, key): # controls for moving orientation elif key.char == "z": - drot = rotation_matrix(angle=0.1 * self.rot_sensitivity, direction=[1.0, 0.0, 0.0])[:3, :3] + drot = rotation_matrix(angle=0.001 * self.rot_sensitivity, direction=[1.0, 0.0, 0.0])[:3, :3] self.rotation = self.rotation.dot(drot) # rotates x - self.raw_drotation[1] -= 0.1 * self.rot_sensitivity + self.raw_drotation[1] -= 0.001 * self.rot_sensitivity elif key.char == "x": - drot = rotation_matrix(angle=-0.1 * self.rot_sensitivity, direction=[1.0, 0.0, 0.0])[:3, :3] + drot = rotation_matrix(angle=-0.001 * self.rot_sensitivity, direction=[1.0, 0.0, 0.0])[:3, :3] self.rotation = self.rotation.dot(drot) # rotates x - self.raw_drotation[1] += 0.1 * self.rot_sensitivity + self.raw_drotation[1] += 0.001 * self.rot_sensitivity elif key.char == "t": - drot = rotation_matrix(angle=0.1 * self.rot_sensitivity, direction=[0.0, 1.0, 0.0])[:3, :3] + drot = rotation_matrix(angle=0.001 * self.rot_sensitivity, direction=[0.0, 1.0, 0.0])[:3, :3] self.rotation = self.rotation.dot(drot) # rotates y - self.raw_drotation[0] += 0.1 * self.rot_sensitivity + self.raw_drotation[0] += 0.001 * self.rot_sensitivity elif key.char == "g": - drot = rotation_matrix(angle=-0.1 * self.rot_sensitivity, direction=[0.0, 1.0, 0.0])[:3, :3] + drot = rotation_matrix(angle=-0.001 * self.rot_sensitivity, direction=[0.0, 1.0, 0.0])[:3, :3] self.rotation = self.rotation.dot(drot) # rotates y - self.raw_drotation[0] -= 0.1 * self.rot_sensitivity + self.raw_drotation[0] -= 0.001 * self.rot_sensitivity elif key.char == "c": - drot = rotation_matrix(angle=0.1 * self.rot_sensitivity, direction=[0.0, 0.0, 1.0])[:3, :3] + drot = rotation_matrix(angle=0.001 * self.rot_sensitivity, direction=[0.0, 0.0, 1.0])[:3, :3] self.rotation = self.rotation.dot(drot) # rotates z - self.raw_drotation[2] += 0.1 * self.rot_sensitivity + self.raw_drotation[2] += 0.001 * self.rot_sensitivity elif key.char == "v": - drot = rotation_matrix(angle=-0.1 * self.rot_sensitivity, direction=[0.0, 0.0, 1.0])[:3, :3] + drot = rotation_matrix(angle=-0.001 * self.rot_sensitivity, direction=[0.0, 0.0, 1.0])[:3, :3] self.rotation = self.rotation.dot(drot) # rotates z - self.raw_drotation[2] -= 0.1 * self.rot_sensitivity + self.raw_drotation[2] -= 0.001 * self.rot_sensitivity except AttributeError as e: pass diff --git a/robosuite/utils/input_utils.py b/robosuite/utils/input_utils.py index 26dee46cce..3146db5e9a 100644 --- a/robosuite/utils/input_utils.py +++ b/robosuite/utils/input_utils.py @@ -208,8 +208,8 @@ def input2action(device, robot, active_arm="right", env_configuration=None): # Flip x drotation[0] = -drotation[0] # Scale rotation for teleoperation (tuned for IK) - drotation *= 10 - dpos *= 5 + drotation *= 1 + dpos *= 1 # relative rotation of desired from current eef orientation # map to quat drotation = T.mat2quat(T.euler2mat(drotation)) @@ -233,9 +233,10 @@ def input2action(device, robot, active_arm="right", env_configuration=None): elif controller.name == "OSC_POSE": # Flip z drotation[2] = -drotation[2] - # Scale rotation for teleoperation (tuned for OSC) -- gains tuned for each device - drotation = drotation * 1.5 if isinstance(device, Keyboard) else drotation * 50 - dpos = dpos * 75 if isinstance(device, Keyboard) else dpos * 125 + # # Scale rotation for teleoperation (tuned for OSC) -- gains tuned for each device + # drotation = drotation * 1.5 if isinstance(device, Keyboard) else drotation * 50 + # dpos = dpos * 75 if isinstance(device, Keyboard) else dpos * 125 + dpos = dpos * 1 if isinstance(device, Keyboard) else dpos * 125 elif controller.name == "OSC_POSITION": dpos = dpos * 75 if isinstance(device, Keyboard) else dpos * 125 else: diff --git a/source_env.sh b/source_env.sh new file mode 100644 index 0000000000..5cd6f47e55 --- /dev/null +++ b/source_env.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +# 获取当前脚本的绝对路径 +current_directory=$(cd "$(dirname "$0")"; pwd) + +# 将当前文件夹路径添加到PYTHONPATH +export PYTHONPATH="$PYTHONPATH:$current_directory" + +echo "Current directory added to PYTHONPATH: $current_directory" \ No newline at end of file From 64948e11ce6e01d5edf691c2ccc3fed17c6847cb Mon Sep 17 00:00:00 2001 From: fanjin <> Date: Mon, 20 Jan 2025 10:05:16 +0800 Subject: [PATCH 4/5] fix: change control to absolute pose --- robosuite/controllers/config/osc_pose.json | 4 +-- robosuite/controllers/osc.py | 29 ---------------------- 2 files changed, 2 insertions(+), 31 deletions(-) diff --git a/robosuite/controllers/config/osc_pose.json b/robosuite/controllers/config/osc_pose.json index 07f0b1b0b4..8e3c3b220d 100644 --- a/robosuite/controllers/config/osc_pose.json +++ b/robosuite/controllers/config/osc_pose.json @@ -4,8 +4,8 @@ "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": 15000, - "damping_ratio": 1, + "kp": 8000, + "damping_ratio": 1.0, "impedance_mode": "fixed", "kp_limits": [0, 300], "damping_ratio_limits": [0, 10], diff --git a/robosuite/controllers/osc.py b/robosuite/controllers/osc.py index a34d6e011f..015bcb65d2 100644 --- a/robosuite/controllers/osc.py +++ b/robosuite/controllers/osc.py @@ -220,35 +220,6 @@ def set_goal(self, action, set_pos=None, set_ori=None): # Update state self.update() - # - set_pos = self.ee_pos + action[:3] - T_local_global = np.array( - [ - [0, 1, 0], - [-1, 0, 0], - [0, 0, 1] - ],dtype=float - ) - cur_ee_rpy_global = R.from_matrix(self.ee_ori_mat@T_local_global.T).as_euler("xyz", degrees=False) - set_rpy_global = action[3:6] + cur_ee_rpy_global - set_mat_gloabl = R.from_euler("xyz", set_rpy_global, degrees=False).as_matrix() - set_mat_local = set_mat_gloabl@T_local_global - set_ori = set_mat_local - - - if np.any(action[:6]!=0): - print(f"ee_pos:{self.ee_pos}") - ee_quat = R.from_matrix(self.ee_ori_mat).as_quat() - print(f"ee_quat(local):{ee_quat}") - ee_quat_gloabl = R.from_matrix(self.ee_ori_mat@T_local_global.T).as_quat() - print(f"ee_quat(global):{ee_quat_gloabl}") - - print(f"set_pos:{set_pos}") - set_quat = R.from_matrix(set_mat_gloabl).as_quat() - print(f"set_quat(global):{set_quat}") - set_rpy = R.from_quat(set_quat).as_euler('xyz', degrees=False) - print(f"set_rpy(global):{set_rpy}") - # Parse action based on the impedance mode, and update kp / kd as necessary if self.impedance_mode == "variable": damping_ratio, kp, delta = action[:6], action[6:12], action[12:] From 82f79bcbc716bb89b5de45e67ff864b207287ffa Mon Sep 17 00:00:00 2001 From: fanjin <> Date: Sat, 8 Feb 2025 22:55:19 +0800 Subject: [PATCH 5/5] feat: abs control --- robosuite/controllers/config/osc_pose.json | 4 +- robosuite/controllers/osc.py | 2 +- robosuite/environments/base.py | 67 ++++++++++++++++++++++ 3 files changed, 70 insertions(+), 3 deletions(-) diff --git a/robosuite/controllers/config/osc_pose.json b/robosuite/controllers/config/osc_pose.json index 8e3c3b220d..cff921a2d3 100644 --- a/robosuite/controllers/config/osc_pose.json +++ b/robosuite/controllers/config/osc_pose.json @@ -4,7 +4,7 @@ "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": 8000, + "kp": 150, "damping_ratio": 1.0, "impedance_mode": "fixed", "kp_limits": [0, 300], @@ -12,7 +12,7 @@ "position_limits": null, "orientation_limits": null, "uncouple_pos_ori": true, - "control_delta": true, + "control_delta": false, "interpolation": null, "ramp_ratio": 0.2 } \ No newline at end of file diff --git a/robosuite/controllers/osc.py b/robosuite/controllers/osc.py index 015bcb65d2..9c8a236920 100644 --- a/robosuite/controllers/osc.py +++ b/robosuite/controllers/osc.py @@ -267,7 +267,7 @@ def set_goal(self, action, set_pos=None, set_ori=None): ) if self.interpolator_pos is not None: - self.interpolator_pos.set_goal(self.goal_pos) + self.interpolator_pos.set_goal(np.array(self.goal_pos)) if self.interpolator_ori is not None: self.ori_ref = np.array(self.ee_ori_mat) # reference is the current orientation at start diff --git a/robosuite/environments/base.py b/robosuite/environments/base.py index 62752b2fa9..2e10c4d43c 100644 --- a/robosuite/environments/base.py +++ b/robosuite/environments/base.py @@ -11,6 +11,9 @@ from robosuite.utils import OpenCVRenderer, SimulationError, XMLError from robosuite.utils.binding_utils import MjRenderContextOffscreen, MjSim +import robosuite.utils.transform_utils as T +import copy + REGISTERED_ENVS = {} @@ -150,6 +153,9 @@ def __init__( # check if viewer has get observations method and set a flag for future use. self.viewer_get_obs = hasattr(self.viewer, "_get_observations") + + # custom control + self.old_gripper_state = -1 def initialize_renderer(self): self.renderer = self.renderer.lower() @@ -379,6 +385,64 @@ def step(self, action): raise ValueError("executing action in terminated episode") self.timestep += 1 + + # # custom control + # T_local_global = np.array( + # [ + # [0, 1, 0], + # [-1, 0, 0], + # [0, 0, 1] + # ],dtype=float + # ) + # CONTROL_ERROR = 1e-3 + # MAX_STEPS = 600 + + # step_cnt = 0 + # cur_ee_pos = self._observables['robot0_eef_pos'].obs + # cur_ee_quat = self._observables['robot0_eef_quat'].obs + # cur_ee_mat = T.quat2mat(cur_ee_quat) + # cur_ee_mat_local = cur_ee_mat@T_local_global + # cur_ee_quat_local = T.mat2quat(cur_ee_mat_local) + # cur_pose1 = np.array([*cur_ee_pos, *cur_ee_quat_local]) + # cur_pose2 = np.array([*cur_ee_pos, *(cur_ee_quat_local*-1)]) + + # target_pose = np.array([*action[:3], *T.axisangle2quat(action[3:6])]) + + # cur_pose_error = min(np.linalg.norm(target_pose - cur_pose1),np.linalg.norm(target_pose - cur_pose2)) + + # policy_step = True + # action_copy = copy.deepcopy(action) + # # action_copy[-1] = self.old_gripper_state + # while cur_pose_error > CONTROL_ERROR or step_cnt < int(self.control_timestep / self.model_timestep): + # self.sim.forward() + # self._pre_action(action_copy, policy_step) + # self.sim.step() + # self._update_observables() + # policy_step = False + # # + # step_cnt += 1 + # # update current error + # cur_ee_pos = self._observables['robot0_eef_pos'].obs + # cur_ee_quat = self._observables['robot0_eef_quat'].obs + # cur_ee_mat = T.quat2mat(cur_ee_quat) + # cur_ee_mat_local = cur_ee_mat@T_local_global + # cur_ee_quat_local = T.mat2quat(cur_ee_mat_local) + # cur_pose1 = np.array([*cur_ee_pos, *cur_ee_quat_local]) + # cur_pose2 = np.array([*cur_ee_pos, *(cur_ee_quat_local*-1)]) + # cur_pose_error = min(np.linalg.norm(target_pose - cur_pose1),np.linalg.norm(target_pose - cur_pose2)) + + # if step_cnt >= MAX_STEPS: + # break + # pass + + # # self.sim.forward() + # # self._pre_action(action, True) + # # self.sim.step() + # # self._update_observables() + # # step_cnt += 1 + # # self.old_gripper_state = action[-1] + + # self.cur_time += self.model_timestep*step_cnt # Since the env.step frequency is slower than the mjsim timestep frequency, the internal controller will output # multiple torque commands in between new high level action commands. Therefore, we need to denote via @@ -397,6 +461,9 @@ def step(self, action): # Note: this is done all at once to avoid floating point inaccuracies self.cur_time += self.control_timestep + + + reward, done, info = self._post_action(action)