Skip to content

Commit 9103fca

Browse files
OceanPresentChaoOceanPresentChao
committed
feat: bluetooth
1 parent ba48f38 commit 9103fca

2 files changed

Lines changed: 145 additions & 51 deletions

File tree

‎robosuite/devices/dualsense.py‎

Lines changed: 144 additions & 50 deletions
Original file line numberDiff line numberDiff line change
@@ -34,23 +34,27 @@
3434

3535

3636
class ConnectionType(IntFlag):
37-
BT = 0x0
3837
USB = 0x1
39-
UNKNOWN = 0x2
38+
BT01 = 0x2
39+
BT31 = 0x4
40+
UNKNOWN = 0x8
4041

4142
@classmethod
4243
def to_string(cls, value) -> str:
4344
if value & cls.UNKNOWN:
4445
return "Unknown"
45-
if value & cls.BT:
46-
return "Bluetooth"
46+
if value & cls.BT01:
47+
return "Bluetooth 01"
48+
if value & cls.BT31:
49+
return "Bluetooth 31"
4750
if value & cls.USB:
4851
return "USB"
4952
return "Unknown"
5053

5154

5255
USB_REPORT_LENGTH = 64
53-
BT_REPORT_LENGTH = 78
56+
BT_REPORT31_LENGTH = 78
57+
BT_REPORT01_LENGTH = 10
5458
AxisSpec = namedtuple("AxisSpec", ["channel", "byte1", "byte2", "scale"])
5559
DUALSENSE_AXIS_LIST = ["LX", "LY", "RX", "RY", "L2_Trigger", "R2_Trigger"]
5660
DUALSENSE_BTN_LIST = [
@@ -195,7 +199,7 @@ def __init__(
195199
self,
196200
env,
197201
vendor_id=macros.DUALSENSE_VENDOR_ID,
198-
product_id=macros.DUALSENSE_PRODUCT_IDs[0],
202+
product_id=macros.DUALSENSE_PRODUCT_ID,
199203
pos_sensitivity=1.0,
200204
rot_sensitivity=1.0,
201205
reverse_xy=False,
@@ -222,6 +226,14 @@ def __init__(
222226
self.input_report_length = -1
223227
self.output_report_length = -1
224228
self.connection_type = self._check_connection_type()
229+
# By default, bluetooth-connected DualSense only sends input report 0x01 which omits motion and touchpad data.
230+
# Reading feature report 0x05 causes it to start sending input report 0x31.
231+
# Note: The Gamepad API will do this for us if it enumerates the gamepad.Other applications like Steam may have also done this already.
232+
if self.connection_type == ConnectionType.BT01:
233+
self.device.get_feature_report(0x05, BT_REPORT31_LENGTH)
234+
self.connection_type = ConnectionType.BT31
235+
self.input_report_length = BT_REPORT31_LENGTH
236+
self.output_report_length = BT_REPORT31_LENGTH
225237
print("DualSense Connection type: %s" % ConnectionType.to_string(self.connection_type))
226238
print("")
227239
print(
@@ -309,45 +321,16 @@ def _check_connection_type(self):
309321
self.input_report_length = USB_REPORT_LENGTH
310322
self.output_report_length = USB_REPORT_LENGTH
311323
return ConnectionType.USB
312-
elif dummy_report_length == BT_REPORT_LENGTH:
313-
self.input_report_length = BT_REPORT_LENGTH
314-
self.output_report_length = BT_REPORT_LENGTH
315-
return ConnectionType.BT
324+
elif dummy_report_length == BT_REPORT01_LENGTH:
325+
self.input_report_length = BT_REPORT01_LENGTH
326+
self.output_report_length = BT_REPORT01_LENGTH
327+
return ConnectionType.BT01
328+
elif dummy_report_length == BT_REPORT31_LENGTH:
329+
self.input_report_length = BT_REPORT31_LENGTH
330+
self.output_report_length = BT_REPORT31_LENGTH
331+
return ConnectionType.BT31
316332
return ConnectionType.UNKNOWN
317333

318-
def _parse_report_bytes(self, state_bytes: bytearray):
319-
new_state = DSState()
320-
# states 0 is always 1
321-
new_state.LX = state_bytes[1] - 128
322-
new_state.LY = state_bytes[2] - 128
323-
new_state.RX = state_bytes[3] - 128
324-
new_state.RY = state_bytes[4] - 128
325-
new_state.L2_Trigger = state_bytes[5]
326-
new_state.R2_Trigger = state_bytes[6]
327-
328-
# state 7 always increments -> not used anywhere
329-
330-
buttonState = state_bytes[8]
331-
new_state.Triangle = (buttonState & (1 << 7)) != 0
332-
new_state.Circle = (buttonState & (1 << 6)) != 0
333-
new_state.Cross = (buttonState & (1 << 5)) != 0
334-
new_state.Square = (buttonState & (1 << 4)) != 0
335-
336-
# dpad
337-
dpad_state = buttonState & 0x0F
338-
new_state.setDPadState(dpad_state)
339-
340-
misc = state_bytes[9]
341-
new_state.L1 = (misc & (1 << 0)) != 0
342-
new_state.R1 = (misc & (1 << 1)) != 0
343-
new_state.L2 = (misc & (1 << 2)) != 0
344-
new_state.R2 = (misc & (1 << 3)) != 0
345-
# new_state.share = (misc & (1 << 4)) != 0
346-
# new_state.options = (misc & (1 << 5)) != 0
347-
new_state.L3 = (misc & (1 << 6)) != 0
348-
new_state.R3 = (misc & (1 << 7)) != 0
349-
return new_state
350-
351334
def _check_btn_changed(self, btn_name: str):
352335
"""
353336
Check if a button has been pressed or released.
@@ -368,15 +351,22 @@ def run(self):
368351
while True:
369352
d = self.device.read(self.input_report_length)
370353
if d is not None and self._enabled:
371-
# the reports for BT and USB are structured the same,
372-
# but there is one more byte at the start of the bluetooth report.
373-
# We drop that byte, so that the format matches up again.
374-
report_bytes: bytearray = (
375-
bytearray(d)[1:] if self.connection_type == ConnectionType.BT else bytearray(d)
376-
)
354+
report_bytes = bytearray(d)
377355
self.report_bytes = report_bytes
378356
self.last_state = self.state
379-
self.state = self._parse_report_bytes(report_bytes)
357+
358+
if self.connection_type == ConnectionType.USB:
359+
self.state = parse_usb_report(self, report_bytes)
360+
elif self.connection_type == ConnectionType.BT01:
361+
# report id 0x01
362+
assert report_bytes[0] == 0x01
363+
self.state = parse_bt01_report(report_bytes)
364+
# report id 0x31
365+
elif self.connection_type == ConnectionType.BT31:
366+
assert report_bytes[0] == 0x31
367+
self.state = parse_bt31_report(report_bytes)
368+
else:
369+
raise NotImplementedError(f"Connection type {self.connection_type} not supported")
380370
self.x = scale_to_control(self.state.LX if not self.reverse_xy else self.state.LY)
381371
self.y = scale_to_control(self.state.LY if not self.reverse_xy else self.state.LX)
382372
self.roll = scale_to_control(self.state.RX if not self.reverse_xy else self.state.RY)
@@ -484,6 +474,110 @@ def _postprocess_device_outputs(self, dpos, drotation):
484474
return dpos, drotation
485475

486476

477+
def parse_usb_report(state_bytes: bytearray) -> DSState:
478+
new_state = DSState()
479+
# states 0 is always 1
480+
new_state.LX = state_bytes[1] - 128
481+
new_state.LY = state_bytes[2] - 128
482+
new_state.RX = state_bytes[3] - 128
483+
new_state.RY = state_bytes[4] - 128
484+
new_state.L2_Trigger = state_bytes[5]
485+
new_state.R2_Trigger = state_bytes[6]
486+
487+
# state 7 always increments -> not used anywhere
488+
489+
buttonState = state_bytes[8]
490+
new_state.Triangle = (buttonState & (1 << 7)) != 0
491+
new_state.Circle = (buttonState & (1 << 6)) != 0
492+
new_state.Cross = (buttonState & (1 << 5)) != 0
493+
new_state.Square = (buttonState & (1 << 4)) != 0
494+
495+
# dpad
496+
dpad_state = buttonState & 0x0F
497+
new_state.setDPadState(dpad_state)
498+
499+
misc = state_bytes[9]
500+
new_state.L1 = (misc & (1 << 0)) != 0
501+
new_state.R1 = (misc & (1 << 1)) != 0
502+
new_state.L2 = (misc & (1 << 2)) != 0
503+
new_state.R2 = (misc & (1 << 3)) != 0
504+
# new_state.share = (misc & (1 << 4)) != 0
505+
# new_state.options = (misc & (1 << 5)) != 0
506+
new_state.L3 = (misc & (1 << 6)) != 0
507+
new_state.R3 = (misc & (1 << 7)) != 0
508+
return new_state
509+
510+
511+
def parse_bt01_report(state_bytes: bytearray) -> DSState:
512+
# states 0 is always 0x01
513+
assert len(state_bytes) == BT_REPORT01_LENGTH
514+
assert state_bytes[0] == 0x01
515+
new_state = DSState()
516+
new_state.LX = state_bytes[1] - DUALSENSE_STICK_Neutral
517+
new_state.LY = state_bytes[2] - DUALSENSE_STICK_Neutral
518+
new_state.RX = state_bytes[3] - DUALSENSE_STICK_Neutral
519+
new_state.RY = state_bytes[4] - DUALSENSE_STICK_Neutral
520+
new_state.L2_Trigger = state_bytes[8]
521+
new_state.R2_Trigger = state_bytes[9]
522+
523+
buttonState = state_bytes[5]
524+
new_state.Square = (buttonState & (1 << 4)) != 0
525+
new_state.Cross = (buttonState & (1 << 5)) != 0
526+
new_state.Circle = (buttonState & (1 << 6)) != 0
527+
new_state.Triangle = (buttonState & (1 << 7)) != 0
528+
529+
# dpad
530+
dpad_state = buttonState & 0x0F
531+
new_state.setDPadState(dpad_state)
532+
533+
misc = state_bytes[6]
534+
new_state.L1 = (misc & (1 << 0)) != 0
535+
new_state.R1 = (misc & (1 << 1)) != 0
536+
new_state.L2 = (misc & (1 << 2)) != 0
537+
new_state.R2 = (misc & (1 << 3)) != 0
538+
# new_state.share = (misc & (1 << 4)) != 0
539+
# new_state.options = (misc & (1 << 5)) != 0
540+
new_state.L3 = (misc & (1 << 6)) != 0
541+
new_state.R3 = (misc & (1 << 7)) != 0
542+
return new_state
543+
544+
545+
def parse_bt31_report(state_bytes: bytearray) -> DSState:
546+
# states 0 is always 0x31
547+
assert len(state_bytes) == BT_REPORT31_LENGTH
548+
assert state_bytes[0] == 0x31
549+
new_state = DSState()
550+
new_state.LX = state_bytes[2] - DUALSENSE_STICK_Neutral
551+
new_state.LY = state_bytes[3] - DUALSENSE_STICK_Neutral
552+
new_state.RX = state_bytes[4] - DUALSENSE_STICK_Neutral
553+
new_state.RY = state_bytes[5] - DUALSENSE_STICK_Neutral
554+
new_state.L2_Trigger = state_bytes[6]
555+
new_state.R2_Trigger = state_bytes[7]
556+
557+
# state 7 always increments -> not used anywhere
558+
559+
buttonState = state_bytes[9]
560+
new_state.Triangle = (buttonState & (1 << 7)) != 0
561+
new_state.Circle = (buttonState & (1 << 6)) != 0
562+
new_state.Cross = (buttonState & (1 << 5)) != 0
563+
new_state.Square = (buttonState & (1 << 4)) != 0
564+
565+
# dpad
566+
dpad_state = buttonState & 0x0F
567+
new_state.setDPadState(dpad_state)
568+
569+
misc = state_bytes[10]
570+
new_state.L1 = (misc & (1 << 0)) != 0
571+
new_state.R1 = (misc & (1 << 1)) != 0
572+
new_state.L2 = (misc & (1 << 2)) != 0
573+
new_state.R2 = (misc & (1 << 3)) != 0
574+
# new_state.share = (misc & (1 << 4)) != 0
575+
# new_state.options = (misc & (1 << 5)) != 0
576+
new_state.L3 = (misc & (1 << 6)) != 0
577+
new_state.R3 = (misc & (1 << 7)) != 0
578+
return new_state
579+
580+
487581
if __name__ == "__main__":
488582
env = make("Lift", robots="Panda")
489583
dualsense = DualSense(env)

‎robosuite/macros.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@
4040

4141
# DualSense settings. Used by DualSense class in robosuite/devices/dualsense.py
4242
DUALSENSE_VENDOR_ID = 0x054C
43-
DUALSENSE_PRODUCT_IDs = [0x0CE6, 0x0DF2]
43+
DUALSENSE_PRODUCT_ID = 0x0CE6
4444

4545
# If LOGGING LEVEL is set to None, the logger will be turned off
4646
CONSOLE_LOGGING_LEVEL = "INFO"

0 commit comments

Comments
 (0)