Mocap

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
 }

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

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

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

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)
 

robosuite/models/arenas/multi_table_arena.py

@@ -1,4 +1,4 @@
-from collections import Iterable
+from collections.abc import Iterable
 
 import numpy as np
 

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:

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"