From 030472693c17858eb11c022ce0822bb07d118f7c Mon Sep 17 00:00:00 2001 From: yangning wu Date: Mon, 17 Jun 2024 10:38:08 +0800 Subject: [PATCH] add joystick support of low_state.wireless_remote() --- example/python/stand_go2.py | 4 +- readme.md | 2 +- .../unitree_sdk2_bridge.cc | 28 ++++++ .../unitree_sdk2_bridge/unitree_sdk2_bridge.h | 17 +++- simulate_python/test/test_unitree_sdk2.py | 2 +- simulate_python/unitree_sdk2py_bridge.py | 95 +++++++++++++++---- 6 files changed, 127 insertions(+), 21 deletions(-) diff --git a/example/python/stand_go2.py b/example/python/stand_go2.py index 154eea5..ca52195 100644 --- a/example/python/stand_go2.py +++ b/example/python/stand_go2.py @@ -29,9 +29,9 @@ input("Press enter to start") if __name__ == '__main__': if len(sys.argv) <2: - ChannelFactortyInitialize(1, "lo") + ChannelFactoryInitialize(1, "lo") else: - ChannelFactortyInitialize(0, sys.argv[1]) + ChannelFactoryInitialize(0, sys.argv[1]) # Create a publisher to publish the data defined in UserData class pub = ChannelPublisher("rt/lowcmd", LowCmd_) diff --git a/readme.md b/readme.md index d35e12c..bc06f93 100644 --- a/readme.md +++ b/readme.md @@ -60,7 +60,7 @@ sudo apt install libyaml-cpp-dev ``` ### 2. Compile unitree_mujoco ```bash -cd simulate/ +cd unitree_mujoco/simulate mkdir build && cd build cmake .. make -j4 diff --git a/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc b/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc index ec6058a..e9223b7 100644 --- a/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc +++ b/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc @@ -66,6 +66,34 @@ void UnitreeSdk2Bridge::PublishLowState() low_state.imu_state().accelerometer()[1] = mj_data_->sensordata[dim_motor_sensor_ + 8]; low_state.imu_state().accelerometer()[2] = mj_data_->sensordata[dim_motor_sensor_ + 9]; } + + if (js_) + { + js_->getState(); + wireless_remote_.btn.components.R1 = js_->button_[js_id_.button["RB"]]; + wireless_remote_.btn.components.L1 = js_->button_[js_id_.button["LB"]]; + wireless_remote_.btn.components.start = js_->button_[js_id_.button["START"]]; + wireless_remote_.btn.components.select = js_->button_[js_id_.button["SELECT"]]; + wireless_remote_.btn.components.R2 = (js_->axis_[js_id_.axis["RT"]] > 0); + wireless_remote_.btn.components.L2 = (js_->axis_[js_id_.axis["LT"]] > 0); + wireless_remote_.btn.components.F1 = 0; + wireless_remote_.btn.components.F2 = 0; + wireless_remote_.btn.components.A = js_->button_[js_id_.button["A"]]; + wireless_remote_.btn.components.B = js_->button_[js_id_.button["B"]]; + wireless_remote_.btn.components.X = js_->button_[js_id_.button["X"]]; + wireless_remote_.btn.components.Y = js_->button_[js_id_.button["Y"]]; + wireless_remote_.btn.components.up = (js_->axis_[js_id_.axis["DY"]] < 0); + wireless_remote_.btn.components.right = (js_->axis_[js_id_.axis["DX"]] > 0); + wireless_remote_.btn.components.down = (js_->axis_[js_id_.axis["DY"]] > 0); + wireless_remote_.btn.components.left = (js_->axis_[js_id_.axis["DX"]] < 0); + + wireless_remote_.lx = double(js_->axis_[js_id_.axis["LX"]]) / max_value_; + wireless_remote_.ly = -double(js_->axis_[js_id_.axis["LY"]]) / max_value_; + wireless_remote_.rx = double(js_->axis_[js_id_.axis["RX"]]) / max_value_; + wireless_remote_.ry = -double(js_->axis_[js_id_.axis["RY"]]) / max_value_; + + memcpy(&low_state.wireless_remote()[0], &wireless_remote_, 40); + } low_state_puber_->Write(low_state); } diff --git a/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.h b/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.h index 84352c0..cec2722 100755 --- a/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.h +++ b/simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.h @@ -48,6 +48,19 @@ typedef union uint16_t value; } xKeySwitchUnion; +typedef struct +{ + uint8_t head[2]; + xKeySwitchUnion btn; + float lx; + float rx; + float ry; + float L2; + float ly; + + uint8_t idle[16]; +} xRockerBtnDataStruct; + // Defaults to xbox gamepad struct JoystickId { @@ -106,7 +119,9 @@ public: ThreadPtr HighStatePuberThreadPtr; ThreadPtr WirelessControllerPuberThreadPtr; - xKeySwitchUnion dds_keys_; + xKeySwitchUnion dds_keys_ = {}; + xRockerBtnDataStruct wireless_remote_ = {}; + JoystickId js_id_; Joystick *js_; int max_value_ = (1 << 15); // 16 bits joystick diff --git a/simulate_python/test/test_unitree_sdk2.py b/simulate_python/test/test_unitree_sdk2.py index 5737e58..771cb4b 100644 --- a/simulate_python/test/test_unitree_sdk2.py +++ b/simulate_python/test/test_unitree_sdk2.py @@ -21,7 +21,7 @@ def LowStateHandler(msg: LowState_): if __name__ == "__main__": - ChannelFactortyInitialize(1, "lo") + ChannelFactoryInitialize(1, "lo") hight_state_suber = ChannelSubscriber("rt/sportmodestate", SportModeState_) low_state_suber = ChannelSubscriber("rt/lowstate", LowState_) diff --git a/simulate_python/unitree_sdk2py_bridge.py b/simulate_python/unitree_sdk2py_bridge.py index 6e5164e..ad80a9b 100644 --- a/simulate_python/unitree_sdk2py_bridge.py +++ b/simulate_python/unitree_sdk2py_bridge.py @@ -2,6 +2,7 @@ import mujoco import numpy as np import pygame import sys +import struct from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelPublisher from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowCmd_ @@ -34,6 +35,8 @@ class UnitreeSdk2Bridge: self.have_frame_sensor = False self.dt = self.mj_model.opt.timestep + self.joystick = None + # Check sensor for i in range(self.dim_motor_sensor, self.mj_model.nsensor): name = mujoco.mj_id2name( @@ -43,7 +46,6 @@ class UnitreeSdk2Bridge: self.have_imu_ = True if name == "frame_pos": self.have_frame_sensor_ = True - # Unitree sdk2 message self.low_state = unitree_go_msg_dds__LowState_() @@ -97,9 +99,6 @@ class UnitreeSdk2Bridge: "left": 15, } - self.joystick = None - - def LowCmdHandler(self, msg: LowCmd_): if self.mj_data != None: for i in range(self.num_motor): @@ -160,6 +159,58 @@ class UnitreeSdk2Bridge: self.dim_motor_sensor + 9 ] + if self.joystick != None: + pygame.event.get() + # Buttons + self.low_state.wireless_remote[2] = int( + "".join( + [ + f"{key}" + for key in [ + 0, + 0, + int(self.joystick.get_axis(self.axis_id["LT"]) > 0), + int(self.joystick.get_axis(self.axis_id["RT"]) > 0), + int(self.joystick.get_button(self.button_id["SELECT"])), + int(self.joystick.get_button(self.button_id["START"])), + int(self.joystick.get_button(self.button_id["LB"])), + int(self.joystick.get_button(self.button_id["RB"])), + ] + ] + ), + 2, + ) + self.low_state.wireless_remote[3] = int( + "".join( + [ + f"{key}" + for key in [ + int(self.joystick.get_hat(0)[0] < 0), # left + int(self.joystick.get_hat(0)[1] < 0), # down + int(self.joystick.get_hat(0)[0] > 0), # right + int(self.joystick.get_hat(0)[1] > 0), # up + int(self.joystick.get_button(self.button_id["Y"])), # Y + int(self.joystick.get_button(self.button_id["X"])), # X + int(self.joystick.get_button(self.button_id["B"])), # B + int(self.joystick.get_button(self.button_id["A"])), # A + ] + ] + ), + 2, + ) + # Axes + sticks = [ + self.joystick.get_axis(self.axis_id["LX"]), + self.joystick.get_axis(self.axis_id["RX"]), + -self.joystick.get_axis(self.axis_id["RY"]), + -self.joystick.get_axis(self.axis_id["LY"]), + ] + packs = list(map(lambda x: struct.pack("f", x), sticks)) + self.low_state.wireless_remote[4:8] = packs[0] + self.low_state.wireless_remote[8:12] = packs[1] + self.low_state.wireless_remote[12:16] = packs[2] + self.low_state.wireless_remote[20:24] = packs[3] + self.low_state_puber.Write(self.low_state) def PublishHighState(self): @@ -190,23 +241,35 @@ class UnitreeSdk2Bridge: def PublishWirelessController(self): if self.joystick != None: pygame.event.get() - key_state = [0]*16 - key_state[self.key_map["R1"]] = self.joystick.get_button(self.button_id["RB"]) - key_state[self.key_map["L1"]] = self.joystick.get_button(self.button_id["LB"]) - key_state[self.key_map["start"]] = self.joystick.get_button(self.button_id["START"]) - key_state[self.key_map["select"]] = self.joystick.get_button(self.button_id["SELECT"]) - key_state[self.key_map["R2"]] = (self.joystick.get_axis(self.axis_id["RT"])>0) - key_state[self.key_map["L2"]] = (self.joystick.get_axis(self.axis_id["LT"])>0) + key_state = [0] * 16 + key_state[self.key_map["R1"]] = self.joystick.get_button( + self.button_id["RB"] + ) + key_state[self.key_map["L1"]] = self.joystick.get_button( + self.button_id["LB"] + ) + key_state[self.key_map["start"]] = self.joystick.get_button( + self.button_id["START"] + ) + key_state[self.key_map["select"]] = self.joystick.get_button( + self.button_id["SELECT"] + ) + key_state[self.key_map["R2"]] = ( + self.joystick.get_axis(self.axis_id["RT"]) > 0 + ) + key_state[self.key_map["L2"]] = ( + self.joystick.get_axis(self.axis_id["LT"]) > 0 + ) key_state[self.key_map["F1"]] = 0 key_state[self.key_map["F2"]] = 0 key_state[self.key_map["A"]] = self.joystick.get_button(self.button_id["A"]) key_state[self.key_map["B"]] = self.joystick.get_button(self.button_id["B"]) key_state[self.key_map["X"]] = self.joystick.get_button(self.button_id["X"]) key_state[self.key_map["Y"]] = self.joystick.get_button(self.button_id["Y"]) - key_state[self.key_map["up"]] = (self.joystick.get_hat(0)[1]>0) - key_state[self.key_map["right"]] = (self.joystick.get_hat(0)[0]>0) - key_state[self.key_map["down"]] = (self.joystick.get_hat(0)[1]<0) - key_state[self.key_map["left"]] = (self.joystick.get_hat(0)[0]<0) + key_state[self.key_map["up"]] = self.joystick.get_hat(0)[1] > 0 + key_state[self.key_map["right"]] = self.joystick.get_hat(0)[0] > 0 + key_state[self.key_map["down"]] = self.joystick.get_hat(0)[1] < 0 + key_state[self.key_map["left"]] = self.joystick.get_hat(0)[0] < 0 key_value = 0 for i in range(16): @@ -230,7 +293,7 @@ class UnitreeSdk2Bridge: else: print("No gamepad detected.") sys.exit() - + if js_type == "xbox": self.axis_id = { "LX": 0, # Left stick axis x