diff --git a/robosuite/controllers/config/osc_pose.json b/robosuite/controllers/config/osc_pose.json index 8dc645e44b..cff921a2d3 100644 --- a/robosuite/controllers/config/osc_pose.json +++ b/robosuite/controllers/config/osc_pose.json @@ -5,14 +5,14 @@ "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, + "damping_ratio": 1.0, "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, + "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 a45843d087..9c8a236920 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,7 +219,7 @@ def set_goal(self, action, set_pos=None, set_ori=None): """ # Update state self.update() - + # 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:] @@ -266,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/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/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) 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 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