206333308
commented
8 months ago My device is omega.7, which can feed back six-axis data in real time (absolute quantity rather than variation), but when I use this module I wrote in demo_device_control.py, there may be some problems when executing input2action() (I ‘m not sure), the phenomenon is: 1. The simulation is stuck 2. The generated action is unreasonable。 Thank you very much for your help
import threading
import time
import numpy as np
import omega
from robosuite.devices import Device
from robosuite.utils.transform_utils import rotation_matrix
class Omega7(Device):
def __init__(
self,
pos_sensitivity=400.0,
rot_sensitivity=1.0,
):
print("Opening Omega7 device")
omega.open_device()
self.pos_sensitivity = pos_sensitivity
self.rot_sensitivity = rot_sensitivity
# 6-DOF variables
self.x, self.y, self.z = 0, 0, 0
self.last_x, self.last_y, self.last_z = 0, 0, 0
self.roll, self.pitch, self.yaw = 0, 0, 0
self.last_roll, self.last_pitch, self.last_yaw = 0, 0, 0
self.single_click_and_hold = False
self._control = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
self._lastcontrol = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
self._reset_state = 0
self.rotation = np.array([[-1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, -1.0]])
self._enabled = False
# launch a new listener thread to listen to SpaceMouse
self.thread = threading.Thread(target=self.run)
self.thread.daemon = True
self.thread.start()
def _reset_internal_state(self):
"""
Resets internal state of controller, except for the reset signal.
"""
self.rotation = np.array([[-1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, -1.0]])
# Reset 6-DOF variables
self.x, self.y, self.z = 0, 0, 0
self.roll, self.pitch, self.yaw = 0, 0, 0
# Reset control
self._control = np.zeros(6)
# Reset grasp
self.single_click_and_hold = False
def start_control(self):
"""
Method that should be called externally before controller can
start receiving commands.
"""
self._reset_internal_state()
self._reset_state = 0
self._enabled = True
def get_controller_state(self):
"""
Grabs the current state of the 3D mouse.
Returns:
dict: A dictionary containing dpos, orn, unmodified orn, grasp, and reset
"""
dpos = (self.control - self._lastcontrol)[:3] * 0.005 * self.pos_sensitivity
# dpos = self.control[:3] * 0.005 * self.pos_sensitivity # 此处0.005可能要改
# roll, pitch, yaw = self.control[3:] * 0.005 * self.rot_sensitivity
roll, pitch, yaw = 0 , 0, 0
self.last_x, self.last_y, self.last_z,self.last_roll, self.last_pitch, self.last_yaw = self.x,self.y,self.z,self.roll,self.pitch,self.yaw
self._lastcontrol = [
self.last_x,
self.last_y,
self.last_z,
self.last_roll,
self.last_pitch,
self.last_yaw,
]
# convert RPY to an absolute orientation
drot1 = rotation_matrix(angle=-pitch, direction=[1.0, 0, 0], point=None)[:3, :3]
drot2 = rotation_matrix(angle=roll, direction=[0, 1.0, 0], point=None)[:3, :3]
drot3 = rotation_matrix(angle=yaw, direction=[0, 0, 1.0], point=None)[:3, :3]
self.rotation = self.rotation.dot(drot1.dot(drot2.dot(drot3)))
return dict(
dpos=dpos,
rotation=self.rotation,
raw_drotation=np.array([roll, pitch, yaw]),
grasp=self.control_gripper,
reset=self._reset_state,
)
def run(self):
while True:
self.x,self.y, self.z,self.roll,self.pitch,self.yaw = omega.get_pos_and_orideg()
self._control = [
self.x,
self.y,
self.z,
self.roll,
self.pitch,
self.yaw,
]
# 夹爪功能待完善
@property
def control(self):
"""
Grabs current pose of Spacemouse
Returns:
np.array: 6-DoF control value
"""
return np.array(self._control)
@property
def control_gripper(self):
"""
Maps internal states into gripper commands.
Returns:
float: Whether we're using single click and hold or not
"""
if self.single_click_and_hold:
return 1.0
return 0
# if __name__ == "__main__":
# omega7 = Omega7()
# for i in range(100):
# # while True:
# print(omega7.control, omega7.control_gripper)
# time.sleep(0.02)
if __name__ == "__main__":
omega7 = Omega7()
for i in range(100): # 或者使用 while True: 以持续运行
# 获取控制器状态
controller_state = omega7.get_controller_state()
# 仅打印 dpos 值
print("dpos:", controller_state["dpos"])
time.sleep(0.02) # 控制循环速度，以便更容易阅读输出
zhuyifengzju
Hi, can you provide more information about this? What kind of device do you want to use? As long as you can get an array of action values, integrating your custom device should be fairly easy.