add joystick support of low_state.wireless_remote()

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_)

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

simulate/src/unitree_sdk2_bridge/unitree_sdk2_bridge.cc

@@ -67,6 +67,34 @@ void UnitreeSdk2Bridge::PublishLowState()
             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);
     }
 }

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

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_)
 

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(
@@ -44,7 +47,6 @@ class UnitreeSdk2Bridge:
             if name == "frame_pos":
                 self.have_frame_sensor_ = True
 
-
         # Unitree sdk2 message
         self.low_state = unitree_go_msg_dds__LowState_()
         self.low_state_puber = ChannelPublisher(TOPIC_LOWSTATE, 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 = [0] * 16
-            key_state[self.key_map["R1"]] = self.joystick.get_button(self.button_id["RB"])
+            key_state[self.key_map["R1"]] = self.joystick.get_button(
-            key_state[self.key_map["L1"]] = self.joystick.get_button(self.button_id["LB"])
+                self.button_id["RB"]
-            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["L1"]] = self.joystick.get_button(
-            key_state[self.key_map["R2"]] = (self.joystick.get_axis(self.axis_id["RT"])>0)
+                self.button_id["LB"]
-            key_state[self.key_map["L2"]] = (self.joystick.get_axis(self.axis_id["LT"])>0)
+            )
+            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["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["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["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["left"]] = self.joystick.get_hat(0)[0] < 0
 
             key_value = 0
             for i in range(16):